--wip-- [skip ci]

crates/control/src/behavior/initial.rs

@@ -1,6 +1,5 @@
 use coordinate_systems::Field;
 use linear_algebra::Point2;
-use spl_network_messages::PlayerNumber;
 use types::{
     camera_position::CameraPosition,
     filtered_game_state::FilteredGameState,
@@ -57,11 +56,11 @@ fn look_at_referee(
     };
 
     match (
-        world_state.robot.player_number,
+        world_state.walk_in_position_index,
         filtered_game_controller_state.own_team_is_home_after_coin_toss,
     ) {
-        (PlayerNumber::Four | PlayerNumber::Seven, true) => {}
-        (PlayerNumber::Two | PlayerNumber::Six, false) => {}
+        (3 | 5, true) => {}
+        (4 | 6, false) => {}
         _ => return None,
     }
 

crates/control/src/behavior/node.rs

@@ -7,7 +7,7 @@ use context_attribute::context;
 use coordinate_systems::Field;
 use framework::{AdditionalOutput, MainOutput};
 use linear_algebra::{point, Point2};
-use spl_network_messages::{GamePhase, PlayerNumber, SubState, Team};
+use spl_network_messages::{GamePhase, SubState, Team};
 use types::{
     action::Action,
     cycle_time::CycleTime,
@@ -62,7 +62,7 @@ pub struct CycleContext {
     world_state: Input<WorldState, "world_state">,
     cycle_time: Input<CycleTime, "cycle_time">,
     is_localization_converged: Input<bool, "is_localization_converged">,
-
+    //todo! filtered_game_controller_state
     parameters: Parameter<BehaviorParameters, "behavior">,
     in_walk_kicks: Parameter<InWalkKicksParameters, "in_walk_kicks">,
     field_dimensions: Parameter<FieldDimensions, "field_dimensions">,
@@ -158,8 +158,8 @@ impl Behavior {
             }
         }
 
-        if matches!(world_state.robot.player_number, PlayerNumber::One)
-            && matches!(world_state.robot.role, Role::Keeper)
+        if matches!(world_state.robot.jersey_number, 1)
+        //#todo! context.filtered_game_controller_state.goalkeeper_jersey_number
         {
             actions.push(Action::WideStance);
         }

crates/control/src/behavior/search.rs

@@ -149,7 +149,7 @@ fn assign_search_role(world_state: &WorldState) -> Option<SearchRole> {
     let penalties = world_state
         .filtered_game_controller_state
         .as_ref()
-        .map(|state| state.penalties)?;
+        .map(|state| state.penalties.clone())?;
     let available_players = penalties
         .iter()
         .filter_map(|(number, penalty)| match penalty {
@@ -160,7 +160,7 @@ fn assign_search_role(world_state: &WorldState) -> Option<SearchRole> {
     available_players
         .zip(search_roles)
         .find_map(|(number, position)| {
-            let is_my_player_number = number == world_state.robot.player_number;
+            let is_my_player_number = *number == world_state.robot.jersey_number;
             is_my_player_number.then_some(position)
         })
 }

crates/control/src/filtered_game_controller_state_timer.rs

@@ -4,11 +4,9 @@ use color_eyre::Result;
 use context_attribute::context;
 use framework::MainOutput;
 use serde::{Deserialize, Serialize};
-use spl_network_messages::Penalty;
 use types::{
     cycle_time::CycleTime, filtered_game_controller_state::FilteredGameControllerState,
     last_filtered_game_controller_state_change::LastFilteredGameControllerStateChanges,
-    players::Players,
 };
 
 #[derive(Deserialize, Serialize)]
@@ -82,58 +80,50 @@ impl FilteredGameControllerStateTimer {
             &mut self.filtered_game_controller_state_changes.kicking_team,
             cycle_start_time,
         );
+
         if context.filtered_game_controller_state.penalties
             != self.last_filtered_game_controller_state.penalties
         {
-            fn update_player_penalty(
-                new_penalty: Option<Penalty>,
-                current_penalty: Option<SystemTime>,
-                cycle_start_time: SystemTime,
-            ) -> Option<SystemTime> {
-                if new_penalty.is_some() {
-                    Some(cycle_start_time)
-                } else {
-                    current_penalty
+            for (jersey_number, penalty) in &context.filtered_game_controller_state.penalties {
+                match (
+                    penalty,
+                    self.filtered_game_controller_state_changes
+                        .penalties
+                        .contains_key(jersey_number),
+                ) {
+                    (None, false) => {
+                        self.filtered_game_controller_state_changes
+                            .penalties
+                            .insert(*jersey_number, None);
+                    }
+                    (Some(_), _) => {
+                        self.filtered_game_controller_state_changes
+                            .penalties
+                            .insert(*jersey_number, Some(cycle_start_time));
+                    }
+                    _ => (),
                 }
             }
-            self.filtered_game_controller_state_changes.penalties = Players {
-                one: update_player_penalty(
-                    context.filtered_game_controller_state.penalties.one,
-                    self.filtered_game_controller_state_changes.penalties.one,
-                    cycle_start_time,
-                ),
-                two: update_player_penalty(
-                    context.filtered_game_controller_state.penalties.two,
-                    self.filtered_game_controller_state_changes.penalties.two,
-                    cycle_start_time,
-                ),
-                three: update_player_penalty(
-                    context.filtered_game_controller_state.penalties.three,
-                    self.filtered_game_controller_state_changes.penalties.three,
-                    cycle_start_time,
-                ),
-                four: update_player_penalty(
-                    context.filtered_game_controller_state.penalties.four,
-                    self.filtered_game_controller_state_changes.penalties.four,
-                    cycle_start_time,
-                ),
-                five: update_player_penalty(
-                    context.filtered_game_controller_state.penalties.five,
-                    self.filtered_game_controller_state_changes.penalties.five,
-                    cycle_start_time,
-                ),
-                six: update_player_penalty(
-                    context.filtered_game_controller_state.penalties.six,
-                    self.filtered_game_controller_state_changes.penalties.six,
-                    cycle_start_time,
-                ),
-                seven: update_player_penalty(
-                    context.filtered_game_controller_state.penalties.seven,
-                    self.filtered_game_controller_state_changes.penalties.seven,
-                    cycle_start_time,
-                ),
-            };
+            let players_to_remove: Vec<usize> = self
+                .filtered_game_controller_state_changes
+                .penalties
+                .keys()
+                .filter(|&&player_number| {
+                    !&context
+                        .filtered_game_controller_state
+                        .penalties
+                        .contains_key(&player_number)
+                })
+                .cloned()
+                .collect();
+
+            for player_number in players_to_remove {
+                self.last_filtered_game_controller_state
+                    .penalties
+                    .remove(&player_number);
+            }
         }
+
         if context.filtered_game_controller_state.sub_state
             != self.last_filtered_game_controller_state.sub_state
         {
@@ -144,7 +134,7 @@ impl FilteredGameControllerStateTimer {
 
         Ok(MainOutputs {
             last_filtered_game_controller_state_changes: Some(
-                self.filtered_game_controller_state_changes,
+                self.filtered_game_controller_state_changes.clone(),
             )
             .into(),
         })

crates/control/src/game_controller_filter.rs

@@ -92,7 +92,7 @@ impl GameControllerFilter {
             .map(|(address, _time)| *address);
 
         Ok(MainOutputs {
-            game_controller_state: self.game_controller_state.into(),
+            game_controller_state: self.game_controller_state.clone().into(),
             game_controller_address: last_address.into(),
         })
     }
@@ -119,6 +119,8 @@ impl GameControllerFilter {
             remaining_amount_of_messages: message.hulks_team.remaining_amount_of_messages,
             sub_state: message.sub_state,
             hulks_team_is_home_after_coin_toss: message.hulks_team_is_home_after_coin_toss,
+            goalkeeper_jersey_number: message.hulks_team.goal_keeper_jersey_number,
+            opponent_goalkeeper_jersey_number: message.opponent_team.goal_keeper_jersey_number,
         });
     }
 

crates/control/src/game_controller_state_filter.rs

@@ -9,7 +9,7 @@ use coordinate_systems::{Field, Ground};
 use framework::{AdditionalOutput, MainOutput};
 use linear_algebra::{distance, Isometry2, Point2, Vector2};
 use serde::{Deserialize, Serialize};
-use spl_network_messages::{GamePhase, GameState, Penalty, PlayerNumber, Team};
+use spl_network_messages::{GamePhase, GameState, Penalty, Team};
 use types::{
     ball_position::BallPosition, cycle_time::CycleTime, field_dimensions::FieldDimensions,
     filtered_game_controller_state::FilteredGameControllerState,
@@ -38,7 +38,7 @@ pub struct CycleContext {
     game_controller_state: RequiredInput<Option<GameControllerState>, "game_controller_state?">,
     config: Parameter<GameStateFilterParameters, "game_state_filter">,
     field_dimensions: Parameter<FieldDimensions, "field_dimensions">,
-    player_number: Parameter<PlayerNumber, "player_number">,
+    jersey_number: Parameter<usize, "jersey_number">,
 
     ground_to_field: CyclerState<Isometry2<Ground, Field>, "ground_to_field">,
 
@@ -64,12 +64,13 @@ impl GameControllerStateFilter {
     pub fn cycle(&mut self, mut context: CycleContext) -> Result<MainOutputs> {
         let (new_own_penalties_last_cycle, new_opponent_penalties_last_cycle) = self
             .last_game_controller_state
+            .as_ref()
             .map(|last| {
                 (
-                    penalty_diff(last.penalties, context.game_controller_state.penalties),
+                    penalty_diff(&last.penalties, &context.game_controller_state.penalties),
                     penalty_diff(
-                        last.opponent_penalties,
-                        context.game_controller_state.opponent_penalties,
+                        &last.opponent_penalties,
+                        &context.game_controller_state.opponent_penalties,
                     ),
                 )
             })
@@ -89,15 +90,22 @@ impl GameControllerStateFilter {
             context.filtered_whistle,
             context.cycle_time,
             *context.visual_referee_proceed_to_ready,
-            *context.player_number,
+            *context.jersey_number,
             did_receive_motion_in_set_penalty,
         );
         let filtered_game_controller_state = FilteredGameControllerState {
             game_state: game_states.own,
             opponent_game_state: game_states.opponent,
             game_phase: context.game_controller_state.game_phase,
             kicking_team: context.game_controller_state.kicking_team,
-            penalties: context.game_controller_state.penalties,
+            penalties: filter_penalties(&context.game_controller_state.penalties.inner),
+            opponent_penalties: filter_penalties(
+                &context.game_controller_state.opponent_penalties.inner,
+            ),
+            goal_keeper_number: context.game_controller_state.goalkeeper_jersey_number,
+            opponent_goal_keeper_number: context
+                .game_controller_state
+                .opponent_goalkeeper_jersey_number,
             remaining_number_of_messages: context
                 .game_controller_state
                 .remaining_amount_of_messages,
@@ -108,11 +116,12 @@ impl GameControllerStateFilter {
             new_own_penalties_last_cycle,
             new_opponent_penalties_last_cycle,
         };
+        let filtered_game_controller_state = filtered_game_controller_state;
         context
             .whistle_in_set_ball_position
             .fill_if_subscribed(|| self.whistle_in_set_ball_position);
 
-        self.last_game_controller_state = Some(*context.game_controller_state);
+        self.last_game_controller_state = Some(context.game_controller_state.clone());
         Ok(MainOutputs {
             filtered_game_controller_state: Some(filtered_game_controller_state).into(),
         })
@@ -129,7 +138,7 @@ impl GameControllerStateFilter {
         filtered_whistle: &FilteredWhistle,
         cycle_time: &CycleTime,
         visual_referee_proceed_to_ready: bool,
-        player_number: PlayerNumber,
+        jersey_number: usize,
         did_receive_motion_in_set_penalty: bool,
     ) -> FilteredGameStates {
         let ball_detected_far_from_any_goal = ball_detected_far_from_any_goal(
@@ -166,7 +175,7 @@ impl GameControllerStateFilter {
             }
         }
         let motion_in_set = matches!(
-            game_controller_state.penalties[player_number],
+            game_controller_state.penalties[jersey_number],
             Some(Penalty::IllegalMotionInSet { .. })
         );
         if matches!(self.state, State::Playing { .. }) || motion_in_set {
@@ -501,22 +510,38 @@ impl State {
 }
 
 fn penalty_diff(
-    last: Players<Option<Penalty>>,
-    current: Players<Option<Penalty>>,
-) -> HashMap<PlayerNumber, Penalty> {
-    let current_penalties = current
+    last: &Players<Option<Penalty>>,
+    current: &Players<Option<Penalty>>,
+) -> HashMap<usize, Penalty> {
+    let current_penalties =
+        current
+            .inner
+            .iter()
+            .enumerate()
+            .fold(HashMap::new(), |mut map, (player, penalty)| {
+                if let Some(penalty) = *penalty {
+                    map.insert(player, penalty);
+                }
+                map
+            });
+    last.inner
         .iter()
-        .fold(HashMap::new(), |mut map, (player, penalty)| {
-            if let Some(penalty) = penalty {
-                map.insert(player, *penalty);
-            }
-            map
-        });
-    last.iter()
+        .enumerate()
         .fold(current_penalties, |mut map, (player, penalty)| {
             if penalty.is_some() {
                 map.remove(&player);
             }
             map
         })
 }
+
+fn filter_penalties(penalties: &Vec<Option<Penalty>>) -> HashMap<usize, Option<Penalty>> {
+    penalties
+        .into_iter()
+        .enumerate()
+        .filter_map(|(index, penalty_option)| match penalty_option {
+            Some(Penalty::Substitute { .. }) => None,
+            _ => Some((index + 1, *penalty_option)),
+        })
+        .collect()
+}