feat: add camerainfo to published images

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):
 

go2_robot_sdk/package.xml

@@ -3,13 +3,14 @@
 <package format="3">
   <name>go2_robot_sdk</name>
   <version>0.0.0</version>
-  <description>TODO: Package description</description>
+  <description>ROS2 unofficial sdk for Unitree Go2</description>
   <maintainer email="[email protected]">brimo</maintainer>
   <license>Apache-2.0</license>
 
   <depend>rclpy</depend>
   <depend>go2_interfaces</depend>
   <depend>unitree_go</depend>
+  <depend>sensor_msgs</depend>
 
   <exec_depend>twist_mux</exec_depend>
   <exec_depend>joy</exec_depend>

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

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', '*'))),