From e4d9c4a7d6f72e2feae5caedfa9df0134f1c600e Mon Sep 17 00:00:00 2001 From: zhenpeng ge Date: Wed, 8 Sep 2021 11:18:05 +0800 Subject: [PATCH 01/16] support manual composed bringup for Nav2 applications Signed-off-by: zhenpeng ge --- nav2_bringup/CMakeLists.txt | 32 +++++++ nav2_bringup/README.md | 13 +++ .../launch/manual_composed_bringup_launch.py | 91 ++++++++++++++++++ nav2_bringup/src/loc_and_nav.cpp | 96 +++++++++++++++++++ 4 files changed, 232 insertions(+) create mode 100644 nav2_bringup/launch/manual_composed_bringup_launch.py create mode 100644 nav2_bringup/src/loc_and_nav.cpp diff --git a/nav2_bringup/CMakeLists.txt b/nav2_bringup/CMakeLists.txt index 629034556e2..f2751829c2c 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 loc_and_nav) +add_executable(${executable_name} + src/loc_and_nav.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 4e0b263bdcc..27ef44db139 100644 --- a/nav2_bringup/README.md +++ b/nav2_bringup/README.md @@ -45,12 +45,25 @@ 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 \ map:= ``` +manual composed bringup + +```bash +source /opt/ros/dashing/setup.bash +ros2 launch nav2_bringup manual_composed_bringup_launch.py use_sim_time:=True autostart:=True \ +map:= +``` + +* composed bringup is based on [ROS2 Composition](https://docs.ros.org/en/galactic/Tutorials/Composition.html), which is useful for embedded systems users that need to make optimizations due to harsh resource constraints. +* some discussions about performance improvement could be found at https://discourse.ros.org/t/nav2-composition/22175 + ### Terminal 4: Run RViz with Nav2 config file ```bash diff --git a/nav2_bringup/launch/manual_composed_bringup_launch.py b/nav2_bringup/launch/manual_composed_bringup_launch.py new file mode 100644 index 00000000000..f64e0792e07 --- /dev/null +++ b/nav2_bringup/launch/manual_composed_bringup_launch.py @@ -0,0 +1,91 @@ + # Copyright (c) 2018 Intel Corporation +# +# 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. + +import os + +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_ros.actions import Node +from nav2_common.launch import RewrittenYaml + + +def generate_launch_description(): + # Get the launch directory + bringup_dir = get_package_share_directory('nav2_bringup') + + namespace = LaunchConfiguration('namespace') + map_yaml_file = LaunchConfiguration('map') + use_sim_time = LaunchConfiguration('use_sim_time') + autostart = LaunchConfiguration('autostart') + params_file = LaunchConfiguration('params_file') + + # 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 = { + 'yaml_filename': map_yaml_file, + 'use_sim_time': use_sim_time, + 'autostart': autostart} + + configured_params = RewrittenYaml( + source_file=params_file, + root_key=namespace, + 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_bringup', + executable='loc_and_nav', + output='screen', + parameters=[configured_params, + {'use_sim_time': use_sim_time}, + {'autostart': autostart}], + remappings=remappings) + ]) \ No newline at end of file diff --git a/nav2_bringup/src/loc_and_nav.cpp b/nav2_bringup/src/loc_and_nav.cpp new file mode 100644 index 00000000000..0ca32f147cf --- /dev/null +++ b/nav2_bringup/src/loc_and_nav.cpp @@ -0,0 +1,96 @@ +// 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" + +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); + + // localiztion nodes + std::vector localiztion_node_names; + auto map_node = std::make_shared(); + localiztion_node_names.push_back(map_node->get_name()); + auto amcl_node = std::make_shared(); + localiztion_node_names.push_back(amcl_node->get_name()); + // lifecycle manager of localiztion + auto loc_manager_options = rclcpp::NodeOptions(); + loc_manager_options.arguments( + {"--ros-args", "-r", std::string("__node:=") + "lifecycle_manager_localization", "--"}); + loc_manager_options.append_parameter_override("node_names", localiztion_node_names); + auto lifecycle_manager_localization_node = + std::make_shared(loc_manager_options); + + // 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(map_node)); + threads.push_back(create_spin_thread(amcl_node)); + threads.push_back(create_spin_thread(lifecycle_manager_localization_node)); + 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; +} \ No newline at end of file From 50ab2b343d83aa1af6f1d43ac05b252141aad10f Mon Sep 17 00:00:00 2001 From: zhenpeng ge Date: Wed, 8 Sep 2021 11:25:32 +0800 Subject: [PATCH 02/16] fix linters Signed-off-by: zhenpeng ge --- nav2_bringup/launch/manual_composed_bringup_launch.py | 4 ++-- nav2_bringup/src/loc_and_nav.cpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/nav2_bringup/launch/manual_composed_bringup_launch.py b/nav2_bringup/launch/manual_composed_bringup_launch.py index f64e0792e07..82c95887370 100644 --- a/nav2_bringup/launch/manual_composed_bringup_launch.py +++ b/nav2_bringup/launch/manual_composed_bringup_launch.py @@ -1,4 +1,4 @@ - # Copyright (c) 2018 Intel Corporation +# Copyright (c) 2018 Intel Corporation # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -88,4 +88,4 @@ def generate_launch_description(): {'use_sim_time': use_sim_time}, {'autostart': autostart}], remappings=remappings) - ]) \ No newline at end of file + ]) diff --git a/nav2_bringup/src/loc_and_nav.cpp b/nav2_bringup/src/loc_and_nav.cpp index 0ca32f147cf..90d45880f1d 100644 --- a/nav2_bringup/src/loc_and_nav.cpp +++ b/nav2_bringup/src/loc_and_nav.cpp @@ -93,4 +93,4 @@ int main(int argc, char ** argv) rclcpp::shutdown(); return 0; -} \ No newline at end of file +} From 88cda3b17a022daad822b52ec1c4d1ac233930fe Mon Sep 17 00:00:00 2001 From: zhenpeng ge Date: Mon, 13 Sep 2021 21:58:33 +0800 Subject: [PATCH 03/16] update Signed-off-by: zhenpeng ge --- nav2_bringup/CMakeLists.txt | 4 +- nav2_bringup/README.md | 19 ++-- ...p_launch.py => composed_bringup_launch.py} | 92 ++++++++++++------- nav2_bringup/launch/tb3_simulation_launch.py | 42 ++++++--- .../{loc_and_nav.cpp => composed_bringup.cpp} | 14 +++ 5 files changed, 121 insertions(+), 50 deletions(-) rename nav2_bringup/launch/{manual_composed_bringup_launch.py => composed_bringup_launch.py} (51%) rename nav2_bringup/src/{loc_and_nav.cpp => composed_bringup.cpp} (81%) diff --git a/nav2_bringup/CMakeLists.txt b/nav2_bringup/CMakeLists.txt index f2751829c2c..15214684e9a 100644 --- a/nav2_bringup/CMakeLists.txt +++ b/nav2_bringup/CMakeLists.txt @@ -15,9 +15,9 @@ find_package(nav2_lifecycle_manager REQUIRED) nav2_package() -set(executable_name loc_and_nav) +set(executable_name composed_bringup) add_executable(${executable_name} - src/loc_and_nav.cpp + src/composed_bringup.cpp ) set(dependencies diff --git a/nav2_bringup/README.md b/nav2_bringup/README.md index 27ef44db139..4f45b9929fa 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 bring up that can be modified for different maps/robots/hardware/worlds/etc. It is our expectation for an application specific thing you're mirroring `nav2_bringup` package and modifying it for your specific maps/robots/bringup needs. So 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 user, in other word, 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, and dynamic composition is more flexible than manual composition, but it could be 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/) @@ -53,17 +62,14 @@ ros2 launch nav2_bringup bringup_launch.py use_sim_time:=True autostart:=True \ map:= ``` -manual composed bringup +manually composed bringup ```bash source /opt/ros/dashing/setup.bash -ros2 launch nav2_bringup manual_composed_bringup_launch.py use_sim_time:=True autostart:=True \ +ros2 launch nav2_bringup composed_bringup_launch.py use_sim_time:=True autostart:=True \ map:= ``` -* composed bringup is based on [ROS2 Composition](https://docs.ros.org/en/galactic/Tutorials/Composition.html), which is useful for embedded systems users that need to make optimizations due to harsh resource constraints. -* some discussions about performance improvement could be found at https://discourse.ros.org/t/nav2-composition/22175 - ### Terminal 4: Run RViz with Nav2 config file ```bash @@ -92,6 +98,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/manual_composed_bringup_launch.py b/nav2_bringup/launch/composed_bringup_launch.py similarity index 51% rename from nav2_bringup/launch/manual_composed_bringup_launch.py rename to nav2_bringup/launch/composed_bringup_launch.py index 82c95887370..194d1edb228 100644 --- a/nav2_bringup/launch/manual_composed_bringup_launch.py +++ b/nav2_bringup/launch/composed_bringup_launch.py @@ -17,21 +17,25 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, SetEnvironmentVariable +from launch.actions import (DeclareLaunchArgument, GroupAction, SetEnvironmentVariable) +from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node from nav2_common.launch import RewrittenYaml +from launch_ros.actions import PushRosNamespace def generate_launch_description(): # Get the launch directory bringup_dir = get_package_share_directory('nav2_bringup') + # Create the launch configuration variables namespace = LaunchConfiguration('namespace') + use_namespace = LaunchConfiguration('use_namespace') map_yaml_file = LaunchConfiguration('map') use_sim_time = LaunchConfiguration('use_sim_time') - autostart = LaunchConfiguration('autostart') params_file = LaunchConfiguration('params_file') + autostart = LaunchConfiguration('autostart') # 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 @@ -44,9 +48,8 @@ def generate_launch_description(): # Create our own temporary YAML files that include substitutions param_substitutions = { - 'yaml_filename': map_yaml_file, 'use_sim_time': use_sim_time, - 'autostart': autostart} + 'yaml_filename': map_yaml_file} configured_params = RewrittenYaml( source_file=params_file, @@ -54,38 +57,65 @@ 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'), - + stdout_linebuf_envvar = SetEnvironmentVariable( + 'RCUTILS_LOGGING_BUFFERED_STREAM', '1') + + declare_namespace_cmd = DeclareLaunchArgument( + 'namespace', + default_value='', + description='Top-level namespace') + + declare_use_namespace_cmd = DeclareLaunchArgument( + 'use_namespace', + default_value='false', + description='Whether to apply a namespace to the navigation stack') + + 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') + + bringup_cmd_group = GroupAction([ + PushRosNamespace( + condition=IfCondition(use_namespace), + namespace=namespace), Node( package='nav2_bringup', - executable='loc_and_nav', + executable='composed_bringup', output='screen', parameters=[configured_params, {'use_sim_time': use_sim_time}, {'autostart': autostart}], remappings=remappings) ]) + # 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_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) + + # Add the actions to launch all of the navigation nodes + ld.add_action(bringup_cmd_group) + + return ld diff --git a/nav2_bringup/launch/tb3_simulation_launch.py b/nav2_bringup/launch/tb3_simulation_launch.py index ec982c3da28..51878288bc8 100644 --- a/nav2_bringup/launch/tb3_simulation_launch.py +++ b/nav2_bringup/launch/tb3_simulation_launch.py @@ -19,7 +19,8 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription +from launch.actions import (DeclareLaunchArgument, ExecuteProcess, + IncludeLaunchDescription, GroupAction) from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression @@ -39,6 +40,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 +103,10 @@ def generate_launch_description(): 'autostart', default_value='true', description='Automatically startup the nav2 stack') + declare_use_composition_cmd = DeclareLaunchArgument( + 'use_composition', default_value='False', + description='Whether to use manually composed bringup') + declare_rviz_config_file_cmd = DeclareLaunchArgument( 'rviz_config_file', default_value=os.path.join( @@ -192,16 +198,29 @@ def generate_launch_description(): 'use_namespace': use_namespace, 'rviz_config': rviz_config_file}.items()) - bringup_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(launch_dir, 'bringup_launch.py')), - launch_arguments={'namespace': namespace, - 'use_namespace': use_namespace, - 'slam': slam, - 'map': map_yaml_file, - 'use_sim_time': use_sim_time, - 'params_file': params_file, - 'autostart': autostart}.items()) + bringup_cmd = GroupAction([ + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_dir, 'composed_bringup_launch.py')), + condition=IfCondition(use_composition), + launch_arguments={'namespace': namespace, + 'use_namespace': use_namespace, + 'map': map_yaml_file, + 'use_sim_time': use_sim_time, + 'params_file': params_file, + 'autostart': autostart}.items()), + IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_dir, 'bringup_launch.py')), + condition=IfCondition(PythonExpression(['not ', use_composition])), + launch_arguments={'namespace': namespace, + 'use_namespace': use_namespace, + 'slam': slam, + 'map': map_yaml_file, + 'use_sim_time': use_sim_time, + 'params_file': params_file, + 'autostart': autostart}.items()), + ]) # Create the launch description and populate ld = LaunchDescription() @@ -214,6 +233,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/loc_and_nav.cpp b/nav2_bringup/src/composed_bringup.cpp similarity index 81% rename from nav2_bringup/src/loc_and_nav.cpp rename to nav2_bringup/src/composed_bringup.cpp index 90d45880f1d..d6a2eedf7a9 100644 --- a/nav2_bringup/src/loc_and_nav.cpp +++ b/nav2_bringup/src/composed_bringup.cpp @@ -26,6 +26,20 @@ #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 kind of under development currently, and QoS limitations (e.g. transient-local +// isn't supported) make some Nav2 Nodes couldn't enable rclcpp intra-process comms, you +// could find more details here https://github.com/ros2/rclcpp/issues/1753. +// +// this is an example for the default nav2 servers and bring up in localization mode, It is +// our expectation for an application specific thing you're mirroring nav2_bringup package +// and modifying it for your sp. maps/robots/bringup needs so this is an applied and working +// demonstration for the default system bringup ONLY. + template std::shared_ptr create_spin_thread(NodeT & node) { From 36641f262c0d649ccffc9305eb0910e6154ed9f5 Mon Sep 17 00:00:00 2001 From: zhenpeng ge Date: Tue, 21 Sep 2021 09:49:27 +0800 Subject: [PATCH 04/16] rm composed_bringup_launch.py Signed-off-by: zhenpeng ge --- nav2_bringup/launch/bringup_launch.py | 41 +++++- .../launch/composed_bringup_launch.py | 121 ------------------ nav2_bringup/launch/tb3_simulation_launch.py | 41 ++---- nav2_bringup/src/composed_bringup.cpp | 25 +--- 4 files changed, 56 insertions(+), 172 deletions(-) delete mode 100644 nav2_bringup/launch/composed_bringup_launch.py diff --git a/nav2_bringup/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py index 8d357ff5ad3..67f0ef9b9bf 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( @@ -100,14 +127,21 @@ def generate_launch_description(): 'params_file': params_file, 'use_lifecycle_mgr': 'false'}.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 +158,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/composed_bringup_launch.py b/nav2_bringup/launch/composed_bringup_launch.py deleted file mode 100644 index 194d1edb228..00000000000 --- a/nav2_bringup/launch/composed_bringup_launch.py +++ /dev/null @@ -1,121 +0,0 @@ -# Copyright (c) 2018 Intel Corporation -# -# 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. - -import os - -from ament_index_python.packages import get_package_share_directory - -from launch import LaunchDescription -from launch.actions import (DeclareLaunchArgument, GroupAction, SetEnvironmentVariable) -from launch.conditions import IfCondition -from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node -from nav2_common.launch import RewrittenYaml -from launch_ros.actions import PushRosNamespace - - -def generate_launch_description(): - # Get the launch directory - bringup_dir = get_package_share_directory('nav2_bringup') - - # Create the launch configuration variables - namespace = LaunchConfiguration('namespace') - use_namespace = LaunchConfiguration('use_namespace') - map_yaml_file = LaunchConfiguration('map') - use_sim_time = LaunchConfiguration('use_sim_time') - params_file = LaunchConfiguration('params_file') - autostart = LaunchConfiguration('autostart') - - # 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') - - declare_namespace_cmd = DeclareLaunchArgument( - 'namespace', - default_value='', - description='Top-level namespace') - - declare_use_namespace_cmd = DeclareLaunchArgument( - 'use_namespace', - default_value='false', - description='Whether to apply a namespace to the navigation stack') - - 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') - - bringup_cmd_group = GroupAction([ - PushRosNamespace( - condition=IfCondition(use_namespace), - namespace=namespace), - Node( - package='nav2_bringup', - executable='composed_bringup', - output='screen', - parameters=[configured_params, - {'use_sim_time': use_sim_time}, - {'autostart': autostart}], - remappings=remappings) - ]) - # 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_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) - - # Add the actions to launch all of the navigation nodes - ld.add_action(bringup_cmd_group) - - return ld diff --git a/nav2_bringup/launch/tb3_simulation_launch.py b/nav2_bringup/launch/tb3_simulation_launch.py index 51878288bc8..116a7151ab7 100644 --- a/nav2_bringup/launch/tb3_simulation_launch.py +++ b/nav2_bringup/launch/tb3_simulation_launch.py @@ -19,8 +19,7 @@ from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import (DeclareLaunchArgument, ExecuteProcess, - IncludeLaunchDescription, GroupAction) +from launch.actions import DeclareLaunchArgument, ExecuteProcess, IncludeLaunchDescription from launch.conditions import IfCondition from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PythonExpression @@ -104,8 +103,8 @@ def generate_launch_description(): description='Automatically startup the nav2 stack') declare_use_composition_cmd = DeclareLaunchArgument( - 'use_composition', default_value='False', - description='Whether to use manually composed bringup') + 'use_composition', default_value='True', + description='Whether to use composed bringup') declare_rviz_config_file_cmd = DeclareLaunchArgument( 'rviz_config_file', @@ -198,29 +197,17 @@ def generate_launch_description(): 'use_namespace': use_namespace, 'rviz_config': rviz_config_file}.items()) - bringup_cmd = GroupAction([ - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(launch_dir, 'composed_bringup_launch.py')), - condition=IfCondition(use_composition), - launch_arguments={'namespace': namespace, - 'use_namespace': use_namespace, - 'map': map_yaml_file, - 'use_sim_time': use_sim_time, - 'params_file': params_file, - 'autostart': autostart}.items()), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(launch_dir, 'bringup_launch.py')), - condition=IfCondition(PythonExpression(['not ', use_composition])), - launch_arguments={'namespace': namespace, - 'use_namespace': use_namespace, - 'slam': slam, - 'map': map_yaml_file, - 'use_sim_time': use_sim_time, - 'params_file': params_file, - 'autostart': autostart}.items()), - ]) + bringup_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(launch_dir, 'bringup_launch.py')), + launch_arguments={'namespace': namespace, + 'use_namespace': use_namespace, + 'slam': slam, + 'map': map_yaml_file, + 'use_sim_time': use_sim_time, + 'params_file': params_file, + 'autostart': autostart, + 'use_composition': use_composition}.items()) # Create the launch description and populate ld = LaunchDescription() diff --git a/nav2_bringup/src/composed_bringup.cpp b/nav2_bringup/src/composed_bringup.cpp index d6a2eedf7a9..df2772d5942 100644 --- a/nav2_bringup/src/composed_bringup.cpp +++ b/nav2_bringup/src/composed_bringup.cpp @@ -35,10 +35,10 @@ // isn't supported) make some Nav2 Nodes couldn't enable rclcpp intra-process comms, you // could find more details here https://github.com/ros2/rclcpp/issues/1753. // -// this is an example for the default nav2 servers and bring up in localization mode, It is -// our expectation for an application specific thing you're mirroring nav2_bringup package -// and modifying it for your sp. maps/robots/bringup needs so this is an applied and working -// demonstration for the default system bringup ONLY. +// this is an example of manual composition for the default nav2 servers, It is our expectation +// for an application specific thing you're mirroring nav2_bringup package and modifying it for +// your sp. maps/robots/bringup needs so this is an applied and working demonstration for the +// default system bringup ONLY. template std::shared_ptr create_spin_thread(NodeT & node) @@ -56,20 +56,6 @@ int main(int argc, char ** argv) { rclcpp::init(argc, argv); - // localiztion nodes - std::vector localiztion_node_names; - auto map_node = std::make_shared(); - localiztion_node_names.push_back(map_node->get_name()); - auto amcl_node = std::make_shared(); - localiztion_node_names.push_back(amcl_node->get_name()); - // lifecycle manager of localiztion - auto loc_manager_options = rclcpp::NodeOptions(); - loc_manager_options.arguments( - {"--ros-args", "-r", std::string("__node:=") + "lifecycle_manager_localization", "--"}); - loc_manager_options.append_parameter_override("node_names", localiztion_node_names); - auto lifecycle_manager_localization_node = - std::make_shared(loc_manager_options); - // navigation nodes std::vector navigation_node_names; auto controller_node = std::make_shared(); @@ -92,9 +78,6 @@ int main(int argc, char ** argv) // spin threads std::vector> threads; - threads.push_back(create_spin_thread(map_node)); - threads.push_back(create_spin_thread(amcl_node)); - threads.push_back(create_spin_thread(lifecycle_manager_localization_node)); threads.push_back(create_spin_thread(controller_node)); threads.push_back(create_spin_thread(planner_node)); threads.push_back(create_spin_thread(recoveries_node)); From 04c6d1853e82bb6f4fb1ab1f64726e8d4d24c8b2 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 28 Sep 2021 10:44:28 -0700 Subject: [PATCH 05/16] Update nav2_bringup/src/composed_bringup.cpp --- nav2_bringup/src/composed_bringup.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_bringup/src/composed_bringup.cpp b/nav2_bringup/src/composed_bringup.cpp index df2772d5942..318e304a06a 100644 --- a/nav2_bringup/src/composed_bringup.cpp +++ b/nav2_bringup/src/composed_bringup.cpp @@ -35,7 +35,7 @@ // isn't supported) make some Nav2 Nodes couldn't enable rclcpp 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 +// This is an example of manual composition for the default nav2 servers, It is our expectation // for an application specific thing you're mirroring nav2_bringup package and modifying it for // your sp. maps/robots/bringup needs so this is an applied and working demonstration for the // default system bringup ONLY. From 3a0491df3975a5b1267a188ddf9f3ad2a88c1b4d Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 28 Sep 2021 10:44:34 -0700 Subject: [PATCH 06/16] Update nav2_bringup/src/composed_bringup.cpp --- nav2_bringup/src/composed_bringup.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_bringup/src/composed_bringup.cpp b/nav2_bringup/src/composed_bringup.cpp index 318e304a06a..b87660fa55c 100644 --- a/nav2_bringup/src/composed_bringup.cpp +++ b/nav2_bringup/src/composed_bringup.cpp @@ -37,7 +37,7 @@ // // This is an example of manual composition for the default nav2 servers, It is our expectation // for an application specific thing you're mirroring nav2_bringup package and modifying it for -// your sp. maps/robots/bringup needs so this is an applied and working demonstration for the +// your sp. maps/robots/bringup needs. This is an applied and working demonstration for the // default system bringup ONLY. template From fdc73349148ad02cdac4d03841e952a14b3fb969 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 28 Sep 2021 10:44:40 -0700 Subject: [PATCH 07/16] Update nav2_bringup/src/composed_bringup.cpp --- nav2_bringup/src/composed_bringup.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_bringup/src/composed_bringup.cpp b/nav2_bringup/src/composed_bringup.cpp index b87660fa55c..edb5b7f5e4b 100644 --- a/nav2_bringup/src/composed_bringup.cpp +++ b/nav2_bringup/src/composed_bringup.cpp @@ -36,7 +36,7 @@ // 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 specific thing you're mirroring nav2_bringup package and modifying it for +// 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. From fde1f0ebc8a31f4252243a2f7210b3c84f5e5f61 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 28 Sep 2021 10:44:49 -0700 Subject: [PATCH 08/16] Update nav2_bringup/src/composed_bringup.cpp --- nav2_bringup/src/composed_bringup.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_bringup/src/composed_bringup.cpp b/nav2_bringup/src/composed_bringup.cpp index edb5b7f5e4b..386a7a4f7b1 100644 --- a/nav2_bringup/src/composed_bringup.cpp +++ b/nav2_bringup/src/composed_bringup.cpp @@ -31,7 +31,7 @@ // 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 kind of under development currently, and QoS limitations (e.g. transient-local +// it's still under development currently, and QoS limitations (e.g. transient-local // isn't supported) make some Nav2 Nodes couldn't enable rclcpp intra-process comms, you // could find more details here https://github.com/ros2/rclcpp/issues/1753. // From 2ce2f43630b1abdda6bd880c7dcb775673e8eb57 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 28 Sep 2021 10:44:58 -0700 Subject: [PATCH 09/16] Update nav2_bringup/src/composed_bringup.cpp --- nav2_bringup/src/composed_bringup.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_bringup/src/composed_bringup.cpp b/nav2_bringup/src/composed_bringup.cpp index 386a7a4f7b1..31d25704f2e 100644 --- a/nav2_bringup/src/composed_bringup.cpp +++ b/nav2_bringup/src/composed_bringup.cpp @@ -28,7 +28,7 @@ // 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. +// 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 From 9424bc415aeea50f5442f79e274ffdaf57b7cf13 Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 28 Sep 2021 10:45:04 -0700 Subject: [PATCH 10/16] Update nav2_bringup/src/composed_bringup.cpp --- nav2_bringup/src/composed_bringup.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_bringup/src/composed_bringup.cpp b/nav2_bringup/src/composed_bringup.cpp index 31d25704f2e..cf4103e9c38 100644 --- a/nav2_bringup/src/composed_bringup.cpp +++ b/nav2_bringup/src/composed_bringup.cpp @@ -32,7 +32,7 @@ // // 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 couldn't enable rclcpp intra-process comms, you +// 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 From 03bf6ac4449d575d96937991ade91dc52801eb4b Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 28 Sep 2021 10:45:13 -0700 Subject: [PATCH 11/16] Update nav2_bringup/README.md --- nav2_bringup/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_bringup/README.md b/nav2_bringup/README.md index 4f45b9929fa..b06d11dca96 100644 --- a/nav2_bringup/README.md +++ b/nav2_bringup/README.md @@ -9,7 +9,7 @@ Usual robot stacks will have a `_nav` package with config/bringup fi Composed bringup (based on [ROS2 Composition](https://docs.ros.org/en/galactic/Tutorials/Composition.html) ) is optional for user, in other word, 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, and dynamic composition is more flexible than manual composition, but it could be applied in nav2 due to various issues, you could find more details here: https://github.com/ros-planning/navigation2/issues/2147. +* 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/) From 8f8069b02ab073c05332856b0016e8fbfddf54de Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 28 Sep 2021 10:45:19 -0700 Subject: [PATCH 12/16] Update nav2_bringup/README.md --- nav2_bringup/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_bringup/README.md b/nav2_bringup/README.md index b06d11dca96..c93fa22c15b 100644 --- a/nav2_bringup/README.md +++ b/nav2_bringup/README.md @@ -8,7 +8,7 @@ Usual robot stacks will have a `_nav` package with config/bringup fi Composed bringup (based on [ROS2 Composition](https://docs.ros.org/en/galactic/Tutorials/Composition.html) ) is optional for user, in other word, 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. +* 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: From 894fd9fdd7e229b447f1d7a4122e1384ac9b107b Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 28 Sep 2021 10:45:24 -0700 Subject: [PATCH 13/16] Update nav2_bringup/README.md --- nav2_bringup/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_bringup/README.md b/nav2_bringup/README.md index c93fa22c15b..d7a485c4ba9 100644 --- a/nav2_bringup/README.md +++ b/nav2_bringup/README.md @@ -6,7 +6,7 @@ This is a very flexible example for nav2 bring up that can be modified for diffe 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 user, in other word, 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. +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. From 1adac5ee019c6a5b7c60f589081fe4db247ba90e Mon Sep 17 00:00:00 2001 From: Steve Macenski Date: Tue, 28 Sep 2021 10:45:29 -0700 Subject: [PATCH 14/16] Update nav2_bringup/README.md --- nav2_bringup/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nav2_bringup/README.md b/nav2_bringup/README.md index d7a485c4ba9..c56925fb123 100644 --- a/nav2_bringup/README.md +++ b/nav2_bringup/README.md @@ -2,7 +2,7 @@ The `nav2_bringup` package is an example bringup system for Nav2 applications. -This is a very flexible example for nav2 bring up that can be modified for different maps/robots/hardware/worlds/etc. It is our expectation for an application specific thing you're mirroring `nav2_bringup` package and modifying it for your specific maps/robots/bringup needs. So this is an applied and working demonstration for the default system bringup with many options that can be easily modified. +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. From b57b17ae5576640cde50be877bcb38639b3e6e94 Mon Sep 17 00:00:00 2001 From: zhenpeng ge Date: Wed, 29 Sep 2021 09:10:56 +0800 Subject: [PATCH 15/16] update nav2_bringup/README.md Signed-off-by: zhenpeng ge --- nav2_bringup/README.md | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/nav2_bringup/README.md b/nav2_bringup/README.md index c56925fb123..b2fd44434da 100644 --- a/nav2_bringup/README.md +++ b/nav2_bringup/README.md @@ -54,21 +54,22 @@ ros2 launch turtlebot3_bringup turtlebot3_state_publisher.launch.py use_sim_time ### Terminal 3: Launch Nav2 -normal bringup +Normal 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:=False\ map:= ``` -manually composed bringup +Manually Composed Bringup ```bash source /opt/ros/dashing/setup.bash -ros2 launch nav2_bringup composed_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 From 7e6993b586918077321283ef0f34f49c49fff4a7 Mon Sep 17 00:00:00 2001 From: zhenpeng ge Date: Thu, 30 Sep 2021 09:57:27 +0800 Subject: [PATCH 16/16] remove unused use_lifecycle_mgr Signed-off-by: zhenpeng ge --- nav2_bringup/launch/bringup_launch.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/nav2_bringup/launch/bringup_launch.py b/nav2_bringup/launch/bringup_launch.py index 67f0ef9b9bf..938ad0cc353 100644 --- a/nav2_bringup/launch/bringup_launch.py +++ b/nav2_bringup/launch/bringup_launch.py @@ -124,8 +124,7 @@ 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),