Recovery: RotateRecovery turns in place when there is no path

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

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.<type>.`. 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. | — |

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.
       </description>
     </class>
+    <class name="easynav_diagnostic_recovery/RotateRecovery" type="easynav::RotateRecovery" base_class_type="easynav_diagnostic_recovery::RecoveryMitigationBase">
+      <description>
+      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.
+      </description>
+    </class>
     <class name="easynav_diagnostic_recovery/CancelMissionRecovery" type="easynav::CancelMissionRecovery" base_class_type="easynav_diagnostic_recovery::RecoveryMitigationBase">
       <description>
       Mission-level last resort: accepts any ERROR diagnostic no other mitigation resolved (or

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 <optional>
+#include <string>
+#include <vector>
+
+#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<std::string> hardware_ids_ {"planner"};
+
+  bool done_this_episode_ {false};   ///< already turned in this episode
+  bool give_up_ {false};             ///< this activation fails at once
+  std::optional<rclcpp::Time> last_stop_time_;
+  rclcpp::Time start_time_;
+  std::optional<rclcpp::Time> 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_

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 <algorithm>
+#include <cmath>
+#include <cstdio>
+#include <string>
+#include <vector>
+
+#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<double>(*node, name + ".angle", angle_);
+  easynav::declare_parameter_if_absent<double>(*node, name + ".angular_speed", angular_speed_);
+  easynav::declare_parameter_if_absent<double>(*node, name + ".settle_time", settle_time_);
+  easynav::declare_parameter_if_absent<double>(*node, name + ".timeout", timeout_);
+  easynav::declare_parameter_if_absent<double>(*node, name + ".episode_gap", episode_gap_);
+  easynav::declare_parameter_if_absent(*node, name + ".hardware_ids", hardware_ids_);
+  node->get_parameter<double>(name + ".angle", angle_);
+  node->get_parameter<double>(name + ".angular_speed", angular_speed_);
+  node->get_parameter<double>(name + ".settle_time", settle_time_);
+  node->get_parameter<double>(name + ".timeout", timeout_);
+  node->get_parameter<double>(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<nav_msgs::msg::Odometry>("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/class_list_macros.hpp>
+PLUGINLIB_EXPORT_CLASS(
+  easynav::RotateRecovery,
+  easynav_diagnostic_recovery::RecoveryMitigationBase)

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)