Add a way to pass extra parameters to ros_gz_bridge (#628)

* Add bridge_params argument to ros_gz_bridge Signed-off-by: Aarav Gupta <[email protected]> Signed-off-by: Alejandro Hernández Cordero <[email protected]> Signed-off-by: Wiktor Bajor <[email protected]> Co-authored-by: Alejandro Hernández Cordero <[email protected]> Co-authored-by: Wiktor Bajor <[email protected]> (cherry picked from commit 558a1cf)
gazebosim · Nov 21, 2024 · 6a92088 · 6a92088
1 parent c2c015e
commit 6a92088
Show file tree

Hide file tree

Showing 7 changed files with 169 additions and 121 deletions.
diff --git a/ros_gz_bridge/launch/ros_gz_bridge.launch b/ros_gz_bridge/launch/ros_gz_bridge.launch
@@ -7,6 +7,7 @@
   <arg name="use_composition" default="False" />
   <arg name="use_respawn" default="False" />
   <arg name="log_level" default="info" />
+  <arg name="bridge_params" default="" />
   <ros_gz_bridge
     bridge_name="$(var bridge_name)"
     config_file="$(var config_file)"
@@ -15,6 +16,7 @@
     namespace="$(var namespace)"
     use_composition="$(var use_composition)"
     use_respawn="$(var use_respawn)"
-    log_level="$(var log_level)">
+    log_level="$(var log_level)"
+    bridge_params="$(var bridge_params)">
   </ros_gz_bridge>
 </launch>
diff --git a/ros_gz_bridge/launch/ros_gz_bridge.launch.py b/ros_gz_bridge/launch/ros_gz_bridge.launch.py
@@ -15,24 +15,13 @@
 """Launch ros_gz bridge in a component container."""
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, GroupAction
-from launch.conditions import IfCondition
-from launch.substitutions import LaunchConfiguration, PythonExpression
-from launch_ros.actions import ComposableNodeContainer, LoadComposableNodes, Node
-from launch_ros.descriptions import ComposableNode
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from ros_gz_bridge.actions import RosGzBridge
 
 
 def generate_launch_description():
 
-    bridge_name = LaunchConfiguration('bridge_name')
-    config_file = LaunchConfiguration('config_file')
-    container_name = LaunchConfiguration('container_name')
-    create_own_container = LaunchConfiguration('create_own_container')
-    namespace = LaunchConfiguration('namespace')
-    use_composition = LaunchConfiguration('use_composition')
-    use_respawn = LaunchConfiguration('use_respawn')
-    log_level = LaunchConfiguration('log_level')
-
     declare_bridge_name_cmd = DeclareLaunchArgument(
         'bridge_name', description='Name of ros_gz_bridge node'
     )
@@ -74,57 +63,20 @@ def generate_launch_description():
         'log_level', default_value='info', description='log level'
     )
 
-    load_nodes = GroupAction(
-        condition=IfCondition(PythonExpression(['not ', use_composition])),
-        actions=[
-            Node(
-                package='ros_gz_bridge',
-                executable='bridge_node',
-                name=bridge_name,
-                namespace=namespace,
-                output='screen',
-                respawn=use_respawn,
-                respawn_delay=2.0,
-                parameters=[{'config_file': config_file}],
-                arguments=['--ros-args', '--log-level', log_level],
-            ),
-        ],
-    )
-
-    load_composable_nodes_with_container = ComposableNodeContainer(
-        condition=IfCondition(
-            PythonExpression([use_composition, ' and ', create_own_container])),
-        name=LaunchConfiguration('container_name'),
-        namespace='',
-        package='rclcpp_components',
-        executable='component_container',
-        composable_node_descriptions=[
-            ComposableNode(
-                package='ros_gz_bridge',
-                plugin='ros_gz_bridge::RosGzBridge',
-                name=bridge_name,
-                namespace=namespace,
-                parameters=[{'config_file': config_file}],
-                extra_arguments=[{'use_intra_process_comms': True}],
-            ),
-        ],
-        output='screen',
+    declare_bridge_params_cmd = DeclareLaunchArgument(
+        'bridge_params', default_value='', description='Extra parameters to pass to the bridge.'
     )
 
-    load_composable_nodes_without_container = LoadComposableNodes(
-        condition=IfCondition(
-            PythonExpression([use_composition, ' and not ', create_own_container])),
-        target_container=container_name,
-        composable_node_descriptions=[
-            ComposableNode(
-                package='ros_gz_bridge',
-                plugin='ros_gz_bridge::RosGzBridge',
-                name=bridge_name,
-                namespace=namespace,
-                parameters=[{'config_file': config_file}],
-                extra_arguments=[{'use_intra_process_comms': True}],
-            ),
-        ],
+    ros_gz_bridge_action = RosGzBridge(
+        bridge_name=LaunchConfiguration('bridge_name'),
+        config_file=LaunchConfiguration('config_file'),
+        container_name=LaunchConfiguration('container_name'),
+        create_own_container=LaunchConfiguration('create_own_container'),
+        namespace=LaunchConfiguration('namespace'),
+        use_composition=LaunchConfiguration('use_composition'),
+        use_respawn=LaunchConfiguration('use_respawn'),
+        log_level=LaunchConfiguration('log_level'),
+        bridge_params=LaunchConfiguration('bridge_params')
     )
 
     # Create the launch description and populate
@@ -139,9 +91,7 @@ def generate_launch_description():
     ld.add_action(declare_use_composition_cmd)
     ld.add_action(declare_use_respawn_cmd)
     ld.add_action(declare_log_level_cmd)
-    # Add the actions to launch all of the bridge nodes
-    ld.add_action(load_nodes)
-    ld.add_action(load_composable_nodes_with_container)
-    ld.add_action(load_composable_nodes_without_container)
-
+    ld.add_action(declare_bridge_params_cmd)
+    # Add the ros_gz_bridge action
+    ld.add_action(ros_gz_bridge_action)
     return ld
diff --git a/ros_gz_bridge/ros_gz_bridge/actions/ros_gz_bridge.py b/ros_gz_bridge/ros_gz_bridge/actions/ros_gz_bridge.py
@@ -18,13 +18,14 @@
 from typing import Optional
 
 from launch.action import Action
-from launch.actions import IncludeLaunchDescription
+from launch.actions import GroupAction
+from launch.conditions import IfCondition
 from launch.frontend import Entity, expose_action, Parser
 from launch.launch_context import LaunchContext
-from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.some_substitutions_type import SomeSubstitutionsType
-from launch.substitutions import PathJoinSubstitution
-from launch_ros.substitutions import FindPackageShare
+from launch.substitutions import PythonExpression
+from launch_ros.actions import ComposableNodeContainer, LoadComposableNodes, Node
+from launch_ros.descriptions import ComposableNode
 
 
 @expose_action('ros_gz_bridge')
@@ -42,14 +43,12 @@ def __init__(
         use_composition: Optional[SomeSubstitutionsType] = 'False',
         use_respawn: Optional[SomeSubstitutionsType] = 'False',
         log_level: Optional[SomeSubstitutionsType] = 'info',
+        bridge_params: Optional[SomeSubstitutionsType] = '',
         **kwargs
     ) -> None:
         """
         Construct a ros_gz bridge action.
 
-        All arguments are forwarded to `ros_gz_bridge.launch.ros_gz_bridge.launch.py`,
-        so see the documentation of that class for further details.
-
         :param: bridge_name Name of ros_gz_bridge  node
         :param: config_file YAML config file.
         :param: container_name Name of container that nodes will load in if use composition.
@@ -58,6 +57,7 @@ def __init__(
         :param: use_composition Use composed bringup if True.
         :param: use_respawn Whether to respawn if a node crashes (when composition is disabled).
         :param: log_level Log level.
+        :param: bridge_params Extra parameters to pass to the bridge.
         """
         super().__init__(**kwargs)
         self.__bridge_name = bridge_name
@@ -68,6 +68,7 @@ def __init__(
         self.__use_composition = use_composition
         self.__use_respawn = use_respawn
         self.__log_level = log_level
+        self.__bridge_params = bridge_params
 
     @classmethod
     def parse(cls, entity: Entity, parser: Parser):
@@ -106,6 +107,10 @@ def parse(cls, entity: Entity, parser: Parser):
             'log_level', data_type=str,
             optional=True)
 
+        bridge_params = entity.get_attr(
+            'bridge_params', data_type=str,
+            optional=True)
+
         if isinstance(bridge_name, str):
             bridge_name = parser.parse_substitution(bridge_name)
             kwargs['bridge_name'] = bridge_name
@@ -139,22 +144,99 @@ def parse(cls, entity: Entity, parser: Parser):
             log_level = parser.parse_substitution(log_level)
             kwargs['log_level'] = log_level
 
+        if isinstance(bridge_params, str):
+            bridge_params = parser.parse_substitution(bridge_params)
+            kwargs['bridge_params'] = bridge_params
+
         return cls, kwargs
 
     def execute(self, context: LaunchContext) -> Optional[List[Action]]:
         """Execute the action."""
-        ros_gz_bridge_description = IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(
-                [PathJoinSubstitution([FindPackageShare('ros_gz_bridge'),
-                                       'launch',
-                                       'ros_gz_bridge.launch.py'])]),
-            launch_arguments=[('bridge_name', self.__bridge_name),
-                              ('config_file', self.__config_file),
-                              ('container_name', self.__container_name),
-                              ('create_own_container', self.__create_own_container),
-                              ('namespace',   self.__namespace),
-                              ('use_composition',  self.__use_composition),
-                              ('use_respawn',  self.__use_respawn),
-                              ('log_level',   self.__log_level), ])
-
-        return [ros_gz_bridge_description]
+        if hasattr(self.__bridge_params, 'perform'):
+            string_bridge_params = self.__bridge_params.perform(context)
+        elif isinstance(self.__bridge_params, list):
+            if hasattr(self.__bridge_params[0], 'perform'):
+                string_bridge_params = self.__bridge_params[0].perform(context)
+        else:
+            string_bridge_params = str(self.__bridge_params)
+        # Remove unnecessary symbols
+        simplified_bridge_params = string_bridge_params.translate(
+            {ord(i): None for i in '{} "\''}
+        )
+        # Parse to dictionary
+        parsed_bridge_params = {}
+        if simplified_bridge_params:
+            bridge_params_pairs = simplified_bridge_params.split(',')
+            parsed_bridge_params = dict(pair.split(':') for pair in bridge_params_pairs)
+
+        if isinstance(self.__use_composition, list):
+            self.__use_composition = self.__use_composition[0]
+
+        if isinstance(self.__create_own_container, list):
+            self.__create_own_container = self.__create_own_container[0]
+
+        # Standard node configuration
+        load_nodes = GroupAction(
+            condition=IfCondition(PythonExpression(['not ', self.__use_composition])),
+            actions=[
+                Node(
+                    package='ros_gz_bridge',
+                    executable='bridge_node',
+                    name=self.__bridge_name,
+                    namespace=self.__namespace,
+                    output='screen',
+                    respawn=self.__use_respawn,
+                    respawn_delay=2.0,
+                    parameters=[{'config_file': self.__config_file, **parsed_bridge_params}],
+                    arguments=['--ros-args', '--log-level', self.__log_level],
+                ),
+            ],
+        )
+
+        # Composable node with container configuration
+        load_composable_nodes_with_container = ComposableNodeContainer(
+            condition=IfCondition(
+                PythonExpression([self.__use_composition, ' and ', self.__create_own_container])
+            ),
+            name=self.__container_name,
+            namespace='',
+            package='rclcpp_components',
+            executable='component_container',
+            composable_node_descriptions=[
+                ComposableNode(
+                    package='ros_gz_bridge',
+                    plugin='ros_gz_bridge::RosGzBridge',
+                    name=self.__bridge_name,
+                    namespace=self.__namespace,
+                    parameters=[{'config_file': self.__config_file, **parsed_bridge_params}],
+                    extra_arguments=[{'use_intra_process_comms': True}],
+                ),
+            ],
+            output='screen',
+        )
+
+        # Composable node without container configuration
+        load_composable_nodes_without_container = LoadComposableNodes(
+            condition=IfCondition(
+                PythonExpression(
+                    [self.__use_composition, ' and not ', self.__create_own_container]
+                )
+            ),
+            target_container=self.__container_name,
+            composable_node_descriptions=[
+                ComposableNode(
+                    package='ros_gz_bridge',
+                    plugin='ros_gz_bridge::RosGzBridge',
+                    name=self.__bridge_name,
+                    namespace=self.__namespace,
+                    parameters=[{'config_file': self.__config_file, **parsed_bridge_params}],
+                    extra_arguments=[{'use_intra_process_comms': True}],
+                ),
+            ],
+        )
+
+        return [
+            load_nodes,
+            load_composable_nodes_with_container,
+            load_composable_nodes_without_container
+        ]
diff --git a/ros_gz_sim/launch/ros_gz_sim.launch b/ros_gz_sim/launch/ros_gz_sim.launch
@@ -7,6 +7,7 @@
   <arg name="use_composition" default="False" />
   <arg name="use_respawn" default="False" />
   <arg name="log_level" default="info" />
+  <arg name="bridge_params" default="" />
   <arg name="world_sdf_file" default="empty.sdf" />
   <arg name="world_sdf_string" default="" />
   <gz_server
@@ -24,6 +25,7 @@
     namespace="$(var namespace)"
     use_composition="$(var use_composition)"
     use_respawn="$(var use_respawn)"
-    log_level="$(var log_level)">
+    log_level="$(var log_level)"
+    bridge_params="$(var bridge_params)">
   </ros_gz_bridge>
 </launch>
diff --git a/ros_gz_sim/launch/ros_gz_sim.launch.py b/ros_gz_sim/launch/ros_gz_sim.launch.py
@@ -19,6 +19,7 @@
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, TextSubstitution
 from launch_ros.substitutions import FindPackageShare
+from ros_gz_bridge.actions import RosGzBridge
 
 
 def generate_launch_description():
@@ -31,6 +32,7 @@ def generate_launch_description():
     use_composition = LaunchConfiguration('use_composition')
     use_respawn = LaunchConfiguration('use_respawn')
     bridge_log_level = LaunchConfiguration('bridge_log_level')
+    bridge_params = LaunchConfiguration('bridge_params')
 
     world_sdf_file = LaunchConfiguration('world_sdf_file')
     world_sdf_string = LaunchConfiguration('world_sdf_string')
@@ -73,6 +75,10 @@ def generate_launch_description():
         'bridge_log_level', default_value='info', description='Bridge log level'
     )
 
+    declare_bridge_params_cmd = DeclareLaunchArgument(
+        'bridge_params', default_value='', description='Extra parameters to pass to the bridge.'
+    )
+
     declare_world_sdf_file_cmd = DeclareLaunchArgument(
         'world_sdf_file', default_value=TextSubstitution(text=''),
         description='Path to the SDF world file'
@@ -94,19 +100,17 @@ def generate_launch_description():
                           ('create_own_container', create_own_container),
                           ('use_composition', use_composition), ])
 
-    bridge_description = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            [PathJoinSubstitution([FindPackageShare('ros_gz_bridge'),
-                                   'launch',
-                                   'ros_gz_bridge.launch.py'])]),
-        launch_arguments=[('bridge_name', bridge_name),
-                          ('config_file', config_file),
-                          ('container_name', container_name),
-                          ('namespace', namespace),
-                          ('create_own_container', str(False)),
-                          ('use_composition', use_composition),
-                          ('use_respawn', use_respawn),
-                          ('bridge_log_level', bridge_log_level), ])
+    ros_gz_bridge_action = RosGzBridge(
+        bridge_name=bridge_name,
+        config_file=config_file,
+        container_name=container_name,
+        create_own_container=str(False),
+        namespace=namespace,
+        use_composition=use_composition,
+        use_respawn=use_respawn,
+        log_level=bridge_log_level,
+        bridge_params=bridge_params,
+    )
 
     # Create the launch description and populate
     ld = LaunchDescription()
@@ -120,10 +124,11 @@ def generate_launch_description():
     ld.add_action(declare_use_composition_cmd)
     ld.add_action(declare_use_respawn_cmd)
     ld.add_action(declare_bridge_log_level_cmd)
+    ld.add_action(declare_bridge_params_cmd)
     ld.add_action(declare_world_sdf_file_cmd)
     ld.add_action(declare_world_sdf_string_cmd)
     # Add the actions to launch all of the bridge + gz_server nodes
     ld.add_action(gz_server_description)
-    ld.add_action(bridge_description)
+    ld.add_action(ros_gz_bridge_action)
 
     return ld