diff --git a/Cargo.lock b/Cargo.lock index 00a00ee0b7..d83e75f07c 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -883,6 +883,7 @@ dependencies = [ "filtering", "geometry", "hsl_network_messages", + "kinematics", "linear_algebra", "log", "nalgebra 0.34.2", @@ -7184,35 +7185,9 @@ dependencies = [ "portable-atomic", "portable-atomic-util", "rawpointer", - "rayon", "serde", ] -[[package]] -name = "ndarray-conv" -version = "0.4.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "61a04cd712622c289a7b523d3195e4fb5e2bf30c44de12355463fe58aa12213b" -dependencies = [ - "ndarray", - "ndarray-rand", - "num", - "realfft", - "rustfft", - "thiserror 2.0.18", -] - -[[package]] -name = "ndarray-rand" -version = "0.15.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f093b3db6fd194718dcdeea6bd8c829417deae904e3fcc7732dabcd4416d25d8" -dependencies = [ - "ndarray", - "rand 0.8.6", - "rand_distr 0.4.3", -] - [[package]] name = "ndk" version = "0.9.0" @@ -7334,20 +7309,6 @@ dependencies = [ "unicode-segmentation", ] -[[package]] -name = "num" -version = "0.4.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "35bd024e8b2ff75562e5f34e7f4905839deb4b22955ef5e73d2fea1b9813cb23" -dependencies = [ - "num-bigint", - "num-complex", - "num-integer", - "num-iter", - "num-rational", - "num-traits", -] - [[package]] name = "num-bigint" version = "0.4.7" @@ -9191,15 +9152,6 @@ dependencies = [ "font-types 0.11.3", ] -[[package]] -name = "realfft" -version = "3.5.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f821338fddb99d089116342c46e9f1fbf3828dba077674613e734e01d6ea8677" -dependencies = [ - "rustfft", -] - [[package]] name = "reborrow" version = "0.5.5" @@ -9941,6 +9893,23 @@ dependencies = [ "tiny-skia", ] +[[package]] +name = "search_heatmap" +version = "0.1.0" +dependencies = [ + "coordinate_systems", + "geometry", + "hsl_network_messages", + "itertools 0.14.0", + "linear_algebra", + "nalgebra 0.34.2", + "ndarray", + "ros-z", + "serde", + "types", + "voronoi", +] + [[package]] name = "search_suggestor" version = "0.1.0" @@ -9953,8 +9922,8 @@ dependencies = [ "linear_algebra", "nalgebra 0.34.2", "ndarray", - "ndarray-conv", "ros-z", + "search_heatmap", "serde", "tokio", "types", diff --git a/Cargo.toml b/Cargo.toml index 643da9298b..08497b374b 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -72,6 +72,7 @@ members = [ "crates/nodes/rule_obstacle_composer", "crates/nodes/safe_pose_checker", "crates/nodes/search_suggestor", + "crates/search_heatmap", "crates/nodes/segment_filter", "crates/nodes/stereo_visual_odometry", "crates/nodes/support_foot_estimator", @@ -320,6 +321,7 @@ rule_obstacle_composer = { path = "crates/nodes/rule_obstacle_composer" } rustfft = "6.2.0" safe_pose_checker = { path = "crates/nodes/safe_pose_checker" } scenario = { path = "crates/scenario" } +search_heatmap = { path = "crates/search_heatmap" } search_suggestor = { path = "crates/nodes/search_suggestor" } segment_filter = { path = "crates/nodes/segment_filter" } semver = "1.0.25" diff --git a/crates/bevyhavior_simulator/src/ball.rs b/crates/bevyhavior_simulator/src/ball.rs index 7265952a0e..838c2336c6 100644 --- a/crates/bevyhavior_simulator/src/ball.rs +++ b/crates/bevyhavior_simulator/src/ball.rs @@ -7,7 +7,7 @@ use linear_algebra::{Isometry2, Point2, Vector2}; use serde::{Deserialize, Serialize}; use types::{ field_dimensions::{GlobalFieldSide, Side}, - world_state::BallState, + world_state::{BallSource, BallState}, }; use crate::{ @@ -42,6 +42,7 @@ impl SimulatedBall { ball_in_ground_velocity: ground_to_world.inverse() * self.velocity, last_seen_ball: now, field_side: self.field_side, + source: BallSource::Own, } } } diff --git a/crates/bevyhavior_simulator/src/behavior_runtime.rs b/crates/bevyhavior_simulator/src/behavior_runtime.rs index c1e6446d05..7051d3ddbb 100644 --- a/crates/bevyhavior_simulator/src/behavior_runtime.rs +++ b/crates/bevyhavior_simulator/src/behavior_runtime.rs @@ -68,6 +68,7 @@ impl SimulatorRobotBehavior { velocity: ball.ball_in_ground_velocity, age: self.blackboard.world_state.now, field_side: ball.field_side, + source: ball.source, }); self.blackboard.last_ball.clone_from(&self.blackboard.ball); } else if let Some(last_ball) = &self.blackboard.ball @@ -118,6 +119,7 @@ impl SimulatorRobotBehavior { world_state: WorldState, hsl_network_parameters: HslNetworkParameters, game_controller_address: Option, + head_yaw: f32, ) -> Vec { self.blackboard.world_state = world_state; self.blackboard.parameters.hsl_network = hsl_network_parameters; @@ -129,7 +131,7 @@ impl SimulatorRobotBehavior { { outgoing_messages.push(message); } - if let Some(message) = self.blackboard.try_sending_state_message() { + if let Some(message) = self.blackboard.try_sending_state_message(head_yaw) { outgoing_messages.push(message); } outgoing_messages diff --git a/crates/bevyhavior_simulator/src/communication.rs b/crates/bevyhavior_simulator/src/communication.rs index f9fbe66570..ae15301e2b 100644 --- a/crates/bevyhavior_simulator/src/communication.rs +++ b/crates/bevyhavior_simulator/src/communication.rs @@ -12,8 +12,8 @@ use types::{ }; use crate::behavior_tree_simulator::{ - SimulationConfig, SimulatorClock, SimulatorGameState, SimulatorRobot, SimulatorRobotBehavior, - SimulatorRobotFrames, SimulatorRobotId, SimulatorWorldStates, + SimulationConfig, SimulatorClock, SimulatorGameState, SimulatorHeadYaw, SimulatorRobot, + SimulatorRobotBehavior, SimulatorRobotFrames, SimulatorRobotId, SimulatorWorldStates, }; #[derive(Resource, Clone, Debug, Default)] @@ -62,11 +62,15 @@ pub fn plan_communication( world_states: Res, mut robot_frames: ResMut, mut outgoing_messages: ResMut, - mut robots: Query<(&SimulatorRobot, &mut SimulatorRobotBehavior)>, + mut robots: Query<( + &SimulatorRobot, + &SimulatorHeadYaw, + &mut SimulatorRobotBehavior, + )>, ) { outgoing_messages.messages.clear(); - for (robot, mut behavior) in &mut robots { + for (robot, head_yaw, mut behavior) in &mut robots { let robot_id = robot.id(); let Some(world_state) = world_states.0.get(&robot_id) else { continue; @@ -76,6 +80,7 @@ pub fn plan_communication( world_state.clone(), hsl_network_parameters.0.clone(), config.game_controller_address, + head_yaw.yaw.angle(), ); if let Some(frame) = robot_frames.0.get_mut(&robot_id) { @@ -215,6 +220,7 @@ mod tests { age: Duration::from_millis(500), position: point![x + 1.0, y], }), + head_yaw: 0.0, }) } diff --git a/crates/bevyhavior_simulator/src/world_states.rs b/crates/bevyhavior_simulator/src/world_states.rs index 03b02785ae..56e5abbb77 100644 --- a/crates/bevyhavior_simulator/src/world_states.rs +++ b/crates/bevyhavior_simulator/src/world_states.rs @@ -260,6 +260,7 @@ mod tests { age: Duration::from_millis(500), position: point![x + 1.0, y], }), + head_yaw: 0.0, }) } diff --git a/crates/hsl_network_messages/src/lib.rs b/crates/hsl_network_messages/src/lib.rs index 95928bf8fd..c5710008cc 100644 --- a/crates/hsl_network_messages/src/lib.rs +++ b/crates/hsl_network_messages/src/lib.rs @@ -63,6 +63,7 @@ pub struct StrikerMessage { pub struct StateMessage { pub player_number: PlayerNumber, pub pose: Pose2, + pub head_yaw: f32, pub ball_position: Option>, } @@ -134,6 +135,7 @@ mod tests { let test_message = HulkMessage::State(StateMessage { player_number: PlayerNumber::Five, pose: Pose2::default(), + head_yaw: 0.0, ball_position: Some(BallPosition { position: Point2::origin(), age: Duration::MAX, diff --git a/crates/nodes/ball_state_composer/src/lib.rs b/crates/nodes/ball_state_composer/src/lib.rs index 6118d82910..a0cec80125 100644 --- a/crates/nodes/ball_state_composer/src/lib.rs +++ b/crates/nodes/ball_state_composer/src/lib.rs @@ -13,7 +13,7 @@ use types::{ field_dimensions::{FieldDimensions, Side}, filtered_game_controller_state::FilteredGameControllerState, primary_state::PrimaryState, - world_state::{BallState, LastBallState}, + world_state::{BallSource, BallState, LastBallState}, }; pub fn run_boxed(ctx: Arc) -> Pin> + Send>> { @@ -99,6 +99,7 @@ async fn run(ctx: Arc) -> Result<()> { ground_to_field * ball_position.position, ball_position.velocity, ball_position.last_seen.to_wallclock(), + BallSource::Own, &mut last_ball_field_side, ); ball_state_pub.publish(&Some(ball)).await?; @@ -120,6 +121,7 @@ async fn run(ctx: Arc) -> Result<()> { team_ball.position, ground_to_field.inverse() * team_ball.velocity, team_ball.last_seen.to_wallclock(), + BallSource::Team, &mut last_ball_field_side, ); ball_state_pub.publish(&Some(ball)).await?; @@ -195,6 +197,7 @@ fn compose_rule_ball_state( penalty_spot_location, Vector2::zeros(), cycle_start_time, + BallSource::Own, last_ball_field_side, )) } @@ -203,6 +206,7 @@ fn compose_rule_ball_state( Point2::origin(), Vector2::zeros(), cycle_start_time, + BallSource::Own, last_ball_field_side, )), _ => None, @@ -214,6 +218,7 @@ fn create_ball_state( ball_in_field: Point2, ball_in_ground_velocity: Vector2, last_seen_ball: SystemTime, + source: BallSource, last_ball_field_side: &mut Side, ) -> BallState { let was_in_left_half = *last_ball_field_side == Side::Left; @@ -232,5 +237,6 @@ fn create_ball_state( ball_in_ground_velocity, last_seen_ball, field_side, + source, } } diff --git a/crates/nodes/behavior_node/Cargo.toml b/crates/nodes/behavior_node/Cargo.toml index ab8fb5cc27..0ba9efb446 100644 --- a/crates/nodes/behavior_node/Cargo.toml +++ b/crates/nodes/behavior_node/Cargo.toml @@ -13,6 +13,7 @@ coordinate_systems = { workspace = true } filtering = { workspace = true } geometry = { workspace = true } hsl_network_messages = { workspace = true } +kinematics = { workspace = true } linear_algebra = { workspace = true } log = { workspace = true } nalgebra = { workspace = true } diff --git a/crates/nodes/behavior_node/src/node.rs b/crates/nodes/behavior_node/src/node.rs index 91b9af6057..26b3639bf9 100644 --- a/crates/nodes/behavior_node/src/node.rs +++ b/crates/nodes/behavior_node/src/node.rs @@ -5,6 +5,7 @@ use color_eyre::Result; use coordinate_systems::{Field, Ground}; use hsl_network_messages::PlayerNumber; +use kinematics::joints::head::HeadJoints; use linear_algebra::{Isometry2, Point2, Pose2, Vector2}; use ros_z::{prelude::*, qos::QosDurability, time::Time}; use serde::{Deserialize, Serialize}; @@ -25,7 +26,7 @@ use types::{ primary_state::PrimaryState, rule_obstacles::RuleObstacle, time_wrapper::TimeWrapper, - world_state::{BallState, PlayerState, RobotState, WorldState}, + world_state::{BallSource, BallState, PlayerState, RobotState, WorldState}, }; use voronoi::VoronoiGrid; @@ -37,6 +38,7 @@ pub struct LastBall { pub velocity: Vector2, pub age: Time, pub field_side: Side, + pub source: BallSource, } #[derive(Debug, Clone, Serialize, Deserialize, Message)] @@ -74,6 +76,13 @@ pub fn run_boxed(ctx: Arc) -> Pin> + Box::pin(run(ctx)) } +fn ball_timeout(parameters: &BehaviorParameters, source: BallSource) -> Duration { + match source { + BallSource::Own => parameters.last_ball_timeout, + BallSource::Team => parameters.team_ball_timeout, + } +} + fn validate_behavior_parameters( parameters: &BehaviorParameters, ) -> std::result::Result<(), String> { @@ -177,6 +186,11 @@ pub async fn run(ctx: Arc) -> Result<()> { .cache(1) .build() .await?; + let head_joints_command_cache = node + .subscriber::>("head_joints_command") + .cache(1) + .build() + .await?; let hypothetical_ball_positions_cache = node .subscriber::>>("hypothetical_ball_positions") .cache(1) @@ -212,7 +226,7 @@ pub async fn run(ctx: Arc) -> Result<()> { .build() .await?; let suggested_search_position_cache = node - .subscriber::>("suggested_search_position") + .subscriber::>>("suggested_search_position") .cache(1) .build() .await?; @@ -351,7 +365,7 @@ pub async fn run(ctx: Arc) -> Result<()> { .unwrap_or_default(); blackboard.world_state.suggested_search_position = suggested_search_position_cache .get_latest() - .map(|position| *position); + .and_then(|position| *position); if let Some(ball) = blackboard.world_state.ball { blackboard.ball = Some(LastBall { @@ -359,11 +373,12 @@ pub async fn run(ctx: Arc) -> Result<()> { velocity: ball.ball_in_ground_velocity, age: blackboard.world_state.now, field_side: ball.field_side, + source: ball.source, }); blackboard.last_ball.clone_from(&blackboard.ball); } else if let Some(last_ball) = &blackboard.ball && blackboard.world_state.now.duration_since(last_ball.age) - >= blackboard.parameters.last_ball_timeout + >= ball_timeout(&blackboard.parameters, last_ball.source) { blackboard.ball = None; } @@ -409,7 +424,11 @@ pub async fn run(ctx: Arc) -> Result<()> { outgoing_message_pub.publish(&message).await?; } - if let Some(message) = blackboard.try_sending_state_message() { + let head_yaw = head_joints_command_cache + .get_latest() + .map(|head_joints| head_joints.yaw) + .unwrap_or_default(); + if let Some(message) = blackboard.try_sending_state_message(head_yaw) { outgoing_message_pub.publish(&message).await?; } diff --git a/crates/nodes/behavior_node/src/search.rs b/crates/nodes/behavior_node/src/search.rs index f256b829e0..28d2dd7366 100644 --- a/crates/nodes/behavior_node/src/search.rs +++ b/crates/nodes/behavior_node/src/search.rs @@ -1,4 +1,6 @@ -use linear_algebra::{Pose2, vector}; +use coordinate_systems::Field; +use linear_algebra::{Orientation2, Point2, Pose2, distance, point, vector}; +use types::field_dimensions::FieldDimensions; use types::{ behavior_tree::Status, motion_command::{BodyMotion, MotionCommand, OrientationMode}, @@ -43,18 +45,55 @@ fn get_leuchtturm_direction(blackboard: &Blackboard) -> f32 { 1.0 } +fn search_cell_target_pose( + cell_center: Point2, + robot_position: Point2, + field_dimensions: FieldDimensions, +) -> Pose2 { + let min_x = (cell_center.x() - 0.5).max(-field_dimensions.length / 2.0); + let max_x = (cell_center.x() + 0.5).min(field_dimensions.length / 2.0); + let min_y = (cell_center.y() - 0.5).max(-field_dimensions.width / 2.0); + let max_y = (cell_center.y() + 0.5).min(field_dimensions.width / 2.0); + + let corners = [ + point![min_x, min_y], + point![min_x, max_y], + point![max_x, min_y], + point![max_x, max_y], + ]; + let nearest_corner = corners + .into_iter() + .min_by(|a, b| distance(*a, robot_position).total_cmp(&distance(*b, robot_position))) + .unwrap_or(cell_center); + let direction_into_cell = cell_center.coords() - nearest_corner.coords(); + let orientation = if direction_into_cell.norm() > f32::EPSILON { + Orientation2::from_vector(direction_into_cell) + } else { + Orientation2::new(0.0) + }; + + Pose2::from_parts(nearest_corner, orientation) +} + pub fn walk_to_search_position(blackboard: &mut Blackboard) -> Status { if let (Some(search_position), Some(ground_to_field)) = ( blackboard.world_state.suggested_search_position, blackboard.world_state.robot.ground_to_field, ) { - let search_position_in_ground = ground_to_field.inverse() * search_position; + let field_to_ground = ground_to_field.inverse(); + let robot_position = ground_to_field.as_pose().position(); + let target_pose_in_field = + search_cell_target_pose(search_position, robot_position, blackboard.field_dimensions); + let search_position_in_ground = field_to_ground * search_position; return walk_to( blackboard, - Pose2::from(search_position_in_ground), + field_to_ground * target_pose_in_field, blackboard.parameters.walk_speed.search, - OrientationMode::AlignWithPath, + OrientationMode::LookAt { + target: search_position_in_ground, + tolerance: blackboard.parameters.walk_and_stand.orientation_tolerance, + }, blackboard .parameters .walk_and_stand diff --git a/crates/nodes/behavior_node/src/send_message.rs b/crates/nodes/behavior_node/src/send_message.rs index 256d59481d..2a352c0850 100644 --- a/crates/nodes/behavior_node/src/send_message.rs +++ b/crates/nodes/behavior_node/src/send_message.rs @@ -62,10 +62,11 @@ impl Blackboard { ) } - pub fn try_sending_state_message(&mut self) -> Option { + pub fn try_sending_state_message(&mut self, head_yaw: f32) -> Option { if self.world_state.robot.primary_state != PrimaryState::Playing { return None; } + let now = self.world_state.now; let remaining_amount_of_messages = self .world_state @@ -102,11 +103,11 @@ impl Blackboard { let message = HulkMessage::State(StateMessage { player_number: self.world_state.robot.player_number, pose, + head_yaw, ball_position, }); self.last_sent_hsl_message_time = Some(now); - Some(OutgoingMessage::Hsl(message)) } else { None diff --git a/crates/nodes/global_parameter_provider/src/lib.rs b/crates/nodes/global_parameter_provider/src/lib.rs index 610c7c621a..041d2040fe 100644 --- a/crates/nodes/global_parameter_provider/src/lib.rs +++ b/crates/nodes/global_parameter_provider/src/lib.rs @@ -5,13 +5,14 @@ use hsl_network_messages::PlayerNumber; use serde::{Deserialize, Serialize}; use ros_z::{prelude::*, qos::QosDurability}; -use types::field_dimensions::FieldDimensions; +use types::{field_dimensions::FieldDimensions, parameters::HslNetworkParameters}; #[derive(Debug, Clone, Serialize, Deserialize, Message)] #[serde(deny_unknown_fields)] pub struct Parameters { pub player_number: PlayerNumber, pub field_dimensions: FieldDimensions, + pub hsl_network: HslNetworkParameters, } pub fn run_boxed(ctx: Arc) -> Pin> + Send>> { @@ -41,12 +42,22 @@ async fn run(ctx: Arc) -> Result<()> { .build() .await?; + let hsl_network_pub = node + .publisher::("hsl_network") + .qos(QosProfile { + durability: QosDurability::TransientLocal, + ..Default::default() + }) + .build() + .await?; + let parameters_snapshot = node_parameters.snapshot(); let parameters = parameters_snapshot.typed(); player_number_pub.publish(¶meters.player_number).await?; field_dimensions_pub .publish(¶meters.field_dimensions) .await?; + hsl_network_pub.publish(¶meters.hsl_network).await?; let mut parameters_receiver = node_parameters.subscribe(); loop { @@ -58,5 +69,6 @@ async fn run(ctx: Arc) -> Result<()> { field_dimensions_pub .publish(¶meters.field_dimensions.clone()) .await?; + hsl_network_pub.publish(¶meters.hsl_network).await?; } } diff --git a/crates/nodes/player_state_receiver/src/lib.rs b/crates/nodes/player_state_receiver/src/lib.rs index 6bb3654801..ac15ca54ab 100644 --- a/crates/nodes/player_state_receiver/src/lib.rs +++ b/crates/nodes/player_state_receiver/src/lib.rs @@ -105,6 +105,7 @@ mod tests { inner: IncomingMessage::Hsl(HulkMessage::State(StateMessage { player_number: PlayerNumber::Two, pose, + head_yaw: 0.0, ball_position: None, })), }, diff --git a/crates/nodes/search_suggestor/Cargo.toml b/crates/nodes/search_suggestor/Cargo.toml index 323517394a..6aa8eae65f 100644 --- a/crates/nodes/search_suggestor/Cargo.toml +++ b/crates/nodes/search_suggestor/Cargo.toml @@ -14,8 +14,8 @@ itertools = { workspace = true } linear_algebra = { workspace = true } nalgebra = { workspace = true } ndarray = { workspace = true } -ndarray-conv = { workspace = true } ros-z = { workspace = true } +search_heatmap = { workspace = true } serde = { workspace = true, features = ["derive"] } tokio = { workspace = true } types = { workspace = true } diff --git a/crates/nodes/search_suggestor/src/heatmap.rs b/crates/nodes/search_suggestor/src/heatmap.rs deleted file mode 100644 index 33c35598ff..0000000000 --- a/crates/nodes/search_suggestor/src/heatmap.rs +++ /dev/null @@ -1,247 +0,0 @@ -use std::{ops::Range, time::SystemTime}; - -use coordinate_systems::{Field, Ground}; -use geometry::direction::{Direction, Rotate90Degrees}; -use hsl_network_messages::{HulkMessage, StateMessage, SubState, Team}; -use itertools::Itertools; -use linear_algebra::{Isometry2, Point2, Vector2, vector}; -use nalgebra::clamp; -use ndarray::Array2; -use ros_z::time::Time; -use serde::{Deserialize, Serialize}; -use types::{ - ball_position::{BallPosition, HypotheticalBallPosition}, - field_dimensions::{FieldDimensions, Half, Side}, - filtered_game_controller_state::FilteredGameControllerState, - heatmap::Heatmap as HeatmapMessage, - messages::IncomingMessage, - parameters::SearchSuggestorParameters, - primary_state::PrimaryState, - time_wrapper::TimeWrapper, -}; - -#[derive(Deserialize, Serialize)] -pub(crate) struct Heatmap { - pub(crate) map: Array2, - pub(crate) cells_per_meter: f32, - pub(crate) last_maximum_heatmap_position: Option<(usize, usize)>, - pub(crate) has_decided_for_heatmap_tile: bool, -} - -impl Heatmap { - pub(crate) fn to_message(&self) -> HeatmapMessage { - let (length, width) = self.map.dim(); - HeatmapMessage { - length: length as u32, - width: width as u32, - values: self.map.iter().copied().collect(), - } - } - - pub(crate) fn update_with_ball_position( - &mut self, - field_dimensions: FieldDimensions, - ball_position: BallPosition, - ground_to_field: Isometry2, - ) { - let heatmap_point = - self.field_to_heatmap(field_dimensions, ground_to_field * ball_position.position); - self.map[heatmap_point] = 1.0; - } - - pub(crate) fn update_with_hypothetical_ball_positions( - &mut self, - field_dimensions: FieldDimensions, - hypothetical_ball_positions: Vec>, - ground_to_field: Isometry2, - parameters: &SearchSuggestorParameters, - ) { - for ball_hypothesis in hypothetical_ball_positions { - let ball_hypothesis_position = ground_to_field * ball_hypothesis.position; - let heatmap_point = self.field_to_heatmap(field_dimensions, ball_hypothesis_position); - self.map[heatmap_point] = (self.map[heatmap_point] - + ball_hypothesis.validity * parameters.own_ball_weight) - / 2.0; - } - } - - pub(crate) fn update_with_rule_ball( - &mut self, - filtered_game_controller_state: &FilteredGameControllerState, - field_dimensions: &FieldDimensions, - primary_state: &PrimaryState, - parameters: &SearchSuggestorParameters, - ) { - for rule_ball_hypothesis in get_rule_hypotheses( - *primary_state, - filtered_game_controller_state, - *field_dimensions, - ) { - let heatmap_point = self.field_to_heatmap(*field_dimensions, rule_ball_hypothesis); - self.map[heatmap_point] += parameters.rule_ball_weight_increment; - } - } - - pub(crate) fn update_with_team_ball( - &mut self, - field_dimensions: FieldDimensions, - network_message: TimeWrapper, - parameters: &SearchSuggestorParameters, - ) { - let IncomingMessage::Hsl(message) = network_message.inner else { - return; - }; - self.add_teamballs( - field_dimensions, - network_message.time.to_wallclock(), - message, - parameters.team_ball_weight, - ); - } - - pub(crate) fn get_maximum_position(&self, minimum_validity: f32) -> Option<(usize, usize)> { - let linear_maximum_heat_heatmap_position = - self.map.iter().position_max_by(|a, b| a.total_cmp(b))?; - let maximum_heat_heatmap_position = ( - linear_maximum_heat_heatmap_position / self.map.dim().1, - linear_maximum_heat_heatmap_position % self.map.dim().1, - ); - if self.map[maximum_heat_heatmap_position] > minimum_validity { - return Some(maximum_heat_heatmap_position); - } - None - } - - pub(crate) fn decay_tiles_in_fov( - &mut self, - field_dimensions: FieldDimensions, - robot_position: Vector2, - left_edge: Vector2, - right_edge: Vector2, - decay_distance_factor: f32, - heatmap_decay_range: Range, - ) { - self.map.indexed_iter_mut().for_each(|((x, y), value)| { - let tile_center_in_field: Vector2 = vector![ - ((x as f32 + 1.0 / 2.0) / self.cells_per_meter - field_dimensions.length / 2.0), - ((y as f32 + 1.0 / 2.0) / self.cells_per_meter - field_dimensions.width / 2.0) - ]; - let robot_to_tile = tile_center_in_field - robot_position; - let is_inside_sight = get_direction(left_edge, robot_to_tile) - == Direction::Counterclockwise - && get_direction(right_edge, robot_to_tile) == Direction::Clockwise; - let distance_to_tile = robot_to_tile.norm(); - let relative_distance_to_tile = - clamp(distance_to_tile / heatmap_decay_range.end, 0.0, 1.0); - if is_inside_sight && heatmap_decay_range.contains(&distance_to_tile) { - *value *= 1.0 - decay_distance_factor * (1.0 - relative_distance_to_tile); - } - }); - } - - fn field_to_heatmap( - &self, - field_dimensions: FieldDimensions, - field_point: Point2, - ) -> (usize, usize) { - let heatmap_point = ( - ((field_point.x() + field_dimensions.length / 2.0) * self.cells_per_meter) as usize, - ((field_point.y() + field_dimensions.width / 2.0) * self.cells_per_meter) as usize, - ); - ( - clamp(heatmap_point.0, 0, self.map.dim().0 - 1), - clamp(heatmap_point.1, 0, self.map.dim().1 - 1), - ) - } - - fn add_teamballs( - &mut self, - field_dimensions: FieldDimensions, - time: SystemTime, - message: HulkMessage, - team_ball_weight: f32, - ) { - let ball = match message { - HulkMessage::State(StateMessage { ball_position, .. }) => { - ball_position.map(|ball| BallPosition { - position: ball.position, - velocity: Vector2::zeros(), - last_seen: Time::from_wallclock(time) - ball.age, - }) - } - }; - if let Some(ball_position) = ball { - let heatmap_point = self.field_to_heatmap(field_dimensions, ball_position.position); - self.map[heatmap_point] = team_ball_weight; - } - } -} - -fn get_rule_hypotheses( - primary_state: PrimaryState, - filtered_game_controller_state: &FilteredGameControllerState, - field_dimensions: FieldDimensions, -) -> Vec> { - let kicking_team_half = kicking_team_half(filtered_game_controller_state.kicking_team); - - match (primary_state, filtered_game_controller_state.sub_state) { - (PrimaryState::Ready, Some(SubState::PenaltyKick)) => { - let kicking_team_half = kicking_team_half.unwrap_or(Half::Own).mirror(); - vec![field_dimensions.penalty_spot(kicking_team_half)] - } - // Kick-off - (PrimaryState::Ready, None) => vec![field_dimensions.center()], - (PrimaryState::Playing, Some(SubState::CornerKick)) => { - if let Some(kicking_team_half) = kicking_team_half { - let kicking_team_half = kicking_team_half.mirror(); - vec![ - field_dimensions.corner(kicking_team_half, Side::Left), - field_dimensions.corner(kicking_team_half, Side::Right), - ] - } else { - vec![ - field_dimensions.corner(Half::Own, Side::Left), - field_dimensions.corner(Half::Opponent, Side::Left), - field_dimensions.corner(Half::Own, Side::Right), - field_dimensions.corner(Half::Opponent, Side::Right), - ] - } - } - (PrimaryState::Playing, Some(SubState::GoalKick)) => { - if let Some(kicking_team_half) = kicking_team_half { - vec![ - field_dimensions.goal_box_corner(kicking_team_half, Side::Left), - field_dimensions.goal_box_corner(kicking_team_half, Side::Right), - ] - } else { - vec![ - field_dimensions.goal_box_corner(Half::Own, Side::Left), - field_dimensions.goal_box_corner(Half::Opponent, Side::Left), - field_dimensions.goal_box_corner(Half::Own, Side::Right), - field_dimensions.goal_box_corner(Half::Opponent, Side::Right), - ] - } - } - (_, _) => Vec::new(), - } -} - -fn kicking_team_half(kicking_team: Option) -> Option { - match kicking_team { - Some(Team::Opponent) => Some(Half::Opponent), - Some(Team::Hulks) => Some(Half::Own), - None => None, - } -} - -fn get_direction(base_vector: Vector2, vector_to_test: Vector2) -> Direction { - let clockwise_normal_vector = base_vector.rotate_90_degrees(Direction::Clockwise); - let directed_cathetus = clockwise_normal_vector.dot(&vector_to_test); - - match directed_cathetus { - 0.0 => Direction::Collinear, - f if f > 0.0 => Direction::Clockwise, - f if f < 0.0 => Direction::Counterclockwise, - f => panic!("directed cathetus was not a real number: {f}"), - } -} diff --git a/crates/nodes/search_suggestor/src/lib.rs b/crates/nodes/search_suggestor/src/lib.rs index 0f17fe8d95..4fb6b69822 100644 --- a/crates/nodes/search_suggestor/src/lib.rs +++ b/crates/nodes/search_suggestor/src/lib.rs @@ -1,29 +1,27 @@ -use color_eyre::{ - Result, - eyre::{WrapErr as _, ensure}, -}; +use color_eyre::Result; use coordinate_systems::{Field, Ground}; -use linear_algebra::{Isometry2, Point2, Vector2, point, vector}; -use ndarray::{Array2, array}; -use ndarray_conv::{ConvExt, ConvMode, PaddingMode}; +use hsl_network_messages::{HulkMessage, PlayerNumber, StateMessage}; +use linear_algebra::{Isometry2, Point2}; use ros_z::{prelude::*, qos::QosDurability}; -use std::{boxed::Box, f32::consts, future::Future, pin::Pin, sync::Arc, time::Duration}; -use tokio::task::block_in_place; +use search_heatmap::{Heatmap, SearchOccluder, SearchVoronoiSelection}; +use serde::{Deserialize, Serialize}; +use std::{boxed::Box, future::Future, pin::Pin, sync::Arc, time::Duration}; use types::{ ball_position::{BallPosition, HypotheticalBallPosition}, field_dimensions::FieldDimensions, filtered_game_controller_state::FilteredGameControllerState, messages::IncomingMessage, - parameters::SearchSuggestorParameters, + obstacles::{Obstacle, ObstacleKind}, + parameters::{HslNetworkParameters, SearchSuggestorParameters}, + players::Players, primary_state::PrimaryState, time_wrapper::TimeWrapper, }; -mod heatmap; -use heatmap::Heatmap; -struct SearchSuggestorOutput { - suggested_search_position: Option>, - additional_heatmap: Option, +#[derive(Clone, Debug, Serialize, Deserialize, Message)] +#[serde(deny_unknown_fields)] +pub struct Parameters { + pub search_suggestor: SearchSuggestorParameters, } pub fn run_boxed(ctx: Arc) -> Pin> + Send>> { @@ -33,7 +31,7 @@ pub fn run_boxed(ctx: Arc) -> Pin> + async fn run(ctx: Arc) -> Result<()> { let node = ctx.create_node("search_suggestor").build().await?; - let parameters = node.bind_parameter_as::("search_suggestor")?; + let node_parameters = node.bind_parameter_as::("search_suggestor")?; let field_dimensions_sub = node .subscriber::("field_dimensions") .qos(QosProfile { @@ -46,6 +44,24 @@ async fn run(ctx: Arc) -> Result<()> { .subscriber::>>("ball_filter/ball_position") .build() .await?; + let player_number_cache = node + .subscriber::("player_number") + .qos(QosProfile { + durability: QosDurability::TransientLocal, + ..Default::default() + }) + .cache(1) + .build() + .await?; + let hsl_network_parameters_cache = node + .subscriber::("hsl_network") + .qos(QosProfile { + durability: QosDurability::TransientLocal, + ..Default::default() + }) + .cache(1) + .build() + .await?; let hypothetical_ball_positions_sub = node .subscriber::>>( "ball_filter/hypothetical_ball_positions", @@ -70,6 +86,11 @@ async fn run(ctx: Arc) -> Result<()> { .subscriber::("filtered_game_controller_state") .build() .await?; + let obstacles_cache = node + .subscriber::>("obstacles") + .cache(1) + .build() + .await?; let network_message_sub = node .subscriber::>("filtered_message") .build() @@ -79,183 +100,272 @@ async fn run(ctx: Arc) -> Result<()> { .build() .await?; let suggested_search_position_pub = node - .publisher::>("suggested_search_position") + .publisher::>>("suggested_search_position") .build() .await?; - let mut timer = node.create_timer(Duration::from_millis(100)); - let field_dimensions = field_dimensions_sub.recv().await?; - let initial_parameters_snapshot = parameters.snapshot(); - let initial_parameters = initial_parameters_snapshot.typed(); - let (heatmap_length, heatmap_width) = ( - (field_dimensions.length * initial_parameters.cells_per_meter).round() as usize, - (field_dimensions.width * initial_parameters.cells_per_meter).round() as usize, - ); - - ensure!( - heatmap_length > 0, - "heatmap_length must at least be 1 - current value is {heatmap_length}" - ); - ensure!( - heatmap_width > 0, - format!("heatmap_width must at least be 1 - current value is {heatmap_width}") - ); - - let mut heatmap = Heatmap { - map: Array2::ones((heatmap_length, heatmap_width)) - / (heatmap_length * heatmap_width) as f32, - cells_per_meter: initial_parameters.cells_per_meter, - last_maximum_heatmap_position: None, - has_decided_for_heatmap_tile: false, - }; + let mut heatmap = Heatmap::new(field_dimensions); + let mut last_ball_position = None; + let mut current_ball_position = None; + let mut latest_filtered_game_controller_state = None; + let mut last_teammate_messages = Players::new(None); + let mut last_priority_update = node.clock().now(); + let mut tick = node.create_timer(Duration::from_millis(20)); loop { - timer.tick().await; - let parameters_snapshot = parameters.snapshot(); - let parameters = parameters_snapshot.typed(); + tick.tick().await; + let now = node.clock().now(); + let elapsed_since_last_priority_update = now.duration_since(last_priority_update); + last_priority_update = now; + + let node_parameters_snapshot = node_parameters.snapshot(); + let node_parameters = node_parameters_snapshot.typed(); + let parameters = &node_parameters.search_suggestor; + let Some(hsl_network_parameters) = hsl_network_parameters_cache.get_latest() else { + continue; + }; + let hsl_network_parameters = hsl_network_parameters.as_ref(); let ground_to_field = ground_to_field_cache .get_latest() .map(|ground_to_field| *ground_to_field); let primary_state = primary_state_cache.get_latest(); let primary_state = primary_state.as_deref(); - - let mut ball_positions = Vec::new(); - let mut hypothetical_ball_positions = Vec::new(); - let mut network_messages = Vec::new(); - let mut filtered_game_controller_states = Vec::new(); + let mut ball_was_seen = false; + let mut received_filtered_game_controller_state = None; while ball_position_sub.is_ready() { - ball_positions.push(ball_position_sub.recv().await?); + match (ball_position_sub.recv().await?, ground_to_field) { + (Some(ball_position), Some(ground_to_field)) => { + ball_was_seen = true; + let ball_position = ground_to_field * ball_position.position; + last_ball_position = Some(ball_position); + current_ball_position = Some(ball_position); + } + (None, _) | (Some(_), None) => { + current_ball_position = None; + } + } } while hypothetical_ball_positions_sub.is_ready() { - hypothetical_ball_positions.push(hypothetical_ball_positions_sub.recv().await?); + if let Some(ground_to_field) = ground_to_field { + let hypothetical_ball_positions = hypothetical_ball_positions_sub.recv().await?; + heatmap.update_with_hypothetical_ball_positions( + field_dimensions, + &hypothetical_ball_positions, + ground_to_field, + parameters, + ); + } else { + hypothetical_ball_positions_sub.recv().await?; + } } while network_message_sub.is_ready() { - network_messages.push(network_message_sub.recv().await?); + let network_message = network_message_sub.recv().await?; + heatmap.update_with_team_ball(field_dimensions, network_message.clone(), parameters); + decay_with_teammate_message( + &mut heatmap, + field_dimensions, + network_message, + &mut last_teammate_messages, + parameters, + hsl_network_parameters, + elapsed_since_last_priority_update, + ); } while filtered_game_controller_state_sub.is_ready() { - filtered_game_controller_states.push(filtered_game_controller_state_sub.recv().await?); + let filtered_game_controller_state = filtered_game_controller_state_sub.recv().await?; + received_filtered_game_controller_state = Some(filtered_game_controller_state.clone()); + latest_filtered_game_controller_state = Some(filtered_game_controller_state); + } + + let restart_regenerated = latest_filtered_game_controller_state.as_ref().is_some_and( + |filtered_game_controller_state| { + heatmap.regenerate_restart_hypotheses( + filtered_game_controller_state, + field_dimensions, + parameters.rule_ball_weight_increment, + ) + }, + ); + if !restart_regenerated + && let (Some(filtered_game_controller_state), Some(primary_state)) = + (&received_filtered_game_controller_state, primary_state) + { + heatmap.update_with_rule_ball( + filtered_game_controller_state, + field_dimensions, + *primary_state, + parameters, + ); } - let output = block_in_place(|| { - let mut ball_was_seen = false; + if !restart_regenerated + && current_ball_position.is_none() + && let Some(last_ball_position) = last_ball_position + { + heatmap.increase_around_last_ball( + field_dimensions, + last_ball_position, + elapsed_since_last_priority_update, + parameters, + ); + } - for ball_position in ball_positions { - if let (Some(ball_position), Some(ground_to_field)) = - (ball_position, ground_to_field) - { - ball_was_seen = true; - heatmap.update_with_ball_position( - field_dimensions, - ball_position, - ground_to_field, - ); - } - } - if let Some(ground_to_field) = ground_to_field { - for hypothetical_ball_positions in hypothetical_ball_positions { - heatmap.update_with_hypothetical_ball_positions( - field_dimensions, - hypothetical_ball_positions, - ground_to_field, - parameters, - ); - } - } - for network_message in network_messages { - heatmap.update_with_team_ball(field_dimensions, network_message, parameters); - } - if let Some(primary_state) = primary_state { - for filtered_game_controller_state in filtered_game_controller_states { - heatmap.update_with_rule_ball( - &filtered_game_controller_state, - &field_dimensions, - primary_state, - parameters, - ); - } - } + if current_ball_position.is_none() + && !ball_was_seen + && let Some(ground_to_field) = ground_to_field + { + let occluders = obstacles_cache + .get_latest() + .map(|obstacles| { + search_occluders_from_obstacles(obstacles.as_ref(), ground_to_field) + }) + .unwrap_or_default(); + heatmap.decay_tiles_in_robot_fov_with_occluders( + field_dimensions, + ground_to_field, + parameters, + &occluders, + ); + } + + if let Some(current_ball_position) = current_ball_position { + heatmap.set_known_ball_position(field_dimensions, current_ball_position); + } - if !ball_was_seen && let Some(ground_to_field) = ground_to_field { - let robot_position = ground_to_field.as_pose().position().coords(); - let body_orientation = ground_to_field.orientation().angle(); - let fov_angle_offset = 45.0 * consts::PI / 180.0; - let left_angle = body_orientation - fov_angle_offset; - let right_angle = body_orientation + fov_angle_offset; - let left_edge: Vector2 = vector!(left_angle.cos(), left_angle.sin()); - let right_edge: Vector2 = vector!(right_angle.cos(), right_angle.sin()); + heatmap.clamp_values(); - heatmap.decay_tiles_in_fov( - field_dimensions, - robot_position, - left_edge, - right_edge, - parameters.decay_distance_factor, - parameters.heatmap_decay_range.clone(), - ); - } + let own_voronoi_site = ground_to_field.zip( + player_number_cache + .get_latest() + .map(|player_number| *player_number), + ); + if let Some(voronoi_selection) = search_voronoi_selection_from_teammates( + field_dimensions, + own_voronoi_site, + &last_teammate_messages, + now, + hsl_network_parameters + .hsl_state_message_send_interval + .mul_f32(1.2), + ) { + heatmap.update_suggested_search_position_with_voronoi( + field_dimensions, + parameters, + ground_to_field, + &voronoi_selection, + ); + } else { + heatmap.clear_suggested_search_position(); + } - let kernel = create_kernel(parameters.heatmap_convolution_kernel_weight); - heatmap.map = heatmap - .map - .conv(&kernel, ConvMode::Same, PaddingMode::Replicate) - .wrap_err("heatmap convolution failed")?; - heatmap.map /= heatmap.map.sum(); + suggested_search_position_pub + .publish(&heatmap.selected_position(field_dimensions)) + .await?; - if !heatmap.has_decided_for_heatmap_tile { - let suggested_search_index = - heatmap.get_maximum_position(parameters.minimum_validity); - if suggested_search_index.is_some() { - heatmap.has_decided_for_heatmap_tile = true; - } - heatmap.last_maximum_heatmap_position = suggested_search_index; - } else if let Some(last_maximum_heatmap_index) = heatmap.last_maximum_heatmap_position { - let global_max_value = heatmap - .get_maximum_position(0.0) - .map_or(0.0, |idx| heatmap.map[idx]); - let current_tile_value = heatmap.map[last_maximum_heatmap_index]; + additional_heatmap_pub + .publish_if_subscribed(|| async { heatmap.to_message() }) + .await?; + } +} - if current_tile_value < global_max_value * parameters.tile_switch_hysteresis { - heatmap.has_decided_for_heatmap_tile = false; - } - } +fn search_occluders_from_obstacles( + obstacles: &[Obstacle], + ground_to_field: Isometry2, +) -> Vec { + obstacles + .iter() + .filter(|obstacle| obstacle.kind != ObstacleKind::Ball) + .map(|obstacle| SearchOccluder { + center: ground_to_field * obstacle.position, + radius: obstacle + .radius_at_foot_height + .max(obstacle.radius_at_hip_height), + }) + .collect() +} - let suggested_search_position = heatmap.last_maximum_heatmap_position.map(|(x, y)| { - point![ - ((x as f32 + 1.0 / 2.0) / heatmap.cells_per_meter - - field_dimensions.length / 2.0), - ((y as f32 + 1.0 / 2.0) / heatmap.cells_per_meter - - field_dimensions.width / 2.0) - ] - }); - let additional_heatmap = additional_heatmap_pub - .has_subscribers() - .then(|| heatmap.to_message()); +#[derive(Clone, Copy, Debug)] +struct TeammateStateMessage { + received_at: ros_z::time::Time, + state: StateMessage, +} - Ok::<_, color_eyre::eyre::Error>(SearchSuggestorOutput { - suggested_search_position, - additional_heatmap, - }) - })?; +fn search_voronoi_selection_from_teammates( + field_dimensions: FieldDimensions, + own_voronoi_site: Option<(Isometry2, PlayerNumber)>, + last_teammate_messages: &Players>, + now: ros_z::time::Time, + freshness_window: Duration, +) -> Option { + let (ground_to_field, own_player_number) = own_voronoi_site?; + let mut sites = vec![(ground_to_field.as_pose(), own_player_number)]; - if let Some(suggested_search_position) = output.suggested_search_position { - suggested_search_position_pub - .publish(&suggested_search_position) - .await?; + for (player_number, teammate_message) in last_teammate_messages.iter() { + let Some(teammate_message) = teammate_message else { + continue; + }; + if player_number == own_player_number || now < teammate_message.received_at { + continue; } - - if let Some(additional_heatmap) = output.additional_heatmap { - additional_heatmap_pub.publish(&additional_heatmap).await?; + if now.duration_since(teammate_message.received_at) <= freshness_window { + sites.push((teammate_message.state.pose, player_number)); } } + + Some(SearchVoronoiSelection::new( + field_dimensions, + own_player_number, + sites, + )) +} + +fn decay_with_teammate_message( + heatmap: &mut Heatmap, + field_dimensions: FieldDimensions, + message: TimeWrapper, + last_teammate_messages: &mut Players>, + parameters: &SearchSuggestorParameters, + hsl_network_parameters: &HslNetworkParameters, + local_tick_duration: Duration, +) { + let TimeWrapper { + time, + inner: IncomingMessage::Hsl(HulkMessage::State(state)), + } = message + else { + return; + }; + + if let Some(previous) = last_teammate_messages[state.player_number] + && time >= previous.received_at + && let elapsed_since_previous_message = time.duration_since(previous.received_at) + && elapsed_since_previous_message + <= hsl_network_parameters + .hsl_state_message_send_interval + .mul_f32(1.2) + { + let tick_count = replay_tick_count(elapsed_since_previous_message, local_tick_duration); + heatmap.decay_tiles_from_teammate_motion( + field_dimensions, + &previous.state, + &state, + tick_count, + parameters, + ); + } + + last_teammate_messages[state.player_number] = Some(TeammateStateMessage { + received_at: time, + state, + }); } -fn create_kernel(alpha: f32) -> Array2 { - array![ - [alpha, alpha, alpha], - [alpha, 1.0 - alpha, alpha], - [alpha, alpha, alpha] - ] / (1.0 + 7.0 * alpha) +fn replay_tick_count(message_elapsed: Duration, local_tick_duration: Duration) -> usize { + let local_tick_duration = local_tick_duration.max(Duration::from_millis(1)); + (message_elapsed.as_secs_f32() / local_tick_duration.as_secs_f32()) + .ceil() + .max(1.0) as usize } diff --git a/crates/nodes/world_state_composer/src/lib.rs b/crates/nodes/world_state_composer/src/lib.rs index 85275fc571..ca9c75f668 100644 --- a/crates/nodes/world_state_composer/src/lib.rs +++ b/crates/nodes/world_state_composer/src/lib.rs @@ -74,7 +74,7 @@ async fn run(ctx: Arc) -> Result<()> { .build() .await?; let _suggested_search_position_sub = node - .subscriber::>("suggested_search_position") + .subscriber::>>("suggested_search_position") .build() .await?; let _world_state_pub = node.publisher::("world_state").build().await?; diff --git a/crates/search_heatmap/Cargo.toml b/crates/search_heatmap/Cargo.toml new file mode 100644 index 0000000000..31a29a0a60 --- /dev/null +++ b/crates/search_heatmap/Cargo.toml @@ -0,0 +1,19 @@ +[package] +name = "search_heatmap" +version.workspace = true +edition.workspace = true +license.workspace = true +homepage.workspace = true + +[dependencies] +coordinate_systems = { workspace = true } +geometry = { workspace = true } +hsl_network_messages = { workspace = true } +itertools = { workspace = true } +linear_algebra = { workspace = true } +nalgebra = { workspace = true } +ndarray = { workspace = true } +ros-z = { workspace = true } +serde = { workspace = true, features = ["derive"] } +types = { workspace = true } +voronoi = { workspace = true } diff --git a/crates/search_heatmap/src/lib.rs b/crates/search_heatmap/src/lib.rs new file mode 100644 index 0000000000..ecf2203fd7 --- /dev/null +++ b/crates/search_heatmap/src/lib.rs @@ -0,0 +1,764 @@ +use std::{f32::consts::LN_2, ops::Range, time::Duration, time::SystemTime}; + +use coordinate_systems::{Field, Ground}; +use geometry::direction::{Direction, Rotate90Degrees}; +use hsl_network_messages::{HulkMessage, PlayerNumber, StateMessage, SubState, Team}; +use itertools::Itertools; +use linear_algebra::{Isometry2, Point2, Pose2, Vector2, point, vector}; +use nalgebra::clamp; +use ndarray::Array2; +use ros_z::time::Time; +use serde::{Deserialize, Serialize}; +use types::{ + ball_position::{BallPosition, HypotheticalBallPosition}, + field_dimensions::{FieldDimensions, Half, Side}, + filtered_game_controller_state::FilteredGameControllerState, + heatmap::Heatmap as HeatmapMessage, + messages::IncomingMessage, + parameters::{SearchSuggestorParameters, VoronoiParameters}, + primary_state::PrimaryState, + time_wrapper::TimeWrapper, +}; +use voronoi::{Ownership, VoronoiBounds, VoronoiGrid}; + +#[derive(Clone, Copy, Debug)] +pub struct SearchOccluder { + pub center: Point2, + pub radius: f32, +} + +struct FieldOfViewDecay<'a> { + distance_factor: f32, + range: Range, + sampled_tick_count: usize, + occluders: &'a [SearchOccluder], + occluded_factor: f32, +} + +#[derive(Clone, Debug)] +pub struct SearchVoronoiSelection { + pub owner: PlayerNumber, + pub grid: VoronoiGrid, +} + +impl SearchVoronoiSelection { + pub fn new( + field_dimensions: FieldDimensions, + owner: PlayerNumber, + sites: impl IntoIterator, PlayerNumber)>, + ) -> Self { + let mut grid = search_voronoi_grid(field_dimensions); + grid.multi_source_dijkstra(&sites.into_iter().collect_vec(), 0.0); + Self { owner, grid } + } + + fn owns_index(&self, field_dimensions: FieldDimensions, index: (usize, usize)) -> bool { + self.grid + .ownership_at(heatmap_tile_center(field_dimensions, index)) + .is_some_and(|ownership| ownership == Ownership::Robot(self.owner)) + } +} + +#[derive(Clone, Debug, Deserialize, Serialize)] +pub struct Heatmap { + map: Array2, + last_maximum_heatmap_position: Option<(usize, usize)>, + has_decided_for_heatmap_tile: bool, +} + +impl Heatmap { + pub fn new(field_dimensions: FieldDimensions) -> Self { + let (length, width) = heatmap_dimensions(field_dimensions); + Self { + map: Array2::zeros((length, width)), + last_maximum_heatmap_position: None, + has_decided_for_heatmap_tile: false, + } + } + + pub fn to_message(&self) -> HeatmapMessage { + let (length, width) = self.map.dim(); + HeatmapMessage { + length: length as u32, + width: width as u32, + values: self.map.iter().copied().collect(), + } + } + + pub fn selected_position(&self, field_dimensions: FieldDimensions) -> Option> { + self.last_maximum_heatmap_position + .map(|index| heatmap_tile_center(field_dimensions, index)) + } + + pub fn set_known_ball_position( + &mut self, + field_dimensions: FieldDimensions, + ball_position: Point2, + ) { + let heatmap_point = self.field_to_heatmap(field_dimensions, ball_position); + let (length, width) = self.map.dim(); + self.map.fill(0.0); + for x in + heatmap_point.0.saturating_sub(1)..=heatmap_point.0.saturating_add(1).min(length - 1) + { + for y in + heatmap_point.1.saturating_sub(1)..=heatmap_point.1.saturating_add(1).min(width - 1) + { + if (x, y) != heatmap_point { + self.map[(x, y)] = 0.5; + } + } + } + self.map[heatmap_point] = 1.0; + self.last_maximum_heatmap_position = Some(heatmap_point); + self.has_decided_for_heatmap_tile = true; + } + + pub fn update_with_hypothetical_ball_positions<'a>( + &mut self, + field_dimensions: FieldDimensions, + hypothetical_ball_positions: impl IntoIterator>, + ground_to_field: Isometry2, + parameters: &SearchSuggestorParameters, + ) { + for ball_hypothesis in hypothetical_ball_positions { + let ball_hypothesis_position = ground_to_field * ball_hypothesis.position; + let heatmap_point = self.field_to_heatmap(field_dimensions, ball_hypothesis_position); + self.map[heatmap_point] = (self.map[heatmap_point] + + ball_hypothesis.validity * parameters.own_ball_weight) + / 2.0; + } + } + + pub fn update_with_rule_ball( + &mut self, + filtered_game_controller_state: &FilteredGameControllerState, + field_dimensions: FieldDimensions, + primary_state: PrimaryState, + parameters: &SearchSuggestorParameters, + ) { + if self.regenerate_restart_hypotheses( + filtered_game_controller_state, + field_dimensions, + parameters.rule_ball_weight_increment, + ) { + return; + } + + for rule_ball_hypothesis in get_rule_hypotheses( + primary_state, + filtered_game_controller_state, + field_dimensions, + ) { + let heatmap_point = self.field_to_heatmap(field_dimensions, rule_ball_hypothesis); + self.map[heatmap_point] += parameters.rule_ball_weight_increment; + } + } + + pub fn update_with_team_ball( + &mut self, + field_dimensions: FieldDimensions, + network_message: TimeWrapper, + parameters: &SearchSuggestorParameters, + ) { + let IncomingMessage::Hsl(message) = network_message.inner else { + return; + }; + self.add_team_ball( + field_dimensions, + network_message.time.to_wallclock(), + message, + parameters.team_ball_weight, + ); + } + + fn add_team_ball( + &mut self, + field_dimensions: FieldDimensions, + time: SystemTime, + message: HulkMessage, + team_ball_weight: f32, + ) { + let ball = match message { + HulkMessage::State(StateMessage { ball_position, .. }) => { + ball_position.map(|ball| BallPosition { + position: ball.position, + velocity: Vector2::zeros(), + last_seen: Time::from_wallclock(time) - ball.age, + }) + } + }; + if let Some(ball_position) = ball { + let heatmap_point = self.field_to_heatmap(field_dimensions, ball_position.position); + self.map[heatmap_point] = team_ball_weight; + } + } + + pub fn clamp_values(&mut self) { + self.map.mapv_inplace(|value| { + if value.is_nan() { + 0.0 + } else { + value.clamp(0.0, 1.0) + } + }); + } + + pub fn increase_around_last_ball( + &mut self, + field_dimensions: FieldDimensions, + last_ball_position: Point2, + elapsed: Duration, + parameters: &SearchSuggestorParameters, + ) { + let rise_time = parameters.last_ball_priority_rise_time.as_secs_f32(); + let rise_fraction = if rise_time <= f32::EPSILON { + 1.0 + } else { + elapsed.as_secs_f32() / rise_time + }; + if rise_fraction <= 0.0 { + return; + } + + let half_distance = parameters + .last_ball_priority_half_distance + .max(f32::EPSILON); + self.map.indexed_iter_mut().for_each(|((x, y), value)| { + let tile_center_in_field = heatmap_tile_center(field_dimensions, (x, y)); + let distance_to_last_ball = (tile_center_in_field - last_ball_position).norm(); + let gaussian = (-LN_2 * (distance_to_last_ball / half_distance).powi(2)).exp(); + let multiplier = parameters.last_ball_priority_minimum + + (parameters.last_ball_priority_maximum - parameters.last_ball_priority_minimum) + * gaussian; + *value += multiplier * rise_fraction; + }); + } + + fn get_maximum_position_matching( + &self, + minimum_validity: f32, + matches_selection: impl Fn((usize, usize)) -> bool, + ) -> Option<(usize, usize)> { + self.map + .indexed_iter() + .filter(|(index, value)| **value > minimum_validity && matches_selection(*index)) + .max_by(|(_, a), (_, b)| a.total_cmp(b)) + .map(|(index, _)| index) + } + + fn get_maximum_position_with_turn_preference_matching( + &self, + minimum_validity: f32, + turn_preference_priority_margin: f32, + field_dimensions: FieldDimensions, + robot_position: Vector2, + robot_heading: Vector2, + matches_selection: impl Fn((usize, usize)) -> bool, + ) -> Option<(usize, usize)> { + let maximum_position = + self.get_maximum_position_matching(minimum_validity, &matches_selection)?; + let maximum_value = self.map[maximum_position]; + self.map + .indexed_iter() + .filter(|(index, value)| { + matches_selection(*index) + && **value > minimum_validity + && **value >= maximum_value - turn_preference_priority_margin.max(0.0) + }) + .max_by(|(a_index, a_value), (b_index, b_value)| { + let a_turn_score = + turn_score(*a_index, field_dimensions, robot_position, robot_heading); + let b_turn_score = + turn_score(*b_index, field_dimensions, robot_position, robot_heading); + a_turn_score + .total_cmp(&b_turn_score) + .then_with(|| a_value.total_cmp(b_value)) + }) + .map(|(index, _)| index) + } + + fn get_suggested_search_index( + &self, + field_dimensions: FieldDimensions, + parameters: &SearchSuggestorParameters, + ground_to_field: Option>, + voronoi_selection: Option<&SearchVoronoiSelection>, + ) -> Option<(usize, usize)> { + let matches_selection = |index| { + voronoi_selection + .map(|selection| selection.owns_index(field_dimensions, index)) + .unwrap_or(true) + }; + ground_to_field.map_or_else( + || self.get_maximum_position_matching(parameters.minimum_validity, matches_selection), + |ground_to_field| { + let robot_position = ground_to_field.as_pose().position().coords(); + let robot_heading_angle = ground_to_field.orientation().angle(); + let robot_heading = vector![robot_heading_angle.cos(), robot_heading_angle.sin()]; + self.get_maximum_position_with_turn_preference_matching( + parameters.minimum_validity, + parameters.turn_preference_priority_margin, + field_dimensions, + robot_position, + robot_heading, + matches_selection, + ) + }, + ) + } + + pub fn update_suggested_search_position_with_voronoi( + &mut self, + field_dimensions: FieldDimensions, + parameters: &SearchSuggestorParameters, + ground_to_field: Option>, + voronoi_selection: &SearchVoronoiSelection, + ) { + self.update_suggested_search_position_with_optional_voronoi( + field_dimensions, + parameters, + ground_to_field, + Some(voronoi_selection), + ); + } + + pub fn clear_suggested_search_position(&mut self) { + self.last_maximum_heatmap_position = None; + self.has_decided_for_heatmap_tile = false; + } + + fn update_suggested_search_position_with_optional_voronoi( + &mut self, + field_dimensions: FieldDimensions, + parameters: &SearchSuggestorParameters, + ground_to_field: Option>, + voronoi_selection: Option<&SearchVoronoiSelection>, + ) { + if !self.has_decided_for_heatmap_tile { + let suggested_search_index = self.get_suggested_search_index( + field_dimensions, + parameters, + ground_to_field, + voronoi_selection, + ); + if suggested_search_index.is_some() { + self.has_decided_for_heatmap_tile = true; + } + self.last_maximum_heatmap_position = suggested_search_index; + } else if let Some(last_maximum_heatmap_index) = self.last_maximum_heatmap_position { + let current_tile_is_selectable = voronoi_selection + .map(|selection| selection.owns_index(field_dimensions, last_maximum_heatmap_index)) + .unwrap_or(true); + let global_max_value = self + .get_maximum_position_matching(0.0, |index| { + voronoi_selection + .map(|selection| selection.owns_index(field_dimensions, index)) + .unwrap_or(true) + }) + .map_or(0.0, |idx| self.map[idx]); + let current_tile_value = self.map[last_maximum_heatmap_index]; + + if !current_tile_is_selectable + || current_tile_value <= parameters.minimum_validity + || current_tile_value < global_max_value * parameters.tile_switch_hysteresis + { + let suggested_search_index = self.get_suggested_search_index( + field_dimensions, + parameters, + ground_to_field, + voronoi_selection, + ); + self.has_decided_for_heatmap_tile = suggested_search_index.is_some(); + self.last_maximum_heatmap_position = suggested_search_index; + } + } + } + + fn decay_tiles_in_fov_for_sampled_ticks( + &mut self, + field_dimensions: FieldDimensions, + robot_position: Vector2, + left_edge: Vector2, + right_edge: Vector2, + decay: FieldOfViewDecay<'_>, + ) { + let sampled_tick_count = decay.sampled_tick_count.max(1).min(i32::MAX as usize) as i32; + self.map.indexed_iter_mut().for_each(|((x, y), value)| { + let tile_center_in_field: Vector2 = vector![ + (x as f32 + 0.5 - field_dimensions.length / 2.0), + (y as f32 + 0.5 - field_dimensions.width / 2.0) + ]; + let robot_to_tile = tile_center_in_field - robot_position; + let is_inside_sight = get_direction(left_edge, robot_to_tile) + == Direction::Counterclockwise + && get_direction(right_edge, robot_to_tile) == Direction::Clockwise; + let distance_to_tile = robot_to_tile.norm(); + let relative_distance_to_tile = clamp(distance_to_tile / decay.range.end, 0.0, 1.0); + if is_inside_sight && decay.range.contains(&distance_to_tile) { + let occlusion_factor = + if is_occluded(robot_position, tile_center_in_field, decay.occluders) { + decay.occluded_factor.clamp(0.0, 1.0) + } else { + 1.0 + }; + let per_tick_decay = + decay.distance_factor * occlusion_factor * (1.0 - relative_distance_to_tile); + let effective_decay = 1.0 - (1.0 - per_tick_decay).powi(sampled_tick_count); + *value *= 1.0 - effective_decay; + } + }); + } + + fn decay_tiles_from_field_pose_and_heading_for_sampled_ticks( + &mut self, + field_dimensions: FieldDimensions, + pose: Pose2, + heading_direction: Vector2, + decay: FieldOfViewDecay<'_>, + ) { + if heading_direction.norm() <= f32::EPSILON { + return; + } + + let robot_position = pose.position().coords(); + let heading_angle = heading_direction.y().atan2(heading_direction.x()); + let fov_angle_offset = 45.0_f32.to_radians(); + let left_angle = heading_angle - fov_angle_offset; + let right_angle = heading_angle + fov_angle_offset; + let left_edge: Vector2 = vector![left_angle.cos(), left_angle.sin()]; + let right_edge: Vector2 = vector![right_angle.cos(), right_angle.sin()]; + + self.decay_tiles_in_fov_for_sampled_ticks( + field_dimensions, + robot_position, + left_edge, + right_edge, + decay, + ); + } + + pub fn decay_tiles_from_teammate_motion( + &mut self, + field_dimensions: FieldDimensions, + previous: &StateMessage, + current: &StateMessage, + tick_count: usize, + parameters: &SearchSuggestorParameters, + ) { + let tick_count = tick_count.max(1); + let replay_stride = parameters.teammate_replay_stride.max(1); + for chunk_start in (0..tick_count).step_by(replay_stride) { + let sampled_tick_count = (tick_count - chunk_start).min(replay_stride); + let alpha = (chunk_start as f32 + sampled_tick_count as f32 * 0.5) / tick_count as f32; + let (pose, heading_direction) = + interpolate_teammate_pose_and_heading(previous, current, alpha); + self.decay_tiles_from_field_pose_and_heading_for_sampled_ticks( + field_dimensions, + pose, + heading_direction, + FieldOfViewDecay { + distance_factor: teammate_decay_factor(parameters), + range: parameters.heatmap_decay_range.clone(), + sampled_tick_count, + occluders: &[], + occluded_factor: 1.0, + }, + ); + } + } + + pub fn decay_tiles_in_robot_fov_with_occluders( + &mut self, + field_dimensions: FieldDimensions, + ground_to_field: Isometry2, + parameters: &SearchSuggestorParameters, + occluders: &[SearchOccluder], + ) { + let body_orientation = ground_to_field.orientation().angle(); + let heading_direction: Vector2 = + vector![body_orientation.cos(), body_orientation.sin()]; + self.decay_tiles_from_field_pose_and_heading_for_sampled_ticks( + field_dimensions, + ground_to_field.as_pose(), + heading_direction, + FieldOfViewDecay { + distance_factor: parameters.decay_distance_factor, + range: parameters.heatmap_decay_range.clone(), + sampled_tick_count: 1, + occluders, + occluded_factor: parameters.occluded_decay_factor, + }, + ); + } + + fn field_to_heatmap( + &self, + field_dimensions: FieldDimensions, + field_point: Point2, + ) -> (usize, usize) { + let heatmap_point = ( + (field_point.x() + field_dimensions.length / 2.0).floor(), + (field_point.y() + field_dimensions.width / 2.0).floor(), + ); + ( + clamp(heatmap_point.0, 0.0, (self.map.dim().0 - 1) as f32) as usize, + clamp(heatmap_point.1, 0.0, (self.map.dim().1 - 1) as f32) as usize, + ) + } + + pub fn regenerate_restart_hypotheses( + &mut self, + filtered_game_controller_state: &FilteredGameControllerState, + field_dimensions: FieldDimensions, + increment: f32, + ) -> bool { + let Some(restart_indices) = + self.restart_hypothesis_indices(filtered_game_controller_state, field_dimensions) + else { + return false; + }; + self.map.indexed_iter_mut().for_each(|(index, value)| { + if !restart_indices.contains(&index) { + *value = 0.0; + } + }); + for index in restart_indices { + self.map[index] += increment; + } + true + } + + fn restart_hypothesis_indices( + &self, + filtered_game_controller_state: &FilteredGameControllerState, + field_dimensions: FieldDimensions, + ) -> Option> { + let mut indices: Vec<_> = match filtered_game_controller_state.sub_state { + Some(SubState::CornerKick) => corner_kick_hypotheses( + filtered_game_controller_state.kicking_team, + field_dimensions, + ) + .into_iter() + .map(|hypothesis| self.field_to_heatmap(field_dimensions, hypothesis)) + .collect(), + Some(SubState::GoalKick) => goal_kick_hypotheses( + filtered_game_controller_state.kicking_team, + field_dimensions, + ) + .into_iter() + .map(|hypothesis| self.field_to_heatmap(field_dimensions, hypothesis)) + .collect(), + Some(SubState::ThrowIn) => { + let last_y = self.map.dim().1 - 1; + (0..self.map.dim().0) + .flat_map(|x| [(x, 0), (x, last_y)]) + .collect() + } + _ => return None, + }; + + indices.sort_unstable(); + indices.dedup(); + Some(indices) + } +} + +fn teammate_decay_factor(parameters: &SearchSuggestorParameters) -> f32 { + if parameters.teammate_decay_factor > 0.0 { + parameters.teammate_decay_factor + } else { + parameters.decay_distance_factor * 0.25 + } +} + +fn interpolate_teammate_pose_and_heading( + previous: &StateMessage, + current: &StateMessage, + alpha: f32, +) -> (Pose2, Vector2) { + let alpha = alpha.clamp(0.0, 1.0); + let previous_position = previous.pose.position(); + let current_position = current.pose.position(); + let interpolated_position = point![ + previous_position.x() + (current_position.x() - previous_position.x()) * alpha, + previous_position.y() + (current_position.y() - previous_position.y()) * alpha + ]; + let previous_heading = previous.pose.orientation().angle() + previous.head_yaw; + let current_heading = current.pose.orientation().angle() + current.head_yaw; + let interpolated_heading = + previous_heading + normalize_angle(current_heading - previous_heading) * alpha; + let heading_direction = vector![interpolated_heading.cos(), interpolated_heading.sin()]; + + ( + Pose2::new(interpolated_position, interpolated_heading), + heading_direction, + ) +} + +fn normalize_angle(angle: f32) -> f32 { + angle.sin().atan2(angle.cos()) +} + +fn heatmap_dimensions(field_dimensions: FieldDimensions) -> (usize, usize) { + ( + field_dimensions.length.ceil().max(1.0) as usize, + field_dimensions.width.ceil().max(1.0) as usize, + ) +} + +fn search_voronoi_grid(field_dimensions: FieldDimensions) -> VoronoiGrid { + let (length, width) = heatmap_dimensions(field_dimensions); + let grid_min = point![ + -field_dimensions.length / 2.0, + -field_dimensions.width / 2.0 + ]; + let grid_max = point![grid_min.x() + length as f32, grid_min.y() + width as f32]; + VoronoiGrid::new( + VoronoiBounds { + grid_min, + grid_max, + centroid_min: grid_min, + centroid_max: grid_max, + }, + VoronoiParameters { + grid_resolution: 1.0, + ..Default::default() + }, + ) +} + +fn heatmap_tile_center(field_dimensions: FieldDimensions, (x, y): (usize, usize)) -> Point2 { + point![ + x as f32 + 0.5 - field_dimensions.length / 2.0, + y as f32 + 0.5 - field_dimensions.width / 2.0 + ] +} + +fn get_rule_hypotheses( + primary_state: PrimaryState, + filtered_game_controller_state: &FilteredGameControllerState, + field_dimensions: FieldDimensions, +) -> Vec> { + match (primary_state, filtered_game_controller_state.sub_state) { + (PrimaryState::Ready, Some(SubState::PenaltyKick)) => { + let kicking_team_half = kicking_team_half(filtered_game_controller_state.kicking_team) + .unwrap_or(Half::Own) + .mirror(); + vec![field_dimensions.penalty_spot(kicking_team_half)] + } + (PrimaryState::Ready, None) => vec![field_dimensions.center()], + (PrimaryState::Playing, Some(SubState::CornerKick)) => corner_kick_hypotheses( + filtered_game_controller_state.kicking_team, + field_dimensions, + ), + (PrimaryState::Playing, Some(SubState::GoalKick)) => goal_kick_hypotheses( + filtered_game_controller_state.kicking_team, + field_dimensions, + ), + (_, _) => Vec::new(), + } +} + +fn corner_kick_hypotheses( + kicking_team: Option, + field_dimensions: FieldDimensions, +) -> Vec> { + if let Some(kicking_team_half) = kicking_team_half(kicking_team) { + let kicking_team_half = kicking_team_half.mirror(); + vec![ + field_dimensions.corner(kicking_team_half, Side::Left), + field_dimensions.corner(kicking_team_half, Side::Right), + ] + } else { + vec![ + field_dimensions.corner(Half::Own, Side::Left), + field_dimensions.corner(Half::Opponent, Side::Left), + field_dimensions.corner(Half::Own, Side::Right), + field_dimensions.corner(Half::Opponent, Side::Right), + ] + } +} + +fn goal_kick_hypotheses( + kicking_team: Option, + field_dimensions: FieldDimensions, +) -> Vec> { + if let Some(kicking_team_half) = kicking_team_half(kicking_team) { + vec![ + field_dimensions.goal_box_corner(kicking_team_half, Side::Left), + field_dimensions.goal_box_corner(kicking_team_half, Side::Right), + ] + } else { + vec![ + field_dimensions.goal_box_corner(Half::Own, Side::Left), + field_dimensions.goal_box_corner(Half::Opponent, Side::Left), + field_dimensions.goal_box_corner(Half::Own, Side::Right), + field_dimensions.goal_box_corner(Half::Opponent, Side::Right), + ] + } +} + +fn kicking_team_half(kicking_team: Option) -> Option { + match kicking_team { + Some(Team::Opponent) => Some(Half::Opponent), + Some(Team::Hulks) => Some(Half::Own), + None => None, + } +} + +fn turn_score( + (x, y): (usize, usize), + field_dimensions: FieldDimensions, + robot_position: Vector2, + robot_heading: Vector2, +) -> f32 { + let tile_center: Vector2 = vector![ + x as f32 + 0.5 - field_dimensions.length / 2.0, + y as f32 + 0.5 - field_dimensions.width / 2.0 + ]; + let robot_to_tile = tile_center - robot_position; + let robot_to_tile_norm = robot_to_tile.norm(); + let robot_heading_norm = robot_heading.norm(); + if robot_to_tile_norm <= f32::EPSILON || robot_heading_norm <= f32::EPSILON { + return 1.0; + } + robot_heading.dot(&robot_to_tile) / (robot_heading_norm * robot_to_tile_norm) +} + +fn is_occluded( + observer: Vector2, + tile_center: Vector2, + occluders: &[SearchOccluder], +) -> bool { + let observer_to_tile = tile_center - observer; + let tile_distance_squared = observer_to_tile.norm_squared(); + if tile_distance_squared <= f32::EPSILON { + return false; + } + + occluders.iter().any(|occluder| { + if occluder.radius <= 0.0 { + return false; + } + let observer_to_occluder = occluder.center.coords() - observer; + let projection = observer_to_occluder.dot(&observer_to_tile) / tile_distance_squared; + if !(0.0..1.0).contains(&projection) { + return false; + } + let closest_point = observer + observer_to_tile * projection; + (occluder.center.coords() - closest_point).norm() <= occluder.radius + }) +} + +fn get_direction(base_vector: Vector2, vector_to_test: Vector2) -> Direction { + let clockwise_normal_vector = base_vector.rotate_90_degrees(Direction::Clockwise); + let directed_cathetus = clockwise_normal_vector.dot(&vector_to_test); + + match directed_cathetus { + 0.0 => Direction::Collinear, + f if f > 0.0 => Direction::Clockwise, + f if f < 0.0 => Direction::Counterclockwise, + f => panic!("directed cathetus was not a real number: {f}"), + } +} diff --git a/crates/types/src/parameters.rs b/crates/types/src/parameters.rs index d953b28c74..4cd4642ae8 100644 --- a/crates/types/src/parameters.rs +++ b/crates/types/src/parameters.rs @@ -91,6 +91,7 @@ pub struct BehaviorParameters { pub goal_keeper_number: PlayerNumber, pub keeper: KeeperParameters, pub last_ball_timeout: Duration, + pub team_ball_timeout: Duration, pub lost_ball: LostBallParameters, pub path_planning: PathPlanningParameters, pub remote_control: RemoteControlParameters, @@ -536,15 +537,20 @@ pub struct CameraMatrixParameters { ros_z::Message, )] pub struct SearchSuggestorParameters { - pub cells_per_meter: f32, - pub heatmap_convolution_kernel_weight: f32, pub minimum_validity: f32, pub own_ball_weight: f32, pub team_ball_weight: f32, - pub rule_ball_weight: f32, pub rule_ball_weight_increment: f32, + pub last_ball_priority_minimum: f32, + pub last_ball_priority_maximum: f32, + pub last_ball_priority_half_distance: f32, + pub last_ball_priority_rise_time: Duration, + pub turn_preference_priority_margin: f32, pub tile_switch_hysteresis: f32, pub decay_distance_factor: f32, + pub occluded_decay_factor: f32, + pub teammate_decay_factor: f32, + pub teammate_replay_stride: usize, pub heatmap_decay_range: Range, } diff --git a/crates/types/src/world_state.rs b/crates/types/src/world_state.rs index c3a8db27fe..ad8a578955 100644 --- a/crates/types/src/world_state.rs +++ b/crates/types/src/world_state.rs @@ -56,6 +56,25 @@ impl Default for WorldState { } } +#[derive( + Clone, + Copy, + Debug, + Default, + Serialize, + Deserialize, + PathSerialize, + PathDeserialize, + PathIntrospect, + PartialEq, + ros_z::Message, +)] +pub enum BallSource { + #[default] + Own, + Team, +} + #[derive( Clone, Copy, @@ -74,6 +93,7 @@ pub struct BallState { pub ball_in_ground_velocity: Vector2, pub last_seen_ball: SystemTime, pub field_side: Side, + pub source: BallSource, } #[derive( @@ -101,6 +121,7 @@ impl Default for BallState { ball_in_ground_velocity: Vector2::zeros(), last_seen_ball: UNIX_EPOCH, field_side: Side::Left, + source: BallSource::Own, } } } @@ -113,6 +134,7 @@ impl BallState { ball_in_ground_velocity: Vector2::zeros(), last_seen_ball: UNIX_EPOCH, field_side: Side::Left, + source: BallSource::Own, } } } diff --git a/etc/parameters/default.json b/etc/parameters/default.json index 0ac14f0c61..595fe5f425 100644 --- a/etc/parameters/default.json +++ b/etc/parameters/default.json @@ -404,9 +404,13 @@ } }, "last_ball_timeout": { - "nanos": 250000000, + "nanos": 500000000, "secs": 0 }, + "team_ball_timeout": { + "nanos": 0, + "secs": 1 + }, "walk_with_velocity": { "max_velocity": 1.0, "max_angular_velocity": 1.0, @@ -564,6 +568,30 @@ "nanos": 0, "secs": 2 } + }, + "search_suggestor": { + "minimum_validity": 0.4, + "own_ball_weight": 1.0, + "team_ball_weight": 1.0, + "rule_ball_weight": 1.0, + "rule_ball_weight_increment": 0.00125, + "last_ball_priority_minimum": 0.5, + "last_ball_priority_maximum": 1.0, + "last_ball_priority_half_distance": 2.0, + "last_ball_priority_rise_time": { + "nanos": 0, + "secs": 20 + }, + "turn_preference_priority_margin": 0.1, + "decay_distance_factor": 0.06, + "occluded_decay_factor": 0.05, + "teammate_decay_factor": 0.015, + "teammate_replay_stride": 20, + "heatmap_decay_range": { + "start": 0.0, + "end": 3.0 + }, + "tile_switch_hysteresis": 0.4 } }, "field_dimensions": { @@ -1062,21 +1090,6 @@ "pitch": 0.5 } }, - "search_suggestor": { - "cells_per_meter": 2.0, - "heatmap_convolution_kernel_weight": 0.001007049, - "minimum_validity": 0.1, - "own_ball_weight": 1.0, - "team_ball_weight": 1.0, - "rule_ball_weight": 1.0, - "rule_ball_weight_increment": 0.005, - "decay_distance_factor": 0.03, - "heatmap_decay_range": { - "start": 0.0, - "end": 3.0 - }, - "tile_switch_hysteresis": 0.5 - }, "rule_obstacles": { "center_circle_obstacle_radius_increase": 0.2, "center_circle_ballspace_free_obstacle_radius": 0.2, diff --git a/etc/parameters/ros_z/base/behavior_node.json5 b/etc/parameters/ros_z/base/behavior_node.json5 index 1be8e77936..4d5228af43 100644 --- a/etc/parameters/ros_z/base/behavior_node.json5 +++ b/etc/parameters/ros_z/base/behavior_node.json5 @@ -9,7 +9,8 @@ stand_up: { nanos: 500000000, secs: 0 }, walk: { nanos: 500000000, secs: 0 }, }, - last_ball_timeout: { nanos: 250000000, secs: 0 }, + last_ball_timeout: { nanos: 500000000, secs: 0 }, + team_ball_timeout: { nanos: 0, secs: 1 }, optional_roles: ["Defender", "Midfielder", "StrikerSupporter"], walk_with_velocity: { max_velocity: 1.0, diff --git a/etc/parameters/ros_z/base/global.json5 b/etc/parameters/ros_z/base/global.json5 index 1f7f14b47f..696cde4df2 100644 --- a/etc/parameters/ros_z/base/global.json5 +++ b/etc/parameters/ros_z/base/global.json5 @@ -18,4 +18,23 @@ goal_post_diameter: 0.1 }, player_number: "Three", + hsl_network: { + game_controller_return_message_interval: { + nanos: 0, + secs: 1, + }, + remaining_amount_of_messages_to_stop_sending: 20, + silence_interval_between_messages: { + nanos: 0, + secs: 1, + }, + hsl_striker_message_receive_timeout: { + nanos: 0, + secs: 3, + }, + hsl_state_message_send_interval: { + nanos: 300000000, + secs: 0, + }, + }, } diff --git a/etc/parameters/ros_z/base/search_suggestor.json5 b/etc/parameters/ros_z/base/search_suggestor.json5 index 959c6b0da0..dcd19fc9ee 100644 --- a/etc/parameters/ros_z/base/search_suggestor.json5 +++ b/etc/parameters/ros_z/base/search_suggestor.json5 @@ -1,15 +1,25 @@ { - cells_per_meter: 2.0, - heatmap_convolution_kernel_weight: 0.001007049, - minimum_validity: 0.01, - own_ball_weight: 1.0, - team_ball_weight: 1.0, - rule_ball_weight: 1.0, - rule_ball_weight_increment: 0.005, - decay_distance_factor: 0.03, - heatmap_decay_range: { - start: 0.0, - end: 3.0 + search_suggestor: { + minimum_validity: 0.4, + own_ball_weight: 1.0, + team_ball_weight: 1.0, + rule_ball_weight_increment: 0.00125, + last_ball_priority_minimum: 0.5, + last_ball_priority_maximum: 1.0, + last_ball_priority_half_distance: 2.0, + last_ball_priority_rise_time: { + nanos: 0, + secs: 20, + }, + turn_preference_priority_margin: 0.1, + decay_distance_factor: 0.06, + occluded_decay_factor: 0.05, + teammate_decay_factor: 0.015, + teammate_replay_stride: 20, + heatmap_decay_range: { + start: 0.0, + end: 3.0, + }, + tile_switch_hysteresis: 0.4, }, - tile_switch_hysteresis: 0.5 }