Path planning alternatives to teb_local_planner and move_base

[Subodh Malgonde]

I am trying to use teb_local_planner + move_base for path planning and motion control. I have observed that teb_local_planner works well where obstacle density is lower and there is a lot of free space for the robot to move around. It does not work very well where you have narrow openings and corridors.

I am exploring other options for path planning. One of them is to implement an algorithm like A* or RRT myself in Python/C++ using occupancy grid maps. I would prefer to use an existing library or package. Any suggestions?

[Frank Hoffmann]

Notice that RRT is an offline planner for global planner in the navigation stack, whereas TEB serves as a local planner in the ROS navigation stack.

I would not recommend to deviate from the ROS navigation stack. Of course you could invoke RRT based on the local costmap and the lookahead

point to plan a local trajectory, the RRT planner is able to consider kinematic constraints, still it does not consider kinodynamic constraints so you 

still need a scheme to convert the planned path to robot motion commands. 

For F1/10 racing I would not assume a cluttered environment but an obstacle free race track of sufficient width of track boundaries, with at most other cars as isolated obstacles.

I do not believe that a F1/10 race track layout would actually require motion reversals.

 

[Subodh Malgonde]

I know that the F1/10 race track will have sufficient width and a clutter free environment. However I want the car to move autonomously around in my house. It has sufficiently wide space for the robot, one can comfortably use teleop in my house. However the teb_local_planner seems to have problems with it.

I have been trying to tune the teb_local_planner for a couple of weeks now. There are a couple of problems:

1. Robot gets stuck entering a narrow pathway

2. Robot stops very often, even on straight paths with no obstacles around

People have posted about these problems way back in 2016, and these problems seem to still persist. There is no solution in sight right now. Have a look at these older issues:

1. Navigation problems when using TEB on UGV

2. teb_local_planner: avoid constant path replanning

3. Trajectory is not feasible

4. Unexpected robot oscillations even with tutorial example

I just want this post to serve as a warning for other people, who would like to use the teb_local_planner.

[Frank Hoffmann]

Did you setup the TEB planner with the correct footprint of your robot? The default footprint assumes a circular shape robot.

Try to run the navigation in simulation with no map uncertainty and perfect sensors, see if the robot is capable of traversing narrow pathways there.

[Subodh Malgonde]

Yes, I set it up with a polygon footprint. I visualized the footprint in rviz and confirmed that it matches with the actual robot.

I also tested teb_local_planner in Gazebo. You can see this rviz visualization, recorded while running the Gazebo simulation:

No matter how I set my move_base costmaps, it has problems entering a narrow corridor. Other people have reported this problem as well. See this issue Problem moving differential drive robot through narrow corridors.

[Frank Hoffmann]

It seems that footprint (thin rectangle) and actual robot do not match, the car is far ahead of the footprint. You need to center the footprint and the center of rotation should be on the rear axle.

[Frank Hoffmann]

The corridor is too small as its width is smaller than footprint + min_obst_dist

- The red arrows denote the TEB poses

- goal is fixed along the global path

- once the robot reaches the corridor the poses in front of the goal pose jump back and forth

- some clutter up in front of the corridor as the soft constraints (obstacle clearance) of the planner push the poses away from the corridor.

- the optimization problem is ill-defined since the obstacle costs exceed other constraints e.g. kinematic constraints

- the resulting trajectory is no longer feasible

- the planner does not realize that as feasibility is only checked for the first „feasibility_check_no_poses“ (ros param) to determine whether footprint at these

poses and  costmap collide

- theoretically you could increase the parameter feasibility_check_no_poses but that has the drawback that approximate solutions (trajectories) which violate soft constraints in the future are immediately deemed as infeasible

[Subodh Malgonde]

Thanks for getting back to me. I appreciate your detailed response.

1. I know there is a slight mismatch between the robot's footprint and the actual robot. However I think the planner should not allow the shifted footprint to get so close to the wall in the first place.

2. The TEB planner just does not work in narrow environments, as myself and other roboticists have pointed out in github issues on the teb_local_planner repository.

I have tried adjusting different params like feasibility_check_no_poses, min_obstacle_distance, inflation radius of the costmaps etc. Made the footprint slightly bigger than the actual robot. Nothing works in narrow environments.

I finally gave up on the teb_local_planner and implemented my own rudimentary local planner. Now my car can go backwards and correct itself if required. It drives more like a human. I am really proud of the way it performs a series of forward and backward turns to do a 180 degree turnaround when it reaches one end of my house. Checkout this video (click on the image, redirects to YouTube):

I am really satisfied with the result now. I will be blogging about the development on this car through a series of blog posts. Here is the first one https://medium.com/@subodh.malgonde/building-an-actual-self-driving-car-53f67ca41566