Pathfind to moving target pose

[jonahsnider]

Is your feature request related to a problem? Please describe.
We are trying to utilize PathPlannerLib's pathfinding functionality to drive to a point that will shift around as we drive closer to it. Currently, the pathfindToPose() command requires a static Pose2d as the target pose, meaning you can't adjust the goal pose as the command runs.

Describe the solution you'd like
A way to run .pathfindToPose() with a Supplier to provide the target pose would be ideal. From skimming through the Pathfinder interface, it seems like doing this kind of continuous replanning would be possible?

[lidor51]

@jonahsnider can you elaborate on your scenario? Does you real intent is switching targets (e.g., a moving target OR decision to aim for a totally different target) VS. providing a more accurate pose estimation of the same original target as you getting closer.

Is there any expected "markers" behavior in this flow, or you're expecting to handle subsystems commands separately?

(pls tag me when you reply so I won't miss your answer).

[jonahsnider]

@lidor51 Our autos use game piece detection to track notes and calculate a pose for each one. We want to leverage PathPlanner's pathfinding functionality when driving to these poses (mostly to leverage obstacle avoidance). Because the camera is constantly recalculating the note poses, we need a way to continuously provide the current position to pathfind to.

We don't need any special behavior other than that.

[lidor51]

Are you doing anything special to accommodate if you ended the path "far" from the start of the next path?

[jonahsnider]

We're not using any paths except for the very beginning of the auto. Everything after is just pathfinding back & forth between detected notes and scoring locations.

[lidor51]

Any notable struggles you had to overcome with solely using pathfinding? Things to be considered on this method?

[lidor51]

Also, if it's working that well for you, why to even use a path in the very beginning of auto at all?

[jonahsnider]

@lidor51 I think we are getting a little off topic, the specifics our implementation don't really matter much for adding this pathfinding functionality.

[lidor51]

Indeed 😉

[mjansen4857]

So after looking at how I would implement this, I don't think this is currently possible due to limitations of how the pathfinding works. The pathfinding is really built for long distance paths at a very low precision, not short paths with something as precise as game piece pickup.

It isn't really a big deal to have the pathfinding command take in a target pose supplier, but it just wouldn't function anywhere near what you are probably expecting it to do. The pathfinding command has some band-aids that smooth over some quirks of the implementation, such as needing to ignore new path updates if close to the end, and being unable to pathfind to a position very close to you (within the same grid node). So, using pathfinding for the use case you describe probably wouldn't give useful results. Pathfinding in general needs a bit of an overhaul, but that's not something I'm targeting for 2025 at this point. A future pathfinding iteration would be able to handle something like this.

For some alternatives for now, I would suggest pathfinding to your initial guess of where the note is to avoid obstacles, then take over with a command that generates new on-the-fly paths if needed when you get close. However, my team personally had best results when we just did dead reckoning driving straight towards a note detection until we picked it up.

[jonahsnider]

Got it, thanks for the info. The approach we were thinking of is similar to the workaround method you mentioned: pathfind to the initial note pose, and once we get close switch to plain PID.

[WarrenReynolds]

For some alternatives for now, I would suggest pathfinding to your initial guess of where the note is to avoid obstacles, then take over with a command that generates new on-the-fly paths if needed when you get close. However, my team personally had best results when we just did dead reckoning driving straight towards a note detection until we picked it up.

How is this done in pathplanner autos without causing a command interruption from the command that generates and follows the on the fly path. Doesn't this command require the drive base, which would would interrupt the auto command group because it has the drivebase subsystem?

We starting trying to do this exact scenario in our autos towards the end of our regional back in March when we were trying to pickup centre notes, and our auto would just stop after it collected the note. We had an in game command that would drive at a note and intake it that worked perfectly, so we added that command to the command map and tried to run it as part of our auto.

It wasn't until after the event that we realized what was happening. We kept thinking that the problem was the on the fly command group that drove to the note and intaked it, wasn't finishing and hence the auto couldn't continue. We even added timeouts to try and make the auto continue. Turns out it was finished, but the auto group had been interrupted and cancelled.

Is it possible to have one of these calculate on the fly and follow commands in the middle of an full pathplanner auto?