Merge pull request #1464 from SteveMacenski/simplify-bringup

Simplify bringup

nav2_amcl/src/amcl_node.cpp

@@ -1079,7 +1079,6 @@ AmclNode::handleMapMessage(const nav_msgs::msg::OccupancyGrid & msg)
 #if NEW_UNIFORM_SAMPLING
   createFreeSpaceVector();
 #endif
-
 }
 
 void

nav2_bringup/bringup/launch/nav2_bringup_launch.py

@@ -53,7 +53,6 @@ def generate_launch_description():
         default_value='false',
         description='Whether to apply a namespace to the navigation stack')
 
-    # Declare the launch arguments
     declare_map_yaml_cmd = DeclareLaunchArgument(
         'map',
         description='Full path to map yaml file to load')
@@ -86,7 +85,8 @@ def generate_launch_description():
             namespace=namespace),
 
         IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(os.path.join(launch_dir, 'nav2_localization_launch.py')),
+            PythonLaunchDescriptionSource(os.path.join(launch_dir,
+                                                       'nav2_localization_launch.py')),
             launch_arguments={'namespace': namespace,
                               'map': map_yaml_file,
                               'use_sim_time': use_sim_time,
@@ -103,21 +103,6 @@ def generate_launch_description():
                               'bt_xml_file': bt_xml_file,
                               'use_lifecycle_mgr': 'false',
                               'map_subscribe_transient_local': 'true'}.items()),
-
-        Node(
-            package='nav2_lifecycle_manager',
-            node_executable='lifecycle_manager',
-            node_name='lifecycle_manager',
-            output='screen',
-            parameters=[{'use_sim_time': use_sim_time},
-                        {'autostart': autostart},
-                        {'node_names': ['map_server',
-                                        'amcl',
-                                        'controller_server',
-                                        'planner_server',
-                                        'recoveries_server',
-                                        'bt_navigator',
-                                        'waypoint_follower']}]),
     ])
 
     # Create the launch description and populate

nav2_bringup/bringup/launch/nav2_localization_launch.py

@@ -33,7 +33,9 @@ def generate_launch_description():
     use_sim_time = LaunchConfiguration('use_sim_time')
     autostart = LaunchConfiguration('autostart')
     params_file = LaunchConfiguration('params_file')
-    use_lifecycle_mgr = LaunchConfiguration('use_lifecycle_mgr')
+    use_remappings = LaunchConfiguration('use_remappings')
+    lifecycle_nodes = ['map_server', 'amcl']
+
 
     # 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
@@ -82,8 +84,8 @@ def generate_launch_description():
             description='Full path to the ROS2 parameters file to use'),
 
         DeclareLaunchArgument(
-            'use_lifecycle_mgr', default_value='true',
-            description='Whether to launch the lifecycle manager'),
+            'use_remappings', default_value='false',
+            description='Arguments to pass to all nodes launched by the file'),
 
         Node(
             package='nav2_map_server',
@@ -102,12 +104,11 @@ def generate_launch_description():
             remappings=remappings),
 
         Node(
-            condition=IfCondition(use_lifecycle_mgr),
             package='nav2_lifecycle_manager',
             node_executable='lifecycle_manager',
             node_name='lifecycle_manager_localization',
             output='screen',
             parameters=[{'use_sim_time': use_sim_time},
                         {'autostart': autostart},
-                        {'node_names': ['map_server', 'amcl']}])
+                        {'node_names': lifecycle_nodes}])
     ])

nav2_bringup/bringup/launch/nav2_navigation_launch.py

@@ -33,9 +33,15 @@ def generate_launch_description():
     autostart = LaunchConfiguration('autostart')
     params_file = LaunchConfiguration('params_file')
     bt_xml_file = LaunchConfiguration('bt_xml_file')
-    use_lifecycle_mgr = LaunchConfiguration('use_lifecycle_mgr')
+    use_remappings = LaunchConfiguration('use_remappings')
     map_subscribe_transient_local = LaunchConfiguration('map_subscribe_transient_local')
 
+    lifecycle_nodes = ['controller_server',
+                       'planner_server',
+                       'recoveries_server',
+                       'bt_navigator',
+                       'waypoint_follower']
+
     # 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
@@ -87,8 +93,8 @@ def generate_launch_description():
             description='Full path to the behavior tree xml file to use'),
 
         DeclareLaunchArgument(
-            'use_lifecycle_mgr', default_value='true',
-            description='Whether to launch the lifecycle manager'),
+            'use_remappings', default_value='false',
+            description='Arguments to pass to all nodes launched by the file'),
 
         DeclareLaunchArgument(
             'map_subscribe_transient_local', default_value='false',
@@ -134,17 +140,12 @@ def generate_launch_description():
             remappings=remappings),
 
         Node(
-            condition=IfCondition(use_lifecycle_mgr),
             package='nav2_lifecycle_manager',
             node_executable='lifecycle_manager',
             node_name='lifecycle_manager_navigation',
             output='screen',
             parameters=[{'use_sim_time': use_sim_time},
                         {'autostart': autostart},
-                        {'node_names': ['controller_server',
-                                        'planner_server',
-                                        'recoveries_server',
-                                        'bt_navigator',
-                                        'waypoint_follower']}]),
+                        {'node_names': lifecycle_nodes}]),
 
     ])

nav2_bringup/bringup/launch/nav2_tb3_simulation_launch.py

@@ -91,7 +91,7 @@ def generate_launch_description():
         description='Full path to the behavior tree xml file to use')
 
     declare_autostart_cmd = DeclareLaunchArgument(
-        'autostart', default_value='false',
+        'autostart', default_value='true',
         description='Automatically startup the nav2 stack')
 
     declare_rviz_config_file_cmd = DeclareLaunchArgument(

nav2_bt_navigator/src/bt_navigator.cpp

@@ -34,8 +34,7 @@ BtNavigator::BtNavigator()
   // Declare this node's parameters
   declare_parameter("bt_xml_filename");
 
-  declare_parameter("plugin_lib_names",
-    rclcpp::ParameterValue(std::vector<std::string>({"nav2_behavior_tree_nodes"})));
+  declare_parameter("plugin_lib_names");
 }
 
 BtNavigator::~BtNavigator()
@@ -78,7 +77,7 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
     "NavigateToPose", std::bind(&BtNavigator::navigateToPose, this), false);
 
   // Get the libraries to pull plugins from
-  get_parameter("plugin_lib_names", plugin_lib_names_);
+  plugin_lib_names_ = get_parameter("plugin_lib_names").as_string_array();
 
   // Create the class that registers our custom nodes and executes the BT
   bt_ = std::make_unique<nav2_behavior_tree::BehaviorTreeEngine>(plugin_lib_names_);

nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager_client.hpp

@@ -45,7 +45,7 @@ public:
   /**
    * @brief A constructor for LifeCycleMangerClient
    */
-  LifecycleManagerClient();
+  LifecycleManagerClient(const std::string & name);
 
   // Client-side interface to the Nav2 lifecycle manager
   /**
@@ -110,8 +110,8 @@ protected:
 
   rclcpp::Client<ManageLifecycleNodes>::SharedPtr manager_client_;
   rclcpp::Client<std_srvs::srv::Trigger>::SharedPtr is_active_client_;
-  std::string manage_service_name_{"lifecycle_manager/manage_nodes"};
-  std::string active_service_name_{"lifecycle_manager/is_active"};
+  std::string manage_service_name_;
+  std::string active_service_name_;
 
   using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped;
 

nav2_lifecycle_manager/src/lifecycle_manager.cpp

@@ -36,27 +36,22 @@ LifecycleManager::LifecycleManager()
 {
   RCLCPP_INFO(get_logger(), "Creating");
 
-  // The default set of node names for the nav2 stack
-  std::vector<std::string> default_node_names{"map_server", "amcl",
-    "planner_server", "controller_server",
-    "recoveries_server", "bt_navigator", "waypoint_follower"};
-
   // The list of names is parameterized, allowing this module to be used with a different set
   // of nodes
-  declare_parameter("node_names", rclcpp::ParameterValue(default_node_names));
+  declare_parameter("node_names");
   declare_parameter("autostart", rclcpp::ParameterValue(false));
 
-  get_parameter("node_names", node_names_);
+  node_names_ = get_parameter("node_names").as_string_array();
   get_parameter("autostart", autostart_);
 
-  manager_srv_ = create_service<ManageLifecycleNodes>("lifecycle_manager/manage_nodes",
+  manager_srv_ = create_service<ManageLifecycleNodes>(get_name() + std::string("/manage_nodes"),
       std::bind(&LifecycleManager::managerCallback, this, _1, _2, _3));
 
-  is_active_srv_ = create_service<std_srvs::srv::Trigger>("lifecycle_manager/is_active",
+  is_active_srv_ = create_service<std_srvs::srv::Trigger>(get_name() + std::string("/is_active"),
       std::bind(&LifecycleManager::isActiveCallback, this, _1, _2, _3));
 
   auto options = rclcpp::NodeOptions().arguments(
-    {"--ros-args", "-r", std::string("__node:=") + get_name() + "service_client", "--"});
+    {"--ros-args", "-r", std::string("__node:=") + get_name() + "_service_client", "--"});
   service_client_node_ = std::make_shared<rclcpp::Node>("_", options);
 
   transition_state_map_[Transition::TRANSITION_CONFIGURE] = State::PRIMARY_STATE_INACTIVE;
@@ -190,7 +185,7 @@ LifecycleManager::startup()
   if (!changeStateForAllNodes(Transition::TRANSITION_CONFIGURE) ||
     !changeStateForAllNodes(Transition::TRANSITION_ACTIVATE))
   {
-    RCLCPP_ERROR(get_logger(), "Failed to bring up nodes: aborting bringup");
+    RCLCPP_ERROR(get_logger(), "Failed to bring up all requested nodes. Aborting bringup.");
     return false;
   }
   message("Managed nodes are active");

nav2_lifecycle_manager/src/lifecycle_manager_client.cpp

@@ -16,6 +16,7 @@
 
 #include <cmath>
 #include <memory>
+#include <string>
 
 #include "tf2_geometry_msgs/tf2_geometry_msgs.h"
 #include "nav2_util/geometry_utils.hpp"
@@ -24,10 +25,13 @@ namespace nav2_lifecycle_manager
 {
 using nav2_util::geometry_utils::orientationAroundZAxis;
 
-LifecycleManagerClient::LifecycleManagerClient()
+LifecycleManagerClient::LifecycleManagerClient(const std::string & name)
 {
+  manage_service_name_ = name + std::string("/manage_nodes");
+  active_service_name_ = name + std::string("/is_active");
+
   // Create the node to use for all of the service clients
-  node_ = std::make_shared<rclcpp::Node>("lifecycle_manager_client_service_client");
+  node_ = std::make_shared<rclcpp::Node>(name + "_service_client");
 
   // Create the service clients
   manager_client_ = node_->create_client<ManageLifecycleNodes>(manage_service_name_);

nav2_map_server/src/map_server.cpp

@@ -34,7 +34,7 @@ MapServer::MapServer()
   RCLCPP_INFO(get_logger(), "Creating");
 
   // Declare the node parameters
-  declare_parameter("yaml_filename", rclcpp::ParameterValue(std::string("map.yaml")));
+  declare_parameter("yaml_filename");
 }
 
 MapServer::~MapServer()
@@ -48,8 +48,7 @@ MapServer::on_configure(const rclcpp_lifecycle::State & state)
   RCLCPP_INFO(get_logger(), "Configuring");
 
   // Get the name of the YAML file to use
-  std::string yaml_filename;
-  get_parameter("yaml_filename", yaml_filename);
+  std::string yaml_filename = get_parameter("yaml_filename").as_string();
 
   // Make sure that there's a valid file there and open it up
   std::ifstream fin(yaml_filename.c_str());

nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp

@@ -99,7 +99,8 @@ private:
   WaypointFollowerGoalHandle::SharedPtr waypoint_follower_goal_handle_;
 
   // The client used to control the nav2 stack
-  nav2_lifecycle_manager::LifecycleManagerClient client_;
+  nav2_lifecycle_manager::LifecycleManagerClient client_nav_;
+  nav2_lifecycle_manager::LifecycleManagerClient client_loc_;
 
   QPushButton * start_reset_button_{nullptr};
   QPushButton * pause_resume_button_{nullptr};
@@ -146,17 +147,34 @@ class InitialThread : public QThread
 public:
   using SystemStatus = nav2_lifecycle_manager::SystemStatus;
 
-  explicit InitialThread(nav2_lifecycle_manager::LifecycleManagerClient & client)
-  : client_(client)
+  explicit InitialThread(
+    nav2_lifecycle_manager::LifecycleManagerClient & client_nav,
+    nav2_lifecycle_manager::LifecycleManagerClient & client_loc)
+  : client_nav_(client_nav), client_loc_(client_loc)
   {}
 
   void run() override
   {
-    SystemStatus status = SystemStatus::TIMEOUT;
-    while (status == SystemStatus::TIMEOUT) {
-      status = client_.is_active(std::chrono::seconds(1));
+    SystemStatus status_nav = SystemStatus::TIMEOUT;
+    SystemStatus status_loc = SystemStatus::TIMEOUT;
+
+    while (status_nav == SystemStatus::TIMEOUT) {
+      if (status_nav == SystemStatus::TIMEOUT) {
+        status_nav = client_nav_.is_active(std::chrono::seconds(1));
+      }
+    }
+
+    // try to communicate twice, might not actually be up if in SLAM mode
+    bool tried_loc_bringup_once = false;
+    while (status_loc == SystemStatus::TIMEOUT) {
+      status_loc = client_loc_.is_active(std::chrono::seconds(1));
+      if (tried_loc_bringup_once) {
+        break;
+      }
+      tried_loc_bringup_once = true;
     }
-    if (status == SystemStatus::ACTIVE) {
+
+    if (status_nav == SystemStatus::ACTIVE) {
       emit activeSystem();
     } else {
       emit inactiveSystem();
@@ -168,7 +186,8 @@ signals:
   void inactiveSystem();
 
 private:
-  nav2_lifecycle_manager::LifecycleManagerClient client_;
+  nav2_lifecycle_manager::LifecycleManagerClient client_nav_;
+  nav2_lifecycle_manager::LifecycleManagerClient client_loc_;
 };
 
 }  // namespace nav2_rviz_plugins

nav2_rviz_plugins/src/nav2_panel.cpp

@@ -33,7 +33,8 @@ using nav2_util::geometry_utils::orientationAroundZAxis;
 GoalPoseUpdater GoalUpdater;
 
 Nav2Panel::Nav2Panel(QWidget * parent)
-: Panel(parent)
+: Panel(parent), client_nav_("lifecycle_manager_navigation"),
+  client_loc_("lifecycle_manager_localization")
 {
   // Create the control button and its tooltip
 
@@ -181,7 +182,7 @@ Nav2Panel::Nav2Panel(QWidget * parent)
   runningTransition->setTargetState(idle_);
   running_->addTransition(runningTransition);
 
-  initial_thread_ = new InitialThread(client_);
+  initial_thread_ = new InitialThread(client_nav_, client_loc_);
   connect(initial_thread_, &InitialThread::finished, initial_thread_, &QObject::deleteLater);
 
   QSignalTransition * activeSignal = new QSignalTransition(initial_thread_,
@@ -261,32 +262,45 @@ Nav2Panel::startThread()
 void
 Nav2Panel::onPause()
 {
-  QFuture<void> future =
-    QtConcurrent::run(std::bind(&nav2_lifecycle_manager::LifecycleManagerClient::pause, &client_));
+  QFuture<void> futureNav =
+    QtConcurrent::run(std::bind(&nav2_lifecycle_manager::LifecycleManagerClient::pause,
+      &client_nav_));
+  QFuture<void> futureLoc =
+    QtConcurrent::run(std::bind(&nav2_lifecycle_manager::LifecycleManagerClient::pause,
+      &client_loc_));
 }
 
 void
 Nav2Panel::onResume()
 {
-  QFuture<void> future =
-    QtConcurrent::run(std::bind(&nav2_lifecycle_manager::LifecycleManagerClient::resume, &client_));
+  QFuture<void> futureNav =
+    QtConcurrent::run(std::bind(&nav2_lifecycle_manager::LifecycleManagerClient::resume,
+      &client_nav_));
+  QFuture<void> futureLoc =
+    QtConcurrent::run(std::bind(&nav2_lifecycle_manager::LifecycleManagerClient::resume,
+      &client_loc_));
 }
 
 void
 Nav2Panel::onStartup()
 {
-  QFuture<void> future =
+  QFuture<void> futureNav =
+    QtConcurrent::run(std::bind(&nav2_lifecycle_manager::LifecycleManagerClient::startup,
+      &client_nav_));
+  QFuture<void> futureLoc =
     QtConcurrent::run(std::bind(&nav2_lifecycle_manager::LifecycleManagerClient::startup,
-      &client_));
+      &client_loc_));
 }
 
 void
 Nav2Panel::onShutdown()
 {
-  QFuture<void> future =
+  QFuture<void> futureNav =
     QtConcurrent::run(std::bind(&nav2_lifecycle_manager::LifecycleManagerClient::reset,
-      &client_));
-
+      &client_nav_));
+  QFuture<void> futureLoc =
+    QtConcurrent::run(std::bind(&nav2_lifecycle_manager::LifecycleManagerClient::reset,
+      &client_loc_));
   timer_.stop();
 }
 

nav2_system_tests/src/updown/test_updown.cpp

@@ -26,14 +26,16 @@ int main(int argc, char ** argv)
 {
   rclcpp::init(argc, argv);
   RCLCPP_INFO(rclcpp::get_logger("test_updown"), "Initializing test");
-  nav2_lifecycle_manager::LifecycleManagerClient client;
+  nav2_lifecycle_manager::LifecycleManagerClient client_nav("lifecycle_manager_navigation");
+  nav2_lifecycle_manager::LifecycleManagerClient client_loc("lifecycle_manager_localization");
   bool test_passed = true;
 
   // Wait for a few seconds to let all of the nodes come up
   std::this_thread::sleep_for(5s);
 
   // Start the nav2 system, bringing it to the ACTIVE state
-  client.startup();
+  client_nav.startup();
+  client_loc.startup();
 
   // Wait for a couple secs to make sure the nodes have processed all discovery
   // info before starting
@@ -42,7 +44,8 @@ int main(int argc, char ** argv)
 
   // The system should now be active
   int retries = 0;
-  while ((client.is_active() != nav2_lifecycle_manager::SystemStatus::ACTIVE) &&
+  while ((client_nav.is_active() != nav2_lifecycle_manager::SystemStatus::ACTIVE) &&
+    (client_loc.is_active() != nav2_lifecycle_manager::SystemStatus::ACTIVE) &&
     (retries < 10))
   {
     std::this_thread::sleep_for(2s);
@@ -55,7 +58,8 @@ int main(int argc, char ** argv)
   }
 
   // Shut down the nav2 system, bringing it to the FINALIZED state
-  client.shutdown();
+  client_nav.shutdown();
+  client_loc.shutdown();
 
   if (test_passed) {
     RCLCPP_INFO(rclcpp::get_logger("test_updown"),