Merge branch 'ros2' into caguero/battery_demo_v2

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>

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

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,102 @@ 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]
+
+        if isinstance(self.__use_respawn, list):
+            self.__use_respawn = self.__use_respawn[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=bool(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
+        ]

ros_gz_sim/CMakeLists.txt

@@ -48,7 +48,17 @@ target_link_libraries(create
   gz-msgs::core
   gz-transport::core
 )
-ament_target_dependencies(create std_msgs)
+
+add_executable(remove src/remove.cpp)
+ament_target_dependencies(remove
+  rclcpp
+  std_msgs
+)
+
+target_link_libraries(remove
+  gz-msgs::core
+  gz-transport::core
+)
 
 add_library(${PROJECT_NAME} SHARED src/Stopwatch.cpp)
 ament_target_dependencies(${PROJECT_NAME}
@@ -104,13 +114,16 @@ install(FILES
   "launch/ros_gz_sim.launch"
   "launch/ros_gz_sim.launch.py"
   "launch/ros_gz_spawn_model.launch.py"
+  "launch/gz_remove_model.launch.py"
   DESTINATION share/${PROJECT_NAME}/launch
 )
 
 install(TARGETS
   create
+  remove
   DESTINATION lib/${PROJECT_NAME}
 )
+
 install(TARGETS
   gzserver
   DESTINATION lib/${PROJECT_NAME}
@@ -151,6 +164,10 @@ if(BUILD_TESTING)
     test/test_create.cpp
   )
 
+  ament_add_gtest_executable(test_remove
+    test/test_remove.cpp
+  )
+
   ament_target_dependencies(test_stopwatch rclcpp)
 
   target_include_directories(test_stopwatch PUBLIC
@@ -165,12 +182,17 @@ if(BUILD_TESTING)
     gz-transport::core
   )
 
+  target_link_libraries(test_remove
+    gz-transport::core
+  )
+
   install(
-    TARGETS test_stopwatch test_create
+    TARGETS test_stopwatch test_create test_remove
     DESTINATION lib/${PROJECT_NAME}
   )
   ament_add_gtest_test(test_stopwatch)
   add_launch_test(test/test_create_node.launch.py TIMEOUT 200)
+  add_launch_test(test/test_remove_node.launch.py TIMEOUT 200)
 endif()
 
 ament_package()