diff --git a/.github/workflows/pull-request.yml b/.github/workflows/pull-request.yml index 61aa4ccd6f..ca765b34eb 100644 --- a/.github/workflows/pull-request.yml +++ b/.github/workflows/pull-request.yml @@ -105,7 +105,7 @@ jobs: strategy: fail-fast: true matrix: - target: [imagine, nao, replayer, webots] + target: [behavior_simulator, imagine, nao, replayer, webots] profile: [release, dev] runs-on: - self-hosted @@ -129,7 +129,6 @@ jobs: [ aliveness, annotato, - behavior_simulator, camera_matrix_extractor, depp, fanta, diff --git a/Cargo.lock b/Cargo.lock index 4c2e0a16c2..a6964e1dc5 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -689,44 +689,6 @@ dependencies = [ "unicode-width", ] -[[package]] -name = "behavior_simulator" -version = "0.1.0" -dependencies = [ - "bincode", - "buffered_watch", - "chrono", - "clap 4.5.4", - "code_generation", - "color-eyre", - "communication", - "control", - "coordinate_systems", - "ctrlc", - "fern", - "framework", - "geometry", - "hardware", - "ittapi", - "linear_algebra", - "log", - "mlua", - "nalgebra", - "parameters", - "parking_lot", - "path_serde", - "proc-macro2", - "projection", - "quote", - "serde", - "source_analyzer", - "spl_network", - "spl_network_messages", - "tokio", - "tokio-util", - "types", -] - [[package]] name = "bincode" version = "1.3.3" @@ -2982,6 +2944,46 @@ dependencies = [ "walking_engine", ] +[[package]] +name = "hulk_behavior_simulator" +version = "0.1.0" +dependencies = [ + "bincode", + "buffered_watch", + "chrono", + "clap 4.5.4", + "code_generation", + "color-eyre", + "communication", + "context_attribute", + "control", + "coordinate_systems", + "ctrlc", + "fern", + "framework", + "geometry", + "hardware", + "ittapi", + "linear_algebra", + "log", + "mlua", + "nalgebra", + "parameters", + "parking_lot", + "path_serde", + "proc-macro2", + "projection", + "quote", + "serde", + "serde_json", + "source_analyzer", + "spl_network", + "spl_network_messages", + "tokio", + "tokio-util", + "types", +] + [[package]] name = "hulk_imagine" version = "0.1.0" diff --git a/Cargo.toml b/Cargo.toml index c3f99e24f1..0106e6abe0 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -19,6 +19,7 @@ members = [ "crates/geometry", "crates/hardware", "crates/hulk", + "crates/hulk_behavior_simulator", "crates/hulk_imagine", "crates/hulk_manifest", "crates/hulk_nao", @@ -45,7 +46,6 @@ members = [ "crates/vision", "crates/walking_engine", "tools/annotato", - "tools/behavior_simulator", "tools/camera_matrix_extractor", "tools/depp", "tools/fanta", diff --git a/crates/code_generation/src/cyclers.rs b/crates/code_generation/src/cyclers.rs index f23f2fd35d..14441fbae8 100644 --- a/crates/code_generation/src/cyclers.rs +++ b/crates/code_generation/src/cyclers.rs @@ -94,7 +94,7 @@ fn generate_database_struct() -> TokenStream { path_serde::PathSerialize, path_serde::PathIntrospect, )] - pub(crate) struct Database { + pub struct Database { pub main_outputs: MainOutputs, pub additional_outputs: AdditionalOutputs, } @@ -130,7 +130,7 @@ fn generate_struct(cycler: &Cycler, cyclers: &Cyclers, mode: CyclerMode) -> Toke own_sender: buffered_watch::Sender, own_subscribed_outputs_receiver: buffered_watch::Receiver>, parameters_receiver: buffered_watch::Receiver, - cycler_state: crate::structs::#module_name::CyclerState, + pub cycler_state: crate::structs::#module_name::CyclerState, #realtime_inputs #input_output_fields #node_fields diff --git a/crates/control/src/lib.rs b/crates/control/src/lib.rs index b3680098bf..f7b123c960 100644 --- a/crates/control/src/lib.rs +++ b/crates/control/src/lib.rs @@ -7,7 +7,6 @@ pub mod button_filter; pub mod camera_matrix_calculator; pub mod center_of_mass_provider; pub mod dribble_path_planner; -pub mod fake_data; pub mod fall_state_estimation; pub mod foot_bumper_filter; pub mod game_controller_filter; diff --git a/tools/behavior_simulator/Cargo.toml b/crates/hulk_behavior_simulator/Cargo.toml similarity index 89% rename from tools/behavior_simulator/Cargo.toml rename to crates/hulk_behavior_simulator/Cargo.toml index c4fd5794c0..10b7ac327f 100644 --- a/tools/behavior_simulator/Cargo.toml +++ b/crates/hulk_behavior_simulator/Cargo.toml @@ -1,12 +1,12 @@ [package] -name = "behavior_simulator" +name = "hulk_behavior_simulator" version = "0.1.0" edition.workspace = true license.workspace = true homepage.workspace = true [[bin]] -name = "behavior_simulator" +name = "hulk_behavior_simulator" [dependencies] bincode = { workspace = true } @@ -15,6 +15,7 @@ chrono = { workspace = true } clap = { workspace = true } color-eyre = { workspace = true } communication = { workspace = true } +context_attribute = { workspace = true } control = { workspace = true } coordinate_systems = { workspace = true } ctrlc = { workspace = true } @@ -32,6 +33,7 @@ parking_lot = { workspace = true } path_serde = { workspace = true } projection = { workspace = true } serde = { workspace = true } +serde_json = { workspace = true } spl_network = { workspace = true } spl_network_messages = { workspace = true } tokio = { workspace = true } diff --git a/tools/behavior_simulator/build.rs b/crates/hulk_behavior_simulator/build.rs similarity index 93% rename from tools/behavior_simulator/build.rs rename to crates/hulk_behavior_simulator/build.rs index 9685b6c194..97ede8acbe 100644 --- a/tools/behavior_simulator/build.rs +++ b/crates/hulk_behavior_simulator/build.rs @@ -17,18 +17,17 @@ fn main() -> Result<()> { name: "Control", kind: CyclerKind::RealTime, instances: vec![""], - setup_nodes: vec!["control::fake_data"], + setup_nodes: vec!["crate::fake_data"], nodes: vec![ "control::active_vision", "control::ball_state_composer", "control::behavior::node", - "control::referee_position_provider", "control::game_controller_state_filter", "control::kick_selector", "control::kick_target_provider", "control::motion::look_around", "control::motion::motion_selector", - "control::referee_pose_detection_filter", + "control::referee_position_provider", "control::role_assignment", "control::rule_obstacle_composer", "control::search_suggestor", @@ -41,7 +40,7 @@ fn main() -> Result<()> { kind: CyclerKind::Perception, instances: vec![""], setup_nodes: vec!["spl_network::message_receiver"], - nodes: vec![], + nodes: vec!["spl_network::message_filter"], }, ], }; @@ -77,7 +76,7 @@ fn main() -> Result<()> { paths .write_to_file("behavior_files.rs") .wrap_err("failed to write generated tests to file")?; - generate(&cyclers, &structs, ExecutionMode::None) + generate(&cyclers, &structs, ExecutionMode::Run) .write_to_file("generated_code.rs") .wrap_err("failed to write generated code to file") } diff --git a/tools/behavior_simulator/default.json b/crates/hulk_behavior_simulator/default.json similarity index 100% rename from tools/behavior_simulator/default.json rename to crates/hulk_behavior_simulator/default.json diff --git a/tools/behavior_simulator/etc b/crates/hulk_behavior_simulator/etc similarity index 100% rename from tools/behavior_simulator/etc rename to crates/hulk_behavior_simulator/etc diff --git a/crates/control/src/fake_data.rs b/crates/hulk_behavior_simulator/src/fake_data.rs similarity index 55% rename from crates/control/src/fake_data.rs rename to crates/hulk_behavior_simulator/src/fake_data.rs index d73f9205b9..3272877df8 100644 --- a/crates/control/src/fake_data.rs +++ b/crates/hulk_behavior_simulator/src/fake_data.rs @@ -22,6 +22,8 @@ use types::{ sensor_data::SensorData, }; +use crate::interfake::FakeDataInterface; + #[derive(Deserialize, Serialize)] pub struct FakeData {} @@ -37,6 +39,7 @@ pub struct CreationContext { #[context] #[allow(dead_code)] pub struct CycleContext { + hardware_interface: HardwareInterface, glance_angle: Parameter, } @@ -51,6 +54,7 @@ pub struct MainOutputs { pub game_controller_address: MainOutput>, pub has_ground_contact: MainOutput, pub hulk_messages: MainOutput>, + pub is_referee_initial_pose_detected: MainOutput, pub hypothetical_ball_positions: MainOutput>>, pub is_localization_converged: MainOutput, pub obstacles: MainOutput>, @@ -67,7 +71,35 @@ impl FakeData { Ok(Self {}) } - pub fn cycle(&mut self, _context: CycleContext) -> Result { - Ok(MainOutputs::default()) + pub fn cycle(&mut self, context: CycleContext) -> Result { + let mut receiver = context + .hardware_interface + .get_last_database_receiver() + .lock(); + let last_database = &receiver.borrow_and_mark_as_seen().main_outputs; + Ok(MainOutputs { + ball_position: last_database.ball_position.into(), + cycle_time: last_database.cycle_time.into(), + fall_state: last_database.fall_state.into(), + filtered_whistle: last_database.filtered_whistle.clone().into(), + game_controller_state: last_database.game_controller_state.into(), + game_controller_address: last_database.game_controller_address.into(), + has_ground_contact: last_database.has_ground_contact.into(), + hulk_messages: last_database.hulk_messages.clone().into(), + is_referee_initial_pose_detected: last_database.is_referee_initial_pose_detected.into(), + hypothetical_ball_positions: last_database.hypothetical_ball_positions.clone().into(), + is_localization_converged: last_database.is_localization_converged.into(), + obstacles: last_database.obstacles.clone().into(), + penalty_shot_direction: last_database.penalty_shot_direction.into(), + primary_state: last_database.primary_state.into(), + ground_to_field: last_database.ground_to_field.into(), + sensor_data: last_database.sensor_data.clone().into(), + stand_up_front_estimated_remaining_duration: last_database + .stand_up_front_estimated_remaining_duration + .into(), + stand_up_back_estimated_remaining_duration: last_database + .stand_up_back_estimated_remaining_duration + .into(), + }) } } diff --git a/crates/hulk_behavior_simulator/src/interfake.rs b/crates/hulk_behavior_simulator/src/interfake.rs new file mode 100644 index 0000000000..68dcfddfac --- /dev/null +++ b/crates/hulk_behavior_simulator/src/interfake.rs @@ -0,0 +1,82 @@ +use std::{ + mem::take, + sync::Arc, + time::{SystemTime, UNIX_EPOCH}, +}; + +use parking_lot::Mutex; + +use buffered_watch::{Receiver, Sender}; +use color_eyre::Result; +use hardware::{NetworkInterface, RecordingInterface, TimeInterface}; +use types::messages::{IncomingMessage, OutgoingMessage}; + +use crate::{cyclers::control::Database, HardwareInterface}; + +pub struct Interfake { + time: SystemTime, + messages: Arc>>, + last_database_receiver: Mutex>, + last_database_sender: Mutex>, +} + +impl Default for Interfake { + fn default() -> Self { + let (last_database_sender, last_database_receiver) = + buffered_watch::channel(Default::default()); + Self { + time: UNIX_EPOCH, + messages: Default::default(), + last_database_receiver: Mutex::new(last_database_receiver), + last_database_sender: Mutex::new(last_database_sender), + } + } +} + +impl NetworkInterface for Interfake { + fn read_from_network(&self) -> Result { + unimplemented!() + } + + fn write_to_network(&self, message: OutgoingMessage) -> Result<()> { + self.messages.lock().push(message); + Ok(()) + } +} + +impl RecordingInterface for Interfake { + fn should_record(&self) -> bool { + false + } + + fn set_whether_to_record(&self, _enable: bool) {} +} + +impl TimeInterface for Interfake { + fn get_now(&self) -> SystemTime { + self.time + } +} + +pub trait FakeDataInterface { + fn get_last_database_receiver(&self) -> &Mutex>; + fn get_last_database_sender(&self) -> &Mutex>; +} + +impl FakeDataInterface for Interfake { + fn get_last_database_receiver(&self) -> &Mutex> { + &self.last_database_receiver + } + + fn get_last_database_sender(&self) -> &Mutex> { + &self.last_database_sender + } +} + +impl Interfake { + pub fn take_outgoing_messages(&self) -> Vec { + take(&mut self.messages.lock()) + } +} + +impl HardwareInterface for Interfake {} diff --git a/tools/behavior_simulator/src/lib.rs b/crates/hulk_behavior_simulator/src/lib.rs similarity index 56% rename from tools/behavior_simulator/src/lib.rs rename to crates/hulk_behavior_simulator/src/lib.rs index 86a2839314..f93908dc73 100644 --- a/tools/behavior_simulator/src/lib.rs +++ b/crates/hulk_behavior_simulator/src/lib.rs @@ -1,6 +1,7 @@ use hardware::{NetworkInterface, RecordingInterface, TimeInterface}; +use interfake::FakeDataInterface; -pub mod cycler; +pub mod fake_data; pub mod interfake; pub mod robot; pub mod server; @@ -9,4 +10,7 @@ pub mod state; include!(concat!(env!("OUT_DIR"), "/generated_code.rs")); -pub trait HardwareInterface: TimeInterface + NetworkInterface + RecordingInterface {} +pub trait HardwareInterface: + TimeInterface + NetworkInterface + RecordingInterface + FakeDataInterface +{ +} diff --git a/tools/behavior_simulator/src/main.rs b/crates/hulk_behavior_simulator/src/main.rs similarity index 97% rename from tools/behavior_simulator/src/main.rs rename to crates/hulk_behavior_simulator/src/main.rs index ade99cd534..0ea006e923 100644 --- a/tools/behavior_simulator/src/main.rs +++ b/crates/hulk_behavior_simulator/src/main.rs @@ -7,7 +7,7 @@ use fern::{Dispatch, InitError}; use log::LevelFilter; use tokio_util::sync::CancellationToken; -use behavior_simulator::{server, simulator::Simulator}; +use hulk_behavior_simulator::{server, simulator::Simulator}; #[derive(Parser)] enum Arguments { diff --git a/tools/behavior_simulator/src/robot.rs b/crates/hulk_behavior_simulator/src/robot.rs similarity index 55% rename from tools/behavior_simulator/src/robot.rs rename to crates/hulk_behavior_simulator/src/robot.rs index f38d09d868..c38d2f6af1 100644 --- a/tools/behavior_simulator/src/robot.rs +++ b/crates/hulk_behavior_simulator/src/robot.rs @@ -1,40 +1,41 @@ use std::{ - collections::BTreeMap, convert::Into, - sync::Arc, + sync::{mpsc, Arc}, time::{Duration, SystemTime}, }; +use buffered_watch::Receiver; use color_eyre::{eyre::WrapErr, Result}; use control::localization::generate_initial_pose; +use framework::{future_queue, Producer, RecordingTrigger}; use linear_algebra::vector; use parameters::directory::deserialize; use projection::camera_matrix::CameraMatrix; -use spl_network_messages::PlayerNumber; +use spl_network_messages::{HulkMessage, PlayerNumber}; use types::{messages::IncomingMessage, motion_selection::MotionSafeExits}; use crate::{ - cycler::{BehaviorCycler, Database}, - interfake::Interfake, - structs::{control::CyclerState, Parameters}, + cyclers::control::{Cycler, CyclerInstance, Database}, + interfake::{FakeDataInterface, Interfake}, + structs::Parameters, }; pub struct Robot { pub interface: Arc, - pub cycler: BehaviorCycler, pub database: Database, - pub cycler_state: CyclerState, pub parameters: Parameters, pub is_penalized: bool, pub last_kick_time: Duration, pub ball_last_seen: Option, + + cycler: Cycler, + control_receiver: Receiver, + spl_network_sender: Producer, } impl Robot { pub fn try_new(player_number: PlayerNumber) -> Result { - let interface: Arc<_> = Interfake::default().into(); - let runtime = tokio::runtime::Builder::new_current_thread() .build() .unwrap(); @@ -49,8 +50,28 @@ impl Robot { })?; parameter.player_number = player_number; - let cycler = BehaviorCycler::new(interface.clone(), ¶meter) - .wrap_err("failed to create cycler")?; + let interface: Arc<_> = Interfake::default().into(); + + let (control_sender, control_receiver) = buffered_watch::channel(Database::default()); + let (mut subscriptions_sender, subscriptions_receiver) = + buffered_watch::channel(Default::default()); + let (mut parameters_sender, parameters_receiver) = + buffered_watch::channel(Default::default()); + let (spl_network_sender, spl_network_consumer) = future_queue(); + let (recording_sender, _recording_receiver) = mpsc::sync_channel(0); + *parameters_sender.borrow_mut() = parameter.clone(); + + let mut cycler = Cycler::new( + CyclerInstance::Control, + interface.clone(), + control_sender, + subscriptions_receiver, + parameters_receiver, + spl_network_consumer, + recording_sender, + RecordingTrigger::new(0), + )?; + cycler.cycler_state.motion_safe_exits = MotionSafeExits::fill(true); let mut database = Database::default(); @@ -63,34 +84,46 @@ impl Robot { ); database.main_outputs.has_ground_contact = true; database.main_outputs.is_localization_converged = true; - - let cycler_state = CyclerState { - motion_safe_exits: MotionSafeExits::fill(true), - ..Default::default() - }; + subscriptions_sender + .borrow_mut() + .insert("additional_outputs".to_string()); Ok(Self { interface, - cycler, database, - cycler_state, parameters: parameter, is_penalized: false, last_kick_time: Duration::default(), ball_last_seen: None, + + cycler, + control_receiver, + spl_network_sender, }) } - pub fn cycle( - &mut self, - messages: BTreeMap>>, - ) -> Result<()> { - self.cycler.cycle( - &mut self.database, - &mut self.cycler_state, - &self.parameters, - messages, + pub fn cycle(&mut self, messages: &[(PlayerNumber, HulkMessage)]) -> Result<()> { + for (source, hulks_message) in messages.iter() { + let source_is_other = *source != self.parameters.player_number; + let message = IncomingMessage::Spl(*hulks_message); + self.spl_network_sender.announce(); + self.spl_network_sender + .finalize(crate::structs::spl_network::MainOutputs { + filtered_message: source_is_other.then(|| message.clone()), + message, + }); + } + buffered_watch::Sender::<_>::borrow_mut( + &mut self.interface.get_last_database_sender().lock(), ) + .main_outputs = self.database.main_outputs.clone(); + + self.cycler.cycle()?; + + let database = self.control_receiver.borrow_and_mark_as_seen(); + self.database.main_outputs = database.main_outputs.clone(); + self.database.additional_outputs = database.additional_outputs.clone(); + Ok(()) } pub fn field_of_view(&self) -> f32 { diff --git a/tools/behavior_simulator/src/server.rs b/crates/hulk_behavior_simulator/src/server.rs similarity index 97% rename from tools/behavior_simulator/src/server.rs rename to crates/hulk_behavior_simulator/src/server.rs index e3ddca812e..52092dafe4 100644 --- a/tools/behavior_simulator/src/server.rs +++ b/crates/hulk_behavior_simulator/src/server.rs @@ -4,7 +4,7 @@ use std::{ }; use crate::{ - cycler::Database, + cyclers::control::Database, robot::to_player_number, simulator::{Frame, Simulator}, state::Ball, @@ -35,7 +35,6 @@ struct BehaviorSimulatorDatabase { main_outputs: MainOutputs, } -#[allow(clippy::too_many_arguments)] async fn timeline_server( keep_running: CancellationToken, mut parameters_reader: buffered_watch::Receiver, @@ -86,7 +85,7 @@ pub fn run( ) -> Result<()> { let communication_server = communication::server::Runtime::::start( addresses, - "tools/behavior_simulator", + "crates/hulk_behavior_simulator", "behavior_simulator".to_string(), "behavior_simulator".to_string(), keep_running.clone(), @@ -118,7 +117,7 @@ pub fn run( let start = Instant::now(); if let Err(error) = simulator.run() { - eprintln!("{}", error.bright_red()) + eprintln!("{}", format!("{:#?}", error).bright_red()) } let duration = Instant::now() - start; println!("Took {:.2} seconds", duration.as_secs_f32()); diff --git a/tools/behavior_simulator/src/simulator.rs b/crates/hulk_behavior_simulator/src/simulator.rs similarity index 98% rename from tools/behavior_simulator/src/simulator.rs rename to crates/hulk_behavior_simulator/src/simulator.rs index 76a001c273..5607135d61 100644 --- a/tools/behavior_simulator/src/simulator.rs +++ b/crates/hulk_behavior_simulator/src/simulator.rs @@ -12,11 +12,9 @@ use linear_algebra::{Isometry2, Point2, Vector2}; use types::{obstacles::Obstacle, players::Players}; use crate::{ - cycler::Database, - robot::to_player_number, - robot::Robot, - state::Ball, - state::{Event, LuaRobot, State}, + cyclers::control::Database, + robot::{to_player_number, Robot}, + state::{Ball, Event, LuaRobot, State}, }; const SERIALIZE_OPTIONS: SerializeOptions = SerializeOptions::new().serialize_none_to_null(false); diff --git a/tools/behavior_simulator/src/state.rs b/crates/hulk_behavior_simulator/src/state.rs similarity index 80% rename from tools/behavior_simulator/src/state.rs rename to crates/hulk_behavior_simulator/src/state.rs index fffda716ca..898808cf00 100644 --- a/tools/behavior_simulator/src/state.rs +++ b/crates/hulk_behavior_simulator/src/state.rs @@ -1,8 +1,8 @@ use std::{ - collections::{BTreeMap, HashMap}, + collections::HashMap, f32::consts::FRAC_PI_4, mem::take, - time::{Duration, UNIX_EPOCH}, + time::{Duration, SystemTime, UNIX_EPOCH}, }; use color_eyre::Result; @@ -12,12 +12,11 @@ use coordinate_systems::{Field, Head}; use geometry::line_segment::LineSegment; use linear_algebra::{vector, Isometry2, Orientation2, Point2, Rotation2, Vector2}; use path_serde::{PathDeserialize, PathIntrospect, PathSerialize}; -use spl_network_messages::{GamePhase, HulkMessage, PlayerNumber, Team}; +use spl_network_messages::{GamePhase, GameState, HulkMessage, PlayerNumber, Team}; use types::{ ball_position::BallPosition, - filtered_game_controller_state::FilteredGameControllerState, - filtered_game_state::FilteredGameState, - messages::{IncomingMessage, OutgoingMessage}, + game_controller_state::GameControllerState, + messages::OutgoingMessage, motion_command::{HeadMotion, KickVariant, MotionCommand, OrientationMode}, planned_path::PathSegment, players::Players, @@ -25,11 +24,7 @@ use types::{ support_foot::Side, }; -use crate::{ - cycler::Database, - robot::Robot, - structs::{control::AdditionalOutputs, Parameters}, -}; +use crate::{cyclers::control::Database, robot::Robot, structs::Parameters}; pub enum Event { Cycle, @@ -51,7 +46,7 @@ pub struct State { pub ball: Option, pub messages: Vec<(PlayerNumber, HulkMessage)>, pub finished: bool, - pub filtered_game_controller_state: FilteredGameControllerState, + pub game_controller_state: GameControllerState, } impl State { @@ -79,7 +74,6 @@ impl State { .as_mut() .expect("simulated robots should always have a known pose"); - robot.database.additional_outputs = AdditionalOutputs::default(); let head_motion = match &robot.database.main_outputs.motion_command { MotionCommand::Walk { head, @@ -183,23 +177,9 @@ impl State { } fn cycle_robots(&mut self, now: std::time::SystemTime) -> Result<()> { - let incoming_messages = take(&mut self.messages); + let messages_sent_last_cycle = take(&mut self.messages); for (player_number, robot) in self.robots.iter_mut() { - let incoming_messages: Vec<_> = incoming_messages - .iter() - .map(|(sender, message)| { - (sender != player_number).then_some(IncomingMessage::Spl(*message)) - }) - .collect(); - let messages_with_time = BTreeMap::from_iter([( - now, - incoming_messages - .iter() - .map(|message| message.as_ref()) - .collect(), - )]); - robot.database.main_outputs.cycle_time.start_time = now; let ground_to_field = robot @@ -233,26 +213,22 @@ impl State { } else { None }; - robot.database.main_outputs.primary_state = match ( - robot.is_penalized, - self.filtered_game_controller_state.game_state, - ) { - (true, _) => PrimaryState::Penalized, - (false, FilteredGameState::Initial) => PrimaryState::Initial, - (false, FilteredGameState::Ready { .. }) => PrimaryState::Ready, - (false, FilteredGameState::Set) => PrimaryState::Set, - (false, FilteredGameState::Playing { .. }) => PrimaryState::Playing, - (false, FilteredGameState::Finished) => PrimaryState::Finished, - }; - robot.database.main_outputs.filtered_game_controller_state = - Some(self.filtered_game_controller_state); - robot.cycle(messages_with_time)?; + robot.database.main_outputs.primary_state = + match (robot.is_penalized, self.game_controller_state.game_state) { + (true, _) => PrimaryState::Penalized, + (false, GameState::Initial) => PrimaryState::Initial, + (false, GameState::Ready { .. }) => PrimaryState::Ready, + (false, GameState::Set) => PrimaryState::Set, + (false, GameState::Playing { .. }) => PrimaryState::Playing, + (false, GameState::Finished) => PrimaryState::Finished, + }; + robot.database.main_outputs.game_controller_state = Some(self.game_controller_state); + robot.cycle(&messages_sent_last_cycle)?; for message in robot.interface.take_outgoing_messages() { if let OutgoingMessage::Spl(message) = message { self.messages.push((*player_number, message)); - self.filtered_game_controller_state - .remaining_number_of_messages -= 1 + self.game_controller_state.remaining_amount_of_messages -= 1 } } } @@ -285,7 +261,7 @@ impl State { finished: self.finished, - filtered_game_controller_state: self.filtered_game_controller_state, + game_controller_state: self.game_controller_state, } } @@ -302,7 +278,7 @@ impl State { self.finished = lua_state.finished; - self.filtered_game_controller_state = lua_state.filtered_game_controller_state; + self.game_controller_state = lua_state.game_controller_state; Ok(()) } @@ -311,11 +287,11 @@ impl State { impl Default for State { fn default() -> Self { let robots = HashMap::new(); - let filtered_game_controller_state = FilteredGameControllerState { - game_state: FilteredGameState::Initial, - opponent_game_state: FilteredGameState::Initial, + let game_controller_state = GameControllerState { + game_state: GameState::Initial, game_phase: GamePhase::Normal, kicking_team: Team::Hulks, + last_game_state_change: SystemTime::UNIX_EPOCH, penalties: Players { one: None, two: None, @@ -325,9 +301,9 @@ impl Default for State { six: None, seven: None, }, - remaining_number_of_messages: 1200, + remaining_amount_of_messages: 1200, sub_state: None, - own_team_is_home_after_coin_toss: false, + hulks_team_is_home_after_coin_toss: true, }; Self { @@ -337,7 +313,7 @@ impl Default for State { ball: None, messages: Vec::new(), finished: false, - filtered_game_controller_state, + game_controller_state, } } } @@ -350,7 +326,7 @@ pub struct LuaState { pub ball: Option, pub messages: Vec<(PlayerNumber, HulkMessage)>, pub finished: bool, - pub filtered_game_controller_state: FilteredGameControllerState, + pub game_controller_state: GameControllerState, } #[derive(Clone, Deserialize, Serialize)] diff --git a/tools/behavior_simulator/tests/scenarios.rs b/crates/hulk_behavior_simulator/tests/scenarios.rs similarity index 90% rename from tools/behavior_simulator/tests/scenarios.rs rename to crates/hulk_behavior_simulator/tests/scenarios.rs index 4e034919f9..8f5d4baef0 100644 --- a/tools/behavior_simulator/tests/scenarios.rs +++ b/crates/hulk_behavior_simulator/tests/scenarios.rs @@ -2,7 +2,7 @@ use std::{path::Path, time::Instant}; use color_eyre::{eyre::Context, Result}; -use behavior_simulator::simulator::Simulator; +use hulk_behavior_simulator::simulator::Simulator; fn test_scenario(path: impl AsRef) -> Result<()> { let mut simulator = Simulator::try_new()?; diff --git a/crates/source_analyzer/src/node.rs b/crates/source_analyzer/src/node.rs index 94188ccd91..a0045912c8 100644 --- a/crates/source_analyzer/src/node.rs +++ b/crates/source_analyzer/src/node.rs @@ -77,6 +77,9 @@ fn file_path_from_module_path(root: &Path, module: syn::Path) -> Result, - search_suggestor: SearchSuggestor, - active_vision: ActiveVision, - ball_state_composer: BallStateComposer, - behavior: Behavior, - kick_selector: KickSelector, - kick_target_provider: KickTargetProvider, - look_around: LookAround, - motion_selector: MotionSelector, - role_assignment: RoleAssignment, - rule_obstacle_composer: RuleObstacleComposer, - world_state_composer: WorldStateComposer, - time_to_reach_kick_position: TimeToReachKickPosition, -} - -impl BehaviorCycler { - pub fn new(hardware_interface: Arc, parameters: &Parameters) -> Result { - let search_suggestor = control::search_suggestor::SearchSuggestor::new( - control::search_suggestor::CreationContext::new( - ¶meters.field_dimensions, - ¶meters.search_suggestor, - ), - ) - .wrap_err("failed to create node `SearchSuggestor`")?; - let time_to_reach_kick_position = - TimeToReachKickPosition::new(time_to_reach_kick_position::CreationContext {}) - .wrap_err("failed to create node `TimeToReachKickPosition`")?; - let active_vision = ActiveVision::new(active_vision::CreationContext::new( - ¶meters.field_dimensions, - )) - .wrap_err("failed to create node `ActiveVision`")?; - let ball_state_composer = BallStateComposer::new(ball_state_composer::CreationContext {}) - .wrap_err("failed to create node `BallStateComposer`")?; - let behavior = Behavior::new(node::CreationContext::new()) - .wrap_err("failed to create node `Behavior`")?; - let kick_selector = KickSelector::new(kick_selector::CreationContext {}) - .wrap_err("failed to create node `KickSelector`")?; - let kick_target_provider = - KickTargetProvider::new(kick_target_provider::CreationContext {}) - .wrap_err("failed to create node `KickTargetProvider`")?; - let look_around = control::motion::look_around::LookAround::new( - control::motion::look_around::CreationContext::new(), - ) - .wrap_err("failed to create node `LookAround`")?; - let motion_selector = MotionSelector::new(motion_selector::CreationContext::new()) - .wrap_err("failed to create node `MotionSelector`")?; - let role_assignment = RoleAssignment::new(role_assignment::CreationContext::new()) - .wrap_err("failed to create node `RoleAssignment`")?; - let rule_obstacle_composer = control::rule_obstacle_composer::RuleObstacleComposer::new( - control::rule_obstacle_composer::CreationContext {}, - ) - .wrap_err("failed to create node `RuleObstacleComposer`")?; - let world_state_composer = - WorldStateComposer::new(world_state_composer::CreationContext::new()) - .wrap_err("failed to create node `WorldStateComposer`")?; - - Ok(Self { - hardware_interface, - search_suggestor, - active_vision, - time_to_reach_kick_position, - ball_state_composer, - behavior, - kick_selector, - kick_target_provider, - look_around, - motion_selector, - role_assignment, - rule_obstacle_composer, - world_state_composer, - }) - } - - pub fn cycle( - &mut self, - own_database: &mut Database, - cycler_state: &mut CyclerState, - parameters: &Parameters, - incoming_messages: BTreeMap>>, - ) -> Result<()> { - let main_outputs = { - self.search_suggestor - .cycle(control::search_suggestor::CycleContext::new( - ¶meters.search_suggestor, - own_database.main_outputs.ball_position.as_ref(), - &own_database.main_outputs.hypothetical_ball_positions, - own_database.main_outputs.ground_to_field.as_ref(), - framework::AdditionalOutput::new( - true, - &mut own_database.additional_outputs.ball_search_heatmap, - ), - )) - .wrap_err("failed to execute cycle of `SearchSuggestor`")? - }; - own_database.main_outputs.suggested_search_position = - main_outputs.suggested_search_position.value; - - if own_database - .main_outputs - .filtered_game_controller_state - .as_ref() - .is_some() - { - let main_outputs = { - self.rule_obstacle_composer - .cycle(control::rule_obstacle_composer::CycleContext::new( - own_database - .main_outputs - .filtered_game_controller_state - .as_ref() - .unwrap(), - own_database.main_outputs.ball_state.as_ref(), - ¶meters.rule_obstacles.center_circle_obstacle_increase, - ¶meters.field_dimensions, - ¶meters.rule_obstacles.free_kick_obstacle_radius, - ¶meters.rule_obstacles.penaltykick_box_extension, - )) - .wrap_err("failed to execute cycle of node `RuleObstacleComposer`")? - }; - own_database.main_outputs.rule_obstacles = main_outputs.rule_obstacles.value; - } else { - own_database.main_outputs.rule_obstacles = Default::default(); - } - { - let main_outputs = self - .role_assignment - .cycle(role_assignment::CycleContext::new( - own_database.main_outputs.ball_position.as_ref(), - &own_database.main_outputs.fall_state, - own_database - .main_outputs - .filtered_game_controller_state - .as_ref(), - &own_database.main_outputs.primary_state, - own_database.main_outputs.ground_to_field.as_ref(), - &own_database.main_outputs.cycle_time, - PerceptionInput { - persistent: incoming_messages, - temporary: Default::default(), - }, - None, - &mut cycler_state.time_to_reach_kick_position, - ¶meters.field_dimensions, - parameters.role_assignment.forced_role.as_ref(), - ¶meters - .role_assignment - .keeper_replacementkeeper_switch_time, - ¶meters.localization.initial_poses, - ¶meters.behavior.optional_roles, - ¶meters.player_number, - ¶meters.spl_network, - &self.hardware_interface, - )) - .wrap_err("failed to execute cycle of node `RoleAssignment`")?; - own_database.main_outputs.team_ball = main_outputs.team_ball.value; - own_database.main_outputs.network_robot_obstacles = - main_outputs.network_robot_obstacles.value; - own_database.main_outputs.role = main_outputs.role.value; - } - { - let main_outputs = self - .ball_state_composer - .cycle(ball_state_composer::CycleContext::new( - &own_database.main_outputs.cycle_time, - own_database.main_outputs.ball_position.as_ref(), - own_database.main_outputs.penalty_shot_direction.as_ref(), - own_database.main_outputs.ground_to_field.as_ref(), - own_database.main_outputs.team_ball.as_ref(), - &own_database.main_outputs.primary_state, - own_database - .main_outputs - .filtered_game_controller_state - .as_ref(), - ¶meters.field_dimensions, - )) - .wrap_err("failed to execute cycle of node `BallStateComposer`")?; - own_database.main_outputs.ball_state = main_outputs.ball_state.value; - own_database.main_outputs.rule_ball_state = main_outputs.rule_ball_state.value; - } - - { - let main_outputs = self - .active_vision - .cycle(active_vision::CycleContext::new( - own_database.main_outputs.ball_state.as_ref(), - own_database.main_outputs.ball_state.as_ref(), - &own_database.main_outputs.cycle_time, - &own_database.main_outputs.obstacles, - ¶meters.behavior.look_action, - own_database.main_outputs.ground_to_field.as_ref(), - )) - .wrap_err("failed to execute cycle of node `ActiveVision`")?; - own_database.main_outputs.position_of_interest = - main_outputs.position_of_interest.value; - } - { - if own_database.main_outputs.ground_to_field.as_ref().is_some() - && own_database.main_outputs.ball_state.as_ref().is_some() - { - let main_outputs = { - self.kick_target_provider - .cycle(kick_target_provider::CycleContext::new( - own_database.main_outputs.ball_state.as_ref().unwrap(), - own_database.main_outputs.ground_to_field.as_ref().unwrap(), - &own_database.main_outputs.obstacles, - ¶meters.field_dimensions, - ¶meters - .kick_target_provider - .ball_radius_for_kick_target_selection, - ¶meters.kick_target_provider.find_kick_targets, - ¶meters - .kick_target_provider - .max_kick_around_obstacle_angle, - ¶meters.kick_target_provider.corner_kick_strength, - )) - .wrap_err("failed to execute cycle of node `KickTargetProvider`")? - }; - own_database.main_outputs.kick_targets = main_outputs.kick_targets.value; - own_database.main_outputs.obstacle_circles = main_outputs.obstacle_circles.value; - } else { - own_database.main_outputs.kick_targets = Default::default(); - own_database.main_outputs.obstacle_circles = Default::default(); - } - } - { - if own_database.main_outputs.ground_to_field.as_ref().is_some() - && own_database.main_outputs.ball_state.as_ref().is_some() - { - let main_outputs = { - self.kick_selector - .cycle(kick_selector::CycleContext::new( - own_database.main_outputs.ground_to_field.as_ref().unwrap(), - own_database.main_outputs.ball_state.as_ref().unwrap(), - &own_database.main_outputs.kick_targets, - &own_database.main_outputs.obstacles, - &own_database.main_outputs.obstacle_circles, - ¶meters.field_dimensions, - ¶meters.in_walk_kicks, - ¶meters.kick_selector.angle_distance_weight, - ¶meters.kick_selector.kick_pose_obstacle_radius, - ¶meters.kick_selector.closer_threshold, - ¶meters.kick_selector.goal_accuracy_margin, - ¶meters.kick_selector.default_kick_strength, - framework::AdditionalOutput::new( - true, - &mut own_database.additional_outputs.instant_kick_targets, - ), - )) - .wrap_err("failed to execute cycle of node `KickSelector`")? - }; - own_database.main_outputs.kick_decisions = main_outputs.kick_decisions.value; - own_database.main_outputs.instant_kick_decisions = - main_outputs.instant_kick_decisions.value; - } else { - own_database.main_outputs.kick_decisions = Default::default(); - own_database.main_outputs.instant_kick_decisions = Default::default(); - } - } - { - let main_outputs = self - .world_state_composer - .cycle(world_state_composer::CycleContext::new( - own_database.main_outputs.ball_state.as_ref(), - &own_database.main_outputs.hypothetical_ball_positions, - own_database.main_outputs.rule_ball_state.as_ref(), - own_database - .main_outputs - .filtered_game_controller_state - .as_ref(), - own_database.main_outputs.ground_to_field.as_ref(), - own_database.main_outputs.suggested_search_position.as_ref(), - own_database.main_outputs.kick_decisions.as_ref(), - own_database.main_outputs.instant_kick_decisions.as_ref(), - ¶meters.player_number, - &own_database.main_outputs.fall_state, - &own_database.main_outputs.has_ground_contact, - &own_database.main_outputs.obstacles, - &own_database.main_outputs.rule_obstacles, - &own_database.main_outputs.primary_state, - &own_database.main_outputs.role, - &own_database.main_outputs.position_of_interest, - )) - .wrap_err("failed to execute cycle of node `WorldStateComposer`")?; - own_database.main_outputs.world_state = main_outputs.world_state.value; - } - { - let main_outputs = self - .behavior - .cycle(node::CycleContext::new( - AdditionalOutput::new( - true, - &mut own_database.additional_outputs.path_obstacles, - ), - AdditionalOutput::new( - true, - &mut own_database.additional_outputs.dribble_path_obstacles, - ), - AdditionalOutput::new(true, &mut own_database.additional_outputs.active_action), - own_database.main_outputs.expected_referee_position.as_ref(), - &true, - &own_database.main_outputs.world_state, - &own_database.main_outputs.cycle_time, - &own_database.main_outputs.is_localization_converged, - ¶meters.behavior, - ¶meters.in_walk_kicks, - ¶meters.field_dimensions, - ¶meters.behavior.lost_ball, - ¶meters.behavior.intercept_ball, - ¶meters.step_planner.max_step_size, - ¶meters.behavior.role_positions.striker_set_position, - )) - .wrap_err("failed to execute cycle of node `Behavior`")?; - own_database.main_outputs.motion_command = main_outputs.motion_command.value; - own_database.main_outputs.dribble_path = main_outputs.dribble_path.value; - } - { - let main_outputs = { - self.motion_selector - .cycle(control::motion::motion_selector::CycleContext::new( - &own_database.main_outputs.motion_command, - &own_database.main_outputs.has_ground_contact, - &mut cycler_state.motion_safe_exits, - )) - .wrap_err("failed to execute cycle of node `MotionSelector`")? - }; - own_database.main_outputs.motion_selection = main_outputs.motion_selection.value; - } - { - let main_outputs = { - self.look_around - .cycle(control::motion::look_around::CycleContext::new( - ¶meters.look_around, - own_database - .main_outputs - .filtered_game_controller_state - .as_ref(), - &own_database.main_outputs.motion_command, - &own_database.main_outputs.motion_selection, - &own_database.main_outputs.cycle_time, - AdditionalOutput::new( - true, - &mut own_database.additional_outputs.look_around_mode, - ), - )) - .wrap_err("failed to execute cycle of node `LookAround`")? - }; - own_database.main_outputs.look_around = main_outputs.look_around.value; - } - { - let _main_outputs = self - .time_to_reach_kick_position - .cycle(control::time_to_reach_kick_position::CycleContext::new( - own_database.main_outputs.dribble_path.as_ref(), - &own_database.main_outputs.motion_command, - framework::AdditionalOutput::new( - true, - &mut own_database.additional_outputs.time_to_turn, - ), - framework::AdditionalOutput::new( - true, - &mut own_database - .additional_outputs - .time_to_reach_kick_position_output, - ), - &mut cycler_state.time_to_reach_kick_position, - ¶meters.behavior, - own_database - .main_outputs - .stand_up_back_estimated_remaining_duration - .as_ref(), - own_database - .main_outputs - .stand_up_front_estimated_remaining_duration - .as_ref(), - )) - .wrap_err("failed to execute cycle of `TimeToReachKickPosition`"); - } - Ok(()) - } -} diff --git a/tools/behavior_simulator/src/interfake.rs b/tools/behavior_simulator/src/interfake.rs deleted file mode 100644 index 23cd8bbcd7..0000000000 --- a/tools/behavior_simulator/src/interfake.rs +++ /dev/null @@ -1,38 +0,0 @@ -use std::{ - mem::take, - sync::{Arc, Mutex}, -}; - -use color_eyre::Result; -use hardware::{NetworkInterface, RecordingInterface}; -use types::messages::{IncomingMessage, OutgoingMessage}; - -#[derive(Default)] -pub struct Interfake { - messages: Arc>>, -} - -impl NetworkInterface for Interfake { - fn read_from_network(&self) -> Result { - unimplemented!() - } - - fn write_to_network(&self, message: OutgoingMessage) -> Result<()> { - self.messages.lock().unwrap().push(message); - Ok(()) - } -} - -impl RecordingInterface for Interfake { - fn should_record(&self) -> bool { - false - } - - fn set_whether_to_record(&self, _enable: bool) {} -} - -impl Interfake { - pub fn take_outgoing_messages(&self) -> Vec { - take(&mut self.messages.lock().unwrap()) - } -}