[wpilib] LinearOpMode: wait for mode change to return from OpModeRun (#8477)

We need to wait, or otherwise OpModeRobot will immediately reinstantiate
and re-run the opmode, which is generally undesirable (e.g. for
autonomous).

Fixes #8475.

wpilibc/src/main/native/cpp/opmode/LinearOpMode.cpp

@@ -4,6 +4,7 @@
 
 #include "wpi/opmode/LinearOpMode.hpp"
 
+#include "wpi/driverstation/DriverStation.hpp"
 #include "wpi/hal/DriverStation.h"
 #include "wpi/internal/DriverStationModeThread.hpp"
 
@@ -14,6 +15,14 @@ void LinearOpMode::OpModeRun(int64_t opModeId) {
   word.SetOpModeId(opModeId);
   internal::DriverStationModeThread bgThread{word};
   Run();
+
+  // Wait for opmode to be stopped or disabled, otherwise OpModeRobot will
+  // recreate and re-run the opmode immediately.
+  while (IsRunning() && DriverStation::IsEnabled() &&
+         DriverStation::GetOpModeId() == opModeId) {
+    using namespace std::chrono_literals;
+    std::this_thread::sleep_for(20ms);
+  }
 }
 
 void LinearOpMode::OpModeStop() {

wpilibj/src/main/java/org/wpilib/opmode/LinearOpMode.java

@@ -67,6 +67,12 @@ public abstract class LinearOpMode implements OpMode {
 
     try (DriverStationModeThread bgThread = new DriverStationModeThread(word)) {
       run();
+
+      // Wait for opmode to be stopped or disabled, otherwise OpModeRobot will recreate and re-run
+      // the opmode immediately.
+      while (isRunning() && DriverStation.isEnabled() && DriverStation.getOpModeId() == opModeId) {
+        Thread.sleep(20);
+      }
     }
   }