Filter kicking_team, including detections

crates/control/src/ball_state_composer.rs

@@ -25,6 +25,8 @@ pub struct CreationContext {}
 
 #[context]
 pub struct CycleContext {
+    last_ball_state: CyclerState<BallState, "last_ball_state">,
+
     cycle_time: Input<CycleTime, "cycle_time">,
     ball_position: Input<Option<BallPosition<Ground>>, "ball_position?">,
     penalty_shot_direction: Input<Option<PenaltyShotDirection>, "penalty_shot_direction?">,
@@ -125,6 +127,8 @@ impl BallStateComposer {
             _ => None,
         };
 
+        *context.last_ball_state = ball.unwrap_or(BallState::default());
+
         Ok(MainOutputs {
             ball_state: ball.into(),
             rule_ball_state: rule_ball.into(),

crates/control/src/free_kick_signal_filter.rs

@@ -14,7 +14,7 @@ use spl_network_messages::{HulkMessage, PlayerNumber, Team, VisualRefereeMessage
 use types::{
     cycle_time::CycleTime,
     field_dimensions::GlobalFieldSide,
-    filtered_game_controller_state::FilteredGameControllerState,
+    game_controller_state::GameControllerState,
     messages::{IncomingMessage, OutgoingMessage},
     players::Players,
     pose_detection::{FreeKickSignalDetectionResult, TimeTaggedKickingTeamDetections},
@@ -39,8 +39,7 @@ pub struct CycleContext {
     referee_pose_kind:
         PerceptionInput<Option<PoseKind>, "ObjectDetectionTop", "referee_pose_kind?">,
     network_message: PerceptionInput<Option<IncomingMessage>, "SplNetwork", "filtered_message?">,
-    filtered_game_controller_state:
-        RequiredInput<Option<FilteredGameControllerState>, "filtered_game_controller_state?">,
+    game_controller_state: RequiredInput<Option<GameControllerState>, "game_controller_state?">,
 
     cycle_time: Input<CycleTime, "cycle_time">,
 
@@ -126,8 +125,8 @@ impl FreeKickSignalFilter {
             let detected_kicking_team = kicking_team_from_free_kick_signal_detection(
                 detection,
                 context
-                    .filtered_game_controller_state
-                    .own_team_is_home_after_coin_toss,
+                    .game_controller_state
+                    .hulks_team_is_home_after_coin_toss,
             );
             if let Some(detected_kicking_team) = detected_kicking_team {
                 self.detected_free_kick_detections_queue

crates/control/src/game_controller_state_filter.rs

@@ -35,6 +35,7 @@ pub struct CycleContext {
     cycle_time: Input<CycleTime, "cycle_time">,
     filtered_whistle: Input<FilteredWhistle, "filtered_whistle">,
     visual_referee_proceed_to_ready: Input<bool, "visual_referee_proceed_to_ready">,
+    filtered_kicking_team: Input<Option<Team>, "filtered_kicking_team?">,
     game_controller_state: RequiredInput<Option<GameControllerState>, "game_controller_state?">,
     config: Parameter<GameStateFilterParameters, "game_state_filter">,
     field_dimensions: Parameter<FieldDimensions, "field_dimensions">,
@@ -97,7 +98,7 @@ impl GameControllerStateFilter {
             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,
+            kicking_team: context.filtered_kicking_team.copied(),
             penalties: context.game_controller_state.penalties,
             remaining_number_of_messages: context
                 .game_controller_state

crates/control/src/kicking_team_filter.rs

@@ -0,0 +1,70 @@
+use color_eyre::Result;
+use serde::{Deserialize, Serialize};
+
+use context_attribute::context;
+use framework::MainOutput;
+use spl_network_messages::{SubState, Team};
+use types::{game_controller_state::GameControllerState, world_state::BallState};
+
+#[derive(Deserialize, Serialize)]
+pub struct KickingTeamFilter {}
+
+#[context]
+pub struct CreationContext {}
+
+#[context]
+pub struct CycleContext {
+    last_ball_state: CyclerState<BallState, "last_ball_position">,
+    game_controller_state: RequiredInput<Option<GameControllerState>, "game_controller_state?">,
+    detected_free_kick_kicking_team: Input<Option<Team>, "detected_free_kick_kicking_team?">,
+}
+
+#[context]
+pub struct MainOutputs {
+    pub filtered_kicking_team: MainOutput<Option<Team>>,
+}
+
+impl KickingTeamFilter {
+    pub fn new(_context: CreationContext) -> Result<Self> {
+        Ok(KickingTeamFilter {})
+    }
+
+    pub fn cycle(&mut self, context: CycleContext) -> Result<MainOutputs> {
+        let game_controller_state = context.game_controller_state;
+        let sub_state = context.game_controller_state.sub_state;
+        let last_ball_state = context.last_ball_state;
+
+        let filtered_kicking_team = if game_controller_state.kicking_team.is_some() {
+            game_controller_state.kicking_team
+        } else if context.detected_free_kick_kicking_team.is_some() {
+            context.detected_free_kick_kicking_team.copied()
+        } else {
+            match sub_state {
+                Some(SubState::CornerKick)
+                    if last_ball_state.ball_in_field.x().is_sign_positive() =>
+                {
+                    Some(Team::Hulks)
+                }
+                Some(SubState::CornerKick)
+                    if last_ball_state.ball_in_field.x().is_sign_negative() =>
+                {
+                    Some(Team::Opponent)
+                }
+                Some(SubState::GoalKick)
+                    if last_ball_state.ball_in_field.x().is_sign_positive() =>
+                {
+                    Some(Team::Opponent)
+                }
+                Some(SubState::GoalKick)
+                    if last_ball_state.ball_in_field.x().is_sign_negative() =>
+                {
+                    Some(Team::Hulks)
+                }
+                _ => None,
+            }
+        };
+        Ok(MainOutputs {
+            filtered_kicking_team: filtered_kicking_team.into(),
+        })
+    }
+}

crates/control/src/lib.rs

@@ -17,6 +17,7 @@ pub mod game_controller_state_filter;
 pub mod ground_contact_detector;
 pub mod ground_provider;
 pub mod kick_selector;
+pub mod kicking_team_filter;
 pub mod kinematics_provider;
 pub mod led_status;
 pub mod localization;

crates/hulk_manifest/src/lib.rs

@@ -59,6 +59,7 @@ pub fn collect_hulk_cyclers() -> Result<Cyclers, Error> {
                     "control::ground_contact_detector",
                     "control::ground_provider",
                     "control::kick_selector",
+                    "control::kicking_team_filter",
                     "control::kinematics_provider",
                     "control::led_status",
                     "control::localization",