Collision polygon is rather conservative

[rokusottervanger]

The polygon (in red) encapsulating all predicted footprints (blue) is rather conservative, to the point where a feasible global plan cannot be traversed due to collision avoidance. See example below:

[Timple]

Interesting... Under the hood it's just a polygon-merge I think. So I cannot really explain this behavior.

[rokusottervanger]

It loops through pairs of two polygons to merge them bilaterally so every polygon is merged once with its neighbor before, and once with its neighbor after it. Finally it joins the lot to get to the red polygon. If you look at the line segment from the front rectangle, it runs from the front of the first rectangle, towards the rear corner of the second until it intersects with the line segment connecting the front corner of the second with the rear of the third.

Not sure what is desirable here. If you sample infinitely many poses you get to the desired shape, but that seems like overkill. Somehow you'd want the collision shape to approach that without sampling the pose with a very high density...

[Timple]

Since we know quite well the heading upfront, I'm quite sure there is some maths to it to do this properly.

But until then, this solution is indeed a slight bit conservative. And you are right: a higher sampling would solve it.

I would say this is desired behavior in this case. You are sampling a 90 degree curve with 3 samples. That is simply too rough I'd say.

[Timple]

In line with https://github.com/nobleo/path_tracking_pid/pull/2#discussion_r794403340:

If the sampling size results in a less than 70% overlap between the poses, we can issue a warning that such artifacts can occur?