Merge pull request #1131 from SteveMacenski/nav2_recovery_server

Nav2 recovery server

nav2_bringup/bringup/launch/nav2_bringup_launch.py

@@ -119,6 +119,7 @@ def generate_launch_description():
                                     'amcl',
                                     'controller_server',
                                     'planner_server',
+                                    'recoveries_server',
                                     'bt_navigator']}])
 
     # Create the launch description and populate

nav2_bringup/bringup/launch/nav2_navigation_launch.py

@@ -120,8 +120,8 @@ def generate_launch_description():
 
         Node(
             package='nav2_recoveries',
-            node_executable='recoveries_node',
-            node_name='recoveries',
+            node_executable='recoveries_server',
+            node_name='recoveries_server',
             output='screen',
             parameters=[{'use_sim_time': use_sim_time}],
             use_remappings=IfCondition(use_remappings),
@@ -145,7 +145,8 @@ def generate_launch_description():
             parameters=[{'use_sim_time': use_sim_time},
                         {'autostart': autostart},
                         {'node_names': ['controller_server',
-                                        'navfn_planner',
+                                        'planner_server',
+                                        'recoveries_server',
                                         'bt_navigator']}]),
 
     ])

nav2_core/include/nav2_core/global_planner.hpp

@@ -49,7 +49,7 @@ public:
    */
   virtual void configure(
     rclcpp_lifecycle::LifecycleNode::SharedPtr parent,
-    std::string name, tf2_ros::Buffer * tf,
+    std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
     std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) = 0;
 
   /**

nav2_core/include/nav2_core/recovery.hpp

@@ -16,6 +16,8 @@
 #define NAV2_CORE__RECOVERY_HPP_
 
 #include <string>
+#include <memory>
+
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_util/lifecycle_node.hpp"
 #include "tf2_ros/buffer.h"
@@ -30,6 +32,8 @@ namespace nav2_core
 class Recovery
 {
 public:
+  using Ptr = std::shared_ptr<Recovery>;
+
   /**
    * @brief Virtual destructor
    */
@@ -42,8 +46,8 @@ public:
    * @param  costmap_ros A pointer to the costmap
    */
   virtual void configure(
-    const nav2_util::LifecycleNode * parent,
-    const std::string & name, tf2_ros::Buffer * tf) = 0;
+    const rclcpp_lifecycle::LifecycleNode::SharedPtr parent,
+    const std::string & name, std::shared_ptr<tf2_ros::Buffer> tf) = 0;
 
   /**
    * @brief Method to cleanup resources used on shutdown.
@@ -65,7 +69,9 @@ public:
    * @param  name The name of this planner
    * @return true if successful, false is failed to execute fully
    */
-  virtual bool executeRecovery() = 0;
+  // TODO(stevemacenski) evaluate design for recoveries to not host
+  // their own servers and utilize a recovery server exposed action.
+  // virtual bool executeRecovery() = 0;
 };
 
 }  // namespace nav2_core

nav2_navfn_planner/CMakeLists.txt

@@ -16,6 +16,7 @@ find_package(geometry_msgs REQUIRED)
 find_package(builtin_interfaces REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(nav2_costmap_2d REQUIRED)
+find_package(pluginlib REQUIRED)
 
 nav2_package()
 
@@ -39,6 +40,7 @@ set(dependencies
   tf2_ros
   nav2_costmap_2d
   nav2_core
+  pluginlib
 )
 
 add_library(${library_name} SHARED

nav2_navfn_planner/include/nav2_navfn_planner/navfn_planner.hpp

@@ -43,7 +43,7 @@ public:
   // plugin configure
   void configure(
     rclcpp_lifecycle::LifecycleNode::SharedPtr parent,
-    std::string name, tf2_ros::Buffer * tf,
+    std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
     std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros) override;
 
   // plugin cleanup
@@ -117,7 +117,7 @@ protected:
   std::unique_ptr<NavFn> planner_;
 
   // TF buffer
-  tf2_ros::Buffer * tf_;
+  std::shared_ptr<tf2_ros::Buffer> tf_;
 
   // node ptr
   nav2_util::LifecycleNode::SharedPtr node_;

nav2_navfn_planner/package.xml

@@ -24,6 +24,7 @@
   <depend>tf2_ros</depend>
   <depend>nav2_costmap_2d</depend>
   <depend>nav2_core</depend>
+  <depend>pluginlib</depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>

nav2_navfn_planner/src/navfn_planner.cpp

@@ -55,7 +55,7 @@ NavfnPlanner::~NavfnPlanner()
 void
 NavfnPlanner::configure(
   rclcpp_lifecycle::LifecycleNode::SharedPtr parent,
-  std::string name, tf2_ros::Buffer * tf,
+  std::string name, std::shared_ptr<tf2_ros::Buffer> tf,
   std::shared_ptr<nav2_costmap_2d::Costmap2DROS> costmap_ros)
 {
   node_ = parent;

nav2_planner/src/planner_server.cpp

@@ -87,7 +87,7 @@ PlannerServer::on_configure(const rclcpp_lifecycle::State & state)
     RCLCPP_INFO(get_logger(), "Created global planner plugin %s",
       planner_plugin_name_.c_str());
     planner_->configure(node,
-      gp_loader_.getName(planner_plugin_name_), tf_.get(), costmap_ros_);
+      gp_loader_.getName(planner_plugin_name_), tf_, costmap_ros_);
   } catch (const pluginlib::PluginlibException & ex) {
     RCLCPP_FATAL(get_logger(), "Failed to create global planner. Exception: %s",
       ex.what());

nav2_recoveries/CMakeLists.txt

@@ -15,6 +15,8 @@ find_package(tf2 REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(nav2_costmap_2d REQUIRED)
+find_package(nav2_core REQUIRED)
+find_package(pluginlib REQUIRED)
 
 nav2_package()
 
@@ -22,8 +24,8 @@ include_directories(
   include
 )
 
-set(library_name recoveries)
-set(executable_name ${library_name}_node)
+set(library_name recoveries_server_core)
+set(executable_name recoveries_server)
 
 set(dependencies
   rclcpp
@@ -38,12 +40,32 @@ set(dependencies
   tf2_geometry_msgs
   geometry_msgs
   nav2_costmap_2d
+  nav2_core
+  pluginlib
 )
 
+# plugins
+add_library(nav2_spin_recovery SHARED
+  plugins/spin.cpp
+)
+
+ament_target_dependencies(nav2_spin_recovery
+  ${dependencies}
+)
+
+add_library(nav2_backup_recovery SHARED
+  plugins/back_up.cpp
+)
+
+ament_target_dependencies(nav2_backup_recovery
+  ${dependencies}
+)
+
+pluginlib_export_plugin_description_file(nav2_core recovery_plugin.xml)
+
 # Library
 add_library(${library_name}
-  src/spin.cpp
-  src/back_up.cpp
+  src/recovery_server.cpp
 )
 
 ament_target_dependencies(${library_name}
@@ -61,7 +83,10 @@ ament_target_dependencies(${executable_name}
   ${dependencies}
 )
 
-install(TARGETS ${executable_name} ${library_name}
+install(TARGETS ${executable_name}
+                ${library_name}
+                nav2_backup_recovery
+                nav2_spin_recovery
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION lib/${PROJECT_NAME}
@@ -71,6 +96,10 @@ install(DIRECTORY include/
   DESTINATION include/
 )
 
+install(FILES recovery_plugin.xml
+  DESTINATION share
+)
+
 if(BUILD_TESTING)
   find_package(ament_cmake_pytest REQUIRED)
   find_package(ament_cmake_gtest REQUIRED)
@@ -80,6 +109,6 @@ if(BUILD_TESTING)
 endif()
 
 ament_export_include_directories(include)
-ament_export_libraries(${library_name})
+ament_export_libraries(${library_name} nav2_backup_recovery nav2_spin_recovery)
 
 ament_package()

nav2_recoveries/include/nav2_recoveries/recovery.hpp

@@ -29,6 +29,7 @@
 #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"
 
 namespace nav2_recoveries
 {
@@ -43,26 +44,20 @@ enum class Status : int8_t
 using namespace std::chrono_literals;  //NOLINT
 
 template<typename ActionT>
-class Recovery
+class Recovery : public nav2_core::Recovery
 {
 public:
-  using ActionServer = nav2_util::SimpleActionServer<ActionT>;
-
-  explicit Recovery(
-    rclcpp::Node::SharedPtr & node, const std::string & recovery_name,
-    std::shared_ptr<tf2_ros::Buffer> tf)
-  : node_(node),
-    recovery_name_(recovery_name),
-    tf_(*tf),
-    action_server_(nullptr),
-    cycle_frequency_(10)
+  using ActionServer = nav2_util::SimpleActionServer<ActionT, rclcpp_lifecycle::LifecycleNode>;
+
+  Recovery()
+  : action_server_(nullptr),
+    cycle_frequency_(10),
+    enabled_(false)
   {
-    configure();
   }
 
   virtual ~Recovery()
   {
-    cleanup();
   }
 
   // Derived classes can override this method to catch the command and perform some checks
@@ -78,21 +73,27 @@ public:
   // It's up to the derived class to define the final commanded velocity.
   virtual Status onCycleUpdate() = 0;
 
-protected:
-  rclcpp::Node::SharedPtr node_;
-  std::string recovery_name_;
-  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr vel_pub_;
-  tf2_ros::Buffer & tf_;
-  std::unique_ptr<ActionServer> action_server_;
+  // an opportunity for derived classes to do something on configuration
+  // if they chose
+  virtual void onConfigure()
+  {
+  }
 
-  std::shared_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_;
-  std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_;
-  std::unique_ptr<nav2_costmap_2d::CollisionChecker> collision_checker_;
-  double cycle_frequency_;
+  // an opportunity for derived classes to do something on cleanup
+  // if they chose
+  virtual void onCleanup()
+  {
+  }
 
-  void configure()
+  void configure(
+    const rclcpp_lifecycle::LifecycleNode::SharedPtr parent,
+    const std::string & name, std::shared_ptr<tf2_ros::Buffer> tf) override
   {
-    RCLCPP_INFO(node_->get_logger(), "Configuring %s", recovery_name_.c_str());
+    RCLCPP_INFO(parent->get_logger(), "Configuring %s", name.c_str());
+
+    node_ = parent;
+    tf_ = tf;
+    recovery_name_ = name;
 
     std::string costmap_topic;
     std::string footprint_topic;
@@ -110,24 +111,55 @@ protected:
       node_, footprint_topic);
 
     collision_checker_ = std::make_unique<nav2_costmap_2d::CollisionChecker>(
-      *costmap_sub_, *footprint_sub_, tf_, node_->get_name(), "odom");
+      *costmap_sub_, *footprint_sub_, *tf_, node_->get_name(), "odom");
 
     vel_pub_ = node_->create_publisher<geometry_msgs::msg::Twist>("cmd_vel", 1);
+
+    onConfigure();
   }
 
-  void cleanup()
+  void cleanup() override
   {
     action_server_.reset();
     vel_pub_.reset();
     footprint_sub_.reset();
     costmap_sub_.reset();
     collision_checker_.reset();
+    onCleanup();
+  }
+
+  void activate() override
+  {
+    enabled_ = true;
   }
 
+  void deactivate() override
+  {
+    enabled_ = false;
+  }
+
+protected:
+  rclcpp_lifecycle::LifecycleNode::SharedPtr node_;
+  std::string recovery_name_;
+  rclcpp::Publisher<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::CostmapSubscriber> costmap_sub_;
+  std::shared_ptr<nav2_costmap_2d::FootprintSubscriber> footprint_sub_;
+  std::unique_ptr<nav2_costmap_2d::CollisionChecker> collision_checker_;
+  double cycle_frequency_;
+  double enabled_;
+
   void execute()
   {
     RCLCPP_INFO(node_->get_logger(), "Attempting %s", recovery_name_.c_str());
 
+    if (!enabled_) {
+      RCLCPP_WARN(node_->get_logger(), "Called while inactive, ignoring request.");
+      return;
+    }
+
     if (onRun(action_server_->get_current_goal()) != Status::SUCCEEDED) {
       RCLCPP_INFO(node_->get_logger(), "Initial checks failed for %s", recovery_name_.c_str());
       action_server_->terminate_goals();

nav2_recoveries/include/nav2_recoveries/recovery_server.hpp

+// Copyright (c) 2018 Samsung Research America
+//
+// 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. Reserved.
+
+
+#include <chrono>
+#include <string>
+#include <memory>
+#include <vector>
+
+#include "nav2_util/lifecycle_node.hpp"
+#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/create_timer_ros.h"
+#include "pluginlib/class_loader.hpp"
+#include "pluginlib/class_list_macros.hpp"
+#include "nav2_core/recovery.hpp"
+
+#ifndef NAV2_RECOVERIES__RECOVERY_SERVER_HPP_
+#define NAV2_RECOVERIES__RECOVERY_SERVER_HPP_
+
+namespace recovery_server
+{
+
+class RecoveryServer : public nav2_util::LifecycleNode
+{
+public:
+  RecoveryServer();
+  ~RecoveryServer();
+
+  void loadRecoveryPlugins();
+
+protected:
+  // Implement the lifecycle interface
+  nav2_util::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
+  nav2_util::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
+  nav2_util::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
+  nav2_util::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
+  nav2_util::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
+  nav2_util::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override;
+
+  std::shared_ptr<tf2_ros::Buffer> tf_;
+  std::shared_ptr<tf2_ros::TransformListener> transform_listener_;
+
+  // Plugins
+  std::vector<pluginlib::UniquePtr<nav2_core::Recovery>> recoveries_;
+  pluginlib::ClassLoader<nav2_core::Recovery> plugin_loader_;
+  std::vector<std::string> plugin_names_, plugin_types_;
+};
+
+}  // namespace recovery_server
+
+#endif  // NAV2_RECOVERIES__RECOVERY_SERVER_HPP_

nav2_recoveries/package.xml

@@ -22,6 +22,8 @@
   <build_depend>tf2_geometry_msgs</build_depend>
   <build_depend>geometry_msgs</build_depend>
   <build_depend>nav2_costmap_2d</build_depend>
+  <build_depend>nav2_core</build_depend>
+  <build_depend>pluginlib</build_depend>
 
   <exec_depend>rclcpp</exec_depend>
   <exec_depend>rclcpp_action</exec_depend>
@@ -32,6 +34,8 @@
   <exec_depend>nav_msgs</exec_depend>
   <exec_depend>geometry_msgs</exec_depend>
   <exec_depend>nav2_costmap_2d</exec_depend>
+  <exec_depend>nav2_core</exec_depend>
+  <exec_depend>pluginlib</exec_depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>
@@ -42,5 +46,6 @@
 
   <export>
     <build_type>ament_cmake</build_type>
+    <nav2_core plugin="${prefix}/recovery_plugin.xml" />
   </export>
 </package>

nav2_recoveries/src/back_up.cpp → nav2_recoveries/plugins/back_up.cpp

@@ -16,25 +16,29 @@
 #include <ctime>
 #include <memory>
 
-#include "nav2_recoveries/back_up.hpp"
+#include "back_up.hpp"
 
 using namespace std::chrono_literals;
 
 namespace nav2_recoveries
 {
 
-BackUp::BackUp(rclcpp::Node::SharedPtr & node, std::shared_ptr<tf2_ros::Buffer> tf)
-: Recovery<BackUpAction>(node, "BackUp", tf)
+BackUp::BackUp()
+: Recovery<BackUpAction>()
 {
   simulate_ahead_time_ = 2.0;
-  node_->declare_parameter("simulate_ahead_time");
-  node_->get_parameter("simulate_ahead_time", simulate_ahead_time_);
 }
 
 BackUp::~BackUp()
 {
 }
 
+void BackUp::onConfigure()
+{
+  node_->declare_parameter("simulate_ahead_time", simulate_ahead_time_);
+  simulate_ahead_time_ = node_->get_parameter("simulate_ahead_time").as_double();
+}
+
 Status BackUp::onRun(const std::shared_ptr<const BackUpAction::Goal> command)
 {
   if (command->target.y != 0.0 || command->target.z != 0.0) {
@@ -44,7 +48,7 @@ Status BackUp::onRun(const std::shared_ptr<const BackUpAction::Goal> command)
 
   command_x_ = command->target.x;
 
-  if (!nav2_util::getCurrentPose(initial_pose_, tf_, "odom")) {
+  if (!nav2_util::getCurrentPose(initial_pose_, *tf_, "odom")) {
     RCLCPP_ERROR(node_->get_logger(), "Initial robot pose is not available.");
     return Status::FAILED;
   }
@@ -55,7 +59,7 @@ Status BackUp::onRun(const std::shared_ptr<const BackUpAction::Goal> command)
 Status BackUp::onCycleUpdate()
 {
   geometry_msgs::msg::PoseStamped current_pose;
-  if (!nav2_util::getCurrentPose(current_pose, tf_, "odom")) {
+  if (!nav2_util::getCurrentPose(current_pose, *tf_, "odom")) {
     RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available.");
     return Status::FAILED;
   }
@@ -118,3 +122,6 @@ bool BackUp::isCollisionFree(
 }
 
 }  // namespace nav2_recoveries
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(nav2_recoveries::BackUp, nav2_core::Recovery)

nav2_recoveries/include/nav2_recoveries/back_up.hpp → nav2_recoveries/plugins/back_up.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef NAV2_RECOVERIES__BACK_UP_HPP_
-#define NAV2_RECOVERIES__BACK_UP_HPP_
+#ifndef NAV2_RECOVERIES__PLUGINS__BACK_UP_HPP_
+#define NAV2_RECOVERIES__PLUGINS__BACK_UP_HPP_
 
 #include <chrono>
 #include <memory>
@@ -28,7 +28,7 @@ using BackUpAction = nav2_msgs::action::BackUp;
 class BackUp : public Recovery<BackUpAction>
 {
 public:
-  explicit BackUp(rclcpp::Node::SharedPtr & node, std::shared_ptr<tf2_ros::Buffer> tf);
+  BackUp();
   ~BackUp();
 
   Status onRun(const std::shared_ptr<const BackUpAction::Goal> command) override;
@@ -41,6 +41,8 @@ protected:
     const geometry_msgs::msg::Twist & cmd_vel,
     geometry_msgs::msg::Pose2D & pose2d);
 
+  void onConfigure() override;
+
   double min_linear_vel_;
   double max_linear_vel_;
   double linear_acc_lim_;
@@ -52,4 +54,4 @@ protected:
 
 }  // namespace nav2_recoveries
 
-#endif  // NAV2_RECOVERIES__BACK_UP_HPP_
+#endif  // NAV2_RECOVERIES__PLUGINS__BACK_UP_HPP_

nav2_recoveries/src/spin.cpp → nav2_recoveries/plugins/spin.cpp

@@ -19,7 +19,7 @@
 #include <algorithm>
 #include <memory>
 
-#include "nav2_recoveries/spin.hpp"
+#include "spin.hpp"
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wpedantic"
 #include "tf2/utils.h"
@@ -32,8 +32,8 @@ using namespace std::chrono_literals;
 namespace nav2_recoveries
 {
 
-Spin::Spin(rclcpp::Node::SharedPtr & node, std::shared_ptr<tf2_ros::Buffer> tf)
-: Recovery<SpinAction>(node, "Spin", tf)
+Spin::Spin()
+: Recovery<SpinAction>()
 {
   // TODO(orduno) #378 Pull values from the robot
   max_rotational_vel_ = 1.0;
@@ -50,10 +50,11 @@ Spin::~Spin()
 Status Spin::onRun(const std::shared_ptr<const SpinAction::Goal> command)
 {
   geometry_msgs::msg::PoseStamped current_pose;
-  if (!nav2_util::getCurrentPose(current_pose, tf_, "odom")) {
+  if (!nav2_util::getCurrentPose(current_pose, *tf_, "odom")) {
     RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available.");
     return Status::FAILED;
   }
+
   initial_yaw_ = tf2::getYaw(current_pose.pose.orientation);
 
   cmd_yaw_ = -command->target_yaw;
@@ -65,7 +66,7 @@ Status Spin::onRun(const std::shared_ptr<const SpinAction::Goal> command)
 Status Spin::onCycleUpdate()
 {
   geometry_msgs::msg::PoseStamped current_pose;
-  if (!nav2_util::getCurrentPose(current_pose, tf_, "odom")) {
+  if (!nav2_util::getCurrentPose(current_pose, *tf_, "odom")) {
     RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available.");
     return Status::FAILED;
   }
@@ -131,3 +132,6 @@ bool Spin::isCollisionFree(
 }
 
 }  // namespace nav2_recoveries
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(nav2_recoveries::Spin, nav2_core::Recovery)

nav2_recoveries/include/nav2_recoveries/spin.hpp → nav2_recoveries/plugins/spin.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef NAV2_RECOVERIES__SPIN_HPP_
-#define NAV2_RECOVERIES__SPIN_HPP_
+#ifndef NAV2_RECOVERIES__PLUGINS__SPIN_HPP_
+#define NAV2_RECOVERIES__PLUGINS__SPIN_HPP_
 
 #include <chrono>
 #include <string>
@@ -30,7 +30,7 @@ using SpinAction = nav2_msgs::action::Spin;
 class Spin : public Recovery<SpinAction>
 {
 public:
-  explicit Spin(rclcpp::Node::SharedPtr & node, std::shared_ptr<tf2_ros::Buffer> tf);
+  Spin();
   ~Spin();
 
   Status onRun(const std::shared_ptr<const SpinAction::Goal> command) override;
@@ -53,4 +53,4 @@ protected:
 
 }  // namespace nav2_recoveries
 
-#endif  // NAV2_RECOVERIES__SPIN_HPP_
+#endif  // NAV2_RECOVERIES__PLUGINS__SPIN_HPP_

nav2_recoveries/recovery_plugin.xml

+<class_libraries>
+	<library path="nav2_spin_recovery">
+	  <class name="nav2_recoveries/Spin" type="nav2_recoveries::Spin" base_class_type="nav2_core::Recovery">
+	    <description></description>
+	  </class>
+	</library>
+
+	<library path="nav2_backup_recovery">
+	  <class name="nav2_recoveries/BackUp" type="nav2_recoveries::BackUp" base_class_type="nav2_core::Recovery">
+	    <description></description>
+	  </class>
+	</library>
+</class_libraries>
\ No newline at end of file

nav2_recoveries/src/main.cpp

 // Copyright (c) 2018 Intel Corporation
+// Copyright (c) 2019 Samsung Research America
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -15,37 +16,15 @@
 #include <string>
 #include <memory>
 
+#include "nav2_recoveries/recovery_server.hpp"
 #include "rclcpp/rclcpp.hpp"
-#include "tf2_ros/transform_listener.h"
-#include "tf2_ros/create_timer_ros.h"
-#include "nav2_recoveries/back_up.hpp"
-#include "nav2_recoveries/spin.hpp"
 
 int main(int argc, char ** argv)
 {
   rclcpp::init(argc, argv);
+  auto recoveries_node = std::make_shared<recovery_server::RecoveryServer>();
 
-  auto recoveries_node = rclcpp::Node::make_shared("recoveries");
-
-  auto tf_buffer = std::make_shared<tf2_ros::Buffer>(recoveries_node->get_clock());
-  auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
-    recoveries_node->get_node_base_interface(),
-    recoveries_node->get_node_timers_interface());
-  tf_buffer->setCreateTimerInterface(timer_interface);
-  auto tf_listener = std::make_shared<tf2_ros::TransformListener>(*tf_buffer);
-
-  recoveries_node->declare_parameter(
-    "costmap_topic", rclcpp::ParameterValue(std::string("local_costmap/costmap_raw")));
-  recoveries_node->declare_parameter(
-    "footprint_topic", rclcpp::ParameterValue(std::string("local_costmap/published_footprint")));
-
-  auto spin = std::make_shared<nav2_recoveries::Spin>(
-    recoveries_node, tf_buffer);
-
-  auto back_up = std::make_shared<nav2_recoveries::BackUp>(
-    recoveries_node, tf_buffer);
-
-  rclcpp::spin(recoveries_node);
+  rclcpp::spin(recoveries_node->get_node_base_interface());
   rclcpp::shutdown();
 
   return 0;

nav2_recoveries/src/recovery_server.cpp

+// Copyright (c) 2018 Samsung Research America
+//
+// 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. Reserved.
+
+#include <memory>
+#include <string>
+#include <vector>
+#include <utility>
+#include "nav2_recoveries/recovery_server.hpp"
+
+namespace recovery_server
+{
+
+RecoveryServer::RecoveryServer()
+: nav2_util::LifecycleNode("nav2_recoveries", "", true),
+  plugin_loader_("nav2_core", "nav2_core::Recovery")
+{
+}
+
+
+RecoveryServer::~RecoveryServer()
+{
+}
+
+nav2_util::CallbackReturn
+RecoveryServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Configuring");
+
+  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());
+  tf_->setCreateTimerInterface(timer_interface);
+  transform_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_);
+
+  declare_parameter("costmap_topic",
+    rclcpp::ParameterValue(std::string("local_costmap/costmap_raw")));
+  declare_parameter("footprint_topic",
+    rclcpp::ParameterValue(std::string("local_costmap/published_footprint")));
+
+  std::vector<std::string> plugin_names{std::string("Spin"),
+    std::string("BackUp")};
+  std::vector<std::string> plugin_types{std::string("nav2_recoveries/Spin"),
+    std::string("nav2_recoveries/BackUp")};
+
+  declare_parameter("plugin_names",
+    rclcpp::ParameterValue(plugin_names));
+  declare_parameter("plugin_types",
+    rclcpp::ParameterValue(plugin_types));
+
+  plugin_names_ = get_parameter("plugin_names").as_string_array();
+  plugin_types_ = get_parameter("plugin_types").as_string_array();
+
+  loadRecoveryPlugins();
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+
+void
+RecoveryServer::loadRecoveryPlugins()
+{
+  auto node = shared_from_this();
+
+  for (uint i = 0; i != plugin_names_.size(); i++) {
+    try {
+      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_);
+    } 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(),
+        ex.what());
+      exit(-1);
+    }
+  }
+}
+
+nav2_util::CallbackReturn
+RecoveryServer::on_activate(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Activating");
+  std::vector<pluginlib::UniquePtr<nav2_core::Recovery>>::iterator iter;
+  for (iter = recoveries_.begin(); iter != recoveries_.end(); ++iter) {
+    (*iter)->activate();
+  }
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+RecoveryServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Deactivating");
+
+  std::vector<pluginlib::UniquePtr<nav2_core::Recovery>>::iterator iter;
+  for (iter = recoveries_.begin(); iter != recoveries_.end(); ++iter) {
+    (*iter)->deactivate();
+  }
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+RecoveryServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(get_logger(), "Cleaning up");
+
+  std::vector<pluginlib::UniquePtr<nav2_core::Recovery>>::iterator iter;
+  for (iter = recoveries_.begin(); iter != recoveries_.end(); ++iter) {
+    (*iter)->cleanup();
+  }
+
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+RecoveryServer::on_error(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_FATAL(get_logger(), "Lifecycle node entered error state");
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+nav2_util::CallbackReturn
+RecoveryServer::on_shutdown(const rclcpp_lifecycle::State &)
+{
+  RCLCPP_INFO(get_logger(), "Shutting down");
+  return nav2_util::CallbackReturn::SUCCESS;
+}
+
+}  // end namespace recovery_server