Lifecycle manager with respawn option + adding respawn options in launch (#2910)

* updated launch files for respawn options

* adding respawn capabilities to lifecycle manager

* closer to working

* updates for get_state (still WIP)

* working lifecycle manager + respawn

* adding delay

* more context in description of launch configurationg

* adding lint exception

* Update config.yml

* fixing CI issues

Author: SteveMacenski

.circleci/config.yml

@@ -33,12 +33,12 @@ _commands:
         - restore_cache:
             name: Restore Cache << parameters.key >>
             keys:
-              - "<< parameters.key >>-v8\
+              - "<< parameters.key >>-v9\
                 -{{ arch }}\
                 -{{ .Branch }}\
                 -{{ .Environment.CIRCLE_PR_NUMBER }}\
                 -{{ checksum  \"<< parameters.workspace >>/lockfile.txt\" }}"
-              - "<< parameters.key >>-v8\
+              - "<< parameters.key >>-v9\
                 -{{ arch }}\
                 -main\
                 -<no value>\
@@ -58,7 +58,7 @@ _commands:
       steps:
         - save_cache:
             name: Save Cache << parameters.key >>
-            key: "<< parameters.key >>-v8\
+            key: "<< parameters.key >>-v9\
               -{{ arch }}\
               -{{ .Branch }}\
               -{{ .Environment.CIRCLE_PR_NUMBER }}\

nav2_bringup/launch/bringup_launch.py

@@ -41,6 +41,7 @@ def generate_launch_description():
     params_file = LaunchConfiguration('params_file')
     autostart = LaunchConfiguration('autostart')
     use_composition = LaunchConfiguration('use_composition')
+    use_respawn = LaunchConfiguration('use_respawn')
 
     # 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
@@ -102,6 +103,10 @@ def generate_launch_description():
         'use_composition', default_value='True',
         description='Whether to use composed bringup')
 
+    declare_use_respawn_cmd = DeclareLaunchArgument(
+        'use_respawn', default_value='False',
+        description='Whether to respawn if a node crashes. Applied when composition is disabled.')
+
     # Specify the actions
     bringup_cmd_group = GroupAction([
         PushRosNamespace(
@@ -123,6 +128,7 @@ def generate_launch_description():
             launch_arguments={'namespace': namespace,
                               'use_sim_time': use_sim_time,
                               'autostart': autostart,
+                              'use_respawn': use_respawn,
                               'params_file': params_file}.items()),
 
         IncludeLaunchDescription(
@@ -135,6 +141,7 @@ def generate_launch_description():
                               'autostart': autostart,
                               'params_file': params_file,
                               'use_composition': use_composition,
+                              'use_respawn': use_respawn,
                               'container_name': 'nav2_container'}.items()),
 
         IncludeLaunchDescription(
@@ -144,6 +151,7 @@ def generate_launch_description():
                               'autostart': autostart,
                               'params_file': params_file,
                               'use_composition': use_composition,
+                              'use_respawn': use_respawn,
                               'container_name': 'nav2_container'}.items()),
     ])
 
@@ -162,6 +170,7 @@ def generate_launch_description():
     ld.add_action(declare_params_file_cmd)
     ld.add_action(declare_autostart_cmd)
     ld.add_action(declare_use_composition_cmd)
+    ld.add_action(declare_use_respawn_cmd)
 
     # Add the actions to launch all of the navigation nodes
     ld.add_action(bringup_cmd_group)

nav2_bringup/launch/localization_launch.py

@@ -37,6 +37,7 @@ def generate_launch_description():
     params_file = LaunchConfiguration('params_file')
     use_composition = LaunchConfiguration('use_composition')
     container_name = LaunchConfiguration('container_name')
+    use_respawn = LaunchConfiguration('use_respawn')
 
     lifecycle_nodes = ['map_server', 'amcl']
 
@@ -94,6 +95,10 @@ def generate_launch_description():
         'container_name', default_value='nav2_container',
         description='the name of conatiner that nodes will load in if use composition')
 
+    declare_use_respawn_cmd = DeclareLaunchArgument(
+        'use_respawn', default_value='False',
+        description='Whether to respawn if a node crashes. Applied when composition is disabled.')
+
     load_nodes = GroupAction(
         condition=IfCondition(PythonExpression(['not ', use_composition])),
         actions=[
@@ -102,13 +107,17 @@ def generate_launch_description():
                 executable='map_server',
                 name='map_server',
                 output='screen',
+                respawn=use_respawn,
+                respawn_delay=2.0,
                 parameters=[configured_params],
                 remappings=remappings),
             Node(
                 package='nav2_amcl',
                 executable='amcl',
                 name='amcl',
                 output='screen',
+                respawn=use_respawn,
+                respawn_delay=2.0,
                 parameters=[configured_params],
                 remappings=remappings),
             Node(
@@ -162,6 +171,7 @@ def generate_launch_description():
     ld.add_action(declare_autostart_cmd)
     ld.add_action(declare_use_composition_cmd)
     ld.add_action(declare_container_name_cmd)
+    ld.add_action(declare_use_respawn_cmd)
 
     # Add the actions to launch all of the localiztion nodes
     ld.add_action(load_nodes)

nav2_bringup/launch/navigation_launch.py

@@ -36,6 +36,7 @@ def generate_launch_description():
     params_file = LaunchConfiguration('params_file')
     use_composition = LaunchConfiguration('use_composition')
     container_name = LaunchConfiguration('container_name')
+    use_respawn = LaunchConfiguration('use_respawn')
 
     lifecycle_nodes = ['controller_server',
                        'smoother_server',
@@ -94,48 +95,64 @@ def generate_launch_description():
         'container_name', default_value='nav2_container',
         description='the name of conatiner that nodes will load in if use composition')
 
+    declare_use_respawn_cmd = DeclareLaunchArgument(
+        'use_respawn', default_value='False',
+        description='Whether to respawn if a node crashes. Applied when composition is disabled.')
+
     load_nodes = GroupAction(
         condition=IfCondition(PythonExpression(['not ', use_composition])),
         actions=[
             Node(
                 package='nav2_controller',
                 executable='controller_server',
                 output='screen',
+                respawn=use_respawn,
+                respawn_delay=2.0,
                 parameters=[configured_params],
                 remappings=remappings),
             Node(
                 package='nav2_smoother',
                 executable='smoother_server',
                 name='smoother_server',
                 output='screen',
+                respawn=use_respawn,
+                respawn_delay=2.0,
                 parameters=[configured_params],
                 remappings=remappings),
             Node(
                 package='nav2_planner',
                 executable='planner_server',
                 name='planner_server',
                 output='screen',
+                respawn=use_respawn,
+                respawn_delay=2.0,
                 parameters=[configured_params],
                 remappings=remappings),
             Node(
                 package='nav2_behaviors',
                 executable='behavior_server',
                 name='behavior_server',
                 output='screen',
+                respawn=use_respawn,
+                respawn_delay=2.0,
                 parameters=[configured_params],
                 remappings=remappings),
             Node(
                 package='nav2_bt_navigator',
                 executable='bt_navigator',
                 name='bt_navigator',
                 output='screen',
+                respawn=use_respawn,
+                respawn_delay=2.0,
                 parameters=[configured_params],
                 remappings=remappings),
             Node(
                 package='nav2_waypoint_follower',
                 executable='waypoint_follower',
                 name='waypoint_follower',
                 output='screen',
+                respawn=use_respawn,
+                respawn_delay=2.0,
                 parameters=[configured_params],
                 remappings=remappings),
             Node(
@@ -212,6 +229,7 @@ def generate_launch_description():
     ld.add_action(declare_autostart_cmd)
     ld.add_action(declare_use_composition_cmd)
     ld.add_action(declare_container_name_cmd)
+    ld.add_action(declare_use_respawn_cmd)
 
     # Add the actions to launch all of the navigation nodes
     ld.add_action(load_nodes)

nav2_bringup/launch/slam_launch.py

@@ -31,6 +31,7 @@ def generate_launch_description():
     params_file = LaunchConfiguration('params_file')
     use_sim_time = LaunchConfiguration('use_sim_time')
     autostart = LaunchConfiguration('autostart')
+    use_respawn = LaunchConfiguration('use_respawn')
 
     # Variables
     lifecycle_nodes = ['map_saver']
@@ -70,12 +71,18 @@ def generate_launch_description():
         'autostart', default_value='True',
         description='Automatically startup the nav2 stack')
 
+    declare_use_respawn_cmd = DeclareLaunchArgument(
+        'use_respawn', default_value='False',
+        description='Whether to respawn if a node crashes. Applied when composition is disabled.')
+
     # Nodes launching commands
 
     start_map_saver_server_cmd = Node(
             package='nav2_map_server',
             executable='map_saver_server',
             output='screen',
+            respawn=use_respawn,
+            respawn_delay=2.0,
             parameters=[configured_params])
 
     start_lifecycle_manager_cmd = Node(
@@ -111,6 +118,7 @@ def generate_launch_description():
     ld.add_action(declare_params_file_cmd)
     ld.add_action(declare_use_sim_time_cmd)
     ld.add_action(declare_autostart_cmd)
+    ld.add_action(declare_use_respawn_cmd)
 
     # Running Map Saver Server
     ld.add_action(start_map_saver_server_cmd)

nav2_bringup/launch/tb3_simulation_launch.py

@@ -40,6 +40,7 @@ def generate_launch_description():
     params_file = LaunchConfiguration('params_file')
     autostart = LaunchConfiguration('autostart')
     use_composition = LaunchConfiguration('use_composition')
+    use_respawn = LaunchConfiguration('use_respawn')
 
     # Launch configuration variables specific to simulation
     rviz_config_file = LaunchConfiguration('rviz_config_file')
@@ -106,6 +107,10 @@ def generate_launch_description():
         'use_composition', default_value='True',
         description='Whether to use composed bringup')
 
+    declare_use_respawn_cmd = DeclareLaunchArgument(
+        'use_respawn', default_value='False',
+        description='Whether to respawn if a node crashes. Applied when composition is disabled.')
+
     declare_rviz_config_file_cmd = DeclareLaunchArgument(
         'rviz_config_file',
         default_value=os.path.join(
@@ -129,7 +134,7 @@ def generate_launch_description():
 
     declare_simulator_cmd = DeclareLaunchArgument(
         'headless',
-        default_value='False',
+        default_value='True',
         description='Whether to execute gzclient)')
 
     declare_world_cmd = DeclareLaunchArgument(
@@ -208,7 +213,8 @@ def generate_launch_description():
                           'use_sim_time': use_sim_time,
                           'params_file': params_file,
                           'autostart': autostart,
-                          'use_composition': use_composition}.items())
+                          'use_composition': use_composition,
+                          'use_respawn': use_respawn}.items())
 
     # Create the launch description and populate
     ld = LaunchDescription()
@@ -231,6 +237,7 @@ def generate_launch_description():
     ld.add_action(declare_world_cmd)
     ld.add_action(declare_robot_name_cmd)
     ld.add_action(declare_robot_sdf_cmd)
+    ld.add_action(declare_use_respawn_cmd)
 
     # Add any conditioned actions
     ld.add_action(start_gazebo_server_cmd)

nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp

@@ -1,4 +1,5 @@
 // Copyright (c) 2019 Intel Corporation
+// Copyright (c) 2022 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.
@@ -32,6 +33,7 @@
 
 namespace nav2_lifecycle_manager
 {
+using namespace std::chrono_literals;  // NOLINT
 
 using nav2_msgs::srv::ManageLifecycleNodes;
 /**
@@ -98,7 +100,7 @@ class LifecycleManager : public rclcpp::Node
    * @brief Reset all the managed nodes.
    * @return true or false
    */
-  bool reset();
+  bool reset(bool hard_reset = false);
   /**
    * @brief Pause all the managed nodes.
    * @return true or false
@@ -151,6 +153,13 @@ class LifecycleManager : public rclcpp::Node
    */
   void checkBondConnections();
 
+  // Support function for checking if bond connections come back after respawn
+  /**
+   * @ brief Support function for checking on bond connections
+   * will bring back the system if something goes from non-responsive to responsive
+   */
+  void checkBondRespawnConnection();
+
   /**
    * @brief For a node, transition to the new target state
    */
@@ -161,7 +170,7 @@ class LifecycleManager : public rclcpp::Node
   /**
    * @brief For each node in the map, transition to the new target state
    */
-  bool changeStateForAllNodes(std::uint8_t transition);
+  bool changeStateForAllNodes(std::uint8_t transition, bool hard_change = false);
 
   // Convenience function to highlight the output on the console
   /**
@@ -172,6 +181,7 @@ class LifecycleManager : public rclcpp::Node
   // Timer thread to look at bond connections
   rclcpp::TimerBase::SharedPtr init_timer_;
   rclcpp::TimerBase::SharedPtr bond_timer_;
+  rclcpp::TimerBase::SharedPtr bond_respawn_timer_;
   std::chrono::milliseconds bond_timeout_;
 
   // A map of all nodes to check bond connection
@@ -190,8 +200,12 @@ class LifecycleManager : public rclcpp::Node
 
   // Whether to automatically start up the system
   bool autostart_;
+  bool attempt_respawn_reconnection_;
 
   bool system_active_{false};
+
+  rclcpp::Time bond_respawn_start_time_{0};
+  rclcpp::Duration bond_respawn_max_duration_{10s};
 };
 
 }  // namespace nav2_lifecycle_manager