[Reg Pure Pursuit] Support reverse direction

[SteveMacenski]

The SMAC planner supports both the Reeds-Shepp & Dubin motion models for ackermann / legged robots.

The Dubin model can only move forward which RPP encapsulates perfectly. However Reeds-Shepp can allow the robot to reverse at a cusp (and point the opposite direction). So at these points we should move the carrot from the front of the robot to the reverse of the robot in those situations and invert the velocity so we can support reversing as well.

This figure shows a simple geometric view of the reverse cusping: https://github.com/leggedrobotics/se2_navigation/blob/master/pure_pursuit_core/doc/path_conventions.pdf

As well on the path smoother, do in sections

[Timple]

Our (similar) ROS1 planner can travel any path in both directions by indeed moving the carrot to the other side of the robot.

However this doesn't mean it can do reeds-shepp since that would change the target velocity half-way a path. We currently solve this by simply dividing into sections and sending them sequentially to the controller. Is this what you're suggesting here?

[SteveMacenski]

Precisely, I’d welcome the contribution if possible!

Glad great minds think alike with what you built internally 😉

[Timple]

Where in the typical global planner -> local planner flow would this splitting best occur? It feels like an intermediate step, like global planner -> plan parser/splitter -> local planner. It could also be completely up to the local planner, but this would require the planner to know which parts are supposed to go backwards.

(Our reeds-shepp capable robot uses a global planner which does the splitting into parts already, so we didn't solve this architectural piece yet).

[SteveMacenski]

Because the RPP controller allows the reversing, I think this logic belongs there. Other algorithms / controllers might not need / want that separation to handle it. I say put it in RPP directly and if we find a reason to use it in multiple places, we can abstract it out into a library in nav2_util for multiple to use.

It could also be completely up to the local planner, but this would require the planner to know which parts are supposed to go backwards.

The only planners that generate backward motion is the Hybrid-A* (and soon to be State Lattice planner). This includes orientation headings, not just the points. You can easily break out the forward / reverse directions if you have the headings! if marching forwards in path points, i, and direction is pointing backwards but the last point was pointing forwards, then split (and vise versa). Any planner that enables backward and forward motion would HAVE to include the orientation information for any controller / algorithm to be able to know that and react accordingly -- or your solution of separating them at planning time. Anything else would be guestimating. That's the minimum viable set of information to describe the explicit request for a change in direction.

P.S. If you're open to sharing more about that global planner, I'd be curious on the algorithm and how happy you are with it (and if you tried the smac planner, especially with these changes https://github.com/ros-planning/navigation2/pull/2352 which speeds up another 30% and dramatically increases path quality, seriously, its night and day due to some new work I added; way smoother, way in the middle of spaces, way faster in footprint checking if not using just the robot radius estimation)

[Timple]

if marching forwards in path points, i, and direction is pointing backwards but the last point was pointing forwards, then split (and vise versa).

This was exactly the code I was trying to prevent. But then again, doesn't seem too bad. Orientation is correct in our paths so no problems there. We do have (like lots of robots/applications?) different requirements for forward and backward speeds. So an interface like this would be welcome, but then for both directions.

Our global planner does full field coverage (I believe you had an issue open for this, can't find). So that cannot be compared to the smac planner. We do want to go ROS2/navigation2 with this FCPP this year, so when we get to this we'll certainly have some interface discussions and code to share :+1:.

[SteveMacenski]

This was exactly the code I was trying to prevent.

Is there something wrong with that? It would seem to be to be a very fast and reliable method (using the dot project to find if the signs change) -- we only need to march through the path in the local costmap window (since any further out isn't used by the controllers) so at most we're talking maybe 20-30 points.

So an interface like this would be welcome, but then for both directions.

As part of the RPP supporting backwards, that seems very reasonable to have a max speed in reverse parameter (or a % of the forward speed)

We do want to go ROS2/navigation2 with this FCPP this year, so when we get to this we'll certainly have some interface discussions and code to share +1.

Awesome!

[Timple]

Is there something wrong with that?

No, not if the paths are proper. But we've had issues in the past where the global plan contained almost duplicate points from segments being stitched together. Almost no local planners seem to care about it. But technically we've had points which where some nanometers behind the last pose. So this logic would conclude a slight movement backward.

Now we should fix the global planner to not do this, but it is something to be aware off.

[padhupradheep]

My further thoughts on navigating the robot in the reverse direction. In the yesterdays work group meeting I came up with the problem of the robot hitting the obstacle when moving in the reverse direction. This still seem to be a valid point. Assume your robot does not have eyes behind and suddenly a dynamic obstacle that pops out it in the environment (maybe in / very close to your global path), this might be the case where the scanners might not pick up the obstacle and if the robot is going in the reverse direction, there maybe some incident which we would not like to have. I would say that it can be added as an parameter and a word of caution needs to be given to the user.

Thoughts?

[SteveMacenski]

It shouldn't be necessary if you don't have a planner that plans backwards. If a plan has backward motion supported, then this controller is not "smart" to know that, it just needs to follow the path given. If you would not like reversing behavior, then the planner giving paths to this controller should not contain them

[padhupradheep]

If a plan has backward motion supported, then this controller is not "smart" to know that, it just needs to follow the path given.

Then I could supposedly assume that, it is the choice that the user has to make on whether they want to use a planner that generates a path supporting a backward motion (Planner using Reeds-Shepp for instance), depending upon their robot. In that case I totally agree on your point that the controller need not be "smart".

[padhupradheep]

I had some time to look into this problem further. It would be great if we have some thoughts from the community on the particular solution I'm proposing:

• On handling the cusp: I see that, by just using the orientation vector of the global path (i.e. doing the dot product), it will not be enough to handle the cusp. Rather, we need to determine the slope of the consecutive waypoints of the global path and if there is a cusp, then the sign of the slope changes, consecutively allowing us to decelerate and change the direction of the heading.
• On handling the reverse heading at the initial stage: Say that, if there is a reverse direction path at the beginning of the global path, then again, we cannot just use the orientation vectors of the global path as such, rather we need to use the carrot relative to the robot in the global frame in order to generate the -ve velocity commands.

Combing them and having them as a single robust solution for the path following, seems to be the challenging part and could use BT to handle the cusps (?). Maybe, there could be a much more effective way to handle this?

P.S: Here we are considering Reeps-Shepps model that is part of SMAC planner for generating the global path!

[Timple]

So the question is: who's in charge of chopping up the path in segments?

• The global planner already knows, but is unable to communicate this properly to a local planner.
• The local controller (increases responsibility of the local controller).
• An intermediate controller that accepts a path (FollowPath), chops it up and sends multiple consecutive paths (FollowPath) to any local controller. (The local controller only has to be capable of doing a single direction of moment at a time).
• Some idea I didn't think of yet.

[padhupradheep]

The global planner already knows, but is unable to communicate this properly to a local planner.

This will be the best solution I guess. Maybe if the global planner is able to provide with some more information such as a particular waypoint is a cusp point or not (?)

An intermediate controller that accepts a path (FollowPath), chops it up and sends multiple consecutive paths (FollowPath) to any local controller. (The local controller only has to be capable of doing a single direction of moment at a time)

Eventually needing the robot to reach to a stop at the cusp, before it gets an another path and continuing the motion.

[SteveMacenski]

So what I think:

• Detect in the local path window (so after the points too far away are removed) when there is a cusp
• At the cusp, throw away all points after it
• When the robot starts to approach it, then it will use that last point as the carrot location, since that distance will shrink, the speed will drop, so you'll naturally slow before the change in direction
• At [some criteria], consider the robot at the cusp point, and then allow issuing the next segment after the cusp, switching the side of the robot the carrot is on
• Repeat

So the 2 most open questions are:

• How do we detect the cusp / change in direction?
• How do we decide when we've "achieved" the cusp so we can issue the next direction movements?

---

This will be the best solution I guess. Maybe if the global planner is able to provide with some more information such as a particular waypoint is a cusp point or not (?)

Is that not what its doing already? Draw a cusp on a piece of paper and then draw tangent lines at some points along it. At some point, the arrows turn from going up into the cusp to down after the cusp. If you projected those vectors on to each other, you should see in the "normal" path that there is indeed some projection from one point to the next. But on a cusp, that wouldn't be true. My recollection of vector algebra is that a projection is the same thing as a dot product. So there's some relationship about the changing of signs to signal that it is a cusp. And since this is vector orientations, its invariant on homogeneous transformations so there's no reason to think that the translation from map->odom frame would interfere with this detection method. I think that should work?

The only concern I'd have is if you tried to feed a path into it that had rigid 90 degree turns. The projection would be 0 just as if it were a cusp (which I suppose in a sense, it is). But honestly, that sounds like kind of a cool behavior to have your robot stop, rotate, and continue at 90 degree turns. Its much less "cool" though if its not just an occasionally designed 90 degree turn but the result of a naive grid A* so there's lots of them frequently. But in that case, the orientation vectors probably aren't populated because there's no meaningful se2 collision checking in a naive-grid-search. So if the vectors aren't populated, the projection will always be 1 and therefore we shouldn't expect it to trigger the behavior.

[padhupradheep]

Quick update: Indeed the dot product did the trick in determining the cusp

Detect in the local path window (so after the points too far away are removed) when there is a cusp

• At the cusp, throw away all points after it
• When the robot starts to approach it, then it will use that last point as the carrot location, since that distance will shrink, the speed will drop, so you'll naturally slow before the change in direction

The only trouble at the moment, I'm having now is to hold the last point as the carrot location, because of the updates from the global planner that consistently changing.

• At [some criteria], consider the robot at the cusp point, and then allow issuing the next segment after the cusp, switching the side of the robot the carrot is on
• Repeat

[SteveMacenski]

You don't need to hold the last point, you can re-detect it each iteration as the updates come in. For all we know, a replan actually made that maneuver move, so you shouldn't be storing that between replans.

[padhupradheep]

This is the expected behavior I believe!

[SteveMacenski]

Its hard to tell from that short gif, can you extend it before and after so we can see the path / transition more clearly? Potentially set your robot to use differential mechanics?

[padhupradheep]

This might help?

[SteveMacenski]

Ooooooooooohhhhhhhhhhhh that looks niiiiiiiiiiiiiice.

[padhupradheep]

Thanks! I need to further test it for some more cases and will give a PR soon!

[SteveMacenski]

Merging imminently