From 6c7f3f7f11ee629b292cc64fef61a44f5de9dd92 Mon Sep 17 00:00:00 2001 From: Tamas Foldi Date: Sat, 21 Sep 2024 20:22:25 +0200 Subject: [PATCH] feat: add camerainfo to published images --- .../go2_robot_sdk/go2_driver_node.py | 25 ++++++++- go2_robot_sdk/package.xml | 3 +- go2_robot_sdk/scripts/go2_camerainfo.py | 53 +++++++++++++++++++ go2_robot_sdk/setup.py | 1 + 4 files changed, 79 insertions(+), 3 deletions(-) create mode 100644 go2_robot_sdk/scripts/go2_camerainfo.py diff --git a/go2_robot_sdk/go2_robot_sdk/go2_driver_node.py b/go2_robot_sdk/go2_robot_sdk/go2_driver_node.py index 3dbe87a..9c77264 100644 --- a/go2_robot_sdk/go2_robot_sdk/go2_driver_node.py +++ b/go2_robot_sdk/go2_robot_sdk/go2_driver_node.py @@ -36,6 +36,7 @@ from scripts.go2_func import gen_command, gen_mov_command from scripts.go2_lidar_decoder import update_meshes_for_cloud2 from scripts.go2_math import get_robot_joints +from scripts.go2_camerainfo import load_camera_info from scripts.webrtc_driver import Go2Connection import rclpy @@ -50,7 +51,7 @@ from sensor_msgs_py import point_cloud2 from std_msgs.msg import Header from nav_msgs.msg import Odometry -from sensor_msgs.msg import Image +from sensor_msgs.msg import Image, CameraInfo logging.basicConfig(level=logging.WARN) @@ -94,6 +95,7 @@ def __init__(self): self.go2_odometry_pub = [] self.imu_pub = [] self.img_pub = [] + self.camera_info_pub = [] if self.conn_mode == 'single': self.joint_pub.append(self.create_publisher( @@ -106,6 +108,7 @@ def __init__(self): self.create_publisher(Odometry, 'odom', qos_profile)) self.imu_pub.append(self.create_publisher(IMU, 'imu', qos_profile)) self.img_pub.append(self.create_publisher(Image, 'camera/image_raw', qos_profile)) + self.camera_info_pub.append(self.create_publisher(CameraInfo, 'camera/camera_info', qos_profile)) else: for i in range(len(self.robot_ip_lst)): @@ -121,10 +124,13 @@ def __init__(self): IMU, f'robot{i}/imu', qos_profile)) self.img_pub.append(self.create_publisher( Image, f'robot{i}/camera/image_raw', qos_profile)) + self.camera_info_pub.append(self.create_publisher( + CameraInfo, f'robot{i}/camera/camera_info', qos_profile)) self.broadcaster = TransformBroadcaster(self, qos=qos_profile) self.bridge = CvBridge() + self.camera_info = load_camera_info() self.robot_cmd_vel = {} self.robot_odom = {} @@ -276,9 +282,24 @@ async def on_video_frame(self, track: MediaStreamTrack, robot_num): # Convert the OpenCV image to ROS Image message ros_image = self.bridge.cv2_to_imgmsg(img, encoding="bgr8") + ros_image.header.stamp = self.get_clock().now().to_msg() - # Publish the image + + # Set the timestamp for both image and camera info + camera_info = self.camera_info + camera_info.header.stamp = ros_image.header.stamp + + if self.conn_mode == 'single': + camera_info.header.frame_id = 'front_camera' + ros_image.header.frame_id = 'front_camera' + else: + camera_info.header.frame_id = f'robot{str(robot_num)}/front_camera' + ros_image.header.frame_id = f'robot{str(robot_num)}/front_camera' + + # Publish image and camera info self.img_pub[robot_num].publish(ros_image) + self.camera_info_pub[robot_num].publish(camera_info) + asyncio.sleep(0) def on_data_channel_message(self, _, msg, robot_num): diff --git a/go2_robot_sdk/package.xml b/go2_robot_sdk/package.xml index 22357a8..ebbea1e 100644 --- a/go2_robot_sdk/package.xml +++ b/go2_robot_sdk/package.xml @@ -3,13 +3,14 @@ go2_robot_sdk 0.0.0 - TODO: Package description + ROS2 unofficial sdk for Unitree Go2 brimo Apache-2.0 rclpy go2_interfaces unitree_go + sensor_msgs twist_mux joy diff --git a/go2_robot_sdk/scripts/go2_camerainfo.py b/go2_robot_sdk/scripts/go2_camerainfo.py new file mode 100644 index 0000000..cbfecb5 --- /dev/null +++ b/go2_robot_sdk/scripts/go2_camerainfo.py @@ -0,0 +1,53 @@ +# Copyright (c) 2024, RoboVerse community +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# 2. Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +# FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +# DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +# SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +# OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +# OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + +import yaml +import logging +from sensor_msgs.msg import CameraInfo +from ament_index_python.packages import get_package_share_directory + +logger = logging.getLogger(__name__) +logging.basicConfig(level=logging.INFO) + +def load_camera_info(): + yaml_file = get_package_share_directory('go2_robot_sdk') + "/calibration/front_camera.yaml" + + logger.info("Loading camera info from file: {}".format(yaml_file)) + + # Load the camera info from the YAML file + with open(yaml_file, "r") as file_handle: + camera_info_data = yaml.safe_load(file_handle) + + # Create a CameraInfo message + camera_info_msg = CameraInfo() + + # Fill in the CameraInfo fields from the YAML data + camera_info_msg.width = camera_info_data["image_width"] + camera_info_msg.height = camera_info_data["image_height"] + camera_info_msg.k = camera_info_data["camera_matrix"]["data"] + camera_info_msg.d = camera_info_data["distortion_coefficients"]["data"] + camera_info_msg.r = camera_info_data["rectification_matrix"]["data"] + camera_info_msg.p = camera_info_data["projection_matrix"]["data"] + camera_info_msg.distortion_model = camera_info_data["distortion_model"] + + return camera_info_msg diff --git a/go2_robot_sdk/setup.py b/go2_robot_sdk/setup.py index 980dddd..edef61f 100644 --- a/go2_robot_sdk/setup.py +++ b/go2_robot_sdk/setup.py @@ -42,6 +42,7 @@ (os.path.join('share', package_name, 'dae'), glob(os.path.join('dae', '*'))), (os.path.join('share', package_name, 'meshes'), glob(os.path.join('meshes', '*'))), (os.path.join('share', package_name, 'config'), glob(os.path.join('config', '*'))), + (os.path.join('share', package_name, 'calibration'), glob(os.path.join('calibration', '*'))), (os.path.join('share', package_name, 'external_lib'), ['external_lib/libvoxel.wasm']), (os.path.join('share', package_name, 'external_lib/aioice'), glob(os.path.join('external_lib/aioice/src/aioice', '*'))),