ROS2: general macro for RGB-D cameras #25

TomasMerva · 2025-11-03T15:30:43Z

Addressing #23

…igned

spurnvoj · 2025-11-03T16:24:38Z

@TomasMerva not sure if you know about this discussion regarding RGBD sensor frames differences gazebosim/gz-sensors#488 and gazebosim/ros_gz#703

…o change the frame name

Copilot

Pull Request Overview

This PR refactors the UAV sensor framework to improve sensor support and unify naming conventions. The changes focus on better handling of different sensor types (RGB cameras, RGBD cameras, depth cameras, and LiDARs) and establishing consistent frame naming patterns.

Key changes include:

Refactored SDF to TF publisher to support optical frames for camera sensors
Added support for RGBD and depth camera sensor types
Unified sensor data structures using enums for better type safety
Renamed variables from "link" to "frame" for consistency with TF conventions

Reviewed Changes

Copilot reviewed 17 out of 17 changed files in this pull request and generated 6 comments.

Show a summary per file

File	Description
spawner_enums.py	Defines enums for sensor types and topic categorization
sdf_tf_enums.py	Defines enums for sensor link data structure keys
sdf_to_tf_publisher.py	Refactored to handle optical frames and multiple sensor types
mrs_drone_spawner.py	Updated to use enums and handle RGBD/depth cameras
generic_components.sdf.jinja	Added macros for RGBD and depth cameras, split 2D/3D LiDAR macros
camera templates	Updated to use new camera macros and add optical frames
lidar templates	Renamed links to frames and updated to use split macros
uav_ros_gz_bridge_config.yaml.jinja	Simplified topic handling using consolidated lists

💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.

Copilot · 2025-11-05T14:22:29Z

mrs_uav_gazebo_simulator/utils/sdf_to_tf_publisher.py

+
+    def _get_transform_from_string_pose(self, pose_rpy_str: str) -> np.ndarray:
+        T_frame = np.eye(4)
+        if pose_rpy_str is not None and(pose_rpy_str != ""):


Missing space after 'and' keyword before the opening parenthesis. Should be and (pose_rpy_str != \"\") for consistency with Python style conventions.

Suggested change

if pose_rpy_str is not None and(pose_rpy_str != ""):

if pose_rpy_str is not None and (pose_rpy_str != ""):

Copilot · 2025-11-05T14:22:29Z

mrs_uav_gazebo_simulator/utils/sdf_to_tf_publisher.py

+        if link_name is None:
+            return ""
+        for link in model_xml.findall('.//link'):
+            if link.attrib["name"] == link_name:


Inconsistent attribute access method. Line 143 uses .get(\"name\") while this line uses .attrib[\"name\"]. The .get() method is safer as it returns None if the attribute doesn't exist, while .attrib[] will raise a KeyError. Use link.get(\"name\") for consistency and error handling.

Suggested change

if link.attrib["name"] == link_name:

if link.get("name") == link_name:

Copilot · 2025-11-05T14:22:30Z

mrs_uav_gazebo_simulator/mrs_drone_spawner.py

-            attached_sensors['cameras'].append(camera)
+            basic_topic_name = '/' + topic[0].firstChild.data
+            camera[SensorTopics.IMAGE] = basic_topic_name
+            camera[SensorTopics.CAMERA_INFO] = basic_topic_name.replace('image', 'camera_info')


The .replace('image', 'camera_info') call may produce unexpected results if 'image' appears multiple times in the topic name. Consider using a more specific replacement or reconstructing the topic path to ensure only the intended part is replaced.

Suggested change

camera[SensorTopics.CAMERA_INFO] = basic_topic_name.replace('image', 'camera_info')

camera[SensorTopics.CAMERA_INFO] = basic_topic_name.rsplit('image', 1)[0] + 'camera_info' if 'image' in basic_topic_name else basic_topic_name + '/camera_info'

Copilot · 2025-11-05T14:22:30Z

mrs_uav_gazebo_simulator/mrs_drone_spawner.py

+            basic_topic_name = '/' + topic[0].firstChild.data 
+            camera[SensorTopics.DEPTH_IMAGE] = basic_topic_name
+            camera[SensorTopics.POINTS] = camera[SensorTopics.DEPTH_IMAGE] + '/points'
+            camera[SensorTopics.CAMERA_INFO] = basic_topic_name.replace('depth_image', 'camera_info')


The .replace('depth_image', 'camera_info') call may produce unexpected results if 'depth_image' appears multiple times in the topic name. Consider using a more specific replacement or reconstructing the topic path to ensure only the intended part is replaced.

Suggested change

camera[SensorTopics.CAMERA_INFO] = basic_topic_name.replace('depth_image', 'camera_info')

# Replace only the last segment if it is 'depth_image'

path_parts = basic_topic_name.rstrip('/').split('/')

if path_parts[-1] == 'depth_image':

path_parts[-1] = 'camera_info'

camera_info_topic = '/'.join(path_parts)

if not camera_info_topic.startswith('/'):

camera_info_topic = '/' + camera_info_topic

camera[SensorTopics.CAMERA_INFO] = camera_info_topic

else:

camera[SensorTopics.CAMERA_INFO] = basic_topic_name + '/camera_info'

Copilot · 2025-11-05T14:22:30Z

models/mrs_robots_description/sdf/components/camera/bluefox.sdf.jinja

        roll = math.radians(90),
        pitch = 0,
        yaw = 0)
        }}


[nitpick] There's an inconsistent blank line pattern. Line 45 has the closing braces followed immediately by the comment on line 46, while in other locations there's typically a blank line. Consider adding a blank line after line 45 for consistency.

Suggested change

}}

}}

Copilot · 2025-11-05T14:22:31Z

config/uav_ros_gz_bridge_config.yaml.jinja

  gz_type_name: "gz.msgs.CameraInfo"
  direction: GZ_TO_ROS
-{%- endfor -%}
+{% endfor %}


[nitpick] Inconsistent Jinja2 template whitespace control. Lines 7 and 15 use {% endfor %} without the - for whitespace control, while line 23 uses {% endfor %}. However, all opening tags use {%-. For consistency, consider using {%- endfor -%} on all three lines.

Do not remove -, because doing so will break the generation of the ros_gz_bridge config file.

TomasMerva · 2025-11-05T14:57:00Z

This update includes:

Updated RGB and depth camera templates with optical frame support to align CameraInfo with sensor data
Refactored sdf_to_tf_publisher.py to support optical frames for cameras

RGB-D camera template (pending bug fix):

The issue is that even when specifying the additional optical frame, the point cloud and CameraInfo are still misaligned due to differences in frame conventions, as discussed in more detail Provide a way to set the optical frame convention used for rendering sensors gazebosim/gz-sensors#488.

The template is prepared and we are successfully publishing point cloud data to ROS. However, users should be aware that the point cloud data must be manually transformed, which is not included in this PR.

TomasMerva · 2025-11-06T13:57:20Z

Update:

modified TF-tree
refactored sdf_to_tf_publisher so that there is only one static TF publisher regardless of number of drones
unified naming for links, sensor plugins, optical frames
added README.md for naming conventions and notes regarding adding more sensors in the future

spurnvoj · 2025-11-06T14:00:52Z

@TomasMerva, please resolve the conflicts, and I will then perform the final check with Copilot.

spurnvoj · 2025-11-06T14:07:01Z

models/mrs_robots_description/sdf/README.md

+{%- set camera_gz_frame_id = spawner_args['name'] + '/' + camera_name + '/color_optical' -%}
+```
+
+The resulting camera-related topics will look like as follows:


This is relevant to the gazebo topic; perhaps we could also discuss how it is implemented on the ROS side.

I meant write it in the README :)

…o that there is only one broadcaster, even if new spawn requests come later

TomasMerva added 4 commits November 3, 2025 14:24

ft: ros_gz_bridge and spawner for RGBD cameras

3c8775d

ft: removing teraranger

dd2cfe0

ft: realsense example but CameraInfo and Pointcloud frames are not al…

e4f7aa7

…igned

formatting changes

c1e32d5

TomasMerva marked this pull request as draft November 3, 2025 15:31

TomasMerva added 7 commits November 4, 2025 11:34

ft: adding optical frame to bluefox

7fde93a

ft: refactored sdf2tf publisher, working optical frame, dont forget t…

1470546

…o change the frame name

ft: renaming, reformating RGB cameras

d20aa17

ft: fixing names, reformating topics

074f739

fix: final check of sdf components

70f5dc4

fix: renaming links to frames

4d5f227

ft: publishing sensor_frame tfs

e77c251

spurnvoj requested a review from Copilot November 5, 2025 14:20

Copilot AI reviewed Nov 5, 2025

View reviewed changes

TomasMerva added 2 commits November 5, 2025 15:34

fix: republishing same frames

6305250

minor formatting changes

307b876

TomasMerva marked this pull request as ready for review November 5, 2025 14:57

TomasMerva marked this pull request as draft November 6, 2025 10:00

TomasMerva added 3 commits November 6, 2025 11:32

ft: modified tf tree, waiting for the naming convention

45f6317

ft: renaming frames, one publisher for all drones

c4a9a78

refactoring sdf_to_tf_publisher

772a35e

TomasMerva marked this pull request as ready for review November 6, 2025 13:57

update readme

b71aae3

typo readme

b2bc258

spurnvoj reviewed Nov 6, 2025

View reviewed changes

Merge branch 'ros2' into ft_ros2_rgbdcam

977b656

TomasMerva added 2 commits November 6, 2025 15:28

fix: typos regarding resolving conflicts, updating readme

d8e68a3

ft: adding the TF broadcaster as a member variable of the singleton s…

d48f9b3

…o that there is only one broadcaster, even if new spawn requests come later

TomasMerva marked this pull request as draft November 6, 2025 16:01

	if pose_rpy_str is not None and(pose_rpy_str != ""):
	if pose_rpy_str is not None and (pose_rpy_str != ""):

	if link.attrib["name"] == link_name:
	if link.get("name") == link_name:

	camera[SensorTopics.CAMERA_INFO] = basic_topic_name.replace('image', 'camera_info')
	camera[SensorTopics.CAMERA_INFO] = basic_topic_name.rsplit('image', 1)[0] + 'camera_info' if 'image' in basic_topic_name else basic_topic_name + '/camera_info'

-            camera[SensorTopics.CAMERA_INFO] = basic_topic_name.replace('depth_image', 'camera_info')
+            # Replace only the last segment if it is 'depth_image'
+            path_parts = basic_topic_name.rstrip('/').split('/')
+            if path_parts[-1] == 'depth_image':
+                path_parts[-1] = 'camera_info'
+                camera_info_topic = '/'.join(path_parts)
+                if not camera_info_topic.startswith('/'):
+                    camera_info_topic = '/' + camera_info_topic
+                camera[SensorTopics.CAMERA_INFO] = camera_info_topic
+            else:
+                camera[SensorTopics.CAMERA_INFO] = basic_topic_name + '/camera_info'

ROS2: general macro for RGB-D cameras #25

Are you sure you want to change the base?

ROS2: general macro for RGB-D cameras #25

Uh oh!

Conversation

TomasMerva commented Nov 3, 2025 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

spurnvoj commented Nov 3, 2025

Uh oh!

Copilot AI left a comment

Choose a reason for hiding this comment

Pull Request Overview

Reviewed Changes

Uh oh!

Copilot AI Nov 5, 2025

Choose a reason for hiding this comment

Uh oh!

TomasMerva Nov 5, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Nov 5, 2025

Choose a reason for hiding this comment

Uh oh!

TomasMerva Nov 5, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Nov 5, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Nov 5, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Nov 5, 2025

Choose a reason for hiding this comment

Uh oh!

Copilot AI Nov 5, 2025

Choose a reason for hiding this comment

Uh oh!

TomasMerva Nov 5, 2025

Choose a reason for hiding this comment

Uh oh!

TomasMerva commented Nov 5, 2025

Uh oh!

TomasMerva commented Nov 6, 2025

Uh oh!

spurnvoj commented Nov 6, 2025

Uh oh!

spurnvoj Nov 6, 2025

Choose a reason for hiding this comment

Uh oh!

spurnvoj Nov 6, 2025

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

3 participants

TomasMerva commented Nov 3, 2025 •

edited

Loading