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..a9ebeaf1 --- /dev/null +++ b/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AmclConvergenceEvaluator.hpp @@ -0,0 +1,60 @@ +// 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. + * + * 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 + * 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..569533b1 --- /dev/null +++ b/localizers/easynav_costmap_localizer/include/easynav_costmap_localizer/AmclRelocalizeMitigation.hpp @@ -0,0 +1,71 @@ +// 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. + * + * 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 +{ +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..06368494 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. +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..b5c1d71d --- /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 & nav_state) +{ + start_time_ = get_node()->now(); + 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) +{ + // 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_) { + 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; + } + + 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/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 842b0864..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,7 +146,7 @@ void CostmapPlanner::update(NavState & nav_state) const auto & goals = nav_state.get("goals"); if (goals.goals.empty()) { - nav_state.set("path", current_path_); + clear_current_path(nav_state); return; } @@ -169,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; } @@ -176,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; } @@ -249,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_); } @@ -313,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) { @@ -330,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; } 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/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..9aff295b --- /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. + * + * 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..7e5dcb63 --- /dev/null +++ b/recovery_evaluators/easynav_no_path_evaluator/src/easynav_no_path_evaluator/NoPathEvaluator.cpp @@ -0,0 +1,68 @@ +// 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/goals.hpp" +#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"; + + // "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 { + // 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..65c0ae72 --- /dev/null +++ b/recovery_evaluators/easynav_no_path_evaluator/tests/no_path_evaluator_tests.cpp @@ -0,0 +1,135 @@ +// 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/goals.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); + } + } + + 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, OkWithoutAnActiveGoal) +{ + // 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_path0"); + + std::this_thread::sleep_for(std::chrono::milliseconds(120)); + easynav::NavState nav_state; + eval.internal_update(nav_state); + + EXPECT_EQ( + 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, ErrorsOnEmptyPathWithAnActiveGoal) +{ + auto node = std::make_shared("test_empty_path_node"); + easynav::NoPathEvaluator eval; + 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_path3").level, + diagnostic_msgs::msg::DiagnosticStatus::ERROR); +} + +TEST_F(NoPathEvaluatorTestCase, OkWhenPathHasPosesWithAnActiveGoal) +{ + auto node = std::make_shared("test_ok_path_node"); + easynav::NoPathEvaluator eval; + 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()); + 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_path4").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..7fa9f03d --- /dev/null +++ b/recovery_evaluators/easynav_obstacle_too_close_evaluator/include/easynav_obstacle_too_close_evaluator/ObstacleTooCloseEvaluator.hpp @@ -0,0 +1,78 @@ +// 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 + +#include "rclcpp/time.hpp" + +#include "easynav_core/RecoveryEvaluatorBase.hpp" + +namespace easynav +{ + +/** + * @class ObstacleTooCloseEvaluator + * @brief Level-1 recovery evaluator: diagnoses "stopped too close to an obstacle". + * + * 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. + * + * "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 +{ +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}; + + /// @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 + +#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..bc1ece62 --- /dev/null +++ b/recovery_evaluators/easynav_obstacle_too_close_evaluator/src/easynav_obstacle_too_close_evaluator/ObstacleTooCloseEvaluator.cpp @@ -0,0 +1,121 @@ +// 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->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) +{ + 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")) { + stopped_since_.reset(); + 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. + 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_) { + // "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; + } + + 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..a53a5e74 --- /dev/null +++ b/recovery_evaluators/easynav_obstacle_too_close_evaluator/tests/obstacle_too_close_evaluator_tests.cpp @@ -0,0 +1,225 @@ +// 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). + 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) +{ + // 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; + 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); +} + +// --------------------------------------------------------------------------- +// 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) +{ + // 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_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/CMakeLists.txt b/recovery_mitigations/easynav_human_assistance_recovery/CMakeLists.txt new file mode 100644 index 00000000..a037c97a --- /dev/null +++ b/recovery_mitigations/easynav_human_assistance_recovery/CMakeLists.txt @@ -0,0 +1,67 @@ +cmake_minimum_required(VERSION 3.20) +project(easynav_human_assistance_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_human_assistance_recovery/HumanAssistanceRecovery.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_human_assistance_recovery_plugins.xml) + +ament_export_dependencies( + easynav_common + easynav_core + pluginlib + rclcpp + diagnostic_msgs +) +ament_package() 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..9cbd4316 --- /dev/null +++ b/recovery_mitigations/easynav_human_assistance_recovery/include/easynav_human_assistance_recovery/HumanAssistanceRecovery.hpp @@ -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. + +/// \file +/// \brief Declaration of the HumanAssistanceRecovery plugin. + +#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 +{ + +/** + * @class HumanAssistanceRecovery + * @brief Level-1 last-resort mitigation: asks a human operator for help. + * + * 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. + * + * 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 +{ +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; + +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 + +#endif // EASYNAV_HUMAN_ASSISTANCE_RECOVERY__HUMANASSISTANCERECOVERY_HPP_ diff --git a/recovery_mitigations/easynav_human_assistance_recovery/package.xml b/recovery_mitigations/easynav_human_assistance_recovery/package.xml new file mode 100644 index 00000000..66cb06d8 --- /dev/null +++ b/recovery_mitigations/easynav_human_assistance_recovery/package.xml @@ -0,0 +1,26 @@ + + + + easynav_human_assistance_recovery + 0.4.2 + 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 + + 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_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..f0d51e83 --- /dev/null +++ b/recovery_mitigations/easynav_human_assistance_recovery/src/easynav_human_assistance_recovery/HumanAssistanceRecovery.cpp @@ -0,0 +1,114 @@ +// 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() +{ + 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( + 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. + 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 + ")"; + } + } + + 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) +{ + // 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; + } + + 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; + } + + 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..9c5f2750 --- /dev/null +++ b/recovery_mitigations/easynav_human_assistance_recovery/tests/human_assistance_recovery_tests.cpp @@ -0,0 +1,168 @@ +// 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_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); +} + +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/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..19b2e328 --- /dev/null +++ b/recovery_mitigations/easynav_safe_retreat_recovery/include/easynav_safe_retreat_recovery/SafeRetreatRecovery.hpp @@ -0,0 +1,66 @@ +// 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. + * + * 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. + * + * 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 +{ +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..b25bcee8 --- /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 & nav_state) +{ + 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) +{ + 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. + 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; + } + + 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); +}