From aa9396e6c805bc787d8a7f3a46bfc16b1315939b Mon Sep 17 00:00:00 2001 From: GoesM <130988564+GoesM@users.noreply.github.com> Date: Fri, 1 Dec 2023 13:49:21 +0800 Subject: [PATCH] fix bug mentioned in #3958 (#3972) * bug fixed * add space * Update planner_server.cpp * add space for code style * add childLifecycleNode mode to costmap_2d_ros * add childLifecycleNode mode to costmap_2d_ros * add childLifecycleNode mode to costmap_2d_ros * add childLifecycleNode mode in costmap_2d_ros * add childLifecycleNode mode in costmap_2d_ros * add childLifecycleNode mode in costmap_2d_ros * add ChildLifecycleNode mode in costmap_2d_ros * NodeOption: is_lifecycle_follower_ * NodeOption: is_lifecycle_follower_ * fit to NodeOption: is_lifecycle_follower_ * NodeOption: is_lifecycle_follower_ * fit to NodeOption: is_lifecycle_follower * fit to NodeOption: is_lifecycle_follower * fit reorder Werror * fix wrong use of is_lifecycle_follower * remove blank line * NodeOption: is_lifecycle_follower_ * NodeOption: is_lifecycle_follower_ * Add files via upload * NodeOption: is_lifecycle_follower_ * NodeOption:is_lifecycle_follower_ * NodeOption:is_lifecycle_follower * NodeOption:is_lifecycle_follower * NodeOption:is_lifecycle_follower * change default * add NodeOption for costmap_2d_ros * add node options for costmap2dros as an independent node * code style reformat * fit to NodeOption of Costmap2DROS * fit to NodeOption of Costmap2DROS * fit to NodeOption of Costmap2DROS * Update nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp Co-authored-by: Steve Macenski * Update nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp Co-authored-by: Steve Macenski * Update nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp Co-authored-by: Steve Macenski * changes * comment changes * change get_parameter into =false * comment modification * missing line * Update nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp * Update nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp * delete last line * change lifecycle_test fit to NodeOption --------- Co-authored-by: GoesM Co-authored-by: Steve Macenski --- nav2_controller/src/controller_server.cpp | 14 +++------- .../nav2_costmap_2d/costmap_2d_ros.hpp | 27 ++++++++++++++++++- nav2_costmap_2d/src/costmap_2d_ros.cpp | 5 ++-- nav2_costmap_2d/test/unit/lifecycle_test.cpp | 2 +- nav2_planner/src/planner_server.cpp | 16 ++--------- 5 files changed, 36 insertions(+), 28 deletions(-) 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) {