diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index ea513d1eb2..76754c5815 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -290,11 +290,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(); @@ -320,11 +316,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(); diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp index f319e52f33..b1ddedceeb 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp +++ b/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 @@ -381,6 +404,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 unpadded_footprint_; diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index c35933c908..bc887eeeb3 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/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(); } diff --git a/nav2_costmap_2d/test/unit/lifecycle_test.cpp b/nav2_costmap_2d/test/unit/lifecycle_test.cpp index 8a657d81ad..a8ab817df3 100644 --- a/nav2_costmap_2d/test/unit/lifecycle_test.cpp +++ b/nav2_costmap_2d/test/unit/lifecycle_test.cpp @@ -22,7 +22,7 @@ TEST(LifecylceTest, CheckInitialTfTimeout) { rclcpp::init(0, nullptr); - auto costmap = std::make_shared("test_costmap"); + auto costmap = std::make_shared(rclcpp::NodeOptions()); costmap->set_parameter({"initial_transform_timeout", 0.0}); std::thread spin_thread{[costmap]() {rclcpp::spin(costmap->get_node_base_interface());}}; diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 84edc6e4df..fea40ea405 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -218,11 +218,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) { @@ -247,15 +243,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) {