From 417f014669051ffdaa6ad35bc32d4d983b6c3190 Mon Sep 17 00:00:00 2001 From: gezp Date: Thu, 27 Jan 2022 00:35:48 +0800 Subject: [PATCH] support dynamic composed bringup (#2750) * support dynamic composed bringup Signed-off-by: zhenpeng ge * update Signed-off-by: zhenpeng ge * update delcares in launch Signed-off-by: zhenpeng ge * use component_container_isolated Signed-off-by: zhenpeng ge * update README Signed-off-by: zhenpeng ge --- nav2_bringup/CMakeLists.txt | 34 --- nav2_bringup/README.md | 3 +- nav2_bringup/launch/bringup_launch.py | 26 ++- nav2_bringup/launch/localization_launch.py | 169 ++++++++++----- nav2_bringup/launch/navigation_launch.py | 241 ++++++++++++++------- nav2_bringup/src/composed_bringup.cpp | 97 --------- 6 files changed, 293 insertions(+), 277 deletions(-) delete mode 100644 nav2_bringup/src/composed_bringup.cpp diff --git a/nav2_bringup/CMakeLists.txt b/nav2_bringup/CMakeLists.txt index dcd20cbed2..629034556e 100644 --- a/nav2_bringup/CMakeLists.txt +++ b/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}) diff --git a/nav2_bringup/README.md b/nav2_bringup/README.md index b47ef57ff9..574a817ecb 100644 --- a/nav2_bringup/README.md +++ b/nav2_bringup/README.md @@ -6,10 +6,9 @@ This is a very flexible example for nav2 bringup that can be modified for differ Usual robot stacks will have a `_nav` package with config/bringup files and this is that for the general case to base a specific robot system off of. -Composed bringup (based on [ROS2 Composition](https://docs.ros.org/en/galactic/Tutorials/Composition.html)) is optional for users. It can be used to compose all Nav2 nodes in a single process instead of launching these nodes separately, which is useful for embedded systems users that need to make optimizations due to harsh resource constraints. Manually composed bring up is used by default, but can be disabled by using the launch argument `use_composition:=False`. +Dynamically composed bringup (based on [ROS2 Composition](https://docs.ros.org/en/galactic/Tutorials/Composition.html)) is optional for users. It can be used to compose all Nav2 nodes in a single process instead of launching these nodes separately, which is useful for embedded systems users that need to make optimizations due to harsh resource constraints. Dynamically composed bringup is used by default, but can be disabled by using the launch argument `use_composition:=False`. * Some discussions about performance improvement of composed bringup could be found here: https://discourse.ros.org/t/nav2-composition/22175. -* Currently, manual composition is used in this package. Dynamic composition is more flexible than manual composition, but is not currently applied in nav2 due to various issues, you could find more details here: https://github.com/ros-planning/navigation2/issues/2147. To use, please see the Nav2 [Getting Started Page](https://navigation.ros.org/getting_started/index.html) on our documentation website. Additional [tutorials will help you](https://navigation.ros.org/tutorials/index.html) go from an initial setup in simulation to testing on a hardware robot, using SLAM, and more. diff --git a/nav2_bringup/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py index 938ad0cc35..b9e129e5db 100644 --- a/nav2_bringup/launch/bringup_launch.py +++ b/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_isolated', + 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 diff --git a/nav2_bringup/launch/localization_launch.py b/nav2_bringup/launch/localization_launch.py index e058bba8c1..33cf3fa5d4 100644 --- a/nav2_bringup/launch/localization_launch.py +++ b/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,54 +60,111 @@ def generate_launch_description(): param_rewrites=param_substitutions, convert_types=True) - return LaunchDescription([ - # Set env var to print messages to stdout immediately - SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), - - DeclareLaunchArgument( - 'namespace', default_value='', - description='Top-level namespace'), - - DeclareLaunchArgument( - 'map', - default_value=os.path.join(bringup_dir, 'maps', 'turtlebot3_world.yaml'), - description='Full path to map yaml file to load'), - - DeclareLaunchArgument( - 'use_sim_time', default_value='false', - description='Use simulation (Gazebo) clock if true'), - - DeclareLaunchArgument( - 'autostart', default_value='true', - description='Automatically startup the nav2 stack'), - - DeclareLaunchArgument( - 'params_file', - 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}]) - ]) + stdout_linebuf_envvar = SetEnvironmentVariable( + 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') + + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', + default_value='', + description='Top-level namespace') + + declare_map_yaml_cmd = DeclareLaunchArgument( + 'map', + description='Full path to map yaml file to load') + + declare_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Use simulation (Gazebo) clock if true') + + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), + description='Full path to the ROS2 parameters file to use for all launched nodes') + + declare_autostart_cmd = DeclareLaunchArgument( + 'autostart', default_value='true', + description='Automatically startup the nav2 stack') + + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', default_value='False', + 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=[ + 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}]), + ], + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Set environment variables + ld.add_action(stdout_linebuf_envvar) + + # Declare the launch options + ld.add_action(declare_namespace_cmd) + ld.add_action(declare_map_yaml_cmd) + ld.add_action(declare_use_sim_time_cmd) + ld.add_action(declare_params_file_cmd) + ld.add_action(declare_autostart_cmd) + ld.add_action(declare_use_composition_cmd) + ld.add_action(declare_container_name_cmd) + + # Add the actions to launch all of the localiztion nodes + ld.add_action(load_nodes) + ld.add_action(load_composable_nodes) + + return ld diff --git a/nav2_bringup/launch/navigation_launch.py b/nav2_bringup/launch/navigation_launch.py index cf139e366c..e35f88c986 100644 --- a/nav2_bringup/launch/navigation_launch.py +++ b/nav2_bringup/launch/navigation_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 @@ -31,6 +34,8 @@ 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 = ['controller_server', 'smoother_server', @@ -59,81 +64,157 @@ def generate_launch_description(): param_rewrites=param_substitutions, convert_types=True) - return LaunchDescription([ - # Set env var to print messages to stdout immediately - SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), - - DeclareLaunchArgument( - 'namespace', default_value='', - description='Top-level namespace'), - - DeclareLaunchArgument( - 'use_sim_time', default_value='false', - description='Use simulation (Gazebo) clock if true'), - - DeclareLaunchArgument( - 'autostart', default_value='true', - description='Automatically startup the nav2 stack'), - - DeclareLaunchArgument( - 'params_file', - default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), - description='Full path to the ROS2 parameters file to use'), - - Node( - package='nav2_controller', - executable='controller_server', - output='screen', - parameters=[configured_params], - remappings=remappings), - - Node( - package='nav2_smoother', - executable='smoother_server', - name='smoother_server', - output='screen', - parameters=[configured_params], - remappings=remappings), - - Node( - package='nav2_planner', - executable='planner_server', - name='planner_server', - output='screen', - parameters=[configured_params], - remappings=remappings), - - Node( - package='nav2_recoveries', - executable='recoveries_server', - name='recoveries_server', - output='screen', - parameters=[configured_params], - remappings=remappings), - - Node( - package='nav2_bt_navigator', - executable='bt_navigator', - name='bt_navigator', - output='screen', - parameters=[configured_params], - remappings=remappings), - - Node( - package='nav2_waypoint_follower', - executable='waypoint_follower', - name='waypoint_follower', - output='screen', - parameters=[configured_params], - remappings=remappings), - - Node( - package='nav2_lifecycle_manager', - executable='lifecycle_manager', - name='lifecycle_manager_navigation', - output='screen', - parameters=[{'use_sim_time': use_sim_time}, - {'autostart': autostart}, - {'node_names': lifecycle_nodes}]), - - ]) + stdout_linebuf_envvar = SetEnvironmentVariable( + 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') + + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', + default_value='', + description='Top-level namespace') + + declare_use_sim_time_cmd = DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Use simulation (Gazebo) clock if true') + + declare_params_file_cmd = DeclareLaunchArgument( + 'params_file', + default_value=os.path.join(bringup_dir, 'params', 'nav2_params.yaml'), + description='Full path to the ROS2 parameters file to use for all launched nodes') + + declare_autostart_cmd = DeclareLaunchArgument( + 'autostart', default_value='true', + description='Automatically startup the nav2 stack') + + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', default_value='False', + 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=[ + Node( + package='nav2_controller', + executable='controller_server', + output='screen', + parameters=[configured_params], + remappings=remappings), + Node( + package='nav2_smoother', + executable='smoother_server', + name='smoother_server', + output='screen', + parameters=[configured_params], + remappings=remappings), + Node( + package='nav2_planner', + executable='planner_server', + name='planner_server', + output='screen', + parameters=[configured_params], + remappings=remappings), + Node( + package='nav2_recoveries', + executable='recoveries_server', + name='recoveries_server', + output='screen', + parameters=[configured_params], + remappings=remappings), + Node( + package='nav2_bt_navigator', + executable='bt_navigator', + name='bt_navigator', + output='screen', + parameters=[configured_params], + remappings=remappings), + Node( + package='nav2_waypoint_follower', + executable='waypoint_follower', + name='waypoint_follower', + output='screen', + parameters=[configured_params], + remappings=remappings), + Node( + package='nav2_lifecycle_manager', + executable='lifecycle_manager', + name='lifecycle_manager_navigation', + 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_controller', + plugin='nav2_controller::ControllerServer', + name='controller_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_smoother', + plugin='nav2_smoother::SmootherServer', + name='smoother_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_planner', + plugin='nav2_planner::PlannerServer', + name='planner_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_recoveries', + plugin='recovery_server::RecoveryServer', + name='recoveries_server', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_bt_navigator', + plugin='nav2_bt_navigator::BtNavigator', + name='bt_navigator', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_waypoint_follower', + plugin='nav2_waypoint_follower::WaypointFollower', + name='waypoint_follower', + parameters=[configured_params], + remappings=remappings), + ComposableNode( + package='nav2_lifecycle_manager', + plugin='nav2_lifecycle_manager::LifecycleManager', + name='lifecycle_manager_navigation', + parameters=[{'use_sim_time': use_sim_time, + 'autostart': autostart, + 'node_names': lifecycle_nodes}]), + ], + ) + + # Create the launch description and populate + ld = LaunchDescription() + + # Set environment variables + ld.add_action(stdout_linebuf_envvar) + + # Declare the launch options + 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_autostart_cmd) + ld.add_action(declare_use_composition_cmd) + ld.add_action(declare_container_name_cmd) + + # Add the actions to launch all of the navigation nodes + ld.add_action(load_nodes) + ld.add_action(load_composable_nodes) + + return ld diff --git a/nav2_bringup/src/composed_bringup.cpp b/nav2_bringup/src/composed_bringup.cpp deleted file mode 100644 index 7da9be9813..0000000000 --- a/nav2_bringup/src/composed_bringup.cpp +++ /dev/null @@ -1,97 +0,0 @@ -// Copyright (c) 2021, Samsung Research America -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. Reserved. - -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "nav2_map_server/map_server.hpp" -#include "nav2_amcl/amcl_node.hpp" -#include "nav2_controller/controller_server.hpp" -#include "nav2_smoother/nav2_smoother.hpp" -#include "nav2_planner/planner_server.hpp" -#include "nav2_recoveries/recovery_server.hpp" -#include "nav2_lifecycle_manager/lifecycle_manager.hpp" -#include "nav2_bt_navigator/bt_navigator.hpp" -#include "nav2_waypoint_follower/waypoint_follower.hpp" - -// in this manually composed node, a bunch of single threaded executors is used instead of -// 1 large multithreaded executor, because 1 large multithreaded executor consumes higher CPU. -// You could find more details here https://discourse.ros.org/t/nav2-composition/22175/10. -// -// rclcpp intra-process comms couldn't be enabled to get efficient communication, because -// it's still under development currently, and QoS limitations (e.g. transient-local -// isn't supported) make some Nav2 Nodes unanable to use intra-process comms, you -// could find more details here https://github.com/ros2/rclcpp/issues/1753. -// -// This is an example of manual composition for the default nav2 servers, It is our expectation -// for an application you're mirroring nav2_bringup package and modifying it for -// your sp. maps/robots/bringup needs. This is an applied and working demonstration for the -// default system bringup ONLY. - -template -std::shared_ptr create_spin_thread(NodeT & node) -{ - return std::make_shared( - [node]() { - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node->get_node_base_interface()); - executor.spin(); - executor.remove_node(node->get_node_base_interface()); - }); -} - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - - // navigation nodes - std::vector navigation_node_names; - auto controller_node = std::make_shared(); - navigation_node_names.push_back(controller_node->get_name()); - auto smoother_node = std::make_shared(); - navigation_node_names.push_back(smoother_node->get_name()); - auto planner_node = std::make_shared(); - navigation_node_names.push_back(planner_node->get_name()); - auto recoveries_node = std::make_shared(); - navigation_node_names.push_back(recoveries_node->get_name()); - auto bt_navigator_node = std::make_shared(); - navigation_node_names.push_back(bt_navigator_node->get_name()); - auto waypoint_follower_node = std::make_shared(); - navigation_node_names.push_back(waypoint_follower_node->get_name()); - // lifecycle manager of navigation - auto nav_manager_options = rclcpp::NodeOptions(); - nav_manager_options.arguments( - {"--ros-args", "-r", std::string("__node:=") + "lifecycle_manager_navigation", "--"}); - nav_manager_options.append_parameter_override("node_names", navigation_node_names); - auto lifecycle_manager_navigation_node = - std::make_shared(nav_manager_options); - - // spin threads - std::vector> threads; - threads.push_back(create_spin_thread(controller_node)); - threads.push_back(create_spin_thread(smoother_node)); - threads.push_back(create_spin_thread(planner_node)); - threads.push_back(create_spin_thread(recoveries_node)); - threads.push_back(create_spin_thread(bt_navigator_node)); - threads.push_back(create_spin_thread(waypoint_follower_node)); - threads.push_back(create_spin_thread(lifecycle_manager_navigation_node)); - for (auto t : threads) { - t->join(); - } - - rclcpp::shutdown(); - return 0; -}