Merge pull request #1382 from SteveMacenski/1239

move collision checker as an input to the configure method of recoveries

nav2_core/include/nav2_core/recovery.hpp

@@ -21,6 +21,7 @@
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_util/lifecycle_node.hpp"
 #include "tf2_ros/buffer.h"
+#include "nav2_costmap_2d/collision_checker.hpp"
 
 namespace nav2_core
 {
@@ -47,7 +48,8 @@ public:
    */
   virtual void configure(
     const rclcpp_lifecycle::LifecycleNode::SharedPtr parent,
-    const std::string & name, std::shared_ptr<tf2_ros::Buffer> tf) = 0;
+    const std::string & name, std::shared_ptr<tf2_ros::Buffer> tf,
+    std::shared_ptr<nav2_costmap_2d::CollisionChecker> collision_checker) = 0;
 
   /**
    * @brief Method to cleanup resources used on shutdown.

nav2_recoveries/include/nav2_recoveries/recovery.hpp

@@ -26,7 +26,6 @@
 #include "tf2_ros/transform_listener.h"
 #include "tf2_ros/create_timer_ros.h"
 #include "geometry_msgs/msg/twist.hpp"
-#include "nav2_costmap_2d/collision_checker.hpp"
 #include "nav2_util/simple_action_server.hpp"
 #include "nav2_util/robot_utils.hpp"
 #include "nav2_core/recovery.hpp"
@@ -87,32 +86,21 @@ public:
 
   void configure(
     const rclcpp_lifecycle::LifecycleNode::SharedPtr parent,
-    const std::string & name, std::shared_ptr<tf2_ros::Buffer> tf) override
+    const std::string & name, std::shared_ptr<tf2_ros::Buffer> tf,
+    std::shared_ptr<nav2_costmap_2d::CollisionChecker> collision_checker) override
   {
     RCLCPP_INFO(parent->get_logger(), "Configuring %s", name.c_str());
 
     node_ = parent;
-    tf_ = tf;
     recovery_name_ = name;
+    tf_ = tf;
 
-    std::string costmap_topic;
-    std::string footprint_topic;
-
-    node_->get_parameter("costmap_topic", costmap_topic);
-    node_->get_parameter("footprint_topic", footprint_topic);
     node_->get_parameter("cycle_frequency", cycle_frequency_);
 
     action_server_ = std::make_unique<ActionServer>(node_, recovery_name_,
         std::bind(&Recovery::execute, this));
 
-    costmap_sub_ = std::make_unique<nav2_costmap_2d::CostmapSubscriber>(
-      node_, costmap_topic);
-
-    footprint_sub_ = std::make_unique<nav2_costmap_2d::FootprintSubscriber>(
-      node_, footprint_topic, 1.0);
-
-    collision_checker_ = std::make_unique<nav2_costmap_2d::CollisionChecker>(
-      *costmap_sub_, *footprint_sub_, *tf_, node_->get_name(), "odom");
+    collision_checker_ = collision_checker;
 
     vel_pub_ = node_->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 1);
 
@@ -123,9 +111,6 @@ public:
   {
     action_server_.reset();
     vel_pub_.reset();
-    footprint_sub_.reset();
-    costmap_sub_.reset();
-    collision_checker_.reset();
     onCleanup();
   }
 
@@ -145,12 +130,10 @@ protected:
   rclcpp_lifecycle::LifecycleNode::SharedPtr node_;
   std::string recovery_name_;
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr vel_pub_;
-  std::shared_ptr<tf2_ros::Buffer> tf_;
   std::unique_ptr<ActionServer> action_server_;
+  std::shared_ptr<nav2_costmap_2d::CollisionChecker> collision_checker_;
+  std::shared_ptr<tf2_ros::Buffer> tf_;
 
-  std::unique_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_;
-  std::unique_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_;
-  std::unique_ptr<nav2_costmap_2d::CollisionChecker> collision_checker_;
   double cycle_frequency_;
   double enabled_;
 

nav2_recoveries/include/nav2_recoveries/recovery_server.hpp

@@ -55,6 +55,11 @@ protected:
   std::vector<pluginlib::UniquePtr<nav2_core::Recovery>> recoveries_;
   pluginlib::ClassLoader<nav2_core::Recovery> plugin_loader_;
   std::vector<std::string> plugin_names_, plugin_types_;
+
+  // Utilities
+  std::unique_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_;
+  std::unique_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_;
+  std::shared_ptr<nav2_costmap_2d::CollisionChecker> collision_checker_;
 };
 
 }  // namespace recovery_server

nav2_recoveries/src/recovery_server.cpp

@@ -55,13 +55,23 @@ RecoveryServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
 
   tf_ = std::make_shared<tf2_ros::Buffer>(get_clock());
   auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
-    get_node_base_interface(),
-    get_node_timers_interface());
+    this->get_node_base_interface(),
+    this->get_node_timers_interface());
   tf_->setCreateTimerInterface(timer_interface);
   transform_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_);
 
-  get_parameter("plugin_names", plugin_names_);
-  get_parameter("plugin_types", plugin_types_);
+  std::string costmap_topic, footprint_topic;
+  this->get_parameter("costmap_topic", costmap_topic);
+  this->get_parameter("footprint_topic", footprint_topic);
+  costmap_sub_ = std::make_unique<nav2_costmap_2d::CostmapSubscriber>(
+    shared_from_this(), costmap_topic);
+  footprint_sub_ = std::make_unique<nav2_costmap_2d::FootprintSubscriber>(
+    shared_from_this(), footprint_topic, 1.0);
+  collision_checker_ = std::make_shared<nav2_costmap_2d::CollisionChecker>(
+    *costmap_sub_, *footprint_sub_, *tf_, this->get_name(), "odom");
+
+  this->get_parameter("plugin_names", plugin_names_);
+  this->get_parameter("plugin_types", plugin_types_);
 
   loadRecoveryPlugins();
 
@@ -79,7 +89,7 @@ RecoveryServer::loadRecoveryPlugins()
       RCLCPP_INFO(get_logger(), "Creating recovery plugin %s of type %s",
         plugin_names_[i].c_str(), plugin_types_[i].c_str());
       recoveries_.push_back(plugin_loader_.createUniqueInstance(plugin_types_[i]));
-      recoveries_.back()->configure(node, plugin_names_[i], tf_);
+      recoveries_.back()->configure(node, plugin_names_[i], tf_, collision_checker_);
     } catch (const pluginlib::PluginlibException & ex) {
       RCLCPP_FATAL(get_logger(), "Failed to create recovery %s of type %s."
         " Exception: %s", plugin_names_[i].c_str(), plugin_types_[i].c_str(),
@@ -127,6 +137,9 @@ RecoveryServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
   recoveries_.clear();
   transform_listener_.reset();
   tf_.reset();
+  footprint_sub_.reset();
+  costmap_sub_.reset();
+  collision_checker_.reset();
 
   return nav2_util::CallbackReturn::SUCCESS;
 }

nav2_recoveries/test/test_recoveries.cpp

@@ -117,8 +117,22 @@ protected:
     tf_buffer_->setCreateTimerInterface(timer_interface);
     tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
 
+    std::string costmap_topic, footprint_topic;
+    node_lifecycle_->get_parameter("costmap_topic", costmap_topic);
+    node_lifecycle_->get_parameter("footprint_topic", footprint_topic);
+    std::unique_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_ =
+      std::make_unique<nav2_costmap_2d::CostmapSubscriber>(
+      node_lifecycle_, costmap_topic);
+    std::unique_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_ =
+      std::make_unique<nav2_costmap_2d::FootprintSubscriber>(
+      node_lifecycle_, footprint_topic, 1.0);
+    std::shared_ptr<nav2_costmap_2d::CollisionChecker> collision_checker_ =
+      std::make_shared<nav2_costmap_2d::CollisionChecker>(
+      *costmap_sub_, *footprint_sub_, *tf_buffer_,
+      node_lifecycle_->get_name(), "odom");
+
     recovery_ = std::make_unique<DummyRecovery>();
-    recovery_->configure(node_lifecycle_, "Recovery", tf_buffer_);
+    recovery_->configure(node_lifecycle_, "Recovery", tf_buffer_, collision_checker_);
     recovery_->activate();
 
     client_ = rclcpp_action::create_client<RecoveryAction>(