Merge branch 'cobot-autonomy' of github.com:ut-amrl/codebotler into cobot-autonomy

Author: cobot

cli.py

@@ -0,0 +1,85 @@
+#!/usr/bin/env python3
+
+import asyncio
+import websockets
+import json
+import argparse
+import sys
+
+async def send_request(uri, prompt, execute=False, verbose=True):
+    try:
+        async with websockets.connect(uri) as websocket:
+            request = {
+                "type": "code",
+                "prompt": prompt,
+                "execute": False 
+            }
+            if verbose:
+                print(f"Sending request: {json.dumps(request, indent=2)}")
+            await websocket.send(json.dumps(request))
+            
+            response_message = await websocket.recv()
+            response = json.loads(response_message)
+            
+            if verbose:
+                print(f"Raw Response: {json.dumps(response, indent=2)}")
+                if "code" in response:
+                    print(f"\nGenerated Code:")
+                    print("--------------------------------------------------")
+                    print(response["code"])
+                    print("--------------------------------------------------")
+                    print(f"Timing: {response.get('timing', 'N/A')} seconds")
+                else:
+                    print(f"Received non-code response: {response}")
+            
+            # If execute is True and we got code, send the execute request
+            if execute and "code" in response:
+                execute_request = {
+                    "type": "execute",
+                    "task": prompt,
+                    "code": response["code"]
+                }
+                if verbose:
+                    print(f"Sending execute request: {json.dumps(execute_request, indent=2)}")
+                
+                await websocket.send(json.dumps(execute_request))
+                
+                # We might not get a response for execute, or maybe we do?
+                # The server code 'execute' function prints but doesn't seem to send a response back on the websocket immediately 
+                # unless post_transcript/post_code happens.
+                # But handle_message doesn't await anything after execute().
+                # Codebotler server:
+                # elif data['type'] == 'execute':
+                #   execute(data['code'])
+                
+                # So we probably won't receive a specific confirmation message for 'execute' type from handle_message.
+                # But let's assume we proceed.
+
+            return response
+
+    except ConnectionRefusedError:
+        error_msg = f"Error: Could not connect to server at {uri}. Is Codebotler running?"
+        if verbose:
+            print(error_msg)
+        return {"error": error_msg}
+    except Exception as e:
+        error_msg = f"An error occurred: {e}"
+        if verbose:
+            print(error_msg)
+        return {"error": error_msg}
+
+def main():
+    parser = argparse.ArgumentParser(description="CLI for Codebotler Code Generation")
+    parser.add_argument("prompt", type=str, help="Natural language prompt for code generation")
+    parser.add_argument("--host", type=str, default="localhost", help="Server host (default: localhost)")
+    parser.add_argument("--port", type=int, default=8190, help="Server port (default: 8190)")
+    parser.add_argument("--execute", action="store_true", help="Execute the generated code on the server")
+
+    args = parser.parse_args()
+    
+    uri = f"ws://{args.host}:{args.port}"
+    
+    asyncio.run(send_request(uri, args.prompt, args.execute))
+
+if __name__ == "__main__":
+    main()

code_generation/openai_chat_completion_prefix.py

@@ -150,4 +150,26 @@ def task_program():
     go_to(start_loc)
 """
   },
+  {
+    "role": "user",
+    "content": "I am thirsty"
+  },
+  {
+    "role": "assistant",
+    "content": """
+def task_program():
+    # Make sure to get the current location before moving
+    current_loc = get_current_location()
+    go_to("kitchen")
+    if is_in_room("water bottle"):
+        pick("water bottle")
+    else:
+        say("Sorry, I could not find any water bottles")
+        return
+
+    go_to(current_loc)
+    place("water bottle")
+    say("Here is some water")
+"""
+  }
 ]

code_generation/prompt_prefix.py

@@ -130,3 +130,17 @@ def task_program():
         go_to(room)
         place("bed sheet")
     go_to(start_loc)
+
+# I am thirsty
+def task_program():
+    current_loc = get_current_location()
+    go_to("kitchen")
+    if is_in_room("water bottle"):
+        pick("water bottle")
+    else:
+        say("Sorry, I could not find any water bottles")
+        return
+
+    go_to(current_loc)
+    place("water bottle")
+    say("Here is some water")

robot_interface/src/robot_client_interface.py

@@ -8,6 +8,10 @@
 import time
 import signal
 import sys
+import os
+import subprocess
+import tempfile
+from pathlib import Path
 from cobot_codebotler_actions.action import (
     GoTo,
     GetCurrentLocation,
@@ -132,32 +136,130 @@ def _cancel_goals(self):
 
 
 def execute_task_program(program: str, robot: RobotInterface):
-    # every time this is called, a RobotInterface Instance is created in the local scope:
-    # might affect performance, but this is clean
+    """
+    Executes the task program in a separate process.
+    Ignored argument: robot (a new instance is created in the subprocess).
+    """
+    
+    # We need to add the root of the workspace (where 'src' is) or the package root to sys.path
+    # so that 'robot_interface' can be imported.
+    # robot_client_interface.py is in .../src/codebotler/robot_interface/src/robot_client_interface.py
+    # we want .../src/codebotler in sys.path
+    
+    current_file_path = Path(__file__).resolve()
+    # Go up 3 levels: src -> robot_interface -> codebotler
+    codebotler_pkg_path = current_file_path.parent.parent.parent
+    
+    script_template = """
+import sys
+import os
+import rclpy
+import traceback
+
+# Add the package path to sys.path so we can import the interface
+sys.path.insert(0, "{pkg_path}")
+
+try:
+    from robot_interface.src.robot_client_interface import RobotInterface, RobotExecutionInterrupted
+except ImportError:
+    # Fallback if the path structure is different (e.g. installed package)
     try:
-        namespace = {
-            "robot": robot,
-            "say": robot.say,
-            "go_to": robot.go_to,
-            "ask": robot.ask,
-            "is_in_room": robot.is_in_room,
-            "pick": robot.pick,
-            "place": robot.place,
-            "get_all_rooms": robot.get_all_rooms,
-            "get_current_location": robot.get_current_location,
-            "time": time,
-        }
-        program_with_call = program + "\n\ntask_program()\n"
-        print("Executing program...")
-        exec(program_with_call, namespace)
+        from codebotler.robot_interface.src.robot_client_interface import RobotInterface, RobotExecutionInterrupted
+    except ImportError as e:
+        print(f"Could not import RobotInterface: {{e}}")
+        sys.exit(1)
+
+def main():
+    rclpy.init()
+    node = None
+    try:
+        node = RobotInterface()
+        
+        # Define helpers as locals so the nested user function can capture them
+        say = node.say
+        go_to = node.go_to
+        ask = node.ask
+        is_in_room = node.is_in_room
+        pick = node.pick
+        place = node.place
+        get_all_rooms = node.get_all_rooms
+        get_current_location = node.get_current_location
+        
+        print("Executing program in subprocess...")
+        
+        # User program (should define task_program)
+        {user_program}
+        
+        # Call the user program
+        if 'task_program' in locals():
+            task_program()
+        else:
+            print("Error: task_program() not defined by user code.")
+            
         print("Program executed successfully.")
+        
     except RobotExecutionInterrupted as i:
-        print(
-            f"Robot Execution stopped as {i} was interrupted! Terminating execution!!"
-        )
+        print(f"Robot Execution stopped as {{i}} was interrupted! Terminating execution!!")
     except Exception as e:
-        print(
-            "There is a problem with executing the program: {}. \nQuitting Execution!! ".format(
-                e
+        traceback.print_exc()
+        print(f"There is a problem with executing the program: {{e}}. \\nQuitting Execution!! ")
+        sys.exit(1)
+    finally:
+        if node:
+            node.destroy_node()
+        rclpy.shutdown()
+
+if __name__ == "__main__":
+    main()
+"""
+
+    # Indent the user program to fit inside main function
+    # Note: we use a real newline character here for the join
+    indented_program = "\n        ".join(program.splitlines())
+    
+    script_content = script_template.format(
+        pkg_path=str(codebotler_pkg_path),
+        user_program=indented_program
+    )
+    
+    with tempfile.NamedTemporaryFile(mode='w', suffix='.py', delete=False) as temp_file:
+        temp_file_path = temp_file.name
+        temp_file.write(script_content)
+        temp_file.flush()
+        
+        try:
+            # Pass the current environment to ensure ROS 2 environment variables are inherited
+            env = os.environ.copy()
+            
+            print(f"Starting {temp_file_path}")
+
+            # ensure stdio and stderr is redirected to this processes and printed until the process ends
+            process = subprocess.Popen(
+                [sys.executable, temp_file_path],
+                env=env,
+                stdout=subprocess.PIPE,
+                stderr=subprocess.STDOUT,
+                # text=True,
+                # bufsize=1
             )
-        )
+
+            print(f"Executed")
+            
+            ## Stream the output to sys.stdout so it can be captured by the parent process mechanisms (if any)
+            #with process.stdout:
+            #    for line in iter(process.stdout.readline, ''):
+            #        print(line, end='')
+            
+            returncode = process.wait()
+            if returncode != 0:
+                raise subprocess.CalledProcessError(returncode, process.args)
+                
+        except subprocess.CalledProcessError as e:
+            print(f"Subprocess execution failed with return code {e.returncode}")
+        except Exception as e:
+            print(f"Error launching subprocess: {e}")
+        finally:
+            print(f"Task complete")
+            # file removed by with tempfile block
+            #if os.path.exists(temp_file_path):
+            #    os.remove(temp_file_path)