Running commands that control the drive subsystem in the middle of a path

[WarrenReynolds]

With the addition of the ability to break paths into segments and run commands between segments I'm wondering how someone with a non turret robot (us) would use an auto aiming command in between two segments of a path.

What we did this year was use individual paths for each segment of the auto and we built a separate command to follow each segment.
Than we build a nested sequential command group that scheduled new SequentialCommandGroups each time we that did something that required the drive subsystem.

For example we had a sequential command group that

• Initialize Odometry to first pose of first path
• Open intake command
• std::move(the command that followed the first part of the path)---> This required the drive subsystem
• instant command that stopped the drive
• Then we added ScheduleCommand ( new SequentialCommandGroup) containing
  - Shoot command--> This used the limelight to check distance and bearing to target, rotate drive base until error to target was less than 1 degree and shoot until empty ----> This command required the drive subsystem (so that it could rotate to the target) this interrupted the top level command group but it didn't matter as the next command group was now running. Once shooting was done we then used a ScheduleCommand to schedule another command group to Open the intake and follow the 2nd segment of the path, stop and then used another ScheduleCommand to schedule another group to shoot etc, etc..

So basically the last command in each level of the command was a ScheduleCommand with a new SequentialCommandGroup that had the remaining steps in the auto routine.

Do you have any ideas how we make this fit into the new model of one command for an entire auto routine?
Code from Robot Container for one of our auto shown here if that helps.

   //H2_H2 4 Ball Auto Paths
  pathplanner::PathPlannerTrajectory Path_H2_H2_Part1 = pathplanner::PathPlanner::loadPath("4-Ball-Part1"/*, 1_mps, 1_mps_sq*/);  // Uses our modified version of loadPath method that reads speeds from path JSON so updates don't need rebuild
  pathplanner::PathPlannerTrajectory Path_H2_H2_Part2 = pathplanner::PathPlanner::loadPath("4-Ball-Part2"/*, 4_mps, 1_mps_sq*/); // Uses our modified version of loadPath that reads speeds from path JSON so updates don't need rebuild
  pathplanner::PathPlannerTrajectory Path_4BallReset =  pathplanner::PathPlanner::loadPath("4-Ball-Return"/*,1_mps, 1_mps_sq*/);  // Uses our modified version of loadPath that reads speeds from path JSON so updates don't need rebuild

          //H2_H2 Paths 4 Ball Auto (all High)
    frc2::SwerveControllerCommand<4> H2_H2_Part1_MoveCMD(
        Path_H2_H2_Part1,[this](){ return m_drive.GetPose(); },m_drive.kDriveKinematics,
        frc2::PIDController(AutoConstants::kPXController, 0, 0), frc2::PIDController(AutoConstants::kPYController, 0, 0), thetaController1,
        [this](auto moduleStates){ m_drive.SetModuleStates(moduleStates); },{&m_drive}); // end of AutoPathCCommand
    
     frc2::SwerveControllerCommand<4> H2_H2_Part2_MoveCMD(
        Path_H2_H2_Part2,[this](){ return m_drive.GetPose(); },m_drive.kDriveKinematics,
        frc2::PIDController(AutoConstants::kPXController, 0, 0), frc2::PIDController(AutoConstants::kPYController, 0, 0), thetaController1,
        [this](auto moduleStates){ m_drive.SetModuleStates(moduleStates); },{&m_drive}); // end of AutoPathCCommand

    frc2::SwerveControllerCommand<4> H2_H2_Return_MoveCMD(
        Path_4BallReset,[this](){ return m_drive.GetPose(); },m_drive.kDriveKinematics,
        frc2::PIDController(AutoConstants::kPXController, 0, 0), frc2::PIDController(AutoConstants::kPYController, 0, 0), thetaController1,
        [this](auto moduleStates){ m_drive.SetModuleStates(moduleStates); },{&m_drive}); // end of AutoPathCCommand

 #define __AUTO_SHOOT 
 SetCalculatedShooterSpeedsThenFeedWhenReady{&m_shooterSUB,&m_storageSUB,&m_drive,&m_limelightSUB}

 #define __AUTO_INTAKE frc2::ScheduleCommand(new 
 IntakeOutAndIntakeDriveInwards{&m_intakeSUB,&m_storageSUB,kLoaderMotorTestSpeed})

 #define __AUTO_STOP frc2::InstantCommand{[this] {m_drive.DriveStop();}}

     //4Ball:H2_H2 Setup REL, pickup 2 ball, Auto Shoot high 1&2, pickup 3&4 ball, Autoshoot High.

m_autos[0] = new frc2::SequentialCommandGroup{
            frc2::InstantCommand([]{ fmt::print("Started H2_H2 Auto at {}\n",frc::Timer::GetFPGATimestamp());}),
            InitializeOdometry(&m_drive, Path_H2_H2_Part1.getInitialState()->pose.Translation(), Path_H2_H2_Part1.getInitialState()->holonomicRotation), 
            __AUTO_INTAKE,
            std::move(H2_H2_Part1_MoveCMD),
            __AUTO_STOP,
            frc2::ScheduleCommand(new frc2::SequentialCommandGroup{
                frc2::InstantCommand([this]{m_shooterSUB.SetDistanceToTarget(3750.0);}), //Manually set distance incase limelight is offline/ not recognising the target
                __AUTO_SHOOT,           //Shoot Ball 1 & 2
                __AUTO_INTAKE,
                frc2::ScheduleCommand(new frc2::SequentialCommandGroup{
                    std::move(H2_H2_Part2_MoveCMD),
                    __AUTO_STOP,
                    frc2::ScheduleCommand(new frc2::SequentialCommandGroup{
                        frc2::InstantCommand([this]{m_shooterSUB.SetDistanceToTarget(4800.0);}),  //Manually set distance incase limelight is offline/ not recognising the target
                        __AUTO_SHOOT       //Shoot Balls 3&4
                        //frc2::WaitCommand(2_s),
                        //frc2::ScheduleCommand(new frc2::SequentialCommandGroup{
                        //    std::move(H2_H2_Return_MoveCMD),
                        //    __AUTO_STOP
                        //})
                    })
                })
            })
        };              

[WarrenReynolds]

Thinking about this, I suppose one option would be to have a duplicate versions of commands (like auto aim and Shoot in our case) that are used in both in Autonomous and Teleop periods of the match.

With the autonomous version of the command "not" requiring the drive subsystem. The drive subsystem would already be required by the path follower command so this would prevent the default drive joystick command from taking control.

Then have another version of the "auto aim and shoot" command that is bound to a button for use during teleop with this command requiring the drive subsystem (So that it does interrupt the default drive command from the joystick) and hence allows the robot to auto aim and shoot.

I wish I had of thought of this at the beginning of the year and it would have saved me a lot of pain with nested sequential command groups that were needed to get around this problem.

Alternatively is there a way to add a command to command group at runtime and ignore the subsystem/s that they require to avoid this command interruption problem that occurs when more then one command in a sequential group require the same subsystem.

[mjansen4857]

I'm not exactly sure what the problem here is. What "interruption problem"? Maybe C++ is different, but there's nothing wrong with doing

new SequentialCommandGroup(
    new PPSwerveControllerCommand(pathGroup[0], ...),
    new TurnToTargetAndShoot(),
    new PPSwerveControllerCommand(pathGroup[1], ...),
    ...
)

at least in Java.

[WarrenReynolds]

I'm not sure how the inner workings of the command groups are executed in regards to the scheduler and subsystem requirements.
I wish the documentation on command groups was more detailed in this area. I just read the 2023 Docs and they are no better at explaining how they execute, but we had issues with our autos this year when we used the layout like yours above. Admittingly we had a lot of command groups that also contained other command groups, and maybe this is where the problem stems from. Or it might have been an if {this} schedule {that} deep down a couple of layer deep in command groups.

Basically the problem for us was that commands that were down deep inside a couple of groups that were requiring a subsystem that had already been required by commands at the top level of a autonomous command group where cancelling the top level command group through the requirements interruption process of the scheduler.

I'm also thinking that if this code

        if(this->m_unpassedMarkers.size() > 0 && currentTime >= this->m_unpassedMarkers[0].time) {
        PathPlannerTrajectory::EventMarker marker = this->m_unpassedMarkers[0];
        this->m_unpassedMarkers.pop_front();

        for(std::string name : marker.names){
          if(this->m_eventMap.find(name) != this->m_eventMap.end()){
            this->m_eventMap[name]->Schedule();

Fires a command that requires a subsystem that is already required by the auto routine (ie drivebase), the scheduler will interrupt the auto routine so that it can acquire the subsystem for the command that was just scheduled.

So, in our case if we used the new proposed Path Planner markers to fire the "auto aim and shoot" command (which is actually a command group) the scheduler would check if the requirements of the "auto aim and shoot" command are available. It would see that "auto aim and shoot" needs the drive subsystem and it would also see that the drive subsystem is currently being used by the auto routine (from the requirements of PPSwerveControllerCommand). So it would cancel the auto routine command group so that the "auto aim and shoot" command can have it's subsystem and do its stuff.

So in this case the auto routine would drive to the first stop point, auto aim and shooting and then it wouldn't continue past this point onto the next segment of the path. We had this exact problem with our autos.

The only work around I can see to make PPSwerveControlerCommand do what you want in regards to the event map stuff would be to make a top level command group that requires no subsystems at all so that it could never be interrupted by a command that requires something. And then from inside this command your schedule individual PPSwerveControllerCommands to follow the path segment, which it turn schedules commands at markers along the path (obviously these commands couldn't both require the drive subsystem). Then at a stop point, the top level command (that requires nothing) you schedule an "Auto Aim and shoot" command. The first segment of the path following is done, so it wouldn't matter about drive subsystem requirements, then after the timeout has finished for this stop point, the next segment of the path can be followed with another scheduling of PPSwerveControllerCommand to follow the next segment.

I hope this makes sense.

[mjansen4857]

So, in our case if we used the new proposed Path Planner markers to fire the "auto aim and shoot" command (which is actually a command group) the scheduler would check if the requirements of the "auto aim and shoot" command are available. It would see that "auto aim and shoot" needs the drive subsystem and it would also see that the drive subsystem is currently being used by the auto routine (from the requirements of PPSwerveControllerCommand). So it would cancel the auto routine command group so that the "auto aim and shoot" command can have it's subsystem and do its stuff.

I wouldn't use a marker for something like an aim and shoot command. I would just put that in between two path following commands. If you load the path as a path group it will give you a vector of paths so you can separate the path into multiple path following commands. This shouldn't cancel anything since it would just run the command group normally instead of triggering a drive command.

I'm also thinking that if this code

    if(this->m_unpassedMarkers.size() > 0 && currentTime >= this->m_unpassedMarkers[0].time) {
    PathPlannerTrajectory::EventMarker marker = this->m_unpassedMarkers[0];
    this->m_unpassedMarkers.pop_front();

    for(std::string name : marker.names){
      if(this->m_eventMap.find(name) != this->m_eventMap.end()){
        this->m_eventMap[name]->Schedule();

Fires a command that requires a subsystem that is already required by the auto routine (ie drivebase), the scheduler will interrupt the auto routine so that it can acquire the subsystem for the command that was just scheduled.

Yeah, you're right. Didn't think about that. That's a bit of an annoying limitation but to get around it you can either make sure your auto command does not have any commands that require any subsystems that the marker-triggered commands will use, or you can make the main auto command uninterruptible just to make sure that it will never be interrupted.

I think in light of this, I will start work on a preliminary version of the auto builder stuff, that will just create a command group for a path following command and its marker commands. So instead of directly scheduling a command at a marker, it will be in a command group so all of the requirements stuff will work correctly and nothing will be interrupted.

[WarrenReynolds]

or you can make the main auto command uninterruptible just to make sure that it will never be interrupted.

This would just prevent the marker command from running at all. If the schedule can't get the requirements it needs for a command to run (because an uninterruptible command currently has the requirement) it won't schedule the command and it will just skip it.

I think in light of this, I will start work on a preliminary version of the auto builder stuff, that will just create a command group for a path following command and its marker commands.

I'm really looking forward to this top level auto executor. My thoughts are there are two ways you could attack this.

You could try and construct a sequential command group that is made up of a series of PPSwerveControllerCommands (that have the marker code as it stands now) followed by the commands that are added at the stop points (it wouldn't matter if these needed the same subsystem because they are in the same sequential command) with this pattern repeated over and over until all path segments in the path group and all the associated stop point commands have been traversed. Then execute this one constructed sequential command group when the auto starts. (i'm guessing that what you are thinking at the moment)

Another alternative is to make this top level command have no commands at all with every command that needs to run, being run with a ->Schedule() call. This way you will never have a subsystem requirement killing the top level auto command. The only problem with this is once a command has been fired with the ->Schedule() method the top level command wouldn't know the state of where that command is up to (ie if its finished or not). You would have to continually poll the scheduler with an isScheduled() call on the command that is currently running in the auto routine (either a ppSwerveControllerCommand or a stop point command.) so that you know if you are allowed to move onto the next command or not. All this checking would be happening in the execute method of the top level auto command.

It gets a bit tricky at the stop points trying to work out if you are allowed to go on or not. Sometimes you might want these stop point commands to be executing without waiting, other times you might not want the auto to continue until the stop point commands are done and other times you might want a time out. I'm not sure I like the wait time being hard coded into the stop point, I feel a better option would be build to a parallel command group on the fly at the stop point from the list of commands to be run at the stop point and then schedule it with a ->Schedule() call from the top level auto command. Ditch the wait time at the stop point and replace this with a true or false boolean at the stop point that indicates if these stop point commands should "execute and continue" or "execute and wait until done" before moving onto the next path segment. If people want to wait for seconds at a stop point they can add a delay command as one of the commands to be executed at the stop point with the wait until done flag set at the stop point. By using this method you give the end user the ability to execute these stop point commands in any way they want. If the end user wants commands to execute sequentially at a stop point they would need to build a sequential command group and add that command to the command map.

If you want more granularity over the control at the stop points you could add an extra fields into the command map at the stop point.(not sure if this can be done). Or you could use the command name member variable of the command as a method to give you more control on how commands are executed at stop points. This rarely get used by teams and top level auto follower could check the name of the commands in the stop point command map using the GetName() method. If its blank or starts with p_ assume this is a command that is going to be run in parallel at the stop point. If it starts with s_ make this command run sequentially at the stop point and keep checking the IsScheduled() to know if you can move onto the next stop point command or onto the next path segment.

I feel autonomous debugging would be easier if you used the later method with everything being run with the ->Schedule() approach. But I also see how building the sequential group completely before running means you wouldn't have to check if thing are finished with repeated IsScheduled() calls.

So instead of directly scheduling a command at a marker, it will be in a command group so all of the requirements stuff will work correctly and nothing will be interrupted.

I actually prefer the scheduling at a marker approach as it stands at the moment, because in order to make this into an all in one command group you will need to create a parallel groups with delay times at the front of the marker commands to run in parallel with the path following commands. And the fact that you running them in parallel with the path following doesn't get around requirement issues anyway. "Additionally, requirements are enforced within all three types of parallel groups - a parallel group may not contain multiple commands that require the same subsystem." Quoted from the docs. I don't actually know what happens if you attempt to do this, I'm assuming it will interrupt itself) So I think it has to be a hard and fast rule that marker commands can't require the drive. This completely makes sense as the robot is following a path at this point in time so the marker commands can't take control of the drive. I'd even schedule the PPServerControllerCommands with the uninterruptible flag so this could never accidental happen.

Personally, I like the option of the top level auto having no commands and no requirement so that it can never be canceled from a mistake of someone adding two commands that run in parallel that require the same subsystem at either marker points or stop points.
If they do it will only prevent the commands at that marker point or stop point from doing what they are supposed to do and it will not kill the auto completely. If you don't do it this way, a command that is launched at a marker point might still be running by the time the next marker point or stop point is reached and a Command launched at the next point might require that same subsystem and hence cancel the whole auto group. On this note, if you do end up going down the path of building the group completely before launching it, every command that is added needs to be added as un-interruptible so that an end user can't kill the auto by a command running in parallel requiring something that is already required.

[WarrenReynolds]

My head hurts. Did you draw this on a piece of paper before you implemented it. I think I need a picture to fully follow what's going on. NB: Don't feel the need to draw one if you didn't draw it, I'll work it out eventually.

In regards to this comment in a previous post.

This would just break how command groups work in general. Default commands for the subsystems will be scheduled whenever there isn't a command for that subsystem currently running in the group. (Like if you have a path following command followed by an intake for time command) This could lead to a lot of undefined behavior that you don't want.

What's an example of the undefined behaviour you are be worried about? I've always found the fact that command groups hold all of the requirements for the duration of the group to finish a real pain. I'd prefer if the individual commands in a sequential group only held the requirements of what they needed while they needed it. (IE while the command is running that requires a particular subsystem.) This is only because this command group feature has caused us grief a couple of times when we have had some conditional tests launching commands. Like if an intake jam is detected launch clear intake jam sequence command. (This would cancel our whole auto) And sometimes I've actually wanted the default commands to run and they cant run because a group that hasn't finished yet has a hold on the subsystem. Maybe I'm not using commands how they are supposed to be used.

Cancel any previous event commands that share any requirements with this event

I like this idea, but does this cause cascading cancelation of the groups that hold the commands all the way up to your top level auto command? I don't know the answer to this question. I know in the past I've had trouble cancelling command groups from inside the group, so this may not be the case.

I really wish there was an in depth manual written on the command groups put out by WPILib. Its either whats written in the FRC docs or the API comments and neither of these explain much about the detailed inner working of command groups.
Is your version followPathWithEvents publicly available in github anywhere?

[mjansen4857]

What's an example of the undefined behaviour you are be worried about?

The main issue here is repeatability. When a command group requires a subsystem, you know that no other commands will be running on that subsystem until the group finishes. If you give up control in the middle of the group, you have no idea what that system has been doing, and you are allowing it to do stuff that you didn't explicitly tell it to do. Sure, you can avoid these problems if you make sure that it will never do anything weird while you give up control. But having this requirement just makes sure that you can't put yourself in a situation where something weird happens.

Like if an intake jam is detected launch clear intake jam sequence command. (This would cancel our whole auto) And sometimes I've actually wanted the default commands to run and they cant run because a group that hasn't finished yet has a hold on the subsystem.

For stuff like this I actually like doing state machines in the subsystem periodic method, then use commands to set the "desired state". So in this case you can run a command in your command group to set the desired state of the intake system to "intake" and then if it detects a jam, the state machine can switch itself to a "clear jam" state and then switch back to "intake" when it's done. All without interrupting anything since its happening in the subsystem.

I like this idea, but does this cause cascading cancelation of the groups that hold the commands all the way up to your top level auto command?

I did test this, and nope, it just cancels the specific command and the command group doesn't care. It just sees that the command ended and moves on.

Is your version followPathWithEvents publicly available in github anywhere?

autobuilder-v1 branch. Java only and lightly tested.

[WarrenReynolds]

I'm still trying to get my head fully around command groups, talk about a steep learning curve. But after doing a bit of reading I don't think the following lines will work.

pathplanner/pathplannerlib/src/main/java/com/pathplanner/lib/auto/BaseAutoBuilder.java

Lines 41 to 46 in 91bf0dc

	new InstantCommand(
	() -> {
	for (WrappedEventCommand c : commandsToCancel) {
	c.cancel();
	}
	}),

Looking at the WPILib source code for command groups and commands

https://github.com/wpilibsuite/allwpilib/blob/main/wpilibNewCommands/src/main/native/cpp/frc2/command/SequentialCommandGroup.cpp#L1-L74

https://github.com/wpilibsuite/allwpilib/blob/main/wpilibNewCommands/src/main/native/cpp/frc2/command/Command.cpp#L120-L126

https://github.com/wpilibsuite/allwpilib/blob/main/wpilibNewCommands/src/main/native/include/frc2/command/Command.h#L290-L303

I don't think the commands that are part of a command groups are actually executed by the command scheduler and I think calling the .cancel() on a command acts on the scheduler instance to cancel commands that are currently in the scheduler. But since the running command is the top level command group and the individual member commands are being executed by logic from inside that command group I don't think the .cancel() call won't do anything towards canceling individual commands in a group (That is unless they have been manually added to the scheduler as individual commands (ie with a ->Schedule() call) .and hence are not a part of group)

I derived this conclusion after finding this statement for isScheduled()

/**

• Whether or not the command is currently scheduled. Note that this does not
• detect whether the command is being run by a CommandGroup, only whether it
• is directly being run by the scheduler.
• @return Whether the command is scheduled.
  */
  bool IsScheduled() const;

It doesn't say the same thing for Cancel() method but after looking at command.cpp source code I came to the same conclusion about Cancel() only working directly on commands and not commands in command groups.

I could be completely wrong with all these conclusions (there is a good chance as I'm a serious amateur) but after looking at the source code for groups I think you should copy SequentialCommandGroup or ParallelCommandGroup as a template for building your AutoCommand that then do like WPIlib does and call the Init, exec, isfinished, etc calls for each command yourself from inside your master command. Then you have complete control over the member commands and you can end them at your will from logic you are controlling.

Food for thought.

[mjansen4857]

I think you're right. Thinking about it, the test I used to see if this would work would actually not notice if the previous command was actually cancelled or not. Easy fix for this though by just overriding the isFinished method of the wrapped command to return true instead of canceling the command. Then the command group will stop running that command as it is saying its finished.

A custom command group does sound like a more elegant solution though, so I will look into that.

[mjansen4857]

Yeah this is definitely the better solution. This custom "command group" also gives people that don't use PPLib path followers the means to trigger events with their custom path following commands without writing the logic for it.

Thanks for the idea! This will actually probably replace the marker functionality within the PPLib commands themselves since it doesn't really make sense to trigger events in a way that isn't safe for command groups when you can just construct a simple FollowPathWithEvents command that is safe to use.

[WarrenReynolds]

I just built PP from scratch. I noticed there is no option to add commands to a stop point command map at this point in time. Are you planning to have a different command map for the "Stop Point" Commands as compared to the "Marker Commands" or, are the stop point commands just normal marker commands with whole number values for their position?

[mjansen4857]

Planning to add the ability to add event names to stop points and start/end points with the more complete auto builder stuff eventually. The handling of events at points like this will be handled differently, as they will be triggered between path following commands, not as the following command runs. But, from the user perspective it will be the same event map as it will just pull the commands from that.

[WarrenReynolds]

Honestly Michael, you are a coding wizard. I go away for the weekend and come back and check out the autobuilder branch and you have written an entire auto framework for multiple drive base types in multiple languages. You must have a time turner or have mastered cloning or something as I can't comprehend how you can achieve what you do in the time that you do it in.

The way that you have structured your code so that teams can pick and choose the bits they want is truly amazing. I have a million questions, but I'm going to try and work them out for myself first before wasting your time.

You are true scholar in this field and I can assure you there are many FRC teams out there that really appreciating the work that you are doing and the fact that you are making it publicly available.

Cheers
Warren

[mjansen4857]

It was a lot easier than I originally thought lol. Biggest issue was dealing with C++ weirdness.

Hoping it raises the floor for auto for teams that may have lacked the knowledge/manpower to do complex autos before.

[WarrenReynolds]

Hoping it raises the floor for auto for teams that may have lacked the knowledge/manpower to do complex autos before.

I'm sure it will. The only thing I can see that is needed now for the teams you mention above is one or two line code snippets to show how a full project might use all of this functionally (especially for the C++ teams with all the pointer and address of stuff). Like what you have done in the wiki for Basic paths, Path Groups, On-the-fly Generation.

Something like in the robotcontainer,
You need to a create a member variable to hold the instance of the eventMap that will hold all of the commands that you want to run during auto.
is this the line? std::unordered_map<std::string, std::shared_ptrfrc2::Command> m_eventMap;

Then the code that adds a "textName", commandptr pairs to this Map.
I'm not even sure what this looks like in C++ yet.

Question, would you build an array of auto commands (for every different auto option that might be chosen by the drive team from the dash board, with each auto option being built from it own pathgroup) at code startup, so they are already to go and loaded into memory before autonomous is started or would you construct the single auto group command on the fly when auto starts based on the auto chooser?

Maybe a one liner in wiki that shows the preferred way to setup the auto chooser.