diff --git a/Cargo.lock b/Cargo.lock index 92096c48bd..b8818e5905 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -1118,6 +1118,7 @@ dependencies = [ name = "bevyhavior_simulator" version = "0.1.0" dependencies = [ + "approx", "ball_filter", "bevy", "bincode", diff --git a/crates/bevyhavior_simulator/Cargo.toml b/crates/bevyhavior_simulator/Cargo.toml index 7bc03a6179..5d3c8fccce 100644 --- a/crates/bevyhavior_simulator/Cargo.toml +++ b/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 } diff --git a/crates/bevyhavior_simulator/src/bin/visual_referee_free_kick_behavior.rs b/crates/bevyhavior_simulator/src/bin/visual_referee_free_kick_behavior.rs new file mode 100644 index 0000000000..62395117e2 --- /dev/null +++ b/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, +) { + 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, + mut game_controller_commands: EventWriter, + time: Res>, + mut exit: EventWriter, + robots: Query<&mut Robot>, + mut ball: ResMut, + 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); + } +} diff --git a/crates/bevyhavior_simulator/src/game_controller.rs b/crates/bevyhavior_simulator/src/game_controller.rs index 8a11bcac66..b54377e64a 100644 --- a/crates/bevyhavior_simulator/src/game_controller.rs +++ b/crates/bevyhavior_simulator/src/game_controller.rs @@ -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 = Some(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(); }