search

ros-navigation / navigation2

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

The robot has collision when faced with an obstacle in front direction and the goal is set behind the obstacle #3798

Closed GiantSeaweed closed 1 year ago

GiantSeaweed commented 1 year ago

Bug report

Required Info:

Steps to reproduce issue

I kept every configuration of foxy-devel branch.

Step1. Launch the nodes and simulation.

(Terminal 1) Started turtlebot3_world with: ros2 launch turtlebot3_gazebo turtlebot3_world.launch.py

(Terminal 2) Start slam_toolbox async mode: ros2 launch slam_toolbox online_async_launch.py

(Terminal 3) Start nav2 navigation: ros2 launch nav2_bringup navigation_launch.py

(Terminal 4) Start rviz2: ros2 launch nav2_bringup rviz_launch.py

Step2. I found when the robot is faced with an obstacle in front direction and the destination is set behind the obstacle, the robot cannot successfully navigate around the obstacle to reach the goal. I attached the Terminal 3 console log and video below.

Expected behavior

The robot may spin to a direction with more free room ahead and bypass the obstacle.

Actual behavior

The robot just got stuck, could not follow the planned path, leading to a failed navigation task.

I am not sure if it is related to configuration problems. I can understand that if the tolerance is too narrow the robot cannot bypass the obstacle to reach the goal. But I am confused why it collides into the obstacle. Thanks!!

https://github.com/ros-planning/navigation2/assets/28821472/e3a1a6e1-2100-4ea0-aaa5-171e82c5608d

[bt_navigator-4] [INFO] [1693968008.686269475] [bt_navigator]: Begin navigating from current location to (-1.27, -0.75)
[planner_server-2] [WARN] [1693968008.686876370] [planner_server]: GridBased: failed to create plan with tolerance 0.50.
[planner_server-2] [WARN] [1693968008.686906395] [planner_server]: Planning algorithm GridBased failed to generate a valid path to (-1.27, -0.75)
[planner_server-2] [WARN] [1693968008.686918807] [planner_server_rclcpp_node]: [compute_path_to_pose] [ActionServer] Aborting handle.
[planner_server-2] [INFO] [1693968008.706985920] [global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[controller_server-1] [INFO] [1693968008.726921680] [controller_server]: Received a goal, begin computing control effort.
[controller_server-1] [INFO] [1693968008.777180826] [controller_server]: Passing new path to controller.
[planner_server-2] [WARN] [1693968009.737098910] [planner_server]: GridBased: failed to create plan with tolerance 0.50.
[planner_server-2] [WARN] [1693968009.737130425] [planner_server]: Planning algorithm GridBased failed to generate a valid path to (-1.27, -0.75)
[planner_server-2] [WARN] [1693968009.737137312] [planner_server_rclcpp_node]: [compute_path_to_pose] [ActionServer] Aborting handle.
[planner_server-2] [INFO] [1693968009.756714712] [global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[controller_server-1] [INFO] [1693968009.777152883] [controller_server]: Passing new path to controller.
[controller_server-1] [ERROR] [1693968010.529579780] [DWBLocalPlanner]: No valid trajectories out of 419! 
[controller_server-1] [ERROR] [1693968010.529606501] [DWBLocalPlanner]: 1.00: BaseObstacle/Trajectory Hits Obstacle.
[controller_server-1] [ERROR] [1693968010.529754903] [controller_server]: No valid trajectories out of 419! 
[controller_server-1] [WARN] [1693968010.529788008] [controller_server_rclcpp_node]: [follow_path] [ActionServer] Aborting handle.
[controller_server-1] [INFO] [1693968010.547144978] [local_costmap.local_costmap]: Received request to clear entirely the local_costmap
[controller_server-1] [INFO] [1693968010.547818969] [controller_server]: Received a goal, begin computing control effort.
[planner_server-2] [WARN] [1693968010.777193461] [planner_server]: GridBased: failed to create plan with tolerance 0.50.
[planner_server-2] [WARN] [1693968010.777241375] [planner_server]: Planning algorithm GridBased failed to generate a valid path to (-1.27, -0.75)
[planner_server-2] [WARN] [1693968010.777250261] [planner_server_rclcpp_node]: [compute_path_to_pose] [ActionServer] Aborting handle.
[planner_server-2] [INFO] [1693968010.796811538] [global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[controller_server-1] [INFO] [1693968010.848096875] [controller_server]: Passing new path to controller.
[controller_server-1] [ERROR] [1693968010.951130298] [DWBLocalPlanner]: No valid trajectories out of 419! 
[controller_server-1] [ERROR] [1693968010.951168582] [DWBLocalPlanner]: 1.00: BaseObstacle/Trajectory Hits Obstacle.
[controller_server-1] [ERROR] [1693968010.951326140] [controller_server]: No valid trajectories out of 419! 
[controller_server-1] [WARN] [1693968010.951359895] [controller_server_rclcpp_node]: [follow_path] [ActionServer] Aborting handle.
[controller_server-1] [INFO] [1693968010.966944051] [local_costmap.local_costmap]: Received request to clear entirely the local_costmap
[planner_server-2] [INFO] [1693968010.967353084] [global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[recoveries_server-3] [INFO] [1693968010.967841050] [recoveries_server]: Attempting spin
[recoveries_server-3] [INFO] [1693968010.967880877] [recoveries_server]: Turning 1.57 for spin recovery.
[recoveries_server-3] [WARN] [1693968011.068059838] [recoveries_server]: Collision Ahead - Exiting Spin
[recoveries_server-3] [INFO] [1693968011.068096342] [recoveries_server]: spin completed successfully
[recoveries_server-3] [INFO] [1693968011.097127969] [recoveries_server]: Attempting wait
[recoveries_server-3] [INFO] [1693968012.097309151] [recoveries_server]: wait running...
[recoveries_server-3] [INFO] [1693968013.097241280] [recoveries_server]: wait running...
[recoveries_server-3] [INFO] [1693968014.097275661] [recoveries_server]: wait running...
[recoveries_server-3] [INFO] [1693968015.097406510] [recoveries_server]: wait running...
[recoveries_server-3] [INFO] [1693968016.097261299] [recoveries_server]: wait running...
[recoveries_server-3] [INFO] [1693968016.097277403] [recoveries_server]: wait completed successfully
[planner_server-2] [WARN] [1693968016.127159300] [planner_server]: GridBased: failed to create plan with tolerance 0.50.
[planner_server-2] [WARN] [1693968016.127191543] [planner_server]: Planning algorithm GridBased failed to generate a valid path to (-1.27, -0.75)
[planner_server-2] [WARN] [1693968016.127198562] [planner_server_rclcpp_node]: [compute_path_to_pose] [ActionServer] Aborting handle.
[planner_server-2] [INFO] [1693968016.146864443] [global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[controller_server-1] [INFO] [1693968016.166880598] [controller_server]: Received a goal, begin computing control effort.
[controller_server-1] [INFO] [1693968016.217096612] [controller_server]: Passing new path to controller.
[planner_server-2] [WARN] [1693968017.176974291] [planner_server]: GridBased: failed to create plan with tolerance 0.50.
[planner_server-2] [WARN] [1693968017.177013590] [planner_server]: Planning algorithm GridBased failed to generate a valid path to (-1.27, -0.75)
[planner_server-2] [WARN] [1693968017.177019992] [planner_server_rclcpp_node]: [compute_path_to_pose] [ActionServer] Aborting handle.
[planner_server-2] [INFO] [1693968017.196915311] [global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[controller_server-1] [INFO] [1693968017.217099720] [controller_server]: Passing new path to controller.
[planner_server-2] [WARN] [1693968018.227162882] [planner_server]: GridBased: failed to create plan with tolerance 0.50.
[planner_server-2] [WARN] [1693968018.227198012] [planner_server]: Planning algorithm GridBased failed to generate a valid path to (-1.27, -0.75)
[planner_server-2] [WARN] [1693968018.227204446] [planner_server_rclcpp_node]: [compute_path_to_pose] [ActionServer] Aborting handle.
[planner_server-2] [INFO] [1693968018.246728488] [global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[controller_server-1] [INFO] [1693968018.267099352] [controller_server]: Passing new path to controller.
[planner_server-2] [WARN] [1693968019.277125813] [planner_server]: GridBased: failed to create plan with tolerance 0.50.
[planner_server-2] [WARN] [1693968019.277162805] [planner_server]: Planning algorithm GridBased failed to generate a valid path to (-1.27, -0.75)
[planner_server-2] [WARN] [1693968019.277169664] [planner_server_rclcpp_node]: [compute_path_to_pose] [ActionServer] Aborting handle.
[planner_server-2] [INFO] [1693968019.296803184] [global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[controller_server-1] [INFO] [1693968019.317096828] [controller_server]: Passing new path to controller.
[planner_server-2] [WARN] [1693968020.317025792] [planner_server]: GridBased: failed to create plan with tolerance 0.50.
[planner_server-2] [WARN] [1693968020.317061451] [planner_server]: Planning algorithm GridBased failed to generate a valid path to (-1.27, -0.75)
[planner_server-2] [WARN] [1693968020.317067898] [planner_server_rclcpp_node]: [compute_path_to_pose] [ActionServer] Aborting handle.
[planner_server-2] [INFO] [1693968020.336863049] [global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[controller_server-1] [INFO] [1693968020.367092687] [controller_server]: Passing new path to controller.
[controller_server-1] [ERROR] [1693968020.519547125] [DWBLocalPlanner]: No valid trajectories out of 419! 
[controller_server-1] [ERROR] [1693968020.519584002] [DWBLocalPlanner]: 1.00: BaseObstacle/Trajectory Hits Obstacle.
[controller_server-1] [ERROR] [1693968020.519691958] [controller_server]: No valid trajectories out of 419! 
[controller_server-1] [WARN] [1693968020.519719528] [controller_server_rclcpp_node]: [follow_path] [ActionServer] Aborting handle.
[controller_server-1] [INFO] [1693968020.536913233] [local_costmap.local_costmap]: Received request to clear entirely the local_costmap
[planner_server-2] [INFO] [1693968020.537170159] [global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[recoveries_server-3] [INFO] [1693968020.537455943] [recoveries_server]: Attempting spin
[recoveries_server-3] [INFO] [1693968020.537492934] [recoveries_server]: Turning 1.57 for spin recovery.
[recoveries_server-3] [WARN] [1693968020.837638800] [recoveries_server]: Collision Ahead - Exiting Spin
[recoveries_server-3] [INFO] [1693968020.837663388] [recoveries_server]: spin completed successfully
[recoveries_server-3] [INFO] [1693968020.867033701] [recoveries_server]: Attempting wait
[planner_server-2] [INFO] [1693968021.042331610] [global_costmap.global_costmap]: StaticLayer: Resizing costmap to 112 X 103 at 0.050000 m/pix
[recoveries_server-3] [INFO] [1693968021.867252066] [recoveries_server]: wait running...
[recoveries_server-3] [INFO] [1693968022.867223945] [recoveries_server]: wait running...
[recoveries_server-3] [INFO] [1693968023.867169114] [recoveries_server]: wait running...
[recoveries_server-3] [INFO] [1693968024.867145222] [recoveries_server]: wait running...
[recoveries_server-3] [INFO] [1693968025.867173069] [recoveries_server]: wait running...
[recoveries_server-3] [INFO] [1693968025.867233742] [recoveries_server]: wait completed successfully
[planner_server-2] [WARN] [1693968025.897063191] [planner_server]: GridBased: failed to create plan with tolerance 0.50.
[planner_server-2] [WARN] [1693968025.897108564] [planner_server]: Planning algorithm GridBased failed to generate a valid path to (-1.27, -0.75)
[planner_server-2] [WARN] [1693968025.897116956] [planner_server_rclcpp_node]: [compute_path_to_pose] [ActionServer] Aborting handle.
[planner_server-2] [INFO] [1693968025.916819276] [global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[controller_server-1] [INFO] [1693968025.936926809] [controller_server]: Received a goal, begin computing control effort.
[controller_server-1] [INFO] [1693968025.987208360] [controller_server]: Passing new path to controller.
[planner_server-2] [INFO] [1693968026.043346755] [global_costmap.global_costmap]: StaticLayer: Resizing costmap to 112 X 103 at 0.050000 m/pix
[planner_server-2] [WARN] [1693968026.937256101] [planner_server]: GridBased: failed to create plan with tolerance 0.50.
[planner_server-2] [WARN] [1693968026.937289025] [planner_server]: Planning algorithm GridBased failed to generate a valid path to (-1.27, -0.75)
[planner_server-2] [WARN] [1693968026.937295420] [planner_server_rclcpp_node]: [compute_path_to_pose] [ActionServer] Aborting handle.
[planner_server-2] [INFO] [1693968026.946863944] [global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[controller_server-1] [INFO] [1693968026.987195058] [controller_server]: Passing new path to controller.
[planner_server-2] [WARN] [1693968027.977018949] [planner_server]: GridBased: failed to create plan with tolerance 0.50.
[planner_server-2] [WARN] [1693968027.977050636] [planner_server]: Planning algorithm GridBased failed to generate a valid path to (-1.27, -0.75)
[planner_server-2] [WARN] [1693968027.977056879] [planner_server_rclcpp_node]: [compute_path_to_pose] [ActionServer] Aborting handle.
[planner_server-2] [INFO] [1693968027.996838778] [global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[controller_server-1] [INFO] [1693968028.037210430] [controller_server]: Passing new path to controller.
[planner_server-2] [WARN] [1693968029.017063121] [planner_server]: GridBased: failed to create plan with tolerance 0.50.
[planner_server-2] [WARN] [1693968029.017098658] [planner_server]: Planning algorithm GridBased failed to generate a valid path to (-1.27, -0.75)
[planner_server-2] [WARN] [1693968029.017108704] [planner_server_rclcpp_node]: [compute_path_to_pose] [ActionServer] Aborting handle.
[planner_server-2] [INFO] [1693968029.036893097] [global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[controller_server-1] [INFO] [1693968029.087206720] [controller_server]: Passing new path to controller.
[planner_server-2] [WARN] [1693968030.067236030] [planner_server]: GridBased: failed to create plan with tolerance 0.50.
[planner_server-2] [WARN] [1693968030.067276120] [planner_server]: Planning algorithm GridBased failed to generate a valid path to (-1.27, -0.75)
[planner_server-2] [WARN] [1693968030.067282894] [planner_server_rclcpp_node]: [compute_path_to_pose] [ActionServer] Aborting handle.
[planner_server-2] [INFO] [1693968030.086842790] [global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[controller_server-1] [INFO] [1693968030.137216010] [controller_server]: Passing new path to controller.
[planner_server-2] [WARN] [1693968031.116963311] [planner_server]: GridBased: failed to create plan with tolerance 0.50.
[planner_server-2] [WARN] [1693968031.116994584] [planner_server]: Planning algorithm GridBased failed to generate a valid path to (-1.27, -0.75)
[planner_server-2] [WARN] [1693968031.117000431] [planner_server_rclcpp_node]: [compute_path_to_pose] [ActionServer] Aborting handle.
[planner_server-2] [INFO] [1693968031.136868667] [global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[controller_server-1] [INFO] [1693968031.187201182] [controller_server]: Passing new path to controller.
[planner_server-2] [WARN] [1693968032.167108711] [planner_server]: GridBased: failed to create plan with tolerance 0.50.
[planner_server-2] [WARN] [1693968032.167155065] [planner_server]: Planning algorithm GridBased failed to generate a valid path to (-1.27, -0.75)
[planner_server-2] [WARN] [1693968032.167164688] [planner_server_rclcpp_node]: [compute_path_to_pose] [ActionServer] Aborting handle.
[planner_server-2] [INFO] [1693968032.186745824] [global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[controller_server-1] [INFO] [1693968032.237175118] [controller_server]: Passing new path to controller.
[planner_server-2] [WARN] [1693968033.216979295] [planner_server]: GridBased: failed to create plan with tolerance 0.50.
[planner_server-2] [WARN] [1693968033.217011864] [planner_server]: Planning algorithm GridBased failed to generate a valid path to (-1.27, -0.75)
[planner_server-2] [WARN] [1693968033.217017793] [planner_server_rclcpp_node]: [compute_path_to_pose] [ActionServer] Aborting handle.
[planner_server-2] [INFO] [1693968033.226898727] [global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[controller_server-1] [INFO] [1693968033.287195106] [controller_server]: Passing new path to controller.
[planner_server-2] [WARN] [1693968034.256921845] [planner_server]: GridBased: failed to create plan with tolerance 0.50.
[planner_server-2] [WARN] [1693968034.256951739] [planner_server]: Planning algorithm GridBased failed to generate a valid path to (-1.27, -0.75)
[planner_server-2] [WARN] [1693968034.256958531] [planner_server_rclcpp_node]: [compute_path_to_pose] [ActionServer] Aborting handle.
[planner_server-2] [INFO] [1693968034.276888081] [global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[controller_server-1] [INFO] [1693968034.287185919] [controller_server]: Passing new path to controller.
[planner_server-2] [WARN] [1693968035.296949653] [planner_server]: GridBased: failed to create plan with tolerance 0.50.
[planner_server-2] [WARN] [1693968035.296986206] [planner_server]: Planning algorithm GridBased failed to generate a valid path to (-1.27, -0.75)
[planner_server-2] [WARN] [1693968035.296992984] [planner_server_rclcpp_node]: [compute_path_to_pose] [ActionServer] Aborting handle.
[planner_server-2] [INFO] [1693968035.306826429] [global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[controller_server-1] [INFO] [1693968035.337221371] [controller_server]: Passing new path to controller.
[controller_server-1] [ERROR] [1693968035.937211680] [controller_server]: Failed to make progress
[controller_server-1] [WARN] [1693968035.937366058] [controller_server_rclcpp_node]: [follow_path] [ActionServer] Aborting handle.
[controller_server-1] [INFO] [1693968035.956797897] [local_costmap.local_costmap]: Received request to clear entirely the local_costmap
[planner_server-2] [INFO] [1693968035.957056211] [global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[recoveries_server-3] [INFO] [1693968035.957307656] [recoveries_server]: Attempting spin
[recoveries_server-3] [INFO] [1693968035.957343306] [recoveries_server]: Turning 1.57 for spin recovery.
[recoveries_server-3] [INFO] [1693968036.957427276] [recoveries_server]: spin running...
[recoveries_server-3] [INFO] [1693968037.957513273] [recoveries_server]: spin running...
SteveMacenski commented 1 year ago

Please display the local costmap's footprint in another gif and zoom in on it (I don't care about the global costmap or its footprint or the TF frames, disable those so we can see clearly)

I assume you didn't disable DWB's obstacle critics - did you do any retuning of configurations of the controller?

Something doesn't seem right here in the first place - why is the planner going straight through an obstacle? What planner are you using? This vaguely reminds me of a synchronization issue with costmap clearing we resolved years ago. Given you're on Foxy, I suspect that may be the cause. If you upgrade to Humble or newer, does that work? You may need to compile from source and make a few superficial API updates from foxy->humble codebase changes, but it shouldn't take long.

Usually I'd close with "foxy's not supported" but collisions shouldn't be happening and I want to make sure this isn't still an issue in a modern version.

GiantSeaweed commented 1 year ago

I disable the global costmap and TF and keep the local costmap. I did not do any retuning of controller configs, just the config in foxy-devel branch. And I attach a new video here.

You can see at 3s, the robot successfully finished the previous navigation task and stopped quite close to the obstacle (within the light blue region).

Then, seven seconds later, I set the goal at 10s. Then the trajectory seems to have overlap with obstacle and the collision happened.

As for the planner, the console shows the info like Planning algorithm GridBased failed to generate a valid path to xxx.

https://github.com/ros-planning/navigation2/assets/28821472/f1cf6f0f-37fe-4ef5-8f8f-4ca1a92e3ddf

I haven't try Humble source build yet, could you give me a pointer on how to make API updates from foxy->humble? Thanks!

SteveMacenski commented 1 year ago

I asked to see the footprint in rviz, I don't see any footprint at all. That may be part of your issue unless you didn't enable it.

I see the pipeline

That aligns with the bug that we resolved long ago w.r.t. costmap clearing mechanics and threading issues, I believe. Try a modern ROS 2 distribution and it very well likely is gone. We didn't backport to Foxy because it was either EOL at the time or the change required substantial reworks that wouldn't be ABI/API compatible (probably this). We've long, long ago abandoned Foxy in Nav2, even before it officially turned EOL because of the new changes that weren't backward compatible. You can try also using Docker with the Humble image and install Nav2 from binaries in the docker image (will be much faster than building) and running in the image. That may be faster.

Else, if you want to compile locally, start building and working through API issues. I can't tell you much more than that without doing it myself.

GiantSeaweed commented 1 year ago

Thanks for your response! I found you said this issue is a wont-fix problem for Foxy at here https://github.com/ros-planning/navigation2/issues/2452. I will try on Humble.

I just attached a video with local costmap's footprint here. I changed the Fixed Frame in Rviz from map to odom, as instructed by here. I think it confirms your speculation.

https://github.com/ros-planning/navigation2/assets/28821472/40d01ad3-0d7b-4dea-873a-8e65fbe71e5a

SteveMacenski commented 1 year ago

OK! Closing then with found solution