Visual Referee during free kicks

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 }
@@ -26,6 +27,7 @@ ittapi = { workspace = true }
 linear_algebra = { workspace = true }
 log = { workspace = true }
 nalgebra = { workspace = true }
+object_detection = { workspace = true }
 parameters = { workspace = true }
 parking_lot = { workspace = true }
 path_serde = { workspace = true }
@@ -39,6 +41,7 @@ spl_network_messages = { workspace = true }
 tokio = { workspace = true }
 tokio-util = { workspace = true }
 types = { workspace = true }
+vision = { workspace = true }
 walking_engine = { workspace = true }
 
 [build-dependencies]

crates/bevyhavior_simulator/build.rs

@@ -22,6 +22,8 @@ fn main() -> Result<()> {
                     "control::behavior::node",
                     "control::game_controller_state_filter",
                     "control::kick_selector",
+                    "control::kicking_team_filter",
+                    "control::free_kick_signal_filter",
                     "control::filtered_game_controller_state_timer",
                     "control::primary_state_filter",
                     "control::motion::look_around",
@@ -42,6 +44,17 @@ fn main() -> Result<()> {
                 setup_nodes: vec!["spl_network::message_receiver"],
                 nodes: vec!["spl_network::message_filter"],
             },
+            CyclerManifest {
+                name: "ObjectDetection",
+                kind: CyclerKind::Perception,
+                instances: vec!["Top"],
+                setup_nodes: vec!["vision::image_receiver"],
+                nodes: vec![
+                    "object_detection::pose_detection",
+                    "object_detection::pose_filter",
+                    "object_detection::pose_interpretation",
+                ],
+            },
         ],
     };
     let root = "../../crates/";

crates/bevyhavior_simulator/src/bin/visual_referee_free_kick_behavior.rs

@@ -0,0 +1,143 @@
+use bevy::{ecs::system::SystemParam, prelude::*};
+
+use approx::AbsDiffEq;
+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::{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> {
+    detecting_robots_when_home: Local<'s, usize>,
+    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_robots in robots.iter().filter(|robot| {
+            matches!(
+                robot.database.main_outputs.role,
+                Role::DefenderRight | Role::MidfielderRight
+            )
+        }) {
+            if let Some(expected_referee_position) = relevant_robots
+                .database
+                .main_outputs
+                .expected_referee_position
+            {
+                let ground_to_field = relevant_robots.ground_to_field();
+                let expected_referee_position_in_ground =
+                    ground_to_field.inverse() * expected_referee_position;
+                if matches!(
+                    relevant_robots.database.main_outputs.motion_command.head_motion(),
+                    Some(HeadMotion::LookAt { target, .. }) if target.abs_diff_eq(&expected_referee_position_in_ground, 1e-4)
+                ) {
+                    *state.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.hulks_team_is_home_after_coin_toss = false;
+
+        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 let Some(expected_referee_position) = relevant_robot
+                .database
+                .main_outputs
+                .expected_referee_position
+            {
+                let ground_to_field = relevant_robot.ground_to_field();
+                let expected_referee_position_in_ground =
+                    ground_to_field.inverse() * expected_referee_position;
+
+                if matches!(
+                    relevant_robot.database.main_outputs.motion_command.head_motion(),
+                    Some(HeadMotion::LookAt { target, .. }) if target.abs_diff_eq(&expected_referee_position_in_ground, 1e-4)
+                ) {
+                    *state.detecting_robots_when_away += 1;
+                }
+            }
+        }
+    }
+
+    if (*state.detecting_robots_when_home == 2) && (*state.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

@@ -2,6 +2,7 @@ use std::{net::SocketAddr, time::Duration};
 
 use color_eyre::Result;
 use linear_algebra::Isometry2;
+use projection::camera_matrices::CameraMatrices;
 use serde::{Deserialize, Serialize};
 
 use context_attribute::context;
@@ -48,6 +49,7 @@ pub struct CycleContext {
 pub struct MainOutputs {
     pub ball_position: MainOutput<Option<BallPosition<Ground>>>,
     pub buttons: MainOutput<Buttons>,
+    pub camera_matrices: MainOutput<Option<CameraMatrices>>,
     pub cycle_time: MainOutput<CycleTime>,
     pub fall_state: MainOutput<FallState>,
     pub filtered_whistle: MainOutput<FilteredWhistle>,
@@ -56,7 +58,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>,
@@ -81,15 +83,16 @@ impl FakeData {
         Ok(MainOutputs {
             ball_position: last_database.ball_position.into(),
             buttons: last_database.buttons.into(),
+            camera_matrices: last_database.camera_matrices.clone().into(),
             cycle_time: last_database.cycle_time.into(),
             fall_state: last_database.fall_state.into(),
             filtered_whistle: last_database.filtered_whistle.clone().into(),
             game_controller_state: last_database.game_controller_state.clone().into(),
             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

@@ -44,17 +44,17 @@ fn game_controller_controller(
                 state.last_state_change = time.as_generic();
             }
             GameControllerCommand::SetKickingTeam(team) => {
-                game_controller.state.kicking_team = team;
+                game_controller.state.kicking_team = Some(team);
                 state.last_state_change = time.as_generic();
             }
             GameControllerCommand::Goal(team) => {
                 match team {
                     Team::Hulks => {
-                        game_controller.state.kicking_team = Team::Opponent;
+                        game_controller.state.kicking_team = Some(Team::Opponent);
                         &mut game_controller.state.hulks_team
                     }
                     Team::Opponent => {
-                        game_controller.state.kicking_team = Team::Hulks;
+                        game_controller.state.kicking_team = Some(Team::Hulks);
                         &mut game_controller.state.opponent_team
                     }
                 }
@@ -70,9 +70,11 @@ fn game_controller_controller(
             }
             GameControllerCommand::SetSubState(sub_state, team) => {
                 game_controller.state.sub_state = sub_state;
-                game_controller.state.kicking_team = team;
                 if sub_state == Some(SubState::PenaltyKick) {
+                    game_controller.state.kicking_team = Some(team);
                     game_controller.state.game_state = GameState::Ready;
+                } else {
+                    game_controller.state.kicking_team = None;
                 }
                 state.last_state_change = time.as_generic();
             }
@@ -129,7 +131,7 @@ impl Default for GameController {
             state: GameControllerState {
                 game_state: GameState::Initial,
                 game_phase: GamePhase::Normal,
-                kicking_team: Team::Hulks,
+                kicking_team: Some(Team::Hulks),
                 last_game_state_change: SystemTime::UNIX_EPOCH,
                 penalties: Players::new(None),
                 opponent_penalties: Players::new(None),

crates/bevyhavior_simulator/src/interfake.rs

@@ -8,7 +8,10 @@ use parking_lot::Mutex;
 
 use buffered_watch::{Receiver, Sender};
 use color_eyre::Result;
-use hardware::{NetworkInterface, RecordingInterface, SpeakerInterface, TimeInterface};
+use hardware::{
+    CameraInterface, NetworkInterface, PathsInterface, RecordingInterface, SpeakerInterface,
+    TimeInterface,
+};
 use types::{
     audio::SpeakerRequest,
     messages::{IncomingMessage, OutgoingMessage},
@@ -65,6 +68,21 @@ impl SpeakerInterface for Interfake {
     fn write_to_speakers(&self, _request: SpeakerRequest) {}
 }
 
+impl PathsInterface for Interfake {
+    fn get_paths(&self) -> types::hardware::Paths {
+        unimplemented!()
+    }
+}
+
+impl CameraInterface for Interfake {
+    fn read_from_camera(
+        &self,
+        _camera_position: types::camera_position::CameraPosition,
+    ) -> Result<types::ycbcr422_image::YCbCr422Image> {
+        unimplemented!()
+    }
+}
+
 pub trait FakeDataInterface {
     fn get_last_database_receiver(&self) -> &Mutex<Receiver<Database>>;
     fn get_last_database_sender(&self) -> &Mutex<Sender<Database>>;

crates/bevyhavior_simulator/src/lib.rs

@@ -1,6 +1,9 @@
 #![allow(unexpected_cfgs)]
 
-use hardware::{NetworkInterface, RecordingInterface, SpeakerInterface, TimeInterface};
+use hardware::{
+    CameraInterface, NetworkInterface, PathsInterface, RecordingInterface, SpeakerInterface,
+    TimeInterface,
+};
 use interfake::FakeDataInterface;
 
 pub mod autoref;
@@ -20,6 +23,12 @@ pub mod whistle;
 include!(concat!(env!("OUT_DIR"), "/generated_code.rs"));
 
 pub trait HardwareInterface:
-    TimeInterface + NetworkInterface + RecordingInterface + FakeDataInterface + SpeakerInterface
+    TimeInterface
+    + NetworkInterface
+    + RecordingInterface
+    + FakeDataInterface
+    + SpeakerInterface
+    + PathsInterface
+    + CameraInterface
 {
 }