Nav2 Controller Server, Goal Preemption
Working with Nav2 Foxy, and implementing a node which handles sending goals to the Nav2 controller server. Goals are cancelled or completely successfully, at which point another goal is sent. However, on a new goal being sent, the controller reports that the previous goal was preempted which in turn forces the new plan to be the old path until the controller loop updates. Logging below shows the issue, before "Passing New Path To Controller" it is using the old goal.
[navigation_control-17] [INFO] [1670446675.441920671] [navigation_control]: Sending goal (Type 7): 30.500000 , 36.400001
[bt_navigator-21] [INFO] [1670446675.446984210] [bt_navigator]: Begin navigating from current location to (30.50, 36.40)
[navigation_control-17] [INFO] [1670446675.457910730] [navigation_control]: Goal accepted by server, waiting for result
[controller_server-18] [INFO] [1670446675.550665196] [controller_server]: Received a goal, begin computing control effort.
[controller_server-18] [INFO] [1670446675.555532174] [controller_server]: Reached the goal!
[controller_server-18] [INFO] [1670446675.565149370] [controller_server]: Received a goal, begin computing control effort.
[controller_server-18] [INFO] [1670446675.576235011] [controller_server]: Reached the goal!
[bt_navigator-21] [INFO] [1670446675.619985254] [bt_navigator]: Navigation succeeded
[navigation_control-17] [INFO] [1670446675.636878917] [navigation_control]: Goal no longer active
[navigation_control-17] [INFO] [1670446675.643558742] [navigation_control]: Sending goal (Type 7): 30.500000 , 38.400001
[bt_navigator-21] [INFO] [1670446675.659430721] [bt_navigator]: Begin navigating from current location to (30.50, 38.40)
[navigation_control-17] [INFO] [1670446675.670092161] [navigation_control]: Goal accepted by server, waiting for result
[controller_server-18] [INFO] [1670446675.755152678] [controller_server]: Received a goal, begin computing control effort.
[controller_server-18] [INFO] [1670446675.755441039] [controller_server]: Passing new path to controller.
[controller_server-18] [INFO] [1670446675.772926784] [controller_server]: Reached the goal!
[bt_navigator-21] [INFO] [1670446675.811669168] [bt_navigator]: Navigation succeeded
Asked by samc11 on 2022-12-07 17:11:51 UTC
Comments
I don't see what you describe in the logs you posted. Can you explain more?
Asked by stevemacenski on 2022-12-08 00:43:46 UTC
Hi, thank you for your response. To elaborate "Passing new path to controller" is logged which appears if the condition "action_server_->is_preempt_requested()" is met. Since I am waiting for goals to succeed or cancel I would not expect preemption. A more detailed log is below, where you can see the bt_navigator outputs "Begin navigating from current location to (5.08, 16.81)" upon receiving a new goal. However the controller_server outputs "Path end point is (11.43, 16.95)" which is the old goal which had been cancelled. It then shows that this goal is preempted and updates the new goal. I hope this helps to further explain my question! Thanks.
1670508439.6482799 [navigation_control-7] [INFO] [1670508439.527812141] [navigation_control]: Sending goal (Type 0): 11.431746 , 16.945609 1670508439.6489308 [navigation_control-7] [INFO] [1670508439.544296306] [navigation_control]: Goal accepted by server, waiting for result 1670508439.6506686 [bt_navigator-11] [INFO] [1670508439.544954156] [bt_navigator]: Begin navigating from current location to (11.43, 16.95) 1670508439.7157698 [controller_server-8] [INFO] [1670508439.601842649] [controller_server]: Received a goal, begin computing control effort. 1670508439.7170134 [controller_server-8] [DEBUG] [1670508439.602150518] [controller_server]: Selected controller: FollowPath. 1670508439.7175984 [controller_server-8] [DEBUG] [1670508439.602203309] [controller_server]: Providing path to the controller FollowPath 1670508439.7190056 [controller_server-8] [DEBUG] [1670508439.602317890] [controller_server]: Path end point is (11.43, 16.95) 1670508439.7313063 [controller_server-8] [INFO] [1670508439.602753537] [controller_server]: Passing new path to controller. 1670508439.7372839 [controller_server-8] [DEBUG] [1670508439.605753996] [controller_server]: Selected controller: FollowPath. 1670508439.7403157 [controller_server-8] [DEBUG] [1670508439.605937205] [controller_server]: Providing path to the controller FollowPath 1670508439.7409306 [controller_server-8] [DEBUG] [1670508439.606421924] [controller_server]: Path end point is (11.43, 16.95) 1670508447.5104737 [controller_server-8] [INFO] [1670508447.402847339] [controller_server]: Goal was canceled. Stopping the robot. 1670508447.5708020 [navigation_control-7] [INFO] [1670508447.422364369] [navigation_control]: Sending back up goal 1670508447.5713003 [navigation_control-7] [INFO] [1670508447.429736077] [navigation_control]: Goal accepted by server, waiting for result 1670508447.5730369 [bt_navigator-11] [INFO] [1670508447.404076077] [bt_navigator]: Navigation canceled 1670508447.5737481 [bt_navigator-11] [WARN] [1670508447.404356115] [bt_navigator]: [navigate_to_pose] [ActionServer] Client requested to cancel the goal. Cancelling. 1670508447.5754657 [recoveries_server-10] [INFO] [1670508447.457726493] [recoveries_server]: Attempting back_up 1670508450.2963548 [recoveries_server-10] [INFO] [1670508450.264971521] [recoveries_server]: back_up completed successfully 1670508450.2983766 [navigation_control-7] [INFO] [1670508450.273197316] [navigation_control]: Goal no longer active 1670508450.3227456 [navigation_control-7] [INFO] [1670508450.321488562] [navigation_control]: Sending spin goal 1670508450.3321536 [navigation_control-7] [INFO] [1670508450.326789052] [navigation_control]: Goal accepted by server, waiting for result 1670508450.3352933 [recoveries_server-10] [INFO] [1670508450.327759954] [recoveries_server]: Attempting spin 1670508450.3361099 [recoveries_server-10] [INFO] [1670508450.328863678] [recoveries_server]: Turning 1.59 for spin recovery. 1670508453.2468665 [recoveries_server-10] [INFO] [1670508453.231341274] [recoveries_server]: spin completed successfully 1670508453.2664487 [navigation_control-7] [INFO] [1670508453.244491975] [navigation_control]: Goal no longer active 1670508453.3268633 [navigation_control-7] [INFO] [1670508453.321070056] [navigation_control]: Sending goal (Type 0): 5.078782 , 16.809747 1670508453.3344581 [navigation_control-7] [INFO] [1670508453.323832682] [navigation_control]: Goal accepted by server, waiting for result 1670508453.3683445 [bt_navigator-11] [INFO] [1670508453.322701115] [bt_navigator]: Begin navigating from current location to (5.08, 16.81) 1670508453.4910707 [controller_server-8] [INFO] [1670508453.363933297] [controller_server]: Received a goal, begin computing control effort. 1670508453.4914818 [controller_server-8] [DEBUG] [1670508453.363988250] [controller_server]: Selected controller: FollowPath. 1670508453.4918528 [controller_server-8] [DEBUG] [1670508453.364033124] [controller_server]: Providing path to the controller FollowPath 1670508453.5028670 [controller_server-8] [DEBUG] [1670508453.370470493] [controller_server]: Path end point is (11.43, 16.95) 1670508453.5032606 [controller_server-8] [INFO] [1670508453.370673625] [controller_server]: Passing new path to controller. 1670508453.5073004 [controller_server-8] [DEBUG] [1670508453.371699606] [controller_server]: Selected controller: FollowPath. 1670508453.5081210 [controller_server-8] [DEBUG] [1670508453.371837228] [controller_server]: Providing path to the controller FollowPath 1670508453.5115290 [controller_server-8] [DEBUG] [1670508453.371899379] [controller_server]: Path end point is (5.08, 16.81)Asked by samc11 on 2022-12-08 09:33:17 UTC