From 3928ed5647642740873890a4fe528ced12fb5e72 Mon Sep 17 00:00:00 2001 From: Peter Johnson Date: Sat, 20 Jan 2024 21:10:02 -0800 Subject: [PATCH] [sim] Sim GUI DS: Add "Disconnected" state and start in it (#6218) The default state for the DS in the simulated HAL is changed to disconnected. The FMS view is now only editable in DS disconnected state. This enables more robot and field-like testing of robot code, as the alliance color and other parameters start in invalid states and are only set when the DS connects. --- glass/src/lib/native/cpp/other/FMS.cpp | 15 ++- .../src/lib/native/include/glass/other/FMS.h | 3 +- .../native/cpp/StandardNetworkTables.cpp | 2 +- hal/src/main/native/sim/DriverStation.cpp | 1 + .../native/sim/mockdata/DriverStationData.cpp | 2 +- .../sim/mockdata/DriverStationDataInternal.h | 2 +- .../src/main/native/cpp/DriverStationGui.cpp | 121 +++++++++++------- .../cpp/simulation/DriverStationSimTest.cpp | 6 +- .../simulation/DriverStationSimTest.java | 6 +- 9 files changed, 105 insertions(+), 53 deletions(-) diff --git a/glass/src/lib/native/cpp/other/FMS.cpp b/glass/src/lib/native/cpp/other/FMS.cpp index 67c3f8c904..d49304e039 100644 --- a/glass/src/lib/native/cpp/other/FMS.cpp +++ b/glass/src/lib/native/cpp/other/FMS.cpp @@ -14,7 +14,7 @@ using namespace glass; static const char* stations[] = {"Invalid", "Red 1", "Red 2", "Red 3", "Blue 1", "Blue 2", "Blue 3"}; -void glass::DisplayFMS(FMSModel* model) { +void glass::DisplayFMS(FMSModel* model, bool editableDsAttached) { if (!model->Exists() || model->IsReadOnly()) { return DisplayFMSReadOnly(model); } @@ -31,10 +31,17 @@ void glass::DisplayFMS(FMSModel* model) { // DS Attached if (auto data = model->GetDsAttachedData()) { bool val = data->GetValue(); - if (ImGui::Checkbox("DS Attached", &val)) { - model->SetDsAttached(val); + if (editableDsAttached) { + if (ImGui::Checkbox("DS Attached", &val)) { + model->SetDsAttached(val); + } + data->EmitDrag(); + } else { + ImGui::Selectable("DS Attached: "); + data->EmitDrag(); + ImGui::SameLine(); + ImGui::TextUnformatted(val ? "Yes" : "No"); } - data->EmitDrag(); } // Alliance Station diff --git a/glass/src/lib/native/include/glass/other/FMS.h b/glass/src/lib/native/include/glass/other/FMS.h index 5970e93e22..f039c15a70 100644 --- a/glass/src/lib/native/include/glass/other/FMS.h +++ b/glass/src/lib/native/include/glass/other/FMS.h @@ -46,8 +46,9 @@ class FMSModel : public Model { * * @param matchTimeEnabled If not null, a checkbox is displayed for * "enable match time" linked to this value + * @param editableDsAttached If true, DS attached should be editable */ -void DisplayFMS(FMSModel* model); +void DisplayFMS(FMSModel* model, bool editableDsAttached); void DisplayFMSReadOnly(FMSModel* model); } // namespace glass diff --git a/glass/src/libnt/native/cpp/StandardNetworkTables.cpp b/glass/src/libnt/native/cpp/StandardNetworkTables.cpp index 9031873ad8..4cc2121269 100644 --- a/glass/src/libnt/native/cpp/StandardNetworkTables.cpp +++ b/glass/src/libnt/native/cpp/StandardNetworkTables.cpp @@ -63,7 +63,7 @@ void glass::AddStandardNetworkTablesViews(NetworkTablesProvider& provider) { [](Window* win, Model* model, const char*) { win->SetFlags(ImGuiWindowFlags_AlwaysAutoResize); return MakeFunctionView( - [=] { DisplayFMS(static_cast(model)); }); + [=] { DisplayFMS(static_cast(model), true); }); }); provider.Register( NTDigitalInputModel::kType, diff --git a/hal/src/main/native/sim/DriverStation.cpp b/hal/src/main/native/sim/DriverStation.cpp index 67b2eb728a..b245eed59d 100644 --- a/hal/src/main/native/sim/DriverStation.cpp +++ b/hal/src/main/native/sim/DriverStation.cpp @@ -382,6 +382,7 @@ void NewDriverStationData() { } lastGiven = given; + SimDriverStationData->dsAttached = true; driverStation->newDataEvents.Wakeup(); SimDriverStationData->CallNewDataCallbacks(); } diff --git a/hal/src/main/native/sim/mockdata/DriverStationData.cpp b/hal/src/main/native/sim/mockdata/DriverStationData.cpp index a0cae1c9a2..d124b21308 100644 --- a/hal/src/main/native/sim/mockdata/DriverStationData.cpp +++ b/hal/src/main/native/sim/mockdata/DriverStationData.cpp @@ -28,7 +28,7 @@ void DriverStationData::ResetData() { test.Reset(false); eStop.Reset(false); fmsAttached.Reset(false); - dsAttached.Reset(true); + dsAttached.Reset(false); allianceStationId.Reset(static_cast(0)); matchTime.Reset(-1.0); diff --git a/hal/src/main/native/sim/mockdata/DriverStationDataInternal.h b/hal/src/main/native/sim/mockdata/DriverStationDataInternal.h index 763b465f32..008329fde8 100644 --- a/hal/src/main/native/sim/mockdata/DriverStationDataInternal.h +++ b/hal/src/main/native/sim/mockdata/DriverStationDataInternal.h @@ -122,7 +122,7 @@ class DriverStationData { SimDataValue eStop{false}; SimDataValue fmsAttached{ false}; - SimDataValue dsAttached{true}; + SimDataValue dsAttached{false}; SimDataValue allianceStationId{static_cast(0)}; diff --git a/simulation/halsim_gui/src/main/native/cpp/DriverStationGui.cpp b/simulation/halsim_gui/src/main/native/cpp/DriverStationGui.cpp index 09192f39ff..42df53efea 100644 --- a/simulation/halsim_gui/src/main/native/cpp/DriverStationGui.cpp +++ b/simulation/halsim_gui/src/main/native/cpp/DriverStationGui.cpp @@ -227,37 +227,25 @@ class FMSSimModel : public glass::FMSModel { glass::DataSource* GetAutonomousData() override { return &m_autonomous; } std::string_view GetGameSpecificMessage( wpi::SmallVectorImpl& buf) override { - HAL_MatchInfo info; - HALSIM_GetMatchInfo(&info); - buf.clear(); - buf.append(info.gameSpecificMessage, - info.gameSpecificMessage + info.gameSpecificMessageSize); - return std::string_view(buf.begin(), buf.size()); + return m_gameMessage; } - void SetFmsAttached(bool val) override { - HALSIM_SetDriverStationFmsAttached(val); - } - void SetDsAttached(bool val) override { - HALSIM_SetDriverStationDsAttached(val); - } + void SetFmsAttached(bool val) override { m_fmsAttached.SetValue(val); } + void SetDsAttached(bool val) override { m_dsAttached.SetValue(val); } void SetAllianceStationId(int val) override { - HALSIM_SetDriverStationAllianceStationId( - static_cast(val)); - } - void SetMatchTime(double val) override { - HALSIM_SetDriverStationMatchTime(val); - } - void SetEStop(bool val) override { HALSIM_SetDriverStationEStop(val); } - void SetEnabled(bool val) override { HALSIM_SetDriverStationEnabled(val); } - void SetTest(bool val) override { HALSIM_SetDriverStationTest(val); } - void SetAutonomous(bool val) override { - HALSIM_SetDriverStationAutonomous(val); + m_allianceStationId.SetValue(val); } + void SetMatchTime(double val) override { m_matchTime.SetValue(val); } + void SetEStop(bool val) override { m_estop.SetValue(val); } + void SetEnabled(bool val) override { m_enabled.SetValue(val); } + void SetTest(bool val) override { m_test.SetValue(val); } + void SetAutonomous(bool val) override { m_autonomous.SetValue(val); } void SetGameSpecificMessage(std::string_view val) override { - HALSIM_SetGameSpecificMessage(val.data(), val.size()); + m_gameMessage = val; } + void UpdateHAL(); + void Update() override; bool Exists() override { return true; } @@ -274,6 +262,7 @@ class FMSSimModel : public glass::FMSModel { glass::DataSource m_test{"FMS:TestMode"}; glass::DataSource m_autonomous{"FMS:AutonomousMode"}; double m_startMatchTime = -1.0; + std::string m_gameMessage; }; } // namespace @@ -1019,6 +1008,21 @@ void RobotJoystick::GetHAL(int i) { HALSIM_GetJoystickPOVs(i, &data.povs); } +static void DriverStationConnect(bool enabled, bool autonomous, bool test) { + if (!HALSIM_GetDriverStationDsAttached()) { + // initialize FMS bits too + gFMSModel->SetDsAttached(true); + gFMSModel->SetEnabled(enabled); + gFMSModel->SetAutonomous(autonomous); + gFMSModel->SetTest(test); + gFMSModel->UpdateHAL(); + } else { + HALSIM_SetDriverStationEnabled(enabled); + HALSIM_SetDriverStationAutonomous(autonomous); + HALSIM_SetDriverStationTest(test); + } +} + static void DriverStationExecute() { // update sources for (int i = 0; i < HAL_kMaxJoysticks; ++i) { @@ -1072,6 +1076,7 @@ static void DriverStationExecute() { joy.Update(); } + bool isAttached = HALSIM_GetDriverStationDsAttached(); bool isEnabled = HALSIM_GetDriverStationEnabled(); bool isAuto = HALSIM_GetDriverStationAutonomous(); bool isTest = HALSIM_GetDriverStationTest(); @@ -1100,38 +1105,45 @@ static void DriverStationExecute() { ImGui::SetNextWindowPos(ImVec2{5, 20}, ImGuiCond_FirstUseEver); ImGui::Begin("Robot State", nullptr, ImGuiWindowFlags_AlwaysAutoResize); - if (ImGui::Selectable("Disabled", !isEnabled) || disableHotkey) { + if (ImGui::Selectable("Disconnected", !isAttached)) { HALSIM_SetDriverStationEnabled(false); + HALSIM_SetDriverStationDsAttached(false); + isAttached = false; + gFMSModel->Update(); } - if (ImGui::Selectable("Autonomous", isEnabled && isAuto && !isTest)) { - HALSIM_SetDriverStationAutonomous(true); - HALSIM_SetDriverStationTest(false); - HALSIM_SetDriverStationEnabled(true); + if (ImGui::Selectable("Disabled", isAttached && !isEnabled) || + disableHotkey) { + DriverStationConnect(false, false, false); } - if (ImGui::Selectable("Teleoperated", isEnabled && !isAuto && !isTest) || + if (ImGui::Selectable("Autonomous", + isAttached && isEnabled && isAuto && !isTest)) { + DriverStationConnect(true, true, false); + } + if (ImGui::Selectable("Teleoperated", + isAttached && isEnabled && !isAuto && !isTest) || enableHotkey) { - HALSIM_SetDriverStationAutonomous(false); - HALSIM_SetDriverStationTest(false); - HALSIM_SetDriverStationEnabled(true); + DriverStationConnect(true, false, false); } if (ImGui::Selectable("Test", isEnabled && isTest)) { - HALSIM_SetDriverStationAutonomous(false); - HALSIM_SetDriverStationTest(true); - HALSIM_SetDriverStationEnabled(true); + DriverStationConnect(true, false, true); } ImGui::End(); } // Update HAL - for (int i = 0, end = gRobotJoysticks.size(); - i < end && i < HAL_kMaxJoysticks; ++i) { - gRobotJoysticks[i].SetHAL(i); + if (isAttached) { + for (int i = 0, end = gRobotJoysticks.size(); + i < end && i < HAL_kMaxJoysticks; ++i) { + gRobotJoysticks[i].SetHAL(i); + } } // Send new data every 20 ms (may be slower depending on GUI refresh rate) static double lastNewDataTime = 0.0; - if ((curTime - lastNewDataTime) > 0.02 && !HALSIM_IsTimingPaused()) { + if ((curTime - lastNewDataTime) > 0.02 && !HALSIM_IsTimingPaused() && + isAttached) { lastNewDataTime = curTime; + gFMSModel->Update(); HALSIM_NotifyDriverStationNewData(); } } @@ -1144,6 +1156,20 @@ FMSSimModel::FMSSimModel() { m_test.SetDigital(true); m_autonomous.SetDigital(true); m_matchTime.SetValue(-1.0); + m_allianceStationId.SetValue(HAL_AllianceStationID_kRed1); +} + +void FMSSimModel::UpdateHAL() { + HALSIM_SetDriverStationFmsAttached(m_fmsAttached.GetValue()); + HALSIM_SetDriverStationAllianceStationId( + static_cast(m_allianceStationId.GetValue())); + HALSIM_SetDriverStationEStop(m_estop.GetValue()); + HALSIM_SetDriverStationEnabled(m_enabled.GetValue()); + HALSIM_SetDriverStationTest(m_test.GetValue()); + HALSIM_SetDriverStationAutonomous(m_autonomous.GetValue()); + HALSIM_SetDriverStationMatchTime(m_matchTime.GetValue()); + HALSIM_SetGameSpecificMessage(m_gameMessage.data(), m_gameMessage.size()); + HALSIM_SetDriverStationDsAttached(m_dsAttached.GetValue()); } void FMSSimModel::Update() { @@ -1176,6 +1202,11 @@ void FMSSimModel::Update() { m_startMatchTime = -1.0; } m_matchTime.SetValue(matchTime); + + HAL_MatchInfo info; + HALSIM_GetMatchInfo(&info); + m_gameMessage.assign(info.gameSpecificMessage, + info.gameSpecificMessage + info.gameSpecificMessageSize); } bool FMSSimModel::IsReadOnly() { @@ -1387,7 +1418,6 @@ void DriverStationGui::GlobalInit() { gFMSModel = std::make_unique(); wpi::gui::AddEarlyExecute(DriverStationExecute); - wpi::gui::AddEarlyExecute([] { gFMSModel->Update(); }); storageRoot.SetCustomApply([&storageRoot] { gpDisableDS = &storageRoot.GetBool("disable", false); @@ -1433,8 +1463,13 @@ void DriverStationGui::GlobalInit() { win->SetDefaultSize(300, 560); } } - if (auto win = - dsManager->AddWindow("FMS", [] { DisplayFMS(gFMSModel.get()); })) { + if (auto win = dsManager->AddWindow("FMS", [] { + if (HALSIM_GetDriverStationDsAttached()) { + DisplayFMSReadOnly(gFMSModel.get()); + } else { + DisplayFMS(gFMSModel.get(), false); + } + })) { win->DisableRenamePopup(); win->SetFlags(ImGuiWindowFlags_AlwaysAutoResize); win->SetDefaultPos(5, 540); diff --git a/wpilibc/src/test/native/cpp/simulation/DriverStationSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/DriverStationSimTest.cpp index 2513252893..8a5941ad37 100644 --- a/wpilibc/src/test/native/cpp/simulation/DriverStationSimTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/DriverStationSimTest.cpp @@ -104,15 +104,19 @@ TEST(DriverStationTest, FmsAttached) { TEST(DriverStationTest, DsAttached) { HAL_Initialize(500, 0); DriverStationSim::ResetData(); + DriverStation::RefreshData(); + EXPECT_FALSE(DriverStationSim::GetDsAttached()); + EXPECT_FALSE(DriverStation::IsDSAttached()); DriverStationSim::NotifyNewData(); + EXPECT_TRUE(DriverStationSim::GetDsAttached()); EXPECT_TRUE(DriverStation::IsDSAttached()); BooleanCallback callback; auto cb = DriverStationSim::RegisterDsAttachedCallback(callback.GetCallback(), false); DriverStationSim::SetDsAttached(false); - DriverStationSim::NotifyNewData(); + DriverStation::RefreshData(); EXPECT_FALSE(DriverStationSim::GetDsAttached()); EXPECT_FALSE(DriverStation::IsDSAttached()); EXPECT_TRUE(callback.WasTriggered()); diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DriverStationSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DriverStationSimTest.java index 9a324d5153..54699e0036 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DriverStationSimTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/DriverStationSimTest.java @@ -108,14 +108,18 @@ class DriverStationSimTest { void testDsAttached() { HAL.initialize(500, 0); DriverStationSim.resetData(); + DriverStation.refreshData(); + assertFalse(DriverStationSim.getDsAttached()); + assertFalse(DriverStation.isDSAttached()); DriverStationSim.notifyNewData(); + assertTrue(DriverStationSim.getDsAttached()); assertTrue(DriverStation.isDSAttached()); BooleanCallback callback = new BooleanCallback(); try (CallbackStore cb = DriverStationSim.registerDsAttachedCallback(callback, false)) { DriverStationSim.setDsAttached(false); - DriverStationSim.notifyNewData(); + DriverStation.refreshData(); assertFalse(DriverStationSim.getDsAttached()); assertFalse(DriverStation.isDSAttached()); assertTrue(callback.wasTriggered());