RecoveryManager - added the calculation of the global plan's age [#122

]

include/humap_local_planner/recovery_manager.h

@@ -9,6 +9,8 @@
 #include <deque>
 #include <stdexcept>
 
+#include <atomic>
+
 namespace humap_local_planner {
 
 template <typename T>
@@ -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<double> global_path_plan_update_timestamp_;
 };
 
 } // namespace humap_local_planner

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;
 }