split player and role numbers

crates/argument_parsers/src/lib.rs

@@ -5,13 +5,13 @@ use std::{
 };
 
 use color_eyre::{
-    eyre::{bail, eyre, WrapErr},
+    eyre::{bail, WrapErr},
     Report, Result,
 };
 use regex::Regex;
 
 use nao::{Network, SystemctlAction};
-use spl_network_messages::PlayerNumber;
+use spl_network_messages::{PlayerNumber, RoleNumber};
 
 pub const SYSTEMCTL_ACTION_POSSIBLE_VALUES: &[&str] =
     &["disable", "enable", "restart", "start", "status", "stop"];
@@ -152,16 +152,18 @@ impl TryFrom<NaoAddress> for NaoNumber {
 pub struct NaoAddressPlayerAssignment {
     pub nao_address: NaoAddress,
     pub player_number: PlayerNumber,
+    pub role_number: RoleNumber,
 }
 
 impl FromStr for NaoAddressPlayerAssignment {
     type Err = Report;
 
     fn from_str(input: &str) -> Result<Self> {
-        let (prefix, player_number) = parse_assignment(input)?;
+        let (prefix, player_number, role_number) = parse_assignment(input)?;
         Ok(Self {
             nao_address: prefix.parse()?,
             player_number,
+            role_number,
         })
     }
 }
@@ -170,16 +172,18 @@ impl FromStr for NaoAddressPlayerAssignment {
 pub struct NaoNumberPlayerAssignment {
     pub nao_number: NaoNumber,
     pub player_number: PlayerNumber,
+    pub role_number: RoleNumber,
 }
 
 impl FromStr for NaoNumberPlayerAssignment {
     type Err = Report;
 
     fn from_str(input: &str) -> Result<Self> {
-        let (prefix, player_number) = parse_assignment(input)?;
+        let (prefix, player_number, role_number) = parse_assignment(input)?;
         Ok(Self {
             nao_number: prefix.parse()?,
             player_number,
+            role_number,
         })
     }
 }
@@ -190,8 +194,15 @@ impl Display for NaoNumberPlayerAssignment {
     }
 }
 
-fn parse_assignment(input: &str) -> Result<(&str, PlayerNumber)> {
-    let (prefix, player_number) = input.rsplit_once(':').ok_or_else(|| eyre!("missing `:`"))?;
+pub fn parse_assignment(input: &str) -> Result<(&str, PlayerNumber, RoleNumber)> {
+    let result = input.splitn(3, ':').collect::<Vec<_>>();
+    let arguments_length = result.len();
+    if !(2..=3).contains(&arguments_length) {
+        bail!("Too many or too few arguements {input}")
+    }
+    let prefix = result[0];
+    let player_number = result[1];
+    let role_number = result.get(2).unwrap_or(&player_number);
     let player_number = match player_number {
         "1" => PlayerNumber::One,
         "2" => PlayerNumber::Two,
@@ -200,9 +211,38 @@ fn parse_assignment(input: &str) -> Result<(&str, PlayerNumber)> {
         "5" => PlayerNumber::Five,
         "6" => PlayerNumber::Six,
         "7" => PlayerNumber::Seven,
+        "8" => PlayerNumber::Eight,
+        "9" => PlayerNumber::Nine,
+        "10" => PlayerNumber::Ten,
+        "11" => PlayerNumber::Eleven,
+        "12" => PlayerNumber::Twelve,
+        "13" => PlayerNumber::Thirteen,
+        "14" => PlayerNumber::Fourteen,
+        "15" => PlayerNumber::Fifteen,
+        "16" => PlayerNumber::Sixteen,
+        "17" => PlayerNumber::Seventeen,
+        "18" => PlayerNumber::Eighteen,
+        "19" => PlayerNumber::Nineteen,
+        "20" => PlayerNumber::Twenty,
         _ => bail!("unexpected player number {player_number}"),
     };
-    Ok((prefix, player_number))
+    let role_number = match *role_number {
+        "1" => RoleNumber::One,
+        "2" => RoleNumber::Two,
+        "3" => RoleNumber::Three,
+        "4" => RoleNumber::Four,
+        "5" => RoleNumber::Five,
+        "6" => RoleNumber::Six,
+        "7" => RoleNumber::Seven,
+        _ => {
+            if arguments_length == 2 {
+                bail!("no role number provided for player number higher than 7")
+            } else {
+                bail!("unexpected role number {role_number}")
+            }
+        }
+    };
+    Ok((prefix, player_number, role_number))
 }
 
 impl TryFrom<NaoAddressPlayerAssignment> for NaoNumberPlayerAssignment {
@@ -215,6 +255,7 @@ impl TryFrom<NaoAddressPlayerAssignment> for NaoNumberPlayerAssignment {
                 .try_into()
                 .wrap_err("failed to convert NAO address into NAO number")?,
             player_number: assignment.player_number,
+            role_number: assignment.role_number,
         })
     }
 }

crates/argument_parsers/tests/assignments.rs

@@ -0,0 +1,26 @@
+use argument_parsers::parse_assignment;
+use spl_network_messages::{PlayerNumber, RoleNumber};
+
+#[test]
+fn test_player_assignment() {
+    assert_eq!(
+        ("37", PlayerNumber::One, RoleNumber::One),
+        parse_assignment("37:1").expect("failed to parse assignments")
+    );
+    assert_eq!(
+        ("37", PlayerNumber::Nine, RoleNumber::One),
+        parse_assignment("37:9:1").expect("failed to parse assignments")
+    );
+}
+
+#[test]
+#[should_panic]
+fn wrong_role_assignment() {
+    let _ = parse_assignment("37:1:9").expect("failed to parse assignments");
+}
+
+#[test]
+#[should_panic]
+fn no_role_assignment_with_player_over_7() {
+    let _ = parse_assignment("37:9").expect("failed to parse assignments");
+}

crates/control/src/behavior/search.rs

@@ -145,7 +145,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.role_number;
             is_my_player_number.then_some(position)
         })
 }

crates/control/src/game_controller_state_filter.rs

@@ -61,12 +61,25 @@ impl GameControllerStateFilter {
             &mut self.opponent_state,
             *context.is_referee_initial_pose_detected,
         );
+        /*
+        let filtered_penalties = context.player_role_mapping.iter.find_in_penalties_or_something.collect();
+        For each role in mapping find the corresponding player and assign that role the penalty of the player based on the game controller penalties
+
+        New node which filters player mappings based on the incoming messages
+
+        When coming in from penalized and not registered in config yet, make sure to register self by sending message
+
+        Allow giving none to role number in pepsi playernumber so substitutes can be deployed at same time as everyone else (explicit None?)
+
+
+         */
         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,
+            //todo!filter penalties from playersnumbers to rolenumbers
             remaining_number_of_messages: context
                 .game_controller_state
                 .remaining_amount_of_messages,

crates/control/src/localization.rs

@@ -16,7 +16,7 @@ use context_attribute::context;
 use coordinate_systems::{Field, Ground};
 use filtering::pose_filter::PoseFilter;
 use framework::{AdditionalOutput, HistoricInput, MainOutput, PerceptionInput};
-use spl_network_messages::{GamePhase, Penalty, PlayerNumber, Team};
+use spl_network_messages::{GamePhase, Penalty, RoleNumber, Team};
 use types::{
     cycle_time::CycleTime,
     field_dimensions::FieldDimensions,
@@ -26,8 +26,8 @@ use types::{
     line_data::LineData,
     localization::{ScoredPose, Update},
     multivariate_normal_distribution::MultivariateNormalDistribution,
-    players::Players,
     primary_state::PrimaryState,
+    roles_enum::Roles,
     support_foot::Side,
 };
 
@@ -76,7 +76,7 @@ pub struct CycleContext {
     initial_hypothesis_covariance:
         Parameter<Matrix3<f32>, "localization.initial_hypothesis_covariance">,
     initial_hypothesis_score: Parameter<f32, "localization.initial_hypothesis_score">,
-    initial_poses: Parameter<Players<InitialPose>, "localization.initial_poses">,
+    initial_poses: Parameter<Roles<InitialPose>, "localization.initial_poses">,
     line_length_acceptance_factor: Parameter<f32, "localization.line_length_acceptance_factor">,
     line_measurement_noise: Parameter<Vector2<f32>, "localization.line_measurement_noise">,
     maximum_amount_of_gradient_descent_iterations:
@@ -85,7 +85,7 @@ pub struct CycleContext {
         Parameter<usize, "localization.maximum_amount_of_outer_iterations">,
     minimum_fit_error: Parameter<f32, "localization.minimum_fit_error">,
     odometry_noise: Parameter<Vector3<f32>, "localization.odometry_noise">,
-    player_number: Parameter<PlayerNumber, "player_number">,
+    role_number: Parameter<RoleNumber, "role_number">,
     penalized_distance: Parameter<f32, "localization.penalized_distance">,
     penalized_hypothesis_covariance:
         Parameter<Matrix3<f32>, "localization.penalized_hypothesis_covariance">,
@@ -141,7 +141,7 @@ impl Localization {
         match (self.last_primary_state, primary_state, game_phase) {
             (PrimaryState::Initial, PrimaryState::Ready, _) => {
                 let initial_pose = generate_initial_pose(
-                    &context.initial_poses[*context.player_number],
+                    &context.initial_poses[*context.role_number],
                     context.field_dimensions,
                 );
                 self.hypotheses = vec![ScoredPose::from_isometry(
@@ -524,7 +524,7 @@ impl Localization {
         let penalty = context
             .filtered_game_controller_state
             .and_then(|game_controller_state| {
-                game_controller_state.penalties[*context.player_number]
+                game_controller_state.penalties[*context.role_number]
             });
         let game_phase = context
             .filtered_game_controller_state
@@ -540,7 +540,7 @@ impl Localization {
         let ground_to_field = match primary_state {
             PrimaryState::Initial => Some(
                 generate_initial_pose(
-                    &context.initial_poses[*context.player_number],
+                    &context.initial_poses[*context.role_number],
                     context.field_dimensions,
                 )
                 .as_transform(),

crates/control/src/primary_state_filter.rs

@@ -5,7 +5,7 @@ use context_attribute::context;
 use framework::MainOutput;
 use hardware::{RecordingInterface, SpeakerInterface};
 use serde::{Deserialize, Serialize};
-use spl_network_messages::PlayerNumber;
+use spl_network_messages::RoleNumber;
 use types::{
     audio::{Sound, SpeakerRequest},
     buttons::Buttons,
@@ -28,7 +28,7 @@ pub struct CycleContext {
     filtered_game_controller_state:
         Input<Option<FilteredGameControllerState>, "filtered_game_controller_state?">,
 
-    player_number: Parameter<PlayerNumber, "player_number">,
+    role_number: Parameter<RoleNumber, "role_number">,
     recorded_primary_states: Parameter<HashSet<PrimaryState>, "recorded_primary_states">,
 
     hardware_interface: HardwareInterface,
@@ -53,7 +53,7 @@ impl PrimaryStateFilter {
     ) -> Result<MainOutputs> {
         let is_penalized = match context.filtered_game_controller_state {
             Some(game_controller_state) => {
-                game_controller_state.penalties[*context.player_number].is_some()
+                game_controller_state.penalties[*context.role_number].is_some()
             }
             None => false,
         };

crates/control/src/referee_pose_detection_filter.rs

@@ -8,15 +8,15 @@ use context_attribute::context;
 use framework::{AdditionalOutput, MainOutput, PerceptionInput};
 use hardware::NetworkInterface;
 use serde::{Deserialize, Serialize};
-use spl_network_messages::PlayerNumber;
+use spl_network_messages::{PlayerNumber, RoleNumber};
 use types::{
-    cycle_time::CycleTime, fall_state::FallState, messages::IncomingMessage, players::Players,
-    pose_kinds::PoseKind,
+    cycle_time::CycleTime, fall_state::FallState, messages::IncomingMessage, pose_kinds::PoseKind,
+    roles_enum::Roles,
 };
 
 #[derive(Deserialize, Serialize)]
 pub struct RefereePoseDetectionFilter {
-    detection_times: Players<Option<SystemTime>>,
+    detection_times: Roles<Option<SystemTime>>,
     detected_above_arm_poses_queue: VecDeque<bool>,
 }
 
@@ -43,9 +43,10 @@ pub struct CycleContext {
     minimum_above_head_arms_detections:
         Parameter<usize, "referee_pose_detection_filter.minimum_above_head_arms_detections">,
     player_number: Parameter<PlayerNumber, "player_number">,
+    role_number: Parameter<RoleNumber, "role_number">,
 
-    player_referee_detection_times:
-        AdditionalOutput<Players<Option<SystemTime>>, "player_referee_detection_times">,
+    role_referee_detection_times:
+        AdditionalOutput<Roles<Option<SystemTime>>, "player_referee_detection_times">,
 
     referee_pose_queue_length:
         Parameter<usize, "object_detection.object_detection_top.referee_pose_queue_length">,
@@ -90,7 +91,7 @@ impl RefereePoseDetectionFilter {
         );
 
         context
-            .player_referee_detection_times
+            .role_referee_detection_times
             .fill_if_subscribed(|| self.detection_times);
 
         context
@@ -124,15 +125,15 @@ impl RefereePoseDetectionFilter {
             .count();
 
         if detected_referee_pose_count >= *context.minimum_number_poses_before_message {
-            self.detection_times[*context.player_number] = Some(context.cycle_time.start_time);
+            self.detection_times[*context.role_number] = Some(context.cycle_time.start_time);
         }
 
         detected_referee_pose_count >= *context.minimum_number_poses_before_message
     }
 }
 
 fn decide(
-    pose_detection_times: Players<Option<SystemTime>>,
+    pose_detection_times: Roles<Option<SystemTime>>,
     cycle_start_time: SystemTime,
     initial_message_grace_period: Duration,
     minimum_above_head_arms_detections: usize,