diff --git a/nav2_core/include/nav2_core/recovery.hpp b/nav2_core/include/nav2_core/recovery.hpp index b3a34bf2d355b48c6f9e7bf12b642de3f927a2ae..1041d74d8e98e1016e33705b3d5bf0c99870023f 100644 --- a/nav2_core/include/nav2_core/recovery.hpp +++ b/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. diff --git a/nav2_recoveries/include/nav2_recoveries/recovery.hpp b/nav2_recoveries/include/nav2_recoveries/recovery.hpp index 27e3945ab90f6bf7200e2e1c53b1afa3502ffd35..bf628149e321a827c21126d171c58730be01b5be 100644 --- a/nav2_recoveries/include/nav2_recoveries/recovery.hpp +++ b/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_; diff --git a/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp b/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp index a8503ca202a48d9c9fe1a8d556354a9ce122028d..026c3ecab4f054051199ad26014f37a487654ec9 100644 --- a/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp +++ b/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 diff --git a/nav2_recoveries/src/recovery_server.cpp b/nav2_recoveries/src/recovery_server.cpp index f6f6aa28c146aeac4313bbdbefd2be663ab95dc8..136f969e09083d21054baa7081cd7847552250ce 100644 --- a/nav2_recoveries/src/recovery_server.cpp +++ b/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; } diff --git a/nav2_recoveries/test/test_recoveries.cpp b/nav2_recoveries/test/test_recoveries.cpp index 84a716cfd15f966562d5c224290bc2aa6be0cf7d..45258a80d4205a10b38290d199ae356e60107bb3 100644 --- a/nav2_recoveries/test/test_recoveries.cpp +++ b/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>(