diff --git a/Cargo.lock b/Cargo.lock index 696c947a..e169e1c1 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -1266,6 +1266,7 @@ name = "control" version = "0.1.0" dependencies = [ "approx", + "bifrost", "color-eyre", "context_attribute", "filtering", @@ -1278,6 +1279,7 @@ dependencies = [ "nalgebra", "ordered-float", "projection", + "rand", "serde", "serde_json", "serialize_hierarchy", @@ -4476,6 +4478,7 @@ dependencies = [ name = "spl_network" version = "0.1.0" dependencies = [ + "bifrost", "color-eyre", "constants", "context_attribute", diff --git a/crates/control/Cargo.toml b/crates/control/Cargo.toml index 44dceb81..d155b939 100644 --- a/crates/control/Cargo.toml +++ b/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 } diff --git a/crates/control/src/lib.rs b/crates/control/src/lib.rs index f913f037..adc792be 100644 --- a/crates/control/src/lib.rs +++ b/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; diff --git a/crates/control/src/referee.rs b/crates/control/src/referee.rs new file mode 100644 index 00000000..3f14e860 --- /dev/null +++ b/crates/control/src/referee.rs @@ -0,0 +1,80 @@ +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, +} + +#[context] +pub struct CreationContext { + pub player_number: Parameter, +} + +#[context] +pub struct CycleContext { + pub filtered_whistle: Input, + pub hardware: HardwareInterface, + pub cycle_time: Input, + pub player_number: Parameter, + pub robot_to_field: Input>, "robot_to_field?">, +} + +impl Referee { + pub fn new(_context: CreationContext) -> Result { + Ok(Self { + last_heard_timestamp: None, + }) + } + + pub fn cycle(&mut self, context: CycleContext) -> Result<()> { + if context.filtered_whistle.started_this_cycle { + if self.last_heard_timestamp.is_none() { + self.send_referee_message(&context, 0.0)?; + } 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, + 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(()) + } +} diff --git a/crates/spl_network/Cargo.toml b/crates/spl_network/Cargo.toml index 909a0b55..3ac3b585 100644 --- a/crates/spl_network/Cargo.toml +++ b/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 } \ No newline at end of file diff --git a/crates/spl_network/src/endpoint.rs b/crates/spl_network/src/endpoint.rs index 66258da3..7a341c9e 100644 --- a/crates/spl_network/src/endpoint.rs +++ b/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 = Vec::new(); + message.encode(&mut data).unwrap(); + // let message: Vec = 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:?}") + } + } }; } } diff --git a/crates/spl_network_messages/src/lib.rs b/crates/spl_network_messages/src/lib.rs index 0be2d42d..41923f1d 100644 --- a/crates/spl_network_messages/src/lib.rs +++ b/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; diff --git a/crates/spl_network_messages/src/referee_return_message.rs b/crates/spl_network_messages/src/referee_return_message.rs new file mode 100644 index 00000000..5667448d --- /dev/null +++ b/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, + } + } +} diff --git a/crates/types/src/messages.rs b/crates/types/src/messages.rs index c3ba5a53..42bb7e79 100644 --- a/crates/types/src/messages.rs +++ b/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 { diff --git a/etc/configuration/head.P0000074A04S8C700011.json b/etc/configuration/head.P0000074A04S8C700011.json index 66dd9543..2d825eeb 100644 --- a/etc/configuration/head.P0000074A04S8C700011.json +++ b/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" } diff --git a/etc/configuration/head.P0000074A04S91300023.json b/etc/configuration/head.P0000074A04S91300023.json index 5a2a2cd3..75451fcb 100644 --- a/etc/configuration/head.P0000074A04S91300023.json +++ b/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 } } diff --git a/etc/configuration/head.P0000074A06S9BH00015.json b/etc/configuration/head.P0000074A06S9BH00015.json index 3aa9eae1..c7bd6c71 100644 --- a/etc/configuration/head.P0000074A06S9BH00015.json +++ b/etc/configuration/head.P0000074A06S9BH00015.json @@ -7,5 +7,6 @@ 1.3600000143051147 ] } - } + }, + "player_number": "Two" }