Visual referee messages sending with RNG handsignal

crates/control/Cargo.toml

@@ -7,6 +7,7 @@ homepage = "https://github.com/hulks/hulk"
 
 [dependencies]
 approx = { workspace = true }
+bifrost = {workspace = true}
 color-eyre = { workspace = true }
 context_attribute = { workspace = true }
 itertools = { workspace = true }
@@ -19,6 +20,7 @@ motionfile = { workspace = true }
 nalgebra = { workspace = true }
 ordered-float = { workspace = true }
 projection = { workspace = true }
+rand = { workspace = true }
 serde = { workspace = true }
 serde_json = { workspace = true }
 serialize_hierarchy = { workspace = true }

crates/control/src/lib.rs

@@ -23,6 +23,7 @@ pub mod orientation_filter;
 pub mod path_planner;
 pub mod penalty_shot_direction_estimation;
 pub mod primary_state_filter;
+pub mod referee;
 pub mod role_assignment;
 pub mod rule_obstacle_composer;
 pub mod sensor_data_receiver;

crates/control/src/referee.rs

@@ -0,0 +1,83 @@
+use color_eyre::{eyre::WrapErr, Result};
+use context_attribute::context;
+use nalgebra::Isometry2;
+use rand::Rng;
+use spl_network_messages::{PlayerNumber, RefereeMessage};
+use std::time::{SystemTime};
+use types::{hardware::Interface, messages::OutgoingMessage, CycleTime, FilteredWhistle};
+
+pub struct Referee {
+    last_heard_timestamp: Option<SystemTime>,
+    first: bool,
+}
+
+#[context]
+pub struct CreationContext {
+    pub player_number: Parameter<PlayerNumber, "player_number">,
+}
+
+#[context]
+pub struct CycleContext {
+    pub filtered_whistle: Input<FilteredWhistle, "filtered_whistle">,
+    pub hardware: HardwareInterface,
+    pub cycle_time: Input<CycleTime, "cycle_time">,
+    pub player_number: Parameter<PlayerNumber, "player_number">,
+    pub robot_to_field: Input<Option<Isometry2<f32>>, "robot_to_field?">,
+}
+
+impl Referee {
+    pub fn new(_context: CreationContext) -> Result<Self> {
+        Ok(Self {
+            last_heard_timestamp: None,
+            first: true,
+        })
+    }
+
+    pub fn cycle(&mut self, context: CycleContext<impl Interface>) -> Result<()> {
+        if context.filtered_whistle.started_this_cycle {
+            if self.first {
+                self.send_referee_message(&context, 0.0)?;
+                self.first = false;
+            } else if let Some(cycle_time) = self.last_heard_timestamp {
+                match context.cycle_time.start_time.duration_since(cycle_time) {
+                    Ok(duration) => {
+                        if duration.as_secs() > 20 {
+                            self.send_referee_message(&context, duration.as_secs_f32())?;
+                        }
+                    }
+                    Err(_err) => {}
+                }
+            }
+        }
+
+        Ok(())
+    }
+
+    fn send_referee_message(
+        &mut self,
+        context: &CycleContext<impl Interface>,
+        duration: f32,
+    ) -> Result<()> {
+        let mut rng_gen = rand::thread_rng();
+        self.last_heard_timestamp = Some(SystemTime::now());
+        let handsignal: u8 = rng_gen.gen_range(1..=16);
+
+        context
+            .hardware
+            .write_to_network(OutgoingMessage::RefereeReturnData(RefereeMessage {
+                header: [82, 71, 114, 116],
+                version: 255,
+                player_num: *context.player_number as u8,
+                team_num: 8,
+                fallen: handsignal,
+                pose: [0.0, 0.0, 0.0],
+                ball_age: duration,
+                ball: [0.0, 0.0],
+            }))
+            .wrap_err("failed to write RefereeMessage to hardware")?;
+
+        println!("sent referee handsignal message");
+
+        Ok(())
+    }
+}

crates/spl_network/Cargo.toml

@@ -6,6 +6,7 @@ license = "GPL-3.0-only"
 homepage = "https://github.com/hulks/hulk"
 
 [dependencies]
+bifrost = { workspace = true }
 color-eyre = { workspace = true }
 constants = { workspace = true }
 context_attribute = { workspace = true }
@@ -15,4 +16,4 @@ serde = { workspace = true }
 thiserror = { workspace = true }
 tokio = { workspace = true }
 types = { workspace = true }
-spl_network_messages = { workspace = true }
+spl_network_messages = { workspace = true }

crates/spl_network/src/endpoint.rs

@@ -3,6 +3,7 @@ use std::{
     net::{Ipv4Addr, SocketAddr, SocketAddrV4},
 };
 
+use bifrost::serialization::Encode;
 use log::warn;
 use serde::Deserialize;
 use thiserror::Error;
@@ -122,6 +123,21 @@ impl Endpoint {
                     warn!("Failed to send UDP datagram via SPL socket: {error:?}")
                 }
             }
+            OutgoingMessage::RefereeReturnData(message) => {
+                let mut data: Vec<u8> = Vec::new();
+                message.encode(&mut data).unwrap();
+                // let message: Vec<u8> = message.try_into().expect("Failed to serialize SPL message");
+                if let Err(error) = self
+                    .spl_socket
+                    .send_to(
+                        data.as_slice(),
+                        SocketAddr::new(Ipv4Addr::BROADCAST.into(), SPL_PORT),
+                    )
+                    .await
+                {
+                    warn!("Failed to send UDP datagram via SPL socket: {error:?}")
+                }
+            }
         };
     }
 }

crates/spl_network_messages/src/lib.rs

@@ -2,6 +2,7 @@ mod bindings;
 mod game_controller_return_message;
 mod game_controller_state_conversion;
 mod game_controller_state_message;
+pub mod referee_return_message;
 
 use std::{
     fmt::{self, Display, Formatter},
@@ -17,6 +18,7 @@ pub use game_controller_state_conversion::{
     SubState, Team, TeamColor, TeamState,
 };
 pub use game_controller_state_message::GameControllerStateMessage;
+pub use referee_return_message::RefereeMessage;
 use serialize_hierarchy::SerializeHierarchy;
 
 pub type HulkMessage = GameControllerReturnMessage;

crates/spl_network_messages/src/referee_return_message.rs

@@ -0,0 +1,65 @@
+use bifrost::serialization::Encode;
+use serde::{Deserialize, Serialize};
+
+#[derive(Encode, Debug, Clone, Deserialize, Serialize)]
+pub struct RefereeMessage {
+    /// "RGrt"
+    pub header: [u8; 4],
+
+    /// Has to be set to GAMECONTROLLER_RETURN_STRUCT_VERSION
+    pub version: u8,
+
+    /// Player number starts with 1
+    pub player_num: u8,
+
+    /// Team number
+    pub team_num: u8,
+
+    /// 1 means that the robot is fallen, 0 means that the robot can play
+    pub fallen: u8,
+
+    /// Position and orientation of the robot
+    ///
+    /// coordinates in millimeters
+    /// 0,0 is in center of field
+    /// +ve x-axis points towards the goal we are attempting to score on
+    /// +ve y-axis is 90 degrees counter clockwise from the +ve x-axis
+    /// angle in radians, 0 along the +x axis, increasing counter clockwise
+    pub pose: [f32; 3], // x,y,theta
+
+    /// ball information
+    pub ball_age: f32, // seconds since this robot last saw the ball. -1.f if we haven't seen it
+
+    /// Position of ball relative to the robot
+    ///
+    /// coordinates in millimeters
+    /// 0,0 is in center of the robot
+    /// +ve x-axis points forward from the robot
+    /// +ve y-axis is 90 degrees counter clockwise from the +ve x-axis
+    pub ball: [f32; 2],
+}
+
+impl RefereeMessage {
+    /// Construct a new [`RoboCupGameControlReturnData`] using the specified arguments.
+    pub fn new(
+        header: [u8; 4],
+        version: u8,
+        player_num: u8,
+        team_num: u8,
+        fallen: u8,
+        pose: [f32; 3],
+        ball_age: f32,
+        ball: [f32; 2],
+    ) -> Self {
+        Self {
+            header,
+            version,
+            player_num,
+            team_num,
+            fallen,
+            pose,
+            ball_age,
+            ball,
+        }
+    }
+}

crates/types/src/messages.rs

@@ -1,6 +1,8 @@
 use serde::{Deserialize, Serialize};
 use serialize_hierarchy::SerializeHierarchy;
-use spl_network_messages::{GameControllerReturnMessage, GameControllerStateMessage, HulkMessage};
+use spl_network_messages::{
+    GameControllerReturnMessage, GameControllerStateMessage, HulkMessage, RefereeMessage,
+};
 
 #[derive(Clone, Debug, Deserialize, Serialize, SerializeHierarchy)]
 pub enum IncomingMessage {
@@ -18,6 +20,7 @@ impl Default for IncomingMessage {
 pub enum OutgoingMessage {
     GameController(GameControllerReturnMessage),
     Spl(HulkMessage),
+    RefereeReturnData(RefereeMessage),
 }
 
 impl Default for OutgoingMessage {

etc/configuration/head.P0000074A04S8C700011.json

@@ -2,10 +2,12 @@
   "camera_matrix_parameters": {
     "vision_top": {
       "extrinsic_rotations": [
-        0.30000001192092896, -3.369999885559082, 0.44699013233184814
+        0.30000001192092896,
+        -3.369999885559082,
+        0.44699013233184814
       ]
     }
   },
   "disable_communication_acceptor": true,
-  "player_number": "One"
+  "player_number": "Three"
 }

etc/configuration/head.P0000074A04S91300023.json

@@ -1,13 +1,17 @@
 {
-  "walking_engine": {
-    "base_foot_lift": 0.012500000186264517,
-    "walk_hip_height": 0.16500000655651093
-  },
   "camera_matrix_parameters": {
     "vision_top": {
       "extrinsic_rotations": [
-        1.2699999809265137, -3.359999895095825, 0.2199999988079071
+        1.2699999809265137,
+        -3.359999895095825,
+        0.2199999988079071
       ]
     }
+  },
+  "disable_communication_acceptor": true,
+  "player_number": "Three",
+  "walking_engine": {
+    "base_foot_lift": 0.012500000186264517,
+    "walk_hip_height": 0.16500000655651093
   }
 }

etc/configuration/head.P0000074A06S9BH00015.json

@@ -7,5 +7,6 @@
         1.3600000143051147
       ]
     }
-  }
+  },
+  "player_number": "Two"
 }