Support manual composed bringup for Nav2 applications (#2555)

* support manual composed bringup for Nav2 applications Signed-off-by: zhenpeng ge <zhenpeng.ge@qq.com> * fix linters Signed-off-by: zhenpeng ge <zhenpeng.ge@qq.com> * update Signed-off-by: zhenpeng ge <zhenpeng.ge@qq.com> * rm composed_bringup_launch.py Signed-off-by: zhenpeng ge <zhenpeng.ge@qq.com> * 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 <zhenpeng.ge@qq.com> * remove unused use_lifecycle_mgr Signed-off-by: zhenpeng ge <zhenpeng.ge@qq.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com>
ros-navigation · Sep 30, 2021 · 232612f · 232612f
1 parent 406bbbe
commit 232612f
Show file tree

Hide file tree

Showing 5 changed files with 195 additions and 8 deletions.
diff --git 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
@@ -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 `<robot_name>_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:=<full/path/to/map.yaml>
+```
+
+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:=<full/path/to/map.yaml>
 ```
+* `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 <settings>
 Where `<settings>` can used to replace any of the default options, for example:
 
 ```
+use_composition:=<True or False>
 world:=<full/path/to/gazebo.world>
 map:=<full/path/to/map.yaml>
 rviz_config_file:=<full/path/to/rviz_config.rviz>

diff --git 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
@@ -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
@@ -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 <memory>
+#include <vector>
+#include <string>
+
+#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<typename NodeT>
+std::shared_ptr<std::thread> create_spin_thread(NodeT & node)
+{
+  return std::make_shared<std::thread>(
+    [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<std::string> navigation_node_names;
+  auto controller_node = std::make_shared<nav2_controller::ControllerServer>();
+  navigation_node_names.push_back(controller_node->get_name());
+  auto planner_node = std::make_shared<nav2_planner::PlannerServer>();
+  navigation_node_names.push_back(planner_node->get_name());
+  auto recoveries_node = std::make_shared<recovery_server::RecoveryServer>();
+  navigation_node_names.push_back(recoveries_node->get_name());
+  auto bt_navigator_node = std::make_shared<nav2_bt_navigator::BtNavigator>();
+  navigation_node_names.push_back(bt_navigator_node->get_name());
+  auto waypoint_follower_node = std::make_shared<nav2_waypoint_follower::WaypointFollower>();
+  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<nav2_lifecycle_manager::LifecycleManager>(nav_manager_options);
+
+  // spin threads
+  std::vector<std::shared_ptr<std::thread>> 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;
+}