Gracefully handle jump back in time

Without this patch, the SimpleActionServer was rejecting new goals from the new (reset) timeline until time catches up.
ros · Sep 7, 2023 · 8a25b03 · 8a25b03
1 parent 3558eb3
commit 8a25b03
Showing 1 changed file with 4 additions and 3 deletions.
diff --git a/actionlib/include/actionlib/server/simple_action_server_imp.h b/actionlib/include/actionlib/server/simple_action_server_imp.h
@@ -295,9 +295,10 @@ void SimpleActionServer<ActionSpec>::goalCallback(GoalHandle goal)
   boost::recursive_mutex::scoped_lock lock(lock_);
   ROS_DEBUG_NAMED("actionlib", "A new goal has been received by the single goal action server");
 
-  // check that the timestamp is past or equal to that of the current goal and the next goal
-  if ((!current_goal_.getGoal() || goal.getGoalID().stamp >= current_goal_.getGoalID().stamp) &&
-    (!next_goal_.getGoal() || goal.getGoalID().stamp >= next_goal_.getGoalID().stamp))
+  // check that the timestamp is past or equal to that of the current goal and the next goal (or time was reset)
+  ros::Time now = ros::Time::now();
+  if ((!current_goal_.getGoal() || goal.getGoalID().stamp >= current_goal_.getGoalID().stamp || now < current_goal_.getGoalID().stamp) &&
+      (!next_goal_.getGoal() || goal.getGoalID().stamp >= next_goal_.getGoalID().stamp || now < next_goal_.getGoalID().stamp))
   {
     // if next_goal has not been accepted already... its going to get bumped, but we need to let the client know we're preempting
     if (next_goal_.getGoal() && (!current_goal_.getGoal() || next_goal_ != current_goal_)) {