[RPP] Adding support for rotate in place cusps

[SteveMacenski]

@Aposhian @vinnnyr @doisyg I could use a sanity check here as I'm proposing a solution to #3089.

Previously, cusps that involve in-place rotations from the state lattice planner are ignored. This is an attempted solution which I don't think has any negative side effects, but its subtle and worth expanding upon for visibility and understandability.

Previously, we check the dot project is negative, meaning that we have a discontinuity in the path that is a direction change. Since in place rotations have the same position, the dot product is 0 which isn't triggered. That's a problem.

So, I adjusted a new check if we're 0 and that the orientations of the poses are different -- that way if we simply have 2 overlapping exactly same points, this won't trigger (only true rotate in place represntations). Rather than parameterizing it, this always triggers with the assumption that if you're providing a feasible path with an in-place rotation, your robot can handle it since you gave the planner a motion model that can do it. Also in the case that the cusp direction changes (not just a 'going forward -> rotate -> going forward' still), you still want to identify the cusp. Its now that 'going forward -> rotate -> going forward' will also now trigger.

So now we've identified the cusp with rotate in place & general rotate in places. Cool. That'll impact the lookahead point selection as any cusp would that we know works, so no need to beat that analysis to death, other than to mention that the min_by finds the closes path point to the robot to prune all path points prior to.

To support this, I needed to change the comparison so that we find the last, closest point, instead of the first, closest point when there are N points the same distance away (e.g. rotate in place, same position). That way, we prune to the final path-pose in the rotation sequence, not the first.

In the ticket, we discussed adding a shouldRotateAlongPath, but actually I think that's not necessary. The shouldRotateToPath should actually implicitly do what we need to do. If rotate-to-path is valid and the error is sufficiently large, we do it. I think that covers our rotate to path heading needs for inplace rotations. When that rotation is very small - - then we don't really functionally need to do it here and it'll take care of itself in the immediate path tracking request curvature velocities. The same use_rotate_to_heading param applies.

So, I think this fully resolves the issue - with relatively minor changes. What do you think?

[Aposhian]

Sounds reasonable enough.

[SteveMacenski]

@Aposhian do you see any regressions in this for your application? I want to make sure there aren't unintended consequences given your larger application

[Aposhian]

Actually we aren't using RPP at the moment, so I can't say if it causes regressions for us.