leg odometry publisher is added.

.devcontainer/scripts/cyclonedds_setup.sh

@@ -0,0 +1,8 @@
+#!/bin/bash
+echo "Setup unitree ros2 environment"
+source /opt/ros/humble/setup.bash
+source /unitree_ros2/cyclonedds_ws/install/setup.bash
+export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
+export CYCLONEDDS_URI='<CycloneDDS><Domain><General><Interfaces>
+                            <NetworkInterface name="eth0" priority="default" multicast="default" />
+                        </Interfaces></General></Domain></CycloneDDS>'

dds_bridge/dds_bridge.py

@@ -15,7 +15,8 @@ RECORD_DATASET = False
 DATASET_LENGTH = 120
 ros2_init()
 map_bridge = PoindCloud2Bridge('/Laser_map')
-fast_lio_odom = ROS2OdometryReader('/Odometry', 'odometry_subscriber')
+fast_lio_odom = ROS2OdometryReader('/Odometry', 'lidar_odometry_subscriber')
+go2_odom = ROS2OdometryReader('/go2/odom', 'leg_odometry_subscriber')
 rgb_camera_bridge = ROS2CameraReader('/camera/color/image_raw', 'rgb_reader')
 depth_camera_bridge = ROS2CameraReader('/camera/depth/image_rect_raw', 'depth_reader')
 
@@ -24,6 +25,7 @@ ros2_exec_manager.add_node(map_bridge)
 ros2_exec_manager.add_node(fast_lio_odom)
 ros2_exec_manager.add_node(rgb_camera_bridge)
 ros2_exec_manager.add_node(depth_camera_bridge)
+ros2_exec_manager.add_node(go2_odom)
 ros2_exec_manager.start()
 
 # The extrinsics of the MID360 LiDAR with respect to the robot body
@@ -45,6 +47,13 @@ def GetLidarPose():
     else:
         return None
     
+def GetLegOdom():
+    odom = go2_odom.get_pose()
+    if odom is not None:
+        return odom
+    else:
+        return None
+
 def GetLidarMap():
     pcd = map_bridge._points
     if pcd is not None:
@@ -61,6 +70,7 @@ map_update_counter = 0
 rgb_imgs = []
 depth_imgs = []
 lio_Ts_robot = []
+odom_Ts_robot = []
 pcd_maps = []
 
 
@@ -70,11 +80,15 @@ start_time = time.time()
 while True:
     tic = time.time()
     lio_T_robot = GetLidarPose()
+    odom_T_robot = GetLegOdom()
     if lio_T_robot is not None:
         dds_server.sendLidarOdom(lio_T_robot)
         if RECORD_DATASET:
             lio_Ts_robot.append(lio_T_robot)
-
+    if odom_T_robot is not None:
+        dds_server.sendLegOdom(odom_T_robot)
+        if RECORD_DATASET:
+            odom_Ts_robot.append(odom_T_robot)
 
     if map_update_counter%10==0:
         lio_pcd = GetLidarMap()

dds_bridge/dds_telemetry.py

@@ -41,16 +41,19 @@ class Go2DDSServer:
         self.depth_topic = Topic(self.domain, f'{self.robot_name}_depth', DepthImage)  
         self.map_topic = Topic(self.domain, f'{self.robot_name}_lio_map', PointCloud)
         self.lidar_odom_topic = Topic(self.domain, f'{self.robot_name}_lio_odom', Pose)
+        self.leg_odom_topic = Topic(self.domain, f'{self.robot_name}_leg_odom', Pose)
         self.high_cmd_topic = Topic(self.domain, f'{self.robot_name}_high_cmd', HighCommand)
         self.rgb_publisher = Publisher(self.domain)
         self.depth_publisher = Publisher(self.domain)
         self.map_publisher = Publisher(self.domain)
         self.lidar_odom_publisher = Publisher(self.domain)
+        self.leg_odom_publisher = Publisher(self.domain)
         self.high_cmd_reader = DataReader(self.domain, self.high_cmd_topic)
         self.rgb_writer = DataWriter(self.rgb_publisher, self.rgb_topic)
         self.depth_writer = DataWriter(self.depth_publisher, self.depth_topic)
         self.map_writer = DataWriter(self.map_publisher, self.map_topic)
         self.lidar_odom_writer = DataWriter(self.lidar_odom_publisher, self.lidar_odom_topic)
+        self.leg_odom_writer = DataWriter(self.leg_odom_publisher, self.leg_odom_topic)
         
         
         
@@ -75,6 +78,12 @@ class Go2DDSServer:
         pose_msg = Pose(quat=q.tolist(), trans=t.tolist(), timestamp='')
         self.lidar_odom_writer.write(pose_msg)
 
+    def sendLegOdom(self, odom_T_body):
+        q = R.from_matrix(odom_T_body[:3,:3]).as_quat()
+        t = odom_T_body[:3,3]
+        pose_msg = Pose(quat=q.tolist(), trans=t.tolist(), timestamp='')
+        self.lidar_odom_writer.write(pose_msg) 
+
     def getHighCmd(self):
         return get_last_msg(self.high_cmd_reader, HighCommand)    
     
@@ -87,13 +96,16 @@ class Go2DDSClient:
         self.depth_topic = Topic(self.domain, f'{self.robot_name}_depth', DepthImage)  
         self.map_topic = Topic(self.domain, f'{self.robot_name}_lio_map', PointCloud)
         self.lidar_odom_topic = Topic(self.domain, f'{self.robot_name}_lio_odom', Pose)
+        self.leg_odom_topic = Topic(self.domain, f'{self.robot_name}_leg_odom   ', Pose)
         self.high_cmd_topic = Topic(self.domain, f'{self.robot_name}_high_cmd', HighCommand)
         self.rgb_reader = DataReader(self.domain, self.rgb_topic)
         self.depth_reader = DataReader(self.domain, self.depth_topic)
         self.map_reader = DataReader(self.domain, self.map_topic)
         self.lidar_odom_reader = DataReader(self.domain, self.lidar_odom_topic)
+        self.leg_odom_reader = DataReader(self.domain, self.leg_odom_topic)
         self.high_cmd_writer = DataWriter(self.domain, self.high_cmd_topic)
 
+
     def getRGB(self):
         return get_last_msg(self.rgb_reader, RGBImage)
     
@@ -106,6 +118,9 @@ class Go2DDSClient:
     def getLidarOdom(self):
         return get_last_msg(self.lidar_odom_reader, Pose)
     
+    def getLegOdom(self):
+        return get_last_msg(self.leg_odom_reader, Pose)
+    
     def sendHighCmd(self, vx, vy, omega):
         self.high_cmd_writer.write(HighCommand(vx=vx, vy=vy, omega=omega, timestamp=''))