how to use custom via_points in teb_local_planner?

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:

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:)

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 close merge delete