diff --git a/nav2_recoveries/CHANGELOG.rst b/nav2_behaviors/CHANGELOG.rst similarity index 100% rename from nav2_recoveries/CHANGELOG.rst rename to nav2_behaviors/CHANGELOG.rst diff --git a/nav2_recoveries/CMakeLists.txt b/nav2_behaviors/CMakeLists.txt similarity index 72% rename from nav2_recoveries/CMakeLists.txt rename to nav2_behaviors/CMakeLists.txt index 8af7c5b2f03..4cea936a67d 100644 --- a/nav2_recoveries/CMakeLists.txt +++ b/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 new file mode 100644 index 00000000000..79f62a53b82 --- /dev/null +++ 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 new file mode 100644 index 00000000000..6c4e4797a6c --- /dev/null +++ b/nav2_behaviors/behavior_plugin.xml @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp b/nav2_behaviors/include/nav2_behaviors/behavior_server.hpp similarity index 72% rename from nav2_recoveries/include/nav2_recoveries/recovery_server.hpp rename to nav2_behaviors/include/nav2_behaviors/behavior_server.hpp index dfcb95bb0a4..ea7a2d4cfa2 100644 --- a/nav2_recoveries/include/nav2_recoveries/recovery_server.hpp +++ b/nav2_behaviors/include/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,12 +81,12 @@ class RecoveryServer : public nav2_util::LifecycleNode std::shared_ptr transform_listener_; // Plugins - pluginlib::ClassLoader plugin_loader_; - std::vector> recoveries_; + pluginlib::ClassLoader plugin_loader_; + std::vector> behaviors_; std::vector default_ids_; std::vector default_types_; - std::vector recovery_ids_; - std::vector recovery_types_; + std::vector behavior_ids_; + std::vector behavior_types_; // Utilities std::unique_ptr costmap_sub_; @@ -94,6 +94,6 @@ class RecoveryServer : public nav2_util::LifecycleNode std::shared_ptr collision_checker_; }; -} // namespace recovery_server +} // namespace behavior_server -#endif // NAV2_RECOVERIES__RECOVERY_SERVER_HPP_ +#endif // NAV2_BEHAVIORS__BEHAVIOR_SERVER_HPP_ diff --git a/nav2_recoveries/include/nav2_recoveries/recovery.hpp b/nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp similarity index 83% rename from nav2_recoveries/include/nav2_recoveries/recovery.hpp rename to nav2_behaviors/include/nav2_behaviors/timed_behavior.hpp index 1a7060e0b97..d696607feb0 100644 --- a/nav2_recoveries/include/nav2_recoveries/recovery.hpp +++ b/nav2_behaviors/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 #include @@ -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 -class Recovery : public nav2_core::Recovery +class TimedBehavior : public nav2_core::Behavior { public: using ActionServer = nav2_util::SimpleActionServer; /** - * @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( - 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::SharedPtr vel_pub_; std::shared_ptr action_server_; std::shared_ptr 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_ diff --git a/nav2_recoveries/package.xml b/nav2_behaviors/package.xml similarity index 95% rename from nav2_recoveries/package.xml rename to nav2_behaviors/package.xml index ca8e0b0af92..5828135c4d4 100644 --- a/nav2_recoveries/package.xml +++ b/nav2_behaviors/package.xml @@ -1,7 +1,7 @@ - nav2_recoveries + nav2_behaviors 1.0.0 TODO Carlos Orduno @@ -43,6 +43,6 @@ ament_cmake - + diff --git a/nav2_recoveries/plugins/back_up.cpp b/nav2_behaviors/plugins/back_up.cpp similarity index 96% rename from nav2_recoveries/plugins/back_up.cpp rename to nav2_behaviors/plugins/back_up.cpp index d078b6c8b8b..aafaaf57859 100644 --- a/nav2_recoveries/plugins/back_up.cpp +++ b/nav2_behaviors/plugins/back_up.cpp @@ -22,11 +22,11 @@ using namespace std::chrono_literals; -namespace nav2_recoveries +namespace nav2_behaviors { BackUp::BackUp() -: Recovery(), +: TimedBehavior(), feedback_(std::make_shared()) { } @@ -159,7 +159,7 @@ bool BackUp::isCollisionFree( return true; } -} // namespace nav2_recoveries +} // namespace nav2_behaviors #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(nav2_recoveries::BackUp, nav2_core::Recovery) +PLUGINLIB_EXPORT_CLASS(nav2_behaviors::BackUp, nav2_core::Behavior) diff --git a/nav2_recoveries/plugins/back_up.hpp b/nav2_behaviors/plugins/back_up.hpp similarity index 76% rename from nav2_recoveries/plugins/back_up.hpp rename to nav2_behaviors/plugins/back_up.hpp index 7c979d153a2..cb9d565b77f 100644 --- a/nav2_recoveries/plugins/back_up.hpp +++ b/nav2_behaviors/plugins/back_up.hpp @@ -12,28 +12,28 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_RECOVERIES__PLUGINS__BACK_UP_HPP_ -#define NAV2_RECOVERIES__PLUGINS__BACK_UP_HPP_ +#ifndef NAV2_BEHAVIORS__PLUGINS__BACK_UP_HPP_ +#define NAV2_BEHAVIORS__PLUGINS__BACK_UP_HPP_ #include #include -#include "nav2_recoveries/recovery.hpp" +#include "nav2_behaviors/timed_behavior.hpp" #include "nav2_msgs/action/back_up.hpp" -namespace nav2_recoveries +namespace nav2_behaviors { using BackUpAction = nav2_msgs::action::BackUp; /** - * @class nav2_recoveries::BackUp - * @brief An action server recovery for spinning in + * @class nav2_behaviors::BackUp + * @brief An action server Behavior for spinning in */ -class BackUp : public Recovery +class BackUp : public TimedBehavior { public: /** - * @brief A constructor for nav2_recoveries::BackUp + * @brief A constructor for nav2_behaviors::BackUp */ BackUp(); ~BackUp(); @@ -41,13 +41,13 @@ class BackUp : public Recovery /** * @brief Initialization to run behavior * @param command Goal to execute - * @return Status of recovery + * @return Status of behavior */ Status onRun(const std::shared_ptr command) override; /** * @brief Loop function to run behavior - * @return Status of recovery + * @return Status of behavior */ Status onCycleUpdate() override; @@ -65,7 +65,7 @@ class BackUp : public Recovery geometry_msgs::msg::Pose2D & pose2d); /** - * @brief Configuration of recovery action + * @brief Configuration of behavior action */ void onConfigure() override; @@ -83,6 +83,6 @@ class BackUp : public Recovery BackUpAction::Feedback::SharedPtr feedback_; }; -} // namespace nav2_recoveries +} // namespace nav2_behaviors -#endif // NAV2_RECOVERIES__PLUGINS__BACK_UP_HPP_ +#endif // NAV2_BEHAVIORS__PLUGINS__BACK_UP_HPP_ diff --git a/nav2_recoveries/plugins/spin.cpp b/nav2_behaviors/plugins/spin.cpp similarity index 96% rename from nav2_recoveries/plugins/spin.cpp rename to nav2_behaviors/plugins/spin.cpp index 893e3b58e5d..641f59064e0 100644 --- a/nav2_recoveries/plugins/spin.cpp +++ b/nav2_behaviors/plugins/spin.cpp @@ -31,11 +31,11 @@ using namespace std::chrono_literals; -namespace nav2_recoveries +namespace nav2_behaviors { Spin::Spin() -: Recovery(), +: TimedBehavior(), feedback_(std::make_shared()), prev_yaw_(0.0) { @@ -89,7 +89,7 @@ Status Spin::onRun(const std::shared_ptr command) cmd_yaw_ = command->target_yaw; RCLCPP_INFO( - logger_, "Turning %0.2f for spin recovery.", + logger_, "Turning %0.2f for spin behavior.", cmd_yaw_); command_time_allowance_ = command->time_allowance; @@ -188,7 +188,7 @@ bool Spin::isCollisionFree( return true; } -} // namespace nav2_recoveries +} // namespace nav2_behaviors #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(nav2_recoveries::Spin, nav2_core::Recovery) +PLUGINLIB_EXPORT_CLASS(nav2_behaviors::Spin, nav2_core::Behavior) diff --git a/nav2_recoveries/plugins/spin.hpp b/nav2_behaviors/plugins/spin.hpp similarity index 77% rename from nav2_recoveries/plugins/spin.hpp rename to nav2_behaviors/plugins/spin.hpp index 632f585c7fe..7174c91f467 100644 --- a/nav2_recoveries/plugins/spin.hpp +++ b/nav2_behaviors/plugins/spin.hpp @@ -12,30 +12,30 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_RECOVERIES__PLUGINS__SPIN_HPP_ -#define NAV2_RECOVERIES__PLUGINS__SPIN_HPP_ +#ifndef NAV2_BEHAVIORS__PLUGINS__SPIN_HPP_ +#define NAV2_BEHAVIORS__PLUGINS__SPIN_HPP_ #include #include #include -#include "nav2_recoveries/recovery.hpp" +#include "nav2_behaviors/timed_behavior.hpp" #include "nav2_msgs/action/spin.hpp" #include "geometry_msgs/msg/quaternion.hpp" -namespace nav2_recoveries +namespace nav2_behaviors { using SpinAction = nav2_msgs::action::Spin; /** - * @class nav2_recoveries::Spin - * @brief An action server recovery for spinning in + * @class nav2_behaviors::Spin + * @brief An action server behavior for spinning in */ -class Spin : public Recovery +class Spin : public TimedBehavior { public: /** - * @brief A constructor for nav2_recoveries::Spin + * @brief A constructor for nav2_behaviors::Spin */ Spin(); ~Spin(); @@ -43,18 +43,18 @@ class Spin : public Recovery /** * @brief Initialization to run behavior * @param command Goal to execute - * @return Status of recovery + * @return Status of behavior */ Status onRun(const std::shared_ptr command) override; /** - * @brief Configuration of recovery action + * @brief Configuration of behavior action */ void onConfigure() override; /** * @brief Loop function to run behavior - * @return Status of recovery + * @return Status of behavior */ Status onCycleUpdate() override; @@ -84,6 +84,6 @@ class Spin : public Recovery rclcpp::Time end_time_; }; -} // namespace nav2_recoveries +} // namespace nav2_behaviors -#endif // NAV2_RECOVERIES__PLUGINS__SPIN_HPP_ +#endif // NAV2_BEHAVIORS__PLUGINS__SPIN_HPP_ diff --git a/nav2_recoveries/plugins/wait.cpp b/nav2_behaviors/plugins/wait.cpp similarity index 90% rename from nav2_recoveries/plugins/wait.cpp rename to nav2_behaviors/plugins/wait.cpp index 66bcabc1e91..7ca00e2abc4 100644 --- a/nav2_recoveries/plugins/wait.cpp +++ b/nav2_behaviors/plugins/wait.cpp @@ -18,11 +18,11 @@ #include "wait.hpp" -namespace nav2_recoveries +namespace nav2_behaviors { Wait::Wait() -: Recovery(), +: TimedBehavior(), feedback_(std::make_shared()) { } @@ -54,7 +54,7 @@ Status Wait::onCycleUpdate() } } -} // namespace nav2_recoveries +} // namespace nav2_behaviors #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(nav2_recoveries::Wait, nav2_core::Recovery) +PLUGINLIB_EXPORT_CLASS(nav2_behaviors::Wait, nav2_core::Behavior) diff --git a/nav2_recoveries/plugins/wait.hpp b/nav2_behaviors/plugins/wait.hpp similarity index 70% rename from nav2_recoveries/plugins/wait.hpp rename to nav2_behaviors/plugins/wait.hpp index 41ade5e93b3..38e85fd14aa 100644 --- a/nav2_recoveries/plugins/wait.hpp +++ b/nav2_behaviors/plugins/wait.hpp @@ -12,29 +12,29 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_RECOVERIES__PLUGINS__WAIT_HPP_ -#define NAV2_RECOVERIES__PLUGINS__WAIT_HPP_ +#ifndef NAV2_BEHAVIORS__PLUGINS__WAIT_HPP_ +#define NAV2_BEHAVIORS__PLUGINS__WAIT_HPP_ #include #include #include -#include "nav2_recoveries/recovery.hpp" +#include "nav2_behaviors/timed_behavior.hpp" #include "nav2_msgs/action/wait.hpp" -namespace nav2_recoveries +namespace nav2_behaviors { using WaitAction = nav2_msgs::action::Wait; /** - * @class nav2_recoveries::Wait - * @brief An action server recovery for waiting a fixed duration + * @class nav2_behaviors::Wait + * @brief An action server behavior for waiting a fixed duration */ -class Wait : public Recovery +class Wait : public TimedBehavior { public: /** - * @brief A constructor for nav2_recoveries::Wait + * @brief A constructor for nav2_behaviors::Wait */ Wait(); ~Wait(); @@ -42,13 +42,13 @@ class Wait : public Recovery /** * @brief Initialization to run behavior * @param command Goal to execute - * @return Status of recovery + * @return Status of behavior */ Status onRun(const std::shared_ptr command) override; /** * @brief Loop function to run behavior - * @return Status of recovery + * @return Status of behavior */ Status onCycleUpdate() override; @@ -57,6 +57,6 @@ class Wait : public Recovery WaitAction::Feedback::SharedPtr feedback_; }; -} // namespace nav2_recoveries +} // namespace nav2_behaviors -#endif // NAV2_RECOVERIES__PLUGINS__WAIT_HPP_ +#endif // NAV2_BEHAVIORS__PLUGINS__WAIT_HPP_ diff --git a/nav2_recoveries/src/recovery_server.cpp b/nav2_behaviors/src/behavior_server.cpp similarity index 66% rename from nav2_recoveries/src/recovery_server.cpp rename to nav2_behaviors/src/behavior_server.cpp index be98df7250d..3b56aa0f209 100644 --- a/nav2_recoveries/src/recovery_server.cpp +++ b/nav2_behaviors/src/behavior_server.cpp @@ -17,16 +17,16 @@ #include #include #include "nav2_util/node_utils.hpp" -#include "nav2_recoveries/recovery_server.hpp" +#include "nav2_behaviors/behavior_server.hpp" -namespace recovery_server +namespace behavior_server { -RecoveryServer::RecoveryServer(const rclcpp::NodeOptions & options) -: LifecycleNode("recoveries_server", "", false, options), - plugin_loader_("nav2_core", "nav2_core::Recovery"), +BehaviorServer::BehaviorServer(const rclcpp::NodeOptions & options) +: LifecycleNode("behavior_server", "", false, options), + plugin_loader_("nav2_core", "nav2_core::Behavior"), default_ids_{"spin", "backup", "wait"}, - default_types_{"nav2_recoveries/Spin", "nav2_recoveries/BackUp", "nav2_recoveries/Wait"} + default_types_{"nav2_behaviors/Spin", "nav2_behaviors/BackUp", "nav2_behaviors/Wait"} { declare_parameter( "costmap_topic", @@ -35,10 +35,10 @@ RecoveryServer::RecoveryServer(const rclcpp::NodeOptions & options) "footprint_topic", rclcpp::ParameterValue(std::string("local_costmap/published_footprint"))); declare_parameter("cycle_frequency", rclcpp::ParameterValue(10.0)); - declare_parameter("recovery_plugins", default_ids_); + declare_parameter("behavior_plugins", default_ids_); - get_parameter("recovery_plugins", recovery_ids_); - if (recovery_ids_ == default_ids_) { + get_parameter("behavior_plugins", behavior_ids_); + if (behavior_ids_ == default_ids_) { for (size_t i = 0; i < default_ids_.size(); ++i) { declare_parameter(default_ids_[i] + ".plugin", default_types_[i]); } @@ -56,13 +56,13 @@ RecoveryServer::RecoveryServer(const rclcpp::NodeOptions & options) } -RecoveryServer::~RecoveryServer() +BehaviorServer::~BehaviorServer() { - recoveries_.clear(); + behaviors_.clear(); } nav2_util::CallbackReturn -RecoveryServer::on_configure(const rclcpp_lifecycle::State & /*state*/) +BehaviorServer::on_configure(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Configuring"); @@ -87,8 +87,8 @@ RecoveryServer::on_configure(const rclcpp_lifecycle::State & /*state*/) collision_checker_ = std::make_shared( *costmap_sub_, *footprint_sub_, this->get_name()); - recovery_types_.resize(recovery_ids_.size()); - if (!loadRecoveryPlugins()) { + behavior_types_.resize(behavior_ids_.size()); + if (!loadBehaviorPlugins()) { return nav2_util::CallbackReturn::FAILURE; } @@ -97,22 +97,22 @@ RecoveryServer::on_configure(const rclcpp_lifecycle::State & /*state*/) bool -RecoveryServer::loadRecoveryPlugins() +BehaviorServer::loadBehaviorPlugins() { auto node = shared_from_this(); - for (size_t i = 0; i != recovery_ids_.size(); i++) { - recovery_types_[i] = nav2_util::get_plugin_type_param(node, recovery_ids_[i]); + for (size_t i = 0; i != behavior_ids_.size(); i++) { + behavior_types_[i] = nav2_util::get_plugin_type_param(node, behavior_ids_[i]); try { RCLCPP_INFO( - get_logger(), "Creating recovery plugin %s of type %s", - recovery_ids_[i].c_str(), recovery_types_[i].c_str()); - recoveries_.push_back(plugin_loader_.createUniqueInstance(recovery_types_[i])); - recoveries_.back()->configure(node, recovery_ids_[i], tf_, collision_checker_); + get_logger(), "Creating behavior plugin %s of type %s", + behavior_ids_[i].c_str(), behavior_types_[i].c_str()); + behaviors_.push_back(plugin_loader_.createUniqueInstance(behavior_types_[i])); + behaviors_.back()->configure(node, behavior_ids_[i], tf_, collision_checker_); } catch (const pluginlib::PluginlibException & ex) { RCLCPP_FATAL( - get_logger(), "Failed to create recovery %s of type %s." - " Exception: %s", recovery_ids_[i].c_str(), recovery_types_[i].c_str(), + get_logger(), "Failed to create behavior %s of type %s." + " Exception: %s", behavior_ids_[i].c_str(), behavior_types_[i].c_str(), ex.what()); return false; } @@ -122,11 +122,11 @@ RecoveryServer::loadRecoveryPlugins() } nav2_util::CallbackReturn -RecoveryServer::on_activate(const rclcpp_lifecycle::State & /*state*/) +BehaviorServer::on_activate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Activating"); - std::vector>::iterator iter; - for (iter = recoveries_.begin(); iter != recoveries_.end(); ++iter) { + std::vector>::iterator iter; + for (iter = behaviors_.begin(); iter != behaviors_.end(); ++iter) { (*iter)->activate(); } @@ -137,12 +137,12 @@ RecoveryServer::on_activate(const rclcpp_lifecycle::State & /*state*/) } nav2_util::CallbackReturn -RecoveryServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) +BehaviorServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Deactivating"); - std::vector>::iterator iter; - for (iter = recoveries_.begin(); iter != recoveries_.end(); ++iter) { + std::vector>::iterator iter; + for (iter = behaviors_.begin(); iter != behaviors_.end(); ++iter) { (*iter)->deactivate(); } @@ -153,16 +153,16 @@ RecoveryServer::on_deactivate(const rclcpp_lifecycle::State & /*state*/) } nav2_util::CallbackReturn -RecoveryServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) +BehaviorServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) { RCLCPP_INFO(get_logger(), "Cleaning up"); - std::vector>::iterator iter; - for (iter = recoveries_.begin(); iter != recoveries_.end(); ++iter) { + std::vector>::iterator iter; + for (iter = behaviors_.begin(); iter != behaviors_.end(); ++iter) { (*iter)->cleanup(); } - recoveries_.clear(); + behaviors_.clear(); transform_listener_.reset(); tf_.reset(); footprint_sub_.reset(); @@ -173,17 +173,17 @@ RecoveryServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/) } nav2_util::CallbackReturn -RecoveryServer::on_shutdown(const rclcpp_lifecycle::State &) +BehaviorServer::on_shutdown(const rclcpp_lifecycle::State &) { RCLCPP_INFO(get_logger(), "Shutting down"); return nav2_util::CallbackReturn::SUCCESS; } -} // end namespace recovery_server +} // end namespace behavior_server #include "rclcpp_components/register_node_macro.hpp" // Register the component with class_loader. // This acts as a sort of entry point, allowing the component to be discoverable when its library // is being loaded into a running process. -RCLCPP_COMPONENTS_REGISTER_NODE(recovery_server::RecoveryServer) +RCLCPP_COMPONENTS_REGISTER_NODE(behavior_server::BehaviorServer) diff --git a/nav2_recoveries/src/main.cpp b/nav2_behaviors/src/main.cpp similarity index 87% rename from nav2_recoveries/src/main.cpp rename to nav2_behaviors/src/main.cpp index 23dea6b90f7..0b06ea9f173 100644 --- a/nav2_recoveries/src/main.cpp +++ b/nav2_behaviors/src/main.cpp @@ -16,13 +16,13 @@ #include #include -#include "nav2_recoveries/recovery_server.hpp" +#include "nav2_behaviors/behavior_server.hpp" #include "rclcpp/rclcpp.hpp" int main(int argc, char ** argv) { rclcpp::init(argc, argv); - auto recoveries_node = std::make_shared(); + auto recoveries_node = std::make_shared(); rclcpp::spin(recoveries_node->get_node_base_interface()); rclcpp::shutdown(); diff --git a/nav2_behaviors/test/CMakeLists.txt b/nav2_behaviors/test/CMakeLists.txt new file mode 100644 index 00000000000..9418db15716 --- /dev/null +++ b/nav2_behaviors/test/CMakeLists.txt @@ -0,0 +1,7 @@ +ament_add_gtest(test_behaviors + test_behaviors.cpp +) + +ament_target_dependencies(test_behaviors + ${dependencies} +) diff --git a/nav2_recoveries/test/test_recoveries.cpp b/nav2_behaviors/test/test_behaviors.cpp similarity index 79% rename from nav2_recoveries/test/test_recoveries.cpp rename to nav2_behaviors/test/test_behaviors.cpp index 47a687538bd..de1253cdc95 100644 --- a/nav2_recoveries/test/test_recoveries.cpp +++ b/nav2_behaviors/test/test_behaviors.cpp @@ -23,30 +23,30 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" -#include "nav2_recoveries/recovery.hpp" -#include "nav2_msgs/action/dummy_recovery.hpp" +#include "nav2_behaviors/timed_behavior.hpp" +#include "nav2_msgs/action/dummy_behavior.hpp" -using nav2_recoveries::Recovery; -using nav2_recoveries::Status; -using RecoveryAction = nav2_msgs::action::DummyRecovery; -using ClientGoalHandle = rclcpp_action::ClientGoalHandle; +using nav2_behaviors::TimedBehavior; +using nav2_behaviors::Status; +using BehaviorAction = nav2_msgs::action::DummyBehavior; +using ClientGoalHandle = rclcpp_action::ClientGoalHandle; using namespace std::chrono_literals; -// A recovery for testing the base class +// A behavior for testing the base class -class DummyRecovery : public Recovery +class DummyBehavior : public TimedBehavior { public: - DummyRecovery() - : Recovery(), + DummyBehavior() + : TimedBehavior(), initialized_(false) {} - ~DummyRecovery() {} + ~DummyBehavior() {} - Status onRun(const std::shared_ptr goal) override + Status onRun(const std::shared_ptr goal) override { - // A normal recovery would catch the command and initialize + // A normal behavior would catch the command and initialize initialized_ = false; command_ = goal->command.data; start_time_ = std::chrono::system_clock::now(); @@ -63,7 +63,7 @@ class DummyRecovery : public Recovery Status onCycleUpdate() override { - // A normal recovery would set the robot in motion in the first call + // A normal behavior would set the robot in motion in the first call // and check for robot states on subsequent calls to check if the movement // was completed. @@ -92,17 +92,17 @@ class DummyRecovery : public Recovery // Define a test class to hold the context for the tests -class RecoveryTest : public ::testing::Test +class BehaviorTest : public ::testing::Test { protected: - RecoveryTest() {SetUp();} - ~RecoveryTest() {} + BehaviorTest() {SetUp();} + ~BehaviorTest() {} void SetUp() { node_lifecycle_ = std::make_shared( - "LifecycleRecoveryTestNode", rclcpp::NodeOptions()); + "LifecycleBehaviorTestNode", rclcpp::NodeOptions()); node_lifecycle_->declare_parameter( "costmap_topic", rclcpp::ParameterValue(std::string("local_costmap/costmap_raw"))); @@ -131,15 +131,15 @@ class RecoveryTest : public ::testing::Test *costmap_sub_, *footprint_sub_, node_lifecycle_->get_name()); - recovery_ = std::make_shared(); - recovery_->configure(node_lifecycle_, "Recovery", tf_buffer_, collision_checker_); - recovery_->activate(); + behavior_ = std::make_shared(); + behavior_->configure(node_lifecycle_, "Behavior", tf_buffer_, collision_checker_); + behavior_->activate(); - client_ = rclcpp_action::create_client( + client_ = rclcpp_action::create_client( node_lifecycle_->get_node_base_interface(), node_lifecycle_->get_node_graph_interface(), node_lifecycle_->get_node_logging_interface(), - node_lifecycle_->get_node_waitables_interface(), "Recovery"); + node_lifecycle_->get_node_waitables_interface(), "Behavior"); std::cout << "Setup complete." << std::endl; } @@ -152,7 +152,7 @@ class RecoveryTest : public ::testing::Test return false; } - auto goal = RecoveryAction::Goal(); + auto goal = BehaviorAction::Goal(); goal.command.data = command; auto future_goal = client_->async_send_goal(goal); @@ -195,58 +195,58 @@ class RecoveryTest : public ::testing::Test } std::shared_ptr node_lifecycle_; - std::shared_ptr recovery_; - std::shared_ptr> client_; - std::shared_ptr> goal_handle_; + std::shared_ptr behavior_; + std::shared_ptr> client_; + std::shared_ptr> goal_handle_; std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; }; // Define the tests -TEST_F(RecoveryTest, testingSuccess) +TEST_F(BehaviorTest, testingSuccess) { ASSERT_TRUE(sendCommand("Testing success")); EXPECT_EQ(getOutcome(), Status::SUCCEEDED); SUCCEED(); } -TEST_F(RecoveryTest, testingFailureOnRun) +TEST_F(BehaviorTest, testingFailureOnRun) { ASSERT_TRUE(sendCommand("Testing failure on run")); EXPECT_EQ(getOutcome(), Status::FAILED); SUCCEED(); } -TEST_F(RecoveryTest, testingFailureOnInit) +TEST_F(BehaviorTest, testingFailureOnInit) { ASSERT_TRUE(sendCommand("Testing failure on init")); EXPECT_EQ(getOutcome(), Status::FAILED); SUCCEED(); } -TEST_F(RecoveryTest, testingSequentialFailures) +TEST_F(BehaviorTest, testingSequentialFailures) { ASSERT_TRUE(sendCommand("Testing failure on run")); EXPECT_EQ(getOutcome(), Status::FAILED); SUCCEED(); } -TEST_F(RecoveryTest, testingTotalElapsedTimeIsGratherThanZeroIfStarted) +TEST_F(BehaviorTest, testingTotalElapsedTimeIsGratherThanZeroIfStarted) { ASSERT_TRUE(sendCommand("Testing success")); EXPECT_GT(getResult().result->total_elapsed_time.sec, 0.0); SUCCEED(); } -TEST_F(RecoveryTest, testingTotalElapsedTimeIsZeroIfFailureOnInit) +TEST_F(BehaviorTest, testingTotalElapsedTimeIsZeroIfFailureOnInit) { ASSERT_TRUE(sendCommand("Testing failure on init")); EXPECT_EQ(getResult().result->total_elapsed_time.sec, 0.0); SUCCEED(); } -TEST_F(RecoveryTest, testingTotalElapsedTimeIsZeroIfFailureOnRun) +TEST_F(BehaviorTest, testingTotalElapsedTimeIsZeroIfFailureOnRun) { ASSERT_TRUE(sendCommand("Testing failure on run")); EXPECT_EQ(getResult().result->total_elapsed_time.sec, 0.0); diff --git a/nav2_bringup/launch/navigation_launch.py b/nav2_bringup/launch/navigation_launch.py index e35f88c9863..318b98f781b 100644 --- a/nav2_bringup/launch/navigation_launch.py +++ b/nav2_bringup/launch/navigation_launch.py @@ -40,7 +40,7 @@ def generate_launch_description(): lifecycle_nodes = ['controller_server', 'smoother_server', 'planner_server', - 'recoveries_server', + 'behavior_server', 'bt_navigator', 'waypoint_follower'] @@ -118,9 +118,9 @@ def generate_launch_description(): parameters=[configured_params], remappings=remappings), Node( - package='nav2_recoveries', - executable='recoveries_server', - name='recoveries_server', + package='nav2_behaviors', + executable='behavior_server', + name='behavior_server', output='screen', parameters=[configured_params], remappings=remappings), @@ -172,9 +172,9 @@ def generate_launch_description(): parameters=[configured_params], remappings=remappings), ComposableNode( - package='nav2_recoveries', - plugin='recovery_server::RecoveryServer', - name='recoveries_server', + package='nav2_behaviors', + plugin='behavior_server::BehaviorServer', + name='behavior_server', parameters=[configured_params], remappings=remappings), ComposableNode( diff --git a/nav2_bringup/params/nav2_multirobot_params_1.yaml b/nav2_bringup/params/nav2_multirobot_params_1.yaml index 9bb39d954e9..15d3197292c 100644 --- a/nav2_bringup/params/nav2_multirobot_params_1.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_1.yaml @@ -260,18 +260,18 @@ smoother_server: ros__parameters: use_sim_time: True -recoveries_server: +behavior_server: ros__parameters: costmap_topic: local_costmap/costmap_raw footprint_topic: local_costmap/published_footprint cycle_frequency: 10.0 - recovery_plugins: ["spin", "backup", "wait"] + behavior_plugins: ["spin", "backup", "wait"] spin: - plugin: "nav2_recoveries/Spin" + plugin: "nav2_behaviors/Spin" backup: - plugin: "nav2_recoveries/BackUp" + plugin: "nav2_behaviors/BackUp" wait: - plugin: "nav2_recoveries/Wait" + plugin: "nav2_behaviors/Wait" global_frame: odom robot_base_frame: base_link transform_tolerance: 0.1 diff --git a/nav2_bringup/params/nav2_multirobot_params_2.yaml b/nav2_bringup/params/nav2_multirobot_params_2.yaml index 27b768de776..8463021d4f2 100644 --- a/nav2_bringup/params/nav2_multirobot_params_2.yaml +++ b/nav2_bringup/params/nav2_multirobot_params_2.yaml @@ -260,18 +260,18 @@ smoother_server: ros__parameters: use_sim_time: True -recoveries_server: +behavior_server: ros__parameters: costmap_topic: local_costmap/costmap_raw footprint_topic: local_costmap/published_footprint cycle_frequency: 10.0 - recovery_plugins: ["spin", "backup", "wait"] + behavior_plugins: ["spin", "backup", "wait"] spin: - plugin: "nav2_recoveries/Spin" + plugin: "nav2_behaviors/Spin" backup: - plugin: "nav2_recoveries/BackUp" + plugin: "nav2_behaviors/BackUp" wait: - plugin: "nav2_recoveries/Wait" + plugin: "nav2_behaviors/Wait" global_frame: odom robot_base_frame: base_link transform_tolerance: 0.1 diff --git a/nav2_bringup/params/nav2_params.yaml b/nav2_bringup/params/nav2_params.yaml index 9bfcf45a07f..b6af9e8781c 100644 --- a/nav2_bringup/params/nav2_params.yaml +++ b/nav2_bringup/params/nav2_params.yaml @@ -307,18 +307,18 @@ smoother_server: max_its: 1000 do_refinement: True -recoveries_server: +behavior_server: ros__parameters: costmap_topic: local_costmap/costmap_raw footprint_topic: local_costmap/published_footprint cycle_frequency: 10.0 - recovery_plugins: ["spin", "backup", "wait"] + behavior_plugins: ["spin", "backup", "wait"] spin: - plugin: "nav2_recoveries/Spin" + plugin: "nav2_behaviors/Spin" backup: - plugin: "nav2_recoveries/BackUp" + plugin: "nav2_behaviors/BackUp" wait: - plugin: "nav2_recoveries/Wait" + plugin: "nav2_behaviors/Wait" global_frame: odom robot_base_frame: base_link transform_tolerance: 0.1 diff --git a/nav2_core/include/nav2_core/recovery.hpp b/nav2_core/include/nav2_core/behavior.hpp similarity index 79% rename from nav2_core/include/nav2_core/recovery.hpp rename to nav2_core/include/nav2_core/behavior.hpp index 42646824bea..f3510e86847 100644 --- a/nav2_core/include/nav2_core/recovery.hpp +++ b/nav2_core/include/nav2_core/behavior.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef NAV2_CORE__RECOVERY_HPP_ -#define NAV2_CORE__RECOVERY_HPP_ +#ifndef NAV2_CORE__BEHAVIOR_HPP_ +#define NAV2_CORE__BEHAVIOR_HPP_ #include #include @@ -27,18 +27,18 @@ namespace nav2_core { /** - * @class Recovery - * @brief Abstract interface for recoveries to adhere to with pluginlib + * @class Behavior + * @brief Abstract interface for behaviors to adhere to with pluginlib */ -class Recovery +class Behavior { public: - using Ptr = std::shared_ptr; + using Ptr = std::shared_ptr; /** * @brief Virtual destructor */ - virtual ~Recovery() {} + virtual ~Behavior() {} /** * @param parent pointer to user's node @@ -57,16 +57,16 @@ class Recovery virtual void cleanup() = 0; /** - * @brief Method to active recovery and any threads involved in execution. + * @brief Method to active Behavior and any threads involved in execution. */ virtual void activate() = 0; /** - * @brief Method to deactive recovery and any threads involved in execution. + * @brief Method to deactive Behavior and any threads involved in execution. */ virtual void deactivate() = 0; }; } // namespace nav2_core -#endif // NAV2_CORE__RECOVERY_HPP_ +#endif // NAV2_CORE__BEHAVIOR_HPP_ diff --git a/nav2_msgs/CMakeLists.txt b/nav2_msgs/CMakeLists.txt index 08722660f27..98320ed31d2 100644 --- a/nav2_msgs/CMakeLists.txt +++ b/nav2_msgs/CMakeLists.txt @@ -39,7 +39,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "action/NavigateThroughPoses.action" "action/Wait.action" "action/Spin.action" - "action/DummyRecovery.action" + "action/DummyBehavior.action" "action/FollowWaypoints.action" DEPENDENCIES builtin_interfaces geometry_msgs std_msgs action_msgs nav_msgs ) diff --git a/nav2_msgs/action/DummyRecovery.action b/nav2_msgs/action/DummyBehavior.action similarity index 100% rename from nav2_msgs/action/DummyRecovery.action rename to nav2_msgs/action/DummyBehavior.action diff --git a/nav2_recoveries/README.md b/nav2_recoveries/README.md deleted file mode 100644 index beecd7cdac6..00000000000 --- a/nav2_recoveries/README.md +++ /dev/null @@ -1,15 +0,0 @@ -# Recoveries - -The `nav2_recoveries` package implements a task server for executing simple controlled robot movements such as rotating on its own axis, assisted teleop, or moving linearly. - -The package defines: -- A `Recovery` template which is used as a base class to implement specific recovery timed action server - but not required. -- The `BackUp`, `Spin` and `Wait` recoveries. - -The only required class a recovery must derive from is the `nav2_core/recovery.hpp` class, which implements the pluginlib interface the recovery server will use to dynamically load your behavior. The `nav2_recoveries/recovery.hpp` derives from this class and implements a generic action server for a timed recovery 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 `recovery.hpp` helps in managing the complexity to simplify new behavior development, described more below. - -The value of the centralized recovery 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-recovery-server.html) for additional parameter descriptions and a [tutorial about writing recovery behaviors](https://navigation.ros.org/plugin_tutorials/docs/writing_new_recovery_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_recoveries/recovery_plugin.xml b/nav2_recoveries/recovery_plugin.xml deleted file mode 100644 index b8ccaca0ad5..00000000000 --- a/nav2_recoveries/recovery_plugin.xml +++ /dev/null @@ -1,19 +0,0 @@ - - - - - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/nav2_recoveries/test/CMakeLists.txt b/nav2_recoveries/test/CMakeLists.txt deleted file mode 100644 index 98cb9a6ec04..00000000000 --- a/nav2_recoveries/test/CMakeLists.txt +++ /dev/null @@ -1,7 +0,0 @@ -ament_add_gtest(test_recoveries - test_recoveries.cpp -) - -ament_target_dependencies(test_recoveries - ${dependencies} -) diff --git a/nav2_system_tests/CMakeLists.txt b/nav2_system_tests/CMakeLists.txt index 96d932136b5..32bfa2b1cef 100644 --- a/nav2_system_tests/CMakeLists.txt +++ b/nav2_system_tests/CMakeLists.txt @@ -61,9 +61,9 @@ if(BUILD_TESTING) add_subdirectory(src/system_failure) add_subdirectory(src/updown) add_subdirectory(src/waypoint_follower) - add_subdirectory(src/recoveries/spin) - add_subdirectory(src/recoveries/wait) - add_subdirectory(src/recoveries/backup) + add_subdirectory(src/behaviors/spin) + add_subdirectory(src/behaviors/wait) + add_subdirectory(src/behaviors/backup) add_subdirectory(src/costmap_filters) install(DIRECTORY maps DESTINATION share/${PROJECT_NAME}) diff --git a/nav2_system_tests/README.md b/nav2_system_tests/README.md index 04564359420..b40cba4ec4d 100644 --- a/nav2_system_tests/README.md +++ b/nav2_system_tests/README.md @@ -1,14 +1,14 @@ # System Tests -The package provides tests for Component-Testing, Subsystem-Testing, and Full-System integration. Its main goal is to provide a location for smoke and integration tests of the navigation system to ensure that things are working properly on a high level. Unit and specific subsystem testing happens in the packages specific to those algorithms. +The package provides tests for Component-Testing, Subsystem-Testing, and Full-System integration. Its main goal is to provide a location for smoke and integration tests of the navigation system to ensure that things are working properly on a high level. Unit and specific subsystem testing happens in the packages specific to those algorithms. -Most tests in this package will spin up Gazebo instances of a robot in an environment to have the robot complete some task in the space while tracking a specific modules results. Some examples include +Most tests in this package will spin up Gazebo instances of a robot in an environment to have the robot complete some task in the space while tracking a specific modules results. Some examples include - System tests of a robot in a sandbox environment trying navigate to pose, navigate through poses, and waypoint following navigation types - Random planning of thousands of paths in a generated environment to ensure default planners are working properly - Testing the system can be brought up and down on the lifecycle transitions successfully multiple times - Testing that the keepout and speed restricted zones work in a practical environment without going into keepout zones and slowing in speed restricted areas -- Testing recovery behaviors in a sandbox environment to ensure they trigger and complete collision checking properly +- Testing behaviors in a sandbox environment to ensure they trigger and complete collision checking properly - Testing system failures are properly recorded and can be recovered from This is primarily for use in Nav2 CI to establish a high degree of maintainer confidence when merging in large architectural changes to the Nav2 project. However, this is also useful to test installs of Nav2 locally or for additional information. diff --git a/nav2_system_tests/src/behaviors/README.md b/nav2_system_tests/src/behaviors/README.md new file mode 100644 index 00000000000..e1ac535ad5c --- /dev/null +++ b/nav2_system_tests/src/behaviors/README.md @@ -0,0 +1,4 @@ +# Behavior Test + +Provides some simple tests for behavior plugins. +It creates an instance of the stack, with the behavior server loading different behavior plugins, and checks for successful behavior behaviors. diff --git a/nav2_system_tests/src/recoveries/backup/CMakeLists.txt b/nav2_system_tests/src/behaviors/backup/CMakeLists.txt similarity index 54% rename from nav2_system_tests/src/recoveries/backup/CMakeLists.txt rename to nav2_system_tests/src/behaviors/backup/CMakeLists.txt index ca3f35402ae..05afe3c50da 100644 --- a/nav2_system_tests/src/recoveries/backup/CMakeLists.txt +++ b/nav2_system_tests/src/behaviors/backup/CMakeLists.txt @@ -1,22 +1,22 @@ -set(test_backup_recovery_exec test_backup_recovery_node) +set(test_backup_behavior test_backup_behavior_node) -ament_add_gtest_executable(${test_backup_recovery_exec} - test_backup_recovery_node.cpp - backup_recovery_tester.cpp +ament_add_gtest_executable(${test_backup_behavior} + test_backup_behavior_node.cpp + backup_behavior_tester.cpp ) -ament_target_dependencies(${test_backup_recovery_exec} +ament_target_dependencies(${test_backup_behavior} ${dependencies} ) ament_add_test(test_backup_recovery GENERATE_RESULT_FOR_RETURN_CODE_ZERO - COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_backup_recovery_launch.py" + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_backup_behavior_launch.py" WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" TIMEOUT 180 ENV TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml - TEST_EXECUTABLE=$ + TEST_EXECUTABLE=$ TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml diff --git a/nav2_system_tests/src/recoveries/backup/backup_recovery_tester.cpp b/nav2_system_tests/src/behaviors/backup/backup_behavior_tester.cpp similarity index 90% rename from nav2_system_tests/src/recoveries/backup/backup_recovery_tester.cpp rename to nav2_system_tests/src/behaviors/backup/backup_behavior_tester.cpp index 7d8bb826fe0..3c407186bc2 100644 --- a/nav2_system_tests/src/recoveries/backup/backup_recovery_tester.cpp +++ b/nav2_system_tests/src/behaviors/backup/backup_behavior_tester.cpp @@ -22,7 +22,7 @@ #include #include -#include "backup_recovery_tester.hpp" +#include "backup_behavior_tester.hpp" #include "nav2_util/geometry_utils.hpp" using namespace std::chrono_literals; @@ -31,11 +31,11 @@ using namespace std::chrono; // NOLINT namespace nav2_system_tests { -BackupRecoveryTester::BackupRecoveryTester() +BackupBehaviorTester::BackupBehaviorTester() : is_active_(false), initial_pose_received_(false) { - node_ = rclcpp::Node::make_shared("backup_recovery_test"); + node_ = rclcpp::Node::make_shared("backup_behavior_test"); tf_buffer_ = std::make_shared(node_->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); @@ -52,19 +52,19 @@ BackupRecoveryTester::BackupRecoveryTester() subscription_ = node_->create_subscription( "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), - std::bind(&BackupRecoveryTester::amclPoseCallback, this, std::placeholders::_1)); + std::bind(&BackupBehaviorTester::amclPoseCallback, this, std::placeholders::_1)); stamp_ = node_->now(); } -BackupRecoveryTester::~BackupRecoveryTester() +BackupBehaviorTester::~BackupBehaviorTester() { if (is_active_) { deactivate(); } } -void BackupRecoveryTester::activate() +void BackupBehaviorTester::activate() { if (is_active_) { throw std::runtime_error("Trying to activate while already active"); @@ -78,7 +78,7 @@ void BackupRecoveryTester::activate() rclcpp::spin_some(node_); } - // Wait for lifecycle_manager_navigation to activate recoveries_server + // Wait for lifecycle_manager_navigation to activate behavior_server std::this_thread::sleep_for(10s); if (!client_ptr_) { @@ -97,7 +97,7 @@ void BackupRecoveryTester::activate() is_active_ = true; } -void BackupRecoveryTester::deactivate() +void BackupBehaviorTester::deactivate() { if (!is_active_) { throw std::runtime_error("Trying to deactivate while already inactive"); @@ -105,7 +105,7 @@ void BackupRecoveryTester::deactivate() is_active_ = false; } -bool BackupRecoveryTester::defaultBackupRecoveryTest( +bool BackupBehaviorTester::defaultBackupBehaviorTest( const float target_dist, const double tolerance) { @@ -114,7 +114,7 @@ bool BackupRecoveryTester::defaultBackupRecoveryTest( return false; } - // Sleep to let recovery server be ready for serving in multiple runs + // Sleep to let behavior server be ready for serving in multiple runs std::this_thread::sleep_for(5s); auto goal_msg = BackUp::Goal(); @@ -193,7 +193,7 @@ bool BackupRecoveryTester::defaultBackupRecoveryTest( return true; } -void BackupRecoveryTester::sendInitialPose() +void BackupBehaviorTester::sendInitialPose() { geometry_msgs::msg::PoseWithCovarianceStamped pose; pose.header.frame_id = "map"; @@ -216,7 +216,7 @@ void BackupRecoveryTester::sendInitialPose() RCLCPP_INFO(node_->get_logger(), "Sent initial pose"); } -void BackupRecoveryTester::amclPoseCallback( +void BackupBehaviorTester::amclPoseCallback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr) { initial_pose_received_ = true; diff --git a/nav2_system_tests/src/recoveries/backup/backup_recovery_tester.hpp b/nav2_system_tests/src/behaviors/backup/backup_behavior_tester.hpp similarity index 88% rename from nav2_system_tests/src/recoveries/backup/backup_recovery_tester.hpp rename to nav2_system_tests/src/behaviors/backup/backup_behavior_tester.hpp index 43e9cb5b8d8..154dc93b9c9 100644 --- a/nav2_system_tests/src/recoveries/backup/backup_recovery_tester.hpp +++ b/nav2_system_tests/src/behaviors/backup/backup_behavior_tester.hpp @@ -13,8 +13,8 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#ifndef RECOVERIES__BACKUP__BACKUP_RECOVERY_TESTER_HPP_ -#define RECOVERIES__BACKUP__BACKUP_RECOVERY_TESTER_HPP_ +#ifndef BEHAVIORS__BACKUP__BACKUP_BEHAVIOR_TESTER_HPP_ +#define BEHAVIORS__BACKUP__BACKUP_BEHAVIOR_TESTER_HPP_ #include #include @@ -38,17 +38,17 @@ namespace nav2_system_tests { -class BackupRecoveryTester +class BackupBehaviorTester { public: using BackUp = nav2_msgs::action::BackUp; using GoalHandleBackup = rclcpp_action::ClientGoalHandle; - BackupRecoveryTester(); - ~BackupRecoveryTester(); + BackupBehaviorTester(); + ~BackupBehaviorTester(); // Runs a single test with given target yaw - bool defaultBackupRecoveryTest( + bool defaultBackupBehaviorTest( float target_dist, double tolerance = 0.1); @@ -87,4 +87,4 @@ class BackupRecoveryTester } // namespace nav2_system_tests -#endif // RECOVERIES__BACKUP__BACKUP_RECOVERY_TESTER_HPP_ +#endif // BEHAVIORS__BACKUP__BACKUP_BEHAVIOR_TESTER_HPP_ diff --git a/nav2_system_tests/src/recoveries/backup/test_backup_recovery_launch.py b/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py similarity index 98% rename from nav2_system_tests/src/recoveries/backup/test_backup_recovery_launch.py rename to nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py index 97affc65168..bf7f4f04f13 100755 --- a/nav2_system_tests/src/recoveries/backup/test_backup_recovery_launch.py +++ b/nav2_system_tests/src/behaviors/backup/test_backup_behavior_launch.py @@ -89,7 +89,7 @@ def main(argv=sys.argv[1:]): test1_action = ExecuteProcess( cmd=[testExecutable], - name='test_backup_recovery_node', + name='test_backup_behavior_node', output='screen') lts = LaunchTestService() diff --git a/nav2_system_tests/src/recoveries/backup/test_backup_recovery_node.cpp b/nav2_system_tests/src/behaviors/backup/test_backup_behavior_node.cpp similarity index 74% rename from nav2_system_tests/src/recoveries/backup/test_backup_recovery_node.cpp rename to nav2_system_tests/src/behaviors/backup/test_backup_behavior_node.cpp index 8802755ec0b..be9933bc0cf 100644 --- a/nav2_system_tests/src/recoveries/backup/test_backup_recovery_node.cpp +++ b/nav2_system_tests/src/behaviors/backup/test_backup_behavior_node.cpp @@ -21,11 +21,11 @@ #include "rclcpp/rclcpp.hpp" -#include "backup_recovery_tester.hpp" +#include "backup_behavior_tester.hpp" using namespace std::chrono_literals; -using nav2_system_tests::BackupRecoveryTester; +using nav2_system_tests::BackupBehaviorTester; std::string testNameGenerator(const testing::TestParamInfo> & param) { @@ -35,41 +35,41 @@ std::string testNameGenerator(const testing::TestParamInfo> { public: static void SetUpTestCase() { - backup_recovery_tester = new BackupRecoveryTester(); - if (!backup_recovery_tester->isActive()) { - backup_recovery_tester->activate(); + backup_behavior_tester = new BackupBehaviorTester(); + if (!backup_behavior_tester->isActive()) { + backup_behavior_tester->activate(); } } static void TearDownTestCase() { - delete backup_recovery_tester; - backup_recovery_tester = nullptr; + delete backup_behavior_tester; + backup_behavior_tester = nullptr; } protected: - static BackupRecoveryTester * backup_recovery_tester; + static BackupBehaviorTester * backup_behavior_tester; }; -BackupRecoveryTester * BackupRecoveryTestFixture::backup_recovery_tester = nullptr; +BackupBehaviorTester * BackupBehaviorTestFixture::backup_behavior_tester = nullptr; -TEST_P(BackupRecoveryTestFixture, testBackupRecovery) +TEST_P(BackupBehaviorTestFixture, testBackupBehavior) { float target_dist = std::get<0>(GetParam()); float tolerance = std::get<1>(GetParam()); - if (!backup_recovery_tester->isActive()) { - backup_recovery_tester->activate(); + if (!backup_behavior_tester->isActive()) { + backup_behavior_tester->activate(); } bool success = false; - success = backup_recovery_tester->defaultBackupRecoveryTest(target_dist, tolerance); + success = backup_behavior_tester->defaultBackupBehaviorTest(target_dist, tolerance); // if intentionally backing into an obstacle, should fail. if (target_dist < -0.1) { @@ -82,11 +82,11 @@ TEST_P(BackupRecoveryTestFixture, testBackupRecovery) // TODO(stevemacenski): See issue #1779, while the 3rd test collides, // it returns success due to technical debt in the BT. This test will // remain as a reminder to update this to a `false` case once the -// recovery server returns true values. +// behavior server returns true values. INSTANTIATE_TEST_SUITE_P( - BackupRecoveryTests, - BackupRecoveryTestFixture, + BackupBehaviorTests, + BackupBehaviorTestFixture, ::testing::Values( std::make_tuple(-0.05, 0.01), std::make_tuple(-0.2, 0.1), diff --git a/nav2_system_tests/src/recoveries/spin/CMakeLists.txt b/nav2_system_tests/src/behaviors/spin/CMakeLists.txt similarity index 52% rename from nav2_system_tests/src/recoveries/spin/CMakeLists.txt rename to nav2_system_tests/src/behaviors/spin/CMakeLists.txt index 804468dda00..84ee7246e59 100644 --- a/nav2_system_tests/src/recoveries/spin/CMakeLists.txt +++ b/nav2_system_tests/src/behaviors/spin/CMakeLists.txt @@ -1,34 +1,34 @@ -set(test_spin_recovery_exec test_spin_recovery_node) +set(test_spin_behavior_exec test_spin_behavior_node) -ament_add_gtest_executable(${test_spin_recovery_exec} - test_spin_recovery_node.cpp - spin_recovery_tester.cpp +ament_add_gtest_executable(${test_spin_behavior_exec} + test_spin_behavior_node.cpp + spin_behavior_tester.cpp ) -ament_target_dependencies(${test_spin_recovery_exec} +ament_target_dependencies(${test_spin_behavior_exec} ${dependencies} ) -ament_add_test(test_spin_recovery +ament_add_test(test_spin_behavior GENERATE_RESULT_FOR_RETURN_CODE_ZERO - COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_spin_recovery_launch.py" + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_spin_behavior_launch.py" WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" TIMEOUT 180 ENV TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml - TEST_EXECUTABLE=$ + TEST_EXECUTABLE=$ TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml ) -ament_add_test(test_spin_recovery_fake +ament_add_test(test_spin_behavior_fake GENERATE_RESULT_FOR_RETURN_CODE_ZERO - COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_spin_recovery_fake_launch.py" + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_spin_behavior_fake_launch.py" WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" TIMEOUT 180 ENV TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml - TEST_EXECUTABLE=$ + TEST_EXECUTABLE=$ MAKE_FAKE_COSTMAP=true ) diff --git a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp b/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp similarity index 93% rename from nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp rename to nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp index 316b35b1fef..557f5b79595 100644 --- a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.cpp +++ b/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.cpp @@ -22,7 +22,7 @@ #include #include -#include "spin_recovery_tester.hpp" +#include "spin_behavior_tester.hpp" using namespace std::chrono_literals; using namespace std::chrono; // NOLINT @@ -30,11 +30,11 @@ using namespace std::chrono; // NOLINT namespace nav2_system_tests { -SpinRecoveryTester::SpinRecoveryTester() +SpinBehaviorTester::SpinBehaviorTester() : is_active_(false), initial_pose_received_(false) { - node_ = rclcpp::Node::make_shared("spin_recovery_test"); + node_ = rclcpp::Node::make_shared("spin_behavior_test"); tf_buffer_ = std::make_shared(node_->get_clock()); @@ -65,19 +65,19 @@ SpinRecoveryTester::SpinRecoveryTester() subscription_ = node_->create_subscription( "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), - std::bind(&SpinRecoveryTester::amclPoseCallback, this, std::placeholders::_1)); + std::bind(&SpinBehaviorTester::amclPoseCallback, this, std::placeholders::_1)); stamp_ = node_->now(); } -SpinRecoveryTester::~SpinRecoveryTester() +SpinBehaviorTester::~SpinBehaviorTester() { if (is_active_) { deactivate(); } } -void SpinRecoveryTester::activate() +void SpinBehaviorTester::activate() { if (is_active_) { throw std::runtime_error("Trying to activate while already active"); @@ -94,7 +94,7 @@ void SpinRecoveryTester::activate() sendFakeOdom(0.0); } - // Wait for lifecycle_manager_navigation to activate recoveries_server + // Wait for lifecycle_manager_navigation to activate behavior_server std::this_thread::sleep_for(10s); if (!client_ptr_) { @@ -113,7 +113,7 @@ void SpinRecoveryTester::activate() is_active_ = true; } -void SpinRecoveryTester::deactivate() +void SpinBehaviorTester::deactivate() { if (!is_active_) { throw std::runtime_error("Trying to deactivate while already inactive"); @@ -121,7 +121,7 @@ void SpinRecoveryTester::deactivate() is_active_ = false; } -bool SpinRecoveryTester::defaultSpinRecoveryTest( +bool SpinBehaviorTester::defaultSpinBehaviorTest( const float target_yaw, const double tolerance) { @@ -130,7 +130,7 @@ bool SpinRecoveryTester::defaultSpinRecoveryTest( return false; } - // Sleep to let recovery server be ready for serving in multiple runs + // Sleep to let behavior server be ready for serving in multiple runs std::this_thread::sleep_for(5s); if (make_fake_costmap_) { @@ -260,7 +260,7 @@ bool SpinRecoveryTester::defaultSpinRecoveryTest( return true; } -void SpinRecoveryTester::sendFakeCostmap(float angle) +void SpinBehaviorTester::sendFakeCostmap(float angle) { nav2_msgs::msg::Costmap fake_costmap; @@ -286,7 +286,7 @@ void SpinRecoveryTester::sendFakeCostmap(float angle) fake_costmap_publisher_->publish(fake_costmap); } -void SpinRecoveryTester::sendInitialPose() +void SpinBehaviorTester::sendInitialPose() { geometry_msgs::msg::PoseWithCovarianceStamped pose; pose.header.frame_id = "map"; @@ -309,7 +309,7 @@ void SpinRecoveryTester::sendInitialPose() RCLCPP_INFO(node_->get_logger(), "Sent initial pose"); } -void SpinRecoveryTester::sendFakeOdom(float angle) +void SpinBehaviorTester::sendFakeOdom(float angle) { geometry_msgs::msg::TransformStamped transformStamped; @@ -342,7 +342,7 @@ void SpinRecoveryTester::sendFakeOdom(float angle) footprint.polygon.points[3].y = 0.22; fake_footprint_publisher_->publish(footprint); } -void SpinRecoveryTester::amclPoseCallback( +void SpinBehaviorTester::amclPoseCallback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr) { initial_pose_received_ = true; diff --git a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.hpp b/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.hpp similarity index 91% rename from nav2_system_tests/src/recoveries/spin/spin_recovery_tester.hpp rename to nav2_system_tests/src/behaviors/spin/spin_behavior_tester.hpp index a00ffa21e41..745cfe42124 100644 --- a/nav2_system_tests/src/recoveries/spin/spin_recovery_tester.hpp +++ b/nav2_system_tests/src/behaviors/spin/spin_behavior_tester.hpp @@ -13,8 +13,8 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#ifndef RECOVERIES__SPIN__SPIN_RECOVERY_TESTER_HPP_ -#define RECOVERIES__SPIN__SPIN_RECOVERY_TESTER_HPP_ +#ifndef BEHAVIORS__SPIN__SPIN_BEHAVIOR_TESTER_HPP_ +#define BEHAVIORS__SPIN__SPIN_BEHAVIOR_TESTER_HPP_ #include #include @@ -45,17 +45,17 @@ namespace nav2_system_tests { -class SpinRecoveryTester +class SpinBehaviorTester { public: using Spin = nav2_msgs::action::Spin; using GoalHandleSpin = rclcpp_action::ClientGoalHandle; - SpinRecoveryTester(); - ~SpinRecoveryTester(); + SpinBehaviorTester(); + ~SpinBehaviorTester(); // Runs a single test with given target yaw - bool defaultSpinRecoveryTest( + bool defaultSpinBehaviorTest( float target_yaw, double tolerance = 0.1); @@ -105,4 +105,4 @@ class SpinRecoveryTester } // namespace nav2_system_tests -#endif // RECOVERIES__SPIN__SPIN_RECOVERY_TESTER_HPP_ +#endif // BEHAVIORS__SPIN__SPIN_BEHAVIOR_TESTER_HPP_ diff --git a/nav2_system_tests/src/recoveries/spin/test_spin_recovery_fake_launch.py b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_fake_launch.py similarity index 96% rename from nav2_system_tests/src/recoveries/spin/test_spin_recovery_fake_launch.py rename to nav2_system_tests/src/behaviors/spin/test_spin_behavior_fake_launch.py index a773d91e09b..1b0ace8f1bb 100755 --- a/nav2_system_tests/src/recoveries/spin/test_spin_recovery_fake_launch.py +++ b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_fake_launch.py @@ -53,7 +53,7 @@ def generate_launch_description(): param_rewrites=param_substitutions, convert_types=True) - lifecycle_nodes = ['recoveries_server'] + lifecycle_nodes = ['behavior_server'] # Map fully qualified names to relative ones so the node's namespace can be prepended. # In case of the transforms (tf), currently, there doesn't seem to be a better alternative @@ -122,9 +122,9 @@ def generate_launch_description(): arguments=['0', '0', '0', '0', '0', '0', 'base_link', 'base_scan']), Node( - package='nav2_recoveries', - executable='recoveries_server', - name='recoveries_server', + package='nav2_behaviors', + executable='behavior_server', + name='behavior_server', output='screen', parameters=[configured_params], remappings=remappings), @@ -147,7 +147,7 @@ def main(argv=sys.argv[1:]): test1_action = ExecuteProcess( cmd=[testExecutable], - name='test_spin_recovery_fake_node', + name='test_spin_behavior_fake_node', output='screen') lts = LaunchTestService() diff --git a/nav2_system_tests/src/recoveries/wait/test_wait_recovery_launch.py b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py similarity index 98% rename from nav2_system_tests/src/recoveries/wait/test_wait_recovery_launch.py rename to nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py index 57a6cf327d9..d26795834b4 100755 --- a/nav2_system_tests/src/recoveries/wait/test_wait_recovery_launch.py +++ b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_launch.py @@ -89,7 +89,7 @@ def main(argv=sys.argv[1:]): test1_action = ExecuteProcess( cmd=[testExecutable], - name='test_wait_recovery_node', + name='test_spin_behavior_node', output='screen') lts = LaunchTestService() diff --git a/nav2_system_tests/src/recoveries/spin/test_spin_recovery_node.cpp b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp similarity index 85% rename from nav2_system_tests/src/recoveries/spin/test_spin_recovery_node.cpp rename to nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp index 059331eaae8..150a6599fab 100644 --- a/nav2_system_tests/src/recoveries/spin/test_spin_recovery_node.cpp +++ b/nav2_system_tests/src/behaviors/spin/test_spin_behavior_node.cpp @@ -20,11 +20,11 @@ #include "rclcpp/rclcpp.hpp" -#include "spin_recovery_tester.hpp" +#include "spin_behavior_tester.hpp" using namespace std::chrono_literals; -using nav2_system_tests::SpinRecoveryTester; +using nav2_system_tests::SpinBehaviorTester; std::string testNameGenerator(const testing::TestParamInfo> & param) { @@ -34,13 +34,13 @@ std::string testNameGenerator(const testing::TestParamInfo> { public: static void SetUpTestCase() { - spin_recovery_tester = new SpinRecoveryTester(); + spin_recovery_tester = new SpinBehaviorTester(); if (!spin_recovery_tester->isActive()) { spin_recovery_tester->activate(); } @@ -53,12 +53,12 @@ class SpinRecoveryTestFixture } protected: - static SpinRecoveryTester * spin_recovery_tester; + static SpinBehaviorTester * spin_recovery_tester; }; -SpinRecoveryTester * SpinRecoveryTestFixture::spin_recovery_tester = nullptr; +SpinBehaviorTester * SpinBehaviorTestFixture::spin_recovery_tester = nullptr; -TEST_P(SpinRecoveryTestFixture, testSpinRecovery) +TEST_P(SpinBehaviorTestFixture, testSpinRecovery) { float target_yaw = std::get<0>(GetParam()); float tolerance = std::get<1>(GetParam()); @@ -66,7 +66,7 @@ TEST_P(SpinRecoveryTestFixture, testSpinRecovery) bool success = false; int num_tries = 3; for (int i = 0; i != num_tries; i++) { - success = success || spin_recovery_tester->defaultSpinRecoveryTest(target_yaw, tolerance); + success = success || spin_recovery_tester->defaultSpinBehaviorTest(target_yaw, tolerance); if (success) { break; } @@ -82,7 +82,7 @@ TEST_P(SpinRecoveryTestFixture, testSpinRecovery) INSTANTIATE_TEST_SUITE_P( SpinRecoveryTests, - SpinRecoveryTestFixture, + SpinBehaviorTestFixture, ::testing::Values( std::make_tuple(-M_PIf32 / 6.0, 0.1), std::make_tuple(M_PI_4f32, 0.1), diff --git a/nav2_system_tests/src/behaviors/wait/CMakeLists.txt b/nav2_system_tests/src/behaviors/wait/CMakeLists.txt new file mode 100644 index 00000000000..d6fb45401d8 --- /dev/null +++ b/nav2_system_tests/src/behaviors/wait/CMakeLists.txt @@ -0,0 +1,23 @@ +set(test_wait_behavior_exec test_wait_behavior_node) + +ament_add_gtest_executable(${test_wait_behavior_exec} + test_wait_behavior_node.cpp + wait_behavior_tester.cpp +) + +ament_target_dependencies(${test_wait_behavior_exec} + ${dependencies} +) + +ament_add_test(test_wait_behavior + GENERATE_RESULT_FOR_RETURN_CODE_ZERO + COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_wait_behavior_launch.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + TIMEOUT 180 + ENV + TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml + TEST_EXECUTABLE=$ + TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world + GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models + BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_behavior.xml +) diff --git a/nav2_system_tests/src/recoveries/spin/test_spin_recovery_launch.py b/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py similarity index 98% rename from nav2_system_tests/src/recoveries/spin/test_spin_recovery_launch.py rename to nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py index 9759d20c8a8..37f2ebd34cf 100755 --- a/nav2_system_tests/src/recoveries/spin/test_spin_recovery_launch.py +++ b/nav2_system_tests/src/behaviors/wait/test_wait_behavior_launch.py @@ -89,7 +89,7 @@ def main(argv=sys.argv[1:]): test1_action = ExecuteProcess( cmd=[testExecutable], - name='test_spin_recovery_node', + name='test_wait_behavior_node', output='screen') lts = LaunchTestService() diff --git a/nav2_system_tests/src/recoveries/wait/test_wait_recovery_node.cpp b/nav2_system_tests/src/behaviors/wait/test_wait_behavior_node.cpp similarity index 75% rename from nav2_system_tests/src/recoveries/wait/test_wait_recovery_node.cpp rename to nav2_system_tests/src/behaviors/wait/test_wait_behavior_node.cpp index 998808bf7ef..5aa00c56e9c 100644 --- a/nav2_system_tests/src/recoveries/wait/test_wait_recovery_node.cpp +++ b/nav2_system_tests/src/behaviors/wait/test_wait_behavior_node.cpp @@ -21,11 +21,11 @@ #include "rclcpp/rclcpp.hpp" -#include "wait_recovery_tester.hpp" +#include "wait_behavior_tester.hpp" using namespace std::chrono_literals; -using nav2_system_tests::WaitRecoveryTester; +using nav2_system_tests::WaitBehaviorTester; std::string testNameGenerator(const testing::TestParamInfo> & param) { @@ -35,31 +35,31 @@ std::string testNameGenerator(const testing::TestParamInfo> { public: static void SetUpTestCase() { - wait_recovery_tester = new WaitRecoveryTester(); - if (!wait_recovery_tester->isActive()) { - wait_recovery_tester->activate(); + wait_behavior_tester = new WaitBehaviorTester(); + if (!wait_behavior_tester->isActive()) { + wait_behavior_tester->activate(); } } static void TearDownTestCase() { - delete wait_recovery_tester; - wait_recovery_tester = nullptr; + delete wait_behavior_tester; + wait_behavior_tester = nullptr; } protected: - static WaitRecoveryTester * wait_recovery_tester; + static WaitBehaviorTester * wait_behavior_tester; }; -WaitRecoveryTester * WaitRecoveryTestFixture::wait_recovery_tester = nullptr; +WaitBehaviorTester * WaitBehaviorTestFixture::wait_behavior_tester = nullptr; -TEST_P(WaitRecoveryTestFixture, testSWaitRecovery) +TEST_P(WaitBehaviorTestFixture, testSWaitBehavior) { float wait_time = std::get<0>(GetParam()); float cancel = std::get<1>(GetParam()); @@ -68,9 +68,9 @@ TEST_P(WaitRecoveryTestFixture, testSWaitRecovery) int num_tries = 3; for (int i = 0; i != num_tries; i++) { if (cancel == 1.0) { - success = success || wait_recovery_tester->recoveryTestCancel(wait_time); + success = success || wait_behavior_tester->behaviorTestCancel(wait_time); } else { - success = success || wait_recovery_tester->recoveryTest(wait_time); + success = success || wait_behavior_tester->behaviorTest(wait_time); } if (success) { break; @@ -81,8 +81,8 @@ TEST_P(WaitRecoveryTestFixture, testSWaitRecovery) } INSTANTIATE_TEST_SUITE_P( - WaitRecoveryTests, - WaitRecoveryTestFixture, + WaitBehaviorTests, + WaitBehaviorTestFixture, ::testing::Values( std::make_tuple(1.0, 0.0), std::make_tuple(2.0, 0.0), diff --git a/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.cpp b/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.cpp similarity index 91% rename from nav2_system_tests/src/recoveries/wait/wait_recovery_tester.cpp rename to nav2_system_tests/src/behaviors/wait/wait_behavior_tester.cpp index 7112289b5af..ea6e58afb7d 100644 --- a/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.cpp +++ b/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.cpp @@ -23,7 +23,7 @@ #include #include -#include "wait_recovery_tester.hpp" +#include "wait_behavior_tester.hpp" using namespace std::chrono_literals; using namespace std::chrono; // NOLINT @@ -31,11 +31,11 @@ using namespace std::chrono; // NOLINT namespace nav2_system_tests { -WaitRecoveryTester::WaitRecoveryTester() +WaitBehaviorTester::WaitBehaviorTester() : is_active_(false), initial_pose_received_(false) { - node_ = rclcpp::Node::make_shared("wait_recovery_test"); + node_ = rclcpp::Node::make_shared("wait_behavior_test"); tf_buffer_ = std::make_shared(node_->get_clock()); tf_listener_ = std::make_shared(*tf_buffer_); @@ -52,17 +52,17 @@ WaitRecoveryTester::WaitRecoveryTester() subscription_ = node_->create_subscription( "amcl_pose", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(), - std::bind(&WaitRecoveryTester::amclPoseCallback, this, std::placeholders::_1)); + std::bind(&WaitBehaviorTester::amclPoseCallback, this, std::placeholders::_1)); } -WaitRecoveryTester::~WaitRecoveryTester() +WaitBehaviorTester::~WaitBehaviorTester() { if (is_active_) { deactivate(); } } -void WaitRecoveryTester::activate() +void WaitBehaviorTester::activate() { if (is_active_) { throw std::runtime_error("Trying to activate while already active"); @@ -76,7 +76,7 @@ void WaitRecoveryTester::activate() rclcpp::spin_some(node_); } - // Wait for lifecycle_manager_navigation to activate recoveries_server + // Wait for lifecycle_manager_navigation to activate behavior_server std::this_thread::sleep_for(10s); if (!client_ptr_) { @@ -95,7 +95,7 @@ void WaitRecoveryTester::activate() is_active_ = true; } -void WaitRecoveryTester::deactivate() +void WaitBehaviorTester::deactivate() { if (!is_active_) { throw std::runtime_error("Trying to deactivate while already inactive"); @@ -103,7 +103,7 @@ void WaitRecoveryTester::deactivate() is_active_ = false; } -bool WaitRecoveryTester::recoveryTest( +bool WaitBehaviorTester::behaviorTest( const float wait_time) { if (!is_active_) { @@ -111,7 +111,7 @@ bool WaitRecoveryTester::recoveryTest( return false; } - // Sleep to let recovery server be ready for serving in multiple runs + // Sleep to let behavior server be ready for serving in multiple runs std::this_thread::sleep_for(5s); auto start_time = node_->now(); @@ -172,7 +172,7 @@ bool WaitRecoveryTester::recoveryTest( return true; } -bool WaitRecoveryTester::recoveryTestCancel( +bool WaitBehaviorTester::behaviorTestCancel( const float wait_time) { if (!is_active_) { @@ -180,7 +180,7 @@ bool WaitRecoveryTester::recoveryTestCancel( return false; } - // Sleep to let recovery server be ready for serving in multiple runs + // Sleep to let behavior server be ready for serving in multiple runs std::this_thread::sleep_for(5s); auto start_time = node_->now(); @@ -249,7 +249,7 @@ bool WaitRecoveryTester::recoveryTestCancel( return false; } -void WaitRecoveryTester::sendInitialPose() +void WaitBehaviorTester::sendInitialPose() { geometry_msgs::msg::PoseWithCovarianceStamped pose; pose.header.frame_id = "map"; @@ -272,7 +272,7 @@ void WaitRecoveryTester::sendInitialPose() RCLCPP_INFO(node_->get_logger(), "Sent initial pose"); } -void WaitRecoveryTester::amclPoseCallback( +void WaitBehaviorTester::amclPoseCallback( const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr) { initial_pose_received_ = true; diff --git a/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.hpp b/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.hpp similarity index 88% rename from nav2_system_tests/src/recoveries/wait/wait_recovery_tester.hpp rename to nav2_system_tests/src/behaviors/wait/wait_behavior_tester.hpp index 4dfb90c6626..71181e24d36 100644 --- a/nav2_system_tests/src/recoveries/wait/wait_recovery_tester.hpp +++ b/nav2_system_tests/src/behaviors/wait/wait_behavior_tester.hpp @@ -14,8 +14,8 @@ // See the License for the specific language governing permissions and // limitations under the License. Reserved. -#ifndef RECOVERIES__WAIT__WAIT_RECOVERY_TESTER_HPP_ -#define RECOVERIES__WAIT__WAIT_RECOVERY_TESTER_HPP_ +#ifndef BEHAVIORS__WAIT__WAIT_BEHAVIOR_TESTER_HPP_ +#define BEHAVIORS__WAIT__WAIT_BEHAVIOR_TESTER_HPP_ #include #include @@ -39,20 +39,20 @@ namespace nav2_system_tests { -class WaitRecoveryTester +class WaitBehaviorTester { public: using Wait = nav2_msgs::action::Wait; using GoalHandleWait = rclcpp_action::ClientGoalHandle; - WaitRecoveryTester(); - ~WaitRecoveryTester(); + WaitBehaviorTester(); + ~WaitBehaviorTester(); // Runs a single test with given target yaw - bool recoveryTest( + bool behaviorTest( float time); - bool recoveryTestCancel(float time); + bool behaviorTestCancel(float time); void activate(); @@ -88,4 +88,4 @@ class WaitRecoveryTester } // namespace nav2_system_tests -#endif // RECOVERIES__WAIT__WAIT_RECOVERY_TESTER_HPP_ +#endif // BEHAVIORS__WAIT__WAIT_BEHAVIOR_TESTER_HPP_ diff --git a/nav2_system_tests/src/costmap_filters/keepout_params.yaml b/nav2_system_tests/src/costmap_filters/keepout_params.yaml index 8a4ed36dfec..0322de1411f 100644 --- a/nav2_system_tests/src/costmap_filters/keepout_params.yaml +++ b/nav2_system_tests/src/costmap_filters/keepout_params.yaml @@ -54,8 +54,8 @@ bt_navigator: robot_base_frame: base_link odom_topic: /odom # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are set in the launch - # files to allow for a commandline change default used is the - # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml & + # files to allow for a commandline change default used is the + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml & # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml plugin_lib_names: - nav2_compute_path_to_pose_action_bt_node @@ -296,18 +296,18 @@ smoother_server: ros__parameters: use_sim_time: True -recoveries_server: +behavior_server: ros__parameters: costmap_topic: local_costmap/costmap_raw footprint_topic: local_costmap/published_footprint cycle_frequency: 10.0 - recovery_plugins: ["spin", "backup", "wait"] + behavior_plugins: ["spin", "backup", "wait"] spin: - plugin: "nav2_recoveries/Spin" + plugin: "nav2_behaviors/Spin" backup: - plugin: "nav2_recoveries/BackUp" + plugin: "nav2_behaviors/BackUp" wait: - plugin: "nav2_recoveries/Wait" + plugin: "nav2_behaviors/Wait" global_frame: odom robot_base_frame: base_link transform_tolerance: 0.1 diff --git a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml index d5983e59a2f..23ac7693729 100644 --- a/nav2_system_tests/src/costmap_filters/speed_global_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_global_params.yaml @@ -54,8 +54,8 @@ bt_navigator: robot_base_frame: base_link odom_topic: /odom # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are set in the launch - # files to allow for a commandline change default used is the - # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml & + # files to allow for a commandline change default used is the + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml & # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml plugin_lib_names: - nav2_compute_path_to_pose_action_bt_node @@ -288,18 +288,18 @@ smoother_server: ros__parameters: use_sim_time: True -recoveries_server: +behavior_server: ros__parameters: costmap_topic: local_costmap/costmap_raw footprint_topic: local_costmap/published_footprint cycle_frequency: 10.0 - recovery_plugins: ["spin", "backup", "wait"] + behavior_plugins: ["spin", "backup", "wait"] spin: - plugin: "nav2_recoveries/Spin" + plugin: "nav2_behaviors/Spin" backup: - plugin: "nav2_recoveries/BackUp" + plugin: "nav2_behaviors/BackUp" wait: - plugin: "nav2_recoveries/Wait" + plugin: "nav2_behaviors/Wait" global_frame: odom robot_base_frame: base_link transform_tolerance: 0.1 diff --git a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml index eb5422a01d7..06e7ec477af 100644 --- a/nav2_system_tests/src/costmap_filters/speed_local_params.yaml +++ b/nav2_system_tests/src/costmap_filters/speed_local_params.yaml @@ -54,8 +54,8 @@ bt_navigator: robot_base_frame: base_link odom_topic: /odom # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are set in the launch - # files to allow for a commandline change default used is the - # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml & + # files to allow for a commandline change default used is the + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml & # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml plugin_lib_names: - nav2_compute_path_to_pose_action_bt_node @@ -288,18 +288,18 @@ smoother_server: ros__parameters: use_sim_time: True -recoveries_server: +behavior_server: ros__parameters: costmap_topic: local_costmap/costmap_raw footprint_topic: local_costmap/published_footprint cycle_frequency: 10.0 - recovery_plugins: ["spin", "backup", "wait"] + behavior_plugins: ["spin", "backup", "wait"] spin: - plugin: "nav2_recoveries/Spin" + plugin: "nav2_behaviors/Spin" backup: - plugin: "nav2_recoveries/BackUp" + plugin: "nav2_behaviors/BackUp" wait: - plugin: "nav2_recoveries/Wait" + plugin: "nav2_behaviors/Wait" global_frame: odom robot_base_frame: base_link transform_tolerance: 0.1 diff --git a/nav2_system_tests/src/recoveries/README.md b/nav2_system_tests/src/recoveries/README.md deleted file mode 100644 index e4b764aaeea..00000000000 --- a/nav2_system_tests/src/recoveries/README.md +++ /dev/null @@ -1,4 +0,0 @@ -# Recoveries Test - -Provides some simple tests for recovery plugins. -It creates an instance of the stack, with the recovery server loading different recovery plugins, and checks for successful recovery behaviors. diff --git a/nav2_system_tests/src/recoveries/wait/CMakeLists.txt b/nav2_system_tests/src/recoveries/wait/CMakeLists.txt deleted file mode 100644 index 6f751cf6aff..00000000000 --- a/nav2_system_tests/src/recoveries/wait/CMakeLists.txt +++ /dev/null @@ -1,23 +0,0 @@ -set(test_wait_recovery_exec test_wait_recovery_node) - -ament_add_gtest_executable(${test_wait_recovery_exec} - test_wait_recovery_node.cpp - wait_recovery_tester.cpp -) - -ament_target_dependencies(${test_wait_recovery_exec} - ${dependencies} -) - -ament_add_test(test_wait_recovery - GENERATE_RESULT_FOR_RETURN_CODE_ZERO - COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_wait_recovery_launch.py" - WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" - TIMEOUT 180 - ENV - TEST_MAP=${PROJECT_SOURCE_DIR}/maps/map_circular.yaml - TEST_EXECUTABLE=$ - TEST_WORLD=${PROJECT_SOURCE_DIR}/worlds/turtlebot3_ros2_demo.world - GAZEBO_MODEL_PATH=${PROJECT_SOURCE_DIR}/models - BT_NAVIGATOR_XML=navigate_to_pose_w_replanning_and_recovery.xml -) diff --git a/navigation2/package.xml b/navigation2/package.xml index 0805f79733d..1560cdc29a7 100644 --- a/navigation2/package.xml +++ b/navigation2/package.xml @@ -25,7 +25,8 @@ nav2_msgs nav2_navfn_planner nav2_planner - nav2_recoveries + nav2_behaviors + nav2_smoother nav2_regulated_pure_pursuit_controller nav2_rotation_shim_controller nav2_rviz_plugins diff --git a/tools/run_test_suite.bash b/tools/run_test_suite.bash index 51323c566ab..3c807c5a9f1 100755 --- a/tools/run_test_suite.bash +++ b/tools/run_test_suite.bash @@ -5,10 +5,10 @@ set -ex SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" # gets the directory of this script # Skip flaky tests. Nav2 system tests will be run later. -colcon test --packages-skip nav2_system_tests nav2_recoveries +colcon test --packages-skip nav2_system_tests nav2_behaviors -# run the stable tests in nav2_recoveries -colcon test --packages-select nav2_recoveries --ctest-args --exclude-regex "test_recoveries" +# run the stable tests in nav2_behaviors +colcon test --packages-select nav2_behaviors --ctest-args --exclude-regex "test_recoveries" # run the linters in nav2_system_tests. They only need to be run once. colcon test --packages-select nav2_system_tests --ctest-args --exclude-regex "test_.*" # run the linters