Wait for replanning_period also when starting a goal

naturerobots · Nov 21, 2023 · e7df6d9 · e7df6d9
1 parent c02460e
commit e7df6d9
Showing 1 changed file with 2 additions and 1 deletion.
diff --git a/mbf_abstract_nav/src/move_base_action.cpp b/mbf_abstract_nav/src/move_base_action.cpp
@@ -531,7 +531,7 @@ bool MoveBaseAction::replanningActive() const
 void MoveBaseAction::replanningThread()
 {
   ros::Duration update_period(0.005);
-  ros::Time last_replan_time(0.0);
+  ros::Time last_replan_time = ros::Time::now();
 
   while (ros::ok() && !replanning_thread_shutdown_)
   {
@@ -560,6 +560,7 @@ void MoveBaseAction::replanningThread()
     }
     else if (!replanningActive())
     {
+      last_replan_time = ros::Time::now();
       update_period.sleep();
     }
     else if (ros::Time::now() - last_replan_time >= replanning_period_)