From d829e86f150387c293ef7356dac7ad1dbde714e3 Mon Sep 17 00:00:00 2001 From: GoesM Date: Mon, 4 Dec 2023 15:57:09 +0800 Subject: [PATCH 1/2] pull main into humble --- nav2_controller/src/controller_server.cpp | 12 ++-------- .../nav2_costmap_2d/costmap_2d_ros.hpp | 24 +++++++++++++++++++ nav2_planner/src/planner_server.cpp | 16 ++----------- 3 files changed, 28 insertions(+), 24 deletions(-) diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index baa9775348..8c8e3577fc 100644 --- a/nav2_controller/src/controller_server.cpp +++ b/nav2_controller/src/controller_server.cpp @@ -254,11 +254,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(); @@ -284,11 +280,7 @@ ControllerServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) goal_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 af93f4c7e0..0d530dc500 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 @@ -121,6 +121,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 @@ -365,6 +387,8 @@ class Costmap2DROS : public nav2_util::LifecycleNode bool track_unknown_space_{false}; double transform_tolerance_{0}; ///< The timeout before transform 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_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 2126503bae..5422146e2b 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -207,11 +207,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) { @@ -236,15 +232,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) { From fed0600eda72a563a9477fb2955b04e6ca33bdfc Mon Sep 17 00:00:00 2001 From: GoesM Date: Tue, 5 Dec 2023 17:46:33 +0800 Subject: [PATCH 2/2] add constructor for setting is_lifecycle_follower_ to false --- .../nav2_costmap_2d/costmap_2d_ros.hpp | 11 ++++++++ nav2_costmap_2d/src/costmap_2d_ros.cpp | 25 ++++++++++++++++--- 2 files changed, 33 insertions(+), 3 deletions(-) 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 0d530dc500..dd5a6c72ac 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 @@ -73,6 +73,12 @@ namespace nav2_costmap_2d class Costmap2DROS : public nav2_util::LifecycleNode { public: + /** + * @brief Constructor for the wrapper + * @param options Additional options to control creation of the node. + */ + Costmap2DROS(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + /** * @brief Constructor for the wrapper, the node will * be placed in a namespace equal to the node's name @@ -91,6 +97,11 @@ class Costmap2DROS : public nav2_util::LifecycleNode const std::string & parent_namespace, const std::string & local_namespace); + /** + * @brief Common initialization for constructors + */ + void init(); + /** * @brief A destructor */ diff --git a/nav2_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 5fbd1beea8..fb576574c8 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -61,6 +61,20 @@ namespace nav2_costmap_2d Costmap2DROS::Costmap2DROS(const std::string & name) : Costmap2DROS(name, "/", name) {} +Costmap2DROS::Costmap2DROS(const rclcpp::NodeOptions & options) +: nav2_util::LifecycleNode("costmap", ""), + name_("costmap"), + default_plugins_{"static_layer", "obstacle_layer", "inflation_layer"}, + default_types_{ + "nav2_costmap_2d::StaticLayer", + "nav2_costmap_2d::ObstacleLayer", + "nav2_costmap_2d::InflationLayer"} +{ + declare_parameter("map_topic", rclcpp::ParameterValue(std::string("map"))); + is_lifecycle_follower_ = false; + init(); +} + Costmap2DROS::Costmap2DROS( const std::string & name, const std::string & parent_namespace, @@ -82,6 +96,14 @@ Costmap2DROS::Costmap2DROS( "nav2_costmap_2d::StaticLayer", "nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::InflationLayer"} +{ + declare_parameter( + "map_topic", rclcpp::ParameterValue( + (parent_namespace_ == "/" ? "/" : parent_namespace_ + "/") + std::string("map"))); + init(); +} + +void Costmap2DROS::init() { RCLCPP_INFO(get_logger(), "Creating Costmap"); @@ -94,9 +116,6 @@ Costmap2DROS::Costmap2DROS( declare_parameter("height", rclcpp::ParameterValue(5)); declare_parameter("width", rclcpp::ParameterValue(5)); declare_parameter("lethal_cost_threshold", rclcpp::ParameterValue(100)); - declare_parameter( - "map_topic", rclcpp::ParameterValue( - (parent_namespace_ == "/" ? "/" : parent_namespace_ + "/") + std::string("map"))); declare_parameter("observation_sources", rclcpp::ParameterValue(std::string(""))); declare_parameter("origin_x", rclcpp::ParameterValue(0.0)); declare_parameter("origin_y", rclcpp::ParameterValue(0.0));