Visual referee update: Free kick signal detection

crates/bevyhavior_simulator/Cargo.toml

@@ -6,6 +6,7 @@ license.workspace = true
 homepage.workspace = true
 
 [dependencies]
+approx = { workspace = true }
 ball_filter = { workspace = true }
 bevy = { workspace = true }
 bincode = { workspace = true }

crates/bevyhavior_simulator/src/bin/visual_referee_free_kick_behavior.rs

@@ -0,0 +1,133 @@
+use bevy::{ecs::system::SystemParam, prelude::*};
+
+use linear_algebra::point;
+use scenario::scenario;
+use spl_network_messages::{GameState, PlayerNumber, SubState, Team};
+
+use bevyhavior_simulator::{
+    ball::BallResource,
+    game_controller::{GameController, GameControllerCommand},
+    robot::Robot,
+    time::{Ticks, TicksTime},
+};
+use types::{field_dimensions::GlobalFieldSide, motion_command::HeadMotion, roles::Role};
+
+/// Is used to generate the test functions for cargo test
+#[scenario]
+fn visual_referee_free_kick_behavior(app: &mut App) {
+    app.add_systems(Startup, startup);
+    app.add_systems(Update, update);
+}
+
+#[derive(SystemParam)]
+struct State<'s> {
+    number_of_detecting_robots_when_home: Local<'s, usize>,
+    number_of_detecting_robots_when_away: Local<'s, usize>,
+}
+
+/// Runs at the start of the behavior simulator and is used to spawn in robots and set GameStates
+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 game_controller: ResMut<GameController>,
+    mut game_controller_commands: EventWriter<GameControllerCommand>,
+    time: Res<Time<Ticks>>,
+    mut exit: EventWriter<AppExit>,
+    robots: Query<&mut Robot>,
+    mut ball: ResMut<BallResource>,
+    mut state: State,
+) {
+    if time.ticks() >= 10_000 {
+        println!("Scenario failed: Time ran out. Behavior for detecting free kick kicking team was not executed correctly.");
+        exit.send(AppExit::from_code(1));
+    }
+
+    if time.ticks() == 3000 {
+        // Set substate
+        game_controller_commands.send(GameControllerCommand::SetSubState(
+            Some(SubState::PushingFreeKick),
+            Team::Opponent,
+        ));
+    }
+
+    if time.ticks() == 3005 {
+        for relevant_robot in robots.iter().filter(|robot| {
+            matches!(
+                robot.database.main_outputs.role,
+                Role::DefenderRight | Role::MidfielderRight | Role::Searcher
+            )
+        }) {
+            if matches!(
+                relevant_robot
+                    .database
+                    .main_outputs
+                    .motion_command
+                    .head_motion(),
+                Some(HeadMotion::LookAtReferee { .. })
+            ) {
+                *state.number_of_detecting_robots_when_home += 1;
+            }
+        }
+    }
+
+    if time.ticks() == 3500 {
+        // Set substate
+        game_controller_commands.send(GameControllerCommand::SetSubState(None, Team::Opponent));
+    }
+
+    if time.ticks() == 4000 {
+        // Set substate
+        game_controller_commands.send(GameControllerCommand::SetSubState(
+            Some(SubState::KickIn),
+            Team::Opponent,
+        ));
+
+        game_controller.state.global_field_side = GlobalFieldSide::Away;
+
+        if let Some(ball) = ball.state.as_mut() {
+            ball.position = point!(2.0, 4.5);
+        }
+    }
+    if time.ticks() == 4002 {
+        for relevant_robot in robots.iter().filter(|robot| {
+            matches!(
+                robot.database.main_outputs.role,
+                Role::DefenderLeft | Role::MidfielderLeft
+            )
+        }) {
+            if matches!(
+                relevant_robot
+                    .database
+                    .main_outputs
+                    .motion_command
+                    .head_motion(),
+                Some(HeadMotion::LookAtReferee { .. })
+            ) {
+                *state.number_of_detecting_robots_when_away += 1;
+            }
+        }
+    }
+
+    if (*state.number_of_detecting_robots_when_home >= 2)
+        && (*state.number_of_detecting_robots_when_away >= 2)
+    {
+        println!("Done! Successfully performed behavior for free kick kicking team detection.");
+        exit.send(AppExit::Success);
+    }
+}

crates/bevyhavior_simulator/src/fake_data.rs

@@ -56,7 +56,7 @@ pub struct MainOutputs {
     pub ground_to_field: MainOutput<Option<Isometry2<Ground, Field>>>,
     pub has_ground_contact: MainOutput<bool>,
     pub hulk_messages: MainOutput<Vec<HulkMessage>>,
-    pub majority_vote_is_referee_ready_pose_detected: MainOutput<bool>,
+    pub is_majority_vote_referee_ready_pose_detected: MainOutput<bool>,
     pub visual_referee_proceed_to_ready: MainOutput<bool>,
     pub hypothetical_ball_positions: MainOutput<Vec<HypotheticalBallPosition<Ground>>>,
     pub is_localization_converged: MainOutput<bool>,
@@ -88,8 +88,8 @@ impl FakeData {
             game_controller_address: last_database.game_controller_address.into(),
             has_ground_contact: last_database.has_ground_contact.into(),
             hulk_messages: last_database.hulk_messages.clone().into(),
-            majority_vote_is_referee_ready_pose_detected: last_database
-                .majority_vote_is_referee_ready_pose_detected
+            is_majority_vote_referee_ready_pose_detected: last_database
+                .is_majority_vote_referee_ready_pose_detected
                 .into(),
             visual_referee_proceed_to_ready: last_database.visual_referee_proceed_to_ready.into(),
             hypothetical_ball_positions: last_database.hypothetical_ball_positions.clone().into(),

crates/bevyhavior_simulator/src/game_controller.rs

@@ -5,7 +5,9 @@ use bevy::prelude::*;
 use spl_network_messages::{
     GamePhase, GameState, Penalty, PlayerNumber, SubState, Team, TeamColor, TeamState,
 };
-use types::{game_controller_state::GameControllerState, players::Players};
+use types::{
+    field_dimensions::GlobalFieldSide, game_controller_state::GameControllerState, players::Players,
+};
 
 use crate::{autoref::autoref, whistle::WhistleResource};
 
@@ -135,7 +137,7 @@ impl Default for GameController {
                 penalties: Players::new(None),
                 opponent_penalties: Players::new(None),
                 sub_state: None,
-                hulks_team_is_home_after_coin_toss: true,
+                global_field_side: GlobalFieldSide::Home,
                 hulks_team: TeamState {
                     team_number: 24,
                     field_player_color: TeamColor::Green,

crates/bevyhavior_simulator/src/robot.rs

@@ -302,7 +302,7 @@ pub fn move_robots(mut robots: Query<&mut Robot>, mut ball: ResMut<BallResource>
                 head
             }
             MotionCommand::SitDown { head } => head,
-            MotionCommand::Stand { head } => head,
+            MotionCommand::Stand { head, .. } => head,
             _ => HeadMotion::Center,
         };
 
@@ -313,6 +313,19 @@ pub fn move_robots(mut robots: Query<&mut Robot>, mut ball: ResMut<BallResource>
                 robot.database.main_outputs.look_around.yaw
             }
             HeadMotion::LookAt { target, .. } => Orientation2::from_vector(target.coords()).angle(),
+            HeadMotion::LookAtReferee { .. } => {
+                if let Some(ground_to_field) = robot.database.main_outputs.ground_to_field {
+                    let expected_referee_position = ground_to_field.inverse()
+                        * robot
+                            .database
+                            .main_outputs
+                            .expected_referee_position
+                            .unwrap_or_default();
+                    Orientation2::from_vector(expected_referee_position.coords()).angle()
+                } else {
+                    0.0
+                }
+            }
             HeadMotion::LookLeftAndRightOf { target } => {
                 let glance_factor = 0.0; //self.time_elapsed.as_secs_f32().sin();
                 target.coords().angle(&Vector2::x_axis())

crates/control/src/behavior/initial.rs

@@ -1,76 +1,53 @@
-use coordinate_systems::Field;
-use linear_algebra::Point2;
 use spl_network_messages::PlayerNumber;
 use types::{
     camera_position::CameraPosition,
+    field_dimensions::GlobalFieldSide,
     filtered_game_state::FilteredGameState,
     motion_command::{HeadMotion, ImageRegion, MotionCommand},
     primary_state::PrimaryState,
     world_state::WorldState,
 };
 
-pub fn execute(
-    world_state: &WorldState,
-    expected_referee_position: Option<Point2<Field>>,
-    enable_pose_detection: bool,
-) -> Option<MotionCommand> {
+pub fn execute(world_state: &WorldState, enable_pose_detection: bool) -> Option<MotionCommand> {
+    if world_state.robot.primary_state != PrimaryState::Initial
+        && world_state.robot.primary_state != PrimaryState::Standby
+    {
+        return None;
+    }
+
     if world_state.robot.primary_state == PrimaryState::Initial {
         return Some(MotionCommand::Initial {
             head: HeadMotion::Center,
-            should_look_for_referee: false,
         });
     }
-    if world_state.robot.primary_state == PrimaryState::Standby {
-        return Some(
-            look_at_referee(
-                expected_referee_position,
-                world_state.clone(),
-                enable_pose_detection,
-            )
-            .unwrap_or(MotionCommand::Initial {
-                head: HeadMotion::Center,
-                should_look_for_referee: false,
-            }),
-        );
-    }
-    None
-}
-
-fn look_at_referee(
-    expected_referee_position: Option<Point2<Field>>,
-    world_state: WorldState,
-    enable_pose_detection: bool,
-) -> Option<MotionCommand> {
-    let ground_to_field = world_state.robot.ground_to_field?;
-    let expected_referee_position = expected_referee_position?;
-    let filtered_game_controller_state = world_state.filtered_game_controller_state.as_ref()?;
-    if !enable_pose_detection
-        || filtered_game_controller_state.game_state != FilteredGameState::Standby
-    {
-        return None;
-    }
 
-    let position = ground_to_field.as_pose().position();
+    let filtered_game_controller_state = world_state.filtered_game_controller_state.clone()?;
 
-    if position.y().signum() == expected_referee_position.y().signum() {
-        return None;
-    };
+    let should_pose_detection_be_active = world_state.robot.primary_state == PrimaryState::Standby
+        && filtered_game_controller_state.game_state == FilteredGameState::Standby
+        && enable_pose_detection;
 
-    match (
-        world_state.robot.player_number,
-        filtered_game_controller_state.own_team_is_home_after_coin_toss,
-    ) {
-        (PlayerNumber::Four | PlayerNumber::Seven, true) => {}
-        (PlayerNumber::Two | PlayerNumber::Six, false) => {}
-        _ => return None,
-    }
+    let player_number_should_look_for_referee = matches!(
+        (
+            world_state.robot.player_number,
+            filtered_game_controller_state.global_field_side,
+        ),
+        (
+            PlayerNumber::Four | PlayerNumber::Seven,
+            GlobalFieldSide::Home
+        ) | (PlayerNumber::Two | PlayerNumber::Six, GlobalFieldSide::Away)
+    );
 
     Some(MotionCommand::Initial {
-        head: HeadMotion::LookAt {
-            target: ground_to_field.inverse() * expected_referee_position,
-            image_region_target: ImageRegion::Bottom,
-            camera: Some(CameraPosition::Top),
+        head: match (
+            should_pose_detection_be_active,
+            player_number_should_look_for_referee,
+        ) {
+            (true, true) => HeadMotion::LookAtReferee {
+                image_region_target: ImageRegion::Bottom,
+                camera: Some(CameraPosition::Top),
+            },
+            _ => HeadMotion::Center,
         },
-        should_look_for_referee: true,
     })
 }

crates/control/src/behavior/look_at_referee.rs

@@ -0,0 +1,17 @@
+use types::{
+    camera_position::CameraPosition,
+    motion_command::{HeadMotion, ImageRegion, MotionCommand},
+};
+
+pub fn execute(enable_pose_detection: bool) -> Option<MotionCommand> {
+    Some(MotionCommand::Stand {
+        head: if enable_pose_detection {
+            HeadMotion::LookAtReferee {
+                image_region_target: ImageRegion::Bottom,
+                camera: Some(CameraPosition::Top),
+            }
+        } else {
+            HeadMotion::Center
+        },
+    })
+}

crates/control/src/behavior/mod.rs

@@ -8,6 +8,7 @@ mod initial;
 mod intercept_ball;
 mod jump;
 mod look_around;
+mod look_at_referee;
 mod lost_ball;
 mod no_ground_contact;
 pub mod node;