allwpilib icon indicating copy to clipboard operation
allwpilib copied to clipboard

`SimHooks.stepTiming()` hangs when called from a callback registered with `SimDeviceSim.registerValueChangedCallback()` if another `SimDeviceSim` has been created

Open brettle opened this issue 3 months ago • 2 comments

Describe the bug SimHooks.stepTiming() hangs when called from a callback registered with SimDeviceSim.registerValueChangedCallback() if another SimDeviceSim has been created.

To Reproduce Run the following test (which uses an ADXRS450_GyroSim as an example of a SimDeviceSim):

package frc.robot;

import java.util.concurrent.CompletableFuture;
import java.util.concurrent.ExecutionException;
import java.util.concurrent.TimeoutException;

import edu.wpi.first.hal.HAL;
import edu.wpi.first.hal.SimDevice;
import edu.wpi.first.hal.SimDevice.Direction;
import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.simulation.ADXRS450_GyroSim;
import edu.wpi.first.wpilibj.simulation.SimDeviceSim;
import edu.wpi.first.wpilibj.simulation.SimHooks;

import org.junit.jupiter.api.AfterEach;
import org.junit.jupiter.api.BeforeEach;
import org.junit.jupiter.api.Test;
import org.junit.jupiter.api.Timeout;
import static org.junit.jupiter.api.Timeout.ThreadMode.SEPARATE_THREAD;

public class RobotTest {
    @BeforeEach
    void setup() {
        assert HAL.initialize(500, 0);
    }

    @AfterEach
    void shutdown() throws Exception {
        HAL.shutdown();
    }

    @Test
    @Timeout(value = 15, threadMode = SEPARATE_THREAD) // Kill the test after 15 seconds
    void doesNotHang() throws InterruptedException, TimeoutException, ExecutionException {
        // Create a simulated device that can be used to step the robot's timing.
        SimDevice device = SimDevice.create("TimeDevice");
        device.createDouble("simTime", Direction.kInput, 0.0);
        SimDeviceSim deviceSim = new SimDeviceSim("TimeDevice");
        deviceSim.registerValueChangedCallback(deviceSim.getDouble("simTime"),
                (name, handle, direction, value) -> {
                    double secsBehind = value.getDouble() - Timer.getFPGATimestamp();
                    if (secsBehind > 0) {
                        System.out.println("Calling stepTiming() should not hang");
                        SimHooks.stepTiming(secsBehind);
                        System.out.println("stepTiming() returned"); // Never printed
                    }
                }, false);

        try (TimedRobot robot = new TimedRobot() {
            @SuppressWarnings("resource")
            ADXRS450_Gyro gyro = new ADXRS450_Gyro();

            @Override
            public void robotInit() {
            }

            @Override
            public void robotPeriodic() {
                if (Timer.getFPGATimestamp() > 5.0) {
                    endCompetition();
                }
            }

            ADXRS450_GyroSim gyroSim = null;

            @Override
            public void simulationInit() {
                // Adding the following line causes SimHook.stepTiming() to block in the simTime
                // callback.
                gyroSim = new ADXRS450_GyroSim(gyro);
            }

            @Override
            public void simulationPeriodic() {
                if (gyroSim != null) {
                    // This isn't necessary to produce the problem, but it should also work.
                    // Using the current timestamp here just to ensure we always get a new number.
                    gyroSim.setAngle(Timer.getFPGATimestamp());
                }
            }

            @Override
            public void close() {
                gyro.close();
                super.close();
            }
        }) {
            // Start the robot on separate thread
            CompletableFuture<Void> competition = CompletableFuture.runAsync(() -> {
                robot.startCompetition();
            });

            // Wait a couple secs to be sure it is running and then pause it
            Thread.sleep(2000);
            SimHooks.pauseTiming();

            // Emulate an external simulator telling it to step the robot time
            // forward to 10 seconds. This doesn't need to be async to produce
            // the problem, but more accurately represents our use case.
            CompletableFuture.runAsync(() -> {
                deviceSim.getDouble("simTime").set(10.0);
            }).join();

            // Wait for the robot to finish (which should occur as soon as the robot time is
            // > 5.0 seconds).
            competition.join();
        }
    }
}

Expected behavior The test should pass (after only 2-3 seconds). Instead, it hangs in the call to SimHooks.stepTiming() (until the test times out after 15 secs).

Desktop (please complete the following information):

  • OS: Linux, Fedora Linux 38 (Workstation Edition)
  • Project Information:
Project Version: 2024.3.1
VS Code Version: 1.85.1
WPILib Extension Version: 2024.3.1
C++ Extension Version: 1.19.1
Java Extension Version: 1.30.0
Java Debug Extension Version: 0.57.2024041008
Java Dependencies Extension Version 0.23.6
Java Version: 17
Java Location: /home/brettle/wpilib/2024/jdk
Vendor Libraries:
    WPILib-New-Commands (1.0.0)

Additional Context The test above uses a ADXRS450_GyroSim but the problem also occurs with a new SimDeviceSim associated with a new SimDevice.

brettle avatar May 20 '24 23:05 brettle