Expose holomonic preview path in PathPlannerPath

[pjreiniger]

Similar to #423 . In a localization-less environment it is handy to know the starting pose for swerve paths. This uses the preview state heading, if it is set so you can reset your pose and drive exactly what you see in the gui.

[mjansen4857]

If you want to reset a holonomic robots pose for every path, use a pose created from the position of the first point and the robot’s current rotation. There’s no need to load and use all of this. This also allows for doing some pretty bad things, like resetting your rotation to, say, 45 degrees when the robot ended the last path with a rotation of 0 degrees. Now the path following for future paths will be completely messed up because it needs to be able to accurately drive field relative, but the robots 0 no longer matches up with the field 0.

[pjreiniger]

This game is a bad example, because you pretty much always know your the rotation you need to start at to get your robot to score. But lets think about 2022.

I can have a 5 ball that starts at ~30 degrees. I might find change around my starting angle to better hit the hub and drive the rest of the waypoints to pickup the other balls. I might also have a two ball on the far side that starts at -20. I like being able to aim the robot in the gui, and have the ability, not requirement, to have the first thing in my auto be "path 0 says you are actually here". Hopefully in time you have localization and can comment out the seeding command and trust your sensors, but when I have just a chassis week 2 or my camera is broken I want that ability.

I agree that it is a terrible idea to reset between Path0 and Path1 if you are stringing them together, just for the very start of your autonomous. That was a pain point last year if you had to do something complicated for stringing paths together, and I'm glad it is no longer the implicit default.

Also, they gryo is a relative sensor, so I can't simply rely on that during boot. If I know I have to line up at 180 this year and seed my state estimate with that, but then the drive team shimmies the robot into position, I read a different state than reality.

[mjansen4857]

I like being able to aim the robot in the gui, and have the ability, not requirement, to have the first thing in my auto be "path 0 says you are actually here".

This is what the auto starting pose in the GUI is for.

Also, they gryo is a relative sensor, so I can't simply rely on that during boot. If I know I have to line up at 180 this year and seed my state estimate with that, but then the drive team shimmies the robot into position, I read a different state than reality.

Your pose should only be reset as the first command in an auto, so it won't do it until actually enabled. This is what auto builder/PathPlannerAuto does. Whatever happens during setup should have no effect on it. If they line it up wrong your angle is going to be messed up regardless unless you are using localization over an initial auto pose.

[pjreiniger]

For something like a two ball last year I probably wouldn't make an auto mode, just use the path directly. I'd personally probably do the same for a five ball, the auto builder API is super nice, but I think we'll still do most of the in-between path stuff ourself in code.

We had like 15 modes where the only thing different was which path we drove after scoring. It seems like a big burden to make 15 autos just to be able to set the initial rotation. If we had to add an additional command in there we would have to touch 15 files instead of our one command that takes the path as a parameter.

We also create test paths purely for tuning, like "drive 10ft and do a 180", and it again feels cumbersome to have to make a path and auto for that

[mjansen4857]

but I think we'll still do most of the in-between path stuff ourself in code

The PathPlannerAuto class has methods to get the starting pose and list of all paths in the auto, so I still suggest using the auto files even if you don't use auto builder.

We had like 15 modes where the only thing different was which path we drove after scoring. It seems like a big burden to make 15 autos just to be able to set the initial rotation. If we had to add an additional command in there we would have to touch 15 files instead of our one command that takes the path as a parameter.

In this case I'd create an auto file for the first path to make use of the starting pose in the auto editor.

Anyways, I'd be OK with a method similar to the ones in PathPlannerAuto like getStartingPoseFromAutoFile, that will just load the preview starting rotation from a path file. There's no real need for the velocity field, and adding a PreviewStartingState object to the path makes using the path constructors more cumbersome for a pretty niche thing.

Another (probably better) option would be to have a previewStartingRotation Rotation2d object in the path that only gets set when using the fromPathFile factory so it doesn't affect the constructors. It can be null otherwise, or even just 0 to avoid having to use optional in C++. It should only ever be used for paths loaded from a file anyways. Then add a getPreviewStartingHolonomicPose method or something along those lines that will combine the first point's position and the preview rotation. Just make it clear in the docs for this method that it can cause problems if used to reset the pose anytime other than the beginning of the first path, and if you're not sure that you need to use it, you shouldn't use it. I worry about things that are easy to misunderstand like this since people will try to use it when they shouldn't and break things.

[pjreiniger]

I made some updates that get it pretty close to option 2. I think you kind of need it to be part of the constructor to keep it consistent when you call the "copy constructor" in replan, etc.

[mjansen4857]

Just a couple nitpick improvements and it should be all good.

[mjansen4857]

Can you add an overload for this constructor that doesn't need a previewStartingRotation? This constructor can be used for on-the-fly generation as well so it would be nice to not need to give it one.