diff --git a/include/humap_local_planner/recovery_manager.h b/include/humap_local_planner/recovery_manager.h index 6eb53794..d8f29479 100644 --- a/include/humap_local_planner/recovery_manager.h +++ b/include/humap_local_planner/recovery_manager.h @@ -9,6 +9,8 @@ #include #include +#include + namespace humap_local_planner { template @@ -100,6 +102,18 @@ class RecoveryManager { */ void setOscillationBufferLength(int length); + /** + * @brief Sets the maximum age of a global path plan + * + * @param global_path_plan_max_age maximum age of a global path plan to consider the plan as outdated + */ + void setGlobalPathPlanMaxAge(double global_path_plan_max_age); + + /** + * @brief Updates the internal state that checks whether the global plan is outdated + */ + void markGlobalPlanUpdate(); + /** * @brief Adds a new twist measurement to the internal buffer and computes a new decision * @@ -147,6 +161,11 @@ class RecoveryManager { ObstacleSeparationCostFunction& obstacle_costfun ); + /** + * @brief Computes the age of the global path plan and returns its value + */ + double computeGlobalPlanAge(); + /** * @brief Searches for a safe pose near the robot to recover from being stuck * @@ -198,6 +217,10 @@ class RecoveryManager { return rr_recovery_goal_; } + inline bool isGlobalPathPlanOutdated() const { + return global_path_plan_outdated_; + } + static bool isPoseCollisionFree(double costmap_cost); /// Helper function that evaluates whether the components of a 2D pose are valid (not NAN and not INF) @@ -243,6 +266,13 @@ class RecoveryManager { /// Stores previous linear velocity of the velocity command send to the mobile base double prev_cmd_vx; double prev_cmd_omega; + + /// True indicates that the global path plan has not been updated for too long + bool global_path_plan_outdated_; + /// Maximum age of the global path plan + double global_path_plan_max_age_; + /// Timestamp (seconds) of the last global path plan update (atomic as it might be updated asynchronously) + std::atomic global_path_plan_update_timestamp_; }; } // namespace humap_local_planner diff --git a/src/recovery_manager.cpp b/src/recovery_manager.cpp index b2e15b1f..e0f94b3f 100644 --- a/src/recovery_manager.cpp +++ b/src/recovery_manager.cpp @@ -15,13 +15,20 @@ RecoveryManager::RecoveryManager(): can_recover_from_collision_(true), rr_recovery_goal_(geometry::Pose(NAN, NAN, NAN, NAN, NAN, NAN, NAN)), prev_cmd_vx(0.0), - prev_cmd_omega(0.0) + prev_cmd_omega(0.0), + global_path_plan_outdated_(false), + global_path_plan_max_age_(3.0), + global_path_plan_update_timestamp_(0.0) {} void RecoveryManager::setOscillationBufferLength(int length) { buffer_.set_capacity(length); } +void RecoveryManager::setGlobalPathPlanMaxAge(double global_path_plan_max_age) { + global_path_plan_max_age_ = global_path_plan_max_age; +} + void RecoveryManager::checkOscillation( double v_x, double v_th, @@ -110,6 +117,13 @@ void RecoveryManager::checkCollision( } } +double RecoveryManager::computeGlobalPlanAge() { + double current_time = ros::Time::now().toSec(); + double path_age = current_time - global_path_plan_update_timestamp_.load(); + global_path_plan_outdated_ = path_age > global_path_plan_max_age_; + return path_age; +} + bool RecoveryManager::planRecoveryRotateAndRecede( double x, double y, @@ -235,6 +249,10 @@ void RecoveryManager::clear() { oscillating_ = false; } +void RecoveryManager::markGlobalPlanUpdate() { + global_path_plan_update_timestamp_.store(ros::Time::now().toSec()); +} + bool RecoveryManager::isPoseCollisionFree(double costmap_cost) { return costmap_cost >= 0.0 && costmap_cost < costmap_2d::LETHAL_OBSTACLE; }