add whistle fake in simulator for opponent kickoff (#1309)

* add whistle fake in simulator for opponent kickoff

* check robot position in opponent kickoff

* set proper ground to field for filtered game controller state

* check locations in kickoff

* review changes

* add whistle to simulator penalty kicks

crates/code_generation/src/cyclers.rs

@@ -124,7 +124,7 @@ fn generate_struct(cycler: &Cycler, cyclers: &Cyclers, mode: CyclerMode) -> Toke
     };
 
     quote! {
-        pub(crate) struct Cycler<HardwareInterface>  {
+        pub struct Cycler<HardwareInterface>  {
             instance: CyclerInstance,
             hardware_interface: std::sync::Arc<HardwareInterface>,
             own_sender: buffered_watch::Sender<(std::time::SystemTime, Database)>,

crates/hulk_behavior_simulator/build.rs

@@ -25,6 +25,7 @@ fn main() -> Result<()> {
                     "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::referee_position_provider",

crates/hulk_behavior_simulator/src/fake_data.rs

@@ -10,6 +10,7 @@ use framework::MainOutput;
 use spl_network_messages::HulkMessage;
 use types::{
     ball_position::{BallPosition, HypotheticalBallPosition},
+    buttons::Buttons,
     calibration::CalibrationCommand,
     cycle_time::CycleTime,
     fall_state::FallState,
@@ -19,7 +20,6 @@ use types::{
     obstacles::Obstacle,
     parameters::{BallFilterParameters, CameraMatrixParameters},
     penalty_shot_direction::PenaltyShotDirection,
-    primary_state::PrimaryState,
     sensor_data::SensorData,
 };
 
@@ -48,6 +48,7 @@ pub struct CycleContext {
 #[derive(Default)]
 pub struct MainOutputs {
     pub ball_position: MainOutput<Option<BallPosition<Ground>>>,
+    pub buttons: MainOutput<Buttons>,
     pub cycle_time: MainOutput<CycleTime>,
     pub fall_state: MainOutput<FallState>,
     pub filtered_whistle: MainOutput<FilteredWhistle>,
@@ -62,7 +63,6 @@ pub struct MainOutputs {
     pub is_localization_converged: MainOutput<bool>,
     pub obstacles: MainOutput<Vec<Obstacle>>,
     pub penalty_shot_direction: MainOutput<Option<PenaltyShotDirection>>,
-    pub primary_state: MainOutput<PrimaryState>,
     pub sensor_data: MainOutput<SensorData>,
     pub stand_up_back_estimated_remaining_duration: MainOutput<Option<Duration>>,
     pub calibration_command: MainOutput<Option<CalibrationCommand>>,
@@ -82,6 +82,7 @@ impl FakeData {
         let last_database = &receiver.borrow_and_mark_as_seen().main_outputs;
         Ok(MainOutputs {
             ball_position: last_database.ball_position.into(),
+            buttons: last_database.buttons.into(),
             cycle_time: last_database.cycle_time.into(),
             fall_state: last_database.fall_state.into(),
             filtered_whistle: last_database.filtered_whistle.clone().into(),
@@ -97,7 +98,6 @@ impl FakeData {
             is_localization_converged: last_database.is_localization_converged.into(),
             obstacles: last_database.obstacles.clone().into(),
             penalty_shot_direction: last_database.penalty_shot_direction.into(),
-            primary_state: last_database.primary_state.into(),
             ground_to_field: last_database.ground_to_field.into(),
             sensor_data: last_database.sensor_data.clone().into(),
             stand_up_front_estimated_remaining_duration: last_database

crates/hulk_behavior_simulator/src/robot.rs

@@ -29,7 +29,7 @@ pub struct Robot {
     pub last_kick_time: Duration,
     pub ball_last_seen: Option<SystemTime>,
 
-    cycler: Cycler<Interfake>,
+    pub cycler: Cycler<Interfake>,
     control_receiver: Receiver<(SystemTime, Database)>,
     spl_network_sender: Producer<crate::structs::spl_network::MainOutputs>,
 }
@@ -78,7 +78,9 @@ impl Robot {
             .as_transform(),
         );
         database.main_outputs.has_ground_contact = true;
+        database.main_outputs.buttons.is_chest_button_pressed_once = true;
         database.main_outputs.is_localization_converged = true;
+
         subscriptions_sender
             .borrow_mut()
             .insert("additional_outputs".to_string());

crates/hulk_behavior_simulator/src/simulator.rs

@@ -1,4 +1,9 @@
-use std::{fs::read_to_string, path::Path, sync::Arc, time::Duration};
+use std::{
+    fs::read_to_string,
+    path::Path,
+    sync::Arc,
+    time::{Duration, UNIX_EPOCH},
+};
 
 use color_eyre::{
     eyre::{eyre, WrapErr},
@@ -174,6 +179,81 @@ impl Simulator {
                 )?,
             )?;
 
+            self.lua.globals().set(
+                "get_robot_pose_x",
+                scope.create_function(|_lua, player_number: usize| {
+                    let player_number =
+                        to_player_number(player_number).map_err(LuaError::external)?;
+
+                    let position_x = self
+                        .state
+                        .lock()
+                        .robots
+                        .get_mut(&player_number)
+                        .unwrap()
+                        .database
+                        .main_outputs
+                        .ground_to_field
+                        .unwrap()
+                        .inner
+                        .translation
+                        .x;
+                    Ok(position_x)
+                })?,
+            )?;
+
+            self.lua.globals().set(
+                "get_robot_pose_y",
+                scope.create_function(|_lua, player_number: usize| {
+                    let player_number =
+                        to_player_number(player_number).map_err(LuaError::external)?;
+
+                    let position_y = self
+                        .state
+                        .lock()
+                        .robots
+                        .get_mut(&player_number)
+                        .unwrap()
+                        .database
+                        .main_outputs
+                        .ground_to_field
+                        .unwrap()
+                        .inner
+                        .translation
+                        .y;
+                    Ok(position_y)
+                })?,
+            )?;
+
+            self.lua.globals().set(
+                "whistle",
+                scope.create_function(|_lua, player_number: usize| {
+                    let player_number =
+                        to_player_number(player_number).map_err(LuaError::external)?;
+
+                    self.state
+                        .lock()
+                        .robots
+                        .get_mut(&player_number)
+                        .unwrap()
+                        .database
+                        .main_outputs
+                        .filtered_whistle
+                        .is_detected = true;
+                    let time = self.state.lock().time_elapsed;
+                    self.state
+                        .lock()
+                        .robots
+                        .get_mut(&player_number)
+                        .unwrap()
+                        .database
+                        .main_outputs
+                        .filtered_whistle
+                        .last_detection = Some(UNIX_EPOCH + time);
+                    Ok(())
+                })?,
+            )?;
+
             self.lua.globals().set(
                 "create_obstacle",
                 scope.create_function(

crates/hulk_behavior_simulator/src/state.rs

@@ -19,7 +19,6 @@ use types::{
     motion_command::{HeadMotion, KickVariant, MotionCommand, OrientationMode},
     planned_path::PathSegment,
     players::Players,
-    primary_state::PrimaryState,
     support_foot::Side,
 };
 
@@ -205,17 +204,8 @@ impl State {
                 } else {
                     None
                 };
-            robot.database.main_outputs.primary_state =
-                match (robot.is_penalized, self.game_controller_state.game_state) {
-                    (true, _) => PrimaryState::Penalized,
-                    (false, GameState::Initial) => PrimaryState::Initial,
-                    (false, GameState::Standby) => PrimaryState::Standby,
-                    (false, GameState::Ready { .. }) => PrimaryState::Ready,
-                    (false, GameState::Set) => PrimaryState::Set,
-                    (false, GameState::Playing { .. }) => PrimaryState::Playing,
-                    (false, GameState::Finished) => PrimaryState::Finished,
-                };
             robot.database.main_outputs.game_controller_state = Some(self.game_controller_state);
+            robot.cycler.cycler_state.ground_to_field = ground_to_field;
             robot.cycle(&messages_sent_last_cycle)?;
 
             for message in robot.interface.take_outgoing_messages() {

crates/types/src/buttons.rs

@@ -2,7 +2,15 @@ use path_serde::{PathDeserialize, PathIntrospect, PathSerialize};
 use serde::{Deserialize, Serialize};
 
 #[derive(
-    Default, Clone, Serialize, Deserialize, PathSerialize, PathDeserialize, PathIntrospect, Debug,
+    Default,
+    Clone,
+    Copy,
+    Serialize,
+    Deserialize,
+    PathSerialize,
+    PathDeserialize,
+    PathIntrospect,
+    Debug,
 )]
 pub struct Buttons {
     pub is_chest_button_pressed_once: bool,

tests/behavior/golden_goal_opponent_kickoff.lua

@@ -42,11 +42,30 @@ function on_cycle()
     end
 
     if state.cycle_count == 1700 then
-        -- TODO: simulate whistle
-        state.ball.velocity = { -3.0, 1.5 }
+        whistle(1)
+        whistle(2)
+        whistle(3)
+        whistle(4)
+        whistle(5)
+        whistle(6)
+        whistle(7)
     end
 
-    if state.cycle_count == 2300 then
+    -- 10 frames in simulator are counted as 10 seconds in the game_controller_state_filter and thus correspond to the opponent kick off time
+    if state.cycle_count >= 1700 and state.cycle_count < 2200 then
+        for i = 1, 7 do
+            is_in_opponent_half = get_robot_pose_x(i) >= 0.0
+            if is_in_opponent_half then
+                error("Illegal Position in Kickoff Playing")
+            end
+            is_in_center_circle = math.sqrt(get_robot_pose_x(i) ^ 2 + get_robot_pose_y(i) ^ 2) <= 0.75
+            if is_in_center_circle then
+                error("Illegal Position in Kickoff Playing")
+            end
+        end
+    end
+
+    if state.cycle_count == 2200 then
         state.game_controller_state.game_state = "Playing"
     end
 

tests/behavior/ingamepenalty_attacking.lua

@@ -59,6 +59,15 @@ function on_cycle()
   if state.cycle_count == 3700 then
     state.game_controller_state.game_state = "Set"
   end
+  if state.cycle_count == 3701 then
+    whistle(1)
+    whistle(2)
+    whistle(3)
+    whistle(4)
+    whistle(5)
+    whistle(6)
+    whistle(7)
+  end
 
   if state.cycle_count == 4000 then
     state.game_controller_state.game_state = "Playing"

tests/behavior/ingamepenalty_defending.lua

@@ -74,7 +74,17 @@ function on_cycle()
     state.game_controller_state.game_state = "Set"
   end
 
-  if state.cycle_count == 4300 then
+  if state.cycle_count == 4001 then
+    whistle(1)
+    whistle(2)
+    whistle(3)
+    whistle(4)
+    whistle(5)
+    whistle(6)
+    whistle(7)
+  end
+
+  if state.cycle_count == 5000 then
     state.game_controller_state.game_state = "Playing"
   end