Post-EKF robot localizer cleanup

docs/fsm-diagrams.md

@@ -11,16 +11,24 @@ direction LR
 Halt --> Stop : [gameStateStopped]\n<i>setupStopPlay</i>
 Halt --> Playing : [gameStatePlaying]\n<i>setupOffensePlay</i>
 Halt --> SetPlay : [gameStateSetupRestart]\n<i>setupSetPlay</i>
+Halt --> Halt : <i>setupOverridePlay</i>
+Halt --> Halt : <i>resetPlaySelection</i>
 Stop --> Halt : [gameStateHalted]\n<i>setupHaltPlay</i>
 Stop --> Playing : [gameStatePlaying]\n<i>setupOffensePlay</i>
 Stop --> SetPlay : [gameStateSetupRestart]\n<i>setupSetPlay</i>
+Stop --> Stop : <i>setupOverridePlay</i>
+Stop --> Halt : <i>resetPlaySelection</i>
 Playing --> Halt : [gameStateHalted]\n<i>setupHaltPlay</i>
 Playing --> Stop : [gameStateStopped]\n<i>setupStopPlay</i>
 Playing --> SetPlay : [gameStateSetupRestart]\n<i>setupSetPlay</i>
+Playing --> Playing : <i>setupOverridePlay</i>
+Playing --> Halt : <i>resetPlaySelection</i>
 SetPlay --> Halt : [gameStateHalted]\n<i>resetSetPlay, setupHaltPlay</i>
 SetPlay --> Stop : [gameStateStopped]\n<i>resetSetPlay, setupStopPlay</i>
 SetPlay --> Playing : [gameStatePlaying]\n<i>resetSetPlay, setupOffensePlay</i>
 SetPlay --> SetPlay : [gameStateSetupRestart]\n<i>setupSetPlay</i>
+SetPlay --> SetPlay : <i>setupOverridePlay</i>
+SetPlay --> Halt : <i>resetPlaySelection</i>
 Terminate:::terminate --> Terminate:::terminate
 
 ```

src/shared/robot_constants.cpp

@@ -55,8 +55,7 @@ RobotConstants createRobotConstants()
         .kalman_process_noise_variance_rad_per_s_4      = 1.0f,
         .kalman_vision_noise_variance_rad_2             = 0.0001f,
         .kalman_motor_sensor_noise_variance_rad_per_s_2 = 0.5f,
-        .kalman_motor_sensor_noise_variance_m_per_s_2 = 0.05f
-	};
+        .kalman_motor_sensor_noise_variance_m_per_s_2   = 0.05f};
 }
 #elif CHECK_VERSION(2021)
 constexpr RobotConstants createRobotConstants()

src/software/ai/ai.cpp

@@ -2,16 +2,13 @@
 
 #include <Tracy.hpp>
 
-#include "software/ai/hl/stp/play/halt_play/halt_play.h"
 #include "software/ai/hl/stp/play/play_factory.h"
 #include "software/tracy/tracy_constants.h"
 
 
 Ai::Ai(std::shared_ptr<const TbotsProto::AiConfig> ai_config_ptr)
     : ai_config_ptr(ai_config_ptr),
       fsm(std::make_unique<FSM<PlaySelectionFSM>>(PlaySelectionFSM{ai_config_ptr})),
-      override_play(nullptr),
-      current_play(std::make_unique<HaltPlay>(ai_config_ptr)),
       ai_config_changed(false)
 {
     auto current_override = ai_config_ptr->ai_control_config().override_ai_play();
@@ -26,12 +23,11 @@ Ai::Ai(std::shared_ptr<const TbotsProto::AiConfig> ai_config_ptr)
 
 void Ai::overridePlay(std::unique_ptr<Play> play)
 {
-    override_play = std::move(play);
+    fsm->process_event(PlaySelectionFSM::Override(std::move(play)));
 }
 
 void Ai::overridePlayFromProto(TbotsProto::Play play_proto)
 {
-    current_override_play_proto = play_proto;
     overridePlay(std::move(createPlay(play_proto, ai_config_ptr)));
 }
 
@@ -44,23 +40,17 @@ void Ai::checkAiConfig()
 {
     if (ai_config_changed)
     {
-        ai_config_changed = false;
-
-        fsm = std::make_unique<FSM<PlaySelectionFSM>>(PlaySelectionFSM{ai_config_ptr});
-
         auto current_override = ai_config_ptr->ai_control_config().override_ai_play();
+        std::unique_ptr<Play> override_play;
         if (current_override != TbotsProto::PlayName::UseAiSelection)
         {
-            // Override to new play if we're not running Ai Selection
             TbotsProto::Play play_proto;
             play_proto.set_name(current_override);
-            overridePlayFromProto(play_proto);
-        }
-        else
-        {
-            // Clear play override if we're running Ai Selection
-            overridePlay(nullptr);
+            override_play = createPlay(play_proto, ai_config_ptr);
         }
+
+        fsm->process_event(PlaySelectionFSM::Reset(std::move(override_play)));
+        ai_config_changed = false;
     }
 }
 
@@ -70,25 +60,12 @@ std::unique_ptr<TbotsProto::PrimitiveSet> Ai::getPrimitives(const WorldPtr& worl
 
     checkAiConfig();
 
-    fsm->process_event(PlaySelectionFSM::Update([this](std::unique_ptr<Play> play)
-                                                { current_play = std::move(play); },
-                                                world_ptr->gameState(), *ai_config_ptr));
+    fsm->process_event(PlaySelectionFSM::Update(world_ptr->gameState(), *ai_config_ptr));
 
-    std::unique_ptr<TbotsProto::PrimitiveSet> primitive_set;
-    if (static_cast<bool>(override_play))
-    {
-        primitive_set = override_play->get(world_ptr, inter_play_communication,
-                                           [this](InterPlayCommunication comm) {
-                                               inter_play_communication = std::move(comm);
-                                           });
-    }
-    else
-    {
-        primitive_set = current_play->get(world_ptr, inter_play_communication,
-                                          [this](InterPlayCommunication comm) {
-                                              inter_play_communication = std::move(comm);
-                                          });
-    }
+    auto primitive_set = static_cast<PlaySelectionFSM&>(*fsm).getSelectedPlay().get(
+        world_ptr, inter_play_communication,
+        [this](InterPlayCommunication comm)
+        { inter_play_communication = std::move(comm); });
 
     FrameMarkEnd(TracyConstants::AI_FRAME_MARKER);
 
@@ -97,14 +74,9 @@ std::unique_ptr<TbotsProto::PrimitiveSet> Ai::getPrimitives(const WorldPtr& worl
 
 TbotsProto::PlayInfo Ai::getPlayInfo() const
 {
-    std::vector<std::string> play_state = current_play->getState();
-    auto tactic_robot_id_assignment     = current_play->getTacticRobotIdAssignment();
-
-    if (static_cast<bool>(override_play))
-    {
-        play_state                 = override_play->getState();
-        tactic_robot_id_assignment = override_play->getTacticRobotIdAssignment();
-    }
+    Play& selected_play = static_cast<const PlaySelectionFSM&>(*fsm).getSelectedPlay();
+    const std::vector<std::string> play_state = selected_play.getState();
+    auto tactic_robot_id_assignment = selected_play.getTacticRobotIdAssignment();
 
     TbotsProto::PlayInfo info;
 

src/software/ai/ai.h

@@ -67,9 +67,6 @@ class Ai final
 
     std::shared_ptr<const TbotsProto::AiConfig> ai_config_ptr;
     std::unique_ptr<FSM<PlaySelectionFSM>> fsm;
-    std::unique_ptr<Play> override_play;
-    std::unique_ptr<Play> current_play;
-    TbotsProto::Play current_override_play_proto;
     bool ai_config_changed;
 
     // inter play communication

src/software/ai/hl/stp/play/kickoff_play_test.py

@@ -12,8 +12,6 @@
 # +------------------+------------------+
 # After ball leaves center: half/CC rules no longer enforced here.
 
-import threading
-
 import proto.import_all_protos as protos
 import pytest
 import software.python_bindings as tbots_cpp
@@ -33,6 +31,8 @@
     RobotNeverEntersRegion,
 )
 
+NORMAL_START_DELAY_S = 4.0
+
 
 @pytest.mark.parametrize("is_friendly_test", [True, False])
 def test_kickoff_play(simulated_test_runner, is_friendly_test):
@@ -87,14 +87,6 @@ def setup(*args):
             blue_play = protos.PlayName.KickoffEnemyPlay
             yellow_play = protos.PlayName.KickoffFriendlyPlay
 
-        # Let robots get ready before starting kickoff
-        threading.Timer(
-            4.0,
-            lambda: simulated_test_runner.send_gamecontroller_command(
-                gc_command=protos.Command.Type.NORMAL_START, team=SslTeam.BLUE
-            ),
-        ).start()
-
         simulated_test_runner.set_plays(blue_play=blue_play, yellow_play=yellow_play)
 
     # Always Validation
@@ -160,6 +152,9 @@ def setup(*args):
         setup=setup,
         inv_eventually_validation_sequence_set=eventually_validation_sequence_set,
         inv_always_validation_sequence_set=always_validation_sequence_set,
+        ci_cmd_with_delay=[
+            (NORMAL_START_DELAY_S, protos.Command.Type.NORMAL_START, SslTeam.BLUE),
+        ],
         test_timeout_s=10,
     )
 

src/software/ai/play_selection_fsm.cpp

@@ -15,10 +15,25 @@
 
 PlaySelectionFSM::PlaySelectionFSM(
     std::shared_ptr<const TbotsProto::AiConfig> ai_config_ptr)
-    : ai_config_ptr(ai_config_ptr), current_set_play(std::nullopt)
+    : ai_config_ptr(ai_config_ptr),
+      current_set_play(std::nullopt),
+      current_play(std::make_shared<HaltPlay>(ai_config_ptr)),
+      override_play(nullptr)
 {
 }
 
+Play& PlaySelectionFSM::getSelectedPlay() const
+{
+    if (override_play)
+    {
+        return *override_play;
+    }
+    else
+    {
+        return *current_play;
+    }
+}
+
 bool PlaySelectionFSM::gameStateStopped(const Update& event)
 {
     return event.game_state.isStopped();
@@ -39,63 +54,73 @@ bool PlaySelectionFSM::gameStateSetupRestart(const Update& event)
     return event.game_state.isSetupRestart();
 }
 
+void PlaySelectionFSM::setupOverridePlay(const Override& event)
+{
+    override_play = event.play;
+}
+
+void PlaySelectionFSM::resetPlaySelection(const Reset& event)
+{
+    current_set_play.reset();
+    setupOverridePlay(event);
+}
+
 void PlaySelectionFSM::setupSetPlay(const Update& event)
 {
     if (event.game_state.isOurBallPlacement())
     {
         if (current_set_play != TbotsProto::PlayName::BallPlacementPlay)
         {
             current_set_play = TbotsProto::PlayName::BallPlacementPlay;
-            event.set_current_play(std::make_unique<BallPlacementPlay>(ai_config_ptr));
+            setCurrentPlay(std::make_unique<BallPlacementPlay>(ai_config_ptr));
         }
     }
     else if (event.game_state.isTheirBallPlacement())
     {
         if (current_set_play != TbotsProto::PlayName::EnemyBallPlacementPlay)
         {
             current_set_play = TbotsProto::PlayName::EnemyBallPlacementPlay;
-            event.set_current_play(
-                std::make_unique<EnemyBallPlacementPlay>(ai_config_ptr));
+            setCurrentPlay(std::make_unique<EnemyBallPlacementPlay>(ai_config_ptr));
         }
     }
     else if (event.game_state.isOurKickoff())
     {
         if (current_set_play != TbotsProto::PlayName::KickoffFriendlyPlay)
         {
             current_set_play = TbotsProto::PlayName::KickoffFriendlyPlay;
-            event.set_current_play(std::make_unique<KickoffFriendlyPlay>(ai_config_ptr));
+            setCurrentPlay(std::make_unique<KickoffFriendlyPlay>(ai_config_ptr));
         }
     }
     else if (event.game_state.isTheirKickoff())
     {
         if (current_set_play != TbotsProto::PlayName::KickoffEnemyPlay)
         {
             current_set_play = TbotsProto::PlayName::KickoffEnemyPlay;
-            event.set_current_play(std::make_unique<KickoffEnemyPlay>(ai_config_ptr));
+            setCurrentPlay(std::make_unique<KickoffEnemyPlay>(ai_config_ptr));
         }
     }
     else if (event.game_state.isOurPenalty())
     {
         if (current_set_play != TbotsProto::PlayName::PenaltyKickPlay)
         {
             current_set_play = TbotsProto::PlayName::PenaltyKickPlay;
-            event.set_current_play(std::make_unique<PenaltyKickPlay>(ai_config_ptr));
+            setCurrentPlay(std::make_unique<PenaltyKickPlay>(ai_config_ptr));
         }
     }
     else if (event.game_state.isTheirPenalty())
     {
         if (current_set_play != TbotsProto::PlayName::PenaltyKickEnemyPlay)
         {
             current_set_play = TbotsProto::PlayName::PenaltyKickEnemyPlay;
-            event.set_current_play(std::make_unique<PenaltyKickEnemyPlay>(ai_config_ptr));
+            setCurrentPlay(std::make_unique<PenaltyKickEnemyPlay>(ai_config_ptr));
         }
     }
     else if (event.game_state.isOurDirectFree() || event.game_state.isOurIndirectFree())
     {
         if (current_set_play != TbotsProto::PlayName::FreeKickPlay)
         {
             current_set_play = TbotsProto::PlayName::FreeKickPlay;
-            event.set_current_play(std::make_unique<FreeKickPlay>(ai_config_ptr));
+            setCurrentPlay(std::make_unique<FreeKickPlay>(ai_config_ptr));
         }
     }
     else if (event.game_state.isTheirDirectFree() ||
@@ -104,27 +129,32 @@ void PlaySelectionFSM::setupSetPlay(const Update& event)
         if (current_set_play != TbotsProto::PlayName::EnemyFreeKickPlay)
         {
             current_set_play = TbotsProto::PlayName::EnemyFreeKickPlay;
-            event.set_current_play(std::make_unique<EnemyFreeKickPlay>(ai_config_ptr));
+            setCurrentPlay(std::make_unique<EnemyFreeKickPlay>(ai_config_ptr));
         }
     }
 }
 
 void PlaySelectionFSM::setupStopPlay(const Update& event)
 {
-    event.set_current_play(std::make_unique<StopPlay>(ai_config_ptr));
+    setCurrentPlay(std::make_unique<StopPlay>(ai_config_ptr));
 }
 
 void PlaySelectionFSM::setupHaltPlay(const Update& event)
 {
-    event.set_current_play(std::make_unique<HaltPlay>(ai_config_ptr));
+    setCurrentPlay(std::make_unique<HaltPlay>(ai_config_ptr));
 }
 
 void PlaySelectionFSM::setupOffensePlay(const Update& event)
 {
-    event.set_current_play(std::make_unique<OffensePlay>(ai_config_ptr));
+    setCurrentPlay(std::make_unique<OffensePlay>(ai_config_ptr));
 }
 
 void PlaySelectionFSM::resetSetPlay(const Update& event)
 {
     current_set_play.reset();
 }
+
+void PlaySelectionFSM::setCurrentPlay(std::unique_ptr<Play> play)
+{
+    current_play = std::move(play);
+}