TebLocalPlannerROS: trajectory is not feasible error from previous successful goal point about teb_local_planner HOT 7 CLOSED

Hi Orhan,

thank you for submitting the issue. Probably, the issue is related to the collision model used for trajectory planning which differs from the actual robot model specified for the navigation stack.

Details:
Currently, the robot is modeled with a circular shape for optimization (in order to simplify collision checking and therefore speeding up the computation time). The implementation specifies the robot as a single point and the distance to the obstacle is given by rosparam min_obstacle_dist (which should include the robot radius itself and the desired separation; okay, the label is not describing this circumstance well).
In contrast, the feasibility check (which is performed before actually controlling the robot) uses the robot footprint specified for the costmaps (e.g. circular or polygon, in your case a rectangle).
The feasibility check iterates the next N poses (N = feasibility_check_no_poses; a ROS parameter)
in order to verify the clearance from obstacles (according to the footprint).

In your example, the robot plans a trajectory which is composed of a short backward motion and a subsequent forward motion. According to your small min_obstacle_dist (0.03 m), the planned trajectory is allowed to be really close to the wall. However, the planned trajectory cannot be executed, since one footprint of the upcoming 5 poses (feasibility_check_no_poses) hits the wall.
Of course, this behavior is undesired. I already started some weeks ago to address this part of the planner for the upcoming version: extended support of robot shapes other than just circular (or actually point-shaped!).
I will also think about how to connect that with the costmap footprint parameter to simplify the configuration of the planner and to make the obstacle avoidance parameters much more intuitive. Or even adding appropriate warning messages.
Unfortunately, due to some limited resources and holidays I need to postpone the completion, probably to end of April.

As a workaround I suggest to increase the min_obstacle_dist to a value higher than the circumscribed radius of the robot. You also might reduce feasibility_check_no_poses to 0 or 1, since the trajectory is refined during runtime and suboptimal future trajectory parts could collide with obstacles.
Edit: After checking your robot shape in the video again, I think that increasing min_obstacle_dist to much could prevent the robot from reaching goals close to the wall. You could also reduce the amount of backward motions (height penalty weight weight_kinematics_forward_drive and low backwards speed).

Please let me know, if the workaround helps or if you have further suggestions/issues.

from teb_local_planner.

Thank you Sir.

And sorry for the late response. I've just finished parameter tuning tests:
Increased min_obstacle_dist parameter to 0.7
Decreased feasibility_check_no_poses parameter to 0,
Increased weight_kinematics_forward_drive parameter to 1000.0.

Now, It's little bit better than before. And when it wasn't very close to wall, It isn't giving feasibility error.

But sometimes (especially close the wall), robot keeps hitting the wall because of the footprint. In this case below, when trying to follow local path's first part (including backward motion, Image 1) it hits the wall.

But, it is first calculation of path and if I stop the robot with my joystick's X button before setting a new goal,TebLocalPlanner keeps calculating path, then changes path to better one (Image 2).

But this is slow process and before doing this, robot starts directly and keeps following first created path and hits the wall.

Now, I've changed dt_ref to 0.5 and decreased dt_hysteresis to 0.05. And my temporary solution is worked for this wall to wall case. The Robot calculates best path fast. I think all of problems caused of footprint, as you said...

And, TebLocalPlanner is definitely the best local planner I've ever seen in ROS Platform. Thank you for writing and publishing this planner.

from teb_local_planner.

With the last commit (cdad3e3) I finally added the possibility to define other (customized) robot shapes (considered for optimization!) using the ROS parameter server.

I would be really interested in knowing if it solves your problems mentioned above @orhangazi44 . Is there any chance that you have time to test it with the new version of the "master" branch?

I will also test the new version before closing this issue and releasing a new version of teb_local_planner.

Checkout the following new tutorial for configuring the footprint model:
Obstacle Avoidance and Robot Footprint Model

from teb_local_planner.

Hi Sir.

I'll try it first at the start of the working hour tomorrow.

from teb_local_planner.

Hi,

Sorry for late response, my boss gave another job at this morning :/
I've just finished trying two_circles, polygon, and line footprint models.
But sorry, Robot is hitting the wall when start point is close the wall(Robot's side is parallel to wall).
I'll try more configuration in simulation at this night at home.

Thank You.

from teb_local_planner.

Thanks!

I found another possible cause for the issue. The strategy of incorporating costmap cells as obstacles behind the robot was not quite good. I ignored points that were located behind the goal-heading with more than a (small) hardcoded distance. Referring to your example images, this might exclude the wall from possible obstacle candidates (goal heading is 90° rotated w.r.t. robot pose!).

The new strategy (commit bc253c3) should resolve the problem. Using the new parameter costmap_obstacles_behind_robot_dist, the area of incorporating obstacles behind the robot can be controlled. Just check rqt_reconfigure. The default value of 1.5m should work in most cases.

With the robot footprint and those new changes, my turtlebot navigates perfectly close to walls as long as the goal pose is physically reachable ;-)

Please let me know, if it works for you.

ps: do not forget to reduce the value of costmap_obstacle_dist (EDIT: sry, I meant min_obstacle_dist) if you add a proper footprint (see Tutorial).

from teb_local_planner.

👍 My robot navigates perfectly too :-) (video)

Now, teb_local_planner is navigating my robot with two_circles model, like an expert, 40 years old, bus driver :)
(In video, footprint is coming late because of my robot needs some performance tuning in other packages. It is not about teb_local_planner. I have already tuned it via its tutorials before.)

I didn't find costmap_obstacle_dist parameter, increased min_obstacle_dist. But it causes struggling when goal point is close to an obstacle. Here is second video about differences between 0.6 and 0.3 values of min_obstacle_dist parameter.

However I think new commit(bc253c3) and its costmap_obstacles_behind_robot_dist parameter are solved this problem.

If turtle goes to somewhere with a safety, I'm closing this issue immediately :-) Thank You again!

from teb_local_planner.