fix bug mentioned in #3958

nav2_controller/src/controller_server.cpp

@@ -130,6 +130,8 @@ ControllerServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
   costmap_ros_->configure();
   // Launch a thread to run the costmap node
   costmap_thread_ = std::make_unique<nav2_util::NodeThread>(costmap_ros_);
+  // work as a child-LifecycleNode of its parent controller_server
+  costmap_ros_->turnChildLifecycleNode();
 
   for (size_t i = 0; i != progress_checker_ids_.size(); i++) {
     try {
@@ -282,11 +284,13 @@ ControllerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
    * 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();
-  }
+//  if (costmap_ros_->get_current_state().id() ==
+//    lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
+//  {
+//    RCLCPP_INFO(get_logger(), "costmap_ros_->cleanup()");
+  costmap_ros_->deactivate();
+//  }
+// after turnChildLifecycleNode(), this double check is not neccessary anymore
 
   publishZeroVelocity();
   vel_publisher_->on_deactivate();
@@ -312,11 +316,13 @@ ControllerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
 
   goal_checkers_.clear();
   progress_checkers_.clear();
-  if (costmap_ros_->get_current_state().id() ==
-    lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
-  {
-    costmap_ros_->cleanup();
-  }
+//  if (costmap_ros_->get_current_state().id() ==
+//    lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
+//  {
+//    RCLCPP_INFO(get_logger(), "costmap_ros_->cleanup()");
+  costmap_ros_->cleanup();
+//  }
+// after turnChildLifecycleNode(), this double check is not neccessary anymore
 
   // Release any allocated resources
   action_server_.reset();

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp

@@ -134,6 +134,42 @@ class Costmap2DROS : public nav2_util::LifecycleNode
    */
   nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
 
+  /**
+   * @brief child-LifecycleNode mode
+   * sometimes costmap may be launched by another LifecycleNode and work as a child-thread
+   * child-LifecycleNode should react to ctrl+C later than its parents to avoid unclean shutdown
+   * Thus, it's neccessary to turnChildLifecycleNode() to change its launch mode
+   *
+   * in ChildLifecycleNode mode, it would react to rcl_preshutdown anymore
+   * all its lifecycle state would be controlled by its parent-LifecycleNode
+   */
+  void turnChildLifecycleNode()
+  {
+    // as a child-LifecycleNode launched by its parent-LifecycleNode
+    // and work as a child-thread
+    is_thread_ = true;
+  }
+  bool isThread()
+  {
+    return is_thread_;
+  }
+  void on_rcl_preshutdown() override
+  {
+    if (isThread() ) {
+      // all of its reaction is up to its parent
+      return;
+    }
+
+    // else, same as ``on_rcl_preshutdown` in lifecycle_node.cpp`
+    RCLCPP_INFO(
+      get_logger(), "Running Nav2 LifecycleNode rcl preshutdown (%s)",
+      this->get_name());
+
+    runCleanups();
+
+    destroyBond();
+  }
+
   /**
    * @brief  Subscribes to sensor topics if necessary and starts costmap
    * updates, can be called to restart the costmap after calls to either
@@ -317,6 +353,9 @@ class Costmap2DROS : public nav2_util::LifecycleNode
   double getRobotRadius() {return robot_radius_;}
 
 protected:
+  // launch mode: as a child-thread or an independent-process
+  bool is_thread_{false};
+
   // Publishers and subscribers
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::PolygonStamped>::SharedPtr
     footprint_pub_;

nav2_planner/src/planner_server.cpp

@@ -88,6 +88,8 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
 
   // Launch a thread to run the costmap node
   costmap_thread_ = std::make_unique<nav2_util::NodeThread>(costmap_ros_);
+  // work as a child-LifecycleNode of its parent planner_server
+  costmap_ros_->turnChildLifecycleNode();
 
   RCLCPP_DEBUG(
     get_logger(), "Costmap size: %d,%d",
@@ -218,11 +220,13 @@ PlannerServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
    * 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();
-  }
+//  if (costmap_ros_->get_current_state().id() ==
+//    lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
+//  {
+//    RCLCPP_INFO(get_logger(), "costmap_ros_->cleanup()");
+  costmap_ros_->deactivate();
+//  }
+// after turnChildLifecycleNode(), this double check is not neccessary anymore
 
   PlannerMap::iterator it;
   for (it = planners_.begin(); it != planners_.end(); ++it) {
@@ -251,11 +255,13 @@ PlannerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
    * 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();
-  }
+//  if (costmap_ros_->get_current_state().id() ==
+//    lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
+//  {
+//    RCLCPP_INFO(get_logger(), "costmap_ros_->cleanup()");
+  costmap_ros_->cleanup();
+//  }
+// after turnChildLifecycleNode(), this double check is not neccessary anymore
 
   PlannerMap::iterator it;
   for (it = planners_.begin(); it != planners_.end(); ++it) {