Skip to content

Zig-zag due to unfavorable angle increments and starting angle #3172

@mangoschorle

Description

@mangoschorle

I am experiencing some interesting behavior when using SmacPlanner Hybrid. I am not quite sure whether this is a common mistake/behavior maybe caused by incorrect usage, or whether it is a known limitation. I currently have no idea how to approach this / solve this. If ideas pop up along this thread, I am more than willing to contribute.

So the issue is that depending on which angle-bin the start node falls into, I am getting either a straight line or a very messy zig-zag.

We have 64 angle bins. Here the start is in bin 32 (pi)
Screenshot from 2022-09-05 21-53-44

In this image the starting angle is in bin 31
Screenshot from 2022-09-05 21-54-39

I also found that due to my choice of resolution (10cm) and min_turning_radius (50cm), the turning motions of the minimal primitive set (Dubins) cover four angle bins at a time, i.e., increments=4
https://github.com/ros-planning/navigation2/blob/634d2e3d9b6bde5558c90fe6583d52b3ed66cf55/nav2_smac_planner/src/node_hybrid.cpp#L104)

This is actually quite bad since you one will therefore never end up in odd angle bins. But only in every fourth bin that are multiples of four. This also causes the zig-zagging because just can't reach an angle bin that would directly go in the right direction.
Also as can be seen in the image, the planner has to expand WAY more nodes to make progress.

This is a distribution of the angle bins. As you can see, we only reach every fourth angle bin.
Screenshot from 2022-08-04 16-42-16

I wonder / am collecting ideas, on how we could find remedy for this zig-zagging issue. I know, we could double the resolution or use a larger turning radius (>2 meters if we want to cover only a single bin) but we are running into other issues if we do so. Maybe there are other ideas. I have tried adding additional turning primitives which turn much less than the min_turning_radius but didn't get good results. Again, if I understand the idea / solution, I'd be more than happy to implement, test, and submit a PR.

Metadata

Metadata

Assignees

No one assigned

    Type

    No type

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions