Fixes move stream issue, Odom receive issue

dimos/msgs/geometry_msgs/Pose.py

@@ -82,17 +82,11 @@ def __init__(
         self.orientation = Quaternion(qx, qy, qz, qw)
 
     @dispatch
-    def __init__(self, position: VectorConvertable) -> None:
-        self.position = Vector3(position)
-        self.orientation = Quaternion()
-
-    @dispatch
-    def __init__(self, orientation: QuaternionConvertable) -> None:
-        self.position = Vector3()
-        self.orientation = Quaternion(orientation)
-
-    @dispatch
-    def __init__(self, position: VectorConvertable, orientation: QuaternionConvertable) -> None:
+    def __init__(
+        self,
+        position: VectorConvertable | Vector3 = [0, 0, 0],
+        orientation: QuaternionConvertable | Quaternion = [0, 0, 0, 1],
+    ) -> None:
         """Initialize a pose with position and orientation."""
         self.position = Vector3(position)
         self.orientation = Quaternion(orientation)
@@ -110,7 +104,7 @@ def __init__(self, pose_dict: dict[str, VectorConvertable | QuaternionConvertabl
         self.orientation = Quaternion(pose_dict["orientation"])
 
     @dispatch
-    def __init__(self, pose: "Pose") -> None:
+    def __init__(self, pose: Pose) -> None:
         """Initialize from another Pose (copy constructor)."""
         self.position = Vector3(pose.position)
         self.orientation = Quaternion(pose.orientation)

dimos/msgs/geometry_msgs/PoseStamped.py

@@ -12,6 +12,8 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+from __future__ import annotations
+
 import struct
 import time
 from io import BytesIO
@@ -46,24 +48,29 @@ class PoseStamped(Pose, Timestamped):
     frame_id: str
 
     @dispatch
-    def __init__(self, *args, ts: float = 0, frame_id: str = "", **kwargs) -> None:
+    def __init__(self, ts: float = 0.0, frame_id: str = "", **kwargs) -> None:
         self.frame_id = frame_id
         self.ts = ts if ts != 0 else time.time()
-        super().__init__(*args, **kwargs)
+        super().__init__(**kwargs)
 
     def lcm_encode(self) -> bytes:
         lcm_mgs = LCMPoseStamped()
         lcm_mgs.pose = self
         [lcm_mgs.header.stamp.sec, lcm_mgs.header.stamp.sec] = sec_nsec(self.ts)
         lcm_mgs.header.frame_id = self.frame_id
-
         return lcm_mgs.encode()
 
     @classmethod
-    def lcm_decode(cls, data: bytes | BinaryIO):
+    def lcm_decode(cls, data: bytes | BinaryIO) -> PoseStamped:
         lcm_msg = LCMPoseStamped.decode(data)
         return cls(
-            pose=Pose(lcm_msg.pose),
             ts=lcm_msg.header.stamp.sec + (lcm_msg.header.stamp.nsec / 1_000_000_000),
             frame_id=lcm_msg.header.frame_id,
+            position=[lcm_msg.pose.position.x, lcm_msg.pose.position.y, lcm_msg.pose.position.z],
+            orientation=[
+                lcm_msg.pose.orientation.x,
+                lcm_msg.pose.orientation.y,
+                lcm_msg.pose.orientation.z,
+                lcm_msg.pose.orientation.w,
+            ],  # noqa: E501,
         )

dimos/msgs/sensor_msgs/PointCloud2.py

@@ -75,7 +75,7 @@ def lcm_encode(self, frame_id: Optional[str] = None) -> bytes:
             msg.is_bigendian = False
             msg.fields_length = 4  # x, y, z, intensity
             msg.fields = self._create_xyz_field()
-            return msg
+            return msg.encode()
 
         # Point cloud dimensions
         msg.height = 1  # Unorganized point cloud

dimos/robot/local_planner/simple.py

@@ -22,7 +22,8 @@
 from reactivex import operators as ops
 
 from dimos.core import In, Module, Out, rpc
-from dimos.msgs.geometry_msgs import Vector3
+from dimos.msgs.geometry_msgs import PoseStamped, Vector3
+from dimos.robot.unitree_webrtc.type.odometry import Odometry
 
 # from dimos.robot.local_planner.local_planner import LocalPlanner
 from dimos.types.costmap import Costmap
@@ -65,24 +66,22 @@ def transform_to_robot_frame(global_vector: Vector, robot_position: Pose) -> Vec
 
 class SimplePlanner(Module):
     path: In[Path] = None
+    odom: In[PoseStamped] = None
     movecmd: Out[Vector3] = None
 
     get_costmap: Callable[[], Costmap]
-    get_robot_pos: Callable[[], Pose]
-    set_move: Callable[[Vector3], Any]
+
+    latest_odom: PoseStamped = None
+
     goal: Optional[Vector] = None
     speed: float = 0.3
 
     def __init__(
         self,
         get_costmap: Callable[[], Costmap],
-        get_robot_pos: Callable[[], Vector],
-        set_move: Callable[[Vector3], Any],
     ):
         Module.__init__(self)
         self.get_costmap = get_costmap
-        self.get_robot_pos = get_robot_pos
-        self.set_move = set_move
 
     def get_move_stream(self, frequency: float = 40.0) -> rx.Observable:
         return rx.interval(1.0 / frequency, scheduler=get_scheduler()).pipe(
@@ -96,9 +95,12 @@ def get_move_stream(self, frequency: float = 40.0) -> rx.Observable:
     @rpc
     def start(self):
         self.path.subscribe(self.set_goal)
-        # weird bug with this
-        # self.get_move_stream(frequency=20.0).subscribe(self.movecmd.publish)
-        self.get_move_stream(frequency=20.0).subscribe(self.set_move)
+
+        def setodom(odom: Odometry):
+            self.latest_odom = odom
+
+        self.odom.subscribe(setodom)
+        self.get_move_stream(frequency=20.0).subscribe(self.movecmd.publish)
 
     @dispatch
     def set_goal(self, goal: Path, stop_event=None, goal_theta=None) -> bool:
@@ -209,7 +211,7 @@ def _calculate_rotation_to_target(self, direction_to_goal: Vector) -> Vector:
         desired_yaw = math.atan2(direction_to_goal.y, direction_to_goal.x)
 
         # Get current robot yaw
-        current_yaw = self.get_robot_pos().rot.z
+        current_yaw = self.latest_odom.orientation.z
 
         # Calculate the yaw error using a more robust method to avoid oscillation
         yaw_error = math.atan2(
@@ -249,9 +251,9 @@ def _calculate_rotation_to_target(self, direction_to_goal: Vector) -> Vector:
 
     def _debug_direction(self, name: str, direction: Vector) -> Vector:
         """Debug helper to log direction information"""
-        robot_pos = self.get_robot_pos()
+        robot_pos = self.latest_odom
         print(
-            f"DEBUG {name}: direction={direction}, robot_pos={robot_pos.pos.to_2d()}, robot_yaw={math.degrees(robot_pos.rot.z):.1f}°, goal={self.goal}"
+            f"DEBUG {name}: direction={direction}, robot_pos={robot_pos.position.to_2d()}, robot_yaw={math.degrees(robot_pos.rot.z):.1f}°, goal={self.goal}"
         )
         return direction
 

dimos/robot/unitree_webrtc/multiprocess/unitree_go2.py

@@ -27,7 +27,7 @@
 import dimos.core.colors as colors
 from dimos import core
 from dimos.core import In, Module, Out, rpc
-from dimos.msgs.geometry_msgs import Vector3
+from dimos.msgs.geometry_msgs import PoseStamped, Vector3
 from dimos.msgs.sensor_msgs import Image
 from dimos.protocol import pubsub
 from dimos.robot.foxglove_bridge import FoxgloveBridge
@@ -134,7 +134,7 @@ def start(self):
         def plancmd():
             time.sleep(4)
             print(colors.red("requesting global plan"))
-            self.plancmd.publish(Vector3([0.750893, -6.017522, 0.307474]))
+            self.plancmd.publish(Vector3(0, 0, 0))
 
         thread = threading.Thread(target=plancmd, daemon=True)
         thread.start()
@@ -150,9 +150,8 @@ async def run(ip):
     pubsub.lcm.autoconf()
 
     connection.lidar.transport = core.LCMTransport("/lidar", LidarMessage)
-    connection.odom.transport = core.LCMTransport("/odom", Odometry)
+    connection.odom.transport = core.LCMTransport("/odom", PoseStamped)
     connection.video.transport = core.LCMTransport("/video", Image)
-    connection.movecmd.transport = core.LCMTransport("/move", Vector3)
 
     mapper = dimos.deploy(Map, voxel_size=0.5, global_publish_interval=2.5)
 
@@ -161,8 +160,6 @@ async def run(ip):
     local_planner = dimos.deploy(
         SimplePlanner,
         get_costmap=connection.get_local_costmap,
-        get_robot_pos=connection.get_pos,
-        set_move=connection.move,
     )
 
     global_planner = dimos.deploy(
@@ -174,13 +171,18 @@ async def run(ip):
     global_planner.path.transport = core.pLCMTransport("/global_path")
 
     local_planner.path.connect(global_planner.path)
+
+    local_planner.odom.connect(connection.odom)
+
+    local_planner.movecmd.transport = core.LCMTransport("/move", Vector3)
     local_planner.movecmd.connect(connection.movecmd)
 
     ctrl = dimos.deploy(ControlModule)
 
     mapper.lidar.connect(connection.lidar)
 
     ctrl.plancmd.transport = core.LCMTransport("/global_target", Vector3)
+
     global_planner.target.connect(ctrl.plancmd)
 
     foxglove_bridge = FoxgloveBridge()
@@ -206,8 +208,8 @@ async def run(ip):
     foxglove_bridge.start()
 
     # uncomment to move the bot
-    # print(colors.green("starting ctrl"))
-    # ctrl.start()
+    print(colors.green("starting ctrl"))
+    ctrl.start()
 
     print(colors.red("READY"))
 

dimos/robot/unitree_webrtc/type/odometry.py

@@ -18,8 +18,7 @@
 
 from scipy.spatial.transform import Rotation as R
 
-from dimos.msgs.geometry_msgs import PoseStamped as LCMPoseStamped
-from dimos.msgs.geometry_msgs import Quaternion, Vector3
+from dimos.msgs.geometry_msgs import PoseStamped, Quaternion, Vector3
 from dimos.robot.unitree_webrtc.type.timeseries import (
     EpochLike,
     Timestamped,
@@ -81,27 +80,29 @@ class RawOdometryMessage(TypedDict):
     data: OdometryData
 
 
-class Odometry(LCMPoseStamped):
+class Odometry(PoseStamped, Timestamped):
     name = "geometry_msgs.PoseStamped"
 
     @classmethod
     def from_msg(cls, msg: RawOdometryMessage) -> "Odometry":
         pose = msg["data"]["pose"]
-        orientation = pose["orientation"]
-        position = pose["position"]
 
         # Extract position
-        pos = Vector3(position.get("x"), position.get("y"), position.get("z"))
+        pos = Vector3(
+            pose["position"].get("x"),
+            pose["position"].get("y"),
+            pose["position"].get("z"),
+        )
 
         rot = Quaternion(
-            orientation.get("x"),
-            orientation.get("y"),
-            orientation.get("z"),
-            orientation.get("w"),
+            pose["orientation"].get("x"),
+            pose["orientation"].get("y"),
+            pose["orientation"].get("z"),
+            pose["orientation"].get("w"),
         )
 
         ts = to_timestamp(msg["data"]["header"]["stamp"])
-        return Odometry(pos, rot, ts=ts, frame_id="lidar")
+        return Odometry(position=pos, orientation=rot, ts=ts, frame_id="lidar")
 
     def __repr__(self) -> str:
         return f"Odom pos({self.position}), rot({self.orientation})"