Scheduling Multiple Ramesete Commands for Path Following

[jdao]

I have a wrapper Command that reads a .csv file that translates the data as points to generate points for my trajectory following. The issue seems to be that when I do this, I am only able to run a single path. Attempting to run multiple paths in succession fails where only the first path runs and the second path doesn’t seem to run. I believe it has to do with the wrapper Command scheduling a ramsete Command directly instead of feeding it in a parallel/sequential command group. Is there a betterway of doing this?:

ParallelCommandGroup:

public class TestSequentialForward extends SequentialCommandGroup {

    public TestSequentialForward(DriveTrain driveTrain) {
        ArrayList<Pose2d> pathA = new ArrayList<>();
        pathA.add(new Pose2d(0,0,new Rotation2d()));
        pathA.add(new Pose2d(Units.feetToMeters(2), Units.feetToMeters(0),new Rotation2d()));
        ArrayList<Pose2d> pathB = new ArrayList<>();
        pathB.add(new Pose2d(Units.feetToMeters(2),0, new Rotation2d()));
        pathB.add(new Pose2d(Units.feetToMeters(4), Units.feetToMeters(0),new Rotation2d()));

        addCommands(new ResetOdometry(driveTrain),
                    new TestPath(driveTrain, pathA, false),
                    new TestPath(driveTrain, pathB, false));
    }
}

TestPath (Note that this is a test wrapper that doesn’t read a .csv file, but the concept is the same):

public class TestPath extends CommandBase {
    @SuppressWarnings({"PMD.UnusedPrivateField", "PMD.SingularField"})
    private final DriveTrain m_driveTrain;
    private Trajectory trajectory;
    private static double m_period = 0.02;
    private Notifier m_notifier;
    private ArrayList<Pose2d> m_path;
    private boolean m_isInverted;
    VitruvianRamseteCommand followTrajectory;

    /**
     * Creates a new ExampleCommand.
     *
     * @param driveTrain The subsystem used by this command.
     */
    public TestPath(DriveTrain driveTrain, ArrayList<Pose2d> path, boolean isInverted) {
        m_driveTrain = driveTrain;
        m_path = path;
        m_isInverted = isInverted;
        // Use addRequirements() here to declare subsystem dependencies.
        addRequirements(m_driveTrain);
    }

    // Called when the command is initially scheduled.
    @Override
    public void initialize() {
        var trajectoryConstraints = new DifferentialDriveKinematicsConstraint(m_driveTrain.getDriveTrainKinematics(),
                3);

        var trajectoryConfig = new TrajectoryConfig(Units.feetToMeters(4), Units.feetToMeters(2));

        trajectoryConfig.setReversed(m_isInverted);

        trajectory = TrajectoryGenerator.generateTrajectory(m_path, trajectoryConfig);

        followTrajectory = new VitruvianRamseteCommand(
                trajectory,
                m_driveTrain::getRobotPose,
                new RamseteController(),
                m_driveTrain.getFeedforward(),
                m_driveTrain.getDriveTrainKinematics(),
                m_driveTrain::getSpeeds,
                m_driveTrain.getLeftPIDController(),
                m_driveTrain.getRightPIDController(),
                m_driveTrain::setVoltageOutput,
                m_driveTrain,
                m_path,
                trajectoryConfig
        );
        CommandScheduler.getInstance().schedule(followTrajectory);
    }

    // Called once the command ends or is interrupted.
    @Override
    public void end(boolean interrupted) {
    }

    // Returns true when the command should end.
    @Override
    public boolean isFinished() {
        return false;
    }
}

VitruvianRamseteCommand:

public class VitruvianRamseteCommand extends RamseteCommand {
    Trajectory m_trajectory;
    TrajectoryConfig m_config;
    ArrayList<Pose2d> m_path;
    DriveTrain m_driveTrain;
    Supplier<Pose2d> m_pose;
    double autoDuration, autoStartTime;

    public VitruvianRamseteCommand(Trajectory trajectory, Supplier<Pose2d> pose, RamseteController controller, SimpleMotorFeedforward feedforward, DifferentialDriveKinematics kinematics, Supplier<DifferentialDriveWheelSpeeds> wheelSpeeds, PIDController leftController, PIDController rightController, BiConsumer<Double, Double> outputVolts, DriveTrain driveTrain, ArrayList<Pose2d> path, TrajectoryConfig config) {
        super(trajectory, pose, controller, feedforward, kinematics, wheelSpeeds, leftController, rightController, outputVolts, driveTrain);
        m_driveTrain = driveTrain;
        m_pose = pose;
        m_trajectory = trajectory;
        m_path = path;
        m_config = config;
    }

    @Override
    public void initialize() {
        super.initialize();
        autoStartTime = Timer.getFPGATimestamp();
        double distance = 0;
        for (int i = 0; i < m_path.size() - 1; i++) {
            var pointA = m_path.get(i);
            var pointB = m_path.get(i + 1);

            double deltaX = pointB.getTranslation().getX() - pointA.getTranslation().getX();
            double deltaY = pointB.getTranslation().getY() - pointA.getTranslation().getY();
            double deltaDistance = Math.sqrt(Math.pow(deltaX, 2) + Math.pow(deltaY, 2));
            distance += deltaDistance;
        }
        autoDuration = (distance / m_config.getMaxVelocity()) + 2;

    }

    @Override
    public boolean isFinished() {
        double deltaX = Units.metersToFeet(Math.abs(m_pose.get().getTranslation().getX() - m_path.get(m_path.size() - 1).getTranslation().getX()));
        double deltaY = Units.metersToFeet(Math.abs(m_pose.get().getTranslation().getY() - m_path.get(m_path.size() - 1).getTranslation().getY()));
        double deltaRot = Math.abs(m_pose.get().getRotation().getDegrees() - m_path.get(m_path.size() - 1).getRotation().getDegrees());
        boolean isFinished  = ((deltaX < 0.25) && (deltaY < 0.25) && (deltaRot < 2)) || (Timer.getFPGATimestamp() > (autoDuration + autoStartTime));
        return isFinished;
    }
}