From 74184becb00ff4832e1f2ade7b75e92467801485 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Fri, 11 Sep 2026 21:11:52 +0200 Subject: [PATCH 1/4] evaluators & mitigations MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../easynav_no_path_evaluator/CMakeLists.txt | 70 +++++++++ .../easynav_no_path_evaluator_plugins.xml | 10 ++ .../NoPathEvaluator.hpp | 48 ++++++ .../easynav_no_path_evaluator/package.xml | 27 ++++ .../NoPathEvaluator.cpp | 58 +++++++ .../tests/CMakeLists.txt | 2 + .../tests/no_path_evaluator_tests.cpp | 89 +++++++++++ .../CMakeLists.txt | 70 +++++++++ ...v_obstacle_too_close_evaluator_plugins.xml | 10 ++ .../ObstacleTooCloseEvaluator.hpp | 65 ++++++++ .../package.xml | 26 ++++ .../ObstacleTooCloseEvaluator.cpp | 102 ++++++++++++ .../tests/CMakeLists.txt | 2 + .../obstacle_too_close_evaluator_tests.cpp | 138 +++++++++++++++++ .../easynav_clear_map_recovery/CMakeLists.txt | 67 ++++++++ .../easynav_clear_map_recovery_plugins.xml | 10 ++ .../ClearMapRecovery.hpp | 61 ++++++++ .../easynav_clear_map_recovery/package.xml | 25 +++ .../ClearMapRecovery.cpp | 51 ++++++ .../tests/CMakeLists.txt | 2 + .../tests/clear_map_recovery_tests.cpp | 80 ++++++++++ .../CMakeLists.txt | 67 ++++++++ .../easynav_force_replan_recovery_plugins.xml | 10 ++ .../ForceReplanRecovery.hpp | 61 ++++++++ .../easynav_force_replan_recovery/package.xml | 25 +++ .../ForceReplanRecovery.cpp | 52 +++++++ .../tests/CMakeLists.txt | 2 + .../tests/force_replan_recovery_tests.cpp | 94 ++++++++++++ .../CMakeLists.txt | 70 +++++++++ ...synav_notify_and_hold_recovery_plugins.xml | 12 ++ .../NotifyAndHoldRecovery.hpp | 72 +++++++++ .../package.xml | 26 ++++ .../NotifyAndHoldRecovery.cpp | 93 +++++++++++ .../tests/CMakeLists.txt | 2 + .../tests/notify_and_hold_recovery_tests.cpp | 133 ++++++++++++++++ .../CMakeLists.txt | 70 +++++++++ .../easynav_safe_retreat_recovery_plugins.xml | 11 ++ .../SafeRetreatRecovery.hpp | 72 +++++++++ .../easynav_safe_retreat_recovery/package.xml | 26 ++++ .../SafeRetreatRecovery.cpp | 91 +++++++++++ .../tests/CMakeLists.txt | 2 + .../tests/safe_retreat_recovery_tests.cpp | 145 ++++++++++++++++++ .../CMakeLists.txt | 70 +++++++++ ...easynav_safe_waypoint_recovery_plugins.xml | 11 ++ .../SafeWaypointRecovery.hpp | 90 +++++++++++ .../package.xml | 26 ++++ .../SafeWaypointRecovery.cpp | 91 +++++++++++ .../tests/CMakeLists.txt | 2 + .../tests/safe_waypoint_recovery_tests.cpp | 113 ++++++++++++++ 49 files changed, 2552 insertions(+) create mode 100644 recovery_evaluators/easynav_no_path_evaluator/CMakeLists.txt create mode 100644 recovery_evaluators/easynav_no_path_evaluator/easynav_no_path_evaluator_plugins.xml create mode 100644 recovery_evaluators/easynav_no_path_evaluator/include/easynav_no_path_evaluator/NoPathEvaluator.hpp create mode 100644 recovery_evaluators/easynav_no_path_evaluator/package.xml create mode 100644 recovery_evaluators/easynav_no_path_evaluator/src/easynav_no_path_evaluator/NoPathEvaluator.cpp create mode 100644 recovery_evaluators/easynav_no_path_evaluator/tests/CMakeLists.txt create mode 100644 recovery_evaluators/easynav_no_path_evaluator/tests/no_path_evaluator_tests.cpp create mode 100644 recovery_evaluators/easynav_obstacle_too_close_evaluator/CMakeLists.txt create mode 100644 recovery_evaluators/easynav_obstacle_too_close_evaluator/easynav_obstacle_too_close_evaluator_plugins.xml create mode 100644 recovery_evaluators/easynav_obstacle_too_close_evaluator/include/easynav_obstacle_too_close_evaluator/ObstacleTooCloseEvaluator.hpp create mode 100644 recovery_evaluators/easynav_obstacle_too_close_evaluator/package.xml create mode 100644 recovery_evaluators/easynav_obstacle_too_close_evaluator/src/easynav_obstacle_too_close_evaluator/ObstacleTooCloseEvaluator.cpp create mode 100644 recovery_evaluators/easynav_obstacle_too_close_evaluator/tests/CMakeLists.txt create mode 100644 recovery_evaluators/easynav_obstacle_too_close_evaluator/tests/obstacle_too_close_evaluator_tests.cpp create mode 100644 recovery_mitigations/easynav_clear_map_recovery/CMakeLists.txt create mode 100644 recovery_mitigations/easynav_clear_map_recovery/easynav_clear_map_recovery_plugins.xml create mode 100644 recovery_mitigations/easynav_clear_map_recovery/include/easynav_clear_map_recovery/ClearMapRecovery.hpp create mode 100644 recovery_mitigations/easynav_clear_map_recovery/package.xml create mode 100644 recovery_mitigations/easynav_clear_map_recovery/src/easynav_clear_map_recovery/ClearMapRecovery.cpp create mode 100644 recovery_mitigations/easynav_clear_map_recovery/tests/CMakeLists.txt create mode 100644 recovery_mitigations/easynav_clear_map_recovery/tests/clear_map_recovery_tests.cpp create mode 100644 recovery_mitigations/easynav_force_replan_recovery/CMakeLists.txt create mode 100644 recovery_mitigations/easynav_force_replan_recovery/easynav_force_replan_recovery_plugins.xml create mode 100644 recovery_mitigations/easynav_force_replan_recovery/include/easynav_force_replan_recovery/ForceReplanRecovery.hpp create mode 100644 recovery_mitigations/easynav_force_replan_recovery/package.xml create mode 100644 recovery_mitigations/easynav_force_replan_recovery/src/easynav_force_replan_recovery/ForceReplanRecovery.cpp create mode 100644 recovery_mitigations/easynav_force_replan_recovery/tests/CMakeLists.txt create mode 100644 recovery_mitigations/easynav_force_replan_recovery/tests/force_replan_recovery_tests.cpp create mode 100644 recovery_mitigations/easynav_notify_and_hold_recovery/CMakeLists.txt create mode 100644 recovery_mitigations/easynav_notify_and_hold_recovery/easynav_notify_and_hold_recovery_plugins.xml create mode 100644 recovery_mitigations/easynav_notify_and_hold_recovery/include/easynav_notify_and_hold_recovery/NotifyAndHoldRecovery.hpp create mode 100644 recovery_mitigations/easynav_notify_and_hold_recovery/package.xml create mode 100644 recovery_mitigations/easynav_notify_and_hold_recovery/src/easynav_notify_and_hold_recovery/NotifyAndHoldRecovery.cpp create mode 100644 recovery_mitigations/easynav_notify_and_hold_recovery/tests/CMakeLists.txt create mode 100644 recovery_mitigations/easynav_notify_and_hold_recovery/tests/notify_and_hold_recovery_tests.cpp create mode 100644 recovery_mitigations/easynav_safe_retreat_recovery/CMakeLists.txt create mode 100644 recovery_mitigations/easynav_safe_retreat_recovery/easynav_safe_retreat_recovery_plugins.xml create mode 100644 recovery_mitigations/easynav_safe_retreat_recovery/include/easynav_safe_retreat_recovery/SafeRetreatRecovery.hpp create mode 100644 recovery_mitigations/easynav_safe_retreat_recovery/package.xml create mode 100644 recovery_mitigations/easynav_safe_retreat_recovery/src/easynav_safe_retreat_recovery/SafeRetreatRecovery.cpp create mode 100644 recovery_mitigations/easynav_safe_retreat_recovery/tests/CMakeLists.txt create mode 100644 recovery_mitigations/easynav_safe_retreat_recovery/tests/safe_retreat_recovery_tests.cpp create mode 100644 recovery_mitigations/easynav_safe_waypoint_recovery/CMakeLists.txt create mode 100644 recovery_mitigations/easynav_safe_waypoint_recovery/easynav_safe_waypoint_recovery_plugins.xml create mode 100644 recovery_mitigations/easynav_safe_waypoint_recovery/include/easynav_safe_waypoint_recovery/SafeWaypointRecovery.hpp create mode 100644 recovery_mitigations/easynav_safe_waypoint_recovery/package.xml create mode 100644 recovery_mitigations/easynav_safe_waypoint_recovery/src/easynav_safe_waypoint_recovery/SafeWaypointRecovery.cpp create mode 100644 recovery_mitigations/easynav_safe_waypoint_recovery/tests/CMakeLists.txt create mode 100644 recovery_mitigations/easynav_safe_waypoint_recovery/tests/safe_waypoint_recovery_tests.cpp diff --git a/recovery_evaluators/easynav_no_path_evaluator/CMakeLists.txt b/recovery_evaluators/easynav_no_path_evaluator/CMakeLists.txt new file mode 100644 index 00000000..2e259f74 --- /dev/null +++ b/recovery_evaluators/easynav_no_path_evaluator/CMakeLists.txt @@ -0,0 +1,70 @@ +cmake_minimum_required(VERSION 3.20) +project(easynav_no_path_evaluator) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(easynav_common REQUIRED) +find_package(easynav_core REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(diagnostic_msgs REQUIRED) + +add_library(${PROJECT_NAME} SHARED + src/easynav_no_path_evaluator/NoPathEvaluator.cpp +) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) +target_link_libraries(${PROJECT_NAME} PUBLIC + easynav_common::easynav_common + easynav_core::easynav_core + pluginlib::pluginlib + rclcpp::rclcpp + ${nav_msgs_TARGETS} + ${diagnostic_msgs_TARGETS} +) + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +install(TARGETS + ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + add_subdirectory(tests) +endif() + +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_libraries(${PROJECT_NAME}) +ament_export_targets(export_${PROJECT_NAME}) + +# Register the recovery evaluator plugin +pluginlib_export_plugin_description_file(easynav_core easynav_no_path_evaluator_plugins.xml) + +ament_export_dependencies( + easynav_common + easynav_core + pluginlib + rclcpp + nav_msgs + diagnostic_msgs +) +ament_package() diff --git a/recovery_evaluators/easynav_no_path_evaluator/easynav_no_path_evaluator_plugins.xml b/recovery_evaluators/easynav_no_path_evaluator/easynav_no_path_evaluator_plugins.xml new file mode 100644 index 00000000..59cfb8c7 --- /dev/null +++ b/recovery_evaluators/easynav_no_path_evaluator/easynav_no_path_evaluator_plugins.xml @@ -0,0 +1,10 @@ + + + + + Diagnoses a missing or empty planner path: WARN if the planner has not published one yet, + ERROR if it published an empty path, OK otherwise. + + + + diff --git a/recovery_evaluators/easynav_no_path_evaluator/include/easynav_no_path_evaluator/NoPathEvaluator.hpp b/recovery_evaluators/easynav_no_path_evaluator/include/easynav_no_path_evaluator/NoPathEvaluator.hpp new file mode 100644 index 00000000..e004ac97 --- /dev/null +++ b/recovery_evaluators/easynav_no_path_evaluator/include/easynav_no_path_evaluator/NoPathEvaluator.hpp @@ -0,0 +1,48 @@ +// 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. + +/// \file +/// \brief Declaration of the NoPathEvaluator plugin. + +#ifndef EASYNAV_NO_PATH_EVALUATOR__NOPATHEVALUATOR_HPP_ +#define EASYNAV_NO_PATH_EVALUATOR__NOPATHEVALUATOR_HPP_ + +#include "easynav_core/RecoveryEvaluatorBase.hpp" + +namespace easynav +{ + +/** + * @class NoPathEvaluator + * @brief Level-1 recovery evaluator that diagnoses a missing or empty planner path. + * + * See docs/recoveries_easynav.md, level 1, evaluator catalog. A generic, domain-agnostic + * evaluator: it only knows about the "path" key that any PlannerMethodBase-derived plugin is + * expected to produce, not about any specific planner's internals. + */ +class NoPathEvaluator : public easynav::RecoveryEvaluatorBase +{ +public: + NoPathEvaluator() = default; + ~NoPathEvaluator() = default; + + void on_initialize() override; + +protected: + void update(NavState & nav_state) override; +}; + +} // namespace easynav + +#endif // EASYNAV_NO_PATH_EVALUATOR__NOPATHEVALUATOR_HPP_ diff --git a/recovery_evaluators/easynav_no_path_evaluator/package.xml b/recovery_evaluators/easynav_no_path_evaluator/package.xml new file mode 100644 index 00000000..1dca8f5f --- /dev/null +++ b/recovery_evaluators/easynav_no_path_evaluator/package.xml @@ -0,0 +1,27 @@ + + + + easynav_no_path_evaluator + 0.4.2 + Easy Navigation: recovery evaluator that diagnoses a missing or empty planner path. + Francisco Martín Rico + Apache-2.0 + + ament_cmake + + easynav_common + easynav_core + pluginlib + rclcpp + nav_msgs + diagnostic_msgs + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + geometry_msgs + + + ament_cmake + + diff --git a/recovery_evaluators/easynav_no_path_evaluator/src/easynav_no_path_evaluator/NoPathEvaluator.cpp b/recovery_evaluators/easynav_no_path_evaluator/src/easynav_no_path_evaluator/NoPathEvaluator.cpp new file mode 100644 index 00000000..980cb1b6 --- /dev/null +++ b/recovery_evaluators/easynav_no_path_evaluator/src/easynav_no_path_evaluator/NoPathEvaluator.cpp @@ -0,0 +1,58 @@ +// 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. + +/// \file +/// \brief Implementation of the NoPathEvaluator class. + +#include "nav_msgs/msg/path.hpp" + +#include "easynav_no_path_evaluator/NoPathEvaluator.hpp" + +namespace easynav +{ + +void NoPathEvaluator::on_initialize() +{ +} + +void NoPathEvaluator::update(NavState & nav_state) +{ + diagnostic_msgs::msg::DiagnosticStatus status; + status.name = get_plugin_name(); + status.hardware_id = "planner"; + + if (!nav_state.has("path")) { + status.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + status.message = "no path published yet"; + } else { + // Both the planner and this evaluator run on SystemNode's single non-RT cycle thread, so + // a plain get() is safe here and avoids copying a potentially large Path (see NavState's + // own get()/get_safe() guidance in NavState.hpp). + const auto & path = nav_state.get("path"); + if (path.poses.empty()) { + status.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + status.message = "planner produced an empty path"; + } else { + status.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + status.message = "path available"; + } + } + + publish_diagnostic(nav_state, status); +} + +} // namespace easynav + +#include +PLUGINLIB_EXPORT_CLASS(easynav::NoPathEvaluator, easynav::RecoveryEvaluatorBase) diff --git a/recovery_evaluators/easynav_no_path_evaluator/tests/CMakeLists.txt b/recovery_evaluators/easynav_no_path_evaluator/tests/CMakeLists.txt new file mode 100644 index 00000000..305966ab --- /dev/null +++ b/recovery_evaluators/easynav_no_path_evaluator/tests/CMakeLists.txt @@ -0,0 +1,2 @@ +ament_add_gtest(no_path_evaluator_tests no_path_evaluator_tests.cpp) +target_link_libraries(no_path_evaluator_tests ${PROJECT_NAME}) diff --git a/recovery_evaluators/easynav_no_path_evaluator/tests/no_path_evaluator_tests.cpp b/recovery_evaluators/easynav_no_path_evaluator/tests/no_path_evaluator_tests.cpp new file mode 100644 index 00000000..f18ef121 --- /dev/null +++ b/recovery_evaluators/easynav_no_path_evaluator/tests/no_path_evaluator_tests.cpp @@ -0,0 +1,89 @@ +// 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 "gtest/gtest.h" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "nav_msgs/msg/path.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" + +#include "easynav_no_path_evaluator/NoPathEvaluator.hpp" + +class NoPathEvaluatorTestCase : public ::testing::Test +{ +protected: + void SetUp() override + { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + } +}; + +TEST_F(NoPathEvaluatorTestCase, WarnsWhenNoPathYet) +{ + auto node = std::make_shared("test_no_path_node"); + easynav::NoPathEvaluator eval; + eval.initialize(node, "no_path"); + + std::this_thread::sleep_for(std::chrono::milliseconds(120)); + easynav::NavState nav_state; + eval.internal_update(nav_state); + + ASSERT_TRUE(nav_state.has("diagnostics.no_path")); + EXPECT_EQ( + nav_state.get("diagnostics.no_path").level, + diagnostic_msgs::msg::DiagnosticStatus::WARN); +} + +TEST_F(NoPathEvaluatorTestCase, ErrorsOnEmptyPath) +{ + auto node = std::make_shared("test_empty_path_node"); + easynav::NoPathEvaluator eval; + eval.initialize(node, "no_path2"); + + easynav::NavState nav_state; + nav_state.set("path", nav_msgs::msg::Path()); + + std::this_thread::sleep_for(std::chrono::milliseconds(120)); + eval.internal_update(nav_state); + + EXPECT_EQ( + nav_state.get("diagnostics.no_path2").level, + diagnostic_msgs::msg::DiagnosticStatus::ERROR); +} + +TEST_F(NoPathEvaluatorTestCase, OkWhenPathHasPoses) +{ + auto node = std::make_shared("test_ok_path_node"); + easynav::NoPathEvaluator eval; + eval.initialize(node, "no_path3"); + + nav_msgs::msg::Path path; + path.poses.push_back(geometry_msgs::msg::PoseStamped()); + easynav::NavState nav_state; + nav_state.set("path", path); + + std::this_thread::sleep_for(std::chrono::milliseconds(120)); + eval.internal_update(nav_state); + + EXPECT_EQ( + nav_state.get("diagnostics.no_path3").level, + diagnostic_msgs::msg::DiagnosticStatus::OK); +} diff --git a/recovery_evaluators/easynav_obstacle_too_close_evaluator/CMakeLists.txt b/recovery_evaluators/easynav_obstacle_too_close_evaluator/CMakeLists.txt new file mode 100644 index 00000000..190a958f --- /dev/null +++ b/recovery_evaluators/easynav_obstacle_too_close_evaluator/CMakeLists.txt @@ -0,0 +1,70 @@ +cmake_minimum_required(VERSION 3.20) +project(easynav_obstacle_too_close_evaluator) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(easynav_common REQUIRED) +find_package(easynav_core REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(diagnostic_msgs REQUIRED) + +add_library(${PROJECT_NAME} SHARED + src/easynav_obstacle_too_close_evaluator/ObstacleTooCloseEvaluator.cpp +) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) +target_link_libraries(${PROJECT_NAME} PUBLIC + easynav_common::easynav_common + easynav_core::easynav_core + pluginlib::pluginlib + rclcpp::rclcpp + ${nav_msgs_TARGETS} + ${diagnostic_msgs_TARGETS} +) + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +install(TARGETS + ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + add_subdirectory(tests) +endif() + +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_libraries(${PROJECT_NAME}) +ament_export_targets(export_${PROJECT_NAME}) + +# Register the recovery evaluator plugin +pluginlib_export_plugin_description_file(easynav_core easynav_obstacle_too_close_evaluator_plugins.xml) + +ament_export_dependencies( + easynav_common + easynav_core + pluginlib + rclcpp + nav_msgs + diagnostic_msgs +) +ament_package() diff --git a/recovery_evaluators/easynav_obstacle_too_close_evaluator/easynav_obstacle_too_close_evaluator_plugins.xml b/recovery_evaluators/easynav_obstacle_too_close_evaluator/easynav_obstacle_too_close_evaluator_plugins.xml new file mode 100644 index 00000000..26b094f9 --- /dev/null +++ b/recovery_evaluators/easynav_obstacle_too_close_evaluator/easynav_obstacle_too_close_evaluator_plugins.xml @@ -0,0 +1,10 @@ + + + + + Diagnoses the robot being stopped (near-zero measured velocity) too close to an + obstacle. Deliberately compound: it does not fire while the robot is still moving/braking. + + + + diff --git a/recovery_evaluators/easynav_obstacle_too_close_evaluator/include/easynav_obstacle_too_close_evaluator/ObstacleTooCloseEvaluator.hpp b/recovery_evaluators/easynav_obstacle_too_close_evaluator/include/easynav_obstacle_too_close_evaluator/ObstacleTooCloseEvaluator.hpp new file mode 100644 index 00000000..334b0f8a --- /dev/null +++ b/recovery_evaluators/easynav_obstacle_too_close_evaluator/include/easynav_obstacle_too_close_evaluator/ObstacleTooCloseEvaluator.hpp @@ -0,0 +1,65 @@ +// 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. + +/// \file +/// \brief Declaration of the ObstacleTooCloseEvaluator plugin. + +#ifndef EASYNAV_OBSTACLE_TOO_CLOSE_EVALUATOR__OBSTACLETOOCLOSEEVALUATOR_HPP_ +#define EASYNAV_OBSTACLE_TOO_CLOSE_EVALUATOR__OBSTACLETOOCLOSEEVALUATOR_HPP_ + +#include "easynav_core/RecoveryEvaluatorBase.hpp" + +namespace easynav +{ + +/** + * @class ObstacleTooCloseEvaluator + * @brief Level-1 recovery evaluator: diagnoses "stopped too close to an obstacle". + * + * See docs/recoveries_easynav.md §5.2/§5.13. This is deliberately a *compound* condition, not + * just "is something close": it requires the robot to already be (near) stationary before + * reporting ERROR. Rationale — the RT-level CollisionSafetyReflex reacts first and stops the + * robot; this evaluator must not fire while that stop is still happening (still decelerating), + * or a movement mitigation like SafeRetreatRecovery could take over mid-brake and substitute an + * unsafe motion for a controlled one. Rather than coupling to the reflex's internal state, the + * "already stopped" condition is checked against an independent, physically measured signal + * (the robot's own commanded/estimated velocity from "robot_pose"), so this evaluator works + * correctly regardless of *why* the robot stopped. + */ +class ObstacleTooCloseEvaluator : public easynav::RecoveryEvaluatorBase +{ +public: + ObstacleTooCloseEvaluator() = default; + ~ObstacleTooCloseEvaluator() = default; + + void on_initialize() override; + +protected: + void update(NavState & nav_state) override; + +private: + /// @brief Distance (m) below which the robot is considered too close to operate normally. + /// Deliberately more conservative (larger) than the level-0 reflex's own trigger distance. + double safe_distance_ {0.6}; + + /// @brief Below this linear speed (m/s), the robot is considered stopped. + double linear_velocity_epsilon_ {0.02}; + + /// @brief Below this angular speed (rad/s), the robot is considered stopped. + double angular_velocity_epsilon_ {0.05}; +}; + +} // namespace easynav + +#endif // EASYNAV_OBSTACLE_TOO_CLOSE_EVALUATOR__OBSTACLETOOCLOSEEVALUATOR_HPP_ diff --git a/recovery_evaluators/easynav_obstacle_too_close_evaluator/package.xml b/recovery_evaluators/easynav_obstacle_too_close_evaluator/package.xml new file mode 100644 index 00000000..ce0e2c04 --- /dev/null +++ b/recovery_evaluators/easynav_obstacle_too_close_evaluator/package.xml @@ -0,0 +1,26 @@ + + + + easynav_obstacle_too_close_evaluator + 0.4.2 + Easy Navigation: recovery evaluator that diagnoses the robot being stopped too close to an obstacle. + Francisco Martín Rico + Apache-2.0 + + ament_cmake + + easynav_common + easynav_core + pluginlib + rclcpp + nav_msgs + diagnostic_msgs + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + + + ament_cmake + + diff --git a/recovery_evaluators/easynav_obstacle_too_close_evaluator/src/easynav_obstacle_too_close_evaluator/ObstacleTooCloseEvaluator.cpp b/recovery_evaluators/easynav_obstacle_too_close_evaluator/src/easynav_obstacle_too_close_evaluator/ObstacleTooCloseEvaluator.cpp new file mode 100644 index 00000000..4a39b579 --- /dev/null +++ b/recovery_evaluators/easynav_obstacle_too_close_evaluator/src/easynav_obstacle_too_close_evaluator/ObstacleTooCloseEvaluator.cpp @@ -0,0 +1,102 @@ +// 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. + +/// \file +/// \brief Implementation of the ObstacleTooCloseEvaluator class. + +#include + +#include "nav_msgs/msg/odometry.hpp" + +#include "easynav_core/ObstacleProximity.hpp" + +#include "easynav_obstacle_too_close_evaluator/ObstacleTooCloseEvaluator.hpp" + +namespace easynav +{ + +void ObstacleTooCloseEvaluator::on_initialize() +{ + auto node = get_node(); + const auto & plugin_name = get_plugin_name(); + + node->declare_parameter(plugin_name + ".safe_distance", safe_distance_); + node->declare_parameter( + plugin_name + ".linear_velocity_epsilon", linear_velocity_epsilon_); + node->declare_parameter( + plugin_name + ".angular_velocity_epsilon", angular_velocity_epsilon_); + + node->get_parameter(plugin_name + ".safe_distance", safe_distance_); + node->get_parameter(plugin_name + ".linear_velocity_epsilon", linear_velocity_epsilon_); + node->get_parameter( + plugin_name + ".angular_velocity_epsilon", angular_velocity_epsilon_); +} + +void ObstacleTooCloseEvaluator::update(NavState & nav_state) +{ + diagnostic_msgs::msg::DiagnosticStatus status; + status.name = get_plugin_name(); + // Shared, string-based convention with SafeRetreatRecovery's can_handle() — no compile-time + // dependency between the two plugins, only this agreed-upon diagnostic vocabulary. + status.hardware_id = "obstacle_proximity"; + status.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + status.message = "no obstacle too close"; + + if (!nav_state.has("robot_pose")) { + publish_diagnostic(nav_state, status); + return; + } + + // "robot_pose" is written by LocalizerNode's RT cycle; this evaluator runs on SystemNode's + // non-RT cycle, so get_safe() (a snapshot copy) is required here, not get(). See NavState's + // own get()/get_safe() guidance. + const auto odom = nav_state.get_safe("robot_pose"); + + const double linear_speed = std::hypot( + odom.twist.twist.linear.x, odom.twist.twist.linear.y); + const double angular_speed = std::abs(odom.twist.twist.angular.z); + const bool stopped = linear_speed < linear_velocity_epsilon_ && + angular_speed < angular_velocity_epsilon_; + + if (!stopped) { + // Still moving (e.g. the level-0 reflex is still braking): too early to judge proximity as + // something this evaluator should act on. See the compound-condition rationale in the + // class doc comment. + publish_diagnostic(nav_state, status); + return; + } + + const auto obstacle = compute_nearest_obstacle(nav_state); + if (std::isfinite(obstacle.distance) && obstacle.distance < safe_distance_) { + status.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + status.message = "stopped too close to an obstacle"; + + diagnostic_msgs::msg::KeyValue distance_kv; + distance_kv.key = "distance"; + distance_kv.value = std::to_string(obstacle.distance); + status.values.push_back(distance_kv); + + diagnostic_msgs::msg::KeyValue bearing_kv; + bearing_kv.key = "bearing"; + bearing_kv.value = std::to_string(obstacle.bearing); + status.values.push_back(bearing_kv); + } + + publish_diagnostic(nav_state, status); +} + +} // namespace easynav + +#include +PLUGINLIB_EXPORT_CLASS(easynav::ObstacleTooCloseEvaluator, easynav::RecoveryEvaluatorBase) diff --git a/recovery_evaluators/easynav_obstacle_too_close_evaluator/tests/CMakeLists.txt b/recovery_evaluators/easynav_obstacle_too_close_evaluator/tests/CMakeLists.txt new file mode 100644 index 00000000..9d9af8ae --- /dev/null +++ b/recovery_evaluators/easynav_obstacle_too_close_evaluator/tests/CMakeLists.txt @@ -0,0 +1,2 @@ +ament_add_gtest(obstacle_too_close_evaluator_tests obstacle_too_close_evaluator_tests.cpp) +target_link_libraries(obstacle_too_close_evaluator_tests ${PROJECT_NAME}) diff --git a/recovery_evaluators/easynav_obstacle_too_close_evaluator/tests/obstacle_too_close_evaluator_tests.cpp b/recovery_evaluators/easynav_obstacle_too_close_evaluator/tests/obstacle_too_close_evaluator_tests.cpp new file mode 100644 index 00000000..cb77f555 --- /dev/null +++ b/recovery_evaluators/easynav_obstacle_too_close_evaluator/tests/obstacle_too_close_evaluator_tests.cpp @@ -0,0 +1,138 @@ +// 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 "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_sensors/types/PointPerception.hpp" + +#include "easynav_obstacle_too_close_evaluator/ObstacleTooCloseEvaluator.hpp" + +class ObstacleTooCloseEvaluatorTestCase : 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); + } + + static nav_msgs::msg::Odometry make_odom(double vx, double wz) + { + nav_msgs::msg::Odometry odom; + odom.twist.twist.linear.x = vx; + odom.twist.twist.angular.z = wz; + return odom; + } + + static easynav::PointPerception make_obstacle_at(double x, double y) + { + easynav::PointPerception perception; + perception.frame_id = "base_link"; + perception.stamp = rclcpp::Time(0); + perception.valid = true; + perception.data.points.resize(1); + perception.data.points[0].x = x; + perception.data.points[0].y = y; + perception.data.points[0].z = 0.0; + return perception; + } + + std::shared_ptr make_ready_evaluator( + const std::shared_ptr & node, const std::string & name) + { + auto eval = std::make_shared(); + eval->initialize(node, name); + std::this_thread::sleep_for(std::chrono::milliseconds(120)); + return eval; + } +}; + +TEST_F(ObstacleTooCloseEvaluatorTestCase, OkWithoutRobotPose) +{ + auto node = std::make_shared("test_no_pose_node"); + auto eval = make_ready_evaluator(node, "close1"); + + easynav::NavState nav_state; + eval->internal_update(nav_state); + + const auto & status = + nav_state.get("diagnostics.close1"); + EXPECT_EQ(status.level, diagnostic_msgs::msg::DiagnosticStatus::OK); +} + +TEST_F(ObstacleTooCloseEvaluatorTestCase, OkWhileStillMovingEvenIfObstacleIsClose) +{ + // Compound condition: must not fire while the robot is still moving (e.g. the level-0 reflex + // is still braking). See docs/recoveries_easynav.md, §5.2. + auto node = std::make_shared("test_moving_node"); + auto eval = make_ready_evaluator(node, "close2"); + + easynav::NavState nav_state; + nav_state.set("robot_pose", make_odom(0.5, 0.0)); // still moving + nav_state.set("obstacle_scan", make_obstacle_at(0.1, 0.0)); // very close + + eval->internal_update(nav_state); + + const auto & status = + nav_state.get("diagnostics.close2"); + EXPECT_EQ(status.level, diagnostic_msgs::msg::DiagnosticStatus::OK); +} + +TEST_F(ObstacleTooCloseEvaluatorTestCase, OkWhenStoppedButNoObstacleNearby) +{ + auto node = std::make_shared("test_stopped_far_node"); + auto eval = make_ready_evaluator(node, "close3"); + + easynav::NavState nav_state; + nav_state.set("robot_pose", make_odom(0.0, 0.0)); + nav_state.set("obstacle_scan", make_obstacle_at(5.0, 0.0)); // far away + + eval->internal_update(nav_state); + + const auto & status = + nav_state.get("diagnostics.close3"); + EXPECT_EQ(status.level, diagnostic_msgs::msg::DiagnosticStatus::OK); +} + +TEST_F(ObstacleTooCloseEvaluatorTestCase, ErrorWhenStoppedTooCloseToAnObstacle) +{ + auto node = std::make_shared("test_stopped_close_node"); + auto eval = make_ready_evaluator(node, "close4"); + + easynav::NavState nav_state; + nav_state.set("robot_pose", make_odom(0.0, 0.0)); + nav_state.set("obstacle_scan", make_obstacle_at(0.2, 0.0)); // well within default safe_distance + + eval->internal_update(nav_state); + + const auto & status = + nav_state.get("diagnostics.close4"); + EXPECT_EQ(status.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); + EXPECT_EQ(status.hardware_id, "obstacle_proximity"); + ASSERT_EQ(status.values.size(), 2u); + EXPECT_EQ(status.values[0].key, "distance"); + EXPECT_NEAR(std::stod(status.values[0].value), 0.2, 1e-3); +} diff --git a/recovery_mitigations/easynav_clear_map_recovery/CMakeLists.txt b/recovery_mitigations/easynav_clear_map_recovery/CMakeLists.txt new file mode 100644 index 00000000..201c066c --- /dev/null +++ b/recovery_mitigations/easynav_clear_map_recovery/CMakeLists.txt @@ -0,0 +1,67 @@ +cmake_minimum_required(VERSION 3.20) +project(easynav_clear_map_recovery) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(easynav_common REQUIRED) +find_package(easynav_core REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(diagnostic_msgs REQUIRED) + +add_library(${PROJECT_NAME} SHARED + src/easynav_clear_map_recovery/ClearMapRecovery.cpp +) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) +target_link_libraries(${PROJECT_NAME} PUBLIC + easynav_common::easynav_common + easynav_core::easynav_core + pluginlib::pluginlib + rclcpp::rclcpp + ${diagnostic_msgs_TARGETS} +) + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +install(TARGETS + ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + add_subdirectory(tests) +endif() + +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_libraries(${PROJECT_NAME}) +ament_export_targets(export_${PROJECT_NAME}) + +# Register the recovery mitigation plugin +pluginlib_export_plugin_description_file(easynav_core easynav_clear_map_recovery_plugins.xml) + +ament_export_dependencies( + easynav_common + easynav_core + pluginlib + rclcpp + diagnostic_msgs +) +ament_package() diff --git a/recovery_mitigations/easynav_clear_map_recovery/easynav_clear_map_recovery_plugins.xml b/recovery_mitigations/easynav_clear_map_recovery/easynav_clear_map_recovery_plugins.xml new file mode 100644 index 00000000..ad96d1ed --- /dev/null +++ b/recovery_mitigations/easynav_clear_map_recovery/easynav_clear_map_recovery_plugins.xml @@ -0,0 +1,10 @@ + + + + + Requests every loaded MapsManagerBase plugin to reset/clear its data (hardware_id + "planner", level ERROR). Does not take control_owner. + + + + diff --git a/recovery_mitigations/easynav_clear_map_recovery/include/easynav_clear_map_recovery/ClearMapRecovery.hpp b/recovery_mitigations/easynav_clear_map_recovery/include/easynav_clear_map_recovery/ClearMapRecovery.hpp new file mode 100644 index 00000000..91c73caa --- /dev/null +++ b/recovery_mitigations/easynav_clear_map_recovery/include/easynav_clear_map_recovery/ClearMapRecovery.hpp @@ -0,0 +1,61 @@ +// 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. + +/// \file +/// \brief Declaration of the ClearMapRecovery plugin. + +#ifndef EASYNAV_CLEAR_MAP_RECOVERY__CLEARMAPRECOVERY_HPP_ +#define EASYNAV_CLEAR_MAP_RECOVERY__CLEARMAPRECOVERY_HPP_ + +#include "easynav_core/RecoveryMitigationBase.hpp" + +namespace easynav +{ + +/** + * @class ClearMapRecovery + * @brief Level-1 mitigation: resets/clears whatever a MapsManagerBase plugin considers + * clearable. See docs/recoveries_easynav.md §5.11 (Nav2 Clear*Costmap precedent). + * + * Selected, like ForceReplanRecovery, for diagnostics with hardware_id == "planner" at level + * ERROR or above — the same "no_path" symptom NoPathEvaluator publishes. This is a deliberate + * simplification versus the design's more general ambition of a dedicated map-health + * evaluator (see docs/recoveries_easynav_implementation.md, Fase 4): no such evaluator exists + * yet, so for now this mitigation reuses the one generic "the planner can't find a path" + * diagnostic, relying on RecoveryManagerNode's per-diagnostic attempt count (mitigation_types + * order: ForceReplanRecovery first, then this one) to be tried only after ForceReplanRecovery + * has already had its chance and failed to unstick the planner. + * + * Does not requires_control(): only requests MapsManagerNode::reset() via the one-shot + * NavState key "maps_manager_reset_requested", consumed by SystemNode::system_cycle() right + * after it cycles MapsManagerNode. Whether that reset actually clears anything depends on + * whether any loaded MapsManagerBase plugin overrides reset() at all (the default is a no-op); + * either way this mitigation reports SUCCEEDED once the request has been handed off. + */ +class ClearMapRecovery : public easynav::RecoveryMitigationBase +{ +public: + ClearMapRecovery() = default; + ~ClearMapRecovery() = default; + + bool can_handle(const diagnostic_msgs::msg::DiagnosticStatus & status) const override; + +protected: + void on_start(NavState & nav_state) override; + RecoveryStatus on_cycle(NavState & nav_state) override; +}; + +} // namespace easynav + +#endif // EASYNAV_CLEAR_MAP_RECOVERY__CLEARMAPRECOVERY_HPP_ diff --git a/recovery_mitigations/easynav_clear_map_recovery/package.xml b/recovery_mitigations/easynav_clear_map_recovery/package.xml new file mode 100644 index 00000000..eb24a96f --- /dev/null +++ b/recovery_mitigations/easynav_clear_map_recovery/package.xml @@ -0,0 +1,25 @@ + + + + easynav_clear_map_recovery + 0.4.2 + Easy Navigation: recovery mitigation that resets/clears map manager data. + Francisco Martín Rico + Apache-2.0 + + ament_cmake + + easynav_common + easynav_core + pluginlib + rclcpp + diagnostic_msgs + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + + + ament_cmake + + diff --git a/recovery_mitigations/easynav_clear_map_recovery/src/easynav_clear_map_recovery/ClearMapRecovery.cpp b/recovery_mitigations/easynav_clear_map_recovery/src/easynav_clear_map_recovery/ClearMapRecovery.cpp new file mode 100644 index 00000000..35c41611 --- /dev/null +++ b/recovery_mitigations/easynav_clear_map_recovery/src/easynav_clear_map_recovery/ClearMapRecovery.cpp @@ -0,0 +1,51 @@ +// 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. + +/// \file +/// \brief Implementation of the ClearMapRecovery class. + +#include "easynav_clear_map_recovery/ClearMapRecovery.hpp" + +namespace easynav +{ + +bool ClearMapRecovery::can_handle(const diagnostic_msgs::msg::DiagnosticStatus & status) const +{ + return status.hardware_id == "planner" && + status.level >= diagnostic_msgs::msg::DiagnosticStatus::ERROR; +} + +void ClearMapRecovery::on_start(NavState & nav_state) +{ + RCLCPP_WARN( + get_node()->get_logger(), "ClearMapRecovery [%s]: requesting a map reset", + get_plugin_name().c_str()); + + nav_state.set("maps_manager_reset_requested", true); +} + +RecoveryStatus ClearMapRecovery::on_cycle(NavState & nav_state) +{ + // Same one-shot pattern as ForceReplanRecovery: SystemNode consumes and resets the flag + // right after cycling MapsManagerNode, one non-RT tick after on_start() set it. + const bool still_pending = nav_state.has("maps_manager_reset_requested") && + nav_state.get("maps_manager_reset_requested"); + + return still_pending ? RecoveryStatus::RUNNING : RecoveryStatus::SUCCEEDED; +} + +} // namespace easynav + +#include +PLUGINLIB_EXPORT_CLASS(easynav::ClearMapRecovery, easynav::RecoveryMitigationBase) diff --git a/recovery_mitigations/easynav_clear_map_recovery/tests/CMakeLists.txt b/recovery_mitigations/easynav_clear_map_recovery/tests/CMakeLists.txt new file mode 100644 index 00000000..5e7061e6 --- /dev/null +++ b/recovery_mitigations/easynav_clear_map_recovery/tests/CMakeLists.txt @@ -0,0 +1,2 @@ +ament_add_gtest(clear_map_recovery_tests clear_map_recovery_tests.cpp) +target_link_libraries(clear_map_recovery_tests ${PROJECT_NAME}) diff --git a/recovery_mitigations/easynav_clear_map_recovery/tests/clear_map_recovery_tests.cpp b/recovery_mitigations/easynav_clear_map_recovery/tests/clear_map_recovery_tests.cpp new file mode 100644 index 00000000..403f4af4 --- /dev/null +++ b/recovery_mitigations/easynav_clear_map_recovery/tests/clear_map_recovery_tests.cpp @@ -0,0 +1,80 @@ +// 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 "gtest/gtest.h" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "easynav_clear_map_recovery/ClearMapRecovery.hpp" + +class ClearMapRecoveryTestCase : public ::testing::Test +{ +protected: + void SetUp() override + { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + } + + std::shared_ptr make_recovery( + const std::shared_ptr & node, const std::string & name) + { + auto rec = std::make_shared(); + rec->initialize(node, name); + return rec; + } +}; + +TEST_F(ClearMapRecoveryTestCase, DoesNotRequireControl) +{ + auto node = std::make_shared("test_rc_node"); + auto rec = make_recovery(node, "clear0"); + EXPECT_FALSE(rec->requires_control()); +} + +TEST_F(ClearMapRecoveryTestCase, CanHandleOnlyPlannerErrors) +{ + auto node = std::make_shared("test_ch_node"); + auto rec = make_recovery(node, "clear1"); + + diagnostic_msgs::msg::DiagnosticStatus matching; + matching.hardware_id = "planner"; + matching.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + EXPECT_TRUE(rec->can_handle(matching)); + + diagnostic_msgs::msg::DiagnosticStatus wrong_hardware = matching; + wrong_hardware.hardware_id = "obstacle_proximity"; + EXPECT_FALSE(rec->can_handle(wrong_hardware)); +} + +TEST_F(ClearMapRecoveryTestCase, RunningUntilSystemNodeConsumesTheRequest) +{ + auto node = std::make_shared("test_run_node"); + auto rec = make_recovery(node, "clear2"); + easynav::NavState nav_state; + + rec->internal_start(nav_state); + + ASSERT_TRUE(nav_state.has("maps_manager_reset_requested")); + EXPECT_TRUE(nav_state.get("maps_manager_reset_requested")); + + EXPECT_EQ(rec->internal_cycle(nav_state), easynav::RecoveryStatus::RUNNING); + + // Simulate SystemNode::system_cycle() consuming the request. + nav_state.set("maps_manager_reset_requested", false); + + EXPECT_EQ(rec->internal_cycle(nav_state), easynav::RecoveryStatus::SUCCEEDED); +} diff --git a/recovery_mitigations/easynav_force_replan_recovery/CMakeLists.txt b/recovery_mitigations/easynav_force_replan_recovery/CMakeLists.txt new file mode 100644 index 00000000..a4fa9875 --- /dev/null +++ b/recovery_mitigations/easynav_force_replan_recovery/CMakeLists.txt @@ -0,0 +1,67 @@ +cmake_minimum_required(VERSION 3.20) +project(easynav_force_replan_recovery) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(easynav_common REQUIRED) +find_package(easynav_core REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(diagnostic_msgs REQUIRED) + +add_library(${PROJECT_NAME} SHARED + src/easynav_force_replan_recovery/ForceReplanRecovery.cpp +) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) +target_link_libraries(${PROJECT_NAME} PUBLIC + easynav_common::easynav_common + easynav_core::easynav_core + pluginlib::pluginlib + rclcpp::rclcpp + ${diagnostic_msgs_TARGETS} +) + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +install(TARGETS + ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + add_subdirectory(tests) +endif() + +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_libraries(${PROJECT_NAME}) +ament_export_targets(export_${PROJECT_NAME}) + +# Register the recovery mitigation plugin +pluginlib_export_plugin_description_file(easynav_core easynav_force_replan_recovery_plugins.xml) + +ament_export_dependencies( + easynav_common + easynav_core + pluginlib + rclcpp + diagnostic_msgs +) +ament_package() diff --git a/recovery_mitigations/easynav_force_replan_recovery/easynav_force_replan_recovery_plugins.xml b/recovery_mitigations/easynav_force_replan_recovery/easynav_force_replan_recovery_plugins.xml new file mode 100644 index 00000000..3e621a6e --- /dev/null +++ b/recovery_mitigations/easynav_force_replan_recovery/easynav_force_replan_recovery_plugins.xml @@ -0,0 +1,10 @@ + + + + + Forces an immediate replan (hardware_id "planner", level ERROR) via + PlannerMethodBase::force_update(). Does not take control_owner. + + + + diff --git a/recovery_mitigations/easynav_force_replan_recovery/include/easynav_force_replan_recovery/ForceReplanRecovery.hpp b/recovery_mitigations/easynav_force_replan_recovery/include/easynav_force_replan_recovery/ForceReplanRecovery.hpp new file mode 100644 index 00000000..024a75db --- /dev/null +++ b/recovery_mitigations/easynav_force_replan_recovery/include/easynav_force_replan_recovery/ForceReplanRecovery.hpp @@ -0,0 +1,61 @@ +// 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. + +/// \file +/// \brief Declaration of the ForceReplanRecovery plugin. + +#ifndef EASYNAV_FORCE_REPLAN_RECOVERY__FORCEREPLANRECOVERY_HPP_ +#define EASYNAV_FORCE_REPLAN_RECOVERY__FORCEREPLANRECOVERY_HPP_ + +#include "easynav_core/RecoveryMitigationBase.hpp" + +namespace easynav +{ + +/** + * @class ForceReplanRecovery + * @brief Level-1 mitigation: forces an immediate replan. See docs/recoveries_easynav.md §5.11. + * + * Selected for diagnostics with hardware_id == "planner" (the vocabulary shared with + * NoPathEvaluator) at level ERROR or above (an empty/missing path is not itself an error while + * the planner simply hasn't run yet — WARN — only an ERROR, i.e. the planner actually produced + * an empty path, warrants forcing a retry). + * + * Does not requires_control(): it never touches "cmd_vel" or "control_owner", only requests + * PlannerMethodBase::force_update() (already used for "a new goal arrived") via the one-shot + * NavState key "force_replan_requested", consumed by SystemNode::system_cycle() right before + * it cycles PlannerNode — see docs/recoveries_easynav_implementation.md, Fase 4. This is a + * request, not a guarantee: whether the new attempt actually produces a non-empty path is up + * to the planner plugin and the environment, not this mitigation, which always reports + * SUCCEEDED once the request has been handed off (one non-RT cycle later). If the diagnostic + * persists, RecoveryManagerNode's per-diagnostic attempt count (see RecoveryManagerNode.hpp) + * lets a later candidate (e.g. ClearMapRecovery) take over instead of retrying this one + * forever. + */ +class ForceReplanRecovery : public easynav::RecoveryMitigationBase +{ +public: + ForceReplanRecovery() = default; + ~ForceReplanRecovery() = default; + + bool can_handle(const diagnostic_msgs::msg::DiagnosticStatus & status) const override; + +protected: + void on_start(NavState & nav_state) override; + RecoveryStatus on_cycle(NavState & nav_state) override; +}; + +} // namespace easynav + +#endif // EASYNAV_FORCE_REPLAN_RECOVERY__FORCEREPLANRECOVERY_HPP_ diff --git a/recovery_mitigations/easynav_force_replan_recovery/package.xml b/recovery_mitigations/easynav_force_replan_recovery/package.xml new file mode 100644 index 00000000..f361037b --- /dev/null +++ b/recovery_mitigations/easynav_force_replan_recovery/package.xml @@ -0,0 +1,25 @@ + + + + easynav_force_replan_recovery + 0.4.2 + Easy Navigation: recovery mitigation that forces an immediate replan when the planner produces an empty path. + Francisco Martín Rico + Apache-2.0 + + ament_cmake + + easynav_common + easynav_core + pluginlib + rclcpp + diagnostic_msgs + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + + + ament_cmake + + diff --git a/recovery_mitigations/easynav_force_replan_recovery/src/easynav_force_replan_recovery/ForceReplanRecovery.cpp b/recovery_mitigations/easynav_force_replan_recovery/src/easynav_force_replan_recovery/ForceReplanRecovery.cpp new file mode 100644 index 00000000..f575557f --- /dev/null +++ b/recovery_mitigations/easynav_force_replan_recovery/src/easynav_force_replan_recovery/ForceReplanRecovery.cpp @@ -0,0 +1,52 @@ +// 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. + +/// \file +/// \brief Implementation of the ForceReplanRecovery class. + +#include "easynav_force_replan_recovery/ForceReplanRecovery.hpp" + +namespace easynav +{ + +bool ForceReplanRecovery::can_handle(const diagnostic_msgs::msg::DiagnosticStatus & status) const +{ + return status.hardware_id == "planner" && + status.level >= diagnostic_msgs::msg::DiagnosticStatus::ERROR; +} + +void ForceReplanRecovery::on_start(NavState & nav_state) +{ + RCLCPP_WARN( + get_node()->get_logger(), "ForceReplanRecovery [%s]: forcing an immediate replan", + get_plugin_name().c_str()); + + nav_state.set("force_replan_requested", true); +} + +RecoveryStatus ForceReplanRecovery::on_cycle(NavState & nav_state) +{ + // SystemNode consumes "force_replan_requested" (sets it back to false) in the same + // non-RT cycle it forces PlannerNode to replan; by the time cycle() reaches + // RecoveryManagerNode again (next non-RT tick), the request has already been handed off. + const bool still_pending = nav_state.has("force_replan_requested") && + nav_state.get("force_replan_requested"); + + return still_pending ? RecoveryStatus::RUNNING : RecoveryStatus::SUCCEEDED; +} + +} // namespace easynav + +#include +PLUGINLIB_EXPORT_CLASS(easynav::ForceReplanRecovery, easynav::RecoveryMitigationBase) diff --git a/recovery_mitigations/easynav_force_replan_recovery/tests/CMakeLists.txt b/recovery_mitigations/easynav_force_replan_recovery/tests/CMakeLists.txt new file mode 100644 index 00000000..015080a7 --- /dev/null +++ b/recovery_mitigations/easynav_force_replan_recovery/tests/CMakeLists.txt @@ -0,0 +1,2 @@ +ament_add_gtest(force_replan_recovery_tests force_replan_recovery_tests.cpp) +target_link_libraries(force_replan_recovery_tests ${PROJECT_NAME}) diff --git a/recovery_mitigations/easynav_force_replan_recovery/tests/force_replan_recovery_tests.cpp b/recovery_mitigations/easynav_force_replan_recovery/tests/force_replan_recovery_tests.cpp new file mode 100644 index 00000000..09d3e25b --- /dev/null +++ b/recovery_mitigations/easynav_force_replan_recovery/tests/force_replan_recovery_tests.cpp @@ -0,0 +1,94 @@ +// 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 "gtest/gtest.h" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "easynav_force_replan_recovery/ForceReplanRecovery.hpp" + +class ForceReplanRecoveryTestCase : public ::testing::Test +{ +protected: + void SetUp() override + { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + } + + std::shared_ptr make_recovery( + const std::shared_ptr & node, const std::string & name) + { + auto rec = std::make_shared(); + rec->initialize(node, name); + return rec; + } +}; + +TEST_F(ForceReplanRecoveryTestCase, DoesNotRequireControl) +{ + auto node = std::make_shared("test_rc_node"); + auto rec = make_recovery(node, "replan0"); + EXPECT_FALSE(rec->requires_control()); +} + +TEST_F(ForceReplanRecoveryTestCase, CanHandleOnlyPlannerErrors) +{ + auto node = std::make_shared("test_ch_node"); + auto rec = make_recovery(node, "replan1"); + + diagnostic_msgs::msg::DiagnosticStatus matching; + matching.hardware_id = "planner"; + matching.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + EXPECT_TRUE(rec->can_handle(matching)); + + diagnostic_msgs::msg::DiagnosticStatus wrong_hardware = matching; + wrong_hardware.hardware_id = "obstacle_proximity"; + EXPECT_FALSE(rec->can_handle(wrong_hardware)); + + diagnostic_msgs::msg::DiagnosticStatus just_warn = matching; + just_warn.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + EXPECT_FALSE(rec->can_handle(just_warn)); +} + +TEST_F(ForceReplanRecoveryTestCase, RunningUntilSystemNodeConsumesTheRequest) +{ + auto node = std::make_shared("test_run_node"); + auto rec = make_recovery(node, "replan2"); + easynav::NavState nav_state; + + rec->internal_start(nav_state); + + ASSERT_TRUE(nav_state.has("force_replan_requested")); + EXPECT_TRUE(nav_state.get("force_replan_requested")); + + // Still pending: SystemNode has not consumed it yet. + EXPECT_EQ(rec->internal_cycle(nav_state), easynav::RecoveryStatus::RUNNING); + + // Simulate SystemNode::system_cycle() consuming the request. + nav_state.set("force_replan_requested", false); + + EXPECT_EQ(rec->internal_cycle(nav_state), easynav::RecoveryStatus::SUCCEEDED); +} + +TEST_F(ForceReplanRecoveryTestCase, SucceedsImmediatelyIfNeverStarted) +{ + auto node = std::make_shared("test_never_node"); + auto rec = make_recovery(node, "replan3"); + easynav::NavState nav_state; // "force_replan_requested" absent + + EXPECT_EQ(rec->internal_cycle(nav_state), easynav::RecoveryStatus::SUCCEEDED); +} diff --git a/recovery_mitigations/easynav_notify_and_hold_recovery/CMakeLists.txt b/recovery_mitigations/easynav_notify_and_hold_recovery/CMakeLists.txt new file mode 100644 index 00000000..3859b2be --- /dev/null +++ b/recovery_mitigations/easynav_notify_and_hold_recovery/CMakeLists.txt @@ -0,0 +1,70 @@ +cmake_minimum_required(VERSION 3.20) +project(easynav_notify_and_hold_recovery) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(easynav_common REQUIRED) +find_package(easynav_core REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(diagnostic_msgs REQUIRED) + +add_library(${PROJECT_NAME} SHARED + src/easynav_notify_and_hold_recovery/NotifyAndHoldRecovery.cpp +) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) +target_link_libraries(${PROJECT_NAME} PUBLIC + easynav_common::easynav_common + easynav_core::easynav_core + pluginlib::pluginlib + rclcpp::rclcpp + ${nav_msgs_TARGETS} + ${diagnostic_msgs_TARGETS} +) + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +install(TARGETS + ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + add_subdirectory(tests) +endif() + +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_libraries(${PROJECT_NAME}) +ament_export_targets(export_${PROJECT_NAME}) + +# Register the recovery mitigation plugin +pluginlib_export_plugin_description_file(easynav_core easynav_notify_and_hold_recovery_plugins.xml) + +ament_export_dependencies( + easynav_common + easynav_core + pluginlib + rclcpp + nav_msgs + diagnostic_msgs +) +ament_package() diff --git a/recovery_mitigations/easynav_notify_and_hold_recovery/easynav_notify_and_hold_recovery_plugins.xml b/recovery_mitigations/easynav_notify_and_hold_recovery/easynav_notify_and_hold_recovery_plugins.xml new file mode 100644 index 00000000..742c208b --- /dev/null +++ b/recovery_mitigations/easynav_notify_and_hold_recovery/easynav_notify_and_hold_recovery_plugins.xml @@ -0,0 +1,12 @@ + + + + + Last-resort mitigation: stops the robot, reports the mission as failed/errored via + GoalManager, and holds (takes control_owner) until a new goal is accepted. Unconditional + can_handle() for any ERROR-or-above diagnostic -- meant to be listed last in + mitigation_types. + + + + diff --git a/recovery_mitigations/easynav_notify_and_hold_recovery/include/easynav_notify_and_hold_recovery/NotifyAndHoldRecovery.hpp b/recovery_mitigations/easynav_notify_and_hold_recovery/include/easynav_notify_and_hold_recovery/NotifyAndHoldRecovery.hpp new file mode 100644 index 00000000..0f01c952 --- /dev/null +++ b/recovery_mitigations/easynav_notify_and_hold_recovery/include/easynav_notify_and_hold_recovery/NotifyAndHoldRecovery.hpp @@ -0,0 +1,72 @@ +// 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. + +/// \file +/// \brief Declaration of the NotifyAndHoldRecovery plugin. + +#ifndef EASYNAV_NOTIFY_AND_HOLD_RECOVERY__NOTIFYANDHOLDRECOVERY_HPP_ +#define EASYNAV_NOTIFY_AND_HOLD_RECOVERY__NOTIFYANDHOLDRECOVERY_HPP_ + +#include "easynav_core/RecoveryMitigationBase.hpp" + +namespace easynav +{ + +/** + * @class NotifyAndHoldRecovery + * @brief Level-1, last-resort mitigation: stops the robot and reports the mission as + * failed/errored via GoalManager, then holds until a new goal is accepted. See + * docs/recoveries_easynav.md §5.8 and §5.11. + * + * Unconditional can_handle() (true for any diagnostic at level ERROR or above): this is meant + * to be the last entry in "mitigation_types", so RecoveryManagerNode's selection (first + * applicable candidate in list order, see RecoveryManagerNode.hpp) only ever reaches it for a + * diagnostic no earlier-listed, more specific mitigation declared can_handle() for. + * + * requires_control() == true, unlike the rest of the Fase-4 catalog (see + * docs/recoveries_easynav_implementation.md for why this was a necessary correction over the + * design table, which did not mark it "(toma control_owner)"): stopping the robot with a single + * zero-velocity write is not enough on its own, because nothing would then stop the nominal + * controller from overwriting "cmd_vel" again on its very next RT cycle while still chasing the + * (now abandoned) path. Taking control_owner and re-affirming zero velocity every RT cycle + * (on_cycle() returns RecoveryStatus::RUNNING) is what actually makes the "hold" in the name + * true. It releases control_owner (returns SUCCEEDED) only once GoalManager reports a new, + * non-empty goal list (checked via NavState's "goals" key, already kept in sync by + * GoalManager::update() — no dependency on easynav_system needed for that), i.e. once someone + * has sent the robot a fresh goal after the failure. + */ +class NotifyAndHoldRecovery : public easynav::RecoveryMitigationBase +{ +public: + NotifyAndHoldRecovery() = default; + ~NotifyAndHoldRecovery() = 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; + RecoveryStatus on_cycle(NavState & nav_state) override; + +private: + /// @brief Whether to report the mission via GoalManager::set_failed() ("failed", the + /// default) or GoalManager::set_error() ("error"). Parameter ".report_as". + std::string report_as_ {"failed"}; +}; + +} // namespace easynav + +#endif // EASYNAV_NOTIFY_AND_HOLD_RECOVERY__NOTIFYANDHOLDRECOVERY_HPP_ diff --git a/recovery_mitigations/easynav_notify_and_hold_recovery/package.xml b/recovery_mitigations/easynav_notify_and_hold_recovery/package.xml new file mode 100644 index 00000000..60c5df73 --- /dev/null +++ b/recovery_mitigations/easynav_notify_and_hold_recovery/package.xml @@ -0,0 +1,26 @@ + + + + easynav_notify_and_hold_recovery + 0.4.2 + Easy Navigation: last-resort recovery mitigation that stops the robot, reports mission failure, and holds. + Francisco Martín Rico + Apache-2.0 + + ament_cmake + + easynav_common + easynav_core + pluginlib + rclcpp + nav_msgs + diagnostic_msgs + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + + + ament_cmake + + diff --git a/recovery_mitigations/easynav_notify_and_hold_recovery/src/easynav_notify_and_hold_recovery/NotifyAndHoldRecovery.cpp b/recovery_mitigations/easynav_notify_and_hold_recovery/src/easynav_notify_and_hold_recovery/NotifyAndHoldRecovery.cpp new file mode 100644 index 00000000..6152df91 --- /dev/null +++ b/recovery_mitigations/easynav_notify_and_hold_recovery/src/easynav_notify_and_hold_recovery/NotifyAndHoldRecovery.cpp @@ -0,0 +1,93 @@ +// 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. + +/// \file +/// \brief Implementation of the NotifyAndHoldRecovery class. + +#include "nav_msgs/msg/goals.hpp" + +#include "easynav_notify_and_hold_recovery/NotifyAndHoldRecovery.hpp" + +namespace easynav +{ + +void NotifyAndHoldRecovery::on_initialize() +{ + auto node = get_node(); + const auto & plugin_name = get_plugin_name(); + + node->declare_parameter(plugin_name + ".report_as", report_as_); + node->get_parameter(plugin_name + ".report_as", report_as_); + + if (report_as_ != "failed" && report_as_ != "error") { + RCLCPP_WARN( + node->get_logger(), + "NotifyAndHoldRecovery [%s]: invalid report_as '%s' (must be 'failed' or 'error'), " + "defaulting to 'failed'", plugin_name.c_str(), report_as_.c_str()); + report_as_ = "failed"; + } +} + +bool NotifyAndHoldRecovery::can_handle(const diagnostic_msgs::msg::DiagnosticStatus & status) const +{ + // Unconditional catch-all: see the class doc comment. This is meant to be the last entry in + // "mitigation_types", so it is only ever reached for a diagnostic nothing earlier-listed + // could handle. + return status.level >= diagnostic_msgs::msg::DiagnosticStatus::ERROR; +} + +void NotifyAndHoldRecovery::on_start(NavState & nav_state) +{ + std::string reason = + "Recovery escalated to mission level: no autonomous mitigation resolved the situation"; + for (const auto & key : nav_state.get_group_keys("diagnostics")) { + if (!nav_state.has(key)) { + continue; + } + const auto & status = nav_state.get(key); + if (status.level >= diagnostic_msgs::msg::DiagnosticStatus::ERROR) { + reason = "[" + status.name + "] (" + status.hardware_id + "): " + status.message; + break; + } + } + + RCLCPP_ERROR( + get_node()->get_logger(), + "NotifyAndHoldRecovery [%s]: %s -- reporting mission as '%s' and holding", + get_plugin_name().c_str(), reason.c_str(), report_as_.c_str()); + + // Handed to SystemNode::system_cycle(), the only place with a real GoalManager instance to + // call set_failed()/set_error() on. See docs/recoveries_easynav_implementation.md, Fase 4. + nav_state.set("goal_manager_request", report_as_); + nav_state.set("goal_manager_reason", reason); +} + +RecoveryStatus NotifyAndHoldRecovery::on_cycle(NavState & nav_state) +{ + stop_robot(nav_state); + + // "goals" crosses the RT/non-RT boundary (written by GoalManager::update() on the non-RT + // thread), so use get_safe() here, in the RT thread. See NavState.hpp's own guidance. + const bool has_active_goal = nav_state.has("goals") && + !nav_state.get_safe("goals").goals.empty(); + + // Someone sent a new goal after the failure: release control_owner and let the nominal + // controller resume against it. Otherwise, keep holding (RUNNING) indefinitely. + return has_active_goal ? RecoveryStatus::SUCCEEDED : RecoveryStatus::RUNNING; +} + +} // namespace easynav + +#include +PLUGINLIB_EXPORT_CLASS(easynav::NotifyAndHoldRecovery, easynav::RecoveryMitigationBase) diff --git a/recovery_mitigations/easynav_notify_and_hold_recovery/tests/CMakeLists.txt b/recovery_mitigations/easynav_notify_and_hold_recovery/tests/CMakeLists.txt new file mode 100644 index 00000000..2f0254de --- /dev/null +++ b/recovery_mitigations/easynav_notify_and_hold_recovery/tests/CMakeLists.txt @@ -0,0 +1,2 @@ +ament_add_gtest(notify_and_hold_recovery_tests notify_and_hold_recovery_tests.cpp) +target_link_libraries(notify_and_hold_recovery_tests ${PROJECT_NAME}) diff --git a/recovery_mitigations/easynav_notify_and_hold_recovery/tests/notify_and_hold_recovery_tests.cpp b/recovery_mitigations/easynav_notify_and_hold_recovery/tests/notify_and_hold_recovery_tests.cpp new file mode 100644 index 00000000..14314c58 --- /dev/null +++ b/recovery_mitigations/easynav_notify_and_hold_recovery/tests/notify_and_hold_recovery_tests.cpp @@ -0,0 +1,133 @@ +// 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 "gtest/gtest.h" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav_msgs/msg/goals.hpp" + +#include "easynav_notify_and_hold_recovery/NotifyAndHoldRecovery.hpp" + +class NotifyAndHoldRecoveryTestCase : public ::testing::Test +{ +protected: + void SetUp() override + { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + } + + std::shared_ptr make_recovery( + const std::shared_ptr & node, const std::string & name) + { + auto rec = std::make_shared(); + rec->initialize(node, name); + return rec; + } +}; + +TEST_F(NotifyAndHoldRecoveryTestCase, RequiresControl) +{ + auto node = std::make_shared("test_rc_node"); + auto rec = make_recovery(node, "hold0"); + EXPECT_TRUE(rec->requires_control()); +} + +TEST_F(NotifyAndHoldRecoveryTestCase, CanHandleAnyErrorOrAboveDiagnostic) +{ + auto node = std::make_shared("test_ch_node"); + auto rec = make_recovery(node, "hold1"); + + diagnostic_msgs::msg::DiagnosticStatus error_status; + error_status.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + EXPECT_TRUE(rec->can_handle(error_status)); + + diagnostic_msgs::msg::DiagnosticStatus warn_status; + warn_status.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + EXPECT_FALSE(rec->can_handle(warn_status)); +} + +TEST_F(NotifyAndHoldRecoveryTestCase, OnStartDefaultsToReportingFailed) +{ + auto node = std::make_shared("test_default_node"); + auto rec = make_recovery(node, "hold2"); + + easynav::NavState nav_state; + rec->internal_start(nav_state); + + ASSERT_TRUE(nav_state.has("goal_manager_request")); + EXPECT_EQ(nav_state.get("goal_manager_request"), "failed"); + ASSERT_TRUE(nav_state.has("goal_manager_reason")); + EXPECT_FALSE(nav_state.get("goal_manager_reason").empty()); +} + +TEST_F(NotifyAndHoldRecoveryTestCase, ReportAsErrorParameterIsHonored) +{ + auto node = std::make_shared( + "test_error_node", + rclcpp::NodeOptions().parameter_overrides({rclcpp::Parameter("hold3.report_as", "error")})); + auto rec = make_recovery(node, "hold3"); + + easynav::NavState nav_state; + rec->internal_start(nav_state); + + EXPECT_EQ(nav_state.get("goal_manager_request"), "error"); +} + +TEST_F(NotifyAndHoldRecoveryTestCase, InvalidReportAsFallsBackToFailed) +{ + auto node = std::make_shared( + "test_invalid_node", + rclcpp::NodeOptions().parameter_overrides({rclcpp::Parameter("hold4.report_as", "bogus")})); + auto rec = make_recovery(node, "hold4"); + + easynav::NavState nav_state; + rec->internal_start(nav_state); + + EXPECT_EQ(nav_state.get("goal_manager_request"), "failed"); +} + +TEST_F(NotifyAndHoldRecoveryTestCase, HoldsAndStopsWhileNoActiveGoal) +{ + auto node = std::make_shared("test_hold_node"); + auto rec = make_recovery(node, "hold5"); + + easynav::NavState nav_state; // "goals" absent -> treated as no active goal + + auto status = rec->internal_cycle(nav_state); + + EXPECT_EQ(status, easynav::RecoveryStatus::RUNNING); + ASSERT_TRUE(nav_state.has("cmd_vel")); + const auto & cmd = nav_state.get("cmd_vel"); + EXPECT_DOUBLE_EQ(cmd.twist.linear.x, 0.0); + EXPECT_DOUBLE_EQ(cmd.twist.angular.z, 0.0); +} + +TEST_F(NotifyAndHoldRecoveryTestCase, ReleasesControlOnceANewGoalIsActive) +{ + auto node = std::make_shared("test_release_node"); + auto rec = make_recovery(node, "hold6"); + + easynav::NavState nav_state; + nav_msgs::msg::Goals goals; + goals.goals.push_back(geometry_msgs::msg::PoseStamped()); + nav_state.set("goals", goals); + + EXPECT_EQ(rec->internal_cycle(nav_state), easynav::RecoveryStatus::SUCCEEDED); +} diff --git a/recovery_mitigations/easynav_safe_retreat_recovery/CMakeLists.txt b/recovery_mitigations/easynav_safe_retreat_recovery/CMakeLists.txt new file mode 100644 index 00000000..49e613fe --- /dev/null +++ b/recovery_mitigations/easynav_safe_retreat_recovery/CMakeLists.txt @@ -0,0 +1,70 @@ +cmake_minimum_required(VERSION 3.20) +project(easynav_safe_retreat_recovery) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(easynav_common REQUIRED) +find_package(easynav_core REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(diagnostic_msgs REQUIRED) + +add_library(${PROJECT_NAME} SHARED + src/easynav_safe_retreat_recovery/SafeRetreatRecovery.cpp +) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) +target_link_libraries(${PROJECT_NAME} PUBLIC + easynav_common::easynav_common + easynav_core::easynav_core + pluginlib::pluginlib + rclcpp::rclcpp + ${geometry_msgs_TARGETS} + ${diagnostic_msgs_TARGETS} +) + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +install(TARGETS + ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + add_subdirectory(tests) +endif() + +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_libraries(${PROJECT_NAME}) +ament_export_targets(export_${PROJECT_NAME}) + +# Register the recovery mitigation plugin +pluginlib_export_plugin_description_file(easynav_core easynav_safe_retreat_recovery_plugins.xml) + +ament_export_dependencies( + easynav_common + easynav_core + pluginlib + rclcpp + geometry_msgs + diagnostic_msgs +) +ament_package() diff --git a/recovery_mitigations/easynav_safe_retreat_recovery/easynav_safe_retreat_recovery_plugins.xml b/recovery_mitigations/easynav_safe_retreat_recovery/easynav_safe_retreat_recovery_plugins.xml new file mode 100644 index 00000000..4b581977 --- /dev/null +++ b/recovery_mitigations/easynav_safe_retreat_recovery/easynav_safe_retreat_recovery_plugins.xml @@ -0,0 +1,11 @@ + + + + + Retreats straight back from a too-close obstacle (hardware_id "obstacle_proximity") until + reaching a safe distance. Fails safely if the obstacle is behind the robot instead of + ahead of it. + + + + diff --git a/recovery_mitigations/easynav_safe_retreat_recovery/include/easynav_safe_retreat_recovery/SafeRetreatRecovery.hpp b/recovery_mitigations/easynav_safe_retreat_recovery/include/easynav_safe_retreat_recovery/SafeRetreatRecovery.hpp new file mode 100644 index 00000000..c4abe1d1 --- /dev/null +++ b/recovery_mitigations/easynav_safe_retreat_recovery/include/easynav_safe_retreat_recovery/SafeRetreatRecovery.hpp @@ -0,0 +1,72 @@ +// 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. + +/// \file +/// \brief Declaration of the SafeRetreatRecovery plugin. + +#ifndef EASYNAV_SAFE_RETREAT_RECOVERY__SAFERETREATRECOVERY_HPP_ +#define EASYNAV_SAFE_RETREAT_RECOVERY__SAFERETREATRECOVERY_HPP_ + +#include "easynav_core/RecoveryMitigationBase.hpp" + +namespace easynav +{ + +/** + * @class SafeRetreatRecovery + * @brief Level-1 movement mitigation: retreats straight back from a too-close obstacle. + * + * See docs/recoveries_easynav.md §5.11/§5.13. Selected for diagnostics with + * hardware_id == "obstacle_proximity" (the vocabulary shared with ObstacleTooCloseEvaluator, + * matched by string, not by a compile-time dependency between the two plugins). Takes control + * of "cmd_vel" (requires_control() == true) and commands a slow, straight-backward motion each + * RT cycle, re-checking the nearest-obstacle distance every cycle until it exceeds this + * mitigation's own safe_distance parameter. + * + * Deliberate simplification versus the full design ambition (see + * docs/recoveries_easynav_implementation.md): this only retreats straight back, which is safe + * and correct when the obstacle is roughly ahead of the robot (the scenario this was designed + * for — an obstacle appearing in the direction of travel), and matches Nav2's own BackUp + * behaviour (also reverse-only) as well as the differential-drive robots this workspace's own + * configs target (a diff-drive base cannot strafe toward an arbitrary escape direction anyway). + * If the nearest obstacle is behind the robot instead, reversing would drive toward it rather + * than away, so on_cycle() recognizes that case and fails safely (stops, returns FAILED) + * instead of blindly reversing. + */ +class SafeRetreatRecovery : public easynav::RecoveryMitigationBase +{ +public: + SafeRetreatRecovery() = default; + ~SafeRetreatRecovery() = 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; + RecoveryStatus on_cycle(NavState & nav_state) override; + +private: + /// @brief Backward linear speed commanded while retreating (m/s, positive magnitude). + double retreat_speed_ {0.15}; + + /// @brief Distance (m) at which the retreat is considered complete. + double safe_distance_ {0.6}; +}; + +} // namespace easynav + +#endif // EASYNAV_SAFE_RETREAT_RECOVERY__SAFERETREATRECOVERY_HPP_ diff --git a/recovery_mitigations/easynav_safe_retreat_recovery/package.xml b/recovery_mitigations/easynav_safe_retreat_recovery/package.xml new file mode 100644 index 00000000..e58bf87e --- /dev/null +++ b/recovery_mitigations/easynav_safe_retreat_recovery/package.xml @@ -0,0 +1,26 @@ + + + + easynav_safe_retreat_recovery + 0.4.2 + Easy Navigation: recovery mitigation that retreats straight back from a too-close obstacle until reaching a safe distance. + Francisco Martín Rico + Apache-2.0 + + ament_cmake + + easynav_common + easynav_core + pluginlib + rclcpp + geometry_msgs + diagnostic_msgs + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + + + ament_cmake + + diff --git a/recovery_mitigations/easynav_safe_retreat_recovery/src/easynav_safe_retreat_recovery/SafeRetreatRecovery.cpp b/recovery_mitigations/easynav_safe_retreat_recovery/src/easynav_safe_retreat_recovery/SafeRetreatRecovery.cpp new file mode 100644 index 00000000..20b8910c --- /dev/null +++ b/recovery_mitigations/easynav_safe_retreat_recovery/src/easynav_safe_retreat_recovery/SafeRetreatRecovery.cpp @@ -0,0 +1,91 @@ +// 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. + +/// \file +/// \brief Implementation of the SafeRetreatRecovery class. + +#include + +#include "geometry_msgs/msg/twist_stamped.hpp" + +#include "easynav_common/RTTFBuffer.hpp" +#include "easynav_core/ObstacleProximity.hpp" + +#include "easynav_safe_retreat_recovery/SafeRetreatRecovery.hpp" + +namespace easynav +{ + +void SafeRetreatRecovery::on_initialize() +{ + auto node = get_node(); + const auto & plugin_name = get_plugin_name(); + + node->declare_parameter(plugin_name + ".retreat_speed", retreat_speed_); + node->declare_parameter(plugin_name + ".safe_distance", safe_distance_); + + node->get_parameter(plugin_name + ".retreat_speed", retreat_speed_); + node->get_parameter(plugin_name + ".safe_distance", safe_distance_); +} + +bool SafeRetreatRecovery::can_handle(const diagnostic_msgs::msg::DiagnosticStatus & status) const +{ + return status.hardware_id == "obstacle_proximity" && + status.level >= diagnostic_msgs::msg::DiagnosticStatus::ERROR; +} + +void SafeRetreatRecovery::on_start(NavState &) +{ + RCLCPP_WARN( + get_node()->get_logger(), "SafeRetreatRecovery [%s]: retreating from a too-close obstacle", + get_plugin_name().c_str()); +} + +RecoveryStatus SafeRetreatRecovery::on_cycle(NavState & nav_state) +{ + const auto obstacle = compute_nearest_obstacle(nav_state); + + if (!std::isfinite(obstacle.distance) || obstacle.distance >= safe_distance_) { + // Nothing to retreat from (perception lost) or already far enough: done. + stop_robot(nav_state); + return RecoveryStatus::SUCCEEDED; + } + + if (std::abs(obstacle.bearing) > M_PI / 2.0) { + // The nearest obstacle is behind the robot: reversing would drive toward it, not away. + // Fail safely instead of guessing a direction. See the class doc comment. + RCLCPP_ERROR( + get_node()->get_logger(), + "SafeRetreatRecovery [%s]: nearest obstacle is behind the robot (bearing=%.2f rad), " + "cannot safely retreat straight back", + get_plugin_name().c_str(), obstacle.bearing); + stop_robot(nav_state); + return RecoveryStatus::FAILED; + } + + geometry_msgs::msg::TwistStamped cmd; + if (auto node = get_node()) { + cmd.header.stamp = node->now(); + } + cmd.header.frame_id = RTTFBuffer::getInstance()->get_tf_info().robot_frame; + cmd.twist.linear.x = -retreat_speed_; + + nav_state.set("cmd_vel", cmd); + return RecoveryStatus::RUNNING; +} + +} // namespace easynav + +#include +PLUGINLIB_EXPORT_CLASS(easynav::SafeRetreatRecovery, easynav::RecoveryMitigationBase) diff --git a/recovery_mitigations/easynav_safe_retreat_recovery/tests/CMakeLists.txt b/recovery_mitigations/easynav_safe_retreat_recovery/tests/CMakeLists.txt new file mode 100644 index 00000000..0f524d4c --- /dev/null +++ b/recovery_mitigations/easynav_safe_retreat_recovery/tests/CMakeLists.txt @@ -0,0 +1,2 @@ +ament_add_gtest(safe_retreat_recovery_tests safe_retreat_recovery_tests.cpp) +target_link_libraries(safe_retreat_recovery_tests ${PROJECT_NAME}) diff --git a/recovery_mitigations/easynav_safe_retreat_recovery/tests/safe_retreat_recovery_tests.cpp b/recovery_mitigations/easynav_safe_retreat_recovery/tests/safe_retreat_recovery_tests.cpp new file mode 100644 index 00000000..81f6f382 --- /dev/null +++ b/recovery_mitigations/easynav_safe_retreat_recovery/tests/safe_retreat_recovery_tests.cpp @@ -0,0 +1,145 @@ +// 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 "gtest/gtest.h" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "easynav_common/RTTFBuffer.hpp" +#include "easynav_sensors/types/PointPerception.hpp" + +#include "easynav_safe_retreat_recovery/SafeRetreatRecovery.hpp" + +class SafeRetreatRecoveryTestCase : 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); + } + + static easynav::PointPerception make_obstacle_at(double x, double y) + { + easynav::PointPerception perception; + perception.frame_id = "base_link"; + perception.stamp = rclcpp::Time(0); + perception.valid = true; + perception.data.points.resize(1); + perception.data.points[0].x = x; + perception.data.points[0].y = y; + perception.data.points[0].z = 0.0; + return perception; + } + + std::shared_ptr make_recovery( + const std::shared_ptr & node, const std::string & name) + { + auto rec = std::make_shared(); + rec->initialize(node, name); + return rec; + } +}; + +TEST_F(SafeRetreatRecoveryTestCase, RequiresControl) +{ + auto node = std::make_shared("test_rc_node"); + auto rec = make_recovery(node, "retreat0"); + EXPECT_TRUE(rec->requires_control()); +} + +TEST_F(SafeRetreatRecoveryTestCase, CanHandleOnlyObstacleProximityErrors) +{ + auto node = std::make_shared("test_ch_node"); + auto rec = make_recovery(node, "retreat1"); + + diagnostic_msgs::msg::DiagnosticStatus matching; + matching.hardware_id = "obstacle_proximity"; + matching.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + EXPECT_TRUE(rec->can_handle(matching)); + + diagnostic_msgs::msg::DiagnosticStatus wrong_hardware = matching; + wrong_hardware.hardware_id = "planner"; + EXPECT_FALSE(rec->can_handle(wrong_hardware)); + + diagnostic_msgs::msg::DiagnosticStatus not_an_error = matching; + not_an_error.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + EXPECT_FALSE(rec->can_handle(not_an_error)); +} + +TEST_F(SafeRetreatRecoveryTestCase, RetreatsBackwardWhileObstacleAheadAndClose) +{ + auto node = std::make_shared("test_retreat_node"); + auto rec = make_recovery(node, "retreat2"); + + easynav::NavState nav_state; + nav_state.set("scan", make_obstacle_at(0.2, 0.0)); // ahead, well within default safe_distance + + auto status = rec->internal_cycle(nav_state); + + EXPECT_EQ(status, easynav::RecoveryStatus::RUNNING); + ASSERT_TRUE(nav_state.has("cmd_vel")); + const auto & cmd = nav_state.get("cmd_vel"); + EXPECT_LT(cmd.twist.linear.x, 0.0); +} + +TEST_F(SafeRetreatRecoveryTestCase, SucceedsOnceFarEnough) +{ + auto node = std::make_shared("test_far_node"); + auto rec = make_recovery(node, "retreat3"); + + easynav::NavState nav_state; + nav_state.set("scan", make_obstacle_at(5.0, 0.0)); // far away + + auto status = rec->internal_cycle(nav_state); + + EXPECT_EQ(status, easynav::RecoveryStatus::SUCCEEDED); + ASSERT_TRUE(nav_state.has("cmd_vel")); + const auto & cmd = nav_state.get("cmd_vel"); + EXPECT_DOUBLE_EQ(cmd.twist.linear.x, 0.0); +} + +TEST_F(SafeRetreatRecoveryTestCase, SucceedsWhenNoObstaclePerceptionAtAll) +{ + auto node = std::make_shared("test_none_node"); + auto rec = make_recovery(node, "retreat4"); + + easynav::NavState nav_state; // no perception at all + + auto status = rec->internal_cycle(nav_state); + + EXPECT_EQ(status, easynav::RecoveryStatus::SUCCEEDED); +} + +TEST_F(SafeRetreatRecoveryTestCase, FailsSafelyWhenObstacleIsBehind) +{ + auto node = std::make_shared("test_behind_node"); + auto rec = make_recovery(node, "retreat5"); + + easynav::NavState nav_state; + nav_state.set("scan", make_obstacle_at(-0.2, 0.0)); // directly behind, close + + auto status = rec->internal_cycle(nav_state); + + EXPECT_EQ(status, easynav::RecoveryStatus::FAILED); + ASSERT_TRUE(nav_state.has("cmd_vel")); + const auto & cmd = nav_state.get("cmd_vel"); + EXPECT_DOUBLE_EQ(cmd.twist.linear.x, 0.0); +} diff --git a/recovery_mitigations/easynav_safe_waypoint_recovery/CMakeLists.txt b/recovery_mitigations/easynav_safe_waypoint_recovery/CMakeLists.txt new file mode 100644 index 00000000..98b1c092 --- /dev/null +++ b/recovery_mitigations/easynav_safe_waypoint_recovery/CMakeLists.txt @@ -0,0 +1,70 @@ +cmake_minimum_required(VERSION 3.20) +project(easynav_safe_waypoint_recovery) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(easynav_common REQUIRED) +find_package(easynav_core REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(diagnostic_msgs REQUIRED) + +add_library(${PROJECT_NAME} SHARED + src/easynav_safe_waypoint_recovery/SafeWaypointRecovery.cpp +) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) +target_link_libraries(${PROJECT_NAME} PUBLIC + easynav_common::easynav_common + easynav_core::easynav_core + pluginlib::pluginlib + rclcpp::rclcpp + ${geometry_msgs_TARGETS} + ${diagnostic_msgs_TARGETS} +) + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +install(TARGETS + ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + add_subdirectory(tests) +endif() + +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_libraries(${PROJECT_NAME}) +ament_export_targets(export_${PROJECT_NAME}) + +# Register the recovery mitigation plugin +pluginlib_export_plugin_description_file(easynav_core easynav_safe_waypoint_recovery_plugins.xml) + +ament_export_dependencies( + easynav_common + easynav_core + pluginlib + rclcpp + geometry_msgs + diagnostic_msgs +) +ament_package() diff --git a/recovery_mitigations/easynav_safe_waypoint_recovery/easynav_safe_waypoint_recovery_plugins.xml b/recovery_mitigations/easynav_safe_waypoint_recovery/easynav_safe_waypoint_recovery_plugins.xml new file mode 100644 index 00000000..fb46ac41 --- /dev/null +++ b/recovery_mitigations/easynav_safe_waypoint_recovery/easynav_safe_waypoint_recovery_plugins.xml @@ -0,0 +1,11 @@ + + + + + Redirects the mission to a preconfigured safe waypoint instead of the current goal. + Opt-in: only can_handle() (any ERROR-or-above diagnostic) when "<instance>.enabled" + is true. Does not take control_owner. + + + + diff --git a/recovery_mitigations/easynav_safe_waypoint_recovery/include/easynav_safe_waypoint_recovery/SafeWaypointRecovery.hpp b/recovery_mitigations/easynav_safe_waypoint_recovery/include/easynav_safe_waypoint_recovery/SafeWaypointRecovery.hpp new file mode 100644 index 00000000..dfbca472 --- /dev/null +++ b/recovery_mitigations/easynav_safe_waypoint_recovery/include/easynav_safe_waypoint_recovery/SafeWaypointRecovery.hpp @@ -0,0 +1,90 @@ +// 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. + +/// \file +/// \brief Declaration of the SafeWaypointRecovery plugin. + +#ifndef EASYNAV_SAFE_WAYPOINT_RECOVERY__SAFEWAYPOINTRECOVERY_HPP_ +#define EASYNAV_SAFE_WAYPOINT_RECOVERY__SAFEWAYPOINTRECOVERY_HPP_ + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "rclcpp/publisher.hpp" + +#include "easynav_core/RecoveryMitigationBase.hpp" + +namespace easynav +{ + +/** + * @class SafeWaypointRecovery + * @brief Level-1, mission-scope mitigation: redirects the mission to a preconfigured safe + * waypoint instead of the goal currently being pursued. See docs/recoveries_easynav.md §5.8 + * (Escenario 5 of TOMASys/SysSelf) and §5.11. + * + * Deliberately opt-in: can_handle() only ever returns true if a safe waypoint has actually + * been configured (parameter ".enabled", default false) — see the "Decisión de + * implementación" note in docs/recoveries_easynav_implementation.md, Fase 4, for why: with no + * configured alternative there would be nothing useful to do, and letting can_handle() return + * true anyway would either loop retrying a no-op or (worse) require NotifyAndHoldRecovery to + * be listed before this one to ever get a turn, inverting the intended escalation order. This + * way an unconfigured instance is simply skipped by RecoveryManagerNode's selection, falling + * through to whatever is listed after it (typically NotifyAndHoldRecovery). + * + * Deviates from the literal design text in one respect: recoveries_easynav.md §5.8 says to + * "use GoalManagerClient" to send the alternate goal. GoalManagerClient's constructor takes an + * rclcpp::Node::SharedPtr, and its accept/preempt protocol runs across several non-RT cycles — + * both awkward for a plugin living inside RecoveryManagerNode (an rclcpp_lifecycle::LifecycleNode) + * that just wants to hand off one pose. Instead this publishes a single PoseStamped on the + * "goal_pose" topic — the exact same "external, single-pose goal injection" channel GoalManager + * already exposes for RViz/GUI use (GoalManager::comanded_pose_callback()), which already + * handles preempting whatever goal is currently active. See + * docs/recoveries_easynav_implementation.md for the full rationale. + * + * Does not requires_control(): PlannerNode/ControllerNode keep running normally against the + * new goal once GoalManager accepts it, same as any other goal change. + */ +class SafeWaypointRecovery : public easynav::RecoveryMitigationBase +{ +public: + SafeWaypointRecovery() = default; + ~SafeWaypointRecovery() = default; + + void on_initialize() override; + + bool can_handle(const diagnostic_msgs::msg::DiagnosticStatus & status) const override; + +protected: + void on_start(NavState & nav_state) override; + RecoveryStatus on_cycle(NavState & nav_state) override; + +private: + /// @brief Opt-in flag: this mitigation only ever can_handle() when true. + bool enabled_ {false}; + + /// @brief Safe waypoint pose, in \ref frame_. + double x_ {0.0}; + double y_ {0.0}; + double yaw_ {0.0}; + + /// @brief Frame the safe waypoint is expressed in. Defaults to the workspace's configured + /// map frame (read from RTTFBuffer) if left empty. + std::string frame_; + + /// @brief Publisher for the single-pose goal injection channel ("goal_pose"). + rclcpp::Publisher::SharedPtr goal_pose_pub_; +}; + +} // namespace easynav + +#endif // EASYNAV_SAFE_WAYPOINT_RECOVERY__SAFEWAYPOINTRECOVERY_HPP_ diff --git a/recovery_mitigations/easynav_safe_waypoint_recovery/package.xml b/recovery_mitigations/easynav_safe_waypoint_recovery/package.xml new file mode 100644 index 00000000..2bec5aa0 --- /dev/null +++ b/recovery_mitigations/easynav_safe_waypoint_recovery/package.xml @@ -0,0 +1,26 @@ + + + + easynav_safe_waypoint_recovery + 0.4.2 + Easy Navigation: recovery mitigation that redirects the mission to a preconfigured safe waypoint. + Francisco Martín Rico + Apache-2.0 + + ament_cmake + + easynav_common + easynav_core + pluginlib + rclcpp + geometry_msgs + diagnostic_msgs + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + + + ament_cmake + + diff --git a/recovery_mitigations/easynav_safe_waypoint_recovery/src/easynav_safe_waypoint_recovery/SafeWaypointRecovery.cpp b/recovery_mitigations/easynav_safe_waypoint_recovery/src/easynav_safe_waypoint_recovery/SafeWaypointRecovery.cpp new file mode 100644 index 00000000..983122f6 --- /dev/null +++ b/recovery_mitigations/easynav_safe_waypoint_recovery/src/easynav_safe_waypoint_recovery/SafeWaypointRecovery.cpp @@ -0,0 +1,91 @@ +// 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. + +/// \file +/// \brief Implementation of the SafeWaypointRecovery class. + +#include + +#include "easynav_common/RTTFBuffer.hpp" + +#include "easynav_safe_waypoint_recovery/SafeWaypointRecovery.hpp" + +namespace easynav +{ + +void SafeWaypointRecovery::on_initialize() +{ + auto node = get_node(); + const auto & plugin_name = get_plugin_name(); + + node->declare_parameter(plugin_name + ".enabled", enabled_); + node->declare_parameter(plugin_name + ".safe_waypoint.x", x_); + node->declare_parameter(plugin_name + ".safe_waypoint.y", y_); + node->declare_parameter(plugin_name + ".safe_waypoint.yaw", yaw_); + node->declare_parameter(plugin_name + ".safe_waypoint.frame", frame_); + + node->get_parameter(plugin_name + ".enabled", enabled_); + node->get_parameter(plugin_name + ".safe_waypoint.x", x_); + node->get_parameter(plugin_name + ".safe_waypoint.y", y_); + node->get_parameter(plugin_name + ".safe_waypoint.yaw", yaw_); + node->get_parameter(plugin_name + ".safe_waypoint.frame", frame_); + + if (frame_.empty()) { + frame_ = RTTFBuffer::getInstance()->get_tf_info().map_frame; + } + + // Same topic GoalManager already subscribes to for RViz/GUI single-pose goals + // (GoalManager::comanded_pose_callback()) — reused here instead of GoalManagerClient, see + // the class doc comment. + goal_pose_pub_ = node->create_publisher("goal_pose", 10); +} + +bool SafeWaypointRecovery::can_handle(const diagnostic_msgs::msg::DiagnosticStatus & status) const +{ + return enabled_ && status.level >= diagnostic_msgs::msg::DiagnosticStatus::ERROR; +} + +void SafeWaypointRecovery::on_start([[maybe_unused]] NavState & nav_state) +{ + RCLCPP_WARN( + get_node()->get_logger(), + "SafeWaypointRecovery [%s]: redirecting the mission to the safe waypoint " + "(%.2f, %.2f, yaw=%.2f) in frame '%s'", + get_plugin_name().c_str(), x_, y_, yaw_, frame_.c_str()); + + geometry_msgs::msg::PoseStamped goal; + goal.header.stamp = get_node()->now(); + goal.header.frame_id = frame_; + goal.pose.position.x = x_; + goal.pose.position.y = y_; + // Planar (yaw-only) rotation: no need to pull in tf2 for this. + goal.pose.orientation.z = std::sin(yaw_ / 2.0); + goal.pose.orientation.w = std::cos(yaw_ / 2.0); + + goal_pose_pub_->publish(goal); +} + +RecoveryStatus SafeWaypointRecovery::on_cycle([[maybe_unused]] NavState & nav_state) +{ + // The handoff already happened in on_start(): publishing the safe waypoint is a one-shot, + // fire-and-forget action. Whether the robot actually reaches it is ordinary navigation from + // here on (PlannerNode/ControllerNode against the new goal), not something this mitigation + // supervises further. + return RecoveryStatus::SUCCEEDED; +} + +} // namespace easynav + +#include +PLUGINLIB_EXPORT_CLASS(easynav::SafeWaypointRecovery, easynav::RecoveryMitigationBase) diff --git a/recovery_mitigations/easynav_safe_waypoint_recovery/tests/CMakeLists.txt b/recovery_mitigations/easynav_safe_waypoint_recovery/tests/CMakeLists.txt new file mode 100644 index 00000000..51f066e7 --- /dev/null +++ b/recovery_mitigations/easynav_safe_waypoint_recovery/tests/CMakeLists.txt @@ -0,0 +1,2 @@ +ament_add_gtest(safe_waypoint_recovery_tests safe_waypoint_recovery_tests.cpp) +target_link_libraries(safe_waypoint_recovery_tests ${PROJECT_NAME}) diff --git a/recovery_mitigations/easynav_safe_waypoint_recovery/tests/safe_waypoint_recovery_tests.cpp b/recovery_mitigations/easynav_safe_waypoint_recovery/tests/safe_waypoint_recovery_tests.cpp new file mode 100644 index 00000000..8971285c --- /dev/null +++ b/recovery_mitigations/easynav_safe_waypoint_recovery/tests/safe_waypoint_recovery_tests.cpp @@ -0,0 +1,113 @@ +// 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 "gtest/gtest.h" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "easynav_safe_waypoint_recovery/SafeWaypointRecovery.hpp" + +using namespace std::chrono_literals; + +class SafeWaypointRecoveryTestCase : public ::testing::Test +{ +protected: + void SetUp() override + { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + } + + std::shared_ptr make_recovery( + const std::shared_ptr & node, const std::string & name) + { + auto rec = std::make_shared(); + rec->initialize(node, name); + return rec; + } +}; + +TEST_F(SafeWaypointRecoveryTestCase, DoesNotRequireControl) +{ + auto node = std::make_shared("test_rc_node"); + auto rec = make_recovery(node, "waypoint0"); + EXPECT_FALSE(rec->requires_control()); +} + +TEST_F(SafeWaypointRecoveryTestCase, DisabledByDefaultCannotHandleAnything) +{ + auto node = std::make_shared("test_disabled_node"); + auto rec = make_recovery(node, "waypoint1"); + + diagnostic_msgs::msg::DiagnosticStatus status; + status.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + EXPECT_FALSE(rec->can_handle(status)); +} + +TEST_F(SafeWaypointRecoveryTestCase, EnabledHandlesAnyErrorOrAboveDiagnostic) +{ + auto node = std::make_shared( + "test_enabled_node", + rclcpp::NodeOptions().parameter_overrides({rclcpp::Parameter("waypoint2.enabled", true)})); + auto rec = make_recovery(node, "waypoint2"); + + diagnostic_msgs::msg::DiagnosticStatus error_status; + error_status.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + EXPECT_TRUE(rec->can_handle(error_status)); + + diagnostic_msgs::msg::DiagnosticStatus warn_status; + warn_status.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + EXPECT_FALSE(rec->can_handle(warn_status)); +} + +TEST_F(SafeWaypointRecoveryTestCase, PublishesTheConfiguredWaypointOnStart) +{ + auto node = std::make_shared( + "test_publish_node", + rclcpp::NodeOptions().parameter_overrides( + { + rclcpp::Parameter("waypoint3.enabled", true), + rclcpp::Parameter("waypoint3.safe_waypoint.x", 1.5), + rclcpp::Parameter("waypoint3.safe_waypoint.y", -2.0), + rclcpp::Parameter("waypoint3.safe_waypoint.frame", "map"), + })); + auto rec = make_recovery(node, "waypoint3"); + + auto listener_node = std::make_shared("test_listener_node"); + geometry_msgs::msg::PoseStamped::SharedPtr received; + auto sub = listener_node->create_subscription( + "goal_pose", 10, + [&received](geometry_msgs::msg::PoseStamped::SharedPtr msg) {received = msg;}); + + rclcpp::executors::SingleThreadedExecutor exe; + exe.add_node(node->get_node_base_interface()); + exe.add_node(listener_node); + + easynav::NavState nav_state; + rec->internal_start(nav_state); + + auto start = listener_node->now(); + while (!received && listener_node->now() - start < 2s) { + exe.spin_some(); + } + + ASSERT_NE(received, nullptr); + EXPECT_EQ(received->header.frame_id, "map"); + EXPECT_DOUBLE_EQ(received->pose.position.x, 1.5); + EXPECT_DOUBLE_EQ(received->pose.position.y, -2.0); + + EXPECT_EQ(rec->internal_cycle(nav_state), easynav::RecoveryStatus::SUCCEEDED); +} From 9c77d1d36cd3bf56557c2ccaf3e2deec879696cd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sat, 12 Sep 2026 00:44:28 +0200 Subject: [PATCH 2/4] Basic cases with priorities MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../easynav_mpc_controller/MPCController.hpp | 1 + .../easynav_mpc_controller/MPCController.cpp | 5 +- .../RegulatedPurePursuitController.hpp | 7 + .../RegulatedPurePursuitController.cpp | 4 + .../easynav_vff_controller/VffController.hpp | 1 + .../easynav_costmap_localizer/CMakeLists.txt | 38 ++++- .../easynav_costmap_localizer_plugins.xml | 16 +++ .../AmclConvergenceEvaluator.hpp | 61 ++++++++ .../AmclRelocalizeMitigation.hpp | 73 ++++++++++ .../easynav_costmap_localizer/package.xml | 1 + .../AMCLLocalizer.cpp | 19 ++- .../AmclConvergenceEvaluator.cpp | 67 +++++++++ .../AmclRelocalizeMitigation.cpp | 93 ++++++++++++ .../tests/CMakeLists.txt | 10 ++ .../amcl_convergence_evaluator_tests.cpp | 90 ++++++++++++ .../amcl_relocalize_mitigation_tests.cpp | 133 ++++++++++++++++++ .../NoPathEvaluator.cpp | 12 +- .../tests/no_path_evaluator_tests.cpp | 70 +++++++-- .../ObstacleTooCloseEvaluator.hpp | 15 ++ .../ObstacleTooCloseEvaluator.cpp | 20 +++ .../obstacle_too_close_evaluator_tests.cpp | 89 +++++++++++- .../easynav_clear_map_recovery_plugins.xml | 10 -- .../ClearMapRecovery.hpp | 61 -------- .../easynav_clear_map_recovery/package.xml | 25 ---- .../ClearMapRecovery.cpp | 51 ------- .../tests/CMakeLists.txt | 2 - .../tests/clear_map_recovery_tests.cpp | 80 ----------- .../CMakeLists.txt | 67 --------- .../easynav_force_replan_recovery_plugins.xml | 10 -- .../ForceReplanRecovery.hpp | 61 -------- .../easynav_force_replan_recovery/package.xml | 25 ---- .../ForceReplanRecovery.cpp | 52 ------- .../tests/CMakeLists.txt | 2 - .../tests/force_replan_recovery_tests.cpp | 94 ------------- .../CMakeLists.txt | 6 +- ...ynav_human_assistance_recovery_plugins.xml | 11 ++ .../HumanAssistanceRecovery.hpp | 63 +++++++++ .../package.xml | 6 +- .../HumanAssistanceRecovery.cpp | 92 ++++++++++++ .../tests/CMakeLists.txt | 2 + .../tests/human_assistance_recovery_tests.cpp | 127 +++++++++++++++++ .../CMakeLists.txt | 70 --------- ...synav_notify_and_hold_recovery_plugins.xml | 12 -- .../NotifyAndHoldRecovery.hpp | 72 ---------- .../NotifyAndHoldRecovery.cpp | 93 ------------ .../tests/CMakeLists.txt | 2 - .../tests/notify_and_hold_recovery_tests.cpp | 133 ------------------ .../CMakeLists.txt | 70 --------- ...easynav_safe_waypoint_recovery_plugins.xml | 11 -- .../SafeWaypointRecovery.hpp | 90 ------------ .../package.xml | 26 ---- .../SafeWaypointRecovery.cpp | 91 ------------ .../tests/CMakeLists.txt | 2 - .../tests/safe_waypoint_recovery_tests.cpp | 113 --------------- 54 files changed, 1108 insertions(+), 1349 deletions(-) create mode 100644 localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AmclConvergenceEvaluator.hpp create mode 100644 localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AmclRelocalizeMitigation.hpp create mode 100644 localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AmclConvergenceEvaluator.cpp create mode 100644 localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AmclRelocalizeMitigation.cpp create mode 100644 localizers/easynav_costmap_localizer/tests/amcl_convergence_evaluator_tests.cpp create mode 100644 localizers/easynav_costmap_localizer/tests/amcl_relocalize_mitigation_tests.cpp delete mode 100644 recovery_mitigations/easynav_clear_map_recovery/easynav_clear_map_recovery_plugins.xml delete mode 100644 recovery_mitigations/easynav_clear_map_recovery/include/easynav_clear_map_recovery/ClearMapRecovery.hpp delete mode 100644 recovery_mitigations/easynav_clear_map_recovery/package.xml delete mode 100644 recovery_mitigations/easynav_clear_map_recovery/src/easynav_clear_map_recovery/ClearMapRecovery.cpp delete mode 100644 recovery_mitigations/easynav_clear_map_recovery/tests/CMakeLists.txt delete mode 100644 recovery_mitigations/easynav_clear_map_recovery/tests/clear_map_recovery_tests.cpp delete mode 100644 recovery_mitigations/easynav_force_replan_recovery/CMakeLists.txt delete mode 100644 recovery_mitigations/easynav_force_replan_recovery/easynav_force_replan_recovery_plugins.xml delete mode 100644 recovery_mitigations/easynav_force_replan_recovery/include/easynav_force_replan_recovery/ForceReplanRecovery.hpp delete mode 100644 recovery_mitigations/easynav_force_replan_recovery/package.xml delete mode 100644 recovery_mitigations/easynav_force_replan_recovery/src/easynav_force_replan_recovery/ForceReplanRecovery.cpp delete mode 100644 recovery_mitigations/easynav_force_replan_recovery/tests/CMakeLists.txt delete mode 100644 recovery_mitigations/easynav_force_replan_recovery/tests/force_replan_recovery_tests.cpp rename recovery_mitigations/{easynav_clear_map_recovery => easynav_human_assistance_recovery}/CMakeLists.txt (87%) create mode 100644 recovery_mitigations/easynav_human_assistance_recovery/easynav_human_assistance_recovery_plugins.xml create mode 100644 recovery_mitigations/easynav_human_assistance_recovery/include/easynav_human_assistance_recovery/HumanAssistanceRecovery.hpp rename recovery_mitigations/{easynav_notify_and_hold_recovery => easynav_human_assistance_recovery}/package.xml (77%) create mode 100644 recovery_mitigations/easynav_human_assistance_recovery/src/easynav_human_assistance_recovery/HumanAssistanceRecovery.cpp create mode 100644 recovery_mitigations/easynav_human_assistance_recovery/tests/CMakeLists.txt create mode 100644 recovery_mitigations/easynav_human_assistance_recovery/tests/human_assistance_recovery_tests.cpp delete mode 100644 recovery_mitigations/easynav_notify_and_hold_recovery/CMakeLists.txt delete mode 100644 recovery_mitigations/easynav_notify_and_hold_recovery/easynav_notify_and_hold_recovery_plugins.xml delete mode 100644 recovery_mitigations/easynav_notify_and_hold_recovery/include/easynav_notify_and_hold_recovery/NotifyAndHoldRecovery.hpp delete mode 100644 recovery_mitigations/easynav_notify_and_hold_recovery/src/easynav_notify_and_hold_recovery/NotifyAndHoldRecovery.cpp delete mode 100644 recovery_mitigations/easynav_notify_and_hold_recovery/tests/CMakeLists.txt delete mode 100644 recovery_mitigations/easynav_notify_and_hold_recovery/tests/notify_and_hold_recovery_tests.cpp delete mode 100644 recovery_mitigations/easynav_safe_waypoint_recovery/CMakeLists.txt delete mode 100644 recovery_mitigations/easynav_safe_waypoint_recovery/easynav_safe_waypoint_recovery_plugins.xml delete mode 100644 recovery_mitigations/easynav_safe_waypoint_recovery/include/easynav_safe_waypoint_recovery/SafeWaypointRecovery.hpp delete mode 100644 recovery_mitigations/easynav_safe_waypoint_recovery/package.xml delete mode 100644 recovery_mitigations/easynav_safe_waypoint_recovery/src/easynav_safe_waypoint_recovery/SafeWaypointRecovery.cpp delete mode 100644 recovery_mitigations/easynav_safe_waypoint_recovery/tests/CMakeLists.txt delete mode 100644 recovery_mitigations/easynav_safe_waypoint_recovery/tests/safe_waypoint_recovery_tests.cpp diff --git a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp index 40e8472d..033871e2 100644 --- a/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp +++ b/controllers/easynav_mpc_controller/include/easynav_mpc_controller/MPCController.hpp @@ -85,6 +85,7 @@ class MPCController : public ControllerMethodBase double last_w_{0.0}; ///< Last value for angular velocity before than collision bool collision_state_{false}; ///< Collision state flag double collision_factor_{0.618033}; ///< Collision avoidance for recalculate velocities + bool collision_checker_active_{false}; ///< Enables the in-loop obstacle constraint above // Fallback goal tolerances if GoalManager does not publish them double fallback_goal_pos_tol_{0.05}; ///< Default positional tolerance (meters). diff --git a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp index e2a32068..fe4c6f31 100644 --- a/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp +++ b/controllers/easynav_mpc_controller/src/easynav_mpc_controller/MPCController.cpp @@ -43,6 +43,8 @@ MPCController::on_initialize() node->declare_parameter(plugin_name + ".fallback_goal_pos_tol", fallback_goal_pos_tol_); node->declare_parameter(plugin_name + ".fallback_goal_yaw_tol", fallback_goal_yaw_tol_); + node->declare_parameter( + plugin_name + ".colision_checker.active", collision_checker_active_); node->get_parameter(plugin_name + ".horizon_steps", horizon_steps_); node->get_parameter(plugin_name + ".dt", dt_); @@ -53,6 +55,7 @@ MPCController::on_initialize() node->get_parameter(plugin_name + ".fallback_goal_pos_tol", fallback_goal_pos_tol_); node->get_parameter(plugin_name + ".fallback_goal_yaw_tol", fallback_goal_yaw_tol_); + node->get_parameter(plugin_name + ".colision_checker.active", collision_checker_active_); optimizer_ = std::make_unique(); @@ -282,7 +285,7 @@ MPCController::update_rt(NavState & nav_state) std::cerr << "Optimization Error: " << e.what() << std::endl; } - if (ControllerMethodBase::collision_checker_active_) { + if (collision_checker_active_) { collision_checker(¶ms, u); } diff --git a/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp b/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp index 607e2075..6094bc13 100644 --- a/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp +++ b/controllers/easynav_regulated_pp_controller/include/easynav_regulated_pp_controller/RegulatedPurePursuitController.hpp @@ -101,6 +101,13 @@ class RegulatedPurePursuitController : public ControllerMethodBase double obstacle_scaling_dist_{0.3}; ///< Distance below which obstacle regulation is triggered (m). double obstacle_scaling_gain_{1.0}; ///< Gain (<=1.0) applied when scaling down the velocity. + // Robot geometry used by computeMinObstacleDistance() (own copy: this is a distinct + // speed-regulation heuristic, unrelated to the level-0 CollisionSafetyReflex). + double robot_radius_{0.35}; ///< Robot radius used when measuring obstacle distance (m). + double safety_margin_{0.1}; ///< Safety margin added to the robot radius (m). + double z_min_filter_{0.0}; ///< Minimum Z considered when filtering point clouds (m). + double robot_height_{0.5}; ///< Vertical extent of the robot used for filtering (m). + // --- Approach to goal --- double min_approach_linear_velocity_{0.05}; ///< Minimum linear velocity while approaching goal. double approach_velocity_scaling_dist_{1.0}; ///< Remaining-path distance at which to start slowing. diff --git a/controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp b/controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp index 761dd28b..13ba1488 100644 --- a/controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp +++ b/controllers/easynav_regulated_pp_controller/src/easynav_regulated_pp_controller/RegulatedPurePursuitController.cpp @@ -96,6 +96,10 @@ RegulatedPurePursuitController::on_initialize() use_obstacle_regulated_linear_velocity_scaling_); declare_and_get("obstacle_scaling_dist", obstacle_scaling_dist_); declare_and_get("obstacle_scaling_gain", obstacle_scaling_gain_); + declare_and_get("robot_radius", robot_radius_); + declare_and_get("safety_margin", safety_margin_); + declare_and_get("z_min_filter", z_min_filter_); + declare_and_get("robot_height", robot_height_); declare_and_get("min_approach_linear_velocity", min_approach_linear_velocity_); declare_and_get("approach_velocity_scaling_dist", approach_velocity_scaling_dist_); diff --git a/controllers/easynav_vff_controller/include/easynav_vff_controller/VffController.hpp b/controllers/easynav_vff_controller/include/easynav_vff_controller/VffController.hpp index 66dd24e4..168b0c72 100644 --- a/controllers/easynav_vff_controller/include/easynav_vff_controller/VffController.hpp +++ b/controllers/easynav_vff_controller/include/easynav_vff_controller/VffController.hpp @@ -20,6 +20,7 @@ #define EASYNAV_CONTROLLER__VFFCONTROLLER_HPP_ #include "pcl/point_cloud.h" +#include "pcl/point_types.h" #include "easynav_core/ControllerMethodBase.hpp" diff --git a/localizers/easynav_costmap_localizer/CMakeLists.txt b/localizers/easynav_costmap_localizer/CMakeLists.txt index 4b23ddce..321b7be5 100644 --- a/localizers/easynav_costmap_localizer/CMakeLists.txt +++ b/localizers/easynav_costmap_localizer/CMakeLists.txt @@ -18,6 +18,7 @@ find_package(tf2_ros REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(nav_msgs REQUIRED) +find_package(diagnostic_msgs REQUIRED) find_package(Eigen3 REQUIRED NO_MODULE) @@ -43,6 +44,34 @@ target_link_libraries(${PROJECT_NAME} PUBLIC ${nav_msgs_TARGETS} ) +add_library(amcl_convergence_evaluator SHARED + src/easynav_costmap_localizer/AmclConvergenceEvaluator.cpp +) +target_include_directories(amcl_convergence_evaluator PUBLIC + $ + $ +) +target_link_libraries(amcl_convergence_evaluator PUBLIC + easynav_core::easynav_core + pluginlib::pluginlib + ${diagnostic_msgs_TARGETS} +) + +add_library(amcl_relocalize_mitigation SHARED + src/easynav_costmap_localizer/AmclRelocalizeMitigation.cpp +) +target_include_directories(amcl_relocalize_mitigation PUBLIC + $ + $ +) +target_link_libraries(amcl_relocalize_mitigation PUBLIC + easynav_common::easynav_common + easynav_core::easynav_core + pluginlib::pluginlib + ${diagnostic_msgs_TARGETS} + ${geometry_msgs_TARGETS} +) + install( DIRECTORY include/ DESTINATION include/${PROJECT_NAME} @@ -50,6 +79,8 @@ install( install(TARGETS ${PROJECT_NAME} + amcl_convergence_evaluator + amcl_relocalize_mitigation EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -67,7 +98,11 @@ if(BUILD_TESTING) endif() ament_export_include_directories("include/${PROJECT_NAME}") -ament_export_libraries(${PROJECT_NAME}) +ament_export_libraries( + ${PROJECT_NAME} + amcl_convergence_evaluator + amcl_relocalize_mitigation +) ament_export_targets(export_${PROJECT_NAME}) # Register the planning plugins @@ -86,6 +121,7 @@ ament_export_dependencies( rclcpp geometry_msgs nav_msgs + diagnostic_msgs Eigen3 ) ament_package() diff --git a/localizers/easynav_costmap_localizer/easynav_costmap_localizer_plugins.xml b/localizers/easynav_costmap_localizer/easynav_costmap_localizer_plugins.xml index d2646b03..f84bb149 100644 --- a/localizers/easynav_costmap_localizer/easynav_costmap_localizer_plugins.xml +++ b/localizers/easynav_costmap_localizer/easynav_costmap_localizer_plugins.xml @@ -6,4 +6,20 @@ + + + + Diagnoses AMCL particle-filter divergence from the pose covariance trace. See + docs/recoveries_easynav.md §5.9. + + + + + + + Rotates in place to help AMCL relocalize after a divergence diagnostic. See + docs/recoveries_easynav.md §5.9. + + + diff --git a/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AmclConvergenceEvaluator.hpp b/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AmclConvergenceEvaluator.hpp new file mode 100644 index 00000000..37d25c7a --- /dev/null +++ b/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AmclConvergenceEvaluator.hpp @@ -0,0 +1,61 @@ +// 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. + +/// \file +/// \brief Declaration of the AmclConvergenceEvaluator plugin. + +#ifndef EASYNAV_COSTMAP_LOCALIZER__AMCLCONVERGENCEEVALUATOR_HPP_ +#define EASYNAV_COSTMAP_LOCALIZER__AMCLCONVERGENCEEVALUATOR_HPP_ + +#include "easynav_core/RecoveryEvaluatorBase.hpp" + +namespace easynav +{ + +/** + * @class AmclConvergenceEvaluator + * @brief Level-1 recovery evaluator: diagnoses AMCL particle-filter divergence. + * + * See docs/recoveries_easynav.md §5.9 — the worked example of "specialized recovery, + * co-located with the component that originates it": only the author of the localizer plugin + * really knows that particle dispersion (here, the trace of the pose covariance AMCLLocalizer + * already computes) is a good indicator of lost convergence, so this evaluator lives in the + * same package as AMCLLocalizer instead of the generic recovery_evaluators catalog. + * + * Reads the fixed key "localizer.amcl.covariance_trace" (written by AMCLLocalizer from both + * its RT and non-RT cycles) and publishes `hardware_id = "localizer.amcl"`, matched by + * AmclRelocalizeMitigation in this same package/manifest — no compile-time dependency between + * the two, only this agreed-upon diagnostic vocabulary. + */ +class AmclConvergenceEvaluator : public easynav::RecoveryEvaluatorBase +{ +public: + AmclConvergenceEvaluator() = default; + ~AmclConvergenceEvaluator() = default; + + void on_initialize() override; + +protected: + void update(NavState & nav_state) override; + +private: + /// @brief Covariance trace (var_x + var_y + var_yaw) above which AMCL is considered + /// diverged. Starting point only — depends on sensor/robot and needs tuning on the real + /// platform. + double covariance_threshold_ {1.0}; +}; + +} // namespace easynav + +#endif // EASYNAV_COSTMAP_LOCALIZER__AMCLCONVERGENCEEVALUATOR_HPP_ diff --git a/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AmclRelocalizeMitigation.hpp b/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AmclRelocalizeMitigation.hpp new file mode 100644 index 00000000..8bf739d1 --- /dev/null +++ b/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AmclRelocalizeMitigation.hpp @@ -0,0 +1,73 @@ +// 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. + +/// \file +/// \brief Declaration of the AmclRelocalizeMitigation plugin. + +#ifndef EASYNAV_COSTMAP_LOCALIZER__AMCLRELOCALIZEMITIGATION_HPP_ +#define EASYNAV_COSTMAP_LOCALIZER__AMCLRELOCALIZEMITIGATION_HPP_ + +#include "rclcpp/time.hpp" + +#include "easynav_core/RecoveryMitigationBase.hpp" + +namespace easynav +{ + +/** + * @class AmclRelocalizeMitigation + * @brief Level-1 movement mitigation: rotates in place to help AMCL relocalize. + * + * See docs/recoveries_easynav.md §5.9. Selected for diagnostics with + * hardware_id == "localizer.amcl" (the vocabulary shared with AmclConvergenceEvaluator, in the + * same package). Takes control of "cmd_vel" (requires_control() == true) and rotates slowly in + * place each RT cycle, re-checking the same covariance trace the evaluator reads, until it + * drops back under threshold — or until `timeout` elapses without that happening, at which + * point it gives up (stops, returns FAILED) rather than spinning forever. RecoveryManagerNode + * then excludes this mitigation from being reselected for the same diagnostic occurrence (see + * docs/recoveries_easynav_implementation.md, Fase 5). + */ +class AmclRelocalizeMitigation : public easynav::RecoveryMitigationBase +{ +public: + AmclRelocalizeMitigation() = default; + ~AmclRelocalizeMitigation() = 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; + RecoveryStatus on_cycle(NavState & nav_state) override; + +private: + /// @brief Angular speed commanded while rotating in place (rad/s, "slowly"). + double rotation_speed_ {0.3}; + + /// @brief Seconds to keep rotating before giving up. See the class doc comment. + double timeout_ {5.0}; + + /// @brief Same threshold as AmclConvergenceEvaluator by default — a deliberate simplification + /// (no hysteresis between evaluator and mitigator thresholds yet), same as already noted for + /// ObstacleTooCloseEvaluator/SafeRetreatRecovery. + double covariance_threshold_ {1.0}; + + rclcpp::Time start_time_; +}; + +} // namespace easynav + +#endif // EASYNAV_COSTMAP_LOCALIZER__AMCLRELOCALIZEMITIGATION_HPP_ diff --git a/localizers/easynav_costmap_localizer/package.xml b/localizers/easynav_costmap_localizer/package.xml index ec7647f1..0de5b26e 100644 --- a/localizers/easynav_costmap_localizer/package.xml +++ b/localizers/easynav_costmap_localizer/package.xml @@ -21,6 +21,7 @@ tf2_geometry_msgs geometry_msgs nav_msgs + diagnostic_msgs eigen rclcpp_lifecycle diff --git a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp index b6ad3188..e762c6b3 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -307,12 +307,25 @@ void printTransform(const tf2::Transform & tf) << rot.w() << "]\n"; } +namespace +{ +// Position + yaw dispersion in one scalar, from the 6x6 row-major covariance that get_pose() +// already fills (indices 0/7 = var_x/var_y, 35 = var_yaw). Consumed by AmclConvergenceEvaluator +// under the fixed key below — see docs/recoveries_easynav.md §5.9. +double covariance_trace(const nav_msgs::msg::Odometry & odom) +{ + return odom.pose.covariance[0] + odom.pose.covariance[7] + odom.pose.covariance[35]; +} +} // namespace + void AMCLLocalizer::update_rt(NavState & nav_state) { predict(nav_state); - nav_state.set("robot_pose", get_pose()); + const auto odom = get_pose(); + nav_state.set("robot_pose", odom); + nav_state.set("localizer.amcl.covariance_trace", covariance_trace(odom)); } void @@ -325,7 +338,9 @@ AMCLLocalizer::update(NavState & nav_state) last_reseed_ = get_node()->now(); } - nav_state.set("robot_pose", get_pose()); + const auto odom = get_pose(); + nav_state.set("robot_pose", odom); + nav_state.set("localizer.amcl.covariance_trace", covariance_trace(odom)); publishParticles(); } diff --git a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AmclConvergenceEvaluator.cpp b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AmclConvergenceEvaluator.cpp new file mode 100644 index 00000000..1c60bae5 --- /dev/null +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AmclConvergenceEvaluator.cpp @@ -0,0 +1,67 @@ +// 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. + +/// \file +/// \brief Implementation of the AmclConvergenceEvaluator class. + +#include "easynav_costmap_localizer/AmclConvergenceEvaluator.hpp" + +namespace easynav +{ + +void AmclConvergenceEvaluator::on_initialize() +{ + auto node = get_node(); + const auto & plugin_name = get_plugin_name(); + + node->declare_parameter(plugin_name + ".covariance_threshold", covariance_threshold_); + node->get_parameter(plugin_name + ".covariance_threshold", covariance_threshold_); +} + +void AmclConvergenceEvaluator::update(NavState & nav_state) +{ + diagnostic_msgs::msg::DiagnosticStatus status; + status.name = get_plugin_name(); + status.hardware_id = "localizer.amcl"; + status.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + status.message = "converged"; + + if (!nav_state.has("localizer.amcl.covariance_trace")) { + status.message = "no covariance data yet"; + publish_diagnostic(nav_state, status); + return; + } + + // Written by AMCLLocalizer from both its RT and non-RT cycles; this evaluator runs on + // RecoveryManagerNode's non-RT cycle, so get_safe() (a snapshot copy) is required here, not + // get(). See NavState's own get()/get_safe() guidance. + const double trace = nav_state.get_safe("localizer.amcl.covariance_trace"); + + if (trace > covariance_threshold_) { + status.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + status.message = "particle filter diverged"; + + diagnostic_msgs::msg::KeyValue trace_kv; + trace_kv.key = "covariance_trace"; + trace_kv.value = std::to_string(trace); + status.values.push_back(trace_kv); + } + + publish_diagnostic(nav_state, status); +} + +} // namespace easynav + +#include +PLUGINLIB_EXPORT_CLASS(easynav::AmclConvergenceEvaluator, easynav::RecoveryEvaluatorBase) diff --git a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AmclRelocalizeMitigation.cpp b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AmclRelocalizeMitigation.cpp new file mode 100644 index 00000000..83e633eb --- /dev/null +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AmclRelocalizeMitigation.cpp @@ -0,0 +1,93 @@ +// 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. + +/// \file +/// \brief Implementation of the AmclRelocalizeMitigation class. + +#include "geometry_msgs/msg/twist_stamped.hpp" + +#include "easynav_common/RTTFBuffer.hpp" + +#include "easynav_costmap_localizer/AmclRelocalizeMitigation.hpp" + +namespace easynav +{ + +void AmclRelocalizeMitigation::on_initialize() +{ + auto node = get_node(); + const auto & plugin_name = get_plugin_name(); + + node->declare_parameter(plugin_name + ".rotation_speed", rotation_speed_); + node->declare_parameter(plugin_name + ".timeout", timeout_); + node->declare_parameter(plugin_name + ".covariance_threshold", covariance_threshold_); + + node->get_parameter(plugin_name + ".rotation_speed", rotation_speed_); + node->get_parameter(plugin_name + ".timeout", timeout_); + node->get_parameter(plugin_name + ".covariance_threshold", covariance_threshold_); +} + +bool AmclRelocalizeMitigation::can_handle( + const diagnostic_msgs::msg::DiagnosticStatus & status) const +{ + return status.hardware_id == "localizer.amcl" && + status.level >= diagnostic_msgs::msg::DiagnosticStatus::ERROR; +} + +void AmclRelocalizeMitigation::on_start(NavState &) +{ + start_time_ = get_node()->now(); + RCLCPP_WARN( + get_node()->get_logger(), + "AmclRelocalizeMitigation [%s]: localization diverged, rotating in place to relocalize", + get_plugin_name().c_str()); +} + +RecoveryStatus AmclRelocalizeMitigation::on_cycle(NavState & nav_state) +{ + // Written by AMCLLocalizer from both its RT and non-RT cycles; read here from + // RecoveryManagerNode's RT cycle (this mitigation requires_control()), so get_safe() (a + // snapshot copy) is required, not get(). See NavState's own get()/get_safe() guidance. + if (nav_state.has("localizer.amcl.covariance_trace")) { + const double trace = nav_state.get_safe("localizer.amcl.covariance_trace"); + if (trace <= covariance_threshold_) { + stop_robot(nav_state); + return RecoveryStatus::SUCCEEDED; + } + } + + if ((get_node()->now() - start_time_).seconds() >= timeout_) { + RCLCPP_ERROR( + get_node()->get_logger(), + "AmclRelocalizeMitigation [%s]: gave up after %.2f s without relocalizing", + get_plugin_name().c_str(), timeout_); + stop_robot(nav_state); + return RecoveryStatus::FAILED; + } + + geometry_msgs::msg::TwistStamped cmd; + if (auto node = get_node()) { + cmd.header.stamp = node->now(); + } + cmd.header.frame_id = RTTFBuffer::getInstance()->get_tf_info().robot_frame; + cmd.twist.angular.z = rotation_speed_; + + nav_state.set("cmd_vel", cmd); + return RecoveryStatus::RUNNING; +} + +} // namespace easynav + +#include +PLUGINLIB_EXPORT_CLASS(easynav::AmclRelocalizeMitigation, easynav::RecoveryMitigationBase) diff --git a/localizers/easynav_costmap_localizer/tests/CMakeLists.txt b/localizers/easynav_costmap_localizer/tests/CMakeLists.txt index b874c059..c4910bf4 100644 --- a/localizers/easynav_costmap_localizer/tests/CMakeLists.txt +++ b/localizers/easynav_costmap_localizer/tests/CMakeLists.txt @@ -9,3 +9,13 @@ target_link_libraries(costmap_localizer_tests rclcpp_lifecycle::rclcpp_lifecycle ${std_srvs_TARGETS} ) + +ament_add_gtest(amcl_convergence_evaluator_tests amcl_convergence_evaluator_tests.cpp) +target_link_libraries(amcl_convergence_evaluator_tests + amcl_convergence_evaluator +) + +ament_add_gtest(amcl_relocalize_mitigation_tests amcl_relocalize_mitigation_tests.cpp) +target_link_libraries(amcl_relocalize_mitigation_tests + amcl_relocalize_mitigation +) diff --git a/localizers/easynav_costmap_localizer/tests/amcl_convergence_evaluator_tests.cpp b/localizers/easynav_costmap_localizer/tests/amcl_convergence_evaluator_tests.cpp new file mode 100644 index 00000000..0c5601fa --- /dev/null +++ b/localizers/easynav_costmap_localizer/tests/amcl_convergence_evaluator_tests.cpp @@ -0,0 +1,90 @@ +// 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 "gtest/gtest.h" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "easynav_costmap_localizer/AmclConvergenceEvaluator.hpp" + +class AmclConvergenceEvaluatorTestCase : public ::testing::Test +{ +protected: + void SetUp() override + { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + } + + std::shared_ptr make_ready_evaluator( + const std::shared_ptr & node, const std::string & name) + { + auto eval = std::make_shared(); + eval->initialize(node, name); + std::this_thread::sleep_for(std::chrono::milliseconds(120)); + return eval; + } +}; + +TEST_F(AmclConvergenceEvaluatorTestCase, OkWithoutCovarianceData) +{ + auto node = std::make_shared("test_no_data_node"); + auto eval = make_ready_evaluator(node, "amcl1"); + + easynav::NavState nav_state; + eval->internal_update(nav_state); + + const auto & status = + nav_state.get("diagnostics.amcl1"); + EXPECT_EQ(status.level, diagnostic_msgs::msg::DiagnosticStatus::OK); +} + +TEST_F(AmclConvergenceEvaluatorTestCase, OkWhenCovarianceBelowThreshold) +{ + auto node = std::make_shared("test_below_threshold_node"); + auto eval = make_ready_evaluator(node, "amcl2"); + + easynav::NavState nav_state; + nav_state.set("localizer.amcl.covariance_trace", 0.1); + eval->internal_update(nav_state); + + const auto & status = + nav_state.get("diagnostics.amcl2"); + EXPECT_EQ(status.level, diagnostic_msgs::msg::DiagnosticStatus::OK); +} + +TEST_F(AmclConvergenceEvaluatorTestCase, ErrorWhenCovarianceAboveThreshold) +{ + auto node = std::make_shared( + "test_above_threshold_node", + rclcpp::NodeOptions().append_parameter_override("amcl3.covariance_threshold", 0.5)); + auto eval = make_ready_evaluator(node, "amcl3"); + + easynav::NavState nav_state; + nav_state.set("localizer.amcl.covariance_trace", 2.5); + eval->internal_update(nav_state); + + const auto & status = + nav_state.get("diagnostics.amcl3"); + EXPECT_EQ(status.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); + EXPECT_EQ(status.hardware_id, "localizer.amcl"); + ASSERT_EQ(status.values.size(), 1u); + EXPECT_EQ(status.values[0].key, "covariance_trace"); + EXPECT_NEAR(std::stod(status.values[0].value), 2.5, 1e-3); +} diff --git a/localizers/easynav_costmap_localizer/tests/amcl_relocalize_mitigation_tests.cpp b/localizers/easynav_costmap_localizer/tests/amcl_relocalize_mitigation_tests.cpp new file mode 100644 index 00000000..f42a6f39 --- /dev/null +++ b/localizers/easynav_costmap_localizer/tests/amcl_relocalize_mitigation_tests.cpp @@ -0,0 +1,133 @@ +// 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 "gtest/gtest.h" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "easynav_common/RTTFBuffer.hpp" + +#include "easynav_costmap_localizer/AmclRelocalizeMitigation.hpp" + +class AmclRelocalizeMitigationTestCase : 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_mitigation( + const std::shared_ptr & node, const std::string & name) + { + auto mit = std::make_shared(); + mit->initialize(node, name); + return mit; + } +}; + +TEST_F(AmclRelocalizeMitigationTestCase, RequiresControl) +{ + auto node = std::make_shared("test_rc_node"); + auto mit = make_mitigation(node, "amcl_mit0"); + EXPECT_TRUE(mit->requires_control()); +} + +TEST_F(AmclRelocalizeMitigationTestCase, CanHandleOnlyLocalizerAmclErrors) +{ + auto node = std::make_shared("test_ch_node"); + auto mit = make_mitigation(node, "amcl_mit1"); + + diagnostic_msgs::msg::DiagnosticStatus matching; + matching.hardware_id = "localizer.amcl"; + matching.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + EXPECT_TRUE(mit->can_handle(matching)); + + diagnostic_msgs::msg::DiagnosticStatus wrong_hardware = matching; + wrong_hardware.hardware_id = "planner"; + EXPECT_FALSE(mit->can_handle(wrong_hardware)); + + diagnostic_msgs::msg::DiagnosticStatus not_an_error = matching; + not_an_error.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + EXPECT_FALSE(mit->can_handle(not_an_error)); +} + +TEST_F(AmclRelocalizeMitigationTestCase, RotatesInPlaceWhileDivergedAndWithinTimeout) +{ + auto node = std::make_shared( + "test_rotate_node", + rclcpp::NodeOptions().append_parameter_override("amcl_mit2.rotation_speed", 0.4)); + auto mit = make_mitigation(node, "amcl_mit2"); + + easynav::NavState nav_state; + nav_state.set("localizer.amcl.covariance_trace", 5.0); // well above default threshold + + mit->internal_start(nav_state); + auto status = mit->internal_cycle(nav_state); + + EXPECT_EQ(status, easynav::RecoveryStatus::RUNNING); + ASSERT_TRUE(nav_state.has("cmd_vel")); + const auto & cmd = nav_state.get("cmd_vel"); + EXPECT_DOUBLE_EQ(cmd.twist.angular.z, 0.4); + EXPECT_DOUBLE_EQ(cmd.twist.linear.x, 0.0); +} + +TEST_F(AmclRelocalizeMitigationTestCase, SucceedsOnceCovarianceDropsBelowThreshold) +{ + auto node = std::make_shared( + "test_succeed_node", + rclcpp::NodeOptions().append_parameter_override("amcl_mit3.covariance_threshold", 0.5)); + auto mit = make_mitigation(node, "amcl_mit3"); + + easynav::NavState nav_state; + nav_state.set("localizer.amcl.covariance_trace", 0.1); // relocalized + + mit->internal_start(nav_state); + auto status = mit->internal_cycle(nav_state); + + EXPECT_EQ(status, easynav::RecoveryStatus::SUCCEEDED); + ASSERT_TRUE(nav_state.has("cmd_vel")); + const auto & cmd = nav_state.get("cmd_vel"); + EXPECT_DOUBLE_EQ(cmd.twist.angular.z, 0.0); +} + +TEST_F(AmclRelocalizeMitigationTestCase, FailsAfterTimeoutWithoutRelocalizing) +{ + auto node = std::make_shared( + "test_timeout_node", + rclcpp::NodeOptions().append_parameter_override("amcl_mit4.timeout", 0.05)); + auto mit = make_mitigation(node, "amcl_mit4"); + + easynav::NavState nav_state; + nav_state.set("localizer.amcl.covariance_trace", 5.0); // still diverged + + mit->internal_start(nav_state); + std::this_thread::sleep_for(std::chrono::milliseconds(60)); // past the 50 ms timeout + auto status = mit->internal_cycle(nav_state); + + EXPECT_EQ(status, easynav::RecoveryStatus::FAILED); + ASSERT_TRUE(nav_state.has("cmd_vel")); + const auto & cmd = nav_state.get("cmd_vel"); + EXPECT_DOUBLE_EQ(cmd.twist.angular.z, 0.0); +} diff --git a/recovery_evaluators/easynav_no_path_evaluator/src/easynav_no_path_evaluator/NoPathEvaluator.cpp b/recovery_evaluators/easynav_no_path_evaluator/src/easynav_no_path_evaluator/NoPathEvaluator.cpp index 980cb1b6..7e5dcb63 100644 --- a/recovery_evaluators/easynav_no_path_evaluator/src/easynav_no_path_evaluator/NoPathEvaluator.cpp +++ b/recovery_evaluators/easynav_no_path_evaluator/src/easynav_no_path_evaluator/NoPathEvaluator.cpp @@ -15,6 +15,7 @@ /// \file /// \brief Implementation of the NoPathEvaluator class. +#include "nav_msgs/msg/goals.hpp" #include "nav_msgs/msg/path.hpp" #include "easynav_no_path_evaluator/NoPathEvaluator.hpp" @@ -32,7 +33,16 @@ void NoPathEvaluator::update(NavState & nav_state) status.name = get_plugin_name(); status.hardware_id = "planner"; - if (!nav_state.has("path")) { + // "goals" is written by GoalManager on the same non-RT thread this evaluator runs on, so a + // plain get() is safe here too. No active goal means there is nothing to plan toward, so a + // missing/empty "path" is expected, not something this evaluator should flag. + const bool has_active_goal = nav_state.has("goals") && + !nav_state.get("goals").goals.empty(); + + if (!has_active_goal) { + status.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + status.message = "no active goal"; + } else if (!nav_state.has("path")) { status.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; status.message = "no path published yet"; } else { diff --git a/recovery_evaluators/easynav_no_path_evaluator/tests/no_path_evaluator_tests.cpp b/recovery_evaluators/easynav_no_path_evaluator/tests/no_path_evaluator_tests.cpp index f18ef121..65c0ae72 100644 --- a/recovery_evaluators/easynav_no_path_evaluator/tests/no_path_evaluator_tests.cpp +++ b/recovery_evaluators/easynav_no_path_evaluator/tests/no_path_evaluator_tests.cpp @@ -20,6 +20,7 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "nav_msgs/msg/goals.hpp" #include "nav_msgs/msg/path.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" @@ -34,56 +35,101 @@ class NoPathEvaluatorTestCase : public ::testing::Test rclcpp::init(0, nullptr); } } + + static void set_active_goal(easynav::NavState & nav_state) + { + nav_msgs::msg::Goals goals; + goals.goals.push_back(geometry_msgs::msg::PoseStamped()); + nav_state.set("goals", goals); + } }; -TEST_F(NoPathEvaluatorTestCase, WarnsWhenNoPathYet) +TEST_F(NoPathEvaluatorTestCase, OkWithoutAnActiveGoal) { - auto node = std::make_shared("test_no_path_node"); + // No "goals" key at all (e.g. before GoalManager's first non-RT cycle): an empty/missing + // path is expected, not a failure. + auto node = std::make_shared("test_no_goals_key_node"); easynav::NoPathEvaluator eval; - eval.initialize(node, "no_path"); + eval.initialize(node, "no_path0"); std::this_thread::sleep_for(std::chrono::milliseconds(120)); easynav::NavState nav_state; eval.internal_update(nav_state); - ASSERT_TRUE(nav_state.has("diagnostics.no_path")); EXPECT_EQ( - nav_state.get("diagnostics.no_path").level, + nav_state.get("diagnostics.no_path0").level, + diagnostic_msgs::msg::DiagnosticStatus::OK); +} + +TEST_F(NoPathEvaluatorTestCase, OkWithEmptyGoalsListEvenWithNoPath) +{ + auto node = std::make_shared("test_empty_goals_node"); + easynav::NoPathEvaluator eval; + eval.initialize(node, "no_path1"); + + easynav::NavState nav_state; + nav_state.set("goals", nav_msgs::msg::Goals()); // present but empty: no active goal + + std::this_thread::sleep_for(std::chrono::milliseconds(120)); + eval.internal_update(nav_state); + + EXPECT_EQ( + nav_state.get("diagnostics.no_path1").level, + diagnostic_msgs::msg::DiagnosticStatus::OK); +} + +TEST_F(NoPathEvaluatorTestCase, WarnsWhenNoPathYetWithAnActiveGoal) +{ + auto node = std::make_shared("test_no_path_node"); + easynav::NoPathEvaluator eval; + eval.initialize(node, "no_path2"); + + easynav::NavState nav_state; + set_active_goal(nav_state); + + std::this_thread::sleep_for(std::chrono::milliseconds(120)); + eval.internal_update(nav_state); + + ASSERT_TRUE(nav_state.has("diagnostics.no_path2")); + EXPECT_EQ( + nav_state.get("diagnostics.no_path2").level, diagnostic_msgs::msg::DiagnosticStatus::WARN); } -TEST_F(NoPathEvaluatorTestCase, ErrorsOnEmptyPath) +TEST_F(NoPathEvaluatorTestCase, ErrorsOnEmptyPathWithAnActiveGoal) { auto node = std::make_shared("test_empty_path_node"); easynav::NoPathEvaluator eval; - eval.initialize(node, "no_path2"); + eval.initialize(node, "no_path3"); easynav::NavState nav_state; + set_active_goal(nav_state); nav_state.set("path", nav_msgs::msg::Path()); std::this_thread::sleep_for(std::chrono::milliseconds(120)); eval.internal_update(nav_state); EXPECT_EQ( - nav_state.get("diagnostics.no_path2").level, + nav_state.get("diagnostics.no_path3").level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); } -TEST_F(NoPathEvaluatorTestCase, OkWhenPathHasPoses) +TEST_F(NoPathEvaluatorTestCase, OkWhenPathHasPosesWithAnActiveGoal) { auto node = std::make_shared("test_ok_path_node"); easynav::NoPathEvaluator eval; - eval.initialize(node, "no_path3"); + eval.initialize(node, "no_path4"); + easynav::NavState nav_state; + set_active_goal(nav_state); nav_msgs::msg::Path path; path.poses.push_back(geometry_msgs::msg::PoseStamped()); - easynav::NavState nav_state; nav_state.set("path", path); std::this_thread::sleep_for(std::chrono::milliseconds(120)); eval.internal_update(nav_state); EXPECT_EQ( - nav_state.get("diagnostics.no_path3").level, + nav_state.get("diagnostics.no_path4").level, diagnostic_msgs::msg::DiagnosticStatus::OK); } diff --git a/recovery_evaluators/easynav_obstacle_too_close_evaluator/include/easynav_obstacle_too_close_evaluator/ObstacleTooCloseEvaluator.hpp b/recovery_evaluators/easynav_obstacle_too_close_evaluator/include/easynav_obstacle_too_close_evaluator/ObstacleTooCloseEvaluator.hpp index 334b0f8a..28d036fd 100644 --- a/recovery_evaluators/easynav_obstacle_too_close_evaluator/include/easynav_obstacle_too_close_evaluator/ObstacleTooCloseEvaluator.hpp +++ b/recovery_evaluators/easynav_obstacle_too_close_evaluator/include/easynav_obstacle_too_close_evaluator/ObstacleTooCloseEvaluator.hpp @@ -18,6 +18,10 @@ #ifndef EASYNAV_OBSTACLE_TOO_CLOSE_EVALUATOR__OBSTACLETOOCLOSEEVALUATOR_HPP_ #define EASYNAV_OBSTACLE_TOO_CLOSE_EVALUATOR__OBSTACLETOOCLOSEEVALUATOR_HPP_ +#include + +#include "rclcpp/time.hpp" + #include "easynav_core/RecoveryEvaluatorBase.hpp" namespace easynav @@ -36,6 +40,11 @@ namespace easynav * "already stopped" condition is checked against an independent, physically measured signal * (the robot's own commanded/estimated velocity from "robot_pose"), so this evaluator works * correctly regardless of *why* the robot stopped. + * + * Per §5.2, "stopped" must also be *sustained* for a short debounce window before it is + * trusted: the RT and non-RT cycles run in parallel, so a single low-velocity sample could + * still be taken mid-brake. While within that window this evaluator reports OK (informational, + * matched by no mitigator's can_handle()), not yet the real proximity check. */ class ObstacleTooCloseEvaluator : public easynav::RecoveryEvaluatorBase { @@ -58,6 +67,12 @@ class ObstacleTooCloseEvaluator : public easynav::RecoveryEvaluatorBase /// @brief Below this angular speed (rad/s), the robot is considered stopped. double angular_velocity_epsilon_ {0.05}; + + /// @brief Seconds the "stopped" condition must hold, uninterrupted, before it is trusted. + double debounce_duration_ {0.2}; + + /// @brief Timestamp since the robot has been continuously stopped, reset the moment it moves. + std::optional stopped_since_; }; } // namespace easynav diff --git a/recovery_evaluators/easynav_obstacle_too_close_evaluator/src/easynav_obstacle_too_close_evaluator/ObstacleTooCloseEvaluator.cpp b/recovery_evaluators/easynav_obstacle_too_close_evaluator/src/easynav_obstacle_too_close_evaluator/ObstacleTooCloseEvaluator.cpp index 4a39b579..0e380890 100644 --- a/recovery_evaluators/easynav_obstacle_too_close_evaluator/src/easynav_obstacle_too_close_evaluator/ObstacleTooCloseEvaluator.cpp +++ b/recovery_evaluators/easynav_obstacle_too_close_evaluator/src/easynav_obstacle_too_close_evaluator/ObstacleTooCloseEvaluator.cpp @@ -36,11 +36,13 @@ void ObstacleTooCloseEvaluator::on_initialize() plugin_name + ".linear_velocity_epsilon", linear_velocity_epsilon_); node->declare_parameter( plugin_name + ".angular_velocity_epsilon", angular_velocity_epsilon_); + node->declare_parameter(plugin_name + ".debounce_duration", debounce_duration_); node->get_parameter(plugin_name + ".safe_distance", safe_distance_); node->get_parameter(plugin_name + ".linear_velocity_epsilon", linear_velocity_epsilon_); node->get_parameter( plugin_name + ".angular_velocity_epsilon", angular_velocity_epsilon_); + node->get_parameter(plugin_name + ".debounce_duration", debounce_duration_); } void ObstacleTooCloseEvaluator::update(NavState & nav_state) @@ -54,6 +56,7 @@ void ObstacleTooCloseEvaluator::update(NavState & nav_state) status.message = "no obstacle too close"; if (!nav_state.has("robot_pose")) { + stopped_since_.reset(); publish_diagnostic(nav_state, status); return; } @@ -73,6 +76,23 @@ void ObstacleTooCloseEvaluator::update(NavState & nav_state) // Still moving (e.g. the level-0 reflex is still braking): too early to judge proximity as // something this evaluator should act on. See the compound-condition rationale in the // class doc comment. + stopped_since_.reset(); + status.message = "still moving"; + publish_diagnostic(nav_state, status); + return; + } + + if (!stopped_since_.has_value()) { + stopped_since_ = get_node()->now(); + } + + const double stopped_for = (get_node()->now() - *stopped_since_).seconds(); + if (stopped_for < debounce_duration_) { + // §5.2: "stopped" must be sustained for a short debounce window before it is trusted — the + // RT and non-RT cycles run in parallel, so a single low-velocity sample could still be + // taken mid-brake. Report as OK (informational, no mitigator's can_handle() matches it) + // rather than evaluating proximity yet. + status.message = "recently stopped, confirming before evaluating proximity"; publish_diagnostic(nav_state, status); return; } diff --git a/recovery_evaluators/easynav_obstacle_too_close_evaluator/tests/obstacle_too_close_evaluator_tests.cpp b/recovery_evaluators/easynav_obstacle_too_close_evaluator/tests/obstacle_too_close_evaluator_tests.cpp index cb77f555..736d066c 100644 --- a/recovery_evaluators/easynav_obstacle_too_close_evaluator/tests/obstacle_too_close_evaluator_tests.cpp +++ b/recovery_evaluators/easynav_obstacle_too_close_evaluator/tests/obstacle_too_close_evaluator_tests.cpp @@ -119,7 +119,11 @@ TEST_F(ObstacleTooCloseEvaluatorTestCase, OkWhenStoppedButNoObstacleNearby) TEST_F(ObstacleTooCloseEvaluatorTestCase, ErrorWhenStoppedTooCloseToAnObstacle) { - auto node = std::make_shared("test_stopped_close_node"); + // debounce_duration is overridden to 0 so a single sample already counts as "sustained + // stopped" — the debounce window itself has its own dedicated tests below. + auto node = std::make_shared( + "test_stopped_close_node", + rclcpp::NodeOptions().append_parameter_override("close4.debounce_duration", 0.0)); auto eval = make_ready_evaluator(node, "close4"); easynav::NavState nav_state; @@ -136,3 +140,86 @@ TEST_F(ObstacleTooCloseEvaluatorTestCase, ErrorWhenStoppedTooCloseToAnObstacle) EXPECT_EQ(status.values[0].key, "distance"); EXPECT_NEAR(std::stod(status.values[0].value), 0.2, 1e-3); } + +// --------------------------------------------------------------------------- +// Debounce window (§5.2): "stopped" must be sustained for a short interval before it is +// trusted, so a single low-velocity sample taken mid-brake (RT and non-RT cycles run in +// parallel) cannot be mistaken for "already stopped". +// --------------------------------------------------------------------------- + +TEST_F(ObstacleTooCloseEvaluatorTestCase, RemainsOkWithinDebounceWindowEvenIfObstacleIsClose) +{ + // Default debounce_duration (0.2 s): a single sample right after stopping must not yet + // trigger ERROR, however close the obstacle is. + auto node = std::make_shared("test_debounce_ok_node"); + auto eval = make_ready_evaluator(node, "close5"); + + easynav::NavState nav_state; + nav_state.set("robot_pose", make_odom(0.0, 0.0)); + nav_state.set("obstacle_scan", make_obstacle_at(0.2, 0.0)); + + eval->internal_update(nav_state); + + const auto & status = + nav_state.get("diagnostics.close5"); + EXPECT_EQ(status.level, diagnostic_msgs::msg::DiagnosticStatus::OK); +} + +TEST_F(ObstacleTooCloseEvaluatorTestCase, ErrorOnceDebounceWindowElapses) +{ + auto node = std::make_shared( + "test_debounce_elapses_node", + rclcpp::NodeOptions() + .append_parameter_override("close6.debounce_duration", 0.05) + .append_parameter_override("close6.freq", 200.0)); + auto eval = make_ready_evaluator(node, "close6"); + + easynav::NavState nav_state; + nav_state.set("robot_pose", make_odom(0.0, 0.0)); + nav_state.set("obstacle_scan", make_obstacle_at(0.2, 0.0)); + + eval->internal_update(nav_state); // starts the debounce timer, still OK + ASSERT_EQ( + nav_state.get("diagnostics.close6").level, + diagnostic_msgs::msg::DiagnosticStatus::OK); + + std::this_thread::sleep_for(std::chrono::milliseconds(60)); // past the 50 ms debounce + eval->internal_update(nav_state); + + EXPECT_EQ( + nav_state.get("diagnostics.close6").level, + diagnostic_msgs::msg::DiagnosticStatus::ERROR); +} + +TEST_F(ObstacleTooCloseEvaluatorTestCase, DebounceResetsIfRobotMovesAgain) +{ + auto node = std::make_shared( + "test_debounce_reset_node", + rclcpp::NodeOptions() + .append_parameter_override("close7.debounce_duration", 0.05) + .append_parameter_override("close7.freq", 200.0)); + auto eval = make_ready_evaluator(node, "close7"); + + easynav::NavState nav_state; + nav_state.set("obstacle_scan", make_obstacle_at(0.2, 0.0)); + + // Stops, most of the way through the debounce window... + nav_state.set("robot_pose", make_odom(0.0, 0.0)); + eval->internal_update(nav_state); + std::this_thread::sleep_for(std::chrono::milliseconds(60)); // would clear a 50 ms debounce + + // ...but moves again before it fires, which must restart the debounce clock. + nav_state.set("robot_pose", make_odom(0.5, 0.0)); + eval->internal_update(nav_state); + // 10 ms << the 50 ms debounce: well within a fresh window if the reset actually happened. + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + + // Stops again: if the clock had NOT been reset, elapsed time since the very first stop would + // already exceed the debounce window and this would incorrectly report ERROR. + nav_state.set("robot_pose", make_odom(0.0, 0.0)); + eval->internal_update(nav_state); + + EXPECT_EQ( + nav_state.get("diagnostics.close7").level, + diagnostic_msgs::msg::DiagnosticStatus::OK); +} diff --git a/recovery_mitigations/easynav_clear_map_recovery/easynav_clear_map_recovery_plugins.xml b/recovery_mitigations/easynav_clear_map_recovery/easynav_clear_map_recovery_plugins.xml deleted file mode 100644 index ad96d1ed..00000000 --- a/recovery_mitigations/easynav_clear_map_recovery/easynav_clear_map_recovery_plugins.xml +++ /dev/null @@ -1,10 +0,0 @@ - - - - - Requests every loaded MapsManagerBase plugin to reset/clear its data (hardware_id - "planner", level ERROR). Does not take control_owner. - - - - diff --git a/recovery_mitigations/easynav_clear_map_recovery/include/easynav_clear_map_recovery/ClearMapRecovery.hpp b/recovery_mitigations/easynav_clear_map_recovery/include/easynav_clear_map_recovery/ClearMapRecovery.hpp deleted file mode 100644 index 91c73caa..00000000 --- a/recovery_mitigations/easynav_clear_map_recovery/include/easynav_clear_map_recovery/ClearMapRecovery.hpp +++ /dev/null @@ -1,61 +0,0 @@ -// 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. - -/// \file -/// \brief Declaration of the ClearMapRecovery plugin. - -#ifndef EASYNAV_CLEAR_MAP_RECOVERY__CLEARMAPRECOVERY_HPP_ -#define EASYNAV_CLEAR_MAP_RECOVERY__CLEARMAPRECOVERY_HPP_ - -#include "easynav_core/RecoveryMitigationBase.hpp" - -namespace easynav -{ - -/** - * @class ClearMapRecovery - * @brief Level-1 mitigation: resets/clears whatever a MapsManagerBase plugin considers - * clearable. See docs/recoveries_easynav.md §5.11 (Nav2 Clear*Costmap precedent). - * - * Selected, like ForceReplanRecovery, for diagnostics with hardware_id == "planner" at level - * ERROR or above — the same "no_path" symptom NoPathEvaluator publishes. This is a deliberate - * simplification versus the design's more general ambition of a dedicated map-health - * evaluator (see docs/recoveries_easynav_implementation.md, Fase 4): no such evaluator exists - * yet, so for now this mitigation reuses the one generic "the planner can't find a path" - * diagnostic, relying on RecoveryManagerNode's per-diagnostic attempt count (mitigation_types - * order: ForceReplanRecovery first, then this one) to be tried only after ForceReplanRecovery - * has already had its chance and failed to unstick the planner. - * - * Does not requires_control(): only requests MapsManagerNode::reset() via the one-shot - * NavState key "maps_manager_reset_requested", consumed by SystemNode::system_cycle() right - * after it cycles MapsManagerNode. Whether that reset actually clears anything depends on - * whether any loaded MapsManagerBase plugin overrides reset() at all (the default is a no-op); - * either way this mitigation reports SUCCEEDED once the request has been handed off. - */ -class ClearMapRecovery : public easynav::RecoveryMitigationBase -{ -public: - ClearMapRecovery() = default; - ~ClearMapRecovery() = default; - - bool can_handle(const diagnostic_msgs::msg::DiagnosticStatus & status) const override; - -protected: - void on_start(NavState & nav_state) override; - RecoveryStatus on_cycle(NavState & nav_state) override; -}; - -} // namespace easynav - -#endif // EASYNAV_CLEAR_MAP_RECOVERY__CLEARMAPRECOVERY_HPP_ diff --git a/recovery_mitigations/easynav_clear_map_recovery/package.xml b/recovery_mitigations/easynav_clear_map_recovery/package.xml deleted file mode 100644 index eb24a96f..00000000 --- a/recovery_mitigations/easynav_clear_map_recovery/package.xml +++ /dev/null @@ -1,25 +0,0 @@ - - - - easynav_clear_map_recovery - 0.4.2 - Easy Navigation: recovery mitigation that resets/clears map manager data. - Francisco Martín Rico - Apache-2.0 - - ament_cmake - - easynav_common - easynav_core - pluginlib - rclcpp - diagnostic_msgs - - ament_lint_auto - ament_lint_common - ament_cmake_gtest - - - ament_cmake - - diff --git a/recovery_mitigations/easynav_clear_map_recovery/src/easynav_clear_map_recovery/ClearMapRecovery.cpp b/recovery_mitigations/easynav_clear_map_recovery/src/easynav_clear_map_recovery/ClearMapRecovery.cpp deleted file mode 100644 index 35c41611..00000000 --- a/recovery_mitigations/easynav_clear_map_recovery/src/easynav_clear_map_recovery/ClearMapRecovery.cpp +++ /dev/null @@ -1,51 +0,0 @@ -// 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. - -/// \file -/// \brief Implementation of the ClearMapRecovery class. - -#include "easynav_clear_map_recovery/ClearMapRecovery.hpp" - -namespace easynav -{ - -bool ClearMapRecovery::can_handle(const diagnostic_msgs::msg::DiagnosticStatus & status) const -{ - return status.hardware_id == "planner" && - status.level >= diagnostic_msgs::msg::DiagnosticStatus::ERROR; -} - -void ClearMapRecovery::on_start(NavState & nav_state) -{ - RCLCPP_WARN( - get_node()->get_logger(), "ClearMapRecovery [%s]: requesting a map reset", - get_plugin_name().c_str()); - - nav_state.set("maps_manager_reset_requested", true); -} - -RecoveryStatus ClearMapRecovery::on_cycle(NavState & nav_state) -{ - // Same one-shot pattern as ForceReplanRecovery: SystemNode consumes and resets the flag - // right after cycling MapsManagerNode, one non-RT tick after on_start() set it. - const bool still_pending = nav_state.has("maps_manager_reset_requested") && - nav_state.get("maps_manager_reset_requested"); - - return still_pending ? RecoveryStatus::RUNNING : RecoveryStatus::SUCCEEDED; -} - -} // namespace easynav - -#include -PLUGINLIB_EXPORT_CLASS(easynav::ClearMapRecovery, easynav::RecoveryMitigationBase) diff --git a/recovery_mitigations/easynav_clear_map_recovery/tests/CMakeLists.txt b/recovery_mitigations/easynav_clear_map_recovery/tests/CMakeLists.txt deleted file mode 100644 index 5e7061e6..00000000 --- a/recovery_mitigations/easynav_clear_map_recovery/tests/CMakeLists.txt +++ /dev/null @@ -1,2 +0,0 @@ -ament_add_gtest(clear_map_recovery_tests clear_map_recovery_tests.cpp) -target_link_libraries(clear_map_recovery_tests ${PROJECT_NAME}) diff --git a/recovery_mitigations/easynav_clear_map_recovery/tests/clear_map_recovery_tests.cpp b/recovery_mitigations/easynav_clear_map_recovery/tests/clear_map_recovery_tests.cpp deleted file mode 100644 index 403f4af4..00000000 --- a/recovery_mitigations/easynav_clear_map_recovery/tests/clear_map_recovery_tests.cpp +++ /dev/null @@ -1,80 +0,0 @@ -// 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 "gtest/gtest.h" - -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" - -#include "easynav_clear_map_recovery/ClearMapRecovery.hpp" - -class ClearMapRecoveryTestCase : public ::testing::Test -{ -protected: - void SetUp() override - { - if (!rclcpp::ok()) { - rclcpp::init(0, nullptr); - } - } - - std::shared_ptr make_recovery( - const std::shared_ptr & node, const std::string & name) - { - auto rec = std::make_shared(); - rec->initialize(node, name); - return rec; - } -}; - -TEST_F(ClearMapRecoveryTestCase, DoesNotRequireControl) -{ - auto node = std::make_shared("test_rc_node"); - auto rec = make_recovery(node, "clear0"); - EXPECT_FALSE(rec->requires_control()); -} - -TEST_F(ClearMapRecoveryTestCase, CanHandleOnlyPlannerErrors) -{ - auto node = std::make_shared("test_ch_node"); - auto rec = make_recovery(node, "clear1"); - - diagnostic_msgs::msg::DiagnosticStatus matching; - matching.hardware_id = "planner"; - matching.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - EXPECT_TRUE(rec->can_handle(matching)); - - diagnostic_msgs::msg::DiagnosticStatus wrong_hardware = matching; - wrong_hardware.hardware_id = "obstacle_proximity"; - EXPECT_FALSE(rec->can_handle(wrong_hardware)); -} - -TEST_F(ClearMapRecoveryTestCase, RunningUntilSystemNodeConsumesTheRequest) -{ - auto node = std::make_shared("test_run_node"); - auto rec = make_recovery(node, "clear2"); - easynav::NavState nav_state; - - rec->internal_start(nav_state); - - ASSERT_TRUE(nav_state.has("maps_manager_reset_requested")); - EXPECT_TRUE(nav_state.get("maps_manager_reset_requested")); - - EXPECT_EQ(rec->internal_cycle(nav_state), easynav::RecoveryStatus::RUNNING); - - // Simulate SystemNode::system_cycle() consuming the request. - nav_state.set("maps_manager_reset_requested", false); - - EXPECT_EQ(rec->internal_cycle(nav_state), easynav::RecoveryStatus::SUCCEEDED); -} diff --git a/recovery_mitigations/easynav_force_replan_recovery/CMakeLists.txt b/recovery_mitigations/easynav_force_replan_recovery/CMakeLists.txt deleted file mode 100644 index a4fa9875..00000000 --- a/recovery_mitigations/easynav_force_replan_recovery/CMakeLists.txt +++ /dev/null @@ -1,67 +0,0 @@ -cmake_minimum_required(VERSION 3.20) -project(easynav_force_replan_recovery) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) -find_package(easynav_common REQUIRED) -find_package(easynav_core REQUIRED) -find_package(pluginlib REQUIRED) -find_package(rclcpp REQUIRED) -find_package(diagnostic_msgs REQUIRED) - -add_library(${PROJECT_NAME} SHARED - src/easynav_force_replan_recovery/ForceReplanRecovery.cpp -) -target_include_directories(${PROJECT_NAME} PUBLIC - $ - $ -) -target_link_libraries(${PROJECT_NAME} PUBLIC - easynav_common::easynav_common - easynav_core::easynav_core - pluginlib::pluginlib - rclcpp::rclcpp - ${diagnostic_msgs_TARGETS} -) - -install( - DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} -) - -install(TARGETS - ${PROJECT_NAME} - EXPORT export_${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() - - find_package(ament_cmake_gtest REQUIRED) - add_subdirectory(tests) -endif() - -ament_export_include_directories("include/${PROJECT_NAME}") -ament_export_libraries(${PROJECT_NAME}) -ament_export_targets(export_${PROJECT_NAME}) - -# Register the recovery mitigation plugin -pluginlib_export_plugin_description_file(easynav_core easynav_force_replan_recovery_plugins.xml) - -ament_export_dependencies( - easynav_common - easynav_core - pluginlib - rclcpp - diagnostic_msgs -) -ament_package() diff --git a/recovery_mitigations/easynav_force_replan_recovery/easynav_force_replan_recovery_plugins.xml b/recovery_mitigations/easynav_force_replan_recovery/easynav_force_replan_recovery_plugins.xml deleted file mode 100644 index 3e621a6e..00000000 --- a/recovery_mitigations/easynav_force_replan_recovery/easynav_force_replan_recovery_plugins.xml +++ /dev/null @@ -1,10 +0,0 @@ - - - - - Forces an immediate replan (hardware_id "planner", level ERROR) via - PlannerMethodBase::force_update(). Does not take control_owner. - - - - diff --git a/recovery_mitigations/easynav_force_replan_recovery/include/easynav_force_replan_recovery/ForceReplanRecovery.hpp b/recovery_mitigations/easynav_force_replan_recovery/include/easynav_force_replan_recovery/ForceReplanRecovery.hpp deleted file mode 100644 index 024a75db..00000000 --- a/recovery_mitigations/easynav_force_replan_recovery/include/easynav_force_replan_recovery/ForceReplanRecovery.hpp +++ /dev/null @@ -1,61 +0,0 @@ -// 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. - -/// \file -/// \brief Declaration of the ForceReplanRecovery plugin. - -#ifndef EASYNAV_FORCE_REPLAN_RECOVERY__FORCEREPLANRECOVERY_HPP_ -#define EASYNAV_FORCE_REPLAN_RECOVERY__FORCEREPLANRECOVERY_HPP_ - -#include "easynav_core/RecoveryMitigationBase.hpp" - -namespace easynav -{ - -/** - * @class ForceReplanRecovery - * @brief Level-1 mitigation: forces an immediate replan. See docs/recoveries_easynav.md §5.11. - * - * Selected for diagnostics with hardware_id == "planner" (the vocabulary shared with - * NoPathEvaluator) at level ERROR or above (an empty/missing path is not itself an error while - * the planner simply hasn't run yet — WARN — only an ERROR, i.e. the planner actually produced - * an empty path, warrants forcing a retry). - * - * Does not requires_control(): it never touches "cmd_vel" or "control_owner", only requests - * PlannerMethodBase::force_update() (already used for "a new goal arrived") via the one-shot - * NavState key "force_replan_requested", consumed by SystemNode::system_cycle() right before - * it cycles PlannerNode — see docs/recoveries_easynav_implementation.md, Fase 4. This is a - * request, not a guarantee: whether the new attempt actually produces a non-empty path is up - * to the planner plugin and the environment, not this mitigation, which always reports - * SUCCEEDED once the request has been handed off (one non-RT cycle later). If the diagnostic - * persists, RecoveryManagerNode's per-diagnostic attempt count (see RecoveryManagerNode.hpp) - * lets a later candidate (e.g. ClearMapRecovery) take over instead of retrying this one - * forever. - */ -class ForceReplanRecovery : public easynav::RecoveryMitigationBase -{ -public: - ForceReplanRecovery() = default; - ~ForceReplanRecovery() = default; - - bool can_handle(const diagnostic_msgs::msg::DiagnosticStatus & status) const override; - -protected: - void on_start(NavState & nav_state) override; - RecoveryStatus on_cycle(NavState & nav_state) override; -}; - -} // namespace easynav - -#endif // EASYNAV_FORCE_REPLAN_RECOVERY__FORCEREPLANRECOVERY_HPP_ diff --git a/recovery_mitigations/easynav_force_replan_recovery/package.xml b/recovery_mitigations/easynav_force_replan_recovery/package.xml deleted file mode 100644 index f361037b..00000000 --- a/recovery_mitigations/easynav_force_replan_recovery/package.xml +++ /dev/null @@ -1,25 +0,0 @@ - - - - easynav_force_replan_recovery - 0.4.2 - Easy Navigation: recovery mitigation that forces an immediate replan when the planner produces an empty path. - Francisco Martín Rico - Apache-2.0 - - ament_cmake - - easynav_common - easynav_core - pluginlib - rclcpp - diagnostic_msgs - - ament_lint_auto - ament_lint_common - ament_cmake_gtest - - - ament_cmake - - diff --git a/recovery_mitigations/easynav_force_replan_recovery/src/easynav_force_replan_recovery/ForceReplanRecovery.cpp b/recovery_mitigations/easynav_force_replan_recovery/src/easynav_force_replan_recovery/ForceReplanRecovery.cpp deleted file mode 100644 index f575557f..00000000 --- a/recovery_mitigations/easynav_force_replan_recovery/src/easynav_force_replan_recovery/ForceReplanRecovery.cpp +++ /dev/null @@ -1,52 +0,0 @@ -// 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. - -/// \file -/// \brief Implementation of the ForceReplanRecovery class. - -#include "easynav_force_replan_recovery/ForceReplanRecovery.hpp" - -namespace easynav -{ - -bool ForceReplanRecovery::can_handle(const diagnostic_msgs::msg::DiagnosticStatus & status) const -{ - return status.hardware_id == "planner" && - status.level >= diagnostic_msgs::msg::DiagnosticStatus::ERROR; -} - -void ForceReplanRecovery::on_start(NavState & nav_state) -{ - RCLCPP_WARN( - get_node()->get_logger(), "ForceReplanRecovery [%s]: forcing an immediate replan", - get_plugin_name().c_str()); - - nav_state.set("force_replan_requested", true); -} - -RecoveryStatus ForceReplanRecovery::on_cycle(NavState & nav_state) -{ - // SystemNode consumes "force_replan_requested" (sets it back to false) in the same - // non-RT cycle it forces PlannerNode to replan; by the time cycle() reaches - // RecoveryManagerNode again (next non-RT tick), the request has already been handed off. - const bool still_pending = nav_state.has("force_replan_requested") && - nav_state.get("force_replan_requested"); - - return still_pending ? RecoveryStatus::RUNNING : RecoveryStatus::SUCCEEDED; -} - -} // namespace easynav - -#include -PLUGINLIB_EXPORT_CLASS(easynav::ForceReplanRecovery, easynav::RecoveryMitigationBase) diff --git a/recovery_mitigations/easynav_force_replan_recovery/tests/CMakeLists.txt b/recovery_mitigations/easynav_force_replan_recovery/tests/CMakeLists.txt deleted file mode 100644 index 015080a7..00000000 --- a/recovery_mitigations/easynav_force_replan_recovery/tests/CMakeLists.txt +++ /dev/null @@ -1,2 +0,0 @@ -ament_add_gtest(force_replan_recovery_tests force_replan_recovery_tests.cpp) -target_link_libraries(force_replan_recovery_tests ${PROJECT_NAME}) diff --git a/recovery_mitigations/easynav_force_replan_recovery/tests/force_replan_recovery_tests.cpp b/recovery_mitigations/easynav_force_replan_recovery/tests/force_replan_recovery_tests.cpp deleted file mode 100644 index 09d3e25b..00000000 --- a/recovery_mitigations/easynav_force_replan_recovery/tests/force_replan_recovery_tests.cpp +++ /dev/null @@ -1,94 +0,0 @@ -// 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 "gtest/gtest.h" - -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" - -#include "easynav_force_replan_recovery/ForceReplanRecovery.hpp" - -class ForceReplanRecoveryTestCase : public ::testing::Test -{ -protected: - void SetUp() override - { - if (!rclcpp::ok()) { - rclcpp::init(0, nullptr); - } - } - - std::shared_ptr make_recovery( - const std::shared_ptr & node, const std::string & name) - { - auto rec = std::make_shared(); - rec->initialize(node, name); - return rec; - } -}; - -TEST_F(ForceReplanRecoveryTestCase, DoesNotRequireControl) -{ - auto node = std::make_shared("test_rc_node"); - auto rec = make_recovery(node, "replan0"); - EXPECT_FALSE(rec->requires_control()); -} - -TEST_F(ForceReplanRecoveryTestCase, CanHandleOnlyPlannerErrors) -{ - auto node = std::make_shared("test_ch_node"); - auto rec = make_recovery(node, "replan1"); - - diagnostic_msgs::msg::DiagnosticStatus matching; - matching.hardware_id = "planner"; - matching.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - EXPECT_TRUE(rec->can_handle(matching)); - - diagnostic_msgs::msg::DiagnosticStatus wrong_hardware = matching; - wrong_hardware.hardware_id = "obstacle_proximity"; - EXPECT_FALSE(rec->can_handle(wrong_hardware)); - - diagnostic_msgs::msg::DiagnosticStatus just_warn = matching; - just_warn.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - EXPECT_FALSE(rec->can_handle(just_warn)); -} - -TEST_F(ForceReplanRecoveryTestCase, RunningUntilSystemNodeConsumesTheRequest) -{ - auto node = std::make_shared("test_run_node"); - auto rec = make_recovery(node, "replan2"); - easynav::NavState nav_state; - - rec->internal_start(nav_state); - - ASSERT_TRUE(nav_state.has("force_replan_requested")); - EXPECT_TRUE(nav_state.get("force_replan_requested")); - - // Still pending: SystemNode has not consumed it yet. - EXPECT_EQ(rec->internal_cycle(nav_state), easynav::RecoveryStatus::RUNNING); - - // Simulate SystemNode::system_cycle() consuming the request. - nav_state.set("force_replan_requested", false); - - EXPECT_EQ(rec->internal_cycle(nav_state), easynav::RecoveryStatus::SUCCEEDED); -} - -TEST_F(ForceReplanRecoveryTestCase, SucceedsImmediatelyIfNeverStarted) -{ - auto node = std::make_shared("test_never_node"); - auto rec = make_recovery(node, "replan3"); - easynav::NavState nav_state; // "force_replan_requested" absent - - EXPECT_EQ(rec->internal_cycle(nav_state), easynav::RecoveryStatus::SUCCEEDED); -} diff --git a/recovery_mitigations/easynav_clear_map_recovery/CMakeLists.txt b/recovery_mitigations/easynav_human_assistance_recovery/CMakeLists.txt similarity index 87% rename from recovery_mitigations/easynav_clear_map_recovery/CMakeLists.txt rename to recovery_mitigations/easynav_human_assistance_recovery/CMakeLists.txt index 201c066c..a037c97a 100644 --- a/recovery_mitigations/easynav_clear_map_recovery/CMakeLists.txt +++ b/recovery_mitigations/easynav_human_assistance_recovery/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.20) -project(easynav_clear_map_recovery) +project(easynav_human_assistance_recovery) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) @@ -13,7 +13,7 @@ find_package(rclcpp REQUIRED) find_package(diagnostic_msgs REQUIRED) add_library(${PROJECT_NAME} SHARED - src/easynav_clear_map_recovery/ClearMapRecovery.cpp + src/easynav_human_assistance_recovery/HumanAssistanceRecovery.cpp ) target_include_directories(${PROJECT_NAME} PUBLIC $ @@ -55,7 +55,7 @@ ament_export_libraries(${PROJECT_NAME}) ament_export_targets(export_${PROJECT_NAME}) # Register the recovery mitigation plugin -pluginlib_export_plugin_description_file(easynav_core easynav_clear_map_recovery_plugins.xml) +pluginlib_export_plugin_description_file(easynav_core easynav_human_assistance_recovery_plugins.xml) ament_export_dependencies( easynav_common diff --git a/recovery_mitigations/easynav_human_assistance_recovery/easynav_human_assistance_recovery_plugins.xml b/recovery_mitigations/easynav_human_assistance_recovery/easynav_human_assistance_recovery_plugins.xml new file mode 100644 index 00000000..d490a816 --- /dev/null +++ b/recovery_mitigations/easynav_human_assistance_recovery/easynav_human_assistance_recovery_plugins.xml @@ -0,0 +1,11 @@ + + + + + Last-resort, generic mitigation: accepts any ERROR diagnostic, holds the robot stopped and + makes the problem observable, and resumes normal operation as soon as no diagnostic is + left at ERROR level. Does not fail the mission (see docs/recoveries_easynav.md §5.15). + + + + diff --git a/recovery_mitigations/easynav_human_assistance_recovery/include/easynav_human_assistance_recovery/HumanAssistanceRecovery.hpp b/recovery_mitigations/easynav_human_assistance_recovery/include/easynav_human_assistance_recovery/HumanAssistanceRecovery.hpp new file mode 100644 index 00000000..45a42459 --- /dev/null +++ b/recovery_mitigations/easynav_human_assistance_recovery/include/easynav_human_assistance_recovery/HumanAssistanceRecovery.hpp @@ -0,0 +1,63 @@ +// 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. + +/// \file +/// \brief Declaration of the HumanAssistanceRecovery plugin. + +#ifndef EASYNAV_HUMAN_ASSISTANCE_RECOVERY__HUMANASSISTANCERECOVERY_HPP_ +#define EASYNAV_HUMAN_ASSISTANCE_RECOVERY__HUMANASSISTANCERECOVERY_HPP_ + +#include "easynav_core/RecoveryMitigationBase.hpp" + +namespace easynav +{ + +/** + * @class HumanAssistanceRecovery + * @brief Level-1 last-resort mitigation: asks a human operator for help. + * + * See docs/recoveries_easynav.md §5.15 — this is a simplified instance of the design's + * HumanAssistanceRecovery (no `teleop` mode, no episode-id `ack`: the "ack" here is physical, + * whatever fixed the problem shows up as the offending diagnostic going back to OK). Generic + * and domain-agnostic by design: unlike SafeRetreatRecovery/AmclRelocalizeMitigation, it does + * not know or care which component raised the diagnostic — can_handle() accepts any ERROR, + * meant to be configured with the lowest priority (or listed last in "mitigation_types") so it + * is only reached once every more specific mitigator has been tried and excluded (see + * RecoveryManagerNode's per-diagnostic exclusion, docs/recoveries_easynav_implementation.md). + * + * Not to be confused with the now-removed NotifyAndHoldRecovery (Fase 4, retired in the + * Sesión 11 of the implementation log), which treated its trigger as a mission failure + * (`GoalManager::set_failed`). This one does not: once every diagnostic is observed back at OK + * — presumably because a human fixed whatever was wrong — it returns control and the robot + * resumes its current mission, exactly as §5.15 specifies for a human-certified recovery. + */ +class HumanAssistanceRecovery : public easynav::RecoveryMitigationBase +{ +public: + HumanAssistanceRecovery() = default; + ~HumanAssistanceRecovery() = 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; + RecoveryStatus on_cycle(NavState & nav_state) override; +}; + +} // namespace easynav + +#endif // EASYNAV_HUMAN_ASSISTANCE_RECOVERY__HUMANASSISTANCERECOVERY_HPP_ diff --git a/recovery_mitigations/easynav_notify_and_hold_recovery/package.xml b/recovery_mitigations/easynav_human_assistance_recovery/package.xml similarity index 77% rename from recovery_mitigations/easynav_notify_and_hold_recovery/package.xml rename to recovery_mitigations/easynav_human_assistance_recovery/package.xml index 60c5df73..66cb06d8 100644 --- a/recovery_mitigations/easynav_notify_and_hold_recovery/package.xml +++ b/recovery_mitigations/easynav_human_assistance_recovery/package.xml @@ -1,9 +1,9 @@ - easynav_notify_and_hold_recovery + easynav_human_assistance_recovery 0.4.2 - Easy Navigation: last-resort recovery mitigation that stops the robot, reports mission failure, and holds. + Easy Navigation: last-resort recovery mitigation that asks a human operator for help and resumes normal operation once the underlying problem is observed to be resolved. Francisco Martín Rico Apache-2.0 @@ -13,12 +13,12 @@ easynav_core pluginlib rclcpp - nav_msgs diagnostic_msgs ament_lint_auto ament_lint_common ament_cmake_gtest + rclcpp_lifecycle ament_cmake diff --git a/recovery_mitigations/easynav_human_assistance_recovery/src/easynav_human_assistance_recovery/HumanAssistanceRecovery.cpp b/recovery_mitigations/easynav_human_assistance_recovery/src/easynav_human_assistance_recovery/HumanAssistanceRecovery.cpp new file mode 100644 index 00000000..3ddf011a --- /dev/null +++ b/recovery_mitigations/easynav_human_assistance_recovery/src/easynav_human_assistance_recovery/HumanAssistanceRecovery.cpp @@ -0,0 +1,92 @@ +// 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. + +/// \file +/// \brief Implementation of the HumanAssistanceRecovery class. + +#include "easynav_human_assistance_recovery/HumanAssistanceRecovery.hpp" + +namespace easynav +{ + +void HumanAssistanceRecovery::on_initialize() +{ +} + +bool HumanAssistanceRecovery::can_handle( + const diagnostic_msgs::msg::DiagnosticStatus & status) const +{ + return status.level >= diagnostic_msgs::msg::DiagnosticStatus::ERROR; +} + +void HumanAssistanceRecovery::on_start(NavState & nav_state) +{ + // Selection (and so on_start()) always runs from RecoveryManagerNode::cycle(), the same + // non-RT thread the evaluators that wrote "diagnostics" run on, so a plain get() is safe + // here. This is the observable "asking a human for help" signal §5.15 requires; how it + // reaches an actual person (app, light, sound) is a deployment decision, not this plugin's. + std::string summary; + for (const auto & key : nav_state.get_group_keys("diagnostics")) { + if (!nav_state.has(key)) {continue;} + const auto & status = nav_state.get(key); + if (status.level >= diagnostic_msgs::msg::DiagnosticStatus::ERROR) { + if (!summary.empty()) {summary += ", ";} + summary += key + " (" + status.message + ")"; + } + } + + RCLCPP_ERROR( + get_node()->get_logger(), + "HumanAssistanceRecovery [%s]: no other mitigation resolved this — requesting human " + "assistance for: %s", + get_plugin_name().c_str(), summary.empty() ? "unknown" : summary.c_str()); +} + +RecoveryStatus HumanAssistanceRecovery::on_cycle(NavState & nav_state) +{ + // Runs on the RT thread (requires_control()), reading a group written on the non-RT thread — + // get_safe() (a snapshot copy) is required here, not get(). See NavState's own + // get()/get_safe() guidance. + bool any_error = false; + for (const auto & key : nav_state.get_group_keys("diagnostics")) { + if (!nav_state.has(key)) {continue;} + const auto status = nav_state.get_safe(key); + if (status.level >= diagnostic_msgs::msg::DiagnosticStatus::ERROR) { + any_error = true; + break; + } + } + + if (!any_error) { + // Whatever was wrong is gone — presumably a human fixed it. Unlike the retired + // NotifyAndHoldRecovery, this does not touch GoalManager: the mission was never failed, + // just paused, so resuming is simply returning control_owner to the nominal controller. + stop_robot(nav_state); + return RecoveryStatus::SUCCEEDED; + } + + RCLCPP_ERROR_THROTTLE( + get_node()->get_logger(), *get_node()->get_clock(), 10000, + "HumanAssistanceRecovery [%s]: still waiting for human assistance", + get_plugin_name().c_str()); + + // No timeout: this is the last resort, it waits as long as it takes. + stop_robot(nav_state); + return RecoveryStatus::RUNNING; +} + +} // namespace easynav + +#include +PLUGINLIB_EXPORT_CLASS(easynav::HumanAssistanceRecovery, easynav::RecoveryMitigationBase) diff --git a/recovery_mitigations/easynav_human_assistance_recovery/tests/CMakeLists.txt b/recovery_mitigations/easynav_human_assistance_recovery/tests/CMakeLists.txt new file mode 100644 index 00000000..2e1f04a1 --- /dev/null +++ b/recovery_mitigations/easynav_human_assistance_recovery/tests/CMakeLists.txt @@ -0,0 +1,2 @@ +ament_add_gtest(human_assistance_recovery_tests human_assistance_recovery_tests.cpp) +target_link_libraries(human_assistance_recovery_tests ${PROJECT_NAME}) diff --git a/recovery_mitigations/easynav_human_assistance_recovery/tests/human_assistance_recovery_tests.cpp b/recovery_mitigations/easynav_human_assistance_recovery/tests/human_assistance_recovery_tests.cpp new file mode 100644 index 00000000..a3a45b98 --- /dev/null +++ b/recovery_mitigations/easynav_human_assistance_recovery/tests/human_assistance_recovery_tests.cpp @@ -0,0 +1,127 @@ +// 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 "gtest/gtest.h" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "geometry_msgs/msg/twist_stamped.hpp" + +#include "easynav_human_assistance_recovery/HumanAssistanceRecovery.hpp" + +class HumanAssistanceRecoveryTestCase : public ::testing::Test +{ +protected: + void SetUp() override + { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + } + + std::shared_ptr make_recovery( + const std::shared_ptr & node, const std::string & name) + { + auto rec = std::make_shared(); + rec->initialize(node, name); + return rec; + } + + static diagnostic_msgs::msg::DiagnosticStatus make_status( + uint8_t level, const std::string & hardware_id = "any_component") + { + diagnostic_msgs::msg::DiagnosticStatus status; + status.level = level; + status.hardware_id = hardware_id; + status.message = "test"; + return status; + } +}; + +TEST_F(HumanAssistanceRecoveryTestCase, RequiresControl) +{ + auto node = std::make_shared("test_rc_node"); + auto rec = make_recovery(node, "human0"); + EXPECT_TRUE(rec->requires_control()); +} + +TEST_F(HumanAssistanceRecoveryTestCase, CanHandleAnyHardwareIdAtErrorLevelOrAbove) +{ + auto node = std::make_shared("test_ch_node"); + auto rec = make_recovery(node, "human1"); + + EXPECT_TRUE( + rec->can_handle(make_status(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "planner"))); + EXPECT_TRUE( + rec->can_handle(make_status(diagnostic_msgs::msg::DiagnosticStatus::ERROR, "localizer.amcl"))); + EXPECT_TRUE( + rec->can_handle(make_status(diagnostic_msgs::msg::DiagnosticStatus::STALE, "whatever"))); +} + +TEST_F(HumanAssistanceRecoveryTestCase, DoesNotHandleWarnOrOk) +{ + auto node = std::make_shared("test_no_ch_node"); + auto rec = make_recovery(node, "human2"); + + EXPECT_FALSE(rec->can_handle(make_status(diagnostic_msgs::msg::DiagnosticStatus::WARN))); + EXPECT_FALSE(rec->can_handle(make_status(diagnostic_msgs::msg::DiagnosticStatus::OK))); +} + +TEST_F(HumanAssistanceRecoveryTestCase, RunsWhileAnyDiagnosticIsStillError) +{ + auto node = std::make_shared("test_running_node"); + auto rec = make_recovery(node, "human3"); + + easynav::NavState nav_state; + nav_state.set("diagnostics.planner", make_status(diagnostic_msgs::msg::DiagnosticStatus::ERROR)); + nav_state.set_group("diagnostics", {"diagnostics.planner"}); + + rec->internal_start(nav_state); + auto status = rec->internal_cycle(nav_state); + + EXPECT_EQ(status, easynav::RecoveryStatus::RUNNING); + ASSERT_TRUE(nav_state.has("cmd_vel")); + const auto & cmd = nav_state.get("cmd_vel"); + EXPECT_DOUBLE_EQ(cmd.twist.linear.x, 0.0); + EXPECT_DOUBLE_EQ(cmd.twist.angular.z, 0.0); +} + +TEST_F(HumanAssistanceRecoveryTestCase, SucceedsOnceEveryDiagnosticIsOkAgain) +{ + auto node = std::make_shared("test_succeed_node"); + auto rec = make_recovery(node, "human4"); + + easynav::NavState nav_state; + nav_state.set("diagnostics.planner", make_status(diagnostic_msgs::msg::DiagnosticStatus::OK)); + nav_state.set_group("diagnostics", {"diagnostics.planner"}); + + rec->internal_start(nav_state); + auto status = rec->internal_cycle(nav_state); + + EXPECT_EQ(status, easynav::RecoveryStatus::SUCCEEDED); +} + +TEST_F(HumanAssistanceRecoveryTestCase, SucceedsWithNoDiagnosticsGroupAtAll) +{ + auto node = std::make_shared("test_no_group_node"); + auto rec = make_recovery(node, "human5"); + + easynav::NavState nav_state; // no "diagnostics" group at all + + rec->internal_start(nav_state); + auto status = rec->internal_cycle(nav_state); + + EXPECT_EQ(status, easynav::RecoveryStatus::SUCCEEDED); +} diff --git a/recovery_mitigations/easynav_notify_and_hold_recovery/CMakeLists.txt b/recovery_mitigations/easynav_notify_and_hold_recovery/CMakeLists.txt deleted file mode 100644 index 3859b2be..00000000 --- a/recovery_mitigations/easynav_notify_and_hold_recovery/CMakeLists.txt +++ /dev/null @@ -1,70 +0,0 @@ -cmake_minimum_required(VERSION 3.20) -project(easynav_notify_and_hold_recovery) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) -find_package(easynav_common REQUIRED) -find_package(easynav_core REQUIRED) -find_package(pluginlib REQUIRED) -find_package(rclcpp REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(diagnostic_msgs REQUIRED) - -add_library(${PROJECT_NAME} SHARED - src/easynav_notify_and_hold_recovery/NotifyAndHoldRecovery.cpp -) -target_include_directories(${PROJECT_NAME} PUBLIC - $ - $ -) -target_link_libraries(${PROJECT_NAME} PUBLIC - easynav_common::easynav_common - easynav_core::easynav_core - pluginlib::pluginlib - rclcpp::rclcpp - ${nav_msgs_TARGETS} - ${diagnostic_msgs_TARGETS} -) - -install( - DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} -) - -install(TARGETS - ${PROJECT_NAME} - EXPORT export_${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() - - find_package(ament_cmake_gtest REQUIRED) - add_subdirectory(tests) -endif() - -ament_export_include_directories("include/${PROJECT_NAME}") -ament_export_libraries(${PROJECT_NAME}) -ament_export_targets(export_${PROJECT_NAME}) - -# Register the recovery mitigation plugin -pluginlib_export_plugin_description_file(easynav_core easynav_notify_and_hold_recovery_plugins.xml) - -ament_export_dependencies( - easynav_common - easynav_core - pluginlib - rclcpp - nav_msgs - diagnostic_msgs -) -ament_package() diff --git a/recovery_mitigations/easynav_notify_and_hold_recovery/easynav_notify_and_hold_recovery_plugins.xml b/recovery_mitigations/easynav_notify_and_hold_recovery/easynav_notify_and_hold_recovery_plugins.xml deleted file mode 100644 index 742c208b..00000000 --- a/recovery_mitigations/easynav_notify_and_hold_recovery/easynav_notify_and_hold_recovery_plugins.xml +++ /dev/null @@ -1,12 +0,0 @@ - - - - - Last-resort mitigation: stops the robot, reports the mission as failed/errored via - GoalManager, and holds (takes control_owner) until a new goal is accepted. Unconditional - can_handle() for any ERROR-or-above diagnostic -- meant to be listed last in - mitigation_types. - - - - diff --git a/recovery_mitigations/easynav_notify_and_hold_recovery/include/easynav_notify_and_hold_recovery/NotifyAndHoldRecovery.hpp b/recovery_mitigations/easynav_notify_and_hold_recovery/include/easynav_notify_and_hold_recovery/NotifyAndHoldRecovery.hpp deleted file mode 100644 index 0f01c952..00000000 --- a/recovery_mitigations/easynav_notify_and_hold_recovery/include/easynav_notify_and_hold_recovery/NotifyAndHoldRecovery.hpp +++ /dev/null @@ -1,72 +0,0 @@ -// 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. - -/// \file -/// \brief Declaration of the NotifyAndHoldRecovery plugin. - -#ifndef EASYNAV_NOTIFY_AND_HOLD_RECOVERY__NOTIFYANDHOLDRECOVERY_HPP_ -#define EASYNAV_NOTIFY_AND_HOLD_RECOVERY__NOTIFYANDHOLDRECOVERY_HPP_ - -#include "easynav_core/RecoveryMitigationBase.hpp" - -namespace easynav -{ - -/** - * @class NotifyAndHoldRecovery - * @brief Level-1, last-resort mitigation: stops the robot and reports the mission as - * failed/errored via GoalManager, then holds until a new goal is accepted. See - * docs/recoveries_easynav.md §5.8 and §5.11. - * - * Unconditional can_handle() (true for any diagnostic at level ERROR or above): this is meant - * to be the last entry in "mitigation_types", so RecoveryManagerNode's selection (first - * applicable candidate in list order, see RecoveryManagerNode.hpp) only ever reaches it for a - * diagnostic no earlier-listed, more specific mitigation declared can_handle() for. - * - * requires_control() == true, unlike the rest of the Fase-4 catalog (see - * docs/recoveries_easynav_implementation.md for why this was a necessary correction over the - * design table, which did not mark it "(toma control_owner)"): stopping the robot with a single - * zero-velocity write is not enough on its own, because nothing would then stop the nominal - * controller from overwriting "cmd_vel" again on its very next RT cycle while still chasing the - * (now abandoned) path. Taking control_owner and re-affirming zero velocity every RT cycle - * (on_cycle() returns RecoveryStatus::RUNNING) is what actually makes the "hold" in the name - * true. It releases control_owner (returns SUCCEEDED) only once GoalManager reports a new, - * non-empty goal list (checked via NavState's "goals" key, already kept in sync by - * GoalManager::update() — no dependency on easynav_system needed for that), i.e. once someone - * has sent the robot a fresh goal after the failure. - */ -class NotifyAndHoldRecovery : public easynav::RecoveryMitigationBase -{ -public: - NotifyAndHoldRecovery() = default; - ~NotifyAndHoldRecovery() = 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; - RecoveryStatus on_cycle(NavState & nav_state) override; - -private: - /// @brief Whether to report the mission via GoalManager::set_failed() ("failed", the - /// default) or GoalManager::set_error() ("error"). Parameter ".report_as". - std::string report_as_ {"failed"}; -}; - -} // namespace easynav - -#endif // EASYNAV_NOTIFY_AND_HOLD_RECOVERY__NOTIFYANDHOLDRECOVERY_HPP_ diff --git a/recovery_mitigations/easynav_notify_and_hold_recovery/src/easynav_notify_and_hold_recovery/NotifyAndHoldRecovery.cpp b/recovery_mitigations/easynav_notify_and_hold_recovery/src/easynav_notify_and_hold_recovery/NotifyAndHoldRecovery.cpp deleted file mode 100644 index 6152df91..00000000 --- a/recovery_mitigations/easynav_notify_and_hold_recovery/src/easynav_notify_and_hold_recovery/NotifyAndHoldRecovery.cpp +++ /dev/null @@ -1,93 +0,0 @@ -// 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. - -/// \file -/// \brief Implementation of the NotifyAndHoldRecovery class. - -#include "nav_msgs/msg/goals.hpp" - -#include "easynav_notify_and_hold_recovery/NotifyAndHoldRecovery.hpp" - -namespace easynav -{ - -void NotifyAndHoldRecovery::on_initialize() -{ - auto node = get_node(); - const auto & plugin_name = get_plugin_name(); - - node->declare_parameter(plugin_name + ".report_as", report_as_); - node->get_parameter(plugin_name + ".report_as", report_as_); - - if (report_as_ != "failed" && report_as_ != "error") { - RCLCPP_WARN( - node->get_logger(), - "NotifyAndHoldRecovery [%s]: invalid report_as '%s' (must be 'failed' or 'error'), " - "defaulting to 'failed'", plugin_name.c_str(), report_as_.c_str()); - report_as_ = "failed"; - } -} - -bool NotifyAndHoldRecovery::can_handle(const diagnostic_msgs::msg::DiagnosticStatus & status) const -{ - // Unconditional catch-all: see the class doc comment. This is meant to be the last entry in - // "mitigation_types", so it is only ever reached for a diagnostic nothing earlier-listed - // could handle. - return status.level >= diagnostic_msgs::msg::DiagnosticStatus::ERROR; -} - -void NotifyAndHoldRecovery::on_start(NavState & nav_state) -{ - std::string reason = - "Recovery escalated to mission level: no autonomous mitigation resolved the situation"; - for (const auto & key : nav_state.get_group_keys("diagnostics")) { - if (!nav_state.has(key)) { - continue; - } - const auto & status = nav_state.get(key); - if (status.level >= diagnostic_msgs::msg::DiagnosticStatus::ERROR) { - reason = "[" + status.name + "] (" + status.hardware_id + "): " + status.message; - break; - } - } - - RCLCPP_ERROR( - get_node()->get_logger(), - "NotifyAndHoldRecovery [%s]: %s -- reporting mission as '%s' and holding", - get_plugin_name().c_str(), reason.c_str(), report_as_.c_str()); - - // Handed to SystemNode::system_cycle(), the only place with a real GoalManager instance to - // call set_failed()/set_error() on. See docs/recoveries_easynav_implementation.md, Fase 4. - nav_state.set("goal_manager_request", report_as_); - nav_state.set("goal_manager_reason", reason); -} - -RecoveryStatus NotifyAndHoldRecovery::on_cycle(NavState & nav_state) -{ - stop_robot(nav_state); - - // "goals" crosses the RT/non-RT boundary (written by GoalManager::update() on the non-RT - // thread), so use get_safe() here, in the RT thread. See NavState.hpp's own guidance. - const bool has_active_goal = nav_state.has("goals") && - !nav_state.get_safe("goals").goals.empty(); - - // Someone sent a new goal after the failure: release control_owner and let the nominal - // controller resume against it. Otherwise, keep holding (RUNNING) indefinitely. - return has_active_goal ? RecoveryStatus::SUCCEEDED : RecoveryStatus::RUNNING; -} - -} // namespace easynav - -#include -PLUGINLIB_EXPORT_CLASS(easynav::NotifyAndHoldRecovery, easynav::RecoveryMitigationBase) diff --git a/recovery_mitigations/easynav_notify_and_hold_recovery/tests/CMakeLists.txt b/recovery_mitigations/easynav_notify_and_hold_recovery/tests/CMakeLists.txt deleted file mode 100644 index 2f0254de..00000000 --- a/recovery_mitigations/easynav_notify_and_hold_recovery/tests/CMakeLists.txt +++ /dev/null @@ -1,2 +0,0 @@ -ament_add_gtest(notify_and_hold_recovery_tests notify_and_hold_recovery_tests.cpp) -target_link_libraries(notify_and_hold_recovery_tests ${PROJECT_NAME}) diff --git a/recovery_mitigations/easynav_notify_and_hold_recovery/tests/notify_and_hold_recovery_tests.cpp b/recovery_mitigations/easynav_notify_and_hold_recovery/tests/notify_and_hold_recovery_tests.cpp deleted file mode 100644 index 14314c58..00000000 --- a/recovery_mitigations/easynav_notify_and_hold_recovery/tests/notify_and_hold_recovery_tests.cpp +++ /dev/null @@ -1,133 +0,0 @@ -// 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 "gtest/gtest.h" - -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" - -#include "geometry_msgs/msg/twist_stamped.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "nav_msgs/msg/goals.hpp" - -#include "easynav_notify_and_hold_recovery/NotifyAndHoldRecovery.hpp" - -class NotifyAndHoldRecoveryTestCase : public ::testing::Test -{ -protected: - void SetUp() override - { - if (!rclcpp::ok()) { - rclcpp::init(0, nullptr); - } - } - - std::shared_ptr make_recovery( - const std::shared_ptr & node, const std::string & name) - { - auto rec = std::make_shared(); - rec->initialize(node, name); - return rec; - } -}; - -TEST_F(NotifyAndHoldRecoveryTestCase, RequiresControl) -{ - auto node = std::make_shared("test_rc_node"); - auto rec = make_recovery(node, "hold0"); - EXPECT_TRUE(rec->requires_control()); -} - -TEST_F(NotifyAndHoldRecoveryTestCase, CanHandleAnyErrorOrAboveDiagnostic) -{ - auto node = std::make_shared("test_ch_node"); - auto rec = make_recovery(node, "hold1"); - - diagnostic_msgs::msg::DiagnosticStatus error_status; - error_status.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - EXPECT_TRUE(rec->can_handle(error_status)); - - diagnostic_msgs::msg::DiagnosticStatus warn_status; - warn_status.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - EXPECT_FALSE(rec->can_handle(warn_status)); -} - -TEST_F(NotifyAndHoldRecoveryTestCase, OnStartDefaultsToReportingFailed) -{ - auto node = std::make_shared("test_default_node"); - auto rec = make_recovery(node, "hold2"); - - easynav::NavState nav_state; - rec->internal_start(nav_state); - - ASSERT_TRUE(nav_state.has("goal_manager_request")); - EXPECT_EQ(nav_state.get("goal_manager_request"), "failed"); - ASSERT_TRUE(nav_state.has("goal_manager_reason")); - EXPECT_FALSE(nav_state.get("goal_manager_reason").empty()); -} - -TEST_F(NotifyAndHoldRecoveryTestCase, ReportAsErrorParameterIsHonored) -{ - auto node = std::make_shared( - "test_error_node", - rclcpp::NodeOptions().parameter_overrides({rclcpp::Parameter("hold3.report_as", "error")})); - auto rec = make_recovery(node, "hold3"); - - easynav::NavState nav_state; - rec->internal_start(nav_state); - - EXPECT_EQ(nav_state.get("goal_manager_request"), "error"); -} - -TEST_F(NotifyAndHoldRecoveryTestCase, InvalidReportAsFallsBackToFailed) -{ - auto node = std::make_shared( - "test_invalid_node", - rclcpp::NodeOptions().parameter_overrides({rclcpp::Parameter("hold4.report_as", "bogus")})); - auto rec = make_recovery(node, "hold4"); - - easynav::NavState nav_state; - rec->internal_start(nav_state); - - EXPECT_EQ(nav_state.get("goal_manager_request"), "failed"); -} - -TEST_F(NotifyAndHoldRecoveryTestCase, HoldsAndStopsWhileNoActiveGoal) -{ - auto node = std::make_shared("test_hold_node"); - auto rec = make_recovery(node, "hold5"); - - easynav::NavState nav_state; // "goals" absent -> treated as no active goal - - auto status = rec->internal_cycle(nav_state); - - EXPECT_EQ(status, easynav::RecoveryStatus::RUNNING); - ASSERT_TRUE(nav_state.has("cmd_vel")); - const auto & cmd = nav_state.get("cmd_vel"); - EXPECT_DOUBLE_EQ(cmd.twist.linear.x, 0.0); - EXPECT_DOUBLE_EQ(cmd.twist.angular.z, 0.0); -} - -TEST_F(NotifyAndHoldRecoveryTestCase, ReleasesControlOnceANewGoalIsActive) -{ - auto node = std::make_shared("test_release_node"); - auto rec = make_recovery(node, "hold6"); - - easynav::NavState nav_state; - nav_msgs::msg::Goals goals; - goals.goals.push_back(geometry_msgs::msg::PoseStamped()); - nav_state.set("goals", goals); - - EXPECT_EQ(rec->internal_cycle(nav_state), easynav::RecoveryStatus::SUCCEEDED); -} diff --git a/recovery_mitigations/easynav_safe_waypoint_recovery/CMakeLists.txt b/recovery_mitigations/easynav_safe_waypoint_recovery/CMakeLists.txt deleted file mode 100644 index 98b1c092..00000000 --- a/recovery_mitigations/easynav_safe_waypoint_recovery/CMakeLists.txt +++ /dev/null @@ -1,70 +0,0 @@ -cmake_minimum_required(VERSION 3.20) -project(easynav_safe_waypoint_recovery) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -find_package(ament_cmake REQUIRED) -find_package(easynav_common REQUIRED) -find_package(easynav_core REQUIRED) -find_package(pluginlib REQUIRED) -find_package(rclcpp REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(diagnostic_msgs REQUIRED) - -add_library(${PROJECT_NAME} SHARED - src/easynav_safe_waypoint_recovery/SafeWaypointRecovery.cpp -) -target_include_directories(${PROJECT_NAME} PUBLIC - $ - $ -) -target_link_libraries(${PROJECT_NAME} PUBLIC - easynav_common::easynav_common - easynav_core::easynav_core - pluginlib::pluginlib - rclcpp::rclcpp - ${geometry_msgs_TARGETS} - ${diagnostic_msgs_TARGETS} -) - -install( - DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} -) - -install(TARGETS - ${PROJECT_NAME} - EXPORT export_${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() - - find_package(ament_cmake_gtest REQUIRED) - add_subdirectory(tests) -endif() - -ament_export_include_directories("include/${PROJECT_NAME}") -ament_export_libraries(${PROJECT_NAME}) -ament_export_targets(export_${PROJECT_NAME}) - -# Register the recovery mitigation plugin -pluginlib_export_plugin_description_file(easynav_core easynav_safe_waypoint_recovery_plugins.xml) - -ament_export_dependencies( - easynav_common - easynav_core - pluginlib - rclcpp - geometry_msgs - diagnostic_msgs -) -ament_package() diff --git a/recovery_mitigations/easynav_safe_waypoint_recovery/easynav_safe_waypoint_recovery_plugins.xml b/recovery_mitigations/easynav_safe_waypoint_recovery/easynav_safe_waypoint_recovery_plugins.xml deleted file mode 100644 index fb46ac41..00000000 --- a/recovery_mitigations/easynav_safe_waypoint_recovery/easynav_safe_waypoint_recovery_plugins.xml +++ /dev/null @@ -1,11 +0,0 @@ - - - - - Redirects the mission to a preconfigured safe waypoint instead of the current goal. - Opt-in: only can_handle() (any ERROR-or-above diagnostic) when "<instance>.enabled" - is true. Does not take control_owner. - - - - diff --git a/recovery_mitigations/easynav_safe_waypoint_recovery/include/easynav_safe_waypoint_recovery/SafeWaypointRecovery.hpp b/recovery_mitigations/easynav_safe_waypoint_recovery/include/easynav_safe_waypoint_recovery/SafeWaypointRecovery.hpp deleted file mode 100644 index dfbca472..00000000 --- a/recovery_mitigations/easynav_safe_waypoint_recovery/include/easynav_safe_waypoint_recovery/SafeWaypointRecovery.hpp +++ /dev/null @@ -1,90 +0,0 @@ -// 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. - -/// \file -/// \brief Declaration of the SafeWaypointRecovery plugin. - -#ifndef EASYNAV_SAFE_WAYPOINT_RECOVERY__SAFEWAYPOINTRECOVERY_HPP_ -#define EASYNAV_SAFE_WAYPOINT_RECOVERY__SAFEWAYPOINTRECOVERY_HPP_ - -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "rclcpp/publisher.hpp" - -#include "easynav_core/RecoveryMitigationBase.hpp" - -namespace easynav -{ - -/** - * @class SafeWaypointRecovery - * @brief Level-1, mission-scope mitigation: redirects the mission to a preconfigured safe - * waypoint instead of the goal currently being pursued. See docs/recoveries_easynav.md §5.8 - * (Escenario 5 of TOMASys/SysSelf) and §5.11. - * - * Deliberately opt-in: can_handle() only ever returns true if a safe waypoint has actually - * been configured (parameter ".enabled", default false) — see the "Decisión de - * implementación" note in docs/recoveries_easynav_implementation.md, Fase 4, for why: with no - * configured alternative there would be nothing useful to do, and letting can_handle() return - * true anyway would either loop retrying a no-op or (worse) require NotifyAndHoldRecovery to - * be listed before this one to ever get a turn, inverting the intended escalation order. This - * way an unconfigured instance is simply skipped by RecoveryManagerNode's selection, falling - * through to whatever is listed after it (typically NotifyAndHoldRecovery). - * - * Deviates from the literal design text in one respect: recoveries_easynav.md §5.8 says to - * "use GoalManagerClient" to send the alternate goal. GoalManagerClient's constructor takes an - * rclcpp::Node::SharedPtr, and its accept/preempt protocol runs across several non-RT cycles — - * both awkward for a plugin living inside RecoveryManagerNode (an rclcpp_lifecycle::LifecycleNode) - * that just wants to hand off one pose. Instead this publishes a single PoseStamped on the - * "goal_pose" topic — the exact same "external, single-pose goal injection" channel GoalManager - * already exposes for RViz/GUI use (GoalManager::comanded_pose_callback()), which already - * handles preempting whatever goal is currently active. See - * docs/recoveries_easynav_implementation.md for the full rationale. - * - * Does not requires_control(): PlannerNode/ControllerNode keep running normally against the - * new goal once GoalManager accepts it, same as any other goal change. - */ -class SafeWaypointRecovery : public easynav::RecoveryMitigationBase -{ -public: - SafeWaypointRecovery() = default; - ~SafeWaypointRecovery() = default; - - void on_initialize() override; - - bool can_handle(const diagnostic_msgs::msg::DiagnosticStatus & status) const override; - -protected: - void on_start(NavState & nav_state) override; - RecoveryStatus on_cycle(NavState & nav_state) override; - -private: - /// @brief Opt-in flag: this mitigation only ever can_handle() when true. - bool enabled_ {false}; - - /// @brief Safe waypoint pose, in \ref frame_. - double x_ {0.0}; - double y_ {0.0}; - double yaw_ {0.0}; - - /// @brief Frame the safe waypoint is expressed in. Defaults to the workspace's configured - /// map frame (read from RTTFBuffer) if left empty. - std::string frame_; - - /// @brief Publisher for the single-pose goal injection channel ("goal_pose"). - rclcpp::Publisher::SharedPtr goal_pose_pub_; -}; - -} // namespace easynav - -#endif // EASYNAV_SAFE_WAYPOINT_RECOVERY__SAFEWAYPOINTRECOVERY_HPP_ diff --git a/recovery_mitigations/easynav_safe_waypoint_recovery/package.xml b/recovery_mitigations/easynav_safe_waypoint_recovery/package.xml deleted file mode 100644 index 2bec5aa0..00000000 --- a/recovery_mitigations/easynav_safe_waypoint_recovery/package.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - - easynav_safe_waypoint_recovery - 0.4.2 - Easy Navigation: recovery mitigation that redirects the mission to a preconfigured safe waypoint. - Francisco Martín Rico - Apache-2.0 - - ament_cmake - - easynav_common - easynav_core - pluginlib - rclcpp - geometry_msgs - diagnostic_msgs - - ament_lint_auto - ament_lint_common - ament_cmake_gtest - - - ament_cmake - - diff --git a/recovery_mitigations/easynav_safe_waypoint_recovery/src/easynav_safe_waypoint_recovery/SafeWaypointRecovery.cpp b/recovery_mitigations/easynav_safe_waypoint_recovery/src/easynav_safe_waypoint_recovery/SafeWaypointRecovery.cpp deleted file mode 100644 index 983122f6..00000000 --- a/recovery_mitigations/easynav_safe_waypoint_recovery/src/easynav_safe_waypoint_recovery/SafeWaypointRecovery.cpp +++ /dev/null @@ -1,91 +0,0 @@ -// 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. - -/// \file -/// \brief Implementation of the SafeWaypointRecovery class. - -#include - -#include "easynav_common/RTTFBuffer.hpp" - -#include "easynav_safe_waypoint_recovery/SafeWaypointRecovery.hpp" - -namespace easynav -{ - -void SafeWaypointRecovery::on_initialize() -{ - auto node = get_node(); - const auto & plugin_name = get_plugin_name(); - - node->declare_parameter(plugin_name + ".enabled", enabled_); - node->declare_parameter(plugin_name + ".safe_waypoint.x", x_); - node->declare_parameter(plugin_name + ".safe_waypoint.y", y_); - node->declare_parameter(plugin_name + ".safe_waypoint.yaw", yaw_); - node->declare_parameter(plugin_name + ".safe_waypoint.frame", frame_); - - node->get_parameter(plugin_name + ".enabled", enabled_); - node->get_parameter(plugin_name + ".safe_waypoint.x", x_); - node->get_parameter(plugin_name + ".safe_waypoint.y", y_); - node->get_parameter(plugin_name + ".safe_waypoint.yaw", yaw_); - node->get_parameter(plugin_name + ".safe_waypoint.frame", frame_); - - if (frame_.empty()) { - frame_ = RTTFBuffer::getInstance()->get_tf_info().map_frame; - } - - // Same topic GoalManager already subscribes to for RViz/GUI single-pose goals - // (GoalManager::comanded_pose_callback()) — reused here instead of GoalManagerClient, see - // the class doc comment. - goal_pose_pub_ = node->create_publisher("goal_pose", 10); -} - -bool SafeWaypointRecovery::can_handle(const diagnostic_msgs::msg::DiagnosticStatus & status) const -{ - return enabled_ && status.level >= diagnostic_msgs::msg::DiagnosticStatus::ERROR; -} - -void SafeWaypointRecovery::on_start([[maybe_unused]] NavState & nav_state) -{ - RCLCPP_WARN( - get_node()->get_logger(), - "SafeWaypointRecovery [%s]: redirecting the mission to the safe waypoint " - "(%.2f, %.2f, yaw=%.2f) in frame '%s'", - get_plugin_name().c_str(), x_, y_, yaw_, frame_.c_str()); - - geometry_msgs::msg::PoseStamped goal; - goal.header.stamp = get_node()->now(); - goal.header.frame_id = frame_; - goal.pose.position.x = x_; - goal.pose.position.y = y_; - // Planar (yaw-only) rotation: no need to pull in tf2 for this. - goal.pose.orientation.z = std::sin(yaw_ / 2.0); - goal.pose.orientation.w = std::cos(yaw_ / 2.0); - - goal_pose_pub_->publish(goal); -} - -RecoveryStatus SafeWaypointRecovery::on_cycle([[maybe_unused]] NavState & nav_state) -{ - // The handoff already happened in on_start(): publishing the safe waypoint is a one-shot, - // fire-and-forget action. Whether the robot actually reaches it is ordinary navigation from - // here on (PlannerNode/ControllerNode against the new goal), not something this mitigation - // supervises further. - return RecoveryStatus::SUCCEEDED; -} - -} // namespace easynav - -#include -PLUGINLIB_EXPORT_CLASS(easynav::SafeWaypointRecovery, easynav::RecoveryMitigationBase) diff --git a/recovery_mitigations/easynav_safe_waypoint_recovery/tests/CMakeLists.txt b/recovery_mitigations/easynav_safe_waypoint_recovery/tests/CMakeLists.txt deleted file mode 100644 index 51f066e7..00000000 --- a/recovery_mitigations/easynav_safe_waypoint_recovery/tests/CMakeLists.txt +++ /dev/null @@ -1,2 +0,0 @@ -ament_add_gtest(safe_waypoint_recovery_tests safe_waypoint_recovery_tests.cpp) -target_link_libraries(safe_waypoint_recovery_tests ${PROJECT_NAME}) diff --git a/recovery_mitigations/easynav_safe_waypoint_recovery/tests/safe_waypoint_recovery_tests.cpp b/recovery_mitigations/easynav_safe_waypoint_recovery/tests/safe_waypoint_recovery_tests.cpp deleted file mode 100644 index 8971285c..00000000 --- a/recovery_mitigations/easynav_safe_waypoint_recovery/tests/safe_waypoint_recovery_tests.cpp +++ /dev/null @@ -1,113 +0,0 @@ -// 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 "gtest/gtest.h" - -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" - -#include "easynav_safe_waypoint_recovery/SafeWaypointRecovery.hpp" - -using namespace std::chrono_literals; - -class SafeWaypointRecoveryTestCase : public ::testing::Test -{ -protected: - void SetUp() override - { - if (!rclcpp::ok()) { - rclcpp::init(0, nullptr); - } - } - - std::shared_ptr make_recovery( - const std::shared_ptr & node, const std::string & name) - { - auto rec = std::make_shared(); - rec->initialize(node, name); - return rec; - } -}; - -TEST_F(SafeWaypointRecoveryTestCase, DoesNotRequireControl) -{ - auto node = std::make_shared("test_rc_node"); - auto rec = make_recovery(node, "waypoint0"); - EXPECT_FALSE(rec->requires_control()); -} - -TEST_F(SafeWaypointRecoveryTestCase, DisabledByDefaultCannotHandleAnything) -{ - auto node = std::make_shared("test_disabled_node"); - auto rec = make_recovery(node, "waypoint1"); - - diagnostic_msgs::msg::DiagnosticStatus status; - status.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - EXPECT_FALSE(rec->can_handle(status)); -} - -TEST_F(SafeWaypointRecoveryTestCase, EnabledHandlesAnyErrorOrAboveDiagnostic) -{ - auto node = std::make_shared( - "test_enabled_node", - rclcpp::NodeOptions().parameter_overrides({rclcpp::Parameter("waypoint2.enabled", true)})); - auto rec = make_recovery(node, "waypoint2"); - - diagnostic_msgs::msg::DiagnosticStatus error_status; - error_status.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - EXPECT_TRUE(rec->can_handle(error_status)); - - diagnostic_msgs::msg::DiagnosticStatus warn_status; - warn_status.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - EXPECT_FALSE(rec->can_handle(warn_status)); -} - -TEST_F(SafeWaypointRecoveryTestCase, PublishesTheConfiguredWaypointOnStart) -{ - auto node = std::make_shared( - "test_publish_node", - rclcpp::NodeOptions().parameter_overrides( - { - rclcpp::Parameter("waypoint3.enabled", true), - rclcpp::Parameter("waypoint3.safe_waypoint.x", 1.5), - rclcpp::Parameter("waypoint3.safe_waypoint.y", -2.0), - rclcpp::Parameter("waypoint3.safe_waypoint.frame", "map"), - })); - auto rec = make_recovery(node, "waypoint3"); - - auto listener_node = std::make_shared("test_listener_node"); - geometry_msgs::msg::PoseStamped::SharedPtr received; - auto sub = listener_node->create_subscription( - "goal_pose", 10, - [&received](geometry_msgs::msg::PoseStamped::SharedPtr msg) {received = msg;}); - - rclcpp::executors::SingleThreadedExecutor exe; - exe.add_node(node->get_node_base_interface()); - exe.add_node(listener_node); - - easynav::NavState nav_state; - rec->internal_start(nav_state); - - auto start = listener_node->now(); - while (!received && listener_node->now() - start < 2s) { - exe.spin_some(); - } - - ASSERT_NE(received, nullptr); - EXPECT_EQ(received->header.frame_id, "map"); - EXPECT_DOUBLE_EQ(received->pose.position.x, 1.5); - EXPECT_DOUBLE_EQ(received->pose.position.y, -2.0); - - EXPECT_EQ(rec->internal_cycle(nav_state), easynav::RecoveryStatus::SUCCEEDED); -} From 96adaf31162580ffc993851c70ff43dc6852791f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sat, 12 Sep 2026 12:55:26 +0200 Subject: [PATCH 3/4] Recovery system complete MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../AmclConvergenceEvaluator.hpp | 9 +- .../AmclRelocalizeMitigation.hpp | 14 +- .../AMCLLocalizer.cpp | 2 +- .../AmclRelocalizeMitigation.cpp | 18 +- .../CostmapPlanner.cpp | 12 + .../CMakeLists.txt | 73 +++++ ...nav_controller_stuck_evaluator_plugins.xml | 11 + .../ControllerStuckEvaluator.hpp | 83 ++++++ .../package.xml | 28 ++ .../ControllerStuckEvaluator.cpp | 173 ++++++++++++ .../tests/CMakeLists.txt | 2 + .../controller_stuck_evaluator_tests.cpp | 251 ++++++++++++++++++ .../NoPathEvaluator.hpp | 6 +- .../ObstacleTooCloseEvaluator.hpp | 24 +- .../ObstacleTooCloseEvaluator.cpp | 7 +- .../obstacle_too_close_evaluator_tests.cpp | 8 +- .../easynav_advance_recovery/CMakeLists.txt | 73 +++++ .../easynav_advance_recovery_plugins.xml | 12 + .../AdvanceRecovery.hpp | 92 +++++++ .../easynav_advance_recovery/package.xml | 27 ++ .../AdvanceRecovery.cpp | 116 ++++++++ .../tests/CMakeLists.txt | 2 + .../tests/advance_recovery_tests.cpp | 195 ++++++++++++++ .../CMakeLists.txt | 67 +++++ ...asynav_cancel_mission_recovery_plugins.xml | 12 + .../CancelMissionRecovery.hpp | 61 +++++ .../package.xml | 26 ++ .../CancelMissionRecovery.cpp | 65 +++++ .../tests/CMakeLists.txt | 2 + .../tests/cancel_mission_recovery_tests.cpp | 102 +++++++ .../HumanAssistanceRecovery.hpp | 43 ++- .../HumanAssistanceRecovery.cpp | 48 +++- .../tests/human_assistance_recovery_tests.cpp | 41 +++ .../SafeRetreatRecovery.hpp | 24 +- .../SafeRetreatRecovery.cpp | 18 +- 35 files changed, 1650 insertions(+), 97 deletions(-) create mode 100644 recovery_evaluators/easynav_controller_stuck_evaluator/CMakeLists.txt create mode 100644 recovery_evaluators/easynav_controller_stuck_evaluator/easynav_controller_stuck_evaluator_plugins.xml create mode 100644 recovery_evaluators/easynav_controller_stuck_evaluator/include/easynav_controller_stuck_evaluator/ControllerStuckEvaluator.hpp create mode 100644 recovery_evaluators/easynav_controller_stuck_evaluator/package.xml create mode 100644 recovery_evaluators/easynav_controller_stuck_evaluator/src/easynav_controller_stuck_evaluator/ControllerStuckEvaluator.cpp create mode 100644 recovery_evaluators/easynav_controller_stuck_evaluator/tests/CMakeLists.txt create mode 100644 recovery_evaluators/easynav_controller_stuck_evaluator/tests/controller_stuck_evaluator_tests.cpp create mode 100644 recovery_mitigations/easynav_advance_recovery/CMakeLists.txt create mode 100644 recovery_mitigations/easynav_advance_recovery/easynav_advance_recovery_plugins.xml create mode 100644 recovery_mitigations/easynav_advance_recovery/include/easynav_advance_recovery/AdvanceRecovery.hpp create mode 100644 recovery_mitigations/easynav_advance_recovery/package.xml create mode 100644 recovery_mitigations/easynav_advance_recovery/src/easynav_advance_recovery/AdvanceRecovery.cpp create mode 100644 recovery_mitigations/easynav_advance_recovery/tests/CMakeLists.txt create mode 100644 recovery_mitigations/easynav_advance_recovery/tests/advance_recovery_tests.cpp create mode 100644 recovery_mitigations/easynav_cancel_mission_recovery/CMakeLists.txt create mode 100644 recovery_mitigations/easynav_cancel_mission_recovery/easynav_cancel_mission_recovery_plugins.xml create mode 100644 recovery_mitigations/easynav_cancel_mission_recovery/include/easynav_cancel_mission_recovery/CancelMissionRecovery.hpp create mode 100644 recovery_mitigations/easynav_cancel_mission_recovery/package.xml create mode 100644 recovery_mitigations/easynav_cancel_mission_recovery/src/easynav_cancel_mission_recovery/CancelMissionRecovery.cpp create mode 100644 recovery_mitigations/easynav_cancel_mission_recovery/tests/CMakeLists.txt create mode 100644 recovery_mitigations/easynav_cancel_mission_recovery/tests/cancel_mission_recovery_tests.cpp diff --git a/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AmclConvergenceEvaluator.hpp b/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AmclConvergenceEvaluator.hpp index 37d25c7a..a9ebeaf1 100644 --- a/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AmclConvergenceEvaluator.hpp +++ b/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AmclConvergenceEvaluator.hpp @@ -27,11 +27,10 @@ namespace easynav * @class AmclConvergenceEvaluator * @brief Level-1 recovery evaluator: diagnoses AMCL particle-filter divergence. * - * See docs/recoveries_easynav.md §5.9 — the worked example of "specialized recovery, - * co-located with the component that originates it": only the author of the localizer plugin - * really knows that particle dispersion (here, the trace of the pose covariance AMCLLocalizer - * already computes) is a good indicator of lost convergence, so this evaluator lives in the - * same package as AMCLLocalizer instead of the generic recovery_evaluators catalog. + * Lives in the same package as AMCLLocalizer instead of the generic recovery_evaluators + * catalog: only the author of the localizer plugin really knows that particle dispersion (here, + * the trace of the pose covariance AMCLLocalizer already computes) is a good indicator of lost + * convergence. * * Reads the fixed key "localizer.amcl.covariance_trace" (written by AMCLLocalizer from both * its RT and non-RT cycles) and publishes `hardware_id = "localizer.amcl"`, matched by diff --git a/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AmclRelocalizeMitigation.hpp b/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AmclRelocalizeMitigation.hpp index 8bf739d1..569533b1 100644 --- a/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AmclRelocalizeMitigation.hpp +++ b/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AmclRelocalizeMitigation.hpp @@ -29,14 +29,12 @@ namespace easynav * @class AmclRelocalizeMitigation * @brief Level-1 movement mitigation: rotates in place to help AMCL relocalize. * - * See docs/recoveries_easynav.md §5.9. Selected for diagnostics with - * hardware_id == "localizer.amcl" (the vocabulary shared with AmclConvergenceEvaluator, in the - * same package). Takes control of "cmd_vel" (requires_control() == true) and rotates slowly in - * place each RT cycle, re-checking the same covariance trace the evaluator reads, until it - * drops back under threshold — or until `timeout` elapses without that happening, at which - * point it gives up (stops, returns FAILED) rather than spinning forever. RecoveryManagerNode - * then excludes this mitigation from being reselected for the same diagnostic occurrence (see - * docs/recoveries_easynav_implementation.md, Fase 5). + * Selected for diagnostics with hardware_id == "localizer.amcl" (shared with + * AmclConvergenceEvaluator, in the same package). Takes control of "cmd_vel" + * (requires_control() == true) and rotates slowly in place each RT cycle, re-checking the same + * covariance trace the evaluator reads, until it drops back under threshold — or until `timeout` + * elapses without that happening, at which point it gives up (stops, returns FAILED) rather than + * spinning forever. */ class AmclRelocalizeMitigation : public easynav::RecoveryMitigationBase { diff --git a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp index e762c6b3..06368494 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AMCLLocalizer.cpp @@ -311,7 +311,7 @@ namespace { // Position + yaw dispersion in one scalar, from the 6x6 row-major covariance that get_pose() // already fills (indices 0/7 = var_x/var_y, 35 = var_yaw). Consumed by AmclConvergenceEvaluator -// under the fixed key below — see docs/recoveries_easynav.md §5.9. +// under the fixed key below. double covariance_trace(const nav_msgs::msg::Odometry & odom) { return odom.pose.covariance[0] + odom.pose.covariance[7] + odom.pose.covariance[35]; diff --git a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AmclRelocalizeMitigation.cpp b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AmclRelocalizeMitigation.cpp index 83e633eb..b5c1d71d 100644 --- a/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AmclRelocalizeMitigation.cpp +++ b/localizers/easynav_costmap_localizer/src/easynav_costmap_localizer/AmclRelocalizeMitigation.cpp @@ -45,13 +45,13 @@ bool AmclRelocalizeMitigation::can_handle( status.level >= diagnostic_msgs::msg::DiagnosticStatus::ERROR; } -void AmclRelocalizeMitigation::on_start(NavState &) +void AmclRelocalizeMitigation::on_start(NavState & nav_state) { start_time_ = get_node()->now(); - RCLCPP_WARN( - get_node()->get_logger(), - "AmclRelocalizeMitigation [%s]: localization diverged, rotating in place to relocalize", - get_plugin_name().c_str()); + report( + nav_state, rcl_interfaces::msg::Log::WARN, + "AmclRelocalizeMitigation [" + get_plugin_name() + + "]: localization diverged, rotating in place to relocalize"); } RecoveryStatus AmclRelocalizeMitigation::on_cycle(NavState & nav_state) @@ -68,10 +68,10 @@ RecoveryStatus AmclRelocalizeMitigation::on_cycle(NavState & nav_state) } if ((get_node()->now() - start_time_).seconds() >= timeout_) { - RCLCPP_ERROR( - get_node()->get_logger(), - "AmclRelocalizeMitigation [%s]: gave up after %.2f s without relocalizing", - get_plugin_name().c_str(), timeout_); + report( + nav_state, rcl_interfaces::msg::Log::ERROR, + "AmclRelocalizeMitigation [" + get_plugin_name() + "]: gave up after " + + std::to_string(timeout_) + " s without relocalizing"); stop_robot(nav_state); return RecoveryStatus::FAILED; } diff --git a/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp b/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp index 842b0864..653bfa24 100644 --- a/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp +++ b/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp @@ -146,6 +146,18 @@ void CostmapPlanner::update(NavState & nav_state) const auto & goals = nav_state.get("goals"); if (goals.goals.empty()) { + // No active goal: the path must reflect that too, not keep republishing whatever was last + // computed — otherwise a controller following a stale path toward a cancelled goal would + // never stop on its own. + if (!current_path_.poses.empty()) { + current_path_.poses.clear(); + current_path_.header.stamp = get_node()->now(); + // Also publish the cleared path once, so RViz does not keep showing the last computed + // path frozen on screen after the mission is cancelled. + if (path_pub_->get_subscription_count() > 0) { + path_pub_->publish(current_path_); + } + } nav_state.set("path", current_path_); return; } diff --git a/recovery_evaluators/easynav_controller_stuck_evaluator/CMakeLists.txt b/recovery_evaluators/easynav_controller_stuck_evaluator/CMakeLists.txt new file mode 100644 index 00000000..16486289 --- /dev/null +++ b/recovery_evaluators/easynav_controller_stuck_evaluator/CMakeLists.txt @@ -0,0 +1,73 @@ +cmake_minimum_required(VERSION 3.20) +project(easynav_controller_stuck_evaluator) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(easynav_common REQUIRED) +find_package(easynav_core REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(diagnostic_msgs REQUIRED) + +add_library(${PROJECT_NAME} SHARED + src/easynav_controller_stuck_evaluator/ControllerStuckEvaluator.cpp +) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) +target_link_libraries(${PROJECT_NAME} PUBLIC + easynav_common::easynav_common + easynav_core::easynav_core + pluginlib::pluginlib + rclcpp::rclcpp + ${nav_msgs_TARGETS} + ${geometry_msgs_TARGETS} + ${diagnostic_msgs_TARGETS} +) + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +install(TARGETS + ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + add_subdirectory(tests) +endif() + +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_libraries(${PROJECT_NAME}) +ament_export_targets(export_${PROJECT_NAME}) + +# Register the recovery evaluator plugin +pluginlib_export_plugin_description_file(easynav_core easynav_controller_stuck_evaluator_plugins.xml) + +ament_export_dependencies( + easynav_common + easynav_core + pluginlib + rclcpp + nav_msgs + geometry_msgs + diagnostic_msgs +) +ament_package() diff --git a/recovery_evaluators/easynav_controller_stuck_evaluator/easynav_controller_stuck_evaluator_plugins.xml b/recovery_evaluators/easynav_controller_stuck_evaluator/easynav_controller_stuck_evaluator_plugins.xml new file mode 100644 index 00000000..51ecb1b0 --- /dev/null +++ b/recovery_evaluators/easynav_controller_stuck_evaluator/easynav_controller_stuck_evaluator_plugins.xml @@ -0,0 +1,11 @@ + + + + + Diagnoses a robot commanded to move ("cmd_vel" above threshold) that is not making + progress (robot_pose barely changing). Skips evaluation while a recovery mitigation owns + control_owner, while any safety reflex is intervening, or without an active goal. + + + + diff --git a/recovery_evaluators/easynav_controller_stuck_evaluator/include/easynav_controller_stuck_evaluator/ControllerStuckEvaluator.hpp b/recovery_evaluators/easynav_controller_stuck_evaluator/include/easynav_controller_stuck_evaluator/ControllerStuckEvaluator.hpp new file mode 100644 index 00000000..6f1d8bd2 --- /dev/null +++ b/recovery_evaluators/easynav_controller_stuck_evaluator/include/easynav_controller_stuck_evaluator/ControllerStuckEvaluator.hpp @@ -0,0 +1,83 @@ +// 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. + +/// \file +/// \brief Declaration of the ControllerStuckEvaluator plugin. + +#ifndef EASYNAV_CONTROLLER_STUCK_EVALUATOR__CONTROLLERSTUCKEVALUATOR_HPP_ +#define EASYNAV_CONTROLLER_STUCK_EVALUATOR__CONTROLLERSTUCKEVALUATOR_HPP_ + +#include + +#include "rclcpp/time.hpp" + +#include "easynav_core/RecoveryEvaluatorBase.hpp" + +namespace easynav +{ + +/** + * @class ControllerStuckEvaluator + * @brief Level-1 recovery evaluator: diagnoses "commanded to move, but not making progress". + * + * Compares "robot_pose" across a debounce window while "cmd_vel" commands non-trivial motion; + * if the robot barely moves for long enough, reports ERROR with hardware_id "controller_stuck". + * + * Four preconditions must hold before this evaluator judges anything, each skipping to OK + * otherwise: + * - Navigation must not be paused: the controller plugin keeps writing a non-trivial "cmd_vel" + * into NavState while paused, oblivious to it (only the value actually published to the robot + * is zeroed, elsewhere). Unlike the other preconditions, this one re-arms + * reference_position_/reference_time_ every cycle instead of freezing them, since the robot + * genuinely does not move while paused and a frozen reference would fire a false ERROR the + * instant navigation resumes. + * - "control_owner" must be "controller": an evaluator that watches cmd_vel/the controller must + * not evaluate while a recovery mitigation owns control, or it would self-diagnose the very + * recovery it is part of as a new failure. This one freezes its progress-tracking state + * instead (see the .cpp for why the two preconditions need different treatment). + * - No SafetyReflexBase-derived reflex may currently be intervening — if the robot isn't moving + * because the level-0 reflex is holding it back from a real obstacle, that is not "stuck". + * - There must be an active goal ("goals" non-empty) — without one, nothing was expected to + * make progress in the first place. + */ +class ControllerStuckEvaluator : public easynav::RecoveryEvaluatorBase +{ +public: + ControllerStuckEvaluator() = default; + ~ControllerStuckEvaluator() = default; + + void on_initialize() override; + +protected: + void update(NavState & nav_state) override; + +private: + /// @brief Below this commanded linear speed (m/s), the robot is not considered "commanded to + /// move" at all. + double linear_velocity_threshold_ {0.02}; + + /// @brief Minimum displacement (m) since the reference position to count as "progress". + double progress_distance_threshold_ {0.05}; + + /// @brief Seconds without progress, while commanded to move, before reporting ERROR. + double stuck_time_threshold_ {2.0}; + + /// @brief Last position considered "progress" (x, y), and when it was recorded. + std::optional> reference_position_; + rclcpp::Time reference_time_; +}; + +} // namespace easynav + +#endif // EASYNAV_CONTROLLER_STUCK_EVALUATOR__CONTROLLERSTUCKEVALUATOR_HPP_ diff --git a/recovery_evaluators/easynav_controller_stuck_evaluator/package.xml b/recovery_evaluators/easynav_controller_stuck_evaluator/package.xml new file mode 100644 index 00000000..f94d1f2e --- /dev/null +++ b/recovery_evaluators/easynav_controller_stuck_evaluator/package.xml @@ -0,0 +1,28 @@ + + + + easynav_controller_stuck_evaluator + 0.4.2 + Easy Navigation: recovery evaluator that diagnoses a robot commanded to move but not making progress. + Francisco Martín Rico + Apache-2.0 + + ament_cmake + + easynav_common + easynav_core + pluginlib + rclcpp + nav_msgs + geometry_msgs + diagnostic_msgs + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + rclcpp_lifecycle + + + ament_cmake + + diff --git a/recovery_evaluators/easynav_controller_stuck_evaluator/src/easynav_controller_stuck_evaluator/ControllerStuckEvaluator.cpp b/recovery_evaluators/easynav_controller_stuck_evaluator/src/easynav_controller_stuck_evaluator/ControllerStuckEvaluator.cpp new file mode 100644 index 00000000..aa2dc919 --- /dev/null +++ b/recovery_evaluators/easynav_controller_stuck_evaluator/src/easynav_controller_stuck_evaluator/ControllerStuckEvaluator.cpp @@ -0,0 +1,173 @@ +// 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. + +/// \file +/// \brief Implementation of the ControllerStuckEvaluator class. + +#include +#include + +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/goals.hpp" +#include "nav_msgs/msg/odometry.hpp" + +#include "easynav_controller_stuck_evaluator/ControllerStuckEvaluator.hpp" + +namespace easynav +{ + +void ControllerStuckEvaluator::on_initialize() +{ + auto node = get_node(); + const auto & plugin_name = get_plugin_name(); + + node->declare_parameter( + plugin_name + ".linear_velocity_threshold", linear_velocity_threshold_); + node->declare_parameter( + plugin_name + ".progress_distance_threshold", progress_distance_threshold_); + node->declare_parameter(plugin_name + ".stuck_time_threshold", stuck_time_threshold_); + + node->get_parameter( + plugin_name + ".linear_velocity_threshold", linear_velocity_threshold_); + node->get_parameter( + plugin_name + ".progress_distance_threshold", progress_distance_threshold_); + node->get_parameter(plugin_name + ".stuck_time_threshold", stuck_time_threshold_); +} + +void ControllerStuckEvaluator::update(NavState & nav_state) +{ + diagnostic_msgs::msg::DiagnosticStatus status; + status.name = get_plugin_name(); + status.hardware_id = "controller_stuck"; + status.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + status.message = "making progress"; + + // While paused, the controller plugin keeps writing a non-trivial "cmd_vel" into NavState as + // if still navigating (only the value actually published to the robot is zeroed elsewhere). + // Unlike the control_owner freeze below, freezing the reference here would not be enough: the + // robot genuinely does not move while paused, so a frozen reference_time_ would already be + // older than stuck_time_threshold_ once navigation resumes, firing an immediate false ERROR. + // Re-arm the reference every cycle instead, so a full window of real non-progress is required + // again after resuming. + if (nav_state.has("navigation_paused") && nav_state.get("navigation_paused")) { + status.message = "navigation paused"; + if (nav_state.has("robot_pose")) { + const auto odom = nav_state.get_safe("robot_pose"); + reference_position_ = {odom.pose.pose.position.x, odom.pose.pose.position.y}; + } else { + reference_position_.reset(); + } + reference_time_ = get_node()->now(); + publish_diagnostic(nav_state, status); + return; + } + + // An evaluator that watches cmd_vel/the controller must not evaluate while a recovery + // mitigation owns control_owner, or it would self-diagnose the recovery itself as a new + // failure. Freeze reference_position_ while this holds, so the first cycle back under + // "controller" compares against a possibly-stale reference — any real movement made by the + // mitigation already counts as progress then. + if (nav_state.has("control_owner") && + nav_state.get("control_owner") != "controller") + { + status.message = "a recovery mitigation owns control_owner"; + publish_diagnostic(nav_state, status); + return; + } + + // Do not compete with (or second-guess) a level-0 safety reflex: if the robot isn't moving + // because a reflex is holding it back from something real, that is not "stuck". + for (const auto & key : nav_state.get_group_keys("diagnostics")) { + if (!nav_state.has(key)) {continue;} + const auto & reflex_status = nav_state.get(key); + if (reflex_status.hardware_id == "safety_reflex" && + reflex_status.level != diagnostic_msgs::msg::DiagnosticStatus::OK) + { + status.message = "a safety reflex is intervening"; + publish_diagnostic(nav_state, status); + return; + } + } + + // Without an active goal, nothing was expected to make progress in the first place. + const bool has_active_goal = nav_state.has("goals") && + !nav_state.get("goals").goals.empty(); + if (!has_active_goal) { + status.message = "no active goal"; + publish_diagnostic(nav_state, status); + return; + } + + if (!nav_state.has("cmd_vel")) { + status.message = "no cmd_vel yet"; + publish_diagnostic(nav_state, status); + return; + } + + // "cmd_vel" and "robot_pose" are written from the RT cycle; this evaluator runs on + // RecoveryManagerNode's non-RT cycle, so get_safe() (a snapshot copy) is required for both, + // not get(). See NavState's own get()/get_safe() guidance. + const auto cmd = nav_state.get_safe("cmd_vel"); + const double commanded_speed = std::hypot(cmd.twist.linear.x, cmd.twist.linear.y); + if (commanded_speed < linear_velocity_threshold_) { + status.message = "not commanded to move"; + publish_diagnostic(nav_state, status); + return; + } + + if (!nav_state.has("robot_pose")) { + status.message = "no robot_pose yet"; + publish_diagnostic(nav_state, status); + return; + } + + const auto odom = nav_state.get_safe("robot_pose"); + const double x = odom.pose.pose.position.x; + const double y = odom.pose.pose.position.y; + const rclcpp::Time now = get_node()->now(); + + if (!reference_position_.has_value()) { + reference_position_ = {x, y}; + reference_time_ = now; + publish_diagnostic(nav_state, status); + return; + } + + const double dx = x - reference_position_->first; + const double dy = y - reference_position_->second; + if (std::hypot(dx, dy) > progress_distance_threshold_) { + reference_position_ = {x, y}; + reference_time_ = now; + publish_diagnostic(nav_state, status); + return; + } + + const double stuck_for = (now - reference_time_).seconds(); + if (stuck_for >= stuck_time_threshold_) { + status.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + status.message = "commanded to move but not making progress"; + + diagnostic_msgs::msg::KeyValue duration_kv; + duration_kv.key = "stuck_duration"; + duration_kv.value = std::to_string(stuck_for); + status.values.push_back(duration_kv); + } + + publish_diagnostic(nav_state, status); +} + +} // namespace easynav + +#include +PLUGINLIB_EXPORT_CLASS(easynav::ControllerStuckEvaluator, easynav::RecoveryEvaluatorBase) diff --git a/recovery_evaluators/easynav_controller_stuck_evaluator/tests/CMakeLists.txt b/recovery_evaluators/easynav_controller_stuck_evaluator/tests/CMakeLists.txt new file mode 100644 index 00000000..04231eaa --- /dev/null +++ b/recovery_evaluators/easynav_controller_stuck_evaluator/tests/CMakeLists.txt @@ -0,0 +1,2 @@ +ament_add_gtest(controller_stuck_evaluator_tests controller_stuck_evaluator_tests.cpp) +target_link_libraries(controller_stuck_evaluator_tests ${PROJECT_NAME}) diff --git a/recovery_evaluators/easynav_controller_stuck_evaluator/tests/controller_stuck_evaluator_tests.cpp b/recovery_evaluators/easynav_controller_stuck_evaluator/tests/controller_stuck_evaluator_tests.cpp new file mode 100644 index 00000000..e28ee2f9 --- /dev/null +++ b/recovery_evaluators/easynav_controller_stuck_evaluator/tests/controller_stuck_evaluator_tests.cpp @@ -0,0 +1,251 @@ +// 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 "gtest/gtest.h" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/goals.hpp" +#include "nav_msgs/msg/odometry.hpp" + +#include "easynav_controller_stuck_evaluator/ControllerStuckEvaluator.hpp" + +class ControllerStuckEvaluatorTestCase : public ::testing::Test +{ +protected: + void SetUp() override + { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + } + + std::shared_ptr make_ready_evaluator( + const std::shared_ptr & node, const std::string & name) + { + auto eval = std::make_shared(); + eval->initialize(node, name); + std::this_thread::sleep_for(std::chrono::milliseconds(120)); + return eval; + } + + static void set_active_goal(easynav::NavState & nav_state) + { + nav_msgs::msg::Goals goals; + goals.goals.push_back(geometry_msgs::msg::PoseStamped()); + nav_state.set("goals", goals); + } + + static void set_commanded_motion(easynav::NavState & nav_state, double linear_x = 0.3) + { + geometry_msgs::msg::TwistStamped cmd; + cmd.twist.linear.x = linear_x; + nav_state.set("cmd_vel", cmd); + } + + static void set_robot_position(easynav::NavState & nav_state, double x, double y) + { + nav_msgs::msg::Odometry odom; + odom.pose.pose.position.x = x; + odom.pose.pose.position.y = y; + nav_state.set("robot_pose", odom); + } +}; + +TEST_F(ControllerStuckEvaluatorTestCase, OkWithoutActiveGoal) +{ + auto node = std::make_shared("test_no_goal_node"); + auto eval = make_ready_evaluator(node, "stuck0"); + + easynav::NavState nav_state; + set_commanded_motion(nav_state); + set_robot_position(nav_state, 0.0, 0.0); + eval->internal_update(nav_state); + + EXPECT_EQ( + nav_state.get("diagnostics.stuck0").level, + diagnostic_msgs::msg::DiagnosticStatus::OK); +} + +TEST_F(ControllerStuckEvaluatorTestCase, OkWhenControlOwnerIsNotController) +{ + auto node = std::make_shared("test_owner_node"); + auto eval = make_ready_evaluator(node, "stuck1"); + + easynav::NavState nav_state; + set_active_goal(nav_state); + set_commanded_motion(nav_state); + set_robot_position(nav_state, 0.0, 0.0); + nav_state.set("control_owner", std::string("recovery:retreat")); + eval->internal_update(nav_state); + + EXPECT_EQ( + nav_state.get("diagnostics.stuck1").level, + diagnostic_msgs::msg::DiagnosticStatus::OK); +} + +TEST_F(ControllerStuckEvaluatorTestCase, OkWhenSafetyReflexIsIntervening) +{ + auto node = std::make_shared("test_reflex_node"); + auto eval = make_ready_evaluator(node, "stuck2"); + + easynav::NavState nav_state; + set_active_goal(nav_state); + set_commanded_motion(nav_state); + set_robot_position(nav_state, 0.0, 0.0); + + diagnostic_msgs::msg::DiagnosticStatus reflex_status; + reflex_status.hardware_id = "safety_reflex"; + reflex_status.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + nav_state.set("diagnostics.collision", reflex_status); + nav_state.set_group("diagnostics", {"diagnostics.collision"}); + + eval->internal_update(nav_state); + + EXPECT_EQ( + nav_state.get("diagnostics.stuck2").level, + diagnostic_msgs::msg::DiagnosticStatus::OK); +} + +TEST_F(ControllerStuckEvaluatorTestCase, OkWhenNotCommandedToMove) +{ + auto node = std::make_shared("test_no_cmd_node"); + auto eval = make_ready_evaluator(node, "stuck3"); + + easynav::NavState nav_state; + set_active_goal(nav_state); + set_commanded_motion(nav_state, 0.0); + set_robot_position(nav_state, 0.0, 0.0); + eval->internal_update(nav_state); + + EXPECT_EQ( + nav_state.get("diagnostics.stuck3").level, + diagnostic_msgs::msg::DiagnosticStatus::OK); +} + +TEST_F(ControllerStuckEvaluatorTestCase, OkWhileMakingProgress) +{ + auto node = std::make_shared( + "test_progress_node", + rclcpp::NodeOptions().append_parameter_override("stuck4.stuck_time_threshold", 0.05)); + auto eval = make_ready_evaluator(node, "stuck4"); + + easynav::NavState nav_state; + set_active_goal(nav_state); + set_commanded_motion(nav_state); + + for (double x = 0.0; x < 0.5; x += 0.2) { + set_robot_position(nav_state, x, 0.0); + eval->internal_update(nav_state); + std::this_thread::sleep_for(std::chrono::milliseconds(60)); + } + + EXPECT_EQ( + nav_state.get("diagnostics.stuck4").level, + diagnostic_msgs::msg::DiagnosticStatus::OK); +} + +TEST_F(ControllerStuckEvaluatorTestCase, OkWhileNavigationPaused) +{ + auto node = std::make_shared( + "test_paused_node", + rclcpp::NodeOptions().append_parameter_override("stuck6.stuck_time_threshold", 0.05)); + auto eval = make_ready_evaluator(node, "stuck6"); + + easynav::NavState nav_state; + set_active_goal(nav_state); + set_commanded_motion(nav_state); + set_robot_position(nav_state, 1.0, 1.0); + nav_state.set("navigation_paused", true); + + eval->internal_update(nav_state); // reference position established, still OK + ASSERT_EQ( + nav_state.get("diagnostics.stuck6").level, + diagnostic_msgs::msg::DiagnosticStatus::OK); + + // Long enough to trip stuck_time_threshold_ if the reference were frozen instead of re-armed. + std::this_thread::sleep_for(std::chrono::milliseconds(60)); + eval->internal_update(nav_state); // still paused, same position: must stay OK + + const auto & status = + nav_state.get("diagnostics.stuck6"); + EXPECT_EQ(status.level, diagnostic_msgs::msg::DiagnosticStatus::OK); + EXPECT_EQ(status.message, "navigation paused"); +} + +TEST_F(ControllerStuckEvaluatorTestCase, OkImmediatelyAfterResumingFromPause) +{ + auto node = std::make_shared( + "test_resume_node", + rclcpp::NodeOptions().append_parameter_override("stuck7.stuck_time_threshold", 0.05)); + auto eval = make_ready_evaluator(node, "stuck7"); + + easynav::NavState nav_state; + set_active_goal(nav_state); + set_commanded_motion(nav_state); + set_robot_position(nav_state, 1.0, 1.0); + nav_state.set("navigation_paused", true); + + eval->internal_update(nav_state); // reference established while paused + + // Elapse (while still paused) past what would be stuck_time_threshold_ if the reference had + // been frozen instead of re-armed each cycle. + std::this_thread::sleep_for(std::chrono::milliseconds(60)); + eval->internal_update(nav_state); + + // Resume: same position (robot has not moved yet), but the pause re-armed the reference on + // the last paused cycle, so this must not immediately report stuck. + nav_state.set("navigation_paused", false); + eval->internal_update(nav_state); + + EXPECT_EQ( + nav_state.get("diagnostics.stuck7").level, + diagnostic_msgs::msg::DiagnosticStatus::OK); +} + +TEST_F(ControllerStuckEvaluatorTestCase, ErrorAfterNotMovingLongEnough) +{ + auto node = std::make_shared( + "test_error_node", + rclcpp::NodeOptions() + .append_parameter_override("stuck5.stuck_time_threshold", 0.05) + .append_parameter_override("stuck5.freq", 200.0)); + auto eval = make_ready_evaluator(node, "stuck5"); + + easynav::NavState nav_state; + set_active_goal(nav_state); + set_commanded_motion(nav_state); + set_robot_position(nav_state, 1.0, 1.0); + + eval->internal_update(nav_state); // establishes the reference position, still OK + ASSERT_EQ( + nav_state.get("diagnostics.stuck5").level, + diagnostic_msgs::msg::DiagnosticStatus::OK); + + std::this_thread::sleep_for(std::chrono::milliseconds(60)); // past the 50 ms debounce + eval->internal_update(nav_state); // same position: stuck + + const auto & status = + nav_state.get("diagnostics.stuck5"); + EXPECT_EQ(status.level, diagnostic_msgs::msg::DiagnosticStatus::ERROR); + EXPECT_EQ(status.hardware_id, "controller_stuck"); + ASSERT_EQ(status.values.size(), 1u); + EXPECT_EQ(status.values[0].key, "stuck_duration"); +} diff --git a/recovery_evaluators/easynav_no_path_evaluator/include/easynav_no_path_evaluator/NoPathEvaluator.hpp b/recovery_evaluators/easynav_no_path_evaluator/include/easynav_no_path_evaluator/NoPathEvaluator.hpp index e004ac97..9aff295b 100644 --- a/recovery_evaluators/easynav_no_path_evaluator/include/easynav_no_path_evaluator/NoPathEvaluator.hpp +++ b/recovery_evaluators/easynav_no_path_evaluator/include/easynav_no_path_evaluator/NoPathEvaluator.hpp @@ -27,9 +27,9 @@ namespace easynav * @class NoPathEvaluator * @brief Level-1 recovery evaluator that diagnoses a missing or empty planner path. * - * See docs/recoveries_easynav.md, level 1, evaluator catalog. A generic, domain-agnostic - * evaluator: it only knows about the "path" key that any PlannerMethodBase-derived plugin is - * expected to produce, not about any specific planner's internals. + * A generic, domain-agnostic evaluator: it only knows about the "path" key that any + * PlannerMethodBase-derived plugin is expected to produce, not about any specific planner's + * internals. */ class NoPathEvaluator : public easynav::RecoveryEvaluatorBase { diff --git a/recovery_evaluators/easynav_obstacle_too_close_evaluator/include/easynav_obstacle_too_close_evaluator/ObstacleTooCloseEvaluator.hpp b/recovery_evaluators/easynav_obstacle_too_close_evaluator/include/easynav_obstacle_too_close_evaluator/ObstacleTooCloseEvaluator.hpp index 28d036fd..7fa9f03d 100644 --- a/recovery_evaluators/easynav_obstacle_too_close_evaluator/include/easynav_obstacle_too_close_evaluator/ObstacleTooCloseEvaluator.hpp +++ b/recovery_evaluators/easynav_obstacle_too_close_evaluator/include/easynav_obstacle_too_close_evaluator/ObstacleTooCloseEvaluator.hpp @@ -31,20 +31,18 @@ namespace easynav * @class ObstacleTooCloseEvaluator * @brief Level-1 recovery evaluator: diagnoses "stopped too close to an obstacle". * - * See docs/recoveries_easynav.md §5.2/§5.13. This is deliberately a *compound* condition, not - * just "is something close": it requires the robot to already be (near) stationary before - * reporting ERROR. Rationale — the RT-level CollisionSafetyReflex reacts first and stops the - * robot; this evaluator must not fire while that stop is still happening (still decelerating), - * or a movement mitigation like SafeRetreatRecovery could take over mid-brake and substitute an - * unsafe motion for a controlled one. Rather than coupling to the reflex's internal state, the - * "already stopped" condition is checked against an independent, physically measured signal - * (the robot's own commanded/estimated velocity from "robot_pose"), so this evaluator works - * correctly regardless of *why* the robot stopped. + * Deliberately a *compound* condition, not just "is something close": it requires the robot to + * already be (near) stationary before reporting ERROR. The RT-level CollisionSafetyReflex reacts + * first and stops the robot; this evaluator must not fire while that stop is still happening + * (still decelerating), or a movement mitigation like SafeRetreatRecovery could take over + * mid-brake and substitute an unsafe motion for a controlled one. Rather than coupling to the + * reflex's internal state, "already stopped" is checked against an independent, physically + * measured signal (the robot's own velocity from "robot_pose"), so this works regardless of + * *why* the robot stopped. * - * Per §5.2, "stopped" must also be *sustained* for a short debounce window before it is - * trusted: the RT and non-RT cycles run in parallel, so a single low-velocity sample could - * still be taken mid-brake. While within that window this evaluator reports OK (informational, - * matched by no mitigator's can_handle()), not yet the real proximity check. + * "Stopped" must also be *sustained* for a short debounce window before it is trusted: the RT + * and non-RT cycles run in parallel, so a single low-velocity sample could still be taken + * mid-brake. Within that window this evaluator reports OK, not yet the real proximity check. */ class ObstacleTooCloseEvaluator : public easynav::RecoveryEvaluatorBase { diff --git a/recovery_evaluators/easynav_obstacle_too_close_evaluator/src/easynav_obstacle_too_close_evaluator/ObstacleTooCloseEvaluator.cpp b/recovery_evaluators/easynav_obstacle_too_close_evaluator/src/easynav_obstacle_too_close_evaluator/ObstacleTooCloseEvaluator.cpp index 0e380890..bc1ece62 100644 --- a/recovery_evaluators/easynav_obstacle_too_close_evaluator/src/easynav_obstacle_too_close_evaluator/ObstacleTooCloseEvaluator.cpp +++ b/recovery_evaluators/easynav_obstacle_too_close_evaluator/src/easynav_obstacle_too_close_evaluator/ObstacleTooCloseEvaluator.cpp @@ -88,10 +88,9 @@ void ObstacleTooCloseEvaluator::update(NavState & nav_state) const double stopped_for = (get_node()->now() - *stopped_since_).seconds(); if (stopped_for < debounce_duration_) { - // §5.2: "stopped" must be sustained for a short debounce window before it is trusted — the - // RT and non-RT cycles run in parallel, so a single low-velocity sample could still be - // taken mid-brake. Report as OK (informational, no mitigator's can_handle() matches it) - // rather than evaluating proximity yet. + // "Stopped" must be sustained for a short debounce window before it is trusted — the RT + // and non-RT cycles run in parallel, so a single low-velocity sample could still be taken + // mid-brake. status.message = "recently stopped, confirming before evaluating proximity"; publish_diagnostic(nav_state, status); return; diff --git a/recovery_evaluators/easynav_obstacle_too_close_evaluator/tests/obstacle_too_close_evaluator_tests.cpp b/recovery_evaluators/easynav_obstacle_too_close_evaluator/tests/obstacle_too_close_evaluator_tests.cpp index 736d066c..a53a5e74 100644 --- a/recovery_evaluators/easynav_obstacle_too_close_evaluator/tests/obstacle_too_close_evaluator_tests.cpp +++ b/recovery_evaluators/easynav_obstacle_too_close_evaluator/tests/obstacle_too_close_evaluator_tests.cpp @@ -86,7 +86,7 @@ TEST_F(ObstacleTooCloseEvaluatorTestCase, OkWithoutRobotPose) TEST_F(ObstacleTooCloseEvaluatorTestCase, OkWhileStillMovingEvenIfObstacleIsClose) { // Compound condition: must not fire while the robot is still moving (e.g. the level-0 reflex - // is still braking). See docs/recoveries_easynav.md, §5.2. + // is still braking). auto node = std::make_shared("test_moving_node"); auto eval = make_ready_evaluator(node, "close2"); @@ -142,9 +142,9 @@ TEST_F(ObstacleTooCloseEvaluatorTestCase, ErrorWhenStoppedTooCloseToAnObstacle) } // --------------------------------------------------------------------------- -// Debounce window (§5.2): "stopped" must be sustained for a short interval before it is -// trusted, so a single low-velocity sample taken mid-brake (RT and non-RT cycles run in -// parallel) cannot be mistaken for "already stopped". +// Debounce window: "stopped" must be sustained for a short interval before it is trusted, so a +// single low-velocity sample taken mid-brake (RT and non-RT cycles run in parallel) cannot be +// mistaken for "already stopped". // --------------------------------------------------------------------------- TEST_F(ObstacleTooCloseEvaluatorTestCase, RemainsOkWithinDebounceWindowEvenIfObstacleIsClose) diff --git a/recovery_mitigations/easynav_advance_recovery/CMakeLists.txt b/recovery_mitigations/easynav_advance_recovery/CMakeLists.txt new file mode 100644 index 00000000..b4db72ab --- /dev/null +++ b/recovery_mitigations/easynav_advance_recovery/CMakeLists.txt @@ -0,0 +1,73 @@ +cmake_minimum_required(VERSION 3.20) +project(easynav_advance_recovery) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(easynav_common REQUIRED) +find_package(easynav_core REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) +find_package(diagnostic_msgs REQUIRED) + +add_library(${PROJECT_NAME} SHARED + src/easynav_advance_recovery/AdvanceRecovery.cpp +) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) +target_link_libraries(${PROJECT_NAME} PUBLIC + easynav_common::easynav_common + easynav_core::easynav_core + pluginlib::pluginlib + rclcpp::rclcpp + ${geometry_msgs_TARGETS} + ${nav_msgs_TARGETS} + ${diagnostic_msgs_TARGETS} +) + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +install(TARGETS + ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + add_subdirectory(tests) +endif() + +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_libraries(${PROJECT_NAME}) +ament_export_targets(export_${PROJECT_NAME}) + +# Register the recovery mitigation plugin +pluginlib_export_plugin_description_file(easynav_core easynav_advance_recovery_plugins.xml) + +ament_export_dependencies( + easynav_common + easynav_core + pluginlib + rclcpp + geometry_msgs + nav_msgs + diagnostic_msgs +) +ament_package() diff --git a/recovery_mitigations/easynav_advance_recovery/easynav_advance_recovery_plugins.xml b/recovery_mitigations/easynav_advance_recovery/easynav_advance_recovery_plugins.xml new file mode 100644 index 00000000..1a1a64e6 --- /dev/null +++ b/recovery_mitigations/easynav_advance_recovery/easynav_advance_recovery_plugins.xml @@ -0,0 +1,12 @@ + + + + + Advances a short configured distance for diagnostics with hardware_id + "controller_stuck" (ControllerStuckEvaluator). Never reports success as "fixed" — each + activation just completes one advance; if the same episode keeps recurring for too long + in total, it gives up and escalates instead of retrying forever. + + + + diff --git a/recovery_mitigations/easynav_advance_recovery/include/easynav_advance_recovery/AdvanceRecovery.hpp b/recovery_mitigations/easynav_advance_recovery/include/easynav_advance_recovery/AdvanceRecovery.hpp new file mode 100644 index 00000000..24538810 --- /dev/null +++ b/recovery_mitigations/easynav_advance_recovery/include/easynav_advance_recovery/AdvanceRecovery.hpp @@ -0,0 +1,92 @@ +// 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. + +/// \file +/// \brief Declaration of the AdvanceRecovery plugin. + +#ifndef EASYNAV_ADVANCE_RECOVERY__ADVANCERECOVERY_HPP_ +#define EASYNAV_ADVANCE_RECOVERY__ADVANCERECOVERY_HPP_ + +#include + +#include "rclcpp/time.hpp" + +#include "easynav_core/RecoveryMitigationBase.hpp" + +namespace easynav +{ + +/** + * @class AdvanceRecovery + * @brief Level-1 movement mitigation: advances a short distance when the robot is stuck. + * + * Selected for diagnostics with hardware_id == "controller_stuck". Takes control of "cmd_vel" + * (requires_control() == true) and commands a slow, straight-forward motion — same single gate + * as any other producer of "cmd_vel" (CollisionSafetyReflex), so "only if there is no obstacle" + * is enforced by the level-0 reflex, not duplicated here. + * + * Deliberately never reports SUCCEEDED as "the stuck condition is fixed": completing one + * advance only means this activation is done, not that the underlying cause is gone. If it + * recurs, ControllerStuckEvaluator will simply diagnose it again and this mitigation activates + * again — by design, "advances a bit" repeatedly for as long as the problem keeps reappearing. + * It only gives up (FAILED, so RecoveryManagerNode escalates to the next candidate) once the + * *total* time spent on this recurring episode — summed across every activation — exceeds + * "escalate_after". A long enough gap between activations resets that clock: see on_start(). + */ +class AdvanceRecovery : public easynav::RecoveryMitigationBase +{ +public: + AdvanceRecovery() = default; + ~AdvanceRecovery() = 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; + RecoveryStatus on_cycle(NavState & nav_state) override; + void on_stop(NavState & nav_state) override; + +private: + /// @brief Distance (m) to advance before considering one activation complete. + double advance_distance_ {0.3}; + + /// @brief Forward linear speed commanded while advancing (m/s, "cautious"). + double advance_speed_ {0.1}; + + /// @brief Total time (s), summed across activations of the same episode, before giving up. + double escalate_after_ {15.0}; + + /// @brief Gap (s) since this mitigation last stopped beyond which the next activation is + /// treated as a new episode instead of a continuation (resets the escalation clock). + double episode_gap_ {10.0}; + + /// @brief When the current episode started (first activation, or the first one after a gap + /// longer than episode_gap_). Reset once escalated. + std::optional episode_start_; + + /// @brief When this mitigation last stopped (SUCCEEDED or FAILED), to measure the gap in the + /// next on_start(). + std::optional last_stop_time_; + + /// @brief Robot position (x, y) when the current activation started, to measure this + /// activation's own advance distance. + std::pair start_position_ {0.0, 0.0}; +}; + +} // namespace easynav + +#endif // EASYNAV_ADVANCE_RECOVERY__ADVANCERECOVERY_HPP_ diff --git a/recovery_mitigations/easynav_advance_recovery/package.xml b/recovery_mitigations/easynav_advance_recovery/package.xml new file mode 100644 index 00000000..17ff4c01 --- /dev/null +++ b/recovery_mitigations/easynav_advance_recovery/package.xml @@ -0,0 +1,27 @@ + + + + easynav_advance_recovery + 0.4.2 + Easy Navigation: recovery mitigation that advances a short configured distance when the robot is commanded to move but stuck. + Francisco Martín Rico + Apache-2.0 + + ament_cmake + + easynav_common + easynav_core + pluginlib + rclcpp + geometry_msgs + nav_msgs + diagnostic_msgs + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + + + ament_cmake + + diff --git a/recovery_mitigations/easynav_advance_recovery/src/easynav_advance_recovery/AdvanceRecovery.cpp b/recovery_mitigations/easynav_advance_recovery/src/easynav_advance_recovery/AdvanceRecovery.cpp new file mode 100644 index 00000000..608b5ef4 --- /dev/null +++ b/recovery_mitigations/easynav_advance_recovery/src/easynav_advance_recovery/AdvanceRecovery.cpp @@ -0,0 +1,116 @@ +// 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. + +/// \file +/// \brief Implementation of the AdvanceRecovery class. + +#include + +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" + +#include "easynav_common/RTTFBuffer.hpp" + +#include "easynav_advance_recovery/AdvanceRecovery.hpp" + +namespace easynav +{ + +void AdvanceRecovery::on_initialize() +{ + auto node = get_node(); + const auto & plugin_name = get_plugin_name(); + + node->declare_parameter(plugin_name + ".advance_distance", advance_distance_); + node->declare_parameter(plugin_name + ".advance_speed", advance_speed_); + node->declare_parameter(plugin_name + ".escalate_after", escalate_after_); + node->declare_parameter(plugin_name + ".episode_gap", episode_gap_); + + node->get_parameter(plugin_name + ".advance_distance", advance_distance_); + node->get_parameter(plugin_name + ".advance_speed", advance_speed_); + node->get_parameter(plugin_name + ".escalate_after", escalate_after_); + node->get_parameter(plugin_name + ".episode_gap", episode_gap_); +} + +bool AdvanceRecovery::can_handle(const diagnostic_msgs::msg::DiagnosticStatus & status) const +{ + return status.hardware_id == "controller_stuck" && + status.level >= diagnostic_msgs::msg::DiagnosticStatus::ERROR; +} + +void AdvanceRecovery::on_start(NavState & nav_state) +{ + const rclcpp::Time now = get_node()->now(); + + if (!episode_start_.has_value() || + (last_stop_time_.has_value() && (now - *last_stop_time_).seconds() > episode_gap_)) + { + // First activation ever, or a long enough gap since we last stopped that this is a new, + // unrelated stuck episode rather than a continuation of the previous one. + episode_start_ = now; + } + + const auto odom = nav_state.get_safe("robot_pose"); + start_position_ = {odom.pose.pose.position.x, odom.pose.pose.position.y}; + + report( + nav_state, rcl_interfaces::msg::Log::WARN, + "AdvanceRecovery [" + get_plugin_name() + "]: robot commanded to move but stuck, advancing " + + std::to_string(advance_distance_) + " m"); +} + +RecoveryStatus AdvanceRecovery::on_cycle(NavState & nav_state) +{ + if ((get_node()->now() - *episode_start_).seconds() >= escalate_after_) { + report( + nav_state, rcl_interfaces::msg::Log::ERROR, + "AdvanceRecovery [" + get_plugin_name() + "]: stuck episode has lasted over " + + std::to_string(escalate_after_) + " s in total, giving up"); + episode_start_.reset(); + stop_robot(nav_state); + return RecoveryStatus::FAILED; + } + + const auto odom = nav_state.get_safe("robot_pose"); + const double dx = odom.pose.pose.position.x - start_position_.first; + const double dy = odom.pose.pose.position.y - start_position_.second; + + if (std::hypot(dx, dy) >= advance_distance_) { + // This advance is done — deliberately NOT claiming the stuck condition itself is fixed: + // episode_start_ is left untouched so a quick reactivation keeps counting toward + // escalate_after_. See the class doc comment. + stop_robot(nav_state); + return RecoveryStatus::SUCCEEDED; + } + + geometry_msgs::msg::TwistStamped cmd; + if (auto node = get_node()) { + cmd.header.stamp = node->now(); + } + cmd.header.frame_id = RTTFBuffer::getInstance()->get_tf_info().robot_frame; + cmd.twist.linear.x = advance_speed_; + + nav_state.set("cmd_vel", cmd); + return RecoveryStatus::RUNNING; +} + +void AdvanceRecovery::on_stop(NavState &) +{ + last_stop_time_ = get_node()->now(); +} + +} // namespace easynav + +#include +PLUGINLIB_EXPORT_CLASS(easynav::AdvanceRecovery, easynav::RecoveryMitigationBase) diff --git a/recovery_mitigations/easynav_advance_recovery/tests/CMakeLists.txt b/recovery_mitigations/easynav_advance_recovery/tests/CMakeLists.txt new file mode 100644 index 00000000..b44a4d18 --- /dev/null +++ b/recovery_mitigations/easynav_advance_recovery/tests/CMakeLists.txt @@ -0,0 +1,2 @@ +ament_add_gtest(advance_recovery_tests advance_recovery_tests.cpp) +target_link_libraries(advance_recovery_tests ${PROJECT_NAME}) diff --git a/recovery_mitigations/easynav_advance_recovery/tests/advance_recovery_tests.cpp b/recovery_mitigations/easynav_advance_recovery/tests/advance_recovery_tests.cpp new file mode 100644 index 00000000..f4bf8ca0 --- /dev/null +++ b/recovery_mitigations/easynav_advance_recovery/tests/advance_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 "gtest/gtest.h" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "easynav_common/RTTFBuffer.hpp" + +#include "easynav_advance_recovery/AdvanceRecovery.hpp" + +class AdvanceRecoveryTestCase : 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_recovery( + const std::shared_ptr & node, const std::string & name) + { + auto rec = std::make_shared(); + rec->initialize(node, name); + return rec; + } + + static void set_position(easynav::NavState & nav_state, double x, double y) + { + nav_msgs::msg::Odometry odom; + odom.pose.pose.position.x = x; + odom.pose.pose.position.y = y; + nav_state.set("robot_pose", odom); + } +}; + +TEST_F(AdvanceRecoveryTestCase, RequiresControl) +{ + auto node = std::make_shared("test_rc_node"); + auto rec = make_recovery(node, "advance0"); + EXPECT_TRUE(rec->requires_control()); +} + +TEST_F(AdvanceRecoveryTestCase, CanHandleOnlyControllerStuckErrors) +{ + auto node = std::make_shared("test_ch_node"); + auto rec = make_recovery(node, "advance1"); + + diagnostic_msgs::msg::DiagnosticStatus matching; + matching.hardware_id = "controller_stuck"; + matching.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + EXPECT_TRUE(rec->can_handle(matching)); + + diagnostic_msgs::msg::DiagnosticStatus wrong_hardware = matching; + wrong_hardware.hardware_id = "planner"; + EXPECT_FALSE(rec->can_handle(wrong_hardware)); + + diagnostic_msgs::msg::DiagnosticStatus not_an_error = matching; + not_an_error.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + EXPECT_FALSE(rec->can_handle(not_an_error)); +} + +TEST_F(AdvanceRecoveryTestCase, AdvancesForwardWhileNotYetAtDistance) +{ + auto node = std::make_shared( + "test_advance_node", + rclcpp::NodeOptions().append_parameter_override("advance2.advance_speed", 0.4)); + auto rec = make_recovery(node, "advance2"); + + easynav::NavState nav_state; + set_position(nav_state, 0.0, 0.0); + + rec->internal_start(nav_state); + auto status = rec->internal_cycle(nav_state); + + EXPECT_EQ(status, easynav::RecoveryStatus::RUNNING); + ASSERT_TRUE(nav_state.has("cmd_vel")); + const auto & cmd = nav_state.get("cmd_vel"); + EXPECT_DOUBLE_EQ(cmd.twist.linear.x, 0.4); +} + +TEST_F(AdvanceRecoveryTestCase, SucceedsOnceDistanceReachedButDoesNotClaimFixed) +{ + auto node = std::make_shared( + "test_succeed_node", + rclcpp::NodeOptions().append_parameter_override("advance3.advance_distance", 0.2)); + auto rec = make_recovery(node, "advance3"); + + easynav::NavState nav_state; + set_position(nav_state, 0.0, 0.0); + rec->internal_start(nav_state); + + set_position(nav_state, 0.25, 0.0); // past advance_distance + auto status = rec->internal_cycle(nav_state); + + EXPECT_EQ(status, easynav::RecoveryStatus::SUCCEEDED); + ASSERT_TRUE(nav_state.has("cmd_vel")); + const auto & cmd = nav_state.get("cmd_vel"); + EXPECT_DOUBLE_EQ(cmd.twist.linear.x, 0.0); +} + +TEST_F(AdvanceRecoveryTestCase, EscalatesAfterTotalEpisodeTimeExceeded) +{ + auto node = std::make_shared( + "test_escalate_node", + rclcpp::NodeOptions().append_parameter_override("advance4.escalate_after", 0.05)); + auto rec = make_recovery(node, "advance4"); + + easynav::NavState nav_state; + set_position(nav_state, 0.0, 0.0); + rec->internal_start(nav_state); + + std::this_thread::sleep_for(std::chrono::milliseconds(60)); // past escalate_after + auto status = rec->internal_cycle(nav_state); + + EXPECT_EQ(status, easynav::RecoveryStatus::FAILED); +} + +TEST_F(AdvanceRecoveryTestCase, AccumulatesTotalTimeAcrossQuickReactivations) +{ + // episode_gap large: the short gap between the two activations below must NOT be treated as + // a new episode, so the second activation's escalation check sees the *total* elapsed time + // since the very first activation. + auto node = std::make_shared( + "test_accumulate_node", + rclcpp::NodeOptions() + .append_parameter_override("advance5.escalate_after", 0.05) + .append_parameter_override("advance5.episode_gap", 1.0) + .append_parameter_override("advance5.advance_distance", 100.0)); // never "reached" here + auto rec = make_recovery(node, "advance5"); + + easynav::NavState nav_state; + set_position(nav_state, 0.0, 0.0); + + rec->internal_start(nav_state); + auto first_status = rec->internal_cycle(nav_state); + ASSERT_EQ(first_status, easynav::RecoveryStatus::RUNNING); // too soon to escalate yet + rec->internal_stop(nav_state); + + std::this_thread::sleep_for(std::chrono::milliseconds(60)); // << episode_gap, same episode + + rec->internal_start(nav_state); + auto second_status = rec->internal_cycle(nav_state); + + EXPECT_EQ(second_status, easynav::RecoveryStatus::FAILED); +} + +TEST_F(AdvanceRecoveryTestCase, LongGapBetweenActivationsStartsAFreshEpisode) +{ + auto node = std::make_shared( + "test_gap_reset_node", + rclcpp::NodeOptions() + .append_parameter_override("advance6.escalate_after", 0.05) + .append_parameter_override("advance6.episode_gap", 0.03) + .append_parameter_override("advance6.advance_distance", 100.0)); + auto rec = make_recovery(node, "advance6"); + + easynav::NavState nav_state; + set_position(nav_state, 0.0, 0.0); + + rec->internal_start(nav_state); + rec->internal_cycle(nav_state); + rec->internal_stop(nav_state); + + std::this_thread::sleep_for(std::chrono::milliseconds(60)); // >> episode_gap: new episode + + rec->internal_start(nav_state); + auto status = rec->internal_cycle(nav_state); + + // If the gap had NOT reset the episode clock, elapsed time since the very first activation + // would already exceed escalate_after and this would incorrectly be FAILED. + EXPECT_EQ(status, easynav::RecoveryStatus::RUNNING); +} diff --git a/recovery_mitigations/easynav_cancel_mission_recovery/CMakeLists.txt b/recovery_mitigations/easynav_cancel_mission_recovery/CMakeLists.txt new file mode 100644 index 00000000..8196df1e --- /dev/null +++ b/recovery_mitigations/easynav_cancel_mission_recovery/CMakeLists.txt @@ -0,0 +1,67 @@ +cmake_minimum_required(VERSION 3.20) +project(easynav_cancel_mission_recovery) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(easynav_common REQUIRED) +find_package(easynav_core REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(diagnostic_msgs REQUIRED) + +add_library(${PROJECT_NAME} SHARED + src/easynav_cancel_mission_recovery/CancelMissionRecovery.cpp +) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) +target_link_libraries(${PROJECT_NAME} PUBLIC + easynav_common::easynav_common + easynav_core::easynav_core + pluginlib::pluginlib + rclcpp::rclcpp + ${diagnostic_msgs_TARGETS} +) + +install( + DIRECTORY include/ + DESTINATION include/${PROJECT_NAME} +) + +install(TARGETS + ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + + find_package(ament_cmake_gtest REQUIRED) + add_subdirectory(tests) +endif() + +ament_export_include_directories("include/${PROJECT_NAME}") +ament_export_libraries(${PROJECT_NAME}) +ament_export_targets(export_${PROJECT_NAME}) + +# Register the recovery mitigation plugin +pluginlib_export_plugin_description_file(easynav_core easynav_cancel_mission_recovery_plugins.xml) + +ament_export_dependencies( + easynav_common + easynav_core + pluginlib + rclcpp + diagnostic_msgs +) +ament_package() diff --git a/recovery_mitigations/easynav_cancel_mission_recovery/easynav_cancel_mission_recovery_plugins.xml b/recovery_mitigations/easynav_cancel_mission_recovery/easynav_cancel_mission_recovery_plugins.xml new file mode 100644 index 00000000..1e38750d --- /dev/null +++ b/recovery_mitigations/easynav_cancel_mission_recovery/easynav_cancel_mission_recovery_plugins.xml @@ -0,0 +1,12 @@ + + + + + Mission-level last resort: accepts any ERROR diagnostic no other mitigation resolved (or + already gave up on), and requests GoalManager to cancel the active mission, reporting the + diagnostics that caused it. Does not take control_owner — see + docs/recoveries_easynav_implementation.md, Fase 5. + + + + diff --git a/recovery_mitigations/easynav_cancel_mission_recovery/include/easynav_cancel_mission_recovery/CancelMissionRecovery.hpp b/recovery_mitigations/easynav_cancel_mission_recovery/include/easynav_cancel_mission_recovery/CancelMissionRecovery.hpp new file mode 100644 index 00000000..ddb773b4 --- /dev/null +++ b/recovery_mitigations/easynav_cancel_mission_recovery/include/easynav_cancel_mission_recovery/CancelMissionRecovery.hpp @@ -0,0 +1,61 @@ +// 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. + +/// \file +/// \brief Declaration of the CancelMissionRecovery plugin. + +#ifndef EASYNAV_CANCEL_MISSION_RECOVERY__CANCELMISSIONRECOVERY_HPP_ +#define EASYNAV_CANCEL_MISSION_RECOVERY__CANCELMISSIONRECOVERY_HPP_ + +#include "easynav_core/RecoveryMitigationBase.hpp" + +namespace easynav +{ + +/** + * @class CancelMissionRecovery + * @brief Level-1 mission-level last resort: cancels the active mission, reporting the error. + * + * The final rung of the escalation ladder: accepts any ERROR diagnostic no other mitigation + * resolved. Meant to be configured with the highest priority number of all (tried last). + * + * Unlike other mitigations, this one does not move the robot — it has no reference to + * GoalManager (only SystemNode does), so it asks for the mission to be cancelled via a one-shot + * NavState signal ("mission_cancel_requested") that GoalManager::update() reads, resets, and + * acts on. requires_control() is false: it only signals and waits, accepting one non-RT-cycle of + * latency. + * + * on_cycle() reports FAILED once the signal is consumed, not SUCCEEDED: cancelling the mission + * does not resolve the diagnostic that triggered it (e.g. an AMCL divergence stays a + * divergence), so claiming success would make it immediately eligible for reselection and + * re-cancel/re-log forever. + */ +class CancelMissionRecovery : public easynav::RecoveryMitigationBase +{ +public: + CancelMissionRecovery() = default; + ~CancelMissionRecovery() = default; + + void on_initialize() override; + + bool can_handle(const diagnostic_msgs::msg::DiagnosticStatus & status) const override; + +protected: + void on_start(NavState & nav_state) override; + RecoveryStatus on_cycle(NavState & nav_state) override; +}; + +} // namespace easynav + +#endif // EASYNAV_CANCEL_MISSION_RECOVERY__CANCELMISSIONRECOVERY_HPP_ diff --git a/recovery_mitigations/easynav_cancel_mission_recovery/package.xml b/recovery_mitigations/easynav_cancel_mission_recovery/package.xml new file mode 100644 index 00000000..83ed8441 --- /dev/null +++ b/recovery_mitigations/easynav_cancel_mission_recovery/package.xml @@ -0,0 +1,26 @@ + + + + easynav_cancel_mission_recovery + 0.4.2 + Easy Navigation: mission-level last-resort recovery mitigation that cancels the active mission, reporting the diagnostics that caused it. + Francisco Martín Rico + Apache-2.0 + + ament_cmake + + easynav_common + easynav_core + pluginlib + rclcpp + diagnostic_msgs + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + rclcpp_lifecycle + + + ament_cmake + + diff --git a/recovery_mitigations/easynav_cancel_mission_recovery/src/easynav_cancel_mission_recovery/CancelMissionRecovery.cpp b/recovery_mitigations/easynav_cancel_mission_recovery/src/easynav_cancel_mission_recovery/CancelMissionRecovery.cpp new file mode 100644 index 00000000..a9f1bc74 --- /dev/null +++ b/recovery_mitigations/easynav_cancel_mission_recovery/src/easynav_cancel_mission_recovery/CancelMissionRecovery.cpp @@ -0,0 +1,65 @@ +// 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. + +/// \file +/// \brief Implementation of the CancelMissionRecovery class. + +#include "easynav_cancel_mission_recovery/CancelMissionRecovery.hpp" + +namespace easynav +{ + +void CancelMissionRecovery::on_initialize() +{ +} + +bool CancelMissionRecovery::can_handle( + const diagnostic_msgs::msg::DiagnosticStatus & status) const +{ + return status.level >= diagnostic_msgs::msg::DiagnosticStatus::ERROR; +} + +void CancelMissionRecovery::on_start(NavState & nav_state) +{ + report( + nav_state, rcl_interfaces::msg::Log::ERROR, + "CancelMissionRecovery [" + get_plugin_name() + + "]: nothing else resolved this — cancelling the active mission"); + + nav_state.set("mission_cancel_requested", true); +} + +RecoveryStatus CancelMissionRecovery::on_cycle(NavState & nav_state) +{ + // Runs on the non-RT cycle (requires_control() is false), the same thread GoalManager's + // update() runs on, so a plain get() is safe here — no cross-thread read. + const bool still_pending = nav_state.has("mission_cancel_requested") && + nav_state.get("mission_cancel_requested"); + + if (still_pending) { + // GoalManager::update() has not consumed the request yet — one non-RT cycle of latency, + // same as every other one-shot NavState signal in this design. + return RecoveryStatus::RUNNING; + } + + // FAILED, not SUCCEEDED: cancelling the mission does not fix whatever diagnostic triggered it + // (e.g. AMCL stays diverged), so claiming success would make this eligible for immediate + // reselection every cycle, re-cancelling and re-logging forever. + return RecoveryStatus::FAILED; +} + +} // namespace easynav + +#include +PLUGINLIB_EXPORT_CLASS(easynav::CancelMissionRecovery, easynav::RecoveryMitigationBase) diff --git a/recovery_mitigations/easynav_cancel_mission_recovery/tests/CMakeLists.txt b/recovery_mitigations/easynav_cancel_mission_recovery/tests/CMakeLists.txt new file mode 100644 index 00000000..72ef9736 --- /dev/null +++ b/recovery_mitigations/easynav_cancel_mission_recovery/tests/CMakeLists.txt @@ -0,0 +1,2 @@ +ament_add_gtest(cancel_mission_recovery_tests cancel_mission_recovery_tests.cpp) +target_link_libraries(cancel_mission_recovery_tests ${PROJECT_NAME}) diff --git a/recovery_mitigations/easynav_cancel_mission_recovery/tests/cancel_mission_recovery_tests.cpp b/recovery_mitigations/easynav_cancel_mission_recovery/tests/cancel_mission_recovery_tests.cpp new file mode 100644 index 00000000..ed38fbe6 --- /dev/null +++ b/recovery_mitigations/easynav_cancel_mission_recovery/tests/cancel_mission_recovery_tests.cpp @@ -0,0 +1,102 @@ +// 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 "gtest/gtest.h" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +#include "easynav_cancel_mission_recovery/CancelMissionRecovery.hpp" + +class CancelMissionRecoveryTestCase : public ::testing::Test +{ +protected: + void SetUp() override + { + if (!rclcpp::ok()) { + rclcpp::init(0, nullptr); + } + } + + std::shared_ptr make_recovery( + const std::shared_ptr & node, const std::string & name) + { + auto rec = std::make_shared(); + rec->initialize(node, name); + return rec; + } +}; + +TEST_F(CancelMissionRecoveryTestCase, DoesNotRequireControl) +{ + auto node = std::make_shared("test_rc_node"); + auto rec = make_recovery(node, "cancel0"); + EXPECT_FALSE(rec->requires_control()); +} + +TEST_F(CancelMissionRecoveryTestCase, CanHandleAnyHardwareIdAtErrorLevelOrAbove) +{ + auto node = std::make_shared("test_ch_node"); + auto rec = make_recovery(node, "cancel1"); + + diagnostic_msgs::msg::DiagnosticStatus matching; + matching.hardware_id = "controller_stuck"; + matching.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + EXPECT_TRUE(rec->can_handle(matching)); + + diagnostic_msgs::msg::DiagnosticStatus not_an_error = matching; + not_an_error.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + EXPECT_FALSE(rec->can_handle(not_an_error)); +} + +TEST_F(CancelMissionRecoveryTestCase, OnStartSetsTheCancelRequestFlag) +{ + auto node = std::make_shared("test_start_node"); + auto rec = make_recovery(node, "cancel2"); + + easynav::NavState nav_state; + rec->internal_start(nav_state); + + ASSERT_TRUE(nav_state.has("mission_cancel_requested")); + EXPECT_TRUE(nav_state.get("mission_cancel_requested")); +} + +TEST_F(CancelMissionRecoveryTestCase, RunsWhileGoalManagerHasNotConsumedTheRequestYet) +{ + auto node = std::make_shared("test_running_node"); + auto rec = make_recovery(node, "cancel3"); + + easynav::NavState nav_state; + rec->internal_start(nav_state); // sets mission_cancel_requested = true + + auto status = rec->internal_cycle(nav_state); + EXPECT_EQ(status, easynav::RecoveryStatus::RUNNING); +} + +TEST_F(CancelMissionRecoveryTestCase, ReportsFailedOnceGoalManagerResetsTheFlag) +{ + auto node = std::make_shared("test_succeed_node"); + auto rec = make_recovery(node, "cancel4"); + + easynav::NavState nav_state; + rec->internal_start(nav_state); + + // Simulate GoalManager::update() having consumed the request. + nav_state.set("mission_cancel_requested", false); + + // FAILED, not SUCCEEDED: cancelling the mission does not resolve the diagnostic that + // triggered this mitigation, so it must not claim success. + auto status = rec->internal_cycle(nav_state); + EXPECT_EQ(status, easynav::RecoveryStatus::FAILED); +} diff --git a/recovery_mitigations/easynav_human_assistance_recovery/include/easynav_human_assistance_recovery/HumanAssistanceRecovery.hpp b/recovery_mitigations/easynav_human_assistance_recovery/include/easynav_human_assistance_recovery/HumanAssistanceRecovery.hpp index 45a42459..9cbd4316 100644 --- a/recovery_mitigations/easynav_human_assistance_recovery/include/easynav_human_assistance_recovery/HumanAssistanceRecovery.hpp +++ b/recovery_mitigations/easynav_human_assistance_recovery/include/easynav_human_assistance_recovery/HumanAssistanceRecovery.hpp @@ -18,6 +18,10 @@ #ifndef EASYNAV_HUMAN_ASSISTANCE_RECOVERY__HUMANASSISTANCERECOVERY_HPP_ #define EASYNAV_HUMAN_ASSISTANCE_RECOVERY__HUMANASSISTANCERECOVERY_HPP_ +#include + +#include "rclcpp/time.hpp" + #include "easynav_core/RecoveryMitigationBase.hpp" namespace easynav @@ -27,20 +31,21 @@ namespace easynav * @class HumanAssistanceRecovery * @brief Level-1 last-resort mitigation: asks a human operator for help. * - * See docs/recoveries_easynav.md §5.15 — this is a simplified instance of the design's - * HumanAssistanceRecovery (no `teleop` mode, no episode-id `ack`: the "ack" here is physical, - * whatever fixed the problem shows up as the offending diagnostic going back to OK). Generic - * and domain-agnostic by design: unlike SafeRetreatRecovery/AmclRelocalizeMitigation, it does - * not know or care which component raised the diagnostic — can_handle() accepts any ERROR, - * meant to be configured with the lowest priority (or listed last in "mitigation_types") so it - * is only reached once every more specific mitigator has been tried and excluded (see - * RecoveryManagerNode's per-diagnostic exclusion, docs/recoveries_easynav_implementation.md). + * Deliberately simplified: no `teleop` mode, no episode-id `ack` — the "ack" here is physical, + * whatever fixed the problem shows up as the offending diagnostic going back to OK. Generic and + * domain-agnostic: unlike SafeRetreatRecovery/AmclRelocalizeMitigation, it does not know or care + * which component raised the diagnostic — can_handle() accepts any ERROR, meant to be configured + * with the lowest priority (or listed last in "mitigation_types") so it is only reached once + * every more specific mitigator has been tried and excluded. * - * Not to be confused with the now-removed NotifyAndHoldRecovery (Fase 4, retired in the - * Sesión 11 of the implementation log), which treated its trigger as a mission failure - * (`GoalManager::set_failed`). This one does not: once every diagnostic is observed back at OK - * — presumably because a human fixed whatever was wrong — it returns control and the robot - * resumes its current mission, exactly as §5.15 specifies for a human-certified recovery. + * Once every diagnostic is observed back at OK — presumably because a human fixed whatever was + * wrong — it returns control and the robot resumes its current mission; it never fails the + * mission itself (that is a different mitigation's job). + * + * Optionally bounded by "timeout" (seconds, default 0.0 = wait forever): if a human has not + * fixed things within that time, this mitigation gives up (FAILED) instead of waiting + * indefinitely, so a lower-priority candidate — e.g. a mission-level "give up" mitigation — can + * take over. */ class HumanAssistanceRecovery : public easynav::RecoveryMitigationBase { @@ -56,6 +61,18 @@ class HumanAssistanceRecovery : public easynav::RecoveryMitigationBase protected: void on_start(NavState & nav_state) override; RecoveryStatus on_cycle(NavState & nav_state) override; + +private: + /// @brief Seconds to wait before giving up. 0.0 (the default) means wait forever. + double timeout_ {0.0}; + + rclcpp::Time start_time_; + + /// @brief When the last "still waiting" report() was sent, to throttle it to once per + /// wait_report_period_ instead of every RT cycle. + std::optional last_wait_report_; + + static constexpr double wait_report_period_ {10.0}; }; } // namespace easynav diff --git a/recovery_mitigations/easynav_human_assistance_recovery/src/easynav_human_assistance_recovery/HumanAssistanceRecovery.cpp b/recovery_mitigations/easynav_human_assistance_recovery/src/easynav_human_assistance_recovery/HumanAssistanceRecovery.cpp index 3ddf011a..f0d51e83 100644 --- a/recovery_mitigations/easynav_human_assistance_recovery/src/easynav_human_assistance_recovery/HumanAssistanceRecovery.cpp +++ b/recovery_mitigations/easynav_human_assistance_recovery/src/easynav_human_assistance_recovery/HumanAssistanceRecovery.cpp @@ -22,6 +22,11 @@ namespace easynav void HumanAssistanceRecovery::on_initialize() { + auto node = get_node(); + const auto & plugin_name = get_plugin_name(); + + node->declare_parameter(plugin_name + ".timeout", timeout_); + node->get_parameter(plugin_name + ".timeout", timeout_); } bool HumanAssistanceRecovery::can_handle( @@ -33,9 +38,7 @@ bool HumanAssistanceRecovery::can_handle( void HumanAssistanceRecovery::on_start(NavState & nav_state) { // Selection (and so on_start()) always runs from RecoveryManagerNode::cycle(), the same - // non-RT thread the evaluators that wrote "diagnostics" run on, so a plain get() is safe - // here. This is the observable "asking a human for help" signal §5.15 requires; how it - // reaches an actual person (app, light, sound) is a deployment decision, not this plugin's. + // non-RT thread the evaluators that wrote "diagnostics" run on, so a plain get() is safe here. std::string summary; for (const auto & key : nav_state.get_group_keys("diagnostics")) { if (!nav_state.has(key)) {continue;} @@ -46,11 +49,15 @@ void HumanAssistanceRecovery::on_start(NavState & nav_state) } } - RCLCPP_ERROR( - get_node()->get_logger(), - "HumanAssistanceRecovery [%s]: no other mitigation resolved this — requesting human " - "assistance for: %s", - get_plugin_name().c_str(), summary.empty() ? "unknown" : summary.c_str()); + report( + nav_state, rcl_interfaces::msg::Log::ERROR, + "HumanAssistanceRecovery [" + get_plugin_name() + "]: no other mitigation resolved this — " + "requesting human assistance for: " + (summary.empty() ? "unknown" : summary)); + + last_wait_report_.reset(); + if (timeout_ > 0.0) { + start_time_ = get_node()->now(); + } } RecoveryStatus HumanAssistanceRecovery::on_cycle(NavState & nav_state) @@ -76,12 +83,27 @@ RecoveryStatus HumanAssistanceRecovery::on_cycle(NavState & nav_state) return RecoveryStatus::SUCCEEDED; } - RCLCPP_ERROR_THROTTLE( - get_node()->get_logger(), *get_node()->get_clock(), 10000, - "HumanAssistanceRecovery [%s]: still waiting for human assistance", - get_plugin_name().c_str()); + if (timeout_ > 0.0 && (get_node()->now() - start_time_).seconds() >= timeout_) { + report( + nav_state, rcl_interfaces::msg::Log::ERROR, + "HumanAssistanceRecovery [" + get_plugin_name() + "]: no human response after " + + std::to_string(timeout_) + " s, giving up"); + stop_robot(nav_state); + return RecoveryStatus::FAILED; + } + + // Manual throttle (replaces RCLCPP_ERROR_THROTTLE): report() only keeps the single latest + // entry, so calling it every RT cycle would still need throttling to avoid flooding it. + const rclcpp::Time now = get_node()->now(); + if (!last_wait_report_.has_value() || + (now - *last_wait_report_).seconds() >= wait_report_period_) + { + report( + nav_state, rcl_interfaces::msg::Log::ERROR, + "HumanAssistanceRecovery [" + get_plugin_name() + "]: still waiting for human assistance"); + last_wait_report_ = now; + } - // No timeout: this is the last resort, it waits as long as it takes. stop_robot(nav_state); return RecoveryStatus::RUNNING; } diff --git a/recovery_mitigations/easynav_human_assistance_recovery/tests/human_assistance_recovery_tests.cpp b/recovery_mitigations/easynav_human_assistance_recovery/tests/human_assistance_recovery_tests.cpp index a3a45b98..9c5f2750 100644 --- a/recovery_mitigations/easynav_human_assistance_recovery/tests/human_assistance_recovery_tests.cpp +++ b/recovery_mitigations/easynav_human_assistance_recovery/tests/human_assistance_recovery_tests.cpp @@ -12,6 +12,9 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include + #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" @@ -125,3 +128,41 @@ TEST_F(HumanAssistanceRecoveryTestCase, SucceedsWithNoDiagnosticsGroupAtAll) EXPECT_EQ(status, easynav::RecoveryStatus::SUCCEEDED); } + +TEST_F(HumanAssistanceRecoveryTestCase, WaitsForeverByDefaultEvenWhenSlow) +{ + // timeout defaults to 0.0 (wait forever): a slow-but-still-unresolved wait must stay RUNNING, + // never FAILED, no matter how much time passes. + auto node = std::make_shared("test_no_timeout_node"); + auto rec = make_recovery(node, "human6"); + + easynav::NavState nav_state; + nav_state.set( + "diagnostics.planner", make_status(diagnostic_msgs::msg::DiagnosticStatus::ERROR)); + nav_state.set_group("diagnostics", {"diagnostics.planner"}); + + rec->internal_start(nav_state); + std::this_thread::sleep_for(std::chrono::milliseconds(60)); + auto status = rec->internal_cycle(nav_state); + + EXPECT_EQ(status, easynav::RecoveryStatus::RUNNING); +} + +TEST_F(HumanAssistanceRecoveryTestCase, FailsAfterTimeoutWithoutHumanResponse) +{ + auto node = std::make_shared( + "test_timeout_node", + rclcpp::NodeOptions().append_parameter_override("human7.timeout", 0.05)); + auto rec = make_recovery(node, "human7"); + + easynav::NavState nav_state; + nav_state.set( + "diagnostics.planner", make_status(diagnostic_msgs::msg::DiagnosticStatus::ERROR)); + nav_state.set_group("diagnostics", {"diagnostics.planner"}); + + rec->internal_start(nav_state); + std::this_thread::sleep_for(std::chrono::milliseconds(60)); // past the 50 ms timeout + auto status = rec->internal_cycle(nav_state); + + EXPECT_EQ(status, easynav::RecoveryStatus::FAILED); +} diff --git a/recovery_mitigations/easynav_safe_retreat_recovery/include/easynav_safe_retreat_recovery/SafeRetreatRecovery.hpp b/recovery_mitigations/easynav_safe_retreat_recovery/include/easynav_safe_retreat_recovery/SafeRetreatRecovery.hpp index c4abe1d1..19b2e328 100644 --- a/recovery_mitigations/easynav_safe_retreat_recovery/include/easynav_safe_retreat_recovery/SafeRetreatRecovery.hpp +++ b/recovery_mitigations/easynav_safe_retreat_recovery/include/easynav_safe_retreat_recovery/SafeRetreatRecovery.hpp @@ -27,22 +27,16 @@ namespace easynav * @class SafeRetreatRecovery * @brief Level-1 movement mitigation: retreats straight back from a too-close obstacle. * - * See docs/recoveries_easynav.md §5.11/§5.13. Selected for diagnostics with - * hardware_id == "obstacle_proximity" (the vocabulary shared with ObstacleTooCloseEvaluator, - * matched by string, not by a compile-time dependency between the two plugins). Takes control - * of "cmd_vel" (requires_control() == true) and commands a slow, straight-backward motion each - * RT cycle, re-checking the nearest-obstacle distance every cycle until it exceeds this - * mitigation's own safe_distance parameter. + * Selected for diagnostics with hardware_id == "obstacle_proximity" (shared with + * ObstacleTooCloseEvaluator, matched by string). Takes control of "cmd_vel" + * (requires_control() == true) and commands a slow, straight-backward motion each RT cycle, + * re-checking the nearest-obstacle distance until it exceeds safe_distance. * - * Deliberate simplification versus the full design ambition (see - * docs/recoveries_easynav_implementation.md): this only retreats straight back, which is safe - * and correct when the obstacle is roughly ahead of the robot (the scenario this was designed - * for — an obstacle appearing in the direction of travel), and matches Nav2's own BackUp - * behaviour (also reverse-only) as well as the differential-drive robots this workspace's own - * configs target (a diff-drive base cannot strafe toward an arbitrary escape direction anyway). - * If the nearest obstacle is behind the robot instead, reversing would drive toward it rather - * than away, so on_cycle() recognizes that case and fails safely (stops, returns FAILED) - * instead of blindly reversing. + * Only retreats straight back — correct when the obstacle is roughly ahead (an obstacle + * appearing in the direction of travel), matching Nav2's own reverse-only BackUp behaviour and + * the differential-drive robots this workspace targets (which cannot strafe anyway). If the + * nearest obstacle is behind the robot instead, reversing would drive toward it, so on_cycle() + * fails safely (stops, returns FAILED) instead of blindly reversing. */ class SafeRetreatRecovery : public easynav::RecoveryMitigationBase { diff --git a/recovery_mitigations/easynav_safe_retreat_recovery/src/easynav_safe_retreat_recovery/SafeRetreatRecovery.cpp b/recovery_mitigations/easynav_safe_retreat_recovery/src/easynav_safe_retreat_recovery/SafeRetreatRecovery.cpp index 20b8910c..b25bcee8 100644 --- a/recovery_mitigations/easynav_safe_retreat_recovery/src/easynav_safe_retreat_recovery/SafeRetreatRecovery.cpp +++ b/recovery_mitigations/easynav_safe_retreat_recovery/src/easynav_safe_retreat_recovery/SafeRetreatRecovery.cpp @@ -45,11 +45,11 @@ bool SafeRetreatRecovery::can_handle(const diagnostic_msgs::msg::DiagnosticStatu status.level >= diagnostic_msgs::msg::DiagnosticStatus::ERROR; } -void SafeRetreatRecovery::on_start(NavState &) +void SafeRetreatRecovery::on_start(NavState & nav_state) { - RCLCPP_WARN( - get_node()->get_logger(), "SafeRetreatRecovery [%s]: retreating from a too-close obstacle", - get_plugin_name().c_str()); + report( + nav_state, rcl_interfaces::msg::Log::WARN, + "SafeRetreatRecovery [" + get_plugin_name() + "]: retreating from a too-close obstacle"); } RecoveryStatus SafeRetreatRecovery::on_cycle(NavState & nav_state) @@ -65,11 +65,11 @@ RecoveryStatus SafeRetreatRecovery::on_cycle(NavState & nav_state) if (std::abs(obstacle.bearing) > M_PI / 2.0) { // The nearest obstacle is behind the robot: reversing would drive toward it, not away. // Fail safely instead of guessing a direction. See the class doc comment. - RCLCPP_ERROR( - get_node()->get_logger(), - "SafeRetreatRecovery [%s]: nearest obstacle is behind the robot (bearing=%.2f rad), " - "cannot safely retreat straight back", - get_plugin_name().c_str(), obstacle.bearing); + report( + nav_state, rcl_interfaces::msg::Log::ERROR, + "SafeRetreatRecovery [" + get_plugin_name() + "]: nearest obstacle is behind the robot " + "(bearing=" + std::to_string(obstacle.bearing) + " rad), cannot safely retreat straight " + "back"); stop_robot(nav_state); return RecoveryStatus::FAILED; } From 889787386215422e272ca338a0e6dd8d7929f3bb Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Francisco=20Mart=C3=ADn=20Rico?= Date: Sat, 12 Sep 2026 13:31:51 +0200 Subject: [PATCH 4/4] Fix A* bug to inaccesible places MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Francisco Martín Rico --- .../CostmapPlanner.hpp | 6 +++ .../CostmapPlanner.cpp | 42 +++++++++++++------ 2 files changed, 35 insertions(+), 13 deletions(-) diff --git a/planners/easynav_costmap_planner/include/easynav_costmap_planner/CostmapPlanner.hpp b/planners/easynav_costmap_planner/include/easynav_costmap_planner/CostmapPlanner.hpp index 67de89db..a6d34ea9 100644 --- a/planners/easynav_costmap_planner/include/easynav_costmap_planner/CostmapPlanner.hpp +++ b/planners/easynav_costmap_planner/include/easynav_costmap_planner/CostmapPlanner.hpp @@ -93,6 +93,12 @@ class CostmapPlanner : public PlannerMethodBase const Costmap2D & map, const geometry_msgs::msg::Pose & start, const geometry_msgs::msg::Pose & goal); + + /// @brief Clears current_path_ (if not already empty), publishes it once to path_pub_ (RViz) + /// and to NavState's "path" (the controller) -- called from every branch of update() that + /// gives up on the current goal (no goal, wrong frame, goal outside the map, A* found no + /// route), so a failed plan is never masked by whatever path was last computed successfully. + void clear_current_path(NavState & nav_state); }; } // namespace easynav diff --git a/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp b/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp index 653bfa24..e5333e74 100644 --- a/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp +++ b/planners/easynav_costmap_planner/src/easynav_costmap_planner/CostmapPlanner.cpp @@ -146,19 +146,7 @@ void CostmapPlanner::update(NavState & nav_state) const auto & goals = nav_state.get("goals"); if (goals.goals.empty()) { - // No active goal: the path must reflect that too, not keep republishing whatever was last - // computed — otherwise a controller following a stale path toward a cancelled goal would - // never stop on its own. - if (!current_path_.poses.empty()) { - current_path_.poses.clear(); - current_path_.header.stamp = get_node()->now(); - // Also publish the cleared path once, so RViz does not keep showing the last computed - // path frozen on screen after the mission is cancelled. - if (path_pub_->get_subscription_count() > 0) { - path_pub_->publish(current_path_); - } - } - nav_state.set("path", current_path_); + clear_current_path(nav_state); return; } @@ -181,6 +169,7 @@ void CostmapPlanner::update(NavState & nav_state) if (goals.header.frame_id != tf_info.map_frame) { RCLCPP_WARN(get_node()->get_logger(), "Goals frame is not 'map': %s", goals.header.frame_id.c_str()); + clear_current_path(nav_state); return; } @@ -188,6 +177,7 @@ void CostmapPlanner::update(NavState & nav_state) if (!map.worldToMap(goal.position.x, goal.position.y, gx, gy)) { RCLCPP_WARN(get_node()->get_logger(), "Goal (%.2f, %.2f) is outside the map", goal.position.x, goal.position.y); + clear_current_path(nav_state); return; } @@ -261,6 +251,22 @@ void CostmapPlanner::update(NavState & nav_state) } last_goal_pose = goal; last_plan_time = get_node()->now(); + nav_state.set("path", current_path_); + } else { + // A* found no route (e.g. the goal is unreachable, walled off) -- clear the path instead of + // silently republishing whatever was last computed for a previous, reachable goal. + clear_current_path(nav_state); + } +} + +void CostmapPlanner::clear_current_path(NavState & nav_state) +{ + if (!current_path_.poses.empty()) { + current_path_.poses.clear(); + current_path_.header.stamp = get_node()->now(); + if (path_pub_->get_subscription_count() > 0) { + path_pub_->publish(current_path_); + } } nav_state.set("path", current_path_); } @@ -325,6 +331,14 @@ std::vector CostmapPlanner::a_star_path( } } + if (!std::isfinite(cost_so_far[idx(static_cast(gx), static_cast(gy))])) { + // The search explored every cell reachable from the start without ever reaching the goal + // cell: it is genuinely unreachable (e.g. walled off), not just "trivially close" -- must + // not be masked as a valid path (see the start == goal case below, which is the only + // legitimate reason for an empty backtrack). + return {}; + } + std::vector path; int cx = static_cast(gx), cy = static_cast(gy); while (parent_x[idx(cx, cy)] != -1) { @@ -342,6 +356,8 @@ std::vector CostmapPlanner::a_star_path( } std::reverse(path.begin(), path.end()); + // Reached (cost_so_far finite, checked above) but zero hops: start and goal are the same + // cell, not a failed search. if (path.empty()) {path.push_back(goal);} return path; }