From 8a25b0392135929b621d7b79cdc67ad9c464a3c1 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 23 Oct 2022 11:14:33 +0200 Subject: [PATCH] Gracefully handle jump back in time Without this patch, the SimpleActionServer was rejecting new goals from the new (reset) timeline until time catches up. --- .../include/actionlib/server/simple_action_server_imp.h | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/actionlib/include/actionlib/server/simple_action_server_imp.h b/actionlib/include/actionlib/server/simple_action_server_imp.h index 67f9104c..b408a0e0 100644 --- 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::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_)) {