--wip-- [skip ci]

crates/control/src/active_player_filter.rs

@@ -0,0 +1,100 @@
+use color_eyre::Result;
+use context_attribute::context;
+use framework::MainOutput;
+use itertools::Itertools;
+use serde::{Deserialize, Serialize};
+use types::filtered_game_controller_state::FilteredGameControllerState;
+
+#[derive(Deserialize, Serialize)]
+pub struct ActivePlayerFilter {}
+
+#[context]
+pub struct CreationContext {}
+
+#[context]
+pub struct CycleContext {
+    filtered_game_controller_state:
+        Input<Option<FilteredGameControllerState>, "filtered_game_controller_state?">,
+
+    jersey_number: Parameter<usize, "jersey_number">,
+}
+
+#[context]
+#[derive(Default)]
+pub struct MainOutputs {
+    pub walk_in_position_index: MainOutput<usize>,
+    pub replacement_keeper_priority: MainOutput<Option<usize>>,
+    pub striker_priority: MainOutput<Option<usize>>,
+}
+
+impl ActivePlayerFilter {
+    pub fn new(_context: CreationContext) -> Result<Self> {
+        Ok(Self {})
+    }
+
+    pub fn cycle(&mut self, context: CycleContext) -> Result<MainOutputs> {
+        let penalties = context
+            .filtered_game_controller_state
+            .map(|game_controller_state| &game_controller_state.penalties);
+        let mut walk_in_position_index = if let Some(list) = penalties {
+            list.keys()
+                .sorted()
+                .position(|&key| key == *context.jersey_number)
+        } else {
+            None
+        }
+        .unwrap_or(7);
+
+        let goalkeeper_jersey_number = context
+            .filtered_game_controller_state
+            .map(|game_controller_state| game_controller_state.goal_keeper_number);
+        if let Some(goalkeeper_number) = goalkeeper_jersey_number {
+            if goalkeeper_number == *context.jersey_number {
+                walk_in_position_index = 0;
+            } else if goalkeeper_number > *context.jersey_number {
+                walk_in_position_index += 1;
+            }
+        }
+
+        let available_field_players = if let Some(game_controller_state) =
+            context.filtered_game_controller_state
+        {
+            game_controller_state
+                .penalties
+                .iter()
+                .filter(|(&jersey_number, penalty)| {
+                    penalty.is_none() && jersey_number != game_controller_state.goal_keeper_number
+                })
+                .map(|(&jersey_number, _)| jersey_number)
+                .sorted()
+                .collect::<Vec<_>>()
+        } else {
+            Vec::new()
+        };
+        let mut replacement_keeper_priority = available_field_players
+            .iter()
+            .position(|&jersey_number| jersey_number == *context.jersey_number);
+        if let (Some(game_controller_state), Some(keeper_priority)) = (
+            context.filtered_game_controller_state,
+            replacement_keeper_priority,
+        ) {
+            match game_controller_state
+                .penalties
+                .get(&game_controller_state.goal_keeper_number)
+            {
+                Some(_penalty) => {}
+                None => replacement_keeper_priority = Some(keeper_priority + 1),
+            }
+        }
+        let striker_priority = available_field_players
+            .iter()
+            .rev()
+            .position(|&jersey_number| jersey_number == *context.jersey_number);
+
+        Ok(MainOutputs {
+            walk_in_position_index: walk_in_position_index.into(),
+            replacement_keeper_priority: replacement_keeper_priority.into(),
+            striker_priority: striker_priority.into(),
+        })
+    }
+}

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,
         });
     }