From 52218baaa31fc4e1b19c4338e1844d691162d267 Mon Sep 17 00:00:00 2001 From: lex Date: Thu, 6 Jul 2023 13:06:35 +0200 Subject: [PATCH 01/15] rng with robocup-23 --- Cargo.lock | 3 ++ crates/control/Cargo.toml | 3 ++ crates/control/src/lib.rs | 1 + crates/control/src/referee.rs | 73 +++++++++++++++++++++++++++++++++++ 4 files changed, 80 insertions(+) create mode 100644 crates/control/src/referee.rs diff --git a/Cargo.lock b/Cargo.lock index 696c947a..ecae5493 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -1267,9 +1267,11 @@ version = "0.1.0" dependencies = [ "approx", "color-eyre", + "compiled-nn", "context_attribute", "filtering", "framework", + "image", "itertools", "kinematics", "kira", @@ -1278,6 +1280,7 @@ dependencies = [ "nalgebra", "ordered-float", "projection", + "rand", "serde", "serde_json", "serialize_hierarchy", diff --git a/crates/control/Cargo.toml b/crates/control/Cargo.toml index 44dceb81..613303a6 100644 --- a/crates/control/Cargo.toml +++ b/crates/control/Cargo.toml @@ -8,10 +8,12 @@ homepage = "https://github.com/hulks/hulk" [dependencies] approx = { workspace = true } color-eyre = { workspace = true } +compiled-nn = { workspace = true } context_attribute = { workspace = true } itertools = { workspace = true } filtering = { workspace = true } framework = { workspace = true } +image = { workspace = true } kinematics = { workspace = true } kira = { workspace = true } log = { workspace = true } @@ -19,6 +21,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..35ee6635 --- /dev/null +++ b/crates/control/src/referee.rs @@ -0,0 +1,73 @@ +use color_eyre::{eyre::WrapErr, Result}; +use context_attribute::context; +use nalgebra::Isometry2; +use rand::Rng; +use spl_network_messages::{GameControllerReturnMessage, PlayerNumber}; +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 let Some(cycle_time) = self.last_heard_timestamp { + match cycle_time.duration_since(cycle_time) { + Ok(duration) => { + if duration.as_secs() < 20 { + let mut rng_gen = rand::thread_rng(); + let handsignal: u8 = rng_gen.gen_range(1..=16); + self.send_referee_message(&context, handsignal)?; + } + } + Err(_err) => {} + } + } + } + + Ok(()) + } + + fn send_referee_message( + &mut self, + context: &CycleContext, + handsignal: u8, + ) -> Result<()> { + // self.last_transmitted_game_controller_return_message = Some(cycle_start_time); + context + .hardware + .write_to_network(OutgoingMessage::GameController( + GameControllerReturnMessage { + player_number: *context.player_number, + fallen: unsafe { std::mem::transmute(handsignal) }, + robot_to_field: context.robot_to_field.copied().unwrap_or_default(), + ball_position: None, + }, + )) + .wrap_err("failed to write GameControllerReturnMessage to hardware")?; + + Ok(()) + } +} From b0c70723b108947c782a92d978fbc603d863383c Mon Sep 17 00:00:00 2001 From: lex Date: Thu, 6 Jul 2023 13:12:04 +0200 Subject: [PATCH 02/15] rng with robocup-23 --- crates/control/src/referee.rs | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/crates/control/src/referee.rs b/crates/control/src/referee.rs index 35ee6635..88944af2 100644 --- a/crates/control/src/referee.rs +++ b/crates/control/src/referee.rs @@ -8,6 +8,7 @@ use types::{hardware::Interface, messages::OutgoingMessage, CycleTime, FilteredW pub struct Referee { last_heard_timestamp: Option, + sent: bool, } #[context] @@ -28,6 +29,7 @@ impl Referee { pub fn new(_context: CreationContext) -> Result { Ok(Self { last_heard_timestamp: None, + sent: false, }) } @@ -36,10 +38,16 @@ impl Referee { if let Some(cycle_time) = self.last_heard_timestamp { match cycle_time.duration_since(cycle_time) { Ok(duration) => { - if duration.as_secs() < 20 { + if duration.as_secs() < 20 && !self.sent { let mut rng_gen = rand::thread_rng(); let handsignal: u8 = rng_gen.gen_range(1..=16); self.send_referee_message(&context, handsignal)?; + self.sent = true; + return Ok(()); + } + else { + self.sent = false; + self.last_heard_timestamp = None; } } Err(_err) => {} From b361e202f5eebb74ddf5ac26f229cc95cbed14eb Mon Sep 17 00:00:00 2001 From: lex Date: Thu, 6 Jul 2023 13:14:27 +0200 Subject: [PATCH 03/15] rng with robocup-23 --- crates/control/src/referee.rs | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/crates/control/src/referee.rs b/crates/control/src/referee.rs index 88944af2..db17dab7 100644 --- a/crates/control/src/referee.rs +++ b/crates/control/src/referee.rs @@ -38,14 +38,14 @@ impl Referee { if let Some(cycle_time) = self.last_heard_timestamp { match cycle_time.duration_since(cycle_time) { Ok(duration) => { - if duration.as_secs() < 20 && !self.sent { + if duration.as_secs() < 20 && !self.sent{ let mut rng_gen = rand::thread_rng(); let handsignal: u8 = rng_gen.gen_range(1..=16); self.send_referee_message(&context, handsignal)?; self.sent = true; return Ok(()); } - else { + else if duration.as_secs() >= 20{ self.sent = false; self.last_heard_timestamp = None; } From 56ed8e307d7594ab810f088ef796099541f6a497 Mon Sep 17 00:00:00 2001 From: lex Date: Thu, 6 Jul 2023 13:21:52 +0200 Subject: [PATCH 04/15] rng with robocup-23 --- crates/control/src/referee.rs | 2 -- 1 file changed, 2 deletions(-) diff --git a/crates/control/src/referee.rs b/crates/control/src/referee.rs index db17dab7..0c7c0ffc 100644 --- a/crates/control/src/referee.rs +++ b/crates/control/src/referee.rs @@ -43,7 +43,6 @@ impl Referee { let handsignal: u8 = rng_gen.gen_range(1..=16); self.send_referee_message(&context, handsignal)?; self.sent = true; - return Ok(()); } else if duration.as_secs() >= 20{ self.sent = false; @@ -63,7 +62,6 @@ impl Referee { context: &CycleContext, handsignal: u8, ) -> Result<()> { - // self.last_transmitted_game_controller_return_message = Some(cycle_start_time); context .hardware .write_to_network(OutgoingMessage::GameController( From 26d51d43d7028b6688de6586dd0dfe4eed69aa5b Mon Sep 17 00:00:00 2001 From: lex Date: Fri, 7 Jul 2023 12:31:27 +0200 Subject: [PATCH 05/15] messages with bifrost --- Cargo.lock | 2 + crates/control/Cargo.toml | 1 + crates/control/src/referee.rs | 23 ++++--- crates/spl_network/Cargo.toml | 3 +- crates/spl_network/src/endpoint.rs | 16 +++++ crates/spl_network_messages/src/lib.rs | 2 + .../src/referee_return_message.rs | 67 +++++++++++++++++++ crates/types/src/messages.rs | 3 +- 8 files changed, 106 insertions(+), 11 deletions(-) create mode 100644 crates/spl_network_messages/src/referee_return_message.rs diff --git a/Cargo.lock b/Cargo.lock index ecae5493..b1cda295 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -1266,6 +1266,7 @@ name = "control" version = "0.1.0" dependencies = [ "approx", + "bifrost", "color-eyre", "compiled-nn", "context_attribute", @@ -4479,6 +4480,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 613303a6..0dfa556c 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 } compiled-nn = { workspace = true } context_attribute = { workspace = true } diff --git a/crates/control/src/referee.rs b/crates/control/src/referee.rs index 0c7c0ffc..78b37a32 100644 --- a/crates/control/src/referee.rs +++ b/crates/control/src/referee.rs @@ -2,8 +2,8 @@ use color_eyre::{eyre::WrapErr, Result}; use context_attribute::context; use nalgebra::Isometry2; use rand::Rng; -use spl_network_messages::{GameControllerReturnMessage, PlayerNumber}; -use std::time::SystemTime; +use spl_network_messages::{PlayerNumber, RefereeMessage}; +use std::time::{SystemTime, Duration}; use types::{hardware::Interface, messages::OutgoingMessage, CycleTime, FilteredWhistle}; pub struct Referee { @@ -41,7 +41,7 @@ impl Referee { if duration.as_secs() < 20 && !self.sent{ let mut rng_gen = rand::thread_rng(); let handsignal: u8 = rng_gen.gen_range(1..=16); - self.send_referee_message(&context, handsignal)?; + self.send_referee_message(&context, handsignal, duration)?; self.sent = true; } else if duration.as_secs() >= 20{ @@ -61,15 +61,20 @@ impl Referee { &mut self, context: &CycleContext, handsignal: u8, + duration: Duration ) -> Result<()> { context .hardware - .write_to_network(OutgoingMessage::GameController( - GameControllerReturnMessage { - player_number: *context.player_number, - fallen: unsafe { std::mem::transmute(handsignal) }, - robot_to_field: context.robot_to_field.copied().unwrap_or_default(), - ball_position: None, + .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.as_secs_f32(), + ball: [0.0, 0.0], }, )) .wrap_err("failed to write GameControllerReturnMessage to hardware")?; 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..352a51cd 100644 --- a/crates/spl_network/src/endpoint.rs +++ b/crates/spl_network/src/endpoint.rs @@ -8,6 +8,7 @@ use serde::Deserialize; use thiserror::Error; use tokio::{net::UdpSocket, select, sync::Mutex}; use types::messages::{IncomingMessage, OutgoingMessage}; +use bifrost::serialization::Encode; use constants::DNT_TEAM_NUMBER; @@ -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..949cbbd0 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}, @@ -18,6 +19,7 @@ pub use game_controller_state_conversion::{ }; pub use game_controller_state_message::GameControllerStateMessage; use serialize_hierarchy::SerializeHierarchy; +pub use referee_return_message::RefereeMessage; 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..9b5bb280 --- /dev/null +++ b/crates/spl_network_messages/src/referee_return_message.rs @@ -0,0 +1,67 @@ +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, + } + } +} \ No newline at end of file diff --git a/crates/types/src/messages.rs b/crates/types/src/messages.rs index c3ba5a53..f41a1040 100644 --- a/crates/types/src/messages.rs +++ b/crates/types/src/messages.rs @@ -1,6 +1,6 @@ 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 +18,7 @@ impl Default for IncomingMessage { pub enum OutgoingMessage { GameController(GameControllerReturnMessage), Spl(HulkMessage), + RefereeReturnData(RefereeMessage), } impl Default for OutgoingMessage { From 3000c93531a5950a11810581442b32d7207bfc6e Mon Sep 17 00:00:00 2001 From: lex Date: Fri, 7 Jul 2023 17:19:01 +0200 Subject: [PATCH 06/15] log print --- crates/control/src/referee.rs | 1 + 1 file changed, 1 insertion(+) diff --git a/crates/control/src/referee.rs b/crates/control/src/referee.rs index 78b37a32..e60eaf61 100644 --- a/crates/control/src/referee.rs +++ b/crates/control/src/referee.rs @@ -43,6 +43,7 @@ impl Referee { let handsignal: u8 = rng_gen.gen_range(1..=16); self.send_referee_message(&context, handsignal, duration)?; self.sent = true; + println!("sent referee handsignal message"); } else if duration.as_secs() >= 20{ self.sent = false; From 83e2acd419f6ea7cce62d7eee4afcf7bbefde047 Mon Sep 17 00:00:00 2001 From: lex Date: Sat, 8 Jul 2023 10:49:58 +0200 Subject: [PATCH 07/15] ugly fix --- crates/control/src/referee.rs | 20 +++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/crates/control/src/referee.rs b/crates/control/src/referee.rs index e60eaf61..068910d8 100644 --- a/crates/control/src/referee.rs +++ b/crates/control/src/referee.rs @@ -9,6 +9,7 @@ use types::{hardware::Interface, messages::OutgoingMessage, CycleTime, FilteredW pub struct Referee { last_heard_timestamp: Option, sent: bool, + first: bool, } #[context] @@ -30,25 +31,30 @@ impl Referee { Ok(Self { last_heard_timestamp: None, sent: false, + first: true, }) } pub fn cycle(&mut self, context: CycleContext) -> Result<()> { if context.filtered_whistle.started_this_cycle { - if let Some(cycle_time) = self.last_heard_timestamp { + if self.first { + let mut rng_gen = rand::thread_rng(); + let handsignal: u8 = rng_gen.gen_range(1..=16); + self.send_referee_message(&context, handsignal, Duration::from_secs_f32(0.0))?; + self.last_heard_timestamp = Some(SystemTime::now()); + self.first = false; + println!("sent referee handsignal message"); + } + else if let Some(cycle_time) = self.last_heard_timestamp { match cycle_time.duration_since(cycle_time) { Ok(duration) => { - if duration.as_secs() < 20 && !self.sent{ + if duration.as_secs() > 20 { let mut rng_gen = rand::thread_rng(); let handsignal: u8 = rng_gen.gen_range(1..=16); self.send_referee_message(&context, handsignal, duration)?; - self.sent = true; + self.last_heard_timestamp = Some(SystemTime::now()); println!("sent referee handsignal message"); } - else if duration.as_secs() >= 20{ - self.sent = false; - self.last_heard_timestamp = None; - } } Err(_err) => {} } From ef3f6d931a063cecb977cc26cba826aa1f79445b Mon Sep 17 00:00:00 2001 From: lex Date: Sat, 8 Jul 2023 10:56:51 +0200 Subject: [PATCH 08/15] removed unused code --- crates/control/src/referee.rs | 2 -- 1 file changed, 2 deletions(-) diff --git a/crates/control/src/referee.rs b/crates/control/src/referee.rs index 068910d8..5b11c24c 100644 --- a/crates/control/src/referee.rs +++ b/crates/control/src/referee.rs @@ -8,7 +8,6 @@ use types::{hardware::Interface, messages::OutgoingMessage, CycleTime, FilteredW pub struct Referee { last_heard_timestamp: Option, - sent: bool, first: bool, } @@ -30,7 +29,6 @@ impl Referee { pub fn new(_context: CreationContext) -> Result { Ok(Self { last_heard_timestamp: None, - sent: false, first: true, }) } From a99cfb5313fdd064b29c186d85a56087460b7ffb Mon Sep 17 00:00:00 2001 From: lex Date: Sat, 8 Jul 2023 11:58:52 +0200 Subject: [PATCH 09/15] cargo fmt --- crates/control/src/referee.rs | 33 +++++------ crates/spl_network/src/endpoint.rs | 2 +- crates/spl_network_messages/src/lib.rs | 2 +- .../src/referee_return_message.rs | 58 +++++++++---------- crates/types/src/messages.rs | 4 +- .../head.P0000074A04S8C700011.json | 6 +- .../head.P0000074A04S91300023.json | 14 +++-- .../head.P0000074A06S9BH00015.json | 3 +- 8 files changed, 63 insertions(+), 59 deletions(-) diff --git a/crates/control/src/referee.rs b/crates/control/src/referee.rs index 5b11c24c..636f6f3e 100644 --- a/crates/control/src/referee.rs +++ b/crates/control/src/referee.rs @@ -3,7 +3,7 @@ use context_attribute::context; use nalgebra::Isometry2; use rand::Rng; use spl_network_messages::{PlayerNumber, RefereeMessage}; -use std::time::{SystemTime, Duration}; +use std::time::{Duration, SystemTime}; use types::{hardware::Interface, messages::OutgoingMessage, CycleTime, FilteredWhistle}; pub struct Referee { @@ -41,9 +41,8 @@ impl Referee { self.send_referee_message(&context, handsignal, Duration::from_secs_f32(0.0))?; self.last_heard_timestamp = Some(SystemTime::now()); self.first = false; - println!("sent referee handsignal message"); - } - else if let Some(cycle_time) = self.last_heard_timestamp { + // println!("sent referee handsignal message"); + } else if let Some(cycle_time) = self.last_heard_timestamp { match cycle_time.duration_since(cycle_time) { Ok(duration) => { if duration.as_secs() > 20 { @@ -51,7 +50,7 @@ impl Referee { let handsignal: u8 = rng_gen.gen_range(1..=16); self.send_referee_message(&context, handsignal, duration)?; self.last_heard_timestamp = Some(SystemTime::now()); - println!("sent referee handsignal message"); + // println!("sent referee handsignal message"); } } Err(_err) => {} @@ -66,22 +65,20 @@ impl Referee { &mut self, context: &CycleContext, handsignal: u8, - duration: Duration + duration: Duration, ) -> Result<()> { 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.as_secs_f32(), - ball: [0.0, 0.0], - }, - )) + .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.as_secs_f32(), + ball: [0.0, 0.0], + })) .wrap_err("failed to write GameControllerReturnMessage to hardware")?; Ok(()) diff --git a/crates/spl_network/src/endpoint.rs b/crates/spl_network/src/endpoint.rs index 352a51cd..7a341c9e 100644 --- a/crates/spl_network/src/endpoint.rs +++ b/crates/spl_network/src/endpoint.rs @@ -3,12 +3,12 @@ use std::{ net::{Ipv4Addr, SocketAddr, SocketAddrV4}, }; +use bifrost::serialization::Encode; use log::warn; use serde::Deserialize; use thiserror::Error; use tokio::{net::UdpSocket, select, sync::Mutex}; use types::messages::{IncomingMessage, OutgoingMessage}; -use bifrost::serialization::Encode; use constants::DNT_TEAM_NUMBER; diff --git a/crates/spl_network_messages/src/lib.rs b/crates/spl_network_messages/src/lib.rs index 949cbbd0..41923f1d 100644 --- a/crates/spl_network_messages/src/lib.rs +++ b/crates/spl_network_messages/src/lib.rs @@ -18,8 +18,8 @@ pub use game_controller_state_conversion::{ SubState, Team, TeamColor, TeamState, }; pub use game_controller_state_message::GameControllerStateMessage; -use serialize_hierarchy::SerializeHierarchy; 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 index 9b5bb280..5667448d 100644 --- a/crates/spl_network_messages/src/referee_return_message.rs +++ b/crates/spl_network_messages/src/referee_return_message.rs @@ -1,46 +1,44 @@ use bifrost::serialization::Encode; use serde::{Deserialize, Serialize}; - #[derive(Encode, Debug, Clone, Deserialize, Serialize)] pub struct RefereeMessage { - /// "RGrt" - pub header: [u8; 4], + /// "RGrt" + pub header: [u8; 4], - /// Has to be set to GAMECONTROLLER_RETURN_STRUCT_VERSION - pub version: u8, + /// Has to be set to GAMECONTROLLER_RETURN_STRUCT_VERSION + pub version: u8, - /// Player number starts with 1 - pub player_num: u8, + /// Player number starts with 1 + pub player_num: u8, - /// Team number - pub team_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, + /// 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 + /// 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 + /// 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], + /// 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( @@ -64,4 +62,4 @@ impl RefereeMessage { ball, } } -} \ No newline at end of file +} diff --git a/crates/types/src/messages.rs b/crates/types/src/messages.rs index f41a1040..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, RefereeMessage}; +use spl_network_messages::{ + GameControllerReturnMessage, GameControllerStateMessage, HulkMessage, RefereeMessage, +}; #[derive(Clone, Debug, Deserialize, Serialize, SerializeHierarchy)] pub enum IncomingMessage { 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" } From e8a95cabaa5d251274c2fefce84274d139ccd021 Mon Sep 17 00:00:00 2001 From: lex Date: Sat, 8 Jul 2023 12:08:36 +0200 Subject: [PATCH 10/15] removed double code --- crates/control/src/referee.rs | 21 +++++++++------------ 1 file changed, 9 insertions(+), 12 deletions(-) diff --git a/crates/control/src/referee.rs b/crates/control/src/referee.rs index 636f6f3e..3ee40e12 100644 --- a/crates/control/src/referee.rs +++ b/crates/control/src/referee.rs @@ -36,21 +36,13 @@ impl Referee { pub fn cycle(&mut self, context: CycleContext) -> Result<()> { if context.filtered_whistle.started_this_cycle { if self.first { - let mut rng_gen = rand::thread_rng(); - let handsignal: u8 = rng_gen.gen_range(1..=16); - self.send_referee_message(&context, handsignal, Duration::from_secs_f32(0.0))?; - self.last_heard_timestamp = Some(SystemTime::now()); + self.send_referee_message(&context, Duration::from_secs_f32(0.0))?; self.first = false; - // println!("sent referee handsignal message"); } else if let Some(cycle_time) = self.last_heard_timestamp { match cycle_time.duration_since(cycle_time) { Ok(duration) => { if duration.as_secs() > 20 { - let mut rng_gen = rand::thread_rng(); - let handsignal: u8 = rng_gen.gen_range(1..=16); - self.send_referee_message(&context, handsignal, duration)?; - self.last_heard_timestamp = Some(SystemTime::now()); - // println!("sent referee handsignal message"); + self.send_referee_message(&context, duration)?; } } Err(_err) => {} @@ -64,9 +56,12 @@ impl Referee { fn send_referee_message( &mut self, context: &CycleContext, - handsignal: u8, duration: Duration, ) -> Result<()> { + self.last_heard_timestamp = Some(SystemTime::now()); + let mut rng_gen = rand::thread_rng(); + let handsignal: u8 = rng_gen.gen_range(1..=16); + context .hardware .write_to_network(OutgoingMessage::RefereeReturnData(RefereeMessage { @@ -79,7 +74,9 @@ impl Referee { ball_age: duration.as_secs_f32(), ball: [0.0, 0.0], })) - .wrap_err("failed to write GameControllerReturnMessage to hardware")?; + .wrap_err("failed to write RefereeMessage to hardware")?; + + println!("sent referee handsignal message"); Ok(()) } From 25f9be07a181822ac352bdc438f68b5f3c11e1b9 Mon Sep 17 00:00:00 2001 From: lex Date: Sat, 8 Jul 2023 12:44:59 +0200 Subject: [PATCH 11/15] removed unused dependencies --- crates/control/Cargo.toml | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/crates/control/Cargo.toml b/crates/control/Cargo.toml index 0dfa556c..d155b939 100644 --- a/crates/control/Cargo.toml +++ b/crates/control/Cargo.toml @@ -9,12 +9,10 @@ homepage = "https://github.com/hulks/hulk" approx = { workspace = true } bifrost = {workspace = true} color-eyre = { workspace = true } -compiled-nn = { workspace = true } context_attribute = { workspace = true } itertools = { workspace = true } filtering = { workspace = true } framework = { workspace = true } -image = { workspace = true } kinematics = { workspace = true } kira = { workspace = true } log = { workspace = true } @@ -22,7 +20,7 @@ motionfile = { workspace = true } nalgebra = { workspace = true } ordered-float = { workspace = true } projection = { workspace = true } -rand.workspace = true +rand = { workspace = true } serde = { workspace = true } serde_json = { workspace = true } serialize_hierarchy = { workspace = true } From 34544d6a465aba9b395251ce47d85793c896cf91 Mon Sep 17 00:00:00 2001 From: lex Date: Sat, 8 Jul 2023 12:48:46 +0200 Subject: [PATCH 12/15] fixed duration --- Cargo.lock | 2 -- crates/control/src/referee.rs | 10 +++++----- 2 files changed, 5 insertions(+), 7 deletions(-) diff --git a/Cargo.lock b/Cargo.lock index b1cda295..e169e1c1 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -1268,11 +1268,9 @@ dependencies = [ "approx", "bifrost", "color-eyre", - "compiled-nn", "context_attribute", "filtering", "framework", - "image", "itertools", "kinematics", "kira", diff --git a/crates/control/src/referee.rs b/crates/control/src/referee.rs index 3ee40e12..077cad43 100644 --- a/crates/control/src/referee.rs +++ b/crates/control/src/referee.rs @@ -36,13 +36,13 @@ impl Referee { pub fn cycle(&mut self, context: CycleContext) -> Result<()> { if context.filtered_whistle.started_this_cycle { if self.first { - self.send_referee_message(&context, Duration::from_secs_f32(0.0))?; + self.send_referee_message(&context, 0.0)?; self.first = false; } else if let Some(cycle_time) = self.last_heard_timestamp { match cycle_time.duration_since(cycle_time) { Ok(duration) => { if duration.as_secs() > 20 { - self.send_referee_message(&context, duration)?; + self.send_referee_message(&context, duration.as_secs_f32())?; } } Err(_err) => {} @@ -56,10 +56,10 @@ impl Referee { fn send_referee_message( &mut self, context: &CycleContext, - duration: Duration, + duration: f32, ) -> Result<()> { - self.last_heard_timestamp = Some(SystemTime::now()); let mut rng_gen = rand::thread_rng(); + self.last_heard_timestamp = Some(SystemTime::now()); let handsignal: u8 = rng_gen.gen_range(1..=16); context @@ -71,7 +71,7 @@ impl Referee { team_num: 8, fallen: handsignal, pose: [0.0, 0.0, 0.0], - ball_age: duration.as_secs_f32(), + ball_age: duration, ball: [0.0, 0.0], })) .wrap_err("failed to write RefereeMessage to hardware")?; From f9482680fe7919877a0167eefb746b70cc17f5a5 Mon Sep 17 00:00:00 2001 From: lex Date: Sat, 8 Jul 2023 12:50:37 +0200 Subject: [PATCH 13/15] removed unused import --- crates/control/src/referee.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crates/control/src/referee.rs b/crates/control/src/referee.rs index 077cad43..55f3590a 100644 --- a/crates/control/src/referee.rs +++ b/crates/control/src/referee.rs @@ -3,7 +3,7 @@ use context_attribute::context; use nalgebra::Isometry2; use rand::Rng; use spl_network_messages::{PlayerNumber, RefereeMessage}; -use std::time::{Duration, SystemTime}; +use std::time::{SystemTime}; use types::{hardware::Interface, messages::OutgoingMessage, CycleTime, FilteredWhistle}; pub struct Referee { From 728039ae20f8a6e500f18077caedbaa7eb5efb95 Mon Sep 17 00:00:00 2001 From: lex Date: Sat, 8 Jul 2023 12:52:38 +0200 Subject: [PATCH 14/15] use context start time --- crates/control/src/referee.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/crates/control/src/referee.rs b/crates/control/src/referee.rs index 55f3590a..79c51bcf 100644 --- a/crates/control/src/referee.rs +++ b/crates/control/src/referee.rs @@ -39,7 +39,7 @@ impl Referee { self.send_referee_message(&context, 0.0)?; self.first = false; } else if let Some(cycle_time) = self.last_heard_timestamp { - match cycle_time.duration_since(cycle_time) { + 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())?; From 88ed7bc376c3dc0f48f9afc4bed3ffe1a91c49d4 Mon Sep 17 00:00:00 2001 From: lex Date: Sat, 8 Jul 2023 15:00:57 +0200 Subject: [PATCH 15/15] removed bool --- crates/control/src/referee.rs | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/crates/control/src/referee.rs b/crates/control/src/referee.rs index 79c51bcf..3f14e860 100644 --- a/crates/control/src/referee.rs +++ b/crates/control/src/referee.rs @@ -3,12 +3,11 @@ use context_attribute::context; use nalgebra::Isometry2; use rand::Rng; use spl_network_messages::{PlayerNumber, RefereeMessage}; -use std::time::{SystemTime}; +use std::time::SystemTime; use types::{hardware::Interface, messages::OutgoingMessage, CycleTime, FilteredWhistle}; pub struct Referee { last_heard_timestamp: Option, - first: bool, } #[context] @@ -29,15 +28,13 @@ impl Referee { pub fn new(_context: CreationContext) -> Result { Ok(Self { last_heard_timestamp: None, - first: true, }) } pub fn cycle(&mut self, context: CycleContext) -> Result<()> { if context.filtered_whistle.started_this_cycle { - if self.first { + if self.last_heard_timestamp.is_none() { 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) => {