From 906638c08fdf55bfa71e34d7a98ec5a75e873fbc Mon Sep 17 00:00:00 2001 From: Andrew Mao Date: Tue, 10 Mar 2026 16:11:35 -0700 Subject: [PATCH 01/57] enable simulated dribbled --- src/software/simulation/er_force_simulator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/software/simulation/er_force_simulator.cpp b/src/software/simulation/er_force_simulator.cpp index b58d4989a3..ca0c1511d0 100644 --- a/src/software/simulation/er_force_simulator.cpp +++ b/src/software/simulation/er_force_simulator.cpp @@ -122,7 +122,7 @@ std::unique_ptr ErForceSimulator::createRealisticRealismCo realism_config->set_vision_delay(35000000); realism_config->set_vision_processing_time(10000000); realism_config->set_missing_ball_detections(0.02f); - realism_config->set_simulate_dribbling(false); + realism_config->set_simulate_dribbling(true); return realism_config; } From 7ab30e9f2d4ef5821110c5f92d87cd3c920f52f8 Mon Sep 17 00:00:00 2001 From: StarrryNight Date: Fri, 3 Apr 2026 19:02:37 -0700 Subject: [PATCH 02/57] temporary working evaluation --- .../stp/tactic/goalie/goalie_tactic_test.py | 196 +++++++++--------- src/software/er_force_simulator_main.cpp | 13 +- .../sensor_fusion/filter/ball_filter_test.cpp | 31 +++ .../simulation/er_force_simulator.cpp | 11 +- src/software/simulation/er_force_simulator.h | 7 + .../game_controller.py | 10 +- .../binary_context_managers/simulator.py | 4 + .../thunderscope/thunderscope_main.py | 1 + 8 files changed, 171 insertions(+), 102 deletions(-) diff --git a/src/software/ai/hl/stp/tactic/goalie/goalie_tactic_test.py b/src/software/ai/hl/stp/tactic/goalie/goalie_tactic_test.py index 2111109524..bdcd66b291 100644 --- a/src/software/ai/hl/stp/tactic/goalie/goalie_tactic_test.py +++ b/src/software/ai/hl/stp/tactic/goalie/goalie_tactic_test.py @@ -18,94 +18,94 @@ @pytest.mark.parametrize( "ball_initial_position,ball_initial_velocity,robot_initial_position", [ - # test panic ball very fast in straight line - (tbots_cpp.Point(0, 0), tbots_cpp.Vector(-5, 0), tbots_cpp.Point(-4, 0)), - # test panic ball very_fast in diagonal line - ( - tbots_cpp.Point(0, 0), - tbots_cpp.Vector(-5.5, 0.25), - tbots_cpp.Field.createSSLDivisionBField().friendlyGoalCenter() - + tbots_cpp.Vector(0, -0.5), - ), - # test ball very fast misses net - (tbots_cpp.Point(0, 0), tbots_cpp.Vector(-5, 1), tbots_cpp.Point(-4.5, 0)), - # test ball very fast get saved - # TODO (#3377): This test is flaky due to inconsistent goalie reach. The linked ticket may provide a permanent fix. + # # test panic ball very fast in straight line + # (tbots_cpp.Point(0, 0), tbots_cpp.Vector(-5, 0), tbots_cpp.Point(-4, 0)), + # # test panic ball very_fast in diagonal line + # ( + # tbots_cpp.Point(0, 0), + # tbots_cpp.Vector(-5.5, 0.25), + # tbots_cpp.Field.createSSLDivisionBField().friendlyGoalCenter() + # + tbots_cpp.Vector(0, -0.5), + # ), + # # test ball very fast misses net + # (tbots_cpp.Point(0, 0), tbots_cpp.Vector(-5, 1), tbots_cpp.Point(-4.5, 0)), + # # test ball very fast get saved + # # TODO (#3377): This test is flaky due to inconsistent goalie reach. The linked ticket may provide a permanent fix. ( tbots_cpp.Point(-2.5, 0), # TODO Revert velocity to (-4.8, 1.1) - tbots_cpp.Vector(-3.6, 0.825), + tbots_cpp.Vector(-4.8, 1.1), tbots_cpp.Point(-4.5, 0), ), # test ball very fast with the goalie out of position saved # TODO (#3377): This test is flaky due to inconsistent goalie reach. The linked ticket may provide a permanent fix. - ( - tbots_cpp.Point(-2, 0), - # TODO Revert velocity to (-5.5,1) - tbots_cpp.Vector(-4.125, 0.75), - tbots_cpp.Point(-4.5, -0.1), - ), - # ball slow inside friendly defense area - (tbots_cpp.Point(-4, 0.8), tbots_cpp.Vector(-0.2, 0), tbots_cpp.Point(0, 0)), - # ball slow inside friendly defense area - (tbots_cpp.Point(-4, 0.8), tbots_cpp.Vector(-0.2, 0), tbots_cpp.Point(0, 2)), - # ball slow inside friendly defense area - (tbots_cpp.Point(-4, 0.8), tbots_cpp.Vector(-0.2, 0), tbots_cpp.Point(-4, 0)), - # ball stationary inside friendly defense area - ( - tbots_cpp.Point(-4, 0.0), - tbots_cpp.Vector(0.0, 0), - tbots_cpp.Field.createSSLDivisionBField().friendlyGoalpostPos(), - ), - # ball stationary inside no-chip rectangle - ( - tbots_cpp.Field.createSSLDivisionBField().friendlyGoalCenter() - + tbots_cpp.Vector(0.1, 0.1), - tbots_cpp.Vector(-0.2, 0), - tbots_cpp.Point(-4, -1), - ), - # ball fast inside no-chip rectangle but no intersection with goal - ( - tbots_cpp.Field.createSSLDivisionBField().friendlyGoalCenter() - + tbots_cpp.Vector(0.1, 0), - tbots_cpp.Vector(0, -0.5), - tbots_cpp.Point(-3.5, 1), - ), - # ball moving out from inside defense area - ( - tbots_cpp.Field.createSSLDivisionBField().friendlyGoalCenter() - + tbots_cpp.Vector(0.5, 0), - tbots_cpp.Vector(0.5, 0), - tbots_cpp.Point(-3.5, 0), - ), - # ball slow inside no-chip rectangle - ( - tbots_cpp.Field.createSSLDivisionBField().friendlyGoalCenter() - + tbots_cpp.Vector(0.1, 0), - tbots_cpp.Vector(0.1, -0.1), - tbots_cpp.Point(-3.5, 1), - ), - # ball moving into goal from inside defense area - ( - tbots_cpp.Field.createSSLDivisionBField().friendlyGoalCenter() - + tbots_cpp.Vector(0.5, 0), - tbots_cpp.Vector(-0.5, 0), - tbots_cpp.Point(-3.5, 0), - ), - # ball moving up and out of defense area - ( - tbots_cpp.Field.createSSLDivisionBField().friendlyGoalCenter() - + tbots_cpp.Vector(0.3, 0), - tbots_cpp.Vector(0, 1), - tbots_cpp.Point(-3.5, 0), - ), - # ball moving down and out goal from defense area - ( - tbots_cpp.Field.createSSLDivisionBField().friendlyGoalCenter() - + tbots_cpp.Vector(0.3, 0), - tbots_cpp.Vector(0, -0.7), - tbots_cpp.Point(-3.5, 0), - ), +# ( +# tbots_cpp.Point(-2, 0), +# # TODO Revert velocity to (-5.5,1) +# tbots_cpp.Vector(-5.5, 1), +# tbots_cpp.Point(-4.5, -0.1), +# ), +# # ball slow inside friendly defense area + # (tbots_cpp.Point(-4, 0.8), tbots_cpp.Vector(-0.2, 0), tbots_cpp.Point(0, 0)), + # # ball slow inside friendly defense area + # (tbots_cpp.Point(-4, 0.8), tbots_cpp.Vector(-0.2, 0), tbots_cpp.Point(0, 2)), + # # ball slow inside friendly defense area + # (tbots_cpp.Point(-4, 0.8), tbots_cpp.Vector(-0.2, 0), tbots_cpp.Point(-4, 0)), + # # ball stationary inside friendly defense area + # ( + # tbots_cpp.Point(-4, 0.0), + # tbots_cpp.Vector(0.0, 0), + # tbots_cpp.Field.createSSLDivisionBField().friendlyGoalpostPos(), + # ), + # # ball stationary inside no-chip rectangle + # ( + # tbots_cpp.Field.createSSLDivisionBField().friendlyGoalCenter() + # + tbots_cpp.Vector(0.1, 0.1), + # tbots_cpp.Vector(-0.2, 0), + # tbots_cpp.Point(-4, -1), + # ), + # # ball fast inside no-chip rectangle but no intersection with goal + # ( + # tbots_cpp.Field.createSSLDivisionBField().friendlyGoalCenter() + # + tbots_cpp.Vector(0.1, 0), + # tbots_cpp.Vector(0, -0.5), + # tbots_cpp.Point(-3.5, 1), + # ), + # # ball moving out from inside defense area + # ( + # tbots_cpp.Field.createSSLDivisionBField().friendlyGoalCenter() + # + tbots_cpp.Vector(0.5, 0), + # tbots_cpp.Vector(0.5, 0), + # tbots_cpp.Point(-3.5, 0), + # ), + # # ball slow inside no-chip rectangle + # ( + # tbots_cpp.Field.createSSLDivisionBField().friendlyGoalCenter() + # + tbots_cpp.Vector(0.1, 0), + # tbots_cpp.Vector(0.1, -0.1), + # tbots_cpp.Point(-3.5, 1), + # ), + # # ball moving into goal from inside defense area + # ( + # tbots_cpp.Field.createSSLDivisionBField().friendlyGoalCenter() + # + tbots_cpp.Vector(0.5, 0), + # tbots_cpp.Vector(-0.5, 0), + # tbots_cpp.Point(-3.5, 0), + # ), + # # ball moving up and out of defense area + # ( + # tbots_cpp.Field.createSSLDivisionBField().friendlyGoalCenter() + # + tbots_cpp.Vector(0.3, 0), + # tbots_cpp.Vector(0, 1), + # tbots_cpp.Point(-3.5, 0), + # ), + # # ball moving down and out goal from defense area + # ( + # tbots_cpp.Field.createSSLDivisionBField().friendlyGoalCenter() + # + tbots_cpp.Vector(0.3, 0), + # tbots_cpp.Vector(0, -0.7), + # tbots_cpp.Point(-3.5, 0), + # ), ], ) def test_goalie_blocks_shot( @@ -167,23 +167,23 @@ def setup(*args): @pytest.mark.parametrize( "ball_position,should_clear", [ - ( - tbots_cpp.Point(-3.45, 0), - True, - ), # ball is just inside the dead zone in the X direction - ( - tbots_cpp.Point(-3.45, 0.9), - True, - ), # ball is just inside the dead zone in the X direction - ( - tbots_cpp.Point(-4.0, 1.05), - True, - ), # ball is just inside the dead zone in the Y direction - ( - tbots_cpp.Point(0, 0), - False, - # ball is just outside the dead zone in the X direction - ), + # ( + # tbots_cpp.Point(-3.45, 0), + # True, + # ), # ball is just inside the dead zone in the X direction + # ( + # tbots_cpp.Point(-3.45, 0.9), + # True, + # ), # ball is just inside the dead zone in the X direction + # ( + # tbots_cpp.Point(-4.0, 1.05), + # True, + # ), # ball is just inside the dead zone in the Y direction + # ( + # tbots_cpp.Point(0, 0), + # False, + # # ball is just outside the dead zone in the X direction + # ), ], ) def test_goalie_clears_from_dead_zone( diff --git a/src/software/er_force_simulator_main.cpp b/src/software/er_force_simulator_main.cpp index 8f8261b025..8515bc32d4 100644 --- a/src/software/er_force_simulator_main.cpp +++ b/src/software/er_force_simulator_main.cpp @@ -48,6 +48,7 @@ int main(int argc, char **argv) { std::string runtime_dir = args.runtime_dir; LoggerSingleton::initializeLogger(runtime_dir, nullptr); + LOG(CSV, "Failed_goalie_tactic_data.csv") << "timestamp_s,fused_x,fused_y,truth_x,truth_y, true_vel_x, true_vel_y,is_occluded\n"; /** * Creates a ER force simulator and sets up the appropriate @@ -220,7 +221,17 @@ int main(int argc, char **argv) yellow_robot_status_output.sendProto(packet); } - simulator_state_output.sendProto(er_force_sim->getSimulatorState()); + auto sim_state = er_force_sim->getSimulatorState(); + LOG(CSV, "Failed_goalie_tactic_data.csv") + << yellow_vision.time_sent().epoch_timestamp_seconds() << "," + << yellow_vision.ball().current_state().global_position().x_meters() + << "," + << yellow_vision.ball().current_state().global_position().y_meters() + << "," << sim_state.ball().p_x() << "," << sim_state.ball().p_y() + << "," << sim_state.ball().v_x() << "," << sim_state.ball().v_y() + << "," << !er_force_sim->isBallVisible() + << "\n"; + simulator_state_output.sendProto(sim_state); }); // This blocks forever without using the CPU diff --git a/src/software/sensor_fusion/filter/ball_filter_test.cpp b/src/software/sensor_fusion/filter/ball_filter_test.cpp index 1de4515861..4c00e8fe76 100644 --- a/src/software/sensor_fusion/filter/ball_filter_test.cpp +++ b/src/software/sensor_fusion/filter/ball_filter_test.cpp @@ -546,3 +546,34 @@ TEST_F(BallFilterTest, ball_moving_along_y_axis) expected_velocity_angle_tolernace, expected_velocity_magnitude_tolerance, num_steps_to_ignore); } + +TEST_F(BallFilterTest, ball_moving_fast_with_vision_delay) +{ + // Setup a fast moving ball (e.g., 5.0 m/s) + Vector ball_velocity(5.0, 0.0); + Point ball_start(0, 0); + + // Simulate a 35ms SSL-Vision processing delay + Duration vision_delay = Duration::fromSeconds(0.035); + + // Feed the filter a few frames to build up velocity + for(int i = 0; i < 10; i++) { + Timestamp t_capture = current_timestamp + Duration::fromSeconds(i * 0.016); + Point pos = ball_start + ball_velocity * (i * 0.016); + + std::vector det = {BallDetection{pos, 0.0, t_capture, 1.0}}; + + // The crucial change: we ask the filter for the state at t_capture + vision_delay + Timestamp t_now = t_capture + vision_delay; + auto filtered_ball = ball_filter.estimateBallState(det, field.fieldBoundary(), t_now); + + + // THE ASSERTION: + // Without latency compensation (current code), the ball will be 17.5cm behind (5.0m/s * 0.035s). + // With your new Look-Ahead logic, the position should match the projected position. + Point expected_projected_pos = ball_start + ball_velocity * (9 * 0.016 + 0.035); + double error = (filtered_ball->position() - expected_projected_pos).length(); + EXPECT_LT(error, 0.05); // Expect error to be less than 5cm + } + +} diff --git a/src/software/simulation/er_force_simulator.cpp b/src/software/simulation/er_force_simulator.cpp index b58d4989a3..84ff63bba3 100644 --- a/src/software/simulation/er_force_simulator.cpp +++ b/src/software/simulation/er_force_simulator.cpp @@ -472,11 +472,13 @@ void ErForceSimulator::stepSimulation(const Duration& time_step) blue_robot_with_ball.reset(); yellow_robot_with_ball.reset(); + ball_is_visible_ = true; for (const auto& response : yellow_radio_responses) { if (response.has_ball_detected() && response.ball_detected()) { yellow_robot_with_ball = response.id(); + ball_is_visible_ = false; } } @@ -485,6 +487,7 @@ void ErForceSimulator::stepSimulation(const Duration& time_step) if (response.has_ball_detected() && response.ball_detected()) { blue_robot_with_ball = response.id(); + ball_is_visible_ = false; } } @@ -546,7 +549,8 @@ std::vector ErForceSimulator::getSSLWrapperPackets( world::SimulatorState ErForceSimulator::getSimulatorState() const { - return er_force_sim->getSimulatorState(); + auto state = er_force_sim->getSimulatorState(); + return state; } Field ErForceSimulator::getField() const @@ -564,6 +568,11 @@ void ErForceSimulator::resetCurrentTime() current_time = Timestamp::fromSeconds(0); } +bool ErForceSimulator::isBallVisible() const +{ + return ball_is_visible_; +} + std::map> ErForceSimulator::getRobotIdToLocalVelocityMap( const google::protobuf::RepeatedPtrField& sim_robots) diff --git a/src/software/simulation/er_force_simulator.h b/src/software/simulation/er_force_simulator.h index b4e7833012..9461d41f99 100644 --- a/src/software/simulation/er_force_simulator.h +++ b/src/software/simulation/er_force_simulator.h @@ -130,6 +130,11 @@ class ErForceSimulator */ void resetCurrentTime(); + /** + * Returns whether the ball is currently visible (not occluded by a robot) + */ + bool isBallVisible() const; + /** * Creates the default realism config using erforce simulator's default config * @return a pointer to default realism config @@ -229,7 +234,9 @@ class ErForceSimulator std::optional yellow_robot_with_ball; bool ramping; + bool ball_is_visible_ = true; const std::string CONFIG_FILE = "simulator/2020"; const std::string CONFIG_DIRECTORY = "extlibs/er_force_sim/config/"; + bool is_ball_visible; }; diff --git a/src/software/thunderscope/binary_context_managers/game_controller.py b/src/software/thunderscope/binary_context_managers/game_controller.py index c51962a677..e933877093 100644 --- a/src/software/thunderscope/binary_context_managers/game_controller.py +++ b/src/software/thunderscope/binary_context_managers/game_controller.py @@ -39,13 +39,17 @@ def __init__( self, suppress_logs: bool = False, use_conventional_port: bool = False, + should_automate_referee: bool = False, ) -> None: """Run Gamecontroller :param suppress_logs: Whether to suppress the logs :param use_conventional_port: whether or not to use the conventional port! + :param should_automate_referee: Whether to automate referee events (e.g. stage changes, goals). + Should only be True in record_stats mode. """ self.suppress_logs = suppress_logs + self.should_automate_referee = should_automate_referee # We default to using a non-conventional port to avoid emitting # on the same port as what other teams may be listening on. @@ -220,8 +224,8 @@ def handle_referee(self, referee: Referee) -> None: block=False, return_cached=True ) - # TODO (#3633): only automate referee events in record_stats mode - self.__automate_referee(referee) + if self.should_automate_referee: + self.__automate_referee(referee) max_allowed_bots_yellow: int = referee.yellow.max_allowed_bots max_allowed_bots_blue: int = referee.blue.max_allowed_bots @@ -503,6 +507,8 @@ def send_ci_input(self, ci_input: proto.ssl_gc_ci_pb2.CiInput) -> list[CiOutput] "error receiving CiOutput proto from the gamecontroller: " + parse_err.args ) + except OSError: + break return ci_output_list diff --git a/src/software/thunderscope/binary_context_managers/simulator.py b/src/software/thunderscope/binary_context_managers/simulator.py index 20cf4b6853..106b394611 100644 --- a/src/software/thunderscope/binary_context_managers/simulator.py +++ b/src/software/thunderscope/binary_context_managers/simulator.py @@ -134,6 +134,10 @@ def setup_proto_unix_io( WorldStateReceivedTrigger, ) + simulator_proto_unix_io.attach_unix_receiver( + self.simulator_runtime_dir, "/log", RobotLog + ) + # setup blue full system unix io for arg in [ (BLUE_WORLD_PATH, World), diff --git a/src/software/thunderscope/thunderscope_main.py b/src/software/thunderscope/thunderscope_main.py index 3bd6c9d68a..30ee0a6a29 100644 --- a/src/software/thunderscope/thunderscope_main.py +++ b/src/software/thunderscope/thunderscope_main.py @@ -472,6 +472,7 @@ def __ticker(tick_rate_ms: int) -> None: log_level=args.log_level, ) as yellow_fs, Gamecontroller( suppress_logs=(not args.verbose), + should_automate_referee=args.record_stats, ) as gamecontroller, ( # Here we only initialize autoref if the --enable_autoref flag is requested. # To avoid nested Python withs, the autoref is initialized as None when this flag doesn't exist. From d2c31536e12696f9b0b250247f1a3349d4e31a26 Mon Sep 17 00:00:00 2001 From: StarrryNight Date: Fri, 3 Apr 2026 20:24:09 -0700 Subject: [PATCH 03/57] add goalie_tactic_test evaluations --- .../stp/tactic/goalie/goalie_tactic_test.py | 48 ++++++++++++++----- src/software/er_force_simulator_main.cpp | 4 +- .../simulated_tests/simulated_test_fixture.py | 2 + 3 files changed, 39 insertions(+), 15 deletions(-) diff --git a/src/software/ai/hl/stp/tactic/goalie/goalie_tactic_test.py b/src/software/ai/hl/stp/tactic/goalie/goalie_tactic_test.py index bdcd66b291..077d9d3126 100644 --- a/src/software/ai/hl/stp/tactic/goalie/goalie_tactic_test.py +++ b/src/software/ai/hl/stp/tactic/goalie/goalie_tactic_test.py @@ -1,3 +1,4 @@ +import os import pytest import software.python_bindings as tbots_cpp @@ -146,22 +147,43 @@ def setup(*args): eventually_validation_sequence_set = [ [ - # Goalie should be in the defense area - RobotEventuallyEntersRegion( - regions=[ - tbots_cpp.Field.createSSLDivisionBField().friendlyDefenseArea() - ] - ), + FriendlyEventuallyHasBallPossession(), ] ] - simulated_test_runner.run_test( - setup=setup, - inv_eventually_validation_sequence_set=eventually_validation_sequence_set, - inv_always_validation_sequence_set=always_validation_sequence_set, - ag_eventually_validation_sequence_set=eventually_validation_sequence_set, - ag_always_validation_sequence_set=always_validation_sequence_set, - ) + _passed = True + try: + simulated_test_runner.run_test( + setup=setup, + inv_eventually_validation_sequence_set=eventually_validation_sequence_set, + inv_always_validation_sequence_set=always_validation_sequence_set, + ag_eventually_validation_sequence_set=eventually_validation_sequence_set, + ag_always_validation_sequence_set=always_validation_sequence_set, + ) + except AssertionError: + _passed = False + raise + finally: + csv_path = os.path.join( + simulated_test_runner.simulator_runtime_dir, "goalie_tactic_data.csv" + ) + if os.path.exists(csv_path): + failed_val = 0 if _passed else 1 + with open(csv_path, "r") as _f: + lines = [l.rstrip("\n") for l in _f.readlines() if l.strip()] + header = lines[0] if lines else "" + already_has_failed = "failed" in header.split(",") + complete_cols = len(header.split(",")) if already_has_failed else len(header.split(",")) + 1 + with open(csv_path, "w") as _f: + for i, line in enumerate(lines): + if len(line.split(",")) < complete_cols: + # Row from current run — not yet labeled + if i == 0: + _f.write(f"{line},failed\n") + else: + _f.write(f"{line},{failed_val}\n") + else: + _f.write(f"{line}\n") @pytest.mark.parametrize( diff --git a/src/software/er_force_simulator_main.cpp b/src/software/er_force_simulator_main.cpp index 8515bc32d4..f3ba1e65b6 100644 --- a/src/software/er_force_simulator_main.cpp +++ b/src/software/er_force_simulator_main.cpp @@ -48,7 +48,7 @@ int main(int argc, char **argv) { std::string runtime_dir = args.runtime_dir; LoggerSingleton::initializeLogger(runtime_dir, nullptr); - LOG(CSV, "Failed_goalie_tactic_data.csv") << "timestamp_s,fused_x,fused_y,truth_x,truth_y, true_vel_x, true_vel_y,is_occluded\n"; + LOG(CSV, "goalie_tactic_data.csv") << "timestamp_s,fused_x,fused_y,truth_x,truth_y, true_vel_x, true_vel_y,is_occluded\n"; /** * Creates a ER force simulator and sets up the appropriate @@ -222,7 +222,7 @@ int main(int argc, char **argv) } auto sim_state = er_force_sim->getSimulatorState(); - LOG(CSV, "Failed_goalie_tactic_data.csv") + LOG(CSV, "goalie_tactic_data.csv") << yellow_vision.time_sent().epoch_timestamp_seconds() << "," << yellow_vision.ball().current_state().global_position().x_meters() << "," diff --git a/src/software/simulated_tests/simulated_test_fixture.py b/src/software/simulated_tests/simulated_test_fixture.py index 25e92dce64..b585ae1ae3 100644 --- a/src/software/simulated_tests/simulated_test_fixture.py +++ b/src/software/simulated_tests/simulated_test_fixture.py @@ -604,4 +604,6 @@ def simulated_test_runner(): gamecontroller, ) + runner.simulator_runtime_dir = f"{args.simulator_runtime_dir}/test/{test_name}" + yield runner From a0186d681fb76813eb1ae0b6b641bd7542b1c36f Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Sat, 4 Apr 2026 16:23:13 -0700 Subject: [PATCH 04/57] Remove all c++ simulated tests --- src/software/ai/hl/stp/play/BUILD | 15 - .../ai/hl/stp/play/crease_defense/BUILD | 14 - .../crease_defense_play_test.cpp | 68 ---- src/software/ai/hl/stp/play/example/BUILD | 14 - .../hl/stp/play/example/example_play_test.cpp | 64 --- src/software/ai/hl/stp/play/free_kick/BUILD | 14 - .../play/free_kick/free_kick_play_test.cpp | 89 ----- .../ai/hl/stp/play/kickoff_enemy/BUILD | 17 - .../kickoff_enemy/kickoff_enemy_play_test.cpp | 63 --- .../ai/hl/stp/play/kickoff_friendly/BUILD | 16 - .../kickoff_friendly_play_test.cpp | 74 ---- .../ai/hl/stp/play/penalty_kick/BUILD | 16 - .../penalty_kick/penalty_kick_play_test.cpp | 113 ------ .../ai/hl/stp/play/penalty_kick_enemy/BUILD | 16 - .../penalty_kick_enemy_play_test.cpp | 156 -------- .../ai/hl/stp/play/shoot_or_chip/BUILD | 14 - .../shoot_or_chip/shoot_or_chip_play_test.cpp | 63 --- .../ai/hl/stp/play/shoot_or_pass/BUILD | 15 - .../shoot_or_pass/shoot_or_pass_play_test.cpp | 47 --- .../ai/hl/stp/play/stop_play_test.cpp | 180 --------- src/software/ai/hl/stp/tactic/attacker/BUILD | 44 --- .../attacker_tactic_keep_away_test.cpp | 241 ------------ .../attacker/attacker_tactic_passing_test.cpp | 141 ------- .../attacker_tactic_shoot_goal_test.cpp | 91 ----- src/software/ai/hl/stp/tactic/chip/BUILD | 13 - .../hl/stp/tactic/chip/chip_tactic_test.cpp | 82 ---- .../ai/hl/stp/tactic/crease_defender/BUILD | 16 - .../crease_defender_tactic_test.cpp | 169 -------- src/software/ai/hl/stp/tactic/dribble/BUILD | 32 -- .../dribble_tactic_push_enemy_test.cpp | 86 ---- .../tactic/dribble/dribble_tactic_test.cpp | 367 ------------------ src/software/ai/hl/stp/tactic/halt/BUILD | 13 - .../hl/stp/tactic/halt/halt_tactic_test.cpp | 70 ---- src/software/ai/hl/stp/tactic/kick/BUILD | 13 - .../hl/stp/tactic/kick/kick_tactic_test.cpp | 82 ---- src/software/ai/hl/stp/tactic/move/BUILD | 15 - .../hl/stp/tactic/move/move_tactic_test.cpp | 249 ------------ .../ai/hl/stp/tactic/penalty_kick/BUILD | 16 - .../penalty_kick/penalty_kick_tactic_test.cpp | 100 ----- .../ai/hl/stp/tactic/pivot_kick/BUILD | 15 - .../pivot_kick/pivot_kick_tactic_test.cpp | 91 ----- src/software/ai/hl/stp/tactic/receiver/BUILD | 15 - .../tactic/receiver/receiver_tactic_test.cpp | 228 ----------- src/software/sensor_fusion/filter/BUILD | 15 - .../filter/ball_occlusion_test.cpp | 119 ------ 45 files changed, 3391 deletions(-) delete mode 100644 src/software/ai/hl/stp/play/crease_defense/crease_defense_play_test.cpp delete mode 100644 src/software/ai/hl/stp/play/example/example_play_test.cpp delete mode 100644 src/software/ai/hl/stp/play/free_kick/free_kick_play_test.cpp delete mode 100644 src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play_test.cpp delete mode 100644 src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_test.cpp delete mode 100644 src/software/ai/hl/stp/play/penalty_kick/penalty_kick_play_test.cpp delete mode 100644 src/software/ai/hl/stp/play/penalty_kick_enemy/penalty_kick_enemy_play_test.cpp delete mode 100644 src/software/ai/hl/stp/play/shoot_or_chip/shoot_or_chip_play_test.cpp delete mode 100644 src/software/ai/hl/stp/play/shoot_or_pass/shoot_or_pass_play_test.cpp delete mode 100644 src/software/ai/hl/stp/play/stop_play_test.cpp delete mode 100644 src/software/ai/hl/stp/tactic/attacker/attacker_tactic_keep_away_test.cpp delete mode 100644 src/software/ai/hl/stp/tactic/attacker/attacker_tactic_passing_test.cpp delete mode 100644 src/software/ai/hl/stp/tactic/attacker/attacker_tactic_shoot_goal_test.cpp delete mode 100644 src/software/ai/hl/stp/tactic/chip/chip_tactic_test.cpp delete mode 100644 src/software/ai/hl/stp/tactic/crease_defender/crease_defender_tactic_test.cpp delete mode 100644 src/software/ai/hl/stp/tactic/dribble/dribble_tactic_push_enemy_test.cpp delete mode 100644 src/software/ai/hl/stp/tactic/dribble/dribble_tactic_test.cpp delete mode 100644 src/software/ai/hl/stp/tactic/halt/halt_tactic_test.cpp delete mode 100644 src/software/ai/hl/stp/tactic/kick/kick_tactic_test.cpp delete mode 100644 src/software/ai/hl/stp/tactic/move/move_tactic_test.cpp delete mode 100644 src/software/ai/hl/stp/tactic/penalty_kick/penalty_kick_tactic_test.cpp delete mode 100644 src/software/ai/hl/stp/tactic/pivot_kick/pivot_kick_tactic_test.cpp delete mode 100644 src/software/ai/hl/stp/tactic/receiver/receiver_tactic_test.cpp delete mode 100644 src/software/sensor_fusion/filter/ball_occlusion_test.cpp diff --git a/src/software/ai/hl/stp/play/BUILD b/src/software/ai/hl/stp/play/BUILD index 8cef5ab8d7..a74371ace5 100644 --- a/src/software/ai/hl/stp/play/BUILD +++ b/src/software/ai/hl/stp/play/BUILD @@ -87,21 +87,6 @@ py_test( ], ) -cc_test( - name = "stop_play_cpp_test", - srcs = ["stop_play_test.cpp"], - deps = [ - "//shared/test_util:tbots_gtest_main", - "//software/ai/hl/stp/play:stop_play", - "//software/simulated_tests:simulated_er_force_sim_play_test_fixture", - "//software/simulated_tests/cpp_validation:validation_function", - "//software/simulated_tests/non_terminating_validation_functions", - "//software/test_util", - "//software/time:duration", - "//software/world", - ], -) - py_test( name = "stop_play_test", srcs = [ diff --git a/src/software/ai/hl/stp/play/crease_defense/BUILD b/src/software/ai/hl/stp/play/crease_defense/BUILD index 30e0bd89b1..ca6980db81 100644 --- a/src/software/ai/hl/stp/play/crease_defense/BUILD +++ b/src/software/ai/hl/stp/play/crease_defense/BUILD @@ -44,20 +44,6 @@ py_test( ], ) -cc_test( - name = "crease_defense_play_cpp_test", - srcs = ["crease_defense_play_test.cpp"], - deps = [ - ":crease_defense_play", - "//shared/test_util:tbots_gtest_main", - "//software/simulated_tests:simulated_er_force_sim_play_test_fixture", - "//software/simulated_tests/cpp_validation:validation_function", - "//software/simulated_tests/non_terminating_validation_functions", - "//software/simulated_tests/terminating_validation_functions", - "//software/test_util", - ], -) - cc_test( name = "crease_defense_play_fsm_test", srcs = ["crease_defense_play_fsm_test.cpp"], diff --git a/src/software/ai/hl/stp/play/crease_defense/crease_defense_play_test.cpp b/src/software/ai/hl/stp/play/crease_defense/crease_defense_play_test.cpp deleted file mode 100644 index 09307e659f..0000000000 --- a/src/software/ai/hl/stp/play/crease_defense/crease_defense_play_test.cpp +++ /dev/null @@ -1,68 +0,0 @@ -#include "software/ai/hl/stp/play/crease_defense/crease_defense_play.h" - -#include - -#include "software/simulated_tests/cpp_validation/validation_function.h" -#include "software/simulated_tests/non_terminating_validation_functions/ball_in_play_or_scored_validation.h" -#include "software/simulated_tests/simulated_er_force_sim_play_test_fixture.h" -#include "software/simulated_tests/terminating_validation_functions/robot_halt_validation.h" -#include "software/simulated_tests/terminating_validation_functions/robot_in_polygon_validation.h" -#include "software/test_util/test_util.h" -#include "software/time/duration.h" -#include "software/world/world.h" - -class CreaseDefensePlayTest : public SimulatedErForceSimPlayTestFixture -{ - protected: - TbotsProto::FieldType field_type = TbotsProto::FieldType::DIV_B; - Field field = Field::createField(field_type); -}; - -TEST_F(CreaseDefensePlayTest, test_defense_play) -{ - BallState ball_state(Point(0.9, 2.85), Vector(0, 0)); - auto friendly_robots = TestUtil::createStationaryRobotStatesWithId( - {Point(-4.5, 0), Point(-3, 1.5), Point(-3, 0.5), Point(-3, -0.5), Point(-3, -1.5), - Point(-3, -3.0)}); - setFriendlyGoalie(0); - auto enemy_robots = TestUtil::createStationaryRobotStatesWithId({ - field.enemyGoalCenter(), - field.enemyDefenseArea().negXNegYCorner(), - field.enemyDefenseArea().negXPosYCorner(), - Point(1, 3), - Point(-1, -0.25), - Point(-2, -1.25), - }); - setEnemyGoalie(0); - - std::unique_ptr play = std::make_unique( - std::make_shared(getAiConfig())); - play->updateControlParams(Point(1, 3)); - - setAiPlay(std::move(play)); - - // We set the referee command to stop so that the robots do not kick/shoot during the - // test - setRefereeCommand(RefereeCommand::STOP, RefereeCommand::STOP); - - std::vector terminating_validation_functions = { - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) - { - // Wait for all robots to come to a halt - robotHalt(world_ptr, yield); - - // Two friendly crease defenders should be close to the goalie - Point goalie_position = world_ptr->friendlyTeam().goalie()->position(); - Rectangle crease_defender_rect( - Point(goalie_position.x(), goalie_position.y()), - Point(goalie_position.x() + 1.5, goalie_position.y() + 1.0)); - robotInPolygon(crease_defender_rect, 3, world_ptr, yield); - }}; - - std::vector non_terminating_validation_functions = { - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) {}}; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(10)); -} diff --git a/src/software/ai/hl/stp/play/example/BUILD b/src/software/ai/hl/stp/play/example/BUILD index 6e11182ac7..3ee72b418f 100644 --- a/src/software/ai/hl/stp/play/example/BUILD +++ b/src/software/ai/hl/stp/play/example/BUILD @@ -22,20 +22,6 @@ cc_library( alwayslink = True, ) -cc_test( - name = "example_play_test", - srcs = ["example_play_test.cpp"], - deps = [ - "//shared/test_util:tbots_gtest_main", - "//software/ai/hl/stp/play/example:example_play", - "//software/simulated_tests:simulated_er_force_sim_play_test_fixture", - "//software/simulated_tests/cpp_validation:validation_function", - "//software/test_util", - "//software/time:duration", - "//software/world", - ], -) - cc_test( name = "example_play_fsm_test", srcs = ["example_play_fsm_test.cpp"], diff --git a/src/software/ai/hl/stp/play/example/example_play_test.cpp b/src/software/ai/hl/stp/play/example/example_play_test.cpp deleted file mode 100644 index 3f4649f4b6..0000000000 --- a/src/software/ai/hl/stp/play/example/example_play_test.cpp +++ /dev/null @@ -1,64 +0,0 @@ -#include "software/ai/hl/stp/play/example/example_play.h" - -#include - -#include "software/simulated_tests/cpp_validation/validation_function.h" -#include "software/simulated_tests/simulated_er_force_sim_play_test_fixture.h" -#include "software/test_util/test_util.h" -#include "software/time/duration.h" -#include "software/world/world.h" - -class ExamplePlayTest : public SimulatedErForceSimPlayTestFixture -{ - protected: - TbotsProto::FieldType field_type = TbotsProto::FieldType::DIV_B; - Field field = Field::createField(field_type); -}; - -TEST_F(ExamplePlayTest, test_example_play) -{ - BallState ball_state(Point(-0.8, 0), Vector(0, 0)); - auto friendly_robots = TestUtil::createStationaryRobotStatesWithId( - {Point(4, 0), Point(0.5, 0), Point(-3, 1), Point(-1, -3), Point(2, 0), - Point(3.5, 3)}); - auto enemy_robots = TestUtil::createStationaryRobotStatesWithId({Point(4, 0)}); - setFriendlyGoalie(0); - setAiPlay(TbotsProto::PlayName::ExamplePlay); - - setRefereeCommand(RefereeCommand::FORCE_START, RefereeCommand::HALT); - - std::vector terminating_validation_functions = { - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) - { - auto friendly_robots_1_meter_from_ball = [](std::shared_ptr world_ptr) - { - Point ball_position = world_ptr->ball().position(); - for (const auto& robot : world_ptr->friendlyTeam().getAllRobots()) - { - // Skips the robot assigned as goalie - if (robot.id() == 0) - { - continue; - } - double abs_error = - std::fabs((robot.position() - ball_position).length() - 1.0); - if (abs_error > 0.05) - { - return false; - } - } - return true; - }; - - while (!friendly_robots_1_meter_from_ball(world_ptr)) - { - yield("Friendly robots not 1 meter away from ball"); - } - }}; - - std::vector non_terminating_validation_functions = {}; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(10)); -} diff --git a/src/software/ai/hl/stp/play/free_kick/BUILD b/src/software/ai/hl/stp/play/free_kick/BUILD index 80c9bc2114..bf0f2ee4df 100644 --- a/src/software/ai/hl/stp/play/free_kick/BUILD +++ b/src/software/ai/hl/stp/play/free_kick/BUILD @@ -34,20 +34,6 @@ cc_library( alwayslink = True, ) -cc_test( - name = "free_kick_play_cpp_test", - srcs = ["free_kick_play_test.cpp"], - deps = [ - "//shared/test_util:tbots_gtest_main", - "//software/ai/hl/stp/play/free_kick:free_kick_play", - "//software/simulated_tests:simulated_er_force_sim_play_test_fixture", - "//software/simulated_tests/cpp_validation:validation_function", - "//software/test_util", - "//software/time:duration", - "//software/world", - ], -) - py_test( name = "free_kick_play_test", srcs = ["free_kick_play_test.py"], diff --git a/src/software/ai/hl/stp/play/free_kick/free_kick_play_test.cpp b/src/software/ai/hl/stp/play/free_kick/free_kick_play_test.cpp deleted file mode 100644 index 218263bb99..0000000000 --- a/src/software/ai/hl/stp/play/free_kick/free_kick_play_test.cpp +++ /dev/null @@ -1,89 +0,0 @@ -#include "software/ai/hl/stp/play/free_kick/free_kick_play.h" - -#include - -#include "software/simulated_tests/cpp_validation/validation_function.h" -#include "software/simulated_tests/simulated_er_force_sim_play_test_fixture.h" -#include "software/test_util/test_util.h" -#include "software/time/duration.h" -#include "software/world/game_state.h" -#include "software/world/world.h" - -class FreeKickPlayTest : public SimulatedErForceSimPlayTestFixture -{ - protected: - TbotsProto::FieldType field_type = TbotsProto::FieldType::DIV_B; - Field field = Field::createField(field_type); -}; - -TEST_F(FreeKickPlayTest, test_free_kick_play_on_enemy_half) -{ - BallState ball_state(Point(1.5, -3), Vector(0, 0)); - auto friendly_robots = TestUtil::createStationaryRobotStatesWithId( - {Point(-4.5, 0), Point(-3, 1.5), Point(-3, 0.5), Point(-3, -0.5), Point(-3, -1.5), - Point(4, -2.5)}); - - setFriendlyGoalie(0); - auto enemy_robots = TestUtil::createStationaryRobotStatesWithId( - {Point(1, 0), Point(1, 2.5), Point(1, -2.5), field.enemyGoalCenter(), - field.enemyDefenseArea().negXNegYCorner(), - field.enemyDefenseArea().negXPosYCorner()}); - - setEnemyGoalie(0); - setAiPlay(TbotsProto::PlayName::FreeKickPlay); - setRefereeCommand(RefereeCommand::NORMAL_START, RefereeCommand::INDIRECT_FREE_US); - - std::vector terminating_validation_functions = { - // This will keep the test running for 9.5 seconds to give everything enough - // time to settle into position and be observed with the Visualizer - // TODO: Implement proper validation - // https://github.com/UBC-Thunderbots/Software/issues/1971 - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) - { - while (world_ptr->getMostRecentTimestamp() < Timestamp::fromSeconds(9.5)) - { - yield("Timestamp not at 9.5s"); - } - }}; - - std::vector non_terminating_validation_functions = {}; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(10)); -} - -TEST_F(FreeKickPlayTest, test_free_kick_play_on_friendly_half) -{ - BallState ball_state(Point(-1.5, -3), Vector(0, 0)); - auto friendly_robots = TestUtil::createStationaryRobotStatesWithId( - {Point(-4.5, 0), Point(-3, 1.5), Point(-3, 0.5), Point(-3, -0.5), Point(-3, -1.5), - Point(4.6, -3.1)}); - setFriendlyGoalie(0); - auto enemy_robots = TestUtil::createStationaryRobotStatesWithId( - {Point(1, 0), Point(1, 2.5), Point(1, -2.5), field.enemyGoalCenter(), - field.enemyDefenseArea().negXNegYCorner(), - field.enemyDefenseArea().negXPosYCorner()}); - setEnemyGoalie(0); - setAiPlay(TbotsProto::PlayName::FreeKickPlay); - setRefereeCommand(RefereeCommand::NORMAL_START, RefereeCommand::INDIRECT_FREE_US); - - std::vector terminating_validation_functions = { - // This will keep the test running for 9.5 seconds to give everything enough - // time to settle into position and be observed with the Visualizer - // TODO: Implement proper validation - // https://github.com/UBC-Thunderbots/Software/issues/1971 - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) - { - while (world_ptr->getMostRecentTimestamp() < Timestamp::fromSeconds(9.5)) - { - yield("Timestamp not at 9.5s"); - } - }}; - - std::vector non_terminating_validation_functions = {}; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(10)); -} diff --git a/src/software/ai/hl/stp/play/kickoff_enemy/BUILD b/src/software/ai/hl/stp/play/kickoff_enemy/BUILD index a04e868874..4ad24c7692 100644 --- a/src/software/ai/hl/stp/play/kickoff_enemy/BUILD +++ b/src/software/ai/hl/stp/play/kickoff_enemy/BUILD @@ -25,20 +25,3 @@ cc_library( ], alwayslink = True, ) - -cc_test( - name = "kickoff_enemy_play_cpp_test", - srcs = ["kickoff_enemy_play_test.cpp"], - deps = [ - "//shared/test_util:tbots_gtest_main", - "//software/ai/hl/stp/play/kickoff_enemy:kickoff_enemy_play", - "//software/geom/algorithms", - "//software/simulated_tests:simulated_er_force_sim_play_test_fixture", - "//software/simulated_tests/cpp_validation:validation_function", - "//software/simulated_tests/non_terminating_validation_functions", - "//software/simulated_tests/terminating_validation_functions", - "//software/test_util", - "//software/time:duration", - "//software/world", - ], -) diff --git a/src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play_test.cpp b/src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play_test.cpp deleted file mode 100644 index d01f13c114..0000000000 --- a/src/software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play_test.cpp +++ /dev/null @@ -1,63 +0,0 @@ -#include "software/ai/hl/stp/play/kickoff_enemy/kickoff_enemy_play.h" - -#include - -#include "software/geom/algorithms/calculate_block_cone.h" -#include "software/geom/algorithms/contains.h" -#include "software/simulated_tests/cpp_validation/validation_function.h" -#include "software/simulated_tests/non_terminating_validation_functions/robots_in_friendly_half_validation.h" -#include "software/simulated_tests/non_terminating_validation_functions/robots_not_in_center_circle_validation.h" -#include "software/simulated_tests/simulated_er_force_sim_play_test_fixture.h" -#include "software/simulated_tests/terminating_validation_functions/robot_in_polygon_validation.h" -#include "software/test_util/test_util.h" -#include "software/time/duration.h" -#include "software/world/world.h" - -class KickoffEnemyPlayTest : public SimulatedErForceSimPlayTestFixture -{ - protected: - TbotsProto::FieldType field_type = TbotsProto::FieldType::DIV_B; - Field field = Field::createField(field_type); -}; - -// TODO (#3105): Re-enable test once destinations are moved outside of obstacles -TEST_F(KickoffEnemyPlayTest, DISABLED_test_kickoff_enemy_play) -{ - BallState ball_state(Point(0, 0), Vector(0, 0)); - auto friendly_robots = TestUtil::createStationaryRobotStatesWithId( - {Point(-3, 2.5), Point(-3, 1.5), Point(-3, 0.5), Point(-3, -0.5), Point(-3, -1.5), - Point(-3, -2.5)}); - setFriendlyGoalie(0); - auto enemy_robots = TestUtil::createStationaryRobotStatesWithId( - {Point(1, 0), Point(1, 2.5), Point(1, -2.5), field.enemyGoalCenter(), - field.enemyDefenseArea().negXNegYCorner(), - field.enemyDefenseArea().negXPosYCorner()}); - setEnemyGoalie(0); - setAiPlay(TbotsProto::PlayName::KickoffEnemyPlay); - setRefereeCommand(RefereeCommand::NORMAL_START, RefereeCommand::PREPARE_KICKOFF_THEM); - - std::vector terminating_validation_functions = { - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) - { - // Two friendly robots in position to shadow enemy robots. Rectangles are - // chosen to be generally in the way of the the front 3 enemy robots and the - // friendly goal, based on where the enemy robots are initialized in the test. - Rectangle shadowing_rect_1(Point(0, 1.5), Point(-0.4, 1.3)); - Rectangle shadowing_rect_2(Point(0, -1.5), Point(-0.4, -1.3)); - Rectangle shadowing_rect_3(Point(-0.60, 0.1), Point(-0.86, -0.1)); - robotInPolygon(shadowing_rect_1, 1, world_ptr, yield); - robotInPolygon(shadowing_rect_2, 1, world_ptr, yield); - robotInPolygon(shadowing_rect_3, 1, world_ptr, yield); - - // Two Friendly robots defending the exterior of defense box - Rectangle robotsDefendingRect(Point(-3.2, 1.1), Point(-3.5, -1.1)); - robotInPolygon(robotsDefendingRect, 2, world_ptr, yield); - }}; - - std::vector non_terminating_validation_functions = { - robotsInFriendlyHalf, robotsNotInCenterCircle}; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(10)); -} diff --git a/src/software/ai/hl/stp/play/kickoff_friendly/BUILD b/src/software/ai/hl/stp/play/kickoff_friendly/BUILD index 17e661499b..69591295b0 100644 --- a/src/software/ai/hl/stp/play/kickoff_friendly/BUILD +++ b/src/software/ai/hl/stp/play/kickoff_friendly/BUILD @@ -24,19 +24,3 @@ cc_library( ], alwayslink = True, ) - -cc_test( - name = "kickoff_friendly_play_cpp_test", - srcs = ["kickoff_friendly_play_test.cpp"], - deps = [ - "//shared/test_util:tbots_gtest_main", - "//software/ai/hl/stp/play/kickoff_friendly:kickoff_friendly_play", - "//software/simulated_tests:simulated_er_force_sim_play_test_fixture", - "//software/simulated_tests/cpp_validation:validation_function", - "//software/simulated_tests/non_terminating_validation_functions", - "//software/simulated_tests/terminating_validation_functions", - "//software/test_util", - "//software/time:duration", - "//software/world", - ], -) diff --git a/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_test.cpp b/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_test.cpp deleted file mode 100644 index 9eede5ac3a..0000000000 --- a/src/software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play_test.cpp +++ /dev/null @@ -1,74 +0,0 @@ -#include "software/ai/hl/stp/play/kickoff_friendly/kickoff_friendly_play.h" - -#include - -#include "software/simulated_tests/cpp_validation/validation_function.h" -#include "software/simulated_tests/non_terminating_validation_functions/robots_in_friendly_half_validation.h" -#include "software/simulated_tests/non_terminating_validation_functions/robots_not_in_center_circle_validation.h" -#include "software/simulated_tests/simulated_er_force_sim_play_test_fixture.h" -#include "software/simulated_tests/terminating_validation_functions/ball_kicked_validation.h" -#include "software/simulated_tests/terminating_validation_functions/robot_in_center_circle_validation.h" -#include "software/simulated_tests/terminating_validation_functions/robot_in_polygon_validation.h" -#include "software/simulated_tests/terminating_validation_functions/robot_received_ball_validation.h" -#include "software/test_util/test_util.h" -#include "software/time/duration.h" -#include "software/world/world.h" - -class KickoffFriendlyPlayTest : public SimulatedErForceSimPlayTestFixture -{ - protected: - TbotsProto::FieldType field_type = TbotsProto::FieldType::DIV_B; - Field field = Field::createField(field_type); -}; - -// TODO (#2608): re-enable when fixed -TEST_F(KickoffFriendlyPlayTest, DISABLED_test_kickoff_friendly_play) -{ - BallState ball_state(Point(0, 0), Vector(0, 0)); - auto friendly_robots = TestUtil::createStationaryRobotStatesWithId( - {Point(-3, 2.5), Point(-3, 1.5), Point(-3, 0.5), Point(-3, -0.5), Point(-3, -1.5), - Point(-3, -2.5)}); - setFriendlyGoalie(0); - auto enemy_robots = TestUtil::createStationaryRobotStatesWithId( - {Point(1, 0), Point(1, 2.5), Point(1, -2.5), field.enemyGoalCenter(), - field.enemyDefenseArea().negXNegYCorner(), - field.enemyDefenseArea().negXPosYCorner()}); - setEnemyGoalie(0); - setAiPlay(TbotsProto::PlayName::KickoffEnemyPlay); - setRefereeCommand(RefereeCommand::NORMAL_START, RefereeCommand::PREPARE_KICKOFF_US); - - std::vector terminating_validation_functions = { - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) - { - // Robot 4 is the only robot allowed to be in the center circle and start - // the kickoff - robotInCenterCircle(world_ptr, yield); - robotReceivedBall(world_ptr, yield); - ballKicked(Angle::zero(), world_ptr, yield); - - // Two friendly robots near the half line setting up for offense - Rectangle robotsOffensiveRect(Point(-0.5, 2.5), Point(-1.5, -2.5)); - robotInPolygon(robotsOffensiveRect, 2, world_ptr, yield); - - - // Two Friendly robots defending the exterior of defense box and one goalie - Rectangle robotsDefensiveRect(Point(-3.2, 1.1), Point(-3.51, -1.1)); - robotInPolygon(robotsDefensiveRect, 3, world_ptr, yield); - }}; - - std::vector non_terminating_validation_functions = { - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) - { - for (RobotId robot_id : {0, 1, 2, 3, 5}) - { - { - robotInFriendlyHalf(robot_id, world_ptr, yield); - robotNotInCenterCircle(robot_id, world_ptr, yield); - } - } - }}; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(10)); -} diff --git a/src/software/ai/hl/stp/play/penalty_kick/BUILD b/src/software/ai/hl/stp/play/penalty_kick/BUILD index 009e8e23c9..751b635868 100644 --- a/src/software/ai/hl/stp/play/penalty_kick/BUILD +++ b/src/software/ai/hl/stp/play/penalty_kick/BUILD @@ -36,19 +36,3 @@ cc_test( "//software/test_util", ], ) - -cc_test( - name = "penalty_kick_play_test", - srcs = ["penalty_kick_play_test.cpp"], - deps = [ - ":penalty_kick_play", - "//shared/test_util:tbots_gtest_main", - "//software/simulated_tests:simulated_er_force_sim_play_test_fixture", - "//software/simulated_tests/cpp_validation:validation_function", - "//software/simulated_tests/non_terminating_validation_functions", - "//software/simulated_tests/terminating_validation_functions", - "//software/test_util", - "//software/time:duration", - "//software/world", - ], -) diff --git a/src/software/ai/hl/stp/play/penalty_kick/penalty_kick_play_test.cpp b/src/software/ai/hl/stp/play/penalty_kick/penalty_kick_play_test.cpp deleted file mode 100644 index 1b39b77061..0000000000 --- a/src/software/ai/hl/stp/play/penalty_kick/penalty_kick_play_test.cpp +++ /dev/null @@ -1,113 +0,0 @@ -#include "software/ai/hl/stp/play/penalty_kick/penalty_kick_play.h" - -#include - -#include "software/simulated_tests/cpp_validation/validation_function.h" -#include "software/simulated_tests/non_terminating_validation_functions/ball_in_play_or_scored_validation.h" -#include "software/simulated_tests/non_terminating_validation_functions/ball_never_moves_backward_validation.h" -#include "software/simulated_tests/non_terminating_validation_functions/robot_not_excessively_dribbling_validation.h" -#include "software/simulated_tests/non_terminating_validation_functions/robots_avoid_ball_validation.h" -#include "software/simulated_tests/simulated_er_force_sim_play_test_fixture.h" -#include "software/simulated_tests/terminating_validation_functions/friendly_scored_validation.h" -#include "software/simulated_tests/terminating_validation_functions/robot_state_validation.h" -#include "software/test_util/equal_within_tolerance.h" -#include "software/test_util/test_util.h" -#include "software/time/duration.h" -#include "software/world/world.h" - -class PenaltyKickPlayTest : public SimulatedErForceSimPlayTestFixture -{ - protected: - TbotsProto::FieldType field_type = TbotsProto::FieldType::DIV_B; - Field field = Field::createField(field_type); -}; - -TEST_F(PenaltyKickPlayTest, test_penalty_kick_setup) -{ - BallState ball_state(field.friendlyPenaltyMark(), Vector(0, 0)); - auto friendly_robots = TestUtil::createStationaryRobotStatesWithId( - {Point(-2, -2), Point(-3, -1), Point(-3, 0), Point(-3, 1), Point(-3, 2), - Point(2, 2.5)}); - setFriendlyGoalie(0); - auto enemy_robots = - TestUtil::createStationaryRobotStatesWithId({field.enemyGoalCenter()}); - setEnemyGoalie(0); - setAiPlay(TbotsProto::PlayName::PenaltyKickPlay); - setRefereeCommand(RefereeCommand::PREPARE_PENALTY_US, RefereeCommand::NORMAL_START); - - RobotId shooter_id = 5; - std::vector terminating_validation_functions = { - [shooter_id](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) - { - robotAtPosition(shooter_id, world_ptr, - world_ptr->field().friendlyPenaltyMark(), 0.3, yield); - }}; - - std::vector non_terminating_validation_functions = { - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) - { - // making sure that the robot doesn't move the ball while the penalty is - // setting up - ASSERT_TRUE( - TestUtil::equalWithinTolerance(world_ptr->field().friendlyPenaltyMark(), - world_ptr->ball().position(), 1e-6)); - }, - [shooter_id](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) - { - // Wait 2 seconds for robots to start moving adequately far away from the ball - if (world_ptr->getMostRecentTimestamp() >= Timestamp::fromSeconds(2)) - { - robotsAvoidBall(1, {shooter_id}, world_ptr, yield); - } - }}; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(9.5)); -} - -// TODO (#3106): Re-enable test once robot 1 and 2 tactic assignment do not oscillate -TEST_F(PenaltyKickPlayTest, DISABLED_test_penalty_kick_take) -{ - Vector behind_ball_direction = - (field.friendlyPenaltyMark() - field.enemyGoalCenter()).normalize(); - - Point behind_ball = field.friendlyPenaltyMark() + - behind_ball_direction.normalize(DIST_TO_FRONT_OF_ROBOT_METERS + - BALL_MAX_RADIUS_METERS + 0.1); - double non_shooter_x_pos = field.friendlyPenaltyMark().x() - 1.5; - BallState ball_state(field.friendlyPenaltyMark(), Vector(0, 0)); - auto friendly_robots = TestUtil::createStationaryRobotStatesWithId( - {Point(-4, 0), behind_ball, Point(non_shooter_x_pos, 0), - Point(non_shooter_x_pos, 1), Point(non_shooter_x_pos, -1), - Point(non_shooter_x_pos, 2)}); - setFriendlyGoalie(0); - Point goalie = Point(field.enemyGoalpostNeg().x(), 0); - auto enemy_robots = TestUtil::createStationaryRobotStatesWithId({goalie}); - setEnemyGoalie(0); - setAiPlay(TbotsProto::PlayName::PenaltyKickPlay); - setRefereeCommand(RefereeCommand::NORMAL_START, RefereeCommand::PREPARE_PENALTY_US); - - std::vector terminating_validation_functions = { - friendlyScored, - }; - - RobotId shooter_id = 1; - std::vector non_terminating_validation_functions = { - ballInPlay, - [shooter_id](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) - { ballNeverMovesBackward(world_ptr, yield); }, - [shooter_id](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) - { robotNotExcessivelyDribbling(shooter_id, world_ptr, yield); }, - [shooter_id](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) - { robotsAvoidBall(1, {shooter_id}, world_ptr, yield); }}; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(10)); -} diff --git a/src/software/ai/hl/stp/play/penalty_kick_enemy/BUILD b/src/software/ai/hl/stp/play/penalty_kick_enemy/BUILD index c8e773a6d8..4f56f4ede0 100644 --- a/src/software/ai/hl/stp/play/penalty_kick_enemy/BUILD +++ b/src/software/ai/hl/stp/play/penalty_kick_enemy/BUILD @@ -31,19 +31,3 @@ cc_test( "//software/test_util", ], ) - -cc_test( - name = "penalty_kick_enemy_play_test", - srcs = ["penalty_kick_enemy_play_test.cpp"], - deps = [ - ":penalty_kick_enemy_play", - "//shared/test_util:tbots_gtest_main", - "//software/simulated_tests:simulated_er_force_sim_play_test_fixture", - "//software/simulated_tests/cpp_validation:validation_function", - "//software/simulated_tests/non_terminating_validation_functions", - "//software/simulated_tests/terminating_validation_functions", - "//software/test_util", - "//software/time:duration", - "//software/world", - ], -) diff --git a/src/software/ai/hl/stp/play/penalty_kick_enemy/penalty_kick_enemy_play_test.cpp b/src/software/ai/hl/stp/play/penalty_kick_enemy/penalty_kick_enemy_play_test.cpp deleted file mode 100644 index abed199951..0000000000 --- a/src/software/ai/hl/stp/play/penalty_kick_enemy/penalty_kick_enemy_play_test.cpp +++ /dev/null @@ -1,156 +0,0 @@ -#include "software/ai/hl/stp/play/penalty_kick_enemy/penalty_kick_enemy_play.h" - -#include - -#include "software/simulated_tests/cpp_validation/validation_function.h" -#include "software/simulated_tests/non_terminating_validation_functions/enemy_never_scores_validation.h" -#include "software/simulated_tests/simulated_er_force_sim_play_test_fixture.h" -#include "software/simulated_tests/terminating_validation_functions/robot_in_polygon_validation.h" -#include "software/simulated_tests/terminating_validation_functions/robot_state_validation.h" -#include "software/test_util/test_util.h" -#include "software/time/duration.h" -#include "software/world/world.h" - -/** - * Simulated tests for adhering to penalty kick rules as specified in link: - * https://robocup-ssl.github.io/ssl-rules/sslrules.html#_penalty_kick - * */ -class PenaltyKickEnemyPlayTest - : public SimulatedErForceSimPlayTestFixture, - public ::testing::WithParamInterface, float>> -{ - protected: - TbotsProto::FieldType field_type = TbotsProto::FieldType::DIV_B; - Field field = Field::createField(field_type); -}; - -TEST_P(PenaltyKickEnemyPlayTest, test_penalty_kick_enemy_play_setup) -{ - RefereeCommand current_command = std::get<0>(GetParam()); - RefereeCommand previous_command = std::get<1>(GetParam()); - BallState ball_state(field.enemyPenaltyMark(), Vector(0, 0)); - - std::vector friendly_robots = std::get<2>(GetParam()); - float enemy_distance_behind_ball = std::get<3>(GetParam()); - - // enemy robots behind the penalty mark - auto enemy_robots = TestUtil::createStationaryRobotStatesWithId({ - Point(field.enemyPenaltyMark().x() + 0.3, 0), // kicker robot - Point(field.enemyPenaltyMark().x() + enemy_distance_behind_ball, 0), - Point(field.enemyPenaltyMark().x() + enemy_distance_behind_ball, - 8 * ROBOT_MAX_RADIUS_METERS), - Point(field.enemyPenaltyMark().x() + enemy_distance_behind_ball, - 16 * ROBOT_MAX_RADIUS_METERS), - Point(field.enemyPenaltyMark().x() + enemy_distance_behind_ball, - -8 * ROBOT_MAX_RADIUS_METERS), - Point(field.enemyPenaltyMark().x() + enemy_distance_behind_ball, - -16 * ROBOT_MAX_RADIUS_METERS), - }); - setFriendlyGoalie(0); - setEnemyGoalie(0); - setAiPlay(TbotsProto::PlayName::PenaltyKickEnemyPlay); - setRefereeCommand(current_command, previous_command); - Rectangle behind_ball_region = - Rectangle(Point(field.enemyPenaltyMark().x() + 1, field.enemyCornerPos().y()), - field.enemyCornerNeg()); - - std::vector terminating_validation_functions = { - [behind_ball_region](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) - { - robotAtOrientation(0, world_ptr, Angle::zero(), Angle::fromDegrees(5), yield); - robotAtPosition(0, world_ptr, world_ptr->field().friendlyGoalCenter(), 0.05, - yield); - robotInPolygon(behind_ball_region, 5, world_ptr, yield); - }}; - - std::vector non_terminating_validation_functions = {}; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(20)); -} - -INSTANTIATE_TEST_CASE_P( - RobotPositions, PenaltyKickEnemyPlayTest, - ::testing::Values( - std::make_tuple(RefereeCommand::PREPARE_PENALTY_THEM, RefereeCommand::HALT, - TestUtil::createStationaryRobotStatesWithId( - {Point(1, 2), Point(-1, -2), Point(-2.5, 3), Point(2, -1), - Point(0, 3), Point(3, 0)}), - 1), - std::make_tuple(RefereeCommand::PREPARE_PENALTY_THEM, RefereeCommand::HALT, - TestUtil::createStationaryRobotStatesWithId( - {Point(2.2, 1.2), Point(-0.5, -2.1), Point(-2.5, 1.3), - Point(1.2, -1.5), Point(0, 2), Point(1, 0)}), - 1.3), - std::make_tuple(RefereeCommand::PREPARE_PENALTY_THEM, RefereeCommand::HALT, - TestUtil::createStationaryRobotStatesWithId( - {Point(2.2, 1.2), Point(-0.5, -2.1), Point(-2.5, 1.3), - Point(1.2, -1.5), Point(0, 2), Point(1, 0)}), - 1.4), - std::make_tuple(RefereeCommand::PREPARE_PENALTY_THEM, RefereeCommand::HALT, - TestUtil::createStationaryRobotStatesWithId( - {Point(2.2, 1.2), Point(-0.5, -2.1), Point(-2.5, 1.3), - Point(1.2, -1.5), Point(0, 2), Point(1, 0)}), - 1.45), - std::make_tuple(RefereeCommand::PREPARE_PENALTY_THEM, RefereeCommand::HALT, - TestUtil::createStationaryRobotStatesWithId( - {Point(2.2, 1.2), Point(-0.5, -2.1), Point(-2.5, 1.3), - Point(1.2, -1.5), Point(0, 2), Point(1, 0)}), - 1.5), - std::make_tuple(RefereeCommand::PREPARE_PENALTY_THEM, RefereeCommand::HALT, - TestUtil::createStationaryRobotStatesWithId( - {Point(2.2, 1.2), Point(-0.5, -2.1), Point(-2.5, 1.3), - Point(1.2, -1.5), Point(0, 2), Point(1, 0)}), - 1.6))); - -TEST_F(PenaltyKickEnemyPlayTest, test_penalty_kick_enemy_play_goalie) -{ - BallState ball_state(field.enemyPenaltyMark(), Vector(-3, 0.2)); - - // friendly robots already in position - auto friendly_robots = TestUtil::createStationaryRobotStatesWithId( - {field.friendlyGoalCenter(), Point(field.enemyPenaltyMark().x() + 1.5, 0), - Point(field.enemyPenaltyMark().x() + 1.5, 8 * ROBOT_MAX_RADIUS_METERS), - Point(field.enemyPenaltyMark().x() + 1.5, -8 * ROBOT_MAX_RADIUS_METERS), - Point(field.enemyPenaltyMark().x() + 1.5, 16 * ROBOT_MAX_RADIUS_METERS), - Point(field.enemyPenaltyMark().x() + 1.5, -16 * ROBOT_MAX_RADIUS_METERS)}); - - // enemy robots behind the penalty mark - auto enemy_robots = TestUtil::createStationaryRobotStatesWithId({ - Point(field.enemyPenaltyMark().x() + 0.3, 0), - Point(field.enemyPenaltyMark().x() + 1, 0), - Point(field.enemyPenaltyMark().x() + 1, 8 * ROBOT_MAX_RADIUS_METERS), - Point(field.enemyPenaltyMark().x() + 1, 16 * ROBOT_MAX_RADIUS_METERS), - Point(field.enemyPenaltyMark().x() + 1, -8 * ROBOT_MAX_RADIUS_METERS), - Point(field.enemyPenaltyMark().x() + 1, -16 * ROBOT_MAX_RADIUS_METERS), - }); - setFriendlyGoalie(0); - setEnemyGoalie(0); - setAiPlay(TbotsProto::PlayName::PenaltyKickEnemyPlay); - setRefereeCommand(RefereeCommand::NORMAL_START, RefereeCommand::PREPARE_PENALTY_THEM); - GameState gameState = GameState(); - gameState.updateRefereeCommand(RefereeCommand::FORCE_START); - setGameState(gameState); - - std::vector terminating_validation_functions = { - // This will keep the test running for 10 seconds to give everything enough - // time to settle into position and be observed with the Visualizer - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) - { - while (world_ptr->getMostRecentTimestamp() < Timestamp::fromSeconds(10)) - { - yield("simulated penalty kick goalie test not finished!"); - } - }}; - - std::vector non_terminating_validation_functions = { - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) - { enemyNeverScores(world_ptr, yield); }}; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(20)); -} diff --git a/src/software/ai/hl/stp/play/shoot_or_chip/BUILD b/src/software/ai/hl/stp/play/shoot_or_chip/BUILD index 51099d98c2..2d74dd1993 100644 --- a/src/software/ai/hl/stp/play/shoot_or_chip/BUILD +++ b/src/software/ai/hl/stp/play/shoot_or_chip/BUILD @@ -31,20 +31,6 @@ cc_library( alwayslink = True, ) -cc_test( - name = "shoot_or_chip_play_cpp_test", - srcs = ["shoot_or_chip_play_test.cpp"], - deps = [ - ":shoot_or_chip_play", - "//shared/test_util:tbots_gtest_main", - "//software/simulated_tests:simulated_er_force_sim_play_test_fixture", - "//software/simulated_tests/cpp_validation:validation_function", - "//software/test_util", - "//software/time:duration", - "//software/world", - ], -) - py_test( name = "shoot_or_chip_play_test", srcs = [ diff --git a/src/software/ai/hl/stp/play/shoot_or_chip/shoot_or_chip_play_test.cpp b/src/software/ai/hl/stp/play/shoot_or_chip/shoot_or_chip_play_test.cpp deleted file mode 100644 index eee62e0479..0000000000 --- a/src/software/ai/hl/stp/play/shoot_or_chip/shoot_or_chip_play_test.cpp +++ /dev/null @@ -1,63 +0,0 @@ -#include "software/ai/hl/stp/play/shoot_or_chip/shoot_or_chip_play.h" - -#include - -#include "software/simulated_tests/cpp_validation/validation_function.h" -#include "software/simulated_tests/simulated_er_force_sim_play_test_fixture.h" -#include "software/test_util/test_util.h" -#include "software/time/duration.h" -#include "software/world/world.h" - -class ShootOrChipPlayTest : public SimulatedErForceSimPlayTestFixture -{ - protected: - TbotsProto::FieldType field_type = TbotsProto::FieldType::DIV_B; - Field field = Field::createField(field_type); -}; - -TEST_F(ShootOrChipPlayTest, test_shoot_or_chip_play) -{ - BallState ball_state(Point(-1.4, 2), Vector(0, 0)); - auto friendly_robots = TestUtil::createStationaryRobotStatesWithId({ - field.friendlyGoalCenter(), - Point(-1.5, 2), - Point(-2, 1.5), - Point(-2, 0.5), - Point(-2, -0.5), - Point(-2, -1.5), - }); - setFriendlyGoalie(0); - auto enemy_robots = TestUtil::createStationaryRobotStatesWithId({ - field.enemyGoalCenter(), - field.enemyDefenseArea().negXNegYCorner(), - field.enemyDefenseArea().negXPosYCorner(), - Point(-1, 0), - Point(1, -2.5), - }); - enemy_robots.emplace_back(RobotStateWithId{ - .id = 5, - .robot_state = RobotState(Point(1, 2), Vector(-4.6, 0), Angle::half(), - AngularVelocity::zero())}); - setEnemyGoalie(0); - setAiPlay(TbotsProto::PlayName::ShootOrChipPlay); - setRefereeCommand(RefereeCommand::FORCE_START, RefereeCommand::STOP); - - std::vector terminating_validation_functions = { - // This will keep the test running for 9.5 seconds to give everything enough - // time to settle into position and be observed with the Visualizer - // TODO: Implement proper validation - // https://github.com/UBC-Thunderbots/Software/issues/1971 - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) - { - while (world_ptr->getMostRecentTimestamp() < Timestamp::fromSeconds(9.5)) - { - yield("Timestamp not at 9.5s"); - } - }}; - - std::vector non_terminating_validation_functions = {}; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(10)); -} diff --git a/src/software/ai/hl/stp/play/shoot_or_pass/BUILD b/src/software/ai/hl/stp/play/shoot_or_pass/BUILD index 988830fbca..440454a35f 100644 --- a/src/software/ai/hl/stp/play/shoot_or_pass/BUILD +++ b/src/software/ai/hl/stp/play/shoot_or_pass/BUILD @@ -44,18 +44,3 @@ cc_test( "//software/test_util", ], ) - -cc_test( - name = "shoot_or_pass_play_test", - srcs = ["shoot_or_pass_play_test.cpp"], - deps = [ - ":shoot_or_pass_play", - "//shared/test_util:tbots_gtest_main", - "//software/simulated_tests:simulated_er_force_sim_play_test_fixture", - "//software/simulated_tests/cpp_validation:validation_function", - "//software/simulated_tests/terminating_validation_functions", - "//software/test_util", - "//software/time:duration", - "//software/world", - ], -) diff --git a/src/software/ai/hl/stp/play/shoot_or_pass/shoot_or_pass_play_test.cpp b/src/software/ai/hl/stp/play/shoot_or_pass/shoot_or_pass_play_test.cpp deleted file mode 100644 index c546df40aa..0000000000 --- a/src/software/ai/hl/stp/play/shoot_or_pass/shoot_or_pass_play_test.cpp +++ /dev/null @@ -1,47 +0,0 @@ -#include "software/ai/hl/stp/play/shoot_or_pass/shoot_or_pass_play.h" - -#include - -#include "software/simulated_tests/cpp_validation/validation_function.h" -#include "software/simulated_tests/simulated_er_force_sim_play_test_fixture.h" -#include "software/simulated_tests/terminating_validation_functions/friendly_scored_validation.h" -#include "software/test_util/test_util.h" -#include "software/time/duration.h" -#include "software/world/world.h" - -class ShootOrPassPlayTest : public SimulatedErForceSimPlayTestFixture -{ - protected: - TbotsProto::FieldType field_type = TbotsProto::FieldType::DIV_B; - Field field = Field::createField(field_type); -}; - -// TODO (#3233): The attacker robot sometimes doesn't kick the ball towards the receiver -TEST_F(ShootOrPassPlayTest, DISABLED_test_shoot_or_pass_play) -{ - BallState ball_state(Point(-4.4, 2.9), Vector(0, 0)); - auto friendly_robots = TestUtil::createStationaryRobotStatesWithId({ - field.friendlyGoalCenter(), - Point(-4.5, 3.0), - Point(-2, 1.5), - Point(-2, 0.5), - Point(-2, -1.7), - Point(-2, -1.5), - }); - setFriendlyGoalie(0); - auto enemy_robots = TestUtil::createStationaryRobotStatesWithId( - {Point(1, 0), Point(1, 2.5), Point(1, -2.5), field.enemyGoalCenter(), - field.enemyDefenseArea().negXNegYCorner(), - field.enemyDefenseArea().negXPosYCorner()}); - setEnemyGoalie(0); - setAiPlay(TbotsProto::PlayName::ShootOrPassPlay); - setRefereeCommand(RefereeCommand::FORCE_START, RefereeCommand::STOP); - - std::vector terminating_validation_functions = {friendlyScored}; - - std::vector non_terminating_validation_functions = {}; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(15)); -} diff --git a/src/software/ai/hl/stp/play/stop_play_test.cpp b/src/software/ai/hl/stp/play/stop_play_test.cpp deleted file mode 100644 index 9a14b15678..0000000000 --- a/src/software/ai/hl/stp/play/stop_play_test.cpp +++ /dev/null @@ -1,180 +0,0 @@ -#include "software/ai/hl/stp/play/stop_play.h" - -#include - -#include "software/simulated_tests/cpp_validation/validation_function.h" -#include "software/simulated_tests/non_terminating_validation_functions/robots_avoid_ball_validation.h" -#include "software/simulated_tests/non_terminating_validation_functions/robots_slow_down_validation.h" -#include "software/simulated_tests/simulated_er_force_sim_play_test_fixture.h" -#include "software/test_util/test_util.h" -#include "software/time/duration.h" -#include "software/world/world.h" - -class StopPlayTest : public SimulatedErForceSimPlayTestFixture -{ - protected: - StopPlayTest() : stop_play_rules(initStopPlayRules()) {} - - std::vector stop_play_rules; - - - std::vector initStopPlayRules() - { - return { - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) - { - // Wait 2 seconds for robots that start too close to the ball to move away - if (world_ptr->getMostRecentTimestamp() >= Timestamp::fromSeconds(8)) - { - robotsSlowDown(1.5, world_ptr, yield); - robotsAvoidBall(0.5, {}, world_ptr, yield); - } - }}; - } - - void SetUp() override - { - SimulatedErForceSimPlayTestFixture::SetUp(); - setFriendlyGoalie(0); - setEnemyGoalie(0); - setAiPlay(TbotsProto::PlayName::StopPlay); - setRefereeCommand(RefereeCommand::STOP, RefereeCommand::STOP); - } - TbotsProto::FieldType field_type = TbotsProto::FieldType::DIV_B; - Field field = Field::createField(field_type); - std::vector enemy_robots = - TestUtil::createStationaryRobotStatesWithId( - {Point(1, 0), Point(1, 2.5), Point(1, -2.5), field.enemyGoalCenter(), - field.enemyDefenseArea().negXNegYCorner(), - field.enemyDefenseArea().negXPosYCorner()}); -}; - -TEST_F(StopPlayTest, test_stop_play_ball_at_centre_robots_spread_out) -{ - BallState ball_state(Point(0, 0), Vector(0, 0)); - auto friendly_robots = TestUtil::createStationaryRobotStatesWithId( - {Point(-4, 0), Point(-0.3, 0), Point(0.3, 0), Point(0, 0.3), Point(-3, -1.5), - Point(4.6, -3.1)}); - - std::vector terminating_validation_functions = {}; - std::vector non_terminating_validation_functions = - stop_play_rules; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(10)); -} - -TEST_F(StopPlayTest, test_stop_play_friendly_half_robots_spread_out) -{ - BallState ball_state(Point(-1, 0), Vector(0, 0)); - auto friendly_robots = TestUtil::createStationaryRobotStatesWithId( - {Point(-4, 0), Point(-2.3, 0), Point(-1.7, 0), Point(-2, 0.3), Point(-3, -1.5), - Point(4.6, -3.1)}); - - std::vector terminating_validation_functions = {}; - std::vector non_terminating_validation_functions = - stop_play_rules; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(10)); -} - -TEST_F(StopPlayTest, test_stop_play_friendly_half_corner_robots_close_together) -{ - BallState ball_state(Point(-4, -2.5), Vector(0, 0)); - auto friendly_robots = TestUtil::createStationaryRobotStatesWithId( - {Point(-3, -2.5), Point(-4, -2), Point(-2, -2.5), Point(-3, -2), Point(-3.5, -2), - Point(-3, -1)}); - - std::vector terminating_validation_functions = {}; - std::vector non_terminating_validation_functions = - stop_play_rules; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(10)); -} - -TEST_F(StopPlayTest, test_stop_play_enemy_half_robots_spread_out) -{ - BallState ball_state(Point(2, 0), Vector(0, 0)); - auto friendly_robots = TestUtil::createStationaryRobotStatesWithId( - {Point(-4, 0), Point(1.7, 0), Point(2.3, 0), Point(2, 0.3), Point(-3, -1.5), - Point(3, -3)}); - - std::vector terminating_validation_functions = {}; - std::vector non_terminating_validation_functions = - stop_play_rules; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(10)); -} - -TEST_F(StopPlayTest, test_stop_play_enemy_half_corner_robots_close_together) -{ - BallState ball_state(Point(4, -2.5), Vector(0, 0)); - auto friendly_robots = TestUtil::createStationaryRobotStatesWithId( - {Point(2, -2.5), Point(4, -1), Point(3, -2.5), Point(3, -2), Point(3.5, -2), - Point(3, -1)}); - - std::vector terminating_validation_functions = {}; - std::vector non_terminating_validation_functions = - stop_play_rules; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(10)); -} - -TEST_F(StopPlayTest, test_stop_play_centre_robots_close_together) -{ - BallState ball_state(Point(0, 0), Vector(0, 0)); - auto friendly_robots = TestUtil::createStationaryRobotStatesWithId( - {Point(-2, 0), Point(0, 0.3), Point(0.3, 0), Point(0, -0.3), Point(-0.3, 0), - Point(0.2, 0.2)}); - - std::vector terminating_validation_functions = {}; - std::vector non_terminating_validation_functions = - stop_play_rules; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(10)); -} - -TEST_F(StopPlayTest, test_stop_play_ball_in_front_of_enemy_defense_area) -{ - BallState ball_state(Point(3, 0), Vector(0, 0)); - auto friendly_robots = TestUtil::createStationaryRobotStatesWithId( - {Point(-4.5, 2), Point(0, 0.3), Point(0.3, 0), Point(0, -0.3), Point(-0.3, 0), - Point(0.2, 0.2)}); - - std::vector terminating_validation_functions = {}; - std::vector non_terminating_validation_functions = - stop_play_rules; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(10)); -} - -// TODO (#2519): The ball is not an obstacle for robots running crease defense tactic, -// as a result, they collide with the ball and the test fails -TEST_F(StopPlayTest, DISABLED_test_stop_play_ball_in_front_of_friendly_defense_area) -{ - BallState ball_state(Point(-3, 0), Vector(0, 0)); - auto friendly_robots = TestUtil::createStationaryRobotStatesWithId( - {Point(-4.5, 2), Point(0, 3), Point(-1, -1), Point(0, 0), Point(-1, 0), - Point(2, 2)}); - - std::vector terminating_validation_functions = {}; - std::vector non_terminating_validation_functions = - stop_play_rules; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(10)); -} diff --git a/src/software/ai/hl/stp/tactic/attacker/BUILD b/src/software/ai/hl/stp/tactic/attacker/BUILD index ce0cdbf432..e7336ee148 100644 --- a/src/software/ai/hl/stp/tactic/attacker/BUILD +++ b/src/software/ai/hl/stp/tactic/attacker/BUILD @@ -32,47 +32,3 @@ cc_test( "//software/test_util", ], ) - -cc_test( - name = "attacker_tactic_passing_test", - srcs = ["attacker_tactic_passing_test.cpp"], - deps = [ - ":attacker_tactic", - "//shared/test_util:tbots_gtest_main", - "//software/simulated_tests:simulated_er_force_sim_play_test_fixture", - "//software/simulated_tests/cpp_validation:validation_function", - "//software/simulated_tests/terminating_validation_functions", - "//software/test_util", - "//software/time:duration", - "//software/world", - ], -) - -cc_test( - name = "attacker_tactic_shoot_goal_test", - srcs = ["attacker_tactic_shoot_goal_test.cpp"], - deps = [ - ":attacker_tactic", - "//shared/test_util:tbots_gtest_main", - "//software/simulated_tests:simulated_er_force_sim_play_test_fixture", - "//software/simulated_tests/cpp_validation:validation_function", - "//software/simulated_tests/terminating_validation_functions", - "//software/test_util", - "//software/time:duration", - "//software/world", - ], -) - -cc_test( - name = "attacker_tactic_keep_away_test", - srcs = ["attacker_tactic_keep_away_test.cpp"], - deps = [ - ":attacker_tactic", - "//shared/test_util:tbots_gtest_main", - "//software/simulated_tests:simulated_er_force_sim_play_test_fixture", - "//software/simulated_tests/non_terminating_validation_functions", - "//software/test_util", - "//software/time:duration", - "//software/world", - ], -) diff --git a/src/software/ai/hl/stp/tactic/attacker/attacker_tactic_keep_away_test.cpp b/src/software/ai/hl/stp/tactic/attacker/attacker_tactic_keep_away_test.cpp deleted file mode 100644 index 03cc79bfe4..0000000000 --- a/src/software/ai/hl/stp/tactic/attacker/attacker_tactic_keep_away_test.cpp +++ /dev/null @@ -1,241 +0,0 @@ -#include - -#include -#include - -#include "software/ai/hl/stp/tactic/attacker/attacker_tactic.h" -#include "software/ai/hl/stp/tactic/move/move_tactic.h" -#include "software/ai/passing/cost_function.h" -#include "software/simulated_tests/cpp_validation/validation_function.h" -#include "software/simulated_tests/non_terminating_validation_functions/robot_not_excessively_dribbling_validation.h" -#include "software/simulated_tests/simulated_er_force_sim_play_test_fixture.h" -#include "software/test_util/test_util.h" -#include "software/time/duration.h" -#include "software/world/world.h" - -class AttackerTacticKeepAwayTest - : public SimulatedErForceSimPlayTestFixture, - public ::testing::WithParamInterface, bool>> -{ - protected: - TbotsProto::FieldType field_type = TbotsProto::FieldType::DIV_B; - Field field = Field::createField(field_type); -}; - -TEST_P(AttackerTacticKeepAwayTest, attacker_test_keep_away) -{ - Pass pass = std::get<0>(GetParam()); - RobotStateWithId robot_state = std::get<1>(GetParam()); - BallState ball_state = std::get<2>(GetParam()); - auto enemy_robots = std::get<3>(GetParam()); - bool ignore_score_checks = std::get<4>(GetParam()); - - auto friendly_robots = TestUtil::createStationaryRobotStatesWithId({Point(-3, 2.5)}); - friendly_robots.emplace_back(robot_state); - - TbotsProto::AiConfig ai_config; - - // force passing for this test by setting min acceptable shot angle very high - ai_config.mutable_attacker_tactic_config()->set_min_open_angle_for_shot_deg(90); - ai_config.mutable_attacker_tactic_config()->set_enemy_about_to_steal_ball_radius( - 0.01); - - auto tactic = std::make_shared( - std::make_shared(ai_config)); - - // force the keep away state - tactic->updateControlParams(pass, false); - setTactic(1, tactic); - - // we use default parameters for testing ratePassEnemyRisk because that's (partially) - // what the play uses to determine pass/no pass. Keep away state should try to - // push the best pass in the play above the threshold to commit to passing. - auto passing_config = TbotsProto::PassingConfig(); - - // we have to create a Team for the enemy here to evaluate the initial enemy risk - // score - std::vector enemy_team_robots; - std::transform(enemy_robots.begin(), enemy_robots.end(), - std::back_inserter(enemy_team_robots), - [](const RobotStateWithId& robot_state) { - return Robot(robot_state.id, robot_state.robot_state, - Timestamp::fromSeconds(0)); - }); - Team enemy_team(enemy_team_robots); - - static const auto CHECK_SCORE_INTERVAL = Duration::fromSeconds(0.5); - static constexpr auto NUM_CHECKS = 5; - - std::vector non_terminating_validation_functions = { - // test the proximity risk every CHECK_SCORE_INTERVAL time and make sure - // it doesn't get substantially worse compared to the last check - // and that it is an improvement compared to the starting state - [&](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) - { - while (ignore_score_checks) - { - yield(""); - } - - auto initial_enemy_proximity_risk = calculateProximityRisk( - world_ptr->ball().position(), world_ptr->enemyTeam(), passing_config); - - while (world_ptr->getMostRecentTimestamp() < Timestamp::fromSeconds(1)) - { - yield(""); - } - - auto last_timestamp = world_ptr->getMostRecentTimestamp(); - - // test runs for NUM_CHECKS * CHECK_SCORE_INTERVAL seconds - for (int i = 0; i < NUM_CHECKS; i++) - { - // only check the enemy risk score every 0.5s to mitigate the "noise" - // inherent in the ratePass______ functions - while (world_ptr->getMostRecentTimestamp() - last_timestamp < - CHECK_SCORE_INTERVAL) - { - yield(""); - } - last_timestamp = world_ptr->getMostRecentTimestamp(); - - auto current_enemy_proximity_risk = calculateProximityRisk( - world_ptr->ball().position(), world_ptr->enemyTeam(), passing_config); - - // make sure we improved over the initial proximity risk score - if (current_enemy_proximity_risk > initial_enemy_proximity_risk + 0.05) - { - std::stringstream ss; - ss << "At " << last_timestamp - << " calculateProximityRisk didn't improve over initial! Went from " - << initial_enemy_proximity_risk << " to " - << current_enemy_proximity_risk; - yield(ss.str()); - } - } - }, - // test the ratePassEnemyRisk every CHECK_SCORE_INTERVAL time and make sure - // it doesn't get substantially worse compared to the last check - // and that it is an improvement compared to the starting state - [&](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) - { - while (ignore_score_checks) - { - yield(""); - } - - auto initial_enemy_risk_score = - ratePassEnemyRisk(enemy_team, pass, passing_config); - - while (world_ptr->getMostRecentTimestamp() < Timestamp::fromSeconds(1)) - { - yield(""); - } - - auto last_timestamp = world_ptr->getMostRecentTimestamp(); - - // test runs for NUM_CHECKS * CHECK_SCORE_INTERVAL seconds - for (int i = 0; i < NUM_CHECKS; i++) - { - // only check the enemy risk score every 0.5s to mitigate the "noise" - // inherent in the ratePass______ functions - while (world_ptr->getMostRecentTimestamp() - last_timestamp < - CHECK_SCORE_INTERVAL) - { - yield(""); - } - last_timestamp = world_ptr->getMostRecentTimestamp(); - - // update the pass to reflect the new passer point, now that - // the ball has (probably) moved - Pass new_pass(world_ptr->ball().position(), pass.receiverPoint(), - pass.speed()); - - auto current_enemy_risk_score = - ratePassEnemyRisk(enemy_team, new_pass, passing_config); - - // make sure we improved over the initial enemy risk score - if (current_enemy_risk_score < (initial_enemy_risk_score - 0.05)) - { - std::stringstream ss; - ss << "At " << last_timestamp - << " ratePassEnemyRisk didn't improve over initial! Went from " - << initial_enemy_risk_score << " to " << current_enemy_risk_score; - yield(ss.str()); - } - } - }, - [&](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) - { robotNotExcessivelyDribbling(1, world_ptr, yield); }, - [&](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) - { - // test that the ball is always in the field boundaries - if (!contains(world_ptr->field().fieldLines(), world_ptr->ball().position())) - { - yield("Ball left the field boundaries!"); - } - else - { - yield(""); - } - }}; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, {}, - non_terminating_validation_functions, Duration::fromSeconds(3.0)); -} - -auto FIELD_TOP_LEFT = Field::createSSLDivisionBField().fieldLines().negXPosYCorner(); - -INSTANTIATE_TEST_CASE_P( - PassEnvironment, AttackerTacticKeepAwayTest, - ::testing::Values( - std::make_tuple( - // the best pass so far to pass into the AttackerTactic - Pass(Point(-0.2, 0.0), Point(-3, 2.5), 5), - // the state of the friendly robot - RobotStateWithId{ - 1, RobotState(Point(0.25, 0), Vector(0, 0), Angle::fromDegrees(180), - Angle::fromDegrees(0))}, - // the state of the ball - BallState(Point(0., 0.), Vector(0, 0)), - // the states of the enemy robots - TestUtil::createStationaryRobotStatesWithId( - {Point(-0.6, 0.25), Point(0., 0.6), Point(-0.25, 0.5), - Point(0.6, -0.25)}), - // whether to ignore the intercept and proximity risk checks in the test - // This test is too unstable, so we ignore the checks - true), - - // TODO(#2909): Enable test once the robot can turn faster and hits the ball with - // std::make_tuple( - // // the best pass so far to pass into the AttackerTactic - // Pass(Point(0.0, 0.0), Point(-3, 2.5), 5), - // // the state of the friendly robot - // RobotStateWithId{1, RobotState(Point(0.25, 0), Vector(0, 0), - // Angle::fromDegrees(0), - // Angle::fromDegrees(0))}, - // // the state of the ball - // BallState(Point(0., 0.), Vector(0, 0)), - // // the states of the enemy robots - // TestUtil::createStationaryRobotStatesWithId({Point(-0.5, 0.5)}), - // // whether to ignore the intercept and proximity risk checks in the test - // false), - - std::make_tuple( - // the best pass so far to pass into the AttackerTactic - Pass(Point(FIELD_TOP_LEFT.x() + 0.05, FIELD_TOP_LEFT.y() - 0.05), Point(0, 0), - 5), - // the state of the friendly robot - RobotStateWithId{1, RobotState(FIELD_TOP_LEFT, Vector(0, 0), - Angle::fromDegrees(0), Angle::fromDegrees(0))}, - // the state of the ball - BallState(Point(FIELD_TOP_LEFT.x() + 0.05, FIELD_TOP_LEFT.y() - 0.2), - Vector(0, 0)), - // the states of the enemy robots - TestUtil::createStationaryRobotStatesWithId( - {Point(-4, 2), Point(-4, 2.25), Point(-4, 2.5), Point(-4, 2.75)}), - // whether to ignore the intercept and proximity risk checks in the test - // we ignore the score checks on this one because we need to make sure that we - // stay in field bounds, even if leaving the field bounds improves the score - true))); diff --git a/src/software/ai/hl/stp/tactic/attacker/attacker_tactic_passing_test.cpp b/src/software/ai/hl/stp/tactic/attacker/attacker_tactic_passing_test.cpp deleted file mode 100644 index 02315a93b4..0000000000 --- a/src/software/ai/hl/stp/tactic/attacker/attacker_tactic_passing_test.cpp +++ /dev/null @@ -1,141 +0,0 @@ -#include - -#include - -#include "software/ai/hl/stp/tactic/attacker/attacker_tactic.h" -#include "software/ai/hl/stp/tactic/move/move_tactic.h" -#include "software/geom/algorithms/contains.h" -#include "software/simulated_tests/cpp_validation/validation_function.h" -#include "software/simulated_tests/simulated_er_force_sim_play_test_fixture.h" -#include "software/simulated_tests/terminating_validation_functions/ball_kicked_validation.h" -#include "software/simulated_tests/terminating_validation_functions/robot_state_validation.h" -#include "software/test_util/test_util.h" -#include "software/time/duration.h" -#include "software/world/world.h" - -class AttackerTacticKeepAwayTest - : public SimulatedErForceSimPlayTestFixture, - public ::testing::WithParamInterface> -{ - protected: - TbotsProto::FieldType field_type = TbotsProto::FieldType::DIV_B; - Field field = Field::createField(field_type); -}; - -TEST_P(AttackerTacticKeepAwayTest, attacker_test_passing) -{ - Pass pass = std::get<0>(GetParam()); - RobotStateWithId robot_state = std::get<1>(GetParam()); - BallState ball_state = std::get<2>(GetParam()); - - auto friendly_robots = TestUtil::createStationaryRobotStatesWithId({Point(-3, 2.5)}); - friendly_robots.emplace_back(robot_state); - auto enemy_robots = TestUtil::createStationaryRobotStatesWithId({Point(4, 0)}); - - TbotsProto::AiConfig ai_config; - // force passing for this test by setting min acceptable shot angle very high - ai_config.mutable_attacker_tactic_config()->set_min_open_angle_for_shot_deg(90); - - auto tactic = std::make_shared( - std::make_shared(ai_config)); - tactic->updateControlParams(pass, true); - setTactic(1, tactic); - - std::vector terminating_validation_functions = { - [pass, tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) - { - // We check if the robot reaches the desired orientation, at the - // desired position before checking if the ball has been kicked. - // - // The tactic should "done" after kicking the ball. - robotAtOrientation(1, world_ptr, pass.passerOrientation(), - Angle::fromDegrees(5), yield); - robotAtPosition(1, world_ptr, pass.passerPoint(), 0.1, yield); - ballKicked(pass.passerOrientation(), world_ptr, yield); - - while (!tactic->done()) - { - yield("Attacker tactic kicked ball but is not done"); - } - }}; - - std::vector non_terminating_validation_functions = {}; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(7)); -} - -INSTANTIATE_TEST_CASE_P( - PassEnvironment, AttackerTacticKeepAwayTest, - ::testing::Values( - // Stationary Ball Tests - // Attacker point != Balls location & Balls location != Robots Location - std::make_tuple(Pass(Point(0.0, 0.5), Point(0, 0), 5), - RobotStateWithId{ - 1, RobotState(Point(0, 0), Vector(0, 0), - Angle::fromDegrees(0), Angle::fromDegrees(0))}, - BallState(Point(0.5, 0.5), Vector(0, 0))), - - // Attacker point == Balls location & Balls location != Robots Location - std::make_tuple(Pass(Point(-0.5, -0.5), Point(0, 0), 5), - RobotStateWithId{ - 1, RobotState(Point(0, 0), Vector(0, 0), - Angle::fromDegrees(0), Angle::fromDegrees(0))}, - BallState(Point(-0.5, -0.5), Vector(0, 0))), - - // Attacker point != Balls location & Balls location == Robots Location - std::make_tuple(Pass(Point(0.5, 0.5), Point(0, 1), 5), - RobotStateWithId{ - 1, RobotState(Point(0.5, 0.5), Vector(0, 0), - Angle::fromDegrees(0), Angle::fromDegrees(0))}, - BallState(Point(-0.5, 0.5), Vector(0, 0))), - - // TODO(#2909) : Enable test once the robot can turn faster and hits the ball with - // Attacker point == Balls location & Balls location == Robots Location - // std::make_tuple(Pass(Point(0.0, 0.0), Point(0, 0), 5), - // RobotStateWithId{ - // 1, RobotState(Point(0, 0), Vector(0, 0), - // Angle::fromDegrees(0), - // Angle::fromDegrees(0))}, - // BallState(Point(0.0, 0.0), Vector(0, 0))), - - // Attacker point far away (not a normal use case, but just to sanity check) - std::make_tuple(Pass(Point(0.0, 0.0), Point(0, 0), 5), - RobotStateWithId{ - 1, RobotState(Point(3.5, 2.5), Vector(0, 0), - Angle::fromDegrees(0), Angle::fromDegrees(0))}, - BallState(Point(0.0, 0.0), Vector(0, 0))), - - // Attacker point != Balls location & Balls location != Robots Location - std::make_tuple(Pass(Point(0.0, 0.5), Point(0, 0), 5), - RobotStateWithId{ - 1, RobotState(Point(0, 0), Vector(0, 0), - Angle::fromDegrees(0), Angle::fromDegrees(0))}, - BallState(Point(0.5, 0.5), Vector(0, 0))), - - // Moving Ball Tests - // Attacker point == Balls location & Balls location != Robots Location - std::make_tuple(Pass(Point(-0.5, -0.5), Point(0, 0), 5), - RobotStateWithId{ - 1, RobotState(Point(0, 0), Vector(0, 0), - Angle::fromDegrees(0), Angle::fromDegrees(0))}, - BallState(Point(-0.5, -0.5), Vector(1, 0))), - - // TODO (#2859): Robot does not kick ball when dribbler is off since it is - // too far away. - // Attacker point != Balls location & Balls location == Robots Location - // std::make_tuple(Pass(Point(0.4, 0.4), Point(0, 1), 5), - // RobotStateWithId{ - // 1, RobotState(Point(0.5, 0.5), Vector(0, 0), - // Angle::fromDegrees(0), - // Angle::fromDegrees(0))}, - // BallState(Point(-0.4, 0.4), Vector(0, 1))), - - // Attacker point == Balls location & Balls location == Robots Location - std::make_tuple(Pass(Point(0.0, 0.0), Point(0, 0), 5), - RobotStateWithId{ - 1, RobotState(Point(0, 0), Vector(0, 0), - Angle::fromDegrees(0), Angle::fromDegrees(0))}, - BallState(Point(0.0, 0.0), Vector(1, 0))))); diff --git a/src/software/ai/hl/stp/tactic/attacker/attacker_tactic_shoot_goal_test.cpp b/src/software/ai/hl/stp/tactic/attacker/attacker_tactic_shoot_goal_test.cpp deleted file mode 100644 index 0e14f1a76a..0000000000 --- a/src/software/ai/hl/stp/tactic/attacker/attacker_tactic_shoot_goal_test.cpp +++ /dev/null @@ -1,91 +0,0 @@ -#include - -#include - -#include "software/ai/hl/stp/tactic/attacker/attacker_tactic.h" -#include "software/ai/hl/stp/tactic/move/move_tactic.h" -#include "software/geom/algorithms/contains.h" -#include "software/simulated_tests/cpp_validation/validation_function.h" -#include "software/simulated_tests/simulated_er_force_sim_play_test_fixture.h" -#include "software/simulated_tests/terminating_validation_functions/ball_kicked_validation.h" -#include "software/simulated_tests/terminating_validation_functions/friendly_scored_validation.h" -#include "software/simulated_tests/terminating_validation_functions/robot_state_validation.h" -#include "software/test_util/test_util.h" -#include "software/time/duration.h" -#include "software/world/world.h" - -class AttackerTacticShootGoalTest - : public SimulatedErForceSimPlayTestFixture, - // Params: initial ball state, initial robot position, enemy team, expected - // chip/kick direction - public ::testing::WithParamInterface< - std::tuple>> -{ - protected: - AttackerTacticShootGoalTest() - { - ai_config.mutable_passing_config()->set_pass_delay_sec(0.0); - } - - TbotsProto::AiConfig ai_config; - TbotsProto::FieldType field_type = TbotsProto::FieldType::DIV_B; - Field field = Field::createField(field_type); -}; - -TEST_P(AttackerTacticShootGoalTest, attacker_test_shoot_goal) -{ - BallState ball_state = std::get<0>(GetParam()); - Point initial_robot_point = std::get<1>(GetParam()); - auto enemy_robots = std::get<2>(GetParam()); - - auto friendly_robots = - TestUtil::createStationaryRobotStatesWithId({initial_robot_point}); - auto tactic = std::make_shared( - std::make_shared(ai_config)); - // Make it very obvious when we decide to chip - tactic->updateControlParams(Point(0, field.fieldLines().yMin())); - setTactic(0, tactic, {TbotsProto::MotionConstraint::FRIENDLY_DEFENSE_AREA}); - - std::vector terminating_validation_functions = { - [tactic](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) - { - while (!tactic->done()) - { - yield("Tactic not done"); - } - }, - friendlyScored}; - std::vector non_terminating_validation_functions = {}; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(9)); -} - -INSTANTIATE_TEST_CASE_P( - ShootGoalEnvironment, AttackerTacticShootGoalTest, - ::testing::Values( - // TODO (#2693): Fix Dribble FSM looping endlessly - // enemy goal blocked by enemy robots with enemy threat right - // std::make_tuple(BallState(Point(2, 1), Vector()), Point(1, 1), - // TestUtil::createStationaryRobotStatesWithId( - // {Point(2.4, 1), Point(3, 0.4), Point(3, 0.8), Point(3.1, - // 0.6), - // Point(3.1, 1), Point(4.2, 1.2)})), - // small opening in enemy formation - std::make_tuple(BallState(Point(2, 1), Vector()), Point(1, 1), - TestUtil::createStationaryRobotStatesWithId( - {Point(1, 0), Point(3, 0.2), Point(3, 0.8), Point(3.1, 0), - Point(3.1, 1), Point(4.2, 1.2)})), - // extreme angle shot - std::make_tuple(BallState(Point(3.8, -1.9), Vector()), Point(1, 1), - TestUtil::createStationaryRobotStatesWithId( - {Point(1, 0), Point(3, 1.2), Point(3, 0.8), Point(3.1, 0.6), - Point(3.1, 1), Point(4.2, 0.5)})), - // enemy trying to steal - std::make_tuple(BallState(Point(2.5, -1), Vector()), Point(1, 1), - TestUtil::createStationaryRobotStatesWithId( - {Point(2.5, -1.4), Point(3, 0.4), Point(3, 0.8), - Point(3.1, 0.6), Point(3.1, 1), Point(4.2, 1.2)})) - - )); diff --git a/src/software/ai/hl/stp/tactic/chip/BUILD b/src/software/ai/hl/stp/tactic/chip/BUILD index 250e5a2b09..8836dc2162 100644 --- a/src/software/ai/hl/stp/tactic/chip/BUILD +++ b/src/software/ai/hl/stp/tactic/chip/BUILD @@ -28,16 +28,3 @@ cc_test( "//software/test_util", ], ) - -cc_test( - name = "chip_tactic_test", - srcs = ["chip_tactic_test.cpp"], - deps = [ - ":chip_tactic", - "//shared/test_util:tbots_gtest_main", - "//software/simulated_tests:simulated_er_force_sim_play_test_fixture", - "//software/simulated_tests/cpp_validation:validation_function", - "//software/simulated_tests/terminating_validation_functions", - "//software/test_util", - ], -) diff --git a/src/software/ai/hl/stp/tactic/chip/chip_tactic_test.cpp b/src/software/ai/hl/stp/tactic/chip/chip_tactic_test.cpp deleted file mode 100644 index 6714284c41..0000000000 --- a/src/software/ai/hl/stp/tactic/chip/chip_tactic_test.cpp +++ /dev/null @@ -1,82 +0,0 @@ -#include "software/ai/hl/stp/tactic/chip/chip_tactic.h" - -#include - -#include - -#include "software/geom/algorithms/contains.h" -#include "software/simulated_tests/cpp_validation/validation_function.h" -#include "software/simulated_tests/simulated_er_force_sim_play_test_fixture.h" -#include "software/simulated_tests/terminating_validation_functions/ball_kicked_validation.h" -#include "software/simulated_tests/terminating_validation_functions/robot_state_validation.h" -#include "software/test_util/test_util.h" -#include "software/time/duration.h" -#include "software/world/world.h" - -class ChipTacticTest : public SimulatedErForceSimPlayTestFixture, - public ::testing::WithParamInterface> -{ - protected: - TbotsProto::FieldType field_type = TbotsProto::FieldType::DIV_B; - Field field = Field::createField(field_type); -}; - -TEST_P(ChipTacticTest, chip_test) -{ - Vector ball_offset_from_robot = std::get<0>(GetParam()); - Angle angle_to_kick_at = std::get<1>(GetParam()); - - Point robot_position = Point(0, 0); - BallState ball_state(robot_position + ball_offset_from_robot, Vector(0, 0)); - - auto friendly_robots = - TestUtil::createStationaryRobotStatesWithId({Point(-3, 2.5), robot_position}); - auto enemy_robots = TestUtil::createStationaryRobotStatesWithId({Point(4, 0)}); - - auto tactic = std::make_shared(std::make_shared()); - tactic->updateControlParams(robot_position + ball_offset_from_robot, angle_to_kick_at, - 5); - setTactic(1, tactic); - - std::vector terminating_validation_functions = { - [angle_to_kick_at, tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) - { - while (!tactic->done()) - { - yield("Tactic did not complete!"); - } - ballKicked(angle_to_kick_at, world_ptr, yield); - }}; - - std::vector non_terminating_validation_functions = {}; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(5)); -} - -INSTANTIATE_TEST_CASE_P( - BallLocations, ChipTacticTest, - ::testing::Values( - // place the ball directly to the left of the robot - std::make_tuple(Vector(0, 0.5), Angle::zero()), - // place the ball directly to the right of the robot - std::make_tuple(Vector(0, -0.5), Angle::zero()), - // place the ball directly infront of the robot - std::make_tuple(Vector(0.5, 0), Angle::zero()), - // place the ball directly behind the robot - std::make_tuple(Vector(-0.5, 0), Angle::zero()), - // place the ball in the robots dribbler - std::make_tuple(Vector(ROBOT_MAX_RADIUS_METERS, 0), Angle::zero()), - // Repeat the same tests but kick in the opposite direction - // place the ball directly to the left of the robot - std::make_tuple(Vector(0, 0.5), Angle::half()), - // place the ball directly to the right of the robot - std::make_tuple(Vector(0, -0.5), Angle::half()), - // place the ball directly infront of the robot - std::make_tuple(Vector(0.5, 0), Angle::half()), - // place the ball directly behind the robot - std::make_tuple(Vector(-0.5, 0), Angle::half()), - // place the ball in the robots dribbler - std::make_tuple(Vector(ROBOT_MAX_RADIUS_METERS, 0), Angle::zero()))); diff --git a/src/software/ai/hl/stp/tactic/crease_defender/BUILD b/src/software/ai/hl/stp/tactic/crease_defender/BUILD index a2923032c3..58467ae6a5 100644 --- a/src/software/ai/hl/stp/tactic/crease_defender/BUILD +++ b/src/software/ai/hl/stp/tactic/crease_defender/BUILD @@ -35,22 +35,6 @@ cc_test( ], ) -cc_test( - name = "crease_defender_tactic_cpp_test", - srcs = ["crease_defender_tactic_test.cpp"], - deps = [ - ":crease_defender_tactic", - "//shared/test_util:tbots_gtest_main", - "//software/simulated_tests:simulated_er_force_sim_play_test_fixture", - "//software/simulated_tests/cpp_validation:validation_function", - "//software/simulated_tests/non_terminating_validation_functions", - "//software/simulated_tests/terminating_validation_functions", - "//software/test_util", - "//software/time:duration", - "//software/world", - ], -) - py_test( name = "crease_defender_tactic_test", srcs = ["crease_defender_tactic_test.py"], diff --git a/src/software/ai/hl/stp/tactic/crease_defender/crease_defender_tactic_test.cpp b/src/software/ai/hl/stp/tactic/crease_defender/crease_defender_tactic_test.cpp deleted file mode 100644 index 2c20098b8d..0000000000 --- a/src/software/ai/hl/stp/tactic/crease_defender/crease_defender_tactic_test.cpp +++ /dev/null @@ -1,169 +0,0 @@ -#include "software/ai/hl/stp/tactic/crease_defender/crease_defender_tactic.h" - -#include - -#include - -#include "software/geom/algorithms/contains.h" -#include "software/simulated_tests/cpp_validation/validation_function.h" -#include "software/simulated_tests/non_terminating_validation_functions/robots_avoid_ball_validation.h" -#include "software/simulated_tests/simulated_er_force_sim_play_test_fixture.h" -#include "software/simulated_tests/terminating_validation_functions/ball_kicked_validation.h" -#include "software/simulated_tests/terminating_validation_functions/robot_in_polygon_validation.h" -#include "software/simulated_tests/terminating_validation_functions/robot_state_validation.h" -#include "software/test_util/test_util.h" -#include "software/time/duration.h" -#include "software/world/world.h" - -class CreaseDefenderTacticTest - : public SimulatedErForceSimPlayTestFixture, - public ::testing::WithParamInterface< - std::tuple> -{ - protected: - TbotsProto::FieldType field_type = TbotsProto::FieldType::DIV_B; - Field field = Field::createField(field_type); - std::shared_ptr ai_config_ptr = - std::make_shared(); -}; - -TEST_F(CreaseDefenderTacticTest, test_not_bumping_ball_towards_net) -{ - Point enemy_threat_point = Point(3, 0.0); - TbotsProto::CreaseDefenderAlignment alignment = - TbotsProto::CreaseDefenderAlignment::CENTRE; - - Point initial_position = Point(0, 0); - BallState ball_state(enemy_threat_point, Vector()); - auto friendly_robots = - TestUtil::createStationaryRobotStatesWithId({initial_position}); - auto enemy_robots = TestUtil::createStationaryRobotStatesWithId({Point(4, 0)}); - - auto tactic = std::make_shared(ai_config_ptr); - tactic->updateControlParams(enemy_threat_point, alignment); - setTactic(0, tactic); - - std::vector terminating_validation_functions = { - [tactic](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) - { - while (!tactic->done()) - { - yield("Tactic not done"); - } - }}; - - std::vector non_terminating_validation_functions = { - [tactic](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) - { - if (world_ptr->ball().velocity().length() > 0.01) - { - yield("Ball was hit"); - } - }}; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(10)); -} - -TEST_P(CreaseDefenderTacticTest, crease_defender_test) -{ - Point enemy_threat_point = std::get<0>(GetParam()); - TbotsProto::CreaseDefenderAlignment alignment = std::get<1>(GetParam()); - unsigned int target_defend_region = std::get<2>(GetParam()); - ASSERT_LE(target_defend_region, 5); - - Point initial_position = Point(-3, 1.5); - BallState ball_state(Point(4.5, -3), Vector(0, 0)); - auto friendly_robots = - TestUtil::createStationaryRobotStatesWithId({initial_position}); - auto enemy_robots = TestUtil::createStationaryRobotStatesWithId( - {Point(1, 0), enemy_threat_point, Point(1, -1.5), field.enemyGoalCenter(), - field.enemyDefenseArea().negXNegYCorner(), - field.enemyDefenseArea().negXPosYCorner()}); - - auto tactic = std::make_shared(ai_config_ptr); - - tactic->updateControlParams(enemy_threat_point, alignment); - setTactic(0, tactic); - - Rectangle defense_area = field.friendlyDefenseArea(); - Rectangle field_lines = field.fieldLines(); - Vector defense_area_half_width = Vector(defense_area.xLength() / 2, 0); - - std::vector defender_regions = { - Rectangle(field.friendlyCornerPos(), - defense_area.negXPosYCorner() + defense_area_half_width), - Rectangle(field.friendlyCornerPos() + defense_area_half_width, - defense_area.posXPosYCorner()), - Rectangle(Point(defense_area.xMax(), field_lines.yMax()), field.centerPoint()), - Rectangle(Point(defense_area.xMax(), field_lines.yMin()), field.centerPoint()), - Rectangle(field.friendlyCornerNeg() + defense_area_half_width, - defense_area.posXNegYCorner()), - Rectangle(field.friendlyCornerNeg(), - defense_area.negXNegYCorner() + defense_area_half_width), - }; - - std::vector terminating_validation_functions = { - [target_defend_region, defender_regions, tactic]( - std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) - { - // Check that tactic is done - while (!tactic->done()) - { - yield("Tactic not done"); - } - // Check that conditions hold for 1000 ticks - unsigned num_ticks = 1000; - for (unsigned i = 0; i < num_ticks; i++) - { - robotInPolygon(defender_regions[target_defend_region], 1, world_ptr, - yield); - } - }}; - - std::vector non_terminating_validation_functions = {}; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(10)); -} - -// We check if the robot is in one of the following regions -// ┌───┬───┬────────────────┐ -// │ │ │ │ -// │ 0 │ 1 │ C -// │ │ │ E -// │ │ │ N -// ├───┴───┤ 2 T -// E │ R -// N Def. │ E -// D │ │ -// L ├────────────────┤ -// I Area │ │ -// N │ │ -// E │ L -// ├───┬───┤ I -// │ │ │ 3 N -// │ 5 │ 4 │ E -// │ │ │ │ -// │ │ │ │ -// └───┴───┴────────────────┘ - -INSTANTIATE_TEST_CASE_P( - CreaseDefenderEnvironment, CreaseDefenderTacticTest, - ::testing::Values( - // Enemy threat in front of crease, LEFT - std::make_tuple(Point(1, 2.5), TbotsProto::CreaseDefenderAlignment::LEFT, 2), - // Enemy threat in front of crease, CENTRE - std::make_tuple(Point(1, -2.5), TbotsProto::CreaseDefenderAlignment::CENTRE, 3), - // Enemy threat in front of crease, RIGHT - std::make_tuple(Point(1.5, 2), TbotsProto::CreaseDefenderAlignment::RIGHT, 2), - // Enemy threat left side of crease, RIGHT - std::make_tuple(Point(-3.5, 2.5), TbotsProto::CreaseDefenderAlignment::RIGHT, 1), - // Enemy threat left side of crease, CENTRE - std::make_tuple(Point(-4, 2.5), TbotsProto::CreaseDefenderAlignment::CENTRE, 0), - // goal Enemy threat right side of crease, RIGHT - std::make_tuple(Point(-4, -2), TbotsProto::CreaseDefenderAlignment::RIGHT, 5), - // Enemy threat right side of crease, LEFT - std::make_tuple(Point(-4.25, -2), TbotsProto::CreaseDefenderAlignment::LEFT, 5))); diff --git a/src/software/ai/hl/stp/tactic/dribble/BUILD b/src/software/ai/hl/stp/tactic/dribble/BUILD index 78f9b00889..329a025209 100644 --- a/src/software/ai/hl/stp/tactic/dribble/BUILD +++ b/src/software/ai/hl/stp/tactic/dribble/BUILD @@ -31,38 +31,6 @@ cc_test( ], ) -cc_test( - name = "dribble_tactic_test", - srcs = ["dribble_tactic_test.cpp"], - deps = [ - ":dribble_tactic", - "//shared/test_util:tbots_gtest_main", - "//software/simulated_tests:simulated_er_force_sim_play_test_fixture", - "//software/simulated_tests/cpp_validation:validation_function", - "//software/simulated_tests/non_terminating_validation_functions", - "//software/simulated_tests/terminating_validation_functions", - "//software/test_util", - "//software/time:duration", - "//software/world", - ], -) - -cc_test( - name = "dribble_tactic_push_enemy_test", - srcs = ["dribble_tactic_push_enemy_test.cpp"], - deps = [ - ":dribble_tactic", - "//shared/test_util:tbots_gtest_main", - "//software/simulated_tests:simulated_er_force_sim_play_test_fixture", - "//software/simulated_tests/cpp_validation:validation_function", - "//software/simulated_tests/non_terminating_validation_functions", - "//software/simulated_tests/terminating_validation_functions", - "//software/test_util", - "//software/time:duration", - "//software/world", - ], -) - py_test( name = "excessive_dribble_test", srcs = [ diff --git a/src/software/ai/hl/stp/tactic/dribble/dribble_tactic_push_enemy_test.cpp b/src/software/ai/hl/stp/tactic/dribble/dribble_tactic_push_enemy_test.cpp deleted file mode 100644 index 1f55717122..0000000000 --- a/src/software/ai/hl/stp/tactic/dribble/dribble_tactic_push_enemy_test.cpp +++ /dev/null @@ -1,86 +0,0 @@ -#include - -#include - -#include "software/ai/hl/stp/tactic/dribble/dribble_tactic.h" -#include "software/geom/algorithms/contains.h" -#include "software/simulated_tests/cpp_validation/validation_function.h" -#include "software/simulated_tests/non_terminating_validation_functions/robot_not_excessively_dribbling_validation.h" -#include "software/simulated_tests/simulated_er_force_sim_play_test_fixture.h" -#include "software/simulated_tests/terminating_validation_functions/ball_at_point_validation.h" -#include "software/simulated_tests/terminating_validation_functions/robot_received_ball_validation.h" -#include "software/simulated_tests/terminating_validation_functions/robot_state_validation.h" -#include "software/test_util/test_util.h" -#include "software/time/duration.h" -#include "software/world/world.h" - -class DribbleTacticPushEnemyTest : public SimulatedErForceSimPlayTestFixture, - public ::testing::WithParamInterface -{ - protected: - void checkPossession(std::shared_ptr tactic, - std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) - { - while (!tactic->done()) - { - yield("Tactic not done"); - } - robotReceivedBall(world_ptr, yield); - auto received_ball_time = world_ptr->getMostRecentTimestamp(); - while (world_ptr->getMostRecentTimestamp() < - received_ball_time + Duration::fromSeconds(1)) - { - yield("Waiting 1 second to see if possession is maintained"); - } - robotReceivedBall(world_ptr, yield); - } - - void SetUp() override - { - SimulatedErForceSimPlayTestFixture::SetUp(); - } - TbotsProto::FieldType field_type = TbotsProto::FieldType::DIV_B; - Field field = Field::createField(field_type); - std::vector enemy_robots = - TestUtil::createStationaryRobotStatesWithId( - {Point(1, 0), Point(1, 2.5), Point(1, -2.5), field.enemyGoalCenter(), - field.enemyDefenseArea().negXNegYCorner(), - field.enemyDefenseArea().negXPosYCorner()}); -}; - -// TODO (#2573): re-enable once fixed -TEST_P(DribbleTacticPushEnemyTest, DISABLED_test_steal_ball_from_behind_enemy) -{ - Point initial_position = GetParam(); - BallState ball_state(Point(1 + DIST_TO_FRONT_OF_ROBOT_METERS, 2.5), Vector()); - Point dribble_destination = Point(0, 2); - Angle dribble_orientation = Angle::zero(); - auto friendly_robots = - TestUtil::createStationaryRobotStatesWithId({Point(-3, -2.5), initial_position}); - - auto tactic = - std::make_shared(std::make_shared()); - tactic->updateControlParams(dribble_destination, dribble_orientation); - setTactic(1, tactic, {TbotsProto::MotionConstraint::ENEMY_DEFENSE_AREA}); - - std::vector terminating_validation_functions = { - [this, tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) - { checkPossession(tactic, world_ptr, yield); }}; - - std::vector non_terminating_validation_functions = {}; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(10)); -} - -INSTANTIATE_TEST_CASE_P(CreaseDefenderEnvironment, DribbleTacticPushEnemyTest, - ::testing::Values( - // Steal ball from behind - Point(-2, 2.5), - // Steal ball from front - Point(3.5, 2.5), - // Steal ball from side - Point(1, 0))); diff --git a/src/software/ai/hl/stp/tactic/dribble/dribble_tactic_test.cpp b/src/software/ai/hl/stp/tactic/dribble/dribble_tactic_test.cpp deleted file mode 100644 index ca878c9c0d..0000000000 --- a/src/software/ai/hl/stp/tactic/dribble/dribble_tactic_test.cpp +++ /dev/null @@ -1,367 +0,0 @@ -#include "software/ai/hl/stp/tactic/dribble/dribble_tactic.h" - -#include - -#include - -#include "software/geom/algorithms/contains.h" -#include "software/simulated_tests/cpp_validation/validation_function.h" -#include "software/simulated_tests/non_terminating_validation_functions/robot_not_excessively_dribbling_validation.h" -#include "software/simulated_tests/simulated_er_force_sim_play_test_fixture.h" -#include "software/simulated_tests/terminating_validation_functions/ball_at_point_validation.h" -#include "software/simulated_tests/terminating_validation_functions/robot_received_ball_validation.h" -#include "software/simulated_tests/terminating_validation_functions/robot_state_validation.h" -#include "software/test_util/test_util.h" -#include "software/time/duration.h" -#include "software/world/world.h" - -class DribbleTacticTest : public SimulatedErForceSimPlayTestFixture -{ - protected: - void checkPossession(std::shared_ptr tactic, - std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) - { - while (!tactic->done()) - { - yield("Tactic not done"); - } - robotReceivedBall(world_ptr, yield); - auto received_ball_time = world_ptr->getMostRecentTimestamp(); - while (world_ptr->getMostRecentTimestamp() < - received_ball_time + Duration::fromSeconds(2)) - { - yield("Waiting 2 second to see if possession is maintained"); - } - robotReceivedBall(world_ptr, yield); - } - - void SetUp() override - { - SimulatedErForceSimPlayTestFixture::SetUp(); - } - TbotsProto::FieldType field_type = TbotsProto::FieldType::DIV_B; - Field field = Field::createField(field_type); - std::vector enemy_robots = - TestUtil::createStationaryRobotStatesWithId( - {Point(1, 0), Point(1, 2.5), Point(1, -2.5), field.enemyGoalCenter(), - field.enemyDefenseArea().negXNegYCorner(), - field.enemyDefenseArea().negXPosYCorner()}); - std::set motion_constraints = { - TbotsProto::MotionConstraint::ENEMY_DEFENSE_AREA}; -}; - -TEST_F(DribbleTacticTest, test_intercept_ball_behind_enemy_robot) -{ - Point initial_position = Point(-3, 1.5); - BallState ball_state(Point(3, -2), Vector(-0.5, 1)); - auto friendly_robots = - TestUtil::createStationaryRobotStatesWithId({Point(-3, 2.5), initial_position}); - - auto tactic = - std::make_shared(std::make_shared()); - setTactic(1, tactic, motion_constraints); - - std::vector terminating_validation_functions = { - [this, tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) - { checkPossession(tactic, world_ptr, yield); }}; - - std::vector non_terminating_validation_functions = {}; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(10)); -} - -TEST_F(DribbleTacticTest, test_stopped_ball) -{ - Point initial_position = Point(-3, 1.5); - BallState ball_state(Point(-1, 1.5), Vector(0, 0)); - auto friendly_robots = - TestUtil::createStationaryRobotStatesWithId({Point(3, 3), initial_position}); - - auto tactic = - std::make_shared(std::make_shared()); - setTactic(1, tactic, motion_constraints); - - std::vector terminating_validation_functions = { - [this, tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) - { checkPossession(tactic, world_ptr, yield); }}; - - std::vector non_terminating_validation_functions = {}; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(10)); -} - -TEST_F(DribbleTacticTest, test_ball_bounce_off_of_enemy_robot) -{ - Point initial_position = Point(-3, 1.5); - BallState ball_state(Point(0, 0), Vector(2.5, 0)); - auto friendly_robots = - TestUtil::createStationaryRobotStatesWithId({Point(3, 3), initial_position}); - - auto tactic = - std::make_shared(std::make_shared()); - setTactic(1, tactic, motion_constraints); - - std::vector terminating_validation_functions = { - [this, tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) - { checkPossession(tactic, world_ptr, yield); }}; - - std::vector non_terminating_validation_functions = {}; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(10)); -} - -TEST_F(DribbleTacticTest, test_moving_ball_dribble_dest) -{ - Point initial_position = Point(-3, 1.5); - Point dribble_destination = Point(-3, 1); - BallState ball_state(Point(3, -2), Vector(-1, 2)); - auto friendly_robots = - TestUtil::createStationaryRobotStatesWithId({Point(-3, 2.5), initial_position}); - - auto tactic = - std::make_shared(std::make_shared()); - tactic->updateControlParams(dribble_destination, std::nullopt); - setTactic(1, tactic, motion_constraints); - - std::vector terminating_validation_functions = { - [this, dribble_destination, tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) - { - checkPossession(tactic, world_ptr, yield); - ballAtPoint(dribble_destination, world_ptr, yield); - checkPossession(tactic, world_ptr, yield); - }}; - - std::vector non_terminating_validation_functions = { - [this](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) - { - // TODO (#2514): tune dribbling and re-enable - // robotNotExcessivelyDribbling(1, world_ptr, yield); - }}; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(15)); -} - -TEST_F(DribbleTacticTest, test_moving_ball_dribble_orientation) -{ - Point initial_position = Point(-3, 1.5); - Angle dribble_orientation = Angle::quarter(); - BallState ball_state(Point(3, -2), Vector(-1, 2)); - auto friendly_robots = - TestUtil::createStationaryRobotStatesWithId({Point(-3, 2.5), initial_position}); - - auto tactic = - std::make_shared(std::make_shared()); - tactic->updateControlParams(std::nullopt, dribble_orientation); - setTactic(1, tactic, motion_constraints); - - std::vector terminating_validation_functions = { - [this, dribble_orientation, tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) - { - checkPossession(tactic, world_ptr, yield); - robotAtOrientation(1, world_ptr, dribble_orientation, Angle::fromDegrees(5), - yield); - checkPossession(tactic, world_ptr, yield); - }}; - - std::vector non_terminating_validation_functions = {}; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(10)); -} - -TEST_F(DribbleTacticTest, test_moving_ball_dribble_dest_and_orientation) -{ - Point initial_position = Point(-2, 1.5); - Point dribble_destination = Point(-1, 2); - Angle dribble_orientation = Angle::zero(); - BallState ball_state(Point(1, 0), Vector(1, 2)); - auto friendly_robots = - TestUtil::createStationaryRobotStatesWithId({Point(-3, 2.5), initial_position}); - - auto tactic = - std::make_shared(std::make_shared()); - tactic->updateControlParams(dribble_destination, dribble_orientation); - setTactic(1, tactic, motion_constraints); - - std::vector terminating_validation_functions = { - [this, dribble_destination, dribble_orientation, tactic]( - std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) - { - checkPossession(tactic, world_ptr, yield); - ballAtPoint(dribble_destination, world_ptr, yield); - robotAtOrientation(1, world_ptr, dribble_orientation, Angle::fromDegrees(5), - yield); - checkPossession(tactic, world_ptr, yield); - }}; - - std::vector non_terminating_validation_functions = { - [this](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) - { - // TODO (#2514): tune dribbling and re-enable - // robotNotExcessivelyDribbling(1, world_ptr, yield); - }}; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(22)); -} - -TEST_F(DribbleTacticTest, test_dribble_dest_and_orientation_around_rectangle) -{ - Point initial_position = Point(3, -3); - Point dribble_destination = Point(4, 2.5); - Angle dribble_orientation = Angle::half(); - BallState ball_state(Point(4, -2.5), Vector(0, 0)); - auto friendly_robots = - TestUtil::createStationaryRobotStatesWithId({Point(-3, 2.5), initial_position}); - auto tactic = - std::make_shared(std::make_shared()); - tactic->updateControlParams(dribble_destination, dribble_orientation); - setTactic(1, tactic, motion_constraints); - - std::vector terminating_validation_functions = { - [this, dribble_destination, dribble_orientation, tactic]( - std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) - { - checkPossession(tactic, world_ptr, yield); - ballAtPoint(dribble_destination, world_ptr, yield); - robotAtOrientation(1, world_ptr, dribble_orientation, Angle::fromDegrees(5), - yield); - checkPossession(tactic, world_ptr, yield); - }}; - - std::vector non_terminating_validation_functions = { - [this](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) - { - // TODO (#2514): tune dribbling and re-enable - // robotNotExcessivelyDribbling(1, world_ptr, yield); - }}; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(25)); -} - -TEST_F(DribbleTacticTest, - test_dribble_dest_and_orientation_around_rectangle_with_excessive_dribbling) -{ - Point dribble_destination = Point(3, 2); - Point initial_position = Point(4.5, -3.0); - Angle dribble_orientation = Angle::half(); - BallState ball_state(Point(4.2, -2.5), Vector(0, 0)); - auto friendly_robots = - TestUtil::createStationaryRobotStatesWithId({Point(-3, 2.5), initial_position}); - auto tactic = - std::make_shared(std::make_shared()); - tactic->updateControlParams(dribble_destination, dribble_orientation, true); - setTactic(1, tactic, motion_constraints); - - std::vector terminating_validation_functions = { - [this, dribble_destination, dribble_orientation, tactic]( - std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) - { - checkPossession(tactic, world_ptr, yield); - ballAtPoint(dribble_destination, world_ptr, yield); - robotAtOrientation(1, world_ptr, dribble_orientation, Angle::fromDegrees(5), - yield); - checkPossession(tactic, world_ptr, yield); - }}; - - std::vector non_terminating_validation_functions = {}; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(12)); -} - -TEST_F(DribbleTacticTest, test_running_into_enemy_robot_knocking_ball_away) -{ - Point initial_position = Point(-2, 1.5); - Point dribble_destination = Point(-1, 2); - Angle dribble_orientation = Angle::half(); - BallState ball_state(Point(2, -2), Vector(2, 4)); - auto friendly_robots = - TestUtil::createStationaryRobotStatesWithId({Point(-3, 2.5), initial_position}); - enemy_robots.emplace_back(RobotStateWithId{ - .id = 7, - .robot_state = RobotState(Point(1, 1.1), Vector(), Angle::fromDegrees(-30), - AngularVelocity::zero())}); - - auto tactic = - std::make_shared(std::make_shared()); - tactic->updateControlParams(dribble_destination, dribble_orientation); - // Don't avoid enemy robots to knock ball away - setTactic(1, tactic, {}); - - std::vector terminating_validation_functions = { - [this, dribble_destination, dribble_orientation, tactic]( - std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) - { - checkPossession(tactic, world_ptr, yield); - ballAtPoint(dribble_destination, world_ptr, yield); - robotAtOrientation(1, world_ptr, dribble_orientation, Angle::fromDegrees(5), - yield); - checkPossession(tactic, world_ptr, yield); - }}; - - std::vector non_terminating_validation_functions = { - [this](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) - { - // TODO (#2514): tune dribbling and re-enable - // robotNotExcessivelyDribbling(1, world_ptr, yield); - }}; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(20)); -} - -TEST_F(DribbleTacticTest, test_robot_not_bumping_ball_when_turning_around) -{ - // The ball is placed right behind the friendly robot. Verify that the robot - // does not bump the ball away when turning around to dribble it. - RobotStateWithId friendly_robot{ - .id = 1, - .robot_state = RobotState(Point(-1, 0), Vector(0, 0), Angle::half(), - AngularVelocity::zero())}; - BallState initial_ball_state(Point(-1 + ROBOT_MAX_RADIUS_METERS, 0), - Vector(0.0, 0.0)); - - auto tactic = - std::make_shared(std::make_shared()); - setTactic(1, tactic, motion_constraints); - - std::vector terminating_validation_functions = { - [this, tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) - { checkPossession(tactic, world_ptr, yield); }}; - - std::vector non_terminating_validation_functions = { - [&](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) - { - while (distance(world_ptr->ball().position(), initial_ball_state.position()) > - 0.05) - { - yield("Robot bumped the ball away while turning around"); - } - }}; - - runTest(field_type, initial_ball_state, {friendly_robot}, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(10)); -} diff --git a/src/software/ai/hl/stp/tactic/halt/BUILD b/src/software/ai/hl/stp/tactic/halt/BUILD index db4cc095f9..977d90bf07 100644 --- a/src/software/ai/hl/stp/tactic/halt/BUILD +++ b/src/software/ai/hl/stp/tactic/halt/BUILD @@ -26,16 +26,3 @@ cc_test( "//software/test_util", ], ) - -cc_test( - name = "halt_tactic_test", - srcs = ["halt_tactic_test.cpp"], - deps = [ - ":halt_tactic", - "//shared/test_util:tbots_gtest_main", - "//software/simulated_tests:simulated_er_force_sim_play_test_fixture", - "//software/simulated_tests/cpp_validation:validation_function", - "//software/simulated_tests/terminating_validation_functions", - "//software/test_util", - ], -) diff --git a/src/software/ai/hl/stp/tactic/halt/halt_tactic_test.cpp b/src/software/ai/hl/stp/tactic/halt/halt_tactic_test.cpp deleted file mode 100644 index efbc6d7bd5..0000000000 --- a/src/software/ai/hl/stp/tactic/halt/halt_tactic_test.cpp +++ /dev/null @@ -1,70 +0,0 @@ -#include "software/ai/hl/stp/tactic/halt/halt_tactic.h" - -#include - -#include "software/simulated_tests/cpp_validation/validation_function.h" -#include "software/simulated_tests/simulated_er_force_sim_play_test_fixture.h" -#include "software/simulated_tests/terminating_validation_functions/robot_halt_validation.h" -#include "software/simulated_tests/terminating_validation_functions/robot_state_validation.h" -#include "software/test_util/test_util.h" - -class HaltTacticTest : public SimulatedErForceSimPlayTestFixture -{ - protected: - TbotsProto::FieldType field_type = TbotsProto::FieldType::DIV_B; - Field field = Field::createField(field_type); -}; - -TEST_F(HaltTacticTest, robot_already_stopped) -{ - BallState ball_state(Point(0, 0.5), Vector(0, 0)); - - auto friendly_robots = TestUtil::createMovingRobotStatesWithId( - {Point(-3, 2.5), Point()}, {Vector(), Vector()}); - auto enemy_robots = TestUtil::createStationaryRobotStatesWithId({Point(4, 0)}); - - auto tactic = std::make_shared(std::make_shared()); - setTactic(1, tactic); - - std::vector terminating_validation_functions = { - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) - { - for (unsigned i = 0; i < 1000; i++) - { - robotHalt(world_ptr, yield); - } - }}; - - std::vector non_terminating_validation_functions = {}; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(5)); -} - -TEST_F(HaltTacticTest, robot_start_moving) -{ - BallState ball_state(Point(0, 0.5), Vector(0, 0)); - - auto friendly_robots = TestUtil::createMovingRobotStatesWithId( - {Point(-3, 2.5), Point()}, {Vector(), Vector(4, 4)}); - auto enemy_robots = TestUtil::createStationaryRobotStatesWithId({Point(4, 0)}); - - auto tactic = std::make_shared(std::make_shared()); - setTactic(1, tactic); - - std::vector terminating_validation_functions = { - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) - { - for (unsigned i = 0; i < 1000; i++) - { - robotHalt(world_ptr, yield); - } - }}; - - std::vector non_terminating_validation_functions = {}; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(6)); -} diff --git a/src/software/ai/hl/stp/tactic/kick/BUILD b/src/software/ai/hl/stp/tactic/kick/BUILD index 89a93a285d..edacd65af5 100644 --- a/src/software/ai/hl/stp/tactic/kick/BUILD +++ b/src/software/ai/hl/stp/tactic/kick/BUILD @@ -28,16 +28,3 @@ cc_test( "//software/test_util", ], ) - -cc_test( - name = "kick_tactic_test", - srcs = ["kick_tactic_test.cpp"], - deps = [ - ":kick_tactic", - "//shared/test_util:tbots_gtest_main", - "//software/simulated_tests:simulated_er_force_sim_play_test_fixture", - "//software/simulated_tests/cpp_validation:validation_function", - "//software/simulated_tests/terminating_validation_functions", - "//software/test_util", - ], -) diff --git a/src/software/ai/hl/stp/tactic/kick/kick_tactic_test.cpp b/src/software/ai/hl/stp/tactic/kick/kick_tactic_test.cpp deleted file mode 100644 index 044cd89164..0000000000 --- a/src/software/ai/hl/stp/tactic/kick/kick_tactic_test.cpp +++ /dev/null @@ -1,82 +0,0 @@ -#include "software/ai/hl/stp/tactic/kick/kick_tactic.h" - -#include - -#include - -#include "software/geom/algorithms/contains.h" -#include "software/simulated_tests/cpp_validation/validation_function.h" -#include "software/simulated_tests/simulated_er_force_sim_play_test_fixture.h" -#include "software/simulated_tests/terminating_validation_functions/ball_kicked_validation.h" -#include "software/simulated_tests/terminating_validation_functions/robot_state_validation.h" -#include "software/test_util/test_util.h" -#include "software/time/duration.h" -#include "software/world/world.h" - -class KickTacticTest : public SimulatedErForceSimPlayTestFixture, - public ::testing::WithParamInterface> -{ - protected: - TbotsProto::FieldType field_type = TbotsProto::FieldType::DIV_B; - Field field = Field::createField(field_type); -}; - -TEST_P(KickTacticTest, kick_test) -{ - Vector ball_offset_from_robot = std::get<0>(GetParam()); - Angle angle_to_kick_at = std::get<1>(GetParam()); - - Point robot_position = Point(0, 0); - BallState ball_state(robot_position + ball_offset_from_robot, Vector(0, 0)); - - auto friendly_robots = - TestUtil::createStationaryRobotStatesWithId({Point(-3, 2.5), robot_position}); - auto enemy_robots = TestUtil::createStationaryRobotStatesWithId({Point(4, 0)}); - - auto tactic = std::make_shared(std::make_shared()); - tactic->updateControlParams(robot_position + ball_offset_from_robot, angle_to_kick_at, - 5); - setTactic(1, tactic); - - std::vector terminating_validation_functions = { - [angle_to_kick_at, tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) - { - while (!tactic->done()) - { - yield("Tactic did not complete!"); - } - ballKicked(angle_to_kick_at, world_ptr, yield); - }}; - - std::vector non_terminating_validation_functions = {}; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(5)); -} - -INSTANTIATE_TEST_CASE_P( - BallLocations, KickTacticTest, - ::testing::Values( - // place the ball directly to the left of the robot - std::make_tuple(Vector(0, 0.5), Angle::zero()), - // place the ball directly to the right of the robot - std::make_tuple(Vector(0, -0.5), Angle::zero()), - // place the ball directly infront of the robot - std::make_tuple(Vector(0.5, 0), Angle::zero()), - // place the ball directly behind the robot - std::make_tuple(Vector(-0.5, 0), Angle::zero()), - // place the ball in the robots dribbler - std::make_tuple(Vector(ROBOT_MAX_RADIUS_METERS, 0), Angle::zero()), - // Repeat the same tests but kick in the opposite direction - // place the ball directly to the left of the robot - std::make_tuple(Vector(0, 0.5), Angle::half()), - // place the ball directly to the right of the robot - std::make_tuple(Vector(0, -0.5), Angle::half()), - // place the ball directly infront of the robot - std::make_tuple(Vector(0.5, 0), Angle::half()), - // place the ball directly behind the robot - std::make_tuple(Vector(-0.5, 0), Angle::half()), - // place the ball in the robots dribbler - std::make_tuple(Vector(ROBOT_MAX_RADIUS_METERS, 0), Angle::zero()))); diff --git a/src/software/ai/hl/stp/tactic/move/BUILD b/src/software/ai/hl/stp/tactic/move/BUILD index deb14c7c5a..683341996f 100644 --- a/src/software/ai/hl/stp/tactic/move/BUILD +++ b/src/software/ai/hl/stp/tactic/move/BUILD @@ -27,18 +27,3 @@ cc_test( "//software/test_util", ], ) - -cc_test( - name = "move_tactic_test", - srcs = ["move_tactic_test.cpp"], - deps = [ - ":move_tactic", - "//shared/test_util:tbots_gtest_main", - "//software/simulated_tests:simulated_er_force_sim_play_test_fixture", - "//software/simulated_tests/cpp_validation:validation_function", - "//software/simulated_tests/terminating_validation_functions", - "//software/test_util", - "//software/time:duration", - "//software/world", - ], -) diff --git a/src/software/ai/hl/stp/tactic/move/move_tactic_test.cpp b/src/software/ai/hl/stp/tactic/move/move_tactic_test.cpp deleted file mode 100644 index 87a9dff534..0000000000 --- a/src/software/ai/hl/stp/tactic/move/move_tactic_test.cpp +++ /dev/null @@ -1,249 +0,0 @@ -#include "software/ai/hl/stp/tactic/move/move_tactic.h" - -#include - -#include - -#include "software/geom/algorithms/contains.h" -#include "software/simulated_tests/cpp_validation/validation_function.h" -#include "software/simulated_tests/simulated_er_force_sim_play_test_fixture.h" -#include "software/simulated_tests/terminating_validation_functions/ball_kicked_validation.h" -#include "software/simulated_tests/terminating_validation_functions/robot_state_validation.h" -#include "software/test_util/test_util.h" -#include "software/time/duration.h" -#include "software/world/world.h" - -class MoveTacticTest : public SimulatedErForceSimPlayTestFixture -{ - protected: - TbotsProto::FieldType field_type = TbotsProto::FieldType::DIV_B; - Field field = Field::createField(field_type); -}; - -TEST_F(MoveTacticTest, test_move_across_field) -{ - Point initial_position = Point(-3, 1.5); - Point destination = Point(2.5, -1.1); - BallState ball_state(Point(4.5, -3), Vector(0, 0)); - auto friendly_robots = - TestUtil::createStationaryRobotStatesWithId({Point(-3, 2.5), initial_position}); - auto enemy_robots = TestUtil::createStationaryRobotStatesWithId( - {Point(1, 0), Point(1, 2.5), Point(1, -2.5), field.enemyGoalCenter(), - field.enemyDefenseArea().negXNegYCorner(), - field.enemyDefenseArea().negXPosYCorner()}); - - auto tactic = std::make_shared(std::make_shared()); - tactic->updateControlParams(destination, Angle::zero()); - setTactic(1, tactic); - - std::vector terminating_validation_functions = { - [destination, tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) - { - while (!tactic->done()) - { - yield("Tactic not done"); - } - robotAtPosition(1, world_ptr, destination, 0.05, yield); - // Check that conditions hold for 1000 ticks - unsigned num_ticks = 1000; - for (unsigned i = 0; i < num_ticks; i++) - { - robotAtPosition(1, world_ptr, destination, 0.05, yield); - } - }}; - - std::vector non_terminating_validation_functions = {}; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(10)); -} - -TEST_F(MoveTacticTest, test_autochip_move) -{ - Point initial_position = Point(-3, 1.5); - Point destination = Point(0, 1.5); - BallState ball_state(Point(0, 1.5), Vector(0, 0)); - auto friendly_robots = - TestUtil::createStationaryRobotStatesWithId({Point(-3, 2.5), initial_position}); - auto enemy_robots = TestUtil::createStationaryRobotStatesWithId( - {Point(1, 0), Point(1, 2.5), Point(1, -2.5), field.enemyGoalCenter(), - field.enemyDefenseArea().negXNegYCorner(), - field.enemyDefenseArea().negXPosYCorner()}); - - auto tactic = std::make_shared(std::make_shared()); - tactic->updateControlParams(destination, Angle::zero(), TbotsProto::DribblerMode::OFF, - TbotsProto::BallCollisionType::ALLOW, - {AutoChipOrKickMode::AUTOCHIP, 2.0}, - TbotsProto::MaxAllowedSpeedMode::COLLISIONS_ALLOWED, - TbotsProto::ObstacleAvoidanceMode::SAFE); - setTactic(1, tactic); - - std::vector terminating_validation_functions = { - [destination, tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) - { - while (!tactic->done()) - { - yield("Tactic not done"); - } - robotAtPosition(1, world_ptr, destination, 0.05, yield); - ballKicked(Angle::zero(), world_ptr, yield); - // Check that conditions hold for 1000 ticks - unsigned num_ticks = 1000; - for (unsigned i = 0; i < num_ticks; i++) - { - robotAtPosition(1, world_ptr, destination, 0.05, yield); - } - }}; - - std::vector non_terminating_validation_functions = {}; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(10)); -} - -TEST_F(MoveTacticTest, test_autokick_move) -{ - Point initial_position = Point(-1, -0.5); - Point destination = Point(-1, -1); - BallState ball_state(Point(-1, -1), Vector(0, 0)); - auto friendly_robots = {RobotStateWithId{ - .id = 0, - .robot_state = RobotState(initial_position, Vector(0, 0), Angle::threeQuarter(), - AngularVelocity::zero())}}; - auto enemy_robots = TestUtil::createStationaryRobotStatesWithId( - {Point(1, 0), Point(1, 2.5), Point(1, -2.5), field.enemyGoalCenter(), - field.enemyDefenseArea().negXNegYCorner(), - field.enemyDefenseArea().negXPosYCorner()}); - - auto tactic = std::make_shared(std::make_shared()); - tactic->updateControlParams( - destination, Angle::threeQuarter(), TbotsProto::DribblerMode::OFF, - TbotsProto::BallCollisionType::ALLOW, {AutoChipOrKickMode::AUTOKICK, 3.0}, - TbotsProto::MaxAllowedSpeedMode::COLLISIONS_ALLOWED, - TbotsProto::ObstacleAvoidanceMode::SAFE); - setTactic(0, tactic); - - std::vector terminating_validation_functions = { - [destination, tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) - { - while (!tactic->done()) - { - yield("Tactic not done"); - } - robotAtPosition(0, world_ptr, destination, 0.05, yield); - ballKicked(Angle::threeQuarter(), world_ptr, yield); - // Check that conditions hold for 1000 ticks - unsigned num_ticks = 1000; - for (unsigned i = 0; i < num_ticks; i++) - { - robotAtPosition(0, world_ptr, destination, 0.05, yield); - } - }}; - - std::vector non_terminating_validation_functions = {}; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(10)); -} - -TEST_F(MoveTacticTest, test_spinning_move_clockwise) -{ - Point initial_position = Point(-4, 2); - Point destination = Point(4, 2); - BallState ball_state(Point(1, 1), Vector(0, 0)); - auto friendly_robots = {RobotStateWithId{ - .id = 0, - .robot_state = RobotState(initial_position, Vector(0, 0), Angle::zero(), - AngularVelocity::quarter())}}; - auto enemy_robots = TestUtil::createStationaryRobotStatesWithId({Point(4, 0)}); - - auto tactic = std::make_shared(std::make_shared()); - tactic->updateControlParams(destination, Angle::zero(), TbotsProto::DribblerMode::OFF, - TbotsProto::BallCollisionType::ALLOW, - {AutoChipOrKickMode::OFF, 0.0}, - TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT, - TbotsProto::ObstacleAvoidanceMode::SAFE); - setTactic(0, tactic); - - std::vector terminating_validation_functions = { - [destination, tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) - { - robotAtAngularVelocity(0, world_ptr, AngularVelocity::fromDegrees(1 * 360), - AngularVelocity::fromDegrees(50), yield); - robotAtPosition(0, world_ptr, destination, 0.05, yield); - robotAtOrientation(0, world_ptr, Angle::zero(), Angle::fromDegrees(5), yield); - while (!tactic->done()) - { - yield("Tactic is not done"); - } - // Check that conditions hold for 1000 ticks - unsigned num_ticks = 1000; - for (unsigned i = 0; i < num_ticks; i++) - { - robotAtPosition(0, world_ptr, destination, 0.05, yield); - robotAtOrientation(0, world_ptr, Angle::zero(), Angle::fromDegrees(5), - yield); - } - }}; - - std::vector non_terminating_validation_functions = {}; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(10)); -} - -TEST_F(MoveTacticTest, test_spinning_move_counter_clockwise) -{ - Point initial_position = Point(4, 2); - Point destination = Point(-4, 2); - BallState ball_state(Point(1, 1), Vector(0, 0)); - auto friendly_robots = {RobotStateWithId{ - .id = 0, - .robot_state = RobotState(initial_position, Vector(0, 0), Angle::quarter(), - AngularVelocity::zero())}}; - auto enemy_robots = TestUtil::createStationaryRobotStatesWithId({Point(4, 0)}); - - auto tactic = std::make_shared(std::make_shared()); - tactic->updateControlParams(destination, Angle::half(), TbotsProto::DribblerMode::OFF, - TbotsProto::BallCollisionType::ALLOW, - {AutoChipOrKickMode::OFF, 0.0}, - TbotsProto::MaxAllowedSpeedMode::PHYSICAL_LIMIT, - TbotsProto::ObstacleAvoidanceMode::SAFE); - setTactic(0, tactic); - - std::vector terminating_validation_functions = { - [destination, tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) - { - robotAtAngularVelocity(0, world_ptr, AngularVelocity::fromDegrees(-4 * 360), - AngularVelocity::fromDegrees(50), yield); - robotAtPosition(0, world_ptr, destination, 0.05, yield); - robotAtOrientation(0, world_ptr, Angle::half(), Angle::fromDegrees(5), yield); - while (!tactic->done()) - { - yield("Tactic is not done"); - } - // Check that conditions hold for 1000 ticks - unsigned num_ticks = 1000; - for (unsigned i = 0; i < num_ticks; i++) - { - robotAtPosition(0, world_ptr, destination, 0.05, yield); - robotAtOrientation(0, world_ptr, Angle::half(), Angle::fromDegrees(5), - yield); - } - }}; - - std::vector non_terminating_validation_functions = {}; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(10)); -} diff --git a/src/software/ai/hl/stp/tactic/penalty_kick/BUILD b/src/software/ai/hl/stp/tactic/penalty_kick/BUILD index 927a632ce3..063613760c 100644 --- a/src/software/ai/hl/stp/tactic/penalty_kick/BUILD +++ b/src/software/ai/hl/stp/tactic/penalty_kick/BUILD @@ -32,19 +32,3 @@ cc_test( "//software/test_util", ], ) - -cc_test( - name = "penalty_kick_tactic_test", - srcs = ["penalty_kick_tactic_test.cpp"], - deps = [ - ":penalty_kick_tactic", - "//shared/test_util:tbots_gtest_main", - "//software/simulated_tests:simulated_er_force_sim_play_test_fixture", - "//software/simulated_tests/cpp_validation:validation_function", - "//software/simulated_tests/non_terminating_validation_functions", - "//software/simulated_tests/terminating_validation_functions", - "//software/test_util", - "//software/time:duration", - "//software/world", - ], -) diff --git a/src/software/ai/hl/stp/tactic/penalty_kick/penalty_kick_tactic_test.cpp b/src/software/ai/hl/stp/tactic/penalty_kick/penalty_kick_tactic_test.cpp deleted file mode 100644 index b0392cf567..0000000000 --- a/src/software/ai/hl/stp/tactic/penalty_kick/penalty_kick_tactic_test.cpp +++ /dev/null @@ -1,100 +0,0 @@ -#include "software/ai/hl/stp/tactic/penalty_kick/penalty_kick_tactic.h" - -#include - -#include - -#include "software/simulated_tests/non_terminating_validation_functions/ball_in_play_or_scored_validation.h" -#include "software/simulated_tests/non_terminating_validation_functions/ball_never_moves_backward_validation.h" -#include "software/simulated_tests/non_terminating_validation_functions/robot_not_excessively_dribbling_validation.h" -#include "software/simulated_tests/simulated_er_force_sim_play_test_fixture.h" -#include "software/simulated_tests/terminating_validation_functions/friendly_scored_validation.h" -#include "software/test_util/test_util.h" -#include "software/world/field.h" - -// goalie -class PenaltyKickTacticTest : public SimulatedErForceSimPlayTestFixture, - public ::testing::WithParamInterface -{ - protected: - TbotsProto::FieldType field_type = TbotsProto::FieldType::DIV_B; - Field field = Field::createField(field_type); - BallState ball = BallState(field.friendlyPenaltyMark(), Vector(0, 0)); - Point initial_position = field.friendlyPenaltyMark() + Vector(-0.1, 0); - RobotStateWithId shooter = { - 0, RobotState(initial_position, Vector(0, 0), Angle::zero(), Angle::zero())}; -}; - -// TODO (#2232): Improve dribbling control so the ball is not lost during this test -TEST_P(PenaltyKickTacticTest, DISABLED_penalty_kick_test) -{ - RobotStateWithId enemy_robot = GetParam(); - - auto tactic = - std::make_shared(std::make_shared()); - static RobotId shooter_id = 0; - setTactic(shooter_id, tactic); - - std::vector terminating_validation_functions = { - friendlyScored, - }; - - std::vector non_terminating_validation_functions = { - ballInPlay, - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) - { ballNeverMovesBackward(world_ptr, yield); }, - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) - { robotNotExcessivelyDribbling(shooter_id, world_ptr, yield); }}; - - runTest(field_type, ball, {shooter}, {enemy_robot}, terminating_validation_functions, - non_terminating_validation_functions, Duration::fromSeconds(10)); -} - -// TODO (#2519): fix and re-enable -TEST_F(PenaltyKickTacticTest, DISABLED_penalty_no_goalie) -{ - auto tactic = - std::make_shared(std::make_shared()); - static RobotId shooter_id = 0; - setTactic(shooter_id, tactic); - - std::vector terminating_validation_functions = { - friendlyScored, - }; - - std::vector non_terminating_validation_functions = { - ballInPlay, - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) - { ballNeverMovesBackward(world_ptr, yield); }, - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) - { robotNotExcessivelyDribbling(shooter_id, world_ptr, yield); }}; - - auto enemy_robots = TestUtil::createStationaryRobotStatesWithId({Point(0, -2.5)}); - - runTest(field_type, ball, {shooter}, enemy_robots, terminating_validation_functions, - non_terminating_validation_functions, Duration::fromSeconds(10)); -} - -INSTANTIATE_TEST_CASE_P( - RobotLocations, PenaltyKickTacticTest, - ::testing::Values( - // enemy robot stationary at centre of goal - RobotStateWithId{0, RobotState(Field::createSSLDivisionBField().enemyGoalCenter(), - Vector(0, 0), Angle::half(), Angle::zero())}, - - // enemy robot stationary left of net - RobotStateWithId{0, - RobotState(Field::createSSLDivisionBField().enemyGoalpostNeg(), - Vector(0, 0), Angle::half(), Angle::zero())}, - // enemy robot stationary right of net - RobotStateWithId{0, - RobotState(Field::createSSLDivisionBField().enemyGoalpostPos(), - Vector(0, 0), Angle::half(), Angle::zero())}, - // enemy robot left of net but moving right - RobotStateWithId{0, - RobotState(Field::createSSLDivisionBField().enemyGoalpostNeg(), - Vector(0, 1.2), Angle::half(), Angle::zero())}, - // enemy robot right of net but moving left - RobotStateWithId{0, - RobotState(Field::createSSLDivisionBField().enemyGoalpostPos(), - Vector(0, -1.2), Angle::half(), Angle::zero())})); diff --git a/src/software/ai/hl/stp/tactic/pivot_kick/BUILD b/src/software/ai/hl/stp/tactic/pivot_kick/BUILD index dff4416de9..4143d1f642 100644 --- a/src/software/ai/hl/stp/tactic/pivot_kick/BUILD +++ b/src/software/ai/hl/stp/tactic/pivot_kick/BUILD @@ -30,18 +30,3 @@ cc_test( "//software/test_util", ], ) - -cc_test( - name = "pivot_kick_tactic_test", - srcs = ["pivot_kick_tactic_test.cpp"], - deps = [ - ":pivot_kick_tactic", - "//shared/test_util:tbots_gtest_main", - "//software/simulated_tests:simulated_er_force_sim_play_test_fixture", - "//software/simulated_tests/cpp_validation:validation_function", - "//software/simulated_tests/terminating_validation_functions", - "//software/test_util", - "//software/time:duration", - "//software/world", - ], -) diff --git a/src/software/ai/hl/stp/tactic/pivot_kick/pivot_kick_tactic_test.cpp b/src/software/ai/hl/stp/tactic/pivot_kick/pivot_kick_tactic_test.cpp deleted file mode 100644 index eb78e2d607..0000000000 --- a/src/software/ai/hl/stp/tactic/pivot_kick/pivot_kick_tactic_test.cpp +++ /dev/null @@ -1,91 +0,0 @@ -#include "software/ai/hl/stp/tactic/pivot_kick/pivot_kick_tactic.h" - -#include - -#include - -#include "software/geom/algorithms/contains.h" -#include "software/simulated_tests/cpp_validation/validation_function.h" -#include "software/simulated_tests/simulated_er_force_sim_play_test_fixture.h" -#include "software/simulated_tests/terminating_validation_functions/ball_kicked_validation.h" -#include "software/simulated_tests/terminating_validation_functions/robot_state_validation.h" -#include "software/test_util/test_util.h" -#include "software/time/duration.h" -#include "software/world/world.h" - -class PivotKickTacticTest - : public SimulatedErForceSimPlayTestFixture, - public ::testing::WithParamInterface> -{ - protected: - TbotsProto::FieldType field_type = TbotsProto::FieldType::DIV_B; - Field field = Field::createField(field_type); -}; - -TEST_P(PivotKickTacticTest, pivot_kick_test) -{ - Vector ball_offset_from_robot = std::get<0>(GetParam()); - Angle angle_to_kick_at = std::get<1>(GetParam()); - - Point robot_position = Point(0, 0); - BallState ball_state(robot_position + ball_offset_from_robot, Vector(0, 0)); - - auto friendly_robots = - TestUtil::createStationaryRobotStatesWithId({Point(-3, 2.5), robot_position}); - auto enemy_robots = TestUtil::createStationaryRobotStatesWithId({Point(4, 0)}); - - auto tactic = - std::make_shared(std::make_shared()); - tactic->updateControlParams(robot_position + ball_offset_from_robot, angle_to_kick_at, - {AutoChipOrKickMode::AUTOKICK, 5}); - setTactic(1, tactic); - - std::vector terminating_validation_functions = { - [angle_to_kick_at, tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) - { - while (!tactic->done()) - { - yield("Tactic did not complete!"); - } - ballKicked(angle_to_kick_at, world_ptr, yield); - }}; - - std::vector non_terminating_validation_functions = {}; - - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validation_functions, non_terminating_validation_functions, - Duration::fromSeconds(5)); -} - -INSTANTIATE_TEST_CASE_P( - BallLocations, PivotKickTacticTest, - ::testing::Values( - // place the ball directly to the left of the robot - std::make_tuple(Vector(0, 0.5), Angle::zero()), - // place the ball directly to the right of the robot - std::make_tuple(Vector(0, -0.5), Angle::zero()), - // place the ball directly infront of the robot - std::make_tuple(Vector(0.5, 0), Angle::zero()), - // place the ball directly behind the robot - std::make_tuple(Vector(-0.5, 0), Angle::zero()), - // place the ball in the robots dribbler - std::make_tuple(Vector(ROBOT_MAX_RADIUS_METERS, 0), Angle::zero()), - - // TODO (#2859): The robot does not dribble far enough into the ball - // Repeat the same tests but kick in the opposite direction - // place the ball directly to the left of the robot - // std::make_tuple(Vector(0, 0.5), Angle::half()), - - // place the ball directly to the right of the robot - std::make_tuple(Vector(0, -0.5), Angle::half()), - // place the ball directly infront of the robot - std::make_tuple(Vector(0.5, 0), Angle::half()), - - // TODO (#2909): Enable test once the robot can turn faster and hits the ball with - // the dribbler. - // place the ball directly behind the robot - // std::make_tuple(Vector(-0.5, 0), Angle::half()), - - // place the ball in the robots dribbler - std::make_tuple(Vector(ROBOT_MAX_RADIUS_METERS, 0), Angle::zero()))); diff --git a/src/software/ai/hl/stp/tactic/receiver/BUILD b/src/software/ai/hl/stp/tactic/receiver/BUILD index d9a3f89d42..a95a31370b 100644 --- a/src/software/ai/hl/stp/tactic/receiver/BUILD +++ b/src/software/ai/hl/stp/tactic/receiver/BUILD @@ -32,18 +32,3 @@ cc_test( "@googletest//:gtest_main", ], ) - -cc_test( - name = "receiver_tactic_test", - srcs = ["receiver_tactic_test.cpp"], - deps = [ - ":receiver_tactic", - "//shared/test_util:tbots_gtest_main", - "//software/simulated_tests:simulated_er_force_sim_play_test_fixture", - "//software/simulated_tests/cpp_validation:validation_function", - "//software/simulated_tests/terminating_validation_functions", - "//software/test_util", - "//software/time:duration", - "//software/world", - ], -) diff --git a/src/software/ai/hl/stp/tactic/receiver/receiver_tactic_test.cpp b/src/software/ai/hl/stp/tactic/receiver/receiver_tactic_test.cpp deleted file mode 100644 index 64daa96a22..0000000000 --- a/src/software/ai/hl/stp/tactic/receiver/receiver_tactic_test.cpp +++ /dev/null @@ -1,228 +0,0 @@ -#include "software/ai/hl/stp/tactic/receiver/receiver_tactic.h" - -#include - -#include - -#include "software/ai/hl/stp/tactic/move/move_tactic.h" -#include "software/geom/algorithms/contains.h" -#include "software/simulated_tests/cpp_validation/validation_function.h" -#include "software/simulated_tests/simulated_er_force_sim_play_test_fixture.h" -#include "software/simulated_tests/terminating_validation_functions/ball_kicked_validation.h" -#include "software/simulated_tests/terminating_validation_functions/friendly_scored_validation.h" -#include "software/simulated_tests/terminating_validation_functions/robot_received_ball_validation.h" -#include "software/simulated_tests/terminating_validation_functions/robot_state_validation.h" -#include "software/test_util/test_util.h" -#include "software/time/duration.h" -#include "software/world/world.h" - -class ReceiverTacticTest - : public virtual SimulatedErForceSimPlayTestFixture, - public ::testing::WithParamInterface> -{ - protected: - TbotsProto::FieldType field_type = TbotsProto::FieldType::DIV_B; - Field field = Field::createField(field_type); -}; - -TEST_P(ReceiverTacticTest, perfect_pass_receiver_test) -{ - Pass pass = std::get<0>(GetParam()); - RobotStateWithId robot_state = std::get<1>(GetParam()); - BallState ball_state = - BallState(pass.passerPoint(), - pass.speed() * (pass.receiverPoint() - pass.passerPoint()).normalize()); - - auto friendly_robots = TestUtil::createStationaryRobotStatesWithId({Point(-3, 2.5)}); - friendly_robots.emplace_back(robot_state); - - auto tactic = - std::make_shared(std::make_shared()); - tactic->updateControlParams(pass); - setTactic(1, tactic); - - std::vector terminating_validation_functions = { - [pass, tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) - { - // We check if the robot reaches the desired orientation, at the - // desired position before checking if the ball has been received. - // - // The tactic should "done" after receiving the ball. - robotAtOrientation(1, world_ptr, pass.receiverOrientation(), - Angle::fromDegrees(5), yield); - - // NOTE: we don't check robotAtPosition for receive and dribble - // because the robot is free to adjust itself to best receive - // the pass (and dribble). We only care if the robot received the ball. - robotReceivedBall(world_ptr, yield); - - while (!tactic->done()) - { - yield("Receiver tactic done but did not receive pass"); - } - }}; - - std::vector non_terminating_validation_functions = {}; - - runTest(field_type, ball_state, friendly_robots, {}, terminating_validation_functions, - non_terminating_validation_functions, Duration::fromSeconds(10)); -} - -INSTANTIATE_TEST_CASE_P( - PassEnvironmentReceiveAndDribble, ReceiverTacticTest, - ::testing::Values( - // TODO(#2909) : Enable test once the robot can turn faster and hits the ball with - //// Robot already at receive point - // std::make_tuple(Pass(Point(0.0, 0.5), Point(2, 2), 4), - // RobotStateWithId{ - // 1, RobotState(Point(2, 2), Vector(0, 0), - // Angle::fromDegrees(0), - // Angle::fromDegrees(0))}), - - //// Robot slighty off from receive point: test 1 - // std::make_tuple(Pass(Point(0.0, 0.4), Point(2, 2), 4), - // RobotStateWithId{ - // 1, RobotState(Point(2, 1.5), Vector(0, 0), - // Angle::fromDegrees(0), - // Angle::fromDegrees(0))}), - - // Robot slighty off from receive point: test 2 - std::make_tuple(Pass(Point(0.0, 0.4), Point(2, 2), 4), - RobotStateWithId{ - 1, RobotState(Point(2.5, 2.0), Vector(0, 0), - Angle::fromDegrees(0), Angle::fromDegrees(0))}), - - // Robot facing away from pass - std::make_tuple(Pass(Point(0.0, 0.0), Point(-3, 0), 4), - RobotStateWithId{1, RobotState(Point(-3, 0), Vector(0, 0), - Angle::fromDegrees(180), - Angle::fromDegrees(0))}), - - // Robot facing towards from pass - std::make_tuple(Pass(Point(0.0, 0.0), Point(-3, 0), 4), - RobotStateWithId{ - 1, RobotState(Point(-3, 0), Vector(0, 0), - Angle::fromDegrees(0), Angle::fromDegrees(0))}), - - // Robot facing towards pass speedy - std::make_tuple(Pass(Point(0.0, 0.0), Point(-3, 0), 5), - RobotStateWithId{ - 1, RobotState(Point(-3, 0), Vector(0, 0), - Angle::fromDegrees(0), Angle::fromDegrees(0))}) - - )); - -class ReceiverTacticTestOneTouch : public ReceiverTacticTest -{ -}; - -TEST_P(ReceiverTacticTestOneTouch, test_one_touch) -{ - Pass pass = std::get<0>(GetParam()); - RobotStateWithId robot_state = std::get<1>(GetParam()); - BallState ball_state = - BallState(pass.passerPoint(), - pass.speed() * (pass.receiverPoint() - pass.passerPoint()).normalize()); - - auto friendly_robots = TestUtil::createStationaryRobotStatesWithId({Point(-3, 2.5)}); - friendly_robots.emplace_back(robot_state); - - auto tactic = - std::make_shared(std::make_shared()); - tactic->updateControlParams(pass); - setTactic(1, tactic); - - std::vector terminating_validation_functions = { - [pass, tactic](std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) - { - // We just care if we scored! - friendlyScored(world_ptr, yield); - }}; - - std::vector non_terminating_validation_functions = {}; - - runTest(field_type, ball_state, friendly_robots, {}, terminating_validation_functions, - non_terminating_validation_functions, Duration::fromSeconds(5)); -} - -INSTANTIATE_TEST_CASE_P( - PassEnvironmentOneTouchShot, ReceiverTacticTestOneTouch, - ::testing::Values( - - // one touch robot on receiver point - // TODO (#2570): re-enable when one-touch works for these tests - // std::make_tuple(Pass(Point(2.0, 0.0), Point(3.5, 2.5), 3.5), - // RobotStateWithId{ - // 1, RobotState(Point(3.5, 2.5), Vector(0, 0), - // Angle::fromDegrees(0), - // Angle::fromDegrees(0))}), - // - // std::make_tuple(Pass(Point(2.0, 0.0), Point(3.5, -2.5), 3.5), - // RobotStateWithId{ - // 1, RobotState(Point(3.5, -2.5), Vector(0, 0), - // Angle::fromDegrees(0), - // Angle::fromDegrees(0))}), - // - // // one touch robot away from receiver point - // std::make_tuple(Pass(Point(1.5, 0.0), Point(2.5, 2.5), 3.5), - // RobotStateWithId{ - // 1, RobotState(Point(2.0, 2.5), Vector(0, 0), - // Angle::fromDegrees(0), - // Angle::fromDegrees(0))}), - // - // std::make_tuple(Pass(Point(1.5, 0.0), Point(2.5, -2.5), 3.5), - // RobotStateWithId{ - // 1, RobotState(Point(2.0, -2.5), Vector(0, 0), - // Angle::fromDegrees(0), - // Angle::fromDegrees(0))}), - - // Sharp angles, these are only a finite set of what - // sort of sharp angles we can achieve. - // - // If we are noticing issues with one-touch on the field, we should - // add more tests here and explore more of the "one-touch" space - - std::make_tuple(Pass(Point(4.0, 1.5), Point(4, -1), 5), - RobotStateWithId{1, RobotState(Point(4.0, -1), Vector(0, 0), - Angle::fromDegrees(180), - Angle::fromDegrees(0))}), - - // TODO (#2570): re-enable when one-touch works for these tests - // std::make_tuple(Pass(Point(4.0, 1.5), Point(3.5, -1), 5), - // RobotStateWithId{ - // 1, RobotState(Point(3.5, -1), Vector(0, 0), - // Angle::fromDegrees(0), - // Angle::fromDegrees(0))}), - - // TODO(#2909) : Enable test once the robot can turn faster and hits the ball with - // std::make_tuple(Pass(Point(4.0, 1.5), Point(3.0, -1), 4.5), - // RobotStateWithId{1, RobotState(Point(3.0, -1), Vector(0, 0), - // Angle::fromDegrees(180), - // Angle::fromDegrees(0))}), - - std::make_tuple(Pass(Point(4.0, -1.5), Point(4, 1), 5), - RobotStateWithId{1, RobotState(Point(4.0, 1), Vector(0, 0), - Angle::fromDegrees(180), - Angle::fromDegrees(0))}), - - // TODO (#2570): re-enable when one-touch works for these tests - // std::make_tuple(Pass(Point(4.0, -1.5), Point(3.5, 1), 5), - // RobotStateWithId{ - // 1, RobotState(Point(3.5, 1), Vector(0, 0), - // Angle::fromDegrees(0), - // Angle::fromDegrees(0))}), - - std::make_tuple(Pass(Point(4.0, -1.5), Point(3.0, 1), 4.5), - RobotStateWithId{ - 1, RobotState(Point(3.0, 1), Vector(0, 0), - Angle::fromDegrees(0), Angle::fromDegrees(0))}) - - // TODO (#2570): re-enable when one-touch works for these tests - // Direct one touch - // std::make_tuple(Pass(Point(3.0, 0.0), Point(2, 0), 4), - // RobotStateWithId{1, RobotState(Point(2, 0), Vector(0, 0), - // Angle::fromDegrees(0), - // Angle::fromDegrees(0))}) - )); diff --git a/src/software/sensor_fusion/filter/BUILD b/src/software/sensor_fusion/filter/BUILD index 9e00ea6b3e..edfed594c6 100644 --- a/src/software/sensor_fusion/filter/BUILD +++ b/src/software/sensor_fusion/filter/BUILD @@ -85,18 +85,3 @@ cc_library( ":robot_team_filter", ], ) - -cc_test( - name = "ball_occlusion_test", - srcs = ["ball_occlusion_test.cpp"], - deps = [ - "//shared/test_util:tbots_gtest_main", - "//software/ai/hl/stp/play/halt_play", - "//software/simulated_tests:simulated_er_force_sim_play_test_fixture", - "//software/simulated_tests/cpp_validation:validation_function", - "//software/simulated_tests/terminating_validation_functions", - "//software/test_util", - "//software/time:duration", - "//software/world", - ], -) diff --git a/src/software/sensor_fusion/filter/ball_occlusion_test.cpp b/src/software/sensor_fusion/filter/ball_occlusion_test.cpp deleted file mode 100644 index 0485cd0b2b..0000000000 --- a/src/software/sensor_fusion/filter/ball_occlusion_test.cpp +++ /dev/null @@ -1,119 +0,0 @@ -#include - -#include - -#include "software/simulated_tests/cpp_validation/validation_function.h" -#include "software/simulated_tests/simulated_er_force_sim_play_test_fixture.h" -#include "software/simulated_tests/terminating_validation_functions/robot_halt_validation.h" -#include "software/test_util/test_util.h" -#include "software/time/duration.h" -#include "software/world/game_state.h" -#include "software/world/world.h" - -class BallOcclusionTest - : public SimulatedErForceSimPlayTestFixture, - public ::testing::WithParamInterface, std::vector>> -{ - protected: - TbotsProto::FieldType field_type = TbotsProto::FieldType::DIV_B; -}; - -TEST_P(BallOcclusionTest, test_ball_occlusion) -{ - BallState ball_state = std::get<0>(GetParam()); - auto friendly_robots = std::get<1>(GetParam()); - auto enemy_robots = std::get<2>(GetParam()); - setFriendlyGoalie(0); - setEnemyGoalie(0); - setAiPlay(TbotsProto::PlayName::HaltPlay); - setRefereeCommand(RefereeCommand::HALT, RefereeCommand::HALT); - std::vector terminating_validating_function = { - [](std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) - { - while (world_ptr->getMostRecentTimestamp() < Timestamp::fromSeconds(9.5)) - { - yield("Timestamp not at 9.5s"); - } - }}; - std::vector non_terminating_validating_function = {}; - runTest(field_type, ball_state, friendly_robots, enemy_robots, - terminating_validating_function, non_terminating_validating_function, - Duration::fromSeconds(10)); -} - -INSTANTIATE_TEST_CASE_P( - BallOcclusionTests, BallOcclusionTest, - ::testing::Values( - std::make_tuple(BallState(Point(-4.5, 3), Vector(1, 0)), - TestUtil::createStationaryRobotStatesWithId( - {Point(-4, 2.8), Point(-2.5, 2.8), Point(-1, 2.8), - Point(0.5, 2.8), Point(2, 2.8), Point(3.5, 2.8)}), - TestUtil::createStationaryRobotStatesWithId({Point(4.5, 0)})), - std::make_tuple(BallState(Point(-4.5, -2), Vector(1, 0)), - TestUtil::createStationaryRobotStatesWithId( - {Point(-4, -1.8), Point(-2.5, -1.8), Point(-1, -1.8), - Point(0.5, -1.8), Point(2, -1.8), Point(3.5, -1.8)}), - TestUtil::createStationaryRobotStatesWithId({Point(4.5, 0)})), - std::make_tuple(BallState(Point(-4.5, 3), Vector(0, -1)), - TestUtil::createStationaryRobotStatesWithId( - {Point(-4.3, -2), Point(-4.3, -1), Point(-4.3, 0), - Point(-4.3, 1), Point(-4.3, 2), Point(-4.3, 2.2)}), - TestUtil::createStationaryRobotStatesWithId({Point(4.5, 0)})), - std::make_tuple(BallState(Point(4.5, 3), Vector(0, -1)), - TestUtil::createStationaryRobotStatesWithId( - {Point(4.3, -2), Point(4.3, -1), Point(4.3, 0), Point(4.3, 1), - Point(4.3, 2), Point(4.3, 2.2)}), - TestUtil::createStationaryRobotStatesWithId({Point(-4.5, 0)})), - std::make_tuple(BallState(Point(-3, 0), Vector(-1, 0)), - TestUtil::createStationaryRobotStatesWithId({Point(-4.5, 0)}), - TestUtil::createStationaryRobotStatesWithId({Point(-2.8, 0)})), - std::make_tuple(BallState(Point(3, 0), Vector(0, 1)), - TestUtil::createStationaryRobotStatesWithId({Point(2.8, 0)}), - TestUtil::createStationaryRobotStatesWithId({Point(4.5, 0)})), - std::make_tuple(BallState(Point(-3.5, -1), Vector(-1, 1)), - TestUtil::createStationaryRobotStatesWithId({Point(-4.5, 0)}), - TestUtil::createStationaryRobotStatesWithId( - {Point(-3.5 + 0.2 / sqrt(2.0), -1 - 0.2 / sqrt(2.0))})), - std::make_tuple(BallState(Point(3.5, -1), Vector(1, 1)), - TestUtil::createStationaryRobotStatesWithId( - {Point(3.5 - 0.2 / sqrt(2.0), -1 - 0.2 / sqrt(2.0))}), - TestUtil::createStationaryRobotStatesWithId({Point(4.5, 0)})), - std::make_tuple(BallState(Point(-3.5, 1), Vector(-1, -1)), - TestUtil::createStationaryRobotStatesWithId({Point(-4.5, 0)}), - TestUtil::createStationaryRobotStatesWithId( - {Point(-3.5 + 0.2 / sqrt(2.0), 1 + 0.2 / sqrt(2.0))})), - std::make_tuple(BallState(Point(3.5, 1), Vector(1, -1)), - TestUtil::createStationaryRobotStatesWithId( - {Point(3.5 - 0.2 / sqrt(2), 1 + 0.2 / sqrt(2))}), - TestUtil::createStationaryRobotStatesWithId({Point(4.5, 0)})), - std::make_tuple(BallState(Point(-4.5 + 0.2 / sqrt(2.0), 3 - 0.2 / sqrt(2.0)), - Vector(3.0 / sqrt(13.0), -2.0 / sqrt(13.0))), - TestUtil::createStationaryRobotStatesWithId({Point(-4.5, 3), - Point(-1.5, 1)}), - TestUtil::createStationaryRobotStatesWithId({Point(4.5, 0)})), - std::make_tuple(BallState(Point(-4.5 + 0.23 / sqrt(2.0), -3 + 0.23 / sqrt(2.0)), - Vector(3.0 / sqrt(13.0), 2.0 / sqrt(13.0))), - TestUtil::createStationaryRobotStatesWithId({Point(-4.5, -3), - Point(-1.5, -1)}), - TestUtil::createStationaryRobotStatesWithId({Point(4.5, 0)})), - std::make_tuple(BallState(Point(4.5 - 0.2 / sqrt(2.0), 3 - 0.2 / sqrt(2.0)), - Vector(-3.0 / sqrt(13.0), -2.0 / sqrt(13.0))), - TestUtil::createStationaryRobotStatesWithId({Point(4.5, 3), - Point(1.5, 1)}), - TestUtil::createStationaryRobotStatesWithId({Point(4.5, 0)})), - std::make_tuple(BallState(Point(4.5 - 0.23 / sqrt(2.0), -3 + 0.23 / sqrt(2.0)), - Vector(-3.0 / sqrt(13.0), 2.0 / sqrt(13.0))), - TestUtil::createStationaryRobotStatesWithId({Point(4.5, -3), - Point(1.5, -1)}), - TestUtil::createStationaryRobotStatesWithId({Point(-4.5, 0)})), - std::make_tuple(BallState(Point(-4, -3), Vector(1, 3)), - TestUtil::createStationaryRobotStatesWithId( - {Point(-3.9, -2.9), Point(-2.9, 0.1), Point(-1.9, 3.1)}), - TestUtil::createStationaryRobotStatesWithId( - {Point(-4.1, -3.1), Point(-3.1, -0.1), Point(-2.1, 2.9)})), - std::make_tuple(BallState(Point(-4, -3), Vector(4, 1)), - TestUtil::createStationaryRobotStatesWithId( - {Point(-3.9, -2.9), Point(0.1, -1.9), Point(4.1, -0.9)}), - TestUtil::createStationaryRobotStatesWithId( - {Point(-4.1, -3.1), Point(-0.1, -2.1), Point(3.9, -1.1)})))); From 9112248785c5802d4d2f64d33a2a5a09d58adaf6 Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Sat, 4 Apr 2026 16:26:58 -0700 Subject: [PATCH 05/57] Remove c++ simulated test fixtures and validations --- src/software/simulated_tests/BUILD | 37 -- .../simulated_tests/cpp_validation/BUILD | 59 -- .../non_terminating_function_validator.cpp | 56 -- .../non_terminating_function_validator.h | 65 -- ...on_terminating_function_validator_test.cpp | 65 -- .../terminating_function_validator.cpp | 55 -- .../terminating_function_validator.h | 73 --- .../terminating_function_validator_test.cpp | 216 ------- .../cpp_validation/validation_function.h | 13 - .../BUILD | 36 -- .../ball_in_play_or_scored_validation.cpp | 12 - .../ball_in_play_or_scored_validation.h | 15 - .../ball_never_moves_backward_validation.cpp | 17 - .../ball_never_moves_backward_validation.h | 18 - .../enemy_never_scores_validation.cpp | 14 - .../enemy_never_scores_validation.h | 13 - ...t_not_excessively_dribbling_validation.cpp | 34 -- ...bot_not_excessively_dribbling_validation.h | 16 - .../robots_avoid_ball_validation.cpp | 27 - .../robots_avoid_ball_validation.h | 18 - .../robots_in_friendly_half_validation.cpp | 29 - .../robots_in_friendly_half_validation.h | 23 - ...robots_not_in_center_circle_validation.cpp | 30 - .../robots_not_in_center_circle_validation.h | 22 - .../robots_slow_down_validation.cpp | 20 - .../robots_slow_down_validation.h | 13 - .../robots_violating_motion_constraint.cpp | 23 - .../robots_violating_motion_constraint.h | 20 - ...mulated_er_force_sim_play_test_fixture.cpp | 108 ---- ...simulated_er_force_sim_play_test_fixture.h | 92 --- .../simulated_er_force_sim_test_fixture.cpp | 558 ------------------ .../simulated_er_force_sim_test_fixture.h | 205 ------- .../terminating_validation_functions/BUILD | 33 -- .../ball_at_point_validation.cpp | 15 - .../ball_at_point_validation.h | 13 - .../ball_kicked_validation.cpp | 15 - .../ball_kicked_validation.h | 14 - .../friendly_scored_validation.cpp | 12 - .../friendly_scored_validation.h | 12 - .../robot_halt_validation.cpp | 18 - .../robot_halt_validation.h | 11 - .../robot_in_center_circle_validation.cpp | 25 - .../robot_in_center_circle_validation.h | 12 - .../robot_in_polygon_validation.cpp | 32 - .../robot_in_polygon_validation.h | 14 - .../robot_received_ball_validation.cpp | 24 - .../robot_received_ball_validation.h | 12 - .../robot_state_validation.cpp | 107 ---- .../robot_state_validation.h | 51 -- 49 files changed, 2422 deletions(-) delete mode 100644 src/software/simulated_tests/cpp_validation/BUILD delete mode 100644 src/software/simulated_tests/cpp_validation/non_terminating_function_validator.cpp delete mode 100644 src/software/simulated_tests/cpp_validation/non_terminating_function_validator.h delete mode 100644 src/software/simulated_tests/cpp_validation/non_terminating_function_validator_test.cpp delete mode 100644 src/software/simulated_tests/cpp_validation/terminating_function_validator.cpp delete mode 100644 src/software/simulated_tests/cpp_validation/terminating_function_validator.h delete mode 100644 src/software/simulated_tests/cpp_validation/terminating_function_validator_test.cpp delete mode 100644 src/software/simulated_tests/cpp_validation/validation_function.h delete mode 100644 src/software/simulated_tests/non_terminating_validation_functions/BUILD delete mode 100644 src/software/simulated_tests/non_terminating_validation_functions/ball_in_play_or_scored_validation.cpp delete mode 100644 src/software/simulated_tests/non_terminating_validation_functions/ball_in_play_or_scored_validation.h delete mode 100644 src/software/simulated_tests/non_terminating_validation_functions/ball_never_moves_backward_validation.cpp delete mode 100644 src/software/simulated_tests/non_terminating_validation_functions/ball_never_moves_backward_validation.h delete mode 100644 src/software/simulated_tests/non_terminating_validation_functions/enemy_never_scores_validation.cpp delete mode 100644 src/software/simulated_tests/non_terminating_validation_functions/enemy_never_scores_validation.h delete mode 100644 src/software/simulated_tests/non_terminating_validation_functions/robot_not_excessively_dribbling_validation.cpp delete mode 100644 src/software/simulated_tests/non_terminating_validation_functions/robot_not_excessively_dribbling_validation.h delete mode 100644 src/software/simulated_tests/non_terminating_validation_functions/robots_avoid_ball_validation.cpp delete mode 100644 src/software/simulated_tests/non_terminating_validation_functions/robots_avoid_ball_validation.h delete mode 100644 src/software/simulated_tests/non_terminating_validation_functions/robots_in_friendly_half_validation.cpp delete mode 100644 src/software/simulated_tests/non_terminating_validation_functions/robots_in_friendly_half_validation.h delete mode 100644 src/software/simulated_tests/non_terminating_validation_functions/robots_not_in_center_circle_validation.cpp delete mode 100644 src/software/simulated_tests/non_terminating_validation_functions/robots_not_in_center_circle_validation.h delete mode 100644 src/software/simulated_tests/non_terminating_validation_functions/robots_slow_down_validation.cpp delete mode 100644 src/software/simulated_tests/non_terminating_validation_functions/robots_slow_down_validation.h delete mode 100644 src/software/simulated_tests/non_terminating_validation_functions/robots_violating_motion_constraint.cpp delete mode 100644 src/software/simulated_tests/non_terminating_validation_functions/robots_violating_motion_constraint.h delete mode 100644 src/software/simulated_tests/simulated_er_force_sim_play_test_fixture.cpp delete mode 100644 src/software/simulated_tests/simulated_er_force_sim_play_test_fixture.h delete mode 100644 src/software/simulated_tests/simulated_er_force_sim_test_fixture.cpp delete mode 100644 src/software/simulated_tests/simulated_er_force_sim_test_fixture.h delete mode 100644 src/software/simulated_tests/terminating_validation_functions/BUILD delete mode 100644 src/software/simulated_tests/terminating_validation_functions/ball_at_point_validation.cpp delete mode 100644 src/software/simulated_tests/terminating_validation_functions/ball_at_point_validation.h delete mode 100644 src/software/simulated_tests/terminating_validation_functions/ball_kicked_validation.cpp delete mode 100644 src/software/simulated_tests/terminating_validation_functions/ball_kicked_validation.h delete mode 100644 src/software/simulated_tests/terminating_validation_functions/friendly_scored_validation.cpp delete mode 100644 src/software/simulated_tests/terminating_validation_functions/friendly_scored_validation.h delete mode 100644 src/software/simulated_tests/terminating_validation_functions/robot_halt_validation.cpp delete mode 100644 src/software/simulated_tests/terminating_validation_functions/robot_halt_validation.h delete mode 100644 src/software/simulated_tests/terminating_validation_functions/robot_in_center_circle_validation.cpp delete mode 100644 src/software/simulated_tests/terminating_validation_functions/robot_in_center_circle_validation.h delete mode 100644 src/software/simulated_tests/terminating_validation_functions/robot_in_polygon_validation.cpp delete mode 100644 src/software/simulated_tests/terminating_validation_functions/robot_in_polygon_validation.h delete mode 100644 src/software/simulated_tests/terminating_validation_functions/robot_received_ball_validation.cpp delete mode 100644 src/software/simulated_tests/terminating_validation_functions/robot_received_ball_validation.h delete mode 100644 src/software/simulated_tests/terminating_validation_functions/robot_state_validation.cpp delete mode 100644 src/software/simulated_tests/terminating_validation_functions/robot_state_validation.h diff --git a/src/software/simulated_tests/BUILD b/src/software/simulated_tests/BUILD index 37dd733098..d1acb78f3e 100644 --- a/src/software/simulated_tests/BUILD +++ b/src/software/simulated_tests/BUILD @@ -55,43 +55,6 @@ py_library( ], ) -# TODO (#2581): remove -cc_library( - name = "simulated_er_force_sim_test_fixture", - testonly = True, - srcs = ["simulated_er_force_sim_test_fixture.cpp"], - hdrs = ["simulated_er_force_sim_test_fixture.h"], - deps = [ - "//software/logger", - "//proto/message_translation:tbots_protobuf", - "//proto:play_info_msg_cc_proto", - "//software/sensor_fusion", - "//software/simulated_tests/cpp_validation:non_terminating_function_validator", - "//software/simulated_tests/cpp_validation:terminating_function_validator", - "//software/simulation:er_force_simulator", - "//software/test_util", - "//software/time:duration", - "//shared/test_util:tbots_gtest_main", - "//shared/test_util", - # TODO (#1889) Remove this dep after optional params are implemented - "//software/ai/hl/stp/play/halt_play", - "//proto/message_translation:er_force_world", - ], -) - -# TODO (#2581): remove -cc_library( - name = "simulated_er_force_sim_play_test_fixture", - testonly = True, - srcs = ["simulated_er_force_sim_play_test_fixture.cpp"], - hdrs = ["simulated_er_force_sim_play_test_fixture.h"], - deps = [ - ":simulated_er_force_sim_test_fixture", - "//software/ai", - "@googletest//:gtest", - ], -) - py_test( name = "simulated_test_ball_model", srcs = [ diff --git a/src/software/simulated_tests/cpp_validation/BUILD b/src/software/simulated_tests/cpp_validation/BUILD deleted file mode 100644 index 80276535e0..0000000000 --- a/src/software/simulated_tests/cpp_validation/BUILD +++ /dev/null @@ -1,59 +0,0 @@ -package(default_visibility = ["//visibility:public"]) - -cc_library( - name = "validation_function", - testonly = True, - hdrs = ["validation_function.h"], - deps = [ - "//software/world", - "@boost//:coroutine2", - "@googletest//:gtest", - ], -) - -cc_library( - name = "terminating_function_validator", - testonly = True, - srcs = ["terminating_function_validator.cpp"], - hdrs = ["terminating_function_validator.h"], - deps = [ - ":validation_function", - "@boost//:coroutine2", - ], -) - -cc_test( - name = "terminating_function_validator_test", - srcs = ["terminating_function_validator_test.cpp"], - deps = [ - ":terminating_function_validator", - ":validation_function", - "//shared/test_util:tbots_gtest_main", - "//software/test_util", - "//software/world", - ], -) - -cc_library( - name = "non_terminating_function_validator", - testonly = True, - srcs = ["non_terminating_function_validator.cpp"], - hdrs = ["non_terminating_function_validator.h"], - deps = [ - ":validation_function", - "@boost//:coroutine2", - "@googletest//:gtest", - ], -) - -cc_test( - name = "non_terminating_function_validator_test", - srcs = ["non_terminating_function_validator_test.cpp"], - deps = [ - ":non_terminating_function_validator", - ":validation_function", - "//shared/test_util:tbots_gtest_main", - "//software/test_util", - "//software/world", - ], -) diff --git a/src/software/simulated_tests/cpp_validation/non_terminating_function_validator.cpp b/src/software/simulated_tests/cpp_validation/non_terminating_function_validator.cpp deleted file mode 100644 index eed7912bfa..0000000000 --- a/src/software/simulated_tests/cpp_validation/non_terminating_function_validator.cpp +++ /dev/null @@ -1,56 +0,0 @@ -#include "software/simulated_tests/cpp_validation/non_terminating_function_validator.h" - -#include - -NonTerminatingFunctionValidator::NonTerminatingFunctionValidator( - ValidationFunction validation_function, std::shared_ptr world) - : // We need to provide the world and validation_function in the coroutine function - // binding so that the wrapper function has access to the correct variable context, - // otherwise the World inside the coroutine will not update properly when the - // pointer is updated, and the wrong validation_function may be run. - validation_sequence( - std::bind(&NonTerminatingFunctionValidator::executeAndCheckForFailuresWrapper, - this, std::placeholders::_1, world, validation_function)), - world_(world), - validation_function_(validation_function) -{ -} - -std::optional NonTerminatingFunctionValidator::executeAndCheckForFailures() -{ - // Check the coroutine status to see if it has any more work to do. - if (!validation_sequence) - { - // Re-start the coroutine by re-creating it - validation_sequence = ValidationCoroutine::pull_type( - std::bind(&NonTerminatingFunctionValidator::executeAndCheckForFailuresWrapper, - this, std::placeholders::_1, world_, validation_function_)); - } - - // Run the coroutine. This will call the bound executeAndCheckForFailuresWrapper - // function - validation_sequence(); - std::string error_msg = validation_sequence.get(); - if (error_msg != "") - { - return error_msg; - } - else - { - return std::nullopt; - } -} - -void NonTerminatingFunctionValidator::executeAndCheckForFailuresWrapper( - ValidationCoroutine::push_type &yield, std::shared_ptr world, - ValidationFunction validation_function) -{ - // Yield the very first time the function is called, so that the validation_function - // is not run until this coroutine / wrapper function is called again by - // executeAndCheckForFailures - yield(""); - - // Anytime after the first function call, the validation_function will be - // used to perform the real logic. - validation_function(world, yield); -} diff --git a/src/software/simulated_tests/cpp_validation/non_terminating_function_validator.h b/src/software/simulated_tests/cpp_validation/non_terminating_function_validator.h deleted file mode 100644 index ce5f234ddd..0000000000 --- a/src/software/simulated_tests/cpp_validation/non_terminating_function_validator.h +++ /dev/null @@ -1,65 +0,0 @@ -#pragma once - -#include "software/simulated_tests/cpp_validation/validation_function.h" - -/** - * This class is a wrapper to make it easier to work with ValidationFunctions. It provides - * an easy way to manage the coroutines required to run ValidationFunctions as well as see - * if the function has succeeded / passed. - * - * This class will run the provided coroutine continuously by restarting it every time the - * coroutine has completed. - * - * If the Validation Function yields a non-empty error message, then the validator will - * add a failure with the error message - */ -class NonTerminatingFunctionValidator -{ - public: - /** - * Creates a new NonTerminatingFunctionValidator. - * - * @param validation_function The ValidationFunction this - * NonTerminatingFunctionValidator should manage and run - * @param world The world that will be given to the ValidationFunction in order to run - * it - */ - explicit NonTerminatingFunctionValidator(ValidationFunction validation_function, - std::shared_ptr world); - - /** - * Runs the ValidationFunction that was given to this NonTerminatingFunctionValidator - * on construction. The ValidationFunction will be restarted if it has completed. As - * such, the ValidationFunction can never be "done" and will only terminate due to - * failures within the ValidationFunction, such as a failed GoogleTest assert - * - * @return Error message if the non-terminating validation function failed - */ - std::optional executeAndCheckForFailures(); - - private: - /** - * A wrapper function for the validation_function. - * - * This function exists because when the coroutine (validation_sequence) is first - * constructed the coroutine is called/entered. This would normally cause the - * validation_sequence function to be run once which may cause unexpected results. - * Additionally, it is easier to bind the coroutine to a wrapper function and have the - * wrapper function pass any arguments to the validation_function rather than pass - * all parameters through coroutines. - * - * @param yield The coroutine push_type for the validation_function - * @param world The world that will be given to the validation_function being run. - * Because it's a shared_ptr any external changes made to the world will be reflected - * inside the validation_function. - * @param validation_function The validation_function to run in the coroutine - */ - void executeAndCheckForFailuresWrapper(ValidationCoroutine::push_type& yield, - std::shared_ptr world, - ValidationFunction validation_function); - - // The coroutine that will be given to the validation function - ValidationCoroutine::pull_type validation_sequence; - std::shared_ptr world_; - ValidationFunction validation_function_; -}; diff --git a/src/software/simulated_tests/cpp_validation/non_terminating_function_validator_test.cpp b/src/software/simulated_tests/cpp_validation/non_terminating_function_validator_test.cpp deleted file mode 100644 index 5d5310e915..0000000000 --- a/src/software/simulated_tests/cpp_validation/non_terminating_function_validator_test.cpp +++ /dev/null @@ -1,65 +0,0 @@ -#include "software/simulated_tests/cpp_validation/non_terminating_function_validator.h" - -#include -#include - -#include - -#include "software/simulated_tests/cpp_validation/validation_function.h" -#include "software/test_util/test_util.h" - -TEST(NonTerminatingFunctionValidatorTest, - test_validation_function_that_does_nothing_does_not_report_failure) -{ - ValidationFunction validation_function = [](std::shared_ptr world, - ValidationCoroutine::push_type& yield) {}; - - auto world = ::TestUtil::createBlankTestingWorld(); - world->updateBall( - Ball(BallState(Point(-0.1, 0), Vector(0, 0)), Timestamp::fromSeconds(0))); - NonTerminatingFunctionValidator function_validator(validation_function, world); - - for (unsigned int i = 0; i < 10; i++) - { - EXPECT_FALSE(function_validator.executeAndCheckForFailures()); - } -} - -TEST(NonTerminatingFunctionValidatorTest, test_yielding_error_message) -{ - // This validation_functions uses exceptions as a way for the test to observe it's - // internal state The exception will not be reached until the 3rd function call - ValidationFunction validation_function = - [](std::shared_ptr world, ValidationCoroutine::push_type& yield) - { - yield("This is an error message 1"); - yield("This is an error message 2"); - }; - - auto world = ::TestUtil::createBlankTestingWorld(); - NonTerminatingFunctionValidator function_validator(validation_function, world); - EXPECT_EQ(function_validator.executeAndCheckForFailures(), - "This is an error message 1"); - EXPECT_EQ(function_validator.executeAndCheckForFailures(), - "This is an error message 2"); -} - -TEST(NonTerminatingFunctionValidatorTest, - test_world_updated_correctly_between_validation_function_restarts) -{ - ValidationFunction validation_function = - [](std::shared_ptr world, ValidationCoroutine::push_type& yield) - { - if (world->ball().position() != Point(1, 1)) - { - yield("Ball not at (1,1)"); - } - }; - - auto world = ::TestUtil::createBlankTestingWorld(); - NonTerminatingFunctionValidator function_validator(validation_function, world); - EXPECT_EQ(function_validator.executeAndCheckForFailures(), "Ball not at (1,1)"); - world->updateBall( - Ball(BallState(Point(1, 1), Vector()), Timestamp::fromSeconds(123))); - EXPECT_FALSE(function_validator.executeAndCheckForFailures()); -} diff --git a/src/software/simulated_tests/cpp_validation/terminating_function_validator.cpp b/src/software/simulated_tests/cpp_validation/terminating_function_validator.cpp deleted file mode 100644 index d52c54679c..0000000000 --- a/src/software/simulated_tests/cpp_validation/terminating_function_validator.cpp +++ /dev/null @@ -1,55 +0,0 @@ -#include "software/simulated_tests/cpp_validation/terminating_function_validator.h" - -TerminatingFunctionValidator::TerminatingFunctionValidator( - ValidationFunction validation_function, - std::shared_ptr world) - : // We need to provide the world and validation_function in the coroutine function - // binding so that the wrapper function has access to the correct variable context, - // otherwise the World inside the coroutine will not update properly when the - // pointer is updated, and the wrong validation_function may be run. - validation_sequence( - std::bind(&TerminatingFunctionValidator::executeAndCheckForSuccessWrapper, this, - std::placeholders::_1, world, validation_function)), - current_error_message("") -{ -} - -std::string TerminatingFunctionValidator::currentErrorMessage() const -{ - return current_error_message; -} - -void TerminatingFunctionValidator::executeAndCheckForSuccessWrapper( - ValidationCoroutine::push_type &yield, std::shared_ptr world, - ValidationFunction validation_function) -{ - // Yield the very first time the function is called, so that the validation_function - // is not run until this coroutine / wrapper function is called again by - // executeAndCheckForSuccess - yield(""); - - // Anytime after the first function call, the validation_function will be - // used to perform the real logic. - validation_function(world, yield); -} - -bool TerminatingFunctionValidator::executeAndCheckForSuccess() -{ - // Check the coroutine status to see if it has any more work to do. - if (validation_sequence) - { - // Run the coroutine. This will call the bound executeAndCheckForSuccessWrapper - // function - validation_sequence(); - current_error_message = validation_sequence.get(); - } - else - { - current_error_message = ""; - } - - // The validation_function is done if the coroutine evaluates to false, which means - // execution has "dropped out" the bottom of the function and there is no more work to - // do. If this is the case then the validation_function has passed successfully - return !static_cast(validation_sequence); -} diff --git a/src/software/simulated_tests/cpp_validation/terminating_function_validator.h b/src/software/simulated_tests/cpp_validation/terminating_function_validator.h deleted file mode 100644 index 09377a9465..0000000000 --- a/src/software/simulated_tests/cpp_validation/terminating_function_validator.h +++ /dev/null @@ -1,73 +0,0 @@ -#pragma once - -#include "software/simulated_tests/cpp_validation/validation_function.h" - -/** - * This class is a wrapper to make it easier to work with ValidationFunctions. It provides - * an easy way to manage the coroutines required to run ValidationFunctions as well as see - * if the function has succeeded / passed. - * - * This class will run the provided coroutine a single time and stop once it has - * completed. The coroutine will NOT be restarted upon completion. - * - * Provides access to the most recent error message returned by the validation function - */ -class TerminatingFunctionValidator -{ - public: - /** - * Creates a new TerminatingFunctionValidator. - * - * @param validation_function The ValidationFunction this - * TerminatingFunctionValidator should manage and run - * @param world The world that will be given to the ValidationFunction in order to run - * it - */ - explicit TerminatingFunctionValidator(ValidationFunction validation_function, - std::shared_ptr world); - - /** - * Runs the ValidationFunction that was given to this TerminatingFunctionValidator on - * construction and returns true if the ValidationFunction has succeeded / passed. - * Returns false otherwise. Once the ValidationFunction has succeeded (returned true - * once), the coroutine will not be restarted and this function will always return - * true without re-running the ValidationFunction. - * - * @return true if the internal ValidationFunction has succeeded / passed, and false - * otherwise - */ - bool executeAndCheckForSuccess(); - - /** - * Returns the current error message for the validator, empty string if no error - * - * @return the current error message - */ - std::string currentErrorMessage() const; - - private: - /** - * A wrapper function for the validation_function. - * - * This function exists because when the coroutine (validation_sequence) is first - * constructed the coroutine is called/entered. This would normally cause the - * validation_sequence function to be run once which may cause unexpected results. - * Additionally, it is easier to bind the coroutine to a wrapper function and have the - * wrapper function pass any arguments to the validation_function rather than pass - * all parameters through coroutines. - * - * @param yield The coroutine push_type for the validation_function - * @param world The world that will be given to the validation_function being run. - * Because it's a shared_ptr any external changes made to the world will be reflected - * inside the validation_function. - * @param validation_function The validation_function to run in the coroutine - */ - void executeAndCheckForSuccessWrapper(ValidationCoroutine::push_type& yield, - std::shared_ptr world, - ValidationFunction validation_function); - - // The coroutine that will be given to the validation function - ValidationCoroutine::pull_type validation_sequence; - - std::string current_error_message; -}; diff --git a/src/software/simulated_tests/cpp_validation/terminating_function_validator_test.cpp b/src/software/simulated_tests/cpp_validation/terminating_function_validator_test.cpp deleted file mode 100644 index 56a3bdec95..0000000000 --- a/src/software/simulated_tests/cpp_validation/terminating_function_validator_test.cpp +++ /dev/null @@ -1,216 +0,0 @@ -#include "software/simulated_tests/cpp_validation/terminating_function_validator.h" - -#include -#include - -#include - -#include "software/simulated_tests/cpp_validation/validation_function.h" -#include "software/test_util/test_util.h" - -TEST(TerminatingFunctionValidatorTest, - test_validation_function_that_does_nothing_reports_success) -{ - ValidationFunction validation_function = [](std::shared_ptr world, - ValidationCoroutine::push_type& yield) {}; - - auto world = ::TestUtil::createBlankTestingWorld(); - TerminatingFunctionValidator function_validator(validation_function, world); - bool result = function_validator.executeAndCheckForSuccess(); - EXPECT_TRUE(result); -} - -TEST(TerminatingFunctionValidatorTest, - test_validation_function_that_has_code_but_does_not_yield_reports_success) -{ - ValidationFunction validation_function = - [](std::shared_ptr world, ValidationCoroutine::push_type& yield) - { - int foo = 0; - int bar = 3; - int baz = foo + bar; - baz++; - }; - - auto world = ::TestUtil::createBlankTestingWorld(); - TerminatingFunctionValidator function_validator(validation_function, world); - bool result = function_validator.executeAndCheckForSuccess(); - EXPECT_TRUE(result); -} - -TEST(TerminatingFunctionValidatorTest, - test_validation_function_that_yields_once_succeeds_on_the_second_execution) -{ - ValidationFunction validation_function = - [](std::shared_ptr world, ValidationCoroutine::push_type& yield) - { yield("Test message"); }; - - auto world = ::TestUtil::createBlankTestingWorld(); - TerminatingFunctionValidator function_validator(validation_function, world); - bool result = function_validator.executeAndCheckForSuccess(); - EXPECT_FALSE(result); - EXPECT_EQ("Test message", function_validator.currentErrorMessage()); - result = function_validator.executeAndCheckForSuccess(); - EXPECT_TRUE(result); -} - -TEST(TerminatingFunctionValidatorTest, - test_validation_function_that_yields_five_time_succeeds_on_the_sixth_execution) -{ - ValidationFunction validation_function = - [](std::shared_ptr world, ValidationCoroutine::push_type& yield) - { - yield("Test message"); - yield("Test message"); - yield("Test message"); - yield("Test message"); - yield("Test message"); - }; - - auto world = ::TestUtil::createBlankTestingWorld(); - TerminatingFunctionValidator function_validator(validation_function, world); - - for (unsigned int i = 0; i < 5; i++) - { - bool result = function_validator.executeAndCheckForSuccess(); - EXPECT_FALSE(result); - EXPECT_EQ("Test message", function_validator.currentErrorMessage()); - } - - bool result = function_validator.executeAndCheckForSuccess(); - EXPECT_TRUE(result); -} - -TEST(TerminatingFunctionValidatorTest, - test_validation_function_with_early_return_reports_success_after_return) -{ - ValidationFunction validation_function = - [](std::shared_ptr world, ValidationCoroutine::push_type& yield) - { - yield("First validation not done yet"); - return; - yield("Second validation not done yet"); - }; - - auto world = ::TestUtil::createBlankTestingWorld(); - TerminatingFunctionValidator function_validator(validation_function, world); - - bool result = function_validator.executeAndCheckForSuccess(); - EXPECT_FALSE(result); - EXPECT_EQ("First validation not done yet", function_validator.currentErrorMessage()); - - result = function_validator.executeAndCheckForSuccess(); - EXPECT_TRUE(result); -} - -TEST(TerminatingFunctionValidatorTest, - test_validation_function_with_single_loop_succeeds_after_loop_termination) -{ - ValidationFunction validation_function = - [](std::shared_ptr world, ValidationCoroutine::push_type& yield) - { - while (world->ball().position().x() < 0) - { - yield("The ball's x position is not less than 0"); - } - }; - - auto world = ::TestUtil::createBlankTestingWorld(); - TerminatingFunctionValidator function_validator(validation_function, world); - - world->updateBall( - Ball(BallState(Point(-1, 0), Vector(0, 0)), Timestamp::fromSeconds(0))); - bool result = function_validator.executeAndCheckForSuccess(); - EXPECT_FALSE(result); - EXPECT_EQ("The ball's x position is not less than 0", - function_validator.currentErrorMessage()); - - world->updateBall( - Ball(BallState(Point(-0.1, 0), Vector(0, 0)), Timestamp::fromSeconds(0))); - result = function_validator.executeAndCheckForSuccess(); - EXPECT_FALSE(result); - EXPECT_EQ("The ball's x position is not less than 0", - function_validator.currentErrorMessage()); - - world->updateBall( - Ball(BallState(Point(0.05, 0), Vector(0, 0)), Timestamp::fromSeconds(0))); - result = function_validator.executeAndCheckForSuccess(); - EXPECT_TRUE(result); -} - -TEST(TerminatingFunctionValidatorTest, - test_validation_function_with_two_loops_succeeds_after_both_loops_terminate_in_order) -{ - // This validation function will only pass if the ball's x-coordinate becomes positive - // before the ball's y-coordinate becomes positive - ValidationFunction validation_function = - [](std::shared_ptr world, ValidationCoroutine::push_type& yield) - { - while (world->ball().position().x() < 0) - { - yield("The ball's x position is not less than 0"); - } - - while (world->ball().position().y() < 0) - { - yield("The ball's y position not less than 0"); - } - }; - - auto world = ::TestUtil::createBlankTestingWorld(); - TerminatingFunctionValidator function_validator(validation_function, world); - - world->updateBall( - Ball(BallState(Point(-1, -1), Vector(0, 0)), Timestamp::fromSeconds(0))); - bool result = function_validator.executeAndCheckForSuccess(); - EXPECT_FALSE(result); - EXPECT_EQ("The ball's x position is not less than 0", - function_validator.currentErrorMessage()); - - world->updateBall( - Ball(BallState(Point(1, -1), Vector(0, 0)), Timestamp::fromSeconds(0))); - result = function_validator.executeAndCheckForSuccess(); - EXPECT_FALSE(result); - EXPECT_EQ("The ball's y position not less than 0", - function_validator.currentErrorMessage()); - - world->updateBall( - Ball(BallState(Point(1, 1), Vector(0, 0)), Timestamp::fromSeconds(0))); - result = function_validator.executeAndCheckForSuccess(); - EXPECT_TRUE(result); -} - -TEST(TerminatingFunctionValidatorTest, test_validation_function_error_message) -{ - // This shows an example of using GoogleTest statements within a validation function. - // Just like regular unit tests, if the condition is not met the test will fail. - ValidationFunction validation_function = - [](std::shared_ptr world, ValidationCoroutine::push_type& yield) - { - while (world->gameState().isStopped()) - { - yield("Test message"); - } - }; - - auto world = ::TestUtil::createBlankTestingWorld(); - TerminatingFunctionValidator function_validator(validation_function, world); - - world->updateRefereeCommand(RefereeCommand::STOP); - world->updateBall( - Ball(BallState(Point(0, 0), Vector(1, 1)), Timestamp::fromSeconds(0))); - for (unsigned int i = 0; i < 10; i++) - { - bool result = function_validator.executeAndCheckForSuccess(); - EXPECT_FALSE(result); - EXPECT_EQ("Test message", function_validator.currentErrorMessage()); - } - - for (unsigned int i = 0; i < World::REFEREE_COMMAND_BUFFER_SIZE; i++) - { - world->updateRefereeCommand(RefereeCommand::HALT); - } - - bool result = function_validator.executeAndCheckForSuccess(); - EXPECT_TRUE(result); -} diff --git a/src/software/simulated_tests/cpp_validation/validation_function.h b/src/software/simulated_tests/cpp_validation/validation_function.h deleted file mode 100644 index 560ff2950c..0000000000 --- a/src/software/simulated_tests/cpp_validation/validation_function.h +++ /dev/null @@ -1,13 +0,0 @@ -#pragma once - -#include -#include - -#include "software/world/world.h" - -// The Validation Coroutine yields an error message in the form of a string when -// the passing condition is not true -using ValidationCoroutine = boost::coroutines2::coroutine; -// The Validation Function analyses the world and yields an error message as appropriate -using ValidationFunction = - std::function, ValidationCoroutine::push_type&)>; diff --git a/src/software/simulated_tests/non_terminating_validation_functions/BUILD b/src/software/simulated_tests/non_terminating_validation_functions/BUILD deleted file mode 100644 index cfe6d74520..0000000000 --- a/src/software/simulated_tests/non_terminating_validation_functions/BUILD +++ /dev/null @@ -1,36 +0,0 @@ -package(default_visibility = ["//visibility:public"]) - -cc_library( - name = "non_terminating_validation_functions", - testonly = True, - srcs = [ - "ball_in_play_or_scored_validation.cpp", - "ball_never_moves_backward_validation.cpp", - "enemy_never_scores_validation.cpp", - "robot_not_excessively_dribbling_validation.cpp", - "robots_avoid_ball_validation.cpp", - "robots_in_friendly_half_validation.cpp", - "robots_not_in_center_circle_validation.cpp", - "robots_slow_down_validation.cpp", - "robots_violating_motion_constraint.cpp", - ], - hdrs = [ - "ball_in_play_or_scored_validation.h", - "ball_never_moves_backward_validation.h", - "enemy_never_scores_validation.h", - "robot_not_excessively_dribbling_validation.h", - "robots_avoid_ball_validation.h", - "robots_in_friendly_half_validation.h", - "robots_not_in_center_circle_validation.h", - "robots_slow_down_validation.h", - "robots_violating_motion_constraint.h", - ], - deps = [ - "//software/ai/navigator/obstacle", - "//software/ai/navigator/obstacle:robot_navigation_obstacle_factory", - "//software/geom/algorithms", - "//software/simulated_tests/cpp_validation:validation_function", - "//software/world", - "@boost//:coroutine2", - ], -) diff --git a/src/software/simulated_tests/non_terminating_validation_functions/ball_in_play_or_scored_validation.cpp b/src/software/simulated_tests/non_terminating_validation_functions/ball_in_play_or_scored_validation.cpp deleted file mode 100644 index 2c72f5b284..0000000000 --- a/src/software/simulated_tests/non_terminating_validation_functions/ball_in_play_or_scored_validation.cpp +++ /dev/null @@ -1,12 +0,0 @@ -#include "software/simulated_tests/non_terminating_validation_functions/ball_in_play_or_scored_validation.h" - -void ballInPlay(std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) -{ - Point ball = world_ptr->ball().position(); - Rectangle inPlayRect = world_ptr->field().fieldLines(); - - if (!contains(inPlayRect, ball) && !contains(world_ptr->field().enemyGoal(), ball)) - { - FAIL() << "The ball is out of play!"; - }; -} diff --git a/src/software/simulated_tests/non_terminating_validation_functions/ball_in_play_or_scored_validation.h b/src/software/simulated_tests/non_terminating_validation_functions/ball_in_play_or_scored_validation.h deleted file mode 100644 index 210d9069b3..0000000000 --- a/src/software/simulated_tests/non_terminating_validation_functions/ball_in_play_or_scored_validation.h +++ /dev/null @@ -1,15 +0,0 @@ -#pragma once - -#include - -#include "software/geom/algorithms/contains.h" -#include "software/simulated_tests/cpp_validation/validation_function.h" -#include "software/world/world.h" - -/** - * Checks if the ball has been scored or is in play. - * - * @param world_ptr the world pointer given by the simulator. Gets updated every tick - * @param yield yields control to the next routine (coroutines) - **/ -void ballInPlay(std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield); diff --git a/src/software/simulated_tests/non_terminating_validation_functions/ball_never_moves_backward_validation.cpp b/src/software/simulated_tests/non_terminating_validation_functions/ball_never_moves_backward_validation.cpp deleted file mode 100644 index f300f04845..0000000000 --- a/src/software/simulated_tests/non_terminating_validation_functions/ball_never_moves_backward_validation.cpp +++ /dev/null @@ -1,17 +0,0 @@ -#include "software/simulated_tests/non_terminating_validation_functions/ball_never_moves_backward_validation.h" - -void ballNeverMovesBackward(std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield, double tolerance) -{ - double previous_x_pos = world_ptr->field().friendlyGoalCenter().x(); - double current_x_pos; - - while (((current_x_pos = world_ptr->ball().position().x()) + tolerance) >= - previous_x_pos) - { - previous_x_pos = world_ptr->ball().position().x(); - yield(""); - } - yield("Ball has moved backward by " + - std::to_string(world_ptr->ball().position().x() - previous_x_pos)); -} diff --git a/src/software/simulated_tests/non_terminating_validation_functions/ball_never_moves_backward_validation.h b/src/software/simulated_tests/non_terminating_validation_functions/ball_never_moves_backward_validation.h deleted file mode 100644 index 5a989fc144..0000000000 --- a/src/software/simulated_tests/non_terminating_validation_functions/ball_never_moves_backward_validation.h +++ /dev/null @@ -1,18 +0,0 @@ -#pragma once - -#include - -#include "software/geom/algorithms/contains.h" -#include "software/simulated_tests/cpp_validation/validation_function.h" -#include "software/world/world.h" - -/** - * Checks that the ball doesn't move backward. - * - * @param world_ptr the world pointer given by the simulator. Gets updated every tick - * @param yield yields control to the next routine (coroutines) - * @param tolerance tolerance given to robot's x-coordinate changes - */ -void ballNeverMovesBackward(std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield, - double tolerance = 0.04); diff --git a/src/software/simulated_tests/non_terminating_validation_functions/enemy_never_scores_validation.cpp b/src/software/simulated_tests/non_terminating_validation_functions/enemy_never_scores_validation.cpp deleted file mode 100644 index 7fd0737025..0000000000 --- a/src/software/simulated_tests/non_terminating_validation_functions/enemy_never_scores_validation.cpp +++ /dev/null @@ -1,14 +0,0 @@ -#include "software/simulated_tests/non_terminating_validation_functions/enemy_never_scores_validation.h" - -#include "software/geom/algorithms/contains.h" -#include "software/logger/logger.h" - -void enemyNeverScores(std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) -{ - while (!contains(world_ptr->field().friendlyGoal(), world_ptr->ball().position())) - { - yield(""); - } - yield("The enemy has scored!"); -} diff --git a/src/software/simulated_tests/non_terminating_validation_functions/enemy_never_scores_validation.h b/src/software/simulated_tests/non_terminating_validation_functions/enemy_never_scores_validation.h deleted file mode 100644 index 838ba12e41..0000000000 --- a/src/software/simulated_tests/non_terminating_validation_functions/enemy_never_scores_validation.h +++ /dev/null @@ -1,13 +0,0 @@ -#pragma once - -#include "software/simulated_tests/cpp_validation/validation_function.h" -#include "software/world/world.h" - -/** - * Checks that the ball never enters the friendly goal - * - * @param world_ptr the world pointer given by the simulator. Gets updated every tick - * @param yield yields control to the next routine (coroutines) with error message - */ -void enemyNeverScores(std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield); diff --git a/src/software/simulated_tests/non_terminating_validation_functions/robot_not_excessively_dribbling_validation.cpp b/src/software/simulated_tests/non_terminating_validation_functions/robot_not_excessively_dribbling_validation.cpp deleted file mode 100644 index 7e547d60eb..0000000000 --- a/src/software/simulated_tests/non_terminating_validation_functions/robot_not_excessively_dribbling_validation.cpp +++ /dev/null @@ -1,34 +0,0 @@ -#include "software/simulated_tests/non_terminating_validation_functions/robot_not_excessively_dribbling_validation.h" - -#include "software/logger/logger.h" - -void robotNotExcessivelyDribbling(RobotId robot_id, std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) -{ - auto robot_has_ball_in_dribbler = [robot_id](std::shared_ptr world_ptr) - { - std::optional robot_optional = - world_ptr->friendlyTeam().getRobotById(robot_id); - CHECK(robot_optional.has_value()) - << "There is no robot with ID: " + std::to_string(robot_id); - - return robot_optional.value().isNearDribbler(world_ptr->ball().position()); - }; - - Point continous_dribbling_start_point = world_ptr->ball().position(); - bool ball_in_dribbler_in_previous_tick = false; - - while (!robot_has_ball_in_dribbler(world_ptr) || - (world_ptr->ball().position() - continous_dribbling_start_point).length() <= - 1.0) - { - yield(""); - if (!robot_has_ball_in_dribbler(world_ptr) || - (robot_has_ball_in_dribbler(world_ptr) && !ball_in_dribbler_in_previous_tick)) - { - continous_dribbling_start_point = world_ptr->ball().position(); - } - ball_in_dribbler_in_previous_tick = robot_has_ball_in_dribbler(world_ptr); - } - yield("Ball was dribbled for more than 1 meter"); -} diff --git a/src/software/simulated_tests/non_terminating_validation_functions/robot_not_excessively_dribbling_validation.h b/src/software/simulated_tests/non_terminating_validation_functions/robot_not_excessively_dribbling_validation.h deleted file mode 100644 index 30999a7d21..0000000000 --- a/src/software/simulated_tests/non_terminating_validation_functions/robot_not_excessively_dribbling_validation.h +++ /dev/null @@ -1,16 +0,0 @@ -#pragma once - -#include - -#include "software/simulated_tests/cpp_validation/validation_function.h" -#include "software/world/world.h" - -/** - * Checks that the robot does not dribble for more than 1 meter at a time - * - * @param robot_id the robot_id to check - * @param world_ptr the world pointer given by the simulator. Gets updated every tick - * @param yield yields control to the next routine (coroutines) - */ -void robotNotExcessivelyDribbling(RobotId robot_id, std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield); diff --git a/src/software/simulated_tests/non_terminating_validation_functions/robots_avoid_ball_validation.cpp b/src/software/simulated_tests/non_terminating_validation_functions/robots_avoid_ball_validation.cpp deleted file mode 100644 index 3d443e6152..0000000000 --- a/src/software/simulated_tests/non_terminating_validation_functions/robots_avoid_ball_validation.cpp +++ /dev/null @@ -1,27 +0,0 @@ -#include "software/simulated_tests/non_terminating_validation_functions/robots_avoid_ball_validation.h" - -#include "software/geom/algorithms/contains.h" -#include "software/logger/logger.h" - -void robotsAvoidBall(double min_distance, std::vector excluded_robots, - std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) -{ - for (size_t i = 0; i < world_ptr->friendlyTeam().getAllRobots().size(); i++) - { - RobotId id = world_ptr->friendlyTeam().getAllRobots().at(i).id(); - double current_distance = - (world_ptr->friendlyTeam().getAllRobots().at(i).position() - - world_ptr->ball().position()) - .length() - - ROBOT_MAX_RADIUS_METERS; - - if (current_distance < min_distance && - std::find(excluded_robots.begin(), excluded_robots.end(), id) == - excluded_robots.end()) - { - yield("Robot " + std::to_string(id) + " is less than " + - std::to_string(min_distance) + " m away from the ball!"); - } - } -} diff --git a/src/software/simulated_tests/non_terminating_validation_functions/robots_avoid_ball_validation.h b/src/software/simulated_tests/non_terminating_validation_functions/robots_avoid_ball_validation.h deleted file mode 100644 index 52aed8a968..0000000000 --- a/src/software/simulated_tests/non_terminating_validation_functions/robots_avoid_ball_validation.h +++ /dev/null @@ -1,18 +0,0 @@ -#pragma once - -#include - -#include "software/simulated_tests/cpp_validation/validation_function.h" -#include "software/world/world.h" - -/** - * Checks if robots are keeping at least the minimum required distance from - * the ball - * @param min_distance the minimum required distance that robots must be from the ball - * @param excluded_robots RobotIds to ignore in this validation function's checking - * @param world_ptr the world pointer given by the simulator. Gets updated every tick - * @param yield yields control to the next routine (coroutines) with error message - */ -void robotsAvoidBall(double min_distance, std::vector excluded_robots, - std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield); diff --git a/src/software/simulated_tests/non_terminating_validation_functions/robots_in_friendly_half_validation.cpp b/src/software/simulated_tests/non_terminating_validation_functions/robots_in_friendly_half_validation.cpp deleted file mode 100644 index f4bc24755a..0000000000 --- a/src/software/simulated_tests/non_terminating_validation_functions/robots_in_friendly_half_validation.cpp +++ /dev/null @@ -1,29 +0,0 @@ -#include "software/simulated_tests/non_terminating_validation_functions/robots_in_friendly_half_validation.h" - -#include "software/logger/logger.h" - -void robotsInFriendlyHalf(std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) -{ - for (const auto& robot : world_ptr->friendlyTeam().getAllRobots()) - { - if (!world_ptr->field().pointInFriendlyHalf(robot.position())) - { - yield("Robot " + std::to_string(robot.id()) + " entered enemy half"); - } - } -} - -void robotInFriendlyHalf(RobotId robot_id, std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) -{ - std::optional robot_optional = - world_ptr->friendlyTeam().getRobotById(robot_id); - CHECK(robot_optional.has_value()) - << "There is no robot with ID: " + std::to_string(robot_id); - Point position = robot_optional.value().position(); - if (!world_ptr->field().pointInFriendlyHalf(position)) - { - yield("Robot " + std::to_string(robot_id) + " entered enemy half"); - } -} diff --git a/src/software/simulated_tests/non_terminating_validation_functions/robots_in_friendly_half_validation.h b/src/software/simulated_tests/non_terminating_validation_functions/robots_in_friendly_half_validation.h deleted file mode 100644 index e45370d2be..0000000000 --- a/src/software/simulated_tests/non_terminating_validation_functions/robots_in_friendly_half_validation.h +++ /dev/null @@ -1,23 +0,0 @@ -#pragma once - -#include "software/simulated_tests/cpp_validation/validation_function.h" -#include "software/world/world.h" - -/** - * Checks if the friendly robots are in the friendly half of the field, else assertion - * fails. - * @param world_ptr the world pointer given by the simulator. Gets updated every tick - * @param yield yields control to the next routine (coroutines) with error message - */ -void robotsInFriendlyHalf(std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield); - -/** - * Checks if a friendly robot is in the friendly half of the field, else assertion fails. - * @param robot_id the ID of the robot in question, there must exist a robot - * for the given robot_id - * @param world_ptr the world pointer given by the simulator. Gets updated every tick - * @param yield yields control to the next routine (coroutines) with error message - */ -void robotInFriendlyHalf(RobotId robot_id, std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield); diff --git a/src/software/simulated_tests/non_terminating_validation_functions/robots_not_in_center_circle_validation.cpp b/src/software/simulated_tests/non_terminating_validation_functions/robots_not_in_center_circle_validation.cpp deleted file mode 100644 index 79b59bb58f..0000000000 --- a/src/software/simulated_tests/non_terminating_validation_functions/robots_not_in_center_circle_validation.cpp +++ /dev/null @@ -1,30 +0,0 @@ -#include "software/simulated_tests/non_terminating_validation_functions/robots_not_in_center_circle_validation.h" - -#include "software/geom/algorithms/contains.h" -#include "software/logger/logger.h" - -void robotsNotInCenterCircle(std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) -{ - for (const auto& robot : world_ptr->friendlyTeam().getAllRobots()) - { - if (contains(world_ptr->field().centerCircle(), robot.position())) - { - yield("Robot " + std::to_string(robot.id()) + " entered center circle"); - } - } -} - -void robotNotInCenterCircle(RobotId robot_id, std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) -{ - std::optional robot_optional = - world_ptr->friendlyTeam().getRobotById(robot_id); - CHECK(robot_optional.has_value()) - << "There is no robot with ID: " + std::to_string(robot_id); - Point position = robot_optional.value().position(); - if (contains(world_ptr->field().centerCircle(), position)) - { - yield("Robot " + std::to_string(robot_id) + " entered center circle"); - } -} diff --git a/src/software/simulated_tests/non_terminating_validation_functions/robots_not_in_center_circle_validation.h b/src/software/simulated_tests/non_terminating_validation_functions/robots_not_in_center_circle_validation.h deleted file mode 100644 index a01b66f4ce..0000000000 --- a/src/software/simulated_tests/non_terminating_validation_functions/robots_not_in_center_circle_validation.h +++ /dev/null @@ -1,22 +0,0 @@ -#pragma once - -#include "software/simulated_tests/cpp_validation/validation_function.h" -#include "software/world/world.h" - -/** - * Checks if the friendly robots are not in center circle of field, else fails assertion. - * @param world_ptr the world pointer given by the simulator. Gets updated every tick - * @param yield yields control to the next routine (coroutines) with error message - */ -void robotsNotInCenterCircle(std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield); - -/** - * Checks if a friendly robot is not in center circle of field, else fails assertion. - * @param robot_id the ID of the robot in question, there must exist a robot - * for the given robot_id - * @param world_ptr the world pointer given by the simulator. Gets updated every tick - * @param yield yields control to the next routine (coroutines) with error message - */ -void robotNotInCenterCircle(RobotId robot_id, std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield); diff --git a/src/software/simulated_tests/non_terminating_validation_functions/robots_slow_down_validation.cpp b/src/software/simulated_tests/non_terminating_validation_functions/robots_slow_down_validation.cpp deleted file mode 100644 index 3cdaca5d43..0000000000 --- a/src/software/simulated_tests/non_terminating_validation_functions/robots_slow_down_validation.cpp +++ /dev/null @@ -1,20 +0,0 @@ -#include "software/simulated_tests/non_terminating_validation_functions/robots_slow_down_validation.h" - -#include "software/geom/algorithms/contains.h" -#include "software/logger/logger.h" - -void robotsSlowDown(double max_speed, std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) -{ - for (const auto& robot : world_ptr->friendlyTeam().getAllRobots()) - { - double speed = robot.velocity().length(); - - if (speed > max_speed) - { - yield("Robot " + std::to_string(robot.id()) + " is moving at " + - std::to_string(speed) + ", faster than " + std::to_string(max_speed) + - " m/s!"); - } - } -} diff --git a/src/software/simulated_tests/non_terminating_validation_functions/robots_slow_down_validation.h b/src/software/simulated_tests/non_terminating_validation_functions/robots_slow_down_validation.h deleted file mode 100644 index ab7bb20be3..0000000000 --- a/src/software/simulated_tests/non_terminating_validation_functions/robots_slow_down_validation.h +++ /dev/null @@ -1,13 +0,0 @@ -#pragma once - -#include "software/simulated_tests/cpp_validation/validation_function.h" -#include "software/world/world.h" - -/** - * Checks if robots have slowed down to the maximum allowed speed - * @param max_speed the maximum speed that robots are allowed to move - * @param world_ptr the world pointer given by the simulator. Gets updated every tick - * @param yield yields control to the next routine (coroutines) with error message - */ -void robotsSlowDown(double max_speed, std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield); diff --git a/src/software/simulated_tests/non_terminating_validation_functions/robots_violating_motion_constraint.cpp b/src/software/simulated_tests/non_terminating_validation_functions/robots_violating_motion_constraint.cpp deleted file mode 100644 index a0d432ff6d..0000000000 --- a/src/software/simulated_tests/non_terminating_validation_functions/robots_violating_motion_constraint.cpp +++ /dev/null @@ -1,23 +0,0 @@ -#include "software/simulated_tests/non_terminating_validation_functions/robots_violating_motion_constraint.h" - -#include "software/logger/logger.h" - -void robotsViolatingMotionConstraint( - std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield, - std::shared_ptr obstacle_factory, - TbotsProto::MotionConstraint constraint) -{ - std::vector obstacles = - obstacle_factory->createObstaclesFromMotionConstraint(constraint, *world_ptr); - for (const auto& robot : world_ptr->friendlyTeam().getAllRobots()) - { - for (auto obstacle_ptr : obstacles) - { - if (obstacle_ptr.get()->contains(robot.position())) - { - yield("Robot " + std::to_string(robot.id()) + - " violated the motion constraint"); - } - } - } -} diff --git a/src/software/simulated_tests/non_terminating_validation_functions/robots_violating_motion_constraint.h b/src/software/simulated_tests/non_terminating_validation_functions/robots_violating_motion_constraint.h deleted file mode 100644 index 30783ff91a..0000000000 --- a/src/software/simulated_tests/non_terminating_validation_functions/robots_violating_motion_constraint.h +++ /dev/null @@ -1,20 +0,0 @@ -#pragma once -#include "proto/primitive.pb.h" -#include "software/ai/navigator/obstacle/obstacle.hpp" -#include "software/ai/navigator/obstacle/robot_navigation_obstacle_factory.h" -#include "software/simulated_tests/cpp_validation/validation_function.h" -#include "software/world/world.h" - -/** - * Checks if the friendly robots do not enter specified motion constraint during the test. - * If they do, assertion fails. - * - * @param world_ptr the world pointer given by the simulator. Gets updated every tick - * @param yield yields control to the next routine (coroutines) with error message - * @param obstacle_factory the obstacle factory object to be passed in - * @param constraint the motion constraint to be specified - */ -void robotsViolatingMotionConstraint( - std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield, - std::shared_ptr obstacle_factory, - TbotsProto::MotionConstraint constraint); diff --git a/src/software/simulated_tests/simulated_er_force_sim_play_test_fixture.cpp b/src/software/simulated_tests/simulated_er_force_sim_play_test_fixture.cpp deleted file mode 100644 index 3f7d1bad72..0000000000 --- a/src/software/simulated_tests/simulated_er_force_sim_play_test_fixture.cpp +++ /dev/null @@ -1,108 +0,0 @@ -#include "software/simulated_tests/simulated_er_force_sim_play_test_fixture.h" - -#include "proto/message_translation/tbots_protobuf.h" -#include "software/ai/hl/stp/play/assigned_tactics_play.h" -#include "software/test_util/test_util.h" - -SimulatedErForceSimPlayTestFixture::SimulatedErForceSimPlayTestFixture() - : game_state(), - ai(std::make_shared(friendly_thunderbots_config.ai_config())) -{ -} - -void SimulatedErForceSimPlayTestFixture::SetUp() -{ - SimulatedErForceSimTestFixture::SetUp(); -} - -void SimulatedErForceSimPlayTestFixture::setFriendlyGoalie(RobotId goalie_id) -{ - friendly_thunderbots_config.mutable_sensor_fusion_config()->set_friendly_goalie_id( - static_cast(goalie_id)); -} - -void SimulatedErForceSimPlayTestFixture::setEnemyGoalie(RobotId goalie_id) -{ - friendly_thunderbots_config.mutable_sensor_fusion_config()->set_enemy_goalie_id( - static_cast(goalie_id)); -} - -void SimulatedErForceSimPlayTestFixture::setAiPlay( - const TbotsProto::PlayName& ai_play_name) -{ - friendly_thunderbots_config.mutable_ai_config() - ->mutable_ai_control_config() - ->set_override_ai_play(ai_play_name); - - ai = Ai( - std::make_shared(friendly_thunderbots_config.ai_config())); -} - -void SimulatedErForceSimPlayTestFixture::setAiPlay(std::unique_ptr play) -{ - ai.overridePlay(std::move(play)); -} - -void SimulatedErForceSimPlayTestFixture::setTactic(RobotId id, - std::shared_ptr tactic) -{ - setTactic(id, tactic, {}); -} - -void SimulatedErForceSimPlayTestFixture::setTactic( - RobotId id, std::shared_ptr tactic, - std::set motion_constraints) -{ - CHECK(static_cast(tactic)) << "Tactic is invalid" << std::endl; - std::unique_ptr play = std::make_unique( - std::make_shared(friendly_thunderbots_config.ai_config())); - std::map> - motion_constraint_override_map; - motion_constraint_override_map[id] = motion_constraints; - play->updateControlParams({{id, tactic}}, motion_constraint_override_map); - setAiPlay(std::move(play)); -} - -void SimulatedErForceSimPlayTestFixture::setRefereeCommand( - const RefereeCommand& current_referee_command, - const RefereeCommand& previous_referee_command) -{ - game_state.updateRefereeCommand(previous_referee_command); - game_state.updateRefereeCommand(current_referee_command); -} - -void SimulatedErForceSimPlayTestFixture::setGameState(const GameState& game_state_) -{ - game_state = game_state_; -} - -void SimulatedErForceSimPlayTestFixture::updatePrimitives( - const World& friendly_world, const World&, - std::shared_ptr simulator_to_update) -{ - auto world_with_updated_game_state = friendly_world; - world_with_updated_game_state.updateGameState(game_state); - - auto start_tick_time = std::chrono::system_clock::now(); - - auto primitive_set_msg = - ai.getPrimitives(std::make_shared(world_with_updated_game_state)); - LOG(VISUALIZE) << ai.getPlayInfo(); - LOG(VISUALIZE) << *primitive_set_msg; - - double duration_ms = ::TestUtil::millisecondsSince(start_tick_time); - registerFriendlyTickTime(duration_ms); - auto world_msg = createWorld(world_with_updated_game_state); - simulator_to_update->setYellowRobotPrimitiveSet(*primitive_set_msg, - std::move(world_msg)); -} - -const TbotsProto::AiConfig SimulatedErForceSimPlayTestFixture::getAiConfig() const -{ - return friendly_thunderbots_config.ai_config(); -} - -std::optional SimulatedErForceSimPlayTestFixture::getPlayInfo() -{ - return ai.getPlayInfo(); -} diff --git a/src/software/simulated_tests/simulated_er_force_sim_play_test_fixture.h b/src/software/simulated_tests/simulated_er_force_sim_play_test_fixture.h deleted file mode 100644 index 561acf93ba..0000000000 --- a/src/software/simulated_tests/simulated_er_force_sim_play_test_fixture.h +++ /dev/null @@ -1,92 +0,0 @@ -#pragma once - -#include - -#include "software/ai/ai.h" -#include "software/simulated_tests/simulated_er_force_sim_test_fixture.h" - -/** - * This is a test fixture designed to make it easy to write integration tests. It provides - * an easy interface to set up robots on the field, and then validate how the world - * changes over time during simulation. This allows us to easily write tests for - * the AI's behaviour. - */ -class SimulatedErForceSimPlayTestFixture : public SimulatedErForceSimTestFixture -{ - public: - explicit SimulatedErForceSimPlayTestFixture(); - - protected: - void SetUp() override; - - /** - * Sets the goalie for the specified team. If this function is not called, - * the goalie will be set to the default ID of the DynamicParameters - * - * @param goalie_id The ID of the robot to be goalie - */ - void setFriendlyGoalie(RobotId goalie_id); - void setEnemyGoalie(RobotId goalie_id); - - /** - * Sets the AI play to run in the simulated test - * - * @param ai_play The AI play name enum - */ - void setAiPlay(const TbotsProto::PlayName& ai_play_name); - - /** - * Sets the tactic to the given tactic - * - * @param id the robot id of the robot to run the tactic on - * @param tactic The friendly tactic - * @param motion_constraints optionally override motion constraints - * - * @throw invalid_argument if any tactic is invalid - */ - void setTactic(RobotId id, std::shared_ptr tactic); - void setTactic(RobotId id, std::shared_ptr tactic, - std::set motion_constraints); - - /** - * Sets the AI play to be used to run in the simulated test - * - * @param play_ The play - */ - void setAiPlay(std::unique_ptr play); - - /** - * Sets the Referee command to override for the simulated test - * - * @param current_referee_command The name of the current referee command to set - * @param previous_referee_command The name of the previous referee command to set - */ - void setRefereeCommand(const RefereeCommand& current_referee_command, - const RefereeCommand& previous_referee_command); - - /** - * Sets the game state to use while testing - * - * @param game_state_ The game state to set - */ - void setGameState(const GameState& game_state_); - - /** - * Gets the configs used in simulation - * Useful for constructing duplicates of Obstacle Factory - * - * @return the Ai Config - */ - const TbotsProto::AiConfig getAiConfig() const; - - std::optional getPlayInfo() override; - - private: - void updatePrimitives(const World& friendly_world, const World& enemy_world, - std::shared_ptr simulator_to_update) override; - - GameState game_state; - - // The AI being tested and used in simulation - Ai ai; -}; diff --git a/src/software/simulated_tests/simulated_er_force_sim_test_fixture.cpp b/src/software/simulated_tests/simulated_er_force_sim_test_fixture.cpp deleted file mode 100644 index a855576af0..0000000000 --- a/src/software/simulated_tests/simulated_er_force_sim_test_fixture.cpp +++ /dev/null @@ -1,558 +0,0 @@ -#include "software/simulated_tests/simulated_er_force_sim_test_fixture.h" - -#include "proto/message_translation/tbots_protobuf.h" - -// TODO (#2419): remove this -#include - -#include -#include - -#include "proto/message_translation/er_force_world.h" -#include "proto/message_translation/ssl_wrapper.h" -#include "proto/message_translation/tbots_protobuf.h" -#include "shared/2021_robot_constants.h" -#include "shared/test_util/test_util.h" -#include "software/logger/logger.h" -#include "software/simulation/er_force_simulator.h" -#include "software/test_util/test_util.h" - -using namespace TestUtil; - -SimulatedErForceSimTestFixture::SimulatedErForceSimTestFixture() - : friendly_thunderbots_config(TbotsProto::ThunderbotsConfig()), - enemy_thunderbots_config(TbotsProto::ThunderbotsConfig()), - friendly_sensor_fusion(friendly_thunderbots_config.sensor_fusion_config()), - enemy_sensor_fusion(enemy_thunderbots_config.sensor_fusion_config()), - run_simulation_in_realtime(false) -{ -} - -void SimulatedErForceSimTestFixture::SetUp() -{ - LoggerSingleton::initializeLogger(TbotsGtestMain::runtime_dir, nullptr); - - // new configs so that callbacks to the previous test's AI are cleared - friendly_thunderbots_config = TbotsProto::ThunderbotsConfig(); - enemy_thunderbots_config = TbotsProto::ThunderbotsConfig(); - - setCommonConfigs(friendly_thunderbots_config); - setCommonConfigs(enemy_thunderbots_config); - - // The friendly team defends the negative side of the field - // and controls the yellow robots - friendly_thunderbots_config.mutable_sensor_fusion_config()->set_friendly_color_yellow( - true); - - // The enemy team defends the positive side of the field - // and controls the blue robots - enemy_thunderbots_config.mutable_sensor_fusion_config()->set_friendly_color_yellow( - false); - - // reinitializing to prevent the previous test's configs from being reused - friendly_sensor_fusion = - SensorFusion(friendly_thunderbots_config.sensor_fusion_config()); - enemy_sensor_fusion = SensorFusion(enemy_thunderbots_config.sensor_fusion_config()); - - if (TbotsGtestMain::run_sim_in_realtime) - { - run_simulation_in_realtime = true; - } - if (TbotsGtestMain::run_sim_in_realtime) - { - run_simulation_in_realtime = true; - } - - // Reset tick duration trackers - total_friendly_tick_duration = 0.0; - total_enemy_tick_duration = 0.0; - // all tick times should be greater than 0 - max_friendly_tick_duration = 0.0; - max_enemy_tick_duration = 0.0; - // all tick times should be less than the max value of a double - min_friendly_tick_duration = std::numeric_limits::max(); - min_enemy_tick_duration = std::numeric_limits::max(); - friendly_tick_count = 0; - enemy_tick_count = 0; -} - -void SimulatedErForceSimTestFixture::setCommonConfigs( - TbotsProto::ThunderbotsConfig &mutable_thunderbots_config) -{ - mutable_thunderbots_config.mutable_ai_config() - ->mutable_ai_control_config() - ->set_run_ai(!TbotsGtestMain::stop_ai_on_start); -} - -bool SimulatedErForceSimTestFixture::validateAndCheckCompletion( - std::vector &terminating_function_validators, - std::vector &non_terminating_function_validators) -{ - for (auto &function_validator : non_terminating_function_validators) - { - auto error_message = function_validator.executeAndCheckForFailures(); - if (error_message) - { - ADD_FAILURE() << error_message.value(); - } - } - - bool validation_successful = std::all_of( - terminating_function_validators.begin(), terminating_function_validators.end(), - [](TerminatingFunctionValidator &fv) { return fv.executeAndCheckForSuccess(); }); - - return terminating_function_validators.empty() ? false : validation_successful; -} - -void SimulatedErForceSimTestFixture::updateSensorFusion( - std::shared_ptr simulator) -{ - // TODO (#2419): remove this to re-enable sigfpe checks - fedisableexcept(FE_INVALID | FE_OVERFLOW); - auto ssl_wrapper_packets = simulator->getSSLWrapperPackets(); - // TODO (#2419): remove this to re-enable sigfpe checks - feenableexcept(FE_INVALID | FE_OVERFLOW); - - auto blue_robot_statuses = simulator->getBlueRobotStatuses(); - auto yellow_robot_statuses = simulator->getYellowRobotStatuses(); - - for (const auto &packet : ssl_wrapper_packets) - { - auto blue_sensor_msg = SensorProto(); - auto yellow_sensor_msg = SensorProto(); - *(blue_sensor_msg.mutable_ssl_vision_msg()) = packet; - *(yellow_sensor_msg.mutable_ssl_vision_msg()) = packet; - for (const auto &msg : blue_robot_statuses) - { - *(blue_sensor_msg.add_robot_status_msgs()) = msg; - } - for (const auto &msg : yellow_robot_statuses) - { - *(yellow_sensor_msg.add_robot_status_msgs()) = msg; - } - - if (friendly_thunderbots_config.sensor_fusion_config().friendly_color_yellow()) - { - friendly_sensor_fusion.processSensorProto(yellow_sensor_msg); - } - else - { - friendly_sensor_fusion.processSensorProto(blue_sensor_msg); - } - - if (enemy_thunderbots_config.sensor_fusion_config().friendly_color_yellow()) - { - enemy_sensor_fusion.processSensorProto(yellow_sensor_msg); - } - else - { - enemy_sensor_fusion.processSensorProto(blue_sensor_msg); - } - } -} - -void SimulatedErForceSimTestFixture::sleep( - const std::chrono::steady_clock::time_point &wall_start_time, - const Duration &desired_wall_tick_time) -{ - auto wall_time_now = std::chrono::steady_clock::now(); - auto current_tick_wall_time_duration = - std::chrono::duration_cast(wall_time_now - - wall_start_time); - auto ms_to_sleep = std::chrono::milliseconds( - static_cast(desired_wall_tick_time.toMilliseconds())) - - current_tick_wall_time_duration; - if (ms_to_sleep > std::chrono::milliseconds(0)) - { - std::this_thread::sleep_for(ms_to_sleep); - } -} - -void SimulatedErForceSimTestFixture::runTest( - const TbotsProto::FieldType &field_type, const BallState &ball, - const std::vector &friendly_robots, - const std::vector &enemy_robots, - const std::vector &terminating_validation_functions, - const std::vector &non_terminating_validation_functions, - const Duration &timeout, const bool ramping) -{ - const Duration simulation_time_step = - Duration::fromSeconds(1.0 / SIMULATED_CAMERA_FPS); - - auto realism_config = ErForceSimulator::createDefaultRealismConfig(); - std::shared_ptr simulator(std::make_shared( - field_type, create2021RobotConstants(), realism_config, ramping)); - - // TODO (#2419): remove this to re-enable sigfpe checks - fedisableexcept(FE_INVALID | FE_OVERFLOW); - simulator->setBallState(ball); - // step the simulator to make sure the robots and the ball are in position - simulator->stepSimulation(simulation_time_step); - simulator->setYellowRobots(friendly_robots); - simulator->setBlueRobots(enemy_robots); - // TODO (#2419): remove this to re-enable sigfpe checks - feenableexcept(FE_INVALID | FE_OVERFLOW); - - updateSensorFusion(simulator); - std::shared_ptr friendly_world; - std::shared_ptr enemy_world; - CHECK(friendly_sensor_fusion.getWorld().has_value()) - << "Invalid friendly world state" << std::endl; - CHECK(enemy_sensor_fusion.getWorld().has_value()) - << "Invalid enemy world state" << std::endl; - friendly_world = std::make_shared(friendly_sensor_fusion.getWorld().value()); - enemy_world = std::make_shared(enemy_sensor_fusion.getWorld().value()); - - for (const auto &validation_function : terminating_validation_functions) - { - terminating_function_validators.emplace_back( - TerminatingFunctionValidator(validation_function, friendly_world)); - } - - for (const auto &validation_function : non_terminating_validation_functions) - { - non_terminating_function_validators.emplace_back( - NonTerminatingFunctionValidator(validation_function, friendly_world)); - } - - const Timestamp timeout_time = simulator->getTimestamp() + timeout; - - double speed_factor = 1 / (TbotsGtestMain::test_speed); - const Duration ai_time_step = - Duration::fromSeconds(simulation_time_step.toSeconds() * speed_factor); - - // declare difference (velocity, position) variables - double ball_displacement; - double ball_velocity_diff; - double sum_ball_displacement; - double sum_ball_velocity; - std::vector robots_displacement; - std::vector robots_velocity_diff; - std::vector sum_robots_displacement; - std::vector sum_robots_velocity; - - // declare struct for ball_displacement, velocity_diff; array of struct for robots - // fields - struct AggregateFunctions ball_displacement_stats = AggregateFunctions(); - struct AggregateFunctions ball_velocity_stats = AggregateFunctions(); - std::vector robots_displacement_stats; - std::vector robots_velocity_stats; - - // Tick one frame to aid with visualization - bool validation_functions_done = tickTest( - simulation_time_step, ai_time_step, friendly_world, enemy_world, simulator, - ball_displacement, ball_velocity_diff, robots_displacement, robots_velocity_diff); - - // Initialize Values - ball_displacement_stats.maximum = ball_displacement; - ball_displacement_stats.minimum = ball_displacement; - ball_velocity_stats.maximum = ball_velocity_diff; - ball_velocity_stats.minimum = ball_velocity_diff; - sum_ball_displacement = ball_displacement; - sum_ball_velocity = ball_velocity_diff; - sum_robots_displacement = robots_displacement; - sum_robots_velocity = robots_velocity_diff; - - size_t num_robots = robots_displacement.size(); - validation_functions_done = false; - while (simulator->getTimestamp() < timeout_time && !validation_functions_done) - { - robots_displacement.clear(); - robots_velocity_diff.clear(); - - if (!friendly_thunderbots_config.ai_config().ai_control_config().run_ai()) - { - validation_functions_done = - tickTest(simulation_time_step, ai_time_step, friendly_world, enemy_world, - simulator, ball_displacement, ball_velocity_diff, - robots_displacement, robots_velocity_diff); - } - - while (simulator->getTimestamp() < timeout_time && !validation_functions_done) - { - if (!friendly_thunderbots_config.ai_config().ai_control_config().run_ai()) - { - auto ms_to_sleep = std::chrono::milliseconds( - static_cast(ai_time_step.toMilliseconds())); - std::this_thread::sleep_for(ms_to_sleep); - continue; - } - - validation_functions_done = - tickTest(simulation_time_step, ai_time_step, friendly_world, enemy_world, - simulator, ball_displacement, ball_velocity_diff, - robots_displacement, robots_velocity_diff); - - sum_ball_displacement += ball_displacement; - sum_ball_velocity += ball_velocity_diff; - - ball_displacement_stats.maximum = - std::max(ball_displacement, ball_displacement_stats.maximum); - ball_displacement_stats.minimum = - std::min(ball_displacement, ball_displacement_stats.minimum); - ball_velocity_stats.maximum = - std::max(ball_velocity_diff, ball_velocity_stats.maximum); - ball_velocity_stats.minimum = - std::min(ball_velocity_diff, ball_velocity_stats.minimum); - - for (size_t i = 0; i < num_robots; i++) - { - robots_displacement_stats.push_back(AggregateFunctions()); - robots_velocity_stats.push_back(AggregateFunctions()); - sum_robots_displacement[i] += robots_displacement[i]; - sum_robots_velocity[i] += robots_velocity_diff[i]; - robots_displacement_stats[i].maximum = std::max( - robots_displacement[i], robots_displacement_stats[i].maximum); - robots_displacement_stats[i].minimum = std::min( - robots_displacement[i], robots_displacement_stats[i].minimum); - robots_velocity_stats[i].maximum = std::max( - robots_displacement[i], robots_displacement_stats[i].maximum); - robots_velocity_stats[i].minimum = - std::min(robots_velocity_diff[i], robots_velocity_stats[i].minimum); - } - } - - auto total_tick_count = friendly_tick_count + enemy_tick_count; - // compute the averages - ball_displacement_stats.average = - (total_tick_count == 0) ? 0 : sum_ball_displacement / total_tick_count; - ball_velocity_stats.average = - (total_tick_count == 0) ? 0 : sum_ball_velocity / total_tick_count; - - for (size_t i = 0; i < num_robots; i++) - { - robots_displacement_stats[i].average = - (total_tick_count == 0) ? 0 - : sum_robots_displacement[i] / total_tick_count; - robots_velocity_stats[i].average = - (total_tick_count == 0) ? 0 : sum_robots_velocity[i] / total_tick_count; - validation_functions_done = - tickTest(simulation_time_step, ai_time_step, friendly_world, enemy_world, - simulator, ball_displacement, ball_velocity_diff, - robots_displacement, robots_velocity_diff); - } - - // Output the statistics for ball and robots - LOG(INFO) << "max ball displacement: " << ball_displacement_stats.maximum - << std::endl; - LOG(INFO) << "min ball displacement: " << ball_displacement_stats.minimum - << std::endl; - LOG(INFO) << "avg ball displacement: " << ball_displacement_stats.average - << std::endl; - LOG(INFO) << "max ball velocity difference: " << ball_velocity_stats.maximum - << std::endl; - LOG(INFO) << "min ball velocity difference: " << ball_velocity_stats.minimum - << std::endl; - LOG(INFO) << "avg ball velocity difference: " << ball_velocity_stats.average - << std::endl; - for (size_t i = 0; i < num_robots; i++) - { - LOG(INFO) << "Robot " << i << std::endl; - LOG(INFO) << "max robot displacement: " << ball_displacement_stats.maximum - << std::endl; - LOG(INFO) << "min robot displacement: " << ball_displacement_stats.minimum - << std::endl; - LOG(INFO) << "avg robot displacement: " << ball_displacement_stats.average - << std::endl; - LOG(INFO) << "max robot velocity difference: " << ball_velocity_stats.maximum - << std::endl; - LOG(INFO) << "min robot velocity difference: " << ball_velocity_stats.minimum - << std::endl; - LOG(INFO) << "avg robot velocity difference: " << ball_velocity_stats.average - << std::endl; - } - - validation_functions_done = - tickTest(simulation_time_step, ai_time_step, friendly_world, enemy_world, - simulator, ball_displacement, ball_velocity_diff, - robots_displacement, robots_velocity_diff); - } - - // Output the tick duration results - if (friendly_tick_count > 0) - { - double avg_friendly_tick_duration = - total_friendly_tick_duration / friendly_tick_count; - LOG(INFO) << "max friendly tick duration: " << max_friendly_tick_duration << "ms" - << std::endl; - LOG(INFO) << "min friendly tick duration: " << min_friendly_tick_duration << "ms" - << std::endl; - LOG(INFO) << "avg friendly tick duration: " << avg_friendly_tick_duration << "ms" - << std::endl; - } - else - { - LOG(WARNING) << "Primitives were never updated for the friendly robots" - << std::endl; - } - - if (enemy_tick_count > 0) - { - double avg_enemy_tick_duration = total_enemy_tick_duration / enemy_tick_count; - LOG(INFO) << "max enemy tick duration: " << max_enemy_tick_duration << "ms" - << std::endl; - LOG(INFO) << "min enemy tick duration: " << min_enemy_tick_duration << "ms" - << std::endl; - LOG(INFO) << "avg enemy tick duration: " << avg_enemy_tick_duration << "ms" - << std::endl; - } - - - if (!validation_functions_done && !terminating_validation_functions.empty()) - { - std::string failure_message = - "Not all validation functions passed within the timeout duration:\n"; - for (const auto &fun : terminating_function_validators) - { - if (fun.currentErrorMessage() != "") - { - failure_message += fun.currentErrorMessage() + std::string("\n"); - } - } - ADD_FAILURE() << failure_message; - } -} - -void SimulatedErForceSimTestFixture::registerFriendlyTickTime(double tick_time_ms) -{ - total_friendly_tick_duration += tick_time_ms; - max_friendly_tick_duration = std::max(max_friendly_tick_duration, tick_time_ms); - min_friendly_tick_duration = std::min(min_friendly_tick_duration, tick_time_ms); - friendly_tick_count++; -} - -void SimulatedErForceSimTestFixture::registerEnemyTickTime(double tick_time_ms) -{ - total_enemy_tick_duration += tick_time_ms; - max_enemy_tick_duration = std::max(max_enemy_tick_duration, tick_time_ms); - min_enemy_tick_duration = std::min(min_enemy_tick_duration, tick_time_ms); - enemy_tick_count++; -} - -bool SimulatedErForceSimTestFixture::tickTest( - Duration simulation_time_step, Duration ai_time_step, - std::shared_ptr friendly_world, std::shared_ptr enemy_world, - std::shared_ptr simulator, double &ball_displacement, - double &ball_velocity_diff, std::vector &robots_displacement, - std::vector &robots_velocity_diff) -{ - /* extract world ball and robot */ - Ball world_ball = friendly_world->ball(); - Team world_friendly_team = friendly_world->friendlyTeam(); - Team world_enemy_team = friendly_world->enemyTeam(); - std::vector world_friendly_robots = world_friendly_team.getAllRobots(); - std::vector world_enemy_robots = world_enemy_team.getAllRobots(); - - /* extract simulator ball and robot */ - world::SimulatorState simulator_state = simulator->getSimulatorState(); - auto simulator_sim_ball = simulator_state.ball(); - auto simulator_friendly_sim_robots = simulator_state.yellow_robots(); - auto simulator_enemy_sim_robots = simulator_state.blue_robots(); - - /* convert simball and simrobot to normal ball and robot in world for comparison */ - Ball simulator_ball = createBall(simulator_sim_ball, Timestamp::fromSeconds(0)); - std::vector simulator_friendly_robots; - std::vector simulator_enemy_robots; - - for (int i = 0; i < simulator_friendly_sim_robots.size(); i++) - { - simulator_friendly_robots.push_back( - createRobot(simulator_friendly_sim_robots[i], Timestamp::fromSeconds(0))); - } - for (int i = 0; i < simulator_enemy_sim_robots.size(); i++) - { - simulator_enemy_robots.push_back( - createRobot(simulator_enemy_sim_robots[i], Timestamp::fromSeconds(0))); - } - - /* compare ball position */ - Point world_ball_pos = world_ball.position(); - Point simulator_ball_pos = simulator_ball.position(); - ball_displacement = (world_ball_pos - simulator_ball_pos).length(); - - /* compare ball velocity */ - Vector world_ball_vel = world_ball.velocity(); - Vector simulator_ball_vel = simulator_ball.velocity(); - ball_velocity_diff = (world_ball_vel - simulator_ball_vel).length(); - - /* compare world and simulator friendly robots */ - for (Robot world_robot : world_friendly_robots) - { - int robot_index = 0; - while (world_robot.id() != simulator_friendly_robots[robot_index++].id()) - ; - Robot simulator_robot = simulator_friendly_robots[robot_index - 1]; - - /* find difference in robot[i] position */ - Point world_robot_pos = world_robot.position(); - Point simulator_robot_pos = simulator_robot.position(); - double robot_displacement = (world_robot_pos - simulator_robot_pos).length(); - - /* find difference in robot[i] velocity */ - Vector world_robot_vel = world_robot.velocity(); - Vector simulator_robot_vel = simulator_robot.velocity(); - double robot_velocity_diff = (world_robot_vel - simulator_robot_vel).length(); - - robots_displacement.push_back(robot_displacement); - robots_velocity_diff.push_back(robot_velocity_diff); - } - - /* compare world and simulator enemy robots */ - for (Robot world_robot : world_enemy_robots) - { - int robot_index = 0; - while (world_robot.id() != simulator_enemy_robots[robot_index++].id()) - ; - Robot simulator_robot = simulator_enemy_robots[robot_index - 1]; - - /* find difference in robot[i] position */ - Point world_robot_pos = world_robot.position(); - Point simulator_robot_pos = simulator_robot.position(); - double robot_displacement = (world_robot_pos - simulator_robot_pos).length(); - - /* find difference in robot[i] velocity */ - Vector world_robot_vel = world_robot.velocity(); - Vector simulator_robot_vel = simulator_robot.velocity(); - double robot_velocity_diff = (world_robot_vel - simulator_robot_vel).length(); - robots_displacement.push_back(robot_displacement); - robots_velocity_diff.push_back(robot_velocity_diff); - } - - auto wall_start_time = std::chrono::steady_clock::now(); - bool validation_functions_done = false; - - // TODO (#2419): remove this to re-enable sigfpe checks - fedisableexcept(FE_INVALID | FE_OVERFLOW); - simulator->stepSimulation(simulation_time_step); - // TODO (#2419): remove this to re-enable sigfpe checks - feenableexcept(FE_INVALID | FE_OVERFLOW); - updateSensorFusion(simulator); - - if (friendly_sensor_fusion.getWorld().has_value() && - enemy_sensor_fusion.getWorld().has_value()) - { - *friendly_world = friendly_sensor_fusion.getWorld().value(); - *enemy_world = enemy_sensor_fusion.getWorld().value(); - LOG(VISUALIZE) << *createWorld(*friendly_world); - - validation_functions_done = validateAndCheckCompletion( - terminating_function_validators, non_terminating_function_validators); - if (validation_functions_done) - { - return validation_functions_done; - } - - updatePrimitives(*friendly_world, *enemy_world, - simulator); // pass friendly and enemy world - - if (run_simulation_in_realtime) - { - sleep(wall_start_time, ai_time_step); - } - } - else - { - LOG(WARNING) << "SensorFusion did not output a valid World"; - } - return validation_functions_done; -} diff --git a/src/software/simulated_tests/simulated_er_force_sim_test_fixture.h b/src/software/simulated_tests/simulated_er_force_sim_test_fixture.h deleted file mode 100644 index 334d69feed..0000000000 --- a/src/software/simulated_tests/simulated_er_force_sim_test_fixture.h +++ /dev/null @@ -1,205 +0,0 @@ -#pragma once - -#include "proto/play_info_msg.pb.h" -#include "shared/test_util/tbots_gtest_main.h" -#include "software/ai/hl/stp/play/halt_play/halt_play.h" -#include "software/sensor_fusion/sensor_fusion.h" -#include "software/simulated_tests/cpp_validation/non_terminating_function_validator.h" -#include "software/simulated_tests/cpp_validation/terminating_function_validator.h" -#include "software/simulation/er_force_simulator.h" - -/** - * This is a test fixture designed to make it easy to write integration tests. It provides - * an easy interface to set up robots on the field, and then validate how the world - * changes over time during simulation. This allows us to easily write tests for - * the AI's behaviour. - */ -class SimulatedErForceSimTestFixture : public ::testing::Test -{ - public: - explicit SimulatedErForceSimTestFixture(); - - protected: - void SetUp() override; - - /** - * Starts the simulation using the current state of the simulator, and runs - * the given ValidationFunctions on each new state of the World. The test - * will succeed if - * - There are no terminating validation functions and all non-terminating - * validation functions pass for the duration of the test. In this case - * the test will run for the duration of the timeout. - * - There is at least one terminating validation function, and all - * validation functions (terminating and non-terminating) pass. In this case - * the test will run until all terminating validation functions have - * completed or the timeout is exceeded, whichever comes first - * - * This function will block until the test has either succeeded or encounters - * a fatal failure. - * - * @pre The robot IDs must not be duplicated and must not match the ID - * of any robot already on the specified team. - * - * @throws runtime_error if any of the given robot ids are duplicated, or a - * robot already exists on the specified team with one of the new IDs - * - * @param field_type The type of field to run the test on - * @param ball The ball state to run the test with - * @param friendly_robots The friendly robot states with ID to run the test with - * @param enemy_robots The enemy robot states with ID to run the test with - * @param terminating_validation_functions The terminating validation functions - * to check during the test - * @param non_terminating_validation_functions The non-terminating validation - * functions to check during the test - * @param timeout The maximum duration of simulated time to run the test for. - * If the test has not passed by the time this timeout is exceeded, the test - * will fail. - */ - void runTest( - const TbotsProto::FieldType &field_type, const BallState &ball, - const std::vector &friendly_robots, - const std::vector &enemy_robots, - const std::vector &terminating_validation_functions, - const std::vector &non_terminating_validation_functions, - const Duration &timeout, const bool ramping = false); - - /** - * Registers a new tick time for calculating friendly tick time statistics - * - * @param tick_time_ms The tick time in milliseconds - */ - void registerFriendlyTickTime(double tick_time_ms); - - /** - * Registers a new tick time for calculating enemy tick time statistics - * - * @param tick_time_ms The tick time in milliseconds - */ - void registerEnemyTickTime(double tick_time_ms); - - // The dynamic params being used in the tests - TbotsProto::ThunderbotsConfig friendly_thunderbots_config; - TbotsProto::ThunderbotsConfig enemy_thunderbots_config; - - private: - /** - * Runs one tick of the test and checks if the validation function is done - * - * @param simulation_time_step time step for stepping the simulation - * @param ai_time_step minimum time for one tick of AI - * @param world the shared_ptr to the world that is updated by this function - * @param simulator The simulator to tick test on - * @param ball_displacement the ball displacement of the ball in this tick - * @param ball_velocity_diff the ball velocity difference in this tick - * @param robots_displacement the array that saves the displacement of each robot in - * id order - * @param robots_velocity_diff the array that saves the velocity difference of each - * robot in id order - * @return if validation functions are done - */ - bool tickTest(Duration simulation_time_step, Duration ai_time_step, - std::shared_ptr friendly_world, - std::shared_ptr enemy_world, - std::shared_ptr simulator, double &ball_displacement, - double &ball_velocity_diff, std::vector &robots_displacement, - std::vector &robots_velocity_diff); - - /** - * Sets configs that are common to the friendly and enemy teams - * - * @param mutable_thunderbots_config A mutable thunderbots config - */ - static void setCommonConfigs( - TbotsProto::ThunderbotsConfig &mutable_thunderbots_config); - - /** - * A helper function that updates SensorFusion with the latest data from the - * ErForceSimulator - * - * @param simulator The simulator to update sensor fusion with - */ - void updateSensorFusion(std::shared_ptr simulator); - - /** - * Updates primitives in the simulator based on the new world - * - * @param friendly_world to update primitives with - * @param enemy_world to update primitives with - * @param simulator_to_update The simulator to update - */ - virtual void updatePrimitives( - const World &friendly_world, const World &enemy_world, - std::shared_ptr simulator_to_update) = 0; - - /** - * Gets play info message for displaying on the FullSystemGUI - * - * @return play info message to display, if any - */ - virtual std::optional getPlayInfo() = 0; - - /** - * Runs the given function validators and returns whether or not the - * FunctionValidators have completed (Note: completed does not necessarily - * mean passed). - * - * @param terminating_function_validators The TerminatingFunctionValidators to check - * @param non_terminating_function_validators The NonTerminatingFunctionValidators to - * check - * - * @return true if there is at least one TerminatingFunctionValidator and all - * TerminatingFunctionValidators have completed, and false otherwise - */ - static bool validateAndCheckCompletion( - std::vector &terminating_function_validators, - std::vector - &non_terminating_function_validators); - - /** - * Puts the current thread to sleep such that each simulation step will take - * the desired amount of real-world "wall" time. - * - * @param wall_start_time The time at which the most recent simulation step started, - * in wall time - * @param desired_wall_tick_time How long each simulation step should take - * in wall-clock time - */ - static void sleep(const std::chrono::steady_clock::time_point &wall_start_time, - const Duration &desired_wall_tick_time); - - // The simulator needs to be a pointer so that we can destroy and re-create - // the object in the SetUp function. Because the simulator has no - // copy assignment operator, we have to make it a dynamically-allocated - // object so we can assign new instances to this variable - std::shared_ptr simulator; - // The SensorFusion being tested and used in simulation - SensorFusion friendly_sensor_fusion; - SensorFusion enemy_sensor_fusion; - - std::vector non_terminating_function_validators; - std::vector terminating_function_validators; - - // If false, runs the simulation as fast as possible. - // If true, introduces artificial delay so that simulation - // time passes at the same speed a real life time - bool run_simulation_in_realtime; - - // These variables track tick time statistics - // Total duration of all ticks registered - double total_friendly_tick_duration; - double total_enemy_tick_duration; - // The max tick duration registered - double max_friendly_tick_duration; - double max_enemy_tick_duration; - // The min tick duration registered - double min_friendly_tick_duration; - double min_enemy_tick_duration; - // Total number of ticks registered - unsigned int friendly_tick_count; - unsigned int enemy_tick_count; - - // The rate at which camera data will be simulated and given to SensorFusion. - // Each sequential "camera frame" will be 1 / SIMULATED_CAMERA_FPS time step - // ahead of the previous one - static constexpr unsigned int SIMULATED_CAMERA_FPS = 60; -}; diff --git a/src/software/simulated_tests/terminating_validation_functions/BUILD b/src/software/simulated_tests/terminating_validation_functions/BUILD deleted file mode 100644 index 75e7767552..0000000000 --- a/src/software/simulated_tests/terminating_validation_functions/BUILD +++ /dev/null @@ -1,33 +0,0 @@ -package(default_visibility = ["//visibility:public"]) - -cc_library( - name = "terminating_validation_functions", - testonly = True, - srcs = [ - "ball_at_point_validation.cpp", - "ball_kicked_validation.cpp", - "friendly_scored_validation.cpp", - "robot_halt_validation.cpp", - "robot_in_center_circle_validation.cpp", - "robot_in_polygon_validation.cpp", - "robot_received_ball_validation.cpp", - "robot_state_validation.cpp", - ], - hdrs = [ - "ball_at_point_validation.h", - "ball_kicked_validation.h", - "friendly_scored_validation.h", - "robot_halt_validation.h", - "robot_in_center_circle_validation.h", - "robot_in_polygon_validation.h", - "robot_received_ball_validation.h", - "robot_state_validation.h", - ], - deps = [ - "//software/geom/algorithms", - "//software/simulated_tests/cpp_validation:validation_function", - "//software/world", - "@boost//:coroutine2", - "@googletest//:gtest", - ], -) diff --git a/src/software/simulated_tests/terminating_validation_functions/ball_at_point_validation.cpp b/src/software/simulated_tests/terminating_validation_functions/ball_at_point_validation.cpp deleted file mode 100644 index 670da9b54d..0000000000 --- a/src/software/simulated_tests/terminating_validation_functions/ball_at_point_validation.cpp +++ /dev/null @@ -1,15 +0,0 @@ -#include "software/simulated_tests/terminating_validation_functions/ball_at_point_validation.h" - -#include "software/logger/logger.h" - - -void ballAtPoint(Point point, std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) -{ - while (!((world_ptr->ball().position() - point).length() < 0.05)) - { - std::stringstream ss; - ss << point; - yield("Ball was not at Point " + ss.str() + " yet"); - } -} diff --git a/src/software/simulated_tests/terminating_validation_functions/ball_at_point_validation.h b/src/software/simulated_tests/terminating_validation_functions/ball_at_point_validation.h deleted file mode 100644 index f9f29b4c80..0000000000 --- a/src/software/simulated_tests/terminating_validation_functions/ball_at_point_validation.h +++ /dev/null @@ -1,13 +0,0 @@ -#pragma once - -#include "software/simulated_tests/cpp_validation/validation_function.h" -#include "software/world/world.h" - -/** - * Checks if the ball is at the point - * @param point the point that the ball is expected to be at - * @param world_ptr the world pointer given by the simulator. Gets updated every tick - * @param yield yields control to the next routine (coroutines) with error message - */ -void ballAtPoint(Point point, std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield); diff --git a/src/software/simulated_tests/terminating_validation_functions/ball_kicked_validation.cpp b/src/software/simulated_tests/terminating_validation_functions/ball_kicked_validation.cpp deleted file mode 100644 index b72f1d4e31..0000000000 --- a/src/software/simulated_tests/terminating_validation_functions/ball_kicked_validation.cpp +++ /dev/null @@ -1,15 +0,0 @@ -#include "software/simulated_tests/terminating_validation_functions/ball_kicked_validation.h" - -#include "software/logger/logger.h" - - -void ballKicked(Angle angle, std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) -{ - while (!world_ptr->ball().hasBallBeenKicked(angle)) - { - std::stringstream ss; - ss << angle; - yield("Ball was not kicked at the angle " + ss.str()); - } -} diff --git a/src/software/simulated_tests/terminating_validation_functions/ball_kicked_validation.h b/src/software/simulated_tests/terminating_validation_functions/ball_kicked_validation.h deleted file mode 100644 index 6cdb5ee77a..0000000000 --- a/src/software/simulated_tests/terminating_validation_functions/ball_kicked_validation.h +++ /dev/null @@ -1,14 +0,0 @@ -#pragma once - -#include "software/simulated_tests/cpp_validation/validation_function.h" -#include "software/world/world.h" - -/** - * Checks if the robot represented by robot_id has kicked the ball at - * any point in the test - * @param angle the angle that we expect the ball to be kicked towards - * @param world_ptr the world pointer given by the simulator. Gets updated every tick - * @param yield yields control to the next routine (coroutines) with error message - */ -void ballKicked(Angle angle, std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield); diff --git a/src/software/simulated_tests/terminating_validation_functions/friendly_scored_validation.cpp b/src/software/simulated_tests/terminating_validation_functions/friendly_scored_validation.cpp deleted file mode 100644 index a54ba99f2c..0000000000 --- a/src/software/simulated_tests/terminating_validation_functions/friendly_scored_validation.cpp +++ /dev/null @@ -1,12 +0,0 @@ -#include "software/simulated_tests/terminating_validation_functions/friendly_scored_validation.h" - -#include "software/geom/algorithms/contains.h" - -void friendlyScored(std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) -{ - while (!contains(world_ptr->field().enemyGoal(), world_ptr->ball().position())) - { - yield("The friendly team has not scored"); - } -} diff --git a/src/software/simulated_tests/terminating_validation_functions/friendly_scored_validation.h b/src/software/simulated_tests/terminating_validation_functions/friendly_scored_validation.h deleted file mode 100644 index 7dff724c4b..0000000000 --- a/src/software/simulated_tests/terminating_validation_functions/friendly_scored_validation.h +++ /dev/null @@ -1,12 +0,0 @@ -#pragma once - -#include "software/simulated_tests/cpp_validation/validation_function.h" -#include "software/world/world.h" - -/** - * Checks if the ball went in the enemies net - * @param world_ptr the world pointer given by the simulator. Gets updated every tick - * @param yield yields control to the next routine (coroutines) - */ -void friendlyScored(std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield); diff --git a/src/software/simulated_tests/terminating_validation_functions/robot_halt_validation.cpp b/src/software/simulated_tests/terminating_validation_functions/robot_halt_validation.cpp deleted file mode 100644 index aac76a85ef..0000000000 --- a/src/software/simulated_tests/terminating_validation_functions/robot_halt_validation.cpp +++ /dev/null @@ -1,18 +0,0 @@ -#include "software/simulated_tests/terminating_validation_functions/robot_halt_validation.h" - -#include "software/geom/algorithms/contains.h" -#include "software/logger/logger.h" - -void robotHalt(std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield) -{ - for (const auto& robot : world_ptr->friendlyTeam().getAllRobots()) - { - double robot_speed = robot.velocity().length(); - - if (robot_speed > 1e-3) - { - yield("Robot " + std::to_string(robot.id()) + - " has not stopped and is moving at " + std::to_string(robot_speed)); - } - } -} diff --git a/src/software/simulated_tests/terminating_validation_functions/robot_halt_validation.h b/src/software/simulated_tests/terminating_validation_functions/robot_halt_validation.h deleted file mode 100644 index 5bed2a28b5..0000000000 --- a/src/software/simulated_tests/terminating_validation_functions/robot_halt_validation.h +++ /dev/null @@ -1,11 +0,0 @@ -#pragma once - -#include "software/simulated_tests/cpp_validation/validation_function.h" -#include "software/world/world.h" - -/** - * Checks if robots have stopped moving - * @param world_ptr the world pointer given by the simulator. Gets updated every tick - * @param yield yields control to the next routine (coroutines) with error message - */ -void robotHalt(std::shared_ptr world_ptr, ValidationCoroutine::push_type& yield); diff --git a/src/software/simulated_tests/terminating_validation_functions/robot_in_center_circle_validation.cpp b/src/software/simulated_tests/terminating_validation_functions/robot_in_center_circle_validation.cpp deleted file mode 100644 index 7c32365c3d..0000000000 --- a/src/software/simulated_tests/terminating_validation_functions/robot_in_center_circle_validation.cpp +++ /dev/null @@ -1,25 +0,0 @@ -#include "software/simulated_tests/terminating_validation_functions/robot_in_center_circle_validation.h" - -#include "software/geom/algorithms/contains.h" -#include "software/logger/logger.h" - -void robotInCenterCircle(std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) -{ - auto robot_in_center_circle = [](std::shared_ptr world_ptr) - { - std::vector robots = world_ptr->friendlyTeam().getAllRobots(); - return std::any_of(robots.begin(), robots.end(), - [world_ptr](Robot robot) - { - Point position = robot.position(); - return contains(world_ptr->field().centerCircle(), - position); - }); - }; - - while (!robot_in_center_circle(world_ptr)) - { - yield("No robot has not entered the center circle"); - } -} diff --git a/src/software/simulated_tests/terminating_validation_functions/robot_in_center_circle_validation.h b/src/software/simulated_tests/terminating_validation_functions/robot_in_center_circle_validation.h deleted file mode 100644 index 755c473664..0000000000 --- a/src/software/simulated_tests/terminating_validation_functions/robot_in_center_circle_validation.h +++ /dev/null @@ -1,12 +0,0 @@ -#pragma once - -#include "software/simulated_tests/cpp_validation/validation_function.h" -#include "software/world/world.h" - -/** - * Checks if a friendly robot is in center circle of field, else fails assertion. - * @param world_ptr the world pointer given by the simulator. Gets updated every tick - * @param yield yields control to the next routine (coroutines) with error message - */ -void robotInCenterCircle(std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield); diff --git a/src/software/simulated_tests/terminating_validation_functions/robot_in_polygon_validation.cpp b/src/software/simulated_tests/terminating_validation_functions/robot_in_polygon_validation.cpp deleted file mode 100644 index 7203cafd6b..0000000000 --- a/src/software/simulated_tests/terminating_validation_functions/robot_in_polygon_validation.cpp +++ /dev/null @@ -1,32 +0,0 @@ -#include "software/simulated_tests/terminating_validation_functions/robot_in_polygon_validation.h" - -#include "software/geom/algorithms/contains.h" -#include "software/logger/logger.h" - -void robotInPolygon(Polygon polygon, int count, std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) -{ - auto num_robots_in_polygon = [polygon](std::shared_ptr world_ptr) - { - std::vector robots = world_ptr->friendlyTeam().getAllRobots(); - int total = 0; - for (Robot robot : robots) - { - Point position = robot.position(); - if (contains(polygon, position)) - { - total++; - } - } - return total; - }; - - while (num_robots_in_polygon(world_ptr) < count) - { - std::stringstream ss_poly; - ss_poly << polygon; - - yield("There were not " + std::to_string(count) + " robots simultaneously in " + - ss_poly.str()); - } -} diff --git a/src/software/simulated_tests/terminating_validation_functions/robot_in_polygon_validation.h b/src/software/simulated_tests/terminating_validation_functions/robot_in_polygon_validation.h deleted file mode 100644 index 4b3c927aa7..0000000000 --- a/src/software/simulated_tests/terminating_validation_functions/robot_in_polygon_validation.h +++ /dev/null @@ -1,14 +0,0 @@ -#pragma once - -#include "software/simulated_tests/cpp_validation/validation_function.h" -#include "software/world/world.h" - -/** - * Checks if the some given number of robots are in the polygon - * @param polygon the polygon the robot should be within - * @param count the number of robots in the polygon - * @param world_ptr the world pointer given by the simulator. Gets updated every tick - * @param yield yields control to the next routine (coroutines) with error message - */ -void robotInPolygon(Polygon polygon, int count, std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield); diff --git a/src/software/simulated_tests/terminating_validation_functions/robot_received_ball_validation.cpp b/src/software/simulated_tests/terminating_validation_functions/robot_received_ball_validation.cpp deleted file mode 100644 index 43b03b1358..0000000000 --- a/src/software/simulated_tests/terminating_validation_functions/robot_received_ball_validation.cpp +++ /dev/null @@ -1,24 +0,0 @@ -#include "software/simulated_tests/terminating_validation_functions/robot_received_ball_validation.h" - -#include "software/logger/logger.h" - - -void robotReceivedBall(std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield) -{ - auto ball_near_dribbler = [](std::shared_ptr world_ptr) - { - std::vector robots = world_ptr->friendlyTeam().getAllRobots(); - return std::any_of(robots.begin(), robots.end(), - [world_ptr](Robot robot) - { - Point ball_position = world_ptr->ball().position(); - return robot.isNearDribbler(ball_position, 0.02); - }); - }; - - while (!ball_near_dribbler(world_ptr)) - { - yield("No robot has received the ball"); - } -} diff --git a/src/software/simulated_tests/terminating_validation_functions/robot_received_ball_validation.h b/src/software/simulated_tests/terminating_validation_functions/robot_received_ball_validation.h deleted file mode 100644 index 0b2c18aed1..0000000000 --- a/src/software/simulated_tests/terminating_validation_functions/robot_received_ball_validation.h +++ /dev/null @@ -1,12 +0,0 @@ -#pragma once - -#include "software/simulated_tests/cpp_validation/validation_function.h" -#include "software/world/world.h" - -/** - * Checks if any robot has received the ball at any point in the test - * @param world_ptr the world pointer given by the simulator. Gets updated every tick - * @param yield yields control to the next routine (coroutines) with error message - */ -void robotReceivedBall(std::shared_ptr world_ptr, - ValidationCoroutine::push_type& yield); diff --git a/src/software/simulated_tests/terminating_validation_functions/robot_state_validation.cpp b/src/software/simulated_tests/terminating_validation_functions/robot_state_validation.cpp deleted file mode 100644 index ad28599b70..0000000000 --- a/src/software/simulated_tests/terminating_validation_functions/robot_state_validation.cpp +++ /dev/null @@ -1,107 +0,0 @@ -#include "software/simulated_tests/terminating_validation_functions/robot_state_validation.h" - -#include "software/logger/logger.h" - -void robotAtPosition(RobotId robot_id, std::shared_ptr world_ptr, - const Point& destination, double close_to_destination_threshold, - ValidationCoroutine::push_type& yield) -{ - auto robot_is_not_at_destination = - [robot_id, destination, close_to_destination_threshold]( - std::shared_ptr world_ptr) -> std::optional - { - std::optional robot_optional = - world_ptr->friendlyTeam().getRobotById(robot_id); - CHECK(robot_optional.has_value()) - << "There is no robot with ID: " + std::to_string(robot_id); - Robot robot = robot_optional.value(); - if ((robot.position() - destination).length() > close_to_destination_threshold) - { - return robot.position(); - } - else - { - return std::nullopt; - } - }; - - while (auto robot_destination_opt = robot_is_not_at_destination(world_ptr)) - { - std::stringstream ss; - ss << "Robot with ID " << robot_id << " is at position " - << robot_destination_opt.value() << ", expected to be at " << destination; - yield(ss.str()); - } -} - -void robotAtOrientation(RobotId robot_id, std::shared_ptr world_ptr, - const Angle& orientation, - const Angle& close_to_orientation_threshold, - ValidationCoroutine::push_type& yield) -{ - auto robot_is_not_at_orientation = - [robot_id, orientation, close_to_orientation_threshold]( - std::shared_ptr world_ptr) -> std::optional - { - std::optional robot_optional = - world_ptr->friendlyTeam().getRobotById(robot_id); - CHECK(robot_optional.has_value()) - << "There is no robot with ID: " + std::to_string(robot_id); - Robot robot = robot_optional.value(); - if (robot.orientation().minDiff(orientation) > close_to_orientation_threshold) - { - return robot.orientation(); - } - else - { - return std::nullopt; - } - }; - - - while (auto robot_orientation_opt = robot_is_not_at_orientation(world_ptr)) - { - std::stringstream ss; - ss << "Robot " << std::to_string(robot_id) << " does not have orientation of " - << orientation << ", instead it has orientation of " - << robot_orientation_opt.value(); - yield(ss.str()); - } -} - -void robotAtAngularVelocity(RobotId robot_id, std::shared_ptr world_ptr, - const AngularVelocity& angular_velocity, - const AngularVelocity& close_to_angular_velocity_threshold, - ValidationCoroutine::push_type& yield) -{ - auto robot_is_not_at_angular_velocity = - [robot_id, angular_velocity, close_to_angular_velocity_threshold]( - std::shared_ptr world_ptr) -> std::optional - { - std::optional robot_optional = - world_ptr->friendlyTeam().getRobotById(robot_id); - CHECK(robot_optional.has_value()) - << "There is no robot with ID: " + std::to_string(robot_id); - - Robot robot = robot_optional.value(); - if (robot.angularVelocity().minDiff(angular_velocity) > - close_to_angular_velocity_threshold) - { - return robot.angularVelocity(); - } - else - { - return std::nullopt; - } - }; - - while (auto robot_angular_velocity_opt = robot_is_not_at_angular_velocity(world_ptr)) - { - std::stringstream ss; - ss << "Robot " << std::to_string(robot_id) - << " does not have angular velocity of " << angular_velocity - << ", instead it has angular velocity of " - << robot_angular_velocity_opt.value(); - yield(ss.str()); - } -} diff --git a/src/software/simulated_tests/terminating_validation_functions/robot_state_validation.h b/src/software/simulated_tests/terminating_validation_functions/robot_state_validation.h deleted file mode 100644 index a65acdd1cf..0000000000 --- a/src/software/simulated_tests/terminating_validation_functions/robot_state_validation.h +++ /dev/null @@ -1,51 +0,0 @@ -#pragma once - -#include "software/simulated_tests/cpp_validation/validation_function.h" -#include "software/world/world.h" - -/** - * Checks if the robot represented by robot_id has arrived at the destination at - * any point in the test - * @param robot_id the ID of the robot in question, there must exist a robot - * for the given robot_id - * @param world_ptr the world pointer given by the simulator. Gets updated every tick - * @param destination The point to arrive at - * @param close_to_destination_threshold The threshold where the robot is considered close - * enough to the destination in metres - * @param yield yields control to the next routine (coroutines) - */ -void robotAtPosition(RobotId robot_id, std::shared_ptr world_ptr, - const Point& destination, double close_to_destination_threshold, - ValidationCoroutine::push_type& yield); - -/** - * Checks if the robot represented by robot_id is at the given orientation at - * any point in the test - * @param robot_id the ID of the robot in question, there must exist a robot - * for the given robot_id - * @param world_ptr the world pointer given by the simulator. Gets updated every tick - * @param orientation The expected orientation that the robot is at - * @param close_to_orientation_threshold The threshold where the robot is considered close - * enough to the expected orientation - * @param yield yields control to the next routine (coroutines) - */ -void robotAtOrientation(RobotId robot_id, std::shared_ptr world_ptr, - const Angle& orientation, - const Angle& close_to_orientation_threshold, - ValidationCoroutine::push_type& yield); - -/** - * Checks if the robot represented by robot_id is at the given angular velocity at - * any point in the test - * @param robot_id the ID of the robot in question, there must exist a robot - * for the given robot_id - * @param world_ptr the world pointer given by the simulator. Gets updated every tick - * @param angular_velocity The expected angular velocity that the robot is at - * @param close_to_angular_velocity_threshold The threshold where the robot is considered - * close enough to the expected orientation - * @param yield yields control to the next routine (coroutines) - */ -void robotAtAngularVelocity(RobotId robot_id, std::shared_ptr world_ptr, - const AngularVelocity& angular_velocity, - const AngularVelocity& close_to_angular_velocity_threshold, - ValidationCoroutine::push_type& yield); From 0c55b0d0a277cf6e09cce23beb6e9674bca8dd9b Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Sat, 4 Apr 2026 16:29:54 -0700 Subject: [PATCH 06/57] Resolve issue #2581 TODOs --- docs/getting-started.md | 20 ++------- .../thunderscope/thunderscope_main.py | 43 ------------------- 2 files changed, 4 insertions(+), 59 deletions(-) diff --git a/docs/getting-started.md b/docs/getting-started.md index cea162cbad..740b37e4c3 100644 --- a/docs/getting-started.md +++ b/docs/getting-started.md @@ -280,24 +280,12 @@ Now that you're setup, if you can run it on the command line, you can run it in - Xbox control allows us to use a connected Xbox controller to control the robots 4. Run our SimulatedPlayTests in Thunderscope - This will launch the visualizer and simulate AI Plays, allowing us to visually see the robots acting according to their roles. - 1. For legacy C++ tests (#2581) with the visualizer: - 1. First run Thunderscope configured for receiving protobufs over unix sockets correctly: `./tbots.py run thunderscope_main --visualize_cpp_test` - 2. Then run `./tbots.py test [some_target_here] --run_sim_in_realtime` - 2. For PyTests: - - With the visualizer: `./tbots.py test [some_target_here] -t` - - Without the visualizer: `./tbots.py test [some_target_here]` - 3. For legacy C++ tests (#2581) without the visualizer: - - `./tbots.py test [some_target_here]` + - With the visualizer: `./tbots.py test [some_target_here] -t` + - Without the visualizer: `./tbots.py test [some_target_here]` 5. Run our SimulatedTacticTests in Thunderscope: - This will launch the visualizer and simulate an AI Tactic on a single robot - 1. For legacy C++ tests (#2581) with the visualizer: - - First, run Thunderscope configured for receiving protobufs over unix sockets correctly: `./tbots.py run thunderscope_main --visualize_cpp_test` - - Then run `./tbots.py test [some_target_here] --run_sim_in_realtime` - 2. For PyTests: - - With the visualizer: `./tbots.py test [some_target_here] -t` - - Without the visualizer: `./tbots.py test [some_target_here]` - 3. For legacy C++ tests (#2581) without the visualizer: - - `./tbots.py test [some_target_here]` + - With the visualizer: `./tbots.py test [some_target_here] -t` + - Without the visualizer: `./tbots.py test [some_target_here]` ## Debugging diff --git a/src/software/thunderscope/thunderscope_main.py b/src/software/thunderscope/thunderscope_main.py index 3290b65ca1..827d8423fc 100644 --- a/src/software/thunderscope/thunderscope_main.py +++ b/src/software/thunderscope/thunderscope_main.py @@ -253,49 +253,6 @@ "--launch_gc has to be ran with --run_blue or --run_yellow argument" ) - ########################################################################### - # Visualize CPP Tests # - ########################################################################### - # TODO (#2581) remove this - if args.visualize_cpp_test: - runtime_dir = "/tmp/tbots/gtest_logs" - - try: - os.makedirs(runtime_dir) - except OSError: - pass - - tscope = Thunderscope( - config=config.configure_two_ai_gamecontroller_view( - args.visualization_buffer_size - ), - layout_path=args.layout, - ) - proto_unix_io = tscope.proto_unix_io_map[ProtoUnixIOTypes.BLUE] - - # Setup LOG(VISUALIZE) handling from full system. We set from_log_visualize - # to true to decode from base64. - for arg in [ - {"proto_class": ObstacleList}, - {"proto_class": PathVisualization}, - {"proto_class": PassVisualization}, - {"proto_class": AttackerVisualization}, - {"proto_class": CostVisualization}, - {"proto_class": DebugShapes}, - {"proto_class": NamedValue}, - {"proto_class": PrimitiveSet}, - {"proto_class": World}, - {"proto_class": PlayInfo}, - {"proto_class": BallPlacementVisualization}, - ]: - proto_unix_io.attach_unix_receiver( - runtime_dir, from_log_visualize=True, **arg - ) - - proto_unix_io.attach_unix_receiver(runtime_dir + "/log", proto_class=RobotLog) - - tscope.show() - ########################################################################### # AI + Robot Communication + Robot Diagnostics # ########################################################################### From 0982bb0b96f9f6fd03a7562e48ee2d6e29450d9c Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Sat, 4 Apr 2026 16:38:24 -0700 Subject: [PATCH 07/57] Remove simulated test args from gtest main --- src/shared/test_util/BUILD | 1 - src/shared/test_util/tbots_gtest_main.cpp | 69 ++--------------------- src/shared/test_util/tbots_gtest_main.h | 28 --------- 3 files changed, 5 insertions(+), 93 deletions(-) delete mode 100644 src/shared/test_util/tbots_gtest_main.h diff --git a/src/shared/test_util/BUILD b/src/shared/test_util/BUILD index 9bd690bc67..73123bd78f 100644 --- a/src/shared/test_util/BUILD +++ b/src/shared/test_util/BUILD @@ -18,7 +18,6 @@ cc_library( name = "tbots_gtest_main", testonly = True, srcs = ["tbots_gtest_main.cpp"], - hdrs = ["tbots_gtest_main.h"], deps = [ "//software/logger", "@boost//:program_options", diff --git a/src/shared/test_util/tbots_gtest_main.cpp b/src/shared/test_util/tbots_gtest_main.cpp index 91e0cc254c..82b96ba1e1 100644 --- a/src/shared/test_util/tbots_gtest_main.cpp +++ b/src/shared/test_util/tbots_gtest_main.cpp @@ -1,75 +1,16 @@ -#include "shared/test_util/tbots_gtest_main.h" - #include +#include -#include - -#include "proto/parameters.pb.h" #include "software/logger/logger.h" -bool TbotsGtestMain::help = false; -bool TbotsGtestMain::enable_visualizer = false; -bool TbotsGtestMain::run_sim_in_realtime = false; -bool TbotsGtestMain::stop_ai_on_start = false; -std::string TbotsGtestMain::runtime_dir = "/tmp/tbots/yellow_test"; -double TbotsGtestMain::test_speed = 1.0; - +std::string runtime_dir = "/tmp/tbots/yellow_test"; -int main(int argc, char **argv) +int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); feenableexcept(FE_INVALID | FE_OVERFLOW); - boost::program_options::options_description desc{"Options"}; - - // TODO #(2510) Remove this once we port over to simulated pytests entirely - desc.add_options()("help,h", - boost::program_options::bool_switch(&TbotsGtestMain::help), - "Help screen"); - desc.add_options()( - "enable_visualizer", - boost::program_options::bool_switch(&TbotsGtestMain::enable_visualizer), - "Displays simulated test on visualizer"); - desc.add_options()( - "run_sim_in_realtime", - boost::program_options::bool_switch(&TbotsGtestMain::run_sim_in_realtime), - "Runs simulation in realtime, useful to look at simulated C++ tests in " - "thunderscope without running ThreadedFullsystemGUI"); - desc.add_options()( - "stop_ai_on_start", - boost::program_options::bool_switch(&TbotsGtestMain::stop_ai_on_start), - "If enable_visualizer is true, then stops the AI when the test starts"); - desc.add_options()( - "runtime_dir", - boost::program_options::value(&TbotsGtestMain::runtime_dir), - "The directory to output logs to. Absolute paths are recommended as the " - "working directory is inside the bazel-out directory."); - desc.add_options()( - "test_speed", boost::program_options::value(&TbotsGtestMain::test_speed), - "The speed adjustment factor. Values in the range [0.1, 1) will play the" - "test slower than realtime, and values in the range (1, 10] will play the" - "test faster than realtime i.e. 0.1 would be 10X slower than realtime, " - "and 10 would be 10X faster than realtime. Default value is 1."); - - boost::program_options::variables_map vm; - boost::program_options::store(parse_command_line(argc, argv, desc), vm); - boost::program_options::notify(vm); - - if (!TbotsGtestMain::help) - { - LoggerSingleton::initializeLogger(TbotsGtestMain::runtime_dir, nullptr); + LoggerSingleton::initializeLogger(runtime_dir, nullptr); - if (TbotsGtestMain::enable_visualizer || TbotsGtestMain::run_sim_in_realtime) - { - // disable floating point errors when using visualizer due to potential - // floating point errors in QT - fedisableexcept(FE_INVALID | FE_OVERFLOW); - } - return RUN_ALL_TESTS(); - } - else - { - std::cout << desc << std::endl; - return 0; - } + return RUN_ALL_TESTS(); } diff --git a/src/shared/test_util/tbots_gtest_main.h b/src/shared/test_util/tbots_gtest_main.h deleted file mode 100644 index bfd5b4af7f..0000000000 --- a/src/shared/test_util/tbots_gtest_main.h +++ /dev/null @@ -1,28 +0,0 @@ -#pragma once - -#include - -struct TbotsGtestMain -{ - // Controls whether the visualizer will be enabled during the tests (if implemented) - // if false, visualizer does not run during tests if true, running tests are displayed - // on the visualizer - static bool enable_visualizer; - - // Controls whether the test should run in real time - static bool run_sim_in_realtime; - - // Controls whether the AI will be stopped when the test starts only if - // enable_visualizer is true and the test uses AI - static bool stop_ai_on_start; - - // Directory to send g3log logger logs - static std::string runtime_dir; - - - // Controls the speed of the simulated test. Values in the range [0.1,1) are the slow - // down factor i.e 0.1 test_speed will play test 10X slower. Values in the range (1, - // 10] will play test faster. Default value is 1. - static double test_speed; - static bool help; -}; From 2f84b6e25187df8379de2afeaa483130611d6539 Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Sat, 4 Apr 2026 16:46:27 -0700 Subject: [PATCH 08/57] Remove TODO --- src/software/backend/BUILD | 1 - 1 file changed, 1 deletion(-) diff --git a/src/software/backend/BUILD b/src/software/backend/BUILD index 0901484cfc..2d27a363a3 100644 --- a/src/software/backend/BUILD +++ b/src/software/backend/BUILD @@ -14,7 +14,6 @@ cc_library( ) cc_library( - # TODO (#2510) Rename to SimulatorBackend name = "unix_simulator_backend", srcs = ["unix_simulator_backend.cpp"], hdrs = ["unix_simulator_backend.h"], From b440d66155e46afe43f86fd003cb12930e81e5f8 Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Sat, 4 Apr 2026 21:30:53 -0700 Subject: [PATCH 09/57] Remove visualizing c++ test flags from thunderscope and tbots.py --- src/cli/cli_params.py | 1 - src/software/thunderscope/thunderscope_main.py | 6 ------ src/tbots.py | 4 ---- 3 files changed, 11 deletions(-) diff --git a/src/cli/cli_params.py b/src/cli/cli_params.py index 30ddc699c7..234e681d05 100644 --- a/src/cli/cli_params.py +++ b/src/cli/cli_params.py @@ -86,7 +86,6 @@ class DebugBinary(str, Enum): ] EnableThunderscopeOption = Annotated[bool, Option("-t", "--enable_thunderscope")] -EnableVisualizerOption = Annotated[bool, Option("-v", "--enable_visualizer")] StopAIOnStartOption = Annotated[bool, Option("-s", "--stop_ai_on_start")] JobsOption = Annotated[str, Option("-j", "--jobs")] diff --git a/src/software/thunderscope/thunderscope_main.py b/src/software/thunderscope/thunderscope_main.py index 827d8423fc..b039227bce 100644 --- a/src/software/thunderscope/thunderscope_main.py +++ b/src/software/thunderscope/thunderscope_main.py @@ -99,12 +99,6 @@ default=False, help="Debug the simulator", ) - parser.add_argument( - "--visualize_cpp_test", - action="store_true", - default=False, - help="Visualize C++ Tests", - ) parser.add_argument( "--log_level", action="store", diff --git a/src/tbots.py b/src/tbots.py index d459267eaa..38ce1b853a 100755 --- a/src/tbots.py +++ b/src/tbots.py @@ -19,7 +19,6 @@ InteractiveModeOption, TracyOption, EnableThunderscopeOption, - EnableVisualizerOption, StopAIOnStartOption, SearchQueryArgument, TestSuiteOption, @@ -109,7 +108,6 @@ def main( interactive_search: InteractiveModeOption = False, tracy: TracyOption = False, enable_thunderscope: EnableThunderscopeOption = False, - enable_visualizer: EnableVisualizerOption = False, stop_ai_on_start: StopAIOnStartOption = False, test_suite: TestSuiteOption = False, jobs_option: JobsOption = "", @@ -182,8 +180,6 @@ def main( bazel_arguments = unknown_args if stop_ai_on_start: bazel_arguments += ["--stop_ai_on_start"] - if enable_visualizer: - bazel_arguments += ["--enable_visualizer"] if enable_thunderscope: bazel_arguments += ["--enable_thunderscope"] if flash_robots: From cf71504936cb6a08a1155a38507a636d586f0239 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sun, 5 Apr 2026 10:00:48 -0700 Subject: [PATCH 10/57] init --- .../filter/ball_kalman_filter.cpp | 381 ++++++++++++++++++ .../sensor_fusion/filter/ball_kalman_filter.h | 196 +++++++++ 2 files changed, 577 insertions(+) create mode 100644 src/software/sensor_fusion/filter/ball_kalman_filter.cpp create mode 100644 src/software/sensor_fusion/filter/ball_kalman_filter.h diff --git a/src/software/sensor_fusion/filter/ball_kalman_filter.cpp b/src/software/sensor_fusion/filter/ball_kalman_filter.cpp new file mode 100644 index 0000000000..0d1fccc31c --- /dev/null +++ b/src/software/sensor_fusion/filter/ball_kalman_filter.cpp @@ -0,0 +1,381 @@ +#include "software/sensor_fusion/filter/ball_kalman_filter.h" + +#include +#include +#include + +#include "shared/constants.h" +#include "software/geom/algorithms/closest_point.h" +#include "software/geom/algorithms/contains.h" +#include "software/math/math_functions.h" + + +BallFilter::BallFilter() : ball_detection_buffer(MAX_BUFFER_SIZE) {} + +std::optional BallFilter::estimateBallState( + const std::vector &new_ball_detections, const Rectangle &filter_area) +{ + addNewDetectionsToBuffer(new_ball_detections, filter_area); + return estimateBallStateFromBuffer(ball_detection_buffer); +} + +void BallFilter::addNewDetectionsToBuffer(std::vector new_ball_detections, + const Rectangle &filter_area) +{ + // Sort the detections in increasing order before processing. This places the oldest + // detections (with the smallest timestamp) at the front of the buffer, and the most + // recent detections (largest timestamp) at the end of the buffer. + std::sort(new_ball_detections.begin(), new_ball_detections.end()); + + for (const auto &detection : new_ball_detections) + { + // Remove any detections outside the filter area + if (!contains(filter_area, detection.position)) + { + continue; + } + + if (!ball_detection_buffer.empty()) + { + // Use the smallest timestamp to minimize time_diffs of 0 + auto detection_with_smallest_timestamp = *std::min_element( + ball_detection_buffer.begin(), ball_detection_buffer.end()); + Duration time_diff = + detection.timestamp - detection_with_smallest_timestamp.timestamp; + + // Ignore any data from the past, and any data that is as old as the oldest + // data in the buffer since it provides no additional value. This also + // prevents division by 0 when calculating the estimated velocity + if (time_diff.toSeconds() <= 0) + { + continue; + } + + // We determine if the detection is noise based on how far it is from a ball + // detection in the buffer. From this, we can calculate how fast the ball + // must have moved to reach the new detection position. If this estimated + // velocity is too far above the maximum allowed velocity, then there is a + // good chance the detection is just noise and not the real ball. In this + // case, we ignore the new "noise" data + double detection_distance = + (detection.position - detection_with_smallest_timestamp.position) + .length(); + double estimated_detection_velocity_magnitude = + detection_distance / time_diff.toSeconds(); + + // Make the maximum acceptable velocity a bit larger than the strict limits + // according to the game rules to account for measurement error, and to be a + // bit on the safe side. We don't want to risk discarding real data. + double maximum_acceptable_velocity_magnitude = + BALL_MAX_SPEED_METERS_PER_SECOND + MAX_ACCEPTABLE_BALL_SPEED_BUFFER; + if (estimated_detection_velocity_magnitude > + maximum_acceptable_velocity_magnitude) + { + // If we determine the data to be noise, remove an entry from the buffer. + // This way if we have messed up and now the ball is too far away for the + // buffer to track, the buffer will rapidly shrink and start tracking the + // ball at its new location once the buffer is empty. + // We sort the vector in decreasing order first so that we can always + // ensure any elements that are ejected from the end of the buffer are the + // oldest data + std::sort(ball_detection_buffer.rbegin(), ball_detection_buffer.rend()); + ball_detection_buffer.pop_back(); + } + else + { + // We sort the vector in decreasing order first so that we can always + // ensure any elements that are ejected from the end of the buffer are the + // oldest data + std::sort(ball_detection_buffer.rbegin(), ball_detection_buffer.rend()); + ball_detection_buffer.push_front(detection); + } + } + else + { + // If there is no data in the buffer, we always add the new data + ball_detection_buffer.push_front(detection); + } + } +} + +std::optional BallFilter::estimateBallStateFromBuffer( + boost::circular_buffer ball_detections) +{ + // Sort the detections in decreasing order before processing. This places the most + // recent detections (with the largest timestamp) at the front of the buffer, and the + // oldest detections (smallest timestamp) at the end of the buffer + std::sort(ball_detections.rbegin(), ball_detections.rend()); + + if (ball_detections.empty()) + { + return std::nullopt; + } + else if (ball_detections.size() == 1) + { + // If there is only 1 entry in the buffer, we can't fit a regression line + // or calculate a velocity so we do our best with just the position + BallState ball_state(ball_detections.front().position, Vector(0, 0), + ball_detections.front().distance_from_ground); + Ball ball(ball_state, ball_detections.front().timestamp); + return ball; + } + + std::optional adjusted_buffer_size = getAdjustedBufferSize(ball_detections); + if (!adjusted_buffer_size) + { + return std::nullopt; + } + ball_detections.resize(*adjusted_buffer_size); + + auto regression = calculateLineOfBestFit(ball_detections); + + Point filtered_position = + estimateBallPosition(ball_detections, regression.regression_line); + + auto estimated_velocity = estimateBallVelocity(ball_detections, std::nullopt); + + if (regression.regression_error < LINEAR_REGRESSION_ERROR_THRESHOLD) + { + estimated_velocity = + estimateBallVelocity(ball_detections, regression.regression_line); + } + if (!estimated_velocity) + { + return std::nullopt; + } + + BallState ball_state(filtered_position, estimated_velocity->average_velocity, + ball_detections.front().distance_from_ground); + return Ball(ball_state, ball_detections.front().timestamp); +} + +std::optional BallFilter::getAdjustedBufferSize( + boost::circular_buffer ball_detections) +{ + // Sort the detections in decreasing order before processing. This places the most + // recent detections (with the largest timestamp) at the front of the buffer, and the + // oldest detections (smallest timestamp) at the end of the buffer + std::sort(ball_detections.rbegin(), ball_detections.rend()); + + double buffer_size_velocity_magnitude_diff = + MAX_BUFFER_SIZE_VELOCITY_MAGNITUDE - MIN_BUFFER_SIZE_VELOCITY_MAGNITUDE; + + unsigned int max_buffer_size = + std::min(MAX_BUFFER_SIZE, static_cast(ball_detections.size())); + unsigned int min_buffer_size = + std::min(MIN_BUFFER_SIZE, static_cast(ball_detections.size())); + double buffer_size_diff = max_buffer_size - min_buffer_size; + + std::optional velocity_estimate = + estimateBallVelocity(ball_detections); + if (!velocity_estimate) + { + return std::nullopt; + } + // Use the average of the min and max velocity magnitudes in the buffer. We use this + // rather than the average so we can quickly respond to drastic changes in the ball + // velocity, such as when the ball goes from being stationary to moving quickly (like + // when it's kicked). If the buffer is large, then it will take more time for the mean + // speed to increase enough to start shrinking the buffer. However, the average of the + // min and max values will immediately increase if the ball starts moving, so the + // buffer can start shrinking more quickly and increase the filter response time to + // these sorts of changes. + double min_max_magnitude_average = velocity_estimate->min_max_magnitude_average; + + // Between the min and max velocity magnitudes, we linearly scale the size of the + // buffer + double linear_offset = + MIN_BUFFER_SIZE_VELOCITY_MAGNITUDE + (buffer_size_velocity_magnitude_diff / 2); + double linear_scaling_factor = linear(min_max_magnitude_average, linear_offset, + buffer_size_velocity_magnitude_diff); + int buffer_size = + max_buffer_size - + static_cast(std::floor(linear_scaling_factor * buffer_size_diff)); + + return static_cast(buffer_size); +} + +BallFilter::LinearRegressionResults BallFilter::calculateLineOfBestFit( + boost::circular_buffer ball_detections) +{ + if (ball_detections.size() < 2) + { + throw std::invalid_argument("At least 2 elements required for linear regression"); + } + + auto x_vs_y_regression = calculateLinearRegression(ball_detections); + + // Linear regression cannot fit a vertical line. To get around this, we fit two lines, + // one with x and y swapped, so any vertical line becomes horizontal. Then we take the + // line of the two that fit the best. + boost::circular_buffer swapped_ball_detections = ball_detections; + for (auto &detection : swapped_ball_detections) + { + detection.position = Point(detection.position.y(), detection.position.x()); + } + auto y_vs_x_regression = calculateLinearRegression(swapped_ball_detections); + // Because we swapped the coordinates of the input, we have to swap the coordinates of + // the output to get back to our expected coordinate space + y_vs_x_regression.regression_line.swapXY(); + + // We use the regression from above with the least error + if (x_vs_y_regression.regression_error < y_vs_x_regression.regression_error) + { + return x_vs_y_regression; + } + else + { + return y_vs_x_regression; + } +} + +BallFilter::LinearRegressionResults BallFilter::calculateLinearRegression( + boost::circular_buffer ball_detections) +{ + if (ball_detections.size() < 2) + { + throw std::invalid_argument("At least 2 elements required for linear regression"); + } + + // Sort the detections in increasing order before processing. This places the oldest + // detections (smallest timestamp) at the front of the buffer, and the most recent + // detections (with the largest timestamp) at the end of the buffer + std::sort(ball_detections.begin(), ball_detections.end()); + + // Construct matrix A and vector b for linear regression. The first column of A + // contains the bias variable, and the second column contains the x coordinates of the + // ball. Vector b contains the y coordinates of the ball. + Eigen::MatrixXf A(ball_detections.size(), 2); + Eigen::VectorXf b(ball_detections.size()); + for (unsigned i = 0; i < ball_detections.size(); i++) + { + // This extra column of 1's is the bias variable, so that we can regress with a + // y-intercept + A(i, 0) = 1.0; + A(i, 1) = static_cast(ball_detections.at(i).position.x()); + + b(i) = static_cast(ball_detections.at(i).position.y()); + } + + // Perform linear regression to find the line of best fit through the ball positions. + // This is solving the formula Ax = b, where x is the vector we want to solve for. + Eigen::Vector2f regression_vector = + A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b); + // How to calculate the error is from + // https://eigen.tuxfamily.org/dox/group__TutorialLinearAlgebra.html + // NOTE: using absolute error instead of relative because coordinates + // values should not affect error, also handles divide by 0 error + double regression_error = (A * regression_vector - b).norm(); // norm() is L2 norm + + // Find 2 points on the regression line that we solved for, and use this to construct + // our own Line class + Eigen::Vector2f p1_vec(1, 0); + Point p1(0, p1_vec.dot(regression_vector)); + Eigen::Vector2f p2_vec(1, 1); + Point p2(1, p2_vec.dot(regression_vector)); + Line regression_line = Line(p1, p2); + + LinearRegressionResults results({regression_line, regression_error}); + + return results; +} + +Point BallFilter::estimateBallPosition( + boost::circular_buffer ball_detections, const Line ®ression_line) +{ + if (ball_detections.empty()) + { + throw std::invalid_argument( + "Non-empty buffer required to estimate ball position"); + } + + // Take the position of the most recent ball position and project it onto the line of + // best fit. We do this because we assume the ball must be travelling along its + // velocity vector (the line), and this allows us to return more stable position + // values since the line of best fit is less likely to fluctuate compared to the raw + // position of a ball detection + BallDetection latest_ball_detection = ball_detections.front(); + return closestPoint(latest_ball_detection.position, regression_line); +} + +std::optional BallFilter::estimateBallVelocity( + boost::circular_buffer ball_detections, + const std::optional &ball_regression_line) +{ + // Sort the detections in increasing order before processing. This places the oldest + // detections (smallest timestamp) at the front of the buffer, and the most recent + // detections (with the largest timestamp) at the end of the buffer + std::sort(ball_detections.begin(), ball_detections.end()); + + std::vector ball_velocities; + std::vector ball_velocity_magnitudes; + for (unsigned i = 1; i < ball_detections.size(); i++) + { + for (unsigned j = i; j < ball_detections.size(); j++) + { + BallDetection previous_detection = ball_detections.at(i - 1); + BallDetection current_detection = ball_detections.at(j); + + Duration time_diff = + current_detection.timestamp - previous_detection.timestamp; + // Avoid division by 0. If we have adjacent detections with the same timestamp + // the velocity cannot be calculated + if (time_diff.toSeconds() == 0) + { + continue; + } + + // Project the detection positions onto the regression line if it was provided + Point current_position; + Point previous_position; + if (ball_regression_line) + { + current_position = closestPoint(current_detection.position, + ball_regression_line.value()); + previous_position = closestPoint(previous_detection.position, + ball_regression_line.value()); + } + else + { + current_position = current_detection.position; + previous_position = previous_detection.position; + } + Vector velocity_vector = current_position - previous_position; + double velocity_magnitude = velocity_vector.length() / time_diff.toSeconds(); + Vector velocity = velocity_vector.normalize(velocity_magnitude); + + ball_velocity_magnitudes.emplace_back(velocity_magnitude); + ball_velocities.emplace_back(velocity); + } + } + + if (ball_velocities.empty() || ball_velocity_magnitudes.empty()) + { + return std::nullopt; + } + + double velocity_magnitude_sum = 0; + for (const auto &velocity_magnitude : ball_velocity_magnitudes) + { + velocity_magnitude_sum += velocity_magnitude; + } + double average_velocity_magnitude = + velocity_magnitude_sum / static_cast(ball_velocity_magnitudes.size()); + double velocity_magnitude_max = *std::max_element(ball_velocity_magnitudes.begin(), + ball_velocity_magnitudes.end()); + double velocity_magnitude_min = *std::min_element(ball_velocity_magnitudes.begin(), + ball_velocity_magnitudes.end()); + double min_max_average = (velocity_magnitude_min + velocity_magnitude_max) / 2.0; + + Vector velocity_vector_sum = Vector(0, 0); + for (const auto &velocity : ball_velocities) + { + velocity_vector_sum += velocity; + } + Vector average_velocity = velocity_vector_sum.normalize(average_velocity_magnitude); + + BallVelocityEstimate velocity_data( + {average_velocity, average_velocity_magnitude, min_max_average}); + + return velocity_data; +} diff --git a/src/software/sensor_fusion/filter/ball_kalman_filter.h b/src/software/sensor_fusion/filter/ball_kalman_filter.h new file mode 100644 index 0000000000..57c83ed291 --- /dev/null +++ b/src/software/sensor_fusion/filter/ball_kalman_filter.h @@ -0,0 +1,196 @@ +#pragma once + +#include +#include + +#include "software/geom/line.h" +#include "software/geom/point.h" +#include "software/geom/rectangle.h" +#include "software/sensor_fusion/filter/vision_detection.h" +#include "software/time/timestamp.h" +#include "software/world/ball.h" + +/** + * Given ball data from SSL Vision, filters and returns the position/velocity of the + * "real" ball. + * + * This ball filter stores a buffer of previous SSL Vision detections, and uses linear + * regression to find the path the ball is travelling on and estimate its position + * and velocity. This buffer/regression system was chosen because it results in a + * very stable output, particularly for the ball velocity. The data we receive isn't + * perfect (which is why we have a filter). If we receive a noisy position that is off + * the ball's current trajectory, it will have minimal impact. This means that as + * the ball is travelling, this filter will return a very steady velocity vector. + * This is important because small deviations in velocity orientation can have large + * effects when the AI tries to predict the future position of the ball. For example, + * consistently receiving a pass relies on the ball's velocity being very stable, + * otherwise the robot would "jiggle" back and forth as the estimated receiver position + * would keep changing. + */ +class BallKalmanFilter +{ + public: + // The min and max sizes of the ball detection buffer. + // As the ball slows down, the buffer size will approach the MAX_BUFFER_SIZE. + // As the ball speeds up, the buffer size will approach the MIN_BUFFER_SIZE. + static constexpr unsigned int MIN_BUFFER_SIZE = 4; + static constexpr unsigned int MAX_BUFFER_SIZE = 10; + // If the estimated ball speed is less than this value, the largest possible buffer + // will be used by the filter + static constexpr double MIN_BUFFER_SIZE_VELOCITY_MAGNITUDE = 0.5; + // If the estimated ball speed is greater than this value, the smallest possible + // buffer will be used by the filter + static constexpr double MAX_BUFFER_SIZE_VELOCITY_MAGNITUDE = 4.0; + // The extra amount beyond the ball's max speed that we treat ball detections as valid + static constexpr double MAX_ACCEPTABLE_BALL_SPEED_BUFFER = 2.0; + // The maximum root mean squared error threshold to considering using the generated + // linear regression. + // TODO (#2752): Investigate different values of error threshold + static constexpr double LINEAR_REGRESSION_ERROR_THRESHOLD = 1000.0; + + /** + * Creates a new Ball Filter + */ + explicit BallFilter(); + + /** + * Update the filter with the new ball detection data, and returns the new + * estimated state of the ball given the new data + * + * @param new_ball_detections A list of new Ball detections + * @param filter_area The area within which the ball filter will work. Any detections + * outside of this area will be ignored. + * + * @return The new ball based on the estimated state of the ball given the new data. + * If a filtered result cannot be calculated, returns std::nullopt + */ + std::optional estimateBallState( + const std::vector& new_ball_detections, + const Rectangle& filter_area); + + private: + /** + * A simple struct we use to pass around velocity estimate data + */ + struct BallVelocityEstimate + { + Vector average_velocity; + double average_velocity_magnitude; + // The average of the max velocity magnitude and min velocity magnitude + double min_max_magnitude_average; + }; + + /** + * A simple struct to pass around linear regression data + */ + struct LinearRegressionResults + { + Line regression_line; + // Regression error is root mean squared error + double regression_error; + }; + + /** + * Adds ball detections to the buffer stored by this filter. This function will ignore + * data if: + * - the data is outside of the filter_area, or + * - the data is too far away from the current known ball position + * (since it is likely to be random noise). + * + * @param new_ball_detections The ball detections to try add to the buffer + * @param filter_area The area within which the ball filter will work. Any detections + * outside of this area will be ignored. + */ + void addNewDetectionsToBuffer(std::vector new_ball_detections, + const Rectangle& filter_area); + + /** + * Uses linear regression to filter the given list of ball detections to find the + * current "real" state of the ball. + * + * @param ball_detections The detections to filter + * + * @return The new ball based on the filtered state. If a filtered result cannot be + * calculated, returns std::nullopt + */ + static std::optional estimateBallStateFromBuffer( + boost::circular_buffer ball_detections); + + /** + * Returns how large the buffer of ball detections should be based on the ball's + * estimated velocity. A slower moving ball will result in a larger buffer size, and a + * faster ball will result in a smaller buffer size. This is because with a slow + * moving ball, we need more data in order to fit a line with reasonable accuracy, + * since the datapoints will be very close to one another. + * + * @param ball_detections The full list of ball detections + * + * @return The size the buffer should be to perform filtering operations. If an error + * occurs that prevents the size from being calculated correctly, returns std::nullopt + */ + static std::optional getAdjustedBufferSize( + boost::circular_buffer ball_detections); + + /** + * Given a buffer of ball detections, returns the line of best fit through + * the detection positions, and calculate the root mean squared error of this + * regression. + * Note: also considers vertical lines. + * + * @throws std::invalid_argument if ball_detections has less than 2 elements + * + * @param ball_detections The ball detections to fit + * + * @return The line of best fit through the given ball detection positions + */ + static LinearRegressionResults calculateLineOfBestFit( + boost::circular_buffer ball_detections); + + /** + * Given a list of ball detections, use linear regression to find a line of best fit + * through the ball positions, and calculate the root mean squared error of this + * regression. + * + * @throws std::invalid_argument if ball_detections has less than 2 elements + * + * @param ball_detections The ball detections to use in the regression + * + * @return A struct containing the regression line and error of the linear regression + */ + static LinearRegressionResults calculateLinearRegression( + boost::circular_buffer ball_detections); + + /** + * Estimates the current position of the ball given a buffer of ball detections + * and the line of best fit through them. + * + * @throws std::invalid_argument if ball_detections has less than 2 elements + * + * @param ball_detections The ball detections + * @param regression_line The line of best fit through the ball positions + * + * @return The estimated position of the ball + */ + static Point estimateBallPosition( + boost::circular_buffer ball_detections, + const Line& regression_line); + + /** + * Estimates the ball's velocity based on the current detections in the given buffer. + * If the ball_regression_line is provided, the detection positions are projected onto + * the line before the velocities are calculated. If no velocity can be estimated, + * std::nullopt is returned. + * + * @param ball_detections The ball detections to use to calculate + * @param ball_regression_line The ball_regression_line to snap detections to before + * calculating velocities. + * + * @return A struct containing various estimates of the ball's velocity based on the + * given detections. If no velocity can be estimated, std::nullopt is returned + */ + static std::optional estimateBallVelocity( + boost::circular_buffer ball_detections, + const std::optional& ball_regression_line = std::nullopt); + + boost::circular_buffer ball_detection_buffer; +}; From 1666a79ebd580fcc6a13bc982453c65b9206dcd9 Mon Sep 17 00:00:00 2001 From: StarrryNight Date: Sun, 5 Apr 2026 13:36:40 -0700 Subject: [PATCH 11/57] finish header file and some cpp --- .../filter/ball_kalman_filter.cpp | 381 ------------------ .../sensor_fusion/filter/ball_kalman_filter.h | 196 --------- .../sensor_fusion/filter/kalman_filter.cpp | 52 +++ .../sensor_fusion/filter/kalman_filter.h | 39 ++ 4 files changed, 91 insertions(+), 577 deletions(-) delete mode 100644 src/software/sensor_fusion/filter/ball_kalman_filter.cpp delete mode 100644 src/software/sensor_fusion/filter/ball_kalman_filter.h create mode 100644 src/software/sensor_fusion/filter/kalman_filter.cpp create mode 100644 src/software/sensor_fusion/filter/kalman_filter.h diff --git a/src/software/sensor_fusion/filter/ball_kalman_filter.cpp b/src/software/sensor_fusion/filter/ball_kalman_filter.cpp deleted file mode 100644 index 0d1fccc31c..0000000000 --- a/src/software/sensor_fusion/filter/ball_kalman_filter.cpp +++ /dev/null @@ -1,381 +0,0 @@ -#include "software/sensor_fusion/filter/ball_kalman_filter.h" - -#include -#include -#include - -#include "shared/constants.h" -#include "software/geom/algorithms/closest_point.h" -#include "software/geom/algorithms/contains.h" -#include "software/math/math_functions.h" - - -BallFilter::BallFilter() : ball_detection_buffer(MAX_BUFFER_SIZE) {} - -std::optional BallFilter::estimateBallState( - const std::vector &new_ball_detections, const Rectangle &filter_area) -{ - addNewDetectionsToBuffer(new_ball_detections, filter_area); - return estimateBallStateFromBuffer(ball_detection_buffer); -} - -void BallFilter::addNewDetectionsToBuffer(std::vector new_ball_detections, - const Rectangle &filter_area) -{ - // Sort the detections in increasing order before processing. This places the oldest - // detections (with the smallest timestamp) at the front of the buffer, and the most - // recent detections (largest timestamp) at the end of the buffer. - std::sort(new_ball_detections.begin(), new_ball_detections.end()); - - for (const auto &detection : new_ball_detections) - { - // Remove any detections outside the filter area - if (!contains(filter_area, detection.position)) - { - continue; - } - - if (!ball_detection_buffer.empty()) - { - // Use the smallest timestamp to minimize time_diffs of 0 - auto detection_with_smallest_timestamp = *std::min_element( - ball_detection_buffer.begin(), ball_detection_buffer.end()); - Duration time_diff = - detection.timestamp - detection_with_smallest_timestamp.timestamp; - - // Ignore any data from the past, and any data that is as old as the oldest - // data in the buffer since it provides no additional value. This also - // prevents division by 0 when calculating the estimated velocity - if (time_diff.toSeconds() <= 0) - { - continue; - } - - // We determine if the detection is noise based on how far it is from a ball - // detection in the buffer. From this, we can calculate how fast the ball - // must have moved to reach the new detection position. If this estimated - // velocity is too far above the maximum allowed velocity, then there is a - // good chance the detection is just noise and not the real ball. In this - // case, we ignore the new "noise" data - double detection_distance = - (detection.position - detection_with_smallest_timestamp.position) - .length(); - double estimated_detection_velocity_magnitude = - detection_distance / time_diff.toSeconds(); - - // Make the maximum acceptable velocity a bit larger than the strict limits - // according to the game rules to account for measurement error, and to be a - // bit on the safe side. We don't want to risk discarding real data. - double maximum_acceptable_velocity_magnitude = - BALL_MAX_SPEED_METERS_PER_SECOND + MAX_ACCEPTABLE_BALL_SPEED_BUFFER; - if (estimated_detection_velocity_magnitude > - maximum_acceptable_velocity_magnitude) - { - // If we determine the data to be noise, remove an entry from the buffer. - // This way if we have messed up and now the ball is too far away for the - // buffer to track, the buffer will rapidly shrink and start tracking the - // ball at its new location once the buffer is empty. - // We sort the vector in decreasing order first so that we can always - // ensure any elements that are ejected from the end of the buffer are the - // oldest data - std::sort(ball_detection_buffer.rbegin(), ball_detection_buffer.rend()); - ball_detection_buffer.pop_back(); - } - else - { - // We sort the vector in decreasing order first so that we can always - // ensure any elements that are ejected from the end of the buffer are the - // oldest data - std::sort(ball_detection_buffer.rbegin(), ball_detection_buffer.rend()); - ball_detection_buffer.push_front(detection); - } - } - else - { - // If there is no data in the buffer, we always add the new data - ball_detection_buffer.push_front(detection); - } - } -} - -std::optional BallFilter::estimateBallStateFromBuffer( - boost::circular_buffer ball_detections) -{ - // Sort the detections in decreasing order before processing. This places the most - // recent detections (with the largest timestamp) at the front of the buffer, and the - // oldest detections (smallest timestamp) at the end of the buffer - std::sort(ball_detections.rbegin(), ball_detections.rend()); - - if (ball_detections.empty()) - { - return std::nullopt; - } - else if (ball_detections.size() == 1) - { - // If there is only 1 entry in the buffer, we can't fit a regression line - // or calculate a velocity so we do our best with just the position - BallState ball_state(ball_detections.front().position, Vector(0, 0), - ball_detections.front().distance_from_ground); - Ball ball(ball_state, ball_detections.front().timestamp); - return ball; - } - - std::optional adjusted_buffer_size = getAdjustedBufferSize(ball_detections); - if (!adjusted_buffer_size) - { - return std::nullopt; - } - ball_detections.resize(*adjusted_buffer_size); - - auto regression = calculateLineOfBestFit(ball_detections); - - Point filtered_position = - estimateBallPosition(ball_detections, regression.regression_line); - - auto estimated_velocity = estimateBallVelocity(ball_detections, std::nullopt); - - if (regression.regression_error < LINEAR_REGRESSION_ERROR_THRESHOLD) - { - estimated_velocity = - estimateBallVelocity(ball_detections, regression.regression_line); - } - if (!estimated_velocity) - { - return std::nullopt; - } - - BallState ball_state(filtered_position, estimated_velocity->average_velocity, - ball_detections.front().distance_from_ground); - return Ball(ball_state, ball_detections.front().timestamp); -} - -std::optional BallFilter::getAdjustedBufferSize( - boost::circular_buffer ball_detections) -{ - // Sort the detections in decreasing order before processing. This places the most - // recent detections (with the largest timestamp) at the front of the buffer, and the - // oldest detections (smallest timestamp) at the end of the buffer - std::sort(ball_detections.rbegin(), ball_detections.rend()); - - double buffer_size_velocity_magnitude_diff = - MAX_BUFFER_SIZE_VELOCITY_MAGNITUDE - MIN_BUFFER_SIZE_VELOCITY_MAGNITUDE; - - unsigned int max_buffer_size = - std::min(MAX_BUFFER_SIZE, static_cast(ball_detections.size())); - unsigned int min_buffer_size = - std::min(MIN_BUFFER_SIZE, static_cast(ball_detections.size())); - double buffer_size_diff = max_buffer_size - min_buffer_size; - - std::optional velocity_estimate = - estimateBallVelocity(ball_detections); - if (!velocity_estimate) - { - return std::nullopt; - } - // Use the average of the min and max velocity magnitudes in the buffer. We use this - // rather than the average so we can quickly respond to drastic changes in the ball - // velocity, such as when the ball goes from being stationary to moving quickly (like - // when it's kicked). If the buffer is large, then it will take more time for the mean - // speed to increase enough to start shrinking the buffer. However, the average of the - // min and max values will immediately increase if the ball starts moving, so the - // buffer can start shrinking more quickly and increase the filter response time to - // these sorts of changes. - double min_max_magnitude_average = velocity_estimate->min_max_magnitude_average; - - // Between the min and max velocity magnitudes, we linearly scale the size of the - // buffer - double linear_offset = - MIN_BUFFER_SIZE_VELOCITY_MAGNITUDE + (buffer_size_velocity_magnitude_diff / 2); - double linear_scaling_factor = linear(min_max_magnitude_average, linear_offset, - buffer_size_velocity_magnitude_diff); - int buffer_size = - max_buffer_size - - static_cast(std::floor(linear_scaling_factor * buffer_size_diff)); - - return static_cast(buffer_size); -} - -BallFilter::LinearRegressionResults BallFilter::calculateLineOfBestFit( - boost::circular_buffer ball_detections) -{ - if (ball_detections.size() < 2) - { - throw std::invalid_argument("At least 2 elements required for linear regression"); - } - - auto x_vs_y_regression = calculateLinearRegression(ball_detections); - - // Linear regression cannot fit a vertical line. To get around this, we fit two lines, - // one with x and y swapped, so any vertical line becomes horizontal. Then we take the - // line of the two that fit the best. - boost::circular_buffer swapped_ball_detections = ball_detections; - for (auto &detection : swapped_ball_detections) - { - detection.position = Point(detection.position.y(), detection.position.x()); - } - auto y_vs_x_regression = calculateLinearRegression(swapped_ball_detections); - // Because we swapped the coordinates of the input, we have to swap the coordinates of - // the output to get back to our expected coordinate space - y_vs_x_regression.regression_line.swapXY(); - - // We use the regression from above with the least error - if (x_vs_y_regression.regression_error < y_vs_x_regression.regression_error) - { - return x_vs_y_regression; - } - else - { - return y_vs_x_regression; - } -} - -BallFilter::LinearRegressionResults BallFilter::calculateLinearRegression( - boost::circular_buffer ball_detections) -{ - if (ball_detections.size() < 2) - { - throw std::invalid_argument("At least 2 elements required for linear regression"); - } - - // Sort the detections in increasing order before processing. This places the oldest - // detections (smallest timestamp) at the front of the buffer, and the most recent - // detections (with the largest timestamp) at the end of the buffer - std::sort(ball_detections.begin(), ball_detections.end()); - - // Construct matrix A and vector b for linear regression. The first column of A - // contains the bias variable, and the second column contains the x coordinates of the - // ball. Vector b contains the y coordinates of the ball. - Eigen::MatrixXf A(ball_detections.size(), 2); - Eigen::VectorXf b(ball_detections.size()); - for (unsigned i = 0; i < ball_detections.size(); i++) - { - // This extra column of 1's is the bias variable, so that we can regress with a - // y-intercept - A(i, 0) = 1.0; - A(i, 1) = static_cast(ball_detections.at(i).position.x()); - - b(i) = static_cast(ball_detections.at(i).position.y()); - } - - // Perform linear regression to find the line of best fit through the ball positions. - // This is solving the formula Ax = b, where x is the vector we want to solve for. - Eigen::Vector2f regression_vector = - A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b); - // How to calculate the error is from - // https://eigen.tuxfamily.org/dox/group__TutorialLinearAlgebra.html - // NOTE: using absolute error instead of relative because coordinates - // values should not affect error, also handles divide by 0 error - double regression_error = (A * regression_vector - b).norm(); // norm() is L2 norm - - // Find 2 points on the regression line that we solved for, and use this to construct - // our own Line class - Eigen::Vector2f p1_vec(1, 0); - Point p1(0, p1_vec.dot(regression_vector)); - Eigen::Vector2f p2_vec(1, 1); - Point p2(1, p2_vec.dot(regression_vector)); - Line regression_line = Line(p1, p2); - - LinearRegressionResults results({regression_line, regression_error}); - - return results; -} - -Point BallFilter::estimateBallPosition( - boost::circular_buffer ball_detections, const Line ®ression_line) -{ - if (ball_detections.empty()) - { - throw std::invalid_argument( - "Non-empty buffer required to estimate ball position"); - } - - // Take the position of the most recent ball position and project it onto the line of - // best fit. We do this because we assume the ball must be travelling along its - // velocity vector (the line), and this allows us to return more stable position - // values since the line of best fit is less likely to fluctuate compared to the raw - // position of a ball detection - BallDetection latest_ball_detection = ball_detections.front(); - return closestPoint(latest_ball_detection.position, regression_line); -} - -std::optional BallFilter::estimateBallVelocity( - boost::circular_buffer ball_detections, - const std::optional &ball_regression_line) -{ - // Sort the detections in increasing order before processing. This places the oldest - // detections (smallest timestamp) at the front of the buffer, and the most recent - // detections (with the largest timestamp) at the end of the buffer - std::sort(ball_detections.begin(), ball_detections.end()); - - std::vector ball_velocities; - std::vector ball_velocity_magnitudes; - for (unsigned i = 1; i < ball_detections.size(); i++) - { - for (unsigned j = i; j < ball_detections.size(); j++) - { - BallDetection previous_detection = ball_detections.at(i - 1); - BallDetection current_detection = ball_detections.at(j); - - Duration time_diff = - current_detection.timestamp - previous_detection.timestamp; - // Avoid division by 0. If we have adjacent detections with the same timestamp - // the velocity cannot be calculated - if (time_diff.toSeconds() == 0) - { - continue; - } - - // Project the detection positions onto the regression line if it was provided - Point current_position; - Point previous_position; - if (ball_regression_line) - { - current_position = closestPoint(current_detection.position, - ball_regression_line.value()); - previous_position = closestPoint(previous_detection.position, - ball_regression_line.value()); - } - else - { - current_position = current_detection.position; - previous_position = previous_detection.position; - } - Vector velocity_vector = current_position - previous_position; - double velocity_magnitude = velocity_vector.length() / time_diff.toSeconds(); - Vector velocity = velocity_vector.normalize(velocity_magnitude); - - ball_velocity_magnitudes.emplace_back(velocity_magnitude); - ball_velocities.emplace_back(velocity); - } - } - - if (ball_velocities.empty() || ball_velocity_magnitudes.empty()) - { - return std::nullopt; - } - - double velocity_magnitude_sum = 0; - for (const auto &velocity_magnitude : ball_velocity_magnitudes) - { - velocity_magnitude_sum += velocity_magnitude; - } - double average_velocity_magnitude = - velocity_magnitude_sum / static_cast(ball_velocity_magnitudes.size()); - double velocity_magnitude_max = *std::max_element(ball_velocity_magnitudes.begin(), - ball_velocity_magnitudes.end()); - double velocity_magnitude_min = *std::min_element(ball_velocity_magnitudes.begin(), - ball_velocity_magnitudes.end()); - double min_max_average = (velocity_magnitude_min + velocity_magnitude_max) / 2.0; - - Vector velocity_vector_sum = Vector(0, 0); - for (const auto &velocity : ball_velocities) - { - velocity_vector_sum += velocity; - } - Vector average_velocity = velocity_vector_sum.normalize(average_velocity_magnitude); - - BallVelocityEstimate velocity_data( - {average_velocity, average_velocity_magnitude, min_max_average}); - - return velocity_data; -} diff --git a/src/software/sensor_fusion/filter/ball_kalman_filter.h b/src/software/sensor_fusion/filter/ball_kalman_filter.h deleted file mode 100644 index 57c83ed291..0000000000 --- a/src/software/sensor_fusion/filter/ball_kalman_filter.h +++ /dev/null @@ -1,196 +0,0 @@ -#pragma once - -#include -#include - -#include "software/geom/line.h" -#include "software/geom/point.h" -#include "software/geom/rectangle.h" -#include "software/sensor_fusion/filter/vision_detection.h" -#include "software/time/timestamp.h" -#include "software/world/ball.h" - -/** - * Given ball data from SSL Vision, filters and returns the position/velocity of the - * "real" ball. - * - * This ball filter stores a buffer of previous SSL Vision detections, and uses linear - * regression to find the path the ball is travelling on and estimate its position - * and velocity. This buffer/regression system was chosen because it results in a - * very stable output, particularly for the ball velocity. The data we receive isn't - * perfect (which is why we have a filter). If we receive a noisy position that is off - * the ball's current trajectory, it will have minimal impact. This means that as - * the ball is travelling, this filter will return a very steady velocity vector. - * This is important because small deviations in velocity orientation can have large - * effects when the AI tries to predict the future position of the ball. For example, - * consistently receiving a pass relies on the ball's velocity being very stable, - * otherwise the robot would "jiggle" back and forth as the estimated receiver position - * would keep changing. - */ -class BallKalmanFilter -{ - public: - // The min and max sizes of the ball detection buffer. - // As the ball slows down, the buffer size will approach the MAX_BUFFER_SIZE. - // As the ball speeds up, the buffer size will approach the MIN_BUFFER_SIZE. - static constexpr unsigned int MIN_BUFFER_SIZE = 4; - static constexpr unsigned int MAX_BUFFER_SIZE = 10; - // If the estimated ball speed is less than this value, the largest possible buffer - // will be used by the filter - static constexpr double MIN_BUFFER_SIZE_VELOCITY_MAGNITUDE = 0.5; - // If the estimated ball speed is greater than this value, the smallest possible - // buffer will be used by the filter - static constexpr double MAX_BUFFER_SIZE_VELOCITY_MAGNITUDE = 4.0; - // The extra amount beyond the ball's max speed that we treat ball detections as valid - static constexpr double MAX_ACCEPTABLE_BALL_SPEED_BUFFER = 2.0; - // The maximum root mean squared error threshold to considering using the generated - // linear regression. - // TODO (#2752): Investigate different values of error threshold - static constexpr double LINEAR_REGRESSION_ERROR_THRESHOLD = 1000.0; - - /** - * Creates a new Ball Filter - */ - explicit BallFilter(); - - /** - * Update the filter with the new ball detection data, and returns the new - * estimated state of the ball given the new data - * - * @param new_ball_detections A list of new Ball detections - * @param filter_area The area within which the ball filter will work. Any detections - * outside of this area will be ignored. - * - * @return The new ball based on the estimated state of the ball given the new data. - * If a filtered result cannot be calculated, returns std::nullopt - */ - std::optional estimateBallState( - const std::vector& new_ball_detections, - const Rectangle& filter_area); - - private: - /** - * A simple struct we use to pass around velocity estimate data - */ - struct BallVelocityEstimate - { - Vector average_velocity; - double average_velocity_magnitude; - // The average of the max velocity magnitude and min velocity magnitude - double min_max_magnitude_average; - }; - - /** - * A simple struct to pass around linear regression data - */ - struct LinearRegressionResults - { - Line regression_line; - // Regression error is root mean squared error - double regression_error; - }; - - /** - * Adds ball detections to the buffer stored by this filter. This function will ignore - * data if: - * - the data is outside of the filter_area, or - * - the data is too far away from the current known ball position - * (since it is likely to be random noise). - * - * @param new_ball_detections The ball detections to try add to the buffer - * @param filter_area The area within which the ball filter will work. Any detections - * outside of this area will be ignored. - */ - void addNewDetectionsToBuffer(std::vector new_ball_detections, - const Rectangle& filter_area); - - /** - * Uses linear regression to filter the given list of ball detections to find the - * current "real" state of the ball. - * - * @param ball_detections The detections to filter - * - * @return The new ball based on the filtered state. If a filtered result cannot be - * calculated, returns std::nullopt - */ - static std::optional estimateBallStateFromBuffer( - boost::circular_buffer ball_detections); - - /** - * Returns how large the buffer of ball detections should be based on the ball's - * estimated velocity. A slower moving ball will result in a larger buffer size, and a - * faster ball will result in a smaller buffer size. This is because with a slow - * moving ball, we need more data in order to fit a line with reasonable accuracy, - * since the datapoints will be very close to one another. - * - * @param ball_detections The full list of ball detections - * - * @return The size the buffer should be to perform filtering operations. If an error - * occurs that prevents the size from being calculated correctly, returns std::nullopt - */ - static std::optional getAdjustedBufferSize( - boost::circular_buffer ball_detections); - - /** - * Given a buffer of ball detections, returns the line of best fit through - * the detection positions, and calculate the root mean squared error of this - * regression. - * Note: also considers vertical lines. - * - * @throws std::invalid_argument if ball_detections has less than 2 elements - * - * @param ball_detections The ball detections to fit - * - * @return The line of best fit through the given ball detection positions - */ - static LinearRegressionResults calculateLineOfBestFit( - boost::circular_buffer ball_detections); - - /** - * Given a list of ball detections, use linear regression to find a line of best fit - * through the ball positions, and calculate the root mean squared error of this - * regression. - * - * @throws std::invalid_argument if ball_detections has less than 2 elements - * - * @param ball_detections The ball detections to use in the regression - * - * @return A struct containing the regression line and error of the linear regression - */ - static LinearRegressionResults calculateLinearRegression( - boost::circular_buffer ball_detections); - - /** - * Estimates the current position of the ball given a buffer of ball detections - * and the line of best fit through them. - * - * @throws std::invalid_argument if ball_detections has less than 2 elements - * - * @param ball_detections The ball detections - * @param regression_line The line of best fit through the ball positions - * - * @return The estimated position of the ball - */ - static Point estimateBallPosition( - boost::circular_buffer ball_detections, - const Line& regression_line); - - /** - * Estimates the ball's velocity based on the current detections in the given buffer. - * If the ball_regression_line is provided, the detection positions are projected onto - * the line before the velocities are calculated. If no velocity can be estimated, - * std::nullopt is returned. - * - * @param ball_detections The ball detections to use to calculate - * @param ball_regression_line The ball_regression_line to snap detections to before - * calculating velocities. - * - * @return A struct containing various estimates of the ball's velocity based on the - * given detections. If no velocity can be estimated, std::nullopt is returned - */ - static std::optional estimateBallVelocity( - boost::circular_buffer ball_detections, - const std::optional& ball_regression_line = std::nullopt); - - boost::circular_buffer ball_detection_buffer; -}; diff --git a/src/software/sensor_fusion/filter/kalman_filter.cpp b/src/software/sensor_fusion/filter/kalman_filter.cpp new file mode 100644 index 0000000000..8b965cf7ed --- /dev/null +++ b/src/software/sensor_fusion/filter/kalman_filter.cpp @@ -0,0 +1,52 @@ +#pragma once + +#include +#include + +class KalmanFilter{ + + KalmanFilter::KalmanFilter( + const Eigen::Matrix& X, + const Eigen::Matrix& P, + const Eigen::Matrix& Q, + const Eigen::Matrix& R, + const Eigen::Matrix& C + ): + X(X), + P(P), + Q(Q), + R(R), + C(C) + { + + } + +void predict(double delta_t){ + + const Eigen::Matrix A; + + //Initialize motion model with constant velocity model + A << 1, 0, delta_t, 0, + 0, 1, 0, delta_t, + 0, 0, damping_term, 0, + 0, 0, 0, damping_term; + + X = A*X + + P = A*P*A.transpose() + R +} + +void update(Eigen::Matrix measurement){ + +} + +Eigen::Matrix getState(){ + return X; +} + +Eigen::Matrix getCovariance(){ + return P; +} + + + diff --git a/src/software/sensor_fusion/filter/kalman_filter.h b/src/software/sensor_fusion/filter/kalman_filter.h new file mode 100644 index 0000000000..45efa4b282 --- /dev/null +++ b/src/software/sensor_fusion/filter/kalman_filter.h @@ -0,0 +1,39 @@ +#pragma once + +#include +/** + * Implementation of a kalman filter + + +**/ +class KalmanFilter{ + + +public: + +KalmanFilter( + const Eigen::Matrix& X, + const Eigen::Matrix& P, + const Eigen::Matrix& Q, + const Eigen::Matrix& R, + const Eigen::Matrix& C + ); + +void predict(double delta_t); + +void update(Eigen::Matrix measurement); + +Eigen::Matrix getState(); + +Eigen::Matrix getCovariance(); + +private: +Eigen::Matrix X; // State +Eigen::Matrix P; // State Covariance +Eigen::Matrix Q; // Measurement noise +Eigen::Matrix R; // process noise +Eigen::Matrix C; // State to measurement +static constexpr damping_term = 0.9; + +}; + From fc7076a178153435fad4c98f41cfabed6b804a60 Mon Sep 17 00:00:00 2001 From: StarrryNight Date: Sun, 5 Apr 2026 14:15:42 -0700 Subject: [PATCH 12/57] finish implmentation of kf --- .../sensor_fusion/filter/kalman_filter.cpp | 72 +++++++++++-------- .../sensor_fusion/filter/kalman_filter.h | 16 +++-- 2 files changed, 53 insertions(+), 35 deletions(-) diff --git a/src/software/sensor_fusion/filter/kalman_filter.cpp b/src/software/sensor_fusion/filter/kalman_filter.cpp index 8b965cf7ed..8e0ea4ef5a 100644 --- a/src/software/sensor_fusion/filter/kalman_filter.cpp +++ b/src/software/sensor_fusion/filter/kalman_filter.cpp @@ -1,52 +1,64 @@ -#pragma once - -#include #include -class KalmanFilter{ - - KalmanFilter::KalmanFilter( - const Eigen::Matrix& X, - const Eigen::Matrix& P, - const Eigen::Matrix& Q, - const Eigen::Matrix& R, - const Eigen::Matrix& C - ): - X(X), - P(P), - Q(Q), - R(R), - C(C) - { - - } - -void predict(double delta_t){ +KalmanFilter::KalmanFilter( +const Eigen::Matrix& X, +const Eigen::Matrix& P_i, +const Eigen::Matrix& Q, +const Eigen::Matrix& R, +const Eigen::Matrix& C, +double damping_term, + ): +X(X), +P(P_i), +P_i(P_i), +Q(Q), +R(R), +C(C), +damping_term(damping_term) +{ +} + +void KalmanFilter::predict(const double delta_t){ - const Eigen::Matrix A; + Eigen::Matrix A; - //Initialize motion model with constant velocity model + // Using the constant velocity model as motion model A << 1, 0, delta_t, 0, 0, 1, 0, delta_t, 0, 0, damping_term, 0, 0, 0, 0, damping_term; - X = A*X + X = A*X; - P = A*P*A.transpose() + R + P = A*P*A.transpose() + R; } -void update(Eigen::Matrix measurement){ +void KalmanFilter::update(const Eigen::Matrix Z){ + Eigen::Matrix Kg; + Eigen::Matrix S =C*P*C.transpose()+Q; + Kg = P*C.transpose() * S.inverse(); + X = X + Kg*(Z-C*X); + P = (Eigen::Matrix::Identity()-Kg*C)*P; +} +void KalmanFilter::reset(const Eigen::Matrix Z){ + X << Z(0), Z(1), 0, 0; + P = P_i; } -Eigen::Matrix getState(){ +double KalmanFilter::getMahalanobisDistance(const Eigen::Matrix& Z){ + Eigen::Matrix S =C*P*C.transpose()+Q ; + // Calculate the mahalanobis distance for gating + double M = (Z - C*X).transpose() * S.inverse() * (Z-C*X); + return M; +} + +Eigen::Matrix KalmanFilter::getState(){ return X; } -Eigen::Matrix getCovariance(){ +Eigen::Matrix KalmanFilter::getCovariance(){ return P; } - diff --git a/src/software/sensor_fusion/filter/kalman_filter.h b/src/software/sensor_fusion/filter/kalman_filter.h index 45efa4b282..54edc7e3af 100644 --- a/src/software/sensor_fusion/filter/kalman_filter.h +++ b/src/software/sensor_fusion/filter/kalman_filter.h @@ -13,15 +13,20 @@ class KalmanFilter{ KalmanFilter( const Eigen::Matrix& X, - const Eigen::Matrix& P, + const Eigen::Matrix& P_i, const Eigen::Matrix& Q, const Eigen::Matrix& R, - const Eigen::Matrix& C + const Eigen::Matrix& C, + double damping_term ); -void predict(double delta_t); +void predict(const double delta_t); -void update(Eigen::Matrix measurement); +void update(const Eigen::Matrix Z); + +void reset(const Eigen::Matrix Z); + +double getMahalanobisDistance(const Eigen::Matrix& Z) const; Eigen::Matrix getState(); @@ -30,10 +35,11 @@ Eigen::Matrix getCovariance(); private: Eigen::Matrix X; // State Eigen::Matrix P; // State Covariance +Eigen::Matrix P_i; // State Covariance Eigen::Matrix Q; // Measurement noise Eigen::Matrix R; // process noise Eigen::Matrix C; // State to measurement -static constexpr damping_term = 0.9; +double damping_term; }; From 660c18565ae008efbd453552e54da582832ea174 Mon Sep 17 00:00:00 2001 From: StarrryNight Date: Sun, 5 Apr 2026 16:59:41 -0700 Subject: [PATCH 13/57] change ball filter --- .../sensor_fusion/filter/ball_filter.cpp | 428 +++--------------- .../sensor_fusion/filter/ball_filter.h | 128 +----- .../sensor_fusion/filter/ball_filter_old.cpp | 381 ++++++++++++++++ .../sensor_fusion/filter/ball_filter_old.h | 196 ++++++++ src/software/sensor_fusion/sensor_fusion.cpp | 4 +- 5 files changed, 651 insertions(+), 486 deletions(-) create mode 100644 src/software/sensor_fusion/filter/ball_filter_old.cpp create mode 100644 src/software/sensor_fusion/filter/ball_filter_old.h diff --git a/src/software/sensor_fusion/filter/ball_filter.cpp b/src/software/sensor_fusion/filter/ball_filter.cpp index 0967a76ff8..d9294d5d27 100644 --- a/src/software/sensor_fusion/filter/ball_filter.cpp +++ b/src/software/sensor_fusion/filter/ball_filter.cpp @@ -8,374 +8,80 @@ #include "software/geom/algorithms/closest_point.h" #include "software/geom/algorithms/contains.h" #include "software/math/math_functions.h" - - -BallFilter::BallFilter() : ball_detection_buffer(MAX_BUFFER_SIZE) {} - -std::optional BallFilter::estimateBallState( - const std::vector &new_ball_detections, const Rectangle &filter_area) -{ - addNewDetectionsToBuffer(new_ball_detections, filter_area); - return estimateBallStateFromBuffer(ball_detection_buffer); -} - -void BallFilter::addNewDetectionsToBuffer(std::vector new_ball_detections, - const Rectangle &filter_area) -{ - // Sort the detections in increasing order before processing. This places the oldest - // detections (with the smallest timestamp) at the front of the buffer, and the most - // recent detections (largest timestamp) at the end of the buffer. - std::sort(new_ball_detections.begin(), new_ball_detections.end()); - - for (const auto &detection : new_ball_detections) - { - // Remove any detections outside the filter area - if (!contains(filter_area, detection.position)) - { - continue; - } - - if (!ball_detection_buffer.empty()) - { - // Use the smallest timestamp to minimize time_diffs of 0 - auto detection_with_smallest_timestamp = *std::min_element( - ball_detection_buffer.begin(), ball_detection_buffer.end()); - Duration time_diff = - detection.timestamp - detection_with_smallest_timestamp.timestamp; - - // Ignore any data from the past, and any data that is as old as the oldest - // data in the buffer since it provides no additional value. This also - // prevents division by 0 when calculating the estimated velocity - if (time_diff.toSeconds() <= 0) - { - continue; - } - - // We determine if the detection is noise based on how far it is from a ball - // detection in the buffer. From this, we can calculate how fast the ball - // must have moved to reach the new detection position. If this estimated - // velocity is too far above the maximum allowed velocity, then there is a - // good chance the detection is just noise and not the real ball. In this - // case, we ignore the new "noise" data - double detection_distance = - (detection.position - detection_with_smallest_timestamp.position) - .length(); - double estimated_detection_velocity_magnitude = - detection_distance / time_diff.toSeconds(); - - // Make the maximum acceptable velocity a bit larger than the strict limits - // according to the game rules to account for measurement error, and to be a - // bit on the safe side. We don't want to risk discarding real data. - double maximum_acceptable_velocity_magnitude = - BALL_MAX_SPEED_METERS_PER_SECOND + MAX_ACCEPTABLE_BALL_SPEED_BUFFER; - if (estimated_detection_velocity_magnitude > - maximum_acceptable_velocity_magnitude) - { - // If we determine the data to be noise, remove an entry from the buffer. - // This way if we have messed up and now the ball is too far away for the - // buffer to track, the buffer will rapidly shrink and start tracking the - // ball at its new location once the buffer is empty. - // We sort the vector in decreasing order first so that we can always - // ensure any elements that are ejected from the end of the buffer are the - // oldest data - std::sort(ball_detection_buffer.rbegin(), ball_detection_buffer.rend()); - ball_detection_buffer.pop_back(); - } - else - { - // We sort the vector in decreasing order first so that we can always - // ensure any elements that are ejected from the end of the buffer are the - // oldest data - std::sort(ball_detection_buffer.rbegin(), ball_detection_buffer.rend()); - ball_detection_buffer.push_front(detection); - } - } - else - { - // If there is no data in the buffer, we always add the new data - ball_detection_buffer.push_front(detection); - } - } -} - -std::optional BallFilter::estimateBallStateFromBuffer( - boost::circular_buffer ball_detections) -{ - // Sort the detections in decreasing order before processing. This places the most - // recent detections (with the largest timestamp) at the front of the buffer, and the - // oldest detections (smallest timestamp) at the end of the buffer - std::sort(ball_detections.rbegin(), ball_detections.rend()); - - if (ball_detections.empty()) - { - return std::nullopt; - } - else if (ball_detections.size() == 1) - { - // If there is only 1 entry in the buffer, we can't fit a regression line - // or calculate a velocity so we do our best with just the position - BallState ball_state(ball_detections.front().position, Vector(0, 0), - ball_detections.front().distance_from_ground); - Ball ball(ball_state, ball_detections.front().timestamp); - return ball; - } - - std::optional adjusted_buffer_size = getAdjustedBufferSize(ball_detections); - if (!adjusted_buffer_size) - { - return std::nullopt; - } - ball_detections.resize(*adjusted_buffer_size); - - auto regression = calculateLineOfBestFit(ball_detections); - - Point filtered_position = - estimateBallPosition(ball_detections, regression.regression_line); - - auto estimated_velocity = estimateBallVelocity(ball_detections, std::nullopt); - - if (regression.regression_error < LINEAR_REGRESSION_ERROR_THRESHOLD) - { - estimated_velocity = - estimateBallVelocity(ball_detections, regression.regression_line); - } - if (!estimated_velocity) - { - return std::nullopt; - } - - BallState ball_state(filtered_position, estimated_velocity->average_velocity, - ball_detections.front().distance_from_ground); - return Ball(ball_state, ball_detections.front().timestamp); +#include "software/sensor_fusion/filter/kalman_filter.h" + +namespace { + const Eigen::Matrix INITIAL_STATE = Eigen::Matrix::Zero(); + + const Eigen::Matrix INITIAL_COV = Eigen::Matrix::Identity() * 1000.0; + + const Eigen::Matrix Q = (Eigen::Matrix() << + 0.1, 0, + 0, 0.1).finished(); + + const Eigen::Matrix R = (Eigen::Matrix() << + 0.1, 0, 0, 0, + 0, 0.1, 0, 0, + 0, 0, 0.01, 0, + 0, 0, 0, 0.01).finished(); + + const Eigen::Matrix C = (Eigen::Matrix() << + 1, 0, 0, 0, + 0, 1, 0, 0).finished(); } -std::optional BallFilter::getAdjustedBufferSize( - boost::circular_buffer ball_detections) +BallFilter::BallFilter() : + mahalanobis_count(0), + kalman_filter(INITIAL_STATE, INITIAL_COV, Q, R, C,0.9) { - // Sort the detections in decreasing order before processing. This places the most - // recent detections (with the largest timestamp) at the front of the buffer, and the - // oldest detections (smallest timestamp) at the end of the buffer - std::sort(ball_detections.rbegin(), ball_detections.rend()); - - double buffer_size_velocity_magnitude_diff = - MAX_BUFFER_SIZE_VELOCITY_MAGNITUDE - MIN_BUFFER_SIZE_VELOCITY_MAGNITUDE; - - unsigned int max_buffer_size = - std::min(MAX_BUFFER_SIZE, static_cast(ball_detections.size())); - unsigned int min_buffer_size = - std::min(MIN_BUFFER_SIZE, static_cast(ball_detections.size())); - double buffer_size_diff = max_buffer_size - min_buffer_size; - - std::optional velocity_estimate = - estimateBallVelocity(ball_detections); - if (!velocity_estimate) - { - return std::nullopt; - } - // Use the average of the min and max velocity magnitudes in the buffer. We use this - // rather than the average so we can quickly respond to drastic changes in the ball - // velocity, such as when the ball goes from being stationary to moving quickly (like - // when it's kicked). If the buffer is large, then it will take more time for the mean - // speed to increase enough to start shrinking the buffer. However, the average of the - // min and max values will immediately increase if the ball starts moving, so the - // buffer can start shrinking more quickly and increase the filter response time to - // these sorts of changes. - double min_max_magnitude_average = velocity_estimate->min_max_magnitude_average; - - // Between the min and max velocity magnitudes, we linearly scale the size of the - // buffer - double linear_offset = - MIN_BUFFER_SIZE_VELOCITY_MAGNITUDE + (buffer_size_velocity_magnitude_diff / 2); - double linear_scaling_factor = linear(min_max_magnitude_average, linear_offset, - buffer_size_velocity_magnitude_diff); - int buffer_size = - max_buffer_size - - static_cast(std::floor(linear_scaling_factor * buffer_size_diff)); - - return static_cast(buffer_size); } -BallFilter::LinearRegressionResults BallFilter::calculateLineOfBestFit( - boost::circular_buffer ball_detections) +std::optional BallFilter::estimateBallState( + const std::vector &new_ball_detections, const Rectangle &filter_area, const Timestamp& current_time) { - if (ball_detections.size() < 2) - { - throw std::invalid_argument("At least 2 elements required for linear regression"); - } - - auto x_vs_y_regression = calculateLinearRegression(ball_detections); - - // Linear regression cannot fit a vertical line. To get around this, we fit two lines, - // one with x and y swapped, so any vertical line becomes horizontal. Then we take the - // line of the two that fit the best. - boost::circular_buffer swapped_ball_detections = ball_detections; - for (auto &detection : swapped_ball_detections) - { - detection.position = Point(detection.position.y(), detection.position.x()); - } - auto y_vs_x_regression = calculateLinearRegression(swapped_ball_detections); - // Because we swapped the coordinates of the input, we have to swap the coordinates of - // the output to get back to our expected coordinate space - y_vs_x_regression.regression_line.swapXY(); - - // We use the regression from above with the least error - if (x_vs_y_regression.regression_error < y_vs_x_regression.regression_error) - { - return x_vs_y_regression; - } - else - { - return y_vs_x_regression; - } + BallDetection best_ball_detection = getBestBallDetection(new_ball_detections); + + if (prev_detection_timestamp){ + double delta_t = (current_time - prev_detection_timestamp).toSeconds(); + kalman_filter.predict(delta_t); + } + if (best_ball_detection){ + prev_detection_timestamp = best_ball_detection.timestamp; + Eigen::Matrix measurement; + measurement << best_ball_detection->position.x(), best_ball_detection->position.y(); + double mahalanobis = kalman_filter.getMahalanobisDistance(measurement); + if (mahalanobis< mahalanobis_threshold){ + kalman_filter.update(measurement); + } + else{ + consecutive_outliers++; + } + + if (consecutive_outliers> mahalanobis_count_threshold){ + kalman_filter.reset(measurement); + consecutive_outliers=0; + } + } + + + Eigen::Matrix kalman_state = kalman_filter.getState(); + Point ball_position = Point(kalman_state(0), kalman_state(1)); + Vector ball_velocity = Vector(kalman_state(2), kalman_state(3)); + double z_height = best_ball_detection->distance_from_ground if best_ball_detection else 0.0; + + BallState ball_state(ball_position, ball_velocity, z_height); + return Ball(ball_state,current_time); + } - -BallFilter::LinearRegressionResults BallFilter::calculateLinearRegression( - boost::circular_buffer ball_detections) -{ - if (ball_detections.size() < 2) - { - throw std::invalid_argument("At least 2 elements required for linear regression"); - } - - // Sort the detections in increasing order before processing. This places the oldest - // detections (smallest timestamp) at the front of the buffer, and the most recent - // detections (with the largest timestamp) at the end of the buffer - std::sort(ball_detections.begin(), ball_detections.end()); - - // Construct matrix A and vector b for linear regression. The first column of A - // contains the bias variable, and the second column contains the x coordinates of the - // ball. Vector b contains the y coordinates of the ball. - Eigen::MatrixXf A(ball_detections.size(), 2); - Eigen::VectorXf b(ball_detections.size()); - for (unsigned i = 0; i < ball_detections.size(); i++) - { - // This extra column of 1's is the bias variable, so that we can regress with a - // y-intercept - A(i, 0) = 1.0; - A(i, 1) = static_cast(ball_detections.at(i).position.x()); - - b(i) = static_cast(ball_detections.at(i).position.y()); - } - - // Perform linear regression to find the line of best fit through the ball positions. - // This is solving the formula Ax = b, where x is the vector we want to solve for. - Eigen::Vector2f regression_vector = - A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b); - // How to calculate the error is from - // https://eigen.tuxfamily.org/dox/group__TutorialLinearAlgebra.html - // NOTE: using absolute error instead of relative because coordinates - // values should not affect error, also handles divide by 0 error - double regression_error = (A * regression_vector - b).norm(); // norm() is L2 norm - - // Find 2 points on the regression line that we solved for, and use this to construct - // our own Line class - Eigen::Vector2f p1_vec(1, 0); - Point p1(0, p1_vec.dot(regression_vector)); - Eigen::Vector2f p2_vec(1, 1); - Point p2(1, p2_vec.dot(regression_vector)); - Line regression_line = Line(p1, p2); - - LinearRegressionResults results({regression_line, regression_error}); - - return results; +std::optional BallFilter::getBestBallDetection(const std::vector &new_ball_detections){ + if (new_ball_detections.empty()){ + return std::nullopt; + } + else{ + return *std::max_element(new_ball_detections.begin(), new_ball_detections.end(),[](const BallDetection& a, const BallDetection& b){ + return a.confidence ball_detections, const Line ®ression_line) -{ - if (ball_detections.empty()) - { - throw std::invalid_argument( - "Non-empty buffer required to estimate ball position"); - } - // Take the position of the most recent ball position and project it onto the line of - // best fit. We do this because we assume the ball must be travelling along its - // velocity vector (the line), and this allows us to return more stable position - // values since the line of best fit is less likely to fluctuate compared to the raw - // position of a ball detection - BallDetection latest_ball_detection = ball_detections.front(); - return closestPoint(latest_ball_detection.position, regression_line); -} - -std::optional BallFilter::estimateBallVelocity( - boost::circular_buffer ball_detections, - const std::optional &ball_regression_line) -{ - // Sort the detections in increasing order before processing. This places the oldest - // detections (smallest timestamp) at the front of the buffer, and the most recent - // detections (with the largest timestamp) at the end of the buffer - std::sort(ball_detections.begin(), ball_detections.end()); - - std::vector ball_velocities; - std::vector ball_velocity_magnitudes; - for (unsigned i = 1; i < ball_detections.size(); i++) - { - for (unsigned j = i; j < ball_detections.size(); j++) - { - BallDetection previous_detection = ball_detections.at(i - 1); - BallDetection current_detection = ball_detections.at(j); - - Duration time_diff = - current_detection.timestamp - previous_detection.timestamp; - // Avoid division by 0. If we have adjacent detections with the same timestamp - // the velocity cannot be calculated - if (time_diff.toSeconds() == 0) - { - continue; - } - - // Project the detection positions onto the regression line if it was provided - Point current_position; - Point previous_position; - if (ball_regression_line) - { - current_position = closestPoint(current_detection.position, - ball_regression_line.value()); - previous_position = closestPoint(previous_detection.position, - ball_regression_line.value()); - } - else - { - current_position = current_detection.position; - previous_position = previous_detection.position; - } - Vector velocity_vector = current_position - previous_position; - double velocity_magnitude = velocity_vector.length() / time_diff.toSeconds(); - Vector velocity = velocity_vector.normalize(velocity_magnitude); - - ball_velocity_magnitudes.emplace_back(velocity_magnitude); - ball_velocities.emplace_back(velocity); - } - } - - if (ball_velocities.empty() || ball_velocity_magnitudes.empty()) - { - return std::nullopt; - } - - double velocity_magnitude_sum = 0; - for (const auto &velocity_magnitude : ball_velocity_magnitudes) - { - velocity_magnitude_sum += velocity_magnitude; - } - double average_velocity_magnitude = - velocity_magnitude_sum / static_cast(ball_velocity_magnitudes.size()); - double velocity_magnitude_max = *std::max_element(ball_velocity_magnitudes.begin(), - ball_velocity_magnitudes.end()); - double velocity_magnitude_min = *std::min_element(ball_velocity_magnitudes.begin(), - ball_velocity_magnitudes.end()); - double min_max_average = (velocity_magnitude_min + velocity_magnitude_max) / 2.0; - - Vector velocity_vector_sum = Vector(0, 0); - for (const auto &velocity : ball_velocities) - { - velocity_vector_sum += velocity; - } - Vector average_velocity = velocity_vector_sum.normalize(average_velocity_magnitude); - - BallVelocityEstimate velocity_data( - {average_velocity, average_velocity_magnitude, min_max_average}); - - return velocity_data; -} diff --git a/src/software/sensor_fusion/filter/ball_filter.h b/src/software/sensor_fusion/filter/ball_filter.h index cf108b2ac6..aa7789ddcf 100644 --- a/src/software/sensor_fusion/filter/ball_filter.h +++ b/src/software/sensor_fusion/filter/ball_filter.h @@ -9,6 +9,7 @@ #include "software/sensor_fusion/filter/vision_detection.h" #include "software/time/timestamp.h" #include "software/world/ball.h" +#include "software/sensor_fusion/filter/kalman_filer.h" /** * Given ball data from SSL Vision, filters and returns the position/velocity of the @@ -69,128 +70,9 @@ class BallFilter const Rectangle& filter_area); private: - /** - * A simple struct we use to pass around velocity estimate data - */ - struct BallVelocityEstimate - { - Vector average_velocity; - double average_velocity_magnitude; - // The average of the max velocity magnitude and min velocity magnitude - double min_max_magnitude_average; - }; - - /** - * A simple struct to pass around linear regression data - */ - struct LinearRegressionResults - { - Line regression_line; - // Regression error is root mean squared error - double regression_error; - }; - - /** - * Adds ball detections to the buffer stored by this filter. This function will ignore - * data if: - * - the data is outside of the filter_area, or - * - the data is too far away from the current known ball position - * (since it is likely to be random noise). - * - * @param new_ball_detections The ball detections to try add to the buffer - * @param filter_area The area within which the ball filter will work. Any detections - * outside of this area will be ignored. - */ - void addNewDetectionsToBuffer(std::vector new_ball_detections, - const Rectangle& filter_area); - - /** - * Uses linear regression to filter the given list of ball detections to find the - * current "real" state of the ball. - * - * @param ball_detections The detections to filter - * - * @return The new ball based on the filtered state. If a filtered result cannot be - * calculated, returns std::nullopt - */ - static std::optional estimateBallStateFromBuffer( - boost::circular_buffer ball_detections); - - /** - * Returns how large the buffer of ball detections should be based on the ball's - * estimated velocity. A slower moving ball will result in a larger buffer size, and a - * faster ball will result in a smaller buffer size. This is because with a slow - * moving ball, we need more data in order to fit a line with reasonable accuracy, - * since the datapoints will be very close to one another. - * - * @param ball_detections The full list of ball detections - * - * @return The size the buffer should be to perform filtering operations. If an error - * occurs that prevents the size from being calculated correctly, returns std::nullopt - */ - static std::optional getAdjustedBufferSize( - boost::circular_buffer ball_detections); - - /** - * Given a buffer of ball detections, returns the line of best fit through - * the detection positions, and calculate the root mean squared error of this - * regression. - * Note: also considers vertical lines. - * - * @throws std::invalid_argument if ball_detections has less than 2 elements - * - * @param ball_detections The ball detections to fit - * - * @return The line of best fit through the given ball detection positions - */ - static LinearRegressionResults calculateLineOfBestFit( - boost::circular_buffer ball_detections); - - /** - * Given a list of ball detections, use linear regression to find a line of best fit - * through the ball positions, and calculate the root mean squared error of this - * regression. - * - * @throws std::invalid_argument if ball_detections has less than 2 elements - * - * @param ball_detections The ball detections to use in the regression - * - * @return A struct containing the regression line and error of the linear regression - */ - static LinearRegressionResults calculateLinearRegression( - boost::circular_buffer ball_detections); - - /** - * Estimates the current position of the ball given a buffer of ball detections - * and the line of best fit through them. - * - * @throws std::invalid_argument if ball_detections has less than 2 elements - * - * @param ball_detections The ball detections - * @param regression_line The line of best fit through the ball positions - * - * @return The estimated position of the ball - */ - static Point estimateBallPosition( - boost::circular_buffer ball_detections, - const Line& regression_line); - - /** - * Estimates the ball's velocity based on the current detections in the given buffer. - * If the ball_regression_line is provided, the detection positions are projected onto - * the line before the velocities are calculated. If no velocity can be estimated, - * std::nullopt is returned. - * - * @param ball_detections The ball detections to use to calculate - * @param ball_regression_line The ball_regression_line to snap detections to before - * calculating velocities. - * - * @return A struct containing various estimates of the ball's velocity based on the - * given detections. If no velocity can be estimated, std::nullopt is returned - */ - static std::optional estimateBallVelocity( - boost::circular_buffer ball_detections, - const std::optional& ball_regression_line = std::nullopt); - + BallDetection BallFilter::getBestBallDetection(const std::vector &new_ball_detections); boost::circular_buffer ball_detection_buffer; + int mahalanobis_count; + KalmanFilter kalman_filter; + std::optional prev_detection_timestamp; }; diff --git a/src/software/sensor_fusion/filter/ball_filter_old.cpp b/src/software/sensor_fusion/filter/ball_filter_old.cpp new file mode 100644 index 0000000000..0967a76ff8 --- /dev/null +++ b/src/software/sensor_fusion/filter/ball_filter_old.cpp @@ -0,0 +1,381 @@ +#include "software/sensor_fusion/filter/ball_filter.h" + +#include +#include +#include + +#include "shared/constants.h" +#include "software/geom/algorithms/closest_point.h" +#include "software/geom/algorithms/contains.h" +#include "software/math/math_functions.h" + + +BallFilter::BallFilter() : ball_detection_buffer(MAX_BUFFER_SIZE) {} + +std::optional BallFilter::estimateBallState( + const std::vector &new_ball_detections, const Rectangle &filter_area) +{ + addNewDetectionsToBuffer(new_ball_detections, filter_area); + return estimateBallStateFromBuffer(ball_detection_buffer); +} + +void BallFilter::addNewDetectionsToBuffer(std::vector new_ball_detections, + const Rectangle &filter_area) +{ + // Sort the detections in increasing order before processing. This places the oldest + // detections (with the smallest timestamp) at the front of the buffer, and the most + // recent detections (largest timestamp) at the end of the buffer. + std::sort(new_ball_detections.begin(), new_ball_detections.end()); + + for (const auto &detection : new_ball_detections) + { + // Remove any detections outside the filter area + if (!contains(filter_area, detection.position)) + { + continue; + } + + if (!ball_detection_buffer.empty()) + { + // Use the smallest timestamp to minimize time_diffs of 0 + auto detection_with_smallest_timestamp = *std::min_element( + ball_detection_buffer.begin(), ball_detection_buffer.end()); + Duration time_diff = + detection.timestamp - detection_with_smallest_timestamp.timestamp; + + // Ignore any data from the past, and any data that is as old as the oldest + // data in the buffer since it provides no additional value. This also + // prevents division by 0 when calculating the estimated velocity + if (time_diff.toSeconds() <= 0) + { + continue; + } + + // We determine if the detection is noise based on how far it is from a ball + // detection in the buffer. From this, we can calculate how fast the ball + // must have moved to reach the new detection position. If this estimated + // velocity is too far above the maximum allowed velocity, then there is a + // good chance the detection is just noise and not the real ball. In this + // case, we ignore the new "noise" data + double detection_distance = + (detection.position - detection_with_smallest_timestamp.position) + .length(); + double estimated_detection_velocity_magnitude = + detection_distance / time_diff.toSeconds(); + + // Make the maximum acceptable velocity a bit larger than the strict limits + // according to the game rules to account for measurement error, and to be a + // bit on the safe side. We don't want to risk discarding real data. + double maximum_acceptable_velocity_magnitude = + BALL_MAX_SPEED_METERS_PER_SECOND + MAX_ACCEPTABLE_BALL_SPEED_BUFFER; + if (estimated_detection_velocity_magnitude > + maximum_acceptable_velocity_magnitude) + { + // If we determine the data to be noise, remove an entry from the buffer. + // This way if we have messed up and now the ball is too far away for the + // buffer to track, the buffer will rapidly shrink and start tracking the + // ball at its new location once the buffer is empty. + // We sort the vector in decreasing order first so that we can always + // ensure any elements that are ejected from the end of the buffer are the + // oldest data + std::sort(ball_detection_buffer.rbegin(), ball_detection_buffer.rend()); + ball_detection_buffer.pop_back(); + } + else + { + // We sort the vector in decreasing order first so that we can always + // ensure any elements that are ejected from the end of the buffer are the + // oldest data + std::sort(ball_detection_buffer.rbegin(), ball_detection_buffer.rend()); + ball_detection_buffer.push_front(detection); + } + } + else + { + // If there is no data in the buffer, we always add the new data + ball_detection_buffer.push_front(detection); + } + } +} + +std::optional BallFilter::estimateBallStateFromBuffer( + boost::circular_buffer ball_detections) +{ + // Sort the detections in decreasing order before processing. This places the most + // recent detections (with the largest timestamp) at the front of the buffer, and the + // oldest detections (smallest timestamp) at the end of the buffer + std::sort(ball_detections.rbegin(), ball_detections.rend()); + + if (ball_detections.empty()) + { + return std::nullopt; + } + else if (ball_detections.size() == 1) + { + // If there is only 1 entry in the buffer, we can't fit a regression line + // or calculate a velocity so we do our best with just the position + BallState ball_state(ball_detections.front().position, Vector(0, 0), + ball_detections.front().distance_from_ground); + Ball ball(ball_state, ball_detections.front().timestamp); + return ball; + } + + std::optional adjusted_buffer_size = getAdjustedBufferSize(ball_detections); + if (!adjusted_buffer_size) + { + return std::nullopt; + } + ball_detections.resize(*adjusted_buffer_size); + + auto regression = calculateLineOfBestFit(ball_detections); + + Point filtered_position = + estimateBallPosition(ball_detections, regression.regression_line); + + auto estimated_velocity = estimateBallVelocity(ball_detections, std::nullopt); + + if (regression.regression_error < LINEAR_REGRESSION_ERROR_THRESHOLD) + { + estimated_velocity = + estimateBallVelocity(ball_detections, regression.regression_line); + } + if (!estimated_velocity) + { + return std::nullopt; + } + + BallState ball_state(filtered_position, estimated_velocity->average_velocity, + ball_detections.front().distance_from_ground); + return Ball(ball_state, ball_detections.front().timestamp); +} + +std::optional BallFilter::getAdjustedBufferSize( + boost::circular_buffer ball_detections) +{ + // Sort the detections in decreasing order before processing. This places the most + // recent detections (with the largest timestamp) at the front of the buffer, and the + // oldest detections (smallest timestamp) at the end of the buffer + std::sort(ball_detections.rbegin(), ball_detections.rend()); + + double buffer_size_velocity_magnitude_diff = + MAX_BUFFER_SIZE_VELOCITY_MAGNITUDE - MIN_BUFFER_SIZE_VELOCITY_MAGNITUDE; + + unsigned int max_buffer_size = + std::min(MAX_BUFFER_SIZE, static_cast(ball_detections.size())); + unsigned int min_buffer_size = + std::min(MIN_BUFFER_SIZE, static_cast(ball_detections.size())); + double buffer_size_diff = max_buffer_size - min_buffer_size; + + std::optional velocity_estimate = + estimateBallVelocity(ball_detections); + if (!velocity_estimate) + { + return std::nullopt; + } + // Use the average of the min and max velocity magnitudes in the buffer. We use this + // rather than the average so we can quickly respond to drastic changes in the ball + // velocity, such as when the ball goes from being stationary to moving quickly (like + // when it's kicked). If the buffer is large, then it will take more time for the mean + // speed to increase enough to start shrinking the buffer. However, the average of the + // min and max values will immediately increase if the ball starts moving, so the + // buffer can start shrinking more quickly and increase the filter response time to + // these sorts of changes. + double min_max_magnitude_average = velocity_estimate->min_max_magnitude_average; + + // Between the min and max velocity magnitudes, we linearly scale the size of the + // buffer + double linear_offset = + MIN_BUFFER_SIZE_VELOCITY_MAGNITUDE + (buffer_size_velocity_magnitude_diff / 2); + double linear_scaling_factor = linear(min_max_magnitude_average, linear_offset, + buffer_size_velocity_magnitude_diff); + int buffer_size = + max_buffer_size - + static_cast(std::floor(linear_scaling_factor * buffer_size_diff)); + + return static_cast(buffer_size); +} + +BallFilter::LinearRegressionResults BallFilter::calculateLineOfBestFit( + boost::circular_buffer ball_detections) +{ + if (ball_detections.size() < 2) + { + throw std::invalid_argument("At least 2 elements required for linear regression"); + } + + auto x_vs_y_regression = calculateLinearRegression(ball_detections); + + // Linear regression cannot fit a vertical line. To get around this, we fit two lines, + // one with x and y swapped, so any vertical line becomes horizontal. Then we take the + // line of the two that fit the best. + boost::circular_buffer swapped_ball_detections = ball_detections; + for (auto &detection : swapped_ball_detections) + { + detection.position = Point(detection.position.y(), detection.position.x()); + } + auto y_vs_x_regression = calculateLinearRegression(swapped_ball_detections); + // Because we swapped the coordinates of the input, we have to swap the coordinates of + // the output to get back to our expected coordinate space + y_vs_x_regression.regression_line.swapXY(); + + // We use the regression from above with the least error + if (x_vs_y_regression.regression_error < y_vs_x_regression.regression_error) + { + return x_vs_y_regression; + } + else + { + return y_vs_x_regression; + } +} + +BallFilter::LinearRegressionResults BallFilter::calculateLinearRegression( + boost::circular_buffer ball_detections) +{ + if (ball_detections.size() < 2) + { + throw std::invalid_argument("At least 2 elements required for linear regression"); + } + + // Sort the detections in increasing order before processing. This places the oldest + // detections (smallest timestamp) at the front of the buffer, and the most recent + // detections (with the largest timestamp) at the end of the buffer + std::sort(ball_detections.begin(), ball_detections.end()); + + // Construct matrix A and vector b for linear regression. The first column of A + // contains the bias variable, and the second column contains the x coordinates of the + // ball. Vector b contains the y coordinates of the ball. + Eigen::MatrixXf A(ball_detections.size(), 2); + Eigen::VectorXf b(ball_detections.size()); + for (unsigned i = 0; i < ball_detections.size(); i++) + { + // This extra column of 1's is the bias variable, so that we can regress with a + // y-intercept + A(i, 0) = 1.0; + A(i, 1) = static_cast(ball_detections.at(i).position.x()); + + b(i) = static_cast(ball_detections.at(i).position.y()); + } + + // Perform linear regression to find the line of best fit through the ball positions. + // This is solving the formula Ax = b, where x is the vector we want to solve for. + Eigen::Vector2f regression_vector = + A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b); + // How to calculate the error is from + // https://eigen.tuxfamily.org/dox/group__TutorialLinearAlgebra.html + // NOTE: using absolute error instead of relative because coordinates + // values should not affect error, also handles divide by 0 error + double regression_error = (A * regression_vector - b).norm(); // norm() is L2 norm + + // Find 2 points on the regression line that we solved for, and use this to construct + // our own Line class + Eigen::Vector2f p1_vec(1, 0); + Point p1(0, p1_vec.dot(regression_vector)); + Eigen::Vector2f p2_vec(1, 1); + Point p2(1, p2_vec.dot(regression_vector)); + Line regression_line = Line(p1, p2); + + LinearRegressionResults results({regression_line, regression_error}); + + return results; +} + +Point BallFilter::estimateBallPosition( + boost::circular_buffer ball_detections, const Line ®ression_line) +{ + if (ball_detections.empty()) + { + throw std::invalid_argument( + "Non-empty buffer required to estimate ball position"); + } + + // Take the position of the most recent ball position and project it onto the line of + // best fit. We do this because we assume the ball must be travelling along its + // velocity vector (the line), and this allows us to return more stable position + // values since the line of best fit is less likely to fluctuate compared to the raw + // position of a ball detection + BallDetection latest_ball_detection = ball_detections.front(); + return closestPoint(latest_ball_detection.position, regression_line); +} + +std::optional BallFilter::estimateBallVelocity( + boost::circular_buffer ball_detections, + const std::optional &ball_regression_line) +{ + // Sort the detections in increasing order before processing. This places the oldest + // detections (smallest timestamp) at the front of the buffer, and the most recent + // detections (with the largest timestamp) at the end of the buffer + std::sort(ball_detections.begin(), ball_detections.end()); + + std::vector ball_velocities; + std::vector ball_velocity_magnitudes; + for (unsigned i = 1; i < ball_detections.size(); i++) + { + for (unsigned j = i; j < ball_detections.size(); j++) + { + BallDetection previous_detection = ball_detections.at(i - 1); + BallDetection current_detection = ball_detections.at(j); + + Duration time_diff = + current_detection.timestamp - previous_detection.timestamp; + // Avoid division by 0. If we have adjacent detections with the same timestamp + // the velocity cannot be calculated + if (time_diff.toSeconds() == 0) + { + continue; + } + + // Project the detection positions onto the regression line if it was provided + Point current_position; + Point previous_position; + if (ball_regression_line) + { + current_position = closestPoint(current_detection.position, + ball_regression_line.value()); + previous_position = closestPoint(previous_detection.position, + ball_regression_line.value()); + } + else + { + current_position = current_detection.position; + previous_position = previous_detection.position; + } + Vector velocity_vector = current_position - previous_position; + double velocity_magnitude = velocity_vector.length() / time_diff.toSeconds(); + Vector velocity = velocity_vector.normalize(velocity_magnitude); + + ball_velocity_magnitudes.emplace_back(velocity_magnitude); + ball_velocities.emplace_back(velocity); + } + } + + if (ball_velocities.empty() || ball_velocity_magnitudes.empty()) + { + return std::nullopt; + } + + double velocity_magnitude_sum = 0; + for (const auto &velocity_magnitude : ball_velocity_magnitudes) + { + velocity_magnitude_sum += velocity_magnitude; + } + double average_velocity_magnitude = + velocity_magnitude_sum / static_cast(ball_velocity_magnitudes.size()); + double velocity_magnitude_max = *std::max_element(ball_velocity_magnitudes.begin(), + ball_velocity_magnitudes.end()); + double velocity_magnitude_min = *std::min_element(ball_velocity_magnitudes.begin(), + ball_velocity_magnitudes.end()); + double min_max_average = (velocity_magnitude_min + velocity_magnitude_max) / 2.0; + + Vector velocity_vector_sum = Vector(0, 0); + for (const auto &velocity : ball_velocities) + { + velocity_vector_sum += velocity; + } + Vector average_velocity = velocity_vector_sum.normalize(average_velocity_magnitude); + + BallVelocityEstimate velocity_data( + {average_velocity, average_velocity_magnitude, min_max_average}); + + return velocity_data; +} diff --git a/src/software/sensor_fusion/filter/ball_filter_old.h b/src/software/sensor_fusion/filter/ball_filter_old.h new file mode 100644 index 0000000000..cf108b2ac6 --- /dev/null +++ b/src/software/sensor_fusion/filter/ball_filter_old.h @@ -0,0 +1,196 @@ +#pragma once + +#include +#include + +#include "software/geom/line.h" +#include "software/geom/point.h" +#include "software/geom/rectangle.h" +#include "software/sensor_fusion/filter/vision_detection.h" +#include "software/time/timestamp.h" +#include "software/world/ball.h" + +/** + * Given ball data from SSL Vision, filters and returns the position/velocity of the + * "real" ball. + * + * This ball filter stores a buffer of previous SSL Vision detections, and uses linear + * regression to find the path the ball is travelling on and estimate its position + * and velocity. This buffer/regression system was chosen because it results in a + * very stable output, particularly for the ball velocity. The data we receive isn't + * perfect (which is why we have a filter). If we receive a noisy position that is off + * the ball's current trajectory, it will have minimal impact. This means that as + * the ball is travelling, this filter will return a very steady velocity vector. + * This is important because small deviations in velocity orientation can have large + * effects when the AI tries to predict the future position of the ball. For example, + * consistently receiving a pass relies on the ball's velocity being very stable, + * otherwise the robot would "jiggle" back and forth as the estimated receiver position + * would keep changing. + */ +class BallFilter +{ + public: + // The min and max sizes of the ball detection buffer. + // As the ball slows down, the buffer size will approach the MAX_BUFFER_SIZE. + // As the ball speeds up, the buffer size will approach the MIN_BUFFER_SIZE. + static constexpr unsigned int MIN_BUFFER_SIZE = 4; + static constexpr unsigned int MAX_BUFFER_SIZE = 10; + // If the estimated ball speed is less than this value, the largest possible buffer + // will be used by the filter + static constexpr double MIN_BUFFER_SIZE_VELOCITY_MAGNITUDE = 0.5; + // If the estimated ball speed is greater than this value, the smallest possible + // buffer will be used by the filter + static constexpr double MAX_BUFFER_SIZE_VELOCITY_MAGNITUDE = 4.0; + // The extra amount beyond the ball's max speed that we treat ball detections as valid + static constexpr double MAX_ACCEPTABLE_BALL_SPEED_BUFFER = 2.0; + // The maximum root mean squared error threshold to considering using the generated + // linear regression. + // TODO (#2752): Investigate different values of error threshold + static constexpr double LINEAR_REGRESSION_ERROR_THRESHOLD = 1000.0; + + /** + * Creates a new Ball Filter + */ + explicit BallFilter(); + + /** + * Update the filter with the new ball detection data, and returns the new + * estimated state of the ball given the new data + * + * @param new_ball_detections A list of new Ball detections + * @param filter_area The area within which the ball filter will work. Any detections + * outside of this area will be ignored. + * + * @return The new ball based on the estimated state of the ball given the new data. + * If a filtered result cannot be calculated, returns std::nullopt + */ + std::optional estimateBallState( + const std::vector& new_ball_detections, + const Rectangle& filter_area); + + private: + /** + * A simple struct we use to pass around velocity estimate data + */ + struct BallVelocityEstimate + { + Vector average_velocity; + double average_velocity_magnitude; + // The average of the max velocity magnitude and min velocity magnitude + double min_max_magnitude_average; + }; + + /** + * A simple struct to pass around linear regression data + */ + struct LinearRegressionResults + { + Line regression_line; + // Regression error is root mean squared error + double regression_error; + }; + + /** + * Adds ball detections to the buffer stored by this filter. This function will ignore + * data if: + * - the data is outside of the filter_area, or + * - the data is too far away from the current known ball position + * (since it is likely to be random noise). + * + * @param new_ball_detections The ball detections to try add to the buffer + * @param filter_area The area within which the ball filter will work. Any detections + * outside of this area will be ignored. + */ + void addNewDetectionsToBuffer(std::vector new_ball_detections, + const Rectangle& filter_area); + + /** + * Uses linear regression to filter the given list of ball detections to find the + * current "real" state of the ball. + * + * @param ball_detections The detections to filter + * + * @return The new ball based on the filtered state. If a filtered result cannot be + * calculated, returns std::nullopt + */ + static std::optional estimateBallStateFromBuffer( + boost::circular_buffer ball_detections); + + /** + * Returns how large the buffer of ball detections should be based on the ball's + * estimated velocity. A slower moving ball will result in a larger buffer size, and a + * faster ball will result in a smaller buffer size. This is because with a slow + * moving ball, we need more data in order to fit a line with reasonable accuracy, + * since the datapoints will be very close to one another. + * + * @param ball_detections The full list of ball detections + * + * @return The size the buffer should be to perform filtering operations. If an error + * occurs that prevents the size from being calculated correctly, returns std::nullopt + */ + static std::optional getAdjustedBufferSize( + boost::circular_buffer ball_detections); + + /** + * Given a buffer of ball detections, returns the line of best fit through + * the detection positions, and calculate the root mean squared error of this + * regression. + * Note: also considers vertical lines. + * + * @throws std::invalid_argument if ball_detections has less than 2 elements + * + * @param ball_detections The ball detections to fit + * + * @return The line of best fit through the given ball detection positions + */ + static LinearRegressionResults calculateLineOfBestFit( + boost::circular_buffer ball_detections); + + /** + * Given a list of ball detections, use linear regression to find a line of best fit + * through the ball positions, and calculate the root mean squared error of this + * regression. + * + * @throws std::invalid_argument if ball_detections has less than 2 elements + * + * @param ball_detections The ball detections to use in the regression + * + * @return A struct containing the regression line and error of the linear regression + */ + static LinearRegressionResults calculateLinearRegression( + boost::circular_buffer ball_detections); + + /** + * Estimates the current position of the ball given a buffer of ball detections + * and the line of best fit through them. + * + * @throws std::invalid_argument if ball_detections has less than 2 elements + * + * @param ball_detections The ball detections + * @param regression_line The line of best fit through the ball positions + * + * @return The estimated position of the ball + */ + static Point estimateBallPosition( + boost::circular_buffer ball_detections, + const Line& regression_line); + + /** + * Estimates the ball's velocity based on the current detections in the given buffer. + * If the ball_regression_line is provided, the detection positions are projected onto + * the line before the velocities are calculated. If no velocity can be estimated, + * std::nullopt is returned. + * + * @param ball_detections The ball detections to use to calculate + * @param ball_regression_line The ball_regression_line to snap detections to before + * calculating velocities. + * + * @return A struct containing various estimates of the ball's velocity based on the + * given detections. If no velocity can be estimated, std::nullopt is returned + */ + static std::optional estimateBallVelocity( + boost::circular_buffer ball_detections, + const std::optional& ball_regression_line = std::nullopt); + + boost::circular_buffer ball_detection_buffer; +}; diff --git a/src/software/sensor_fusion/sensor_fusion.cpp b/src/software/sensor_fusion/sensor_fusion.cpp index 2737022044..f238de0052 100644 --- a/src/software/sensor_fusion/sensor_fusion.cpp +++ b/src/software/sensor_fusion/sensor_fusion.cpp @@ -12,7 +12,7 @@ SensorFusion::SensorFusion(TbotsProto::SensorFusionConfig sensor_fusion_config) game_state(), referee_stage(std::nullopt), dribble_displacement(std::nullopt), - ball_filter(), + ball_filter(0), friendly_team_filter(), enemy_team_filter(), possession(TeamPossession::FRIENDLY_TEAM), @@ -512,7 +512,7 @@ void SensorFusion::resetWorldComponents() enemy_team = Team(); game_state = GameState(); referee_stage = std::nullopt; - ball_filter = BallFilter(); + ball_filter = BallFilter(0); friendly_team_filter = RobotTeamFilter(); enemy_team_filter = RobotTeamFilter(); possession = TeamPossession::FRIENDLY_TEAM; From 1ade146875c66275ee61000e8c8e5960b880e7dd Mon Sep 17 00:00:00 2001 From: StarrryNight Date: Sun, 5 Apr 2026 17:15:36 -0700 Subject: [PATCH 14/57] finish WITHOUT TUNING --- .../sensor_fusion/filter/ball_filter.cpp | 18 +++++-- .../sensor_fusion/filter/ball_filter.h | 52 ++++--------------- 2 files changed, 22 insertions(+), 48 deletions(-) diff --git a/src/software/sensor_fusion/filter/ball_filter.cpp b/src/software/sensor_fusion/filter/ball_filter.cpp index d9294d5d27..5b3027f74f 100644 --- a/src/software/sensor_fusion/filter/ball_filter.cpp +++ b/src/software/sensor_fusion/filter/ball_filter.cpp @@ -39,14 +39,22 @@ BallFilter::BallFilter() : std::optional BallFilter::estimateBallState( const std::vector &new_ball_detections, const Rectangle &filter_area, const Timestamp& current_time) { - BallDetection best_ball_detection = getBestBallDetection(new_ball_detections); + std::optional best_ball_detection = getBestBallDetection(new_ball_detections); if (prev_detection_timestamp){ - double delta_t = (current_time - prev_detection_timestamp).toSeconds(); + double delta_t; + if (best_ball_detection){ + delta_t = (best_ball_detection->timestamp - *prev_detection_timestamp).toSeconds(); + prev_detection_timestamp = best_ball_detection->timestamp; + } + else{ + delta_t = (current_time - *prev_detection_timestamp).toSeconds(); + prev_detection_timestamp = current_time; + } kalman_filter.predict(delta_t); } if (best_ball_detection){ - prev_detection_timestamp = best_ball_detection.timestamp; + prev_detection_timestamp = best_ball_detection->timestamp; Eigen::Matrix measurement; measurement << best_ball_detection->position.x(), best_ball_detection->position.y(); double mahalanobis = kalman_filter.getMahalanobisDistance(measurement); @@ -57,7 +65,7 @@ std::optional BallFilter::estimateBallState( consecutive_outliers++; } - if (consecutive_outliers> mahalanobis_count_threshold){ + if (consecutive_outliers> consecutive_outliers_threshold){ kalman_filter.reset(measurement); consecutive_outliers=0; } @@ -67,7 +75,7 @@ std::optional BallFilter::estimateBallState( Eigen::Matrix kalman_state = kalman_filter.getState(); Point ball_position = Point(kalman_state(0), kalman_state(1)); Vector ball_velocity = Vector(kalman_state(2), kalman_state(3)); - double z_height = best_ball_detection->distance_from_ground if best_ball_detection else 0.0; + double z_height = best_ball_detection ? best_ball_detection->distance_from_ground : 0.0; BallState ball_state(ball_position, ball_velocity, z_height); return Ball(ball_state,current_time); diff --git a/src/software/sensor_fusion/filter/ball_filter.h b/src/software/sensor_fusion/filter/ball_filter.h index aa7789ddcf..3f37a45159 100644 --- a/src/software/sensor_fusion/filter/ball_filter.h +++ b/src/software/sensor_fusion/filter/ball_filter.h @@ -9,50 +9,14 @@ #include "software/sensor_fusion/filter/vision_detection.h" #include "software/time/timestamp.h" #include "software/world/ball.h" -#include "software/sensor_fusion/filter/kalman_filer.h" - -/** - * Given ball data from SSL Vision, filters and returns the position/velocity of the - * "real" ball. - * - * This ball filter stores a buffer of previous SSL Vision detections, and uses linear - * regression to find the path the ball is travelling on and estimate its position - * and velocity. This buffer/regression system was chosen because it results in a - * very stable output, particularly for the ball velocity. The data we receive isn't - * perfect (which is why we have a filter). If we receive a noisy position that is off - * the ball's current trajectory, it will have minimal impact. This means that as - * the ball is travelling, this filter will return a very steady velocity vector. - * This is important because small deviations in velocity orientation can have large - * effects when the AI tries to predict the future position of the ball. For example, - * consistently receiving a pass relies on the ball's velocity being very stable, - * otherwise the robot would "jiggle" back and forth as the estimated receiver position - * would keep changing. - */ +#include "software/sensor_fusion/filter/kalman_filter.h" class BallFilter { - public: - // The min and max sizes of the ball detection buffer. - // As the ball slows down, the buffer size will approach the MAX_BUFFER_SIZE. - // As the ball speeds up, the buffer size will approach the MIN_BUFFER_SIZE. - static constexpr unsigned int MIN_BUFFER_SIZE = 4; - static constexpr unsigned int MAX_BUFFER_SIZE = 10; - // If the estimated ball speed is less than this value, the largest possible buffer - // will be used by the filter - static constexpr double MIN_BUFFER_SIZE_VELOCITY_MAGNITUDE = 0.5; - // If the estimated ball speed is greater than this value, the smallest possible - // buffer will be used by the filter - static constexpr double MAX_BUFFER_SIZE_VELOCITY_MAGNITUDE = 4.0; - // The extra amount beyond the ball's max speed that we treat ball detections as valid - static constexpr double MAX_ACCEPTABLE_BALL_SPEED_BUFFER = 2.0; - // The maximum root mean squared error threshold to considering using the generated - // linear regression. - // TODO (#2752): Investigate different values of error threshold - static constexpr double LINEAR_REGRESSION_ERROR_THRESHOLD = 1000.0; - + public: /** * Creates a new Ball Filter */ - explicit BallFilter(); + BallFilter(); /** * Update the filter with the new ball detection data, and returns the new @@ -67,12 +31,14 @@ class BallFilter */ std::optional estimateBallState( const std::vector& new_ball_detections, - const Rectangle& filter_area); + const Rectangle& filter_area, + const Timestamp& current_time); private: - BallDetection BallFilter::getBestBallDetection(const std::vector &new_ball_detections); - boost::circular_buffer ball_detection_buffer; - int mahalanobis_count; + std::optional getBestBallDetection(const std::vector &new_ball_detections); + int consecutive_outliers; + double mahalanobis_threshold=1; + int consecutive_outliers_threshold=10; KalmanFilter kalman_filter; std::optional prev_detection_timestamp; }; From f5cfcd917d37468fb8e7431678af2a3301e8e65b Mon Sep 17 00:00:00 2001 From: StarrryNight Date: Sun, 5 Apr 2026 17:18:17 -0700 Subject: [PATCH 15/57] fix build issues --- src/software/sensor_fusion/filter/BUILD | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/software/sensor_fusion/filter/BUILD b/src/software/sensor_fusion/filter/BUILD index 9e00ea6b3e..4705a77e6e 100644 --- a/src/software/sensor_fusion/filter/BUILD +++ b/src/software/sensor_fusion/filter/BUILD @@ -19,6 +19,7 @@ cc_library( hdrs = ["ball_filter.h"], deps = [ ":vision_detection", + ":kalman_filter", "//software/geom/algorithms", "//software/math:math_functions", "//software/world:ball", @@ -27,6 +28,14 @@ cc_library( "@eigen", ], ) +cc_library( + name = "kalman_filter", + srcs = ["kalman_filter.cpp"], + hdrs = ["kalman_filter.h"], + deps = [ + "@eigen", + ], +) cc_test( name = "ball_filter_test", From 15f5042e392f1c9a6f0f4d6633158be4c0a001b2 Mon Sep 17 00:00:00 2001 From: StarrryNight Date: Sun, 5 Apr 2026 17:19:30 -0700 Subject: [PATCH 16/57] change import issues --- src/software/sensor_fusion/filter/BUILD | 16 ++++++++-------- .../sensor_fusion/filter/kalman_filter.cpp | 2 +- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/software/sensor_fusion/filter/BUILD b/src/software/sensor_fusion/filter/BUILD index 4705a77e6e..0ea19f8ed6 100644 --- a/src/software/sensor_fusion/filter/BUILD +++ b/src/software/sensor_fusion/filter/BUILD @@ -13,6 +13,14 @@ cc_library( ], ) +cc_library( + name = "kalman_filter", + srcs = ["kalman_filter.cpp"], + hdrs = ["kalman_filter.h"], + deps = [ + "@eigen", + ], +) cc_library( name = "ball_filter", srcs = ["ball_filter.cpp"], @@ -28,14 +36,6 @@ cc_library( "@eigen", ], ) -cc_library( - name = "kalman_filter", - srcs = ["kalman_filter.cpp"], - hdrs = ["kalman_filter.h"], - deps = [ - "@eigen", - ], -) cc_test( name = "ball_filter_test", diff --git a/src/software/sensor_fusion/filter/kalman_filter.cpp b/src/software/sensor_fusion/filter/kalman_filter.cpp index 8e0ea4ef5a..9b15d3a485 100644 --- a/src/software/sensor_fusion/filter/kalman_filter.cpp +++ b/src/software/sensor_fusion/filter/kalman_filter.cpp @@ -1,4 +1,4 @@ -#include +#include KalmanFilter::KalmanFilter( const Eigen::Matrix& X, From ca76a684e7ae861d152764e71ead0f2760a13eb1 Mon Sep 17 00:00:00 2001 From: StarrryNight Date: Sun, 5 Apr 2026 18:14:23 -0700 Subject: [PATCH 17/57] finish integrating kalman filter --> needs tuning --- src/software/er_force_simulator_main.cpp | 4 ++-- src/software/sensor_fusion/filter/ball_filter.cpp | 12 +++--------- .../sensor_fusion/filter/kalman_filter.cpp | 8 ++++---- src/software/sensor_fusion/sensor_fusion.cpp | 14 ++++++++------ src/software/sensor_fusion/sensor_fusion.h | 2 +- 5 files changed, 18 insertions(+), 22 deletions(-) diff --git a/src/software/er_force_simulator_main.cpp b/src/software/er_force_simulator_main.cpp index f3ba1e65b6..5efa61fdde 100644 --- a/src/software/er_force_simulator_main.cpp +++ b/src/software/er_force_simulator_main.cpp @@ -48,7 +48,7 @@ int main(int argc, char **argv) { std::string runtime_dir = args.runtime_dir; LoggerSingleton::initializeLogger(runtime_dir, nullptr); - LOG(CSV, "goalie_tactic_data.csv") << "timestamp_s,fused_x,fused_y,truth_x,truth_y, true_vel_x, true_vel_y,is_occluded\n"; + LOG(CSV, "new_filter_data.csv") << "timestamp_s,fused_x,fused_y,truth_x,truth_y, true_vel_x, true_vel_y,is_occluded\n"; /** * Creates a ER force simulator and sets up the appropriate @@ -222,7 +222,7 @@ int main(int argc, char **argv) } auto sim_state = er_force_sim->getSimulatorState(); - LOG(CSV, "goalie_tactic_data.csv") + LOG(CSV, "new_filter_data.csv") << yellow_vision.time_sent().epoch_timestamp_seconds() << "," << yellow_vision.ball().current_state().global_position().x_meters() << "," diff --git a/src/software/sensor_fusion/filter/ball_filter.cpp b/src/software/sensor_fusion/filter/ball_filter.cpp index 5b3027f74f..5598844184 100644 --- a/src/software/sensor_fusion/filter/ball_filter.cpp +++ b/src/software/sensor_fusion/filter/ball_filter.cpp @@ -31,7 +31,7 @@ namespace { } BallFilter::BallFilter() : - mahalanobis_count(0), + consecutive_outliers(0), kalman_filter(INITIAL_STATE, INITIAL_COV, Q, R, C,0.9) { } @@ -43,14 +43,8 @@ std::optional BallFilter::estimateBallState( if (prev_detection_timestamp){ double delta_t; - if (best_ball_detection){ - delta_t = (best_ball_detection->timestamp - *prev_detection_timestamp).toSeconds(); - prev_detection_timestamp = best_ball_detection->timestamp; - } - else{ - delta_t = (current_time - *prev_detection_timestamp).toSeconds(); - prev_detection_timestamp = current_time; - } + delta_t = (current_time - *prev_detection_timestamp).toSeconds(); + prev_detection_timestamp = current_time; kalman_filter.predict(delta_t); } if (best_ball_detection){ diff --git a/src/software/sensor_fusion/filter/kalman_filter.cpp b/src/software/sensor_fusion/filter/kalman_filter.cpp index 9b15d3a485..e051402eab 100644 --- a/src/software/sensor_fusion/filter/kalman_filter.cpp +++ b/src/software/sensor_fusion/filter/kalman_filter.cpp @@ -1,4 +1,4 @@ -#include +#include "software/sensor_fusion/filter/kalman_filter.h" KalmanFilter::KalmanFilter( const Eigen::Matrix& X, @@ -6,7 +6,7 @@ const Eigen::Matrix& P_i, const Eigen::Matrix& Q, const Eigen::Matrix& R, const Eigen::Matrix& C, -double damping_term, +double damping_term ): X(X), P(P_i), @@ -46,10 +46,10 @@ void KalmanFilter::reset(const Eigen::Matrix Z){ P = P_i; } -double KalmanFilter::getMahalanobisDistance(const Eigen::Matrix& Z){ +double KalmanFilter::getMahalanobisDistance(const Eigen::Matrix& Z) const { Eigen::Matrix S =C*P*C.transpose()+Q ; // Calculate the mahalanobis distance for gating - double M = (Z - C*X).transpose() * S.inverse() * (Z-C*X); + double M = ((Z - C*X).transpose() * S.inverse() * (Z-C*X))(0,0); return M; } diff --git a/src/software/sensor_fusion/sensor_fusion.cpp b/src/software/sensor_fusion/sensor_fusion.cpp index f238de0052..c3c67cfd3a 100644 --- a/src/software/sensor_fusion/sensor_fusion.cpp +++ b/src/software/sensor_fusion/sensor_fusion.cpp @@ -12,7 +12,7 @@ SensorFusion::SensorFusion(TbotsProto::SensorFusionConfig sensor_fusion_config) game_state(), referee_stage(std::nullopt), dribble_displacement(std::nullopt), - ball_filter(0), + ball_filter(), friendly_team_filter(), enemy_team_filter(), possession(TeamPossession::FRIENDLY_TEAM), @@ -298,7 +298,9 @@ void SensorFusion::updateWorld(const SSLProto::SSL_DetectionFrame &ssl_detection .timestamp = Timestamp::fromSeconds(ssl_detection_frame.t_capture()), .confidence = 1}}; - std::optional new_ball = createBall(dribbler_in_ball_detection); + + + std::optional new_ball = createBall(dribbler_in_ball_detection, Timestamp::fromSeconds(ssl_detection_frame.t_capture())); if (new_ball) { @@ -307,7 +309,7 @@ void SensorFusion::updateWorld(const SSLProto::SSL_DetectionFrame &ssl_detection } else { - std::optional new_ball = createBall(ball_detections); + std::optional new_ball = createBall(ball_detections, Timestamp::fromSeconds(ssl_detection_frame.t_capture())); if (new_ball) { // If vision detected a new ball, then use that one @@ -349,12 +351,12 @@ void SensorFusion::updateBall(Ball new_ball) } std::optional SensorFusion::createBall( - const std::vector &ball_detections) + const std::vector &ball_detections, const Timestamp& current_time) { if (field) { std::optional new_ball = - ball_filter.estimateBallState(ball_detections, field.value().fieldBoundary()); + ball_filter.estimateBallState(ball_detections, field.value().fieldBoundary(), current_time); return new_ball; } return std::nullopt; @@ -512,7 +514,7 @@ void SensorFusion::resetWorldComponents() enemy_team = Team(); game_state = GameState(); referee_stage = std::nullopt; - ball_filter = BallFilter(0); + ball_filter = BallFilter(); friendly_team_filter = RobotTeamFilter(); enemy_team_filter = RobotTeamFilter(); possession = TeamPossession::FRIENDLY_TEAM; diff --git a/src/software/sensor_fusion/sensor_fusion.h b/src/software/sensor_fusion/sensor_fusion.h index 29c3eec624..0036818676 100644 --- a/src/software/sensor_fusion/sensor_fusion.h +++ b/src/software/sensor_fusion/sensor_fusion.h @@ -95,7 +95,7 @@ class SensorFusion * * @return Ball if filtered from ball detections */ - std::optional createBall(const std::vector &ball_detections); + std::optional createBall(const std::vector &ball_detections, const Timestamp& current_time); /** * Create team from a list of robot detections From adaa05e5665d2d213c3cb6a47cabc65d04495932 Mon Sep 17 00:00:00 2001 From: StarrryNight Date: Mon, 6 Apr 2026 13:36:07 -0700 Subject: [PATCH 18/57] finish initial tuning --- .../stp/tactic/goalie/goalie_tactic_test.py | 4 ++-- src/software/er_force_simulator_main.cpp | 12 +++++++--- .../sensor_fusion/filter/ball_filter.cpp | 23 +++++++++++-------- .../sensor_fusion/filter/kalman_filter.cpp | 4 ++-- .../sensor_fusion/filter/kalman_filter.h | 4 ++-- 5 files changed, 29 insertions(+), 18 deletions(-) diff --git a/src/software/ai/hl/stp/tactic/goalie/goalie_tactic_test.py b/src/software/ai/hl/stp/tactic/goalie/goalie_tactic_test.py index 077d9d3126..1708f6e851 100644 --- a/src/software/ai/hl/stp/tactic/goalie/goalie_tactic_test.py +++ b/src/software/ai/hl/stp/tactic/goalie/goalie_tactic_test.py @@ -33,9 +33,9 @@ # # test ball very fast get saved # # TODO (#3377): This test is flaky due to inconsistent goalie reach. The linked ticket may provide a permanent fix. ( - tbots_cpp.Point(-2.5, 0), + tbots_cpp.Point(4.5, 0), # TODO Revert velocity to (-4.8, 1.1) - tbots_cpp.Vector(-4.8, 1.1), + tbots_cpp.Vector(-3.0, 0.5), tbots_cpp.Point(-4.5, 0), ), # test ball very fast with the goalie out of position saved diff --git a/src/software/er_force_simulator_main.cpp b/src/software/er_force_simulator_main.cpp index 5efa61fdde..5b2e52bae2 100644 --- a/src/software/er_force_simulator_main.cpp +++ b/src/software/er_force_simulator_main.cpp @@ -48,7 +48,7 @@ int main(int argc, char **argv) { std::string runtime_dir = args.runtime_dir; LoggerSingleton::initializeLogger(runtime_dir, nullptr); - LOG(CSV, "new_filter_data.csv") << "timestamp_s,fused_x,fused_y,truth_x,truth_y, true_vel_x, true_vel_y,is_occluded\n"; + LOG(CSV, "filter_data_2.csv") << "timestamp_s,fused_x,fused_y,truth_x,truth_y, true_vel_x, true_vel_y,is_occluded\n"; /** * Creates a ER force simulator and sets up the appropriate @@ -133,6 +133,7 @@ int main(int argc, char **argv) runtime_dir + WORLD_STATE_RECEIVED_TRIGGER_PATH); bool has_sent_world_state_trigger = false; + double start_timestamp_s = 0.0; // Inputs // World State Input: Configures the ERForceSimulator @@ -222,8 +223,13 @@ int main(int argc, char **argv) } auto sim_state = er_force_sim->getSimulatorState(); - LOG(CSV, "new_filter_data.csv") - << yellow_vision.time_sent().epoch_timestamp_seconds() << "," + double current_ts = yellow_vision.time_sent().epoch_timestamp_seconds(); + if (start_timestamp_s == 0.0) + { + start_timestamp_s = current_ts; + } + LOG(CSV, "filter_data_2.csv") + << (current_ts - start_timestamp_s) << "," << yellow_vision.ball().current_state().global_position().x_meters() << "," << yellow_vision.ball().current_state().global_position().y_meters() diff --git a/src/software/sensor_fusion/filter/ball_filter.cpp b/src/software/sensor_fusion/filter/ball_filter.cpp index 5598844184..cd74e2ba96 100644 --- a/src/software/sensor_fusion/filter/ball_filter.cpp +++ b/src/software/sensor_fusion/filter/ball_filter.cpp @@ -15,24 +15,29 @@ namespace { const Eigen::Matrix INITIAL_COV = Eigen::Matrix::Identity() * 1000.0; + // Calculated friction in simualtor + const Eigen::Matrix R = (Eigen::Matrix() << + 1.68e-8, 2.01e-6, 0, 0, + 2.01e-6, 2.42e-4, 0, 0, + 0, 0, 1.68e-8, 2.01e-6, + 0, 0, 2.01e-6, 2.42e-4).finished(); + const Eigen::Matrix Q = (Eigen::Matrix() << - 0.1, 0, - 0, 0.1).finished(); - - const Eigen::Matrix R = (Eigen::Matrix() << - 0.1, 0, 0, 0, - 0, 0.1, 0, 0, - 0, 0, 0.01, 0, - 0, 0, 0, 0.01).finished(); + 0.0226, 0, + 0, 0.00445).finished(); const Eigen::Matrix C = (Eigen::Matrix() << 1, 0, 0, 0, 0, 1, 0, 0).finished(); + + // Empirically measured + const double DAMPING = 0.9889; + } BallFilter::BallFilter() : consecutive_outliers(0), - kalman_filter(INITIAL_STATE, INITIAL_COV, Q, R, C,0.9) + kalman_filter(INITIAL_STATE, INITIAL_COV, R, Q, C, DAMPING) { } diff --git a/src/software/sensor_fusion/filter/kalman_filter.cpp b/src/software/sensor_fusion/filter/kalman_filter.cpp index e051402eab..c5ff92a95f 100644 --- a/src/software/sensor_fusion/filter/kalman_filter.cpp +++ b/src/software/sensor_fusion/filter/kalman_filter.cpp @@ -3,16 +3,16 @@ KalmanFilter::KalmanFilter( const Eigen::Matrix& X, const Eigen::Matrix& P_i, +const Eigen::Matrix & R, const Eigen::Matrix& Q, -const Eigen::Matrix& R, const Eigen::Matrix& C, double damping_term ): X(X), P(P_i), P_i(P_i), -Q(Q), R(R), +Q(Q), C(C), damping_term(damping_term) { diff --git a/src/software/sensor_fusion/filter/kalman_filter.h b/src/software/sensor_fusion/filter/kalman_filter.h index 54edc7e3af..7c81413a3e 100644 --- a/src/software/sensor_fusion/filter/kalman_filter.h +++ b/src/software/sensor_fusion/filter/kalman_filter.h @@ -14,8 +14,8 @@ class KalmanFilter{ KalmanFilter( const Eigen::Matrix& X, const Eigen::Matrix& P_i, - const Eigen::Matrix& Q, const Eigen::Matrix& R, + const Eigen::Matrix& Q, const Eigen::Matrix& C, double damping_term ); @@ -36,8 +36,8 @@ Eigen::Matrix getCovariance(); Eigen::Matrix X; // State Eigen::Matrix P; // State Covariance Eigen::Matrix P_i; // State Covariance -Eigen::Matrix Q; // Measurement noise Eigen::Matrix R; // process noise +Eigen::Matrix Q; // Measurement noise Eigen::Matrix C; // State to measurement double damping_term; From bb002528dad461ce746d7694c1d0cd0381d9a319 Mon Sep 17 00:00:00 2001 From: StarrryNight Date: Mon, 6 Apr 2026 17:12:29 -0700 Subject: [PATCH 19/57] tuning values... --- .../stp/tactic/goalie/goalie_tactic_test.py | 20 ++++++++++++++++++- src/software/er_force_simulator_main.cpp | 8 ++++++-- .../sensor_fusion/filter/ball_filter.cpp | 10 +++++----- 3 files changed, 30 insertions(+), 8 deletions(-) diff --git a/src/software/ai/hl/stp/tactic/goalie/goalie_tactic_test.py b/src/software/ai/hl/stp/tactic/goalie/goalie_tactic_test.py index 1708f6e851..b0e6b8e9b9 100644 --- a/src/software/ai/hl/stp/tactic/goalie/goalie_tactic_test.py +++ b/src/software/ai/hl/stp/tactic/goalie/goalie_tactic_test.py @@ -33,11 +33,29 @@ # # test ball very fast get saved # # TODO (#3377): This test is flaky due to inconsistent goalie reach. The linked ticket may provide a permanent fix. ( - tbots_cpp.Point(4.5, 0), + tbots_cpp.Point(4.5, -1.5), # TODO Revert velocity to (-4.8, 1.1) tbots_cpp.Vector(-3.0, 0.5), tbots_cpp.Point(-4.5, 0), ), + ( + tbots_cpp.Point(4.5, 3), + # TODO Revert velocity to (-4.8, 1.1) + tbots_cpp.Vector(-1.0, -2.5), + tbots_cpp.Point(-4.5, 0), + ), + ( + tbots_cpp.Point(4.5, -3.0), + # TODO Revert velocity to (-4.8, 1.1) + tbots_cpp.Vector(-4.0, 3), + tbots_cpp.Point(-4.5, 0), + ), + ( + tbots_cpp.Point(4.5, 3), + # TODO Revert velocity to (-4.8, 1.1) + tbots_cpp.Vector(-3.0, -2.0), + tbots_cpp.Point(-4.5, 0), + ), # test ball very fast with the goalie out of position saved # TODO (#3377): This test is flaky due to inconsistent goalie reach. The linked ticket may provide a permanent fix. # ( diff --git a/src/software/er_force_simulator_main.cpp b/src/software/er_force_simulator_main.cpp index 5b2e52bae2..eae5db2b15 100644 --- a/src/software/er_force_simulator_main.cpp +++ b/src/software/er_force_simulator_main.cpp @@ -48,7 +48,7 @@ int main(int argc, char **argv) { std::string runtime_dir = args.runtime_dir; LoggerSingleton::initializeLogger(runtime_dir, nullptr); - LOG(CSV, "filter_data_2.csv") << "timestamp_s,fused_x,fused_y,truth_x,truth_y, true_vel_x, true_vel_y,is_occluded\n"; + LOG(CSV, "filter_data_6.csv") << "timestamp_s,fused_x,fused_y,fused_vel_x, fused_vel_y, truth_x,truth_y, true_vel_x, true_vel_y,is_occluded\n"; /** * Creates a ER force simulator and sets up the appropriate @@ -228,11 +228,15 @@ int main(int argc, char **argv) { start_timestamp_s = current_ts; } - LOG(CSV, "filter_data_2.csv") + LOG(CSV, "filter_data_6.csv") << (current_ts - start_timestamp_s) << "," << yellow_vision.ball().current_state().global_position().x_meters() << "," << yellow_vision.ball().current_state().global_position().y_meters() + << "," + << yellow_vision.ball().current_state().global_velocity().x_component_meters() + << "," + << yellow_vision.ball().current_state().global_velocity().y_component_meters() << "," << sim_state.ball().p_x() << "," << sim_state.ball().p_y() << "," << sim_state.ball().v_x() << "," << sim_state.ball().v_y() << "," << !er_force_sim->isBallVisible() diff --git a/src/software/sensor_fusion/filter/ball_filter.cpp b/src/software/sensor_fusion/filter/ball_filter.cpp index cd74e2ba96..22925aebf1 100644 --- a/src/software/sensor_fusion/filter/ball_filter.cpp +++ b/src/software/sensor_fusion/filter/ball_filter.cpp @@ -16,11 +16,11 @@ namespace { const Eigen::Matrix INITIAL_COV = Eigen::Matrix::Identity() * 1000.0; // Calculated friction in simualtor - const Eigen::Matrix R = (Eigen::Matrix() << - 1.68e-8, 2.01e-6, 0, 0, - 2.01e-6, 2.42e-4, 0, 0, - 0, 0, 1.68e-8, 2.01e-6, - 0, 0, 2.01e-6, 2.42e-4).finished(); +const Eigen::Matrix R = (Eigen::Matrix() << + 2.222e-8, 0, 2.000e-6, 0, + 0, 2.222e-8, 0, 2.000e-6, + 2.000e-6, 0, 2.400e-4, 0, + 0, 2.000e-6, 0, 2.400e-4).finished(); const Eigen::Matrix Q = (Eigen::Matrix() << 0.0226, 0, From 6fbb0ed61e8e97f7f803dfcac691e971c694523a Mon Sep 17 00:00:00 2001 From: StarrryNight Date: Mon, 6 Apr 2026 19:00:46 -0700 Subject: [PATCH 20/57] Tuning mostly done --- src/software/er_force_simulator_main.cpp | 4 ++-- .../sensor_fusion/filter/ball_filter.cpp | 20 ++++++++++--------- .../sensor_fusion/filter/ball_filter.h | 2 -- 3 files changed, 13 insertions(+), 13 deletions(-) diff --git a/src/software/er_force_simulator_main.cpp b/src/software/er_force_simulator_main.cpp index eae5db2b15..96eff8ec7c 100644 --- a/src/software/er_force_simulator_main.cpp +++ b/src/software/er_force_simulator_main.cpp @@ -48,7 +48,7 @@ int main(int argc, char **argv) { std::string runtime_dir = args.runtime_dir; LoggerSingleton::initializeLogger(runtime_dir, nullptr); - LOG(CSV, "filter_data_6.csv") << "timestamp_s,fused_x,fused_y,fused_vel_x, fused_vel_y, truth_x,truth_y, true_vel_x, true_vel_y,is_occluded\n"; + LOG(CSV, "filter_data_10_perfect.csv") << "timestamp_s,fused_x,fused_y,fused_vel_x, fused_vel_y, truth_x,truth_y, true_vel_x, true_vel_y,is_occluded\n"; /** * Creates a ER force simulator and sets up the appropriate @@ -228,7 +228,7 @@ int main(int argc, char **argv) { start_timestamp_s = current_ts; } - LOG(CSV, "filter_data_6.csv") + LOG(CSV, "filter_data_10_perfect.csv") << (current_ts - start_timestamp_s) << "," << yellow_vision.ball().current_state().global_position().x_meters() << "," diff --git a/src/software/sensor_fusion/filter/ball_filter.cpp b/src/software/sensor_fusion/filter/ball_filter.cpp index 22925aebf1..fd27ae9c89 100644 --- a/src/software/sensor_fusion/filter/ball_filter.cpp +++ b/src/software/sensor_fusion/filter/ball_filter.cpp @@ -15,12 +15,11 @@ namespace { const Eigen::Matrix INITIAL_COV = Eigen::Matrix::Identity() * 1000.0; - // Calculated friction in simualtor -const Eigen::Matrix R = (Eigen::Matrix() << - 2.222e-8, 0, 2.000e-6, 0, - 0, 2.222e-8, 0, 2.000e-6, - 2.000e-6, 0, 2.400e-4, 0, - 0, 2.000e-6, 0, 2.400e-4).finished(); + const Eigen::Matrix R = (Eigen::Matrix() << + 2.222e-8, 0, 2.000e-6, 0, + 0, 2.222e-8, 0, 2.000e-6, + 2.000e-6, 0, 2.400e-4, 0, + 0, 2.000e-6, 0, 2.400e-4).finished(); const Eigen::Matrix Q = (Eigen::Matrix() << 0.0226, 0, @@ -33,6 +32,9 @@ const Eigen::Matrix R = (Eigen::Matrix() << // Empirically measured const double DAMPING = 0.9889; + const double MAHANALOGIS_THRESHOLD=1; + const int CONSECUTIVE_OUTLIERS_THRESHOLD=3; + } BallFilter::BallFilter() : @@ -53,18 +55,18 @@ std::optional BallFilter::estimateBallState( kalman_filter.predict(delta_t); } if (best_ball_detection){ - prev_detection_timestamp = best_ball_detection->timestamp; + prev_detection_timestamp = current_time; Eigen::Matrix measurement; measurement << best_ball_detection->position.x(), best_ball_detection->position.y(); double mahalanobis = kalman_filter.getMahalanobisDistance(measurement); - if (mahalanobis< mahalanobis_threshold){ + if (mahalanobis< MAHANALOGIS_THRESHOLD){ kalman_filter.update(measurement); } else{ consecutive_outliers++; } - if (consecutive_outliers> consecutive_outliers_threshold){ + if (consecutive_outliers> CONSECUTIVE_OUTLIERS_THRESHOLD){ kalman_filter.reset(measurement); consecutive_outliers=0; } diff --git a/src/software/sensor_fusion/filter/ball_filter.h b/src/software/sensor_fusion/filter/ball_filter.h index 3f37a45159..6ed636ca99 100644 --- a/src/software/sensor_fusion/filter/ball_filter.h +++ b/src/software/sensor_fusion/filter/ball_filter.h @@ -37,8 +37,6 @@ class BallFilter private: std::optional getBestBallDetection(const std::vector &new_ball_detections); int consecutive_outliers; - double mahalanobis_threshold=1; - int consecutive_outliers_threshold=10; KalmanFilter kalman_filter; std::optional prev_detection_timestamp; }; From 72ee29715ccb6ff1686a2fd3f7952fb5ffff61de Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Thu, 16 Apr 2026 21:20:19 -0700 Subject: [PATCH 21/57] write javadoc and rename ball filter to ball_tracker --- src/software/sensor_fusion/filter/BUILD | 14 ++-- .../sensor_fusion/filter/ball_filter.h | 42 ----------- .../{ball_filter.cpp => ball_tracker.cpp} | 28 ++++---- .../sensor_fusion/filter/ball_tracker.h | 64 +++++++++++++++++ ..._filter_test.cpp => ball_tracker_test.cpp} | 42 +++++------ .../sensor_fusion/filter/kalman_filter.cpp | 17 +++-- .../sensor_fusion/filter/kalman_filter.h | 72 ++++++++++++++++--- src/software/sensor_fusion/sensor_fusion.cpp | 6 +- src/software/sensor_fusion/sensor_fusion.h | 4 +- 9 files changed, 179 insertions(+), 110 deletions(-) delete mode 100644 src/software/sensor_fusion/filter/ball_filter.h rename src/software/sensor_fusion/filter/{ball_filter.cpp => ball_tracker.cpp} (83%) create mode 100644 src/software/sensor_fusion/filter/ball_tracker.h rename src/software/sensor_fusion/filter/{ball_filter_test.cpp => ball_tracker_test.cpp} (95%) diff --git a/src/software/sensor_fusion/filter/BUILD b/src/software/sensor_fusion/filter/BUILD index 0ea19f8ed6..4d21f61956 100644 --- a/src/software/sensor_fusion/filter/BUILD +++ b/src/software/sensor_fusion/filter/BUILD @@ -22,9 +22,9 @@ cc_library( ], ) cc_library( - name = "ball_filter", - srcs = ["ball_filter.cpp"], - hdrs = ["ball_filter.h"], + name = "ball_tracker", + srcs = ["ball_tracker.cpp"], + hdrs = ["ball_tracker.h"], deps = [ ":vision_detection", ":kalman_filter", @@ -38,10 +38,10 @@ cc_library( ) cc_test( - name = "ball_filter_test", - srcs = ["ball_filter_test.cpp"], + name = "ball_tracker_test", + srcs = ["ball_tracker_test.cpp"], deps = [ - ":ball_filter", + ":ball_tracker", "//shared/test_util:tbots_gtest_main", "//software/world:field", ], @@ -90,7 +90,7 @@ cc_test( cc_library( name = "sensor_fusion_filters", deps = [ - ":ball_filter", + ":ball_tracker", ":robot_team_filter", ], ) diff --git a/src/software/sensor_fusion/filter/ball_filter.h b/src/software/sensor_fusion/filter/ball_filter.h deleted file mode 100644 index 6ed636ca99..0000000000 --- a/src/software/sensor_fusion/filter/ball_filter.h +++ /dev/null @@ -1,42 +0,0 @@ -#pragma once - -#include -#include - -#include "software/geom/line.h" -#include "software/geom/point.h" -#include "software/geom/rectangle.h" -#include "software/sensor_fusion/filter/vision_detection.h" -#include "software/time/timestamp.h" -#include "software/world/ball.h" -#include "software/sensor_fusion/filter/kalman_filter.h" -class BallFilter -{ - public: - /** - * Creates a new Ball Filter - */ - BallFilter(); - - /** - * Update the filter with the new ball detection data, and returns the new - * estimated state of the ball given the new data - * - * @param new_ball_detections A list of new Ball detections - * @param filter_area The area within which the ball filter will work. Any detections - * outside of this area will be ignored. - * - * @return The new ball based on the estimated state of the ball given the new data. - * If a filtered result cannot be calculated, returns std::nullopt - */ - std::optional estimateBallState( - const std::vector& new_ball_detections, - const Rectangle& filter_area, - const Timestamp& current_time); - - private: - std::optional getBestBallDetection(const std::vector &new_ball_detections); - int consecutive_outliers; - KalmanFilter kalman_filter; - std::optional prev_detection_timestamp; -}; diff --git a/src/software/sensor_fusion/filter/ball_filter.cpp b/src/software/sensor_fusion/filter/ball_tracker.cpp similarity index 83% rename from src/software/sensor_fusion/filter/ball_filter.cpp rename to src/software/sensor_fusion/filter/ball_tracker.cpp index fd27ae9c89..d05aa9d5c1 100644 --- a/src/software/sensor_fusion/filter/ball_filter.cpp +++ b/src/software/sensor_fusion/filter/ball_tracker.cpp @@ -1,4 +1,4 @@ -#include "software/sensor_fusion/filter/ball_filter.h" +#include "software/sensor_fusion/filter/ball_tracker.h" #include #include @@ -12,19 +12,19 @@ namespace { const Eigen::Matrix INITIAL_STATE = Eigen::Matrix::Zero(); - + const Eigen::Matrix INITIAL_COV = Eigen::Matrix::Identity() * 1000.0; - - const Eigen::Matrix R = (Eigen::Matrix() << + + const Eigen::Matrix Q = (Eigen::Matrix() << 2.222e-8, 0, 2.000e-6, 0, 0, 2.222e-8, 0, 2.000e-6, 2.000e-6, 0, 2.400e-4, 0, 0, 2.000e-6, 0, 2.400e-4).finished(); - const Eigen::Matrix Q = (Eigen::Matrix() << + const Eigen::Matrix R = (Eigen::Matrix() << 0.0226, 0, 0, 0.00445).finished(); - + const Eigen::Matrix C = (Eigen::Matrix() << 1, 0, 0, 0, 0, 1, 0, 0).finished(); @@ -37,16 +37,16 @@ namespace { } -BallFilter::BallFilter() : +BallTracker::BallTracker() : consecutive_outliers(0), - kalman_filter(INITIAL_STATE, INITIAL_COV, R, Q, C, DAMPING) + kalman_filter(INITIAL_STATE, INITIAL_COV, Q, R, C, DAMPING) { } -std::optional BallFilter::estimateBallState( +std::optional BallTracker::estimateBallState( const std::vector &new_ball_detections, const Rectangle &filter_area, const Timestamp& current_time) { - std::optional best_ball_detection = getBestBallDetection(new_ball_detections); + std::optional best_ball_detection = getBestBallDetection(new_ball_detections); if (prev_detection_timestamp){ double delta_t; @@ -77,12 +77,12 @@ std::optional BallFilter::estimateBallState( Point ball_position = Point(kalman_state(0), kalman_state(1)); Vector ball_velocity = Vector(kalman_state(2), kalman_state(3)); double z_height = best_ball_detection ? best_ball_detection->distance_from_ground : 0.0; - + BallState ball_state(ball_position, ball_velocity, z_height); return Ball(ball_state,current_time); - + } -std::optional BallFilter::getBestBallDetection(const std::vector &new_ball_detections){ +std::optional BallTracker::getBestBallDetection(const std::vector &new_ball_detections){ if (new_ball_detections.empty()){ return std::nullopt; } @@ -92,5 +92,3 @@ std::optional BallFilter::getBestBallDetection(const std::vector< }); } } - - diff --git a/src/software/sensor_fusion/filter/ball_tracker.h b/src/software/sensor_fusion/filter/ball_tracker.h new file mode 100644 index 0000000000..ad3c6a6fd6 --- /dev/null +++ b/src/software/sensor_fusion/filter/ball_tracker.h @@ -0,0 +1,64 @@ +#pragma once + +#include +#include + +#include "software/geom/line.h" +#include "software/geom/point.h" +#include "software/geom/rectangle.h" +#include "software/sensor_fusion/filter/vision_detection.h" +#include "software/time/timestamp.h" +#include "software/world/ball.h" +#include "software/sensor_fusion/filter/kalman_filter.h" + +/** + * Tracks a ball across frames using a Kalman filter with a constant-velocity + * motion model. Outlier detections are rejected via Mahalanobis-distance gating; + * if too many consecutive detections are rejected the filter is hard-reset to the + * latest measurement. + */ +class BallTracker +{ + public: + /** + * Creates a new BallTracker with default noise parameters and initial state. + */ + BallTracker(); + + /** + * Updates the tracker with the latest ball detections and returns an estimated + * ball state. + * + * The best detection (highest confidence) within the filter area is selected + * and used to update the Kalman filter. If the selected detection is an + * outlier (Mahalanobis distance exceeds threshold), it is rejected and the + * outlier count is incremented. Once the outlier count exceeds the threshold, + * the filter is reset to the outlier detection. + * + * @param new_ball_detections A list of new ball detections from vision + * @param filter_area The area within which ball detections are considered + * valid; detections outside this area are ignored + * @param current_time The timestamp of the current frame + * + * @return The estimated Ball state. If no valid detection has ever been + * received, returns a Ball at the origin with zero velocity. + */ + std::optional estimateBallState( + const std::vector& new_ball_detections, + const Rectangle& filter_area, + const Timestamp& current_time); + + private: + /** + * Selects the highest-confidence detection from the given list. + * + * @param new_ball_detections A list of ball detections to choose from + * + * @return The detection with the highest confidence, or std::nullopt if the + * list is empty + */ + std::optional getBestBallDetection(const std::vector &new_ball_detections); + int consecutive_outliers; + KalmanFilter kalman_filter; + std::optional prev_detection_timestamp; +}; diff --git a/src/software/sensor_fusion/filter/ball_filter_test.cpp b/src/software/sensor_fusion/filter/ball_tracker_test.cpp similarity index 95% rename from src/software/sensor_fusion/filter/ball_filter_test.cpp rename to src/software/sensor_fusion/filter/ball_tracker_test.cpp index 4c00e8fe76..5de695766f 100644 --- a/src/software/sensor_fusion/filter/ball_filter_test.cpp +++ b/src/software/sensor_fusion/filter/ball_tracker_test.cpp @@ -1,4 +1,4 @@ -#include "software/sensor_fusion/filter/ball_filter.h" +#include "software/sensor_fusion/filter/ball_tracker.h" #include @@ -12,12 +12,12 @@ #include "software/geom/segment.h" #include "software/world/field.h" -class BallFilterTest : public ::testing::Test +class BallTrackerTest : public ::testing::Test { protected: - BallFilterTest() + BallTrackerTest() : field(Field::createSSLDivisionBField()), - ball_filter(), + ball_tracker(), current_timestamp(Timestamp::fromSeconds(123)), time_step(Duration::fromSeconds(1.0 / 60.0)) { @@ -201,7 +201,7 @@ class BallFilterTest : public ::testing::Test // Get the filtered result given the new detection information auto filtered_ball = - ball_filter.estimateBallState(ball_detections, field.fieldBoundary()); + ball_tracker.estimateBallState(ball_detections, field.fieldBoundary()); if (i < num_steps_to_ignore) { continue; @@ -237,7 +237,7 @@ class BallFilterTest : public ::testing::Test } Field field; - BallFilter ball_filter; + BallTracker ball_tracker; Timestamp current_timestamp; Duration time_step; std::mt19937 random_generator; @@ -246,7 +246,7 @@ class BallFilterTest : public ::testing::Test static constexpr double BALL_DISTANCE_FROM_GROUND = 0.0; }; -TEST_F(BallFilterTest, ball_sitting_still_with_low_noise) +TEST_F(BallTrackerTest, ball_sitting_still_with_low_noise) { Ray ball_trajectory = Ray(Point(0, 0), Vector(0, 0)); double ball_velocity_magnitude = 0; @@ -269,7 +269,7 @@ TEST_F(BallFilterTest, ball_sitting_still_with_low_noise) num_simulation_steps, num_steps_to_ignore); } -TEST_F(BallFilterTest, ball_sitting_still_with_moderate_noise) +TEST_F(BallTrackerTest, ball_sitting_still_with_moderate_noise) { Ray ball_trajectory = Ray(Point(0, 0), Vector(0, 0)); double ball_velocity_magnitude = 0; @@ -292,7 +292,7 @@ TEST_F(BallFilterTest, ball_sitting_still_with_moderate_noise) num_simulation_steps, num_steps_to_ignore); } -TEST_F(BallFilterTest, ball_moving_slow_in_a_straight_line_with_no_noise_in_data) +TEST_F(BallTrackerTest, ball_moving_slow_in_a_straight_line_with_no_noise_in_data) { Segment ball_path = Segment(field.friendlyCornerNeg(), field.enemyCornerPos()); double ball_velocity_magnitude = 0.31; @@ -311,7 +311,7 @@ TEST_F(BallFilterTest, ball_moving_slow_in_a_straight_line_with_no_noise_in_data num_steps_to_ignore); } -TEST_F(BallFilterTest, ball_moving_slow_in_a_straight_line_with_small_noise_in_data) +TEST_F(BallTrackerTest, ball_moving_slow_in_a_straight_line_with_small_noise_in_data) { Segment ball_path = Segment(field.friendlyCornerNeg(), field.enemyCornerPos()); double ball_velocity_magnitude = 0.3; @@ -330,7 +330,7 @@ TEST_F(BallFilterTest, ball_moving_slow_in_a_straight_line_with_small_noise_in_d num_steps_to_ignore); } -TEST_F(BallFilterTest, ball_moving_slow_in_a_straight_line_with_medium_noise_in_data) +TEST_F(BallTrackerTest, ball_moving_slow_in_a_straight_line_with_medium_noise_in_data) { Segment ball_path = Segment(field.friendlyCornerNeg(), field.enemyCornerPos()); double ball_velocity_magnitude = 0.3; @@ -349,7 +349,7 @@ TEST_F(BallFilterTest, ball_moving_slow_in_a_straight_line_with_medium_noise_in_ num_steps_to_ignore); } -TEST_F(BallFilterTest, ball_moving_fast_in_a_straight_line_with_no_noise_in_data) +TEST_F(BallTrackerTest, ball_moving_fast_in_a_straight_line_with_no_noise_in_data) { Segment ball_path = Segment(field.friendlyCornerNeg(), field.enemyCornerPos()); double ball_velocity_magnitude = 6.2; @@ -368,7 +368,7 @@ TEST_F(BallFilterTest, ball_moving_fast_in_a_straight_line_with_no_noise_in_data num_steps_to_ignore); } -TEST_F(BallFilterTest, ball_moving_fast_in_a_straight_line_with_small_noise_in_data) +TEST_F(BallTrackerTest, ball_moving_fast_in_a_straight_line_with_small_noise_in_data) { Segment ball_path = Segment(field.friendlyCornerNeg(), field.enemyCornerPos()); double ball_velocity_magnitude = 5.72; @@ -387,7 +387,7 @@ TEST_F(BallFilterTest, ball_moving_fast_in_a_straight_line_with_small_noise_in_d num_steps_to_ignore); } -TEST_F(BallFilterTest, ball_moving_fast_in_a_straight_line_with_medium_noise_in_data) +TEST_F(BallTrackerTest, ball_moving_fast_in_a_straight_line_with_medium_noise_in_data) { Segment ball_path = Segment(field.friendlyCornerNeg(), field.enemyCornerPos()); double ball_velocity_magnitude = 5.04; @@ -406,7 +406,7 @@ TEST_F(BallFilterTest, ball_moving_fast_in_a_straight_line_with_medium_noise_in_ num_steps_to_ignore); } -TEST_F(BallFilterTest, +TEST_F(BallTrackerTest, ball_moving_fast_in_a_straight_line_and_then_bouncing_with_no_noise_in_data) { Segment ball_path = Segment(field.friendlyCornerNeg(), field.enemyCornerPos()); @@ -440,7 +440,7 @@ TEST_F(BallFilterTest, num_steps_to_ignore); } -TEST_F(BallFilterTest, +TEST_F(BallTrackerTest, ball_moving_fast_in_a_straight_line_and_then_bouncing_with_no_noise_in_data_2) { Segment ball_path = @@ -475,7 +475,7 @@ TEST_F(BallFilterTest, num_steps_to_ignore); } -TEST_F(BallFilterTest, +TEST_F(BallTrackerTest, ball_moving_fast_in_a_straight_line_and_then_bouncing_with_no_noise_in_data_3) { Segment ball_path = Segment(Point(-3, -1), Point(0, 0)); @@ -509,7 +509,7 @@ TEST_F(BallFilterTest, num_steps_to_ignore); } -TEST_F(BallFilterTest, ball_moving_along_x_axis) +TEST_F(BallTrackerTest, ball_moving_along_x_axis) { Segment ball_path = Segment(field.friendlyGoalCenter(), field.enemyGoalCenter()); double ball_velocity_magnitude = 5; @@ -528,7 +528,7 @@ TEST_F(BallFilterTest, ball_moving_along_x_axis) num_steps_to_ignore); } -TEST_F(BallFilterTest, ball_moving_along_y_axis) +TEST_F(BallTrackerTest, ball_moving_along_y_axis) { Segment ball_path = field.halfwayLine(); double ball_velocity_magnitude = 5; @@ -547,7 +547,7 @@ TEST_F(BallFilterTest, ball_moving_along_y_axis) num_steps_to_ignore); } -TEST_F(BallFilterTest, ball_moving_fast_with_vision_delay) +TEST_F(BallTrackerTest, ball_moving_fast_with_vision_delay) { // Setup a fast moving ball (e.g., 5.0 m/s) Vector ball_velocity(5.0, 0.0); @@ -565,7 +565,7 @@ TEST_F(BallFilterTest, ball_moving_fast_with_vision_delay) // The crucial change: we ask the filter for the state at t_capture + vision_delay Timestamp t_now = t_capture + vision_delay; - auto filtered_ball = ball_filter.estimateBallState(det, field.fieldBoundary(), t_now); + auto filtered_ball = ball_tracker.estimateBallState(det, field.fieldBoundary(), t_now); // THE ASSERTION: diff --git a/src/software/sensor_fusion/filter/kalman_filter.cpp b/src/software/sensor_fusion/filter/kalman_filter.cpp index c5ff92a95f..ddd5cc1c47 100644 --- a/src/software/sensor_fusion/filter/kalman_filter.cpp +++ b/src/software/sensor_fusion/filter/kalman_filter.cpp @@ -3,23 +3,23 @@ KalmanFilter::KalmanFilter( const Eigen::Matrix& X, const Eigen::Matrix& P_i, -const Eigen::Matrix & R, -const Eigen::Matrix& Q, +const Eigen::Matrix & Q, +const Eigen::Matrix& R, const Eigen::Matrix& C, double damping_term - ): + ): X(X), P(P_i), P_i(P_i), -R(R), Q(Q), +R(R), C(C), damping_term(damping_term) { } void KalmanFilter::predict(const double delta_t){ - + Eigen::Matrix A; // Using the constant velocity model as motion model @@ -30,12 +30,12 @@ void KalmanFilter::predict(const double delta_t){ X = A*X; - P = A*P*A.transpose() + R; + P = A*P*A.transpose() + Q; } void KalmanFilter::update(const Eigen::Matrix Z){ Eigen::Matrix Kg; - Eigen::Matrix S =C*P*C.transpose()+Q; + Eigen::Matrix S =C*P*C.transpose()+R; Kg = P*C.transpose() * S.inverse(); X = X + Kg*(Z-C*X); P = (Eigen::Matrix::Identity()-Kg*C)*P; @@ -47,7 +47,7 @@ void KalmanFilter::reset(const Eigen::Matrix Z){ } double KalmanFilter::getMahalanobisDistance(const Eigen::Matrix& Z) const { - Eigen::Matrix S =C*P*C.transpose()+Q ; + Eigen::Matrix S =C*P*C.transpose()+R ; // Calculate the mahalanobis distance for gating double M = ((Z - C*X).transpose() * S.inverse() * (Z-C*X))(0,0); return M; @@ -61,4 +61,3 @@ Eigen::Matrix KalmanFilter::getCovariance(){ return P; } - diff --git a/src/software/sensor_fusion/filter/kalman_filter.h b/src/software/sensor_fusion/filter/kalman_filter.h index 7c81413a3e..e0eb365043 100644 --- a/src/software/sensor_fusion/filter/kalman_filter.h +++ b/src/software/sensor_fusion/filter/kalman_filter.h @@ -1,45 +1,95 @@ #pragma once #include -/** - * Implementation of a kalman filter - -**/ +/** + * A Kalman filter for tracking a 2D object using a constant velocity motion model. + * + * The state vector is [x, y, vx, vy]^T. Measurements are 2D positions [x, y]^T. + * The filter supports predict/update steps as well as Mahalanobis-distance-based + * gating and a hard reset when the tracked object jumps unexpectedly. + */ class KalmanFilter{ public: +/** + * Creates a new KalmanFilter + * + * @param X The initial state vector [x, y, vx, vy]^T + * @param P_i The initial state covariance matrix (4x4) + * @param Q The process noise covariance matrix (4x4), representing uncertainty + * introduced by the motion model at each prediction step + * @param R The measurement noise covariance matrix (2x2), representing + * uncertainty in position measurements + * @param C The measurement matrix that maps the state space to the measurement + * space (2x4) + * @param damping_term A scalar in [0, 1] applied to the velocity states each + * predict step to model drag/friction + */ KalmanFilter( const Eigen::Matrix& X, const Eigen::Matrix& P_i, - const Eigen::Matrix& R, - const Eigen::Matrix& Q, + const Eigen::Matrix& Q, + const Eigen::Matrix& R, const Eigen::Matrix& C, double damping_term ); +/** + * Propagates the state estimate forward in time using the constant-velocity + * motion model and adds process noise to the covariance. + * + * @param delta_t Time elapsed since the last predict step, in seconds + */ void predict(const double delta_t); +/** + * Corrects the state estimate using a new position measurement. + * + * @param Z The measurement vector [x, y]^T + */ void update(const Eigen::Matrix Z); +/** + * Resets the filter to the given measurement, zeroing velocities and + * restoring the initial covariance. + * + * @param Z The measurement vector [x, y]^T to reset the state to + */ void reset(const Eigen::Matrix Z); +/** + * Returns the squared Mahalanobis distance between the given measurement and + * the current predicted measurement. Used for gating outlier detections. + * + * @param Z The measurement vector [x, y]^T to evaluate + * @return The squared Mahalanobis distance + */ double getMahalanobisDistance(const Eigen::Matrix& Z) const; +/** + * Returns the current state estimate vector [x, y, vx, vy]^T. + * + * @return The current state vector (4x1) + */ Eigen::Matrix getState(); +/** + * Returns the current state covariance matrix. + * + * @return The current covariance matrix (4x4) + */ Eigen::Matrix getCovariance(); private: Eigen::Matrix X; // State Eigen::Matrix P; // State Covariance -Eigen::Matrix P_i; // State Covariance -Eigen::Matrix R; // process noise -Eigen::Matrix Q; // Measurement noise -Eigen::Matrix C; // State to measurement +Eigen::Matrix P_i; // Initial state covariance +Eigen::Matrix Q; // Process noise covariance +Eigen::Matrix R; // Measurement noise covariance +Eigen::Matrix C; // State to measurement matrix double damping_term; }; - diff --git a/src/software/sensor_fusion/sensor_fusion.cpp b/src/software/sensor_fusion/sensor_fusion.cpp index c3c67cfd3a..acd7d4ecbf 100644 --- a/src/software/sensor_fusion/sensor_fusion.cpp +++ b/src/software/sensor_fusion/sensor_fusion.cpp @@ -12,7 +12,7 @@ SensorFusion::SensorFusion(TbotsProto::SensorFusionConfig sensor_fusion_config) game_state(), referee_stage(std::nullopt), dribble_displacement(std::nullopt), - ball_filter(), + ball_tracker(), friendly_team_filter(), enemy_team_filter(), possession(TeamPossession::FRIENDLY_TEAM), @@ -356,7 +356,7 @@ std::optional SensorFusion::createBall( if (field) { std::optional new_ball = - ball_filter.estimateBallState(ball_detections, field.value().fieldBoundary(), current_time); + ball_tracker.estimateBallState(ball_detections, field.value().fieldBoundary(), current_time); return new_ball; } return std::nullopt; @@ -514,7 +514,7 @@ void SensorFusion::resetWorldComponents() enemy_team = Team(); game_state = GameState(); referee_stage = std::nullopt; - ball_filter = BallFilter(); + ball_tracker = BallTracker(); friendly_team_filter = RobotTeamFilter(); enemy_team_filter = RobotTeamFilter(); possession = TeamPossession::FRIENDLY_TEAM; diff --git a/src/software/sensor_fusion/sensor_fusion.h b/src/software/sensor_fusion/sensor_fusion.h index 0036818676..06a99da995 100644 --- a/src/software/sensor_fusion/sensor_fusion.h +++ b/src/software/sensor_fusion/sensor_fusion.h @@ -7,7 +7,7 @@ #include "proto/message_translation/ssl_referee.h" #include "proto/parameters.pb.h" #include "proto/sensor_msg.pb.h" -#include "software/sensor_fusion/filter/ball_filter.h" +#include "software/sensor_fusion/filter/ball_tracker.h" #include "software/sensor_fusion/filter/robot_team_filter.h" #include "software/sensor_fusion/filter/vision_detection.h" #include "software/sensor_fusion/possession/possession_tracker.h" @@ -191,7 +191,7 @@ class SensorFusion std::optional dribble_displacement; - BallFilter ball_filter; + BallTracker ball_tracker; RobotTeamFilter friendly_team_filter; RobotTeamFilter enemy_team_filter; From 9d967fb92cb531633c4e390f51a54103b5216a8c Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Thu, 16 Apr 2026 21:21:33 -0700 Subject: [PATCH 22/57] remove old filter --- .../sensor_fusion/filter/ball_filter_old.cpp | 381 ------------------ .../sensor_fusion/filter/ball_filter_old.h | 196 --------- 2 files changed, 577 deletions(-) delete mode 100644 src/software/sensor_fusion/filter/ball_filter_old.cpp delete mode 100644 src/software/sensor_fusion/filter/ball_filter_old.h diff --git a/src/software/sensor_fusion/filter/ball_filter_old.cpp b/src/software/sensor_fusion/filter/ball_filter_old.cpp deleted file mode 100644 index 0967a76ff8..0000000000 --- a/src/software/sensor_fusion/filter/ball_filter_old.cpp +++ /dev/null @@ -1,381 +0,0 @@ -#include "software/sensor_fusion/filter/ball_filter.h" - -#include -#include -#include - -#include "shared/constants.h" -#include "software/geom/algorithms/closest_point.h" -#include "software/geom/algorithms/contains.h" -#include "software/math/math_functions.h" - - -BallFilter::BallFilter() : ball_detection_buffer(MAX_BUFFER_SIZE) {} - -std::optional BallFilter::estimateBallState( - const std::vector &new_ball_detections, const Rectangle &filter_area) -{ - addNewDetectionsToBuffer(new_ball_detections, filter_area); - return estimateBallStateFromBuffer(ball_detection_buffer); -} - -void BallFilter::addNewDetectionsToBuffer(std::vector new_ball_detections, - const Rectangle &filter_area) -{ - // Sort the detections in increasing order before processing. This places the oldest - // detections (with the smallest timestamp) at the front of the buffer, and the most - // recent detections (largest timestamp) at the end of the buffer. - std::sort(new_ball_detections.begin(), new_ball_detections.end()); - - for (const auto &detection : new_ball_detections) - { - // Remove any detections outside the filter area - if (!contains(filter_area, detection.position)) - { - continue; - } - - if (!ball_detection_buffer.empty()) - { - // Use the smallest timestamp to minimize time_diffs of 0 - auto detection_with_smallest_timestamp = *std::min_element( - ball_detection_buffer.begin(), ball_detection_buffer.end()); - Duration time_diff = - detection.timestamp - detection_with_smallest_timestamp.timestamp; - - // Ignore any data from the past, and any data that is as old as the oldest - // data in the buffer since it provides no additional value. This also - // prevents division by 0 when calculating the estimated velocity - if (time_diff.toSeconds() <= 0) - { - continue; - } - - // We determine if the detection is noise based on how far it is from a ball - // detection in the buffer. From this, we can calculate how fast the ball - // must have moved to reach the new detection position. If this estimated - // velocity is too far above the maximum allowed velocity, then there is a - // good chance the detection is just noise and not the real ball. In this - // case, we ignore the new "noise" data - double detection_distance = - (detection.position - detection_with_smallest_timestamp.position) - .length(); - double estimated_detection_velocity_magnitude = - detection_distance / time_diff.toSeconds(); - - // Make the maximum acceptable velocity a bit larger than the strict limits - // according to the game rules to account for measurement error, and to be a - // bit on the safe side. We don't want to risk discarding real data. - double maximum_acceptable_velocity_magnitude = - BALL_MAX_SPEED_METERS_PER_SECOND + MAX_ACCEPTABLE_BALL_SPEED_BUFFER; - if (estimated_detection_velocity_magnitude > - maximum_acceptable_velocity_magnitude) - { - // If we determine the data to be noise, remove an entry from the buffer. - // This way if we have messed up and now the ball is too far away for the - // buffer to track, the buffer will rapidly shrink and start tracking the - // ball at its new location once the buffer is empty. - // We sort the vector in decreasing order first so that we can always - // ensure any elements that are ejected from the end of the buffer are the - // oldest data - std::sort(ball_detection_buffer.rbegin(), ball_detection_buffer.rend()); - ball_detection_buffer.pop_back(); - } - else - { - // We sort the vector in decreasing order first so that we can always - // ensure any elements that are ejected from the end of the buffer are the - // oldest data - std::sort(ball_detection_buffer.rbegin(), ball_detection_buffer.rend()); - ball_detection_buffer.push_front(detection); - } - } - else - { - // If there is no data in the buffer, we always add the new data - ball_detection_buffer.push_front(detection); - } - } -} - -std::optional BallFilter::estimateBallStateFromBuffer( - boost::circular_buffer ball_detections) -{ - // Sort the detections in decreasing order before processing. This places the most - // recent detections (with the largest timestamp) at the front of the buffer, and the - // oldest detections (smallest timestamp) at the end of the buffer - std::sort(ball_detections.rbegin(), ball_detections.rend()); - - if (ball_detections.empty()) - { - return std::nullopt; - } - else if (ball_detections.size() == 1) - { - // If there is only 1 entry in the buffer, we can't fit a regression line - // or calculate a velocity so we do our best with just the position - BallState ball_state(ball_detections.front().position, Vector(0, 0), - ball_detections.front().distance_from_ground); - Ball ball(ball_state, ball_detections.front().timestamp); - return ball; - } - - std::optional adjusted_buffer_size = getAdjustedBufferSize(ball_detections); - if (!adjusted_buffer_size) - { - return std::nullopt; - } - ball_detections.resize(*adjusted_buffer_size); - - auto regression = calculateLineOfBestFit(ball_detections); - - Point filtered_position = - estimateBallPosition(ball_detections, regression.regression_line); - - auto estimated_velocity = estimateBallVelocity(ball_detections, std::nullopt); - - if (regression.regression_error < LINEAR_REGRESSION_ERROR_THRESHOLD) - { - estimated_velocity = - estimateBallVelocity(ball_detections, regression.regression_line); - } - if (!estimated_velocity) - { - return std::nullopt; - } - - BallState ball_state(filtered_position, estimated_velocity->average_velocity, - ball_detections.front().distance_from_ground); - return Ball(ball_state, ball_detections.front().timestamp); -} - -std::optional BallFilter::getAdjustedBufferSize( - boost::circular_buffer ball_detections) -{ - // Sort the detections in decreasing order before processing. This places the most - // recent detections (with the largest timestamp) at the front of the buffer, and the - // oldest detections (smallest timestamp) at the end of the buffer - std::sort(ball_detections.rbegin(), ball_detections.rend()); - - double buffer_size_velocity_magnitude_diff = - MAX_BUFFER_SIZE_VELOCITY_MAGNITUDE - MIN_BUFFER_SIZE_VELOCITY_MAGNITUDE; - - unsigned int max_buffer_size = - std::min(MAX_BUFFER_SIZE, static_cast(ball_detections.size())); - unsigned int min_buffer_size = - std::min(MIN_BUFFER_SIZE, static_cast(ball_detections.size())); - double buffer_size_diff = max_buffer_size - min_buffer_size; - - std::optional velocity_estimate = - estimateBallVelocity(ball_detections); - if (!velocity_estimate) - { - return std::nullopt; - } - // Use the average of the min and max velocity magnitudes in the buffer. We use this - // rather than the average so we can quickly respond to drastic changes in the ball - // velocity, such as when the ball goes from being stationary to moving quickly (like - // when it's kicked). If the buffer is large, then it will take more time for the mean - // speed to increase enough to start shrinking the buffer. However, the average of the - // min and max values will immediately increase if the ball starts moving, so the - // buffer can start shrinking more quickly and increase the filter response time to - // these sorts of changes. - double min_max_magnitude_average = velocity_estimate->min_max_magnitude_average; - - // Between the min and max velocity magnitudes, we linearly scale the size of the - // buffer - double linear_offset = - MIN_BUFFER_SIZE_VELOCITY_MAGNITUDE + (buffer_size_velocity_magnitude_diff / 2); - double linear_scaling_factor = linear(min_max_magnitude_average, linear_offset, - buffer_size_velocity_magnitude_diff); - int buffer_size = - max_buffer_size - - static_cast(std::floor(linear_scaling_factor * buffer_size_diff)); - - return static_cast(buffer_size); -} - -BallFilter::LinearRegressionResults BallFilter::calculateLineOfBestFit( - boost::circular_buffer ball_detections) -{ - if (ball_detections.size() < 2) - { - throw std::invalid_argument("At least 2 elements required for linear regression"); - } - - auto x_vs_y_regression = calculateLinearRegression(ball_detections); - - // Linear regression cannot fit a vertical line. To get around this, we fit two lines, - // one with x and y swapped, so any vertical line becomes horizontal. Then we take the - // line of the two that fit the best. - boost::circular_buffer swapped_ball_detections = ball_detections; - for (auto &detection : swapped_ball_detections) - { - detection.position = Point(detection.position.y(), detection.position.x()); - } - auto y_vs_x_regression = calculateLinearRegression(swapped_ball_detections); - // Because we swapped the coordinates of the input, we have to swap the coordinates of - // the output to get back to our expected coordinate space - y_vs_x_regression.regression_line.swapXY(); - - // We use the regression from above with the least error - if (x_vs_y_regression.regression_error < y_vs_x_regression.regression_error) - { - return x_vs_y_regression; - } - else - { - return y_vs_x_regression; - } -} - -BallFilter::LinearRegressionResults BallFilter::calculateLinearRegression( - boost::circular_buffer ball_detections) -{ - if (ball_detections.size() < 2) - { - throw std::invalid_argument("At least 2 elements required for linear regression"); - } - - // Sort the detections in increasing order before processing. This places the oldest - // detections (smallest timestamp) at the front of the buffer, and the most recent - // detections (with the largest timestamp) at the end of the buffer - std::sort(ball_detections.begin(), ball_detections.end()); - - // Construct matrix A and vector b for linear regression. The first column of A - // contains the bias variable, and the second column contains the x coordinates of the - // ball. Vector b contains the y coordinates of the ball. - Eigen::MatrixXf A(ball_detections.size(), 2); - Eigen::VectorXf b(ball_detections.size()); - for (unsigned i = 0; i < ball_detections.size(); i++) - { - // This extra column of 1's is the bias variable, so that we can regress with a - // y-intercept - A(i, 0) = 1.0; - A(i, 1) = static_cast(ball_detections.at(i).position.x()); - - b(i) = static_cast(ball_detections.at(i).position.y()); - } - - // Perform linear regression to find the line of best fit through the ball positions. - // This is solving the formula Ax = b, where x is the vector we want to solve for. - Eigen::Vector2f regression_vector = - A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b); - // How to calculate the error is from - // https://eigen.tuxfamily.org/dox/group__TutorialLinearAlgebra.html - // NOTE: using absolute error instead of relative because coordinates - // values should not affect error, also handles divide by 0 error - double regression_error = (A * regression_vector - b).norm(); // norm() is L2 norm - - // Find 2 points on the regression line that we solved for, and use this to construct - // our own Line class - Eigen::Vector2f p1_vec(1, 0); - Point p1(0, p1_vec.dot(regression_vector)); - Eigen::Vector2f p2_vec(1, 1); - Point p2(1, p2_vec.dot(regression_vector)); - Line regression_line = Line(p1, p2); - - LinearRegressionResults results({regression_line, regression_error}); - - return results; -} - -Point BallFilter::estimateBallPosition( - boost::circular_buffer ball_detections, const Line ®ression_line) -{ - if (ball_detections.empty()) - { - throw std::invalid_argument( - "Non-empty buffer required to estimate ball position"); - } - - // Take the position of the most recent ball position and project it onto the line of - // best fit. We do this because we assume the ball must be travelling along its - // velocity vector (the line), and this allows us to return more stable position - // values since the line of best fit is less likely to fluctuate compared to the raw - // position of a ball detection - BallDetection latest_ball_detection = ball_detections.front(); - return closestPoint(latest_ball_detection.position, regression_line); -} - -std::optional BallFilter::estimateBallVelocity( - boost::circular_buffer ball_detections, - const std::optional &ball_regression_line) -{ - // Sort the detections in increasing order before processing. This places the oldest - // detections (smallest timestamp) at the front of the buffer, and the most recent - // detections (with the largest timestamp) at the end of the buffer - std::sort(ball_detections.begin(), ball_detections.end()); - - std::vector ball_velocities; - std::vector ball_velocity_magnitudes; - for (unsigned i = 1; i < ball_detections.size(); i++) - { - for (unsigned j = i; j < ball_detections.size(); j++) - { - BallDetection previous_detection = ball_detections.at(i - 1); - BallDetection current_detection = ball_detections.at(j); - - Duration time_diff = - current_detection.timestamp - previous_detection.timestamp; - // Avoid division by 0. If we have adjacent detections with the same timestamp - // the velocity cannot be calculated - if (time_diff.toSeconds() == 0) - { - continue; - } - - // Project the detection positions onto the regression line if it was provided - Point current_position; - Point previous_position; - if (ball_regression_line) - { - current_position = closestPoint(current_detection.position, - ball_regression_line.value()); - previous_position = closestPoint(previous_detection.position, - ball_regression_line.value()); - } - else - { - current_position = current_detection.position; - previous_position = previous_detection.position; - } - Vector velocity_vector = current_position - previous_position; - double velocity_magnitude = velocity_vector.length() / time_diff.toSeconds(); - Vector velocity = velocity_vector.normalize(velocity_magnitude); - - ball_velocity_magnitudes.emplace_back(velocity_magnitude); - ball_velocities.emplace_back(velocity); - } - } - - if (ball_velocities.empty() || ball_velocity_magnitudes.empty()) - { - return std::nullopt; - } - - double velocity_magnitude_sum = 0; - for (const auto &velocity_magnitude : ball_velocity_magnitudes) - { - velocity_magnitude_sum += velocity_magnitude; - } - double average_velocity_magnitude = - velocity_magnitude_sum / static_cast(ball_velocity_magnitudes.size()); - double velocity_magnitude_max = *std::max_element(ball_velocity_magnitudes.begin(), - ball_velocity_magnitudes.end()); - double velocity_magnitude_min = *std::min_element(ball_velocity_magnitudes.begin(), - ball_velocity_magnitudes.end()); - double min_max_average = (velocity_magnitude_min + velocity_magnitude_max) / 2.0; - - Vector velocity_vector_sum = Vector(0, 0); - for (const auto &velocity : ball_velocities) - { - velocity_vector_sum += velocity; - } - Vector average_velocity = velocity_vector_sum.normalize(average_velocity_magnitude); - - BallVelocityEstimate velocity_data( - {average_velocity, average_velocity_magnitude, min_max_average}); - - return velocity_data; -} diff --git a/src/software/sensor_fusion/filter/ball_filter_old.h b/src/software/sensor_fusion/filter/ball_filter_old.h deleted file mode 100644 index cf108b2ac6..0000000000 --- a/src/software/sensor_fusion/filter/ball_filter_old.h +++ /dev/null @@ -1,196 +0,0 @@ -#pragma once - -#include -#include - -#include "software/geom/line.h" -#include "software/geom/point.h" -#include "software/geom/rectangle.h" -#include "software/sensor_fusion/filter/vision_detection.h" -#include "software/time/timestamp.h" -#include "software/world/ball.h" - -/** - * Given ball data from SSL Vision, filters and returns the position/velocity of the - * "real" ball. - * - * This ball filter stores a buffer of previous SSL Vision detections, and uses linear - * regression to find the path the ball is travelling on and estimate its position - * and velocity. This buffer/regression system was chosen because it results in a - * very stable output, particularly for the ball velocity. The data we receive isn't - * perfect (which is why we have a filter). If we receive a noisy position that is off - * the ball's current trajectory, it will have minimal impact. This means that as - * the ball is travelling, this filter will return a very steady velocity vector. - * This is important because small deviations in velocity orientation can have large - * effects when the AI tries to predict the future position of the ball. For example, - * consistently receiving a pass relies on the ball's velocity being very stable, - * otherwise the robot would "jiggle" back and forth as the estimated receiver position - * would keep changing. - */ -class BallFilter -{ - public: - // The min and max sizes of the ball detection buffer. - // As the ball slows down, the buffer size will approach the MAX_BUFFER_SIZE. - // As the ball speeds up, the buffer size will approach the MIN_BUFFER_SIZE. - static constexpr unsigned int MIN_BUFFER_SIZE = 4; - static constexpr unsigned int MAX_BUFFER_SIZE = 10; - // If the estimated ball speed is less than this value, the largest possible buffer - // will be used by the filter - static constexpr double MIN_BUFFER_SIZE_VELOCITY_MAGNITUDE = 0.5; - // If the estimated ball speed is greater than this value, the smallest possible - // buffer will be used by the filter - static constexpr double MAX_BUFFER_SIZE_VELOCITY_MAGNITUDE = 4.0; - // The extra amount beyond the ball's max speed that we treat ball detections as valid - static constexpr double MAX_ACCEPTABLE_BALL_SPEED_BUFFER = 2.0; - // The maximum root mean squared error threshold to considering using the generated - // linear regression. - // TODO (#2752): Investigate different values of error threshold - static constexpr double LINEAR_REGRESSION_ERROR_THRESHOLD = 1000.0; - - /** - * Creates a new Ball Filter - */ - explicit BallFilter(); - - /** - * Update the filter with the new ball detection data, and returns the new - * estimated state of the ball given the new data - * - * @param new_ball_detections A list of new Ball detections - * @param filter_area The area within which the ball filter will work. Any detections - * outside of this area will be ignored. - * - * @return The new ball based on the estimated state of the ball given the new data. - * If a filtered result cannot be calculated, returns std::nullopt - */ - std::optional estimateBallState( - const std::vector& new_ball_detections, - const Rectangle& filter_area); - - private: - /** - * A simple struct we use to pass around velocity estimate data - */ - struct BallVelocityEstimate - { - Vector average_velocity; - double average_velocity_magnitude; - // The average of the max velocity magnitude and min velocity magnitude - double min_max_magnitude_average; - }; - - /** - * A simple struct to pass around linear regression data - */ - struct LinearRegressionResults - { - Line regression_line; - // Regression error is root mean squared error - double regression_error; - }; - - /** - * Adds ball detections to the buffer stored by this filter. This function will ignore - * data if: - * - the data is outside of the filter_area, or - * - the data is too far away from the current known ball position - * (since it is likely to be random noise). - * - * @param new_ball_detections The ball detections to try add to the buffer - * @param filter_area The area within which the ball filter will work. Any detections - * outside of this area will be ignored. - */ - void addNewDetectionsToBuffer(std::vector new_ball_detections, - const Rectangle& filter_area); - - /** - * Uses linear regression to filter the given list of ball detections to find the - * current "real" state of the ball. - * - * @param ball_detections The detections to filter - * - * @return The new ball based on the filtered state. If a filtered result cannot be - * calculated, returns std::nullopt - */ - static std::optional estimateBallStateFromBuffer( - boost::circular_buffer ball_detections); - - /** - * Returns how large the buffer of ball detections should be based on the ball's - * estimated velocity. A slower moving ball will result in a larger buffer size, and a - * faster ball will result in a smaller buffer size. This is because with a slow - * moving ball, we need more data in order to fit a line with reasonable accuracy, - * since the datapoints will be very close to one another. - * - * @param ball_detections The full list of ball detections - * - * @return The size the buffer should be to perform filtering operations. If an error - * occurs that prevents the size from being calculated correctly, returns std::nullopt - */ - static std::optional getAdjustedBufferSize( - boost::circular_buffer ball_detections); - - /** - * Given a buffer of ball detections, returns the line of best fit through - * the detection positions, and calculate the root mean squared error of this - * regression. - * Note: also considers vertical lines. - * - * @throws std::invalid_argument if ball_detections has less than 2 elements - * - * @param ball_detections The ball detections to fit - * - * @return The line of best fit through the given ball detection positions - */ - static LinearRegressionResults calculateLineOfBestFit( - boost::circular_buffer ball_detections); - - /** - * Given a list of ball detections, use linear regression to find a line of best fit - * through the ball positions, and calculate the root mean squared error of this - * regression. - * - * @throws std::invalid_argument if ball_detections has less than 2 elements - * - * @param ball_detections The ball detections to use in the regression - * - * @return A struct containing the regression line and error of the linear regression - */ - static LinearRegressionResults calculateLinearRegression( - boost::circular_buffer ball_detections); - - /** - * Estimates the current position of the ball given a buffer of ball detections - * and the line of best fit through them. - * - * @throws std::invalid_argument if ball_detections has less than 2 elements - * - * @param ball_detections The ball detections - * @param regression_line The line of best fit through the ball positions - * - * @return The estimated position of the ball - */ - static Point estimateBallPosition( - boost::circular_buffer ball_detections, - const Line& regression_line); - - /** - * Estimates the ball's velocity based on the current detections in the given buffer. - * If the ball_regression_line is provided, the detection positions are projected onto - * the line before the velocities are calculated. If no velocity can be estimated, - * std::nullopt is returned. - * - * @param ball_detections The ball detections to use to calculate - * @param ball_regression_line The ball_regression_line to snap detections to before - * calculating velocities. - * - * @return A struct containing various estimates of the ball's velocity based on the - * given detections. If no velocity can be estimated, std::nullopt is returned - */ - static std::optional estimateBallVelocity( - boost::circular_buffer ball_detections, - const std::optional& ball_regression_line = std::nullopt); - - boost::circular_buffer ball_detection_buffer; -}; From 10c9de2026d917fc3b5e656e55d84cf50f6f5b59 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sun, 26 Apr 2026 11:55:55 -0700 Subject: [PATCH 23/57] revert goalie tests --- .../stp/tactic/goalie/goalie_tactic_test.py | 272 ++++++++---------- 1 file changed, 116 insertions(+), 156 deletions(-) diff --git a/src/software/ai/hl/stp/tactic/goalie/goalie_tactic_test.py b/src/software/ai/hl/stp/tactic/goalie/goalie_tactic_test.py index b0e6b8e9b9..bb890132d8 100644 --- a/src/software/ai/hl/stp/tactic/goalie/goalie_tactic_test.py +++ b/src/software/ai/hl/stp/tactic/goalie/goalie_tactic_test.py @@ -1,130 +1,111 @@ -import os import pytest - import software.python_bindings as tbots_cpp + from proto.import_all_protos import * -from software.simulated_tests.validation.robot_enters_region import * +from proto.message_translation.tbots_protobuf import create_world_state +from software.simulated_tests.simulated_test_fixture import ( + pytest_main, +) from software.simulated_tests.validation.ball_enters_region import * from software.simulated_tests.validation.ball_moves_in_direction import * -from software.simulated_tests.validation.friendly_has_ball_possession import * from software.simulated_tests.validation.ball_speed_threshold import * -from software.simulated_tests.validation.robot_speed_threshold import * from software.simulated_tests.validation.excessive_dribbling import * -from software.simulated_tests.simulated_test_fixture import ( - pytest_main, -) -from proto.message_translation.tbots_protobuf import create_world_state +from software.simulated_tests.validation.friendly_has_ball_possession import * +from software.simulated_tests.validation.robot_enters_region import * +from software.simulated_tests.validation.robot_speed_threshold import * @pytest.mark.parametrize( "ball_initial_position,ball_initial_velocity,robot_initial_position", [ - # # test panic ball very fast in straight line - # (tbots_cpp.Point(0, 0), tbots_cpp.Vector(-5, 0), tbots_cpp.Point(-4, 0)), - # # test panic ball very_fast in diagonal line - # ( - # tbots_cpp.Point(0, 0), - # tbots_cpp.Vector(-5.5, 0.25), - # tbots_cpp.Field.createSSLDivisionBField().friendlyGoalCenter() - # + tbots_cpp.Vector(0, -0.5), - # ), - # # test ball very fast misses net - # (tbots_cpp.Point(0, 0), tbots_cpp.Vector(-5, 1), tbots_cpp.Point(-4.5, 0)), - # # test ball very fast get saved - # # TODO (#3377): This test is flaky due to inconsistent goalie reach. The linked ticket may provide a permanent fix. + # test panic ball very fast in straight line + (tbots_cpp.Point(0, 0), tbots_cpp.Vector(-5, 0), tbots_cpp.Point(-4, 0)), + # test panic ball very_fast in diagonal line ( - tbots_cpp.Point(4.5, -1.5), - # TODO Revert velocity to (-4.8, 1.1) - tbots_cpp.Vector(-3.0, 0.5), - tbots_cpp.Point(-4.5, 0), + tbots_cpp.Point(0, 0), + tbots_cpp.Vector(-5.5, 0.25), + tbots_cpp.Field.createSSLDivisionBField().friendlyGoalCenter() + + tbots_cpp.Vector(0, -0.5), ), + # test ball very fast misses net + (tbots_cpp.Point(0, 0), tbots_cpp.Vector(-5, 1), tbots_cpp.Point(-4.5, 0)), + # test ball very fast get saved + # TODO (#3377): This test is flaky due to inconsistent goalie reach. The linked ticket may provide a permanent fix. ( - tbots_cpp.Point(4.5, 3), + tbots_cpp.Point(-2.5, 0), # TODO Revert velocity to (-4.8, 1.1) - tbots_cpp.Vector(-1.0, -2.5), + tbots_cpp.Vector(-3.6, 0.825), tbots_cpp.Point(-4.5, 0), ), + # test ball very fast with the goalie out of position saved + # TODO (#3377): This test is flaky due to inconsistent goalie reach. The linked ticket may provide a permanent fix. ( - tbots_cpp.Point(4.5, -3.0), - # TODO Revert velocity to (-4.8, 1.1) - tbots_cpp.Vector(-4.0, 3), - tbots_cpp.Point(-4.5, 0), + tbots_cpp.Point(-2, 0), + # TODO Revert velocity to (-5.5,1) + tbots_cpp.Vector(-4.125, 0.75), + tbots_cpp.Point(-4.5, -0.1), ), + # ball slow inside friendly defense area + (tbots_cpp.Point(-4, 0.8), tbots_cpp.Vector(-0.2, 0), tbots_cpp.Point(0, 0)), + # ball slow inside friendly defense area + (tbots_cpp.Point(-4, 0.8), tbots_cpp.Vector(-0.2, 0), tbots_cpp.Point(0, 2)), + # ball slow inside friendly defense area + (tbots_cpp.Point(-4, 0.8), tbots_cpp.Vector(-0.2, 0), tbots_cpp.Point(-4, 0)), + # ball stationary inside friendly defense area ( - tbots_cpp.Point(4.5, 3), - # TODO Revert velocity to (-4.8, 1.1) - tbots_cpp.Vector(-3.0, -2.0), - tbots_cpp.Point(-4.5, 0), + tbots_cpp.Point(-4, 0.0), + tbots_cpp.Vector(0.0, 0), + tbots_cpp.Field.createSSLDivisionBField().friendlyGoalpostPos(), + ), + # ball stationary inside no-chip rectangle + ( + tbots_cpp.Field.createSSLDivisionBField().friendlyGoalCenter() + + tbots_cpp.Vector(0.1, 0.1), + tbots_cpp.Vector(-0.2, 0), + tbots_cpp.Point(-4, -1), + ), + # ball fast inside no-chip rectangle but no intersection with goal + ( + tbots_cpp.Field.createSSLDivisionBField().friendlyGoalCenter() + + tbots_cpp.Vector(0.1, 0), + tbots_cpp.Vector(0, -0.5), + tbots_cpp.Point(-3.5, 1), + ), + # ball moving out from inside defense area + ( + tbots_cpp.Field.createSSLDivisionBField().friendlyGoalCenter() + + tbots_cpp.Vector(0.5, 0), + tbots_cpp.Vector(0.5, 0), + tbots_cpp.Point(-3.5, 0), + ), + # ball slow inside no-chip rectangle + ( + tbots_cpp.Field.createSSLDivisionBField().friendlyGoalCenter() + + tbots_cpp.Vector(0.1, 0), + tbots_cpp.Vector(0.1, -0.1), + tbots_cpp.Point(-3.5, 1), + ), + # ball moving into goal from inside defense area + ( + tbots_cpp.Field.createSSLDivisionBField().friendlyGoalCenter() + + tbots_cpp.Vector(0.5, 0), + tbots_cpp.Vector(-0.5, 0), + tbots_cpp.Point(-3.5, 0), + ), + # ball moving up and out of defense area + ( + tbots_cpp.Field.createSSLDivisionBField().friendlyGoalCenter() + + tbots_cpp.Vector(0.3, 0), + tbots_cpp.Vector(0, 1), + tbots_cpp.Point(-3.5, 0), + ), + # ball moving down and out goal from defense area + ( + tbots_cpp.Field.createSSLDivisionBField().friendlyGoalCenter() + + tbots_cpp.Vector(0.3, 0), + tbots_cpp.Vector(0, -0.7), + tbots_cpp.Point(-3.5, 0), ), - # test ball very fast with the goalie out of position saved - # TODO (#3377): This test is flaky due to inconsistent goalie reach. The linked ticket may provide a permanent fix. -# ( -# tbots_cpp.Point(-2, 0), -# # TODO Revert velocity to (-5.5,1) -# tbots_cpp.Vector(-5.5, 1), -# tbots_cpp.Point(-4.5, -0.1), -# ), -# # ball slow inside friendly defense area - # (tbots_cpp.Point(-4, 0.8), tbots_cpp.Vector(-0.2, 0), tbots_cpp.Point(0, 0)), - # # ball slow inside friendly defense area - # (tbots_cpp.Point(-4, 0.8), tbots_cpp.Vector(-0.2, 0), tbots_cpp.Point(0, 2)), - # # ball slow inside friendly defense area - # (tbots_cpp.Point(-4, 0.8), tbots_cpp.Vector(-0.2, 0), tbots_cpp.Point(-4, 0)), - # # ball stationary inside friendly defense area - # ( - # tbots_cpp.Point(-4, 0.0), - # tbots_cpp.Vector(0.0, 0), - # tbots_cpp.Field.createSSLDivisionBField().friendlyGoalpostPos(), - # ), - # # ball stationary inside no-chip rectangle - # ( - # tbots_cpp.Field.createSSLDivisionBField().friendlyGoalCenter() - # + tbots_cpp.Vector(0.1, 0.1), - # tbots_cpp.Vector(-0.2, 0), - # tbots_cpp.Point(-4, -1), - # ), - # # ball fast inside no-chip rectangle but no intersection with goal - # ( - # tbots_cpp.Field.createSSLDivisionBField().friendlyGoalCenter() - # + tbots_cpp.Vector(0.1, 0), - # tbots_cpp.Vector(0, -0.5), - # tbots_cpp.Point(-3.5, 1), - # ), - # # ball moving out from inside defense area - # ( - # tbots_cpp.Field.createSSLDivisionBField().friendlyGoalCenter() - # + tbots_cpp.Vector(0.5, 0), - # tbots_cpp.Vector(0.5, 0), - # tbots_cpp.Point(-3.5, 0), - # ), - # # ball slow inside no-chip rectangle - # ( - # tbots_cpp.Field.createSSLDivisionBField().friendlyGoalCenter() - # + tbots_cpp.Vector(0.1, 0), - # tbots_cpp.Vector(0.1, -0.1), - # tbots_cpp.Point(-3.5, 1), - # ), - # # ball moving into goal from inside defense area - # ( - # tbots_cpp.Field.createSSLDivisionBField().friendlyGoalCenter() - # + tbots_cpp.Vector(0.5, 0), - # tbots_cpp.Vector(-0.5, 0), - # tbots_cpp.Point(-3.5, 0), - # ), - # # ball moving up and out of defense area - # ( - # tbots_cpp.Field.createSSLDivisionBField().friendlyGoalCenter() - # + tbots_cpp.Vector(0.3, 0), - # tbots_cpp.Vector(0, 1), - # tbots_cpp.Point(-3.5, 0), - # ), - # # ball moving down and out goal from defense area - # ( - # tbots_cpp.Field.createSSLDivisionBField().friendlyGoalCenter() - # + tbots_cpp.Vector(0.3, 0), - # tbots_cpp.Vector(0, -0.7), - # tbots_cpp.Point(-3.5, 0), - # ), ], ) def test_goalie_blocks_shot( @@ -165,65 +146,44 @@ def setup(*args): eventually_validation_sequence_set = [ [ - FriendlyEventuallyHasBallPossession(), + # Goalie should be in the defense area + RobotEventuallyEntersRegion( + regions=[ + tbots_cpp.Field.createSSLDivisionBField().friendlyDefenseArea() + ] + ), ] ] - _passed = True - try: - simulated_test_runner.run_test( - setup=setup, - inv_eventually_validation_sequence_set=eventually_validation_sequence_set, - inv_always_validation_sequence_set=always_validation_sequence_set, - ag_eventually_validation_sequence_set=eventually_validation_sequence_set, - ag_always_validation_sequence_set=always_validation_sequence_set, - ) - except AssertionError: - _passed = False - raise - finally: - csv_path = os.path.join( - simulated_test_runner.simulator_runtime_dir, "goalie_tactic_data.csv" - ) - if os.path.exists(csv_path): - failed_val = 0 if _passed else 1 - with open(csv_path, "r") as _f: - lines = [l.rstrip("\n") for l in _f.readlines() if l.strip()] - header = lines[0] if lines else "" - already_has_failed = "failed" in header.split(",") - complete_cols = len(header.split(",")) if already_has_failed else len(header.split(",")) + 1 - with open(csv_path, "w") as _f: - for i, line in enumerate(lines): - if len(line.split(",")) < complete_cols: - # Row from current run — not yet labeled - if i == 0: - _f.write(f"{line},failed\n") - else: - _f.write(f"{line},{failed_val}\n") - else: - _f.write(f"{line}\n") + simulated_test_runner.run_test( + setup=setup, + inv_eventually_validation_sequence_set=eventually_validation_sequence_set, + inv_always_validation_sequence_set=always_validation_sequence_set, + ag_eventually_validation_sequence_set=eventually_validation_sequence_set, + ag_always_validation_sequence_set=always_validation_sequence_set, + ) @pytest.mark.parametrize( "ball_position,should_clear", [ - # ( - # tbots_cpp.Point(-3.45, 0), - # True, - # ), # ball is just inside the dead zone in the X direction - # ( - # tbots_cpp.Point(-3.45, 0.9), - # True, - # ), # ball is just inside the dead zone in the X direction - # ( - # tbots_cpp.Point(-4.0, 1.05), - # True, - # ), # ball is just inside the dead zone in the Y direction - # ( - # tbots_cpp.Point(0, 0), - # False, - # # ball is just outside the dead zone in the X direction - # ), + ( + tbots_cpp.Point(-3.45, 0), + True, + ), # ball is just inside the dead zone in the X direction + ( + tbots_cpp.Point(-3.45, 0.9), + True, + ), # ball is just inside the dead zone in the X direction + ( + tbots_cpp.Point(-4.0, 1.05), + True, + ), # ball is just inside the dead zone in the Y direction + ( + tbots_cpp.Point(0, 0), + False, + # ball is just outside the dead zone in the X direction + ), ], ) def test_goalie_clears_from_dead_zone( From 334c4de5b7a1424d7d04a3d71b275d3bfb21ea91 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Sun, 26 Apr 2026 12:13:34 -0700 Subject: [PATCH 24/57] test --- .../stp/tactic/goalie/goalie_tactic_test.py | 7 ++- src/software/simulated_tests/BUILD | 1 + .../simulated_tests/simulated_test_fixture.py | 6 ++ .../simulated_tests/tbots_test_runner.py | 7 +++ src/software/simulated_tests/validation/BUILD | 4 ++ .../validation/avoid_collisions.py | 2 +- .../validation/ball_enters_region.py | 2 +- .../validation/ball_enters_region_true.py | 61 ++++++++++++++++++ .../validation/ball_is_off_ground.py | 2 +- .../validation/ball_moves_in_direction.py | 4 +- .../validation/ball_speed_threshold.py | 2 +- .../validation/ball_speed_threshold_true.py | 63 +++++++++++++++++++ .../validation/ball_stops_in_region.py | 2 +- .../validation/ball_stops_in_region_true.py | 63 +++++++++++++++++++ .../validation/excessive_dribbling.py | 2 +- .../friendly_has_ball_possession.py | 2 +- .../validation/friendly_receives_ball_slow.py | 2 +- .../validation/friendly_team_scored.py | 2 +- .../validation/or_validation.py | 7 ++- .../robot_enters_placement_region.py | 2 +- .../validation/robot_enters_region.py | 2 +- .../robot_enters_region_and_stops.py | 2 +- .../validation/robot_speed_threshold.py | 2 +- .../simulated_tests/validation/validation.py | 36 ++++++++--- 24 files changed, 256 insertions(+), 29 deletions(-) create mode 100644 src/software/simulated_tests/validation/ball_enters_region_true.py create mode 100644 src/software/simulated_tests/validation/ball_speed_threshold_true.py create mode 100644 src/software/simulated_tests/validation/ball_stops_in_region_true.py diff --git a/src/software/ai/hl/stp/tactic/goalie/goalie_tactic_test.py b/src/software/ai/hl/stp/tactic/goalie/goalie_tactic_test.py index 2111109524..58f5072ae0 100644 --- a/src/software/ai/hl/stp/tactic/goalie/goalie_tactic_test.py +++ b/src/software/ai/hl/stp/tactic/goalie/goalie_tactic_test.py @@ -4,6 +4,7 @@ from proto.import_all_protos import * from software.simulated_tests.validation.robot_enters_region import * from software.simulated_tests.validation.ball_enters_region import * +from software.simulated_tests.validation.ball_enters_region_true import * from software.simulated_tests.validation.ball_moves_in_direction import * from software.simulated_tests.validation.friendly_has_ball_possession import * from software.simulated_tests.validation.ball_speed_threshold import * @@ -137,7 +138,7 @@ def setup(*args): RobotNeverEntersRegion( regions=[tbots_cpp.Field.createSSLDivisionBField().enemyDefenseArea()] ), - BallNeverEntersRegion( + BallNeverEntersRegionTrue( regions=[tbots_cpp.Field.createSSLDivisionBField().friendlyGoal()] ), NeverExcessivelyDribbles(), @@ -215,7 +216,7 @@ def setup(*args): always_validation_sequence_set = [ [ - BallNeverEntersRegion( + BallNeverEntersRegionTrue( regions=[ tbots_cpp.Field.createSSLDivisionBField().friendlyDefenseArea() ] @@ -230,7 +231,7 @@ def setup(*args): eventually_validation_sequence_set = [ [ # Goalie should be in the defense area - BallEventuallyExitsRegion( + BallEventuallyExitsRegionTrue( regions=[ tbots_cpp.Field.createSSLDivisionBField().friendlyDefenseArea() ] diff --git a/src/software/simulated_tests/BUILD b/src/software/simulated_tests/BUILD index 37dd733098..f6e3406fb7 100644 --- a/src/software/simulated_tests/BUILD +++ b/src/software/simulated_tests/BUILD @@ -18,6 +18,7 @@ py_library( "//software:py_constants.so", ], deps = [ + "//extlibs/er_force_sim/src/protobuf:erforce_py_proto", "//proto:import_all_protos", "//software/logger:py_logger", "//software/networking/unix:threaded_unix_listener_py", diff --git a/src/software/simulated_tests/simulated_test_fixture.py b/src/software/simulated_tests/simulated_test_fixture.py index 25e92dce64..8e275b8bc7 100644 --- a/src/software/simulated_tests/simulated_test_fixture.py +++ b/src/software/simulated_tests/simulated_test_fixture.py @@ -200,6 +200,11 @@ def runner( if self.thunderscope and tick_duration_s > processing_time: time.sleep(tick_duration_s - processing_time) + # Fetch the latest true simulator state (non-blocking; None if not yet available) + simulator_state = self.simulator_state_buffer.get( + block=False, return_cached=False + ) + # Validate ( eventually_validation_proto_set, @@ -208,6 +213,7 @@ def runner( world, eventually_validation_sequence_set, always_validation_sequence_set, + simulator_state=simulator_state, ) # Set the test name diff --git a/src/software/simulated_tests/tbots_test_runner.py b/src/software/simulated_tests/tbots_test_runner.py index 03e7d4ae85..bc983c0b0e 100644 --- a/src/software/simulated_tests/tbots_test_runner.py +++ b/src/software/simulated_tests/tbots_test_runner.py @@ -1,4 +1,5 @@ from proto.import_all_protos import * +from extlibs.er_force_sim.src.protobuf.world_pb2 import SimulatorState from software.logger.logger import create_logger from software.thunderscope.thread_safe_buffer import ThreadSafeBuffer from abc import abstractmethod @@ -46,6 +47,9 @@ def __init__( self.robot_status_buffer = ThreadSafeBuffer( buffer_size=1, protobuf_type=RobotStatus ) + self.simulator_state_buffer = ThreadSafeBuffer( + buffer_size=20, protobuf_type=SimulatorState + ) self.blue_full_system_proto_unix_io.register_observer( SSL_WrapperPacket, self.ssl_wrapper_buffer @@ -53,6 +57,9 @@ def __init__( self.blue_full_system_proto_unix_io.register_observer( RobotStatus, self.robot_status_buffer ) + self.blue_full_system_proto_unix_io.register_observer( + SimulatorState, self.simulator_state_buffer + ) if self.is_yellow_friendly: self.yellow_full_system_proto_unix_io.register_observer( World, self.world_buffer diff --git a/src/software/simulated_tests/validation/BUILD b/src/software/simulated_tests/validation/BUILD index 547439483f..ec530a107c 100644 --- a/src/software/simulated_tests/validation/BUILD +++ b/src/software/simulated_tests/validation/BUILD @@ -5,10 +5,13 @@ py_library( srcs = [ "avoid_collisions.py", "ball_enters_region.py", + "ball_enters_region_true.py", "ball_is_off_ground.py", "ball_moves_in_direction.py", "ball_speed_threshold.py", + "ball_speed_threshold_true.py", "ball_stops_in_region.py", + "ball_stops_in_region_true.py", "excessive_dribbling.py", "friendly_has_ball_possession.py", "friendly_receives_ball_slow.py", @@ -26,6 +29,7 @@ py_library( "//software:python_bindings.so", ], deps = [ + "//extlibs/er_force_sim/src/protobuf:erforce_py_proto", "//proto:software_py_proto", "//proto:tbots_py_proto", "//software/logger:py_logger", diff --git a/src/software/simulated_tests/validation/avoid_collisions.py b/src/software/simulated_tests/validation/avoid_collisions.py index 41953cd994..109ba2c06e 100644 --- a/src/software/simulated_tests/validation/avoid_collisions.py +++ b/src/software/simulated_tests/validation/avoid_collisions.py @@ -21,7 +21,7 @@ def __init__(self): self.fouled_robots = [] @override - def get_validation_status(self, world: World) -> ValidationStatus: + def get_validation_status(self, world: World, simulator_state=None) -> ValidationStatus: """Checks if any 2 robots in the world have collided :param world: the World message to validate diff --git a/src/software/simulated_tests/validation/ball_enters_region.py b/src/software/simulated_tests/validation/ball_enters_region.py index ff6e840f59..15c1cb6cb2 100644 --- a/src/software/simulated_tests/validation/ball_enters_region.py +++ b/src/software/simulated_tests/validation/ball_enters_region.py @@ -17,7 +17,7 @@ def __init__(self, regions=None): self.ball_position = None @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if the ball enters the provided regions :param world: The world msg to validate diff --git a/src/software/simulated_tests/validation/ball_enters_region_true.py b/src/software/simulated_tests/validation/ball_enters_region_true.py new file mode 100644 index 0000000000..c18cfb58f0 --- /dev/null +++ b/src/software/simulated_tests/validation/ball_enters_region_true.py @@ -0,0 +1,61 @@ +import software.python_bindings as tbots_cpp +from proto.import_all_protos import * + +from software.simulated_tests.validation.validation import ( + Validation, + create_validation_geometry, + create_validation_types, +) +from typing import override + + +class BallEntersRegionTrue(Validation): + """Checks if the ball's true (simulator) position enters any of the provided regions.""" + + def __init__(self, regions=None): + self.regions = regions if regions else [] + self.ball_position = None + + @override + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: + """Checks if the ball's true position enters the provided regions. + + Falls back to the sensor-fused world position when simulator_state is unavailable. + + :param world: The world msg (used as fallback) + :param simulator_state: The simulator state with true ball position + :return: FAILING until the ball enters any region, PASSING when it does + """ + if simulator_state is not None and simulator_state.HasField("ball"): + self.ball_position = tbots_cpp.Point( + simulator_state.ball.p_x, simulator_state.ball.p_y + ) + else: + self.ball_position = tbots_cpp.createPoint( + world.ball.current_state.global_position + ) + + for region in self.regions: + if tbots_cpp.contains(region, self.ball_position): + return ValidationStatus.PASSING + return ValidationStatus.FAILING + + @override + def get_validation_geometry(self, world) -> ValidationGeometry: + return create_validation_geometry(self.regions) + + @override + def __repr__(self): + return ( + "Checking true ball position in regions " + + ",".join(repr(region) for region in self.regions) + + (", ball position: " + str(self.ball_position) if self.ball_position else "") + ) + + +( + BallEventuallyEntersRegionTrue, + BallEventuallyExitsRegionTrue, + BallAlwaysStaysInRegionTrue, + BallNeverEntersRegionTrue, +) = create_validation_types(BallEntersRegionTrue) diff --git a/src/software/simulated_tests/validation/ball_is_off_ground.py b/src/software/simulated_tests/validation/ball_is_off_ground.py index 880150c838..3b6c84442b 100644 --- a/src/software/simulated_tests/validation/ball_is_off_ground.py +++ b/src/software/simulated_tests/validation/ball_is_off_ground.py @@ -21,7 +21,7 @@ def __init__(self, threshold=0.2): self.threshold = threshold @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if the ball has is threshold meters off the ground :param world: The world msg to validate diff --git a/src/software/simulated_tests/validation/ball_moves_in_direction.py b/src/software/simulated_tests/validation/ball_moves_in_direction.py index 533d818fc7..f4f138308a 100644 --- a/src/software/simulated_tests/validation/ball_moves_in_direction.py +++ b/src/software/simulated_tests/validation/ball_moves_in_direction.py @@ -24,7 +24,7 @@ def __init__(self, initial_ball_position: tbots.Point, tolerance: float = 0.1): self.tolerance = tolerance @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if ball is moving forward :param world: The world msg to validate @@ -84,7 +84,7 @@ def __init__(self, initial_ball_position, regions=[], tolerance: float = 0.1): self.regions = regions @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: for region in self.regions: if tbots.contains( region, tbots.createPoint(world.ball.current_state.global_position) diff --git a/src/software/simulated_tests/validation/ball_speed_threshold.py b/src/software/simulated_tests/validation/ball_speed_threshold.py index f1f8dcec46..a1ba9a34e4 100644 --- a/src/software/simulated_tests/validation/ball_speed_threshold.py +++ b/src/software/simulated_tests/validation/ball_speed_threshold.py @@ -22,7 +22,7 @@ def __init__(self, speed_threshold): self.speed_threshold = speed_threshold @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if the ball speed is at or above some threshold :param world: The world msg to validate diff --git a/src/software/simulated_tests/validation/ball_speed_threshold_true.py b/src/software/simulated_tests/validation/ball_speed_threshold_true.py new file mode 100644 index 0000000000..14718f8b79 --- /dev/null +++ b/src/software/simulated_tests/validation/ball_speed_threshold_true.py @@ -0,0 +1,63 @@ +import software.python_bindings as tbots_cpp +from proto.import_all_protos import * + +from software.simulated_tests.validation.validation import ( + Validation, + create_validation_geometry, + create_validation_types, +) +from typing import override + + +class BallSpeedThresholdTrue(Validation): + """Checks if the ball's true (simulator) speed is at or above some threshold.""" + + def __init__(self, speed_threshold): + """Constructor + + :param speed_threshold: The speed threshold in m/s + """ + self.speed_threshold = speed_threshold + + @override + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: + """Checks if the ball's true speed is at or above the threshold. + + Falls back to sensor-fused velocity when simulator_state is unavailable. + + :param world: The world msg (used as fallback) + :param simulator_state: The simulator state with true ball velocity + :return: FAILING if below threshold, PASSING if at or above + """ + if simulator_state is not None and simulator_state.HasField("ball"): + speed = tbots_cpp.Vector( + simulator_state.ball.v_x, simulator_state.ball.v_y + ).length() + else: + speed = tbots_cpp.createVector( + world.ball.current_state.global_velocity + ).length() + + if speed >= self.speed_threshold: + return ValidationStatus.PASSING + + return ValidationStatus.FAILING + + @override + def get_validation_geometry(self, world) -> ValidationGeometry: + return create_validation_geometry([]) + + @override + def __repr__(self): + return ( + "Check that the true ball speed is at or above " + + str(self.speed_threshold) + ) + + +( + BallSpeedEventuallyAtOrAboveThresholdTrue, + BallSpeedEventuallyBelowThresholdTrue, + BallSpeedAlwaysAtOrAboveThresholdTrue, + BallSpeedAlwaysBelowThresholdTrue, +) = create_validation_types(BallSpeedThresholdTrue) diff --git a/src/software/simulated_tests/validation/ball_stops_in_region.py b/src/software/simulated_tests/validation/ball_stops_in_region.py index 38b0312481..f7b46efd2a 100644 --- a/src/software/simulated_tests/validation/ball_stops_in_region.py +++ b/src/software/simulated_tests/validation/ball_stops_in_region.py @@ -16,7 +16,7 @@ def __init__(self, regions=None): self.regions = regions if regions else [] @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if the ball stops in the provided regions :param world: The world msg to validate diff --git a/src/software/simulated_tests/validation/ball_stops_in_region_true.py b/src/software/simulated_tests/validation/ball_stops_in_region_true.py new file mode 100644 index 0000000000..d805f1d875 --- /dev/null +++ b/src/software/simulated_tests/validation/ball_stops_in_region_true.py @@ -0,0 +1,63 @@ +import software.python_bindings as tbots_cpp +from proto.import_all_protos import * + +from software.simulated_tests.validation.validation import ( + Validation, + create_validation_geometry, + create_validation_types, +) +from typing import override + + +class BallStopsInRegionTrue(Validation): + """Checks if the ball's true (simulator) position stops in any of the provided regions.""" + + def __init__(self, regions=None): + self.regions = regions if regions else [] + + @override + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: + """Checks if the ball's true position stops in the provided regions. + + Falls back to the sensor-fused world position when simulator_state is unavailable. + + :param world: The world msg (used as fallback and for velocity) + :param simulator_state: The simulator state with true ball position/velocity + :return: FAILING until the ball stops in any region, PASSING when it does + """ + if simulator_state is not None and simulator_state.HasField("ball"): + ball_point = tbots_cpp.Point( + simulator_state.ball.p_x, simulator_state.ball.p_y + ) + ball_speed = tbots_cpp.Vector( + simulator_state.ball.v_x, simulator_state.ball.v_y + ).length() + else: + ball_point = tbots_cpp.createPoint(world.ball.current_state.global_position) + ball_speed = tbots_cpp.createVector( + world.ball.current_state.global_velocity + ).length() + + for region in self.regions: + if tbots_cpp.contains(region, ball_point) and ball_speed <= 0.01: + return ValidationStatus.PASSING + + return ValidationStatus.FAILING + + @override + def get_validation_geometry(self, world) -> ValidationGeometry: + return create_validation_geometry(self.regions) + + @override + def __repr__(self): + return "Checking true ball position stops in regions " + ",".join( + repr(region) for region in self.regions + ) + + +( + BallEventuallyStopsInRegionTrue, + BallEventuallyMovesInRegionTrue, + BallAlwaysStopsInRegionTrue, + BallNeverStopsInRegionTrue, +) = create_validation_types(BallStopsInRegionTrue) diff --git a/src/software/simulated_tests/validation/excessive_dribbling.py b/src/software/simulated_tests/validation/excessive_dribbling.py index 98a386b055..24f842fb9f 100644 --- a/src/software/simulated_tests/validation/excessive_dribbling.py +++ b/src/software/simulated_tests/validation/excessive_dribbling.py @@ -19,7 +19,7 @@ def __init__(self): self.dribbling_error_margin = 0.05 @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if any friendly robot is excessively dribbling the ball past the max dribble displacement minus the dribbling error margin diff --git a/src/software/simulated_tests/validation/friendly_has_ball_possession.py b/src/software/simulated_tests/validation/friendly_has_ball_possession.py index bf1013f7ed..58d73e3269 100644 --- a/src/software/simulated_tests/validation/friendly_has_ball_possession.py +++ b/src/software/simulated_tests/validation/friendly_has_ball_possession.py @@ -22,7 +22,7 @@ def __init__(self, robot_id=None, tolerance=0.01): self.tolerance = tolerance @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if friendly robot has possession of the ball :param world: The world msg to validate diff --git a/src/software/simulated_tests/validation/friendly_receives_ball_slow.py b/src/software/simulated_tests/validation/friendly_receives_ball_slow.py index 219271e001..68579e9550 100644 --- a/src/software/simulated_tests/validation/friendly_receives_ball_slow.py +++ b/src/software/simulated_tests/validation/friendly_receives_ball_slow.py @@ -24,7 +24,7 @@ def __init__(self, robot_id, max_receive_speed): self.max_receive_speed = max_receive_speed @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if the specified robot receives the ball too fast :param world: The world msg to validate diff --git a/src/software/simulated_tests/validation/friendly_team_scored.py b/src/software/simulated_tests/validation/friendly_team_scored.py index 458502f152..f0f4912ec6 100644 --- a/src/software/simulated_tests/validation/friendly_team_scored.py +++ b/src/software/simulated_tests/validation/friendly_team_scored.py @@ -16,7 +16,7 @@ def __init__(self): self.region = tbots_cpp.Field.createSSLDivisionBField().enemyGoal() @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if the ball enters the provided regions :param world: The world msg to validate diff --git a/src/software/simulated_tests/validation/or_validation.py b/src/software/simulated_tests/validation/or_validation.py index b14affe8c3..110b2b66de 100644 --- a/src/software/simulated_tests/validation/or_validation.py +++ b/src/software/simulated_tests/validation/or_validation.py @@ -17,9 +17,12 @@ def __init__(self, validations): self.validations = validations @override - def get_validation_status(self, world): + def get_validation_status(self, world, simulator_state=None): for validation in self.validations: - if validation.get_validation_status(world) == ValidationStatus.PASSING: + if ( + validation.get_validation_status(world, simulator_state=simulator_state) + == ValidationStatus.PASSING + ): return ValidationStatus.PASSING return ValidationStatus.FAILING diff --git a/src/software/simulated_tests/validation/robot_enters_placement_region.py b/src/software/simulated_tests/validation/robot_enters_placement_region.py index db6275e1f2..296d1da958 100644 --- a/src/software/simulated_tests/validation/robot_enters_placement_region.py +++ b/src/software/simulated_tests/validation/robot_enters_placement_region.py @@ -26,7 +26,7 @@ def __init__(self, placement_point): ) @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if robots enter the ball placement region :param world: The world msg to validate diff --git a/src/software/simulated_tests/validation/robot_enters_region.py b/src/software/simulated_tests/validation/robot_enters_region.py index d5d6584487..6b1b3a553a 100644 --- a/src/software/simulated_tests/validation/robot_enters_region.py +++ b/src/software/simulated_tests/validation/robot_enters_region.py @@ -24,7 +24,7 @@ def __init__(self, regions, req_robot_cnt): self.robot_in_zone = {} @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if a specific number of robots enter the provided set of regions :param world: The world msg to validate diff --git a/src/software/simulated_tests/validation/robot_enters_region_and_stops.py b/src/software/simulated_tests/validation/robot_enters_region_and_stops.py index 5cc4a5b052..ebdf6296fd 100644 --- a/src/software/simulated_tests/validation/robot_enters_region_and_stops.py +++ b/src/software/simulated_tests/validation/robot_enters_region_and_stops.py @@ -32,7 +32,7 @@ def __init__(self, regions=None, num_ticks=1): self.passing_robot_id = None @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if a robot is in the provided region Then checks if that robot is stationary within a threshold for the provided number of ticks diff --git a/src/software/simulated_tests/validation/robot_speed_threshold.py b/src/software/simulated_tests/validation/robot_speed_threshold.py index 19e20db786..c7a679849c 100644 --- a/src/software/simulated_tests/validation/robot_speed_threshold.py +++ b/src/software/simulated_tests/validation/robot_speed_threshold.py @@ -23,7 +23,7 @@ def __init__(self, speed_threshold): self.speed_threshold = speed_threshold @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if the friendly robots' speed is at or above some threshold :param world: The world msg to validate diff --git a/src/software/simulated_tests/validation/validation.py b/src/software/simulated_tests/validation/validation.py index 3b5e70d538..19df55e4cb 100644 --- a/src/software/simulated_tests/validation/validation.py +++ b/src/software/simulated_tests/validation/validation.py @@ -8,7 +8,7 @@ class Validation: """A validation function""" @abstractmethod - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: raise NotImplementedError("get_validation_status is not implemented") @abstractmethod @@ -66,15 +66,16 @@ def constructor(self, *args, **kwargs): """ self.validation = validation_class(*args, **kwargs) - def flip_validation(self, world): + def flip_validation(self, world, simulator_state=None): """Flip the validation status :param world: The world msg to validate on + :param simulator_state: The simulator state with true positions, or None """ return { ValidationStatus.FAILING: ValidationStatus.PASSING, ValidationStatus.PASSING: ValidationStatus.FAILING, - }[self.validation.get_validation_status(world)] + }[self.validation.get_validation_status(world, simulator_state=simulator_state)] # Generate the types: specifically, all Eventually validations will return # EVENTUALLY when get_validation_type is called, and all Always validations @@ -95,7 +96,10 @@ def flip_validation(self, world): + repr(self.validation), "get_validation_type": lambda self: ValidationType.EVENTUALLY, "get_validation_status": lambda self, - world: self.validation.get_validation_status(world), + world, + simulator_state=None: self.validation.get_validation_status( + world, simulator_state=simulator_state + ), }, ) @@ -107,7 +111,11 @@ def flip_validation(self, world): "__repr__": lambda self: "EventuallyFalseValidation: " + repr(self.validation), "get_validation_type": lambda self: ValidationType.EVENTUALLY, - "get_validation_status": lambda self, world: flip_validation(self, world), + "get_validation_status": lambda self, + world, + simulator_state=None: flip_validation( + self, world, simulator_state=simulator_state + ), }, ) @@ -119,7 +127,10 @@ def flip_validation(self, world): "__repr__": lambda self: "AlwaysTrueValidation: " + repr(self.validation), "get_validation_type": lambda self: ValidationType.ALWAYS, "get_validation_status": lambda self, - world: self.validation.get_validation_status(world), + world, + simulator_state=None: self.validation.get_validation_status( + world, simulator_state=simulator_state + ), }, ) @@ -130,7 +141,11 @@ def flip_validation(self, world): **common, "__repr__": lambda self: "AlwaysFalseValidation: " + repr(self.validation), "get_validation_type": lambda self: ValidationType.ALWAYS, - "get_validation_status": lambda self, world: flip_validation(self, world), + "get_validation_status": lambda self, + world, + simulator_state=None: flip_validation( + self, world, simulator_state=simulator_state + ), }, ) @@ -138,7 +153,10 @@ def flip_validation(self, world): def run_validation_sequence_sets( - world, eventually_validation_sequence_set, always_validation_sequence_set + world, + eventually_validation_sequence_set, + always_validation_sequence_set, + simulator_state=None, ): """Given both eventually and always validation sequence sets, (and world) run validation and aggregate the results in a validation proto set. @@ -170,7 +188,7 @@ def create_validation_proto_helper(validation_proto_set, validation): validation_proto = ValidationProto() # Get status - status = validation.get_validation_status(world) + status = validation.get_validation_status(world, simulator_state=simulator_state) # Create validation proto validation_proto.status = status From 5dad31b815030e4d53dc06da6ea08a202e197611 Mon Sep 17 00:00:00 2001 From: Eric Xiao Date: Sun, 10 May 2026 21:31:22 -0700 Subject: [PATCH 25/57] Update iterfzf version --- environment_setup/macos_requirements.txt | 2 +- environment_setup/ubuntu24_requirements.txt | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/environment_setup/macos_requirements.txt b/environment_setup/macos_requirements.txt index b5c3db8667..d35a5ec2da 100644 --- a/environment_setup/macos_requirements.txt +++ b/environment_setup/macos_requirements.txt @@ -1,7 +1,7 @@ ansible-lint==26.4.0 pyqtgraph==0.13.7 thefuzz==0.19.0 -iterfzf==0.5.0.20.0 +iterfzf==1.9.0.67.0 python-Levenshtein==0.25.1 psutil==5.9.0 PyOpenGL==3.1.6 diff --git a/environment_setup/ubuntu24_requirements.txt b/environment_setup/ubuntu24_requirements.txt index e3cad5dd7d..68c667fa79 100644 --- a/environment_setup/ubuntu24_requirements.txt +++ b/environment_setup/ubuntu24_requirements.txt @@ -1,7 +1,7 @@ ansible-lint==26.4.0 pyqtgraph==0.13.7 thefuzz==0.19.0 -iterfzf==0.5.0.20.0 +iterfzf==1.9.0.67.0 python-Levenshtein==0.25.1 psutil==5.9.0 PyOpenGL==3.1.6 From cf03a69c1c62ce5a5dd0be7ed185ad64553f296e Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Wed, 13 May 2026 21:47:09 -0700 Subject: [PATCH 26/57] temp --- .../ai/hl/stp/tactic/goalie/goalie_fsm.cpp | 2 +- .../stp/tactic/goalie/goalie_tactic_test.py | 7 +-- src/software/sensor_fusion/filter/BUILD | 2 + .../sensor_fusion/filter/ball_tracker.cpp | 44 ++++++++++++- .../sensor_fusion/filter/ball_tracker.h | 4 +- src/software/sensor_fusion/sensor_fusion.cpp | 20 ++---- src/software/sensor_fusion/sensor_fusion.h | 4 +- src/software/simulated_tests/validation/BUILD | 3 - .../validation/ball_enters_region_true.py | 61 ------------------ .../validation/ball_speed_threshold_true.py | 63 ------------------- .../validation/ball_stops_in_region_true.py | 63 ------------------- 11 files changed, 59 insertions(+), 214 deletions(-) delete mode 100644 src/software/simulated_tests/validation/ball_enters_region_true.py delete mode 100644 src/software/simulated_tests/validation/ball_speed_threshold_true.py delete mode 100644 src/software/simulated_tests/validation/ball_stops_in_region_true.py diff --git a/src/software/ai/hl/stp/tactic/goalie/goalie_fsm.cpp b/src/software/ai/hl/stp/tactic/goalie/goalie_fsm.cpp index b779a3d2fd..4078c94b95 100644 --- a/src/software/ai/hl/stp/tactic/goalie/goalie_fsm.cpp +++ b/src/software/ai/hl/stp/tactic/goalie/goalie_fsm.cpp @@ -254,7 +254,7 @@ void GoalieFSM::positionToBlock(const Update& event) event.common.set_primitive(std::make_unique( event.common.robot, goalie_pos, goalie_orientation, max_allowed_speed_mode, TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE, TbotsProto::DribblerMode::OFF, - TbotsProto::BallCollisionType::ALLOW, + TbotsProto::BallCollisionType::AVOID, AutoChipOrKick{AutoChipOrKickMode::AUTOCHIP, YEET_CHIP_DISTANCE_METERS})); } diff --git a/src/software/ai/hl/stp/tactic/goalie/goalie_tactic_test.py b/src/software/ai/hl/stp/tactic/goalie/goalie_tactic_test.py index 8e17d01b39..bb890132d8 100644 --- a/src/software/ai/hl/stp/tactic/goalie/goalie_tactic_test.py +++ b/src/software/ai/hl/stp/tactic/goalie/goalie_tactic_test.py @@ -7,7 +7,6 @@ pytest_main, ) from software.simulated_tests.validation.ball_enters_region import * -from software.simulated_tests.validation.ball_enters_region_true import * from software.simulated_tests.validation.ball_moves_in_direction import * from software.simulated_tests.validation.ball_speed_threshold import * from software.simulated_tests.validation.excessive_dribbling import * @@ -138,7 +137,7 @@ def setup(*args): RobotNeverEntersRegion( regions=[tbots_cpp.Field.createSSLDivisionBField().enemyDefenseArea()] ), - BallNeverEntersRegionTrue( + BallNeverEntersRegion( regions=[tbots_cpp.Field.createSSLDivisionBField().friendlyGoal()] ), NeverExcessivelyDribbles(), @@ -216,7 +215,7 @@ def setup(*args): always_validation_sequence_set = [ [ - BallNeverEntersRegionTrue( + BallNeverEntersRegion( regions=[ tbots_cpp.Field.createSSLDivisionBField().friendlyDefenseArea() ] @@ -231,7 +230,7 @@ def setup(*args): eventually_validation_sequence_set = [ [ # Goalie should be in the defense area - BallEventuallyExitsRegionTrue( + BallEventuallyExitsRegion( regions=[ tbots_cpp.Field.createSSLDivisionBField().friendlyDefenseArea() ] diff --git a/src/software/sensor_fusion/filter/BUILD b/src/software/sensor_fusion/filter/BUILD index 4d21f61956..c8ad1e071d 100644 --- a/src/software/sensor_fusion/filter/BUILD +++ b/src/software/sensor_fusion/filter/BUILD @@ -32,6 +32,8 @@ cc_library( "//software/math:math_functions", "//software/world:ball", "//software/world:field", + "//software/world:robot", + "//software:constants", "@boost//:circular_buffer", "@eigen", ], diff --git a/src/software/sensor_fusion/filter/ball_tracker.cpp b/src/software/sensor_fusion/filter/ball_tracker.cpp index d05aa9d5c1..6776977f37 100644 --- a/src/software/sensor_fusion/filter/ball_tracker.cpp +++ b/src/software/sensor_fusion/filter/ball_tracker.cpp @@ -5,6 +5,7 @@ #include #include "shared/constants.h" +#include "software/constants.h" #include "software/geom/algorithms/closest_point.h" #include "software/geom/algorithms/contains.h" #include "software/math/math_functions.h" @@ -35,6 +36,9 @@ namespace { const double MAHANALOGIS_THRESHOLD=1; const int CONSECUTIVE_OUTLIERS_THRESHOLD=3; + // Max distance from the dribbler position for a vision detection to be trusted + // while a robot is dribbling. Detections farther away are likely spurious. + const double DRIBBLING_MEASUREMENT_MAX_DISTANCE_METERS = 0.15; } BallTracker::BallTracker() : @@ -44,8 +48,19 @@ BallTracker::BallTracker() : } std::optional BallTracker::estimateBallState( - const std::vector &new_ball_detections, const Rectangle &filter_area, const Timestamp& current_time) + const std::vector &new_ball_detections, const Rectangle &filter_area, + const Timestamp& current_time, std::optional dribbling_robot) { + Point dribbler_pos; + if (dribbling_robot.has_value()) + { + dribbler_pos = + dribbling_robot->position() + + Vector::createFromAngle(dribbling_robot->orientation()) + .normalize(DIST_TO_FRONT_OF_ROBOT_METERS + + BALL_TO_FRONT_OF_ROBOT_DISTANCE_WHEN_DRIBBLING); + } + std::optional best_ball_detection = getBestBallDetection(new_ball_detections); if (prev_detection_timestamp){ @@ -54,7 +69,32 @@ std::optional BallTracker::estimateBallState( prev_detection_timestamp = current_time; kalman_filter.predict(delta_t); } - if (best_ball_detection){ + + if (dribbling_robot.has_value()) + { + // When dribbling, only trust a vision measurement if it shows the ball + // close to the dribbler. Otherwise reset to the kinematic dribbler position. + bool near_dribbler = + best_ball_detection.has_value() && + (best_ball_detection->position - dribbler_pos).length() <= + DRIBBLING_MEASUREMENT_MAX_DISTANCE_METERS; + + Eigen::Matrix measurement; + if (near_dribbler) + { + measurement << best_ball_detection->position.x(), + best_ball_detection->position.y(); + kalman_filter.update(measurement); + } + else + { + measurement << dribbler_pos.x(), dribbler_pos.y(); + kalman_filter.reset(measurement); + } + prev_detection_timestamp = current_time; + consecutive_outliers = 0; + } + else if (best_ball_detection){ prev_detection_timestamp = current_time; Eigen::Matrix measurement; measurement << best_ball_detection->position.x(), best_ball_detection->position.y(); diff --git a/src/software/sensor_fusion/filter/ball_tracker.h b/src/software/sensor_fusion/filter/ball_tracker.h index ad3c6a6fd6..7e05490652 100644 --- a/src/software/sensor_fusion/filter/ball_tracker.h +++ b/src/software/sensor_fusion/filter/ball_tracker.h @@ -9,6 +9,7 @@ #include "software/sensor_fusion/filter/vision_detection.h" #include "software/time/timestamp.h" #include "software/world/ball.h" +#include "software/world/robot.h" #include "software/sensor_fusion/filter/kalman_filter.h" /** @@ -46,7 +47,8 @@ class BallTracker std::optional estimateBallState( const std::vector& new_ball_detections, const Rectangle& filter_area, - const Timestamp& current_time); + const Timestamp& current_time, + std::optional dribbling_robot = std::nullopt); private: /** diff --git a/src/software/sensor_fusion/sensor_fusion.cpp b/src/software/sensor_fusion/sensor_fusion.cpp index fc88553132..7a4e571786 100644 --- a/src/software/sensor_fusion/sensor_fusion.cpp +++ b/src/software/sensor_fusion/sensor_fusion.cpp @@ -288,19 +288,7 @@ void SensorFusion::updateWorld(const SSLProto::SSL_DetectionFrame& ssl_detection std::optional robot_with_ball_in_dribbler = friendly_team.getRobotById(friendly_robot_id_with_ball_in_dribbler.value()); - std::vector dribbler_in_ball_detection = {BallDetection{ - .position = - robot_with_ball_in_dribbler->position() + - Vector::createFromAngle(robot_with_ball_in_dribbler->orientation()) - .normalize(DIST_TO_FRONT_OF_ROBOT_METERS + - BALL_TO_FRONT_OF_ROBOT_DISTANCE_WHEN_DRIBBLING), - .distance_from_ground = 0, - .timestamp = Timestamp::fromSeconds(ssl_detection_frame.t_capture()), - .confidence = 1}}; - - - - std::optional new_ball = createBall(dribbler_in_ball_detection, Timestamp::fromSeconds(ssl_detection_frame.t_capture())); + std::optional new_ball = createBall({}, Timestamp::fromSeconds(ssl_detection_frame.t_capture()), robot_with_ball_in_dribbler); if (new_ball) { @@ -351,12 +339,14 @@ void SensorFusion::updateBall(Ball new_ball) } std::optional SensorFusion::createBall( - const std::vector &ball_detections, const Timestamp& current_time) + const std::vector &ball_detections, const Timestamp& current_time, + std::optional dribbling_robot) { if (field) { std::optional new_ball = - ball_tracker.estimateBallState(ball_detections, field.value().fieldBoundary(), current_time); + ball_tracker.estimateBallState(ball_detections, field.value().fieldBoundary(), + current_time, dribbling_robot); return new_ball; } return std::nullopt; diff --git a/src/software/sensor_fusion/sensor_fusion.h b/src/software/sensor_fusion/sensor_fusion.h index 5cb838eb2a..d6c90169ef 100644 --- a/src/software/sensor_fusion/sensor_fusion.h +++ b/src/software/sensor_fusion/sensor_fusion.h @@ -95,7 +95,9 @@ class SensorFusion * * @return Ball if filtered from ball detections */ - std::optional createBall(const std::vector &ball_detections, const Timestamp& current_time); + std::optional createBall(const std::vector &ball_detections, + const Timestamp& current_time, + std::optional dribbling_robot = std::nullopt); /** * Create team from a list of robot detections diff --git a/src/software/simulated_tests/validation/BUILD b/src/software/simulated_tests/validation/BUILD index ec530a107c..ed0573b5a4 100644 --- a/src/software/simulated_tests/validation/BUILD +++ b/src/software/simulated_tests/validation/BUILD @@ -5,13 +5,10 @@ py_library( srcs = [ "avoid_collisions.py", "ball_enters_region.py", - "ball_enters_region_true.py", "ball_is_off_ground.py", "ball_moves_in_direction.py", "ball_speed_threshold.py", - "ball_speed_threshold_true.py", "ball_stops_in_region.py", - "ball_stops_in_region_true.py", "excessive_dribbling.py", "friendly_has_ball_possession.py", "friendly_receives_ball_slow.py", diff --git a/src/software/simulated_tests/validation/ball_enters_region_true.py b/src/software/simulated_tests/validation/ball_enters_region_true.py deleted file mode 100644 index c18cfb58f0..0000000000 --- a/src/software/simulated_tests/validation/ball_enters_region_true.py +++ /dev/null @@ -1,61 +0,0 @@ -import software.python_bindings as tbots_cpp -from proto.import_all_protos import * - -from software.simulated_tests.validation.validation import ( - Validation, - create_validation_geometry, - create_validation_types, -) -from typing import override - - -class BallEntersRegionTrue(Validation): - """Checks if the ball's true (simulator) position enters any of the provided regions.""" - - def __init__(self, regions=None): - self.regions = regions if regions else [] - self.ball_position = None - - @override - def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: - """Checks if the ball's true position enters the provided regions. - - Falls back to the sensor-fused world position when simulator_state is unavailable. - - :param world: The world msg (used as fallback) - :param simulator_state: The simulator state with true ball position - :return: FAILING until the ball enters any region, PASSING when it does - """ - if simulator_state is not None and simulator_state.HasField("ball"): - self.ball_position = tbots_cpp.Point( - simulator_state.ball.p_x, simulator_state.ball.p_y - ) - else: - self.ball_position = tbots_cpp.createPoint( - world.ball.current_state.global_position - ) - - for region in self.regions: - if tbots_cpp.contains(region, self.ball_position): - return ValidationStatus.PASSING - return ValidationStatus.FAILING - - @override - def get_validation_geometry(self, world) -> ValidationGeometry: - return create_validation_geometry(self.regions) - - @override - def __repr__(self): - return ( - "Checking true ball position in regions " - + ",".join(repr(region) for region in self.regions) - + (", ball position: " + str(self.ball_position) if self.ball_position else "") - ) - - -( - BallEventuallyEntersRegionTrue, - BallEventuallyExitsRegionTrue, - BallAlwaysStaysInRegionTrue, - BallNeverEntersRegionTrue, -) = create_validation_types(BallEntersRegionTrue) diff --git a/src/software/simulated_tests/validation/ball_speed_threshold_true.py b/src/software/simulated_tests/validation/ball_speed_threshold_true.py deleted file mode 100644 index 14718f8b79..0000000000 --- a/src/software/simulated_tests/validation/ball_speed_threshold_true.py +++ /dev/null @@ -1,63 +0,0 @@ -import software.python_bindings as tbots_cpp -from proto.import_all_protos import * - -from software.simulated_tests.validation.validation import ( - Validation, - create_validation_geometry, - create_validation_types, -) -from typing import override - - -class BallSpeedThresholdTrue(Validation): - """Checks if the ball's true (simulator) speed is at or above some threshold.""" - - def __init__(self, speed_threshold): - """Constructor - - :param speed_threshold: The speed threshold in m/s - """ - self.speed_threshold = speed_threshold - - @override - def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: - """Checks if the ball's true speed is at or above the threshold. - - Falls back to sensor-fused velocity when simulator_state is unavailable. - - :param world: The world msg (used as fallback) - :param simulator_state: The simulator state with true ball velocity - :return: FAILING if below threshold, PASSING if at or above - """ - if simulator_state is not None and simulator_state.HasField("ball"): - speed = tbots_cpp.Vector( - simulator_state.ball.v_x, simulator_state.ball.v_y - ).length() - else: - speed = tbots_cpp.createVector( - world.ball.current_state.global_velocity - ).length() - - if speed >= self.speed_threshold: - return ValidationStatus.PASSING - - return ValidationStatus.FAILING - - @override - def get_validation_geometry(self, world) -> ValidationGeometry: - return create_validation_geometry([]) - - @override - def __repr__(self): - return ( - "Check that the true ball speed is at or above " - + str(self.speed_threshold) - ) - - -( - BallSpeedEventuallyAtOrAboveThresholdTrue, - BallSpeedEventuallyBelowThresholdTrue, - BallSpeedAlwaysAtOrAboveThresholdTrue, - BallSpeedAlwaysBelowThresholdTrue, -) = create_validation_types(BallSpeedThresholdTrue) diff --git a/src/software/simulated_tests/validation/ball_stops_in_region_true.py b/src/software/simulated_tests/validation/ball_stops_in_region_true.py deleted file mode 100644 index d805f1d875..0000000000 --- a/src/software/simulated_tests/validation/ball_stops_in_region_true.py +++ /dev/null @@ -1,63 +0,0 @@ -import software.python_bindings as tbots_cpp -from proto.import_all_protos import * - -from software.simulated_tests.validation.validation import ( - Validation, - create_validation_geometry, - create_validation_types, -) -from typing import override - - -class BallStopsInRegionTrue(Validation): - """Checks if the ball's true (simulator) position stops in any of the provided regions.""" - - def __init__(self, regions=None): - self.regions = regions if regions else [] - - @override - def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: - """Checks if the ball's true position stops in the provided regions. - - Falls back to the sensor-fused world position when simulator_state is unavailable. - - :param world: The world msg (used as fallback and for velocity) - :param simulator_state: The simulator state with true ball position/velocity - :return: FAILING until the ball stops in any region, PASSING when it does - """ - if simulator_state is not None and simulator_state.HasField("ball"): - ball_point = tbots_cpp.Point( - simulator_state.ball.p_x, simulator_state.ball.p_y - ) - ball_speed = tbots_cpp.Vector( - simulator_state.ball.v_x, simulator_state.ball.v_y - ).length() - else: - ball_point = tbots_cpp.createPoint(world.ball.current_state.global_position) - ball_speed = tbots_cpp.createVector( - world.ball.current_state.global_velocity - ).length() - - for region in self.regions: - if tbots_cpp.contains(region, ball_point) and ball_speed <= 0.01: - return ValidationStatus.PASSING - - return ValidationStatus.FAILING - - @override - def get_validation_geometry(self, world) -> ValidationGeometry: - return create_validation_geometry(self.regions) - - @override - def __repr__(self): - return "Checking true ball position stops in regions " + ",".join( - repr(region) for region in self.regions - ) - - -( - BallEventuallyStopsInRegionTrue, - BallEventuallyMovesInRegionTrue, - BallAlwaysStopsInRegionTrue, - BallNeverStopsInRegionTrue, -) = create_validation_types(BallStopsInRegionTrue) From 73d908b68acae7ed6f964f37a7cd61c4d01676bd Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Wed, 13 May 2026 22:28:16 -0700 Subject: [PATCH 27/57] fixed tests --- src/software/ai/hl/stp/tactic/goalie/goalie_fsm.h | 7 ++++--- src/software/sensor_fusion/filter/ball_tracker.cpp | 4 ++++ 2 files changed, 8 insertions(+), 3 deletions(-) diff --git a/src/software/ai/hl/stp/tactic/goalie/goalie_fsm.h b/src/software/ai/hl/stp/tactic/goalie/goalie_fsm.h index 9e5a1ef6de..2d0f320ffb 100644 --- a/src/software/ai/hl/stp/tactic/goalie/goalie_fsm.h +++ b/src/software/ai/hl/stp/tactic/goalie/goalie_fsm.h @@ -233,9 +233,10 @@ struct GoalieFSM : TacticFSM Panic_S + Update_E / panic_A, PivotKickFSM_S + Update_E[shouldMoveToGoalLine_G] / moveToGoalLine_A = MoveToGoalLine_S, - PivotKickFSM_S + Update_E[ballInInflatedDefenseArea_G] / updatePivotKick_A, - PivotKickFSM_S + Update_E[!ballInInflatedDefenseArea_G] / positionToBlock_A = - PositionToBlock_S, + PivotKickFSM_S + + Update_E[ballInInflatedDefenseArea_G && + (shouldPivotChip_G || shouldPanic_G)] / updatePivotKick_A, + PivotKickFSM_S + Update_E / positionToBlock_A = PositionToBlock_S, MoveToGoalLine_S + Update_E[shouldMoveToGoalLine_G] / moveToGoalLine_A = MoveToGoalLine_S, MoveToGoalLine_S + Update_E[!shouldMoveToGoalLine_G] / positionToBlock_A = diff --git a/src/software/sensor_fusion/filter/ball_tracker.cpp b/src/software/sensor_fusion/filter/ball_tracker.cpp index 6776977f37..f262c158de 100644 --- a/src/software/sensor_fusion/filter/ball_tracker.cpp +++ b/src/software/sensor_fusion/filter/ball_tracker.cpp @@ -113,6 +113,10 @@ std::optional BallTracker::estimateBallState( } + if (!prev_detection_timestamp && !dribbling_robot.has_value()){ + return std::nullopt; + } + Eigen::Matrix kalman_state = kalman_filter.getState(); Point ball_position = Point(kalman_state(0), kalman_state(1)); Vector ball_velocity = Vector(kalman_state(2), kalman_state(3)); From 4794b727ea1b4a1fed79ab5af7a88cc9f819ff28 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Wed, 13 May 2026 22:46:43 -0700 Subject: [PATCH 28/57] change implementation and interface to fit higher-level-mp --- src/software/er_force_simulator_main.cpp | 4 +- .../sensor_fusion/filter/ball_tracker.cpp | 61 +++++--- .../sensor_fusion/filter/ball_tracker.h | 2 +- .../sensor_fusion/filter/kalman_filter.cpp | 106 +++++++------ .../sensor_fusion/filter/kalman_filter.h | 141 ++++++++---------- 5 files changed, 160 insertions(+), 154 deletions(-) diff --git a/src/software/er_force_simulator_main.cpp b/src/software/er_force_simulator_main.cpp index f578eaa9a4..0abb916e0a 100644 --- a/src/software/er_force_simulator_main.cpp +++ b/src/software/er_force_simulator_main.cpp @@ -48,7 +48,7 @@ int main(int argc, char** argv) { std::string runtime_dir = args.runtime_dir; LoggerSingleton::initializeLogger(runtime_dir, nullptr); - LOG(CSV, "filter_data_10_perfect.csv") << "timestamp_s,fused_x,fused_y,fused_vel_x, fused_vel_y, truth_x,truth_y, true_vel_x, true_vel_y,is_occluded\n"; + LOG(CSV, "realism_kalman_filter_v1.csv") << "timestamp_s,fused_x,fused_y,fused_vel_x, fused_vel_y, truth_x,truth_y, true_vel_x, true_vel_y,is_occluded\n"; /** * Creates a ER force simulator and sets up the appropriate @@ -228,7 +228,7 @@ int main(int argc, char** argv) { start_timestamp_s = current_ts; } - LOG(CSV, "filter_data_10_perfect.csv") + LOG(CSV, "realism_kalman_filter_v1.csv") << (current_ts - start_timestamp_s) << "," << yellow_vision.ball().current_state().global_position().x_meters() << "," diff --git a/src/software/sensor_fusion/filter/ball_tracker.cpp b/src/software/sensor_fusion/filter/ball_tracker.cpp index f262c158de..33e75ddb2c 100644 --- a/src/software/sensor_fusion/filter/ball_tracker.cpp +++ b/src/software/sensor_fusion/filter/ball_tracker.cpp @@ -12,7 +12,7 @@ #include "software/sensor_fusion/filter/kalman_filter.h" namespace { - const Eigen::Matrix INITIAL_STATE = Eigen::Matrix::Zero(); + const Eigen::Vector INITIAL_STATE = Eigen::Vector::Zero(); const Eigen::Matrix INITIAL_COV = Eigen::Matrix::Identity() * 1000.0; @@ -33,8 +33,8 @@ namespace { // Empirically measured const double DAMPING = 0.9889; - const double MAHANALOGIS_THRESHOLD=1; - const int CONSECUTIVE_OUTLIERS_THRESHOLD=3; + const double MAHANALOGIS_THRESHOLD = 1; + const int CONSECUTIVE_OUTLIERS_THRESHOLD = 3; // Max distance from the dribbler position for a vision detection to be trusted // while a robot is dribbling. Detections farther away are likely spurious. @@ -43,7 +43,11 @@ namespace { BallTracker::BallTracker() : consecutive_outliers(0), - kalman_filter(INITIAL_STATE, INITIAL_COV, Q, R, C, DAMPING) + kalman_filter(INITIAL_STATE, INITIAL_COV, + Eigen::Matrix::Identity(), + Q, + Eigen::Matrix::Zero(), + C, R) { } @@ -64,10 +68,16 @@ std::optional BallTracker::estimateBallState( std::optional best_ball_detection = getBestBallDetection(new_ball_detections); if (prev_detection_timestamp){ - double delta_t; - delta_t = (current_time - *prev_detection_timestamp).toSeconds(); + double delta_t = (current_time - *prev_detection_timestamp).toSeconds(); prev_detection_timestamp = current_time; - kalman_filter.predict(delta_t); + + Eigen::Matrix A; + A << 1, 0, delta_t, 0, + 0, 1, 0, delta_t, + 0, 0, DAMPING, 0, + 0, 0, 0, DAMPING; + kalman_filter.process_model = A; + kalman_filter.predict(Eigen::Vector::Zero()); } if (dribbling_robot.has_value()) @@ -79,7 +89,7 @@ std::optional BallTracker::estimateBallState( (best_ball_detection->position - dribbler_pos).length() <= DRIBBLING_MEASUREMENT_MAX_DISTANCE_METERS; - Eigen::Matrix measurement; + Eigen::Vector measurement; if (near_dribbler) { measurement << best_ball_detection->position.x(), @@ -89,43 +99,50 @@ std::optional BallTracker::estimateBallState( else { measurement << dribbler_pos.x(), dribbler_pos.y(); - kalman_filter.reset(measurement); + kalman_filter.state_estimate << measurement(0), measurement(1), 0, 0; + kalman_filter.state_covariance = INITIAL_COV; } prev_detection_timestamp = current_time; consecutive_outliers = 0; } else if (best_ball_detection){ prev_detection_timestamp = current_time; - Eigen::Matrix measurement; + Eigen::Vector measurement; measurement << best_ball_detection->position.x(), best_ball_detection->position.y(); - double mahalanobis = kalman_filter.getMahalanobisDistance(measurement); - if (mahalanobis< MAHANALOGIS_THRESHOLD){ + + const Eigen::Vector innovation = + measurement - kalman_filter.measurement_model * kalman_filter.state_estimate; + const Eigen::Matrix S = + kalman_filter.measurement_model * kalman_filter.state_covariance * + kalman_filter.measurement_model.transpose() + kalman_filter.measurement_covariance; + double mahalanobis = (innovation.transpose() * S.inverse() * innovation)(0, 0); + + if (mahalanobis < MAHANALOGIS_THRESHOLD){ kalman_filter.update(measurement); } else{ consecutive_outliers++; } - if (consecutive_outliers> CONSECUTIVE_OUTLIERS_THRESHOLD){ - kalman_filter.reset(measurement); - consecutive_outliers=0; + if (consecutive_outliers > CONSECUTIVE_OUTLIERS_THRESHOLD){ + kalman_filter.state_estimate << measurement(0), measurement(1), 0, 0; + kalman_filter.state_covariance = INITIAL_COV; + consecutive_outliers = 0; } } - if (!prev_detection_timestamp && !dribbling_robot.has_value()){ return std::nullopt; } - Eigen::Matrix kalman_state = kalman_filter.getState(); - Point ball_position = Point(kalman_state(0), kalman_state(1)); - Vector ball_velocity = Vector(kalman_state(2), kalman_state(3)); - double z_height = best_ball_detection ? best_ball_detection->distance_from_ground : 0.0; + Point ball_position = Point(kalman_filter.state_estimate(0), kalman_filter.state_estimate(1)); + Vector ball_velocity = Vector(kalman_filter.state_estimate(2), kalman_filter.state_estimate(3)); + double z_height = best_ball_detection ? best_ball_detection->distance_from_ground : 0.0; BallState ball_state(ball_position, ball_velocity, z_height); - return Ball(ball_state,current_time); - + return Ball(ball_state, current_time); } + std::optional BallTracker::getBestBallDetection(const std::vector &new_ball_detections){ if (new_ball_detections.empty()){ return std::nullopt; diff --git a/src/software/sensor_fusion/filter/ball_tracker.h b/src/software/sensor_fusion/filter/ball_tracker.h index 7e05490652..532e16df64 100644 --- a/src/software/sensor_fusion/filter/ball_tracker.h +++ b/src/software/sensor_fusion/filter/ball_tracker.h @@ -61,6 +61,6 @@ class BallTracker */ std::optional getBestBallDetection(const std::vector &new_ball_detections); int consecutive_outliers; - KalmanFilter kalman_filter; + KalmanFilter<4, 2, 1> kalman_filter; std::optional prev_detection_timestamp; }; diff --git a/src/software/sensor_fusion/filter/kalman_filter.cpp b/src/software/sensor_fusion/filter/kalman_filter.cpp index ddd5cc1c47..33d8b4f4e1 100644 --- a/src/software/sensor_fusion/filter/kalman_filter.cpp +++ b/src/software/sensor_fusion/filter/kalman_filter.cpp @@ -1,63 +1,71 @@ #include "software/sensor_fusion/filter/kalman_filter.h" -KalmanFilter::KalmanFilter( -const Eigen::Matrix& X, -const Eigen::Matrix& P_i, -const Eigen::Matrix & Q, -const Eigen::Matrix& R, -const Eigen::Matrix& C, -double damping_term - ): -X(X), -P(P_i), -P_i(P_i), -Q(Q), -R(R), -C(C), -damping_term(damping_term) +template +KalmanFilter::KalmanFilter() + : state_estimate(Eigen::Vector::Zero()), + state_covariance(Eigen::Matrix::Zero()), + process_model(Eigen::Matrix::Zero()), + process_covariance(Eigen::Matrix::Zero()), + control_model(Eigen::Matrix::Zero()), + measurement_model(Eigen::Matrix::Zero()), + measurement_covariance(Eigen::Matrix::Zero()) { } -void KalmanFilter::predict(const double delta_t){ - - Eigen::Matrix A; - - // Using the constant velocity model as motion model - A << 1, 0, delta_t, 0, - 0, 1, 0, delta_t, - 0, 0, damping_term, 0, - 0, 0, 0, damping_term; - - X = A*X; - - P = A*P*A.transpose() + Q; +template +KalmanFilter::KalmanFilter( + Eigen::Vector initial_state, + Eigen::Matrix initial_state_covariance, + Eigen::Matrix initial_process_model, + Eigen::Matrix initial_process_covariance, + Eigen::Matrix initial_control_model, + Eigen::Matrix initial_measurement_model, + Eigen::Matrix initial_measurement_covariance) + : state_estimate(initial_state), + state_covariance(initial_state_covariance), + process_model(initial_process_model), + process_covariance(initial_process_covariance), + control_model(initial_control_model), + measurement_model(initial_measurement_model), + measurement_covariance(initial_measurement_covariance) +{ } -void KalmanFilter::update(const Eigen::Matrix Z){ - Eigen::Matrix Kg; - Eigen::Matrix S =C*P*C.transpose()+R; - Kg = P*C.transpose() * S.inverse(); - X = X + Kg*(Z-C*X); - P = (Eigen::Matrix::Identity()-Kg*C)*P; +template +void KalmanFilter::predict(Eigen::Vector control_input) +{ + state_estimate = process_model * state_estimate + control_model * control_input; + state_covariance = + process_model * state_covariance * process_model.transpose() + process_covariance; } -void KalmanFilter::reset(const Eigen::Matrix Z){ - X << Z(0), Z(1), 0, 0; - P = P_i; -} +template +void KalmanFilter::update(Eigen::Vector measurement) +{ + const Eigen::Vector innovation = + measurement - measurement_model * state_estimate; -double KalmanFilter::getMahalanobisDistance(const Eigen::Matrix& Z) const { - Eigen::Matrix S =C*P*C.transpose()+R ; - // Calculate the mahalanobis distance for gating - double M = ((Z - C*X).transpose() * S.inverse() * (Z-C*X))(0,0); - return M; -} + const Eigen::Matrix innovation_covariance = + measurement_model * state_covariance * measurement_model.transpose() + + measurement_covariance; + const Eigen::Matrix regularized_innovation_covariance = + innovation_covariance.unaryExpr( + [](double value) { return (std::abs(value) < 1.0e-20) ? 0.0 : value; }); -Eigen::Matrix KalmanFilter::getState(){ - return X; -} + const Eigen::Matrix kalman_gain = + state_covariance * + (measurement_model.transpose() * + regularized_innovation_covariance.completeOrthogonalDecomposition() + .pseudoInverse()); + + state_estimate = state_estimate + kalman_gain * innovation; -Eigen::Matrix KalmanFilter::getCovariance(){ - return P; + // Joseph form — more numerically stable than P = (I - K*H) * P + const Eigen::Matrix factor = + Eigen::Matrix::Identity() - kalman_gain * measurement_model; + state_covariance = factor * state_covariance * factor.transpose() + + kalman_gain * measurement_covariance * kalman_gain.transpose(); } +// Explicit instantiation for the ball tracker +template class KalmanFilter<4, 2, 1>; diff --git a/src/software/sensor_fusion/filter/kalman_filter.h b/src/software/sensor_fusion/filter/kalman_filter.h index e0eb365043..566faf042b 100644 --- a/src/software/sensor_fusion/filter/kalman_filter.h +++ b/src/software/sensor_fusion/filter/kalman_filter.h @@ -1,95 +1,76 @@ #pragma once #include +#include /** - * A Kalman filter for tracking a 2D object using a constant velocity motion model. + * Linear Kalman filter for discrete-time state estimation. * - * The state vector is [x, y, vx, vy]^T. Measurements are 2D positions [x, y]^T. - * The filter supports predict/update steps as well as Mahalanobis-distance-based - * gating and a hard reset when the tracked object jumps unexpectedly. - */ -class KalmanFilter{ - - -public: - -/** - * Creates a new KalmanFilter + * A Kalman filter combines a process model (how the state evolves over time) + * with noisy measurements (what sensors report) to produce an optimal estimate + * of the system state over time (assuming system model and noise are Gaussian). * - * @param X The initial state vector [x, y, vx, vy]^T - * @param P_i The initial state covariance matrix (4x4) - * @param Q The process noise covariance matrix (4x4), representing uncertainty - * introduced by the motion model at each prediction step - * @param R The measurement noise covariance matrix (2x2), representing - * uncertainty in position measurements - * @param C The measurement matrix that maps the state space to the measurement - * space (2x4) - * @param damping_term A scalar in [0, 1] applied to the velocity states each - * predict step to model drag/friction - */ -KalmanFilter( - const Eigen::Matrix& X, - const Eigen::Matrix& P_i, - const Eigen::Matrix& Q, - const Eigen::Matrix& R, - const Eigen::Matrix& C, - double damping_term - ); - -/** - * Propagates the state estimate forward in time using the constant-velocity - * motion model and adds process noise to the covariance. + * It alternates between two steps: + * 1) predict: propagate state/covariance forward through the process model + * 2) update: correct that prediction with a new measurement * - * @param delta_t Time elapsed since the last predict step, in seconds - */ -void predict(const double delta_t); - -/** - * Corrects the state estimate using a new position measurement. + * Resources: + * - https://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/ + * - https://kalmanfilter.net/ + * - https://github.com/rlabbe/Kalman-and-Bayesian-Filters-in-Python + * - https://web.mit.edu/kirtley/kirtley/binlustuff/literature/control/Kalman%20filter.pdf * - * @param Z The measurement vector [x, y]^T + * @tparam DimX The dimension of the state + * @tparam DimY The dimension of measurement space + * @tparam DimU The dimension of control space */ -void update(const Eigen::Matrix Z); +template +class KalmanFilter +{ + public: + /** + * Creates a Kalman filter with all internal matrices and vectors set to zero. + */ + KalmanFilter(); -/** - * Resets the filter to the given measurement, zeroing velocities and - * restoring the initial covariance. - * - * @param Z The measurement vector [x, y]^T to reset the state to - */ -void reset(const Eigen::Matrix Z); - -/** - * Returns the squared Mahalanobis distance between the given measurement and - * the current predicted measurement. Used for gating outlier detections. - * - * @param Z The measurement vector [x, y]^T to evaluate - * @return The squared Mahalanobis distance - */ -double getMahalanobisDistance(const Eigen::Matrix& Z) const; - -/** - * Returns the current state estimate vector [x, y, vx, vy]^T. - * - * @return The current state vector (4x1) - */ -Eigen::Matrix getState(); + /** + * Creates a Kalman filter with the given initial state and model parameters. + * + * @param initial_state Initial state estimate (x) + * @param initial_state_covariance Initial state covariance (P) + * @param initial_process_model Initial process/state transition model (F) + * @param initial_process_covariance Initial process noise covariance (Q) + * @param initial_control_model Initial control-to-state transformation (B) + * @param initial_measurement_model Initial state-to-measurement transformation (H) + * @param initial_measurement_covariance Initial measurement noise covariance (R) + */ + KalmanFilter(Eigen::Vector initial_state, + Eigen::Matrix initial_state_covariance, + Eigen::Matrix initial_process_model, + Eigen::Matrix initial_process_covariance, + Eigen::Matrix initial_control_model, + Eigen::Matrix initial_measurement_model, + Eigen::Matrix initial_measurement_covariance); -/** - * Returns the current state covariance matrix. - * - * @return The current covariance matrix (4x4) - */ -Eigen::Matrix getCovariance(); + /** + * Predict the next state estimate. + * + * @param control_input Control input vector + */ + void predict(Eigen::Vector control_input); -private: -Eigen::Matrix X; // State -Eigen::Matrix P; // State Covariance -Eigen::Matrix P_i; // Initial state covariance -Eigen::Matrix Q; // Process noise covariance -Eigen::Matrix R; // Measurement noise covariance -Eigen::Matrix C; // State to measurement matrix -double damping_term; + /** + * Correct the current state estimate with the given measurement. + * + * @param measurement Measurement vector + */ + void update(Eigen::Vector measurement); + Eigen::Vector state_estimate; + Eigen::Matrix state_covariance; + Eigen::Matrix process_model; + Eigen::Matrix process_covariance; + Eigen::Matrix control_model; + Eigen::Matrix measurement_model; + Eigen::Matrix measurement_covariance; }; From df3b4a77e113837ee80faab190875f6c724468c5 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Wed, 13 May 2026 23:15:47 -0700 Subject: [PATCH 29/57] add velocity initialization --- src/software/er_force_simulator_main.cpp | 4 ++-- .../sensor_fusion/filter/ball_tracker.cpp | 24 +++++++++++++++---- .../sensor_fusion/filter/ball_tracker.h | 2 ++ 3 files changed, 24 insertions(+), 6 deletions(-) diff --git a/src/software/er_force_simulator_main.cpp b/src/software/er_force_simulator_main.cpp index 0abb916e0a..3fe057a513 100644 --- a/src/software/er_force_simulator_main.cpp +++ b/src/software/er_force_simulator_main.cpp @@ -48,7 +48,7 @@ int main(int argc, char** argv) { std::string runtime_dir = args.runtime_dir; LoggerSingleton::initializeLogger(runtime_dir, nullptr); - LOG(CSV, "realism_kalman_filter_v1.csv") << "timestamp_s,fused_x,fused_y,fused_vel_x, fused_vel_y, truth_x,truth_y, true_vel_x, true_vel_y,is_occluded\n"; + LOG(CSV, "realism_kalman_filter_v2.csv") << "timestamp_s,fused_x,fused_y,fused_vel_x, fused_vel_y, truth_x,truth_y, true_vel_x, true_vel_y,is_occluded\n"; /** * Creates a ER force simulator and sets up the appropriate @@ -228,7 +228,7 @@ int main(int argc, char** argv) { start_timestamp_s = current_ts; } - LOG(CSV, "realism_kalman_filter_v1.csv") + LOG(CSV, "realism_kalman_filter_v2.csv") << (current_ts - start_timestamp_s) << "," << yellow_vision.ball().current_state().global_position().x_meters() << "," diff --git a/src/software/sensor_fusion/filter/ball_tracker.cpp b/src/software/sensor_fusion/filter/ball_tracker.cpp index 33e75ddb2c..251e8b8150 100644 --- a/src/software/sensor_fusion/filter/ball_tracker.cpp +++ b/src/software/sensor_fusion/filter/ball_tracker.cpp @@ -67,13 +67,14 @@ std::optional BallTracker::estimateBallState( std::optional best_ball_detection = getBestBallDetection(new_ball_detections); + double dt = 0.0; if (prev_detection_timestamp){ - double delta_t = (current_time - *prev_detection_timestamp).toSeconds(); + dt = (current_time - *prev_detection_timestamp).toSeconds(); prev_detection_timestamp = current_time; Eigen::Matrix A; - A << 1, 0, delta_t, 0, - 0, 1, 0, delta_t, + A << 1, 0, dt, 0, + 0, 1, 0, dt, 0, 0, DAMPING, 0, 0, 0, 0, DAMPING; kalman_filter.process_model = A; @@ -106,10 +107,25 @@ std::optional BallTracker::estimateBallState( consecutive_outliers = 0; } else if (best_ball_detection){ - prev_detection_timestamp = current_time; Eigen::Vector measurement; measurement << best_ball_detection->position.x(), best_ball_detection->position.y(); + if (!prev_detection_timestamp){ + // First detection: store position to compute velocity on the next frame + initial_detection_pos_ = measurement; + prev_detection_timestamp = current_time; + return std::nullopt; + } + + // Second detection: seed Kalman velocity from finite-difference + if (initial_detection_pos_.has_value() && dt > 0){ + double vx = (measurement(0) - (*initial_detection_pos_)(0)) / dt; + double vy = (measurement(1) - (*initial_detection_pos_)(1)) / dt; + kalman_filter.state_estimate << measurement(0), measurement(1), vx, vy; + kalman_filter.state_covariance = INITIAL_COV; + initial_detection_pos_ = std::nullopt; + } + const Eigen::Vector innovation = measurement - kalman_filter.measurement_model * kalman_filter.state_estimate; const Eigen::Matrix S = diff --git a/src/software/sensor_fusion/filter/ball_tracker.h b/src/software/sensor_fusion/filter/ball_tracker.h index 532e16df64..79a767f3f2 100644 --- a/src/software/sensor_fusion/filter/ball_tracker.h +++ b/src/software/sensor_fusion/filter/ball_tracker.h @@ -63,4 +63,6 @@ class BallTracker int consecutive_outliers; KalmanFilter<4, 2, 1> kalman_filter; std::optional prev_detection_timestamp; + // Position from the first detection — used to seed velocity on the second detection + std::optional> initial_detection_pos_; }; From e9ed81d5fe5d90aafa2906c8a1fad60cedda9304 Mon Sep 17 00:00:00 2001 From: StarrryNight Date: Thu, 14 May 2026 00:29:55 -0700 Subject: [PATCH 30/57] remove bad heuristics --- src/software/er_force_simulator_main.cpp | 4 +- .../sensor_fusion/filter/ball_tracker.cpp | 61 ++----------------- .../sensor_fusion/filter/ball_tracker.h | 6 +- src/software/sensor_fusion/sensor_fusion.cpp | 10 +-- src/software/sensor_fusion/sensor_fusion.h | 3 +- 5 files changed, 11 insertions(+), 73 deletions(-) diff --git a/src/software/er_force_simulator_main.cpp b/src/software/er_force_simulator_main.cpp index 3fe057a513..862ab1637a 100644 --- a/src/software/er_force_simulator_main.cpp +++ b/src/software/er_force_simulator_main.cpp @@ -48,7 +48,7 @@ int main(int argc, char** argv) { std::string runtime_dir = args.runtime_dir; LoggerSingleton::initializeLogger(runtime_dir, nullptr); - LOG(CSV, "realism_kalman_filter_v2.csv") << "timestamp_s,fused_x,fused_y,fused_vel_x, fused_vel_y, truth_x,truth_y, true_vel_x, true_vel_y,is_occluded\n"; + LOG(CSV, "realism_kalman_filter_v4.csv") << "timestamp_s,fused_x,fused_y,fused_vel_x, fused_vel_y, truth_x,truth_y, true_vel_x, true_vel_y,is_occluded\n"; /** * Creates a ER force simulator and sets up the appropriate @@ -228,7 +228,7 @@ int main(int argc, char** argv) { start_timestamp_s = current_ts; } - LOG(CSV, "realism_kalman_filter_v2.csv") + LOG(CSV, "realism_kalman_filter_v4.csv") << (current_ts - start_timestamp_s) << "," << yellow_vision.ball().current_state().global_position().x_meters() << "," diff --git a/src/software/sensor_fusion/filter/ball_tracker.cpp b/src/software/sensor_fusion/filter/ball_tracker.cpp index 251e8b8150..223adaf80d 100644 --- a/src/software/sensor_fusion/filter/ball_tracker.cpp +++ b/src/software/sensor_fusion/filter/ball_tracker.cpp @@ -36,9 +36,6 @@ namespace { const double MAHANALOGIS_THRESHOLD = 1; const int CONSECUTIVE_OUTLIERS_THRESHOLD = 3; - // Max distance from the dribbler position for a vision detection to be trusted - // while a robot is dribbling. Detections farther away are likely spurious. - const double DRIBBLING_MEASUREMENT_MAX_DISTANCE_METERS = 0.15; } BallTracker::BallTracker() : @@ -53,18 +50,8 @@ BallTracker::BallTracker() : std::optional BallTracker::estimateBallState( const std::vector &new_ball_detections, const Rectangle &filter_area, - const Timestamp& current_time, std::optional dribbling_robot) + const Timestamp& current_time) { - Point dribbler_pos; - if (dribbling_robot.has_value()) - { - dribbler_pos = - dribbling_robot->position() + - Vector::createFromAngle(dribbling_robot->orientation()) - .normalize(DIST_TO_FRONT_OF_ROBOT_METERS + - BALL_TO_FRONT_OF_ROBOT_DISTANCE_WHEN_DRIBBLING); - } - std::optional best_ball_detection = getBestBallDetection(new_ball_detections); double dt = 0.0; @@ -81,51 +68,10 @@ std::optional BallTracker::estimateBallState( kalman_filter.predict(Eigen::Vector::Zero()); } - if (dribbling_robot.has_value()) - { - // When dribbling, only trust a vision measurement if it shows the ball - // close to the dribbler. Otherwise reset to the kinematic dribbler position. - bool near_dribbler = - best_ball_detection.has_value() && - (best_ball_detection->position - dribbler_pos).length() <= - DRIBBLING_MEASUREMENT_MAX_DISTANCE_METERS; - - Eigen::Vector measurement; - if (near_dribbler) - { - measurement << best_ball_detection->position.x(), - best_ball_detection->position.y(); - kalman_filter.update(measurement); - } - else - { - measurement << dribbler_pos.x(), dribbler_pos.y(); - kalman_filter.state_estimate << measurement(0), measurement(1), 0, 0; - kalman_filter.state_covariance = INITIAL_COV; - } - prev_detection_timestamp = current_time; - consecutive_outliers = 0; - } - else if (best_ball_detection){ + if (best_ball_detection){ Eigen::Vector measurement; measurement << best_ball_detection->position.x(), best_ball_detection->position.y(); - if (!prev_detection_timestamp){ - // First detection: store position to compute velocity on the next frame - initial_detection_pos_ = measurement; - prev_detection_timestamp = current_time; - return std::nullopt; - } - - // Second detection: seed Kalman velocity from finite-difference - if (initial_detection_pos_.has_value() && dt > 0){ - double vx = (measurement(0) - (*initial_detection_pos_)(0)) / dt; - double vy = (measurement(1) - (*initial_detection_pos_)(1)) / dt; - kalman_filter.state_estimate << measurement(0), measurement(1), vx, vy; - kalman_filter.state_covariance = INITIAL_COV; - initial_detection_pos_ = std::nullopt; - } - const Eigen::Vector innovation = measurement - kalman_filter.measurement_model * kalman_filter.state_estimate; const Eigen::Matrix S = @@ -145,9 +91,10 @@ std::optional BallTracker::estimateBallState( kalman_filter.state_covariance = INITIAL_COV; consecutive_outliers = 0; } + prev_detection_timestamp = current_time; } - if (!prev_detection_timestamp && !dribbling_robot.has_value()){ + if (!prev_detection_timestamp){ return std::nullopt; } diff --git a/src/software/sensor_fusion/filter/ball_tracker.h b/src/software/sensor_fusion/filter/ball_tracker.h index 79a767f3f2..2e8c5d463a 100644 --- a/src/software/sensor_fusion/filter/ball_tracker.h +++ b/src/software/sensor_fusion/filter/ball_tracker.h @@ -9,7 +9,6 @@ #include "software/sensor_fusion/filter/vision_detection.h" #include "software/time/timestamp.h" #include "software/world/ball.h" -#include "software/world/robot.h" #include "software/sensor_fusion/filter/kalman_filter.h" /** @@ -47,8 +46,7 @@ class BallTracker std::optional estimateBallState( const std::vector& new_ball_detections, const Rectangle& filter_area, - const Timestamp& current_time, - std::optional dribbling_robot = std::nullopt); + const Timestamp& current_time); private: /** @@ -63,6 +61,4 @@ class BallTracker int consecutive_outliers; KalmanFilter<4, 2, 1> kalman_filter; std::optional prev_detection_timestamp; - // Position from the first detection — used to seed velocity on the second detection - std::optional> initial_detection_pos_; }; diff --git a/src/software/sensor_fusion/sensor_fusion.cpp b/src/software/sensor_fusion/sensor_fusion.cpp index 7a4e571786..22fc3d7c86 100644 --- a/src/software/sensor_fusion/sensor_fusion.cpp +++ b/src/software/sensor_fusion/sensor_fusion.cpp @@ -285,10 +285,7 @@ void SensorFusion::updateWorld(const SSLProto::SSL_DetectionFrame& ssl_detection { // friendly_robot_id_with_ball_in_dribbler will always have a value since this is // checked by the member function shouldTrustRobotStatus - std::optional robot_with_ball_in_dribbler = - friendly_team.getRobotById(friendly_robot_id_with_ball_in_dribbler.value()); - - std::optional new_ball = createBall({}, Timestamp::fromSeconds(ssl_detection_frame.t_capture()), robot_with_ball_in_dribbler); + std::optional new_ball = createBall({}, Timestamp::fromSeconds(ssl_detection_frame.t_capture())); if (new_ball) { @@ -339,14 +336,13 @@ void SensorFusion::updateBall(Ball new_ball) } std::optional SensorFusion::createBall( - const std::vector &ball_detections, const Timestamp& current_time, - std::optional dribbling_robot) + const std::vector &ball_detections, const Timestamp& current_time) { if (field) { std::optional new_ball = ball_tracker.estimateBallState(ball_detections, field.value().fieldBoundary(), - current_time, dribbling_robot); + current_time); return new_ball; } return std::nullopt; diff --git a/src/software/sensor_fusion/sensor_fusion.h b/src/software/sensor_fusion/sensor_fusion.h index d6c90169ef..312e1c1658 100644 --- a/src/software/sensor_fusion/sensor_fusion.h +++ b/src/software/sensor_fusion/sensor_fusion.h @@ -96,8 +96,7 @@ class SensorFusion * @return Ball if filtered from ball detections */ std::optional createBall(const std::vector &ball_detections, - const Timestamp& current_time, - std::optional dribbling_robot = std::nullopt); + const Timestamp& current_time); /** * Create team from a list of robot detections From 96373237a77d9706b9f9dffeb8f595fb5ebcc145 Mon Sep 17 00:00:00 2001 From: StarrryNight Date: Thu, 14 May 2026 01:14:38 -0700 Subject: [PATCH 31/57] add dribbling heuristic --- src/software/er_force_simulator_main.cpp | 4 +- .../sensor_fusion/filter/ball_tracker.cpp | 42 ++++++++++++++++--- .../sensor_fusion/filter/ball_tracker.h | 4 +- src/software/sensor_fusion/sensor_fusion.cpp | 9 ++-- src/software/sensor_fusion/sensor_fusion.h | 3 +- 5 files changed, 49 insertions(+), 13 deletions(-) diff --git a/src/software/er_force_simulator_main.cpp b/src/software/er_force_simulator_main.cpp index 862ab1637a..0f837748ae 100644 --- a/src/software/er_force_simulator_main.cpp +++ b/src/software/er_force_simulator_main.cpp @@ -48,7 +48,7 @@ int main(int argc, char** argv) { std::string runtime_dir = args.runtime_dir; LoggerSingleton::initializeLogger(runtime_dir, nullptr); - LOG(CSV, "realism_kalman_filter_v4.csv") << "timestamp_s,fused_x,fused_y,fused_vel_x, fused_vel_y, truth_x,truth_y, true_vel_x, true_vel_y,is_occluded\n"; + LOG(CSV, "realism_kalman_filter_v7.csv") << "timestamp_s,fused_x,fused_y,fused_vel_x, fused_vel_y, truth_x,truth_y, true_vel_x, true_vel_y,is_occluded\n"; /** * Creates a ER force simulator and sets up the appropriate @@ -228,7 +228,7 @@ int main(int argc, char** argv) { start_timestamp_s = current_ts; } - LOG(CSV, "realism_kalman_filter_v4.csv") + LOG(CSV, "realism_kalman_filter_v7.csv") << (current_ts - start_timestamp_s) << "," << yellow_vision.ball().current_state().global_position().x_meters() << "," diff --git a/src/software/sensor_fusion/filter/ball_tracker.cpp b/src/software/sensor_fusion/filter/ball_tracker.cpp index 223adaf80d..8ed13e3de3 100644 --- a/src/software/sensor_fusion/filter/ball_tracker.cpp +++ b/src/software/sensor_fusion/filter/ball_tracker.cpp @@ -36,6 +36,7 @@ namespace { const double MAHANALOGIS_THRESHOLD = 1; const int CONSECUTIVE_OUTLIERS_THRESHOLD = 3; + const double DRIBBLING_MEASUREMENT_MAX_DISTANCE_METERS = 0.08; } BallTracker::BallTracker() : @@ -50,8 +51,18 @@ BallTracker::BallTracker() : std::optional BallTracker::estimateBallState( const std::vector &new_ball_detections, const Rectangle &filter_area, - const Timestamp& current_time) + const Timestamp& current_time, std::optional dribbling_robot) { + Point dribbler_pos; + if (dribbling_robot.has_value()) + { + dribbler_pos = + dribbling_robot->position() + + Vector::createFromAngle(dribbling_robot->orientation()) + .normalize(DIST_TO_FRONT_OF_ROBOT_METERS + + BALL_TO_FRONT_OF_ROBOT_DISTANCE_WHEN_DRIBBLING); + } + std::optional best_ball_detection = getBestBallDetection(new_ball_detections); double dt = 0.0; @@ -68,7 +79,30 @@ std::optional BallTracker::estimateBallState( kalman_filter.predict(Eigen::Vector::Zero()); } - if (best_ball_detection){ + if (dribbling_robot.has_value()) + { + bool near_dribbler = + best_ball_detection.has_value() && + (best_ball_detection->position - dribbler_pos).length() <= + DRIBBLING_MEASUREMENT_MAX_DISTANCE_METERS; + + Eigen::Vector measurement; + if (near_dribbler) + { + measurement << best_ball_detection->position.x(), + best_ball_detection->position.y(); + kalman_filter.update(measurement); + } + else + { + measurement << dribbler_pos.x(), dribbler_pos.y(); + kalman_filter.state_estimate << measurement(0), measurement(1), 0, 0; + kalman_filter.state_covariance = INITIAL_COV; + } + prev_detection_timestamp = current_time; + consecutive_outliers = 0; + } + else if (best_ball_detection){ Eigen::Vector measurement; measurement << best_ball_detection->position.x(), best_ball_detection->position.y(); @@ -94,10 +128,6 @@ std::optional BallTracker::estimateBallState( prev_detection_timestamp = current_time; } - if (!prev_detection_timestamp){ - return std::nullopt; - } - Point ball_position = Point(kalman_filter.state_estimate(0), kalman_filter.state_estimate(1)); Vector ball_velocity = Vector(kalman_filter.state_estimate(2), kalman_filter.state_estimate(3)); double z_height = best_ball_detection ? best_ball_detection->distance_from_ground : 0.0; diff --git a/src/software/sensor_fusion/filter/ball_tracker.h b/src/software/sensor_fusion/filter/ball_tracker.h index 2e8c5d463a..8e8ab40f77 100644 --- a/src/software/sensor_fusion/filter/ball_tracker.h +++ b/src/software/sensor_fusion/filter/ball_tracker.h @@ -9,6 +9,7 @@ #include "software/sensor_fusion/filter/vision_detection.h" #include "software/time/timestamp.h" #include "software/world/ball.h" +#include "software/world/robot.h" #include "software/sensor_fusion/filter/kalman_filter.h" /** @@ -46,7 +47,8 @@ class BallTracker std::optional estimateBallState( const std::vector& new_ball_detections, const Rectangle& filter_area, - const Timestamp& current_time); + const Timestamp& current_time, + std::optional dribbling_robot = std::nullopt); private: /** diff --git a/src/software/sensor_fusion/sensor_fusion.cpp b/src/software/sensor_fusion/sensor_fusion.cpp index 22fc3d7c86..592b96bf2b 100644 --- a/src/software/sensor_fusion/sensor_fusion.cpp +++ b/src/software/sensor_fusion/sensor_fusion.cpp @@ -285,7 +285,9 @@ void SensorFusion::updateWorld(const SSLProto::SSL_DetectionFrame& ssl_detection { // friendly_robot_id_with_ball_in_dribbler will always have a value since this is // checked by the member function shouldTrustRobotStatus - std::optional new_ball = createBall({}, Timestamp::fromSeconds(ssl_detection_frame.t_capture())); + std::optional robot_with_ball_in_dribbler = + friendly_team.getRobotById(friendly_robot_id_with_ball_in_dribbler.value()); + std::optional new_ball = createBall({}, Timestamp::fromSeconds(ssl_detection_frame.t_capture()), robot_with_ball_in_dribbler); if (new_ball) { @@ -336,13 +338,14 @@ void SensorFusion::updateBall(Ball new_ball) } std::optional SensorFusion::createBall( - const std::vector &ball_detections, const Timestamp& current_time) + const std::vector &ball_detections, const Timestamp& current_time, + std::optional dribbling_robot) { if (field) { std::optional new_ball = ball_tracker.estimateBallState(ball_detections, field.value().fieldBoundary(), - current_time); + current_time, dribbling_robot); return new_ball; } return std::nullopt; diff --git a/src/software/sensor_fusion/sensor_fusion.h b/src/software/sensor_fusion/sensor_fusion.h index 312e1c1658..d6c90169ef 100644 --- a/src/software/sensor_fusion/sensor_fusion.h +++ b/src/software/sensor_fusion/sensor_fusion.h @@ -96,7 +96,8 @@ class SensorFusion * @return Ball if filtered from ball detections */ std::optional createBall(const std::vector &ball_detections, - const Timestamp& current_time); + const Timestamp& current_time, + std::optional dribbling_robot = std::nullopt); /** * Create team from a list of robot detections From 597cf7e478968264033f2d4f5ef426e825bf6209 Mon Sep 17 00:00:00 2001 From: StarrryNight Date: Thu, 14 May 2026 01:22:29 -0700 Subject: [PATCH 32/57] add out of bounds heuristic --- src/software/sensor_fusion/filter/ball_tracker.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/software/sensor_fusion/filter/ball_tracker.cpp b/src/software/sensor_fusion/filter/ball_tracker.cpp index 8ed13e3de3..3fab3f7e7d 100644 --- a/src/software/sensor_fusion/filter/ball_tracker.cpp +++ b/src/software/sensor_fusion/filter/ball_tracker.cpp @@ -106,6 +106,13 @@ std::optional BallTracker::estimateBallState( Eigen::Vector measurement; measurement << best_ball_detection->position.x(), best_ball_detection->position.y(); + Point predicted_pos(kalman_filter.state_estimate(0), kalman_filter.state_estimate(1)); + if (!contains(filter_area, predicted_pos)){ + kalman_filter.state_estimate << measurement(0), measurement(1), 0, 0; + kalman_filter.state_covariance = INITIAL_COV; + consecutive_outliers = 0; + } + const Eigen::Vector innovation = measurement - kalman_filter.measurement_model * kalman_filter.state_estimate; const Eigen::Matrix S = From 76be0ff3a56e9fccf1df0bad784c7b5da7c7a1af Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Thu, 14 May 2026 13:15:31 -0700 Subject: [PATCH 33/57] fix mahalanobis gating --- src/software/er_force_simulator_main.cpp | 4 +- .../game_controller.py | 51 +++++++++++++++---- 2 files changed, 44 insertions(+), 11 deletions(-) diff --git a/src/software/er_force_simulator_main.cpp b/src/software/er_force_simulator_main.cpp index 0f837748ae..c6faf80429 100644 --- a/src/software/er_force_simulator_main.cpp +++ b/src/software/er_force_simulator_main.cpp @@ -48,7 +48,7 @@ int main(int argc, char** argv) { std::string runtime_dir = args.runtime_dir; LoggerSingleton::initializeLogger(runtime_dir, nullptr); - LOG(CSV, "realism_kalman_filter_v7.csv") << "timestamp_s,fused_x,fused_y,fused_vel_x, fused_vel_y, truth_x,truth_y, true_vel_x, true_vel_y,is_occluded\n"; + LOG(CSV, "realism_kalman_filter_v8_2.csv") << "timestamp_s,fused_x,fused_y,fused_vel_x, fused_vel_y, truth_x,truth_y, true_vel_x, true_vel_y,is_occluded\n"; /** * Creates a ER force simulator and sets up the appropriate @@ -228,7 +228,7 @@ int main(int argc, char** argv) { start_timestamp_s = current_ts; } - LOG(CSV, "realism_kalman_filter_v7.csv") + LOG(CSV, "realism_kalman_filter_v8_2.csv") << (current_ts - start_timestamp_s) << "," << yellow_vision.ball().current_state().global_position().x_meters() << "," diff --git a/src/software/thunderscope/binary_context_managers/game_controller.py b/src/software/thunderscope/binary_context_managers/game_controller.py index e8f5d97992..ddfc700c03 100644 --- a/src/software/thunderscope/binary_context_managers/game_controller.py +++ b/src/software/thunderscope/binary_context_managers/game_controller.py @@ -29,7 +29,8 @@ class Gamecontroller: """Gamecontroller Context Manager""" - CI_MODE_LAUNCH_DELAY_S = 0.3 + CI_MODE_LAUNCH_DELAY_S = 0.1 + CI_MODE_CONNECT_TIMEOUT_S = 5.0 CI_MODE_OUTPUT_RECEIVE_BUFFER_SIZE = 9000 REFEREE_IP = "224.5.23.1" RESET_MATCH_DELAY_S = 1 @@ -99,17 +100,45 @@ def __enter__(self) -> Gamecontroller: command += ["-publishAddress", f"{self.REFEREE_IP}:{self.referee_port}"] command += ["-ciAddress", f"localhost:{self.ci_port}"] - if self.suppress_logs: - with open(os.devnull, "w") as fp: - self.gamecontroller_proc = Popen(command, stdout=fp, stderr=fp) + # Run in a temp dir so the gamecontroller always starts with a clean + # state store — the state-store.json.stream can get corrupted when the + # process is killed mid-write, causing a panic on the next startup. + import tempfile + self._gc_workdir = tempfile.mkdtemp(prefix="gamecontroller_") + if self.suppress_logs: + self._gc_log = open( + os.path.join(self._gc_workdir, "gamecontroller.log"), "w" + ) + self.gamecontroller_proc = Popen( + command, stdout=self._gc_log, stderr=self._gc_log, + cwd=self._gc_workdir, + ) else: - self.gamecontroller_proc = Popen(command) + self._gc_log = None + self.gamecontroller_proc = Popen(command, cwd=self._gc_workdir) - # We can't connect to the ci port right away, it takes - # CI_MODE_LAUNCH_DELAY_S to start up the gamecontroller - time.sleep(Gamecontroller.CI_MODE_LAUNCH_DELAY_S) - self.ci_socket = SslSocket(self.ci_port) + # Poll until the gamecontroller CI port is ready, up to CI_MODE_CONNECT_TIMEOUT_S + deadline = time.time() + Gamecontroller.CI_MODE_CONNECT_TIMEOUT_S + while True: + if self.gamecontroller_proc.poll() is not None: + log_contents = "" + log_path = os.path.join(self._gc_workdir, "gamecontroller.log") + if os.path.exists(log_path): + with open(log_path) as f: + log_contents = f.read() + raise RuntimeError( + f"Gamecontroller process exited with code {self.gamecontroller_proc.returncode} " + f"before CI port {self.ci_port} became available" + + (f"\nGamecontroller output:\n{log_contents}" if log_contents else "") + ) + try: + self.ci_socket = SslSocket(self.ci_port) + break + except ConnectionRefusedError: + if time.time() >= deadline: + raise + time.sleep(Gamecontroller.CI_MODE_LAUNCH_DELAY_S) return self @@ -123,6 +152,10 @@ def __exit__(self, type, value, traceback) -> None: self.gamecontroller_proc.terminate() self.gamecontroller_proc.wait() self.ci_socket.close() + if self._gc_log is not None: + self._gc_log.close() + import shutil + shutil.rmtree(self._gc_workdir, ignore_errors=True) def refresh(self): """Gets any manual gamecontroller commands from the buffer and executes them""" From e27a65ebc4269056b7ddb5a1274c9c5d96109c55 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Thu, 14 May 2026 19:08:18 -0700 Subject: [PATCH 34/57] cleanup --- .../ai/hl/stp/tactic/goalie/goalie_fsm.cpp | 2 +- .../hl/stp/tactic/goalie/goalie_tactic_test.py | 16 ++++++++-------- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/software/ai/hl/stp/tactic/goalie/goalie_fsm.cpp b/src/software/ai/hl/stp/tactic/goalie/goalie_fsm.cpp index 4078c94b95..b779a3d2fd 100644 --- a/src/software/ai/hl/stp/tactic/goalie/goalie_fsm.cpp +++ b/src/software/ai/hl/stp/tactic/goalie/goalie_fsm.cpp @@ -254,7 +254,7 @@ void GoalieFSM::positionToBlock(const Update& event) event.common.set_primitive(std::make_unique( event.common.robot, goalie_pos, goalie_orientation, max_allowed_speed_mode, TbotsProto::ObstacleAvoidanceMode::AGGRESSIVE, TbotsProto::DribblerMode::OFF, - TbotsProto::BallCollisionType::AVOID, + TbotsProto::BallCollisionType::ALLOW, AutoChipOrKick{AutoChipOrKickMode::AUTOCHIP, YEET_CHIP_DISTANCE_METERS})); } diff --git a/src/software/ai/hl/stp/tactic/goalie/goalie_tactic_test.py b/src/software/ai/hl/stp/tactic/goalie/goalie_tactic_test.py index bb890132d8..2111109524 100644 --- a/src/software/ai/hl/stp/tactic/goalie/goalie_tactic_test.py +++ b/src/software/ai/hl/stp/tactic/goalie/goalie_tactic_test.py @@ -1,18 +1,18 @@ import pytest -import software.python_bindings as tbots_cpp +import software.python_bindings as tbots_cpp from proto.import_all_protos import * -from proto.message_translation.tbots_protobuf import create_world_state -from software.simulated_tests.simulated_test_fixture import ( - pytest_main, -) +from software.simulated_tests.validation.robot_enters_region import * from software.simulated_tests.validation.ball_enters_region import * from software.simulated_tests.validation.ball_moves_in_direction import * -from software.simulated_tests.validation.ball_speed_threshold import * -from software.simulated_tests.validation.excessive_dribbling import * from software.simulated_tests.validation.friendly_has_ball_possession import * -from software.simulated_tests.validation.robot_enters_region import * +from software.simulated_tests.validation.ball_speed_threshold import * from software.simulated_tests.validation.robot_speed_threshold import * +from software.simulated_tests.validation.excessive_dribbling import * +from software.simulated_tests.simulated_test_fixture import ( + pytest_main, +) +from proto.message_translation.tbots_protobuf import create_world_state @pytest.mark.parametrize( From e4f2e132a866bf7ebe77295ed20aabb198e154f6 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Thu, 14 May 2026 19:15:59 -0700 Subject: [PATCH 35/57] init (just moved the kf branch stuff here --- src/software/simulated_tests/BUILD | 1 + .../simulated_tests/simulated_test_fixture.py | 6 ++++ .../simulated_tests/tbots_test_runner.py | 7 ++++ src/software/simulated_tests/validation/BUILD | 1 + .../validation/avoid_collisions.py | 2 +- .../validation/ball_enters_region.py | 2 +- .../validation/ball_is_off_ground.py | 2 +- .../validation/ball_kicked_in_direction.py | 2 +- .../validation/ball_moves_in_direction.py | 4 +-- .../validation/ball_speed_threshold.py | 2 +- .../validation/ball_stops_in_region.py | 2 +- .../validation/delay_validation.py | 2 +- .../validation/duration_validation.py | 2 +- .../validation/excessive_dribbling.py | 2 +- .../friendly_has_ball_possession.py | 2 +- .../validation/friendly_receives_ball_slow.py | 2 +- .../validation/friendly_team_scored.py | 2 +- .../validation/or_validation.py | 7 ++-- .../validation/robot_at_angular_velocity.py | 2 +- .../validation/robot_at_orientation.py | 2 +- .../validation/robot_at_position.py | 2 +- .../robot_enters_placement_region.py | 2 +- .../validation/robot_enters_region.py | 2 +- .../robot_enters_region_and_stops.py | 2 +- .../validation/robot_received_ball.py | 2 +- .../validation/robot_speed_threshold.py | 2 +- .../simulated_tests/validation/validation.py | 36 ++++++++++++++----- 27 files changed, 69 insertions(+), 33 deletions(-) diff --git a/src/software/simulated_tests/BUILD b/src/software/simulated_tests/BUILD index 37dd733098..f6e3406fb7 100644 --- a/src/software/simulated_tests/BUILD +++ b/src/software/simulated_tests/BUILD @@ -18,6 +18,7 @@ py_library( "//software:py_constants.so", ], deps = [ + "//extlibs/er_force_sim/src/protobuf:erforce_py_proto", "//proto:import_all_protos", "//software/logger:py_logger", "//software/networking/unix:threaded_unix_listener_py", diff --git a/src/software/simulated_tests/simulated_test_fixture.py b/src/software/simulated_tests/simulated_test_fixture.py index 25e92dce64..8e275b8bc7 100644 --- a/src/software/simulated_tests/simulated_test_fixture.py +++ b/src/software/simulated_tests/simulated_test_fixture.py @@ -200,6 +200,11 @@ def runner( if self.thunderscope and tick_duration_s > processing_time: time.sleep(tick_duration_s - processing_time) + # Fetch the latest true simulator state (non-blocking; None if not yet available) + simulator_state = self.simulator_state_buffer.get( + block=False, return_cached=False + ) + # Validate ( eventually_validation_proto_set, @@ -208,6 +213,7 @@ def runner( world, eventually_validation_sequence_set, always_validation_sequence_set, + simulator_state=simulator_state, ) # Set the test name diff --git a/src/software/simulated_tests/tbots_test_runner.py b/src/software/simulated_tests/tbots_test_runner.py index 03e7d4ae85..bc983c0b0e 100644 --- a/src/software/simulated_tests/tbots_test_runner.py +++ b/src/software/simulated_tests/tbots_test_runner.py @@ -1,4 +1,5 @@ from proto.import_all_protos import * +from extlibs.er_force_sim.src.protobuf.world_pb2 import SimulatorState from software.logger.logger import create_logger from software.thunderscope.thread_safe_buffer import ThreadSafeBuffer from abc import abstractmethod @@ -46,6 +47,9 @@ def __init__( self.robot_status_buffer = ThreadSafeBuffer( buffer_size=1, protobuf_type=RobotStatus ) + self.simulator_state_buffer = ThreadSafeBuffer( + buffer_size=20, protobuf_type=SimulatorState + ) self.blue_full_system_proto_unix_io.register_observer( SSL_WrapperPacket, self.ssl_wrapper_buffer @@ -53,6 +57,9 @@ def __init__( self.blue_full_system_proto_unix_io.register_observer( RobotStatus, self.robot_status_buffer ) + self.blue_full_system_proto_unix_io.register_observer( + SimulatorState, self.simulator_state_buffer + ) if self.is_yellow_friendly: self.yellow_full_system_proto_unix_io.register_observer( World, self.world_buffer diff --git a/src/software/simulated_tests/validation/BUILD b/src/software/simulated_tests/validation/BUILD index 3bf65f83e6..28f9f9790c 100644 --- a/src/software/simulated_tests/validation/BUILD +++ b/src/software/simulated_tests/validation/BUILD @@ -33,6 +33,7 @@ py_library( "//software:python_bindings.so", ], deps = [ + "//extlibs/er_force_sim/src/protobuf:erforce_py_proto", "//proto:software_py_proto", "//proto:tbots_py_proto", "//software/logger:py_logger", diff --git a/src/software/simulated_tests/validation/avoid_collisions.py b/src/software/simulated_tests/validation/avoid_collisions.py index 41953cd994..109ba2c06e 100644 --- a/src/software/simulated_tests/validation/avoid_collisions.py +++ b/src/software/simulated_tests/validation/avoid_collisions.py @@ -21,7 +21,7 @@ def __init__(self): self.fouled_robots = [] @override - def get_validation_status(self, world: World) -> ValidationStatus: + def get_validation_status(self, world: World, simulator_state=None) -> ValidationStatus: """Checks if any 2 robots in the world have collided :param world: the World message to validate diff --git a/src/software/simulated_tests/validation/ball_enters_region.py b/src/software/simulated_tests/validation/ball_enters_region.py index ff6e840f59..15c1cb6cb2 100644 --- a/src/software/simulated_tests/validation/ball_enters_region.py +++ b/src/software/simulated_tests/validation/ball_enters_region.py @@ -17,7 +17,7 @@ def __init__(self, regions=None): self.ball_position = None @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if the ball enters the provided regions :param world: The world msg to validate diff --git a/src/software/simulated_tests/validation/ball_is_off_ground.py b/src/software/simulated_tests/validation/ball_is_off_ground.py index 880150c838..3b6c84442b 100644 --- a/src/software/simulated_tests/validation/ball_is_off_ground.py +++ b/src/software/simulated_tests/validation/ball_is_off_ground.py @@ -21,7 +21,7 @@ def __init__(self, threshold=0.2): self.threshold = threshold @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if the ball has is threshold meters off the ground :param world: The world msg to validate diff --git a/src/software/simulated_tests/validation/ball_kicked_in_direction.py b/src/software/simulated_tests/validation/ball_kicked_in_direction.py index 3bd99978ce..55f5339158 100644 --- a/src/software/simulated_tests/validation/ball_kicked_in_direction.py +++ b/src/software/simulated_tests/validation/ball_kicked_in_direction.py @@ -33,7 +33,7 @@ def __init__( ) @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if the ball has been kicked in the expected direction :param world: The world msg to validate diff --git a/src/software/simulated_tests/validation/ball_moves_in_direction.py b/src/software/simulated_tests/validation/ball_moves_in_direction.py index 533d818fc7..f4f138308a 100644 --- a/src/software/simulated_tests/validation/ball_moves_in_direction.py +++ b/src/software/simulated_tests/validation/ball_moves_in_direction.py @@ -24,7 +24,7 @@ def __init__(self, initial_ball_position: tbots.Point, tolerance: float = 0.1): self.tolerance = tolerance @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if ball is moving forward :param world: The world msg to validate @@ -84,7 +84,7 @@ def __init__(self, initial_ball_position, regions=[], tolerance: float = 0.1): self.regions = regions @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: for region in self.regions: if tbots.contains( region, tbots.createPoint(world.ball.current_state.global_position) diff --git a/src/software/simulated_tests/validation/ball_speed_threshold.py b/src/software/simulated_tests/validation/ball_speed_threshold.py index f1f8dcec46..a1ba9a34e4 100644 --- a/src/software/simulated_tests/validation/ball_speed_threshold.py +++ b/src/software/simulated_tests/validation/ball_speed_threshold.py @@ -22,7 +22,7 @@ def __init__(self, speed_threshold): self.speed_threshold = speed_threshold @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if the ball speed is at or above some threshold :param world: The world msg to validate diff --git a/src/software/simulated_tests/validation/ball_stops_in_region.py b/src/software/simulated_tests/validation/ball_stops_in_region.py index 38b0312481..f7b46efd2a 100644 --- a/src/software/simulated_tests/validation/ball_stops_in_region.py +++ b/src/software/simulated_tests/validation/ball_stops_in_region.py @@ -16,7 +16,7 @@ def __init__(self, regions=None): self.regions = regions if regions else [] @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if the ball stops in the provided regions :param world: The world msg to validate diff --git a/src/software/simulated_tests/validation/delay_validation.py b/src/software/simulated_tests/validation/delay_validation.py index b51725f7d4..1c23c16ea7 100644 --- a/src/software/simulated_tests/validation/delay_validation.py +++ b/src/software/simulated_tests/validation/delay_validation.py @@ -16,7 +16,7 @@ def __init__(self, delay_s, validation): self.validation = validation @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks validation after delay finished. If during delay, returns default validation status. :param world: The world msg to validate diff --git a/src/software/simulated_tests/validation/duration_validation.py b/src/software/simulated_tests/validation/duration_validation.py index 1020e024d2..0cc4d4bdb3 100644 --- a/src/software/simulated_tests/validation/duration_validation.py +++ b/src/software/simulated_tests/validation/duration_validation.py @@ -23,7 +23,7 @@ def __init__(self, duration_s, validation): self.validation = validation @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if validation has been consecutively PASSING for a duration. :param world: The world msg to validate diff --git a/src/software/simulated_tests/validation/excessive_dribbling.py b/src/software/simulated_tests/validation/excessive_dribbling.py index 98a386b055..24f842fb9f 100644 --- a/src/software/simulated_tests/validation/excessive_dribbling.py +++ b/src/software/simulated_tests/validation/excessive_dribbling.py @@ -19,7 +19,7 @@ def __init__(self): self.dribbling_error_margin = 0.05 @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if any friendly robot is excessively dribbling the ball past the max dribble displacement minus the dribbling error margin diff --git a/src/software/simulated_tests/validation/friendly_has_ball_possession.py b/src/software/simulated_tests/validation/friendly_has_ball_possession.py index bf1013f7ed..58d73e3269 100644 --- a/src/software/simulated_tests/validation/friendly_has_ball_possession.py +++ b/src/software/simulated_tests/validation/friendly_has_ball_possession.py @@ -22,7 +22,7 @@ def __init__(self, robot_id=None, tolerance=0.01): self.tolerance = tolerance @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if friendly robot has possession of the ball :param world: The world msg to validate diff --git a/src/software/simulated_tests/validation/friendly_receives_ball_slow.py b/src/software/simulated_tests/validation/friendly_receives_ball_slow.py index 219271e001..68579e9550 100644 --- a/src/software/simulated_tests/validation/friendly_receives_ball_slow.py +++ b/src/software/simulated_tests/validation/friendly_receives_ball_slow.py @@ -24,7 +24,7 @@ def __init__(self, robot_id, max_receive_speed): self.max_receive_speed = max_receive_speed @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if the specified robot receives the ball too fast :param world: The world msg to validate diff --git a/src/software/simulated_tests/validation/friendly_team_scored.py b/src/software/simulated_tests/validation/friendly_team_scored.py index 458502f152..f0f4912ec6 100644 --- a/src/software/simulated_tests/validation/friendly_team_scored.py +++ b/src/software/simulated_tests/validation/friendly_team_scored.py @@ -16,7 +16,7 @@ def __init__(self): self.region = tbots_cpp.Field.createSSLDivisionBField().enemyGoal() @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if the ball enters the provided regions :param world: The world msg to validate diff --git a/src/software/simulated_tests/validation/or_validation.py b/src/software/simulated_tests/validation/or_validation.py index b14affe8c3..110b2b66de 100644 --- a/src/software/simulated_tests/validation/or_validation.py +++ b/src/software/simulated_tests/validation/or_validation.py @@ -17,9 +17,12 @@ def __init__(self, validations): self.validations = validations @override - def get_validation_status(self, world): + def get_validation_status(self, world, simulator_state=None): for validation in self.validations: - if validation.get_validation_status(world) == ValidationStatus.PASSING: + if ( + validation.get_validation_status(world, simulator_state=simulator_state) + == ValidationStatus.PASSING + ): return ValidationStatus.PASSING return ValidationStatus.FAILING diff --git a/src/software/simulated_tests/validation/robot_at_angular_velocity.py b/src/software/simulated_tests/validation/robot_at_angular_velocity.py index f9fec65db8..7650762bde 100644 --- a/src/software/simulated_tests/validation/robot_at_angular_velocity.py +++ b/src/software/simulated_tests/validation/robot_at_angular_velocity.py @@ -24,7 +24,7 @@ def __init__(self, robot_id, angular_velocity, threshold=0.1): self.threshold = threshold @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if the robot is at the expected angular velocity :param world: The world msg to validate diff --git a/src/software/simulated_tests/validation/robot_at_orientation.py b/src/software/simulated_tests/validation/robot_at_orientation.py index df3c113257..a30458203b 100644 --- a/src/software/simulated_tests/validation/robot_at_orientation.py +++ b/src/software/simulated_tests/validation/robot_at_orientation.py @@ -26,7 +26,7 @@ def __init__(self, robot_id, orientation, threshold=0.1): self.threshold = threshold @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if the robot is at the expected orientation :param world: The world msg to validate diff --git a/src/software/simulated_tests/validation/robot_at_position.py b/src/software/simulated_tests/validation/robot_at_position.py index 0637585254..9dc8a9644c 100644 --- a/src/software/simulated_tests/validation/robot_at_position.py +++ b/src/software/simulated_tests/validation/robot_at_position.py @@ -27,7 +27,7 @@ def __init__( self.threshold = threshold @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if the robot is at the target position :param world: The world msg to validate diff --git a/src/software/simulated_tests/validation/robot_enters_placement_region.py b/src/software/simulated_tests/validation/robot_enters_placement_region.py index db6275e1f2..296d1da958 100644 --- a/src/software/simulated_tests/validation/robot_enters_placement_region.py +++ b/src/software/simulated_tests/validation/robot_enters_placement_region.py @@ -26,7 +26,7 @@ def __init__(self, placement_point): ) @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if robots enter the ball placement region :param world: The world msg to validate diff --git a/src/software/simulated_tests/validation/robot_enters_region.py b/src/software/simulated_tests/validation/robot_enters_region.py index d5d6584487..6b1b3a553a 100644 --- a/src/software/simulated_tests/validation/robot_enters_region.py +++ b/src/software/simulated_tests/validation/robot_enters_region.py @@ -24,7 +24,7 @@ def __init__(self, regions, req_robot_cnt): self.robot_in_zone = {} @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if a specific number of robots enter the provided set of regions :param world: The world msg to validate diff --git a/src/software/simulated_tests/validation/robot_enters_region_and_stops.py b/src/software/simulated_tests/validation/robot_enters_region_and_stops.py index 5cc4a5b052..ebdf6296fd 100644 --- a/src/software/simulated_tests/validation/robot_enters_region_and_stops.py +++ b/src/software/simulated_tests/validation/robot_enters_region_and_stops.py @@ -32,7 +32,7 @@ def __init__(self, regions=None, num_ticks=1): self.passing_robot_id = None @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if a robot is in the provided region Then checks if that robot is stationary within a threshold for the provided number of ticks diff --git a/src/software/simulated_tests/validation/robot_received_ball.py b/src/software/simulated_tests/validation/robot_received_ball.py index 674f75c800..2f057dbfc0 100644 --- a/src/software/simulated_tests/validation/robot_received_ball.py +++ b/src/software/simulated_tests/validation/robot_received_ball.py @@ -22,7 +22,7 @@ def __init__(self, robot_id, tolerance=0.02): self.tolerance = tolerance @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if the specific robot has received the ball :param world: The world msg to validate diff --git a/src/software/simulated_tests/validation/robot_speed_threshold.py b/src/software/simulated_tests/validation/robot_speed_threshold.py index 19e20db786..c7a679849c 100644 --- a/src/software/simulated_tests/validation/robot_speed_threshold.py +++ b/src/software/simulated_tests/validation/robot_speed_threshold.py @@ -23,7 +23,7 @@ def __init__(self, speed_threshold): self.speed_threshold = speed_threshold @override - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: """Checks if the friendly robots' speed is at or above some threshold :param world: The world msg to validate diff --git a/src/software/simulated_tests/validation/validation.py b/src/software/simulated_tests/validation/validation.py index 3b5e70d538..19df55e4cb 100644 --- a/src/software/simulated_tests/validation/validation.py +++ b/src/software/simulated_tests/validation/validation.py @@ -8,7 +8,7 @@ class Validation: """A validation function""" @abstractmethod - def get_validation_status(self, world) -> ValidationStatus: + def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: raise NotImplementedError("get_validation_status is not implemented") @abstractmethod @@ -66,15 +66,16 @@ def constructor(self, *args, **kwargs): """ self.validation = validation_class(*args, **kwargs) - def flip_validation(self, world): + def flip_validation(self, world, simulator_state=None): """Flip the validation status :param world: The world msg to validate on + :param simulator_state: The simulator state with true positions, or None """ return { ValidationStatus.FAILING: ValidationStatus.PASSING, ValidationStatus.PASSING: ValidationStatus.FAILING, - }[self.validation.get_validation_status(world)] + }[self.validation.get_validation_status(world, simulator_state=simulator_state)] # Generate the types: specifically, all Eventually validations will return # EVENTUALLY when get_validation_type is called, and all Always validations @@ -95,7 +96,10 @@ def flip_validation(self, world): + repr(self.validation), "get_validation_type": lambda self: ValidationType.EVENTUALLY, "get_validation_status": lambda self, - world: self.validation.get_validation_status(world), + world, + simulator_state=None: self.validation.get_validation_status( + world, simulator_state=simulator_state + ), }, ) @@ -107,7 +111,11 @@ def flip_validation(self, world): "__repr__": lambda self: "EventuallyFalseValidation: " + repr(self.validation), "get_validation_type": lambda self: ValidationType.EVENTUALLY, - "get_validation_status": lambda self, world: flip_validation(self, world), + "get_validation_status": lambda self, + world, + simulator_state=None: flip_validation( + self, world, simulator_state=simulator_state + ), }, ) @@ -119,7 +127,10 @@ def flip_validation(self, world): "__repr__": lambda self: "AlwaysTrueValidation: " + repr(self.validation), "get_validation_type": lambda self: ValidationType.ALWAYS, "get_validation_status": lambda self, - world: self.validation.get_validation_status(world), + world, + simulator_state=None: self.validation.get_validation_status( + world, simulator_state=simulator_state + ), }, ) @@ -130,7 +141,11 @@ def flip_validation(self, world): **common, "__repr__": lambda self: "AlwaysFalseValidation: " + repr(self.validation), "get_validation_type": lambda self: ValidationType.ALWAYS, - "get_validation_status": lambda self, world: flip_validation(self, world), + "get_validation_status": lambda self, + world, + simulator_state=None: flip_validation( + self, world, simulator_state=simulator_state + ), }, ) @@ -138,7 +153,10 @@ def flip_validation(self, world): def run_validation_sequence_sets( - world, eventually_validation_sequence_set, always_validation_sequence_set + world, + eventually_validation_sequence_set, + always_validation_sequence_set, + simulator_state=None, ): """Given both eventually and always validation sequence sets, (and world) run validation and aggregate the results in a validation proto set. @@ -170,7 +188,7 @@ def create_validation_proto_helper(validation_proto_set, validation): validation_proto = ValidationProto() # Get status - status = validation.get_validation_status(world) + status = validation.get_validation_status(world, simulator_state=simulator_state) # Create validation proto validation_proto.status = status From c01a2231c332a609b1bceb9a15d9cd74ce84eb8c Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Thu, 14 May 2026 19:20:53 -0700 Subject: [PATCH 36/57] more cleanup --- .../simulated_tests/simulated_test_fixture.py | 71 ++++++++++--------- 1 file changed, 36 insertions(+), 35 deletions(-) diff --git a/src/software/simulated_tests/simulated_test_fixture.py b/src/software/simulated_tests/simulated_test_fixture.py index 15469fa564..fa468b6aad 100644 --- a/src/software/simulated_tests/simulated_test_fixture.py +++ b/src/software/simulated_tests/simulated_test_fixture.py @@ -1,26 +1,25 @@ -import threading -import queue import argparse -import time -import sys import os +import queue +import sys +import threading +import time +from typing import override import pytest -from proto.import_all_protos import * +from software.py_constants import MILLISECONDS_PER_SECOND -from software.simulated_tests.validation import validation +from proto.import_all_protos import * +from software.logger.logger import create_logger from software.simulated_tests.tbots_test_runner import TbotsTestRunner -from software.thunderscope.thunderscope import Thunderscope -from software.thunderscope.proto_unix_io import ProtoUnixIO -from software.py_constants import MILLISECONDS_PER_SECOND +from software.simulated_tests.validation import validation from software.thunderscope.binary_context_managers.full_system import FullSystem -from software.thunderscope.binary_context_managers.simulator import Simulator from software.thunderscope.binary_context_managers.game_controller import Gamecontroller -from software.thunderscope.thunderscope_config import configure_simulated_test_view +from software.thunderscope.binary_context_managers.simulator import Simulator +from software.thunderscope.proto_unix_io import ProtoUnixIO from software.thunderscope.thread_safe_buffer import ThreadSafeBuffer - -from software.logger.logger import create_logger -from typing import override +from software.thunderscope.thunderscope import Thunderscope +from software.thunderscope.thunderscope_config import configure_simulated_test_view logger = create_logger(__name__) @@ -538,25 +537,29 @@ def simulated_test_runner(): test_name = current_test.split("-")[0] # Launch all binaries - with Simulator( - f"{args.simulator_runtime_dir}/test/{test_name}", - args.debug_simulator, - args.enable_realism, - ) as simulator, FullSystem( - "software/unix_full_system", - f"{args.blue_full_system_runtime_dir}/test/{test_name}", - args.debug_blue_full_system, - False, - should_restart_on_crash=False, - running_in_realtime=args.enable_thunderscope, - ) as blue_fs, FullSystem( - "software/unix_full_system", - f"{args.yellow_full_system_runtime_dir}/test/{test_name}", - args.debug_yellow_full_system, - True, - should_restart_on_crash=False, - running_in_realtime=args.enable_thunderscope, - ) as yellow_fs: + with ( + Simulator( + f"{args.simulator_runtime_dir}/test/{test_name}", + args.debug_simulator, + args.enable_realism, + ) as simulator, + FullSystem( + "software/unix_full_system", + f"{args.blue_full_system_runtime_dir}/test/{test_name}", + args.debug_blue_full_system, + False, + should_restart_on_crash=False, + running_in_realtime=args.enable_thunderscope, + ) as blue_fs, + FullSystem( + "software/unix_full_system", + f"{args.yellow_full_system_runtime_dir}/test/{test_name}", + args.debug_yellow_full_system, + True, + should_restart_on_crash=False, + running_in_realtime=args.enable_thunderscope, + ) as yellow_fs, + ): with Gamecontroller( suppress_logs=(not args.show_gamecontroller_logs) ) as gamecontroller: @@ -610,6 +613,4 @@ def simulated_test_runner(): gamecontroller, ) - runner.simulator_runtime_dir = f"{args.simulator_runtime_dir}/test/{test_name}" - yield runner From 46b86b0973e786387dd45a143693acae71769096 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Thu, 14 May 2026 19:21:37 -0700 Subject: [PATCH 37/57] remove the ball tracker tests for now --- .../filter/ball_tracker_test.cpp | 579 ------------------ 1 file changed, 579 deletions(-) delete mode 100644 src/software/sensor_fusion/filter/ball_tracker_test.cpp diff --git a/src/software/sensor_fusion/filter/ball_tracker_test.cpp b/src/software/sensor_fusion/filter/ball_tracker_test.cpp deleted file mode 100644 index 5de695766f..0000000000 --- a/src/software/sensor_fusion/filter/ball_tracker_test.cpp +++ /dev/null @@ -1,579 +0,0 @@ -#include "software/sensor_fusion/filter/ball_tracker.h" - -#include - -#include -#include -#include - -#include "shared/constants.h" -#include "software/geom/algorithms/distance.h" -#include "software/geom/ray.h" -#include "software/geom/segment.h" -#include "software/world/field.h" - -class BallTrackerTest : public ::testing::Test -{ - protected: - BallTrackerTest() - : field(Field::createSSLDivisionBField()), - ball_tracker(), - current_timestamp(Timestamp::fromSeconds(123)), - time_step(Duration::fromSeconds(1.0 / 60.0)) - { - } - - void SetUp() override - { - // Use a constant seed to results are deterministic - random_generator.seed(1); - } - - /** - * Simulates the ball moving in a certain direction with a specified amount of noise - * in the measurements, and verifies that the filtered ball data is within the given - * tolerances of the real ball. - * - * @param start_time The time at which the ball starts moving - * @param ball_trajectory A ray indicating the ball's starting position and direction - * of travel - * @param ball_velocity_magnitude How fast the ball is moving - * @param ball_position_variance The variance in the noise the ball's position will be - * sampled with - * @param time_step_variance The variance in the timestep the ball will be sampled - * with - * @param expected_position_tolerance How close to the real position the filtered ball - * position should be - * @param expected_velocity_angle_tolerance How close to the real velocity angle the - * filtered ball velocity angle should be - * @param expected_velocity_magnitude_tolerance How close to the real velocity - * magnitude the filtered ball velocity magnitude should be - * @param num_iterations For how many iterations the simulation should run - * @param num_steps_to_ignore The number of iterations at the beginning of the - * simulation to not check tolerances - */ - void testFilterAlongBallTrajectory( - const Timestamp& start_time, const Ray& ball_trajectory, - double ball_velocity_magnitude, double ball_position_variance, - double time_step_variance, double expected_position_tolerance, - Angle expected_velocity_angle_tolerance, - double expected_velocity_magnitude_tolerance, unsigned int num_iterations, - unsigned int num_steps_to_ignore) - { - Point ball_starting_position = ball_trajectory.getStart(); - Vector ball_velocity = - ball_trajectory.toUnitVector().normalize(ball_velocity_magnitude); - Duration max_ball_travel_duration = - Duration::fromSeconds(std::numeric_limits::max()); - - testFilterHelper(start_time, ball_starting_position, ball_velocity, - ball_position_variance, time_step_variance, - expected_position_tolerance, expected_velocity_angle_tolerance, - expected_velocity_magnitude_tolerance, num_iterations, - max_ball_travel_duration, num_steps_to_ignore); - } - - /** - * Simulates the ball moving in a line, starting at one point and ending at another - * with a specified amount of noise in the measurements, and verifies the filtered - * ball data is within the given tolerances of the real ball. - * - * @param start_time The time at which the ball starts moving - * @param ball_path The segment along which the ball will travel - * @param ball_velocity_magnitude How fast the ball is moving - * @param ball_position_variance The variance in the noise the ball's position will be - * sampled with - * @param time_step_variance The variance in the timestep the ball will be sampled - * with - * @param expected_position_tolerance How close to the real position the filtered ball - * position should be - * @param expected_velocity_angle_tolerance How close to the real velocity angle the - * filtered ball velocity angle should be - * @param expected_velocity_magnitude_tolerance How close to the real velocity - * magnitude the filtered ball velocity magnitude should be - * @param num_steps_to_ignore The number of iterations at the beginning of the - * simulation to not check tolerances - */ - void testFilterAlongLineSegment(const Timestamp& start_time, const Segment& ball_path, - double ball_velocity_magnitude, - double ball_position_variance, - double time_step_variance, - double expected_position_tolerance, - Angle expected_velocity_angle_tolerance, - double expected_velocity_magnitude_tolerance, - unsigned int num_steps_to_ignore) - { - Point ball_starting_position = ball_path.getStart(); - Vector ball_velocity = ball_path.toVector().normalize(ball_velocity_magnitude); - // Check for division by 0 - if (ball_velocity_magnitude == 0) - { - throw std::invalid_argument( - "ball_velocity_magnitude with value of 0 given, this will result in division by 0"); - } - // Calculate how many simulation steps to take, given the ball's velocity and the - // time step in order for the ball to reach the end of the given segment. - Duration max_ball_travel_duration = - Duration::fromSeconds(ball_path.length() / ball_velocity_magnitude); - int num_iterations = static_cast( - std::round(max_ball_travel_duration.toSeconds() / time_step.toSeconds())); - - testFilterHelper(start_time, ball_starting_position, ball_velocity, - ball_position_variance, time_step_variance, - expected_position_tolerance, expected_velocity_angle_tolerance, - expected_velocity_magnitude_tolerance, num_iterations, - max_ball_travel_duration, num_steps_to_ignore); - } - - /** - * A helper function that simulates the ball moving in a straight line. The ball's - * position will be sampled with a given amount of noise / variance, and the filtered - * data will be checked to verify it is within the specified tolerances compared to - * the real ball - * @param start_time The time at which the ball starts moving - * @param ball_starting_position The position the ball starts from - * @param ball_velocity The velocity of the ball - * @param ball_position_variance The variance in the noise the ball's position will be - * sampled with - * @param time_step_variance The variance in the timestep the ball will be sampled - * with - * @param expected_position_tolerance How close to the real position the filtered ball - * position should be - * @param expected_velocity_angle_tolerance How close to the real velocity angle the - * filtered ball velocity angle should be - * @param expected_velocity_magnitude_tolerance How close to the real velocity - * magnitude the filtered ball velocity magnitude should be - * @param num_iterations For how many iterations the simulation should run - * @param max_ball_travel_duration The maximum duration for which the ball may travel - * / be simulated - * @param num_steps_to_ignore The number of iterations at the beginning of the - * simulation to not check tolerances - */ - void testFilterHelper(const Timestamp& start_time, - const Point& ball_starting_position, - const Vector& ball_velocity, double ball_position_variance, - double time_step_variance, double expected_position_tolerance, - const Angle& expected_velocity_angle_tolerance, - double expected_velocity_magnitude_tolerance, - unsigned int num_iterations, - const Duration& max_ball_travel_duration, - unsigned int num_steps_to_ignore = 0) - { - // Create the distributions we use to generate noise when sampling the ball - std::normal_distribution position_noise_distribution( - 0, ball_position_variance); - std::normal_distribution time_step_noise_distribution(0, - time_step_variance); - - for (unsigned i = 0; i < num_iterations; i++) - { - // Generate the noise that will be added to the position and time step to - // simulate imperfect data - Vector position_noise(position_noise_distribution(random_generator), - position_noise_distribution(random_generator)); - Duration time_step_noise = - Duration::fromSeconds(time_step_noise_distribution(random_generator)); - - // Calculate the current time and add noise - // We make sure the applied noise doesn't cause the timestamp to be larger - // than expected on the last iteration so the ball's final position is close - // to what's expected by the caller of this function - Timestamp current_timestamp_with_noise = - start_time + Duration::fromSeconds(i * time_step.toSeconds() + - time_step_noise.toSeconds()); - this->current_timestamp = std::min(current_timestamp_with_noise, - start_time + max_ball_travel_duration); - - // Take the time difference from the start time and calculate the ball's - // current position based on it's velocity and the elapsed time - Duration time_diff = current_timestamp - start_time; - Point current_ball_position = - ball_starting_position + - ball_velocity.normalize(ball_velocity.length() * time_diff.toSeconds()); - - // Apply noise to the ball's position to simulate measurement noise - Point ball_position_with_noise = current_ball_position + position_noise; - - // Create the detection that would have been seen by the vision system - std::vector ball_detections = { - BallDetection{ball_position_with_noise, BALL_DISTANCE_FROM_GROUND, - current_timestamp, 0.9}}; - - // Get the filtered result given the new detection information - auto filtered_ball = - ball_tracker.estimateBallState(ball_detections, field.fieldBoundary()); - if (i < num_steps_to_ignore) - { - continue; - } - - ASSERT_TRUE(filtered_ball); - double ball_position_difference = - (filtered_ball->position() - current_ball_position).length(); - EXPECT_LT(ball_position_difference, expected_position_tolerance); - // Only check the velocity once we have more than 1 data entry in the filter - // since the filter can't return a realistic velocity with only a single - // detection - if (i > 0) - { - // Check the direction of the velocity - double velocity_orientation_difference = - std::fabs(filtered_ball->velocity() - .orientation() - .minDiff(ball_velocity.orientation()) - .toDegrees()); - EXPECT_LE(velocity_orientation_difference, - expected_velocity_angle_tolerance.toDegrees()); - // Check the magnitude of the velocity - double velocity_magnitude_difference = std::fabs( - filtered_ball->velocity().length() - ball_velocity.length()); - EXPECT_LE(velocity_magnitude_difference, - expected_velocity_magnitude_tolerance); - } - - // Make sure the timestamps are always increasing - EXPECT_GE(filtered_ball->timestamp(), current_timestamp); - } - } - - Field field; - BallTracker ball_tracker; - Timestamp current_timestamp; - Duration time_step; - std::mt19937 random_generator; - // For these tests, the ball is always on the ground. The filters - // are not designed for filtering balls in the air - static constexpr double BALL_DISTANCE_FROM_GROUND = 0.0; -}; - -TEST_F(BallTrackerTest, ball_sitting_still_with_low_noise) -{ - Ray ball_trajectory = Ray(Point(0, 0), Vector(0, 0)); - double ball_velocity_magnitude = 0; - double ball_position_variance = 0.001; - double time_step_variance = 0.001; - double expected_position_tolerance = 0.005; - // When the ball is sitting still, the velocity could be any direction so we do - // not use a strict tolerance for this test. We only really care about the - // magnitude of the velocity to make sure it's small enough - Angle expected_velocity_angle_tolernace = Angle::threeQuarter(); - double expected_velocity_magnitude_tolerance = 0.075; - int num_simulation_steps = 200; - int num_steps_to_ignore = 5; - Timestamp start_time = current_timestamp; - - testFilterAlongBallTrajectory( - start_time, ball_trajectory, ball_velocity_magnitude, ball_position_variance, - time_step_variance, expected_position_tolerance, - expected_velocity_angle_tolernace, expected_velocity_magnitude_tolerance, - num_simulation_steps, num_steps_to_ignore); -} - -TEST_F(BallTrackerTest, ball_sitting_still_with_moderate_noise) -{ - Ray ball_trajectory = Ray(Point(0, 0), Vector(0, 0)); - double ball_velocity_magnitude = 0; - double ball_position_variance = 0.003; - double time_step_variance = 0.003; - double expected_position_tolerance = 0.016; - // When the ball is sitting still, the velocity could be any direction so we do - // not use a strict tolerance for this test. We only really care about the - // magnitude of the velocity to make sure it's small enough - Angle expected_velocity_angle_tolernace = Angle::threeQuarter(); - double expected_velocity_magnitude_tolerance = 1.0; - int num_simulation_steps = 10; - int num_steps_to_ignore = 5; - Timestamp start_time = current_timestamp; - - testFilterAlongBallTrajectory( - start_time, ball_trajectory, ball_velocity_magnitude, ball_position_variance, - time_step_variance, expected_position_tolerance, - expected_velocity_angle_tolernace, expected_velocity_magnitude_tolerance, - num_simulation_steps, num_steps_to_ignore); -} - -TEST_F(BallTrackerTest, ball_moving_slow_in_a_straight_line_with_no_noise_in_data) -{ - Segment ball_path = Segment(field.friendlyCornerNeg(), field.enemyCornerPos()); - double ball_velocity_magnitude = 0.31; - double ball_position_variance = 0; - double time_step_variance = 0; - double expected_position_tolerance = 0.001; - Angle expected_velocity_angle_tolernace = Angle::fromDegrees(0.01); - double expected_velocity_magnitude_tolerance = 0.01; - int num_steps_to_ignore = 5; - Timestamp start_time = current_timestamp; - - testFilterAlongLineSegment( - start_time, ball_path, ball_velocity_magnitude, ball_position_variance, - time_step_variance, expected_position_tolerance, - expected_velocity_angle_tolernace, expected_velocity_magnitude_tolerance, - num_steps_to_ignore); -} - -TEST_F(BallTrackerTest, ball_moving_slow_in_a_straight_line_with_small_noise_in_data) -{ - Segment ball_path = Segment(field.friendlyCornerNeg(), field.enemyCornerPos()); - double ball_velocity_magnitude = 0.3; - double ball_position_variance = 0.001; - double time_step_variance = 0.001; - double expected_position_tolerance = 0.004; - Angle expected_velocity_angle_tolernace = Angle::fromDegrees(5.5); - double expected_velocity_magnitude_tolerance = 0.04; - int num_steps_to_ignore = 5; - Timestamp start_time = current_timestamp; - - testFilterAlongLineSegment( - start_time, ball_path, ball_velocity_magnitude, ball_position_variance, - time_step_variance, expected_position_tolerance, - expected_velocity_angle_tolernace, expected_velocity_magnitude_tolerance, - num_steps_to_ignore); -} - -TEST_F(BallTrackerTest, ball_moving_slow_in_a_straight_line_with_medium_noise_in_data) -{ - Segment ball_path = Segment(field.friendlyCornerNeg(), field.enemyCornerPos()); - double ball_velocity_magnitude = 0.3; - double ball_position_variance = 0.003; - double time_step_variance = 0.001; - double expected_position_tolerance = 0.011; - Angle expected_velocity_angle_tolernace = Angle::fromDegrees(16); - double expected_velocity_magnitude_tolerance = 0.11; - int num_steps_to_ignore = 5; - Timestamp start_time = current_timestamp; - - testFilterAlongLineSegment( - start_time, ball_path, ball_velocity_magnitude, ball_position_variance, - time_step_variance, expected_position_tolerance, - expected_velocity_angle_tolernace, expected_velocity_magnitude_tolerance, - num_steps_to_ignore); -} - -TEST_F(BallTrackerTest, ball_moving_fast_in_a_straight_line_with_no_noise_in_data) -{ - Segment ball_path = Segment(field.friendlyCornerNeg(), field.enemyCornerPos()); - double ball_velocity_magnitude = 6.2; - double ball_position_variance = 0; - double time_step_variance = 0; - double expected_position_tolerance = 0.001; - Angle expected_velocity_angle_tolernace = Angle::fromDegrees(0.01); - double expected_velocity_magnitude_tolerance = 0.01; - int num_steps_to_ignore = 5; - Timestamp start_time = current_timestamp; - - testFilterAlongLineSegment( - start_time, ball_path, ball_velocity_magnitude, ball_position_variance, - time_step_variance, expected_position_tolerance, - expected_velocity_angle_tolernace, expected_velocity_magnitude_tolerance, - num_steps_to_ignore); -} - -TEST_F(BallTrackerTest, ball_moving_fast_in_a_straight_line_with_small_noise_in_data) -{ - Segment ball_path = Segment(field.friendlyCornerNeg(), field.enemyCornerPos()); - double ball_velocity_magnitude = 5.72; - double ball_position_variance = 0.001; - double time_step_variance = 0.001; - double expected_position_tolerance = 0.003; - Angle expected_velocity_angle_tolernace = Angle::fromDegrees(0.9); - double expected_velocity_magnitude_tolerance = 0.07; - int num_steps_to_ignore = 5; - Timestamp start_time = current_timestamp; - - testFilterAlongLineSegment( - start_time, ball_path, ball_velocity_magnitude, ball_position_variance, - time_step_variance, expected_position_tolerance, - expected_velocity_angle_tolernace, expected_velocity_magnitude_tolerance, - num_steps_to_ignore); -} - -TEST_F(BallTrackerTest, ball_moving_fast_in_a_straight_line_with_medium_noise_in_data) -{ - Segment ball_path = Segment(field.friendlyCornerNeg(), field.enemyCornerPos()); - double ball_velocity_magnitude = 5.04; - double ball_position_variance = 0.003; - double time_step_variance = 0.001; - double expected_position_tolerance = 0.008; - Angle expected_velocity_angle_tolerance = Angle::fromDegrees(3.0); - double expected_velocity_magnitude_tolerance = 0.21; - int num_steps_to_ignore = 5; - Timestamp start_time = current_timestamp; - - testFilterAlongLineSegment( - start_time, ball_path, ball_velocity_magnitude, ball_position_variance, - time_step_variance, expected_position_tolerance, - expected_velocity_angle_tolerance, expected_velocity_magnitude_tolerance, - num_steps_to_ignore); -} - -TEST_F(BallTrackerTest, - ball_moving_fast_in_a_straight_line_and_then_bouncing_with_no_noise_in_data) -{ - Segment ball_path = Segment(field.friendlyCornerNeg(), field.enemyCornerPos()); - double ball_velocity_magnitude = 5.04; - double ball_position_variance = 0; - double time_step_variance = 0; - double expected_position_tolerance = 0.0001; - Angle expected_velocity_angle_tolernace = Angle::fromDegrees(0.01); - double expected_velocity_magnitude_tolerance = 0.01; - int num_steps_to_ignore = 5; - Timestamp start_time = current_timestamp; - - testFilterAlongLineSegment( - start_time, ball_path, ball_velocity_magnitude, ball_position_variance, - time_step_variance, expected_position_tolerance, - expected_velocity_angle_tolernace, expected_velocity_magnitude_tolerance, - num_steps_to_ignore); - - ball_path = Segment(field.enemyCornerPos(), field.enemyCornerNeg()); - ball_velocity_magnitude = 4.8; - expected_position_tolerance = 0.0001; - expected_velocity_angle_tolernace = Angle::fromDegrees(0.01); - expected_velocity_magnitude_tolerance = 0.01; - num_steps_to_ignore = 5; - start_time = current_timestamp; - - testFilterAlongLineSegment( - start_time, ball_path, ball_velocity_magnitude, ball_position_variance, - time_step_variance, expected_position_tolerance, - expected_velocity_angle_tolernace, expected_velocity_magnitude_tolerance, - num_steps_to_ignore); -} - -TEST_F(BallTrackerTest, - ball_moving_fast_in_a_straight_line_and_then_bouncing_with_no_noise_in_data_2) -{ - Segment ball_path = - Segment(field.friendlyCornerNeg(), field.friendlyHalf().posXPosYCorner()); - double ball_velocity_magnitude = 5.04; - double ball_position_variance = 0; - double time_step_variance = 0; - double expected_position_tolerance = 0.0001; - Angle expected_velocity_angle_tolernace = Angle::fromDegrees(0.1); - double expected_velocity_magnitude_tolerance = 0.1; - int num_steps_to_ignore = 5; - Timestamp start_time = current_timestamp; - - testFilterAlongLineSegment( - start_time, ball_path, ball_velocity_magnitude, ball_position_variance, - time_step_variance, expected_position_tolerance, - expected_velocity_angle_tolernace, expected_velocity_magnitude_tolerance, - num_steps_to_ignore); - - ball_path = Segment(field.friendlyHalf().posXPosYCorner(), field.enemyCornerNeg()); - ball_velocity_magnitude = 4.8; - expected_position_tolerance = 0.0001; - expected_velocity_angle_tolernace = Angle::fromDegrees(0.1); - expected_velocity_magnitude_tolerance = 0.1; - num_steps_to_ignore = 5; - start_time = current_timestamp; - - testFilterAlongLineSegment( - start_time, ball_path, ball_velocity_magnitude, ball_position_variance, - time_step_variance, expected_position_tolerance, - expected_velocity_angle_tolernace, expected_velocity_magnitude_tolerance, - num_steps_to_ignore); -} - -TEST_F(BallTrackerTest, - ball_moving_fast_in_a_straight_line_and_then_bouncing_with_no_noise_in_data_3) -{ - Segment ball_path = Segment(Point(-3, -1), Point(0, 0)); - double ball_velocity_magnitude = 5.04; - double ball_position_variance = 0; - double time_step_variance = 0; - double expected_position_tolerance = 0.0001; - Angle expected_velocity_angle_tolernace = Angle::fromDegrees(0.1); - double expected_velocity_magnitude_tolerance = 0.1; - int num_steps_to_ignore = 5; - Timestamp start_time = current_timestamp; - - testFilterAlongLineSegment( - start_time, ball_path, ball_velocity_magnitude, ball_position_variance, - time_step_variance, expected_position_tolerance, - expected_velocity_angle_tolernace, expected_velocity_magnitude_tolerance, - num_steps_to_ignore); - - ball_path = Segment(Point(0, 0), Point(3, 0)); - ball_velocity_magnitude = 4.8; - expected_position_tolerance = 0.0001; - expected_velocity_angle_tolernace = Angle::fromDegrees(0.1); - expected_velocity_magnitude_tolerance = 0.1; - num_steps_to_ignore = 5; - start_time = current_timestamp; - - testFilterAlongLineSegment( - start_time, ball_path, ball_velocity_magnitude, ball_position_variance, - time_step_variance, expected_position_tolerance, - expected_velocity_angle_tolernace, expected_velocity_magnitude_tolerance, - num_steps_to_ignore); -} - -TEST_F(BallTrackerTest, ball_moving_along_x_axis) -{ - Segment ball_path = Segment(field.friendlyGoalCenter(), field.enemyGoalCenter()); - double ball_velocity_magnitude = 5; - double ball_position_variance = 0; - double time_step_variance = 0; - double expected_position_tolerance = 0.001; - Angle expected_velocity_angle_tolernace = Angle::fromDegrees(0.01); - double expected_velocity_magnitude_tolerance = 0.01; - int num_steps_to_ignore = 5; - Timestamp start_time = current_timestamp; - - testFilterAlongLineSegment( - start_time, ball_path, ball_velocity_magnitude, ball_position_variance, - time_step_variance, expected_position_tolerance, - expected_velocity_angle_tolernace, expected_velocity_magnitude_tolerance, - num_steps_to_ignore); -} - -TEST_F(BallTrackerTest, ball_moving_along_y_axis) -{ - Segment ball_path = field.halfwayLine(); - double ball_velocity_magnitude = 5; - double ball_position_variance = 0; - double time_step_variance = 0; - double expected_position_tolerance = 0.001; - Angle expected_velocity_angle_tolernace = Angle::fromDegrees(0.01); - double expected_velocity_magnitude_tolerance = 0.01; - int num_steps_to_ignore = 5; - Timestamp start_time = current_timestamp; - - testFilterAlongLineSegment( - start_time, ball_path, ball_velocity_magnitude, ball_position_variance, - time_step_variance, expected_position_tolerance, - expected_velocity_angle_tolernace, expected_velocity_magnitude_tolerance, - num_steps_to_ignore); -} - -TEST_F(BallTrackerTest, ball_moving_fast_with_vision_delay) -{ - // Setup a fast moving ball (e.g., 5.0 m/s) - Vector ball_velocity(5.0, 0.0); - Point ball_start(0, 0); - - // Simulate a 35ms SSL-Vision processing delay - Duration vision_delay = Duration::fromSeconds(0.035); - - // Feed the filter a few frames to build up velocity - for(int i = 0; i < 10; i++) { - Timestamp t_capture = current_timestamp + Duration::fromSeconds(i * 0.016); - Point pos = ball_start + ball_velocity * (i * 0.016); - - std::vector det = {BallDetection{pos, 0.0, t_capture, 1.0}}; - - // The crucial change: we ask the filter for the state at t_capture + vision_delay - Timestamp t_now = t_capture + vision_delay; - auto filtered_ball = ball_tracker.estimateBallState(det, field.fieldBoundary(), t_now); - - - // THE ASSERTION: - // Without latency compensation (current code), the ball will be 17.5cm behind (5.0m/s * 0.035s). - // With your new Look-Ahead logic, the position should match the projected position. - Point expected_projected_pos = ball_start + ball_velocity * (9 * 0.016 + 0.035); - double error = (filtered_ball->position() - expected_projected_pos).length(); - EXPECT_LT(error, 0.05); // Expect error to be less than 5cm - } - -} From 815f028b5b11c67613852f84e26b5e48ad0691e4 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Thu, 14 May 2026 21:14:07 -0700 Subject: [PATCH 38/57] pass argument for simulated hrvo test --- src/software/ai/navigator/trajectory/simulated_hrvo_test.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/software/ai/navigator/trajectory/simulated_hrvo_test.py b/src/software/ai/navigator/trajectory/simulated_hrvo_test.py index 5bcd2a99d2..c518183571 100644 --- a/src/software/ai/navigator/trajectory/simulated_hrvo_test.py +++ b/src/software/ai/navigator/trajectory/simulated_hrvo_test.py @@ -49,7 +49,7 @@ def __init__( self.is_stationary = True - def get_validation_status(self, world: tbots.World) -> ValidationStatus: + def get_validation_status(self, world: tbots.World, simulator_state=None) -> ValidationStatus: """Checks if a specific robot id is in the provided region Then checks if that robot is stationary within a threshold for the provided number of ticks From 0f583b211f476c1c93ac57b8fa93644677fc6ff6 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci-lite[bot]" <117423508+pre-commit-ci-lite[bot]@users.noreply.github.com> Date: Fri, 15 May 2026 04:22:16 +0000 Subject: [PATCH 39/57] [pre-commit.ci lite] apply automatic fixes --- src/software/ai/navigator/trajectory/simulated_hrvo_test.py | 4 +++- src/software/simulated_tests/validation/avoid_collisions.py | 4 +++- src/software/simulated_tests/validation/validation.py | 4 +++- 3 files changed, 9 insertions(+), 3 deletions(-) diff --git a/src/software/ai/navigator/trajectory/simulated_hrvo_test.py b/src/software/ai/navigator/trajectory/simulated_hrvo_test.py index c518183571..fa4bc27dee 100644 --- a/src/software/ai/navigator/trajectory/simulated_hrvo_test.py +++ b/src/software/ai/navigator/trajectory/simulated_hrvo_test.py @@ -49,7 +49,9 @@ def __init__( self.is_stationary = True - def get_validation_status(self, world: tbots.World, simulator_state=None) -> ValidationStatus: + def get_validation_status( + self, world: tbots.World, simulator_state=None + ) -> ValidationStatus: """Checks if a specific robot id is in the provided region Then checks if that robot is stationary within a threshold for the provided number of ticks diff --git a/src/software/simulated_tests/validation/avoid_collisions.py b/src/software/simulated_tests/validation/avoid_collisions.py index 109ba2c06e..44d71805d1 100644 --- a/src/software/simulated_tests/validation/avoid_collisions.py +++ b/src/software/simulated_tests/validation/avoid_collisions.py @@ -21,7 +21,9 @@ def __init__(self): self.fouled_robots = [] @override - def get_validation_status(self, world: World, simulator_state=None) -> ValidationStatus: + def get_validation_status( + self, world: World, simulator_state=None + ) -> ValidationStatus: """Checks if any 2 robots in the world have collided :param world: the World message to validate diff --git a/src/software/simulated_tests/validation/validation.py b/src/software/simulated_tests/validation/validation.py index 19df55e4cb..3f74e41278 100644 --- a/src/software/simulated_tests/validation/validation.py +++ b/src/software/simulated_tests/validation/validation.py @@ -188,7 +188,9 @@ def create_validation_proto_helper(validation_proto_set, validation): validation_proto = ValidationProto() # Get status - status = validation.get_validation_status(world, simulator_state=simulator_state) + status = validation.get_validation_status( + world, simulator_state=simulator_state + ) # Create validation proto validation_proto.status = status From 9f5e314f3cc4509e31e3d94f711ae21b316b3ab2 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Fri, 15 May 2026 11:47:00 -0700 Subject: [PATCH 40/57] add dribbling heuristic -- Decrease measuerement noise when near dribbler --- .../sensor_fusion/filter/ball_tracker.cpp | 6 ++++++ .../validation/excessive_dribbling.py | 4 +++- .../simulated_tests/validation/validation.py | 16 ++++++++++++++++ 3 files changed, 25 insertions(+), 1 deletion(-) diff --git a/src/software/sensor_fusion/filter/ball_tracker.cpp b/src/software/sensor_fusion/filter/ball_tracker.cpp index 3fab3f7e7d..ad504a9b6c 100644 --- a/src/software/sensor_fusion/filter/ball_tracker.cpp +++ b/src/software/sensor_fusion/filter/ball_tracker.cpp @@ -26,6 +26,10 @@ namespace { 0.0226, 0, 0, 0.00445).finished(); + const Eigen::Matrix R_DRIBBLING = (Eigen::Matrix() << + 0.0001, 0, + 0, 0.0001).finished(); + const Eigen::Matrix C = (Eigen::Matrix() << 1, 0, 0, 0, 0, 1, 0, 0).finished(); @@ -91,7 +95,9 @@ std::optional BallTracker::estimateBallState( { measurement << best_ball_detection->position.x(), best_ball_detection->position.y(); + kalman_filter.measurement_covariance = R_DRIBBLING; kalman_filter.update(measurement); + kalman_filter.measurement_covariance = R; } else { diff --git a/src/software/simulated_tests/validation/excessive_dribbling.py b/src/software/simulated_tests/validation/excessive_dribbling.py index 24f842fb9f..78b330d729 100644 --- a/src/software/simulated_tests/validation/excessive_dribbling.py +++ b/src/software/simulated_tests/validation/excessive_dribbling.py @@ -5,6 +5,7 @@ Validation, create_validation_geometry, create_validation_types, + get_ball_pos, ) from typing import override @@ -27,7 +28,8 @@ def get_validation_status(self, world, simulator_state=None) -> ValidationStatus :return: FAILING when the robot is excessively dribbling PASSING when the robot is not excessively dribbling """ - ball_position = tbots_cpp.createPoint(world.ball.current_state.global_position) + bx, by = get_ball_pos(world, simulator_state) + ball_position = tbots_cpp.Point(bx, by) for robot in world.friendly_team.team_robots: if tbots_cpp.Robot(robot).isNearDribbler( ball_position, self.dribbler_tolerance diff --git a/src/software/simulated_tests/validation/validation.py b/src/software/simulated_tests/validation/validation.py index 19df55e4cb..b44cccb95f 100644 --- a/src/software/simulated_tests/validation/validation.py +++ b/src/software/simulated_tests/validation/validation.py @@ -239,6 +239,22 @@ def check_validation(validation_proto_set): raise AssertionError(validation_proto.failure_msg) +def get_ball_vel(world, simulator_state): + """Returns (vx, vy) using true simulator velocity if available.""" + if simulator_state is not None and simulator_state.HasField("ball"): + return simulator_state.ball.v_x, simulator_state.ball.v_y + v = world.ball.current_state.global_velocity + return v.x_component_meters, v.y_component_meters + + +def get_ball_pos(world, simulator_state): + """Returns (x_m, y_m) using true simulator position if available.""" + if simulator_state is not None and simulator_state.HasField("ball"): + return simulator_state.ball.p_x, simulator_state.ball.p_y + p = world.ball.current_state.global_position + return p.x_meters, p.y_meters + + def create_validation_geometry(geometry=[]) -> ValidationGeometry: """Creates a ValidationGeometry which is a visual representation of the validation to be rendered as either green (PASSING) or red (FAILING) From 079ba91c24f96479dbc30bf1f12593cadde30776 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Mon, 18 May 2026 11:21:56 -0700 Subject: [PATCH 41/57] new --- src/software/er_force_simulator_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/software/er_force_simulator_main.cpp b/src/software/er_force_simulator_main.cpp index c6faf80429..e5fbaf9b61 100644 --- a/src/software/er_force_simulator_main.cpp +++ b/src/software/er_force_simulator_main.cpp @@ -48,7 +48,7 @@ int main(int argc, char** argv) { std::string runtime_dir = args.runtime_dir; LoggerSingleton::initializeLogger(runtime_dir, nullptr); - LOG(CSV, "realism_kalman_filter_v8_2.csv") << "timestamp_s,fused_x,fused_y,fused_vel_x, fused_vel_y, truth_x,truth_y, true_vel_x, true_vel_y,is_occluded\n"; + LOG(CSV, "realism_kalman_filter_v10.csv") << "timestamp_s,fused_x,fused_y,fused_vel_x, fused_vel_y, truth_x,truth_y, true_vel_x, true_vel_y,is_occluded\n"; /** * Creates a ER force simulator and sets up the appropriate @@ -228,7 +228,7 @@ int main(int argc, char** argv) { start_timestamp_s = current_ts; } - LOG(CSV, "realism_kalman_filter_v8_2.csv") + LOG(CSV, "realism_kalman_filter_v10.csv") << (current_ts - start_timestamp_s) << "," << yellow_vision.ball().current_state().global_position().x_meters() << "," From 9c19dab1fd6b9cb62039ec6dba81f9c2c2560222 Mon Sep 17 00:00:00 2001 From: StarrryNight Date: Mon, 18 May 2026 11:41:08 -0700 Subject: [PATCH 42/57] add validation --- .../simulated_tests/validation/ball_speed_threshold.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/software/simulated_tests/validation/ball_speed_threshold.py b/src/software/simulated_tests/validation/ball_speed_threshold.py index a1ba9a34e4..2db2e02b04 100644 --- a/src/software/simulated_tests/validation/ball_speed_threshold.py +++ b/src/software/simulated_tests/validation/ball_speed_threshold.py @@ -7,6 +7,7 @@ Validation, create_validation_geometry, create_validation_types, + get_ball_vel, ) from typing import override @@ -29,10 +30,8 @@ def get_validation_status(self, world, simulator_state=None) -> ValidationStatus :return: FAILING if the ball speed is below some threshold PASSING if the ball speed is at or above some threshold """ - if ( - tbots_cpp.createVector(world.ball.current_state.global_velocity).length() - >= self.speed_threshold - ): + vx, vy = get_ball_vel(world, simulator_state) + if (vx**2 + vy**2) ** 0.5 >= self.speed_threshold: return ValidationStatus.PASSING return ValidationStatus.FAILING From cc195490f1cbadb1c59fb44999b7adde09c0d015 Mon Sep 17 00:00:00 2001 From: StarrryNight Date: Mon, 18 May 2026 11:49:44 -0700 Subject: [PATCH 43/57] Use simulator ball position and velocity --- .../validation/ball_enters_region.py | 7 +++---- .../validation/ball_kicked_in_direction.py | 11 ++++------- .../validation/ball_moves_in_direction.py | 9 ++++----- .../validation/ball_speed_threshold.py | 6 ++---- .../validation/ball_stops_in_region.py | 11 ++++------- .../validation/excessive_dribbling.py | 3 ++- .../validation/friendly_has_ball_possession.py | 3 ++- .../validation/friendly_receives_ball_slow.py | 7 +++---- .../validation/robot_received_ball.py | 3 ++- .../simulated_tests/validation/validation.py | 14 ++++++++++++++ 10 files changed, 40 insertions(+), 34 deletions(-) diff --git a/src/software/simulated_tests/validation/ball_enters_region.py b/src/software/simulated_tests/validation/ball_enters_region.py index 15c1cb6cb2..7363002664 100644 --- a/src/software/simulated_tests/validation/ball_enters_region.py +++ b/src/software/simulated_tests/validation/ball_enters_region.py @@ -5,6 +5,7 @@ Validation, create_validation_geometry, create_validation_types, + get_ball_pos, ) from typing import override @@ -24,11 +25,9 @@ def get_validation_status(self, world, simulator_state=None) -> ValidationStatus :return: FAILING until a ball enters any of the regions PASSING when a ball enters """ - self.ball_position = world.ball.current_state.global_position + self.ball_position = get_ball_pos(world, simulator_state) for region in self.regions: - if tbots_cpp.contains( - region, tbots_cpp.createPoint(world.ball.current_state.global_position) - ): + if tbots_cpp.contains(region, self.ball_position): return ValidationStatus.PASSING return ValidationStatus.FAILING diff --git a/src/software/simulated_tests/validation/ball_kicked_in_direction.py b/src/software/simulated_tests/validation/ball_kicked_in_direction.py index 55f5339158..b3dab03848 100644 --- a/src/software/simulated_tests/validation/ball_kicked_in_direction.py +++ b/src/software/simulated_tests/validation/ball_kicked_in_direction.py @@ -5,6 +5,8 @@ Validation, create_validation_geometry, create_validation_types, + get_ball_pos, + get_ball_vel, ) from typing import override @@ -40,13 +42,8 @@ def get_validation_status(self, world, simulator_state=None) -> ValidationStatus :return: FAILING if the ball has not been kicked in the expected direction PASSING if the ball has been kicked in the expected direction """ - ball_pos = world.ball.current_state.global_position - ball_vel = world.ball.current_state.global_velocity - - point = tbots_cpp.Point(ball_pos.x_meters, ball_pos.y_meters) - vector = tbots_cpp.Vector( - ball_vel.x_component_meters, ball_vel.y_component_meters - ) + point = get_ball_pos(world, simulator_state) + vector = get_ball_vel(world, simulator_state) ball = tbots_cpp.Ball(point, vector, tbots_cpp.Timestamp()) diff --git a/src/software/simulated_tests/validation/ball_moves_in_direction.py b/src/software/simulated_tests/validation/ball_moves_in_direction.py index f4f138308a..97d9f74f4e 100644 --- a/src/software/simulated_tests/validation/ball_moves_in_direction.py +++ b/src/software/simulated_tests/validation/ball_moves_in_direction.py @@ -5,6 +5,7 @@ Validation, create_validation_geometry, create_validation_types, + get_ball_pos, ) from typing import override @@ -31,7 +32,7 @@ def get_validation_status(self, world, simulator_state=None) -> ValidationStatus :return: FAILING if ball doesn't move in the direction PASSING if ball moves in the direction """ - current_ball_position = world.ball.current_state.global_position.x_meters + current_ball_position = get_ball_pos(world, simulator_state).x() # if max displacement is not set or current ball is moving in the right direction # set it and return PASSING @@ -86,10 +87,8 @@ def __init__(self, initial_ball_position, regions=[], tolerance: float = 0.1): @override def get_validation_status(self, world, simulator_state=None) -> ValidationStatus: for region in self.regions: - if tbots.contains( - region, tbots.createPoint(world.ball.current_state.global_position) - ): - return super().get_validation_status(world) + if tbots.contains(region, get_ball_pos(world, simulator_state)): + return super().get_validation_status(world, simulator_state=simulator_state) return ValidationStatus.PASSING diff --git a/src/software/simulated_tests/validation/ball_speed_threshold.py b/src/software/simulated_tests/validation/ball_speed_threshold.py index a1ba9a34e4..76e41b4ea0 100644 --- a/src/software/simulated_tests/validation/ball_speed_threshold.py +++ b/src/software/simulated_tests/validation/ball_speed_threshold.py @@ -7,6 +7,7 @@ Validation, create_validation_geometry, create_validation_types, + get_ball_vel, ) from typing import override @@ -29,10 +30,7 @@ def get_validation_status(self, world, simulator_state=None) -> ValidationStatus :return: FAILING if the ball speed is below some threshold PASSING if the ball speed is at or above some threshold """ - if ( - tbots_cpp.createVector(world.ball.current_state.global_velocity).length() - >= self.speed_threshold - ): + if get_ball_vel(world, simulator_state).length() >= self.speed_threshold: return ValidationStatus.PASSING return ValidationStatus.FAILING diff --git a/src/software/simulated_tests/validation/ball_stops_in_region.py b/src/software/simulated_tests/validation/ball_stops_in_region.py index f7b46efd2a..e8d6273e09 100644 --- a/src/software/simulated_tests/validation/ball_stops_in_region.py +++ b/src/software/simulated_tests/validation/ball_stops_in_region.py @@ -5,6 +5,8 @@ Validation, create_validation_geometry, create_validation_types, + get_ball_pos, + get_ball_vel, ) from typing import override @@ -25,13 +27,8 @@ def get_validation_status(self, world, simulator_state=None) -> ValidationStatus """ for region in self.regions: if tbots_cpp.contains( - region, tbots_cpp.createPoint(world.ball.current_state.global_position) - ) and ( - tbots_cpp.createVector( - world.ball.current_state.global_velocity - ).length() - <= 0.01 - ): + region, get_ball_pos(world, simulator_state) + ) and get_ball_vel(world, simulator_state).length() <= 0.01: return ValidationStatus.PASSING return ValidationStatus.FAILING diff --git a/src/software/simulated_tests/validation/excessive_dribbling.py b/src/software/simulated_tests/validation/excessive_dribbling.py index 24f842fb9f..d3f1ea70ca 100644 --- a/src/software/simulated_tests/validation/excessive_dribbling.py +++ b/src/software/simulated_tests/validation/excessive_dribbling.py @@ -5,6 +5,7 @@ Validation, create_validation_geometry, create_validation_types, + get_ball_pos, ) from typing import override @@ -27,7 +28,7 @@ def get_validation_status(self, world, simulator_state=None) -> ValidationStatus :return: FAILING when the robot is excessively dribbling PASSING when the robot is not excessively dribbling """ - ball_position = tbots_cpp.createPoint(world.ball.current_state.global_position) + ball_position = get_ball_pos(world, simulator_state) for robot in world.friendly_team.team_robots: if tbots_cpp.Robot(robot).isNearDribbler( ball_position, self.dribbler_tolerance diff --git a/src/software/simulated_tests/validation/friendly_has_ball_possession.py b/src/software/simulated_tests/validation/friendly_has_ball_possession.py index 58d73e3269..292c281bc6 100644 --- a/src/software/simulated_tests/validation/friendly_has_ball_possession.py +++ b/src/software/simulated_tests/validation/friendly_has_ball_possession.py @@ -5,6 +5,7 @@ Validation, create_validation_geometry, create_validation_types, + get_ball_pos, ) from typing import override @@ -29,7 +30,7 @@ def get_validation_status(self, world, simulator_state=None) -> ValidationStatus :return: FAILING when friendly robot does not have possession of the ball PASSING when friendly robot has possession of the ball """ - ball_position = tbots_cpp.createPoint(world.ball.current_state.global_position) + ball_position = get_ball_pos(world, simulator_state) for robot in world.friendly_team.team_robots: if self.robot_id is not None and robot.id != self.robot_id: continue diff --git a/src/software/simulated_tests/validation/friendly_receives_ball_slow.py b/src/software/simulated_tests/validation/friendly_receives_ball_slow.py index 68579e9550..14d8420c0d 100644 --- a/src/software/simulated_tests/validation/friendly_receives_ball_slow.py +++ b/src/software/simulated_tests/validation/friendly_receives_ball_slow.py @@ -3,6 +3,7 @@ from software.simulated_tests.validation.validation import ( create_validation_types, + get_ball_vel, ) from software.simulated_tests.validation.friendly_has_ball_possession import ( FriendlyHasBallPossession, @@ -33,10 +34,8 @@ def get_validation_status(self, world, simulator_state=None) -> ValidationStatus PASSING if the ball is not near the dribbler, or if it is near the dribbler at a speed slower than the max """ - if super().get_validation_status(world) == ValidationStatus.PASSING: - ball_velocity = tbots_cpp.createVector( - world.ball.current_state.global_velocity - ) + if super().get_validation_status(world, simulator_state=simulator_state) == ValidationStatus.PASSING: + ball_velocity = get_ball_vel(world, simulator_state) if ball_velocity.length() - self.max_receive_speed > 0.2: return ValidationStatus.FAILING return ValidationStatus.PASSING diff --git a/src/software/simulated_tests/validation/robot_received_ball.py b/src/software/simulated_tests/validation/robot_received_ball.py index 2f057dbfc0..a5599660f5 100644 --- a/src/software/simulated_tests/validation/robot_received_ball.py +++ b/src/software/simulated_tests/validation/robot_received_ball.py @@ -5,6 +5,7 @@ Validation, create_validation_geometry, create_validation_types, + get_ball_pos, ) from typing import override @@ -29,7 +30,7 @@ def get_validation_status(self, world, simulator_state=None) -> ValidationStatus :return: FAILING when the robot does not have the ball PASSING when the robot has the ball """ - ball_position = tbots_cpp.createPoint(world.ball.current_state.global_position) + ball_position = get_ball_pos(world, simulator_state) for robot in world.friendly_team.team_robots: if robot.id == self.robot_id: if tbots_cpp.Robot(robot).isNearDribbler(ball_position, self.tolerance): diff --git a/src/software/simulated_tests/validation/validation.py b/src/software/simulated_tests/validation/validation.py index 3f74e41278..cea43656b7 100644 --- a/src/software/simulated_tests/validation/validation.py +++ b/src/software/simulated_tests/validation/validation.py @@ -4,6 +4,20 @@ from abc import abstractmethod +def get_ball_pos(world, simulator_state=None): + """Return true ball position from simulator if available, else from world.""" + if simulator_state is not None and simulator_state.HasField("ball"): + return tbots_cpp.Point(simulator_state.ball.p_x, simulator_state.ball.p_y) + return tbots_cpp.createPoint(world.ball.current_state.global_position) + + +def get_ball_vel(world, simulator_state=None): + """Return true ball velocity from simulator if available, else from world.""" + if simulator_state is not None and simulator_state.HasField("ball"): + return tbots_cpp.Vector(simulator_state.ball.v_x, simulator_state.ball.v_y) + return tbots_cpp.createVector(world.ball.current_state.global_velocity) + + class Validation: """A validation function""" From bdb6c7e731994fbd60c46fc797c2f583cbd119f8 Mon Sep 17 00:00:00 2001 From: StarrryNight Date: Mon, 18 May 2026 12:26:03 -0700 Subject: [PATCH 44/57] allow stale buffer --- src/software/simulated_tests/simulated_test_fixture.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/software/simulated_tests/simulated_test_fixture.py b/src/software/simulated_tests/simulated_test_fixture.py index 8e275b8bc7..217ec022b1 100644 --- a/src/software/simulated_tests/simulated_test_fixture.py +++ b/src/software/simulated_tests/simulated_test_fixture.py @@ -202,7 +202,7 @@ def runner( # Fetch the latest true simulator state (non-blocking; None if not yet available) simulator_state = self.simulator_state_buffer.get( - block=False, return_cached=False + block=False, return_cached=True ) # Validate From 13d7832a184b6d0019efbaf3edcc0f9532d90a8a Mon Sep 17 00:00:00 2001 From: StarrryNight Date: Mon, 18 May 2026 13:40:08 -0700 Subject: [PATCH 45/57] tuning fixes crease defender test --- src/software/sensor_fusion/filter/ball_tracker.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/software/sensor_fusion/filter/ball_tracker.cpp b/src/software/sensor_fusion/filter/ball_tracker.cpp index ad504a9b6c..8031d06e1c 100644 --- a/src/software/sensor_fusion/filter/ball_tracker.cpp +++ b/src/software/sensor_fusion/filter/ball_tracker.cpp @@ -14,7 +14,11 @@ namespace { const Eigen::Vector INITIAL_STATE = Eigen::Vector::Zero(); - const Eigen::Matrix INITIAL_COV = Eigen::Matrix::Identity() * 1000.0; + const Eigen::Matrix INITIAL_COV = (Eigen::Matrix() << + 1000, 0, 0, 0, + 0, 1000, 0,0, + 0, 0, 10, 0, + 0, 0, 0, 10).finished(); const Eigen::Matrix Q = (Eigen::Matrix() << 2.222e-8, 0, 2.000e-6, 0, From cb4ebd818010a3b61c65bab5cce78924767557b1 Mon Sep 17 00:00:00 2001 From: StarrryNight Date: Mon, 18 May 2026 13:43:16 -0700 Subject: [PATCH 46/57] remove ball tracker --- src/software/sensor_fusion/filter/BUILD | 9 --------- src/software/simulated_tests/simulated_test_fixture.py | 1 - 2 files changed, 10 deletions(-) diff --git a/src/software/sensor_fusion/filter/BUILD b/src/software/sensor_fusion/filter/BUILD index dcee019f9a..ef8220817d 100644 --- a/src/software/sensor_fusion/filter/BUILD +++ b/src/software/sensor_fusion/filter/BUILD @@ -41,15 +41,6 @@ cc_library( ], ) -cc_test( - name = "ball_tracker_test", - srcs = ["ball_tracker_test.cpp"], - deps = [ - ":ball_tracker", - "//shared/test_util:tbots_gtest_main", - "//software/world:field", - ], -) cc_library( name = "robot_filter", diff --git a/src/software/simulated_tests/simulated_test_fixture.py b/src/software/simulated_tests/simulated_test_fixture.py index 444bf73d43..d220a7af7f 100644 --- a/src/software/simulated_tests/simulated_test_fixture.py +++ b/src/software/simulated_tests/simulated_test_fixture.py @@ -199,7 +199,6 @@ def runner( if self.thunderscope and tick_duration_s > processing_time: time.sleep(tick_duration_s - processing_time) - # Fetch the latest true simulator state (non-blocking; None if not yet available) simulator_state = self.simulator_state_buffer.get( block=False, return_cached=True ) From 6812d2729b67ec46ec75d42a1faec34ddc732453 Mon Sep 17 00:00:00 2001 From: StarrryNight Date: Mon, 18 May 2026 13:56:17 -0700 Subject: [PATCH 47/57] add sme logs for goalie tactci test to debug --- .../sensor_fusion/filter/ball_tracker.cpp | 10 +++++----- .../simulated_tests/simulated_test_fixture.py | 20 +++++++++++++++++++ 2 files changed, 25 insertions(+), 5 deletions(-) diff --git a/src/software/sensor_fusion/filter/ball_tracker.cpp b/src/software/sensor_fusion/filter/ball_tracker.cpp index 8031d06e1c..57351f605e 100644 --- a/src/software/sensor_fusion/filter/ball_tracker.cpp +++ b/src/software/sensor_fusion/filter/ball_tracker.cpp @@ -14,11 +14,11 @@ namespace { const Eigen::Vector INITIAL_STATE = Eigen::Vector::Zero(); - const Eigen::Matrix INITIAL_COV = (Eigen::Matrix() << - 1000, 0, 0, 0, - 0, 1000, 0,0, - 0, 0, 10, 0, - 0, 0, 0, 10).finished(); + const Eigen::Matrix INITIAL_COV = (Eigen::Matrix() << + 1000, 0, 0, 0, + 0, 1000, 0, 0, + 0, 0, 10, 0, + 0, 0, 0, 10).finished(); const Eigen::Matrix Q = (Eigen::Matrix() << 2.222e-8, 0, 2.000e-6, 0, diff --git a/src/software/simulated_tests/simulated_test_fixture.py b/src/software/simulated_tests/simulated_test_fixture.py index d220a7af7f..e27ddd90c5 100644 --- a/src/software/simulated_tests/simulated_test_fixture.py +++ b/src/software/simulated_tests/simulated_test_fixture.py @@ -203,6 +203,26 @@ def runner( block=False, return_cached=True ) + sf_pos = world.ball.current_state.global_position + sf_vel = world.ball.current_state.global_velocity + has_sim = simulator_state is not None and simulator_state.HasField("ball") + if has_sim: + sim_b = simulator_state.ball + print( + f"[BallState] t={time_elapsed_s:.3f} " + f"sf_pos=({sf_pos.x_meters:.3f},{sf_pos.y_meters:.3f}) " + f"sf_vel=({sf_vel.x_component_meters:.3f},{sf_vel.y_component_meters:.3f}) " + f"sim_pos=({sim_b.p_x:.3f},{sim_b.p_y:.3f}) " + f"sim_vel=({sim_b.v_x:.3f},{sim_b.v_y:.3f}) using=sim" + ) + else: + print( + f"[BallState] t={time_elapsed_s:.3f} " + f"sf_pos=({sf_pos.x_meters:.3f},{sf_pos.y_meters:.3f}) " + f"sf_vel=({sf_vel.x_component_meters:.3f},{sf_vel.y_component_meters:.3f}) " + f"using=sf" + ) + # Validate ( eventually_validation_proto_set, From 536820460452eec06708c6093ddc999db4e2b1ab Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Mon, 18 May 2026 14:49:20 -0700 Subject: [PATCH 48/57] fix path --- .../thunderscope/thunderscope_main.py | 179 ++++++++++-------- 1 file changed, 95 insertions(+), 84 deletions(-) diff --git a/src/software/thunderscope/thunderscope_main.py b/src/software/thunderscope/thunderscope_main.py index 45958eac3e..17c64601ea 100644 --- a/src/software/thunderscope/thunderscope_main.py +++ b/src/software/thunderscope/thunderscope_main.py @@ -7,33 +7,31 @@ import google.protobuf from google.protobuf.internal import api_implementation +from software.py_constants import * +import software.thunderscope.thunderscope_config as config +from proto.import_all_protos import * +from software.thunderscope.binary_context_managers import * +from software.thunderscope.binary_context_managers.full_system import FullSystem +from software.thunderscope.binary_context_managers.game_controller import Gamecontroller from software.thunderscope.binary_context_managers.runtime_manager import ( runtime_manager_instance, ) -from software.thunderscope.log.stats.stats import Stats - -from software.thunderscope.thunderscope import Thunderscope -from software.thunderscope.constants import LogLevels -from software.thunderscope.binary_context_managers import * -from proto.import_all_protos import * -from software.py_constants import * -from software.thunderscope.robot_communication import RobotCommunication -from software.thunderscope.wifi_communication_manager import WifiCommunicationManager +from software.thunderscope.binary_context_managers.simulator import Simulator +from software.thunderscope.binary_context_managers.tigers_autoref import TigersAutoref from software.thunderscope.constants import ( + CI_DURATION_S, EstopMode, + LogLevels, ProtoUnixIOTypes, ) from software.thunderscope.estop_helpers import get_estop_config +from software.thunderscope.log.stats.stats import Stats from software.thunderscope.proto_unix_io import ProtoUnixIO -import software.thunderscope.thunderscope_config as config -from software.thunderscope.constants import CI_DURATION_S +from software.thunderscope.robot_communication import RobotCommunication +from software.thunderscope.thunderscope import Thunderscope from software.thunderscope.util import * - -from software.thunderscope.binary_context_managers.full_system import FullSystem -from software.thunderscope.binary_context_managers.simulator import Simulator -from software.thunderscope.binary_context_managers.game_controller import Gamecontroller -from software.thunderscope.binary_context_managers.tigers_autoref import TigersAutoref +from software.thunderscope.wifi_communication_manager import WifiCommunicationManager protobuf_impl_type = api_implementation.Type() assert protobuf_impl_type == "upb", ( @@ -299,26 +297,30 @@ ) with ( - Gamecontroller( - suppress_logs=(not args.verbose), - use_conventional_port=False, - ) - if args.launch_gc - else contextlib.nullcontext() - ) as gamecontroller, WifiCommunicationManager( - current_proto_unix_io=current_proto_unix_io, - multicast_channel=getRobotMulticastChannel(args.channel), - should_setup_full_system=(args.run_blue or args.run_yellow), - interface=args.interface, - referee_port=gamecontroller.get_referee_port() - if gamecontroller - else SSL_REFEREE_PORT, - ) as wifi_communication_manager, RobotCommunication( - current_proto_unix_io=current_proto_unix_io, - communication_manager=wifi_communication_manager, - estop_mode=estop_mode, - estop_path=estop_path, - ) as robot_communication: + ( + Gamecontroller( + suppress_logs=(not args.verbose), + use_conventional_port=False, + ) + if args.launch_gc + else contextlib.nullcontext() + ) as gamecontroller, + WifiCommunicationManager( + current_proto_unix_io=current_proto_unix_io, + multicast_channel=getRobotMulticastChannel(args.channel), + should_setup_full_system=(args.run_blue or args.run_yellow), + interface=args.interface, + referee_port=gamecontroller.get_referee_port() + if gamecontroller + else SSL_REFEREE_PORT, + ) as wifi_communication_manager, + RobotCommunication( + current_proto_unix_io=current_proto_unix_io, + communication_manager=wifi_communication_manager, + estop_mode=estop_mode, + estop_path=estop_path, + ) as robot_communication, + ): if estop_mode == EstopMode.KEYBOARD_ESTOP: tscope.keyboard_estop_shortcut.activated.connect( robot_communication.toggle_keyboard_estop @@ -335,11 +337,12 @@ else args.yellow_full_system_runtime_dir ) with FullSystem( + path_to_binary=full_system_runtime_dir, full_system_runtime_dir=runtime_dir, debug_full_system=debug, friendly_colour_yellow=friendly_colour_yellow, should_restart_on_crash=True, - run_sudo=args.sudo, + run_sudo=True, log_level=args.log_level, ) as full_system: full_system.setup_proto_unix_io(current_proto_unix_io) @@ -413,54 +416,62 @@ def __ticker(tick_rate_ms: int) -> None: runtime_config = runtime_manager_instance.fetch_runtime_config() # Launch all binaries - with Simulator( - args.simulator_runtime_dir, args.debug_simulator, args.enable_realism - ) as simulator, FullSystem( - path_to_binary=runtime_config.get_blue_runtime_path(), - full_system_runtime_dir=args.blue_full_system_runtime_dir, - debug_full_system=args.debug_blue_full_system, - friendly_colour_yellow=False, - should_restart_on_crash=False, - run_sudo=args.sudo, - running_in_realtime=(not args.ci_mode), - log_level=args.log_level, - ) as blue_fs, FullSystem( - path_to_binary=runtime_config.get_yellow_runtime_path(), - full_system_runtime_dir=args.yellow_full_system_runtime_dir, - debug_full_system=args.debug_yellow_full_system, - friendly_colour_yellow=True, - should_restart_on_crash=False, - run_sudo=args.sudo, - running_in_realtime=(not args.ci_mode), - log_level=args.log_level, - ) as yellow_fs, Gamecontroller( - suppress_logs=(not args.verbose), - automate_referee=args.enable_autogc, - ) as gamecontroller, ( - # Here we only initialize autoref if the --enable_autoref flag is requested. - # To avoid nested Python withs, the autoref is initialized as None when this flag doesn't exist. - # All calls to autoref should be guarded with args.enable_autoref - TigersAutoref( - ci_mode=True, - gc=gamecontroller, + with ( + Simulator( + args.simulator_runtime_dir, args.debug_simulator, args.enable_realism + ) as simulator, + FullSystem( + path_to_binary=runtime_config.get_blue_runtime_path(), + full_system_runtime_dir=args.blue_full_system_runtime_dir, + debug_full_system=args.debug_blue_full_system, + friendly_colour_yellow=False, + should_restart_on_crash=False, + run_sudo=args.sudo, + running_in_realtime=(not args.ci_mode), + log_level=args.log_level, + ) as blue_fs, + FullSystem( + path_to_binary=runtime_config.get_yellow_runtime_path(), + full_system_runtime_dir=args.yellow_full_system_runtime_dir, + debug_full_system=args.debug_yellow_full_system, + friendly_colour_yellow=True, + should_restart_on_crash=False, + run_sudo=args.sudo, + running_in_realtime=(not args.ci_mode), + log_level=args.log_level, + ) as yellow_fs, + Gamecontroller( suppress_logs=(not args.verbose), - tick_rate_ms=DEFAULT_SIMULATOR_TICK_RATE_MILLISECONDS_PER_TICK, - show_gui=args.show_autoref_gui, - ) - if args.enable_autoref - else contextlib.nullcontext() - ) as autoref, ( - Stats( - proto_unix_io=tscope.proto_unix_io_map[ProtoUnixIOTypes.BLUE], - record_enemy_stats=True, - ) - if args.record_stats - else contextlib.nullcontext() - ) as blue_stats, ( - Stats(proto_unix_io=tscope.proto_unix_io_map[ProtoUnixIOTypes.YELLOW]) - if args.record_stats - else contextlib.nullcontext() - ) as yellow_stats: + automate_referee=args.enable_autogc, + ) as gamecontroller, + ( + # Here we only initialize autoref if the --enable_autoref flag is requested. + # To avoid nested Python withs, the autoref is initialized as None when this flag doesn't exist. + # All calls to autoref should be guarded with args.enable_autoref + TigersAutoref( + ci_mode=True, + gc=gamecontroller, + suppress_logs=(not args.verbose), + tick_rate_ms=DEFAULT_SIMULATOR_TICK_RATE_MILLISECONDS_PER_TICK, + show_gui=args.show_autoref_gui, + ) + if args.enable_autoref + else contextlib.nullcontext() + ) as autoref, + ( + Stats( + proto_unix_io=tscope.proto_unix_io_map[ProtoUnixIOTypes.BLUE], + record_enemy_stats=True, + ) + if args.record_stats + else contextlib.nullcontext() + ) as blue_stats, + ( + Stats(proto_unix_io=tscope.proto_unix_io_map[ProtoUnixIOTypes.YELLOW]) + if args.record_stats + else contextlib.nullcontext() + ) as yellow_stats, + ): tscope.register_refresh_function(gamecontroller.refresh) autoref_proto_unix_io = ProtoUnixIO() From a3d9065f0831252f8bb7ceb29b91f1d0776fe2b0 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Mon, 18 May 2026 22:51:28 -0700 Subject: [PATCH 49/57] temp vision testing --- src/software/constants.h | 6 +++--- src/software/networking/unix/proto_unix_listener.hpp | 3 +++ src/software/sensor_fusion/filter/ball_tracker.cpp | 2 +- src/software/thunderscope/thunderscope_main.py | 2 +- src/software/thunderscope/wifi_communication_manager.py | 8 ++++---- 5 files changed, 12 insertions(+), 9 deletions(-) diff --git a/src/software/constants.h b/src/software/constants.h index d1592e54b1..1d32b7d5d9 100644 --- a/src/software/constants.h +++ b/src/software/constants.h @@ -53,7 +53,7 @@ const std::string ROBOT_KICK_EXP_COEFF_CONFIG_KEY = "kick_coeff"; const std::string ROBOT_CHIP_PULSE_WIDTH_CONFIG_KEY = "chip_pulse_width"; const std::string SSL_VISION_ADDRESS = "224.5.23.2"; -static constexpr unsigned int SSL_VISION_PORT = 10020; +static constexpr unsigned int SSL_VISION_PORT = 10006; -const std::string SSL_REFEREE_ADDRESS = "224.5.23.1"; -static constexpr unsigned int SSL_REFEREE_PORT = 10003; +const std::string SSL_REFEREE_ADDRESS = "224.5.23.2"; +static constexpr unsigned int SSL_REFEREE_PORT = 10005; diff --git a/src/software/networking/unix/proto_unix_listener.hpp b/src/software/networking/unix/proto_unix_listener.hpp index c984eff065..57f68329e3 100644 --- a/src/software/networking/unix/proto_unix_listener.hpp +++ b/src/software/networking/unix/proto_unix_listener.hpp @@ -1,5 +1,7 @@ #pragma once +#include + #include #include @@ -73,6 +75,7 @@ ProtoUnixListener::ProtoUnixListener( socket_.open(); socket_.bind(listen_endpoint_); + ::chmod(unix_path.c_str(), 0777); boost::asio::local::datagram_protocol::socket::receive_buffer_size option( UNIX_BUFFER_SIZE); diff --git a/src/software/sensor_fusion/filter/ball_tracker.cpp b/src/software/sensor_fusion/filter/ball_tracker.cpp index 57351f605e..51bb2d9ecd 100644 --- a/src/software/sensor_fusion/filter/ball_tracker.cpp +++ b/src/software/sensor_fusion/filter/ball_tracker.cpp @@ -39,7 +39,7 @@ namespace { 0, 1, 0, 0).finished(); // Empirically measured - const double DAMPING = 0.9889; + const double DAMPING = 1; const double MAHANALOGIS_THRESHOLD = 1; const int CONSECUTIVE_OUTLIERS_THRESHOLD = 3; diff --git a/src/software/thunderscope/thunderscope_main.py b/src/software/thunderscope/thunderscope_main.py index 17c64601ea..58284bfd22 100644 --- a/src/software/thunderscope/thunderscope_main.py +++ b/src/software/thunderscope/thunderscope_main.py @@ -337,7 +337,7 @@ else args.yellow_full_system_runtime_dir ) with FullSystem( - path_to_binary=full_system_runtime_dir, + path_to_binary="software/unix_full_system", full_system_runtime_dir=runtime_dir, debug_full_system=debug, friendly_colour_yellow=friendly_colour_yellow, diff --git a/src/software/thunderscope/wifi_communication_manager.py b/src/software/thunderscope/wifi_communication_manager.py index 28ad2c68ba..5411fbe866 100644 --- a/src/software/thunderscope/wifi_communication_manager.py +++ b/src/software/thunderscope/wifi_communication_manager.py @@ -78,6 +78,10 @@ def __init__( referee_interface=DISCONNECTED, vision_interface=DISCONNECTED, ) + ## Thread Management ## + self.running = True + self.broadcast_ip: Thread | None = None + if interface: self.accept_next_network_config = False self.__setup_robot_communication(interface) @@ -89,10 +93,6 @@ def __init__( referee_interface=interface, vision_interface=interface ) - ## Thread Management ## - self.running = True - self.broadcast_ip: Thread | None = None - logger.debug("[WifiCommunicationManager] Initialized") self.__print_current_network_config() From a50d26a61d4126e6a25f019e5f1df8714098ad6d Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Tue, 19 May 2026 00:47:45 -0700 Subject: [PATCH 50/57] Increase ball tracker damping during extended occlusion Blends damping from 0.994 toward 0.3 after 1s grace period to prevent the filter from predicting unbounded motion when the ball is occluded. --- .../sensor_fusion/filter/ball_tracker.cpp | 27 ++++++++++++++----- .../sensor_fusion/filter/ball_tracker.h | 1 + 2 files changed, 22 insertions(+), 6 deletions(-) diff --git a/src/software/sensor_fusion/filter/ball_tracker.cpp b/src/software/sensor_fusion/filter/ball_tracker.cpp index 51bb2d9ecd..084a609bc9 100644 --- a/src/software/sensor_fusion/filter/ball_tracker.cpp +++ b/src/software/sensor_fusion/filter/ball_tracker.cpp @@ -38,8 +38,14 @@ namespace { 1, 0, 0, 0, 0, 1, 0, 0).finished(); - // Empirically measured - const double DAMPING = 1; + // Empirically measured per-second velocity retention during normal tracking + const double DAMPING = 0.994; + // Per-second retention when fully occluded long enough (velocity decays much faster) + const double DAMPING_OCCLUDED = 0.3; + // Occlusion time (seconds) before damping starts to increase + const double OCCLUSION_GRACE_SECONDS = 1.0; + // Occlusion time (seconds) past the grace period at which damping fully transitions to DAMPING_OCCLUDED + const double MAX_OCCLUSION_SECONDS = 1.0; const double MAHANALOGIS_THRESHOLD = 1; const int CONSECUTIVE_OUTLIERS_THRESHOLD = 3; @@ -78,11 +84,17 @@ std::optional BallTracker::estimateBallState( dt = (current_time - *prev_detection_timestamp).toSeconds(); prev_detection_timestamp = current_time; + const double occlusion_seconds = last_measurement_timestamp.has_value() + ? (current_time - *last_measurement_timestamp).toSeconds() + : 0.0; + const double alpha = std::min(std::max(occlusion_seconds - OCCLUSION_GRACE_SECONDS, 0.0) / MAX_OCCLUSION_SECONDS, 1.0); + const double effective_damping = DAMPING * (1.0 - alpha) + DAMPING_OCCLUDED * alpha; + Eigen::Matrix A; A << 1, 0, dt, 0, 0, 1, 0, dt, - 0, 0, DAMPING, 0, - 0, 0, 0, DAMPING; + 0, 0, effective_damping, 0, + 0, 0, 0, effective_damping; kalman_filter.process_model = A; kalman_filter.predict(Eigen::Vector::Zero()); } @@ -109,8 +121,9 @@ std::optional BallTracker::estimateBallState( kalman_filter.state_estimate << measurement(0), measurement(1), 0, 0; kalman_filter.state_covariance = INITIAL_COV; } - prev_detection_timestamp = current_time; - consecutive_outliers = 0; + prev_detection_timestamp = current_time; + last_measurement_timestamp = current_time; + consecutive_outliers = 0; } else if (best_ball_detection){ Eigen::Vector measurement; @@ -132,6 +145,7 @@ std::optional BallTracker::estimateBallState( if (mahalanobis < MAHANALOGIS_THRESHOLD){ kalman_filter.update(measurement); + last_measurement_timestamp = current_time; } else{ consecutive_outliers++; @@ -140,6 +154,7 @@ std::optional BallTracker::estimateBallState( if (consecutive_outliers > CONSECUTIVE_OUTLIERS_THRESHOLD){ kalman_filter.state_estimate << measurement(0), measurement(1), 0, 0; kalman_filter.state_covariance = INITIAL_COV; + last_measurement_timestamp = current_time; consecutive_outliers = 0; } prev_detection_timestamp = current_time; diff --git a/src/software/sensor_fusion/filter/ball_tracker.h b/src/software/sensor_fusion/filter/ball_tracker.h index 8e8ab40f77..75768fd7c3 100644 --- a/src/software/sensor_fusion/filter/ball_tracker.h +++ b/src/software/sensor_fusion/filter/ball_tracker.h @@ -63,4 +63,5 @@ class BallTracker int consecutive_outliers; KalmanFilter<4, 2, 1> kalman_filter; std::optional prev_detection_timestamp; + std::optional last_measurement_timestamp; }; From 925314fb1b2aaa75cd5438a9d1f798d2648cf77b Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Tue, 19 May 2026 21:48:12 -0700 Subject: [PATCH 51/57] temp --- .../simulated_tests/simulated_test_fixture.py | 16 ---------------- 1 file changed, 16 deletions(-) diff --git a/src/software/simulated_tests/simulated_test_fixture.py b/src/software/simulated_tests/simulated_test_fixture.py index e27ddd90c5..3be0084728 100644 --- a/src/software/simulated_tests/simulated_test_fixture.py +++ b/src/software/simulated_tests/simulated_test_fixture.py @@ -206,22 +206,6 @@ def runner( sf_pos = world.ball.current_state.global_position sf_vel = world.ball.current_state.global_velocity has_sim = simulator_state is not None and simulator_state.HasField("ball") - if has_sim: - sim_b = simulator_state.ball - print( - f"[BallState] t={time_elapsed_s:.3f} " - f"sf_pos=({sf_pos.x_meters:.3f},{sf_pos.y_meters:.3f}) " - f"sf_vel=({sf_vel.x_component_meters:.3f},{sf_vel.y_component_meters:.3f}) " - f"sim_pos=({sim_b.p_x:.3f},{sim_b.p_y:.3f}) " - f"sim_vel=({sim_b.v_x:.3f},{sim_b.v_y:.3f}) using=sim" - ) - else: - print( - f"[BallState] t={time_elapsed_s:.3f} " - f"sf_pos=({sf_pos.x_meters:.3f},{sf_pos.y_meters:.3f}) " - f"sf_vel=({sf_vel.x_component_meters:.3f},{sf_vel.y_component_meters:.3f}) " - f"using=sf" - ) # Validate ( From 52691ad50d5c2d6b9a9376ef14a218079f9fc0b2 Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Tue, 19 May 2026 22:21:32 -0700 Subject: [PATCH 52/57] add velocity initialization --- .../sensor_fusion/filter/ball_tracker.cpp | 21 +++++++++++++++++++ .../sensor_fusion/filter/ball_tracker.h | 1 + 2 files changed, 22 insertions(+) diff --git a/src/software/sensor_fusion/filter/ball_tracker.cpp b/src/software/sensor_fusion/filter/ball_tracker.cpp index 084a609bc9..9a1305f7cd 100644 --- a/src/software/sensor_fusion/filter/ball_tracker.cpp +++ b/src/software/sensor_fusion/filter/ball_tracker.cpp @@ -55,6 +55,7 @@ namespace { BallTracker::BallTracker() : consecutive_outliers(0), + velocity_initialized(false), kalman_filter(INITIAL_STATE, INITIAL_COV, Eigen::Matrix::Identity(), Q, @@ -84,6 +85,23 @@ std::optional BallTracker::estimateBallState( dt = (current_time - *prev_detection_timestamp).toSeconds(); prev_detection_timestamp = current_time; + // Hard-initialize velocity from finite difference on the first step so + // hasBallBeenKicked() fires immediately. Set P_vel = 2R/dt^2 (the exact + // variance of a finite-diff estimate) so the filter knows the estimate is + // noisy and can correct it aggressively over the next few frames. + if (!velocity_initialized && dt > 0 && best_ball_detection) + { + kalman_filter.state_estimate(2) = + (best_ball_detection->position.x() - kalman_filter.state_estimate(0)) / dt; + kalman_filter.state_estimate(3) = + (best_ball_detection->position.y() - kalman_filter.state_estimate(1)) / dt; + kalman_filter.state_covariance(2, 2) = + std::min(2.0 * R(0, 0) / (dt * dt), 1000.0); + kalman_filter.state_covariance(3, 3) = + std::min(2.0 * R(1, 1) / (dt * dt), 1000.0); + velocity_initialized = true; + } + const double occlusion_seconds = last_measurement_timestamp.has_value() ? (current_time - *last_measurement_timestamp).toSeconds() : 0.0; @@ -120,6 +138,7 @@ std::optional BallTracker::estimateBallState( measurement << dribbler_pos.x(), dribbler_pos.y(); kalman_filter.state_estimate << measurement(0), measurement(1), 0, 0; kalman_filter.state_covariance = INITIAL_COV; + velocity_initialized = false; } prev_detection_timestamp = current_time; last_measurement_timestamp = current_time; @@ -134,6 +153,7 @@ std::optional BallTracker::estimateBallState( kalman_filter.state_estimate << measurement(0), measurement(1), 0, 0; kalman_filter.state_covariance = INITIAL_COV; consecutive_outliers = 0; + velocity_initialized = false; } const Eigen::Vector innovation = @@ -156,6 +176,7 @@ std::optional BallTracker::estimateBallState( kalman_filter.state_covariance = INITIAL_COV; last_measurement_timestamp = current_time; consecutive_outliers = 0; + velocity_initialized = false; } prev_detection_timestamp = current_time; } diff --git a/src/software/sensor_fusion/filter/ball_tracker.h b/src/software/sensor_fusion/filter/ball_tracker.h index 75768fd7c3..a9037b0bdb 100644 --- a/src/software/sensor_fusion/filter/ball_tracker.h +++ b/src/software/sensor_fusion/filter/ball_tracker.h @@ -61,6 +61,7 @@ class BallTracker */ std::optional getBestBallDetection(const std::vector &new_ball_detections); int consecutive_outliers; + bool velocity_initialized; KalmanFilter<4, 2, 1> kalman_filter; std::optional prev_detection_timestamp; std::optional last_measurement_timestamp; From ca54a5a295642af60398350bdd1f0b67220ff92e Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Wed, 20 May 2026 11:36:44 -0700 Subject: [PATCH 53/57] FIX GOALIE TACTIC TEST --- .../stp/tactic/goalie/goalie_tactic_test.py | 20 ++-- src/software/er_force_simulator_main.cpp | 4 +- .../sensor_fusion/filter/ball_tracker.cpp | 99 +++++++++++++++---- .../sensor_fusion/filter/ball_tracker.h | 17 +++- .../simulated_tests/simulated_test_fixture.py | 16 ++- 5 files changed, 123 insertions(+), 33 deletions(-) diff --git a/src/software/ai/hl/stp/tactic/goalie/goalie_tactic_test.py b/src/software/ai/hl/stp/tactic/goalie/goalie_tactic_test.py index 2111109524..d76e072715 100644 --- a/src/software/ai/hl/stp/tactic/goalie/goalie_tactic_test.py +++ b/src/software/ai/hl/stp/tactic/goalie/goalie_tactic_test.py @@ -1,18 +1,18 @@ import pytest - import software.python_bindings as tbots_cpp + from proto.import_all_protos import * -from software.simulated_tests.validation.robot_enters_region import * +from proto.message_translation.tbots_protobuf import create_world_state +from software.simulated_tests.simulated_test_fixture import ( + pytest_main, +) from software.simulated_tests.validation.ball_enters_region import * from software.simulated_tests.validation.ball_moves_in_direction import * -from software.simulated_tests.validation.friendly_has_ball_possession import * from software.simulated_tests.validation.ball_speed_threshold import * -from software.simulated_tests.validation.robot_speed_threshold import * from software.simulated_tests.validation.excessive_dribbling import * -from software.simulated_tests.simulated_test_fixture import ( - pytest_main, -) -from proto.message_translation.tbots_protobuf import create_world_state +from software.simulated_tests.validation.friendly_has_ball_possession import * +from software.simulated_tests.validation.robot_enters_region import * +from software.simulated_tests.validation.robot_speed_threshold import * @pytest.mark.parametrize( @@ -28,7 +28,7 @@ + tbots_cpp.Vector(0, -0.5), ), # test ball very fast misses net - (tbots_cpp.Point(0, 0), tbots_cpp.Vector(-5, 1), tbots_cpp.Point(-4.5, 0)), + (tbots_cpp.Point(0, 0), tbots_cpp.Vector(-5, 1), tbots_cpp.Point(-4.3, 0)), # test ball very fast get saved # TODO (#3377): This test is flaky due to inconsistent goalie reach. The linked ticket may provide a permanent fix. ( @@ -74,7 +74,7 @@ # ball moving out from inside defense area ( tbots_cpp.Field.createSSLDivisionBField().friendlyGoalCenter() - + tbots_cpp.Vector(0.5, 0), + + tbots_cpp.Vector(0.4, 0), tbots_cpp.Vector(0.5, 0), tbots_cpp.Point(-3.5, 0), ), diff --git a/src/software/er_force_simulator_main.cpp b/src/software/er_force_simulator_main.cpp index 858e99854b..fe371634ec 100644 --- a/src/software/er_force_simulator_main.cpp +++ b/src/software/er_force_simulator_main.cpp @@ -48,7 +48,7 @@ int main(int argc, char** argv) { std::string runtime_dir = args.runtime_dir; LoggerSingleton::initializeLogger(runtime_dir, nullptr); - LOG(CSV, "realism_kalman_filter_v10.csv") << "timestamp_s,fused_x,fused_y,fused_vel_x, fused_vel_y, truth_x,truth_y, true_vel_x, true_vel_y,is_occluded\n"; + LOG(CSV, "realism_kalman_filter_v12.csv") << "timestamp_s,fused_x,fused_y,fused_vel_x, fused_vel_y, truth_x,truth_y, true_vel_x, true_vel_y,is_occluded\n"; /** * Creates a ER force simulator and sets up the appropriate @@ -230,7 +230,7 @@ int main(int argc, char** argv) { start_timestamp_s = current_ts; } - LOG(CSV, "realism_kalman_filter_v10.csv") + LOG(CSV, "realism_kalman_filter_v12.csv") << (current_ts - start_timestamp_s) << "," << yellow_vision.ball().current_state().global_position().x_meters() << "," diff --git a/src/software/sensor_fusion/filter/ball_tracker.cpp b/src/software/sensor_fusion/filter/ball_tracker.cpp index 9a1305f7cd..ec124daa81 100644 --- a/src/software/sensor_fusion/filter/ball_tracker.cpp +++ b/src/software/sensor_fusion/filter/ball_tracker.cpp @@ -17,8 +17,8 @@ namespace { const Eigen::Matrix INITIAL_COV = (Eigen::Matrix() << 1000, 0, 0, 0, 0, 1000, 0, 0, - 0, 0, 10, 0, - 0, 0, 0, 10).finished(); + 0, 0, 1, 0, + 0, 0, 0, 1).finished(); const Eigen::Matrix Q = (Eigen::Matrix() << 2.222e-8, 0, 2.000e-6, 0, @@ -51,6 +51,11 @@ namespace { const int CONSECUTIVE_OUTLIERS_THRESHOLD = 3; const double DRIBBLING_MEASUREMENT_MAX_DISTANCE_METERS = 0.08; + + // Speed gate for the init buffer: reject detections that imply a ball jump + // beyond this threshold from the last known position. + // BALL_MAX_SPEED_METERS_PER_SECOND (6.5) + 2.5 buffer for measurement error. + const double INIT_BUFFER_MAX_SPEED_M_PER_S = BALL_MAX_SPEED_METERS_PER_SECOND + 2.5; } BallTracker::BallTracker() : @@ -80,28 +85,24 @@ std::optional BallTracker::estimateBallState( std::optional best_ball_detection = getBestBallDetection(new_ball_detections); + if (!velocity_initialized) + { + if (best_ball_detection) + velocity_initialized = tryInitVelocityFromBuffer( + best_ball_detection->position, current_time); + + const Point pos = best_ball_detection + ? best_ball_detection->position + : Point(kalman_filter.state_estimate(0), kalman_filter.state_estimate(1)); + const double z = best_ball_detection ? best_ball_detection->distance_from_ground : 0.0; + return Ball(BallState(pos, Vector(0, 0), z), current_time); + } + double dt = 0.0; if (prev_detection_timestamp){ dt = (current_time - *prev_detection_timestamp).toSeconds(); prev_detection_timestamp = current_time; - // Hard-initialize velocity from finite difference on the first step so - // hasBallBeenKicked() fires immediately. Set P_vel = 2R/dt^2 (the exact - // variance of a finite-diff estimate) so the filter knows the estimate is - // noisy and can correct it aggressively over the next few frames. - if (!velocity_initialized && dt > 0 && best_ball_detection) - { - kalman_filter.state_estimate(2) = - (best_ball_detection->position.x() - kalman_filter.state_estimate(0)) / dt; - kalman_filter.state_estimate(3) = - (best_ball_detection->position.y() - kalman_filter.state_estimate(1)) / dt; - kalman_filter.state_covariance(2, 2) = - std::min(2.0 * R(0, 0) / (dt * dt), 1000.0); - kalman_filter.state_covariance(3, 3) = - std::min(2.0 * R(1, 1) / (dt * dt), 1000.0); - velocity_initialized = true; - } - const double occlusion_seconds = last_measurement_timestamp.has_value() ? (current_time - *last_measurement_timestamp).toSeconds() : 0.0; @@ -139,6 +140,7 @@ std::optional BallTracker::estimateBallState( kalman_filter.state_estimate << measurement(0), measurement(1), 0, 0; kalman_filter.state_covariance = INITIAL_COV; velocity_initialized = false; + velocity_init_buffer.clear(); } prev_detection_timestamp = current_time; last_measurement_timestamp = current_time; @@ -154,6 +156,7 @@ std::optional BallTracker::estimateBallState( kalman_filter.state_covariance = INITIAL_COV; consecutive_outliers = 0; velocity_initialized = false; + velocity_init_buffer.clear(); } const Eigen::Vector innovation = @@ -177,6 +180,7 @@ std::optional BallTracker::estimateBallState( last_measurement_timestamp = current_time; consecutive_outliers = 0; velocity_initialized = false; + velocity_init_buffer.clear(); } prev_detection_timestamp = current_time; } @@ -189,6 +193,63 @@ std::optional BallTracker::estimateBallState( return Ball(ball_state, current_time); } +bool BallTracker::tryInitVelocityFromBuffer(const Point& position, + const Timestamp& current_time) +{ + // Gate: reject detections that imply the ball teleported from the last + // known position. Clear the buffer and reanchor rather than poisoning the + // regression with a noise spike. + if (!velocity_init_buffer.empty()) + { + const Point last_pos(kalman_filter.state_estimate(0), + kalman_filter.state_estimate(1)); + const double dt_s = current_time.toSeconds() - velocity_init_buffer.back().second; + if (dt_s > 0 && + (position - last_pos).length() / dt_s > INIT_BUFFER_MAX_SPEED_M_PER_S) + { + velocity_init_buffer.clear(); + kalman_filter.state_estimate(0) = position.x(); + kalman_filter.state_estimate(1) = position.y(); + prev_detection_timestamp = current_time; + return false; + } + } + + velocity_init_buffer.emplace_back(position, current_time.toSeconds()); + kalman_filter.state_estimate(0) = position.x(); + kalman_filter.state_estimate(1) = position.y(); + prev_detection_timestamp = current_time; + + if (static_cast(velocity_init_buffer.size()) < VELOCITY_INIT_BUFFER_SIZE) + return false; + + const int N = static_cast(velocity_init_buffer.size()); + double t_mean = 0.0, x_mean = 0.0, y_mean = 0.0; + for (const auto& [pos, t] : velocity_init_buffer) { + t_mean += t; x_mean += pos.x(); y_mean += pos.y(); + } + t_mean /= N; x_mean /= N; y_mean /= N; + + double Sxx = 0.0, Stx = 0.0, Sty = 0.0; + for (const auto& [pos, t] : velocity_init_buffer) { + const double dt_i = t - t_mean; + Sxx += dt_i * dt_i; + Stx += dt_i * (pos.x() - x_mean); + Sty += dt_i * (pos.y() - y_mean); + } + if (Sxx > 1e-10) + { + kalman_filter.state_estimate(2) = Stx / Sxx; + kalman_filter.state_estimate(3) = Sty / Sxx; + kalman_filter.state_covariance(2, 2) = R(0, 0) / Sxx; + kalman_filter.state_covariance(3, 3) = R(1, 1) / Sxx; + } + last_measurement_timestamp = current_time; + consecutive_outliers = 0; + velocity_init_buffer.clear(); + return true; +} + std::optional BallTracker::getBestBallDetection(const std::vector &new_ball_detections){ if (new_ball_detections.empty()){ return std::nullopt; diff --git a/src/software/sensor_fusion/filter/ball_tracker.h b/src/software/sensor_fusion/filter/ball_tracker.h index a9037b0bdb..75182e3840 100644 --- a/src/software/sensor_fusion/filter/ball_tracker.h +++ b/src/software/sensor_fusion/filter/ball_tracker.h @@ -1,7 +1,8 @@ #pragma once -#include #include +#include +#include #include "software/geom/line.h" #include "software/geom/point.h" @@ -60,9 +61,23 @@ class BallTracker * list is empty */ std::optional getBestBallDetection(const std::vector &new_ball_detections); + + /** + * Appends the detection to the velocity init buffer and, once + * VELOCITY_INIT_BUFFER_SIZE samples have accumulated, fits a least-squares + * line through them to initialize the filter's velocity state and covariance. + * + * @param position The ball position for this sample + * @param current_time The timestamp of this sample + * @return true if velocity was initialized this call (buffer just became full) + */ + bool tryInitVelocityFromBuffer(const Point& position, const Timestamp& current_time); + int consecutive_outliers; bool velocity_initialized; KalmanFilter<4, 2, 1> kalman_filter; std::optional prev_detection_timestamp; std::optional last_measurement_timestamp; + static constexpr int VELOCITY_INIT_BUFFER_SIZE = 4; + std::vector> velocity_init_buffer; }; diff --git a/src/software/simulated_tests/simulated_test_fixture.py b/src/software/simulated_tests/simulated_test_fixture.py index 3be0084728..6a16809c06 100644 --- a/src/software/simulated_tests/simulated_test_fixture.py +++ b/src/software/simulated_tests/simulated_test_fixture.py @@ -205,7 +205,21 @@ def runner( sf_pos = world.ball.current_state.global_position sf_vel = world.ball.current_state.global_velocity - has_sim = simulator_state is not None and simulator_state.HasField("ball") + if simulator_state is not None: + sim_b = simulator_state.ball + print( + f"[BallState] t={time_elapsed_s:.3f} " + f"sf_pos=({sf_pos.x_meters:.3f},{sf_pos.y_meters:.3f}) " + f"sf_vel=({sf_vel.x_component_meters:.3f},{sf_vel.y_component_meters:.3f}) " + f"sim_pos=({sim_b.p_x:.3f},{sim_b.p_y:.3f}) " + f"sim_vel=({sim_b.v_x:.3f},{sim_b.v_y:.3f})" + ) + else: + print( + f"[BallState] t={time_elapsed_s:.3f} " + f"sf_pos=({sf_pos.x_meters:.3f},{sf_pos.y_meters:.3f}) " + f"sf_vel=({sf_vel.x_component_meters:.3f},{sf_vel.y_component_meters:.3f})" + ) # Validate ( From e1f44be547d160797e52d37872946abaee233acf Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Wed, 20 May 2026 11:54:52 -0700 Subject: [PATCH 54/57] a --- src/software/ai/hl/stp/tactic/goalie/goalie_fsm.cpp | 7 ++++++- src/software/ai/hl/stp/tactic/goalie/goalie_tactic_test.py | 2 +- 2 files changed, 7 insertions(+), 2 deletions(-) diff --git a/src/software/ai/hl/stp/tactic/goalie/goalie_fsm.cpp b/src/software/ai/hl/stp/tactic/goalie/goalie_fsm.cpp index b779a3d2fd..9ead30b9e0 100644 --- a/src/software/ai/hl/stp/tactic/goalie/goalie_fsm.cpp +++ b/src/software/ai/hl/stp/tactic/goalie/goalie_fsm.cpp @@ -4,6 +4,7 @@ #include "software/ai/evaluation/intercept.h" #include "software/ai/hl/stp/tactic/move_primitive.h" #include "software/math/math_functions.h" +#include "software/logger/logger.h" GoalieFSM::GoalieFSM(std::shared_ptr ai_config_ptr, TbotsProto::MaxAllowedSpeedMode max_allowed_speed_mode) @@ -167,9 +168,13 @@ bool GoalieFSM::shouldPanic(const Update& event) bool GoalieFSM::shouldPivotChip(const Update& event) { double ball_speed_panic = ai_config_ptr->goalie_tactic_config().ball_speed_panic(); - return event.common.world_ptr->ball().velocity().length() <= ball_speed_panic && + bool chip = event.common.world_ptr->ball().velocity().length() <= ball_speed_panic && event.common.world_ptr->field().pointInFriendlyDefenseArea( event.common.world_ptr->ball().position()); + if (chip){LOG(INFO)<<"CHIPPED" < Date: Wed, 20 May 2026 12:10:32 -0700 Subject: [PATCH 55/57] revert goalie chages --- src/software/ai/hl/stp/tactic/goalie/goalie_fsm.h | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/software/ai/hl/stp/tactic/goalie/goalie_fsm.h b/src/software/ai/hl/stp/tactic/goalie/goalie_fsm.h index 2d0f320ffb..9e5a1ef6de 100644 --- a/src/software/ai/hl/stp/tactic/goalie/goalie_fsm.h +++ b/src/software/ai/hl/stp/tactic/goalie/goalie_fsm.h @@ -233,10 +233,9 @@ struct GoalieFSM : TacticFSM Panic_S + Update_E / panic_A, PivotKickFSM_S + Update_E[shouldMoveToGoalLine_G] / moveToGoalLine_A = MoveToGoalLine_S, - PivotKickFSM_S + - Update_E[ballInInflatedDefenseArea_G && - (shouldPivotChip_G || shouldPanic_G)] / updatePivotKick_A, - PivotKickFSM_S + Update_E / positionToBlock_A = PositionToBlock_S, + PivotKickFSM_S + Update_E[ballInInflatedDefenseArea_G] / updatePivotKick_A, + PivotKickFSM_S + Update_E[!ballInInflatedDefenseArea_G] / positionToBlock_A = + PositionToBlock_S, MoveToGoalLine_S + Update_E[shouldMoveToGoalLine_G] / moveToGoalLine_A = MoveToGoalLine_S, MoveToGoalLine_S + Update_E[!shouldMoveToGoalLine_G] / positionToBlock_A = From c8e8d9655859248c3da892c5dbc73a3d397e963b Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Wed, 20 May 2026 15:06:59 -0700 Subject: [PATCH 56/57] dribbling test work --- .../sensor_fusion/filter/ball_tracker.cpp | 22 +++++++++++------ src/software/sensor_fusion/sensor_fusion.cpp | 24 ++++++++++++++++++- 2 files changed, 38 insertions(+), 8 deletions(-) diff --git a/src/software/sensor_fusion/filter/ball_tracker.cpp b/src/software/sensor_fusion/filter/ball_tracker.cpp index ec124daa81..375e4a2e3e 100644 --- a/src/software/sensor_fusion/filter/ball_tracker.cpp +++ b/src/software/sensor_fusion/filter/ball_tracker.cpp @@ -1,6 +1,7 @@ #include "software/sensor_fusion/filter/ball_tracker.h" #include +#include "software/logger/logger.h" #include #include @@ -125,23 +126,25 @@ std::optional BallTracker::estimateBallState( (best_ball_detection->position - dribbler_pos).length() <= DRIBBLING_MEASUREMENT_MAX_DISTANCE_METERS; + // Use ball detection if available and near dribbler, otherwise fall back to + // dribbler position (ball likely occluded by robot body) Eigen::Vector measurement; if (near_dribbler) { measurement << best_ball_detection->position.x(), best_ball_detection->position.y(); - kalman_filter.measurement_covariance = R_DRIBBLING; - kalman_filter.update(measurement); - kalman_filter.measurement_covariance = R; } else { measurement << dribbler_pos.x(), dribbler_pos.y(); - kalman_filter.state_estimate << measurement(0), measurement(1), 0, 0; - kalman_filter.state_covariance = INITIAL_COV; - velocity_initialized = false; - velocity_init_buffer.clear(); } + LOG(INFO) << "[BallTracker] dribbling: tight update at (" + << measurement(0) << "," << measurement(1) << ")"; + kalman_filter.measurement_covariance = R_DRIBBLING; + kalman_filter.update(measurement); + kalman_filter.measurement_covariance = R; + kalman_filter.state_estimate(2) = 0; + kalman_filter.state_estimate(3) = 0; prev_detection_timestamp = current_time; last_measurement_timestamp = current_time; consecutive_outliers = 0; @@ -152,6 +155,7 @@ std::optional BallTracker::estimateBallState( Point predicted_pos(kalman_filter.state_estimate(0), kalman_filter.state_estimate(1)); if (!contains(filter_area, predicted_pos)){ + LOG(INFO) << "[BallTracker] predicted pos out of bounds, reset to measurement"; kalman_filter.state_estimate << measurement(0), measurement(1), 0, 0; kalman_filter.state_covariance = INITIAL_COV; consecutive_outliers = 0; @@ -171,10 +175,12 @@ std::optional BallTracker::estimateBallState( last_measurement_timestamp = current_time; } else{ + LOG(INFO) << "[BallTracker] outlier rejected (mahalanobis=" << mahalanobis << ", consecutive=" << consecutive_outliers + 1 << ")"; consecutive_outliers++; } if (consecutive_outliers > CONSECUTIVE_OUTLIERS_THRESHOLD){ + LOG(INFO) << "[BallTracker] too many consecutive outliers, hard reset to measurement"; kalman_filter.state_estimate << measurement(0), measurement(1), 0, 0; kalman_filter.state_covariance = INITIAL_COV; last_measurement_timestamp = current_time; @@ -207,6 +213,7 @@ bool BallTracker::tryInitVelocityFromBuffer(const Point& position, if (dt_s > 0 && (position - last_pos).length() / dt_s > INIT_BUFFER_MAX_SPEED_M_PER_S) { + LOG(INFO) << "[BallTracker] init buffer: noisy detection rejected, reanchor"; velocity_init_buffer.clear(); kalman_filter.state_estimate(0) = position.x(); kalman_filter.state_estimate(1) = position.y(); @@ -244,6 +251,7 @@ bool BallTracker::tryInitVelocityFromBuffer(const Point& position, kalman_filter.state_covariance(2, 2) = R(0, 0) / Sxx; kalman_filter.state_covariance(3, 3) = R(1, 1) / Sxx; } + LOG(INFO) << "[BallTracker] velocity initialized from buffer: vx=" << kalman_filter.state_estimate(2) << " vy=" << kalman_filter.state_estimate(3); last_measurement_timestamp = current_time; consecutive_outliers = 0; velocity_init_buffer.clear(); diff --git a/src/software/sensor_fusion/sensor_fusion.cpp b/src/software/sensor_fusion/sensor_fusion.cpp index 592b96bf2b..f810b7886b 100644 --- a/src/software/sensor_fusion/sensor_fusion.cpp +++ b/src/software/sensor_fusion/sensor_fusion.cpp @@ -1,8 +1,11 @@ #include "software/sensor_fusion/sensor_fusion.h" +#include "software/constants.h" #include "software/geom/algorithms/distance.h" #include "software/logger/logger.h" +static constexpr double PROXIMITY_DRIBBLING_THRESHOLD_METERS = 0.12; + SensorFusion::SensorFusion(TbotsProto::SensorFusionConfig sensor_fusion_config) : sensor_fusion_config(sensor_fusion_config), field(std::nullopt), @@ -296,7 +299,26 @@ void SensorFusion::updateWorld(const SSLProto::SSL_DetectionFrame& ssl_detection } else { - std::optional new_ball = createBall(ball_detections, Timestamp::fromSeconds(ssl_detection_frame.t_capture())); + std::optional proximity_dribbling_robot; + if (ball) + { + std::optional nearest_friendly = friendly_team.getNearestRobot(ball->position()); + if (nearest_friendly.has_value()) + { + Point dribbler_pos = + nearest_friendly->position() + + Vector::createFromAngle(nearest_friendly->orientation()) + .normalize(DIST_TO_FRONT_OF_ROBOT_METERS + + BALL_TO_FRONT_OF_ROBOT_DISTANCE_WHEN_DRIBBLING); + if (distance(dribbler_pos, ball->position()) <= + PROXIMITY_DRIBBLING_THRESHOLD_METERS) + { + proximity_dribbling_robot = nearest_friendly; + } + } + } + + std::optional new_ball = createBall(ball_detections, Timestamp::fromSeconds(ssl_detection_frame.t_capture()), proximity_dribbling_robot); if (new_ball) { // If vision detected a new ball, then use that one From 7768f1b39e857d1282d3750626f6f10b0241e68e Mon Sep 17 00:00:00 2001 From: Samuel Ubuntu Laptop Date: Wed, 20 May 2026 15:51:10 -0700 Subject: [PATCH 57/57] fix goalie crease defender tests --- src/software/sensor_fusion/filter/ball_tracker.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/software/sensor_fusion/filter/ball_tracker.cpp b/src/software/sensor_fusion/filter/ball_tracker.cpp index 375e4a2e3e..32c3378888 100644 --- a/src/software/sensor_fusion/filter/ball_tracker.cpp +++ b/src/software/sensor_fusion/filter/ball_tracker.cpp @@ -126,8 +126,6 @@ std::optional BallTracker::estimateBallState( (best_ball_detection->position - dribbler_pos).length() <= DRIBBLING_MEASUREMENT_MAX_DISTANCE_METERS; - // Use ball detection if available and near dribbler, otherwise fall back to - // dribbler position (ball likely occluded by robot body) Eigen::Vector measurement; if (near_dribbler) { @@ -143,8 +141,6 @@ std::optional BallTracker::estimateBallState( kalman_filter.measurement_covariance = R_DRIBBLING; kalman_filter.update(measurement); kalman_filter.measurement_covariance = R; - kalman_filter.state_estimate(2) = 0; - kalman_filter.state_estimate(3) = 0; prev_detection_timestamp = current_time; last_measurement_timestamp = current_time; consecutive_outliers = 0;