WPILib C++ SwerveControllerCommand example crashes in Automonous

Hi Everyone,
As an off season project we decided to try out the Swerve Drive Specialties Swerve modules. Configuration is Mk4 Modules with L2 gearing, Falcons on the steer and drive and CANCoder as the absolute angle measuring device and a pigeon to measure robot heading. We have managed to get them operational with the JAVA sample code from SDS’s github and are very very happy with the results.

My limited programming background is in c++, so we have been looking at the WPILib SwerveControllerCommand c++ sample code. We have massaged this code to work with the Falcons and CANCoder and have also managed to get the odometry working. (still needs tuning on the velocity PIDF on the falcons drive motors, but seems to give positions that are close)

Now we are trying to get the example Auto Routine (an S curve) going that uses the SwerveControllerCommand objects in the example code, but this auto routine crashes the roborio. I have no idea how I should troubleshoot this. I’ve worked out that the auto command gets scheduled and if the autoroutine includes the SwerveControllerCommand in the SequentialCommandGroup it crashes. Before I pull out the last of my remaining hair I’d like to check if anyone else has go this sample code to work and in particular with the SDS swerve modules? Alternatively are there any C++ teams out there using the SDS modules with Falcons that use the new command based framework that would like to share their code with us. We would really love to get some nice swerve drive auto paths happening and this new WPILib path following stuff looks amazing.
Any help greatly appreciated.
Link to sample code
WPILib c++ Swerve Controller Command

Have you seen this issue in the wpilib repository?

Thanks for your reply. I’m not using simulation, this is running on a physical robot. Following is the code that returns a pointer to the auto command to execute. This code is located in RobotContainer.cpp. Does anyone know what needs to change in here to make it work? (ie not crash)?

frc2::Command* RobotContainer::GetAutonomousCommand() {
// Set up config for trajectory
frc::TrajectoryConfig config(AutoConstants::kMaxSpeed,
AutoConstants::kMaxAcceleration);
// Add kinematics to ensure max speed is actually obeyed
config.SetKinematics(m_drive.kDriveKinematics);

// An example trajectory to follow. All units in meters.
auto exampleTrajectory = frc::TrajectoryGenerator::GenerateTrajectory(
// Start at the origin facing the +X direction
frc::Pose2d(0_m, 0_m, frc::Rotation2d(0_deg)),
// Pass through these two interior waypoints, making an ‘s’ curve path
{frc::Translation2d(1_m, 1_m), frc::Translation2d(2_m, -1_m)},
// End 3 meters straight ahead of where we started, facing forward
frc::Pose2d(3_m, 0_m, frc::Rotation2d(0_deg)),
// Pass the config
config);

frc::ProfiledPIDController<units::radians> thetaController{
AutoConstants::kPThetaController, 0, 0,
AutoConstants::kThetaControllerConstraints};

thetaController.EnableContinuousInput(units::radian_t(-wpi::numbers::pi),
units::radian_t(wpi::numbers::pi));

frc2::SwerveControllerCommand<4> swerveControllerCommand(
exampleTrajectory, [this] { return m_drive.GetPose(); },

  m_drive.kDriveKinematics,

  frc2::PIDController(AutoConstants::kPXController, 0, 0),
  frc2::PIDController(AutoConstants::kPYController, 0, 0), thetaController,

  [this](auto moduleStates) { m_drive.SetModuleStates(moduleStates); },

  {&m_drive});

// Reset odometry to the starting pose of the trajectory.
m_drive.ResetOdometry(exampleTrajectory.InitialPose());

// no auto
return new frc2::SequentialCommandGroup(
std::move(swerveControllerCommand), std::move(swerveControllerCommand),
frc2::InstantCommand(
[this] {
m_drive.Drive(units::meters_per_second_t(0),
units::meters_per_second_t(0),
units::radians_per_second_t(0), false);
},
{}));
}

Posting the stack traceback would probably help, if you can.

You cannot capture this in a pointer from a command that is part of a command group, because the pointer is left dangling after the command is std::move'd into the group.

1 Like

HI Oblarg,
That code is straight out of the sample code from WPILib. And my coding level is more at the level of look at sample code, try and work out what it is doing and than patch it together to make it do what you need it to do. I know this is probably not the best way to teach coding to FRC students, but trying to get a high school aged student (that usually starts with zero text based coding knowledge) up to point of free writing c++ code using the command based framework to the level required to be competitive in the FRC world is no easy task. Up until now this method of teaching has allowed the teams that I’ve helped to create pretty good robots. Unfortunately this method of coding instruction falls down in a big heap when the sample code doesn’t work.

In regards to the floating pointer issue, I thought this only happened if a pointer was created to an object that went out of scope. Doesn’t the instance the robotcontainer object live for the entire life of the robot program, and doesn’t that mean that the reference to [this] should always point to something that exists?

Or is it the std::move that is causing the code to crash? I don’t fully under std::move yet.

For the purpose of testing should I be able to ditch the SequentialCommandGroup and just return the SwerveControllerCommand object? (or should it be a pointer to this object). I know this will potentially leave the robot in motion at the end of the path, but it would be nice to see it follow the path. (Baby Steps).

Does anyone know where there is any documentation on the SwerveControllerCommand other than what is returned by intellisense?

Can anyone put forward a re-write of this block of code so that it will work?

Link to WPILib Sample Code on Github

Cheers and thanks again to those people that read these posts and help out the FRC community.

It is a bug with the current library; SwerveControllerCommand and MecanumControllerCommand both cause crashes at runtime due to unsafe capture of the command’s this pointer.

Hi Nuttle,
Sorry, that’s a bit outside my skill set. I tried, but couldn’t work out how to capture the stack at the point of failure in the code. The code was crashing after the command was scheduled. (I worked this out from lots of cout statments.) The other issue I had was the code wasn’t stopping at this point it would just restart automatically. So I thought I’d have to set breakpoints inside WPILib code and that just got too hard.

1 Like

I was able to get it to work if I didn’t return a command group object but instead returned a new swervecontrollercommand object instead.

A github commit has been merged just recently since my post started claiming to fix the problem… I’ll try the updated github code again tomorrow and report back.

Thanks again to all the people that read these posts and share their expertise.

Update.
Good news, the update to the wpilib code in SwerveControllerCommand.inc looks like it has fixed the problem with dangling pointer in regards to crashing the code when running autonomous.

A new issue now.
Paths that start at a pose of 0 degrees and end at pose of 0 degrees work great. Even had the S-curve from the sample code running at 5 m/s and it would finish within centimetres of correct position at insanely fast speeds.
But If I change the path to end at an angle other than zero (for example a path from x=0,y=0,theta=0 through x=0.707, y=-0.707, ending at x=1.0,y=-1.0,theta=-90, basically arcing through a quarter of a circle to the right at a radius of 1 metre), the output from the HomolonicDriveController has the swerve modules heading in all directions and fighting against themselves and scrubbing all over the place.
NB: This robot drives fine under fieldcentric joystick control, so I don’t think its a problem with Kinematics or gyro, etc. Has anyone else got the swerve drive sample code to follow paths that end at a different angle than 0 degrees?

Send as much telemetry to the dashboard as you can, and try to figure out where in your control stack the setpoints stop making sense.

1 Like

Update for everyone reading this. The initial problem has been fixed in a github update and I believe the secondary issue is addressed here. I believe I found the issue with the SwerveControllerCommand sample code not following auto paths with non zero headings. I’ve reported the bug Issue 3814 and attempted to provide a pull request to fix the problem in the code Pull Request 3815. There is every chance I haven’t done this the correct way (as I’m very new to github) so I’ll give details of what fixed the problem for me here.
The code in DriveSubsystem.cpp inside SetModuleStates method currently reads

m_rearLeft.SetDesiredState(desiredStates[1]);
m_frontRight.SetDesiredState(desiredStates[2]);
link to github code

Should read
m_frontRight.SetDesiredState(desiredStates[1]);
m_rearLeft.SetDesiredState(desiredStates[2]);

Check out the github Issue for more details of why this change is required.

This topic was automatically closed 365 days after the last reply. New replies are no longer allowed.