From c4b00cd678e1e9e01f91de3dd071732b4abdc366 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 | 13 ++--- .../nav2_costmap_2d/costmap_2d_ros.hpp | 30 +++++++++++ nav2_costmap_2d/src/costmap_2d_ros.cpp | 41 ++++++++++++++ nav2_costmap_2d/test/unit/lifecycle_test.cpp | 54 +++++++++++++++++++ nav2_planner/src/planner_server.cpp | 16 +----- 5 files changed, 130 insertions(+), 24 deletions(-) create mode 100644 nav2_costmap_2d/test/unit/lifecycle_test.cpp diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp index 9c6cb01a29..65353c5a6c 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,8 @@ 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 f4f365e848..599f7c6f69 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 @@ -121,6 +127,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 @@ -366,6 +394,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_costmap_2d/src/costmap_2d_ros.cpp b/nav2_costmap_2d/src/costmap_2d_ros.cpp index 98bcb0ff36..96c32f955e 100644 --- a/nav2_costmap_2d/src/costmap_2d_ros.cpp +++ b/nav2_costmap_2d/src/costmap_2d_ros.cpp @@ -61,6 +61,47 @@ namespace nav2_costmap_2d Costmap2DROS::Costmap2DROS(const std::string & name) : Costmap2DROS(name, "/", name) {} +Costmap2DROS::Costmap2DROS(const rclcpp::NodeOptions & options) +: nav2_util::LifecycleNode("costmap", "", options), + name_("costmap"), + default_plugins_{"static_layer", "obstacle_layer", "inflation_layer"}, + default_types_{ + "nav2_costmap_2d::StaticLayer", + "nav2_costmap_2d::ObstacleLayer", + "nav2_costmap_2d::InflationLayer"} +{ + is_lifecycle_follower_ = false; + + RCLCPP_INFO(get_logger(), "Creating Costmap"); + + declare_parameter("always_send_full_costmap", rclcpp::ParameterValue(false)); + declare_parameter("footprint_padding", rclcpp::ParameterValue(0.01f)); + declare_parameter("footprint", rclcpp::ParameterValue(std::string("[]"))); + declare_parameter("global_frame", rclcpp::ParameterValue(std::string("map"))); + 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)); + declare_parameter("plugins", rclcpp::ParameterValue(default_plugins_)); + declare_parameter("filters", rclcpp::ParameterValue(std::vector())); + declare_parameter("publish_frequency", rclcpp::ParameterValue(1.0)); + declare_parameter("resolution", rclcpp::ParameterValue(0.1)); + declare_parameter("robot_base_frame", rclcpp::ParameterValue(std::string("base_link"))); + declare_parameter("robot_radius", rclcpp::ParameterValue(0.1)); + declare_parameter("rolling_window", rclcpp::ParameterValue(false)); + declare_parameter("track_unknown_space", rclcpp::ParameterValue(false)); + declare_parameter("transform_tolerance", rclcpp::ParameterValue(0.3)); + declare_parameter("trinary_costmap", rclcpp::ParameterValue(true)); + declare_parameter("unknown_cost_value", rclcpp::ParameterValue(static_cast(0xff))); + declare_parameter("update_frequency", rclcpp::ParameterValue(5.0)); + declare_parameter("use_maximum", rclcpp::ParameterValue(false)); +} + Costmap2DROS::Costmap2DROS( const std::string & name, const std::string & parent_namespace, diff --git a/nav2_costmap_2d/test/unit/lifecycle_test.cpp b/nav2_costmap_2d/test/unit/lifecycle_test.cpp new file mode 100644 index 0000000000..a8ab817df3 --- /dev/null +++ b/nav2_costmap_2d/test/unit/lifecycle_test.cpp @@ -0,0 +1,54 @@ +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "gtest/gtest.h" + +#include "nav2_costmap_2d/costmap_2d_ros.hpp" +#include "rclcpp/rclcpp.hpp" +#include "lifecycle_msgs/msg/state.hpp" + + +TEST(LifecylceTest, CheckInitialTfTimeout) { + rclcpp::init(0, nullptr); + + 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());}}; + + { + const auto state_after_configure = costmap->configure(); + ASSERT_EQ(state_after_configure.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + // Without providing the transform from global to robot base the activation should fail + // and the costmap should transition into the inactive state. + const auto state_after_activate = costmap->activate(); + ASSERT_EQ(state_after_activate.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE); + } + + // Set a dummy transform from global to robot base + geometry_msgs::msg::TransformStamped transform_global_to_robot{}; + transform_global_to_robot.header.frame_id = costmap->getGlobalFrameID(); + transform_global_to_robot.child_frame_id = costmap->getBaseFrameID(); + costmap->getTfBuffer()->setTransform(transform_global_to_robot, "test", true); + // Now the costmap should successful transition into the active state + { + const auto state_after_activate = costmap->activate(); + ASSERT_EQ(state_after_activate.id(), lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE); + } + + rclcpp::shutdown(); + if (spin_thread.joinable()) { + spin_thread.join(); + } +} diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 29957209ec..e1a79b8306 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) {