diff --git a/Cargo.lock b/Cargo.lock index c87810f5fa..3c909d18ec 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -2654,6 +2654,7 @@ dependencies = [ "ordered-float 4.6.0", "path_serde", "projection", + "proptest", "rand 0.9.0", "rand_chacha 0.9.0", "serde", @@ -4874,7 +4875,7 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "b77d01e822461baa8409e156015a1d91735549f0f2c17691bd2d996bef238f7f" dependencies = [ "byteorder-lite", - "quick-error", + "quick-error 2.0.1", ] [[package]] @@ -6787,6 +6788,26 @@ dependencies = [ "types", ] +[[package]] +name = "proptest" +version = "1.6.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "14cae93065090804185d3b75f0bf93b8eeda30c7a9b4a33d3bdb3988d6229e50" +dependencies = [ + "bit-set 0.8.0", + "bit-vec 0.8.0", + "bitflags 2.9.0", + "lazy_static", + "num-traits", + "rand 0.8.5", + "rand_chacha 0.3.1", + "rand_xorshift", + "regex-syntax 0.8.5", + "rusty-fork", + "tempfile", + "unarray", +] + [[package]] name = "qoi" version = "0.4.1" @@ -6796,6 +6817,12 @@ dependencies = [ "bytemuck", ] +[[package]] +name = "quick-error" +version = "1.2.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "a1d01941d82fa2ab50be1e79e6714289dd7cde78eba4c074bc5a4374f650dfe0" + [[package]] name = "quick-error" version = "2.0.1" @@ -6936,6 +6963,15 @@ dependencies = [ "rand 0.9.0", ] +[[package]] +name = "rand_xorshift" +version = "0.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "d25bf25ec5ae4a3f1b92f929810509a2f53d7dca2f50b794ff57e3face536c8f" +dependencies = [ + "rand_core 0.6.4", +] + [[package]] name = "range-alloc" version = "0.1.4" @@ -7005,7 +7041,7 @@ dependencies = [ "avif-serialize", "imgref", "loop9", - "quick-error", + "quick-error 2.0.1", "rav1e", "rayon", "rgb", @@ -7382,6 +7418,18 @@ version = "1.0.20" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "eded382c5f5f786b989652c49544c4877d9f015cc22e145a5ea8ea66c2921cd2" +[[package]] +name = "rusty-fork" +version = "0.3.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "cb3dcc6e454c328bb824492db107ab7c0ae8fcffe4ad210136ef014458c1bc4f" +dependencies = [ + "fnv", + "quick-error 1.2.3", + "tempfile", + "wait-timeout", +] + [[package]] name = "rustybuzz" version = "0.14.1" @@ -8757,6 +8805,12 @@ dependencies = [ "winapi", ] +[[package]] +name = "unarray" +version = "0.1.4" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "eaea85b334db583fe3274d12b4cd1880032beab409c0d774be044d4480ab9a94" + [[package]] name = "unicase" version = "2.8.1" @@ -8974,6 +9028,15 @@ dependencies = [ "types", ] +[[package]] +name = "wait-timeout" +version = "0.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "09ac3b126d3914f9849036f826e054cbabdc8519970b8998ddaf3b5bd3c65f11" +dependencies = [ + "libc", +] + [[package]] name = "walkdir" version = "2.5.0" diff --git a/Cargo.toml b/Cargo.toml index c857a4b77b..84f1e69e32 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -176,6 +176,7 @@ prettyplease = "0.2.29" proc-macro-error = "1.0.4" proc-macro2 = { version = "1.0.93", features = ["span-locations"] } projection = { path = "crates/projection" } +proptest = "1.6.0" quote = "1.0.38" rand = "0.9.0" rand_chacha = { version = "0.9.0", features = ["serde"] } diff --git a/crates/bevyhavior_simulator/build.rs b/crates/bevyhavior_simulator/build.rs index 1194bce6ca..2de77c1a5a 100644 --- a/crates/bevyhavior_simulator/build.rs +++ b/crates/bevyhavior_simulator/build.rs @@ -20,17 +20,19 @@ fn main() -> Result<()> { "control::active_vision", "control::ball_state_composer", "control::behavior::node", + "control::dribble_path_planner", + "control::filtered_game_controller_state_timer", "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::penalty_shot_direction_estimation", + "control::primary_state_filter", "control::referee_position_provider", "control::role_assignment", "control::rule_obstacle_composer", "control::search_suggestor", + "control::team_ball_receiver", "control::time_to_reach_kick_position", "control::world_state_composer", ], diff --git a/crates/bevyhavior_simulator/src/autoref.rs b/crates/bevyhavior_simulator/src/autoref.rs index ed25ebe935..17f6007887 100644 --- a/crates/bevyhavior_simulator/src/autoref.rs +++ b/crates/bevyhavior_simulator/src/autoref.rs @@ -141,7 +141,7 @@ pub fn auto_assistant_referee( GameControllerCommand::SetGamePhase(_) => {} GameControllerCommand::SetSubState(Some(SubState::CornerKick), team) => { let side = if let Some(ball) = ball.state.as_mut() { - if ball.position.x() >= 0.0 { + if ball.position.y() <= 0.0 { Side::Left } else { Side::Right diff --git a/crates/bevyhavior_simulator/src/bin/golden_goal_striker_penalized.rs b/crates/bevyhavior_simulator/src/bin/golden_goal_striker_penalized.rs index dde36d8c11..cb6c8bfee5 100644 --- a/crates/bevyhavior_simulator/src/bin/golden_goal_striker_penalized.rs +++ b/crates/bevyhavior_simulator/src/bin/golden_goal_striker_penalized.rs @@ -10,7 +10,7 @@ use bevyhavior_simulator::{ robot::Robot, time::{Ticks, TicksTime}, }; -use types::roles::Role; +use types::{primary_state::PrimaryState, roles::Role}; #[scenario] fn golden_goal(app: &mut App) { @@ -59,14 +59,15 @@ fn update( let striker_count = robots .iter() + .filter(|robot| robot.database.main_outputs.primary_state != PrimaryState::Penalized) .filter(|robot| robot.database.main_outputs.role == Role::Striker) .count(); if game_controller.state.game_state == GameState::Set { if striker_count == 1 { - println!("Striker is present"); + println!("One striker is present"); exit.send(AppExit::Success); } else { - println!("No striker"); + println!("Error: Found {striker_count} strikers!"); exit.send(AppExit::from_code(1)); } } diff --git a/crates/bevyhavior_simulator/src/bin/keeper_returns_after_loser.rs b/crates/bevyhavior_simulator/src/bin/keeper_returns_after_loser.rs new file mode 100644 index 0000000000..41cb7ee5f2 --- /dev/null +++ b/crates/bevyhavior_simulator/src/bin/keeper_returns_after_loser.rs @@ -0,0 +1,77 @@ +use bevy::prelude::*; + +use bevyhavior_simulator::{ + ball::BallResource, + game_controller::{GameController, GameControllerCommand}, + robot::Robot, + time::{Ticks, TicksTime}, +}; +use linear_algebra::{point, Vector2}; +use scenario::scenario; +use spl_network_messages::{GameState, PlayerNumber}; +use types::roles::Role; + +/// Regression test against an offensive keeper staying loser and never returning to the goal when losing the ball. +/// We lead the keeper away from the goal, then put the ball in front of the goal again. +/// If implemented correctly, the keeper should switch from loser to keeper after a short amount of +/// time. +#[scenario] +fn keeper_returns_after_loser(app: &mut App) { + app.add_systems(Startup, startup); + app.add_systems(Update, update); +} + +fn startup( + mut commands: Commands, + mut game_controller_commands: EventWriter, +) { + for number in [PlayerNumber::One, PlayerNumber::Seven] { + commands.spawn(Robot::new(number)); + } + game_controller_commands.send(GameControllerCommand::SetGameState(GameState::Ready)); +} + +fn update( + game_controller: ResMut, + time: Res>, + mut ball: ResMut, + robots: Query<&Robot>, + mut exit: EventWriter, + mut keeper_was_striker_again: Local, +) { + if time.ticks() == 2800 { + if let Some(ball) = ball.state.as_mut() { + ball.position = point![-3.8, 0.0]; + ball.velocity = Vector2::zeros(); + } + } + if time.ticks() == 6000 { + if let Some(ball) = ball.state.as_mut() { + ball.position = point![-3.8, 0.0]; + ball.velocity = Vector2::zeros(); + } + } + + if time.ticks() > 6500 { + for robot in robots.iter() { + if robot.parameters.player_number == PlayerNumber::One + && robot.database.main_outputs.role == Role::Striker + { + *keeper_was_striker_again = true; + } + } + } + + if game_controller.state.hulks_team.score > 0 { + println!("Done"); + exit.send(AppExit::Success); + } + + if time.ticks() >= 15_000 { + if !*keeper_was_striker_again { + println!("Error: Keeper did not become striker again"); + } + println!("No goal was scored :("); + exit.send(AppExit::from_code(1)); + } +} diff --git a/crates/bevyhavior_simulator/src/bin/reappearing_ball_in_front_of_keeper.rs b/crates/bevyhavior_simulator/src/bin/reappearing_ball_in_front_of_keeper.rs index e903d81a99..92345a98bd 100644 --- a/crates/bevyhavior_simulator/src/bin/reappearing_ball_in_front_of_keeper.rs +++ b/crates/bevyhavior_simulator/src/bin/reappearing_ball_in_front_of_keeper.rs @@ -1,15 +1,15 @@ use bevy::prelude::*; -use linear_algebra::point; -use scenario::scenario; -use spl_network_messages::{GameState, PlayerNumber}; - use bevyhavior_simulator::{ ball::BallResource, game_controller::{GameController, GameControllerCommand}, robot::Robot, time::{Ticks, TicksTime}, }; +use linear_algebra::point; +use scenario::scenario; +use spl_network_messages::{GameState, PlayerNumber}; +use types::roles::Role; #[scenario] fn reappearing_ball_in_front_of_keeper(app: &mut App) { @@ -39,14 +39,31 @@ fn update( game_controller: ResMut, time: Res>, mut ball: ResMut, + robots: Query<&Robot>, mut exit: EventWriter, + mut keeper_was_striker_once: Local, ) { if time.ticks() == 2800 { if let Some(ball) = ball.state.as_mut() { ball.position = point![-3.8, 0.0]; } } + if game_controller.state.game_state == GameState::Playing { + for robot in robots.iter() { + if robot.parameters.player_number == PlayerNumber::One + && robot.database.main_outputs.role == Role::Striker + { + *keeper_was_striker_once = true; + } + } + } + if game_controller.state.hulks_team.score > 0 { + if !*keeper_was_striker_once { + println!("Error: Keeper never became striker"); + exit.send(AppExit::from_code(2)); + return; + } println!("Done"); exit.send(AppExit::Success); } diff --git a/crates/bevyhavior_simulator/src/bin/replacement_keeper_test.rs b/crates/bevyhavior_simulator/src/bin/replacement_keeper.rs similarity index 98% rename from crates/bevyhavior_simulator/src/bin/replacement_keeper_test.rs rename to crates/bevyhavior_simulator/src/bin/replacement_keeper.rs index e4654b063d..659c0aa707 100644 --- a/crates/bevyhavior_simulator/src/bin/replacement_keeper_test.rs +++ b/crates/bevyhavior_simulator/src/bin/replacement_keeper.rs @@ -14,7 +14,7 @@ use bevyhavior_simulator::{ use types::roles::Role; #[scenario] -fn replacement_keeper_test(app: &mut App) { +fn replacement_keeper(app: &mut App) { app.add_systems(Startup, startup); app.add_systems(Update, update); } diff --git a/crates/bevyhavior_simulator/src/bin/striker_dies.rs b/crates/bevyhavior_simulator/src/bin/striker_dies.rs new file mode 100644 index 0000000000..1c4afca6af --- /dev/null +++ b/crates/bevyhavior_simulator/src/bin/striker_dies.rs @@ -0,0 +1,58 @@ +use bevy::prelude::*; + +use scenario::scenario; +use spl_network_messages::{GameState, PlayerNumber}; + +use bevyhavior_simulator::{ + game_controller::{GameController, GameControllerCommand}, + robot::Robot, + time::{Ticks, TicksTime}, +}; +use types::roles::Role; + +#[scenario] +fn striker_dies(app: &mut App) { + app.add_systems(Startup, startup); + app.add_systems(Update, update); +} + +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 commands: Commands, + game_controller: ResMut, + time: Res>, + mut exit: EventWriter, + robots: Query<(Entity, &Robot)>, +) { + if time.ticks() == 5000 { + robots + .iter() + .filter(|(_, robot)| robot.database.main_outputs.role == Role::Striker) + .for_each(|(entity, _)| commands.entity(entity).despawn()); + } + if game_controller.state.hulks_team.score > 0 { + println!("Done"); + exit.send(AppExit::Success); + } + if time.ticks() >= 10_000 { + println!("No goal was scored :("); + exit.send(AppExit::from_code(1)); + } +} diff --git a/crates/bevyhavior_simulator/src/bin/striker_from_unseen_ball.rs b/crates/bevyhavior_simulator/src/bin/striker_from_unseen_ball.rs new file mode 100644 index 0000000000..c054638c4e --- /dev/null +++ b/crates/bevyhavior_simulator/src/bin/striker_from_unseen_ball.rs @@ -0,0 +1,66 @@ +use bevy::prelude::*; + +use linear_algebra::{point, vector, Isometry2, Vector2}; +use scenario::scenario; +use spl_network_messages::{GameState, PlayerNumber}; + +use bevyhavior_simulator::{ + ball::BallResource, + game_controller::{GameController, GameControllerCommand}, + robot::Robot, + time::{Ticks, TicksTime}, +}; +use types::{ball_position::SimulatorBallState, roles::Role}; + +#[scenario] +fn striker_from_unseen_ball(app: &mut App) { + app.add_systems(Startup, startup); + app.add_systems(Update, update); +} + +fn startup( + mut commands: Commands, + mut ball: ResMut, + mut game_controller_commands: EventWriter, +) { + let mut one = Robot::new(PlayerNumber::One); + *one.ground_to_field_mut() = Isometry2::from_parts(vector![-2.0, -0.2], 0.0); + commands.spawn(one); + let mut two = Robot::new(PlayerNumber::Two); + // 0.00001 is necessary to avoid #1038 for some reason + *two.ground_to_field_mut() = Isometry2::from_parts(vector![0.0, 0.00001], 0.0); + commands.spawn(two); + + ball.state = Some(SimulatorBallState { + position: point![0.0, -0.3], + velocity: Vector2::zeros(), + }); + + game_controller_commands.send(GameControllerCommand::SetGameState(GameState::Playing)); +} + +fn update( + game_controller: ResMut, + time: Res>, + robots: Query<&Robot>, + mut exit: EventWriter, +) { + if time.ticks() == 40 + && !robots.iter().any(|robot| { + robot.parameters.player_number == PlayerNumber::Two + && robot.database.main_outputs.role == Role::Striker + }) + { + println!("Error: Two didn't become striker when sent a nearby ball position"); + exit.send(AppExit::from_code(2)); + } + + if game_controller.state.hulks_team.score > 0 { + println!("Done"); + exit.send(AppExit::Success); + } + if time.ticks() >= 10_000 { + println!("No goal was scored :("); + exit.send(AppExit::from_code(1)); + } +} diff --git a/crates/bevyhavior_simulator/src/bin/walk_around_ball.rs b/crates/bevyhavior_simulator/src/bin/walk_around_ball.rs index 285102a1b0..397baa62ac 100644 --- a/crates/bevyhavior_simulator/src/bin/walk_around_ball.rs +++ b/crates/bevyhavior_simulator/src/bin/walk_around_ball.rs @@ -1,15 +1,18 @@ +use std::time::SystemTime; + use bevy::prelude::*; -use linear_algebra::point; +use linear_algebra::{point, Vector2}; use scenario::scenario; use spl_network_messages::{GameState, PlayerNumber}; use bevyhavior_simulator::{ ball::BallResource, - game_controller::GameControllerCommand, + game_controller::{GameController, GameControllerCommand}, robot::Robot, time::{Ticks, TicksTime}, }; +use types::ball_position::BallPosition; #[scenario] fn walk_around_ball(app: &mut App) { @@ -25,14 +28,32 @@ fn startup( game_controller_commands.send(GameControllerCommand::SetGameState(GameState::Ready)); } -fn update(time: Res>, mut ball: ResMut, mut exit: EventWriter) { - if time.ticks() == 2_800 { +fn update( + game_controller: ResMut, + time: Res>, + mut robots: Query<&mut Robot>, + mut ball: ResMut, + mut exit: EventWriter, +) { + if time.ticks() == 3200 { if let Some(ball) = ball.state.as_mut() { ball.position = point![0.0, 0.0]; + + // loser never looks behind itself, so we need to tell it where the ball is + let mut robot = robots.get_single_mut().unwrap(); + robot.database.main_outputs.ball_position = Some(BallPosition { + position: robot.ground_to_field().inverse() * ball.position, + velocity: Vector2::zeros(), + last_seen: SystemTime::UNIX_EPOCH + time.elapsed(), + }); } } - if time.ticks() >= 10_000 { + if game_controller.state.hulks_team.score > 0 { println!("Done"); exit.send(AppExit::Success); } + if time.ticks() >= 10_000 { + println!("No goal was scored :("); + exit.send(AppExit::from_code(1)); + } } diff --git a/crates/bevyhavior_simulator/src/interfake.rs b/crates/bevyhavior_simulator/src/interfake.rs index a8329a143a..c54e5e11f0 100644 --- a/crates/bevyhavior_simulator/src/interfake.rs +++ b/crates/bevyhavior_simulator/src/interfake.rs @@ -17,7 +17,7 @@ use types::{ use crate::{cyclers::control::Database, HardwareInterface}; pub struct Interfake { - time: SystemTime, + time: Mutex, messages: Arc>>, last_database_receiver: Mutex>, last_database_sender: Mutex>, @@ -28,7 +28,7 @@ impl Default for Interfake { let (last_database_sender, last_database_receiver) = buffered_watch::channel(Default::default()); Self { - time: UNIX_EPOCH, + time: Mutex::new(UNIX_EPOCH), messages: Default::default(), last_database_receiver: Mutex::new(last_database_receiver), last_database_sender: Mutex::new(last_database_sender), @@ -57,7 +57,7 @@ impl RecordingInterface for Interfake { impl TimeInterface for Interfake { fn get_now(&self) -> SystemTime { - self.time + *self.time.lock() } } @@ -81,6 +81,10 @@ impl FakeDataInterface for Interfake { } impl Interfake { + pub fn set_time(&self, now: SystemTime) { + *self.time.lock() = now; + } + pub fn take_outgoing_messages(&self) -> Vec { take(&mut self.messages.lock()) } diff --git a/crates/bevyhavior_simulator/src/robot.rs b/crates/bevyhavior_simulator/src/robot.rs index e51958e7c0..549a62d4e1 100644 --- a/crates/bevyhavior_simulator/src/robot.rs +++ b/crates/bevyhavior_simulator/src/robot.rs @@ -216,6 +216,7 @@ pub fn move_robots(mut robots: Query<&mut Robot>, mut ball: ResMut for mut robot in &mut robots { if let Some(ball) = robot.database.main_outputs.ball_position.as_mut() { ball.position += ball.velocity * time.delta_secs(); + ball.velocity *= 0.98 } let parameters = &robot.parameters; @@ -284,9 +285,11 @@ pub fn move_robots(mut robots: Query<&mut Robot>, mut ball: ResMut Side::Right => 1.0, }; - // TODO: Check if ball is even in range - // let kick_location = ground_to_field * (); - if (time.elapsed() - robot.last_kick_time).as_secs_f32() > 1.0 { + let in_range = + (robot.ground_to_field().as_pose().position() - ball.position).norm() < 0.3; + let previous_kick_finished = + (time.elapsed() - robot.last_kick_time).as_secs_f32() > 1.0; + if in_range && previous_kick_finished { let direction = match kick { KickVariant::Forward => vector![1.0, 0.0], KickVariant::Turn => vector![0.707, 0.707 * side], @@ -389,6 +392,7 @@ pub fn cycle_robots( }; robot.database.main_outputs.game_controller_state = Some(game_controller.state.clone()); robot.cycler.cycler_state.ground_to_field = robot.ground_to_field(); + robot.interface.set_time(now); robot.cycle(&messages_sent_last_cycle).unwrap(); for message in robot.interface.take_outgoing_messages() { diff --git a/crates/bevyhavior_simulator/src/scenario.rs b/crates/bevyhavior_simulator/src/scenario.rs index 68636ee3db..3834b392c2 100644 --- a/crates/bevyhavior_simulator/src/scenario.rs +++ b/crates/bevyhavior_simulator/src/scenario.rs @@ -1,8 +1,4 @@ -use bevy::app::{App, Plugins}; use clap::Parser; -use color_eyre::Result; - -use crate::simulator::{AppExt, SimulatorPlugin}; #[derive(Parser)] pub struct Arguments { @@ -10,12 +6,3 @@ pub struct Arguments { #[arg(short, long)] pub run: bool, } - -pub fn run_scenario(plugin: impl Plugins, with_recording: bool) -> Result<()> { - let args = Arguments::try_parse().unwrap(); - - App::new() - .add_plugins(SimulatorPlugin::default().with_recording(!args.run && with_recording)) - .add_plugins(plugin) - .run_to_completion() -} diff --git a/crates/control/Cargo.toml b/crates/control/Cargo.toml index 2bcbad54d4..18ecbd3e37 100644 --- a/crates/control/Cargo.toml +++ b/crates/control/Cargo.toml @@ -30,6 +30,7 @@ num-traits = { workspace = true } ordered-float = { workspace = true } path_serde = { workspace = true } projection = { workspace = true } +proptest = { workspace = true } rand = { workspace = true } rand_chacha = { workspace = true } serde = { workspace = true } diff --git a/crates/control/src/behavior/defend.rs b/crates/control/src/behavior/defend.rs index f037681c56..c24bd59499 100644 --- a/crates/control/src/behavior/defend.rs +++ b/crates/control/src/behavior/defend.rs @@ -231,22 +231,13 @@ fn defend_pose( } else { role_positions.defender_y_offset }; - let position_to_defend = point![x_offset, y_offset]; let in_passive_mode = ball.ball_in_ground.coords().norm() >= role_positions.defender_passive_distance; - - let mut distance_to_target = match (in_passive_mode, field_side, ball.field_side) { - (true, _, _) => role_positions.defender_aggressive_ring_radius, - (_, Side::Left, Side::Left) | (_, Side::Right, Side::Right) => { - role_positions.defender_aggressive_ring_radius - } - _ => role_positions.defender_passive_ring_radius, - }; - if in_passive_mode { - let passive_target_position = position_to_defend + (Vector2::x_axis() * distance_to_target); + let passive_target_position = position_to_defend + + (Vector2::x_axis() * role_positions.defender_aggressive_ring_radius); return Some( ground_to_field.inverse() * Pose2::::new( @@ -256,12 +247,19 @@ fn defend_pose( ); } - distance_to_target = penalty_kick_defender_radius( + let distance_to_target = if field_side == ball.field_side { + role_positions.defender_aggressive_ring_radius + } else { + role_positions.defender_passive_ring_radius + }; + + let distance_to_target = penalty_kick_defender_radius( distance_to_target, world_state.filtered_game_controller_state.as_ref(), field_dimensions, ); let defend_pose = block_on_circle(ball.ball_in_field, position_to_defend, distance_to_target); + Some(ground_to_field.inverse() * defend_pose) } diff --git a/crates/control/src/behavior/dribble.rs b/crates/control/src/behavior/dribble.rs index 02fa323a23..5a67116895 100644 --- a/crates/control/src/behavior/dribble.rs +++ b/crates/control/src/behavior/dribble.rs @@ -1,6 +1,5 @@ use coordinate_systems::{Ground, UpcomingSupport}; -use geometry::look_at::LookAt; -use linear_algebra::{Isometry2, Point, Pose2}; +use linear_algebra::{Isometry2, Pose2}; use spl_network_messages::GamePhase; use types::{ camera_position::CameraPosition, @@ -13,7 +12,7 @@ use types::{ world_state::WorldState, }; -use super::walk_to_pose::{hybrid_alignment, WalkPathPlanner}; +use super::walk_to_pose::WalkPathPlanner; #[allow(clippy::too_many_arguments)] pub fn execute( @@ -21,7 +20,7 @@ pub fn execute( walk_path_planner: &WalkPathPlanner, in_walk_kicks: &InWalkKicksParameters, parameters: &DribblingParameters, - dribble_path: Option>, + dribble_path_plan: Option<(OrientationMode, Vec)>, mut walk_speed: WalkSpeed, ) -> Option { let ball_position = world_state.ball?.ball_in_ground; @@ -62,27 +61,6 @@ pub fn execute( return Some(command); } - let best_kick_decision = match kick_decisions.first() { - Some(decision) => decision, - None => return Some(MotionCommand::Stand { head }), - }; - - let best_pose = best_kick_decision.kick_pose; - - let hybrid_orientation_mode = hybrid_alignment( - best_pose, - parameters.hybrid_align_distance, - parameters.distance_to_be_aligned, - ); - let orientation_mode = match hybrid_orientation_mode { - types::motion_command::OrientationMode::AlignWithPath - if ball_position.coords().norm() > 0.0 => - { - OrientationMode::Override(Point::origin().look_at(&ball_position)) - } - orientation_mode => orientation_mode, - }; - if let Some(FilteredGameControllerState { game_phase: GamePhase::PenaltyShootout { .. }, .. @@ -91,8 +69,8 @@ pub fn execute( walk_speed = WalkSpeed::Slow; } - match dribble_path { - Some(path) => Some(walk_path_planner.walk_with_obstacle_avoiding_arms( + match dribble_path_plan { + Some((orientation_mode, path)) => Some(walk_path_planner.walk_with_obstacle_avoiding_arms( head, orientation_mode, path, diff --git a/crates/control/src/behavior/node.rs b/crates/control/src/behavior/node.rs index b71c5f310e..2f2b6590d9 100644 --- a/crates/control/src/behavior/node.rs +++ b/crates/control/src/behavior/node.rs @@ -14,7 +14,7 @@ use types::{ field_dimensions::{FieldDimensions, Side}, filtered_game_controller_state::FilteredGameControllerState, filtered_game_state::FilteredGameState, - motion_command::{MotionCommand, WalkSpeed}, + motion_command::{MotionCommand, OrientationMode, WalkSpeed}, parameters::{ BehaviorParameters, InWalkKicksParameters, InterceptBallParameters, KeeperMotionParameters, LostBallParameters, @@ -27,8 +27,6 @@ use types::{ world_state::WorldState, }; -use crate::dribble_path_planner; - use super::{ animation, calibrate, defend::Defend, @@ -42,7 +40,6 @@ use super::{ #[derive(Deserialize, Serialize)] pub struct Behavior { - last_motion_command: MotionCommand, last_known_ball_position: Point2, active_since: Option, previous_role: Role, @@ -53,13 +50,10 @@ pub struct CreationContext {} #[context] pub struct CycleContext { - path_obstacles_output: AdditionalOutput, "path_obstacles">, - dribble_path_obstacles_output: AdditionalOutput, "dribble_path_obstacles">, - active_action_output: AdditionalOutput, - expected_referee_position: Input>, "expected_referee_position?">, has_ground_contact: Input, world_state: Input, + dribble_path_plan: Input)>, "dribble_path_plan?">, cycle_time: Input, is_localization_converged: Input, @@ -82,19 +76,22 @@ pub struct CycleContext { support_walk_speed: Parameter, walk_to_kickoff_walk_speed: Parameter, walk_to_penalty_kick_walk_speed: Parameter, + + path_obstacles_output: AdditionalOutput, "path_obstacles">, + active_action_output: AdditionalOutput, + + last_motion_command: CyclerState, } #[context] #[derive(Default)] pub struct MainOutputs { pub motion_command: MainOutput, - pub dribble_path: MainOutput>>, } impl Behavior { pub fn new(_context: CreationContext) -> Result { Ok(Self { - last_motion_command: MotionCommand::Unstiff, last_known_ball_position: point![0.0, 0.0], active_since: None, previous_role: Role::Searcher, @@ -106,7 +103,6 @@ impl Behavior { if let Some(command) = &context.parameters.injected_motion_command { return Ok(MainOutputs { motion_command: command.clone().into(), - dribble_path: None.into(), }); } @@ -235,13 +231,13 @@ impl Behavior { context.field_dimensions, &world_state.obstacles, &context.parameters.path_planning, - &self.last_motion_command, + context.last_motion_command, ); let walk_and_stand = WalkAndStand::new( world_state, &context.parameters.walk_and_stand, &walk_path_planner, - &self.last_motion_command, + context.last_motion_command, ); let look_action = LookAction::new(world_state); let defend = Defend::new( @@ -252,23 +248,6 @@ impl Behavior { &look_action, ); - let mut dribble_path_obstacles = None; - let mut dribble_path_obstacles_output = AdditionalOutput::new( - context.path_obstacles_output.is_subscribed() - || context.dribble_path_obstacles_output.is_subscribed(), - &mut dribble_path_obstacles, - ); - - let dribble_path = dribble_path_planner::plan( - &walk_path_planner, - world_state, - &context.parameters.dribbling, - &mut dribble_path_obstacles_output, - ); - context - .dribble_path_obstacles_output - .fill_if_subscribed(|| dribble_path_obstacles.clone().unwrap_or_default()); - let (action, motion_command) = actions .iter() .find_map(|action| { @@ -368,7 +347,7 @@ impl Behavior { &walk_path_planner, context.in_walk_kicks, &context.parameters.dribbling, - dribble_path.clone(), + context.dribble_path_plan.cloned(), *context.dribble_walk_speed, ), Action::Jump => jump::execute(world_state), @@ -498,17 +477,10 @@ impl Behavior { }); context.active_action_output.fill_if_subscribed(|| *action); - self.last_motion_command = motion_command.clone(); - - if matches!(action, Action::Dribble) { - context - .path_obstacles_output - .fill_if_subscribed(|| dribble_path_obstacles.unwrap_or_default()) - } + *context.last_motion_command = motion_command.clone(); Ok(MainOutputs { motion_command: motion_command.into(), - dribble_path: dribble_path.into(), }) } } diff --git a/crates/control/src/dribble_path_planner.rs b/crates/control/src/dribble_path_planner.rs index 0e62112e80..5ff9726361 100644 --- a/crates/control/src/dribble_path_planner.rs +++ b/crates/control/src/dribble_path_planner.rs @@ -1,37 +1,150 @@ -use framework::AdditionalOutput; -use spl_network_messages::Team; use std::f32::consts::PI; + +use coordinate_systems::{Field, Ground}; +use geometry::look_at::LookAt; +use linear_algebra::{Isometry2, Point, Pose2}; +use serde::{Deserialize, Serialize}; + +use color_eyre::{eyre::Ok, Result}; +use context_attribute::context; +use framework::{AdditionalOutput, MainOutput}; +use spl_network_messages::Team; use types::{ - filtered_game_controller_state::FilteredGameControllerState, parameters::DribblingParameters, - path_obstacles::PathObstacle, planned_path::PathSegment, world_state::WorldState, + field_dimensions::FieldDimensions, + filtered_game_controller_state::FilteredGameControllerState, + kick_decision::KickDecision, + motion_command::{MotionCommand, OrientationMode}, + obstacles::Obstacle, + parameters::{DribblingParameters, PathPlanningParameters}, + path_obstacles::PathObstacle, + planned_path::PathSegment, + rule_obstacles::RuleObstacle, + world_state::BallState, }; -use crate::behavior::walk_to_pose::WalkPathPlanner; +use crate::behavior::walk_to_pose::{hybrid_alignment, WalkPathPlanner}; + +#[derive(Deserialize, Serialize)] +pub struct DribblePathPlanner {} + +#[context] +pub struct CreationContext {} + +#[context] +pub struct CycleContext { + ball: RequiredInput, "ball_state?">, + kick_decisions: RequiredInput>, "kick_decisions?">, + ground_to_field: RequiredInput>, "ground_to_field?">, + obstacles: Input, "obstacles">, + rule_obstacles: Input, "rule_obstacles">, + filtered_game_controller_state: + Input, "filtered_game_controller_state?">, + + dribbling_parameters: Parameter, + path_planning_parameters: Parameter, + field_dimensions: Parameter, + + dribble_path_obstacles_output: AdditionalOutput, "dribble_path_obstacles">, + + last_motion_command: CyclerState, +} + +#[context] +#[derive(Default)] +pub struct MainOutputs { + pub dribble_path_plan: MainOutput)>>, +} + +impl DribblePathPlanner { + pub fn new(_context: CreationContext) -> Result { + Ok(Self {}) + } + + pub fn cycle(&mut self, mut context: CycleContext) -> Result { + let walk_path_planner = WalkPathPlanner::new( + context.field_dimensions, + context.obstacles, + context.path_planning_parameters, + context.last_motion_command, + ); + let best_kick_decision = match context.kick_decisions.first() { + Some(decision) => decision, + None => { + return Ok(MainOutputs { + dribble_path_plan: None.into(), + }) + } + }; + let best_kick_pose = best_kick_decision.kick_pose; + + let mut dribble_path_obstacles = None; + let mut dribble_path_obstacles_output = AdditionalOutput::new( + context.dribble_path_obstacles_output.is_subscribed() + || context.dribble_path_obstacles_output.is_subscribed(), + &mut dribble_path_obstacles, + ); + + let Some(dribble_path) = plan( + &walk_path_planner, + *context.ball, + best_kick_pose, + *context.ground_to_field, + context.obstacles, + context.rule_obstacles, + context.filtered_game_controller_state, + context.dribbling_parameters, + &mut dribble_path_obstacles_output, + ) else { + return Ok(MainOutputs { + dribble_path_plan: None.into(), + }); + }; + context + .dribble_path_obstacles_output + .fill_if_subscribed(|| dribble_path_obstacles.clone().unwrap_or_default()); + + let hybrid_orientation_mode = hybrid_alignment( + best_kick_pose, + context.dribbling_parameters.hybrid_align_distance, + context.dribbling_parameters.distance_to_be_aligned, + ); + let ball_position = &context.ball.ball_in_ground; + let orientation_mode = match hybrid_orientation_mode { + types::motion_command::OrientationMode::AlignWithPath + if ball_position.coords().norm() > 0.0 => + { + OrientationMode::Override(Point::origin().look_at(ball_position)) + } + orientation_mode => orientation_mode, + }; + + Ok(MainOutputs { + dribble_path_plan: Some((orientation_mode, dribble_path)).into(), + }) + } +} + +#[allow(clippy::too_many_arguments)] pub fn plan( walk_path_planner: &WalkPathPlanner, - world_state: &WorldState, + ball: BallState, + best_pose: Pose2, + ground_to_field: Isometry2, + obstacles: &[Obstacle], + rule_obstacles: &[RuleObstacle], + filtered_game_controller_state: Option<&FilteredGameControllerState>, dribbling_parameters: &DribblingParameters, path_obstacles_output: &mut AdditionalOutput>, ) -> Option> { - let kick_decisions = world_state.kick_decisions.as_ref()?; - let best_kick_decision = kick_decisions.first()?; - let ball = world_state.ball?; - let ground_to_field = world_state.robot.ground_to_field?; - - let ball_position_in_ground = ball.ball_in_ground; - let ball_position_in_field = ball.ball_in_field; - let best_pose = best_kick_decision.kick_pose; - let robot_to_ball = ball_position_in_ground.coords(); - let dribble_pose_to_ball = ball_position_in_ground - best_pose.position(); - + let robot_to_ball = ball.ball_in_ground.coords(); + let dribble_pose_to_ball = ball.ball_in_ground - best_pose.position(); let angle = robot_to_ball.angle(&dribble_pose_to_ball); - let should_avoid_ball = angle > dribbling_parameters.angle_to_approach_ball_from_threshold; - let ball_obstacle = should_avoid_ball.then_some(ball_position_in_ground); + let ball_obstacle = should_avoid_ball.then_some(ball.ball_in_ground); let ball_is_between_robot_and_own_goal = - ball_position_in_field.coords().x() - ground_to_field.translation().x() < 0.0; + ball.ball_in_field.coords().x() - ground_to_field.translation().x() < 0.0; let ball_obstacle_radius_factor = if ball_is_between_robot_and_own_goal { 1.0f32 } else { @@ -39,26 +152,18 @@ pub fn plan( / (PI - dribbling_parameters.angle_to_approach_ball_from_threshold) }; - let is_near_ball = matches!( - world_state.ball, - Some(ball) if ball.ball_in_ground.coords().norm() < dribbling_parameters.ignore_robot_when_near_ball_radius, - ); - let obstacles = if is_near_ball { - &[] - } else { - world_state.obstacles.as_slice() - }; + let ball_is_near = ball.ball_in_ground.coords().norm() + < dribbling_parameters.ignore_robot_when_near_ball_radius; + let hulks_is_kicking_team = + filtered_game_controller_state.is_some_and(|filtered_game_controller_state| { + filtered_game_controller_state.kicking_team == Team::Hulks + }); - let rule_obstacles = if matches!( - world_state.filtered_game_controller_state, - Some(FilteredGameControllerState { - kicking_team: Team::Hulks, - .. - }) - ) { + let obstacles = if ball_is_near { &[] } else { obstacles }; + let rule_obstacles = if hulks_is_kicking_team { &[] } else { - world_state.rule_obstacles.as_slice() + rule_obstacles }; Some(walk_path_planner.plan( diff --git a/crates/control/src/lib.rs b/crates/control/src/lib.rs index bdea72aac2..4a676c2fe4 100644 --- a/crates/control/src/lib.rs +++ b/crates/control/src/lib.rs @@ -21,6 +21,7 @@ pub mod led_status; pub mod localization; pub mod motion; pub mod obstacle_filter; +pub mod obstacle_receiver; pub mod odometry; pub mod orientation_filter; pub mod path_planner; @@ -36,6 +37,7 @@ pub mod sensor_data_receiver; pub mod sole_pressure_filter; pub mod sonar_filter; pub mod support_foot_estimation; +pub mod team_ball_receiver; pub mod time_to_reach_kick_position; pub mod whistle_filter; pub mod world_state_composer; diff --git a/crates/control/src/motion/stand_up_back.rs b/crates/control/src/motion/stand_up_back.rs index 326a81c9e8..163a14dff7 100644 --- a/crates/control/src/motion/stand_up_back.rs +++ b/crates/control/src/motion/stand_up_back.rs @@ -1,6 +1,6 @@ -use std::time::Duration; - use color_eyre::Result; +use serde::{Deserialize, Serialize}; + use context_attribute::context; use coordinate_systems::Robot; use filtering::low_pass_filter::LowPassFilter; @@ -8,12 +8,12 @@ use framework::MainOutput; use hardware::PathsInterface; use linear_algebra::Vector3; use motionfile::{MotionFile, MotionInterpolator}; -use serde::{Deserialize, Serialize}; use types::{ condition_input::ConditionInput, cycle_time::CycleTime, joints::Joints, motion_selection::{MotionSafeExits, MotionSelection, MotionType}, + stand_up::RemainingStandUpDuration, }; #[derive(Deserialize, Serialize)] @@ -39,13 +39,14 @@ pub struct CycleContext { Input, "sensor_data.inertial_measurement_unit.angular_velocity">, motion_safe_exits: CyclerState, + stand_up_back_estimated_remaining_duration: + CyclerState, } #[context] #[derive(Default)] pub struct MainOutputs { pub stand_up_back_positions: MainOutput>, - pub stand_up_back_estimated_remaining_duration: MainOutput>, } impl StandUpBack { @@ -62,19 +63,19 @@ impl StandUpBack { } pub fn cycle(&mut self, context: CycleContext) -> Result { - let stand_up_back_estimated_remaining_duration = - if let MotionType::StandUpBack = context.motion_selection.current_motion { + let estimated_remaining_duration = + if context.motion_selection.current_motion == MotionType::StandUpBack { let last_cycle_duration = context.cycle_time.last_cycle_duration; let condition_input = context.condition_input; - self.interpolator .advance_by(last_cycle_duration, condition_input); - Some(self.interpolator.estimated_remaining_duration()) + RemainingStandUpDuration::Running(self.interpolator.estimated_remaining_duration()) } else { self.interpolator.reset(); - None + RemainingStandUpDuration::NotRunning }; + context.motion_safe_exits[MotionType::StandUpBack] = self.interpolator.is_finished(); self.filtered_gyro.update(context.angular_velocity.inner); @@ -86,10 +87,10 @@ impl StandUpBack { positions.right_leg.ankle_pitch += context.leg_balancing_factor.y * gyro.y; positions.right_leg.ankle_roll += context.leg_balancing_factor.x * gyro.x; + *context.stand_up_back_estimated_remaining_duration = estimated_remaining_duration; + Ok(MainOutputs { stand_up_back_positions: positions.into(), - stand_up_back_estimated_remaining_duration: stand_up_back_estimated_remaining_duration - .into(), }) } } diff --git a/crates/control/src/motion/stand_up_front.rs b/crates/control/src/motion/stand_up_front.rs index 17d863d032..0dffc9899a 100644 --- a/crates/control/src/motion/stand_up_front.rs +++ b/crates/control/src/motion/stand_up_front.rs @@ -1,6 +1,6 @@ -use std::time::Duration; - use color_eyre::Result; +use serde::{Deserialize, Serialize}; + use context_attribute::context; use coordinate_systems::Robot; use filtering::low_pass_filter::LowPassFilter; @@ -8,12 +8,12 @@ use framework::MainOutput; use hardware::PathsInterface; use linear_algebra::Vector3; use motionfile::{MotionFile, MotionInterpolator}; -use serde::{Deserialize, Serialize}; use types::{ condition_input::ConditionInput, cycle_time::CycleTime, joints::Joints, motion_selection::{MotionSafeExits, MotionSelection, MotionType}, + stand_up::RemainingStandUpDuration, }; #[derive(Deserialize, Serialize)] @@ -39,13 +39,14 @@ pub struct CycleContext { Input, "sensor_data.inertial_measurement_unit.angular_velocity">, motion_safe_exits: CyclerState, + stand_up_front_estimated_remaining_duration: + CyclerState, } #[context] #[derive(Default)] pub struct MainOutputs { pub stand_up_front_positions: MainOutput>, - pub stand_up_front_estimated_remaining_duration: MainOutput>, } impl StandUpFront { @@ -62,18 +63,18 @@ impl StandUpFront { } pub fn cycle(&mut self, context: CycleContext) -> Result { - let stand_up_front_estimated_remaining_duration = - if let MotionType::StandUpFront = context.motion_selection.current_motion { + let estimated_remaining_duration = + if context.motion_selection.current_motion == MotionType::StandUpFront { let last_cycle_duration = context.cycle_time.last_cycle_duration; let condition_input = context.condition_input; self.interpolator .advance_by(last_cycle_duration, condition_input); - Some(self.interpolator.estimated_remaining_duration()) + RemainingStandUpDuration::Running(self.interpolator.estimated_remaining_duration()) } else { self.interpolator.reset(); - None + RemainingStandUpDuration::NotRunning }; context.motion_safe_exits[MotionType::StandUpFront] = self.interpolator.is_finished(); @@ -86,10 +87,10 @@ impl StandUpFront { positions.right_leg.ankle_pitch += context.leg_balancing_factor.y * gyro.y; positions.right_leg.ankle_roll += context.leg_balancing_factor.x * gyro.x; + *context.stand_up_front_estimated_remaining_duration = estimated_remaining_duration; + Ok(MainOutputs { stand_up_front_positions: positions.into(), - stand_up_front_estimated_remaining_duration: - stand_up_front_estimated_remaining_duration.into(), }) } } diff --git a/crates/control/src/motion/stand_up_sitting.rs b/crates/control/src/motion/stand_up_sitting.rs index 2d80ce04cd..4a0585b299 100644 --- a/crates/control/src/motion/stand_up_sitting.rs +++ b/crates/control/src/motion/stand_up_sitting.rs @@ -1,6 +1,6 @@ -use std::time::Duration; - use color_eyre::Result; +use serde::{Deserialize, Serialize}; + use context_attribute::context; use coordinate_systems::Robot; use filtering::low_pass_filter::LowPassFilter; @@ -8,12 +8,12 @@ use framework::MainOutput; use hardware::PathsInterface; use linear_algebra::Vector3; use motionfile::{MotionFile, MotionInterpolator}; -use serde::{Deserialize, Serialize}; use types::{ condition_input::ConditionInput, cycle_time::CycleTime, joints::Joints, motion_selection::{MotionSafeExits, MotionSelection, MotionType}, + stand_up::RemainingStandUpDuration, }; #[derive(Deserialize, Serialize)] @@ -40,13 +40,14 @@ pub struct CycleContext { Input, "sensor_data.inertial_measurement_unit.angular_velocity">, motion_safe_exits: CyclerState, + stand_up_sitting_estimated_remaining_duration: + CyclerState, } #[context] #[derive(Default)] pub struct MainOutputs { pub stand_up_sitting_positions: MainOutput>, - pub stand_up_sitting_estimated_remaining_duration: MainOutput>, } impl StandUpSitting { @@ -64,17 +65,17 @@ impl StandUpSitting { pub fn cycle(&mut self, context: CycleContext) -> Result { let estimated_remaining_duration = - if let MotionType::StandUpSitting = context.motion_selection.current_motion { + if context.motion_selection.current_motion == MotionType::StandUpSitting { let last_cycle_duration = context.cycle_time.last_cycle_duration; let condition_input = context.condition_input; self.interpolator .advance_by(last_cycle_duration, condition_input); - Some(self.interpolator.estimated_remaining_duration()) + RemainingStandUpDuration::Running(self.interpolator.estimated_remaining_duration()) } else { self.interpolator.reset(); - None + RemainingStandUpDuration::NotRunning }; context.motion_safe_exits[MotionType::StandUpSitting] = self.interpolator.is_finished(); @@ -87,9 +88,10 @@ impl StandUpSitting { positions.right_leg.ankle_pitch += context.leg_balancing_factor.y * gyro.y; positions.right_leg.ankle_roll += context.leg_balancing_factor.x * gyro.x; + *context.stand_up_sitting_estimated_remaining_duration = estimated_remaining_duration; + Ok(MainOutputs { stand_up_sitting_positions: positions.into(), - stand_up_sitting_estimated_remaining_duration: estimated_remaining_duration.into(), }) } } diff --git a/crates/control/src/obstacle_receiver.rs b/crates/control/src/obstacle_receiver.rs new file mode 100644 index 0000000000..e18d9231e0 --- /dev/null +++ b/crates/control/src/obstacle_receiver.rs @@ -0,0 +1,78 @@ +use color_eyre::eyre::Result; +use serde::{Deserialize, Serialize}; + +use context_attribute::context; +use coordinate_systems::{Field, Ground}; +use framework::{MainOutput, PerceptionInput}; +use linear_algebra::{Isometry2, Point2}; +use spl_network_messages::{GamePhase, HulkMessage, SubState}; +use types::{ + filtered_game_controller_state::FilteredGameControllerState, messages::IncomingMessage, +}; + +#[derive(Deserialize, Serialize)] +pub struct ObstacleReceiver {} + +#[context] +pub struct CreationContext {} + +#[context] +pub struct CycleContext { + filtered_game_controller_state: + Input, "filtered_game_controller_state?">, + ground_to_field: RequiredInput>, "ground_to_field?">, + network_message: PerceptionInput, "SplNetwork", "filtered_message?">, +} + +#[context] +#[derive(Default)] +pub struct MainOutputs { + pub network_robot_obstacles: MainOutput>>, +} + +impl ObstacleReceiver { + pub fn new(_context: CreationContext) -> Result { + Ok(Self {}) + } + + pub fn cycle(&mut self, context: CycleContext) -> Result { + let messages = context + .network_message + .persistent + .values() + .flat_map(|messages| messages.iter().filter_map(|message| (*message))) + .filter_map(|message| match message { + IncomingMessage::Spl(message) => Some(*message), + _ => None, + }); + let mut network_robot_obstacles = Vec::new(); + for message in messages.clone() { + let pose = match message { + HulkMessage::Striker(striker_message) => striker_message.pose, + HulkMessage::Loser(loser_message) => loser_message.pose, + HulkMessage::VisualReferee(_) => continue, + }; + let sender_position = context.ground_to_field.inverse() * pose.position(); + network_robot_obstacles.push(sender_position); + } + + // Ignore everything during penalty_* + if let Some(game_controller_state) = context.filtered_game_controller_state { + let in_penalty_shootout = matches!( + game_controller_state.game_phase, + GamePhase::PenaltyShootout { .. } + ); + let in_penalty_kick = game_controller_state.sub_state == Some(SubState::PenaltyKick); + + if in_penalty_shootout || in_penalty_kick { + return Ok(MainOutputs { + network_robot_obstacles: Vec::new().into(), + }); + } + } + + Ok(MainOutputs { + network_robot_obstacles: network_robot_obstacles.into(), + }) + } +} diff --git a/crates/control/src/role_assignment.rs b/crates/control/src/role_assignment.rs index cb5b5a74df..418c287592 100644 --- a/crates/control/src/role_assignment.rs +++ b/crates/control/src/role_assignment.rs @@ -1,19 +1,23 @@ use std::{ + collections::VecDeque, net::SocketAddr, time::{Duration, SystemTime}, }; -use color_eyre::{eyre::WrapErr, Result}; +use color_eyre::{ + eyre::{OptionExt, WrapErr}, + Result, +}; use serde::{Deserialize, Serialize}; use context_attribute::context; use coordinate_systems::{Field, Ground}; use framework::{AdditionalOutput, MainOutput, PerceptionInput}; use hardware::NetworkInterface; -use linear_algebra::{Isometry2, Point2, Vector}; +use linear_algebra::Isometry2; use spl_network_messages::{ - GameControllerReturnMessage, GamePhase, HulkMessage, Penalty, PlayerNumber, StrikerMessage, - SubState, Team, + GameControllerReturnMessage, GamePhase, HulkMessage, LoserMessage, Penalty, PlayerNumber, + StrikerMessage, SubState, Team, }; use types::{ ball_position::BallPosition, @@ -33,17 +37,18 @@ use crate::localization::generate_initial_pose; #[derive(Deserialize, Serialize)] pub struct RoleAssignment { - last_received_spl_striker_message: Option, + last_received_striker_message: Option, last_system_time_transmitted_game_controller_return_message: Option, - last_transmitted_spl_striker_message: Option, + last_transmitted_spl_message: Option, role: Role, - role_initialized: bool, - team_ball: Option>, last_time_player_was_penalized: Players>, } #[context] -pub struct CreationContext {} +pub struct CreationContext { + optional_roles: Parameter, "behavior.optional_roles">, + player_number: Parameter, +} #[context] pub struct CycleContext { @@ -56,12 +61,14 @@ pub struct CycleContext { cycle_time: Input, network_message: PerceptionInput, "SplNetwork", "filtered_message?">, game_controller_address: Input, "game_controller_address?">, - time_to_reach_kick_position: CyclerState, + time_to_reach_kick_position: Input, "time_to_reach_kick_position?">, + team_ball: Input>, "team_ball?">, field_dimensions: Parameter, forced_role: Parameter, "role_assignment.forced_role?">, keeper_replacementkeeper_switch_time: Parameter, + striker_trusts_team_ball: Parameter, initial_poses: Parameter, "localization.initial_poses">, optional_roles: Parameter, "behavior.optional_roles">, player_number: Parameter, @@ -76,29 +83,28 @@ pub struct CycleContext { #[context] #[derive(Default)] pub struct MainOutputs { - pub team_ball: MainOutput>>, - pub network_robot_obstacles: MainOutput>>, pub role: MainOutput, } impl RoleAssignment { - pub fn new(_context: CreationContext) -> Result { + pub fn new(context: CreationContext) -> Result { + #[allow(clippy::get_first)] + let role = match context.player_number { + PlayerNumber::One => Some(Role::Keeper), + PlayerNumber::Two => context.optional_roles.get(0).copied(), + PlayerNumber::Three => context.optional_roles.get(1).copied(), + PlayerNumber::Four => context.optional_roles.get(2).copied(), + PlayerNumber::Five => context.optional_roles.get(3).copied(), + PlayerNumber::Six => context.optional_roles.get(4).copied(), + PlayerNumber::Seven => Some(Role::Striker), + } + .unwrap_or(Role::Striker); Ok(Self { - last_received_spl_striker_message: None, + last_received_striker_message: None, last_system_time_transmitted_game_controller_return_message: None, - last_transmitted_spl_striker_message: None, - role: Role::Striker, - role_initialized: false, - team_ball: None, - last_time_player_was_penalized: Players { - one: None, - two: None, - three: None, - four: None, - five: None, - six: None, - seven: None, - }, + last_transmitted_spl_message: None, + role, + last_time_player_was_penalized: Players::new(None), }) } @@ -108,35 +114,104 @@ impl RoleAssignment { ) -> Result { let cycle_start_time = context.cycle_time.start_time; let primary_state = *context.primary_state; - let mut new_role = self.role; - - let ground_to_field = - context - .ground_to_field - .copied() - .unwrap_or_else(|| match context.primary_state { - PrimaryState::Initial => generate_initial_pose( - &context.initial_poses[*context.player_number], - context.field_dimensions, - ) - .as_transform(), - _ => Default::default(), + + self.try_sending_game_controller_return_message(&context)?; + + if let Some(game_controller_state) = context.filtered_game_controller_state { + for player in self + .last_time_player_was_penalized + .clone() + .iter() + .map(|(playernumber, ..)| playernumber) + { + if game_controller_state.penalties[player].is_some() { + self.last_time_player_was_penalized[player] = Some(cycle_start_time); + } + } + } + + let role_from_state_machine = + self.role_from_state_machine(&context, cycle_start_time, self.role); + + let mut new_role = [ + context.forced_role.copied(), + self.role_for_ready_and_set(&context), + role_for_penalty_shootout(context.filtered_game_controller_state), + keep_current_role_in_penalty_kick(context.filtered_game_controller_state, self.role), + keep_current_role_if_not_in_playing(primary_state, self.role), + Some(role_from_state_machine), + ] + .iter() + .find_map(|maybe_role| *maybe_role) + .expect("at least role_from_state_machine should be Some"); + + if self.role == Role::ReplacementKeeper && new_role != Role::Striker { + let lowest_player_number_without_penalty = self + .last_time_player_was_penalized + .iter() + .find_map(|(player_number, penalized_time)| { + match penalized_time { + None => true, + Some(penalized_time) => { + let since_last_penalized = cycle_start_time + .duration_since(*penalized_time) + .expect("time ran backwards"); + since_last_penalized >= *context.keeper_replacementkeeper_switch_time + } + } + .then_some(player_number) }); + if Some(*context.player_number) == lowest_player_number_without_penalty { + new_role = Role::ReplacementKeeper; + } + } + + context + .last_time_player_was_penalized + .fill_if_subscribed(|| self.last_time_player_was_penalized); + if is_allowed_to_send_messages(&context) { + match (self.role, new_role) { + (Role::Striker, Role::Striker) => { + if self.is_striker_beacon_cooldown_elapsed(&context) { + self.try_sending_striker_message(&context)?; + } + } + (_other_role, Role::Striker) => { + self.try_sending_striker_message(&context)?; + } + + (Role::Striker, Role::Loser) => { + self.try_sending_loser_message(&context)?; + } + _ => {} + } + } + + self.role = new_role; + + Ok(MainOutputs { + role: self.role.into(), + }) + } - if !self.role_initialized - || primary_state == PrimaryState::Ready - || primary_state == PrimaryState::Set + fn role_for_ready_and_set( + &mut self, + context: &CycleContext<'_, impl NetworkInterface>, + ) -> Option { + if *context.primary_state == PrimaryState::Ready + || *context.primary_state == PrimaryState::Set { #[allow(clippy::get_first)] let mut player_roles = Players { - one: Role::Keeper, - two: context.optional_roles.get(0).copied().unwrap_or_default(), - three: context.optional_roles.get(1).copied().unwrap_or_default(), - four: context.optional_roles.get(2).copied().unwrap_or_default(), - five: context.optional_roles.get(3).copied().unwrap_or_default(), - six: context.optional_roles.get(4).copied().unwrap_or_default(), - seven: Role::Striker, - }; + one: Some(Role::Keeper), + two: context.optional_roles.get(0).copied(), + three: context.optional_roles.get(1).copied(), + four: context.optional_roles.get(2).copied(), + five: context.optional_roles.get(3).copied(), + six: context.optional_roles.get(4).copied(), + seven: Some(Role::Striker), + } + .map(|role| role.unwrap_or(Role::Striker)); if let Some(game_controller_state) = context.filtered_game_controller_state { if let Some(striker) = [ @@ -151,510 +226,429 @@ impl RoleAssignment { player_roles[striker] = Role::Striker; } } - new_role = player_roles[*context.player_number]; - self.role_initialized = true; - self.last_received_spl_striker_message = Some(cycle_start_time); - self.team_ball = None; - } + self.last_received_striker_message = None; - let send_game_controller_return_message = self - .last_system_time_transmitted_game_controller_return_message - .is_none() - || cycle_start_time.duration_since( - self.last_system_time_transmitted_game_controller_return_message - .unwrap(), - )? > context.spl_network.game_controller_return_message_interval; + return Some(player_roles[*context.player_number]); + } - let mut send_spl_striker_message = self.last_transmitted_spl_striker_message.is_none() - || cycle_start_time - .duration_since(self.last_transmitted_spl_striker_message.unwrap())? - > context.spl_network.spl_striker_message_send_interval; + None + } - let spl_striker_message_timeout = match self.last_received_spl_striker_message { + fn role_from_state_machine( + &mut self, + context: &CycleContext<'_, impl NetworkInterface>, + cycle_start_time: SystemTime, + current_role: Role, + ) -> Role { + let spl_striker_message_timeout = match self.last_received_striker_message { None => false, Some(last_received_spl_striker_message) => { - cycle_start_time.duration_since(last_received_spl_striker_message)? + if cycle_start_time + .duration_since(last_received_spl_striker_message) + .expect("time ran backwards") > context.spl_network.spl_striker_message_receive_timeout - } - }; - - let silence_interval_has_passed = match self.last_transmitted_spl_striker_message { - Some(last_transmitted_spl_striker_message) => { - cycle_start_time.duration_since(last_transmitted_spl_striker_message)? - > context.spl_network.silence_interval_between_messages - } - None => true, - }; - - if send_game_controller_return_message { - self.last_system_time_transmitted_game_controller_return_message = - Some(cycle_start_time); - if let Some(address) = context.game_controller_address { - context - .hardware - .write_to_network(OutgoingMessage::GameController( - *address, - GameControllerReturnMessage { - player_number: *context.player_number, - fallen: matches!(context.fall_state, FallState::Fallen { .. }), - pose: ground_to_field.as_pose(), - ball: seen_ball_to_game_controller_ball_position( - context.ball_position, - cycle_start_time, - ), - }, - )) - .wrap_err("failed to write GameControllerReturnMessage to hardware")?; - } - } - - let mut team_ball = self.team_ball; - - let is_in_penalty_kick = matches!( - context.filtered_game_controller_state, - Some(FilteredGameControllerState { - sub_state: Some(SubState::PenaltyKick), - .. - }) - ); - if spl_striker_message_timeout && !is_in_penalty_kick { - match new_role { - Role::Keeper => { - team_ball = None; - } - Role::ReplacementKeeper => { - team_ball = None; - } - Role::Striker => { - send_spl_striker_message = true; - team_ball = None; - new_role = Role::Loser; - } - Role::Loser if *context.player_number == PlayerNumber::One => { - new_role = Role::Keeper; - } - _ => { - send_spl_striker_message = false; - team_ball = None; - new_role = Role::Searcher + { + self.last_received_striker_message = None; + true + } else { + false } } - } + }; + let striker_message_timeout_event = spl_striker_message_timeout + .then_some(Event::Loser) + .into_iter(); - let mut network_robot_obstacles = vec![]; - let mut spl_messages = context + let messages: Vec<_> = context .network_message .persistent - .into_values() + .values() .flatten() .filter_map(|message| match message { - Some(IncomingMessage::Spl(HulkMessage::Striker(message))) => Some(message), + Some(IncomingMessage::Spl(HulkMessage::Striker(StrikerMessage { + player_number, + time_to_reach_kick_position, + .. + }))) => Some(Event::Striker(StrikerEvent { + player_number: *player_number, + time_to_reach_kick_position: *time_to_reach_kick_position, + })), + Some(IncomingMessage::Spl(HulkMessage::Loser(..))) => Some(Event::Loser), _ => None, }) - .peekable(); - if spl_messages.peek().is_none() { - (new_role, send_spl_striker_message, team_ball) = process_role_state_machine( + .collect(); + + let events = striker_message_timeout_event + .chain(messages) + // Update the state machine at least once + .chain([Event::None]); + + let mut new_role = current_role; + for event in events { + if let Event::Striker(_) = event { + self.last_received_striker_message = Some(cycle_start_time) + } + + new_role = update_role_state_machine( new_role, - ground_to_field, - context.ball_position, - primary_state, - None, - Some(*context.time_to_reach_kick_position), - send_spl_striker_message, - team_ball, + context.ball_position.is_some(), + event, + context.time_to_reach_kick_position.copied(), + context.team_ball.copied(), cycle_start_time, context.filtered_game_controller_state, *context.player_number, - context.spl_network.striker_trusts_team_ball, + *context.striker_trusts_team_ball, context.optional_roles, ); - } else { - for spl_message in spl_messages { - self.last_received_spl_striker_message = Some(cycle_start_time); - let sender_position = ground_to_field.inverse() * spl_message.pose.position(); - if spl_message.player_number != *context.player_number { - network_robot_obstacles.push(sender_position); - } - (new_role, send_spl_striker_message, team_ball) = process_role_state_machine( - new_role, - ground_to_field, - context.ball_position, - primary_state, - Some(spl_message), - Some(*context.time_to_reach_kick_position), - send_spl_striker_message, - team_ball, - cycle_start_time, - context.filtered_game_controller_state, - *context.player_number, - context.spl_network.striker_trusts_team_ball, - context.optional_roles, - ); - } } - if self.role == Role::ReplacementKeeper { - let mut other_players_with_lower_number = self - .last_time_player_was_penalized - .iter() - .filter(|(player_number, _)| player_number < context.player_number); - let is_lowest_number_without = - other_players_with_lower_number.all(|(_, penalized_time)| { - penalized_time - .map(|system_time| { - let since_last_penalized = cycle_start_time - .duration_since(system_time) - .expect("penalty time to be in the past"); - since_last_penalized < *context.keeper_replacementkeeper_switch_time - }) - .unwrap_or(false) - }); - if !send_spl_striker_message && is_lowest_number_without { - new_role = Role::ReplacementKeeper; - } + + new_role + } + + fn is_return_message_cooldown_elapsed( + &self, + context: &CycleContext, + ) -> bool { + is_cooldown_elapsed( + context.cycle_time.start_time, + self.last_system_time_transmitted_game_controller_return_message, + context.spl_network.game_controller_return_message_interval, + ) + } + + fn is_striker_beacon_cooldown_elapsed( + &self, + context: &CycleContext, + ) -> bool { + is_cooldown_elapsed( + context.cycle_time.start_time, + self.last_transmitted_spl_message, + context.spl_network.spl_striker_message_send_interval, + ) + } + + fn is_striker_silence_period_elapsed( + &self, + context: &CycleContext, + ) -> bool { + is_cooldown_elapsed( + context.cycle_time.start_time, + self.last_transmitted_spl_message, + context.spl_network.silence_interval_between_messages, + ) + } + + fn try_sending_game_controller_return_message( + &mut self, + context: &CycleContext, + ) -> Result<()> { + if !self.is_return_message_cooldown_elapsed(context) { + return Ok(()); } + let Some(address) = context.game_controller_address else { + return Ok(()); + }; + let ground_to_field = ground_to_field_or_initial_pose(context); + + self.last_system_time_transmitted_game_controller_return_message = + Some(context.cycle_time.start_time); context - .last_time_player_was_penalized - .fill_if_subscribed(|| self.last_time_player_was_penalized); + .hardware + .write_to_network(OutgoingMessage::GameController( + *address, + GameControllerReturnMessage { + player_number: *context.player_number, + fallen: matches!(context.fall_state, FallState::Fallen { .. }), + pose: ground_to_field.as_pose(), + ball: seen_ball_to_game_controller_ball_position( + context.ball_position, + context.cycle_time.start_time, + ), + }, + )) + .wrap_err("failed to write GameControllerReturnMessage to hardware") + } - if send_spl_striker_message - && primary_state == PrimaryState::Playing - && silence_interval_has_passed - { - self.last_transmitted_spl_striker_message = Some(cycle_start_time); - self.last_received_spl_striker_message = Some(cycle_start_time); - if let Some(game_controller_state) = context.filtered_game_controller_state { - if game_controller_state.remaining_number_of_messages - > context - .spl_network - .remaining_amount_of_messages_to_stop_sending - { - let ball_position = if context.ball_position.is_none() && team_ball.is_some() { - team_ball_to_network_ball_position(team_ball, cycle_start_time) - } else { - seen_ball_to_hulks_network_ball_position( - context.ball_position, - ground_to_field, - cycle_start_time, - ) - }; - context.hardware.write_to_network(OutgoingMessage::Spl( - HulkMessage::Striker(StrikerMessage { - player_number: *context.player_number, - pose: ground_to_field.as_pose(), - ball_position, - time_to_reach_kick_position: Some(*context.time_to_reach_kick_position), - }), - ))?; - } - } + fn try_sending_striker_message( + &mut self, + context: &CycleContext, + ) -> Result<()> { + if !self.is_striker_silence_period_elapsed(context) { + return Ok(()); } - - if let Some(forced_role) = context.forced_role { - self.role = *forced_role; - } else { - self.role = new_role; + if !is_enough_message_budget_left(context) { + return Ok(()); } - self.team_ball = team_ball; - if let Some(game_controller_state) = context.filtered_game_controller_state { - for player in self - .last_time_player_was_penalized - .clone() - .iter() - .map(|(playernumber, ..)| playernumber) - { - if game_controller_state.penalties[player].is_some() { - self.last_time_player_was_penalized[player] = Some(cycle_start_time); - } - } + self.last_transmitted_spl_message = Some(context.cycle_time.start_time); + self.last_received_striker_message = None; + + let ground_to_field = ground_to_field_or_initial_pose(context); + let pose = ground_to_field.as_pose(); + let team_network_ball = context.team_ball.map(|team_ball| { + team_ball_to_network_ball_position(*team_ball, context.cycle_time.start_time) + }); + let own_network_ball = context.ball_position.map(|seen_ball| { + own_ball_to_hulks_network_ball_position( + *seen_ball, + ground_to_field, + context.cycle_time.start_time, + ) + }); + let ball_position = own_network_ball + .or(team_network_ball) + .ok_or_eyre("we are striker without a ball, this should never happen")?; + + context + .hardware + .write_to_network(OutgoingMessage::Spl(HulkMessage::Striker(StrikerMessage { + player_number: *context.player_number, + pose, + ball_position, + time_to_reach_kick_position: *context.time_to_reach_kick_position.unwrap(), + }))) + .wrap_err("failed to write StrikerMessage to hardware") + } + + fn try_sending_loser_message( + &mut self, + context: &CycleContext, + ) -> Result<()> { + if !is_enough_message_budget_left(context) { + return Ok(()); } - Ok(MainOutputs { - role: self.role.into(), - team_ball: self.team_ball.into(), - network_robot_obstacles: network_robot_obstacles.into(), + self.last_transmitted_spl_message = Some(context.cycle_time.start_time); + self.last_received_striker_message = None; + + let ground_to_field = ground_to_field_or_initial_pose(context); + context + .hardware + .write_to_network(OutgoingMessage::Spl(HulkMessage::Loser(LoserMessage { + player_number: *context.player_number, + pose: ground_to_field.as_pose(), + }))) + .wrap_err("failed to write LoserMessage to hardware") + } +} + +fn ground_to_field_or_initial_pose( + context: &CycleContext<'_, impl NetworkInterface>, +) -> Isometry2 { + context + .ground_to_field + .copied() + .unwrap_or_else(|| match context.primary_state { + PrimaryState::Initial => generate_initial_pose( + &context.initial_poses[*context.player_number], + context.field_dimensions, + ) + .as_transform(), + _ => Default::default(), }) +} + +fn is_allowed_to_send_messages(context: &CycleContext<'_, impl NetworkInterface>) -> bool { + let is_playing = *context.primary_state == PrimaryState::Playing; + let is_penalty_kick = + context + .filtered_game_controller_state + .is_some_and(|game_controller_state| { + matches!( + game_controller_state.game_phase, + GamePhase::PenaltyShootout { .. } + ) || matches!(game_controller_state.sub_state, Some(SubState::PenaltyKick)) + }); + + is_playing && !is_penalty_kick +} + +fn is_cooldown_elapsed(now: SystemTime, last: Option, cooldown: Duration) -> bool { + match last { + None => true, + Some(last_time) => now.duration_since(last_time).expect("time ran backwards") > cooldown, } } +fn is_enough_message_budget_left(context: &CycleContext) -> bool { + context + .filtered_game_controller_state + .is_some_and(|game_controller_state| { + game_controller_state.remaining_number_of_messages + > context + .spl_network + .remaining_amount_of_messages_to_stop_sending + }) +} + +#[derive(Clone, Copy, Debug)] +enum Event { + None, + Striker(StrikerEvent), + Loser, +} + +#[derive(Clone, Copy, Debug)] +struct StrikerEvent { + player_number: PlayerNumber, + time_to_reach_kick_position: Duration, +} + #[allow(clippy::too_many_arguments)] -fn process_role_state_machine( +fn update_role_state_machine( current_role: Role, - current_pose: Isometry2, - detected_own_ball: Option<&BallPosition>, - primary_state: PrimaryState, - incoming_message: Option<&StrikerMessage>, + detected_own_ball: bool, + event: Event, time_to_reach_kick_position: Option, - send_spl_striker_message: bool, team_ball: Option>, cycle_start_time: SystemTime, filtered_game_controller_state: Option<&FilteredGameControllerState>, player_number: PlayerNumber, striker_trusts_team_ball: Duration, optional_roles: &[Role], -) -> (Role, bool, Option>) { - if let Some(game_controller_state) = filtered_game_controller_state { - match game_controller_state.game_phase { - GamePhase::PenaltyShootout { - kicking_team: Team::Hulks, - } => return (Role::Striker, false, None), - GamePhase::PenaltyShootout { - kicking_team: Team::Opponent, - } => return (Role::Keeper, false, None), - _ => {} - }; - if let Some(SubState::PenaltyKick) = game_controller_state.sub_state { - return (current_role, false, None); - } - } - - if primary_state != PrimaryState::Playing { - match detected_own_ball { - None => return (current_role, false, team_ball), - Some(..) => { - return ( - current_role, - false, - team_ball_from_seen_ball(detected_own_ball, current_pose, cycle_start_time), - ) - } - } - } +) -> Role { + let striker_trusts_team_ball = |team_ball: BallPosition| { + cycle_start_time + .duration_since(team_ball.last_seen) + .expect("time ran backwards") + <= striker_trusts_team_ball + }; - match (current_role, detected_own_ball, incoming_message) { - //Striker maybe lost Ball - (Role::Striker, None, None) => match team_ball { - None => (Role::Loser, true, None), - Some(team_ball) => { - if cycle_start_time - .duration_since(team_ball.last_seen) - .unwrap() - > striker_trusts_team_ball - { - (Role::Loser, true, None) - } else { - (Role::Striker, send_spl_striker_message, Some(team_ball)) - } + match (current_role, detected_own_ball, event) { + // Striker lost Ball + (Role::Striker, false, Event::None | Event::Loser) => match team_ball { + Some(team_ball) if striker_trusts_team_ball(team_ball) => Role::Striker, + _ => match player_number{ + PlayerNumber::One => Role::Keeper, + _ => Role::Loser, } }, - // Striker maybe lost Ball but got a message (edge-case) - (Role::Striker, None, Some(spl_message)) => match &spl_message.ball_position { - None => { - // another Striker became Loser - match team_ball { - None => (Role::Loser, true, None), - Some(team_ball) => { - if cycle_start_time - .duration_since(team_ball.last_seen) - .unwrap() - > striker_trusts_team_ball - { - (Role::Loser, true, None) - } else { - (Role::Striker, send_spl_striker_message, Some(team_ball)) - } - } - } - } - _ => decide_if_claiming_striker_or_other_role( - spl_message, - time_to_reach_kick_position, - player_number, - cycle_start_time, - filtered_game_controller_state, - optional_roles, - ), - }, + (_other_role, _, Event::Striker(striker_event)) => claim_striker_or_other_role( + striker_event, + time_to_reach_kick_position, + player_number, + filtered_game_controller_state, + optional_roles, + ), - //Striker remains Striker, sends message after timeout - (Role::Striker, Some(..), None) => (Role::Striker, send_spl_striker_message, team_ball), - - // Striker got a message (either another Player claims Stiker role or Edge-case of a second Striker) - (Role::Striker, Some(..), Some(spl_message)) => match &spl_message.ball_position { - None => { - // another Striker became Loser, so we claim striker since we see a ball - ( - Role::Striker, - true, - team_ball_from_seen_ball(detected_own_ball, current_pose, cycle_start_time), - ) - } - _ => decide_if_claiming_striker_or_other_role( - spl_message, - time_to_reach_kick_position, - player_number, - cycle_start_time, - filtered_game_controller_state, - optional_roles, - ), - }, + // Striker remains Striker + (Role::Striker, true, Event::None) => Role::Striker, - //Loser remains Loser - (Role::Loser, None, None) => (Role::Loser, false, team_ball), + // Edge-case, another Striker became Loser, so we claim striker since we see a ball + (Role::Striker, true, Event::Loser) => Role::Striker, - (Role::Loser, None, Some(spl_message)) => match &spl_message.ball_position { - None => (Role::Loser, false, None), //edge-case, a striker (which should not exist) lost the ball - _ => decide_if_claiming_striker_or_other_role( - spl_message, - time_to_reach_kick_position, - player_number, - cycle_start_time, - filtered_game_controller_state, - optional_roles, - ), - }, + // Loser remains Loser + (Role::Loser, false, Event::None) => Role::Loser, + (Role::Loser, false, Event::Loser) => Role::Loser, - //Loser found ball and becomes Striker - (Role::Loser, Some(..), None) => ( - Role::Striker, - true, - team_ball_from_seen_ball(detected_own_ball, current_pose, cycle_start_time), - ), - - // Edge-case, Loser found Ball at the same time as receiving a message - (Role::Loser, Some(..), Some(spl_message)) => match &spl_message.ball_position { - None => { - // another Striker became Loser, so we claim striker since we see a ball - ( - Role::Striker, - true, - team_ball_from_seen_ball(detected_own_ball, current_pose, cycle_start_time), - ) - } - _ => decide_if_claiming_striker_or_other_role( - spl_message, - time_to_reach_kick_position, - player_number, - cycle_start_time, - filtered_game_controller_state, - optional_roles, - ), - }, + // Loser found ball and becomes Striker + (Role::Loser, true, Event::None) => Role::Striker, - //Searcher remains Searcher - (Role::Searcher, None, None) => (Role::Searcher, false, team_ball), + // Edge-case, Loser found Ball at the same time as receiving a loser message + (Role::Loser, true, Event::Loser) => Role::Striker, - (Role::Searcher, None, Some(spl_message)) => match &spl_message.ball_position { - None => (Role::Searcher, false, team_ball), //edge-case, a striker (which should not exist) lost the ball - _ => decide_if_claiming_striker_or_other_role( - spl_message, - time_to_reach_kick_position, - player_number, - cycle_start_time, - filtered_game_controller_state, - optional_roles, - ), + // Searcher remains Searcher + (Role::Searcher, false, Event::None) | + // Edge-case, a striker (which should not exist) lost the ball + (Role::Searcher, false, Event::Loser) => { + pick_keeper_or_searcher(player_number, filtered_game_controller_state) }, - //Searcher found ball and becomes Striker - (Role::Searcher, Some(..), None) => ( - Role::Striker, - true, - team_ball_from_seen_ball(detected_own_ball, current_pose, cycle_start_time), - ), + // Searcher found ball and becomes Striker + (Role::Searcher, true, Event::None) => Role::Striker, - // TODO: Searcher found Ball at the same time as receiving a message - (Role::Searcher, Some(..), Some(spl_message)) => match &spl_message.ball_position { - None => ( - Role::Striker, - true, - team_ball_from_seen_ball(detected_own_ball, current_pose, cycle_start_time), - ), - _ => decide_if_claiming_striker_or_other_role( - spl_message, - time_to_reach_kick_position, - player_number, - cycle_start_time, - filtered_game_controller_state, - optional_roles, - ), - }, + // Searcher found Ball at the same time as receiving a message + (Role::Searcher, true, Event::Loser) => Role::Striker, - // remain in other_role - (other_role, None, None) => (other_role, false, team_ball), + // Remain in other_role + (other_role, false, Event::None) => other_role, // Either someone found or lost a ball. if found: do I want to claim striker ? - (other_role, None, Some(spl_message)) => match &spl_message.ball_position { - None => { - if other_role != Role::Keeper && other_role != Role::ReplacementKeeper { - (Role::Searcher, false, None) - } else { - (other_role, false, None) - } + (other_role, false, Event::Loser) => { + if other_role == Role::Keeper || other_role == Role::ReplacementKeeper { + other_role + } else { + Role::Searcher } - _ => decide_if_claiming_striker_or_other_role( - spl_message, - time_to_reach_kick_position, - player_number, - cycle_start_time, - filtered_game_controller_state, - optional_roles, - ), - }, + } - // Claim Striker if team-ball position is None - (other_role, Some(..), None) => match team_ball { - None => ( - Role::Striker, - true, - team_ball_from_seen_ball(detected_own_ball, current_pose, cycle_start_time), - ), - Some(..) => ( - other_role, - false, - team_ball_from_seen_ball(detected_own_ball, current_pose, cycle_start_time), - ), + // Claim Striker if team-ball is None + (other_role, true, Event::None) => match team_ball { + None => Role::Striker, + Some(..) => other_role, }, - // if message is Ball-Lost => Striker, claim Striker ? design-decision: which ball to trust ? - (_other_role, Some(..), Some(spl_message)) => match &spl_message.ball_position { - None => ( - Role::Striker, - true, - team_ball_from_seen_ball(detected_own_ball, current_pose, cycle_start_time), - ), - _ => decide_if_claiming_striker_or_other_role( - spl_message, - time_to_reach_kick_position, - player_number, - cycle_start_time, - filtered_game_controller_state, - optional_roles, - ), - }, + // Striker lost ball but we see one, claim striker + (_other_role, true, Event::Loser) => Role::Striker, } } -fn decide_if_claiming_striker_or_other_role( - spl_message: &StrikerMessage, +fn keep_current_role_if_not_in_playing( + primary_state: PrimaryState, + current_role: Role, +) -> Option { + if primary_state != PrimaryState::Playing { + return Some(current_role); + } + None +} + +fn role_for_penalty_shootout( + filtered_game_controller_state: Option<&FilteredGameControllerState>, +) -> Option { + if let Some(game_controller_state) = filtered_game_controller_state { + if let GamePhase::PenaltyShootout { kicking_team } = game_controller_state.game_phase { + return Some(match kicking_team { + Team::Hulks => Role::Striker, + Team::Opponent => Role::Keeper, + }); + }; + } + None +} + +fn keep_current_role_in_penalty_kick( + filtered_game_controller_state: Option<&FilteredGameControllerState>, + current_role: Role, +) -> Option { + if let Some(game_controller_state) = filtered_game_controller_state { + if let Some(SubState::PenaltyKick) = game_controller_state.sub_state { + return Some(current_role); + } + } + None +} + +fn claim_striker_or_other_role( + striker_event: StrikerEvent, time_to_reach_kick_position: Option, player_number: PlayerNumber, - cycle_start_time: SystemTime, filtered_game_controller_state: Option<&FilteredGameControllerState>, optional_roles: &[Role], -) -> (Role, bool, Option>) { - if time_to_reach_kick_position < spl_message.time_to_reach_kick_position - && time_to_reach_kick_position.is_some_and(|duration| duration < Duration::from_secs(1200)) - { - ( - Role::Striker, - true, - team_ball_from_spl_message(cycle_start_time, spl_message), - ) - } else { - ( - generate_role( - player_number, - filtered_game_controller_state, - spl_message.player_number, - optional_roles, - ), - false, - team_ball_from_spl_message(cycle_start_time, spl_message), - ) +) -> Role { + let shorter_time_to_reach = time_to_reach_kick_position + .is_some_and(|duration| duration < striker_event.time_to_reach_kick_position); + let time_to_reach_viable = + time_to_reach_kick_position.is_some_and(|duration| duration < Duration::from_secs(1200)); + + if shorter_time_to_reach && time_to_reach_viable { + return Role::Striker; } + + let Some(filtered_game_controller_state) = filtered_game_controller_state else { + // This case only happens if we don't have a game controller state + return Role::Striker; + }; + + pick_role_with_penalties( + player_number, + &filtered_game_controller_state.penalties, + striker_event.player_number, + optional_roles, + ) } fn seen_ball_to_game_controller_ball_position( @@ -667,70 +661,26 @@ fn seen_ball_to_game_controller_ball_position( }) } -fn seen_ball_to_hulks_network_ball_position( - ball: Option<&BallPosition>, +fn own_ball_to_hulks_network_ball_position( + ball: BallPosition, ground_to_field: Isometry2, cycle_start_time: SystemTime, -) -> Option> { - ball.map(|ball| spl_network_messages::BallPosition { +) -> spl_network_messages::BallPosition { + spl_network_messages::BallPosition { age: cycle_start_time.duration_since(ball.last_seen).unwrap(), position: ground_to_field * ball.position, - }) + } } fn team_ball_to_network_ball_position( - team_ball: Option>, + team_ball: BallPosition, cycle_start_time: SystemTime, -) -> Option> { - team_ball.map(|team_ball| spl_network_messages::BallPosition { +) -> spl_network_messages::BallPosition { + spl_network_messages::BallPosition { age: cycle_start_time .duration_since(team_ball.last_seen) .unwrap(), position: team_ball.position, - }) -} - -fn team_ball_from_spl_message( - cycle_start_time: SystemTime, - spl_message: &StrikerMessage, -) -> Option> { - spl_message - .ball_position - .as_ref() - .map(|ball_position| BallPosition { - position: ball_position.position, - velocity: Vector::zeros(), - last_seen: cycle_start_time - ball_position.age, - }) -} - -fn team_ball_from_seen_ball( - ball: Option<&BallPosition>, - ground_to_field: Isometry2, - cycle_start_time: SystemTime, -) -> Option> { - ball.as_ref().map(|ball| BallPosition { - position: (ground_to_field * ball.position), - velocity: Vector::zeros(), - last_seen: cycle_start_time, - }) -} - -fn generate_role( - own_player_number: PlayerNumber, - game_controller_state: Option<&FilteredGameControllerState>, - striker_player_number: PlayerNumber, - optional_roles: &[Role], -) -> Role { - if let Some(state) = game_controller_state { - pick_role_with_penalties( - own_player_number, - &state.penalties, - striker_player_number, - optional_roles, - ) - } else { - Role::Striker // This case only happens if we don't have a game controller state } } @@ -740,85 +690,115 @@ fn pick_role_with_penalties( striker_player_number: PlayerNumber, optional_roles: &[Role], ) -> Role { - let mut role_assignment: Players> = Players { - one: None, - two: None, - three: None, - four: None, - five: None, - six: None, - seven: None, - }; - - role_assignment[striker_player_number] = Some(Role::Striker); - let mut unassigned_robots = 6; - - unassigned_robots -= penalties + let mut unassigned_players: VecDeque<_> = penalties .iter() - .filter(|(_player, &penalty)| penalty.is_some()) - .count(); + .filter_map(|(player_number, penalty)| { + (player_number != striker_player_number && penalty.is_none()).then_some(player_number) + }) + .collect(); - if unassigned_robots > 0 { - unassigned_robots = - assign_keeper_or_replacement_keeper(unassigned_robots, penalties, &mut role_assignment); + let mut role_assignment: Players> = Players::new(None); + role_assignment[striker_player_number] = Some(Role::Striker); + if let Some(keeper) = unassigned_players.pop_front() { + role_assignment[keeper] = Some(match keeper { + PlayerNumber::One => Role::Keeper, + _ => Role::ReplacementKeeper, + }) } - for &optional_role in optional_roles.iter().take(unassigned_robots) { - if needs_assignment(PlayerNumber::Two, penalties, &role_assignment) { - role_assignment[PlayerNumber::Two] = Some(optional_role); - } else if needs_assignment(PlayerNumber::Three, penalties, &role_assignment) { - role_assignment[PlayerNumber::Three] = Some(optional_role); - } else if needs_assignment(PlayerNumber::Four, penalties, &role_assignment) { - role_assignment[PlayerNumber::Four] = Some(optional_role); - } else if needs_assignment(PlayerNumber::Five, penalties, &role_assignment) { - role_assignment[PlayerNumber::Five] = Some(optional_role); - } else if needs_assignment(PlayerNumber::Six, penalties, &role_assignment) { - role_assignment[PlayerNumber::Six] = Some(optional_role); - } else if needs_assignment(PlayerNumber::Seven, penalties, &role_assignment) { - role_assignment[PlayerNumber::Seven] = Some(optional_role); - } + for (player_number, &optional_role) in unassigned_players.into_iter().zip(optional_roles) { + role_assignment[player_number] = Some(optional_role) } - role_assignment[own_player_number].unwrap_or_default() + role_assignment[own_player_number].unwrap_or(Role::Striker) } -fn needs_assignment( - player_number: PlayerNumber, - penalties: &Players>, - role_assignment: &Players>, -) -> bool { - role_assignment[player_number].is_none() && penalties[player_number].is_none() -} +fn pick_keeper_or_searcher( + own_player_number: PlayerNumber, + filtered_game_controller_state: Option<&FilteredGameControllerState>, +) -> Role { + let Some(filtered_game_controller_state) = filtered_game_controller_state else { + // This case only happens if we don't have a game controller state + return Role::Searcher; + }; -fn assign_keeper_or_replacement_keeper( - unassigned_robots: usize, - penalties: &Players>, - role_assignment: &mut Players>, -) -> usize { - if needs_assignment(PlayerNumber::One, penalties, role_assignment) { - role_assignment[PlayerNumber::One] = Some(Role::Keeper); - return unassigned_robots - 1; - } + let mut unassigned_players: VecDeque<_> = filtered_game_controller_state + .penalties + .iter() + .filter_map(|(player_number, penalty)| penalty.is_none().then_some(player_number)) + .collect(); - if needs_assignment(PlayerNumber::Two, penalties, role_assignment) { - role_assignment[PlayerNumber::Two] = Some(Role::ReplacementKeeper); - return unassigned_robots - 1; - } else if needs_assignment(PlayerNumber::Three, penalties, role_assignment) { - role_assignment[PlayerNumber::Three] = Some(Role::ReplacementKeeper); - return unassigned_robots - 1; - } else if needs_assignment(PlayerNumber::Four, penalties, role_assignment) { - role_assignment[PlayerNumber::Four] = Some(Role::ReplacementKeeper); - return unassigned_robots - 1; - } else if needs_assignment(PlayerNumber::Five, penalties, role_assignment) { - role_assignment[PlayerNumber::Five] = Some(Role::ReplacementKeeper); - return unassigned_robots - 1; - } else if needs_assignment(PlayerNumber::Six, penalties, role_assignment) { - role_assignment[PlayerNumber::Six] = Some(Role::ReplacementKeeper); - return unassigned_robots - 1; - } else if needs_assignment(PlayerNumber::Seven, penalties, role_assignment) { - role_assignment[PlayerNumber::Seven] = Some(Role::ReplacementKeeper); - return unassigned_robots - 1; + if unassigned_players.pop_front() == Some(own_player_number) { + return match own_player_number { + PlayerNumber::One => Role::Keeper, + _ => Role::ReplacementKeeper, + }; } - unassigned_robots + Role::Searcher +} + +#[cfg(test)] +mod test { + use proptest::prelude::*; + + use super::*; + + proptest! { + #[allow(clippy::too_many_arguments)] + #[test] + fn process_role_state_machine_should_be_idempotent_with_event_none( + initial_role in prop_oneof![ + Just(Role::DefenderLeft), + Just(Role::DefenderRight), + Just(Role::Keeper), + Just(Role::Loser), + Just(Role::MidfielderLeft), + Just(Role::MidfielderRight), + Just(Role::ReplacementKeeper), + Just(Role::Searcher), + Just(Role::Striker), + Just(Role::StrikerSupporter), + ], + detected_own_ball: bool, + event in Just(Event::None), + time_to_reach_kick_position in prop_oneof![Just(None), Just(Some(Duration::ZERO)), Just(Some(Duration::from_secs(10_000)))], + team_ball in prop_oneof![ + Just(None), + Just(Some(BallPosition::{ last_seen: SystemTime::UNIX_EPOCH, position: Default::default(), velocity: Default::default() })), + Just(Some(BallPosition{ last_seen: SystemTime::UNIX_EPOCH + Duration::from_secs(10), position: Default::default(), velocity: Default::default() })) + ], + cycle_start_time in prop_oneof![Just(SystemTime::UNIX_EPOCH + Duration::from_secs(11))], + filtered_game_controller_state in prop_oneof![Just(None), Just(Some(FilteredGameControllerState{game_phase: GamePhase::PenaltyShootout{kicking_team: Team::Hulks}, ..Default::default()}))], + player_number in Just(PlayerNumber::Five), + striker_trusts_team_ball_duration in Just(Duration::from_secs(5)), + optional_roles in Just(&[Role::DefenderLeft, Role::StrikerSupporter]) + ) { + let new_role = update_role_state_machine( + initial_role, + detected_own_ball, + event, + time_to_reach_kick_position, + team_ball, + cycle_start_time, + filtered_game_controller_state.as_ref(), + player_number, + striker_trusts_team_ball_duration, + optional_roles, + ); + let third_role = update_role_state_machine( + new_role, + detected_own_ball, + Event::None, + time_to_reach_kick_position, + team_ball, + cycle_start_time, + filtered_game_controller_state.as_ref(), + player_number, + striker_trusts_team_ball_duration, + optional_roles, + ); + assert_eq!(new_role, third_role); + } + } } diff --git a/crates/control/src/team_ball_receiver.rs b/crates/control/src/team_ball_receiver.rs new file mode 100644 index 0000000000..ec5b70d1ab --- /dev/null +++ b/crates/control/src/team_ball_receiver.rs @@ -0,0 +1,141 @@ +use std::{ + collections::BTreeMap, + time::{Duration, SystemTime}, +}; + +use color_eyre::eyre::Result; +use serde::{Deserialize, Serialize}; + +use context_attribute::context; +use coordinate_systems::Field; +use framework::{AdditionalOutput, MainOutput, PerceptionInput}; +use linear_algebra::Vector2; +use spl_network_messages::{GamePhase, HulkMessage, SubState}; +use types::{ + ball_position::BallPosition, cycle_time::CycleTime, + filtered_game_controller_state::FilteredGameControllerState, messages::IncomingMessage, + players::Players, +}; + +#[derive(Deserialize, Serialize)] +pub struct TeamBallReceiver { + received_balls: Players>>, +} + +#[context] +pub struct CreationContext {} + +#[context] +pub struct CycleContext { + cycle_time: Input, + filtered_game_controller_state: + Input, "filtered_game_controller_state?">, + network_message: PerceptionInput, "SplNetwork", "filtered_message?">, + + maximum_age: Parameter, + + team_balls: AdditionalOutput>>, "team_balls">, +} + +#[context] +pub struct MainOutputs { + pub team_ball: MainOutput>>, +} + +impl TeamBallReceiver { + pub fn new(_context: CreationContext) -> Result { + Ok(Self { + received_balls: Players::default(), + }) + } + + pub fn cycle(&mut self, mut context: CycleContext) -> Result { + let messages = get_spl_messages(&context.network_message.persistent); + for (time, message) in messages { + self.process_message(time, message); + } + + // Ignore everything during penalty_* + if let Some(game_controller_state) = context.filtered_game_controller_state { + let in_penalty_shootout = matches!( + game_controller_state.game_phase, + GamePhase::PenaltyShootout { .. } + ); + let in_penalty_kick = game_controller_state.sub_state == Some(SubState::PenaltyKick); + + if in_penalty_shootout || in_penalty_kick { + return Ok(MainOutputs { + team_ball: None.into(), + }); + } + } + + let team_ball = + self.get_best_received_ball(context.cycle_time.start_time, *context.maximum_age); + + context.team_balls.fill_if_subscribed(|| { + self.received_balls.map(|ball| { + ball.filter(|ball| { + context + .cycle_time + .start_time + .duration_since(ball.last_seen) + .expect("time ran backwards") + < *context.maximum_age + }) + }) + }); + + Ok(MainOutputs { + team_ball: team_ball.into(), + }) + } + + fn process_message(&mut self, time: SystemTime, message: HulkMessage) { + let (player, ball) = match message { + HulkMessage::Striker(striker_message) => ( + striker_message.player_number, + Some(BallPosition { + position: striker_message.ball_position.position, + velocity: Vector2::zeros(), + last_seen: time - striker_message.ball_position.age, + }), + ), + HulkMessage::Loser(loser_message) => (loser_message.player_number, None), + HulkMessage::VisualReferee(_) => return, + }; + self.received_balls[player] = ball; + } + + fn get_best_received_ball( + &self, + now: SystemTime, + trust_duration: Duration, + ) -> Option> { + self.received_balls + .iter() + .filter_map(|(_player_number, ball)| *ball) + .max_by_key(|ball| ball.last_seen) + .filter(|ball| { + now.duration_since(ball.last_seen) + .expect("time ran backwards") + < trust_duration + }) + } +} + +fn get_spl_messages<'a>( + persistent_messages: &'a BTreeMap>>, +) -> impl Iterator + 'a { + persistent_messages + .iter() + .flat_map(|(time, messages)| { + messages + .iter() + .filter_map(|message| Some((*time, (*message)?))) + }) + .filter_map(|(time, message)| match message { + IncomingMessage::Spl(message) => Some((time, *message)), + _ => None, + }) +} diff --git a/crates/control/src/time_to_reach_kick_position.rs b/crates/control/src/time_to_reach_kick_position.rs index a234f2ce15..7a7a434745 100644 --- a/crates/control/src/time_to_reach_kick_position.rs +++ b/crates/control/src/time_to_reach_kick_position.rs @@ -1,13 +1,12 @@ -use std::{f32::consts::FRAC_1_PI, time::Duration}; +use std::{f32::consts::PI, time::Duration}; -use color_eyre::Result; -use framework::AdditionalOutput; +use color_eyre::{eyre::Ok, Result}; +use framework::MainOutput; use linear_algebra::Vector2; use serde::{Deserialize, Serialize}; use types::{ - motion_command::{MotionCommand, OrientationMode}, - parameters::BehaviorParameters, - planned_path::PathSegment, + motion_command::OrientationMode, parameters::BehaviorParameters, planned_path::PathSegment, + stand_up::RemainingStandUpDuration, }; #[derive(Deserialize, Serialize)] @@ -16,101 +15,82 @@ pub struct TimeToReachKickPosition {} use context_attribute::context; #[context] pub struct CycleContext { - dribble_path: Input>, "dribble_path?">, - motion_command: Input, - - time_to_turn: AdditionalOutput, - time_to_reach_kick_position_output: - AdditionalOutput, "time_to_reach_kick_position_output">, - - time_to_reach_kick_position: CyclerState, + dribble_path_plan: Input)>, "dribble_path_plan?">, configuration: Parameter, stand_up_back_estimated_remaining_duration: - Input, "stand_up_back_estimated_remaining_duration?">, + CyclerState, stand_up_front_estimated_remaining_duration: - Input, "stand_up_front_estimated_remaining_duration?">, + CyclerState, + stand_up_sitting_estimated_remaining_duration: + CyclerState, } #[context] pub struct CreationContext {} #[context] -pub struct MainOutputs {} +pub struct MainOutputs { + pub time_to_reach_kick_position: MainOutput>, +} impl TimeToReachKickPosition { pub fn new(_: CreationContext) -> Result { Ok(Self {}) } - pub fn cycle(&mut self, mut context: CycleContext) -> Result { - let walk_time = context - .dribble_path - .as_ref() - .map(|path| { - path.iter() - .map(|segment: &PathSegment| { - let length = segment.length(); - match segment { - PathSegment::LineSegment(_) => { - length / context.configuration.path_planning.line_walking_speed - } - PathSegment::Arc(_) => { - length / context.configuration.path_planning.arc_walking_speed - } - } - }) - .sum() - }) - .map(Duration::from_secs_f32); - let turning_angle = match context.motion_command { - MotionCommand::Walk { - orientation_mode: OrientationMode::Override(orientation), - .. - } => Some(orientation.angle().abs()), - _ => { - let turning_angle_towards_path = match context.dribble_path { - Some(path) => match path.first() { - Some(PathSegment::LineSegment(line_segment)) => { - Some(line_segment.1.coords().angle(&Vector2::x_axis()).abs()) - } - _ => None, - }, - _ => None, - }; - turning_angle_towards_path - } + pub fn cycle(&mut self, context: CycleContext) -> Result { + let Some((orientation_mode, dribble_path)) = context.dribble_path_plan else { + return Ok(MainOutputs { + time_to_reach_kick_position: None.into(), + }); }; - let time_to_turn = turning_angle.map_or(Duration::ZERO, |angle| { - context - .configuration - .path_planning - .half_rotation - .mul_f32(angle * FRAC_1_PI) - }); - let time_to_reach_kick_position = walk_time.map(|walk_time| { - [ - walk_time, - *context - .stand_up_back_estimated_remaining_duration - .unwrap_or(&Duration::ZERO), - *context - .stand_up_front_estimated_remaining_duration - .unwrap_or(&Duration::ZERO), - time_to_turn, - ] - .into_iter() - .fold(Duration::ZERO, Duration::saturating_add) - }); + let walk_time = dribble_path + .iter() + .map(|segment: &PathSegment| { + let length = segment.length(); + match segment { + PathSegment::LineSegment(_) => { + length / context.configuration.path_planning.line_walking_speed + } + PathSegment::Arc(_) => { + length / context.configuration.path_planning.arc_walking_speed + } + } + }) + .sum(); + let walk_duration = Duration::from_secs_f32(walk_time); + + let turn_angle = match orientation_mode { + OrientationMode::Override(orientation) => orientation.angle().abs(), + _ => match dribble_path.first() { + Some(PathSegment::LineSegment(line_segment)) => { + line_segment.1.coords().angle(&Vector2::x_axis()).abs() + } + _ => 0.0, + }, + }; + let turn_duration = context + .configuration + .path_planning + .half_rotation + .mul_f32(turn_angle / PI); - context.time_to_turn.fill_if_subscribed(|| time_to_turn); - context - .time_to_reach_kick_position_output - .fill_if_subscribed(|| time_to_reach_kick_position); - *context.time_to_reach_kick_position = time_to_reach_kick_position.unwrap_or(Duration::MAX); + let time_to_reach_kick_position = [ + Some(walk_duration), + (*context.stand_up_back_estimated_remaining_duration).into(), + (*context.stand_up_front_estimated_remaining_duration).into(), + (*context.stand_up_sitting_estimated_remaining_duration).into(), + Some(turn_duration), + ] + .into_iter() + .flatten() + .fold(Duration::ZERO, Duration::saturating_add); - Ok(MainOutputs {}) + Ok(MainOutputs { + time_to_reach_kick_position: Some(time_to_reach_kick_position).into(), + }) } } diff --git a/crates/hulk_manifest/src/lib.rs b/crates/hulk_manifest/src/lib.rs index 28d506befd..692ed3dc4d 100644 --- a/crates/hulk_manifest/src/lib.rs +++ b/crates/hulk_manifest/src/lib.rs @@ -50,6 +50,7 @@ pub fn collect_hulk_cyclers() -> Result { "control::calibration_controller", "control::camera_matrix_calculator", "control::center_of_mass_provider", + "control::dribble_path_planner", "control::fall_state_estimation", "control::filtered_game_controller_state_timer", "control::foot_bumper_filter", @@ -64,6 +65,7 @@ pub fn collect_hulk_cyclers() -> Result { "control::motion::animation", "control::motion::arms_up_squat", "control::motion::arms_up_stand", + "control::motion::center_jump", "control::motion::command_sender", "control::motion::condition_input_provider", "control::motion::dispatching_interpolator", @@ -71,37 +73,38 @@ pub fn collect_hulk_cyclers() -> Result { "control::motion::head_motion", "control::motion::jump_left", "control::motion::jump_right", - "control::motion::center_jump", + "control::motion::keeper_jump_left", + "control::motion::keeper_jump_right", "control::motion::look_around", "control::motion::look_at", "control::motion::motion_selector", - "control::motion::obstacle_avoiding_arms", "control::motion::motor_commands_collector", "control::motion::motor_commands_optimizer", + "control::motion::obstacle_avoiding_arms", "control::motion::sit_down", "control::motion::stand_up_back", "control::motion::stand_up_front", "control::motion::stand_up_sitting", "control::motion::step_planner", "control::motion::walk_manager", - "control::motion::wide_stance", - "control::motion::keeper_jump_right", - "control::motion::keeper_jump_left", "control::motion::walking_engine", + "control::motion::wide_stance", "control::obstacle_filter", + "control::obstacle_receiver", "control::odometry", "control::orientation_filter", "control::penalty_shot_direction_estimation", "control::primary_state_filter", + "control::referee_pose_detection_filter", + "control::referee_position_provider", "control::role_assignment", "control::rule_obstacle_composer", - "control::referee_position_provider", - "control::referee_pose_detection_filter", "control::sacrificial_lamb", + "control::search_suggestor", "control::sole_pressure_filter", "control::sonar_filter", - "control::search_suggestor", "control::support_foot_estimation", + "control::team_ball_receiver", "control::time_to_reach_kick_position", "control::whistle_filter", "control::world_state_composer", diff --git a/crates/spl_network_messages/src/lib.rs b/crates/spl_network_messages/src/lib.rs index 4d17b426a9..85a48b5a5d 100644 --- a/crates/spl_network_messages/src/lib.rs +++ b/crates/spl_network_messages/src/lib.rs @@ -21,6 +21,7 @@ pub use game_controller_state_message::{ #[derive(Clone, Copy, Debug, Deserialize, Serialize)] pub enum HulkMessage { Striker(StrikerMessage), + Loser(LoserMessage), VisualReferee(VisualRefereeMessage), } @@ -34,8 +35,14 @@ impl Default for HulkMessage { pub struct StrikerMessage { pub player_number: PlayerNumber, pub pose: Pose2, - pub ball_position: Option>, - pub time_to_reach_kick_position: Option, + pub ball_position: BallPosition, + pub time_to_reach_kick_position: Duration, +} + +#[derive(Clone, Copy, Debug, Default, Deserialize, Serialize)] +pub struct LoserMessage { + pub player_number: PlayerNumber, + pub pose: Pose2, } #[derive(Clone, Copy, Debug, Default, Deserialize, Serialize)] @@ -106,22 +113,29 @@ impl Display for PlayerNumber { #[cfg(test)] mod tests { - use std::time::Duration; - - use linear_algebra::{Point, Pose2}; + use super::*; - use crate::{BallPosition, HulkMessage, PlayerNumber, StrikerMessage, VisualRefereeMessage}; + use linear_algebra::Point; #[test] fn hulk_striker_message_size() { let test_message = HulkMessage::Striker(StrikerMessage { player_number: PlayerNumber::Seven, pose: Pose2::default(), - ball_position: Some(BallPosition { + ball_position: BallPosition { position: Point::origin(), age: Duration::MAX, - }), - time_to_reach_kick_position: Some(Duration::MAX), + }, + time_to_reach_kick_position: Duration::MAX, + }); + assert!(bincode::serialize(&test_message).unwrap().len() <= 128) + } + + #[test] + fn hulk_loser_message_size() { + let test_message = HulkMessage::Loser(LoserMessage { + player_number: PlayerNumber::Seven, + pose: Pose2::default(), }); assert!(bincode::serialize(&test_message).unwrap().len() <= 128) } diff --git a/crates/types/src/lib.rs b/crates/types/src/lib.rs index 65c3aa2633..a0d273e948 100644 --- a/crates/types/src/lib.rs +++ b/crates/types/src/lib.rs @@ -67,6 +67,7 @@ pub mod sensor_data; pub mod sole_pressure; pub mod sonar_obstacle; pub mod sonar_values; +pub mod stand_up; pub mod step; pub mod support_foot; pub mod walk_command; diff --git a/crates/types/src/motion_command.rs b/crates/types/src/motion_command.rs index 36f6933de3..78d513e168 100644 --- a/crates/types/src/motion_command.rs +++ b/crates/types/src/motion_command.rs @@ -29,7 +29,9 @@ pub enum WalkSpeed { Fast, } -#[derive(Clone, Copy, Debug, Serialize, Deserialize)] +#[derive( + Clone, Copy, Debug, Serialize, Deserialize, PathSerialize, PathDeserialize, PathIntrospect, +)] pub enum OrientationMode { AlignWithPath, Override(Orientation2), diff --git a/crates/types/src/parameters.rs b/crates/types/src/parameters.rs index ce70fcb94f..d4c5df9dd3 100644 --- a/crates/types/src/parameters.rs +++ b/crates/types/src/parameters.rs @@ -24,7 +24,15 @@ pub struct WhistleDetectionParameters { } #[derive( - Clone, Debug, Default, Deserialize, Serialize, PathSerialize, PathDeserialize, PathIntrospect, + Copy, + Clone, + Debug, + Default, + Deserialize, + Serialize, + PathSerialize, + PathDeserialize, + PathIntrospect, )] pub struct StepPlannerParameters { pub injected_step: Option, @@ -70,7 +78,15 @@ pub struct LookActionParameters { } #[derive( - Clone, Debug, Default, Deserialize, Serialize, PathSerialize, PathDeserialize, PathIntrospect, + Copy, + Clone, + Debug, + Default, + Deserialize, + Serialize, + PathSerialize, + PathDeserialize, + PathIntrospect, )] pub struct RolePositionsParameters { pub defender_aggressive_ring_radius: f32, @@ -93,7 +109,15 @@ pub struct RolePositionsParameters { } #[derive( - Clone, Debug, Default, Deserialize, Serialize, PathSerialize, PathDeserialize, PathIntrospect, + Copy, + Clone, + Debug, + Default, + Deserialize, + Serialize, + PathSerialize, + PathDeserialize, + PathIntrospect, )] pub struct SearchParameters { pub position_reached_distance: f32, @@ -101,7 +125,15 @@ pub struct SearchParameters { } #[derive( - Clone, Debug, Default, Deserialize, Serialize, PathSerialize, PathDeserialize, PathIntrospect, + Copy, + Clone, + Debug, + Default, + Deserialize, + Serialize, + PathSerialize, + PathDeserialize, + PathIntrospect, )] pub struct InWalkKicksParameters { pub forward: InWalkKickInfoParameters, @@ -122,7 +154,15 @@ impl Index for InWalkKicksParameters { } #[derive( - Clone, Debug, Default, Deserialize, Serialize, PathSerialize, PathDeserialize, PathIntrospect, + Copy, + Clone, + Debug, + Default, + Deserialize, + Serialize, + PathSerialize, + PathDeserialize, + PathIntrospect, )] pub struct InWalkKickInfoParameters { pub position: nalgebra::Point2, @@ -134,7 +174,15 @@ pub struct InWalkKickInfoParameters { } #[derive( - Clone, Debug, Default, Deserialize, Serialize, PathSerialize, PathDeserialize, PathIntrospect, + Copy, + Clone, + Debug, + Default, + Deserialize, + Serialize, + PathSerialize, + PathDeserialize, + PathIntrospect, )] pub struct DribblingParameters { pub hybrid_align_distance: f32, @@ -145,7 +193,15 @@ pub struct DribblingParameters { } #[derive( - Clone, Debug, Default, Deserialize, Serialize, PathSerialize, PathDeserialize, PathIntrospect, + Copy, + Clone, + Debug, + Default, + Deserialize, + Serialize, + PathSerialize, + PathDeserialize, + PathIntrospect, )] pub struct WalkAndStandParameters { pub hysteresis: nalgebra::Vector2, @@ -156,7 +212,15 @@ pub struct WalkAndStandParameters { } #[derive( - Clone, Debug, Default, Deserialize, Serialize, PathSerialize, PathDeserialize, PathIntrospect, + Copy, + Clone, + Debug, + Default, + Deserialize, + Serialize, + PathSerialize, + PathDeserialize, + PathIntrospect, )] pub struct LostBallParameters { pub offset_to_last_ball_location: Vector2, @@ -249,7 +313,6 @@ pub struct SplNetworkParameters { pub silence_interval_between_messages: Duration, pub spl_striker_message_receive_timeout: Duration, pub spl_striker_message_send_interval: Duration, - pub striker_trusts_team_ball: Duration, } #[derive( diff --git a/crates/types/src/players.rs b/crates/types/src/players.rs index 7229f3bd96..b65654542e 100644 --- a/crates/types/src/players.rs +++ b/crates/types/src/players.rs @@ -28,6 +28,20 @@ pub struct Players { pub seven: T, } +impl Players { + pub fn map(self, mut f: impl FnMut(From) -> To) -> Players { + Players { + one: f(self.one), + two: f(self.two), + three: f(self.three), + four: f(self.four), + five: f(self.five), + six: f(self.six), + seven: f(self.seven), + } + } +} + impl Players { pub fn new(value: T) -> Self { Self { diff --git a/crates/types/src/stand_up.rs b/crates/types/src/stand_up.rs new file mode 100644 index 0000000000..c67d2b00e9 --- /dev/null +++ b/crates/types/src/stand_up.rs @@ -0,0 +1,31 @@ +use std::time::Duration; + +use serde::{Deserialize, Serialize}; + +use path_serde::{PathDeserialize, PathIntrospect, PathSerialize}; + +#[derive( + Clone, + Copy, + Debug, + Default, + Deserialize, + Serialize, + PathSerialize, + PathDeserialize, + PathIntrospect, +)] +pub enum RemainingStandUpDuration { + Running(Duration), + #[default] + NotRunning, +} + +impl From for Option { + fn from(val: RemainingStandUpDuration) -> Self { + match val { + RemainingStandUpDuration::Running(duration) => Some(duration), + RemainingStandUpDuration::NotRunning => None, + } + } +} diff --git a/etc/parameters/default.json b/etc/parameters/default.json index 3846372990..ab491ab623 100644 --- a/etc/parameters/default.json +++ b/etc/parameters/default.json @@ -1119,7 +1119,8 @@ }, "role_assignment": { "forced_role": null, - "keeper_replacementkeeper_switch_time": { "nanos": 0, "secs": 12 } + "keeper_replacementkeeper_switch_time": { "nanos": 0, "secs": 12 }, + "striker_trusts_team_ball": { "nanos": 0, "secs": 1 } }, "walk_speed": { "defend": "Normal", @@ -1400,10 +1401,12 @@ "spl_striker_message_send_interval": { "nanos": 0, "secs": 2 - }, - "striker_trusts_team_ball": { - "nanos": 0, - "secs": 1 + } + }, + "team_ball": { + "maximum_age": { + "nanos": 500000000, + "secs": 4 } }, "joint_calibration_offsets": { diff --git a/tools/twix/src/panels/look_at.rs b/tools/twix/src/panels/look_at.rs index a77cb1bcab..c9c384ff26 100644 --- a/tools/twix/src/panels/look_at.rs +++ b/tools/twix/src/panels/look_at.rs @@ -90,16 +90,15 @@ impl Widget for &mut LookAtPanel { None } }; - let is_safe_to_override_current_motion_command = - current_motion_command.as_ref().is_some_and(|command| { - matches!( - command, - MotionCommand::Penalized - | MotionCommand::Stand { .. } - | MotionCommand::Walk { .. } - | MotionCommand::InWalkKick { .. } - ) - }); + let is_safe_to_override_current_motion_command = matches!( + current_motion_command, + Some( + MotionCommand::Penalized + | MotionCommand::Stand { .. } + | MotionCommand::Walk { .. } + | MotionCommand::InWalkKick { .. } + ) + ); if !is_safe_to_override_current_motion_command { status_text_job.append( "Cannot safely override motion, please put the NAO into a standing position!", diff --git a/tools/twix/src/panels/map/layers/behavior_simulator.rs b/tools/twix/src/panels/map/layers/behavior_simulator.rs index ae658e8279..3bd8129a8b 100644 --- a/tools/twix/src/panels/map/layers/behavior_simulator.rs +++ b/tools/twix/src/panels/map/layers/behavior_simulator.rs @@ -1,13 +1,16 @@ use std::sync::Arc; use color_eyre::{eyre::Context, Result}; -use eframe::epaint::{Color32, Stroke}; +use eframe::{ + egui::{Align2, FontId}, + epaint::{Color32, Stroke}, +}; use coordinate_systems::{Field, Ground}; use linear_algebra::{IntoFramed, Isometry2, Point2}; use types::{ ball_position::SimulatorBallState, field_dimensions::FieldDimensions, - motion_command::MotionCommand, + motion_command::MotionCommand, roles::Role, }; use crate::{ @@ -20,6 +23,7 @@ const TRANSPARENT_LIGHT_BLUE: Color32 = Color32::from_rgba_premultiplied(136, 17 pub struct BehaviorSimulator { ground_to_field: PlayersBufferHandle>>, + role: PlayersBufferHandle, motion_command: PlayersBufferHandle, head_yaw: PlayersBufferHandle, ball: BufferHandle>, @@ -35,6 +39,12 @@ impl Layer for BehaviorSimulator { "main_outputs.ground_to_field", ) .unwrap(); + let role = PlayersBufferHandle::try_new( + nao.clone(), + "BehaviorSimulator.main_outputs.databases", + "main_outputs.role", + ) + .unwrap(); let motion_command = PlayersBufferHandle::try_new( nao.clone(), "BehaviorSimulator.main_outputs.databases", @@ -50,6 +60,7 @@ impl Layer for BehaviorSimulator { let ball = nao.subscribe_value("BehaviorSimulator.main_outputs.ball"); Self { ground_to_field, + role, motion_command, head_yaw: sensor_data, ball, @@ -70,7 +81,23 @@ impl Layer for BehaviorSimulator { continue; }; - let pose_color = Color32::from_white_alpha(63); + let pose_color = match self.role.0[player_number] + .get_last_value() + .wrap_err("role")? + { + Some( + Role::DefenderLeft + | Role::DefenderRight + | Role::MidfielderLeft + | Role::MidfielderRight, + ) => Color32::BLUE, + Some(Role::Keeper | Role::ReplacementKeeper) => Color32::YELLOW, + Some(Role::Loser) => Color32::BLACK, + Some(Role::Searcher) => Color32::WHITE, + Some(Role::Striker) => Color32::RED, + Some(Role::StrikerSupporter) => Color32::LIGHT_BLUE, + None => Color32::PLACEHOLDER, + }; let pose_stroke = Stroke { width: 0.02, color: Color32::BLACK, @@ -117,6 +144,15 @@ impl Layer for BehaviorSimulator { pose_color, pose_stroke, ); + let mut font = FontId::default(); + font.size *= 2.0; + painter.floating_text( + ground_to_field.as_pose().position(), + Align2::CENTER_CENTER, + format!("{player_number}"), + font, + Color32::BLACK, + ); } if let Some(ball_state) = self.ball.get_last_value().wrap_err("ball state")?.flatten() { diff --git a/tools/twix/src/panels/map/layers/robot_pose.rs b/tools/twix/src/panels/map/layers/robot_pose.rs index e32875b869..e6db838723 100644 --- a/tools/twix/src/panels/map/layers/robot_pose.rs +++ b/tools/twix/src/panels/map/layers/robot_pose.rs @@ -23,7 +23,7 @@ impl Layer for RobotPose { painter: &TwixPainter, _field_dimensions: &FieldDimensions, ) -> Result<()> { - let pose_color = Color32::from_white_alpha(187); + let pose_color = Color32::from_white_alpha(127); let pose_stroke = Stroke { width: 0.02, color: Color32::BLACK,