Recovery to behavior (ros-navigation#2867)

* rename recovery_server to behavior_server * renaming * renamming * renaming * readme update * renamming tests
wnsrl970219 · Apr 4, 2022 · 4234a6e · 4234a6e
1 parent 97d3149
commit 4234a6e
Show file tree

Hide file tree

Showing 53 changed files with 452 additions and 451 deletions.
diff --git a/nav2_recoveries/CHANGELOG.rst → nav2_behaviors/CHANGELOG.rst b/nav2_recoveries/CHANGELOG.rst → nav2_behaviors/CHANGELOG.rst
diff --git a/nav2_recoveries/CMakeLists.txt → nav2_behaviors/CMakeLists.txt b/nav2_recoveries/CMakeLists.txt → nav2_behaviors/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.5)
-project(nav2_recoveries)
+project(nav2_behaviors)
 
 find_package(ament_cmake REQUIRED)
 find_package(nav2_common REQUIRED)
@@ -25,8 +25,8 @@ include_directories(
   include
 )
 
-set(library_name recoveries_server_core)
-set(executable_name recoveries_server)
+set(library_name behavior_server_core)
+set(executable_name behavior_server)
 
 set(dependencies
   rclcpp
@@ -47,35 +47,35 @@ set(dependencies
 )
 
 # plugins
-add_library(nav2_spin_recovery SHARED
+add_library(nav2_spin_behavior SHARED
   plugins/spin.cpp
 )
 
-ament_target_dependencies(nav2_spin_recovery
+ament_target_dependencies(nav2_spin_behavior
   ${dependencies}
 )
 
-add_library(nav2_backup_recovery SHARED
+add_library(nav2_backup_behavior SHARED
   plugins/back_up.cpp
 )
 
-ament_target_dependencies(nav2_backup_recovery
+ament_target_dependencies(nav2_backup_behavior
   ${dependencies}
 )
 
-add_library(nav2_wait_recovery SHARED
+add_library(nav2_wait_behavior SHARED
   plugins/wait.cpp
 )
 
-ament_target_dependencies(nav2_wait_recovery
+ament_target_dependencies(nav2_wait_behavior
   ${dependencies}
 )
 
-pluginlib_export_plugin_description_file(nav2_core recovery_plugin.xml)
+pluginlib_export_plugin_description_file(nav2_core behavior_plugin.xml)
 
 # Library
 add_library(${library_name} SHARED
-  src/recovery_server.cpp
+  src/behavior_server.cpp
 )
 
 ament_target_dependencies(${library_name}
@@ -93,12 +93,12 @@ ament_target_dependencies(${executable_name}
   ${dependencies}
 )
 
-rclcpp_components_register_nodes(${library_name} "recovery_server::RecoveryServer")
+rclcpp_components_register_nodes(${library_name} "behavior_server::BehaviorServer")
 
 install(TARGETS ${library_name}
-                nav2_backup_recovery
-                nav2_spin_recovery
-                nav2_wait_recovery
+                nav2_backup_behavior
+                nav2_spin_behavior
+                nav2_wait_behavior
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
@@ -112,7 +112,7 @@ install(DIRECTORY include/
   DESTINATION include/
 )
 
-install(FILES recovery_plugin.xml
+install(FILES behavior_plugin.xml
   DESTINATION share/${PROJECT_NAME}
 )
 
@@ -125,9 +125,9 @@ endif()
 
 ament_export_include_directories(include)
 ament_export_libraries(${library_name}
-  nav2_backup_recovery
-  nav2_spin_recovery
-  nav2_wait_recovery
+  nav2_backup_behavior
+  nav2_spin_behavior
+  nav2_wait_behavior
 )
 ament_export_dependencies(${dependencies})
 ament_package()
diff --git a/nav2_behaviors/README.md b/nav2_behaviors/README.md
@@ -0,0 +1,15 @@
+# Behaviors
+
+The `nav2_behaviors` package implements a task server for executing behaviors.
+
+The package defines:
+- A `TimedBehavior` template which is used as a base class to implement specific timed behavior action server - but not required.
+- The `BackUp`, `Spin` and `Wait` behaviors.
+
+The only required class a behavior must derive from is the `nav2_core/behavior.hpp` class, which implements the pluginlib interface the behavior server will use to dynamically load your behavior. The `nav2_behaviors/timed_behavior.hpp` derives from this class and implements a generic action server for a timed behavior behavior (e.g. calls an implmentation function on a regular time interval to compute a value) but **this is not required** if it is not helpful. A behavior does not even need to be an action if you do not wish, it may be a service or other interface. However, most motion and behavior primitives are probably long-running and make sense to be modeled as actions, so the provided `timed_behavior.hpp` helps in managing the complexity to simplify new behavior development, described more below.
+
+The value of the centralized behavior server is to **share resources** amongst several behaviors that would otherwise be independent nodes. Subscriptions to TF, costmaps, and more can be quite heavy and add non-trivial compute costs to a robot system. By combining these independent behaviors into a single server, they may share these resources while retaining complete independence in execution and interface.
+
+See its [Configuration Guide Page](https://navigation.ros.org/configuration/packages/configuring-behavior-server.html) for additional parameter descriptions and a [tutorial about writing behaviors](https://navigation.ros.org/plugin_tutorials/docs/writing_new_behavior_plugin.html).
+
+See the [Navigation Plugin list](https://navigation.ros.org/plugins/index.html) for a list of the currently known and available planner plugins.
diff --git a/nav2_behaviors/behavior_plugin.xml b/nav2_behaviors/behavior_plugin.xml
@@ -0,0 +1,19 @@
+<class_libraries>
+	<library path="nav2_spin_behavior">
+	  <class name="nav2_behaviors/Spin" type="nav2_behaviors::Spin" base_class_type="nav2_core::Behavior">
+	    <description></description>
+	  </class>
+	</library>
+
+	<library path="nav2_backup_behavior">
+	  <class name="nav2_behaviors/BackUp" type="nav2_behaviors::BackUp" base_class_type="nav2_core::Behavior">
+	    <description></description>
+	  </class>
+	</library>
+
+	<library path="nav2_wait_behavior">
+	  <class name="nav2_behaviors/Wait" type="nav2_behaviors::Wait" base_class_type="nav2_core::Behavior">
+	    <description></description>
+	  </class>
+	</library>
+</class_libraries>
diff --git a/...clude/nav2_recoveries/recovery_server.hpp → ...nclude/nav2_behaviors/behavior_server.hpp b/...clude/nav2_recoveries/recovery_server.hpp → ...nclude/nav2_behaviors/behavior_server.hpp
@@ -23,33 +23,33 @@
 #include "tf2_ros/create_timer_ros.h"
 #include "pluginlib/class_loader.hpp"
 #include "pluginlib/class_list_macros.hpp"
-#include "nav2_core/recovery.hpp"
+#include "nav2_core/behavior.hpp"
 
-#ifndef NAV2_RECOVERIES__RECOVERY_SERVER_HPP_
-#define NAV2_RECOVERIES__RECOVERY_SERVER_HPP_
+#ifndef NAV2_BEHAVIORS__BEHAVIOR_SERVER_HPP_
+#define NAV2_BEHAVIORS__BEHAVIOR_SERVER_HPP_
 
-namespace recovery_server
+namespace behavior_server
 {
 
 /**
- * @class recovery_server::RecoveryServer
- * @brief An server hosting a map of recovery plugins
+ * @class behavior_server::BehaviorServer
+ * @brief An server hosting a map of behavior plugins
  */
-class RecoveryServer : public nav2_util::LifecycleNode
+class BehaviorServer : public nav2_util::LifecycleNode
 {
 public:
   /**
-   * @brief A constructor for recovery_server::RecoveryServer
+   * @brief A constructor for behavior_server::BehaviorServer
    * @param options Additional options to control creation of the node.
    */
-  explicit RecoveryServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
-  ~RecoveryServer();
+  explicit BehaviorServer(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
+  ~BehaviorServer();
 
   /**
-   * @brief Loads recovery plugins from parameter file
+   * @brief Loads behavior plugins from parameter file
    * @return bool if successfully loaded the plugins
    */
-  bool loadRecoveryPlugins();
+  bool loadBehaviorPlugins();
 
 protected:
   /**
@@ -81,19 +81,19 @@ class RecoveryServer : public nav2_util::LifecycleNode
   std::shared_ptr<tf2_ros::TransformListener> transform_listener_;
 
   // Plugins
-  pluginlib::ClassLoader<nav2_core::Recovery> plugin_loader_;
-  std::vector<pluginlib::UniquePtr<nav2_core::Recovery>> recoveries_;
+  pluginlib::ClassLoader<nav2_core::Behavior> plugin_loader_;
+  std::vector<pluginlib::UniquePtr<nav2_core::Behavior>> behaviors_;
   std::vector<std::string> default_ids_;
   std::vector<std::string> default_types_;
-  std::vector<std::string> recovery_ids_;
-  std::vector<std::string> recovery_types_;
+  std::vector<std::string> behavior_ids_;
+  std::vector<std::string> behavior_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::CostmapTopicCollisionChecker> collision_checker_;
 };
 
-}  // namespace recovery_server
+}  // namespace behavior_server
 
-#endif  // NAV2_RECOVERIES__RECOVERY_SERVER_HPP_
+#endif  // NAV2_BEHAVIORS__BEHAVIOR_SERVER_HPP_
diff --git a/...ries/include/nav2_recoveries/recovery.hpp → ...include/nav2_behaviors/timed_behavior.hpp b/...ries/include/nav2_recoveries/recovery.hpp → ...include/nav2_behaviors/timed_behavior.hpp
@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef NAV2_RECOVERIES__RECOVERY_HPP_
-#define NAV2_RECOVERIES__RECOVERY_HPP_
+#ifndef NAV2_BEHAVIORS__TIMED_BEHAVIOR_HPP_
+#define NAV2_BEHAVIORS__TIMED_BEHAVIOR_HPP_
 
 #include <memory>
 #include <string>
@@ -29,13 +29,13 @@
 #include "geometry_msgs/msg/twist.hpp"
 #include "nav2_util/simple_action_server.hpp"
 #include "nav2_util/robot_utils.hpp"
-#include "nav2_core/recovery.hpp"
+#include "nav2_core/behavior.hpp"
 #pragma GCC diagnostic push
 #pragma GCC diagnostic ignored "-Wpedantic"
 #include "tf2/utils.h"
 #pragma GCC diagnostic pop
 
-namespace nav2_recoveries
+namespace nav2_behaviors
 {
 
 enum class Status : int8_t
@@ -48,26 +48,26 @@ enum class Status : int8_t
 using namespace std::chrono_literals;  //NOLINT
 
 /**
- * @class nav2_recoveries::Recovery
- * @brief An action server recovery base class implementing the action server and basic factory.
+ * @class nav2_behaviors::Behavior
+ * @brief An action server Behavior base class implementing the action server and basic factory.
  */
 template<typename ActionT>
-class Recovery : public nav2_core::Recovery
+class TimedBehavior : public nav2_core::Behavior
 {
 public:
   using ActionServer = nav2_util::SimpleActionServer<ActionT>;
 
   /**
-   * @brief A Recovery constructor
+   * @brief A TimedBehavior constructor
    */
-  Recovery()
+  TimedBehavior()
   : action_server_(nullptr),
     cycle_frequency_(10.0),
     enabled_(false)
   {
   }
 
-  virtual ~Recovery()
+  virtual ~TimedBehavior()
   {
   }
 
@@ -80,7 +80,7 @@ class Recovery : public nav2_core::Recovery
   // This is the method derived classes should mainly implement
   // and will be called cyclically while it returns RUNNING.
   // Implement the behavior such that it runs some unit of work on each call
-  // and provides a status. The recovery will finish once SUCCEEDED is returned
+  // and provides a status. The Behavior will finish once SUCCEEDED is returned
   // It's up to the derived class to define the final commanded velocity.
   virtual Status onCycleUpdate() = 0;
 
@@ -109,7 +109,7 @@ class Recovery : public nav2_core::Recovery
 
     RCLCPP_INFO(logger_, "Configuring %s", name.c_str());
 
-    recovery_name_ = name;
+    behavior_name_ = name;
     tf_ = tf;
 
     node->get_parameter("cycle_frequency", cycle_frequency_);
@@ -118,8 +118,8 @@ class Recovery : public nav2_core::Recovery
     node->get_parameter("transform_tolerance", transform_tolerance_);
 
     action_server_ = std::make_shared<ActionServer>(
-      node, recovery_name_,
-      std::bind(&Recovery::execute, this));
+      node, behavior_name_,
+      std::bind(&TimedBehavior::execute, this));
 
     collision_checker_ = collision_checker;
 
@@ -139,7 +139,7 @@ class Recovery : public nav2_core::Recovery
   // Activate server on lifecycle transition
   void activate() override
   {
-    RCLCPP_INFO(logger_, "Activating %s", recovery_name_.c_str());
+    RCLCPP_INFO(logger_, "Activating %s", behavior_name_.c_str());
 
     vel_pub_->on_activate();
     action_server_->activate();
@@ -157,7 +157,7 @@ class Recovery : public nav2_core::Recovery
 protected:
   rclcpp_lifecycle::LifecycleNode::WeakPtr node_;
 
-  std::string recovery_name_;
+  std::string behavior_name_;
   rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr vel_pub_;
   std::shared_ptr<ActionServer> action_server_;
   std::shared_ptr<nav2_costmap_2d::CostmapTopicCollisionChecker> collision_checker_;
@@ -173,13 +173,13 @@ class Recovery : public nav2_core::Recovery
   rclcpp::Clock steady_clock_{RCL_STEADY_TIME};
 
   // Logger
-  rclcpp::Logger logger_{rclcpp::get_logger("nav2_recoveries")};
+  rclcpp::Logger logger_{rclcpp::get_logger("nav2_behaviors")};
 
-  // Main execution callbacks for the action server implementation calling the recovery's
+  // Main execution callbacks for the action server implementation calling the Behavior's
   // onRun and cycle functions to execute a specific behavior
   void execute()
   {
-    RCLCPP_INFO(logger_, "Attempting %s", recovery_name_.c_str());
+    RCLCPP_INFO(logger_, "Attempting %s", behavior_name_.c_str());
 
     if (!enabled_) {
       RCLCPP_WARN(
@@ -191,7 +191,7 @@ class Recovery : public nav2_core::Recovery
     if (onRun(action_server_->get_current_goal()) != Status::SUCCEEDED) {
       RCLCPP_INFO(
         logger_,
-        "Initial checks failed for %s", recovery_name_.c_str());
+        "Initial checks failed for %s", behavior_name_.c_str());
       action_server_->terminate_current();
       return;
     }
@@ -206,7 +206,7 @@ class Recovery : public nav2_core::Recovery
       auto timer = node->create_wall_timer(
         1s,
         [&]()
-        {RCLCPP_INFO(logger_, "%s running...", recovery_name_.c_str());});
+        {RCLCPP_INFO(logger_, "%s running...", behavior_name_.c_str());});
     }
 
     auto start_time = steady_clock_.now();
@@ -218,19 +218,19 @@ class Recovery : public nav2_core::Recovery
 
     while (rclcpp::ok()) {
       if (action_server_->is_cancel_requested()) {
-        RCLCPP_INFO(logger_, "Canceling %s", recovery_name_.c_str());
+        RCLCPP_INFO(logger_, "Canceling %s", behavior_name_.c_str());
         stopRobot();
         result->total_elapsed_time = steady_clock_.now() - start_time;
         action_server_->terminate_all(result);
         return;
       }
 
-      // TODO(orduno) #868 Enable preempting a Recovery on-the-fly without stopping
+      // TODO(orduno) #868 Enable preempting a Behavior on-the-fly without stopping
       if (action_server_->is_preempt_requested()) {
         RCLCPP_ERROR(
           logger_, "Received a preemption request for %s,"
           " however feature is currently not implemented. Aborting and stopping.",
-          recovery_name_.c_str());
+          behavior_name_.c_str());
         stopRobot();
         result->total_elapsed_time = steady_clock_.now() - start_time;
         action_server_->terminate_current(result);
@@ -241,13 +241,13 @@ class Recovery : public nav2_core::Recovery
         case Status::SUCCEEDED:
           RCLCPP_INFO(
             logger_,
-            "%s completed successfully", recovery_name_.c_str());
+            "%s completed successfully", behavior_name_.c_str());
           result->total_elapsed_time = steady_clock_.now() - start_time;
           action_server_->succeeded_current(result);
           return;
 
         case Status::FAILED:
-          RCLCPP_WARN(logger_, "%s failed", recovery_name_.c_str());
+          RCLCPP_WARN(logger_, "%s failed", behavior_name_.c_str());
           result->total_elapsed_time = steady_clock_.now() - start_time;
           action_server_->terminate_current(result);
           return;
@@ -273,6 +273,6 @@ class Recovery : public nav2_core::Recovery
   }
 };
 
-}  // namespace nav2_recoveries
+}  // namespace nav2_behaviors
 
-#endif  // NAV2_RECOVERIES__RECOVERY_HPP_
+#endif  // NAV2_BEHAVIORS__TIMED_BEHAVIOR_HPP_