Introduce ball communication

crates/bifrost/src/serialization/codec.rs

@@ -404,6 +404,44 @@ where
     }
 }
 
+impl<T> Encode for Option<T>
+where
+    T: Encode,
+{
+    fn encode(&self, mut write: impl Write) -> Result<()> {
+        match self {
+            Some(inner) => {
+                write.write_u8(1)?;
+                inner.encode(write)
+            }
+            None => Ok(write.write_u8(0)?),
+        }
+    }
+
+    fn encode_len(&self) -> usize {
+        match self {
+            Some(inner) => inner.encode_len() + 1,
+            None => 1,
+        }
+    }
+}
+
+impl<T> Decode for Option<T>
+where
+    T: Decode,
+{
+    fn decode(mut read: impl Read) -> Result<Self>
+    where
+        Self: Sized,
+    {
+        match read.read_u8()? {
+            0 => Ok(None),
+            1 => Ok(Some(T::decode(read)?)),
+            n => Err(Error::InvalidVariantDiscriminant(n as usize, "Option")),
+        }
+    }
+}
+
 impl<T> Encode for Vec<T>
 where
     T: Encode,

crates/bifrost/src/serialization/nalgebra.rs

@@ -2,7 +2,10 @@
 
 use std::io::{Read, Write};
 
-use nalgebra::{Dim, Matrix, Scalar, Storage, StorageMut};
+use nalgebra::{
+    Dim, DimName, Matrix, OPoint, Scalar, Storage, StorageMut, allocator::Allocator,
+    base::default_allocator::DefaultAllocator,
+};
 
 use super::{Decode, Encode};
 
@@ -56,6 +59,36 @@ where
     }
 }
 
+impl<T, D> Encode for OPoint<T, D>
+where
+    T: Scalar + Encode,
+    D: DimName,
+    DefaultAllocator: Allocator<D>,
+{
+    fn encode(&self, mut write: impl Write) -> Result<()> {
+        self.coords.encode(&mut write)
+    }
+
+    fn encode_len(&self) -> usize {
+        self.coords.encode_len()
+    }
+}
+
+impl<T, D> Decode for OPoint<T, D>
+where
+    T: Scalar + Decode,
+    D: DimName,
+    DefaultAllocator: Allocator<D>,
+    <DefaultAllocator as Allocator<D>>::Buffer<T>: Default,
+{
+    fn decode(mut read: impl Read) -> Result<Self>
+    where
+        Self: Sized,
+    {
+        Ok(Matrix::into(Matrix::decode(&mut read)?))
+    }
+}
+
 #[cfg(test)]
 mod tests {
     use super::*;

yggdrasil/src/behavior/roles/striker.rs

@@ -19,7 +19,7 @@ use crate::{
     localization::RobotPose,
     motion::{step_planner::Target, walking_engine::step::Step},
     nao::{NaoManager, Priority},
-    vision::ball_detection::ball_tracker::BallTracker,
+    vision::ball_detection::{TeamBallPosition, ball_tracker::BallTracker},
 };
 
 use std::time::Duration;
@@ -98,16 +98,18 @@ fn reset_striker_role(mut nao_manager: ResMut<NaoManager>) {
     nao_manager.set_right_eye_led(RightEye::fill(color::f32::EMPTY), Priority::default());
 }
 
+#[allow(clippy::too_many_arguments)]
 pub fn striker_role(
     mut commands: Commands,
     pose: Res<RobotPose>,
     layout_config: Res<LayoutConfig>,
+    detected_ball_position: Res<TeamBallPosition>,
     ball_tracker: Res<BallTracker>,
     mut nao_manager: ResMut<NaoManager>,
     lost_ball_timer: Option<ResMut<LostBallSearchTimer>>,
     time: Res<Time>,
 ) {
-    let Some(relative_ball) = ball_tracker.stationary_ball() else {
+    let Some(relative_ball) = detected_ball_position.0 else {
         if let Some(mut timer) = lost_ball_timer {
             timer.timer.tick(time.delta()); // <- tick the timer
 
@@ -129,8 +131,6 @@ pub fn striker_role(
         }
         return;
     };
-    let absolute_ball: nalgebra::OPoint<f32, nalgebra::Const<2>> =
-        pose.robot_to_world(&relative_ball);
 
     if ball_tracker.timestamp.elapsed().as_secs_f32() > 0.5 {
         if lost_ball_timer.is_none() {
@@ -143,6 +143,7 @@ pub fn striker_role(
         commands.remove_resource::<LostBallSearchTimer>();
     }
 
+    let absolute_ball = pose.robot_to_world(&relative_ball);
     let ball_angle = pose.angle_to(&absolute_ball);
     let ball_distance = relative_ball.coords.norm();
     let ball_target: nalgebra::OPoint<f32, nalgebra::Const<3>> =
@@ -210,12 +211,12 @@ pub fn striker_role(
 //TODO: Make this a separate stand-alone behavior
 fn set_play(
     mut commands: Commands,
-    ball_tracker: Res<BallTracker>,
+    detected_ball_position: Res<TeamBallPosition>,
     pose: Res<RobotPose>,
     behavior_state: Res<State<BehaviorState>>,
     walk: Option<Res<Walk>>,
 ) {
-    let Some(relative_ball) = ball_tracker.stationary_ball() else {
+    let Some(relative_ball) = detected_ball_position.0 else {
         return;
     };
     let absolute_ball = pose.robot_to_world(&relative_ball);

yggdrasil/src/communication/team.rs

@@ -4,6 +4,7 @@ use std::time::Duration;
 
 use bevy::prelude::{App, *};
 use miette::IntoDiagnostic;
+use nalgebra as na;
 use tracing::{debug, warn};
 
 use crate::core::config::showtime::ShowtimeConfig;
@@ -189,6 +190,7 @@ pub enum TeamMessage {
     Ping,
     Pong,
     DetectedWhistle,
+    DetectedBall(Option<na::Point2<f32>>),
     RecognizedRefereePose(RefereePose),
 }
 

yggdrasil/src/vision/ball_detection/communication.rs

@@ -0,0 +1,121 @@
+use bevy::prelude::*;
+use nalgebra::{self as na, Point2};
+
+use crate::{
+    communication::{TeamCommunication, TeamMessage},
+    core::debug::DebugContext,
+    localization::RobotPose,
+    nao::Cycle,
+};
+
+// Import camera proposals
+use super::ball_tracker::BallTracker;
+
+// Constant for the minimum acceptable change
+const MIN_CHANGE: f32 = 0.1;
+
+pub struct CommunicatedBallsPlugin;
+
+#[derive(Resource, Default, Debug)]
+pub struct LastReceivedBall {
+    pub position: Option<Point2<f32>>,
+}
+
+impl Plugin for CommunicatedBallsPlugin {
+    fn build(&self, app: &mut App) {
+        app.init_resource::<TeamBallPosition>()
+            .init_resource::<CommunicatedBalls>()
+            .init_resource::<LastReceivedBall>()
+            .add_systems(Update, communicate_balls_system);
+    }
+}
+
+#[derive(Resource, Default, Debug)]
+pub struct TeamBallPosition(pub Option<Point2<f32>>);
+
+#[derive(Resource, Debug, Default)]
+pub struct CommunicatedBalls {
+    /// For keeping track what position we've sent out.
+    sent: Option<na::Point2<f32>>,
+}
+
+impl CommunicatedBalls {
+    /// Check it the position has changed enough from last frame.
+    fn change_enough(&mut self, ball: Option<&na::Point2<f32>>) -> bool {
+        match (ball, &self.sent) {
+            (None, None) => false,
+            (None, Some(_)) => true,
+            (Some(_), None) => true,
+            (Some(old), Some(new)) => na::distance(old, new) > MIN_CHANGE,
+        }
+    }
+
+    /// Send your ball position (even if it's None) as a message.
+    fn send_message(&mut self, ball_position: Option<na::Point2<f32>>, tc: &mut TeamCommunication) {
+        tc.outbound_mut()
+            .update_or_push(TeamMessage::DetectedBall(ball_position))
+            .expect("Unable to encode detected ball");
+        self.sent = ball_position;
+    }
+
+    /// Receive messages.
+    // 2.A.a. If no other robot are detecting a ball, we return the same None we had
+    // 2.A.b. If there are other robots detecting a ball, we take one from theirs as our own.
+    fn receive_messages(
+        comms: &mut TeamCommunication,
+        pose: &RobotPose,
+    ) -> Option<na::Point2<f32>> {
+        let mut received_ball = None;
+
+        while let Some((_, _, ball)) = comms.inbound_mut().take_map(|_, _, what| match what {
+            TeamMessage::DetectedBall(ball) => Some(*ball),
+            _ => None,
+        }) {
+            received_ball = received_ball.or(ball);
+        }
+        // If we received a ball, transform it from world coordinates to robot coordinates
+        received_ball.map(|ball| pose.world_to_robot(&ball))
+    }
+}
+
+#[allow(clippy::too_many_arguments)]
+fn communicate_balls_system(
+    mut communicated_balls: ResMut<CommunicatedBalls>,
+    mut tc: ResMut<TeamCommunication>,
+    ball_tracker: Res<BallTracker>,
+    mut team_ball_position: ResMut<TeamBallPosition>,
+    pose: Res<RobotPose>,
+    mut last_received: ResMut<LastReceivedBall>,
+    ctx: DebugContext,
+    cycle: Res<Cycle>,
+) {
+    let optional_ball_position = ball_tracker.stationary_ball();
+
+    // 1. Check if it has changed enough and if so, we send a message.
+    // let optional_ball_position = ball_position.map(|ball_position| ball_position.0);
+    if communicated_balls.change_enough(optional_ball_position.as_ref()) {
+        let transformed_position = optional_ball_position.map(|pos| pose.robot_to_world(&pos));
+        communicated_balls.send_message(transformed_position, &mut tc);
+    }
+
+    if let Some(new_pos) = CommunicatedBalls::receive_messages(&mut tc, &pose) {
+        last_received.position = Some(new_pos);
+    } else {
+        last_received.position = None;
+    }
+
+    team_ball_position.0 = optional_ball_position.or_else(|| last_received.position);
+
+    if let Some(pos) = team_ball_position.0 {
+        let global = pose.robot_to_world(&pos);
+        ctx.log_with_cycle(
+            "/team_ball",
+            *cycle,
+            &rerun::Points3D::new([(global.x, global.y, 0.01)])
+                .with_radii([0.1])
+                .with_labels(["team_ball"]),
+        );
+    } else {
+        ctx.log_with_cycle("/team_ball", *cycle, &rerun::Clear::recursive());
+    }
+}

yggdrasil/src/vision/ball_detection/mod.rs

@@ -2,13 +2,16 @@
 
 pub mod ball_tracker;
 pub mod classifier;
+mod communication;
 pub mod proposal;
 
 use std::{sync::Arc, time::Duration};
 
 pub use ball_tracker::BallHypothesis;
 use ball_tracker::BallTracker;
 use bevy::prelude::*;
+use communication::CommunicatedBallsPlugin;
+pub use communication::TeamBallPosition;
 use heimdall::{Bottom, CameraLocation, Top};
 use nidhogg::types::{FillExt, LeftEye, color};
 use proposal::BallProposalConfigs;
@@ -31,6 +34,7 @@ pub struct BallDetectionPlugin;
 
 impl Plugin for BallDetectionPlugin {
     fn build(&self, app: &mut App) {
+        app.add_plugins(CommunicatedBallsPlugin);
         app.init_config::<BallDetectionConfig>();
         app.add_plugins((
             proposal::BallProposalPlugin::<Top>::default(),
@@ -144,6 +148,7 @@ fn log_3d_balls(
 
     if let BallHypothesis::Stationary(max_variance) = state {
         let pos = robot_pose.robot_to_world(&ball_tracker.state());
+
         if last_logged.is_none_or(|last_logged_cycle| last_ball_tracker_update > last_logged_cycle)
         {
             *last_logged = Some(last_ball_tracker_update);