Address code cov loss due to unclean shutdown (issue #3271) (#3377)

* Ensure that we break bonds in lifecycle node/manager prior to rcl_shutdown.

Partial fix for Issue #3271

Signed-off-by: mbryan <matthew.bryan@dell.com>

* Add unit tests for lifecycle_node rcl shutdown cb.

For Issue #3271.

Signed-off-by: mbryan <matthew.bryan@dell.com>

* Ensure planner_server shuts down cleanly on SIGINT.

Needed to ensure code coverage flushes. See Issue #3271.

Signed-off-by: mbryan <matthew.bryan@dell.com>

* Ensure costmap_2d_cloud does not seg fault on SIGINT.

One of several clean shutdown issues being address via Issue #3271.

Signed-off-by: mbryan <matthew.bryan@dell.com>

* Fix flake8 issues in some test files

Signed-off-by: mbryan <matthew.bryan@dell.com>

---------

Signed-off-by: mbryan <matthew.bryan@dell.com>
Co-authored-by: mbryan <matthew.bryan@dell.com>

nav2_bringup/launch/localization_launch.py

@@ -107,7 +107,7 @@ def generate_launch_description():
     load_nodes = GroupAction(
         condition=IfCondition(PythonExpression(['not ', use_composition])),
         actions=[
-            SetParameter("use_sim_time", use_sim_time),
+            SetParameter('use_sim_time', use_sim_time),
             Node(
                 condition=LaunchConfigurationEquals('map', ''),
                 package='nav2_map_server',
@@ -165,7 +165,7 @@ def generate_launch_description():
     load_composable_nodes = GroupAction(
         condition=IfCondition(use_composition),
         actions=[
-            SetParameter("use_sim_time", use_sim_time),
+            SetParameter('use_sim_time', use_sim_time),
             LoadComposableNodes(
                 target_container=container_name_full,
                 condition=LaunchConfigurationEquals('map', ''),

nav2_bringup/launch/navigation_launch.py

@@ -108,7 +108,7 @@ def generate_launch_description():
     load_nodes = GroupAction(
         condition=IfCondition(PythonExpression(['not ', use_composition])),
         actions=[
-            SetParameter("use_sim_time", use_sim_time),
+            SetParameter('use_sim_time', use_sim_time),
             Node(
                 package='nav2_controller',
                 executable='controller_server',
@@ -193,7 +193,7 @@ def generate_launch_description():
     load_composable_nodes = GroupAction(
         condition=IfCondition(use_composition),
         actions=[
-            SetParameter("use_sim_time", use_sim_time),
+            SetParameter('use_sim_time', use_sim_time),
             LoadComposableNodes(
                 target_container=container_name_full,
                 composable_node_descriptions=[

nav2_bringup/launch/slam_launch.py

@@ -17,7 +17,7 @@
 from ament_index_python.packages import get_package_share_directory
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction
+from launch.actions import DeclareLaunchArgument, GroupAction, IncludeLaunchDescription
 from launch.conditions import IfCondition, UnlessCondition
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch.substitutions import LaunchConfiguration
@@ -81,7 +81,7 @@ def generate_launch_description():
 
     start_map_server = GroupAction(
         actions=[
-            SetParameter("use_sim_time", use_sim_time),
+            SetParameter('use_sim_time', use_sim_time),
             Node(
                 package='nav2_map_server',
                 executable='map_saver_server',

nav2_controller/src/controller_server.cpp

@@ -19,6 +19,7 @@
 #include <utility>
 #include <limits>
 
+#include "lifecycle_msgs/msg/state.hpp"
 #include "nav2_core/controller_exceptions.hpp"
 #include "nav_2d_utils/conversions.hpp"
 #include "nav_2d_utils/tf_help.hpp"
@@ -246,7 +247,19 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
   for (it = controllers_.begin(); it != controllers_.end(); ++it) {
     it->second->deactivate();
   }
-  costmap_ros_->deactivate();
+
+  /*
+   * The costmap is also a lifecycle node, so it may have already fired on_deactivate
+   * via rcl preshutdown cb. Despite the rclcpp docs saying on_shutdown callbacks fire
+   * in the order added, the preshutdown callbacks clearly don't per se, due to using an
+   * unordered_set iteration. Once this issue is resolved, we can maybe make a stronger
+   * ordering assumption: https://github.com/ros2/rclcpp/issues/2096
+   */
+  if (costmap_ros_->get_current_state().id() ==
+    lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
+  {
+    costmap_ros_->deactivate();
+  }
 
   publishZeroVelocity();
   vel_publisher_->on_deactivate();
@@ -271,11 +284,16 @@ ControllerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
   controllers_.clear();
 
   goal_checkers_.clear();
-  costmap_ros_->cleanup();
+  if (costmap_ros_->get_current_state().id() ==
+    lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
+  {
+    costmap_ros_->cleanup();
+  }
 
   // Release any allocated resources
   action_server_.reset();
   odom_sub_.reset();
+  costmap_thread_.reset();
   vel_publisher_.reset();
   speed_limit_sub_.reset();
 

nav2_costmap_2d/src/costmap_2d_cloud.cpp

@@ -225,6 +225,11 @@ int main(int argc, char ** argv)
     "voxel_grid", rclcpp::SystemDefaultsQoS(), voxelCallback);
 
   rclcpp::spin(g_node->get_node_base_interface());
+
+  g_node.reset();
+  pub_marked.reset();
+  pub_unknown.reset();
+
   rclcpp::shutdown();
 
   return 0;

nav2_costmap_2d/src/costmap_2d_ros.cpp

@@ -299,19 +299,22 @@ Costmap2DROS::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
 
   dyn_params_handler.reset();
 
+  stop();
+
+  // Map thread stuff
+  map_update_thread_shutdown_ = true;
+
+  if (map_update_thread_->joinable()) {
+    map_update_thread_->join();
+  }
+
   footprint_pub_->on_deactivate();
   costmap_publisher_->on_deactivate();
 
   for (auto & layer_pub : layer_publishers_) {
     layer_pub->on_deactivate();
   }
 
-  stop();
-
-  // Map thread stuff
-  map_update_thread_shutdown_ = true;
-  map_update_thread_->join();
-
   return nav2_util::CallbackReturn::SUCCESS;
 }
 
@@ -575,18 +578,23 @@ void
 Costmap2DROS::stop()
 {
   stop_updates_ = true;
-  std::vector<std::shared_ptr<Layer>> * plugins = layered_costmap_->getPlugins();
-  std::vector<std::shared_ptr<Layer>> * filters = layered_costmap_->getFilters();
-  // unsubscribe from topics
-  for (std::vector<std::shared_ptr<Layer>>::iterator plugin = plugins->begin();
-    plugin != plugins->end(); ++plugin)
-  {
-    (*plugin)->deactivate();
-  }
-  for (std::vector<std::shared_ptr<Layer>>::iterator filter = filters->begin();
-    filter != filters->end(); ++filter)
-  {
-    (*filter)->deactivate();
+
+  // layered_costmap_ is set only if on_configure has been called
+  if (layered_costmap_) {
+    std::vector<std::shared_ptr<Layer>> * plugins = layered_costmap_->getPlugins();
+    std::vector<std::shared_ptr<Layer>> * filters = layered_costmap_->getFilters();
+
+    // unsubscribe from topics
+    for (std::vector<std::shared_ptr<Layer>>::iterator plugin = plugins->begin();
+      plugin != plugins->end(); ++plugin)
+    {
+      (*plugin)->deactivate();
+    }
+    for (std::vector<std::shared_ptr<Layer>>::iterator filter = filters->begin();
+      filter != filters->end(); ++filter)
+    {
+      (*filter)->deactivate();
+    }
   }
   initialized_ = false;
   stopped_ = true;

nav2_lifecycle_manager/include/nav2_lifecycle_manager/lifecycle_manager.hpp

@@ -114,6 +114,13 @@ class LifecycleManager : public rclcpp::Node
    */
   bool resume();
 
+  /**
+   * @brief Perform preshutdown activities before our Context is shutdown.
+   * Note that this is related to our Context's shutdown sequence, not the
+   * lifecycle node state machine or shutdown().
+   */
+  void onRclPreshutdown();
+
   // Support function for creating service clients
   /**
    * @brief Support function for creating service clients
@@ -186,6 +193,14 @@ class LifecycleManager : public rclcpp::Node
    */
   void CreateActiveDiagnostic(diagnostic_updater::DiagnosticStatusWrapper & stat);
 
+  /**
+   * Register our preshutdown callback for this Node's rcl Context.
+   * The callback fires before this Node's Context is shutdown.
+   * Note this is not directly related to the lifecycle state machine or the
+   * shutdown() instance function.
+   */
+  void registerRclPreshutdownCallback();
+
   // Timer thread to look at bond connections
   rclcpp::TimerBase::SharedPtr init_timer_;
   rclcpp::TimerBase::SharedPtr bond_timer_;

nav2_lifecycle_manager/src/lifecycle_manager.cpp

@@ -45,6 +45,8 @@ LifecycleManager::LifecycleManager(const rclcpp::NodeOptions & options)
   declare_parameter("bond_respawn_max_duration", 10.0);
   declare_parameter("attempt_respawn_reconnection", true);
 
+  registerRclPreshutdownCallback();
+
   node_names_ = get_parameter("node_names").as_string_array();
   get_parameter("autostart", autostart_);
   double bond_timeout_s;
@@ -378,6 +380,35 @@ LifecycleManager::destroyBondTimer()
   }
 }
 
+void
+LifecycleManager::onRclPreshutdown()
+{
+  RCLCPP_INFO(
+    get_logger(), "Running Nav2 LifecycleManager rcl preshutdown (%s)",
+    this->get_name());
+
+  destroyBondTimer();
+
+  /*
+   * Dropping the bond map is what we really need here, but we drop the others
+   * to prevent the bond map being used. Likewise, squash the service thread.
+   */
+  service_thread_.reset();
+  node_names_.clear();
+  node_map_.clear();
+  bond_map_.clear();
+}
+
+void
+LifecycleManager::registerRclPreshutdownCallback()
+{
+  rclcpp::Context::SharedPtr context = get_node_base_interface()->get_context();
+
+  context->add_pre_shutdown_callback(
+    std::bind(&LifecycleManager::onRclPreshutdown, this)
+  );
+}
+
 void
 LifecycleManager::checkBondConnections()
 {

nav2_planner/src/planner_server.cpp

@@ -25,6 +25,7 @@
 #include <utility>
 
 #include "builtin_interfaces/msg/duration.hpp"
+#include "lifecycle_msgs/msg/state.hpp"
 #include "nav2_util/costmap.hpp"
 #include "nav2_util/node_utils.hpp"
 #include "nav2_util/geometry_utils.hpp"
@@ -69,6 +70,10 @@ PlannerServer::PlannerServer(const rclcpp::NodeOptions & options)
 
 PlannerServer::~PlannerServer()
 {
+  /*
+   * Backstop ensuring this state is destroyed, even if deactivate/cleanup are
+   * never called.
+   */
   planners_.clear();
   costmap_thread_.reset();
 }
@@ -194,7 +199,19 @@ PlannerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
   action_server_pose_->deactivate();
   action_server_poses_->deactivate();
   plan_publisher_->on_deactivate();
-  costmap_ros_->deactivate();
+
+  /*
+   * The costmap is also a lifecycle node, so it may have already fired on_deactivate
+   * via rcl preshutdown cb. Despite the rclcpp docs saying on_shutdown callbacks fire
+   * in the order added, the preshutdown callbacks clearly don't per se, due to using an
+   * unordered_set iteration. Once this issue is resolved, we can maybe make a stronger
+   * ordering assumption: https://github.com/ros2/rclcpp/issues/2096
+   */
+  if (costmap_ros_->get_current_state().id() ==
+    lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
+  {
+    costmap_ros_->deactivate();
+  }
 
   PlannerMap::iterator it;
   for (it = planners_.begin(); it != planners_.end(); ++it) {
@@ -218,13 +235,24 @@ PlannerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
   action_server_poses_.reset();
   plan_publisher_.reset();
   tf_.reset();
-  costmap_ros_->cleanup();
+
+  /*
+   * Double check whether something else transitioned it to INACTIVE
+   * already, e.g. the rcl preshutdown callback.
+   */
+  if (costmap_ros_->get_current_state().id() ==
+    lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
+  {
+    costmap_ros_->cleanup();
+  }
 
   PlannerMap::iterator it;
   for (it = planners_.begin(); it != planners_.end(); ++it) {
     it->second->cleanup();
   }
+
   planners_.clear();
+  costmap_thread_.reset();
   costmap_ = nullptr;
   return nav2_util::CallbackReturn::SUCCESS;
 }

nav2_simple_commander/nav2_simple_commander/footprint_collision_checker.py

@@ -22,10 +22,10 @@
 """
 
 from math import cos, sin
-from nav2_simple_commander.line_iterator import LineIterator
+
+from geometry_msgs.msg import Point32, Polygon
 from nav2_simple_commander.costmap_2d import PyCostmap2D
-from geometry_msgs.msg import Polygon
-from geometry_msgs.msg import Point32
+from nav2_simple_commander.line_iterator import LineIterator
 
 NO_INFORMATION = 255
 LETHAL_OBSTACLE = 254
@@ -146,7 +146,7 @@ def worldToMapValidated(self, wx: float, wy: float):
         """
         if self.costmap_ is None:
             raise ValueError(
-                "Costmap not specified, use setCostmap to specify the costmap first")
+                'Costmap not specified, use setCostmap to specify the costmap first')
         return self.costmap_.worldToMapValidated(wx, wy)
 
     def pointCost(self, x: int, y: int):
@@ -165,7 +165,7 @@ def pointCost(self, x: int, y: int):
         """
         if self.costmap_ is None:
             raise ValueError(
-                "Costmap not specified, use setCostmap to specify the costmap first")
+                'Costmap not specified, use setCostmap to specify the costmap first')
         return self.costmap_.getCostXY(x, y)
 
     def setCostmap(self, costmap: PyCostmap2D):

nav2_simple_commander/nav2_simple_commander/line_iterator.py

@@ -50,22 +50,22 @@ def __init__(self, x0, y0, x1, y1, step_size=1.0):
 
         """
         if type(x0) not in [int, float]:
-            raise TypeError("x0 must be a number (int or float)")
+            raise TypeError('x0 must be a number (int or float)')
 
         if type(y0) not in [int, float]:
-            raise TypeError("y0 must be a number (int or float)")
+            raise TypeError('y0 must be a number (int or float)')
 
         if type(x1) not in [int, float]:
-            raise TypeError("x1 must be a number (int or float)")
+            raise TypeError('x1 must be a number (int or float)')
 
         if type(y1) not in [int, float]:
-            raise TypeError("y1 must be a number (int or float)")
+            raise TypeError('y1 must be a number (int or float)')
 
         if type(step_size) not in [int, float]:
-            raise TypeError("step_size must be a number (int or float)")
+            raise TypeError('step_size must be a number (int or float)')
 
         if step_size <= 0:
-            raise ValueError("step_size must be a positive number")
+            raise ValueError('step_size must be a positive number')
 
         self.x0_ = x0
         self.y0_ = y0
@@ -88,7 +88,7 @@ def __init__(self, x0, y0, x1, y1, step_size=1.0):
         else:
             self.valid_ = False
             raise ValueError(
-                "Line has zero length (All 4 points have same coordinates)")
+                'Line has zero length (All 4 points have same coordinates)')
 
     def isValid(self):
         """Check if line is valid."""