Twist message for all move command, added keyboard teleop for easy robot control in sim (#570)

* Switched to using Twist instead of vector3 for all cmd_vel

* increased object tracker frame alignment time tolerance

* added camera_info timestamp

* added and tested keyboard teleop

* passing tests

* remove the pynput based keyboard twist

* foxglove extension for map visualization

* add keyboard control

* CI code cleanup

* add package json

---------

Co-authored-by: Paul Nechifor <paul@nechifor.net>
Co-authored-by: paul-nechifor <1262969+paul-nechifor@users.noreply.github.com>
Former-commit-id: aa5005d

Author: alexlin2

bin/explore-cmd

@@ -22,7 +22,7 @@
 
 import numpy as np
 
-from dimos.msgs.geometry_msgs import Vector3
+from dimos.msgs.geometry_msgs import Twist, Vector3
 from dimos.navigation.local_planner import BaseLocalPlanner
 from dimos.utils.transform_utils import quaternion_to_euler, normalize_angle, get_distance
 
@@ -73,12 +73,12 @@ def __init__(
         # Previous velocity for filtering (vx, vy, vtheta)
         self.v_prev = np.array([0.0, 0.0, 0.0])
 
-    def compute_velocity(self) -> Optional[Vector3]:
+    def compute_velocity(self) -> Optional[Twist]:
         """
         Compute velocity commands using GLAP algorithm.
 
         Returns:
-            Vector3 with x, y velocities in robot frame and z as angular velocity
+            Twist with linear and angular velocities in robot frame
         """
         if self.latest_odom is None or self.latest_path is None or self.latest_costmap is None:
             return None
@@ -151,7 +151,10 @@ def compute_velocity(self) -> Optional[Vector3]:
         v_filtered = self.alpha * v_raw + (1 - self.alpha) * self.v_prev
         self.v_prev = v_filtered
 
-        return Vector3(v_filtered[0], v_filtered[1], v_filtered[2])
+        return Twist(
+            linear=Vector3(v_filtered[0], v_filtered[1], 0.0),
+            angular=Vector3(0.0, 0.0, v_filtered[2]),
+        )
 
     def _compute_path_following(self, pose: np.ndarray, path: np.ndarray) -> np.ndarray:
         """

dimos/navigation/local_planner/holonomic_local_planner.py

@@ -25,7 +25,7 @@
 from typing import Optional
 
 from dimos.core import Module, In, Out, rpc
-from dimos.msgs.geometry_msgs import Vector3, PoseStamped
+from dimos.msgs.geometry_msgs import Twist, Vector3, PoseStamped
 from dimos.msgs.nav_msgs import OccupancyGrid, Path
 from dimos.utils.logging_config import setup_logger
 from dimos.utils.transform_utils import get_distance, quaternion_to_euler, normalize_angle
@@ -52,7 +52,7 @@ class BaseLocalPlanner(Module):
     path: In[Path] = None
 
     # LCM outputs
-    cmd_vel: Out[Vector3] = None
+    cmd_vel: Out[Twist] = None
 
     def __init__(
         self,
@@ -122,7 +122,7 @@ def _follow_path_loop(self):
         while not self.stop_planning.is_set():
             if self.is_goal_reached():
                 self.stop_planning.set()
-                stop_cmd = Vector3(0, 0, 0)
+                stop_cmd = Twist()
                 self.cmd_vel.publish(stop_cmd)
                 break
 
@@ -139,13 +139,13 @@ def _plan(self):
             self.cmd_vel.publish(cmd_vel)
 
     @abstractmethod
-    def compute_velocity(self) -> Optional[Vector3]:
+    def compute_velocity(self) -> Optional[Twist]:
         """
         Compute velocity commands based on current costmap, odometry, and path.
         Must be implemented by derived classes.
 
         Returns:
-            Vector3 message with velocity commands x, y, theta, or None if no command
+            Twist message with linear and angular velocity commands, or None if no command
         """
         pass
 
@@ -196,7 +196,7 @@ def stop(self):
             self.stop_planning.set()
             self.planning_thread.join(timeout=1.0)
             self.planning_thread = None
-        stop_cmd = Vector3(0, 0, 0)
+        stop_cmd = Twist()
         self.cmd_vel.publish(stop_cmd)
 
         logger.info("Local planner stopped")

dimos/navigation/local_planner/local_planner.py

@@ -76,9 +76,9 @@ def test_straight_path_no_obstacles(self, planner, empty_costmap):
 
         # Should move along +X
         assert vel is not None
-        assert vel.x > 0.9  # Close to v_max
-        assert abs(vel.y) < 0.1  # Near zero
-        assert abs(vel.z) < 0.1  # Small angular velocity when aligned with path
+        assert vel.linear.x > 0.9  # Close to v_max
+        assert abs(vel.linear.y) < 0.1  # Near zero
+        assert abs(vel.angular.z) < 0.1  # Small angular velocity when aligned with path
 
     def test_obstacle_gradient_repulsion(self, planner):
         """Test that obstacle gradients create repulsive forces."""
@@ -110,7 +110,7 @@ def test_obstacle_gradient_repulsion(self, planner):
 
         # Should have positive Y component (pushed north by gradient)
         assert vel is not None
-        assert vel.y > 0.1  # Repulsion pushes north
+        assert vel.linear.y > 0.1  # Repulsion pushes north
 
     def test_lowpass_filter(self):
         """Test that low-pass filter smooths velocity commands."""
@@ -146,7 +146,7 @@ def test_lowpass_filter(self):
         assert vel1 is not None
 
         # Store first velocity
-        first_vx = vel1.x
+        first_vx = vel1.linear.x
 
         # Second call - should be filtered
         vel2 = planner.compute_velocity()
@@ -156,8 +156,8 @@ def test_lowpass_filter(self):
         # v2 = 0.5 * v_raw + 0.5 * v1
         # The filtering effect should be visible
         # v2 should be between v1 and the raw velocity
-        assert vel2.x != first_vx  # Should be different due to filtering
-        assert 0 < vel2.x <= planner.v_max  # Should still be positive and within limits
+        assert vel2.linear.x != first_vx  # Should be different due to filtering
+        assert 0 < vel2.linear.x <= planner.v_max  # Should still be positive and within limits
 
     def test_no_path(self, planner, empty_costmap):
         """Test that planner returns None when no path is available."""
@@ -221,8 +221,8 @@ def test_goal_reached(self, planner, empty_costmap):
 
         # Should have near-zero velocity
         assert vel is not None
-        assert abs(vel.x) < 0.1
-        assert abs(vel.y) < 0.1
+        assert abs(vel.linear.x) < 0.1
+        assert abs(vel.linear.y) < 0.1
 
     def test_velocity_saturation(self, planner, empty_costmap):
         """Test that velocities are capped at v_max."""
@@ -247,8 +247,9 @@ def test_velocity_saturation(self, planner, empty_costmap):
 
         # Velocity should be saturated at v_max
         assert vel is not None
-        assert abs(vel.x) <= planner.v_max + 0.01  # Small tolerance
-        assert abs(vel.y) <= planner.v_max + 0.01
+        assert abs(vel.linear.x) <= planner.v_max + 0.01  # Small tolerance
+        assert abs(vel.linear.y) <= planner.v_max + 0.01
+        assert abs(vel.angular.z) <= planner.v_max + 0.01
 
     def test_lookahead_interpolation(self, planner, empty_costmap):
         """Test that lookahead point is correctly interpolated on path."""
@@ -274,8 +275,8 @@ def test_lookahead_interpolation(self, planner, empty_costmap):
 
         # Should move forward along path
         assert vel is not None
-        assert vel.x > 0.5  # Moving forward
-        assert abs(vel.y) < 0.1  # Staying on path
+        assert vel.linear.x > 0.5  # Moving forward
+        assert abs(vel.linear.y) < 0.1  # Staying on path
 
     def test_curved_path_following(self, planner, empty_costmap):
         """Test following a curved path."""
@@ -303,10 +304,10 @@ def test_curved_path_following(self, planner, empty_costmap):
         # Should have both X and Y components for curved motion
         assert vel is not None
         # Test general behavior: should be moving (not exact values)
-        assert vel.x > 0  # Moving forward (any positive value)
-        assert vel.y > 0  # Turning left (any positive value)
+        assert vel.linear.x > 0  # Moving forward (any positive value)
+        assert vel.linear.y > 0  # Turning left (any positive value)
         # Ensure we have meaningful movement, not just noise
-        total_linear = np.sqrt(vel.x**2 + vel.y**2)
+        total_linear = np.sqrt(vel.linear.x**2 + vel.linear.y**2)
         assert total_linear > 0.1  # Some reasonable movement
 
     def test_robot_frame_transformation(self, empty_costmap):
@@ -346,11 +347,11 @@ def test_robot_frame_transformation(self, empty_costmap):
         assert vel is not None
         # Test relative magnitudes and signs rather than exact values
         # Path is to the right, so Y velocity should be negative
-        assert vel.y < 0  # Should move right (negative Y in robot frame)
+        assert vel.linear.y < 0  # Should move right (negative Y in robot frame)
         # Should turn to align with path
-        assert vel.z < 0  # Should turn right (negative angular velocity)
+        assert vel.angular.z < 0  # Should turn right (negative angular velocity)
         # X velocity should be relatively small compared to Y
-        assert abs(vel.x) < abs(vel.y)  # Lateral movement dominates
+        assert abs(vel.linear.x) < abs(vel.linear.y)  # Lateral movement dominates
 
     def test_angular_velocity_computation(self, empty_costmap):
         """Test that angular velocity is computed to align with path."""
@@ -386,11 +387,13 @@ def test_angular_velocity_computation(self, empty_costmap):
         # Should have positive angular velocity to turn left
         assert vel is not None
         # Test general behavior without exact thresholds
-        assert vel.x > 0  # Moving forward (any positive value)
-        assert vel.y > 0  # Moving left (holonomic, any positive value)
-        assert vel.z > 0  # Turning left (positive angular velocity)
+        assert vel.linear.x > 0  # Moving forward (any positive value)
+        assert vel.linear.y > 0  # Moving left (holonomic, any positive value)
+        assert vel.angular.z > 0  # Turning left (positive angular velocity)
         # Verify the robot is actually moving with reasonable speed
-        total_linear = np.sqrt(vel.x**2 + vel.y**2)
+        total_linear = np.sqrt(vel.linear.x**2 + vel.linear.y**2)
         assert total_linear > 0.1  # Some meaningful movement
         # Since path is diagonal, X and Y should be similar magnitude
-        assert abs(vel.x - vel.y) < max(vel.x, vel.y) * 0.5  # Within 50% of each other
+        assert (
+            abs(vel.linear.x - vel.linear.y) < max(vel.linear.x, vel.linear.y) * 0.5
+        )  # Within 50% of each other

dimos/navigation/local_planner/test_base_local_planner.py

@@ -60,7 +60,6 @@ class ObjectTracking(Module):
 
     def __init__(
         self,
-        camera_intrinsics: Optional[List[float]] = None,  # [fx, fy, cx, cy]
         reid_threshold: int = 10,
         reid_fail_tolerance: int = 5,
         frame_id: str = "camera_link",
@@ -79,8 +78,7 @@ def __init__(
         # Call parent Module init
         super().__init__()
 
-        self.camera_intrinsics = camera_intrinsics
-        self._camera_info_received = False
+        self.camera_intrinsics = None
         self.reid_threshold = reid_threshold
         self.reid_fail_tolerance = reid_fail_tolerance
         self.frame_id = frame_id
@@ -141,7 +139,7 @@ def on_aligned_frames(frames_tuple):
             self.color_image.observable(),
             self.depth.observable(),
             buffer_size=2.0,  # 2 second buffer
-            match_tolerance=0.05,  # 50ms tolerance
+            match_tolerance=0.5,  # 500ms tolerance
         )
         self._aligned_frames_subscription = aligned_frames.subscribe(on_aligned_frames)
 
@@ -156,11 +154,6 @@ def on_camera_info(camera_info_msg: CameraInfo):
                 camera_info_msg.K[2],
                 camera_info_msg.K[5],
             ]
-            if not self._camera_info_received:
-                self._camera_info_received = True
-                logger.info(
-                    f"Camera intrinsics received from camera_info: {self.camera_intrinsics}"
-                )
 
         self.camera_info.subscribe(on_camera_info)
 

dimos/perception/object_tracker.py

@@ -30,7 +30,7 @@
 from reactivex.subject import Subject
 
 from dimos.core import In, Module, Out, rpc
-from dimos.msgs.geometry_msgs import Pose, Transform, Vector3
+from dimos.msgs.geometry_msgs import Pose, Transform, Twist, Vector3
 from dimos.msgs.sensor_msgs import Image
 from dimos.robot.connection_interface import ConnectionInterface
 from dimos.robot.unitree_webrtc.type.lidar import LidarMessage
@@ -80,20 +80,17 @@ def start_background_loop():
         self.thread.start()
         self.connection_ready.wait()
 
-    def move(self, velocity: Vector3, duration: float = 0.0) -> bool:
-        """Send movement command to the robot using velocity commands.
+    def move(self, twist: Twist, duration: float = 0.0) -> bool:
+        """Send movement command to the robot using Twist commands.
 
         Args:
-            velocity: Velocity vector [x, y, yaw] where:
-                     x: Forward/backward velocity (m/s)
-                     y: Left/right velocity (m/s)
-                     yaw: Rotational velocity (rad/s)
+            twist: Twist message with linear and angular velocities
             duration: How long to move (seconds). If 0, command is continuous
 
         Returns:
             bool: True if command was sent successfully
         """
-        x, y, yaw = velocity.x, velocity.y, velocity.z
+        x, y, yaw = twist.linear.x, twist.linear.y, twist.angular.z
 
         # WebRTC coordinate mapping:
         # x - Positive right, negative left
@@ -289,7 +286,7 @@ def stop(self) -> bool:
         Returns:
             bool: True if stop command was sent successfully
         """
-        return self.move(Vector3(0.0, 0.0, 0.0))
+        return self.move(Twist())
 
     def disconnect(self) -> None:
         """Disconnect from the robot and clean up resources."""

dimos/robot/unitree_webrtc/connection.py

@@ -24,7 +24,7 @@
 
 from reactivex import Observable
 
-from dimos.msgs.geometry_msgs import Vector3
+from dimos.msgs.geometry_msgs import Twist
 from dimos.msgs.sensor_msgs import Image
 from dimos.utils.data import get_data
 
@@ -169,9 +169,9 @@ def dispose():
 
         return Observable(on_subscribe)
 
-    def move(self, vector: Vector3, duration: float = 0.0):
+    def move(self, twist: Twist, duration: float = 0.0):
         if not self._is_cleaned_up:
-            self.mujoco_thread.move(vector, duration)
+            self.mujoco_thread.move(twist, duration)
 
     def stop(self):
         """Stop the MuJoCo connection gracefully."""

dimos/robot/unitree_webrtc/mujoco_connection.py

@@ -18,7 +18,7 @@
 
 from dimos import core
 from dimos.core import Module, Out, rpc
-from dimos.msgs.geometry_msgs import PoseStamped, Vector3, Quaternion
+from dimos.msgs.geometry_msgs import PoseStamped, Twist, Vector3, Quaternion
 from dimos.msgs.sensor_msgs import Image
 from dimos.msgs.nav_msgs import OccupancyGrid
 from dimos.protocol import pubsub
@@ -39,7 +39,7 @@
 class MovementControlModule(Module):
     """Simple module to send movement commands for testing."""
 
-    movecmd: Out[Vector3] = None
+    movecmd: Out[Twist] = None
 
     def __init__(self):
         super().__init__()
@@ -48,7 +48,7 @@ def __init__(self):
     @rpc
     def send_move_command(self, x: float, y: float, yaw: float):
         """Send a movement command."""
-        cmd = Vector3(x, y, yaw)
+        cmd = Twist(linear=Vector3(x, y, 0.0), angular=Vector3(0.0, 0.0, yaw))
         self.movecmd.publish(cmd)
         self.commands_sent.append(cmd)
         logger.info(f"Sent move command: x={x}, y={y}, yaw={yaw}")
@@ -102,7 +102,7 @@ async def test_unitree_go2_navigation_stack(self):
             navigator.goal_reached.transport = core.LCMTransport("/goal_reached", Bool)
             navigator.global_costmap.transport = core.LCMTransport("/global_costmap", OccupancyGrid)
             global_planner.path.transport = core.LCMTransport("/global_path", Path)
-            local_planner.cmd_vel.transport = core.LCMTransport("/cmd_vel", Vector3)
+            local_planner.cmd_vel.transport = core.LCMTransport("/cmd_vel", Twist)
 
             # Configure navigation connections
             global_planner.target.connect(navigator.goal)
@@ -118,7 +118,7 @@ async def test_unitree_go2_navigation_stack(self):
 
             # Deploy movement control module for testing
             movement = dimos.deploy(MovementControlModule)
-            movement.movecmd.transport = core.LCMTransport("/test_move", Vector3)
+            movement.movecmd.transport = core.LCMTransport("/test_move", Twist)
             connection.movecmd.connect(movement.movecmd)
 
             # Start all modules