Refactor sim_time and composable node usage in Collision Monitor

nav2_collision_monitor/launch/collision_monitor_node.launch.py

@@ -19,10 +19,12 @@
 from ament_index_python.packages import get_package_share_directory
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
-from launch.substitutions import LaunchConfiguration
+from launch.actions import DeclareLaunchArgument, GroupAction
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration, PythonExpression
+from launch_ros.actions import LoadComposableNodes, SetParameter
 from launch_ros.actions import Node
-from nav2_common.launch import RewrittenYaml
+from launch_ros.descriptions import ComposableNode
 
 
 def generate_launch_description():
@@ -38,6 +40,9 @@ def generate_launch_description():
     namespace = LaunchConfiguration('namespace')
     use_sim_time = LaunchConfiguration('use_sim_time')
     params_file = LaunchConfiguration('params_file')
+    use_composition = LaunchConfiguration('use_composition')
+    container_name = LaunchConfiguration('container_name')
+    container_name_full = (namespace, '/', container_name)
 
     # 2. Declare the launch arguments
     declare_namespace_cmd = DeclareLaunchArgument(
@@ -55,43 +60,69 @@ def generate_launch_description():
         default_value=os.path.join(package_dir, 'params', 'collision_monitor_params.yaml'),
         description='Full path to the ROS2 parameters file to use for all launched nodes')
 
-    # Create our own temporary YAML files that include substitutions
-    param_substitutions = {
-        'use_sim_time': use_sim_time}
-
-    configured_params = RewrittenYaml(
-        source_file=params_file,
-        root_key=namespace,
-        param_rewrites=param_substitutions,
-        convert_types=True)
-
-    # Nodes launching commands
-    start_lifecycle_manager_cmd = Node(
-            package='nav2_lifecycle_manager',
-            executable='lifecycle_manager',
-            name='lifecycle_manager',
-            output='screen',
-            emulate_tty=True,  # https://github.com/ros2/launch/issues/188
-            parameters=[{'use_sim_time': use_sim_time},
-                        {'autostart': autostart},
-                        {'node_names': lifecycle_nodes}])
-
-    start_collision_monitor_cmd = Node(
-            package='nav2_collision_monitor',
-            executable='collision_monitor',
-            output='screen',
-            emulate_tty=True,  # https://github.com/ros2/launch/issues/188
-            parameters=[configured_params])
+    declare_use_composition_cmd = DeclareLaunchArgument(
+        'use_composition', default_value='True',
+        description='Use composed bringup if True')
+
+    declare_container_name_cmd = DeclareLaunchArgument(
+        'container_name', default_value='nav2_container',
+        description='the name of conatiner that nodes will load in if use composition')
+
+    load_nodes = GroupAction(
+        condition=IfCondition(PythonExpression(['not ', use_composition])),
+        actions=[
+            SetParameter('use_sim_time', use_sim_time),
+            Node(
+                package='nav2_lifecycle_manager',
+                executable='lifecycle_manager',
+                name='lifecycle_manager_collision_monitor',
+                output='screen',
+                emulate_tty=True,  # https://github.com/ros2/launch/issues/188
+                parameters=[{'autostart': autostart},
+                            {'node_names': lifecycle_nodes}]),
+            Node(
+                package='nav2_collision_monitor',
+                executable='collision_monitor',
+                output='screen',
+                emulate_tty=True,  # https://github.com/ros2/launch/issues/188
+                parameters=[params_file])
+        ]
+    )
+
+    load_composable_nodes = GroupAction(
+        condition=IfCondition(use_composition),
+        actions=[
+            SetParameter('use_sim_time', use_sim_time),
+            LoadComposableNodes(
+                target_container=container_name_full,
+                composable_node_descriptions=[
+                    ComposableNode(
+                        package='nav2_lifecycle_manager',
+                        plugin='nav2_lifecycle_manager::LifecycleManager',
+                        name='lifecycle_manager_collision_monitor',
+                        parameters=[{'autostart': autostart},
+                                    {'node_names': lifecycle_nodes}]),
+                    ComposableNode(
+                        package='nav2_collision_monitor',
+                        plugin='nav2_collision_monitor::CollisionMonitor',
+                        name='collision_monitor',
+                        parameters=[params_file]),
+                ],
+            )
+        ]
+    )
 
     ld = LaunchDescription()
 
     # Launch arguments
     ld.add_action(declare_namespace_cmd)
     ld.add_action(declare_use_sim_time_cmd)
     ld.add_action(declare_params_file_cmd)
+    ld.add_action(declare_use_composition_cmd)
+    ld.add_action(declare_container_name_cmd)
 
     # Node launching commands
-    ld.add_action(start_lifecycle_manager_cmd)
-    ld.add_action(start_collision_monitor_cmd)
+    ld.add_action(load_nodes)
+    ld.add_action(load_composable_nodes)
 
     return ld