Visual referee update: Kicking team filtering

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,8 +22,10 @@ fn main() -> Result<()> {
                     "control::behavior::node",
                     "control::dribble_path_planner",
                     "control::filtered_game_controller_state_timer",
+                    "control::free_kick_signal_filter",
                     "control::game_controller_state_filter",
                     "control::kick_selector",
+                    "control::kicking_team_filter",
                     "control::motion::look_around",
                     "control::motion::motion_selector",
                     "control::penalty_shot_direction_estimation",
@@ -44,6 +46,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/corner_kicks.rs

@@ -1,13 +1,16 @@
 use bevy::prelude::*;
 
+use linear_algebra::{point, vector};
 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::ball_position::SimulatorBallState;
 
 #[scenario]
 fn corner_kicks(app: &mut App) {
@@ -38,13 +41,22 @@ fn update(
     mut game_controller_commands: EventWriter<GameControllerCommand>,
     time: Res<Time<Ticks>>,
     mut exit: EventWriter<AppExit>,
+    mut ball: ResMut<BallResource>,
 ) {
     if time.ticks() == 3000 {
         game_controller_commands.send(GameControllerCommand::SetSubState(
             Some(SubState::CornerKick),
             Team::Hulks,
         ));
     }
+
+    if time.ticks() == 4500 {
+        ball.state = Some(SimulatorBallState {
+            position: point!(-2.25, 1.0),
+            velocity: vector![-6.0, 2.0],
+        });
+    }
+
     if time.ticks() == 5000 {
         game_controller_commands.send(GameControllerCommand::SetSubState(
             Some(SubState::CornerKick),

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

@@ -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

@@ -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};
 
@@ -44,17 +46,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 +72,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();
             }
@@ -130,12 +134,12 @@ impl Default for GameController {
                 game_state: GameState::Initial,
                 game_phase: GamePhase::Normal,
                 remaining_time_in_half: Duration::ZERO,
-                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),
                 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/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>>;