From 88cda3b17a022daad822b52ec1c4d1ac233930fe Mon Sep 17 00:00:00 2001 From: zhenpeng ge Date: Mon, 13 Sep 2021 21:58:33 +0800 Subject: [PATCH] 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 f2751829c2..15214684e9 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 27ef44db13..4f45b9929f 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 82c9588737..194d1edb22 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 ec982c3da2..51878288bc 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 90d45880f1..d6a2eedf7a 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) {