search

ros-navigation / navigation2

ROS 2 Navigation Framework and System
https://nav2.org/
Other
2.6k stars 1.3k forks source link

Nav Through Poses has problems with loops in path using DWB #3224

Closed wilcobonestroo closed 2 years ago

wilcobonestroo commented 2 years ago

Bug report

Steps to reproduce issue

I started the default setup using the following commands:

source /opt/ros/humble/setup.bash
export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/opt/ros/humble/share/turtlebot3_gazebo/models
ros2 launch nav2_bringup tb3_simulation_launch.py headless:=False

I gave the initial pose estimation. Then I clicked Waypoint / Nav through Poses button. I entered some waypoints that include a loop or crossing. When the robot gets at the intersection, it starts rotating.

image

Expected behavior

The robot should follow the path, going through waypoint 1, 2, etc.

Actual behavior

The robot gets stuck at the intersection. The console shows the following errors:

[component_container_isolated-6] [INFO] [1664204403.282001500] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1664204406.282359100] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1664204409.332028400] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [ERROR] [1664204409.432006700] [controller_server]: Failed to make progress
[component_container_isolated-6] [WARN] [1664204409.432204900] [controller_server]: [follow_path] [ActionServer] Aborting handle.
[component_container_isolated-6] [INFO] [1664204409.448672000] [local_costmap.local_costmap]: Received request to clear entirely the local_costmap
[component_container_isolated-6] [INFO] [1664204409.451335100] [controller_server]: Received a goal, begin computing control effort.
[component_container_isolated-6] [WARN] [1664204409.522955600] [controller_server]: Control loop missed its desired rate of 20.0000Hz
[component_container_isolated-6] [INFO] [1664204412.358360200] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1664204415.408384000] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1664204418.458356800] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [INFO] [1664204421.458438800] [controller_server]: Passing new path to controller.
[component_container_isolated-6] [ERROR] [1664204421.558496500] [controller_server]: Failed to make progress
[component_container_isolated-6] [WARN] [1664204421.558721700] [controller_server]: [follow_path] [ActionServer] Aborting handle.
[component_container_isolated-6] [INFO] [1664204421.578224000] [local_costmap.local_costmap]: Received request to clear entirely the local_costmap
[component_container_isolated-6] [INFO] [1664204421.578585500] [global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[component_container_isolated-6] [WARN] [1664204421.803100300] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 4.4701 Hz
[component_container_isolated-6] [INFO] [1664204421.824858300] [controller_server]: Received a goal, begin computing control effort.
[component_container_isolated-6] [INFO] [1664204424.875638700] [controller_server]: Passing new path to controller.

Sometimes I also get these.

[component_container_isolated-6] [WARN] [1664203791.993742800] [controller_server]: No valid trajectories out of 419!
[component_container_isolated-6] [ERROR] [1664203792.040393900] [DWBLocalPlanner]: No valid trajectories out of 419!
[component_container_isolated-6] [ERROR] [1664203792.040527500] [DWBLocalPlanner]: 1.00: BaseObstacle/Trajectory Hits Obstacle.
[component_container_isolated-6] [WARN] [1664203792.041014400] [controller_server]: No valid trajectories out of 419!
[component_container_isolated-6] [ERROR] [1664203792.090633200] [DWBLocalPlanner]: No valid trajectories out of 419!
[component_container_isolated-6] [ERROR] [1664203792.092851800] [DWBLocalPlanner]: 1.00: BaseObstacle/Trajectory Hits Obstacle.
[component_container_isolated-6] [WARN] [1664203792.094528000] [controller_server]: No valid trajectories out of 419!
[component_container_isolated-6] [ERROR] [1664203792.096045500] [controller_server]: Controller patience exceeded
[component_container_isolated-6] [WARN] [1664203792.096126300] [controller_server]: [follow_path] [ActionServer] Aborting handle.
[component_container_isolated-6] [INFO] [1664203792.115866700] [behavior_server]: Attempting backup
[component_container_isolated-6] [WARN] [1664203792.116672200] [behavior_server]: Collision Ahead - Exiting DriveOnHeading
[component_container_isolated-6] [WARN] [1664203792.116730700] [behavior_server]: backup failed
[component_container_isolated-6] [WARN] [1664203792.116753700] [behavior_server]: [backup] [ActionServer] Aborting handle.
[component_container_isolated-6] [INFO] [1664203792.141993900] [local_costmap.local_costmap]: Received request to clear entirely the local_costmap
[component_container_isolated-6] [INFO] [1664203792.142767400] [global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[component_container_isolated-6] [WARN] [1664203792.799159500] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 1.5282 Hz
[component_container_isolated-6] [INFO] [1664203792.818036800] [controller_server]: Received a goal, begin computing control effort.
[component_container_isolated-6] [ERROR] [1664203792.825989200] [DWBLocalPlanner]: No valid trajectories out of 419!
[component_container_isolated-6] [ERROR] [1664203792.826280000] [DWBLocalPlanner]: 1.00: BaseObstacle/Trajectory Hits Obstacle.
[component_container_isolated-6] [WARN] [1664203792.826748200] [controller_server]: No valid trajectories out of 419!
[component_container_isolated-6] [ERROR] [1664203792.875194100] [DWBLocalPlanner]: No valid trajectories out of 419!
[component_container_isolated-6] [ERROR] [1664203792.875318400] [DWBLocalPlanner]: 1.00: BaseObstacle/Trajectory Hits Obstacle.
[component_container_isolated-6] [WARN] [1664203792.875847900] [controller_server]: No valid trajectories out of 419!
[component_container_isolated-6] [ERROR] [1664203792.924911100] [DWBLocalPlanner]: No valid trajectories out of 419!

It keeps doing this for a while and then gives up.

Additional information

SteveMacenski commented 2 years ago

This is nav through poses, not waypoint follower. The waypoint follower won't have all those paths set up to have awareness of that loop on the global scale.

See https://github.com/ros-planning/navigation2/issues/2622 where this was discussed at length and we added fixes for how the controllers use that information. It looks like that work was applied to DWB, but check the linked PRs and let me know if we missed something there. Have you tested with RPP? That one was the main testing ground for that work so that should work properly as a baseline.

This is largely dependent on how the controllers handle the path inputs, rather than being an artifact of the rest of the stack / nav through poses.

wilcobonestroo commented 2 years ago

It is indeed the same issue as #2622. I will have a try with RPP.

SteveMacenski commented 2 years ago

Well it should work with DWB too, so if you find it works with RPP and not DWB, that's worth opening the ticket again for and seeing what we missed when we ported the work from RPP to DWB.

hg5j9k6a commented 6 months ago

I'm not sure is it good to leave comment here.

I'm currently facing the same issue of intersecting path, and I'm using released branch of 1.1.12 , RPP controller and Smac Hybrid-A* Planner.

most of the time, robot will suddenly track the end part of path at intersection of path as video below:

https://github.com/ros-navigation/navigation2/assets/48769492/e6b6f508-37a4-44bd-a17e-83f836a9f6b7

due to the RPP controller is using function: nav2_util::geometry_utils::min_by to find the closest pose on the path, i tried to modify some code from /geometry_utils.hpp

the result is good to me, and here is the result video:

https://github.com/ros-navigation/navigation2/assets/48769492/a90b6448-fe86-4612-a564-355c8f943aa0

https://github.com/ros-navigation/navigation2/assets/48769492/bd3ae864-0d51-4568-9a18-e61e174df8ab

the code i changed is shown below: added new min_by function for RPP: image image

the idea is came from #2622 from @SteveMacenski , and the few assumption i made is:

  1. the RPP controller should be robust enough to handle a small error of start pose from path if the error is within resolution value.
  2. based on grid map, if intersection occurred in same grid (or within resolution value) , then we choose the old pose we found before.
  3. RPP controller erases the passed parts of path.

however, in case of given a static path and robot position off the path, where robot is closer to reset part of path, it will change the tracking path to the reset part of path as video below:

https://github.com/ros-navigation/navigation2/assets/48769492/00917bcc-a3ce-422d-8d76-f61b6014d727

i believe this result is acceptable and if robot off the path, it should replan the path.

SteveMacenski commented 5 months ago

My concern there is the hyper parameter of map resolution - is this quantity really handled by the map resolution? I think as you showed, the point it selected wasn't within the same cell. Because the robot didn't exactly follow the path, it was slightly closer to the other cell. That distance of path tracking error could be small/large depending on the algorithm or the choice of parameters. I don't think its proportionate to the map resolution - though I can see how that's an improvement for the particular situation you point to in this constrained environment.

We introduce the max_robot_pose_search_dist for this reason to limit the maximum distance in the future to search for the closest point. What value do you have for that? Perhaps that should be reduced so that you don't search unnecessarily far into the future. By default we have it set to the costmap's window size / 2 (https://github.com/ros-navigation/navigation2/blob/main/nav2_regulated_pure_pursuit_controller/src/parameter_handler.cpp#L94), but that can/should be reduced for any given application. By default, we wanted to set it to the highest theoretical needed for robot motion for higher speed applications. You're no where near those speeds where the robot might hit the edge of the local costmap during a control cycle, so you can reduce that significantly which would negate these issues. Try something like 1m and you should be good to go. You just need to make sure its larger than the max speed * control rate + some margin.

https://github.com/ros-navigation/navigation2/blob/main/nav2_regulated_pure_pursuit_controller/src/path_handler.cpp#L69

hg5j9k6a commented 5 months ago

Thank you for your reply, it inspired me to solve my problem in a different way.

As you mention:

That distance of path tracking error could be small/large depending on the algorithm or the choice of parameters.

I really agree with that, because my assumption is based on perfect controller with no error, and it will be hard to find a reasonable value when it comes error..

But after you point to max_robot_pose_search_dist, i start wondering if there was any good method to find a boundary of path that interested me. Then i tried to find a path without fallback for closest_pose_upper_bound

Here is the thing i tried: I created a parameter robot_pose_search_fallback_tolerance for checking upper bound of path that won't have fallback part, and robot_pose_search_fallback_tolerance should greater than or equal to xy_tolerance. and I added a new function of first_after_integrated_distance_without_fallback: image

The results shown below are even better than the results I showed earlier.

https://github.com/ros-navigation/navigation2/assets/48769492/a04b6234-2934-40ee-aceb-63c4587d345f

https://github.com/ros-navigation/navigation2/assets/48769492/d506fd85-e46b-4354-82ce-922552f08627

If you have any other concern or suggestion please let me know.

SteveMacenski commented 5 months ago

I don't fully understand the fallback bit, why is that necessary if you set the max search distance appropriately for your application? What does that add in addition that makes it more robust? I certainly don't mind modifications / improvements, but I don't immediately see

hg5j9k6a commented 5 months ago

yes, you're right!!! If I set the maximum search distance appropriately, the behavior is what I expect and I don't need to modify anything. max_robot_pose_search_dist solved my problem. thanks for your help.