Port team ball

Cargo.toml

@@ -65,7 +65,7 @@ members = [
   "crates/nodes/segment_filter",
   "crates/nodes/stereo_visual_odometry",
   "crates/nodes/support_foot_estimator",
-  "crates/nodes/team_ball_receiver",
+  "crates/nodes/team_ball_filter",
   "crates/nodes/time_to_reach_kick_position",
   "crates/nodes/trigger",
   "crates/nodes/visual_kick_ball_selector",
@@ -279,7 +279,8 @@ splines = { version = "=4.2.0", features = ["serde"] }
 stereo_visual_odometry = { path = "crates/nodes/stereo_visual_odometry" }
 support_foot_estimator = { path = "crates/nodes/support_foot_estimator" }
 syn = { version = "2.0.98", features = ["extra-traits", "full"] }
-team_ball_receiver = { path = "crates/nodes/team_ball_receiver" }
+systemd = "0.10.0"
+team_ball_filter = { path = "crates/nodes/team_ball_filter" }
 tempfile = "3.17.0"
 thiserror = "2.0.11"
 time_to_reach_kick_position = { path = "crates/nodes/time_to_reach_kick_position" }

crates/hulk_ros_z/Cargo.toml

@@ -56,7 +56,7 @@ search_suggestor = { workspace = true }
 segment_filter = { workspace = true }
 stereo_visual_odometry = { workspace = true, features = ["ort-dynamic"] }
 support_foot_estimator = { workspace = true }
-team_ball_receiver = { workspace = true }
+team_ball_filter = { workspace = true }
 time_to_reach_kick_position = { workspace = true }
 tokio = { workspace = true, features = ["rt-multi-thread", "macros", "signal"] }
 tracing-subscriber = { workspace = true, features = ["env-filter"] }

crates/hulk_ros_z/src/main.rs

@@ -176,7 +176,7 @@ async fn spawn_all(ctx: Arc<Context>, log_path: Option<PathBuf>) -> Result<Runni
     join_set.spawn(segment_filter::run_boxed(ctx.clone()));
     join_set.spawn(stereo_visual_odometry::run_boxed(ctx.clone()));
     join_set.spawn(support_foot_estimator::run_boxed(ctx.clone()));
-    join_set.spawn(team_ball_receiver::run_boxed(ctx.clone()));
+    join_set.spawn(team_ball_filter::run_boxed(ctx.clone()));
     join_set.spawn(time_to_reach_kick_position::run_boxed(ctx.clone()));
     join_set.spawn(trigger::run_boxed(ctx.clone()));
     join_set.spawn(whistle_detection::run_boxed(ctx.clone()));

crates/nodes/ball_state_composer/src/lib.rs

@@ -42,7 +42,7 @@ async fn run(ctx: Arc<Context>) -> Result<()> {
         .build()
         .await?;
     let team_ball_sub = node
-        .subscriber::<BallPosition<Field>>("team_ball")
+        .subscriber::<Option<BallPosition<Field>>>("team_ball")
         .build()
         .await?;
     let primary_state_cache = node
@@ -108,7 +108,11 @@ async fn run(ctx: Arc<Context>) -> Result<()> {
                 });
             }
             received_team_ball = team_ball_sub.recv() => {
-                let team_ball = received_team_ball?;
+                let Some(team_ball) = received_team_ball? else {
+                    ball_state_pub.publish(&None).await?;
+                    last_ball_state = None;
+                    continue;
+                };
 
                 let Some(ground_to_field) = ground_to_field_cache.get_latest() else {
                     continue;

crates/nodes/team_ball_receiver/Cargo.toml → crates/nodes/team_ball_filter/Cargo.toml

@@ -1,5 +1,5 @@
 [package]
-name = "team_ball_receiver"
+name = "team_ball_filter"
 version.workspace = true
 edition.workspace = true
 rust-version.workspace = true
@@ -9,6 +9,8 @@ homepage.workspace = true
 [dependencies]
 color-eyre = { workspace = true }
 coordinate_systems = { workspace = true }
+hsl_network_messages = { workspace = true }
 ros-z = { workspace = true }
 serde = { workspace = true, features = ["derive"] }
+tokio = { workspace = true }
 types = { workspace = true }

crates/nodes/team_ball_filter/src/lib.rs

@@ -0,0 +1,119 @@
+use std::{boxed::Box, future::Future, pin::Pin};
+use std::{sync::Arc, time::Duration};
+
+use color_eyre::Result;
+use hsl_network_messages::{GamePhase, SubState};
+use serde::{Deserialize, Serialize};
+
+use coordinate_systems::Field;
+use ros_z::prelude::*;
+use types::{
+    ball_position::BallPosition, filtered_game_controller_state::FilteredGameControllerState,
+    players::Players, time_wrapper::TimeWrapper, world_state::PlayerState,
+};
+
+#[derive(Debug, Clone, Serialize, Deserialize, Message)]
+#[serde(deny_unknown_fields)]
+pub struct Parameters {
+    pub maximum_age: Duration,
+}
+
+pub fn run_boxed(ctx: Arc<Context>) -> Pin<Box<dyn Future<Output = Result<()>> + Send>> {
+    Box::pin(run(ctx))
+}
+
+async fn run(ctx: Arc<Context>) -> Result<()> {
+    let node = ctx.create_node("team_ball_filter").build().await?;
+
+    let parameters = node.bind_parameter_as::<Parameters>("team_ball_filter")?;
+    let filtered_game_controller_state_sub = node
+        .subscriber::<FilteredGameControllerState>("filtered_game_controller_state")
+        .build()
+        .await?;
+    let player_states_sub = node
+        .subscriber::<Players<Option<TimeWrapper<PlayerState>>>>("player_states")
+        .build()
+        .await?;
+    let team_balls_pub = node
+        .publisher::<Players<Option<BallPosition<Field>>>>("team_balls")
+        .build()
+        .await?;
+    let team_ball_pub = node
+        .publisher::<Option<BallPosition<Field>>>("team_ball")
+        .build()
+        .await?;
+
+    let mut team_ball_filter = TeamBallFilter::default();
+    let mut filtered_game_controller_state = None;
+
+    loop {
+        let parameters_snapshot = parameters.snapshot();
+        let parameters = parameters_snapshot.typed();
+
+        tokio::select! {
+            received_player_states = player_states_sub.recv() => {
+                team_ball_filter.update_received_balls(received_player_states?);
+            }
+            received_filtered_game_controller_state = filtered_game_controller_state_sub.recv() => {
+                filtered_game_controller_state = Some(received_filtered_game_controller_state?);
+            }
+        }
+
+        let now = node.clock().now();
+        if filtered_game_controller_state
+            .as_ref()
+            .is_some_and(is_penalty_phase_or_sub_state)
+        {
+            team_ball_pub.publish(&None).await?;
+            continue;
+        }
+
+        let team_ball = team_ball_filter.get_best_received_ball(now, parameters.maximum_age);
+        team_balls_pub
+            .publish_if_subscribed(|| async {
+                team_ball_filter.filtered_received_balls(now, parameters.maximum_age)
+            })
+            .await?;
+        team_ball_pub.publish(&team_ball).await?;
+    }
+}
+
+#[derive(Default)]
+struct TeamBallFilter {
+    received_balls: Players<Option<BallPosition<Field>>>,
+}
+
+impl TeamBallFilter {
+    fn update_received_balls(&mut self, player_states: Players<Option<TimeWrapper<PlayerState>>>) {
+        self.received_balls = player_states
+            .map(|player_state| player_state.and_then(|state| state.inner.ball_position));
+    }
+
+    fn filtered_received_balls(
+        &self,
+        now: ros_z::time::Time,
+        maximum_age: Duration,
+    ) -> Players<Option<BallPosition<Field>>> {
+        self.received_balls
+            .map(|ball| ball.filter(|ball| ball.age_at(now).is_some_and(|age| age < maximum_age)))
+    }
+
+    fn get_best_received_ball(
+        &self,
+        now: ros_z::time::Time,
+        maximum_age: Duration,
+    ) -> Option<BallPosition<Field>> {
+        self.received_balls
+            .iter()
+            .filter_map(|(_player_number, ball)| *ball)
+            .max_by_key(|ball| ball.last_seen)
+            .filter(|ball| ball.age_at(now).is_some_and(|age| age < maximum_age))
+    }
+}
+
+fn is_penalty_phase_or_sub_state(game_controller_state: &FilteredGameControllerState) -> bool {
+    matches!(
+        game_controller_state.game_phase,
+        GamePhase::PenaltyShootout { .. }
+    ) || game_controller_state.sub_state == Some(SubState::PenaltyKick)
+}

etc/parameters/base/team_ball_filter.json5

@@ -0,0 +1,6 @@
+{
+  maximum_age: {
+    nanos: 500000000,
+    secs: 4,
+  },
+}