Module 4 Capstone: The Autonomous Humanoid

# humanoid_ai/voice_to_action_node.py (Snippet)
# Implement your chosen speech-to-text solution here.
# Example using a generic STT library:
import rclpy
from rclpy.node import Node
from std_msgs.msg import String
import speech_recognition as sr # Example library
import sounddevice as sd
import numpy as np
import scipy.io.wavfile as wav
import tempfile
import os

class VoiceToActionNode(Node):
    def __init__(self):
        super().__init__('voice_to_action_node')
        self.publisher_ = self.create_publisher(String, '/voice_command', 10)
        self.get_logger().info('Voice command node started. Listening...')
        self.recognizer = sr.Recognizer()
        self.listen_and_transcribe()

    def listen_and_transcribe(self):
        with sr.Microphone() as source:
            self.recognizer.adjust_for_ambient_noise(source)
            self.get_logger().info("Say something!")
            audio = self.recognizer.listen(source)

        try:
            # Example: using Google Web Speech API (requires internet, may have usage limits)
            text = self.recognizer.recognize_google(audio)
            self.get_logger().info(f'Heard: {text}')
            msg = String()
            msg.data = text
            self.publisher_.publish(msg)
        except sr.UnknownValueError:
            self.get_logger().warn("Speech Recognition could not understand audio")
        except sr.RequestError as e:
            self.get_logger().error(f"Could not request results from Speech Recognition service; {e}")

# main function and rclpy init/shutdown omitted for brevity

# humanoid_ai/llm_planner_node.py (Snippet)
import google.genai as genai
import os

class LLMPlannerNode(Node):
    def __init__(self):
        super().__init__('llm_planner_node')
        self.subscription = self.create_subscription(
            String, '/voice_command', self.command_callback, 10)
        # Add clients for Nav2 and MoveIt2 actions
        # Add publisher to send goals
        self.state = "IDLE"
        
        # --- LLM Setup ---
        try:
            genai.configure(api_key=os.environ["GEMINI_API_KEY"])
            self.model = genai.GenerativeModel('gemini-pro')
        except Exception as e:
            self.get_logger().error(f"Failed to initialize Gemini client: {e}")
            self.get_logger().error("Please make sure the GEMINI_API_KEY environment variable is set.")
            return


    def command_callback(self, msg):
        if self.state == "IDLE":
            self.get_logger().info(f'Received command: "{msg.data}"')
            self.state = "PLANNING"
            self.generate_plan(msg.data)

    def generate_plan(self, command):
        prompt = f"""
        You are the cognitive core for a humanoid robot.
        Translate the user's command into a numbered list of robotic actions.
        Available actions:
        - find_object(object_name)
        - go_to(x, y, z)
        - pick_up(object_name)
        - done()

        Command: "{command}"
        Plan:
        """
        # Call Gemini API with this prompt
        response = self.model.generate_content(prompt)

        # For this example, let's hardcode the plan
        plan = [
            "1. find_object('soda_can')",
            "2. go_to(2.5, 1.0, 0.8)", # Assume object location is returned
            "3. pick_up('soda_can')",
            "4. done()"
        ]
        self.execute_plan(plan)

    def execute_plan(self, plan):
        # A state machine to execute each step of the plan
        # This involves calling ROS 2 actions and services
        self.get_logger().info("Executing plan...")
        # ... implementation of plan execution ...

# humanoid_ai/perception_node.py (Snippet)
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
from ultralytics import YOLO

class PerceptionNode(Node):
    def __init__(self):
        super().__init__('perception_node')
        self.model = YOLO('yolov8n.pt')  # Load pretrained model
        self.bridge = CvBridge()
        self.subscription = self.create_subscription(
            Image, '/camera/image_raw', self.image_callback, 10)
        self.object_publisher = self.create_publisher(...) # Custom message for object location

    def image_callback(self, msg):
        cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
        results = self.model(cv_image)

        for r in results:
            boxes = r.boxes
            for box in boxes:
                # Get class name
                cls = int(box.cls[0])
                class_name = self.model.names[cls]
                if class_name == 'soda can': # Or whatever YOLO calls it
                    # ... calculate 3D position and publish it
                    self.get_logger().info(f'Found a soda can!')

# humanoid_bringup/launch/capstone_project.launch.py
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
import os

def generate_launch_description():
    humanoid_ai_pkg = get_package_share_directory('humanoid_ai')
    nav_pkg = get_package_share_directory('humanoid_navigation')

    return LaunchDescription([
        # Launch Gazebo, Nav2, MoveIt2...
        # ...
        
        # Launch AI Nodes
        Node(
            package='humanoid_ai',
            executable='voice_to_action_node',
            name='voice_to_action_node',
            output='screen'),
        Node(
            package='humanoid_ai',
            executable='llm_planner_node',
            name='llm_planner_node',
            output='screen'),
        Node(
            package='humanoid_ai',
            executable='perception_node',
            name='perception_node',
            output='screen'),
    ])