Voice Output of Object Detection Results

#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import rclpy
from rclpy.node import Node
from std_msgs.msg import String
from kachaka_interfaces.action import ExecKachakaCommand
from kachaka_interfaces.msg import KachakaCommand
from rclpy.action import ActionClient
import json
import time


class KachakaSpeakDetection(Node):
    """
    Node for speaking detected objects
    """
    def __init__(self):
        super().__init__('kachaka_speak_detection')
        
        # Set QoS profile for object detection data
        qos = rclpy.qos.QoSProfile(
            reliability=rclpy.qos.ReliabilityPolicy.BEST_EFFORT,
            durability=rclpy.qos.DurabilityPolicy.VOLATILE,
            history=rclpy.qos.HistoryPolicy.KEEP_LAST,
            depth=10
        )
        
        # Subscribe to object detection information from Lesson7
        self.subscription = self.create_subscription(
            String,
            '/image_yolo_detection/objects',  # Topic from Lesson7
            self.object_callback,
            qos
        )

        # Action client for speech output
        self._action_client = ActionClient(
            self, ExecKachakaCommand, "/kachaka/kachaka_command/execute"
        )
        self._action_client.wait_for_server()
        
        # Dictionary for converting English object names (kept in English for voice output)
        self.object_names = {
            'person': 'person',
            'car': 'car',
            'chair': 'chair',
            'bottle': 'bottle',
            'cup': 'cup',
            'laptop': 'laptop',
            'mouse': 'mouse',
            'keyboard': 'keyboard',
            'cell phone': 'smartphone',
            'book': 'book',
            # Add more as needed
        }
        
        # Store the last detected objects
        self.last_objects = []
        
        # Create a timer to periodically speak the detected objects
        self.timer = self.create_timer(5.0, self.timer_callback)
        
        self.get_logger().info("Kachaka speak detection node has started.")

    def object_callback(self, msg):
        """
        Process detected object information and store it
        """
        try:
            # Convert JSON string to Python object
            object_info = json.loads(msg.data)
            self.last_objects = object_info.get("objects", [])
            
        except Exception as e:
            self.get_logger().error(f"Error processing object information: {str(e)}")
            
    def timer_callback(self):
        """
        Periodically speak the detected objects.
        
        This callback is triggered every 5 seconds by the timer. It creates
        a natural-sounding sentence describing the detected objects and
        sends it to the robot for voice output.
        """
        # If no objects are detected, don't say anything
        if not self.last_objects:
            return
            
        # Convert all detected objects and create a summary message
        # Map English object names to display names (or keep original if not mapped)
        object_names = []
        for obj in self.last_objects:
            # Get the display name from dictionary, or use original name if not found
            object_name = self.object_names.get(obj, obj)
            object_names.append(object_name)
        
        # Create a natural-sounding summary message
        # Handle singular vs plural cases for better speech
        if len(object_names) == 1:
            # Single object: "I detected person"
            text = f"I detected {object_names[0]}"
        else:
            # Multiple objects: "I detected person, car and chair"
            # Join all but the last with commas, then add "and" before the last one
            text = f"I detected {', '.join(object_names[:-1])} and {object_names[-1]}"
        
        # Execute speech output by sending the text to the robot
        self.send_goal(text)
        
        # Log what was said for debugging
        self.get_logger().info(f"Speech output: {text}")
            
    def send_goal(self, text):
        """
        Send a speech command to Kachaka using the action server.
        
        This method creates a command to make the robot speak the given text.
        The command is sent asynchronously, so it doesn't block the node.
        
        Parameters:
            text: The text string that the robot should speak
        """
        # Create a KachakaCommand message
        command = KachakaCommand()
        # Set command type to SPEAK_COMMAND (tells robot to use voice output)
        command.command_type = KachakaCommand.SPEAK_COMMAND
        # Set the text content to be spoken
        command.speak_command_text = text

        # Create a goal message for the action
        goal_msg = ExecKachakaCommand.Goal()
        # Attach the command to the goal
        goal_msg.kachaka_command = command

        # Send the goal asynchronously (non-blocking)
        # This returns a Future that will complete when the server responds
        future = self._action_client.send_goal_async(goal_msg)
        # Set a callback to handle when the server accepts or rejects the goal
        future.add_done_callback(self.goal_response_callback)

    def goal_response_callback(self, future):
        """
        Callback for when the goal is accepted or rejected
        """
        goal_handle = future.result()
        if not goal_handle.accepted:
            self.get_logger().info("Goal rejected")
            return
        self.get_logger().info("Goal accepted")
        result_future = goal_handle.get_result_async()
        result_future.add_done_callback(self.get_result_callback)

    def get_result_callback(self, future):
        """
        Callback for when the goal is completed
        """
        result = future.result().result
        self.get_logger().info(f"Result: {result}")


def main(args=None):
    rclpy.init(args=args)
    node = KachakaSpeakDetection()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        node.get_logger().info("Shutting down...")
    finally:
        node.destroy_node()
        rclpy.shutdown()


if __name__ == '__main__':
    main()

# Move to workspace src directory
cd ~/ros2_ws/src

# Create package
ros2 pkg create --build-type ament_python kachaka_speak_detection --dependencies rclpy std_msgs kachaka_interfaces --node-name kachaka_speak_detection

# Move to workspace root directory
cd ~/ros2_ws

# Build package
colcon build --packages-select kachaka_speak_detection

# Set up environment
source install/setup.bash

ros2 run image_yolo_detection image_yolo_detection

ros2 run kachaka_speak_detection kachaka_speak_detection