diff --git a/crates/bifrost/src/serialization/codec.rs b/crates/bifrost/src/serialization/codec.rs index 6c29df7c7..0692b2c5e 100644 --- a/crates/bifrost/src/serialization/codec.rs +++ b/crates/bifrost/src/serialization/codec.rs @@ -404,6 +404,44 @@ where } } +impl Encode for Option +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 Decode for Option +where + T: Decode, +{ + fn decode(mut read: impl Read) -> Result + 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 Encode for Vec where T: Encode, diff --git a/crates/bifrost/src/serialization/nalgebra.rs b/crates/bifrost/src/serialization/nalgebra.rs index d49cf3ec4..497d1f156 100644 --- a/crates/bifrost/src/serialization/nalgebra.rs +++ b/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 Encode for OPoint +where + T: Scalar + Encode, + D: DimName, + DefaultAllocator: Allocator, +{ + fn encode(&self, mut write: impl Write) -> Result<()> { + self.coords.encode(&mut write) + } + + fn encode_len(&self) -> usize { + self.coords.encode_len() + } +} + +impl Decode for OPoint +where + T: Scalar + Decode, + D: DimName, + DefaultAllocator: Allocator, + >::Buffer: Default, +{ + fn decode(mut read: impl Read) -> Result + where + Self: Sized, + { + Ok(Matrix::into(Matrix::decode(&mut read)?)) + } +} + #[cfg(test)] mod tests { use super::*; diff --git a/yggdrasil/src/behavior/roles/striker.rs b/yggdrasil/src/behavior/roles/striker.rs index ec6e36df1..f69dce4a6 100644 --- a/yggdrasil/src/behavior/roles/striker.rs +++ b/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) { 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, layout_config: Res, + detected_ball_position: Res, ball_tracker: Res, mut nao_manager: ResMut, lost_ball_timer: Option>, time: Res