PathPlannerPath::samplePath() can attempt to access a bad optional

[aolgeirson]

Describe the bug
Occasionally, when calling PathPlannerPath::samplePath() on a short path with only a start and end waypoint defined by the user, the robot code will crash.

To Reproduce
See https://www.chiefdelphi.com/t/pathplanner-throwing-a-bad-optional-access-exception-on-path-construction/498617/2 for more info (example code, callstacks, etc.)

Expected behavior
The code won't crash.

Versions: (please complete the following information):

• OS: Arch Linux
• GUI Version: 2025.2.2
• PPLib Version: 2025.2.2
• PPLib Language: C++
  Additional context
  The issue occurs on line 929 of pathplanner/pathplannerlib/src/main/native/cpp/pathplanner/lib/path/PathPlannerPath.cpp due to calling .value() without first checking for validity. I am more than happy to create a PR for this issue, but I don't know what the desired behaviour should be if a cubic lerp cannot be created due to too few paths.

[mjansen4857]

It should be impossible for those optionals to be accessed incorrectly as long as the data being fed in is valid. The issue here lies with whatever is creating the waypoints, which in your case is the waypointsFromPoses method. The only thing I see wrong with that currently is that you're passing in robot poses instead of creating a pose from position/heading. That shouldn't keep the waypoints from being valid but it would create a bad path.

[mjansen4857]

I don't see any errors in the waypointsFromPoses logic. Are you manually creating waypoints anywhere?

[aolgeirson]

We are doing the following to initialize our path

                                   std::vector<Waypoint> waypoints = PathPlannerPath::waypointsFromPoses({m_odometry->getPose(),
                                                                                                           goal_pose});

                                    // The constraints for the path. TODO CHANGE IT
                                    PathConstraints constraints(1_mps, 2_mps_sq, 90_deg_per_s, 180_deg_per_s_sq);

                                    frc::ChassisSpeeds speeds = m_odometry->getFieldRelativeSpeeds();
                                    units::meters_per_second_t vel = static_cast<units::meters_per_second_t>(MathUtils::pythag(speeds.vx(), speeds.vy()));

                                    auto path = std::make_shared<PathPlannerPath>(
                                        waypoints,
                                        constraints,
                                        IdealStartingState(vel, m_odometry->getPose().Rotation()), // The ideal starting state, might need to be changed
                                        GoalEndState(0.0_mps, goal_pose.Rotation())                // Goal end state. You can set a holonomic rotation here.
                                    );

And this to follow it:

return AutoBuilder::followPath(path).Repeatedly().Until([this, goal_pose] -> bool {
                                      return MathUtils::getDistance(goal_pose, m_odometry->getPose()) < CONSTANTS::FIELD_POSITIONS::PATH_FINISHED_DIST_THRESHOLD &&
                                              units::math::abs(goal_pose.Rotation().Degrees() - m_odometry->getPose().Rotation().Degrees()) < CONSTANTS::FIELD_POSITIONS::PATH_FINISHED_ANGLE_THRESHOLD;
                                    }).Until([this] -> bool {
                                      const double t = CONSTANTS::FIELD_POSITIONS::DRIVER_OVERRIDE_THRESHOLD;
                                      return m_timer.HasElapsed(0.5_s) && (std::abs(m_stick->GetRightX()) > t || 
                                             std::abs(m_stick->GetRightY()) > t || 
                                             std::abs(m_stick->GetLeftX()) > t || 
                                             std::abs(m_stick->GetLeftY()) > t);
                                    }).AndThen([this] {
                                      m_timer.Stop();
                                    }); });

[mjansen4857]

Can you try changing your waypointsFromPoses usage to use the robot's position and direction of travel (not rotation) for the poses? I'm unsure why this would cause a waypoint not to get its proper control points but I see nothing else wrong.

[aolgeirson]

I'll try that tonight and update you in around 5 hours

[aolgeirson]

So we ran a test with new code that uses the heading, and the same behavior occurs. The code change is as follows and the same bug occurs in the same conditions.

                                    frc::ChassisSpeeds speeds = m_odometry->getFieldRelativeSpeeds();
                                    // Makes a vector of waypoints. Waypoint 1 is the current pos, 2 is the goal
                                    std::vector<Waypoint> waypoints = PathPlannerPath::waypointsFromPoses({frc::Pose2d(m_odometry->getPose().Translation(), units::math::atan(speeds.vy/speeds.vx)),
                                                                                                           goal_pose});

                                    // The constraints for the path. TODO CHANGE IT
                                    PathConstraints constraints(1_mps, 2_mps_sq, 90_deg_per_s, 180_deg_per_s_sq);

                                    units::meters_per_second_t vel = static_cast<units::meters_per_second_t>(MathUtils::pythag(speeds.vx(), speeds.vy()));

                                    auto path = std::make_shared<PathPlannerPath>(
                                        waypoints,
                                        constraints,
                                        // Heading is the direction of movment, so $\theta = tan^{-1}(y/x)$
                                        IdealStartingState(vel, units::math::atan((speeds.vy/speeds.vx)) ), // The ideal starting state, might need to be changed
                                        GoalEndState(0.0_mps, goal_pose.Rotation())                // Goal end state. You can set a holonomic rotation here.
                                    );

                                    path->preventFlipping = true;
                                    m_timer.Restart();

                                    return AutoBuilder::followPath(path).Repeatedly().Until([this, goal_pose] -> bool {
                                      return MathUtils::getDistance(goal_pose, m_odometry->getPose()) < CONSTANTS::FIELD_POSITIONS::PATH_FINISHED_DIST_THRESHOLD &&
                                              units::math::abs(goal_pose.Rotation().Degrees() - m_odometry->getPose().Rotation().Degrees()) < CONSTANTS::FIELD_POSITIONS::PATH_FINISHED_ANGLE_THRESHOLD;
                                    }).Until([this] -> bool {
                                      const double t = CONSTANTS::FIELD_POSITIONS::DRIVER_OVERRIDE_THRESHOLD;
                                      return m_timer.HasElapsed(0.5_s) && (std::abs(m_stick->GetRightX()) > t || 
                                             std::abs(m_stick->GetRightY()) > t || 
                                             std::abs(m_stick->GetLeftX()) > t || 
                                             std::abs(m_stick->GetLeftY()) > t);
                                    }).AndThen([this] {
                                      m_timer.Stop();
                                    }); });

[mjansen4857]

I just can't reproduce that issue or find anything that should allow for it to happen with the way you're using it. Does it happen all the time or only sometimes? Can you provide a set of poses that cause this?

[aolgeirson]

Unfortunately, the robot is currently on a truck to Texas with the logs onboard. We attempted to check for bad poses but couldn't quite find a pattern. Right now we're just running a try catch around path construction. We'll add some more logging and I can give you the logging USB at champs.

[totozZ]

hello, do you fix this problem, we meet this problem too.

[aolgeirson]

It's been a while, but I believe we used a workaround in wrapping the construction of the PathPlannerPath object in a try/catch. For whatever reason, we didn't have any issues with continuously running the fallible function until it eventually worked. I don't remember a lot of the design decisions and full debugging process, but our workaround was here: https://github.com/FRC2240/Reefscape2025/blob/d28751346f05499b3d57df18e863e4a3a3ed5593/src/main/cpp/swerve/Trajectory.cpp#L145

Best of luck.