support dynamic composed bringup

nav2_bringup/CMakeLists.txt

@@ -4,43 +4,9 @@ project(nav2_bringup)
 find_package(ament_cmake REQUIRED)
 find_package(nav2_common REQUIRED)
 find_package(navigation2 REQUIRED)
-find_package(nav2_map_server REQUIRED)
-find_package(nav2_amcl REQUIRED)
-find_package(nav2_controller REQUIRED)
-find_package(nav2_smoother REQUIRED)
-find_package(nav2_planner REQUIRED)
-find_package(nav2_recoveries REQUIRED)
-find_package(nav2_bt_navigator REQUIRED)
-find_package(nav2_waypoint_follower REQUIRED)
-find_package(nav2_lifecycle_manager REQUIRED)
 
 nav2_package()
 
-set(executable_name composed_bringup)
-add_executable(${executable_name}
-  src/composed_bringup.cpp
-)
-
-set(dependencies
-  nav2_map_server
-  nav2_amcl
-  nav2_controller
-  nav2_smoother
-  nav2_planner
-  nav2_recoveries
-  nav2_bt_navigator
-  nav2_waypoint_follower
-  nav2_lifecycle_manager
-)
-
-ament_target_dependencies(${executable_name}
-  ${dependencies}
-)
-
-install(TARGETS ${executable_name}
-  RUNTIME DESTINATION lib/${PROJECT_NAME}
-)
-
 install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
 install(DIRECTORY maps DESTINATION share/${PROJECT_NAME})
 install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME})

nav2_bringup/launch/bringup_launch.py

@@ -108,6 +108,15 @@ def generate_launch_description():
             condition=IfCondition(use_namespace),
             namespace=namespace),
 
+        Node(
+            condition=IfCondition(use_composition),
+            name='nav2_container',
+            package='rclcpp_components',
+            executable='component_container_mt',
+            parameters=[configured_params, {'autostart': autostart}],
+            remappings=remappings,
+            output='screen'),
+
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource(os.path.join(launch_dir, 'slam_launch.py')),
             condition=IfCondition(slam),
@@ -124,23 +133,18 @@ def generate_launch_description():
                               'map': map_yaml_file,
                               'use_sim_time': use_sim_time,
                               'autostart': autostart,
-                              'params_file': params_file}.items()),
-
-        Node(
-            condition=IfCondition(use_composition),
-            package='nav2_bringup',
-            executable='composed_bringup',
-            output='screen',
-            parameters=[configured_params, {'autostart': autostart}],
-            remappings=remappings),
+                              'params_file': params_file,
+                              'use_composition': use_composition,
+                              'container_name': 'nav2_container'}.items()),
 
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource(os.path.join(launch_dir, 'navigation_launch.py')),
-            condition=IfCondition(PythonExpression(['not ', use_composition])),
             launch_arguments={'namespace': namespace,
                               'use_sim_time': use_sim_time,
                               'autostart': autostart,
-                              'params_file': params_file}.items()),
+                              'params_file': params_file,
+                              'use_composition': use_composition,
+                              'container_name': 'nav2_container'}.items()),
     ])
 
     # Create the launch description and populate

nav2_bringup/launch/localization_launch.py

@@ -17,9 +17,12 @@
 from ament_index_python.packages import get_package_share_directory
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable
-from launch.substitutions import LaunchConfiguration
+from launch.actions import DeclareLaunchArgument, GroupAction, SetEnvironmentVariable
+from launch.conditions import IfCondition
+from launch.substitutions import LaunchConfiguration, PythonExpression
 from launch_ros.actions import Node
+from launch_ros.actions import LoadComposableNodes
+from launch_ros.descriptions import ComposableNode
 from nav2_common.launch import RewrittenYaml
 
 
@@ -32,6 +35,9 @@ def generate_launch_description():
     use_sim_time = LaunchConfiguration('use_sim_time')
     autostart = LaunchConfiguration('autostart')
     params_file = LaunchConfiguration('params_file')
+    use_composition = LaunchConfiguration('use_composition')
+    container_name = LaunchConfiguration('container_name')
+
     lifecycle_nodes = ['map_server', 'amcl']
 
     # Map fully qualified names to relative ones so the node's namespace can be prepended.
@@ -54,6 +60,60 @@ def generate_launch_description():
         param_rewrites=param_substitutions,
         convert_types=True)
 
+    load_nodes = GroupAction(
+        condition=IfCondition(PythonExpression(['not ', use_composition])),
+        actions=[
+            Node(
+                package='nav2_map_server',
+                executable='map_server',
+                name='map_server',
+                output='screen',
+                parameters=[configured_params],
+                remappings=remappings),
+            Node(
+                package='nav2_amcl',
+                executable='amcl',
+                name='amcl',
+                output='screen',
+                parameters=[configured_params],
+                remappings=remappings),
+            Node(
+                package='nav2_lifecycle_manager',
+                executable='lifecycle_manager',
+                name='lifecycle_manager_localization',
+                output='screen',
+                parameters=[{'use_sim_time': use_sim_time},
+                            {'autostart': autostart},
+                            {'node_names': lifecycle_nodes}])
+        ]
+    )
+
+    load_composable_nodes = LoadComposableNodes(
+        condition=IfCondition(use_composition),
+        target_container=container_name,
+        composable_node_descriptions=[
+            ComposableNode(
+                package='nav2_map_server',
+                plugin='nav2_map_server::MapServer',
+                name='map_server',
+                parameters=[configured_params],
+                remappings=remappings),
+            ComposableNode(
+                package='nav2_amcl',
+                plugin='nav2_amcl::AmclNode',
+                name='amcl',
+                parameters=[configured_params],
+                remappings=remappings),
+            ComposableNode(
+                package='nav2_lifecycle_manager',
+                plugin='nav2_lifecycle_manager::LifecycleManager',
+                name='lifecycle_manager_localization',
+                parameters=[{'use_sim_time': use_sim_time,
+                             'autostart': autostart,
+                             'node_names': lifecycle_nodes}]),
+        ],
+    )
+
     return LaunchDescription([
         # Set env var to print messages to stdout immediately
         SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'),
@@ -80,28 +140,15 @@ def generate_launch_description():
             default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'),
             description='Full path to the ROS2 parameters file to use'),
 
-        Node(
-            package='nav2_map_server',
-            executable='map_server',
-            name='map_server',
-            output='screen',
-            parameters=[configured_params],
-            remappings=remappings),
-
-        Node(
-            package='nav2_amcl',
-            executable='amcl',
-            name='amcl',
-            output='screen',
-            parameters=[configured_params],
-            remappings=remappings),
-
-        Node(
-            package='nav2_lifecycle_manager',
-            executable='lifecycle_manager',
-            name='lifecycle_manager_localization',
-            output='screen',
-            parameters=[{'use_sim_time': use_sim_time},
-                        {'autostart': autostart},
-                        {'node_names': lifecycle_nodes}])
+        DeclareLaunchArgument(
+            'use_composition', default_value='False',
+            description='Use composed bringup if True'),
+
+        DeclareLaunchArgument(
+            'container_name', default_value='nav2_container',
+            description='the name of conatiner that nodes will load in if use composition'),
+
+        load_nodes,
+
+        load_composable_nodes,
     ])