diff --git a/recoveries/easynav_diagnostic_recovery/CMakeLists.txt b/recoveries/easynav_diagnostic_recovery/CMakeLists.txt index f719c717..8b31cdaf 100644 --- a/recoveries/easynav_diagnostic_recovery/CMakeLists.txt +++ b/recoveries/easynav_diagnostic_recovery/CMakeLists.txt @@ -33,6 +33,7 @@ add_library(${PROJECT_NAME} SHARED src/easynav_diagnostic_recovery/evaluators/RosGraphEvaluator.cpp src/easynav_diagnostic_recovery/evaluators/SafetyChannelEvaluator.cpp src/easynav_diagnostic_recovery/mitigations/AdvanceRecovery.cpp + src/easynav_diagnostic_recovery/mitigations/RotateRecovery.cpp src/easynav_diagnostic_recovery/mitigations/CancelMissionRecovery.cpp src/easynav_diagnostic_recovery/mitigations/HumanAssistanceRecovery.cpp src/easynav_diagnostic_recovery/mitigations/SafeRetreatRecovery.cpp diff --git a/recoveries/easynav_diagnostic_recovery/README.md b/recoveries/easynav_diagnostic_recovery/README.md index 97b1349a..eeac7a7d 100644 --- a/recoveries/easynav_diagnostic_recovery/README.md +++ b/recoveries/easynav_diagnostic_recovery/README.md @@ -46,7 +46,7 @@ under `tests/`, with the same subfolders: | (top level) | The manager, the three plugin interfaces, `ObstacleProximity`, and dummy plugins (`easynav_diagnostic_recovery` namespace) | | `reflexes/` | `CollisionSafetyReflex` | | `evaluators/` | `NoPathEvaluator`, `ObstacleTooCloseEvaluator`, `ControllerStuckEvaluator`, `RosGraphEvaluator`, `SafetyChannelEvaluator` | -| `mitigations/` | `SafeRetreatRecovery`, `AdvanceRecovery`, `ShutdownRecovery`, `HumanAssistanceRecovery`, `CancelMissionRecovery` | +| `mitigations/` | `SafeRetreatRecovery`, `AdvanceRecovery`, `RotateRecovery`, `ShutdownRecovery`, `HumanAssistanceRecovery`, `CancelMissionRecovery` | A component can also ship recovery for its own failures. For example, `easynav_costmap_localizer` ships `AmclConvergenceEvaluator` and `AmclRelocalizeMitigation`. @@ -66,6 +66,7 @@ Each plugin's parameters are under `recovery_manager..`. Every mitigation | `easynav_diagnostic_recovery/SafetyChannelEvaluator` | `WARN` (`safety_channel`) during a protective stop of the safety channel, or with its status lost (`safety_status`, see `system_node`'s `safety.status.timeout`); `ERROR` once it lasts `max_stop_time` s. | `max_stop_time` (0: never `ERROR`) | | `easynav_diagnostic_recovery/SafeRetreatRecovery` | Handles `obstacle_proximity`: moves straight away until `safe_distance` (from the robot center): backward from an obstacle ahead, forward from one behind or beside (the other way if that one is blocked and the obstacle is beside). Fails, stopped, without perception or a clear way (`min_clearance` along its corridor). | `safe_distance` (0.6), `retreat_speed` (0.15), `min_clearance` (0.05), `z_min_filter` (0.0) | | `easynav_diagnostic_recovery/AdvanceRecovery` | Handles `controller_stuck`: moves forward a little. Gives up after `escalate_after` s of repeated attempts. | `advance_distance` (0.3), `advance_speed` (0.1), `escalate_after` (15.0), `episode_gap` (10.0) | +| `easynav_diagnostic_recovery/RotateRecovery` | Handles `planner` (no path): turns in place `angle` so the sensors look all around (with a map that remembers obstacles, e.g. the NavMap `ObstacleFilter` with `memory.enabled`, this clears those that have gone), then stands still `settle_time` s for the planner. Once per episode: if there is still no path within `episode_gap` s, it fails so the next mitigation (e.g. `HumanAssistanceRecovery`) takes over; also if the turn takes longer than `timeout` (`0`: twice the expected plus 5 s). Give it a lower priority number than `HumanAssistanceRecovery`. | `angle` (2π; negative turns clockwise), `angular_speed` (0.5), `settle_time` (2.0), `timeout` (0.0), `episode_gap` (30.0), `hardware_ids` ([`planner`]) | | `easynav_diagnostic_recovery/ShutdownRecovery` | Handles the listed diagnostics by terminating EasyNav. | `handled_hardware_ids` ([`ros_graph`]) | | `easynav_diagnostic_recovery/HumanAssistanceRecovery` | Handles any `ERROR` not in `ignored_hardware_ids`: stops and waits until those clear. `timeout` 0 waits forever. | `timeout` (0.0), `ignored_hardware_ids` ([`ros_graph`]) | | `easynav_diagnostic_recovery/CancelMissionRecovery` | Handles any `ERROR`: aborts the mission. The last resort. | — | diff --git a/recoveries/easynav_diagnostic_recovery/easynav_diagnostic_recovery_components_plugins.xml b/recoveries/easynav_diagnostic_recovery/easynav_diagnostic_recovery_components_plugins.xml index 5e4388d7..88c57b5b 100644 --- a/recoveries/easynav_diagnostic_recovery/easynav_diagnostic_recovery_components_plugins.xml +++ b/recoveries/easynav_diagnostic_recovery/easynav_diagnostic_recovery_components_plugins.xml @@ -41,6 +41,14 @@ in total, it gives up and escalates instead of retrying forever. + + + Turns in place (a full turn by default) for ERROR diagnostics of the "planner" (no path), + so the sensors look all around (and clear remembered obstacles) before anyone is asked for + help; then stands still for the planner. Once per episode: if there is still no path, it + fails so the next mitigation (e.g. HumanAssistanceRecovery) takes over. + + Mission-level last resort: accepts any ERROR diagnostic no other mitigation resolved (or diff --git a/recoveries/easynav_diagnostic_recovery/include/easynav_diagnostic_recovery/mitigations/RotateRecovery.hpp b/recoveries/easynav_diagnostic_recovery/include/easynav_diagnostic_recovery/mitigations/RotateRecovery.hpp new file mode 100644 index 00000000..86b4f5c8 --- /dev/null +++ b/recoveries/easynav_diagnostic_recovery/include/easynav_diagnostic_recovery/mitigations/RotateRecovery.hpp @@ -0,0 +1,82 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef EASYNAV_DIAGNOSTIC_RECOVERY__MITIGATIONS__ROTATERECOVERY_HPP_ +#define EASYNAV_DIAGNOSTIC_RECOVERY__MITIGATIONS__ROTATERECOVERY_HPP_ + +#include +#include +#include + +#include "rclcpp/time.hpp" +#include "easynav_diagnostic_recovery/RecoveryMitigationBase.hpp" + +namespace easynav +{ + +/** + * @class RotateRecovery + * @brief Level-1 movement mitigation: turns in place (a full turn by default) when there is no + * path, so the sensors look all around before anyone is asked for help. + * + * With a map that remembers obstacles (e.g. the NavMap ObstacleFilter with memory), an obstacle + * that has gone stays in the map until a sensor sees through it again; a turn in place clears + * it, and the planner may find a path again. Selected for ERROR diagnostics whose hardware_id is + * in "hardware_ids" ("planner": NoPathEvaluator). Takes control of "cmd_vel"; the level-0 reflex + * still gates the motion. + * + * One activation turns "angle" at "angular_speed", then stands still "settle_time" seconds for + * the planner to replan with what the sensors saw, and SUCCEEDS. It turns only once per episode: + * if the diagnostic comes back within "episode_gap" seconds of the last activation, the turn did + * not help, and it FAILS at once so RecoveryManagerNode escalates to the next mitigation (e.g. + * HumanAssistanceRecovery). It also FAILS if the turn takes longer than "timeout" (blocked). + */ +class RotateRecovery : public easynav_diagnostic_recovery::RecoveryMitigationBase +{ +public: + RotateRecovery() = default; + ~RotateRecovery() = default; + + void on_initialize() override; + + bool can_handle(const diagnostic_msgs::msg::DiagnosticStatus & status) const override; + bool requires_control() const override {return true;} + +protected: + void on_start(NavState & nav_state) override; + easynav_diagnostic_recovery::RecoveryStatus on_cycle(NavState & nav_state) override; + void on_stop(NavState & nav_state) override; + +private: + double yaw(NavState & nav_state) const; + + double angle_ {6.283185307179586}; ///< rad to turn (a full turn); its sign is the direction + double angular_speed_ {0.5}; ///< rad/s + double settle_time_ {2.0}; ///< s standing still after the turn + double timeout_ {0.0}; ///< s for the turn; <= 0: twice the expected plus 5 s + double episode_gap_ {30.0}; ///< s: a new activation sooner is the same episode + std::vector hardware_ids_ {"planner"}; + + bool done_this_episode_ {false}; ///< already turned in this episode + bool give_up_ {false}; ///< this activation fails at once + std::optional last_stop_time_; + rclcpp::Time start_time_; + std::optional settle_start_; + double turned_ {0.0}; ///< rad turned in this activation (absolute) + double last_yaw_ {0.0}; +}; + +} // namespace easynav + +#endif // EASYNAV_DIAGNOSTIC_RECOVERY__MITIGATIONS__ROTATERECOVERY_HPP_ diff --git a/recoveries/easynav_diagnostic_recovery/src/easynav_diagnostic_recovery/mitigations/RotateRecovery.cpp b/recoveries/easynav_diagnostic_recovery/src/easynav_diagnostic_recovery/mitigations/RotateRecovery.cpp new file mode 100644 index 00000000..8ced313b --- /dev/null +++ b/recoveries/easynav_diagnostic_recovery/src/easynav_diagnostic_recovery/mitigations/RotateRecovery.cpp @@ -0,0 +1,165 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include + +#include "easynav_common/Parameters.hpp" +#include "easynav_common/RTTFBuffer.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" + +#include "easynav_diagnostic_recovery/mitigations/RotateRecovery.hpp" + +namespace easynav +{ + +namespace +{ +std::string fmt(double v) +{ + char b[32]; + std::snprintf(b, sizeof(b), "%.0f", v); + return b; +} +} // namespace + +void RotateRecovery::on_initialize() +{ + auto node = get_node(); + const auto & name = get_plugin_name(); + easynav::declare_parameter_if_absent(*node, name + ".angle", angle_); + easynav::declare_parameter_if_absent(*node, name + ".angular_speed", angular_speed_); + easynav::declare_parameter_if_absent(*node, name + ".settle_time", settle_time_); + easynav::declare_parameter_if_absent(*node, name + ".timeout", timeout_); + easynav::declare_parameter_if_absent(*node, name + ".episode_gap", episode_gap_); + easynav::declare_parameter_if_absent(*node, name + ".hardware_ids", hardware_ids_); + node->get_parameter(name + ".angle", angle_); + node->get_parameter(name + ".angular_speed", angular_speed_); + node->get_parameter(name + ".settle_time", settle_time_); + node->get_parameter(name + ".timeout", timeout_); + node->get_parameter(name + ".episode_gap", episode_gap_); + node->get_parameter(name + ".hardware_ids", hardware_ids_); + + if (!(std::abs(angular_speed_) > 0.0) || !std::isfinite(angular_speed_)) { + RCLCPP_WARN( + node->get_logger(), "[%s] angular_speed must be non-zero: using 0.5 rad/s", name.c_str()); + angular_speed_ = 0.5; + } + angular_speed_ = std::abs(angular_speed_); + if (!std::isfinite(angle_) || angle_ == 0.0) { + RCLCPP_WARN(node->get_logger(), "[%s] angle must be non-zero: using a full turn", name.c_str()); + angle_ = 2.0 * M_PI; + } + if (!(settle_time_ >= 0.0)) {settle_time_ = 0.0;} + if (timeout_ <= 0.0 || !std::isfinite(timeout_)) { + timeout_ = 2.0 * std::abs(angle_) / angular_speed_ + 5.0; + } +} + +bool RotateRecovery::can_handle(const diagnostic_msgs::msg::DiagnosticStatus & status) const +{ + return status.level >= diagnostic_msgs::msg::DiagnosticStatus::ERROR && + std::find(hardware_ids_.begin(), hardware_ids_.end(), status.hardware_id) != + hardware_ids_.end(); +} + +double RotateRecovery::yaw(NavState & nav_state) const +{ + const auto q = nav_state.get_safe("robot_pose").pose.pose.orientation; + return std::atan2(2.0 * (q.w * q.z + q.x * q.y), 1.0 - 2.0 * (q.y * q.y + q.z * q.z)); +} + +void RotateRecovery::on_start(NavState & nav_state) +{ + const rclcpp::Time now = get_node()->now(); + if (last_stop_time_.has_value() && (now - *last_stop_time_).seconds() > episode_gap_) { + done_this_episode_ = false; // long enough since the last one: a new episode + } + // Once per episode: back so soon means the turn did not help + give_up_ = done_this_episode_; + start_time_ = now; + settle_start_.reset(); + turned_ = 0.0; + last_yaw_ = yaw(nav_state); + if (give_up_) { + report( + nav_state, rcl_interfaces::msg::Log::WARN, + "RotateRecovery [" + get_plugin_name() + "]: already turned in place, still no path: " + "escalating"); + return; + } + report( + nav_state, rcl_interfaces::msg::Log::WARN, + "RotateRecovery [" + get_plugin_name() + "]: no path, turning in place " + + fmt(std::abs(angle_) * 180.0 / M_PI) + " deg to look around"); +} + +easynav_diagnostic_recovery::RecoveryStatus RotateRecovery::on_cycle(NavState & nav_state) +{ + using easynav_diagnostic_recovery::RecoveryStatus; + if (give_up_) { + stop_robot(nav_state); + return RecoveryStatus::FAILED; + } + const rclcpp::Time now = get_node()->now(); + + // Turned enough: stand still for the planner, then done + if (settle_start_.has_value()) { + stop_robot(nav_state); + return (now - *settle_start_).seconds() >= settle_time_ ? RecoveryStatus::SUCCEEDED : + RecoveryStatus::RUNNING; + } + + const double y = yaw(nav_state); + turned_ += std::abs(std::remainder(y - last_yaw_, 2.0 * M_PI)); + last_yaw_ = y; + if (turned_ >= std::abs(angle_)) { + done_this_episode_ = true; + settle_start_ = now; + stop_robot(nav_state); + return settle_time_ <= 0.0 ? RecoveryStatus::SUCCEEDED : RecoveryStatus::RUNNING; + } + if ((now - start_time_).seconds() >= timeout_) { + done_this_episode_ = true; + report( + nav_state, rcl_interfaces::msg::Log::ERROR, + "RotateRecovery [" + get_plugin_name() + "]: could not turn in " + fmt(timeout_) + + " s (turned " + fmt(turned_ * 180.0 / M_PI) + " deg): escalating"); + stop_robot(nav_state); + return RecoveryStatus::FAILED; + } + + geometry_msgs::msg::TwistStamped cmd; + cmd.header.stamp = now; + cmd.header.frame_id = RTTFBuffer::getInstance()->get_tf_info().robot_frame; + cmd.twist.angular.z = angle_ > 0.0 ? angular_speed_ : -angular_speed_; + command_velocity(nav_state, cmd); + return RecoveryStatus::RUNNING; +} + +void RotateRecovery::on_stop(NavState &) +{ + last_stop_time_ = get_node()->now(); +} + +} // namespace easynav + +#include +PLUGINLIB_EXPORT_CLASS( + easynav::RotateRecovery, + easynav_diagnostic_recovery::RecoveryMitigationBase) diff --git a/recoveries/easynav_diagnostic_recovery/tests/CMakeLists.txt b/recoveries/easynav_diagnostic_recovery/tests/CMakeLists.txt index f8da8b3a..3cf4d5ee 100644 --- a/recoveries/easynav_diagnostic_recovery/tests/CMakeLists.txt +++ b/recoveries/easynav_diagnostic_recovery/tests/CMakeLists.txt @@ -35,6 +35,7 @@ foreach(test advance_recovery_tests cancel_mission_recovery_tests human_assistance_recovery_tests + rotate_recovery_tests safe_retreat_recovery_tests shutdown_recovery_tests) ament_add_gtest(${test} mitigations/${test}.cpp) diff --git a/recoveries/easynav_diagnostic_recovery/tests/mitigations/rotate_recovery_tests.cpp b/recoveries/easynav_diagnostic_recovery/tests/mitigations/rotate_recovery_tests.cpp new file mode 100644 index 00000000..29c0397f --- /dev/null +++ b/recoveries/easynav_diagnostic_recovery/tests/mitigations/rotate_recovery_tests.cpp @@ -0,0 +1,195 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include +#include +#include + +#include "gtest/gtest.h" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "nav_msgs/msg/odometry.hpp" +#include "easynav_common/RTTFBuffer.hpp" + +#include "easynav_diagnostic_recovery/mitigations/RotateRecovery.hpp" +#include "easynav_core/VelocityCommand.hpp" + +using easynav_diagnostic_recovery::RecoveryStatus; + +class RotateRecoveryTest : public ::testing::Test +{ +protected: + void SetUp() override + { + if (!rclcpp::ok()) {rclcpp::init(0, nullptr);} + easynav::TFInfo tf_info; + tf_info.robot_frame = "base_link"; + easynav::RTTFBuffer::getInstance()->set_tf_info(tf_info); + } + + std::shared_ptr make( + const std::string & name, const std::vector & params = {}) + { + static int count = 0; + rclcpp::NodeOptions opts; + for (const auto & p : params) { + opts.append_parameter_override(name + "." + p.get_name(), p.get_parameter_value()); + } + node_ = std::make_shared( + "rotate_recovery_" + std::to_string(count++), opts); + auto rec = std::make_shared(); + rec->initialize(node_, name); + return rec; + } + + void yaw(double a) + { + nav_msgs::msg::Odometry odom; + odom.pose.pose.orientation.z = std::sin(a / 2.0); + odom.pose.pose.orientation.w = std::cos(a / 2.0); + nav_state_.set("robot_pose", odom); + } + + double commanded_wz() + { + const auto cmd = easynav::velocity_command::peek(nav_state_, easynav::VelocitySource::TAKEOVER); + EXPECT_TRUE(cmd.has_value()); + return cmd ? cmd->twist.angular.z : NAN; + } + + // Turns a full turn in steps of 1.5 rad (crossing +-pi) and returns the status after it + RecoveryStatus full_turn(easynav::RotateRecovery & rec) + { + RecoveryStatus st = RecoveryStatus::RUNNING; + for (double a = 0.0; a <= 2.0 * M_PI + 1.5 && st == RecoveryStatus::RUNNING; a += 1.5) { + yaw(std::remainder(a, 2.0 * M_PI)); + st = rec.internal_cycle(nav_state_); + if (a + 1.5 <= 2.0 * M_PI) {EXPECT_EQ(st, RecoveryStatus::RUNNING) << a;} + } + return st; + } + + rclcpp_lifecycle::LifecycleNode::SharedPtr node_; + easynav::NavState nav_state_; +}; + +TEST_F(RotateRecoveryTest, HandlesNoPathErrorsAndTakesControl) +{ + auto rec = make("rotate"); + EXPECT_TRUE(rec->requires_control()); + diagnostic_msgs::msg::DiagnosticStatus s; + s.hardware_id = "planner"; + s.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + EXPECT_TRUE(rec->can_handle(s)); + s.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; // no path published yet + EXPECT_FALSE(rec->can_handle(s)); + s.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + s.hardware_id = "controller_stuck"; + EXPECT_FALSE(rec->can_handle(s)); + auto custom = make( + "rotate", {rclcpp::Parameter("hardware_ids", std::vector{"controller_stuck"})}); + EXPECT_TRUE(custom->can_handle(s)); +} + +TEST_F(RotateRecoveryTest, TurnsAFullTurnThenSettlesAndSucceeds) +{ + auto rec = make("rotate", {rclcpp::Parameter("settle_time", 0.05)}); + yaw(0.0); + rec->internal_start(nav_state_); + EXPECT_EQ(rec->internal_cycle(nav_state_), RecoveryStatus::RUNNING); + EXPECT_DOUBLE_EQ(commanded_wz(), 0.5); + // Through +-pi: the turned angle accumulates + EXPECT_EQ(full_turn(*rec), RecoveryStatus::RUNNING); // turned: now standing still + EXPECT_DOUBLE_EQ(commanded_wz(), 0.0); + EXPECT_EQ(rec->internal_cycle(nav_state_), RecoveryStatus::RUNNING); + std::this_thread::sleep_for(std::chrono::milliseconds(60)); + EXPECT_EQ(rec->internal_cycle(nav_state_), RecoveryStatus::SUCCEEDED); + EXPECT_DOUBLE_EQ(commanded_wz(), 0.0); +} + +TEST_F(RotateRecoveryTest, ANegativeAngleTurnsClockwiseAndNoSettleSucceedsAtOnce) +{ + auto rec = make( + "rotate", {rclcpp::Parameter("angle", -M_PI), rclcpp::Parameter("angular_speed", 0.8), + rclcpp::Parameter("settle_time", 0.0)}); + yaw(0.0); + rec->internal_start(nav_state_); + EXPECT_EQ(rec->internal_cycle(nav_state_), RecoveryStatus::RUNNING); + EXPECT_DOUBLE_EQ(commanded_wz(), -0.8); + yaw(-2.0); + EXPECT_EQ(rec->internal_cycle(nav_state_), RecoveryStatus::RUNNING); + yaw(2.9); // -3.38 rad: past half a turn, across -pi + EXPECT_EQ(rec->internal_cycle(nav_state_), RecoveryStatus::SUCCEEDED); +} + +TEST_F(RotateRecoveryTest, OncePerEpisodeThenEscalates) +{ + auto rec = make("rotate", {rclcpp::Parameter("settle_time", 0.0)}); + yaw(0.0); + rec->internal_start(nav_state_); + EXPECT_EQ(full_turn(*rec), RecoveryStatus::SUCCEEDED); + rec->internal_stop(nav_state_); + // Still no path right after: the turn did not help, give way to the next mitigation + rec->internal_start(nav_state_); + EXPECT_EQ(rec->internal_cycle(nav_state_), RecoveryStatus::FAILED); + EXPECT_DOUBLE_EQ(commanded_wz(), 0.0); + rec->internal_stop(nav_state_); + rec->internal_start(nav_state_); + EXPECT_EQ(rec->internal_cycle(nav_state_), RecoveryStatus::FAILED); +} + +TEST_F(RotateRecoveryTest, ALongGapStartsANewEpisode) +{ + auto rec = make( + "rotate", {rclcpp::Parameter("settle_time", 0.0), rclcpp::Parameter("episode_gap", 0.03)}); + yaw(0.0); + rec->internal_start(nav_state_); + EXPECT_EQ(full_turn(*rec), RecoveryStatus::SUCCEEDED); + rec->internal_stop(nav_state_); + std::this_thread::sleep_for(std::chrono::milliseconds(60)); + rec->internal_start(nav_state_); + EXPECT_EQ(rec->internal_cycle(nav_state_), RecoveryStatus::RUNNING); + EXPECT_DOUBLE_EQ(commanded_wz(), 0.5); +} + +TEST_F(RotateRecoveryTest, FailsIfTheRobotDoesNotTurn) +{ + auto rec = make("rotate", {rclcpp::Parameter("timeout", 0.05)}); + yaw(1.0); + rec->internal_start(nav_state_); + EXPECT_EQ(rec->internal_cycle(nav_state_), RecoveryStatus::RUNNING); + std::this_thread::sleep_for(std::chrono::milliseconds(60)); + EXPECT_EQ(rec->internal_cycle(nav_state_), RecoveryStatus::FAILED); // blocked: never turned + EXPECT_DOUBLE_EQ(commanded_wz(), 0.0); + rec->internal_stop(nav_state_); + rec->internal_start(nav_state_); // and it does not try again in this episode + EXPECT_EQ(rec->internal_cycle(nav_state_), RecoveryStatus::FAILED); +} + +TEST_F(RotateRecoveryTest, InvalidParametersFallBackToTheDefaults) +{ + auto rec = make( + "rotate", {rclcpp::Parameter("angle", 0.0), rclcpp::Parameter("angular_speed", 0.0), + rclcpp::Parameter("settle_time", 0.0)}); + yaw(0.0); + rec->internal_start(nav_state_); + EXPECT_EQ(rec->internal_cycle(nav_state_), RecoveryStatus::RUNNING); + EXPECT_DOUBLE_EQ(commanded_wz(), 0.5); + EXPECT_EQ(full_turn(*rec), RecoveryStatus::SUCCEEDED); // a full turn +}