hubero_ros - NavigationRos: overriding the action result status b…

…locked in the action feedback callback (only for some time)

- increases the robustness of finishing complex tasks relying on navigation
- somehow related to [#38]

hubero_ros/include/hubero_ros/navigation_ros.h

@@ -42,6 +42,12 @@ class NavigationRos: public NavigationBase {
 	 */
 	static const int QUATERNION_RANDOM_RETRY_NUM;
 
+	/**
+	 * @detail Defines for how long (since the last action result callback) any feedback of the action won't override
+	 * @ref feedback_
+	 */
+	static const double ACTION_RESULT_FREEZE_TIME_SEC;
+
 	/**
 	 * @brief Constructor
 	 */
@@ -200,6 +206,8 @@ class NavigationRos: public NavigationBase {
 	ros::Subscriber sub_feedback_;
 	/// @brief Subscriber of the move_base simple action server's result topic
 	ros::Subscriber sub_result_;
+	/// @brief Stores a timestamp of the last result message
+	ros::Time cb_result_timestamp_;
 
 	/// Helper typedefs
 	typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseActionClient;

hubero_ros/src/navigation_ros.cpp

@@ -15,6 +15,7 @@ namespace hubero {
 const int NavigationRos::SUBSCRIBER_QUEUE_SIZE = 10;
 const int NavigationRos::PUBLISHER_QUEUE_SIZE = 15;
 const int NavigationRos::QUATERNION_RANDOM_RETRY_NUM = 10;
+const double NavigationRos::ACTION_RESULT_FREEZE_TIME_SEC = 2.0;
 
 NavigationRos::NavigationRos():
 	NavigationBase::NavigationBase(),
@@ -556,6 +557,17 @@ void NavigationRos::callbackCmdVel(const geometry_msgs::Twist::ConstPtr& msg) {
 
 void NavigationRos::callbackFeedback(const move_base_msgs::MoveBaseActionFeedback::ConstPtr& msg) {
 	const std::lock_guard<std::mutex> lock(mutex_callback_);
+	// The aim is to prevent a late action feedback overriding the result (stored in @ref feedback_)
+	if ((ros::Time::now() - cb_result_timestamp_).toSec() <= NavigationRos::ACTION_RESULT_FREEZE_TIME_SEC) {
+		HUBERO_LOG(
+			"[%s].[NavigationRos] Action feedback won't be overwritten because the action result has recently been updated (to %d). "
+			"Ignored feedback status is %d\r\n",
+			actor_name_.c_str(),
+			feedback_,
+			msg->status.status
+		);
+		return;
+	}
 	auto fb_type = convertActionStatusToTaskFeedback(msg->status.status);
 	if (fb_type == TASK_FEEDBACK_UNDEFINED) {
 		return;
@@ -570,6 +582,8 @@ void NavigationRos::callbackResult(const move_base_msgs::MoveBaseActionResult::C
 		return;
 	}
 	feedback_ = fb_type;
+	// save for later use in @ref callbackFeedback
+	cb_result_timestamp_ = ros::Time::now();
 }
 
 std::tuple<bool, Pose3> NavigationRos::findTransform(const std::string& frame_source, const std::string& frame_target) const {