add agressive and defensive ready positions depending on robot availability

crates/nodes/behavior_node/src/walk.rs

@@ -1,15 +1,16 @@
 use coordinate_systems::{Field, Ground};
 use filtering::hysteresis::less_than_with_relative_hysteresis;
-use hsl_network_messages::PlayerNumber;
+use hsl_network_messages::{PlayerNumber, Team};
 use linear_algebra::{Isometry2, Orientation2, Point, Point2, Pose2, point};
 use path_planner::path_planner::PathPlanner;
 use types::{
     behavior_tree::Status,
     field_dimensions::FieldDimensions,
     motion_command::{BodyMotion, MotionCommand, OrientationMode},
     motion_type::MotionType,
-    parameters::VoronoiParameters,
+    parameters::{KickOffPose, KickoffParameters, VoronoiParameters},
     path::{Path, direct_path},
+    world_state::WorldState,
 };
 use voronoi::{Ownership, VoronoiGrid};
 
@@ -18,7 +19,6 @@ use crate::{
     actions::stand,
     behavior_tree::Node,
     condition,
-    conditions::hulks_is_kicking_team,
     kick::{kick, select_kick_target, use_last_kick_power},
     node::Blackboard,
     selection, sequence, subtree,
@@ -213,39 +213,65 @@ pub fn walk_to_block_position(blackboard: &mut Blackboard) -> Status {
 }
 
 pub fn walk_to_kickoff_pose(blackboard: &mut Blackboard) -> Status {
-    if let (Some(ground_to_field), player_number) = (
-        blackboard.world_state.robot.ground_to_field,
-        blackboard.world_state.robot.player_number,
-    ) {
-        let field_to_ground = ground_to_field.inverse();
-        let kickoff = &blackboard.parameters.kickoff;
-        let standard_pose = kickoff.standard_positions[player_number];
-        let striker_position = kickoff.striker_position;
-        let walk_and_stand = blackboard.parameters.walking.walk_and_stand;
-        let walk_to_kickoff_speed = blackboard.parameters.walking.speed.walk_to_kickoff;
-
-        let mut target_position = standard_pose.position;
+    let Some(ground_to_field) = blackboard.world_state.robot.ground_to_field else {
+        return Status::Failure;
+    };
+    let Some(kickoff_pose) = select_kickoff_pose(
+        &blackboard.world_state,
+        blackboard.parameters.goalkeeper.player_number,
+        &blackboard.parameters.kickoff,
+    ) else {
+        return Status::Failure;
+    };
 
-        if hulks_is_kicking_team(blackboard) && player_number == PlayerNumber::Three {
-            target_position = striker_position;
-        }
+    let kickoff_pose_in_field = Pose2::from_parts(
+        kickoff_pose.position,
+        Orientation2::new(kickoff_pose.rotation),
+    );
+    let walk_and_stand = blackboard.parameters.walking.walk_and_stand;
+
+    walk_to(
+        blackboard,
+        ground_to_field.inverse() * kickoff_pose_in_field,
+        blackboard.parameters.walking.speed.walk_to_kickoff,
+        OrientationMode::AlignWithPath,
+        walk_and_stand.normal_distance_to_be_aligned,
+        walk_and_stand.hysteresis,
+    )
+}
 
-        let kickoff_pose_in_field =
-            Pose2::from_parts(target_position, Orientation2::new(standard_pose.rotation));
+fn select_kickoff_pose(
+    world_state: &WorldState,
+    goalkeeper_player_number: PlayerNumber,
+    parameters: &KickoffParameters,
+) -> Option<KickOffPose> {
+    let player_number = world_state.robot.player_number;
+    let game_controller_state = world_state.filtered_game_controller_state.as_ref()?;
+    if game_controller_state.penalties[player_number].is_some() {
+        return None;
+    }
 
-        let kickoff_pose_in_ground = field_to_ground * kickoff_pose_in_field;
+    if player_number == goalkeeper_player_number {
+        return Some(parameters.goalkeeper_pose);
+    }
 
-        walk_to(
-            blackboard,
-            kickoff_pose_in_ground,
-            walk_to_kickoff_speed,
-            OrientationMode::AlignWithPath,
-            walk_and_stand.normal_distance_to_be_aligned,
-            walk_and_stand.hysteresis,
-        );
-        Status::Success
+    let field_player_rank = game_controller_state
+        .penalties
+        .iter()
+        .rev()
+        .filter(|(number, penalty)| *number != goalkeeper_player_number && penalty.is_none())
+        .position(|(number, _)| number == player_number)?;
+
+    if game_controller_state.kicking_team == Some(Team::Hulks) {
+        match field_player_rank {
+            0 => Some(parameters.striker_pose),
+            rank => parameters.aggressive_positions.get(rank - 1).copied(),
+        }
     } else {
-        Status::Failure
+        parameters
+            .defensive_positions
+            .get(field_player_rank)
+            .copied()
     }
 }
 

crates/types/src/parameters.rs

@@ -8,7 +8,7 @@ use serde::{Deserialize, Serialize};
 use coordinate_systems::{Camera, Field, Ground, NormalizedPixel, Pixel, Robot};
 use linear_algebra::{Framed, Point2, Vector2, Vector3};
 
-use crate::{field_color::FieldColorParameters, motion_command::MotionCommand, players::Players};
+use crate::{field_color::FieldColorParameters, motion_command::MotionCommand};
 
 #[derive(Clone, Debug, Default, Deserialize, Serialize, ros_z::Message)]
 pub struct WhistleDetectionParameters {
@@ -101,8 +101,12 @@ pub struct KickOffPose {
 
 #[derive(Clone, Debug, Default, Deserialize, Serialize, ros_z::Message)]
 pub struct KickoffParameters {
-    pub striker_position: Point2<Field>,
-    pub standard_positions: Players<KickOffPose>,
+    pub goalkeeper_pose: KickOffPose,
+    pub striker_pose: KickOffPose,
+    /// Own-kickoff support slots, filled in order by descending active player number.
+    pub aggressive_positions: [KickOffPose; 3],
+    /// Opponent-kickoff slots, filled in order by descending active player number.
+    pub defensive_positions: [KickOffPose; 4],
 }
 
 #[derive(Copy, Clone, Debug, Default, Deserialize, Serialize, ros_z::Message)]

docs/robotics/behavior/overview.md

@@ -48,6 +48,36 @@ branches first check the robot's `PrimaryState`:
 
 If no branch succeeds, behavior falls back to a safe standing command.
 
+## Ready Formations
+
+Ready uses static poses from `behavior_node.kickoff`, expressed in field
+coordinates (meters, with the own goal at negative x) and rotations in radians.
+The base poses are starting values for a 9 × 6 m field and can be overridden for
+other field dimensions.
+
+- The player selected by `goalkeeper.player_number` uses `goalkeeper_pose`.
+- On our kickoff, the highest-numbered active field player uses `striker_pose`.
+  The remaining field players fill `aggressive_positions` in array order, from
+  highest to lowest player number.
+- On opponent or unknown kickoff, all field players fill `defensive_positions`
+  in the same order.
+
+The GameController's unpenalized lineup determines the active players.
+Substitutes, sent-off players, and players with another penalty do not occupy
+formation slots. Ready requires a GameController state; a missing state or a
+penalty for the current robot produces the standing fallback. For example, with
+only players 3, 4, and 5 eligible, our kickoff places 5 at `striker_pose`, 4 at
+`aggressive_positions[0]`, and 3 at `aggressive_positions[1]`.
+
+The GameController operator must keep the selected lineup up to date. In the
+behavior simulator, robots absent from the scenario are marked as substitutes;
+penalties explicitly assigned to simulated robots are preserved.
+
+Assignments depend on the available roster and kickoff ownership; the target
+coordinates stay fixed while teammates move. Path planning continues to account
+for obstacles. These assignments apply during Ready; Playing selects its tasks
+using the playing tree below.
+
 ## Playing
 
 The playing subtree assigns one of four high-level tasks:
@@ -106,16 +136,24 @@ teammates and obstacles. If no support position can be produced, it stands.
 
 ## Team Communication
 
-While playing, behavior periodically creates a State message. It contains the
-player number, the robot pose, and the observed ball position and age when a
-ball is available. A message is sent only when the robot has a field pose, the
-send interval has elapsed, and the remaining game message budget is high
-enough.
+During Playing, behavior periodically creates a State message. It
+contains the player number, the robot pose, and the observed ball position and
+age when a ball is available. A message is sent only when the robot has a field
+pose, the send interval has elapsed, and the remaining game message budget is
+high enough.
+
+The interval is configured by `network.hsl_playing_state_message_send_interval`
+(300 ms by default). Received teammate states expire after
+`player_states_receiver.playing_maximum_age` (one second).
+
+Ready formation assignment uses the GameController lineup and sends no team
+State messages. GameController return messages are sent independently during
+Ready and the other states.
 
 Received teammate states provide the poses used for closest-to-ball selection
-and supporter positioning. Team communication can be absent or delayed, so the
-tree retains branches for simple operation, the last active robot, and missing
-ball information.
+and supporter positioning during Playing. Team communication can be absent or
+delayed, so the tree retains branches for simple operation, the last active
+robot, and missing ball information.
 
 ## Motion Output
 

etc/parameters/base/behavior_node.json5

@@ -91,14 +91,21 @@
     penalty_kick_target_y_scale: 0.7,
   },
   kickoff: {
-    striker_position: [-1.5, 0.0],
-    standard_positions: {
-      one: { position: [-4.2, 0.0], rotation: 0 },
-      two: { position: [-3, -0.5], rotation: 0 },
-      three: { position: [-2, 0], rotation: 0 },
-      four: { position: [-2.5, -1.5], rotation: 0.4 },
-      five: { position: [-2.5, 1.5], rotation: -0.4 },
-    },
+    goalkeeper_pose: { position: [-4.2, 0.0], rotation: 0.0 },
+    striker_pose: { position: [-0.6, 0.0], rotation: 0.0 },
+    // Remaining players fill these support slots in descending player-number order.
+    aggressive_positions: [
+      { position: [-1.7, 1.5], rotation: -0.7 },
+      { position: [-1.7, -1.5], rotation: 0.7 },
+      { position: [-3.0, 0.0], rotation: 0.0 },
+    ],
+    // On opponent/unknown kickoff, all field players fill these slots in order.
+    defensive_positions: [
+      { position: [-1.8, 0.0], rotation: 0.0 },
+      { position: [-2.8, 1.6], rotation: -0.5 },
+      { position: [-2.8, -1.6], rotation: 0.5 },
+      { position: [-3.2, 0.0], rotation: 0.0 },
+    ],
   },
   voronoi: {
     grid_resolution: 0.2,