(do move_base to Waypoint) after executing non-move_base action
bfalacerda opened this issue · 6 comments
I just saw a situation where the robot tries to go through a closed door forever. The inside door waypoint is not completely facing the outside door waypoint, so the robot rotates a little bit when door passing starts. Then, door passing fails bacause the door is closed. Execute policy mode tries again (as it should), but since the robot has now rotated a bit, move_base to inside door waypoint is called and he rotates a bit back. This is successful, which resets the monitored navigation fails counter. He then tries door passing again, fails, and so on. Here's the output:
[INFO] [WallTime: 1428941132.152326] Front laser door check results. closed_door_counter=59 , open_door_counter=0 [144/1984]
[INFO] [WallTime: 1428941132.153087] Door is closed. Disabling monitored navigation recoveries.
[INFO] [WallTime: 1428941132.172776] Waiting for monitored navigation to stop executing
[INFO] [WallTime: 1428941132.196063] State machine transitioning 'NAVIGATION':'planner_failure'-->'RECOVER_NAVIGATION'
[INFO] [WallTime: 1428941132.202127] State machine starting in initial state 'SLEEP_AND_RETRY' with userdata:
['goal', 'n_fails']
[WARN] [WallTime: 1428941132.207578] Recover state sleep_and_retry is not active. Not executing.
[INFO] [WallTime: 1428941132.208504] State machine transitioning 'SLEEP_AND_RETRY':'not_active'-->'BACKTRACK'
[WARN] [WallTime: 1428941132.211917] Recover state backtrack is not active. Not executing.
[INFO] [WallTime: 1428941132.212617] State machine transitioning 'BACKTRACK':'not_active'-->'NAV_HELP'
[WARN] [WallTime: 1428941132.217441] Recover state nav_help is not active. Not executing.
[INFO] [WallTime: 1428941132.218024] State machine terminating 'NAV_HELP':'not_active':'not_recovered_without_help'
[INFO] [WallTime: 1428941132.218902] State machine terminating 'RECOVER_NAVIGATION':'not_recovered_without_help':'not_recovered_without_help'
[INFO] [WallTime: 1428941132.219424] Concurrent state 'NAV_SM' returned outcome 'not_recovered_without_help' on termination.
[INFO] [WallTime: 1428941132.224702] Concurrent state 'stuck_on_carpet' returned outcome 'preempted' on termination.
[INFO] [WallTime: 1428941132.224783] Concurrent state 'bumper' returned outcome 'preempted' on termination.
[INFO] [WallTime: 1428941132.232808] Concurrent state 'service_pause_nav' returned outcome 'preempted' on termination.
[INFO] [WallTime: 1428941132.236819] Concurrent state 'pad_pause_nav' returned outcome 'preempted' on termination.
[INFO] [WallTime: 1428941132.237406] Concurrent state 'magnetic_strip' returned outcome 'preempted' on termination.
[INFO] [WallTime: 1428941132.240545] Concurrent Outcomes: {'stuck_on_carpet': 'preempted', 'magnetic_strip': 'preempted', 'service_pause_nav': 'preemp$
ed', 'bumper': 'preempted', 'pad_pause_nav': 'preempted', 'NAV_SM': 'not_recovered_without_help'}
[INFO] [WallTime: 1428941132.241069] State machine terminating 'MONITORED_NAV':'not_recovered_without_help':'not_recovered_without_help'
[INFO] [WallTime: 1428941132.241555] ABORTED
[INFO] [WallTime: 1428941132.273524] Monitored navigation stopped executing. Resetting monitored navigation recoveries.
[INFO] [WallTime: 1428941132.351508] Waiting for services...
[INFO] [WallTime: 1428941132.366709] Done
Do move_base to WayPoint8
[INFO] [WallTime: 1428941132.708934] State machine starting in initial state 'MONITORED_NAV' with userdata:
['goal', 'result']
[INFO] [WallTime: 1428941132.710239] Concurrence starting with userdata:
['goal']
[INFO] [WallTime: 1428941132.725103] State machine starting in initial state 'NAVIGATION' with userdata:
['goal', 'n_fails']
[ INFO] [1428941132.881659829]: Got new plan
[WARN] [WallTime: 1428941133.224235] Inbound TCP/IP connection failed: connection from sender terminated before handshake header received. 0 bytes wer$
received. Please check sender for additional details.
[ INFO] [1428941133.581672232]: Goal reached
[INFO] [WallTime: 1428941133.605997] State machine terminating 'NAVIGATION':'succeeded':'succeeded'
The robot should not reset the fail counter when he succeeds on a "Do move_base to WayPoint" thing. Also, I'm not sure if "Do move_base to WayPoint" should even be called when a non-move_base action fails. @Jailander what do you think?
just saw that this happens all the time, (Do move base to Waypoint) is always called, regardless of the pose of the robot
the move_base to waypoint was introduced to make sure the robot navigates to the waypoint when its within the influence zone of a node, maybe I can add some verification if this is needed, not sure I'll take a look, ASAP
yes, i remember we discussing that. Now, i think it should only be done in initialisation though. after that i think it creates more issues than it solves in most cases. also it shouldnt be used to reset the consecutive failure count
yes I think so too, I will change as soon as possible is there any simulation file or something I can try to test it?
not really... an easy way is to test door pass with a closed door. use my fork though, as the current one upstream asks for help when the door is closed and it takes a long time to see anything:
Ok, I'll try to find a good test for it then :)