How to .schedule() PathplannerLib Pathfinding Command

Technical
,
1

We are trying to implement pathplannerlib pathfinding on our robot. Currently when we try to schedule a pathfinding path generated by pathfinding, the robot does not execute it. We are currently using the AutoBuilder.pathfindToPose() to generate a path and then using the .schedule() to attempt to execute it.

All the code for our robot can be found here: GitHub - Team4169/Swerve2024: Git Repo for the 2023 - 2024 season. Here is the most relevant sections of code:

Path generation and attempted execution:

 def getObjectDetectionPathfinding(self) -> commands2.Command:
        """Returns the pathfinding command from position"""

        # changeHorizontal = self.datatable.getEntry("objHorizontal").getValue()
        # changeDistance = self.datatable.getEntry("objDistance").getValue()
        changeHorizontal = 1000
        changeDistance = 1000

        changeRot = math.atan2(changeHorizontal, changeDistance)
        print(f"Change Rot: {changeRot}")
        targetPose = Pose2d(changeDistance, changeHorizontal, Rotation2d.fromRotations(changeRot / (math.pi * 2)))
        print(f"Target Pose: {targetPose}")

        pathfindingCommand = AutoBuilder.pathfindToPose(
            targetPose,
            AutoConstants.constraints,
            goal_end_vel=0.0,
            rotation_delay_distance=0.0
        )

        
        if pathfindingCommand:
            print(pathfindingCommand)
            pathfindingCommand.schedule()

Pathplanner initialization in swerve subsystem:

   AutoBuilder.configureHolonomic(
            self.getPose,
            self.resetOdometry,
            self.getChassisSpeeds,
            self.driveChassisSpeeds,
            AutoConstants.pathFollowerConfig,
            self.shouldFlipPath,
            self
        )

Note: When we print the pathfindingCommand we get a <pathplannerlib.commands.PathfindHolonomic object>. In addition, our pathplanner auto works and uses the .schedule() command, but uses a <pathplannerlib.auto.PathPlannerAuto object>.

2

Hello everyone, I wanted to follow up to see if anyone is familiar with this. I really appreciate it. Thank you, Sean

3

You would need

 CommandScheduler.getInstance().run();

In your robotPeriodic or autonomousPeriodic to run the scheduled command.

You’re going to want that code in your periodic because it will need to continuously execute the command in order to run the path all the way through.

Please see Command Scheduler and Command Based Programming for more information.

4

Sorry, my code syntax here is in Java. There should be a Python equivalent but I’m having trouble finding it at the moment. I will see if I can find it after I get some coffee :slight_smile:

5

Looks like for python it will be

commands2.CommandScheduler.run()

It would be worth checking the docs for the Python CommandScheduler though. I am not entirely sure about the syntax.

I would still recommend at least a cursory read-through of command based while referencing the Python implementation.

6

Thank you! We tried this but ran into an error involving our pathplannerlib.commands.PathfindHolonomic not having a ._disable() function. Do you have any other ideas?

7

Can anyone help with this? we are having the exact same issue.

8

Something like the following should work…. If it doesn’t then perhaps your AutoBuilder isn’t configured? Or perhaps you have a command requirement conflict.


// Create the constraints to use while pathfinding
            PathConstraints constraints = new PathConstraints(
                    2.0, 2.0,
                    Units.degreesToRadians(540), Units.degreesToRadians(720));

            // Since AutoBuilder is configured, we can use it to build pathfinding commands
            c = AutoBuilder
                    .pathfindToPose(
                            targetPose,
                            constraints // Goal end velocity in meters/sec
                    );
c.schedule();

(I regret that I can’t format this well on my phone…)