cherry-pick into Iron: fix multi-bugs casued by costmap_ros_'s early shutdown

nav2_controller/src/controller_server.cpp

@@ -304,11 +304,7 @@ 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();
-  }
+  costmap_ros_->deactivate();
 
   publishZeroVelocity();
   vel_publisher_->on_deactivate();
@@ -334,11 +330,9 @@ 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();
-  }
+
+  costmap_ros_->cleanup();
+
 
   // Release any allocated resources
   action_server_.reset();

nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp

@@ -75,8 +75,9 @@ class Costmap2DROS : public nav2_util::LifecycleNode
 public:
   /**
    * @brief  Constructor for the wrapper
+   * @param options Additional options to control creation of the node.
    */
-  Costmap2DROS();
+  Costmap2DROS(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
 
   /**
    * @brief  Constructor for the wrapper, the node will
@@ -134,6 +135,28 @@ class Costmap2DROS : public nav2_util::LifecycleNode
    */
   nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
 
+  /**
+   * @brief as a child-LifecycleNode :
+   * Costmap2DROS may be launched by another Lifecycle Node as a composed module
+   * If composed, its parents will handle the shutdown, which includes this module
+   */
+  void on_rcl_preshutdown() override
+  {
+    if (is_lifecycle_follower_) {
+      // Transitioning handled by parent node
+      return;
+    }
+
+    // Else, if this is an independent node, this node needs to handle itself.
+    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
@@ -382,6 +405,8 @@ class Costmap2DROS : public nav2_util::LifecycleNode
   double transform_tolerance_{0};           ///< The timeout before transform errors
   double initial_transform_timeout_{0};   ///< The timeout before activation of the node errors
 
+  bool is_lifecycle_follower_{true};     ///< whether is a child-LifecycleNode or an independent node
+
   // Derived parameters
   bool use_radius_{false};
   std::vector<geometry_msgs::msg::Point> unpadded_footprint_;

nav2_costmap_2d/src/costmap_2d_ros.cpp

@@ -61,8 +61,8 @@ namespace nav2_costmap_2d
 Costmap2DROS::Costmap2DROS(const std::string & name, const bool & use_sim_time)
 : Costmap2DROS(name, "/", name, use_sim_time) {}
 
-Costmap2DROS::Costmap2DROS()
-: nav2_util::LifecycleNode("costmap", ""),
+Costmap2DROS::Costmap2DROS(const rclcpp::NodeOptions & options)
+: nav2_util::LifecycleNode("costmap", "", options),
   name_("costmap"),
   default_plugins_{"static_layer", "obstacle_layer", "inflation_layer"},
   default_types_{
@@ -71,6 +71,7 @@ Costmap2DROS::Costmap2DROS()
     "nav2_costmap_2d::InflationLayer"}
 {
   declare_parameter("map_topic", rclcpp::ParameterValue(std::string("map")));
+  is_lifecycle_follower_ = false;
   init();
 }
 

nav2_costmap_2d/test/unit/lifecycle_test.cpp

@@ -22,7 +22,7 @@
 TEST(LifecylceTest, CheckInitialTfTimeout) {
   rclcpp::init(0, nullptr);
 
-  auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>("test_costmap");
+  auto costmap = std::make_shared<nav2_costmap_2d::Costmap2DROS>(rclcpp::NodeOptions());
   costmap->set_parameter({"initial_transform_timeout", 0.0});
 
   std::thread spin_thread{[costmap]() {rclcpp::spin(costmap->get_node_base_interface());}};

nav2_planner/src/planner_server.cpp

@@ -223,11 +223,7 @@ 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();
-  }
+  costmap_ros_->deactivate();
 
   PlannerMap::iterator it;
   for (it = planners_.begin(); it != planners_.end(); ++it) {
@@ -252,15 +248,7 @@ PlannerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
   plan_publisher_.reset();
   tf_.reset();
 
-  /*
-   * 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();
-  }
+  costmap_ros_->cleanup();
 
   PlannerMap::iterator it;
   for (it = planners_.begin(); it != planners_.end(); ++it) {