Correctly use non-default last_ball_state

Author: ThagonDuarte

crates/bevyhavior_simulator/src/bin/corner_kicks.rs

@@ -1,13 +1,16 @@
 use bevy::prelude::*;
 
+use linear_algebra::{point, vector};
 use scenario::scenario;
 use spl_network_messages::{GameState, PlayerNumber, SubState, Team};
 
 use bevyhavior_simulator::{
+    ball::BallResource,
     game_controller::{GameController, GameControllerCommand},
     robot::Robot,
     time::{Ticks, TicksTime},
 };
+use types::ball_position::SimulatorBallState;
 
 #[scenario]
 fn corner_kicks(app: &mut App) {
@@ -38,13 +41,22 @@ fn update(
     mut game_controller_commands: EventWriter<GameControllerCommand>,
     time: Res<Time<Ticks>>,
     mut exit: EventWriter<AppExit>,
+    mut ball: ResMut<BallResource>,
 ) {
     if time.ticks() == 3000 {
         game_controller_commands.send(GameControllerCommand::SetSubState(
             Some(SubState::CornerKick),
             Team::Hulks,
         ));
     }
+
+    if time.ticks() == 4500 {
+        ball.state = Some(SimulatorBallState {
+            position: point!(-2.25, 1.0),
+            velocity: vector![-6.0, 2.0],
+        });
+    }
+
     if time.ticks() == 5000 {
         game_controller_commands.send(GameControllerCommand::SetSubState(
             Some(SubState::CornerKick),

crates/control/src/kicking_team_filter.rs

@@ -1,26 +1,37 @@
+use std::time::{Duration, SystemTime};
+
 use color_eyre::Result;
 use serde::{Deserialize, Serialize};
 
 use context_attribute::context;
-use framework::MainOutput;
+use framework::{AdditionalOutput, MainOutput};
 use spl_network_messages::{SubState, Team};
 use types::{
     filtered_whistle::FilteredWhistle, game_controller_state::GameControllerState,
     world_state::BallState,
 };
 
 #[derive(Deserialize, Serialize)]
-pub struct KickingTeamFilter {}
+pub struct KickingTeamFilter {
+    time_last_ball_state_became_default: SystemTime,
+    last_ball_state: BallState,
+    last_non_default_ball_state: Option<BallState>,
+}
 
 #[context]
 pub struct CreationContext {}
 
 #[context]
 pub struct CycleContext {
-    last_ball_state: CyclerState<BallState, "last_ball_position">,
+    last_ball_state: CyclerState<BallState, "last_ball_state">,
     game_controller_state: RequiredInput<Option<GameControllerState>, "game_controller_state?">,
     detected_free_kick_kicking_team: Input<Option<Team>, "detected_free_kick_kicking_team?">,
     filtered_whistle: Input<FilteredWhistle, "filtered_whistle">,
+
+    duration_to_keep_non_default_last_ball_state:
+        Parameter<Duration, "kicking_team_filter.duration_to_keep_non_default_last_ball_state">,
+
+    additonal_last_ball_state: AdditionalOutput<BallState, "last_ball_state">,
 }
 
 #[context]
@@ -30,50 +41,92 @@ pub struct MainOutputs {
 
 impl KickingTeamFilter {
     pub fn new(_context: CreationContext) -> Result<Self> {
-        Ok(KickingTeamFilter {})
+        Ok(KickingTeamFilter {
+            time_last_ball_state_became_default: SystemTime::now(),
+            last_ball_state: Default::default(),
+            last_non_default_ball_state: Default::default(),
+        })
     }
 
-    pub fn cycle(&mut self, context: CycleContext) -> Result<MainOutputs> {
+    pub fn cycle(&mut self, mut context: CycleContext) -> Result<MainOutputs> {
+        context
+            .additonal_last_ball_state
+            .fill_if_subscribed(|| *context.last_ball_state);
+
         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 duration_since_last_non_default_ball_state =
+            SystemTime::now().duration_since(self.time_last_ball_state_became_default)?;
+
+        if *context.last_ball_state == BallState::default()
+            && self.last_ball_state != BallState::default()
+        {
+            self.time_last_ball_state_became_default = SystemTime::now();
+        }
+        self.last_ball_state = *context.last_ball_state;
+
+        if *context.last_ball_state != BallState::default()
+            || *context.last_ball_state == BallState::default()
+                && duration_since_last_non_default_ball_state
+                    >= *context.duration_to_keep_non_default_last_ball_state
+        {
+            self.last_non_default_ball_state = Some(*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 {
+        } else if let Some(last_non_default_ball_state) = self.last_non_default_ball_state {
             match sub_state {
                 Some(SubState::CornerKick)
-                    if last_ball_state.ball_in_field.x().is_sign_positive() =>
+                    if last_non_default_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() =>
+                    if last_non_default_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() =>
+                    if last_non_default_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() =>
+                    if last_non_default_ball_state
+                        .ball_in_field
+                        .x()
+                        .is_sign_negative() =>
                 {
                     Some(Team::Hulks)
                 }
                 None => match (
                     context.filtered_whistle.is_detected,
-                    last_ball_state.ball_in_field.x().is_sign_positive(),
+                    last_non_default_ball_state
+                        .ball_in_field
+                        .x()
+                        .is_sign_positive(),
                 ) {
                     (true, true) => Some(Team::Opponent),
                     (true, false) => Some(Team::Hulks),
                     _ => None,
                 },
                 _ => None,
             }
+        } else {
+            None
         };
         Ok(MainOutputs {
             filtered_kicking_team: filtered_kicking_team.into(),

crates/types/src/penalty_shot_direction.rs

@@ -11,6 +11,7 @@ use serde::{Deserialize, Serialize};
     PathSerialize,
     PathDeserialize,
     PathIntrospect,
+    PartialEq,
 )]
 pub enum PenaltyShotDirection {
     #[default]

crates/types/src/world_state.rs

@@ -32,7 +32,15 @@ pub struct WorldState {
 }
 
 #[derive(
-    Clone, Copy, Debug, Serialize, Deserialize, PathSerialize, PathDeserialize, PathIntrospect,
+    Clone,
+    Copy,
+    Debug,
+    Serialize,
+    Deserialize,
+    PathSerialize,
+    PathDeserialize,
+    PathIntrospect,
+    PartialEq,
 )]
 pub struct BallState {
     pub ball_in_ground: Point2<Ground>,

etc/parameters/default.json

@@ -641,6 +641,9 @@
       }
     ]
   },
+  "kicking_team_filter": {
+    "duration_to_keep_non_default_last_ball_state": { "nanos": 3000000000, "secs": 3 }
+  },
   "localization": {
     "circle_measurement_noise": [1000.0, 1000.0],
     "gradient_convergence_threshold": 1e-2,