fix bug mentioned in ros-navigation#3958 (ros-navigation#3972) (ros-n…

…avigation#4463) * 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 * Update nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp * Update nav2_costmap_2d/include/nav2_costmap_2d/costmap_2d_ros.hpp * 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 <[email protected]> Co-authored-by: Steve Macenski <[email protected]>
EnjoyRobotics · Sep 26, 2024 · 3be92cb · 3be92cb
1 parent a4b406b
commit 3be92cb
Show file tree

Hide file tree

Showing 5 changed files with 130 additions and 24 deletions.
diff --git a/nav2_controller/src/controller_server.cpp b/nav2_controller/src/controller_server.cpp
@@ -264,11 +264,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();
 
   intermediate_planner_->deactivate();
 
@@ -296,11 +292,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();
+
 
   intermediate_planner_->cleanup();
 

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
@@ -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<geometry_msgs::msg::Point> unpadded_footprint_;

diff --git 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<std::string>()));
+  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<unsigned char>(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
@@ -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 <memory>
+
+#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<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());}};
+
+  {
+    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
@@ -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) {