how to use custom via_points in teb_local_planner?

asked 2021-11-03 19:32:50 -0500

vrichard gravatar image

updated 2021-11-04 03:14:30 -0500

I am running teb_local_planner in simulation for a carlike robot.

I want to operate the car in real-time so I have to limit the costmap size, which has an impact on the planned route quality. In order to help the planner to find better paths I thought about publishing a "good" path to ~/via_points. This good path needs to be better than the global planner path so I am not using global_plan_viapoint_sep.

To create this "good" path, I thought about first running the simulation very slowly and with a very large costmap. That way teb_local_planner has a lot of time to figure out a good path and drive along it and I record the followed path to a nav_msgs/Path message. The resulting path looks pretty good to me.

Then, to run the simulation in real-time (with a small costmap) I simply publish the "good" path to ~/via_points after the goal as been published. I would expect that teb_local_planner would use the "good" path as a guide, but in practice the generated paths are all absolutely crazy and certainly don't follow the via_points I set:

viapoints

I have tried to tune weight_viapoint from 1.0 to 100.0 and filter the points I publish (by keeping only 1 point every N points) but I always end up with a similar result.

is viapoints not supposed to be used like this? should I write a custom global planner that simply publish the path I want?

UPDATE: I have switched to move_base_flex costmap based navigation. I can now publish the recorded path as ExePathActionGoal instead of just 1 goal point. However I can't manage to force teb_local_planner to follow the path. Maybe it is just because the path looks too complex to the planner which prefers its own alternatives (it can't see it yet, but it will not have space to do a 3-point turn later:)

image description

Is there a way I can force teb_local_planner to follow the path more strictly? I have tried to use viapoints_all_candidates=True and to use global_plan_viapoint_sep>0 with a big weight_viapoint value but teb_local_planner always ends up choosing its own way. Is there anything else I could try to tune?

edit retag flag offensive close merge delete

Comments

1

Hi @vrichard. Have you considered using move base flex exe path action. There is a question that refers to it https://answers.ros.org/question/3136...

What I read in the repo this is a way to get your via_points https://github.com/rst-tu-dortmund/te...

osilva gravatar image osilva  ( 2021-11-03 20:20:06 -0500 )edit

Thank you! I'll check move base flex

vrichard gravatar image vrichard  ( 2021-11-03 20:51:54 -0500 )edit