Time to reach rework (#1634)

* Time to reach preparation

* Remove team_ball from role assignment outputs

* Add test to ensure that role state machine is idempotent for Event::None

* Make use of idempotence

* Add testcase for LoserMessage byte size

* Divorce role state machine from logic if we want to send a striker message

* Clean up message sending logic

* Visualize role colors in behavior simulator panel

* Correct striker counting in golden_goal_striker_penalized

* Use correct axis to determine corner kick side

* Decay velocity in simulated ball model

* Set the correct time in simulator interfake

* Add additional outputs for team ball data

* Implement sending loser messages

* Clean up team ball receiver

* Clean up

* Don't send striker messages in penalty shootout

* Make sure team ball is kept longer than striker beacon interval + maximum ball age

* Render player number

* Add range check to kicking ball

* Check if keeper becomes striker in reappearing_ball_in_front_of_keeper

* Clean up unused code

* Fix replacement keeper switch time logic

* Check if goal is scored in walk_around_ball scenario and tell robot where the ball teleported

* uSe oF UnStAbLe lIbRaRy fEaTuRe 'is_none_or'

* Replace test-case with proptest

* Fix stand_up_back motion

* Add regression test for keeper getting stuck in loser role

* Make time to reach an Option

* Allow robots to become (replacement)keeper instead of loser/searcher

* Only consider striker events for striker message timeout

* Add test scenario to ensure robots can become striker from being sent a nearby ball

* Make time to reach not an Option in Striker Message

* Clean up duration calculations

* Process loser messages

* Ignore team balls and network obstacles in penalty kick states

* Reimplement using network messages as obstacle percepts

* Move obstacle extraction to separate node

* Avoid using Role::default()

* Simplify unwrap_or(Striker)

* Clarify variable name

* Update last_time_player_was_penalized before using it this cycle

* Remove note about sending message from comment

* Make staying (replacement) keeper easier to read

* Drop todo about striker edgecase

* Don't use a default ground_to_field globally

* Initialize role in new()

* Extract gathering messages to function

* Refactor the way roles from different sources are combined

* Prevent One from becoming Loser

* Convert striker message timeout into loser event

* Add scenario where striker disappears without sending a loser message

* Clean up comment

* Clean up team ball parameters

* Hopefully fix Cargo.lock

* Update cycler states

* Use "Option" for remaining standup durations

* Fix crash when time to reach is none

* Also use stand up sitting remaining duration

* Update last_time_player_was_penalized before using it this cycle (again)

* Remove unused import

Cargo.toml

@@ -176,6 +176,7 @@ prettyplease = "0.2.29"
 proc-macro-error = "1.0.4"
 proc-macro2 = { version = "1.0.93", features = ["span-locations"] }
 projection = { path = "crates/projection" }
+proptest = "1.6.0"
 quote = "1.0.38"
 rand = "0.9.0"
 rand_chacha = { version = "0.9.0", features = ["serde"] }

crates/bevyhavior_simulator/build.rs

@@ -20,17 +20,19 @@ fn main() -> Result<()> {
                     "control::active_vision",
                     "control::ball_state_composer",
                     "control::behavior::node",
+                    "control::dribble_path_planner",
+                    "control::filtered_game_controller_state_timer",
                     "control::game_controller_state_filter",
                     "control::kick_selector",
-                    "control::filtered_game_controller_state_timer",
-                    "control::primary_state_filter",
                     "control::motion::look_around",
                     "control::motion::motion_selector",
                     "control::penalty_shot_direction_estimation",
+                    "control::primary_state_filter",
                     "control::referee_position_provider",
                     "control::role_assignment",
                     "control::rule_obstacle_composer",
                     "control::search_suggestor",
+                    "control::team_ball_receiver",
                     "control::time_to_reach_kick_position",
                     "control::world_state_composer",
                 ],

crates/bevyhavior_simulator/src/autoref.rs

@@ -141,7 +141,7 @@ pub fn auto_assistant_referee(
             GameControllerCommand::SetGamePhase(_) => {}
             GameControllerCommand::SetSubState(Some(SubState::CornerKick), team) => {
                 let side = if let Some(ball) = ball.state.as_mut() {
-                    if ball.position.x() >= 0.0 {
+                    if ball.position.y() <= 0.0 {
                         Side::Left
                     } else {
                         Side::Right

crates/bevyhavior_simulator/src/bin/golden_goal_striker_penalized.rs

@@ -10,7 +10,7 @@ use bevyhavior_simulator::{
     robot::Robot,
     time::{Ticks, TicksTime},
 };
-use types::roles::Role;
+use types::{primary_state::PrimaryState, roles::Role};
 
 #[scenario]
 fn golden_goal(app: &mut App) {
@@ -59,14 +59,15 @@ fn update(
 
     let striker_count = robots
         .iter()
+        .filter(|robot| robot.database.main_outputs.primary_state != PrimaryState::Penalized)
         .filter(|robot| robot.database.main_outputs.role == Role::Striker)
         .count();
     if game_controller.state.game_state == GameState::Set {
         if striker_count == 1 {
-            println!("Striker is present");
+            println!("One striker is present");
             exit.send(AppExit::Success);
         } else {
-            println!("No striker");
+            println!("Error: Found {striker_count} strikers!");
             exit.send(AppExit::from_code(1));
         }
     }

crates/bevyhavior_simulator/src/bin/keeper_returns_after_loser.rs

@@ -0,0 +1,77 @@
+use bevy::prelude::*;
+
+use bevyhavior_simulator::{
+    ball::BallResource,
+    game_controller::{GameController, GameControllerCommand},
+    robot::Robot,
+    time::{Ticks, TicksTime},
+};
+use linear_algebra::{point, Vector2};
+use scenario::scenario;
+use spl_network_messages::{GameState, PlayerNumber};
+use types::roles::Role;
+
+/// Regression test against an offensive keeper staying loser and never returning to the goal when losing the ball.
+/// We lead the keeper away from the goal, then put the ball in front of the goal again.
+/// If implemented correctly, the keeper should switch from loser to keeper after a short amount of
+/// time.
+#[scenario]
+fn keeper_returns_after_loser(app: &mut App) {
+    app.add_systems(Startup, startup);
+    app.add_systems(Update, update);
+}
+
+fn startup(
+    mut commands: Commands,
+    mut game_controller_commands: EventWriter<GameControllerCommand>,
+) {
+    for number in [PlayerNumber::One, PlayerNumber::Seven] {
+        commands.spawn(Robot::new(number));
+    }
+    game_controller_commands.send(GameControllerCommand::SetGameState(GameState::Ready));
+}
+
+fn update(
+    game_controller: ResMut<GameController>,
+    time: Res<Time<Ticks>>,
+    mut ball: ResMut<BallResource>,
+    robots: Query<&Robot>,
+    mut exit: EventWriter<AppExit>,
+    mut keeper_was_striker_again: Local<bool>,
+) {
+    if time.ticks() == 2800 {
+        if let Some(ball) = ball.state.as_mut() {
+            ball.position = point![-3.8, 0.0];
+            ball.velocity = Vector2::zeros();
+        }
+    }
+    if time.ticks() == 6000 {
+        if let Some(ball) = ball.state.as_mut() {
+            ball.position = point![-3.8, 0.0];
+            ball.velocity = Vector2::zeros();
+        }
+    }
+
+    if time.ticks() > 6500 {
+        for robot in robots.iter() {
+            if robot.parameters.player_number == PlayerNumber::One
+                && robot.database.main_outputs.role == Role::Striker
+            {
+                *keeper_was_striker_again = true;
+            }
+        }
+    }
+
+    if game_controller.state.hulks_team.score > 0 {
+        println!("Done");
+        exit.send(AppExit::Success);
+    }
+
+    if time.ticks() >= 15_000 {
+        if !*keeper_was_striker_again {
+            println!("Error: Keeper did not become striker again");
+        }
+        println!("No goal was scored :(");
+        exit.send(AppExit::from_code(1));
+    }
+}

crates/bevyhavior_simulator/src/bin/reappearing_ball_in_front_of_keeper.rs

@@ -1,15 +1,15 @@
 use bevy::prelude::*;
 
-use linear_algebra::point;
-use scenario::scenario;
-use spl_network_messages::{GameState, PlayerNumber};
-
 use bevyhavior_simulator::{
     ball::BallResource,
     game_controller::{GameController, GameControllerCommand},
     robot::Robot,
     time::{Ticks, TicksTime},
 };
+use linear_algebra::point;
+use scenario::scenario;
+use spl_network_messages::{GameState, PlayerNumber};
+use types::roles::Role;
 
 #[scenario]
 fn reappearing_ball_in_front_of_keeper(app: &mut App) {
@@ -39,14 +39,31 @@ fn update(
     game_controller: ResMut<GameController>,
     time: Res<Time<Ticks>>,
     mut ball: ResMut<BallResource>,
+    robots: Query<&Robot>,
     mut exit: EventWriter<AppExit>,
+    mut keeper_was_striker_once: Local<bool>,
 ) {
     if time.ticks() == 2800 {
         if let Some(ball) = ball.state.as_mut() {
             ball.position = point![-3.8, 0.0];
         }
     }
+    if game_controller.state.game_state == GameState::Playing {
+        for robot in robots.iter() {
+            if robot.parameters.player_number == PlayerNumber::One
+                && robot.database.main_outputs.role == Role::Striker
+            {
+                *keeper_was_striker_once = true;
+            }
+        }
+    }
+
     if game_controller.state.hulks_team.score > 0 {
+        if !*keeper_was_striker_once {
+            println!("Error: Keeper never became striker");
+            exit.send(AppExit::from_code(2));
+            return;
+        }
         println!("Done");
         exit.send(AppExit::Success);
     }

crates/bevyhavior_simulator/src/bin/replacement_keeper_test.rs → crates/bevyhavior_simulator/src/bin/replacement_keeper.rs

@@ -14,7 +14,7 @@ use bevyhavior_simulator::{
 use types::roles::Role;
 
 #[scenario]
-fn replacement_keeper_test(app: &mut App) {
+fn replacement_keeper(app: &mut App) {
     app.add_systems(Startup, startup);
     app.add_systems(Update, update);
 }

crates/bevyhavior_simulator/src/bin/striker_dies.rs

@@ -0,0 +1,58 @@
+use bevy::prelude::*;
+
+use scenario::scenario;
+use spl_network_messages::{GameState, PlayerNumber};
+
+use bevyhavior_simulator::{
+    game_controller::{GameController, GameControllerCommand},
+    robot::Robot,
+    time::{Ticks, TicksTime},
+};
+use types::roles::Role;
+
+#[scenario]
+fn striker_dies(app: &mut App) {
+    app.add_systems(Startup, startup);
+    app.add_systems(Update, update);
+}
+
+fn startup(
+    mut commands: Commands,
+    mut game_controller_commands: EventWriter<GameControllerCommand>,
+) {
+    for number in [
+        PlayerNumber::One,
+        PlayerNumber::Two,
+        PlayerNumber::Three,
+        PlayerNumber::Four,
+        PlayerNumber::Five,
+        PlayerNumber::Six,
+        PlayerNumber::Seven,
+    ] {
+        commands.spawn(Robot::new(number));
+    }
+    game_controller_commands.send(GameControllerCommand::SetGameState(GameState::Ready));
+}
+
+fn update(
+    mut commands: Commands,
+    game_controller: ResMut<GameController>,
+    time: Res<Time<Ticks>>,
+    mut exit: EventWriter<AppExit>,
+    robots: Query<(Entity, &Robot)>,
+) {
+    if time.ticks() == 5000 {
+        robots
+            .iter()
+            .filter(|(_, robot)| robot.database.main_outputs.role == Role::Striker)
+            .for_each(|(entity, _)| commands.entity(entity).despawn());
+    }
+    if game_controller.state.hulks_team.score > 0 {
+        println!("Done");
+        exit.send(AppExit::Success);
+    }
+    if time.ticks() >= 10_000 {
+        println!("No goal was scored :(");
+        exit.send(AppExit::from_code(1));
+    }
+}