Empty Field Decimator

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 }

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::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<bool>,
+}
+
+#[context]
+pub struct CreationContext {}
+
+#[context]
+pub struct CycleContext {
+ own_ball_contact_count: CyclerState<usize, "own_ball_contact_count">,
+ other_striker_had_ball_contact: CyclerState<bool, "other_striker_had_ball_contact">,
+
+ motion_command: Input<MotionCommand, "motion_command">,
+ ball_position: Input<Option<BallPosition<Ground>>, "ball_position?">,
+ filtered_messages: PerceptionInput<Option<IncomingMessage>, "SplNetwork", "filtered_message?">,
+ filtered_game_controller_state:
+ Input<Option<FilteredGameControllerState>, "filtered_game_controller_state?">,
+
+ x: AdditionalOutput<Value, "x">,
+}
+
+#[context]
+#[derive(Default)]
+pub struct MainOutputs {
+ pub x: MainOutput<Value>,
+}
+
+impl BallContactCounter {
+ pub fn new(_context: CreationContext) -> Result<Self> {
+ Ok(Self {
+ state: StateMachine::Start,
+ own_contact_count: 0,
+ other_players_had_contact: Players::default(),
+ })
+ }
+
+ pub fn cycle(&mut self, mut context: CycleContext) -> Result<MainOutputs> {
+ 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)) = 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,
+}

crates/control/src/kick_selector.rs

@@ -39,6 +39,7 @@ pub struct CycleContext {
  Input<Option<FilteredGameControllerState>, "filtered_game_controller_state?">,
  ground_to_upcoming_support:
  CyclerState<Isometry2<Ground, UpcomingSupport>, "ground_to_upcoming_support">,
+ other_striker_had_ball_contact: CyclerState<bool, "other_striker_had_ball_contact">,
 
  decision_parameters: Parameter<DecisionParameters, "kick_selector">,
  field_dimensions: Parameter<FieldDimensions, "field_dimensions">,
@@ -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);
@@ -128,6 +137,7 @@ impl KickSelector {
  *context.ground_to_field,
  context.filtered_game_controller_state,
  context.decision_parameters,
+ true, //*context.other_striker_had_ball_contact,
  );
  instant_kick_decisions.sort_by(|left, right| {
  compare_decisions(
@@ -168,6 +178,7 @@ fn determine_playing_situation(
  ball_position: Point2<Field>,
  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);
@@ -193,7 +204,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
+ }
+ }
  }
 }
 
@@ -205,9 +222,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<Point2<Ground>> {
+ 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<Point2<Ground>> {
  let field_to_ground = context.ground_to_field.inverse();
@@ -220,57 +255,47 @@ fn generate_corner_kick_targets(context: &CycleContext) -> Vec<Point2<Ground>> {
  vec![target]
 }
 
-fn generate_goal_line_kick_targets(context: &CycleContext) -> Vec<Point2<Ground>> {
+fn generate_penalty_shot_kick_targets(context: &CycleContext) -> Vec<Point2<Ground>> {
  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<Point2<Ground>> {
+fn generate_indirect_goal_dance_targees(context: &CycleContext) -> Vec<Point2<Ground>> {
  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<Point2<Ground>> {
+fn generate_goal_line_kick_targets(context: &CycleContext) -> Vec<Point2<Ground>> {
  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(
@@ -349,6 +374,7 @@ fn generate_decisions_for_instant_kicks(
  ground_to_field: Isometry2<Ground, Field>,
  filtered_game_controller_state: Option<&FilteredGameControllerState>,
  parameters: &DecisionParameters,
+ allowed_to_score_goal: bool,
 ) -> Vec<KickDecision> {
  let field_to_ground = ground_to_field.inverse();
 
@@ -418,7 +444,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,

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;

crates/control/src/role_assignment.rs

@@ -57,6 +57,7 @@ pub struct CycleContext {
  network_message: PerceptionInput<Option<IncomingMessage>, "SplNetwork", "filtered_message?">,
  game_controller_address: Input<Option<SocketAddr>, "game_controller_address?">,
  time_to_reach_kick_position: CyclerState<Duration, "time_to_reach_kick_position">,
+ own_ball_contact_count: CyclerState<usize, "own_ball_contact_count">,
 
  field_dimensions: Parameter<FieldDimensions, "field_dimensions">,
  forced_role: Parameter<Option<Role>, "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,
  }),
  ))?;
  }

crates/control/src/time_to_reach_kick_position.rs

@@ -24,6 +24,7 @@ pub struct CycleContext {
  AdditionalOutput<Option<Duration>, "time_to_reach_kick_position_output">,
 
  time_to_reach_kick_position: CyclerState<Duration, "time_to_reach_kick_position">,
+ own_ball_contact_count: CyclerState<usize, "own_ball_contact_count">,
 
  configuration: Parameter<BehaviorParameters, "behavior">,
 
@@ -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)