GazeboBridge creates a camera_info topic for all bridged image topics

README.md

@@ -221,7 +221,7 @@ An instance is created with: `bridge = GazeboBridge(<gazebo_topic>, <ros_topic>,
 The Gazebo message type is deduced from the ros message type. Remapping will be set to the given `ros_topic`.
 
 The SimpleLauncher instance can then run all created bridges with: `sl.create_gz_bridge([bridges], <node_name>)`, as illustrated in the examples at this end of this document.
-If some bridges involve `sensor_msgs/Image` then a dedicated `ros_gz_image` bridge will be used.
+If some bridges involve `sensor_msgs/Image` then a dedicated `ros_gz_image` bridge will be used. The corresponding `camera_info` topic will be automatically bridged.
 
 ## Other shortcuts
 

simple_launch/__init__.py

@@ -72,7 +72,7 @@ def __init__(self, namespace = '', use_sim_time = None):
         self.ns_graph = {0: -1}
         self.composed = False
         self.sim_time = None
-        self.gz_axes = []
+        self.gz_axes = ('x','y','z','yaw','pitch','roll')
         self.__context = None
 
         if namespace:
@@ -442,7 +442,7 @@ def robot_description(self, package=None, description_file=None, description_dir
         cmd = SimpleSubstitution('xacro ', description_file)
         if xacro_args is not None:
             cmd += adapt_type(xacro_args, XACRO_ARGS)
-        return SimpleSubstitution("'", Command(cmd), "'")
+        return SimpleSubstitution("'", Command(cmd,on_stderr='warn'), "'")
 
     def robot_state_publisher(self, package=None, description_file=None, description_dir=None, xacro_args=None, prefix_gz_plugins=False,
                               namespaced_tf = False, **node_args):
@@ -508,7 +508,7 @@ def joint_state_publisher(self, use_gui = True, **node_args):
     def gz_prefix():
         return 'ign' if SimpleLauncher.ros_version() < 'humble' else 'gz'
 
-    def create_gz_bridge(self, bridges, name = 'gz_bridge'):
+    def create_gz_bridge(self, bridges: list[GazeboBridge], name = 'gz_bridge'):
         '''
         Create a ros_gz_bridge::parameter_bridge with the passed GazeboBridge instances
         The bridge has a default name if not specified
@@ -522,9 +522,25 @@ def create_gz_bridge(self, bridges, name = 'gz_bridge'):
         gz = self.gz_prefix()
         ros_gz = f'ros_{gz}'
 
-        std_config = sum([bridge.yaml(gz) for bridge in bridges if not bridge.is_image], [])
+        # add camera_info for image bridges
         im_bridges = [bridge for bridge in bridges if bridge.is_image]
 
+        for bridge in im_bridges:
+            gz_head, gz_tail = bridge.gz_topic.split_tail()
+            ros_head, ros_tail = bridge.ros_topic.split_tail()
+
+            if not all(isinstance(tail, Text) and 'image' in tail for tail in (ros_tail, gz_tail)):
+                continue
+
+            cam = []
+            for tail in (gz_tail, ros_tail):
+                idx = tail.rfind('/image')
+                cam.append(tail[:idx] + '/camera_info')
+
+            bridges.append(GazeboBridge(gz_head + [cam[0]], ros_head + [cam[1]], 'sensor_msgs/CameraInfo', GazeboBridge.gz2ros))
+
+        std_config = sum([bridge.yaml(gz) for bridge in bridges if not bridge.is_image], [])
+
         if std_config.has_elems():
             # use YAML-based configuration, handles Gazebo topics that are invalid to ROS
             from tempfile import NamedTemporaryFile

simple_launch/gazebo.py

@@ -73,7 +73,7 @@ def read_models():
             print('\033[93mThis launch file will request information on a running Gazebo instance at the time of the launch\033[0m')
             return
 
-        # TODO adapt to gz vs ig
+        # TODO adapt to gz vs ign
         models = silent_exec('ign model --list')
         for line in models:
             if line.startswith('Requesting'):
@@ -102,8 +102,8 @@ def __init__(self, gz_topic, ros_topic, msg, direction):
             print(f'Cannot build ros <-> gz bridge with direction "{direction}": should be in {{[,],@}}')
             return
 
-        self.gz_topic = gz_topic
-        self.ros_topic = ros_topic
+        self.gz_topic = SimpleSubstitution(gz_topic)
+        self.ros_topic = SimpleSubstitution(ros_topic)
 
         # Images with gz2ros use ros_ign_image bridge
         if msg == 'sensor_msgs/msg/Image':

simple_launch/simple_substitution.py

@@ -1,9 +1,9 @@
 from launch import Substitution
-from launch.substitutions import TextSubstitution
 from launch.launch_context import LaunchContext
 from typing import Text, List, Tuple
 from os.path import sep
 
+
 def flatten(nested):
     try:
         for i, elem in enumerate(nested):
@@ -13,11 +13,12 @@ def flatten(nested):
     except IndexError:
         pass
     return [elem for elem in nested if elem is not None]
-    return filter(lambda elem: elem is not None, self.__subs)
+
 
 def is_basic(elem):
     return isinstance(elem, (Text, bool, int, float))
 
+
 class SimpleSubstitution(Substitution):
     '''
     A container for other substitutions that provides concatenation (+) and path building (/) operators
@@ -30,6 +31,15 @@ def __init__(self, *elems):
     def has_elems(self) -> bool:
         return len(self.__subs) > 0
 
+    def split_tail(self) -> tuple:
+        if not self.has_elems():
+            return [],None
+        subs = self.substitutions()
+        if isinstance(subs[-1], SimpleSubstitution):
+            head, tail = subs[-1].split_tail()
+            return subs[:-1]+head, tail
+        return subs[:-1], subs[-1]
+
     @staticmethod
     def is_none(elem) -> bool:
         return elem is None or (isinstance(elem, SimpleSubstitution) and elem.__subs and all([sub is None for sub in elem.__subs]))