From 0707d635cfd42f000761cfcd2a5f5e5f4d6d4fc2 Mon Sep 17 00:00:00 2001 From: Rooholla-KhorramBakht Date: Sat, 25 Jan 2025 15:20:39 +0000 Subject: [PATCH] leg odometry publisher is added. --- .devcontainer/scripts/cyclonedds_setup.sh | 8 ++++++++ dds_bridge/dds_bridge.py | 18 ++++++++++++++++-- dds_bridge/dds_telemetry.py | 15 +++++++++++++++ 3 files changed, 39 insertions(+), 2 deletions(-) create mode 100644 .devcontainer/scripts/cyclonedds_setup.sh diff --git a/.devcontainer/scripts/cyclonedds_setup.sh b/.devcontainer/scripts/cyclonedds_setup.sh new file mode 100644 index 0000000..2e6bfa7 --- /dev/null +++ b/.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=' + + ' \ No newline at end of file diff --git a/dds_bridge/dds_bridge.py b/dds_bridge/dds_bridge.py index 9e77b73..10a8f77 100644 --- a/dds_bridge/dds_bridge.py +++ b/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 @@ -44,6 +46,13 @@ def GetLidarPose(): return lio_T_body 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 @@ -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() diff --git a/dds_bridge/dds_telemetry.py b/dds_bridge/dds_telemetry.py index d31959a..5f91665 100644 --- a/dds_bridge/dds_telemetry.py +++ b/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='')) \ No newline at end of file