RamseteCommand with trajectory generated later

[andrew28]

I’m utilizing RamseteCommands in my autonomous command groups. When constructing the RamseteCommands when making the groups, I must provide a trajectory. However, sometimes I want to use a trajectory that is to be generated when the command is ran, not earlier.

I thought a potential solution would have been to use a RunCommand or some other custom command to generate a RamseteCommand and schedule it. The problem though is that if I did this inside a command group, and the command group were to be cancelled, that scheduled RamseteCommand would not be cancelled because it has “forked” off and isn’t associated with the group.

I decided on making a wrapper class around RamseteCommand. It takes in a Supplier<Trajectory> so that a trajectory can be generated the moment the command is ran, not when the command is instantiated.
(full robot code: https://github.com/FRC-4277/2020InfiniteRecharge)

public class LazyRamseteCommand extends CommandBase {
    private DriveTrain driveTrain;
    private Supplier<Trajectory> trajectorySupplier;
    private RamseteCommand ramseteCommand;

    public LazyRamseteCommand(DriveTrain driveTrain, Supplier<Trajectory> trajectorySupplier) {
        this.driveTrain = driveTrain;
        this.trajectorySupplier = trajectorySupplier;
        addRequirements(driveTrain);
    }

    @Override
    public void initialize() {

    }

    @Override
    public void execute() {
        if (ramseteCommand == null) {
            ramseteCommand = driveTrain.generateRamseteCommand(trajectorySupplier.get(), false);
            ramseteCommand.initialize();
            return;
        }
        ramseteCommand.execute();
    }

    @Override
    public void end(boolean interrupted) {
        if (ramseteCommand != null) {
            ramseteCommand.end(interrupted);
        }
    }

    @Override
    public boolean isFinished() {
        return ramseteCommand != null && ramseteCommand.isFinished();
    }
}

and I utilize it in command groups like this:

new ParallelCommandGroup(
        new IdleHopperCommand(verticalHopper),
        // Go back to the start, so we can shoot again
        new LazyRamseteCommand(driveTrain, () -> {
          Pose2d startingPosition = new Pose2d(0, 0, new Rotation2d(0));
          Pose2d current = driveTrain.getPose();
          return driveTrain.generateTrajectory(current, startingPosition, true);
        })
      )

I have not had a chance to test it on a robot. I’m worried that there would be some sort of conflict or something wrong with the way I used a command (RamseteCommand) inside a command. Also, are there any better ways to approach this problem?