search

rst-tu-dortmund / teb_local_planner

An optimal trajectory planner considering distinctive topologies for mobile robots based on Timed-Elastic-Bands (ROS Package)
http://wiki.ros.org/teb_local_planner
BSD 3-Clause "New" or "Revised" License
1.03k stars 546 forks source link

`max_vel_x_backwards` has no impact on car-like driving #330

Closed VRichardJP closed 2 years ago

VRichardJP commented 2 years ago

I am testing teb_local_planner's test_optim_node for a car-like vehicle. I have setup a world with wall obstacles like this, with the start position on the left (blue point) and the goal position on the right (red point):

backward_10_above (2)

Interestingly, the teb_local_planner always wants to drive backward on some portion of the path (green path), while obviously the yellow path drawn by hand should be way more efficient and faster. This is even more true that I normally set max_vel_x to 10.0 and max_vel_x_backwards to 2.5

For testing, I tried to tune max_vel_x_backwards and see if by any chance, decreasing the value would make the planner realize there is a way more efficient path. But whether I use 0.01 or 10.0, the shape absolutely doesn't change.

For example, I have tested with 0.01 and 10.0 and set visualize_with_time_as_z_axis_scale to 1.0 to visualize the time spent along the path according to the planner. Logically, I would expect the portion driven backward to be extremely long (time-wise) when I set max_vel_x_backwards to 0.01. But it is actually not what I observe. For both 0.01 and 10.0 value, the side view is exactly the same and it looks like the vehicle can drive both backward and forward at the same speed:

backward_10 (1)

May it be that max_vel_x_backwards is not correctly used?

corot commented 2 years ago

The plots are rather confusing; what's the global path sent to TEB? Can you show them on RViz, and set pose to to "axis"? This will show you the direction of the global path TEB will try to move with the same orientation as the global path, so that's a very important piece of information

VRichardJP commented 2 years ago

Sorry if the view is confusing.

I am using the test_optim_node.cpp to call teb local planner directly. The 2 screenshots are taken from Rviz. The first one is a top to down view on the x-y plane. The second one is a side view using the orbit camera mode. The camera is placed close to the start point (blue point) on the left side of the first image. The view shows the x-z plane (z up). The thick green line at the bottom is z=0 (all my static obstacles), and the green curve going from the blue point to the red point is the same path than the first image, but showing travel time to goal with the z-axis.

Actually i have found my issue. I had set weight_max_vel_x to 1.0 but some other weights were way bigger and prevented teb from fixing the speed along the path. Increasing the weight value fixed the issue: during optimization, teb made the portion driven backward slower and slower, and eventually realized there are better strategies.