From 155b6ca506e168bfd16179301e055c65543c2049 Mon Sep 17 00:00:00 2001 From: zhenpeng ge Date: Mon, 20 Dec 2021 10:47:17 +0800 Subject: [PATCH 1/5] support dynamic composed bringup Signed-off-by: zhenpeng ge --- nav2_bringup/CMakeLists.txt | 34 ---- nav2_bringup/launch/bringup_launch.py | 28 +-- nav2_bringup/launch/localization_launch.py | 99 +++++++--- nav2_bringup/launch/navigation_launch.py | 179 ++++++++++++------ nav2_bringup/src/composed_bringup.cpp | 97 ---------- nav2_smoother/CMakeLists.txt | 6 +- .../include/nav2_smoother/nav2_smoother.hpp | 6 +- nav2_smoother/package.xml | 1 + nav2_smoother/src/nav2_smoother.cpp | 11 +- 9 files changed, 229 insertions(+), 232 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/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py index 938ad0cc35..bb90b2225c 100644 --- a/nav2_bringup/launch/bringup_launch.py +++ b/nav2_bringup/launch/bringup_launch.py @@ -99,7 +99,7 @@ def generate_launch_description(): description='Automatically startup the nav2 stack') declare_use_composition_cmd = DeclareLaunchArgument( - 'use_composition', default_value='True', + 'use_composition', default_value='true', description='Whether to use composed bringup') # Specify the actions @@ -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 diff --git a/nav2_bringup/launch/localization_launch.py b/nav2_bringup/launch/localization_launch.py index e058bba8c1..01144a587f 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,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, ]) diff --git a/nav2_bringup/launch/navigation_launch.py b/nav2_bringup/launch/navigation_launch.py index cf139e366c..59ad722f54 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,6 +64,111 @@ def generate_launch_description(): param_rewrites=param_substitutions, convert_types=True) + 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}]), + ], + ) + return LaunchDescription([ # Set env var to print messages to stdout immediately SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1'), @@ -80,60 +190,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_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}]), + 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, ]) 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; -} diff --git a/nav2_smoother/CMakeLists.txt b/nav2_smoother/CMakeLists.txt index 6ae13c2244..fe2e99c91e 100644 --- a/nav2_smoother/CMakeLists.txt +++ b/nav2_smoother/CMakeLists.txt @@ -6,6 +6,7 @@ find_package(nav2_core REQUIRED) find_package(nav2_common REQUIRED) find_package(angles REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(rclcpp_action REQUIRED) find_package(std_msgs REQUIRED) find_package(nav2_util REQUIRED) @@ -32,13 +33,14 @@ add_executable(${executable_name} set(library_name ${executable_name}_core) -add_library(${library_name} +add_library(${library_name} SHARED src/nav2_smoother.cpp ) set(dependencies angles rclcpp + rclcpp_components rclcpp_action std_msgs nav2_msgs @@ -68,6 +70,8 @@ ament_target_dependencies(${executable_name} target_link_libraries(${executable_name} ${library_name}) +rclcpp_components_register_nodes(${library_name} "nav2_smoother::SmootherServer") + install( TARGETS ${library_name} diff --git a/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp b/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp index 14bb20e533..1a4d416503 100644 --- a/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp +++ b/nav2_smoother/include/nav2_smoother/nav2_smoother.hpp @@ -48,10 +48,10 @@ class SmootherServer : public nav2_util::LifecycleNode using SmootherMap = std::unordered_map; /** - * @brief Constructor for nav2_smoother::SmootherServer + * @brief A constructor for nav2_smoother::SmootherServer + * @param options Additional options to control creation of the node. */ - SmootherServer(); - + explicit SmootherServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); /** * @brief Destructor for nav2_smoother::SmootherServer */ diff --git a/nav2_smoother/package.xml b/nav2_smoother/package.xml index fc4801fafb..44eb0b892a 100644 --- a/nav2_smoother/package.xml +++ b/nav2_smoother/package.xml @@ -11,6 +11,7 @@ nav2_common angles rclcpp + rclcpp_components rclcpp_action std_msgs nav2_util diff --git a/nav2_smoother/src/nav2_smoother.cpp b/nav2_smoother/src/nav2_smoother.cpp index 1d3546f3be..c07478d577 100644 --- a/nav2_smoother/src/nav2_smoother.cpp +++ b/nav2_smoother/src/nav2_smoother.cpp @@ -33,8 +33,8 @@ using namespace std::chrono_literals; namespace nav2_smoother { -SmootherServer::SmootherServer() -: LifecycleNode("smoother_server", "", false), +SmootherServer::SmootherServer(const rclcpp::NodeOptions & options) +: LifecycleNode("smoother_server", "", false, options), lp_loader_("nav2_core", "nav2_core::Smoother"), default_ids_{}, default_types_{ "nav2_smoother::CeresCostawareSmoother"} @@ -320,3 +320,10 @@ void SmootherServer::smoothPlan() } } // namespace nav2_smoother + +#include "rclcpp_components/register_node_macro.hpp" + +// Register the component with class_loader. +// This acts as a sort of entry point, allowing the component to be discoverable when its library +// is being loaded into a running process. +RCLCPP_COMPONENTS_REGISTER_NODE(nav2_smoother::SmootherServer) From 046a3e8a32c7b2462f2640eef50a9b3f81c7956e Mon Sep 17 00:00:00 2001 From: zhenpeng ge Date: Mon, 20 Dec 2021 16:21:36 +0800 Subject: [PATCH 2/5] update Signed-off-by: zhenpeng ge --- nav2_bringup/launch/bringup_launch.py | 2 +- nav2_bringup/launch/localization_launch.py | 4 ++-- nav2_bringup/launch/navigation_launch.py | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/nav2_bringup/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py index bb90b2225c..e1ca53d9f9 100644 --- a/nav2_bringup/launch/bringup_launch.py +++ b/nav2_bringup/launch/bringup_launch.py @@ -99,7 +99,7 @@ def generate_launch_description(): description='Automatically startup the nav2 stack') declare_use_composition_cmd = DeclareLaunchArgument( - 'use_composition', default_value='true', + 'use_composition', default_value='True', description='Whether to use composed bringup') # Specify the actions diff --git a/nav2_bringup/launch/localization_launch.py b/nav2_bringup/launch/localization_launch.py index 01144a587f..0e8da13caa 100644 --- a/nav2_bringup/launch/localization_launch.py +++ b/nav2_bringup/launch/localization_launch.py @@ -141,8 +141,8 @@ def generate_launch_description(): description='Full path to the ROS2 parameters file to use'), DeclareLaunchArgument( - 'use_composition', default_value='false', - description='Use composed bringup if true'), + 'use_composition', default_value='False', + description='Use composed bringup if True'), DeclareLaunchArgument( 'container_name', default_value='nav2_container', diff --git a/nav2_bringup/launch/navigation_launch.py b/nav2_bringup/launch/navigation_launch.py index 59ad722f54..79dcb37304 100644 --- a/nav2_bringup/launch/navigation_launch.py +++ b/nav2_bringup/launch/navigation_launch.py @@ -191,8 +191,8 @@ def generate_launch_description(): description='Full path to the ROS2 parameters file to use'), DeclareLaunchArgument( - 'use_composition', default_value='false', - description='Use composed bringup if true'), + 'use_composition', default_value='False', + description='Use composed bringup if True'), DeclareLaunchArgument( 'container_name', default_value='nav2_container', From 26493eb82e1745d3a59c82d5d75c8dbfb6aa5e9e Mon Sep 17 00:00:00 2001 From: zhenpeng ge Date: Sun, 9 Jan 2022 22:24:38 +0800 Subject: [PATCH 3/5] update delcares in launch Signed-off-by: zhenpeng ge --- nav2_bringup/launch/localization_launch.py | 84 +++++++++++++--------- nav2_bringup/launch/navigation_launch.py | 74 +++++++++++-------- 2 files changed, 95 insertions(+), 63 deletions(-) diff --git a/nav2_bringup/launch/localization_launch.py b/nav2_bringup/launch/localization_launch.py index 0e8da13caa..33cf3fa5d4 100644 --- a/nav2_bringup/launch/localization_launch.py +++ b/nav2_bringup/launch/localization_launch.py @@ -60,6 +60,40 @@ def generate_launch_description(): param_rewrites=param_substitutions, convert_types=True) + 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=[ @@ -114,41 +148,23 @@ def generate_launch_description(): ], ) - 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'), + # Create the launch description and populate + ld = LaunchDescription() - DeclareLaunchArgument( - 'use_composition', default_value='False', - description='Use composed bringup if True'), + # Set environment variables + ld.add_action(stdout_linebuf_envvar) - DeclareLaunchArgument( - 'container_name', default_value='nav2_container', - description='the name of conatiner that nodes will load in if use composition'), + # 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) - load_nodes, + # Add the actions to launch all of the localiztion nodes + ld.add_action(load_nodes) + ld.add_action(load_composable_nodes) - load_composable_nodes, - ]) + return ld diff --git a/nav2_bringup/launch/navigation_launch.py b/nav2_bringup/launch/navigation_launch.py index 79dcb37304..e35f88c986 100644 --- a/nav2_bringup/launch/navigation_launch.py +++ b/nav2_bringup/launch/navigation_launch.py @@ -64,6 +64,36 @@ def generate_launch_description(): param_rewrites=param_substitutions, convert_types=True) + 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=[ @@ -169,36 +199,22 @@ def generate_launch_description(): ], ) - 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'), + # Create the launch description and populate + ld = LaunchDescription() - DeclareLaunchArgument( - 'use_composition', default_value='False', - description='Use composed bringup if True'), + # Set environment variables + ld.add_action(stdout_linebuf_envvar) - DeclareLaunchArgument( - 'container_name', default_value='nav2_container', - description='the name of conatiner that nodes will load in if use composition'), + # 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) - load_nodes, + # Add the actions to launch all of the navigation nodes + ld.add_action(load_nodes) + ld.add_action(load_composable_nodes) - load_composable_nodes, - ]) + return ld From 3e66f49a4fb1233a04c2e954607d4973ef008779 Mon Sep 17 00:00:00 2001 From: zhenpeng ge Date: Thu, 20 Jan 2022 21:28:49 +0800 Subject: [PATCH 4/5] use component_container_isolated Signed-off-by: zhenpeng ge --- nav2_bringup/launch/bringup_launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_bringup/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py index e1ca53d9f9..b9e129e5db 100644 --- a/nav2_bringup/launch/bringup_launch.py +++ b/nav2_bringup/launch/bringup_launch.py @@ -112,7 +112,7 @@ def generate_launch_description(): condition=IfCondition(use_composition), name='nav2_container', package='rclcpp_components', - executable='component_container_mt', + executable='component_container_isolated', parameters=[configured_params, {'autostart': autostart}], remappings=remappings, output='screen'), From 1e6e1ab8f4d733f7f8b65f09d5356b6056d1bbec Mon Sep 17 00:00:00 2001 From: zhenpeng ge Date: Wed, 26 Jan 2022 21:37:14 +0800 Subject: [PATCH 5/5] update README Signed-off-by: zhenpeng ge --- nav2_bringup/README.md | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) 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.