when combined with robot_localization, teb module will collide with wall.

[asimay]

hi, dear @croesmann , i just combined with robot_localization, in order to get more better pose data, so i add:

in stage launch file. but when arrive at end, it seems not get very good result.
please see below link, especially sim time at 38 sec and at last:
https://youtu.be/pbxcLVV57rs

Thanks!

[asimay]

And I consult Tom Moore, he said its not the robot_localization module's problem..
he said: Looking at the video, it appears that your local planner is planning a path through the wall. Your robot is not colliding with the wall. Furthermore, this behavior would likely have nothing to do with robot_localization.

could you please check and verify for this cause? Thank you very much!

[croesmann]

Indeed, your issues are not related to robot_localization.

I checked your video and investigated your parameter configuration according to #7:
Your planner configuration is not well suited for the map:

Let's have a look at the following parameters:

• robot footprint length = 0.6 [meters/inches]; extracted from your polygonal footprint
• min_obst_dist = 0.3
• penalty_epislon = 0.1

Hence, the robot requires more(!) than 0.6+2*(0.3+0.1) = 1.3 [meters/inches] space in order to rotate (actually you really need more space, since you cannot rotate on place).
However, by reducing your parameter to min_obst_dist=0.1 it should probably work in that environment.

38sec@video / 32sec@sim:
Try to slightly increase param costmap_obstacles_behind_robot_dist in order to enlarge the region of obstacles taken into account behind the robot (this should avoid finding this alternative, but requires more computational resources; otherwise reduce the costmap size or horizon length, e.g. by max_global_plan_lookahead_dist).
Maybe, I need to adapt the default parameter setting accordingly.

1min:02sec@video / 55sec@sim:
Note, by looking at the optimizer's solution for the turning motion, this cannot be feasible (refer to the parameter description with the required distances above).
Afterwards, the planner thinks that the robot can rotate beyond the local costmap (yeah, since these obstacles/walls are not taken into account). Thus the robot proceeds finding a location in order to turn around until it hits the wall.
Since your max_vel_x is set 15 m/s, acc_lim_x to 2 m/s^2 (Btw, I would really like to see that robot, especially in conjunction with your specified dimensions ;-) ) and feasibility_check_no_poses the robot detects the infeasibility too late in order to brake before colliding. Try to increase feasibility_check_no_poses and decrease your max_vel_x if possible.
Indeed, hitting the wall here must be prevented by the planner anyway. Probably, I need to further investigate/improve the strategy here or add some additional safety checks independent of the current plan but e.g. just by checking the distance to the closest obstacle (w.r.t. current driving direction) and slow down accordingly.

[asimay]

hi, dear @croesmann, with your detailed instruction, i set:
max_global_plan_lookahead_dist: 2.0 #3.0, old value
feasibility_check_no_poses: 5 #2
min_obstacle_dist: 0.1 #0.3
costmap_obstacles_behind_robot_dist: 1.5 #1.0
the car runs very well, and no trajectory cross the wall when it turn at left bottom of map.
and with
min_obst_dist=0.1,
it really can turn around. Thank you very much! teb module is really a very good planner system I met since now!
I still left: max_vel_x :15, in simulator it runs well, because in real it is a vehicle, SUV, 15m/s equals to about 30km/h. I want to test your system on the real experiment car to verify it.

[asimay]

hi, @croesmann , i find some issues when combined with robot_localization EKF, this maybe well illustrated the reason sometimes teb acts not very stable when combined with EKF & stageros.
when i run roswtf:

EKF and stage publish same tf transform, so sometimes the behavior is not very stable, but can work.
if under stage env, we maybe need to exclude robot_location package (because there is no switch of tf publish in robot_localization package).
it gets good result and as I wish.
Let you know. Thanks!