search

ros-navigation / navigation2

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

The local planner failed to follow global trajectory #3807

Closed GiantSeaweed closed 1 year ago

GiantSeaweed commented 1 year ago

Bug report

Required Info:

Steps to reproduce issue

Enter the Humble docker and install the binary nav2 inside docker. I did not change any default configuration.

source /opt/ros/humble/setup.bash
export TURTLEBOT3_MODEL=waffle
export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/opt/ros/humble/share/turtlebot3_gazebo/models

#(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

Expected behavior

The local planner is expected to follow a feasible global trajectory.

Actual behavior

The local planner failed to follow the global trajectory and make further progress. I attached a video below. I found it often failed in such C-shape route. Could you explain why this is the case? Or could you provide some hint about how to investigate such case? Thank you!

https://github.com/ros-planning/navigation2/assets/28821472/b71694ac-bad9-4085-ae03-dbd8038e7b9e

SteveMacenski commented 1 year ago

Please provide logs and information w.r.t. your algorithms being used and how you tuned them.

Have you tried our bringup files? I make no promises regarding turtlebot3_gazebo, I have no relationship with that package or team

GiantSeaweed commented 1 year ago

Thanks for your response. I used the default algorithms with Humble version (GridBased for global planner and DWB for local planner).

I turn to nav2_bringup packages and run the following commands:

#(Terminal 1) Start turtlebot3_world and Rviz with:
ros2 launch nav2_bringup tb3_simulation_launch.py headless:=False

#(Terminal 2) Start slam:
ros2 launch nav2_bringup slam_launch.py

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

The video and log of Terminal 3 are shown below. 0-6s: The robot successfully finished the previous task and finally faced towards south-east corner. 6-30s: The robot received a new goal. It first turned west and tried to follow the path but failed. 30-36s: The robot ran backup. It seems that a collision happened at 35s. Also an Error message appeared at [1694655418.759985480]. 36-end: The robot retried several times and finally aborted.

https://github.com/ros-planning/navigation2/assets/28821472/116afd02-4f01-49d9-957d-2232c67ce9d8

...
[controller_server-1] [INFO] [1694655371.041600339] [controller_server]: Reached the goal!
[bt_navigator-5] [INFO] [1694655371.074975396] [bt_navigator]: Goal succeeded
[bt_navigator-5] [INFO] [1694655376.958818240] [bt_navigator]: Begin navigating from current location (-1.47, -0.84) to (-0.87, -1.38)
[controller_server-1] [INFO] [1694655376.979656153] [controller_server]: Received a goal, begin computing control effort.
[controller_server-1] [INFO] [1694655378.029837954] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655379.079848300] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655380.079864238] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655381.129837390] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655382.179861922] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655383.179834643] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655384.229848590] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655385.229844042] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655386.279840916] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655387.279835251] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655388.082913139] [controller_server]: Reached the goal!
[bt_navigator-5] [INFO] [1694655388.118970997] [bt_navigator]: Goal succeeded
[bt_navigator-5] [INFO] [1694655391.423235638] [bt_navigator]: Begin navigating from current location (-1.06, -1.56) to (-1.14, -0.76)
[controller_server-1] [INFO] [1694655391.445665870] [controller_server]: Received a goal, begin computing control effort.
[controller_server-1] [INFO] [1694655392.495844464] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655393.545847048] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655394.545888688] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655395.595852909] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655396.595846086] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655397.645849240] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655398.695886593] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655399.695847121] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655400.745848017] [controller_server]: Passing new path to controller.
[controller_server-1] [ERROR] [1694655401.445979428] [controller_server]: Failed to make progress
[controller_server-1] [WARN] [1694655401.446181558] [controller_server]: [follow_path] [ActionServer] Aborting handle.
[controller_server-1] [INFO] [1694655401.465593628] [local_costmap.local_costmap]: Received request to clear entirely the local_costmap
[controller_server-1] [INFO] [1694655401.466018968] [controller_server]: Received a goal, begin computing control effort.
[controller_server-1] [WARN] [1694655401.590561128] [controller_server]: Control loop missed its desired rate of 20.0000Hz
[controller_server-1] [INFO] [1694655401.752517989] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655402.790628816] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655403.840648664] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655404.840668954] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655405.890626827] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655406.940685443] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655407.940653498] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655408.990678016] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655409.990677447] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655411.040627911] [controller_server]: Passing new path to controller.
[controller_server-1] [ERROR] [1694655411.590701527] [controller_server]: Failed to make progress
[controller_server-1] [WARN] [1694655411.590833228] [controller_server]: [follow_path] [ActionServer] Aborting handle.
[behavior_server-4] [INFO] [1694655411.606450876] [behavior_server]: Running backup
[behavior_server-4] [INFO] [1694655418.708834792] [behavior_server]: backup completed successfully
[controller_server-1] [INFO] [1694655418.756072266] [controller_server]: Received a goal, begin computing control effort.
[controller_server-1] [ERROR] [1694655418.759833601] [DWBLocalPlanner]: No valid trajectories out of 419!
[controller_server-1] [ERROR] [1694655418.759985480] [DWBLocalPlanner]: 1.00: BaseObstacle/Trajectory Hits Obstacle.
[controller_server-1] [WARN] [1694655418.760401049] [controller_server]: No valid trajectories out of 419!
[controller_server-1] [INFO] [1694655419.806360569] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655420.856316200] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655421.856340936] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655422.906367392] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655423.906318982] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655424.956488409] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655426.006346426] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655427.006343607] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655428.056317003] [controller_server]: Passing new path to controller.
[controller_server-1] [ERROR] [1694655428.756376374] [controller_server]: Failed to make progress
[controller_server-1] [WARN] [1694655428.756438787] [controller_server]: [follow_path] [ActionServer] Aborting handle.
[controller_server-1] [INFO] [1694655428.775637380] [local_costmap.local_costmap]: Received request to clear entirely the local_costmap
[controller_server-1] [INFO] [1694655428.777141183] [controller_server]: Received a goal, begin computing control effort.
[controller_server-1] [INFO] [1694655429.077390667] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655430.077317778] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655431.127302570] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655432.178394609] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655433.177336988] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655434.227350535] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655435.227300027] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655436.277301759] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655437.327301714] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655438.327308666] [controller_server]: Passing new path to controller.
[controller_server-1] [ERROR] [1694655438.827451592] [controller_server]: Failed to make progress
[controller_server-1] [WARN] [1694655438.827704398] [controller_server]: [follow_path] [ActionServer] Aborting handle.
[controller_server-1] [INFO] [1694655438.845561383] [local_costmap.local_costmap]: Received request to clear entirely the local_costmap
[planner_server-3] [INFO] [1694655438.845741843] [global_costmap.global_costmap]: Received request to clear entirely the global_costmap
[planner_server-3] [WARN] [1694655439.646657508] [planner_server]: Planner loop missed its desired rate of 20.0000 Hz. Current loop rate is 1.2491 Hz
[controller_server-1] [INFO] [1694655439.665805188] [controller_server]: Received a goal, begin computing control effort.
[controller_server-1] [INFO] [1694655440.716074288] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655441.766056964] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655442.766077652] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655443.816056277] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655444.816052879] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655445.866141069] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655446.916099092] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655447.916098166] [controller_server]: Passing new path to controller.
[controller_server-1] [INFO] [1694655448.966058499] [controller_server]: Passing new path to controller.
[controller_server-1] [ERROR] [1694655449.666115859] [controller_server]: Failed to make progress
[controller_server-1] [WARN] [1694655449.666196302] [controller_server]: [follow_path] [ActionServer] Aborting handle.
...
SteveMacenski commented 1 year ago

I can see that on my side too, this is likely a tuning thing. What's likely happening here is that the critics to align with the path have some offset from the goal pose its to select (and another driving towards the goal) and since both of those basically in this situation are scoring primitives to move forward towards the obstacle, none of those are valid so they're being rejected and the robot's not making progress.

When I do the exact same thing with the next pillar over in that position, it goes off without a hitch, so its not that the DWB controller isn't able to get out of that position - it absolutely can - its just that since the goal pose is so close just around the pillar its being enacted and there are no incentives to try to do something else because DWB is a purely reactive planner, not predictive like MPPI or TEB.

I know that's not a satesfying answer - its a tuning problem you can resolve by changing the path or goal distances or the relative weights given to them - but that's also why this ticket was open until very recently https://github.com/ros-planning/navigation2/issues/2045 after it became clear that MPPI was superior in every way and its my intention to change it to the default algorithm after the Jazy release (so MPPI has another full distribution to bake in and improve before its set as default in an LTS distribution).

This is why tuning controllers is a pain in the butt and really dependent on what you're asking a robot to do. If you're not driving directly from one small pillar to the other side of the same pillar, then you wouldn't run into this issue. But if you are, then you need to tune appropriately. All the trajectory planners are to some degree a pain to tune. I'll say DWAs are a special kind of annoying though and is often why the rest of the ROS Navigation Stack was thrown out with the bathwater when it really was just the trajectory planner that needed the most attention. If you just want to blindly follow the path though, which we know is valid, consider the RPP controller. It won't deviate from the path, but it won't ever get stuck in situations like this.

Ironically, this is an entire point on a slide I'm writing right now for ROSCon 2023 on MPPI :laughing: