diff --git a/docs/fsm-diagrams.md b/docs/fsm-diagrams.md
index 247ef514bc..90a556a0b4 100644
--- a/docs/fsm-diagrams.md
+++ b/docs/fsm-diagrams.md
@@ -11,16 +11,24 @@ direction LR
Halt --> Stop : [gameStateStopped]\nsetupStopPlay
Halt --> Playing : [gameStatePlaying]\nsetupOffensePlay
Halt --> SetPlay : [gameStateSetupRestart]\nsetupSetPlay
+Halt --> Halt : setupOverridePlay
+Halt --> Halt : resetPlaySelection
Stop --> Halt : [gameStateHalted]\nsetupHaltPlay
Stop --> Playing : [gameStatePlaying]\nsetupOffensePlay
Stop --> SetPlay : [gameStateSetupRestart]\nsetupSetPlay
+Stop --> Stop : setupOverridePlay
+Stop --> Halt : resetPlaySelection
Playing --> Halt : [gameStateHalted]\nsetupHaltPlay
Playing --> Stop : [gameStateStopped]\nsetupStopPlay
Playing --> SetPlay : [gameStateSetupRestart]\nsetupSetPlay
+Playing --> Playing : setupOverridePlay
+Playing --> Halt : resetPlaySelection
SetPlay --> Halt : [gameStateHalted]\nresetSetPlay, setupHaltPlay
SetPlay --> Stop : [gameStateStopped]\nresetSetPlay, setupStopPlay
SetPlay --> Playing : [gameStatePlaying]\nresetSetPlay, setupOffensePlay
SetPlay --> SetPlay : [gameStateSetupRestart]\nsetupSetPlay
+SetPlay --> SetPlay : setupOverridePlay
+SetPlay --> Halt : resetPlaySelection
Terminate:::terminate --> Terminate:::terminate
```
diff --git a/src/shared/robot_constants.cpp b/src/shared/robot_constants.cpp
index cbca9ea90d..77949c94f6 100644
--- a/src/shared/robot_constants.cpp
+++ b/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()
diff --git a/src/software/ai/ai.cpp b/src/software/ai/ai.cpp
index 088ce15cef..71ea9da6fc 100644
--- a/src/software/ai/ai.cpp
+++ b/src/software/ai/ai.cpp
@@ -2,7 +2,6 @@
#include
-#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"
@@ -10,8 +9,6 @@
Ai::Ai(std::shared_ptr ai_config_ptr)
: ai_config_ptr(ai_config_ptr),
fsm(std::make_unique>(PlaySelectionFSM{ai_config_ptr})),
- override_play(nullptr),
- current_play(std::make_unique(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 ai_config_ptr)
void Ai::overridePlay(std::unique_ptr 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>(PlaySelectionFSM{ai_config_ptr});
-
auto current_override = ai_config_ptr->ai_control_config().override_ai_play();
+ std::unique_ptr 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 Ai::getPrimitives(const WorldPtr& worl
checkAiConfig();
- fsm->process_event(PlaySelectionFSM::Update([this](std::unique_ptr 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 primitive_set;
- if (static_cast(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(*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 Ai::getPrimitives(const WorldPtr& worl
TbotsProto::PlayInfo Ai::getPlayInfo() const
{
- std::vector play_state = current_play->getState();
- auto tactic_robot_id_assignment = current_play->getTacticRobotIdAssignment();
-
- if (static_cast(override_play))
- {
- play_state = override_play->getState();
- tactic_robot_id_assignment = override_play->getTacticRobotIdAssignment();
- }
+ Play& selected_play = static_cast(*fsm).getSelectedPlay();
+ const std::vector play_state = selected_play.getState();
+ auto tactic_robot_id_assignment = selected_play.getTacticRobotIdAssignment();
TbotsProto::PlayInfo info;
diff --git a/src/software/ai/ai.h b/src/software/ai/ai.h
index 6146e8082b..ef7626cbbe 100644
--- a/src/software/ai/ai.h
+++ b/src/software/ai/ai.h
@@ -67,9 +67,6 @@ class Ai final
std::shared_ptr ai_config_ptr;
std::unique_ptr> fsm;
- std::unique_ptr override_play;
- std::unique_ptr current_play;
- TbotsProto::Play current_override_play_proto;
bool ai_config_changed;
// inter play communication
diff --git a/src/software/ai/hl/stp/play/kickoff_play_test.py b/src/software/ai/hl/stp/play/kickoff_play_test.py
index 9c27ca7675..09d289bce9 100644
--- a/src/software/ai/hl/stp/play/kickoff_play_test.py
+++ b/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,
)
diff --git a/src/software/ai/play_selection_fsm.cpp b/src/software/ai/play_selection_fsm.cpp
index a72d79cf7f..09cc9a6fae 100644
--- a/src/software/ai/play_selection_fsm.cpp
+++ b/src/software/ai/play_selection_fsm.cpp
@@ -15,10 +15,25 @@
PlaySelectionFSM::PlaySelectionFSM(
std::shared_ptr 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(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,6 +54,17 @@ 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())
@@ -46,7 +72,7 @@ void PlaySelectionFSM::setupSetPlay(const Update& event)
if (current_set_play != TbotsProto::PlayName::BallPlacementPlay)
{
current_set_play = TbotsProto::PlayName::BallPlacementPlay;
- event.set_current_play(std::make_unique(ai_config_ptr));
+ setCurrentPlay(std::make_unique(ai_config_ptr));
}
}
else if (event.game_state.isTheirBallPlacement())
@@ -54,8 +80,7 @@ void PlaySelectionFSM::setupSetPlay(const Update& event)
if (current_set_play != TbotsProto::PlayName::EnemyBallPlacementPlay)
{
current_set_play = TbotsProto::PlayName::EnemyBallPlacementPlay;
- event.set_current_play(
- std::make_unique(ai_config_ptr));
+ setCurrentPlay(std::make_unique(ai_config_ptr));
}
}
else if (event.game_state.isOurKickoff())
@@ -63,7 +88,7 @@ void PlaySelectionFSM::setupSetPlay(const Update& event)
if (current_set_play != TbotsProto::PlayName::KickoffFriendlyPlay)
{
current_set_play = TbotsProto::PlayName::KickoffFriendlyPlay;
- event.set_current_play(std::make_unique(ai_config_ptr));
+ setCurrentPlay(std::make_unique(ai_config_ptr));
}
}
else if (event.game_state.isTheirKickoff())
@@ -71,7 +96,7 @@ void PlaySelectionFSM::setupSetPlay(const Update& event)
if (current_set_play != TbotsProto::PlayName::KickoffEnemyPlay)
{
current_set_play = TbotsProto::PlayName::KickoffEnemyPlay;
- event.set_current_play(std::make_unique(ai_config_ptr));
+ setCurrentPlay(std::make_unique(ai_config_ptr));
}
}
else if (event.game_state.isOurPenalty())
@@ -79,7 +104,7 @@ void PlaySelectionFSM::setupSetPlay(const Update& event)
if (current_set_play != TbotsProto::PlayName::PenaltyKickPlay)
{
current_set_play = TbotsProto::PlayName::PenaltyKickPlay;
- event.set_current_play(std::make_unique(ai_config_ptr));
+ setCurrentPlay(std::make_unique(ai_config_ptr));
}
}
else if (event.game_state.isTheirPenalty())
@@ -87,7 +112,7 @@ void PlaySelectionFSM::setupSetPlay(const Update& event)
if (current_set_play != TbotsProto::PlayName::PenaltyKickEnemyPlay)
{
current_set_play = TbotsProto::PlayName::PenaltyKickEnemyPlay;
- event.set_current_play(std::make_unique(ai_config_ptr));
+ setCurrentPlay(std::make_unique(ai_config_ptr));
}
}
else if (event.game_state.isOurDirectFree() || event.game_state.isOurIndirectFree())
@@ -95,7 +120,7 @@ void PlaySelectionFSM::setupSetPlay(const Update& event)
if (current_set_play != TbotsProto::PlayName::FreeKickPlay)
{
current_set_play = TbotsProto::PlayName::FreeKickPlay;
- event.set_current_play(std::make_unique(ai_config_ptr));
+ setCurrentPlay(std::make_unique(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(ai_config_ptr));
+ setCurrentPlay(std::make_unique(ai_config_ptr));
}
}
}
void PlaySelectionFSM::setupStopPlay(const Update& event)
{
- event.set_current_play(std::make_unique(ai_config_ptr));
+ setCurrentPlay(std::make_unique(ai_config_ptr));
}
void PlaySelectionFSM::setupHaltPlay(const Update& event)
{
- event.set_current_play(std::make_unique(ai_config_ptr));
+ setCurrentPlay(std::make_unique(ai_config_ptr));
}
void PlaySelectionFSM::setupOffensePlay(const Update& event)
{
- event.set_current_play(std::make_unique(ai_config_ptr));
+ setCurrentPlay(std::make_unique(ai_config_ptr));
}
void PlaySelectionFSM::resetSetPlay(const Update& event)
{
current_set_play.reset();
}
+
+void PlaySelectionFSM::setCurrentPlay(std::unique_ptr play)
+{
+ current_play = std::move(play);
+}
diff --git a/src/software/ai/play_selection_fsm.h b/src/software/ai/play_selection_fsm.h
index 4a682a1bf4..3351449545 100644
--- a/src/software/ai/play_selection_fsm.h
+++ b/src/software/ai/play_selection_fsm.h
@@ -10,22 +10,28 @@ struct PlaySelectionFSM
class Playing;
class Stop;
class SetPlay;
- class OverridePlay;
struct Update
{
- Update(const std::function)>& set_current_play,
- const GameState& game_state, const TbotsProto::AiConfig& ai_config)
- : set_current_play(set_current_play),
- game_state(game_state),
- ai_config(ai_config)
+ Update(const GameState& game_state, const TbotsProto::AiConfig& ai_config)
+ : game_state(game_state), ai_config(ai_config)
{
}
- std::function)> set_current_play;
GameState game_state;
TbotsProto::AiConfig ai_config;
};
+ struct Override
+ {
+ explicit Override(std::unique_ptr play) : play(std::move(play)) {}
+ std::shared_ptr play;
+ };
+
+ struct Reset : Override
+ {
+ using Override::Override;
+ };
+
/**
* Creates a play selection FSM
*
@@ -33,6 +39,13 @@ struct PlaySelectionFSM
*/
explicit PlaySelectionFSM(std::shared_ptr ai_config_ptr);
+ /**
+ * Gets the currently selected play
+ *
+ * @return the override play if one exists, otherwise the current play
+ */
+ Play& getSelectedPlay() const;
+
/**
* Guards for whether the game state is stopped, halted, playing, or in set up
*
@@ -46,7 +59,21 @@ struct PlaySelectionFSM
bool gameStateSetupRestart(const Update& event);
/**
- * Action to set up the OverridePlay, SetPlay, StopPlay, HaltPlay, or OffensePlay
+ * Action to set up the override play
+ *
+ * @param event The PlaySelection::Override event
+ */
+ void setupOverridePlay(const Override& event);
+
+ /**
+ * Action to reset play selection and set up the override play
+ *
+ * @param event The PlaySelection::Reset event
+ */
+ void resetPlaySelection(const Reset& event);
+
+ /**
+ * Action to set up the SetPlay, StopPlay, HaltPlay, or OffensePlay
*
* @param event The PlaySelection::Update event
*
@@ -63,6 +90,13 @@ struct PlaySelectionFSM
*/
void resetSetPlay(const Update& event);
+ /**
+ * Sets the current play
+ *
+ * @param play the new current play
+ */
+ void setCurrentPlay(std::unique_ptr play);
+
auto operator()()
{
using namespace boost::sml;
@@ -78,7 +112,11 @@ struct PlaySelectionFSM
DEFINE_SML_GUARD(gameStateSetupRestart)
DEFINE_SML_EVENT(Update)
+ DEFINE_SML_EVENT(Override)
+ DEFINE_SML_EVENT(Reset)
+ DEFINE_SML_ACTION(setupOverridePlay)
+ DEFINE_SML_ACTION(resetPlaySelection)
DEFINE_SML_ACTION(setupSetPlay)
DEFINE_SML_ACTION(setupStopPlay)
DEFINE_SML_ACTION(setupHaltPlay)
@@ -93,18 +131,24 @@ struct PlaySelectionFSM
*Halt_S + Update_E[gameStateStopped_G] / setupStopPlay_A = Stop_S,
Halt_S + Update_E[gameStatePlaying_G] / setupOffensePlay_A = Playing_S,
Halt_S + Update_E[gameStateSetupRestart_G] / setupSetPlay_A = SetPlay_S,
+ Halt_S + Override_E / setupOverridePlay_A,
+ Halt_S + Reset_E / resetPlaySelection_A = Halt_S,
// Check for transitions to other states, if not then default to running the
// current play
Stop_S + Update_E[gameStateHalted_G] / setupHaltPlay_A = Halt_S,
Stop_S + Update_E[gameStatePlaying_G] / setupOffensePlay_A = Playing_S,
Stop_S + Update_E[gameStateSetupRestart_G] / setupSetPlay_A = SetPlay_S,
+ Stop_S + Override_E / setupOverridePlay_A,
+ Stop_S + Reset_E / resetPlaySelection_A = Halt_S,
// Check for transitions to other states, if not then default to running the
// current play
Playing_S + Update_E[gameStateHalted_G] / setupHaltPlay_A = Halt_S,
Playing_S + Update_E[gameStateStopped_G] / setupStopPlay_A = Stop_S,
Playing_S + Update_E[gameStateSetupRestart_G] / setupSetPlay_A = SetPlay_S,
+ Playing_S + Override_E / setupOverridePlay_A,
+ Playing_S + Reset_E / resetPlaySelection_A = Halt_S,
// Check for transitions to other states, if not then default to running the
// current play
@@ -115,6 +159,8 @@ struct PlaySelectionFSM
SetPlay_S + Update_E[gameStatePlaying_G] /
(resetSetPlay_A, setupOffensePlay_A) = Playing_S,
SetPlay_S + Update_E[gameStateSetupRestart_G] / setupSetPlay_A,
+ SetPlay_S + Override_E / setupOverridePlay_A,
+ SetPlay_S + Reset_E / resetPlaySelection_A = Halt_S,
X + Update_E = X);
}
@@ -122,4 +168,6 @@ struct PlaySelectionFSM
private:
std::shared_ptr ai_config_ptr;
std::optional current_set_play;
+ std::shared_ptr current_play;
+ std::shared_ptr override_play;
};
diff --git a/src/software/ai/play_selection_fsm_test.cpp b/src/software/ai/play_selection_fsm_test.cpp
index f314ce8f9f..b5886e961f 100644
--- a/src/software/ai/play_selection_fsm_test.cpp
+++ b/src/software/ai/play_selection_fsm_test.cpp
@@ -17,257 +17,247 @@ class PlaySelectionFSMTest : public ::testing::Test
std::unique_ptr> fsm =
std::make_unique>(PlaySelectionFSM{ai_config_ptr});
GameState game_state;
+
+ Play& selectedPlay() const
+ {
+ return static_cast(*fsm).getSelectedPlay();
+ }
+
+ void update()
+ {
+ fsm->process_event(PlaySelectionFSM::Update(game_state, ai_config));
+ }
};
-TEST_F(PlaySelectionFSMTest, test_transition_out_of_penalty_kick)
+TEST_F(PlaySelectionFSMTest, test_override_preserves_ai_selection_state)
{
- std::unique_ptr current_play = std::make_unique(ai_config_ptr);
+ // Stop
+ game_state.updateRefereeCommand(RefereeCommand::STOP);
+ update();
+ EXPECT_TRUE(fsm->is(boost::sml::state));
+ EXPECT_EQ("StopPlay", objectTypeName(selectedPlay()));
+
+ // Override stop play with halt
+ fsm->process_event(
+ PlaySelectionFSM::Override(std::make_unique(ai_config_ptr)));
+ EXPECT_EQ("HaltPlay", objectTypeName(selectedPlay()));
+ // Play selection should continue while overridden
+ game_state.updateRefereeCommand(RefereeCommand::FORCE_START);
+ update();
+ EXPECT_TRUE(fsm->is(boost::sml::state));
+ EXPECT_EQ("HaltPlay", objectTypeName(selectedPlay()));
+
+ // Remove override, should show new selected play from previous step
+ fsm->process_event(PlaySelectionFSM::Override(nullptr));
+ EXPECT_EQ("OffensePlay", objectTypeName(selectedPlay()));
+}
+
+TEST_F(PlaySelectionFSMTest, test_reset_clears_override_and_resets_selection_state)
+{
+ // Start
+ game_state.updateRefereeCommand(RefereeCommand::FORCE_START);
+ update();
+ EXPECT_TRUE(fsm->is(boost::sml::state));
+
+ // Override offense play with halt
+ fsm->process_event(
+ PlaySelectionFSM::Override(std::make_unique(ai_config_ptr)));
+ EXPECT_EQ("HaltPlay", objectTypeName(selectedPlay()));
+
+ // Play selection should be reset
+ fsm->process_event(PlaySelectionFSM::Reset(nullptr));
+ EXPECT_TRUE(fsm->is(boost::sml::state));
+ EXPECT_EQ("OffensePlay", objectTypeName(selectedPlay()));
+}
+
+TEST_F(PlaySelectionFSMTest, test_transition_out_of_penalty_kick)
+{
// Start in halt
- fsm->process_event(PlaySelectionFSM::Update(
- [¤t_play](std::unique_ptr play) { current_play = std::move(play); },
- game_state, ai_config));
+ update();
EXPECT_TRUE(fsm->is(boost::sml::state));
- EXPECT_EQ("HaltPlay", objectTypeName(*current_play));
+ EXPECT_EQ("HaltPlay", objectTypeName(selectedPlay()));
// Stop
game_state.updateRefereeCommand(RefereeCommand::STOP);
- fsm->process_event(PlaySelectionFSM::Update(
- [¤t_play](std::unique_ptr play) { current_play = std::move(play); },
- game_state, ai_config));
+ update();
EXPECT_TRUE(fsm->is(boost::sml::state));
- EXPECT_EQ("StopPlay", objectTypeName(*current_play));
+ EXPECT_EQ("StopPlay", objectTypeName(selectedPlay()));
// Penalty kick preparation
game_state.updateRefereeCommand(RefereeCommand::PREPARE_PENALTY_US);
- fsm->process_event(PlaySelectionFSM::Update(
- [¤t_play](std::unique_ptr play) { current_play = std::move(play); },
- game_state, ai_config));
+ update();
EXPECT_TRUE(fsm->is(boost::sml::state));
- EXPECT_EQ("PenaltyKickPlay", objectTypeName(*current_play));
+ EXPECT_EQ("PenaltyKickPlay", objectTypeName(selectedPlay()));
// Normal start
game_state.updateRefereeCommand(RefereeCommand::NORMAL_START);
EXPECT_TRUE(game_state.isReadyState());
- fsm->process_event(PlaySelectionFSM::Update(
- [¤t_play](std::unique_ptr play) { current_play = std::move(play); },
- game_state, ai_config));
+ update();
EXPECT_TRUE(fsm->is(boost::sml::state));
- EXPECT_EQ("PenaltyKickPlay", objectTypeName(*current_play));
+ EXPECT_EQ("PenaltyKickPlay", objectTypeName(selectedPlay()));
// Playing
game_state.updateRefereeCommand(RefereeCommand::HALT);
game_state.updateRefereeCommand(RefereeCommand::FORCE_START);
EXPECT_TRUE(game_state.isPlaying());
- fsm->process_event(PlaySelectionFSM::Update(
- [¤t_play](std::unique_ptr play) { current_play = std::move(play); },
- game_state, ai_config));
+ update();
EXPECT_TRUE(fsm->is(boost::sml::state));
- EXPECT_EQ("OffensePlay", objectTypeName(*current_play));
+ EXPECT_EQ("OffensePlay", objectTypeName(selectedPlay()));
}
TEST_F(PlaySelectionFSMTest, test_transition_out_of_penalty_kick_enemy_when_goal_conceded)
{
- std::unique_ptr current_play = std::make_unique(ai_config_ptr);
-
// Start in halt
- fsm->process_event(PlaySelectionFSM::Update(
- [¤t_play](std::unique_ptr play) { current_play = std::move(play); },
- game_state, ai_config));
+ update();
EXPECT_TRUE(fsm->is(boost::sml::state));
- EXPECT_EQ("HaltPlay", objectTypeName(*current_play));
+ EXPECT_EQ("HaltPlay", objectTypeName(selectedPlay()));
// Stop
game_state.updateRefereeCommand(RefereeCommand::STOP);
- fsm->process_event(PlaySelectionFSM::Update(
- [¤t_play](std::unique_ptr play) { current_play = std::move(play); },
- game_state, ai_config));
+ update();
EXPECT_TRUE(fsm->is(boost::sml::state));
- EXPECT_EQ("StopPlay", objectTypeName(*current_play));
+ EXPECT_EQ("StopPlay", objectTypeName(selectedPlay()));
// Penalty kick preparation
game_state.updateRefereeCommand(RefereeCommand::PREPARE_PENALTY_THEM);
- fsm->process_event(PlaySelectionFSM::Update(
- [¤t_play](std::unique_ptr play) { current_play = std::move(play); },
- game_state, ai_config));
+ update();
EXPECT_TRUE(game_state.isTheirPenalty());
EXPECT_TRUE(fsm->is(boost::sml::state));
- EXPECT_EQ("PenaltyKickEnemyPlay", objectTypeName(*current_play));
+ EXPECT_EQ("PenaltyKickEnemyPlay", objectTypeName(selectedPlay()));
// Normal start
game_state.updateRefereeCommand(RefereeCommand::NORMAL_START);
- fsm->process_event(PlaySelectionFSM::Update(
- [¤t_play](std::unique_ptr play) { current_play = std::move(play); },
- game_state, ai_config));
+ update();
EXPECT_TRUE(game_state.isReadyState());
EXPECT_TRUE(fsm->is(boost::sml::state));
- EXPECT_EQ("PenaltyKickEnemyPlay", objectTypeName(*current_play));
+ EXPECT_EQ("PenaltyKickEnemyPlay", objectTypeName(selectedPlay()));
// Goal conceded
game_state.updateRefereeCommand(RefereeCommand::GOAL_THEM);
- fsm->process_event(PlaySelectionFSM::Update(
- [¤t_play](std::unique_ptr play) { current_play = std::move(play); },
- game_state, ai_config));
+ update();
EXPECT_TRUE(game_state.isStopped());
EXPECT_TRUE(fsm->is(boost::sml::state));
- EXPECT_EQ("StopPlay", objectTypeName(*current_play));
+ EXPECT_EQ("StopPlay", objectTypeName(selectedPlay()));
// Kickoff preparation
game_state.updateRefereeCommand(RefereeCommand::PREPARE_KICKOFF_US);
- fsm->process_event(PlaySelectionFSM::Update(
- [¤t_play](std::unique_ptr play) { current_play = std::move(play); },
- game_state, ai_config));
+ update();
EXPECT_TRUE(game_state.isSetupState());
EXPECT_TRUE(fsm->is(boost::sml::state));
- EXPECT_EQ("KickoffFriendlyPlay", objectTypeName(*current_play));
+ EXPECT_EQ("KickoffFriendlyPlay", objectTypeName(selectedPlay()));
// Normal start
game_state.updateRefereeCommand(RefereeCommand::NORMAL_START);
- fsm->process_event(PlaySelectionFSM::Update(
- [¤t_play](std::unique_ptr play) { current_play = std::move(play); },
- game_state, ai_config));
+ update();
EXPECT_TRUE(game_state.isReadyState());
EXPECT_TRUE(fsm->is(boost::sml::state));
- EXPECT_EQ("KickoffFriendlyPlay", objectTypeName(*current_play));
+ EXPECT_EQ("KickoffFriendlyPlay", objectTypeName(selectedPlay()));
// Ball is kicked and restart state is cleared, enter playing state
game_state.setRestartCompleted();
- fsm->process_event(PlaySelectionFSM::Update(
- [¤t_play](std::unique_ptr play) { current_play = std::move(play); },
- game_state, ai_config));
+ update();
EXPECT_TRUE(game_state.isPlaying());
EXPECT_TRUE(fsm->is(boost::sml::state));
- EXPECT_EQ("OffensePlay", objectTypeName(*current_play));
+ EXPECT_EQ("OffensePlay", objectTypeName(selectedPlay()));
}
TEST_F(PlaySelectionFSMTest,
test_transition_out_of_penalty_kick_enemy_when_no_goal_conceded)
{
- std::unique_ptr current_play = std::make_unique(ai_config_ptr);
-
// Start in halt
- fsm->process_event(PlaySelectionFSM::Update(
- [¤t_play](std::unique_ptr play) { current_play = std::move(play); },
- game_state, ai_config));
+ update();
EXPECT_TRUE(fsm->is(boost::sml::state));
- EXPECT_EQ("HaltPlay", objectTypeName(*current_play));
+ EXPECT_EQ("HaltPlay", objectTypeName(selectedPlay()));
// Stop
game_state.updateRefereeCommand(RefereeCommand::STOP);
- fsm->process_event(PlaySelectionFSM::Update(
- [¤t_play](std::unique_ptr play) { current_play = std::move(play); },
- game_state, ai_config));
+ update();
EXPECT_TRUE(fsm->is(boost::sml::state));
- EXPECT_EQ("StopPlay", objectTypeName(*current_play));
+ EXPECT_EQ("StopPlay", objectTypeName(selectedPlay()));
// Penalty kick preparation
game_state.updateRefereeCommand(RefereeCommand::PREPARE_PENALTY_THEM);
- fsm->process_event(PlaySelectionFSM::Update(
- [¤t_play](std::unique_ptr play) { current_play = std::move(play); },
- game_state, ai_config));
+ update();
EXPECT_TRUE(game_state.isTheirPenalty());
EXPECT_TRUE(fsm->is(boost::sml::state));
- EXPECT_EQ("PenaltyKickEnemyPlay", objectTypeName(*current_play));
+ EXPECT_EQ("PenaltyKickEnemyPlay", objectTypeName(selectedPlay()));
// Normal start
game_state.updateRefereeCommand(RefereeCommand::NORMAL_START);
- fsm->process_event(PlaySelectionFSM::Update(
- [¤t_play](std::unique_ptr play) { current_play = std::move(play); },
- game_state, ai_config));
+ update();
EXPECT_TRUE(game_state.isReadyState());
EXPECT_TRUE(fsm->is(boost::sml::state));
- EXPECT_EQ("PenaltyKickEnemyPlay", objectTypeName(*current_play));
+ EXPECT_EQ("PenaltyKickEnemyPlay", objectTypeName(selectedPlay()));
// Stop because no goal
game_state.updateRefereeCommand(RefereeCommand::STOP);
- fsm->process_event(PlaySelectionFSM::Update(
- [¤t_play](std::unique_ptr play) { current_play = std::move(play); },
- game_state, ai_config));
+ update();
EXPECT_TRUE(game_state.isStopped());
EXPECT_TRUE(fsm->is(boost::sml::state));
- EXPECT_EQ("StopPlay", objectTypeName(*current_play));
+ EXPECT_EQ("StopPlay", objectTypeName(selectedPlay()));
// Free kick
game_state.updateRefereeCommand(RefereeCommand::DIRECT_FREE_US);
- fsm->process_event(PlaySelectionFSM::Update(
- [¤t_play](std::unique_ptr play) { current_play = std::move(play); },
- game_state, ai_config));
+ update();
EXPECT_TRUE(game_state.isOurDirectFree());
EXPECT_TRUE(game_state.isReadyState());
EXPECT_TRUE(fsm->is(boost::sml::state));
- EXPECT_EQ("FreeKickPlay", objectTypeName(*current_play));
+ EXPECT_EQ("FreeKickPlay", objectTypeName(selectedPlay()));
// Ball is kicked and restart state is cleared, enter playing state
game_state.setRestartCompleted();
- fsm->process_event(PlaySelectionFSM::Update(
- [¤t_play](std::unique_ptr play) { current_play = std::move(play); },
- game_state, ai_config));
+ update();
EXPECT_TRUE(game_state.isPlaying());
EXPECT_TRUE(fsm->is(boost::sml::state));
- EXPECT_EQ("OffensePlay", objectTypeName(*current_play));
+ EXPECT_EQ("OffensePlay", objectTypeName(selectedPlay()));
}
TEST_F(PlaySelectionFSMTest, test_transition_between_ball_placement_and_free_kick)
{
- std::unique_ptr current_play = std::make_unique(ai_config_ptr);
-
// Start in halt
- fsm->process_event(PlaySelectionFSM::Update(
- [¤t_play](std::unique_ptr play) { current_play = std::move(play); },
- game_state, ai_config));
+ update();
EXPECT_TRUE(fsm->is(boost::sml::state));
- EXPECT_EQ("HaltPlay", objectTypeName(*current_play));
+ EXPECT_EQ("HaltPlay", objectTypeName(selectedPlay()));
// Stop
game_state.updateRefereeCommand(RefereeCommand::STOP);
- fsm->process_event(PlaySelectionFSM::Update(
- [¤t_play](std::unique_ptr play) { current_play = std::move(play); },
- game_state, ai_config));
+ update();
EXPECT_TRUE(fsm->is(boost::sml::state));
- EXPECT_EQ("StopPlay", objectTypeName(*current_play));
+ EXPECT_EQ("StopPlay", objectTypeName(selectedPlay()));
// Friendly ball placement
game_state.updateRefereeCommand(RefereeCommand::BALL_PLACEMENT_US);
- fsm->process_event(PlaySelectionFSM::Update(
- [¤t_play](std::unique_ptr play) { current_play = std::move(play); },
- game_state, ai_config));
+ update();
EXPECT_TRUE(game_state.isOurBallPlacement());
EXPECT_TRUE(fsm->is(boost::sml::state));
- EXPECT_EQ("BallPlacementPlay", objectTypeName(*current_play));
+ EXPECT_EQ("BallPlacementPlay", objectTypeName(selectedPlay()));
// Friendly free kick
game_state.updateRefereeCommand(RefereeCommand::DIRECT_FREE_US);
- fsm->process_event(PlaySelectionFSM::Update(
- [¤t_play](std::unique_ptr play) { current_play = std::move(play); },
- game_state, ai_config));
+ update();
EXPECT_TRUE(game_state.isOurDirectFree());
EXPECT_TRUE(fsm->is(boost::sml::state));
- EXPECT_EQ("FreeKickPlay", objectTypeName(*current_play));
+ EXPECT_EQ("FreeKickPlay", objectTypeName(selectedPlay()));
// Enemy ball placement
game_state.updateRefereeCommand(RefereeCommand::BALL_PLACEMENT_THEM);
- fsm->process_event(PlaySelectionFSM::Update(
- [¤t_play](std::unique_ptr play) { current_play = std::move(play); },
- game_state, ai_config));
+ update();
EXPECT_TRUE(game_state.isTheirBallPlacement());
EXPECT_TRUE(fsm->is(boost::sml::state));
- EXPECT_EQ("EnemyBallPlacementPlay", objectTypeName(*current_play));
+ EXPECT_EQ("EnemyBallPlacementPlay", objectTypeName(selectedPlay()));
// Enemy free kick
game_state.updateRefereeCommand(RefereeCommand::DIRECT_FREE_THEM);
- fsm->process_event(PlaySelectionFSM::Update(
- [¤t_play](std::unique_ptr play) { current_play = std::move(play); },
- game_state, ai_config));
+ update();
EXPECT_TRUE(game_state.isTheirDirectFree());
EXPECT_TRUE(fsm->is(boost::sml::state));
- EXPECT_EQ("EnemyFreeKickPlay", objectTypeName(*current_play));
+ EXPECT_EQ("EnemyFreeKickPlay", objectTypeName(selectedPlay()));
// Ball is kicked and restart state is cleared, enter playing state
game_state.setRestartCompleted();
- fsm->process_event(PlaySelectionFSM::Update(
- [¤t_play](std::unique_ptr play) { current_play = std::move(play); },
- game_state, ai_config));
+ update();
EXPECT_TRUE(game_state.isPlaying());
EXPECT_TRUE(fsm->is(boost::sml::state));
- EXPECT_EQ("OffensePlay", objectTypeName(*current_play));
+ EXPECT_EQ("OffensePlay", objectTypeName(selectedPlay()));
}
diff --git a/src/software/embedded/BUILD b/src/software/embedded/BUILD
index 6f236fc70d..bdd2a95b4f 100644
--- a/src/software/embedded/BUILD
+++ b/src/software/embedded/BUILD
@@ -39,7 +39,7 @@ cc_library(
"//proto/primitive:primitive_msg_factory",
"//software/ai/navigator/trajectory:bang_bang_trajectory_1d_angular",
"//software/ai/navigator/trajectory:trajectory_path",
- "//software/embedded:robot_localizer",
+ "//software/embedded/robot_localizer:robot_localizer",
"//software/embedded/motion_control:orientation_controller",
"//software/embedded/motion_control:position_controller",
"//software/math:math_functions",
@@ -72,7 +72,7 @@ cc_library(
}),
deps = [
":primitive_executor",
- ":robot_localizer",
+ "//software/embedded/robot_localizer:robot_localizer",
"//proto:tbots_cc_proto",
"//software/embedded/services:imu",
"//software/embedded/services:motor",
@@ -114,34 +114,3 @@ filegroup(
srcs = ["hash_thunderloop_binary.sh"],
)
-cc_library(
- name = "robot_localizer",
- srcs = ["robot_localizer.cpp"],
- hdrs = ["robot_localizer.h"],
- deps = [
- "//proto:tbots_cc_proto",
- "//proto/primitive:primitive_msg_factory",
- "//software:constants",
- "//software/embedded/services:imu",
- "//software/geom:angle",
- "//software/geom:angular_velocity",
- "//software/geom:point",
- "//software/geom:vector",
- "//software/physics:velocity_conversion_util",
- "//software/sensor_fusion/filter:extended_kalman_filter",
- "//software/sensor_fusion/filter:kalman_filter",
- "//software/world:robot_state",
- "@eigen",
- ],
-)
-
-cc_test(
- name = "robot_localizer_test",
- srcs = ["robot_localizer_test.cpp"],
- deps = [
- ":robot_localizer",
- "//shared:constants",
- "//shared/test_util:tbots_gtest_main",
- "//software/physics:velocity_conversion_util",
- ],
-)
diff --git a/src/software/embedded/primitive_executor.cpp b/src/software/embedded/primitive_executor.cpp
index 4fbad709cb..401e23d8e5 100644
--- a/src/software/embedded/primitive_executor.cpp
+++ b/src/software/embedded/primitive_executor.cpp
@@ -8,7 +8,7 @@
#include "proto/primitive/primitive_msg_factory.h"
#include "proto/tbots_software_msgs.pb.h"
#include "proto/visualization.pb.h"
-#include "software/embedded/robot_localizer.h"
+#include "software/embedded/robot_localizer/robot_localizer.h"
#include "software/geom/algorithms/distance.h"
#include "software/logger/logger.h"
#include "software/physics/velocity_conversion_util.h"
diff --git a/src/software/embedded/robot_localizer/BUILD b/src/software/embedded/robot_localizer/BUILD
new file mode 100644
index 0000000000..5be401f50d
--- /dev/null
+++ b/src/software/embedded/robot_localizer/BUILD
@@ -0,0 +1,39 @@
+load("@bazel_skylib//rules:common_settings.bzl", "bool_flag", "string_flag")
+load("@rules_cc//cc:cc_binary.bzl", "cc_binary")
+load("@rules_cc//cc:cc_library.bzl", "cc_library")
+load("@rules_cc//cc:cc_test.bzl", "cc_test")
+load("@rules_shell//shell:sh_binary.bzl", "sh_binary")
+
+package(default_visibility = ["//visibility:public"])
+
+cc_library(
+ name = "robot_localizer",
+ srcs = ["robot_localizer.cpp"],
+ hdrs = ["robot_localizer.h", "robot_localizer_constants.h"],
+ deps = [
+ "//proto:tbots_cc_proto",
+ "//proto/primitive:primitive_msg_factory",
+ "//software:constants",
+ "//software/embedded/services:imu",
+ "//software/geom:angle",
+ "//software/geom:angular_velocity",
+ "//software/geom:point",
+ "//software/geom:vector",
+ "//software/physics:velocity_conversion_util",
+ "//software/sensor_fusion/filter:extended_kalman_filter",
+ "//software/sensor_fusion/filter:kalman_filter",
+ "//software/world:robot_state",
+ "@eigen",
+ ],
+)
+
+cc_test(
+ name = "robot_localizer_test",
+ srcs = ["robot_localizer_test.cpp"],
+ deps = [
+ ":robot_localizer",
+ "//shared:constants",
+ "//shared/test_util:tbots_gtest_main",
+ "//software/physics:velocity_conversion_util",
+ ],
+)
diff --git a/src/software/embedded/robot_localizer.cpp b/src/software/embedded/robot_localizer/robot_localizer.cpp
similarity index 75%
rename from src/software/embedded/robot_localizer.cpp
rename to src/software/embedded/robot_localizer/robot_localizer.cpp
index fb154b740a..c63ab78d24 100644
--- a/src/software/embedded/robot_localizer.cpp
+++ b/src/software/embedded/robot_localizer/robot_localizer.cpp
@@ -6,18 +6,16 @@
#include "shared/constants.h"
#include "software/physics/velocity_conversion_util.h"
-RobotLocalizer::RobotLocalizer(const RobotLocalizerConfig& config)
- : process_linear_velocity_noise_variance_(config.process_noise_variance),
- process_angular_acceleration_noise_variance_(config.process_noise_variance)
+RobotLocalizer::RobotLocalizer()
{
filter_.state_covariance =
- Eigen::Vector(1, 1, 1, 1, 1, 1).asDiagonal();
+ PROCESS_MODEL_INITIAL_VARIANCE * Eigen::Vector(1, 1, 1, 1, 1, 1).asDiagonal();
filter_.measurement_covariance =
Eigen::Vector(
- config.vision_noise_variance, config.vision_noise_variance,
- config.vision_noise_variance, config.motor_sensor_noise_variance,
- config.motor_sensor_noise_variance, config.motor_sensor_noise_variance,
+ VISION_X_INITIAL_VARIANCE_M, VISION_Y_INITIAL_VARIANCE_M,
+ VISION_THETA_INITIAL_VARIANCE_RAD, MOTOR_X_INITIAL_VARIANCE_M_S,
+ MOTOR_Y_INITIAL_VARIANCE_M_S, MOTOR_THETA_INITIAL_VARIANCE_RAD_S,
ImuService::IMU_VARIANCE)
.asDiagonal();
}
@@ -57,8 +55,8 @@ void RobotLocalizer::update(const VisionData& data)
[&](const FilterStep& step)
{ return (current_time_seconds_ - step.time_seconds) >= data.age_seconds; });
- // If rollback point is at the start, vision is newer than all history steps
- // So we empty history and apply vision
+ // If rollback point is at the start, vision is newer than all history steps
+ // So we empty history and apply vision
if (rollback_point == history.begin())
{
updateFilterWithVision(data.position, data.orientation);
@@ -66,8 +64,8 @@ void RobotLocalizer::update(const VisionData& data)
return;
}
- // If rollback point is at the end, vision is older than all history steps
- // So rollback ever step
+ // If rollback point is at the end, vision is older than all history steps
+ // So rollback ever step
if (rollback_point == history.end())
{
rollback_point = std::prev(history.end());
@@ -127,7 +125,7 @@ void RobotLocalizer::updateFilterWithVision(const Point& position,
measurement(static_cast(MeasurementIndex::VISION_Y_POSITION)) =
position.y();
- // Integrating omega for position makes angule goes out of bounds so we wrap it around
+ // Integrating omega for position makes angule goes out of bounds so we wrap it around
measurement(static_cast(MeasurementIndex::VISION_ORIENTATION)) =
orientation_estimate +
(orientation - Angle::fromRadians(orientation_estimate)).clamp().toRadians();
@@ -221,8 +219,7 @@ RobotState RobotLocalizer::getRobotState() const
return RobotState(getPosition(), getGlobalVelocity(), getOrientation(),
getAngularVelocity());
}
-
-// TODO: Investigate proces models/variances/etc
+// TODO: Investigate process models/variances/etc
void RobotLocalizer::generatedPredictionMatrices(double delta_time_seconds)
{
// Velocity is estimated in the robot's local frame (see StateIndex), but position
@@ -236,12 +233,9 @@ void RobotLocalizer::generatedPredictionMatrices(double delta_time_seconds)
filter_.process_model_function =
[delta_time_seconds](Eigen::Vector state)
{
- const double theta =
- state(static_cast(StateIndex::ORIENTATION));
- const double local_vx =
- state(static_cast(StateIndex::X_VELOCITY));
- const double local_vy =
- state(static_cast(StateIndex::Y_VELOCITY));
+ const double theta = state(static_cast(StateIndex::ORIENTATION));
+ const double local_vx = state(static_cast(StateIndex::X_VELOCITY));
+ const double local_vy = state(static_cast(StateIndex::Y_VELOCITY));
Eigen::Vector next_state =
Eigen::Vector::Zero();
@@ -266,20 +260,19 @@ void RobotLocalizer::generatedPredictionMatrices(double delta_time_seconds)
filter_.process_model_jacobian_function =
[delta_time_seconds](Eigen::Vector state)
{
- const auto x_position_index = static_cast(StateIndex::X_POSITION);
- const auto y_position_index = static_cast(StateIndex::Y_POSITION);
- const auto orientation_index =
- static_cast(StateIndex::ORIENTATION);
- const auto x_velocity_index = static_cast(StateIndex::X_VELOCITY);
- const auto y_velocity_index = static_cast(StateIndex::Y_VELOCITY);
+ const auto x_position_index = static_cast(StateIndex::X_POSITION);
+ const auto y_position_index = static_cast(StateIndex::Y_POSITION);
+ const auto orientation_index = static_cast(StateIndex::ORIENTATION);
+ const auto x_velocity_index = static_cast(StateIndex::X_VELOCITY);
+ const auto y_velocity_index = static_cast(StateIndex::Y_VELOCITY);
const auto angular_velocity_index =
static_cast(StateIndex::ANGULAR_VELOCITY);
- const double theta = state(orientation_index);
- const double local_vx = state(x_velocity_index);
- const double local_vy = state(y_velocity_index);
- const double cos_theta = std::cos(theta);
- const double sin_theta = std::sin(theta);
+ const double theta = state(orientation_index);
+ const double local_vx = state(x_velocity_index);
+ const double local_vy = state(y_velocity_index);
+ const double cos_theta = std::cos(theta);
+ const double sin_theta = std::sin(theta);
Eigen::Matrix jacobian =
Eigen::Matrix::Identity();
@@ -309,25 +302,24 @@ void RobotLocalizer::generatedPredictionMatrices(double delta_time_seconds)
const double delta_time_cubed = delta_time_squared * delta_time_seconds;
const double delta_time_fourth = delta_time_cubed * delta_time_seconds;
- // Linear terms model velocity itself as the noisy quantity (how much actual
- // velocity deviates from the commanded target velocity), integrated once into
- // position, rather than a noisy acceleration integrated twice.
const double linear_position_variance =
- delta_time_cubed * process_linear_velocity_noise_variance_;
+ delta_time_cubed * PROCESS_LINEAR_VELOCITY_NOISE_VARIANCE;
+
const double linear_position_velocity_covariance =
- delta_time_squared * process_linear_velocity_noise_variance_;
+ delta_time_squared * PROCESS_LINEAR_VELOCITY_NOISE_VARIANCE;
+
const double linear_velocity_variance =
- delta_time_seconds * process_linear_velocity_noise_variance_;
-
- // Angular terms are unchanged: angular velocity has no control input, so it's
- // still modeled as a noisy acceleration integrated twice.
+ delta_time_seconds * PROCESS_LINEAR_VELOCITY_NOISE_VARIANCE;
+
const double angular_position_variance =
- delta_time_fourth / 4 * process_angular_acceleration_noise_variance_;
+ (delta_time_fourth / 4.0) * PROCESS_ANGULAR_ACCELERATION_NOISE_VARIANCE;
+
const double angular_position_velocity_covariance =
- delta_time_cubed / 2 * process_angular_acceleration_noise_variance_;
+ (delta_time_cubed / 2.0) * PROCESS_ANGULAR_ACCELERATION_NOISE_VARIANCE;
+
const double angular_velocity_variance =
- delta_time_squared * process_angular_acceleration_noise_variance_;
-
+ delta_time_squared * PROCESS_ANGULAR_ACCELERATION_NOISE_VARIANCE;
+
// State order: X_POSITION, Y_POSITION, ORIENTATION, X_VELOCITY, Y_VELOCITY,
// ANGULAR_VELOCITY
// clang-format off
@@ -355,18 +347,15 @@ void RobotLocalizer::generatedPredictionMatrices(double delta_time_seconds)
control_model.setZero();
control_model(static_cast(StateIndex::X_VELOCITY),
- static_cast(ControlIndex::X_VELOCITY_TARGET)) =
- cos_theta;
+ static_cast(ControlIndex::X_VELOCITY_TARGET)) = cos_theta;
control_model(static_cast(StateIndex::X_VELOCITY),
- static_cast(ControlIndex::Y_VELOCITY_TARGET)) =
- sin_theta;
+ static_cast(ControlIndex::Y_VELOCITY_TARGET)) = sin_theta;
control_model(static_cast(StateIndex::Y_VELOCITY),
static_cast(ControlIndex::X_VELOCITY_TARGET)) =
-sin_theta;
control_model(static_cast(StateIndex::Y_VELOCITY),
- static_cast(ControlIndex::Y_VELOCITY_TARGET)) =
- cos_theta;
+ static_cast(ControlIndex::Y_VELOCITY_TARGET)) = cos_theta;
}
void RobotLocalizer::generateMeasurementModel(FilterStepType source)
@@ -376,34 +365,15 @@ void RobotLocalizer::generateMeasurementModel(FilterStepType source)
switch (source)
{
case FilterStepType::VISION_DATA:
- filter_.measurement_model(
- static_cast(MeasurementIndex::VISION_X_POSITION),
- static_cast(StateIndex::X_POSITION)) = 1;
- filter_.measurement_model(
- static_cast(MeasurementIndex::VISION_Y_POSITION),
- static_cast(StateIndex::Y_POSITION)) = 1;
- filter_.measurement_model(
- static_cast(MeasurementIndex::VISION_ORIENTATION),
- static_cast(StateIndex::ORIENTATION)) = 1;
+ filter_.measurement_model = VISION_MEASUREMENT_MODEL;
break;
case FilterStepType::MOTOR_DATA:
- filter_.measurement_model(
- static_cast(MeasurementIndex::MOTOR_X_VELOCITY),
- static_cast(StateIndex::X_VELOCITY)) = 1;
- filter_.measurement_model(
- static_cast(MeasurementIndex::MOTOR_Y_VELOCITY),
- static_cast(StateIndex::Y_VELOCITY)) = 1;
- filter_.measurement_model(
- static_cast(MeasurementIndex::MOTOR_ANGULAR_VELOCITY),
- static_cast(StateIndex::ANGULAR_VELOCITY)) = 1;
+ filter_.measurement_model = MOTOR_MEASUREMENT_MODEL;
break;
case FilterStepType::IMU_DATA:
- filter_.measurement_model(
- static_cast(MeasurementIndex::IMU_ANGULAR_VELOCITY),
- static_cast(StateIndex::ANGULAR_VELOCITY)) = 1;
+ filter_.measurement_model = IMU_MEASUREMENT_MODEL;
break;
case FilterStepType::PREDICT:
- // Never called with PREDICT; predict steps use generatedPredictionMatrices.
break;
}
}
diff --git a/src/software/embedded/robot_localizer.h b/src/software/embedded/robot_localizer/robot_localizer.h
similarity index 79%
rename from src/software/embedded/robot_localizer.h
rename to src/software/embedded/robot_localizer/robot_localizer.h
index 14d023ca80..0035697b8b 100644
--- a/src/software/embedded/robot_localizer.h
+++ b/src/software/embedded/robot_localizer/robot_localizer.h
@@ -13,22 +13,7 @@
#include "software/time/duration.h"
#include "software/util/make_enum/make_enum.hpp"
#include "software/world/robot_state.h"
-
-// X_POSITION/Y_POSITION are in world space; X_VELOCITY/Y_VELOCITY are in the robot's
-// local frame (see velocity_conversion_util.h), matching what the motor sensors report
-// directly and avoiding a lossy conversion through the orientation estimate.
-MAKE_ENUM(StateIndex, X_POSITION, Y_POSITION, ORIENTATION, X_VELOCITY, Y_VELOCITY,
- ANGULAR_VELOCITY);
-
-// MOTOR_X_VELOCITY/MOTOR_Y_VELOCITY are in the robot's local frame, matching
-// StateIndex::X_VELOCITY/Y_VELOCITY.
-MAKE_ENUM(MeasurementIndex, VISION_X_POSITION, VISION_Y_POSITION, VISION_ORIENTATION,
- MOTOR_X_VELOCITY, MOTOR_Y_VELOCITY, MOTOR_ANGULAR_VELOCITY,
- IMU_ANGULAR_VELOCITY);
-
-MAKE_ENUM(ControlIndex, X_VELOCITY_TARGET, Y_VELOCITY_TARGET);
-
-MAKE_ENUM(FilterStepType, PREDICT, MOTOR_DATA, IMU_DATA, VISION_DATA);
+#include "software/embedded/robot_localizer/robot_localizer_constants.h"
/**
* Estimates robot position, orientation, velocity, and angular velocity using an
@@ -66,12 +51,6 @@ class RobotLocalizer
AngularVelocity angular_velocity;
};
- struct RobotLocalizerConfig
- {
- double process_noise_variance;
- double vision_noise_variance;
- double motor_sensor_noise_variance;
- };
/**
* Creates a new robot localizer.
@@ -80,7 +59,7 @@ class RobotLocalizer
*
* @param config Configuration for the localizer variances.
*/
- explicit RobotLocalizer(const RobotLocalizerConfig& config);
+ explicit RobotLocalizer();
/**
* Runs one prediction step over the given elapsed time.
@@ -192,9 +171,6 @@ class RobotLocalizer
*/
void generateMeasurementModel(FilterStepType source);
- static constexpr size_t STATE_SIZE = reflective_enum::size();
- static constexpr size_t MEASUREMENT_SIZE = reflective_enum::size();
- static constexpr size_t CONTROL_SIZE = reflective_enum::size();
/**
* Snapshot of a Kalman filter predict/update step needed for rollback/replay.
@@ -213,7 +189,7 @@ class RobotLocalizer
// during replay (see generateMeasurementModel).
std::optional> measurement;
- // Post operation state
+ // Post operation state
Eigen::Vector state_estimate;
Eigen::Matrix state_covariance;
@@ -222,12 +198,6 @@ class RobotLocalizer
ExtendedKalmanFilter filter_;
- // Process noise variance used in prediction. The linear term models how much
- // actual velocity deviates from the commanded target velocity (a rate, per unit
- // time); the angular term models unmeasured angular acceleration disturbance.
- double process_linear_velocity_noise_variance_;
- double process_angular_acceleration_noise_variance_;
-
// History is ordered newest-first (front is the most recent step)
std::deque history;
diff --git a/src/software/embedded/robot_localizer/robot_localizer_constants.h b/src/software/embedded/robot_localizer/robot_localizer_constants.h
new file mode 100644
index 0000000000..eb7e17637a
--- /dev/null
+++ b/src/software/embedded/robot_localizer/robot_localizer_constants.h
@@ -0,0 +1,64 @@
+#include
+MAKE_ENUM(StateIndex, X_POSITION, Y_POSITION, ORIENTATION, X_VELOCITY, Y_VELOCITY,
+ ANGULAR_VELOCITY);
+
+MAKE_ENUM(MeasurementIndex, VISION_X_POSITION, VISION_Y_POSITION, VISION_ORIENTATION,
+ MOTOR_X_VELOCITY, MOTOR_Y_VELOCITY, MOTOR_ANGULAR_VELOCITY,
+ IMU_ANGULAR_VELOCITY);
+
+MAKE_ENUM(ControlIndex, X_VELOCITY_TARGET, Y_VELOCITY_TARGET);
+
+MAKE_ENUM(FilterStepType, PREDICT, MOTOR_DATA, IMU_DATA, VISION_DATA);
+
+
+static constexpr size_t STATE_SIZE = reflective_enum::size();
+static constexpr size_t MEASUREMENT_SIZE = reflective_enum::size();
+static constexpr size_t CONTROL_SIZE = reflective_enum::size();
+
+// Initial Covariances
+static constexpr double VISION_X_INITIAL_VARIANCE_M = 0.00001;
+static constexpr double VISION_Y_INITIAL_VARIANCE_M = 0.00001;
+static constexpr double VISION_THETA_INITIAL_VARIANCE_RAD = 0.00001;
+
+static constexpr double MOTOR_X_INITIAL_VARIANCE_M_S = 0.5;
+static constexpr double MOTOR_Y_INITIAL_VARIANCE_M_S = 0.5;
+static constexpr double MOTOR_THETA_INITIAL_VARIANCE_RAD_S = 0.5;
+
+static constexpr double PROCESS_MODEL_INITIAL_VARIANCE = 1;
+
+
+static constexpr double PROCESS_LINEAR_VELOCITY_NOISE_VARIANCE = 1;
+static constexpr double PROCESS_ANGULAR_ACCELERATION_NOISE_VARIANCE = 1;
+
+static const Eigen::Matrix VISION_MEASUREMENT_MODEL = []{
+ Eigen::Matrix m;
+ m << 1,0,0,0,0,0,0,
+ 0,1,0,0,0,0,0,
+ 0,0,1,0,0,0,0,
+ 0,0,0,0,0,0,0,
+ 0,0,0,0,0,0,0,
+ 0,0,0,0,0,0,0;
+ return m;
+}();
+
+static const Eigen::Matrix MOTOR_MEASUREMENT_MODEL = []{
+ Eigen::Matrix m;
+ m << 0,0,0,0,0,0,0,
+ 0,0,0,0,0,0,0,
+ 0,0,0,0,0,0,0,
+ 0,0,0,1,0,0,0,
+ 0,0,0,0,1,0,0,
+ 0,0,0,0,0,1,0;
+ return m;
+}();
+
+static const Eigen::Matrix IMU_MEASUREMENT_MODEL = []{
+ Eigen::Matrix m;
+ m << 0,0,0,0,0,0,0,
+ 0,0,0,0,0,0,0,
+ 0,0,0,0,0,0,0,
+ 0,0,0,0,0,0,0,
+ 0,0,0,0,0,0,0,
+ 0,0,0,0,0,0,1;
+ return m;
+}();
diff --git a/src/software/embedded/robot_localizer_test.cpp b/src/software/embedded/robot_localizer/robot_localizer_test.cpp
similarity index 87%
rename from src/software/embedded/robot_localizer_test.cpp
rename to src/software/embedded/robot_localizer/robot_localizer_test.cpp
index 52efe91fcb..31e2a1733b 100644
--- a/src/software/embedded/robot_localizer_test.cpp
+++ b/src/software/embedded/robot_localizer/robot_localizer_test.cpp
@@ -1,4 +1,4 @@
-#include "software/embedded/robot_localizer.h"
+#include "robot_localizer.h"
#include
@@ -11,12 +11,6 @@
namespace
{
// Mirror the values thunderloop constructs the localizer with (DivB constants).
-RobotLocalizer::RobotLocalizerConfig makeConfig()
-{
- return RobotLocalizer::RobotLocalizerConfig{/*process_noise_variance=*/1.0,
- /*vision_noise_variance=*/0.01 * 0.01,
- /*motor_sensor_noise_variance=*/0.5};
-}
constexpr double LOOP_HZ = 300.0;
constexpr double DT = 1.0 / LOOP_HZ;
@@ -27,7 +21,7 @@ constexpr double DT = 1.0 / LOOP_HZ;
// provided (isolates whether the periodic vision fix corrupts the velocity estimate).
RobotLocalizer runConstantVelocity(bool feed_vision, double vision_age = RTT_S / 2)
{
- RobotLocalizer localizer(makeConfig());
+ RobotLocalizer localizer();
const Vector true_velocity(1.0, 0.0);
const Angle true_orientation = Angle::zero();
@@ -44,7 +38,8 @@ RobotLocalizer runConstantVelocity(bool feed_vision, double vision_age = RTT_S /
const Vector local_velocity =
globalToLocalVelocity(true_velocity, true_orientation);
- localizer.update(RobotLocalizer::MotorData{local_velocity, AngularVelocity::zero()});
+ localizer.update(
+ RobotLocalizer::MotorData{local_velocity, AngularVelocity::zero()});
localizer.predict(Vector(0.0, 0.0), Duration::fromSeconds(DT));
@@ -69,8 +64,9 @@ TEST(RobotLocalizer, tracks_constant_forward_velocity)
const RobotLocalizer localizer = runConstantVelocity(/*feed_vision=*/true);
std::cerr << "[motor+vision] pos=(" << localizer.getPosition().x() << ", "
- << localizer.getPosition().y() << ") vel=(" << localizer.getGlobalVelocity().x()
- << ", " << localizer.getGlobalVelocity().y()
+ << localizer.getPosition().y() << ") vel=("
+ << localizer.getGlobalVelocity().x() << ", "
+ << localizer.getGlobalVelocity().y()
<< ") orient=" << localizer.getOrientation().toDegrees() << "deg\n";
// NOTE: we assert on velocity and orientation, not absolute position. RobotLocalizer
diff --git a/src/software/embedded/thunderloop.cpp b/src/software/embedded/thunderloop.cpp
index c3348d124a..57cdbe891a 100644
--- a/src/software/embedded/thunderloop.cpp
+++ b/src/software/embedded/thunderloop.cpp
@@ -148,10 +148,7 @@ Thunderloop::Thunderloop(const robot_constants::RobotConstants& robot_constants,
LOG(INFO) << "THUNDERLOOP: IMU Service initialized!";
robot_localizer_ =
- std::make_unique(RobotLocalizer::RobotLocalizerConfig{
- robot_constants.kalman_process_noise_variance_rad_per_s_4,
- robot_constants.kalman_vision_noise_variance_rad_2,
- robot_constants.kalman_motor_sensor_noise_variance_rad_per_s_2});
+ std::make_unique();
LOG(INFO) << "THUNDERLOOP: Robot Localizer initialized!";
primitive_executor_ = std::make_unique(robot_constants, robot_id);
@@ -271,24 +268,32 @@ void Thunderloop::updateRobotLocalizer(const TbotsProto::RobotStatus& robot_stat
{
// Seperate update is okay because measurement model is linear
if (robot_status.has_imu_status()){
- AngularVelocity res = createAngularVelocity(robot_status.imu_status().angular_velocity());
- if (res <0.1){
- res = 0;
- }
robot_localizer_->update(RobotLocalizer::ImuData{
- createAngularVelocity(0)});
+ createAngularVelocity(robot_status.imu_status().angular_velocity())
+ });
+
}
if (robot_status.has_motor_status())
- Vector velocity = robot_status.motor_status().local_velocity();
- if ( velocity.x() <0.05 && velocity.y() <0.05 ) {
- velocity = Vector(0,0);
- }
- AngularVelocity angular_velocity = robot_status.motor_status().angular_velocity();
- if ( angular_velocity.toRadians() <0.1 ) {
- angular_velocity = Angle::zero();
- }
- robot_localizer_->update(RobotLocalizer::MotorData{
- velocity, angular_velocity
-});
+ {
+ AngularVelocity res =
+ createAngularVelocity(robot_status.imu_status().angular_velocity());
+ if (res < 0.1)
+ {
+ res = 0;
+ }
+ robot_localizer_->update(RobotLocalizer::ImuData{createAngularVelocity(0)});
+ }
+ if (robot_status.has_motor_status())
+ Vector velocity = robot_status.motor_status().local_velocity();
+ if (velocity.x() < 0.05 && velocity.y() < 0.05)
+ {
+ velocity = Vector(0, 0);
}
+ AngularVelocity angular_velocity = robot_status.motor_status().angular_velocity();
+ if (angular_velocity.toRadians() < 0.1)
+ {
+ angular_velocity = Angle::zero();
+ }
+ robot_localizer_->update(RobotLocalizer::MotorData{velocity, angular_velocity});
+}
}
diff --git a/src/software/embedded/thunderloop.h b/src/software/embedded/thunderloop.h
index 68e4ff5766..137e63a468 100644
--- a/src/software/embedded/thunderloop.h
+++ b/src/software/embedded/thunderloop.h
@@ -4,7 +4,7 @@
#include "shared/robot_constants.h"
#include "software/embedded/primitive_executor.h"
-#include "software/embedded/robot_localizer.h"
+#include "software/embedded/robot_localizer/robot_localizer.h"
#include "software/embedded/services/imu.h"
#include "software/embedded/services/motor.h"
#include "software/embedded/services/network/network.h"