Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

nav2_simple_commander skips waypoints after every recovery action #3107

Closed
MrOCW opened this issue Aug 11, 2022 · 4 comments
Closed

nav2_simple_commander skips waypoints after every recovery action #3107

MrOCW opened this issue Aug 11, 2022 · 4 comments

Comments

@MrOCW
Copy link

MrOCW commented Aug 11, 2022

Bug report

Required Info:

  • Operating System:
    • Ubuntu 22.04 docker container
  • ROS2 Version:
    • Humble
  • Version or commit hash: 1.1.0-2jammy.20220621.010619
  • DDS implementation:
    • CycloneDDS

Steps to reproduce issue

Use more goal poses in example_waypoint_follower.py

Expected behavior

Only execute next waypoint when robot is near the waypoint

Actual behavior

The waypoints are skipped very quickly, presumably during the recovery actions
@SteveMacenski
Copy link
Member

Can you provide logs? It could be that navigation straight up failed and said it was moving onto the next waypoint because you had stop_on_failure set to false, so it moved into the next one. It would be helpful to see the logs from what you mention to determine if this is not due to navigation failure but some other issue specific to the waypoint follower (which I suspect has nothing to do with the simple commander API, but I could be wrong).

Also, do you have some reproducable set of waypoints / map you can share? That would make helping fix this much easier.

@SteveMacenski
Copy link
Member

SteveMacenski commented Aug 19, 2022 •
edited
Loading

Can you see if #3130 also fixes your problem? From a bunch of testing in that, I could not reproduce any kind of skipping, but I could see how this might be the same root cause.

If some extraneous result's come in late, it could cause a failure, which causes a failure, and could trickle out for a hot minute resulting in a few failures in a row very quickly. This patch checks if the goal IDs match before accepting its result to process, so that should put an end to that kind of error.

But logs would be superbly helpful if this doesn't fix your issue @MrOCW

@MrOCW
Copy link
Author

MrOCW commented Aug 22, 2022

Here is a portion of the logs. It seems that the waypoints move on immediately once the goal fails now. Is this expected behaviour? If i would like the robot to try several times, I should increase the number of retries parameter in the behaviour tree?

nav2_1    | [bt_navigator-11] [INFO] [1661136773.276493586] [bt_navigator]: Begin navigating from current location to (97.51, 4.31)
nav2_1    | [planner_server-9] [WARN] [1661136774.418519992] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 0.8820 Hz
nav2_1    | [controller_server-7] [INFO] [1661136774.465664858] [controller_server]: Received a goal, begin computing control effort.
nav2_1    | [controller_server-7] [WARN] [1661136777.520982181] [controller_server]: TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
nav2_1    | [controller_server-7] [WARN] [1661136777.867463541] [controller_server]: Control loop missed its desired rate of 5.0000Hz
nav2_1    | [controller_server-7] [WARN] [1661136778.334963232] [controller_server]: TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
nav2_1    | [controller_server-7] [WARN] [1661136778.675395155] [controller_server]: Control loop missed its desired rate of 5.0000Hz
nav2_1    | [controller_server-7] [WARN] [1661136778.738055393] [controller_server]: TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
nav2_1    | [controller_server-7] [WARN] [1661136779.073171550] [controller_server]: Control loop missed its desired rate of 5.0000Hz
nav2_1    | [planner_server-9] [WARN] [1661136779.189834102] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 0.8469 Hz
nav2_1    | [bt_navigator-11] [WARN] [1661136779.242474424] [bt_navigator_navigate_to_pose_rclcpp_node]: Timed out while waiting for action server to acknowledge goal request for follow_path
nav2_1    | [controller_server-7] [INFO] [1661136779.243496078] [local_costmap.local_costmap]: Received request to clear entirely the local_costmap
nav2_1    | [bt_navigator-11] [WARN] [1661136779.265946980] [bt_navigator_navigate_to_pose_rclcpp_node]: Node timed out while executing service call to local_costmap/clear_entirely_local_costmap.
nav2_1    | [controller_server-7] [INFO] [1661136779.271185078] [local_costmap.local_costmap]: KeepoutFilter: Subscribing to "/costmap_filter_info" topic for filter info...
nav2_1    | [controller_server-7] [INFO] [1661136779.279633488] [local_costmap.local_costmap]: Received request to clear entirely the local_costmap
nav2_1    | [bt_navigator-11] [WARN] [1661136779.288937719] [bt_navigator_navigate_to_pose_rclcpp_node]: Node timed out while executing service call to local_costmap/clear_entirely_local_costmap.
nav2_1    | [controller_server-7] [INFO] [1661136779.291856944] [local_costmap.local_costmap]: KeepoutFilter: Subscribing to "/costmap_filter_info" topic for filter info...
nav2_1    | [behavior_server-10] [INFO] [1661136779.302867472] [behavior_server]: Attempting backup
nav2_1    | [controller_server-7] [INFO] [1661136779.310350775] [local_costmap.local_costmap]: KeepoutFilter: Received filter info from /costmap_filter_info topic.
nav2_1    | [controller_server-7] [INFO] [1661136779.310473182] [local_costmap.local_costmap]: KeepoutFilter: Subscribing to "/keepout_filter_mask" topic for filter mask...
nav2_1    | [behavior_server-10] [WARN] [1661136779.315353693] [behavior_server]: Collision Ahead - Exiting DriveOnHeading
nav2_1    | [behavior_server-10] [WARN] [1661136779.315450242] [behavior_server]: backup failed
nav2_1    | [behavior_server-10] [WARN] [1661136779.315497637] [behavior_server]: [backup] [ActionServer] Aborting handle.
nav2_1    | [bt_navigator-11] [ERROR] [1661136779.344600535] [bt_navigator]: Goal failed
nav2_1    | [bt_navigator-11] [WARN] [1661136779.344723710] [bt_navigator]: [navigate_to_pose] [ActionServer] Aborting handle.
nav2_1    | [controller_server-7] [INFO] [1661136779.417573985] [controller_server]: Passing new path to controller.
nav2_1    | [controller_server-7] [WARN] [1661136779.495357447] [controller_server]: Control loop missed its desired rate of 5.0000Hz
nav2_1    | [waypoint_follower-12] [INFO] [1661136780.022212083] [waypoint_follower]: Failed to process waypoint 3, moving to next.
nav2_1    | [waypoint_follower-12] [INFO] [1661136780.072020225] [waypoint_follower]: Processing waypoint 4...
nav2_1    | [bt_navigator-11] [INFO] [1661136780.077128940] [bt_navigator]: Begin navigating from current location to (120.20, 1.24)
nav2_1    | [waypoint_follower-12] [INFO] [1661136780.121884978] [waypoint_follower]: Processing waypoint 4...
nav2_1    | [waypoint_follower-12] [INFO] [1661136780.171814886] [waypoint_follower]: Processing waypoint 4...
nav2_1    | [waypoint_follower-12] [INFO] [1661136780.222203987] [waypoint_follower]: Processing waypoint 4...
nav2_1    | [waypoint_follower-12] [INFO] [1661136780.272195146] [waypoint_follower]: Processing waypoint 4...
nav2_1    | [waypoint_follower-12] [INFO] [1661136780.331481640] [waypoint_follower]: Processing waypoint 4...
nav2_1    | [waypoint_follower-12] [INFO] [1661136780.372248765] [waypoint_follower]: Processing waypoint 4...
nav2_1    | [waypoint_follower-12] [INFO] [1661136780.423071808] [waypoint_follower]: Processing waypoint 4...
nav2_1    | [waypoint_follower-12] [INFO] [1661136780.472516443] [waypoint_follower]: Processing waypoint 4...
nav2_1    | [waypoint_follower-12] [INFO] [1661136780.525312261] [waypoint_follower]: Processing waypoint 4...
nav2_1    | [waypoint_follower-12] [INFO] [1661136780.573037926] [waypoint_follower]: Processing waypoint 4...
nav2_1    | [waypoint_follower-12] [INFO] [1661136780.622619720] [waypoint_follower]: Processing waypoint 4...
nav2_1    | [waypoint_follower-12] [INFO] [1661136780.671786612] [waypoint_follower]: Processing waypoint 4...
nav2_1    | [waypoint_follower-12] [INFO] [1661136780.722445071] [waypoint_follower]: Processing waypoint 4...
nav2_1    | [waypoint_follower-12] [INFO] [1661136780.773655334] [waypoint_follower]: Processing waypoint 4...
nav2_1    | [waypoint_follower-12] [INFO] [1661136780.827807543] [waypoint_follower]: Processing waypoint 4...
nav2_1    | [waypoint_follower-12] [INFO] [1661136780.872313200] [waypoint_follower]: Processing waypoint 4...
nav2_1    | [waypoint_follower-12] [INFO] [1661136780.923498182] [waypoint_follower]: Processing waypoint 4...
nav2_1    | [waypoint_follower-12] [INFO] [1661136780.971743778] [waypoint_follower]: Processing waypoint 4...
nav2_1    | [controller_server-7] [WARN] [1661136781.569261151] [controller_server]: TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
nav2_1    | [planner_server-9] [WARN] [1661136781.693443008] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 0.6241 Hz
nav2_1    | [controller_server-7] [INFO] [1661136781.866213679] [controller_server]: Passing new path to controller.
nav2_1    | [controller_server-7] [WARN] [1661136782.915411420] [controller_server]: TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
nav2_1    | [controller_server-7] [WARN] [1661136783.282736110] [controller_server]: Control loop missed its desired rate of 5.0000Hz
nav2_1    | [controller_server-7] [WARN] [1661136783.527088223] [controller_server]: TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
nav2_1    | [controller_server-7] [ERROR] [1661136784.469319727] [controller_server]: Failed to make progress
nav2_1    | [controller_server-7] [WARN] [1661136784.472586138] [controller_server]: [follow_path] [ActionServer] Aborting handle.
nav2_1    | [controller_server-7] [INFO] [1661136784.491193800] [local_costmap.local_costmap]: Received request to clear entirely the local_costmap
nav2_1    | [bt_navigator-11] [WARN] [1661136784.505196964] [bt_navigator_navigate_to_pose_rclcpp_node]: Node timed out while executing service call to local_costmap/clear_entirely_local_costmap.
nav2_1    | [controller_server-7] [INFO] [1661136784.512280983] [local_costmap.local_costmap]: KeepoutFilter: Subscribing to "/costmap_filter_info" topic for filter info...
nav2_1    | [controller_server-7] [INFO] [1661136784.513772645] [local_costmap.local_costmap]: Received request to clear entirely the local_costmap
nav2_1    | [controller_server-7] [INFO] [1661136784.525750520] [local_costmap.local_costmap]: KeepoutFilter: Subscribing to "/costmap_filter_info" topic for filter info...
nav2_1    | [bt_navigator-11] [WARN] [1661136784.527724383] [bt_navigator_navigate_to_pose_rclcpp_node]: Node timed out while executing service call to local_costmap/clear_entirely_local_costmap.
nav2_1    | [behavior_server-10] [INFO] [1661136784.538463889] [behavior_server]: Attempting backup
nav2_1    | [controller_server-7] [INFO] [1661136784.539667184] [local_costmap.local_costmap]: KeepoutFilter: Received filter info from /costmap_filter_info topic.
nav2_1    | [controller_server-7] [INFO] [1661136784.539935902] [local_costmap.local_costmap]: KeepoutFilter: Subscribing to "/keepout_filter_mask" topic for filter mask...
nav2_1    | [behavior_server-10] [WARN] [1661136784.558909663] [behavior_server]: Collision Ahead - Exiting DriveOnHeading
nav2_1    | [behavior_server-10] [WARN] [1661136784.559033061] [behavior_server]: backup failed
nav2_1    | [behavior_server-10] [WARN] [1661136784.559093736] [behavior_server]: [backup] [ActionServer] Aborting handle.
nav2_1    | [bt_navigator-11] [ERROR] [1661136784.579316490] [bt_navigator]: Goal failed
nav2_1    | [bt_navigator-11] [WARN] [1661136784.579462546] [bt_navigator]: [navigate_to_pose] [ActionServer] Aborting handle.
nav2_1    | [waypoint_follower-12] [INFO] [1661136785.273343329] [waypoint_follower]: Failed to process waypoint 4, moving to next.
nav2_1    | [bt_navigator-11] [INFO] [1661136785.324675199] [bt_navigator]: Begin navigating from current location to (143.25, -4.90)

@SteveMacenski
Copy link
Member

It seems that the waypoints move on immediately once the goal fails now. Is this expected behaviour?

Yes, retry mechanisms should be exhausted as part of the behavior tree. If navigation tells us it has failed, the waypoint follow are basically have to take it at face value. You can stop on failure using a parameter though if you’d rather stop the waypoint following task entirely on a failure.

I should increase the number of retries parameter in the behaviour tree

That would be my recommendation. Optionally, we could consider adding a retry mechanism into the waypoint follower. but it seems like the likely best place for it is the behavior tree itself so it can be more informed.

Sounds like the PR / that explanation buttons things up!

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

No branches or pull requests
2 participants
@SteveMacenski @MrOCW