Wait for replanning_period also when starting a goal

naturerobots · Nov 23, 2023 · b6d30ba · b6d30ba
1 parent 85898d0
commit b6d30ba
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
@@ -535,7 +535,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_)
   {
@@ -564,6 +564,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_)