Ball search behavior

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"

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,
         }
     }
 }

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<SocketAddr>,
+        head_yaw: f32,
     ) -> Vec<OutgoingMessage> {
         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

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<SimulatorWorldStates>,
     mut robot_frames: ResMut<SimulatorRobotFrames>,
     mut outgoing_messages: ResMut<SimulatorOutgoingMessages>,
-    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,
         })
     }
 

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,
         })
     }
 

crates/hsl_network_messages/src/lib.rs

@@ -63,6 +63,7 @@ pub struct StrikerMessage {
 pub struct StateMessage {
     pub player_number: PlayerNumber,
     pub pose: Pose2<Field>,
+    pub head_yaw: f32,
     pub ball_position: Option<BallPosition<Field>>,
 }
 
@@ -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,

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<Context>) -> Pin<Box<dyn Future<Output = Result<()>> + Send>> {
@@ -99,6 +99,7 @@ async fn run(ctx: Arc<Context>) -> 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<Context>) -> 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<Field>,
     ball_in_ground_velocity: Vector2<Ground>,
     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,
     }
 }

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 }