diff --git a/Cargo.lock b/Cargo.lock index 778a953dec..381079e832 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -1464,6 +1464,7 @@ dependencies = [ "rand", "rand_chacha", "serde", + "serde_json", "smallvec", "spl_network_messages", "splines", @@ -4556,6 +4557,7 @@ dependencies = [ "num-traits", "path_serde_derive", "serde", + "serde_json", "thiserror", ] diff --git a/crates/control/Cargo.toml b/crates/control/Cargo.toml index 69a06a98c8..6c8437925d 100644 --- a/crates/control/Cargo.toml +++ b/crates/control/Cargo.toml @@ -31,6 +31,7 @@ projection = { workspace = true } rand = { workspace = true } rand_chacha = { workspace = true } serde = { workspace = true } +serde_json = { workspace = true } smallvec = { workspace = true } spl_network_messages = { workspace = true } splines = { workspace = true } diff --git a/crates/control/src/ball_contact_counter.rs b/crates/control/src/ball_contact_counter.rs new file mode 100644 index 0000000000..d8ab9a98c9 --- /dev/null +++ b/crates/control/src/ball_contact_counter.rs @@ -0,0 +1,129 @@ +use color_eyre::Result; +use serde::{Deserialize, Serialize}; + +use context_attribute::context; +use coordinate_systems::Ground; +use framework::{AdditionalOutput, MainOutput, PerceptionInput}; +use serde_json::Value; +use spl_network_messages::{GameState, HulkMessage}; +use types::{ + ball_position::BallPosition, filtered_game_controller_state::FilteredGameControllerState, + filtered_game_state::FilteredGameState, messages::IncomingMessage, + motion_command::MotionCommand, players::Players, +}; + +#[derive(Deserialize, Serialize)] +pub struct BallContactCounter { + state: StateMachine, + own_contact_count: usize, + other_players_had_contact: Players, +} + +#[context] +pub struct CreationContext {} + +#[context] +pub struct CycleContext { + own_ball_contact_count: CyclerState, + other_striker_had_ball_contact: CyclerState, + + motion_command: Input, + ball_position: Input>, "ball_position?">, + filtered_messages: PerceptionInput, "SplNetwork", "filtered_message?">, + filtered_game_controller_state: + Input, "filtered_game_controller_state?">, + + x: AdditionalOutput, +} + +#[context] +#[derive(Default)] +pub struct MainOutputs { + pub x: MainOutput, +} + +impl BallContactCounter { + pub fn new(_context: CreationContext) -> Result { + Ok(Self { + state: StateMachine::Start, + own_contact_count: 0, + other_players_had_contact: Players::default(), + }) + } + + pub fn cycle(&mut self, mut context: CycleContext) -> Result { + self.state = match self.state { + StateMachine::Start => { + if context + .ball_position + .is_some_and(|ball| ball.position.coords().norm() < 0.3) + { + StateMachine::BallWasClose + } else { + StateMachine::Start + } + } + StateMachine::BallWasClose => { + if matches!(context.motion_command, MotionCommand::InWalkKick { .. }) { + StateMachine::Kicked + } else { + StateMachine::BallWasClose + } + } + StateMachine::Kicked => { + if context + .ball_position + .is_some_and(|ball| ball.position.coords().norm() > 0.5) + { + self.own_contact_count += 1; + StateMachine::Start + } else { + StateMachine::Kicked + } + } + }; + + for message in context + .filtered_messages + .persistent + .values() + .flatten() + .filter_map(|message| *message) + { + if let IncomingMessage::Spl(HulkMessage::Striker(striker_message)) = dbg!(message) { + if striker_message.number_of_ball_contacts > 0 { + self.other_players_had_contact[striker_message.player_number] = true; + } + } + } + + if let Some(state) = context.filtered_game_controller_state { + if state.game_state == FilteredGameState::Set || state.sub_state.is_some() { + self.other_players_had_contact = Players::default(); + self.own_contact_count = 0; + self.state = StateMachine::Start; + } + } + + *context.other_striker_had_ball_contact = self + .other_players_had_contact + .iter() + .any(|(_, had_contact)| *had_contact); + *context.own_ball_contact_count = self.own_contact_count; + + context + .x + .fill_if_subscribed(|| serde_json::to_value(&self).unwrap()); + + Ok(MainOutputs { + x: serde_json::to_value(&self).unwrap().into(), + }) + } +} + +#[derive(Copy, Clone, Deserialize, Serialize)] +enum StateMachine { + Start, + BallWasClose, + Kicked, +} diff --git a/crates/control/src/kick_selector.rs b/crates/control/src/kick_selector.rs index 23259eac42..6b2462b8e6 100644 --- a/crates/control/src/kick_selector.rs +++ b/crates/control/src/kick_selector.rs @@ -39,6 +39,7 @@ pub struct CycleContext { Input, "filtered_game_controller_state?">, ground_to_upcoming_support: CyclerState, "ground_to_upcoming_support">, + other_striker_had_ball_contact: CyclerState, decision_parameters: Parameter, field_dimensions: Parameter, @@ -68,6 +69,7 @@ impl KickSelector { ground_to_field * ball_position, context.field_dimensions, context.decision_parameters, + *context.other_striker_had_ball_contact, ); context .playing_situation @@ -79,7 +81,9 @@ impl KickSelector { PlayingSituation::PenaltyShot => { &context.decision_parameters.penalty_shot_kick_variants } - PlayingSituation::Normal => &context.decision_parameters.default_kick_variants, + PlayingSituation::IndirectGoalDance | PlayingSituation::Normal => { + &context.decision_parameters.default_kick_variants + } } .iter() .filter(|variant| match variant { @@ -94,7 +98,12 @@ impl KickSelector { PlayingSituation::KickOff => context.decision_parameters.kick_off_kick_strength, PlayingSituation::CornerKick => context.decision_parameters.corner_kick_strength, PlayingSituation::PenaltyShot => context.decision_parameters.penalty_shot_kick_strength, - PlayingSituation::Normal => context.decision_parameters.default_kick_strength, + PlayingSituation::IndirectGoalDance => { + context + .decision_parameters + .indirect_goal_dance_kick_strength + } + PlayingSituation::Normal => context.decision_parameters.kick_off_kick_strength, }; let targets = collect_kick_targets(&context, playing_situation); @@ -127,6 +136,7 @@ impl KickSelector { *context.ground_to_field, context.filtered_game_controller_state, context.decision_parameters, + *context.other_striker_had_ball_contact, ); instant_kick_decisions.sort_by(|left, right| { compare_decisions( @@ -167,6 +177,7 @@ fn determine_playing_situation( ball_position: Point2, field_dimensions: &FieldDimensions, parameters: &DecisionParameters, + other_striker_had_ball_contact: bool, ) -> PlayingSituation { let is_ball_in_opponent_corner = is_ball_in_opponents_corners(ball_position, field_dimensions, parameters); @@ -192,7 +203,13 @@ fn determine_playing_situation( .. }) => PlayingSituation::PenaltyShot, _ if is_ball_in_opponent_corner => PlayingSituation::CornerKick, - _ => PlayingSituation::Normal, + _ => { + if other_striker_had_ball_contact { + PlayingSituation::Normal + } else { + PlayingSituation::IndirectGoalDance + } + } } } @@ -204,9 +221,27 @@ fn collect_kick_targets( PlayingSituation::KickOff => generate_kick_off_kick_targets(context), PlayingSituation::CornerKick => generate_corner_kick_targets(context), PlayingSituation::PenaltyShot => generate_penalty_shot_kick_targets(context), + PlayingSituation::IndirectGoalDance => generate_indirect_goal_dance_targees(context), PlayingSituation::Normal => generate_goal_line_kick_targets(context), } } +fn generate_kick_off_kick_targets(context: &CycleContext) -> Vec> { + let field_to_ground = context.ground_to_field.inverse(); + let field_dimensions = &context.field_dimensions; + + let left_kick_off_target = field_to_ground + * point![ + 0.0, + field_dimensions.width / 2.0 - field_dimensions.center_circle_diameter, + ]; + let right_kick_off_target = field_to_ground + * point![ + 0.0, + -(field_dimensions.width / 2.0 - field_dimensions.center_circle_diameter), + ]; + + vec![left_kick_off_target, right_kick_off_target] +} fn generate_corner_kick_targets(context: &CycleContext) -> Vec> { let field_to_ground = context.ground_to_field.inverse(); @@ -219,57 +254,47 @@ fn generate_corner_kick_targets(context: &CycleContext) -> Vec> { vec![target] } -fn generate_goal_line_kick_targets(context: &CycleContext) -> Vec> { +fn generate_penalty_shot_kick_targets(context: &CycleContext) -> Vec> { let field_to_ground = context.ground_to_field.inverse(); let field_dimensions = &context.field_dimensions; - let left_goal_half = field_to_ground + let left_target = field_to_ground * point![ - field_dimensions.length / 2.0 + 0.1, + field_dimensions.length / 2.0, field_dimensions.goal_inner_width / 4.0 ]; - let right_goal_half = field_to_ground + let right_target = field_to_ground * point![ - field_dimensions.length / 2.0 + 0.1, + field_dimensions.length / 2.0, -field_dimensions.goal_inner_width / 4.0 ]; - vec![left_goal_half, right_goal_half] + + vec![left_target, right_target] } -fn generate_kick_off_kick_targets(context: &CycleContext) -> Vec> { +fn generate_indirect_goal_dance_targees(context: &CycleContext) -> Vec> { let field_to_ground = context.ground_to_field.inverse(); let field_dimensions = &context.field_dimensions; + // let parameters = &context.decision_parameters; - let left_kick_off_target = field_to_ground - * point![ - 0.0, - field_dimensions.width / 2.0 - field_dimensions.center_circle_diameter, - ]; - let right_kick_off_target = field_to_ground - * point![ - 0.0, - -(field_dimensions.width / 2.0 - field_dimensions.center_circle_diameter), - ]; - - vec![left_kick_off_target, right_kick_off_target] + vec![field_to_ground * field_dimensions.penalty_spot(Half::Opponent)] } -fn generate_penalty_shot_kick_targets(context: &CycleContext) -> Vec> { +fn generate_goal_line_kick_targets(context: &CycleContext) -> Vec> { let field_to_ground = context.ground_to_field.inverse(); let field_dimensions = &context.field_dimensions; - let left_target = field_to_ground + let left_goal_half = field_to_ground * point![ - field_dimensions.length / 2.0, + field_dimensions.length / 2.0 + 0.1, field_dimensions.goal_inner_width / 4.0 ]; - let right_target = field_to_ground + let right_goal_half = field_to_ground * point![ - field_dimensions.length / 2.0, + field_dimensions.length / 2.0 + 0.1, -field_dimensions.goal_inner_width / 4.0 ]; - - vec![left_target, right_target] + vec![left_goal_half, right_goal_half] } fn compare_decisions( @@ -346,6 +371,7 @@ fn generate_decisions_for_instant_kicks( ground_to_field: Isometry2, filtered_game_controller_state: Option<&FilteredGameControllerState>, parameters: &DecisionParameters, + allowed_to_score_goal: bool, ) -> Vec { let field_to_ground = ground_to_field.inverse(); @@ -417,7 +443,9 @@ fn generate_decisions_for_instant_kicks( kick_pose, strength: parameters.kick_off_kick_strength, }) - } else if !is_own_kick_off && (is_inside_field && is_strategic_target || scores_goal) { + } else if !is_own_kick_off + && (is_inside_field && is_strategic_target || scores_goal && allowed_to_score_goal) + { let kick_pose = compute_kick_pose(ball_position, target, kick_info, kicking_side); Some(KickDecision { target, diff --git a/crates/control/src/lib.rs b/crates/control/src/lib.rs index bdea72aac2..b2cb6ecef1 100644 --- a/crates/control/src/lib.rs +++ b/crates/control/src/lib.rs @@ -1,5 +1,6 @@ pub mod a_star; pub mod active_vision; +pub mod ball_contact_counter; pub mod ball_filter; pub mod ball_state_composer; pub mod behavior; diff --git a/crates/control/src/role_assignment.rs b/crates/control/src/role_assignment.rs index fd920e889a..1277f00315 100644 --- a/crates/control/src/role_assignment.rs +++ b/crates/control/src/role_assignment.rs @@ -57,6 +57,7 @@ pub struct CycleContext { network_message: PerceptionInput, "SplNetwork", "filtered_message?">, game_controller_address: Input, "game_controller_address?">, time_to_reach_kick_position: CyclerState, + own_ball_contact_count: CyclerState, field_dimensions: Parameter, forced_role: Parameter, "role_assignment.forced_role?">, @@ -334,6 +335,7 @@ impl RoleAssignment { pose: ground_to_field.as_pose(), ball_position, time_to_reach_kick_position: Some(*context.time_to_reach_kick_position), + number_of_ball_contacts: *context.own_ball_contact_count, }), ))?; } diff --git a/crates/control/src/time_to_reach_kick_position.rs b/crates/control/src/time_to_reach_kick_position.rs index 907b2ea68d..e1b4f09fad 100644 --- a/crates/control/src/time_to_reach_kick_position.rs +++ b/crates/control/src/time_to_reach_kick_position.rs @@ -24,6 +24,7 @@ pub struct CycleContext { AdditionalOutput, "time_to_reach_kick_position_output">, time_to_reach_kick_position: CyclerState, + own_ball_contact_count: CyclerState, configuration: Parameter, @@ -89,6 +90,7 @@ impl TimeToReachKickPosition { .half_rotation .mul_f32(angle * FRAC_1_PI) }); + let ball_contacts_penalty = Duration::from_secs_f32(*context.own_ball_contact_count as f32); let time_to_reach_kick_position = walk_time.map(|walk_time| { [ walk_time, @@ -99,6 +101,7 @@ impl TimeToReachKickPosition { .stand_up_front_estimated_remaining_duration .unwrap_or(&Duration::ZERO), time_to_turn, + ball_contacts_penalty, ] .into_iter() .fold(Duration::ZERO, Duration::saturating_add) diff --git a/crates/hulk_behavior_simulator/build.rs b/crates/hulk_behavior_simulator/build.rs index 31e6bb7a2a..e86b5718bf 100644 --- a/crates/hulk_behavior_simulator/build.rs +++ b/crates/hulk_behavior_simulator/build.rs @@ -20,11 +20,12 @@ fn main() -> Result<()> { setup_nodes: vec!["crate::fake_data"], nodes: vec![ "control::active_vision", + "control::ball_contact_counter", "control::ball_state_composer", "control::behavior::node", + "control::filtered_game_controller_state_timer", "control::game_controller_state_filter", "control::kick_selector", - "control::filtered_game_controller_state_timer", "control::motion::look_around", "control::motion::motion_selector", "control::referee_position_provider", diff --git a/crates/hulk_manifest/src/lib.rs b/crates/hulk_manifest/src/lib.rs index bcc8d81d21..ef14a9b813 100644 --- a/crates/hulk_manifest/src/lib.rs +++ b/crates/hulk_manifest/src/lib.rs @@ -45,6 +45,7 @@ pub fn collect_hulk_cyclers() -> Result { "control::active_vision", "control::ball_filter", "control::ball_state_composer", + "control::ball_contact_counter", "control::behavior::node", "control::button_filter", "control::calibration_controller", diff --git a/crates/path_serde/Cargo.toml b/crates/path_serde/Cargo.toml index a58127fcd8..b98c536205 100644 --- a/crates/path_serde/Cargo.toml +++ b/crates/path_serde/Cargo.toml @@ -10,4 +10,5 @@ nalgebra = { workspace = true } num-traits = { workspace = true } path_serde_derive = { workspace = true } serde = { workspace = true } +serde_json = { workspace = true } thiserror = { workspace = true } diff --git a/crates/path_serde/src/not_supported.rs b/crates/path_serde/src/not_supported.rs index 9cf0419dbc..8f50e4409b 100644 --- a/crates/path_serde/src/not_supported.rs +++ b/crates/path_serde/src/not_supported.rs @@ -8,6 +8,7 @@ use std::{ use crate::{deserialize, serialize, PathDeserialize, PathIntrospect, PathSerialize}; use nalgebra::{DMatrix, Rotation3, SMatrix}; use serde::{Deserializer, Serializer}; +use serde_json::Value; macro_rules! implement_as_not_supported { ($type:ty) => { @@ -107,3 +108,4 @@ implement_as_not_supported!(String); implement_as_not_supported!(SystemTime); implement_as_not_supported!(Vec, T); implement_as_not_supported!(VecDeque, T); +implement_as_not_supported!(Value); diff --git a/crates/spl_network_messages/src/lib.rs b/crates/spl_network_messages/src/lib.rs index 4d17b426a9..bbbf957a1c 100644 --- a/crates/spl_network_messages/src/lib.rs +++ b/crates/spl_network_messages/src/lib.rs @@ -36,6 +36,7 @@ pub struct StrikerMessage { pub pose: Pose2, pub ball_position: Option>, pub time_to_reach_kick_position: Option, + pub number_of_ball_contacts: usize, } #[derive(Clone, Copy, Debug, Default, Deserialize, Serialize)] @@ -122,6 +123,7 @@ mod tests { age: Duration::MAX, }), time_to_reach_kick_position: Some(Duration::MAX), + number_of_ball_contacts: 0, }); assert!(bincode::serialize(&test_message).unwrap().len() <= 128) } diff --git a/crates/types/src/kick_decision.rs b/crates/types/src/kick_decision.rs index f4383aee35..36eedf1873 100644 --- a/crates/types/src/kick_decision.rs +++ b/crates/types/src/kick_decision.rs @@ -20,6 +20,7 @@ pub enum PlayingSituation { KickOff, CornerKick, PenaltyShot, + IndirectGoalDance, #[default] Normal, } @@ -50,6 +51,7 @@ pub struct DecisionParameters { pub penalty_shot_kick_variants: Vec, pub default_kick_strength: f32, + pub indirect_goal_dance_kick_strength: f32, pub corner_kick_strength: f32, pub kick_off_kick_strength: f32, pub penalty_shot_kick_strength: f32, diff --git a/etc/parameters/default.json b/etc/parameters/default.json index 62f24c0776..1b5907bcab 100644 --- a/etc/parameters/default.json +++ b/etc/parameters/default.json @@ -1082,6 +1082,7 @@ "kick_off_kick_variants": ["Forward", "Side", "Turn"], "penalty_shot_kick_variants": ["Forward"], "default_kick_strength": 1.0, + "indirect_goal_dance_kick_strength": 0.5, "corner_kick_strength": 0.25, "kick_off_kick_strength": 0.25, "penalty_shot_kick_strength": 2.0,