robi_butler.py

#!/usr/bin/env python
"""
Robi Butler - Main Robot Control System

This module serves as the main orchestrator for the Robi robot system.
It integrates:
- Zoom interface for remote human-robot communication
- LLM-based chat agent for natural language understanding
- Fetch agent for robot manipulation and navigation
- ROS topic-based communication

The system receives user instructions, processes them through an LLM planner,
and executes robot actions while providing real-time feedback.

Author: Robot Communication Team
License: MIT
"""

import rospy
from std_msgs.msg import String
from geometry_msgs.msg import Point32
import sys
import os
import re
import time
import threading
from typing import Optional, List, Dict, Any

# Import local modules
from llm_planner import ChatAgent
from fetch_agent import FetchAgent


class RobiButler:
    """
    Main robot butler system for coordinating robot actions.
    
    This class integrates:
    - Chat agent (LLM-based natural language understanding)
    - Fetch agent (robot manipulation and navigation)
    - ROS communication (topics and services)
    
    It processes user instructions through the LLM to generate action sequences,
    executes them on the robot, and provides feedback through multiple channels.
    
    Attributes:
        chat_agent: LLM-based agent for natural language processing
        fetch_agent: Robot control agent for manipulation tasks
        user_instruction_sub: ROS subscriber for user commands
        user_point_sub: ROS subscriber for pointing coordinates
        robot_feedback_pub: ROS publisher for robot responses
    """
    
    def __init__(self, use_gesture: bool = True):
        """
        Initialize the Robi Butler system.
        
        Args:
            use_gesture: Whether gesture/pointing input is enabled
        """
        # Initialize agents
        rospy.loginfo("Initializing Chat Agent...")
        self.chat_agent = ChatAgent(has_gesture=use_gesture)
        
        rospy.loginfo("Initializing Fetch Agent...")
        self.fetch_agent = FetchAgent()
        
        # ROS Publishers and Subscribers
        self.user_instruction_sub = rospy.Subscriber(
            '/user_instruction',
            String,
            self.user_instruction_callback
        )
        
        # Subscribe to user pointing coordinates
        self.user_point_sub = rospy.Subscriber(
            '/user_point',
            Point32,
            self.user_point_callback
        )
        
        self.robot_feedback_pub = rospy.Publisher(
            '/robot_feedback',
            String,
            queue_size=10
        )
        
        # State variables
        self.last_user_instruction = ""
        self.processing_lock = threading.Lock()
        
        rospy.loginfo("Robi Butler initialized and ready!")

    def user_instruction_callback(self, msg: String):
        """
        ROS callback for receiving user instructions.
        Processes instructions in a separate thread to avoid blocking.
        
        Args:
            msg: String message containing user instruction
        """
        instruction = msg.data
        
        if instruction:
            self.last_user_instruction = instruction
            rospy.loginfo(f"Received user instruction: {instruction}")
            
            # Process instruction in a separate thread to avoid blocking
            processing_thread = threading.Thread(
                target=self.process_instruction,
                args=(instruction,),
                daemon=True,
                name="InstructionProcessor"
            )
            processing_thread.start()

    def user_point_callback(self, msg: Point32):
        """
        ROS callback for receiving user pointing gestures.
        Adds points to the fetch agent's point buffer.
        
        Args:
            msg: Point32 message with normalized coordinates (0-1)
        """
        point = msg
        
        # Convert normalized coordinates to pixel coordinates
        # Assuming 640x480 resolution
        y = int(point.y * 480)
        x = int(point.x * 640)
        point_coord = [[x, y]]
        
        self.fetch_agent.points.append(point_coord)
        
        # Keep only last 5 points
        if len(self.fetch_agent.points) > 5:
            self.fetch_agent.points.pop(0)
        
        rospy.loginfo(f"Received user point: ({x}, {y})")
        rospy.loginfo(f"Total points in buffer: {len(self.fetch_agent.points)}")

    def process_instruction(self, instruction: str):
        """
        Process user instruction through LLM and execute resulting commands.
        Thread-safe processing with lock to prevent concurrent execution.
        
        Args:
            instruction: Natural language instruction from user
        """
        with self.processing_lock:
            try:
                # Get current robot state
                state = self.fetch_agent.get_state()
                
                # Prepare input for LLM (include state information)
                full_input = f"state: {str(state)}\n{instruction}"
                
                # Get response from chat agent (LLM)
                response = self.chat_agent.get_response(full_input)
                
                rospy.loginfo(f"Chat agent response: {response}")
                
                # Execute the generated code
                self.execute_code(response, human_input=instruction)
                
            except Exception as e:
                error_msg = f"Error processing instruction: {str(e)}"
                rospy.logerr(error_msg)
                self.publish_feedback(error_msg)

    def publish_feedback(self, feedback: str):
        """
        Publish feedback message to /robot_feedback topic.
        
        Args:
            feedback: Feedback message to publish
        """
        feedback_msg = String()
        feedback_msg.data = feedback
        self.robot_feedback_pub.publish(feedback_msg)
        rospy.loginfo(f"Published feedback: {feedback}")

    def execute_code(self, input_text: str, human_input: Optional[str] = None):
        """
        Execute robot commands extracted from LLM response.
        
        Parses the LLM output for action commands in the format:
        [action1("arg1"), action2("arg2"), ...]
        
        Args:
            input_text: LLM response text containing commands
            human_input: Original human instruction (optional)
        """
        # Extract commands from the input (look for [...] block)
        start = input_text.find('[')
        end = input_text.find(']')
        
        if start == -1 or end == -1:
            # No code found, just publish the response as feedback
            if input_text:
                self.publish_feedback(input_text)
                # Also speak the response
                self.fetch_agent.speak_to_zoom(input_text)
            # Send end signal
            
            return

        # Extract voice response (text before [...])
        voice = input_text[0:start]
        voice = voice.replace("Code:", "").strip()
        
        # Publish voice response
        if voice:
            self.publish_feedback(voice)
            self.fetch_agent.speak_to_zoom(voice)

        # Extract and parse commands
        commands_str = input_text[start+1:end]
        rospy.loginfo(f"Commands to execute: {commands_str}")
        
        # Count pointing commands (marked with *)
        num_pointing = commands_str.count('*')
        
        # Split commands by ), separator
        if '),' in commands_str:
            pattern = r'\),\s?'
            commands = re.split(pattern, commands_str)
        else:
            commands = [commands_str]

        rospy.loginfo(f"Number of pointing commands: {num_pointing}")
        
        # Execute each command
        j = 0  # Index for pointing commands
        for i, command in enumerate(commands):
            try:
                if '(' not in command:
                    continue
                
                # Parse action and argument
                action, arg = command.split("(", 1)
                action = action.strip()
                arg = arg.replace(")", "").strip('"')

                rospy.loginfo(f"Executing: {action}({arg})")
                
                # Check if action exists in fetch agent
                if hasattr(self.fetch_agent, action):
                    # Validate location for pick/place operations
                    if (action in ["pick", "placeon"] and
                        self.fetch_agent.current_location not in 
                        self.fetch_agent.special_locations):
                        error_msg = "Sorry, currently I can't do that from this location."
                        self.publish_feedback(error_msg)
                        self.fetch_agent.speak_to_zoom(error_msg)
                        
                        return

                    # Handle pointing commands (marked with *)
                    if '*' in arg:
                        if len(self.fetch_agent.points):
                            result = self._execute_pointing_command(
                                action, arg, j, num_pointing
                            )
                            j += 1
                        else:
                            error_msg = "Please point to the object you are referring to."
                            self.publish_feedback(error_msg)
                            self.fetch_agent.speak_to_zoom(error_msg)
                            
                            return
                    else:
                        # Handle regular commands
                        result = self._execute_regular_command(action, arg)

                    # Handle action results
                    should_continue = self.handle_action_result(action, result, arg)
                    if not should_continue:
                        return
                    
                else:
                    error_msg = f"Action '{action}' not recognized"
                    rospy.logwarn(error_msg)
                    self.publish_feedback(error_msg)
                    
                    return
                    
            except Exception as e:
                error_msg = f"Error executing command '{command}': {str(e)}"
                rospy.logerr(error_msg)
                self.publish_feedback(error_msg)
                
                return
                
        # Send end signal after successful completion of all commands
        
    
    def _execute_pointing_command(self, action: str, arg: str, 
                                   point_index: int, num_pointing: int) -> Any:
        """
        Execute command that requires pointing input.
        
        Args:
            action: Action name
            arg: Action argument
            point_index: Index of current pointing command
            num_pointing: Total number of pointing commands
            
        Returns:
            Result from action execution
        """
        point = self.fetch_agent.points[-num_pointing + point_index]
        
        if action == "vqa":
            return self.fetch_agent.vqa_point(arg, point)
        elif action == "pick":
            return self.fetch_agent.pick_point(point)
        elif action == "placeon":
            return self.fetch_agent.placeon_point2(point)
        elif action == "move":
            return self.fetch_agent.move_point(point)
        else:
            error_msg = f"Sorry, I can't do {action} with pointing."
            self.publish_feedback(error_msg)
            self.fetch_agent.speak_to_zoom(error_msg)
            
            return None
    
    def _execute_regular_command(self, action: str, arg: str) -> Any:
        """
        Execute regular (non-pointing) command.
        
        Args:
            action: Action name
            arg: Action argument
            
        Returns:
            Result from action execution
        """
        # Provide feedback for certain actions
        if action == "move":
            self.publish_feedback(f"Moving to the {arg}")
            self.fetch_agent.speak_to_zoom(f"Moving to the {arg}")
        elif action == "open":
            self.publish_feedback(f"Let me try to open the {arg}, it might take a while.")
            self.fetch_agent.speak_to_zoom(f"Let me try to open the {arg}, it might take a while.")
        elif action == "close":
            self.publish_feedback(f"Let me try to close the {arg}, it might take a while.")
            self.fetch_agent.speak_to_zoom(f"Let me try to close the {arg}, it might take a while.")
        
        # Execute the action
        return getattr(self.fetch_agent, action)(arg)

    def handle_action_result(self, action: str, result: Any, arg: str) -> bool:
        """
        Handle the results of robot actions and provide appropriate feedback.
        
        Args:
            action: Name of the action that was executed
            result: Return value from the action
            arg: Argument that was passed to the action
            
        Returns:
            True if execution should continue, False if it should stop
        """
        try:
            if action == "vqa":
                if result:
                    self.publish_feedback(result)
                    self.fetch_agent.speak_to_zoom(result)
                    self.chat_agent.add_message({"role": "assistant", "content": result})

            elif action == "move":
                if result == 1:
                    error_msg = "Sorry, this location is not in the map"
                    self.publish_feedback(error_msg)
                    self.fetch_agent.speak_to_zoom(error_msg)
                    
                    return False

            elif action == "open":
                if result == 1:
                    error_msg = "Sorry, I currently can't open this."
                    self.publish_feedback(error_msg)
                    self.fetch_agent.speak_to_zoom(error_msg)
                    
                    return False
                elif result == 2:
                    retry_msg = "Sorry, I think I failed to open this, let me try again."
                    self.publish_feedback(retry_msg)
                    self.fetch_agent.speak_to_zoom(retry_msg)
                    # Retry once
                    retry_result = self.fetch_agent.open(arg)
                    if retry_result == 2:
                        fail_msg = "Very sorry, I still failed to open this."
                        self.publish_feedback(fail_msg)
                        self.fetch_agent.speak_to_zoom(fail_msg)
                        
                        return False

            elif action == "close":
                if result == 1:
                    error_msg = "Sorry, I currently can't close this."
                    self.publish_feedback(error_msg)
                    self.fetch_agent.speak_to_zoom(error_msg)
                    
                    return False

            elif action == "placeon":
                if result == 1:
                    error_msg = "I don't have anything in my hand, the object might have slipped."
                    self.publish_feedback(error_msg)
                    self.fetch_agent.speak_to_zoom(error_msg)
                    
                    return False

            elif action == "pick" or "pick" in action:
                if result == 2:
                    clarify_msg = "Which one are you referring to? Please point to the object."
                    self.publish_feedback(clarify_msg)
                    self.fetch_agent.speak_to_zoom(clarify_msg)
                    self.chat_agent.add_message({"role": "assistant", "content": clarify_msg})
                elif result == 1:
                    error_msg = "Sorry, I can't grasp this object. It might be too far away or too big."
                    self.publish_feedback(error_msg)
                    self.fetch_agent.speak_to_zoom(error_msg)
                    self.chat_agent.add_message({"role": "assistant", "content": error_msg})
                    
                    return False
                elif result == 3:
                    error_msg = "Sorry, I can't detect the object."
                    self.publish_feedback(error_msg)
                    self.fetch_agent.speak_to_zoom(error_msg)
                    self.chat_agent.add_message({"role": "assistant", "content": error_msg})
                    
                    return False
                elif result == 4:
                    retry_msg = "Seems like the object slipped from my hand. Let me try again."
                    self.publish_feedback(retry_msg)
                    self.fetch_agent.speak_to_zoom(retry_msg)
                    # Retry once
                    retry_result = self.fetch_agent.pick(arg)
                    if retry_result in [1, 3, 4]:
                        fail_msg = "Sorry, I still can't grasp the object successfully."
                        self.publish_feedback(fail_msg)
                        self.fetch_agent.speak_to_zoom(fail_msg)
                        self.chat_agent.add_message({"role": "assistant", "content": fail_msg})
                        
                        return False
            
            return True
                        
        except Exception as e:
            rospy.logerr(f"Error handling action result: {e}")
            return True

    def run(self):
        """Main run loop for the Robi Butler system."""
        rospy.loginfo("Robi Butler is running and waiting for instructions...")
        try:
            rospy.spin()
        except KeyboardInterrupt:
            rospy.loginfo("Shutting down Robi Butler...")


def main():
    """Main entry point for the Robi Butler node."""
    rospy.init_node('robi_butler')
    rospy.loginfo("Starting Robi Butler...")
    
    # Get parameters
    use_gesture = rospy.get_param('~use_gesture', True)
    
    # Create butler instance
    butler = RobiButler(use_gesture=use_gesture)
    
    # Run the butler
    butler.run()


if __name__ == "__main__":
    main()