Applying a SwerveRequest introduces a significant amount of GC spikes

[mjansen4857]

We started noticing significant amounts of GC spikes (or possibly just general lockups) in our loop times when running our teleop drive command, and tracked down the cause to applying a swerve request, specifically ApplyChassisSpeeds. Simply commenting out the drivetrain.setControl() call without changing any other code resolves the issue with our loop timings.

When running our code while enabled without setting the drive request, we see loop times hovering between 10-15ms, with only occasional noticeable GC spikes every second or so. Uncommenting the setControl call sees our loop times hovering at ~20ms with multiple 50ms+ spikes per second.

Not really sure what would be causing it to perform like this.

[bhall-ctre]

Just to confirm a couple things:

1. This is with the new 2025 swerve API with the native implementation, NOT the legacy API. In which case ApplyChassisSpeeds should be marked deprecated in favor of ApplyRobotSpeeds (they do the same thing).
   • GC spikes are expected with the 2024 API and LegacySwerveDrivetrain, which is one of the reasons why we switched to a native implementation in 2025.
2. You're reusing an already-allocated ApplyRobotSpeeds object between calls to setControl, not allocating a new instance every loop.

The 2025 ApplyRobotSpeeds itself does no allocations, as can be seen in the implementation here. If you have a telemetry function registered or call getState() periodically, something that could impact things is changes in the SwerveDriveState, in which case just pushing the robot around by hand should see GC spikes.

[mjansen4857]

My bad, I still confuse ApplyChassisSpeeds and ApplyRobotSpeeds. Yes, we are using the 2025 swerve API and ApplyRobotSpeeds. We are not allocating a new instance every loop. The loop timing issues are seen even if the robot state doesn’t change, just setting control to 0 speeds and letting it sit there sees a jump in timing.

[bhall-ctre]

The loop timing issues are seen even if the robot state doesn’t change, just setting control to 0 speeds and letting it sit there sees a jump in timing.

That's interesting. One last question, are you using the module force feedforward parameters in this context? If so, does the issue still reproduce if you don't?

[mjansen4857]

We are not. The only thing changed about the ApplyRobotSpeeds request besides setting speeds to zero is changing the drive request type to velocity and the steer request type to position.

[bhall-ctre]

As a last few tests:

1. Does the issue reproduce if you call setControl with a SwerveRequest.Idle?
2. Does the issue reproduce if you call addVisionMeasurement/setVisionMeasurementStdDevs periodically (with a constant Pose2d and double[]) instead of setControl?
3. Does the issue reproduce if you call seedFieldCentric periodically instead of setControl?

I expect the answers to 1 and 3 to be the same, and the issue should reproduce with 2 if it reproduces with ApplyRobotSpeeds.

[mjansen4857]

I can test 1 & 3 later tonight, but addVisionMeasurement was called periodically both with and without the setControl call, and the loop timing was only an issue with the setControl call. Calling addVisionMeasurement without setControl had fine loop timing. This wasn't a constant pose, but I can test that as well.

[bhall-ctre]

For the addVisionMeasurement test, I'm assuming you're not using the stddevs parameter? If so, retest with the optional standard deviations parameter. I expect the issue will reproduce if you include stddevs instead of just the pose, and call it every 20 ms. You can just use a constant array of size 3.

[mjansen4857]

Yeah we include stdDevs as well, they are calculated every loop based on tag distance.

[bhall-ctre]

Yeah we include stdDevs as well, they are calculated every loop based on tag distance.

In that case, I find it kind of surprising that you didn't see the issue with setting vision standard deviations periodically, that and ApplyRobotSpeeds have identical impacts on GC. So if you find that the issue does NOT reproduce with SwerveRequest.Idle, then one more test I'd like to add on is to try using RobotCentric instead of ApplyRobotSpeeds.

[mjansen4857]

Ok, I'll test that tonight.

One thing I forgot to mention is that the timing issue reproduces even without calling setControl periodically. Just calling it once is enough. For example, moving our setControl call from our command's execute to initialize still causes the timing issues.

[mjansen4857]

The loop timing issue is not reproducible with the Idle or RobotCentric requests.

[bhall-ctre]

We've done some testing with this on our own swerve robot, and so far we can't reproduce the issue with either of the following two test cases using our latest release (25.2.1).

private final SwerveRequest.RobotCentric m_robotCentric = new SwerveRequest.RobotCentric();
private final SwerveRequest.ApplyRobotSpeeds m_applySpeeds = new SwerveRequest.ApplyRobotSpeeds();

private final ChassisSpeeds m_speeds = new ChassisSpeeds();

public void configureBindings() {
    // Enable one of the cases below

    // case 1: robot centric
    drivetrain.setDefaultCommand(drivetrain.applyRequest(() ->
        m_robotCentric.withVelocityX(-joystick.getY() * MaxSpeed)
    ));
    // case 2: apply robot speeds
    drivetrain.setDefaultCommand(drivetrain.applyRequest(() -> {
        m_speeds.vxMetersPerSecond = -joystick.getY() * MaxSpeed;
        return m_applySpeeds.withSpeeds(m_speeds);
    }));
}

We also analyzed our implementation, and based on your test results listed below, there are several results that, if they're all correct, go against setControl itself being the culprit:

• The issue was encountered with our built-in SwerveRequest.ApplyRobotSpeeds, which is a NativeSwerveRequest.
• The issue reproduces with ApplyRobotSpeeds, but not RobotCentric. The only difference between these two that could make an impact on performance is the wheel force feedforward arrays.
• The issue does not reproduce from periodically setting vision measurement standard deviations, which also uses arrays.
• The issue reproduces when setControl is called only once, in which case the feedforward arrays no longer have any impact on performance, so performance matches RobotCentric.

Based on these test results, the only way that setControl could be influencing things should be if one of the following is true:

1. Adding a few additional setVisionMeasurementStdDevs() calls that run every robotPeriodic() eventually reproduces the issue, and the issue does not reproduce if setControl is called only once.
2. Given your ApplyRobotSpeeds request req, (req instanceof NativeSwerveRequest) == false.
3. There's an allocation in your ApplyRobotSpeeds test code that wasn't in your other test code (such as new ChassisSpeeds(...)).

So far, we've been focusing on the first case, as that's the most likely cause if the issue is on our end, but we haven't seen anything substantial. Is there anything else unique to your setup that could be impacting things?

[CoryNessCTR]

One more ask, when you're reporting the increase in loop timings, what exactly are you reporting? The time between periodic calls? Time it takes to perform a periodic call? Are you doing any memory profiling to confirm the spikes are GC? We've been primarily focused on GC-pressure related things, but if the only symptom is timing related and unrelated to memory it might be a thread lock somewhere instead. If your setControl is a nonnative request and doing an expensive calculation that could also explain a lot.

Are you able to take a profile of the RIO with the issue reproducing? Or can you reproduce it in Sim?

[mjansen4857]

No there's not really anything unique, the only thing that was changed when we tested ApplyRobotSpeeds vs RobotCentric was which request was being given to the setControl call. Both were still allocated and all the same code was being run.

For our loop timings, we are graphing the total robot loop time in AdvantageScope as logged by AdvantageKit (/AdvantageKit/RealOutputs/LoggedRobot/FullCycleMS entry). By increase in loop timings I mean the average of that graph increases by a few milliseconds and the amount of spikes causing a loop overrun significantly increases. Haven't done any memory profiling, but I can try that. I'm more inclined to believe it is some sort of thread lock over GC now anyways, since ApplyRobotSpeeds shouldn't be allocating a lot of stuff, and we have code elsewhere that allocates a whole lot more stuff but doesn't have this impact (the issue we see with ApplyRobotSpeeds still reproduces even with all this other allocation heavy code commented out).

It's not reproducible in sim as far as I can tell, but that could also be due to our loop timing being so low and stable running in sim vs a RIO that I just can't tell.