From 232612fa8e05eb4a5894be62dd01c9491f757b11 Mon Sep 17 00:00:00 2001 From: gezp Date: Fri, 1 Oct 2021 00:39:40 +0800 Subject: [PATCH] Support manual composed bringup for Nav2 applications (#2555) * support manual composed bringup for Nav2 applications Signed-off-by: zhenpeng ge * fix linters Signed-off-by: zhenpeng ge * update Signed-off-by: zhenpeng ge * rm composed_bringup_launch.py Signed-off-by: zhenpeng ge * Update nav2_bringup/src/composed_bringup.cpp * Update nav2_bringup/src/composed_bringup.cpp * Update nav2_bringup/src/composed_bringup.cpp * Update nav2_bringup/src/composed_bringup.cpp * Update nav2_bringup/src/composed_bringup.cpp * Update nav2_bringup/src/composed_bringup.cpp * Update nav2_bringup/README.md * Update nav2_bringup/README.md * Update nav2_bringup/README.md * Update nav2_bringup/README.md * update nav2_bringup/README.md Signed-off-by: zhenpeng ge * remove unused use_lifecycle_mgr Signed-off-by: zhenpeng ge Co-authored-by: Steve Macenski --- nav2_bringup/CMakeLists.txt | 32 +++++++ nav2_bringup/README.md | 25 +++++- nav2_bringup/launch/bringup_launch.py | 44 +++++++-- nav2_bringup/launch/tb3_simulation_launch.py | 9 +- nav2_bringup/src/composed_bringup.cpp | 93 ++++++++++++++++++++ 5 files changed, 195 insertions(+), 8 deletions(-) create mode 100644 nav2_bringup/src/composed_bringup.cpp diff --git a/nav2_bringup/CMakeLists.txt b/nav2_bringup/CMakeLists.txt index 629034556e..15214684e9 100644 --- a/nav2_bringup/CMakeLists.txt +++ b/nav2_bringup/CMakeLists.txt @@ -4,9 +4,41 @@ 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_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_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 4e0b263bdc..b2fd44434d 100644 --- a/nav2_bringup/README.md +++ b/nav2_bringup/README.md @@ -1,6 +1,15 @@ # nav2_bringup -The `nav2_bringup` package is an example bringup system for Nav2 applications. +The `nav2_bringup` package is an example bringup system for Nav2 applications. + +This is a very flexible example for nav2 bringup that can be modified for different maps/robots/hardware/worlds/etc. It is our expectation for an application specific robot system that you're mirroring `nav2_bringup` package and modifying it for your specific maps/robots/bringup needs. This is an applied and working demonstration for the default system bringup with many options that can be easily modified. + +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. + +* 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. ### Pre-requisites: * [Install ROS 2](https://index.ros.org/doc/ros2/Installation/Dashing/) @@ -45,11 +54,22 @@ ros2 launch turtlebot3_bringup turtlebot3_state_publisher.launch.py use_sim_time ### Terminal 3: Launch Nav2 +Normal Bringup + +```bash +source /opt/ros/dashing/setup.bash +ros2 launch nav2_bringup bringup_launch.py use_sim_time:=True autostart:=True use_composition:=False\ +map:= +``` + +Manually Composed Bringup + ```bash source /opt/ros/dashing/setup.bash -ros2 launch nav2_bringup bringup_launch.py use_sim_time:=True autostart:=True \ +ros2 launch nav2_bringup bringup_launch.py use_sim_time:=True autostart:=True use_composition:=True\ map:= ``` +* `use_composition` is True by default ### Terminal 4: Run RViz with Nav2 config file @@ -79,6 +99,7 @@ ros2 launch nav2_bringup tb3_simulation_launch.py Where `` can used to replace any of the default options, for example: ``` +use_composition:= world:= map:= rviz_config_file:= diff --git a/nav2_bringup/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py index 8d357ff5ad..938ad0cc35 100644 --- a/nav2_bringup/launch/bringup_launch.py +++ b/nav2_bringup/launch/bringup_launch.py @@ -23,6 +23,8 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression from launch_ros.actions import PushRosNamespace +from launch_ros.actions import Node +from nav2_common.launch import RewrittenYaml def generate_launch_description(): @@ -38,6 +40,27 @@ def generate_launch_description(): use_sim_time = LaunchConfiguration('use_sim_time') params_file = LaunchConfiguration('params_file') autostart = LaunchConfiguration('autostart') + use_composition = LaunchConfiguration('use_composition') + + # Map fully qualified names to relative ones so the node's namespace can be prepended. + # In case of the transforms (tf), currently, there doesn't seem to be a better alternative + # https://github.com/ros/geometry2/issues/32 + # https://github.com/ros/robot_state_publisher/pull/30 + # TODO(orduno) Substitute with `PushNodeRemapping` + # https://github.com/ros2/launch_ros/issues/56 + remappings = [('/tf', 'tf'), + ('/tf_static', 'tf_static')] + + # Create our own temporary YAML files that include substitutions + param_substitutions = { + 'use_sim_time': use_sim_time, + 'yaml_filename': map_yaml_file} + + configured_params = RewrittenYaml( + source_file=params_file, + root_key=namespace, + param_rewrites=param_substitutions, + convert_types=True) stdout_linebuf_envvar = SetEnvironmentVariable( 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') @@ -75,6 +98,10 @@ def generate_launch_description(): 'autostart', default_value='true', description='Automatically startup the nav2 stack') + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', default_value='True', + description='Whether to use composed bringup') + # Specify the actions bringup_cmd_group = GroupAction([ PushRosNamespace( @@ -97,17 +124,23 @@ def generate_launch_description(): 'map': map_yaml_file, 'use_sim_time': use_sim_time, 'autostart': autostart, - 'params_file': params_file, - 'use_lifecycle_mgr': 'false'}.items()), + '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), 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, - 'use_lifecycle_mgr': 'false', - 'map_subscribe_transient_local': 'true'}.items()), + 'params_file': params_file}.items()), ]) # Create the launch description and populate @@ -124,6 +157,7 @@ def generate_launch_description(): 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) # Add the actions to launch all of the navigation nodes ld.add_action(bringup_cmd_group) diff --git a/nav2_bringup/launch/tb3_simulation_launch.py b/nav2_bringup/launch/tb3_simulation_launch.py index ec982c3da2..116a7151ab 100644 --- a/nav2_bringup/launch/tb3_simulation_launch.py +++ b/nav2_bringup/launch/tb3_simulation_launch.py @@ -39,6 +39,7 @@ def generate_launch_description(): use_sim_time = LaunchConfiguration('use_sim_time') params_file = LaunchConfiguration('params_file') autostart = LaunchConfiguration('autostart') + use_composition = LaunchConfiguration('use_composition') # Launch configuration variables specific to simulation rviz_config_file = LaunchConfiguration('rviz_config_file') @@ -101,6 +102,10 @@ def generate_launch_description(): 'autostart', default_value='true', description='Automatically startup the nav2 stack') + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', default_value='True', + description='Whether to use composed bringup') + declare_rviz_config_file_cmd = DeclareLaunchArgument( 'rviz_config_file', default_value=os.path.join( @@ -201,7 +206,8 @@ def generate_launch_description(): 'map': map_yaml_file, 'use_sim_time': use_sim_time, 'params_file': params_file, - 'autostart': autostart}.items()) + 'autostart': autostart, + 'use_composition': use_composition}.items()) # Create the launch description and populate ld = LaunchDescription() @@ -214,6 +220,7 @@ def generate_launch_description(): 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_rviz_config_file_cmd) ld.add_action(declare_use_simulator_cmd) diff --git a/nav2_bringup/src/composed_bringup.cpp b/nav2_bringup/src/composed_bringup.cpp new file mode 100644 index 0000000000..cf4103e9c3 --- /dev/null +++ b/nav2_bringup/src/composed_bringup.cpp @@ -0,0 +1,93 @@ +// 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/nav2_controller.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 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(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; +}