PathController: Get next maneuver

[krisgry]

Hi,
For vehicles with a limited turning radius, it makes sense to consider the maximal turning radius when switching to the next maneuver. To enable a smooth transition between two straight lines, I would like to add the possibility to dynamically adjust the TimeFactor based on how sharp the turn is: transition earlier the more acute the angle between the two straight lines is. From the PathController side, this can be achieved by overloading the getTimeFactor function, but the problem is that it will require knowledge of the next maneuver before the transition is made. What is the best way to achieve this?

I've looked into this, and have arrived at the following suggestion:

• When PathController starts a new maneuver, also have it request the next maneuver. This may be done by a planControl request, and parse the returning arg to find the maneuver after the current man_id (obtained through consumption of PlanControlState?).
• make the information about the next maneuver easily available for child PathControllers, by adding the function getNextManeuver().
• This will enable overloading of getTimeFactor by:
  • Finding the angle between the current maneuver and the next (by calling getNextManeuver())
  • inscribe a circle with radius equivalent to the minimal turning radius
  • Find the distance/time between the intersection of the circle and the current line, to determine the appropriate TimeFactor for transitioning to the next maneuver

How does this sound? Do you agree that this is the most straight forward approach? I know that this will introduce some (minimally invasive) changes to the "holy" PathController base class, so I would like some input before I potentially start this process.

Thanks!

[oysstu]

Can you request the next maneuver when the vehicle is at the worst case distance (e.g. distance of max turn radius), and then delay the switch according to the angle in the path controller rather than exchanging PC/PCS messages? Or am I missing something here

[krisgry]

I interpret your comment to suggest that I signal completion of a maneuver at a "worst case" distance (possibly far away from the actual completion of the maneuver), but then still use the old value of TrackingState (start/stop_lat/lon etc) until the actual completion of the maneuver, and then start using the next TrackingState.
Such an approach could work, but it requires some considerations for situation when using multiple PathControllers (longitudinal + lateral, for example); when the first pathcontroller signals completion, that will signal completion to all path controllers. In other words; all path controllers will need to have a means to handle this. Sounds like a simple workaround that I could use, but I doubt it will be accepted upstream :)

[oysstu]

Thanks for the clarification, I didn't consider the case of multiple path controllers. Another possibility is to add fields to the DesiredPath message, such that the fields consist of start_lat/start_lon, end_lat/end_lon and next_lat/next_lon or something along those lines (DesiredTrajectory?).

Anyway, I don't have too much further to add this. I think smoother motion between maneuvers is something that definitely makes sense though.

[krisgry]

I have finally had some time to think more about this, and have arrived at the following suggestion on how to pass information about the upcoming maneuver all the way down to the pathcontroller (following an idea from @paulosousadias , I believe). The idea is to mimic how a maneuver is passed down from PlanEngine and ends up as a DesiredPath in PathController. These are the suggested changes:

Plan Engine changes
Implement plan->peekManeuver(), that is identical to loadNextManeuver, but does not set m_last_if
Dispatch PeekManeuver (from within startManeuver?)
Maneuvers/Maneuver changes
Consume PeekManeuver, call onPeekManeuver (if active)
Multiplexer changes
Implement onPeekManeuver: call onPeekManeuver in the appropriate muxed maneuver (The maneuver implementation of the next maneuver)
Muxed maneuver changes
generic onPeekManeuver: m_task->debug(«Not implemented for this maneuver»);
Goto/FollowPath/… changes
make "Maneuver->DesiredPath"-conversion a function
oveload onPeekManeuver, and have it call "Maneuver->DesiredPath"-conversion function on the peeked maneuver, and dispatch PeekDesiredPath
FollowReference, and other specific maneuvers
generic onPeekManeuver: m_task->debug(«Not implemented for this maneuver»);
(specific) PathController changes
consume PeekDesiredPath. This is where the PathController for example can calculate the optimal TimeFactor, based on how sharp the upcoming turn is, to make the transition between maneuvers more smooth.

In addition to the changes to the tasks above, this will require 2 new IMC messages: PeekManeuver and PeedDesiredPath, which are exact copies of Maneuver and DesiredPath. I decided to call it PeekManeuver instead of NextManeuver, to not risk confusion with some maneuver that we are about to change to.

Any comments on this?
An alternative could be to pass a PeekManeuver straight to the PathController, but this feels like a hack, and will require implementation of the "Maneuver->DesiredPath"-conversion for each maneuver that the PathController should support.

I'm also working in a class of "path smoothers" that I plan to use to smooth the transition between straight line paths. My plan is to make this a class that can be inherited to implement different types of smoothing strategies between straight paths (e.g. inscribing a circle, using a clothoid or a fermat spiral, etc, see this paper by my colleague Tasos Lekkas)

• The path smoother will be initialized in the consume PeekDesiredPath function in the PathController.
• in the PathController step-function, I check if we should start the smoothing. If so, the start.x/y/z and track_bearing of TrackingState is propagated. The challenge here is the propagation, which is done by propagating a path variable according to some rule (there are several alternatives to choose from, and I'm currently looking into this). The coordinates of the path is parameterized wrt this path variable, so given an updated path variable, the new coordinates are easily found.
• PathController getEta needs to be modified to subtract the length of the straight line path into the corner that the vehicle no longer will take (since it has started following a smoothed path), and add the ETA from the getEta-function of the path smoother (which corresponds to the length/speed of the smooth path)

Open for inputs on this as well, even though I realize that this is more specific, and that it might be a challenge to ever get this part merged :)