Unitree WebRTC implementation on rebased dev

.gitmodules

@@ -1,13 +1,9 @@
 [submodule "dimos/external/openMVS"]
 	path = dimos/external/openMVS
 	url = https://github.com/cdcseacave/openMVS.git
-
 [submodule "dimos/external/vcpkg"]
 	path = dimos/external/vcpkg
 	url = https://github.com/microsoft/vcpkg.git
-[submodule "dimos/robot/unitree/external/go2_webrtc_connect"]
-	path = dimos/robot/unitree/external/go2_webrtc_connect
-	url = https://github.com/spomichter/go2_webrtc_connect
 [submodule "dimos/robot/unitree/external/go2_ros2_sdk"]
 	path = dimos/robot/unitree/external/go2_ros2_sdk
 	url = https://github.com/dimensionalOS/go2_ros2_sdk

dimos/robot/abstract_robot.py

@@ -0,0 +1,66 @@
+"""Abstract base class for all DIMOS robot implementations.
+
+This module defines the AbstractRobot class which serves as the foundation for
+all robot implementations in DIMOS, establishing a common interface regardless
+of the underlying hardware or communication protocol (ROS, WebRTC, etc).
+"""
+
+from abc import ABC, abstractmethod
+from typing import Any, Union, Optional
+from reactivex.observable import Observable
+import numpy as np
+
+
+class AbstractRobot(ABC):
+    """Abstract base class for all robot implementations.
+
+    This class defines the minimal interface that all robot implementations
+    must provide, regardless of whether they use ROS, WebRTC, or other
+    communication protocols.
+    """
+
+    @abstractmethod
+    def connect(self) -> bool:
+        """Establish a connection to the robot.
+
+        This method should handle all necessary setup to establish 
+        communication with the robot hardware.
+
+        Returns:
+            bool: True if connection was successful, False otherwise.
+        """
+        pass
+
+    @abstractmethod
+    def move(self, *args, **kwargs) -> bool:
+        """Move the robot.
+
+        This is a generic movement interface that should be implemented
+        by all robot classes. The exact parameters will depend on the
+        specific robot implementation.
+
+        Returns:
+            bool: True if movement command was successfully sent.
+        """
+        pass
+
+    @abstractmethod
+    def get_video_stream(self, fps: int = 30) -> Observable:
+        """Get a video stream from the robot's camera.
+
+        Args:
+            fps: Frames per second for the video stream. Defaults to 30.
+
+        Returns:
+            Observable: An observable stream of video frames.
+        """
+        pass
+
+    @abstractmethod
+    def stop(self) -> None:
+        """Clean up resources and stop the robot.
+
+        This method should handle all necessary cleanup when shutting down
+        the robot connection, including stopping any ongoing movements.
+        """
+        pass

dimos/robot/global_planner/planner.py

@@ -37,11 +37,11 @@ def plan(self, goal: VectorLike) -> Path: ...
     def set_goal(
         self, goal: VectorLike, goal_theta: Optional[float] = None, stop_event: Optional[threading.Event] = None
     ):
-        goal = to_vector(goal).to_2d()
         path = self.plan(goal)
         if not path:
             logger.warning("No path found to the goal.")
             return False
+        print("pathing success", path)
         return self.set_local_nav(path, stop_event=stop_event, goal_theta=goal_theta)
 
 
@@ -53,12 +53,14 @@ class AstarPlanner(Planner):
     conservativism: int = 8
 
     def plan(self, goal: VectorLike) -> Path:
-        pos = self.get_robot_pos()
+        goal = to_vector(goal).to_2d()
+        pos = self.get_robot_pos().to_2d()
         costmap = self.get_costmap().smudge(preserve_unknown=False)
 
-        self.vis("planner_costmap", costmap)
+        # self.vis("costmap", costmap)
         self.vis("target", goal)
 
+        print("ASTAR ", costmap, goal, pos)
         path = astar(costmap, goal, pos)
 
         if path:

dimos/robot/unitree/unitree_go2.py

@@ -30,8 +30,7 @@
 from dimos.utils.logging_config import setup_logger
 from dimos.perception.person_tracker import PersonTrackingStream
 from dimos.perception.object_tracker import ObjectTrackingStream
-from dimos.robot.local_planner.vfh_local_planner import VFHPurePursuitPlanner
-from dimos.robot.local_planner.local_planner import navigate_path_local
+from dimos.robot.local_planner import VFHPurePursuitPlanner, navigate_path_local
 from dimos.robot.global_planner.planner import AstarPlanner
 from dimos.types.path import Path
 from dimos.types.costmap import Costmap
@@ -170,7 +169,7 @@ def __init__(
             robot_width=0.36,  # Unitree Go2 width in meters
             robot_length=0.6,  # Unitree Go2 length in meters
             max_linear_vel=0.5,
-            lookahead_distance=1.0,
+            lookahead_distance=2.0,
             visualization_size=500,  # 500x500 pixel visualization
         )
 

dimos/robot/unitree/unitree_skills.py

@@ -237,7 +237,10 @@ def __call__(self):
     # region Class-based Skills
 
     class Move(AbstractRobotSkill):
-        """Move the robot using direct velocity commands."""
+        """Move the robot using direct velocity commands.
+
+        This skill works with both ROS and WebRTC robot implementations.
+        """
 
         x: float = Field(..., description="Forward velocity (m/s).")
         y: float = Field(default=0.0, description="Left/right velocity (m/s)")
@@ -246,10 +249,52 @@ class Move(AbstractRobotSkill):
 
         def __call__(self):
             super().__call__()
-            return self._robot.move_vel(x=self.x, y=self.y, yaw=self.yaw, duration=self.duration)
+
+            from dimos.types.vector import Vector
+            vector = Vector(self.x, self.y, self.yaw)
+
+            # Handle duration for continuous movement
+            if self.duration > 0:
+                import time
+                import threading
+                import asyncio
+
+                # Create a stop event
+                stop_event = threading.Event()
+
+                # Function to continuously send movement commands
+                async def continuous_move():
+                    loop = asyncio.new_event_loop()
+                    asyncio.set_event_loop(loop)
+                    start_time = time.time()
+                    try:
+                        while not stop_event.is_set() and (time.time() - start_time) < self.duration:
+                            self._robot.move(vector)
+                            await asyncio.sleep(0.001)  # Send commands at 1000Hz
+                        # Always stop at the end
+                        self._robot.move(Vector(0, 0, 0))
+                    finally:
+                        loop.close()
+
+                # Run movement in a separate thread with asyncio event loop
+                move_thread = threading.Thread(target=lambda: asyncio.run(continuous_move()))
+                move_thread.daemon = True
+                move_thread.start()
+
+                # Wait for the full duration
+                time.sleep(self.duration)
+                stop_event.set()
+                move_thread.join(timeout=0.5)  # Wait for thread to finish with timeout
+            else:
+                # Just execute the move command once for continuous movement
+                self._robot.move(vector)
+            return True
 
     class Reverse(AbstractRobotSkill):
-        """Reverse the robot using direct velocity commands."""
+        """Reverse the robot using direct velocity commands.
+
+        This skill works with both ROS and WebRTC robot implementations.
+        """
 
         x: float = Field(..., description="Backward velocity (m/s). Positive values move backward.")
         y: float = Field(default=0.0, description="Left/right velocity (m/s)")
@@ -258,8 +303,46 @@ class Reverse(AbstractRobotSkill):
 
         def __call__(self):
             super().__call__()
-            # Use move_vel with negative x for backward movement
-            return self._robot.move_vel(x=-self.x, y=self.y, yaw=self.yaw, duration=self.duration)
+            from dimos.types.vector import Vector
+            # Use negative x for backward movement
+            vector = Vector(-self.x, self.y, self.yaw)
+
+            # Handle duration for continuous movement
+            if self.duration > 0:
+                import time
+                import threading
+                import asyncio
+
+                # Create a stop event
+                stop_event = threading.Event()
+
+                # Function to continuously send movement commands
+                async def continuous_move():
+                    loop = asyncio.new_event_loop()
+                    asyncio.set_event_loop(loop)
+                    start_time = time.time()
+                    try:
+                        while not stop_event.is_set() and (time.time() - start_time) < self.duration:
+                            self._robot.move(vector)
+                            await asyncio.sleep(0.001)  # Send commands at 1000Hz
+                        # Always stop at the end
+                        self._robot.move(Vector(0, 0, 0))
+                    finally:
+                        loop.close()
+
+                # Run movement in a separate thread with asyncio event loop
+                move_thread = threading.Thread(target=lambda: asyncio.run(continuous_move()))
+                move_thread.daemon = True
+                move_thread.start()
+
+                # Wait for the full duration
+                time.sleep(self.duration)
+                stop_event.set()
+                move_thread.join(timeout=0.5)  # Wait for thread to finish with timeout
+            else:
+                # Just execute the move command once for continuous movement
+                self._robot.move(vector)
+            return True
 
     class SpinLeft(AbstractRobotSkill):
         """Spin the robot left using degree commands."""