Searcher Standing Delay (HULKs#1735)

paudar · knoellle · commit 995a2fceaf43 · 2025-07-22T02:11:22.000-03:00
* add timeout for defender to become searcher

move functionality of delayed movement of defender that became earcher in searcher

make the standing time after role change dynamic depending on last known ball posiiton

add senario and fmt

rename variable

undo change of if condition

add new edge detection

change ball position in passive_defender_positioning

* only affect searchers that where defender

* add check for y-position of searcher

* fmt file

* stand only when in own half

* make previous role the real previous role

* forever keeper

* more edge-cases in standing seracher bevi_test

* change bevihavior scenario

* add aditional assertion at standing_searcher bevytest

* add assertions to bevihavior standing_searcher

* change parameter for later search motion
diff --git a/crates/bevyhavior_simulator/src/bin/passive_defender_positioning.rs b/crates/bevyhavior_simulator/src/bin/passive_defender_positioning.rs
@@ -6,7 +6,7 @@ use spl_network_messages::{GameState, PlayerNumber};
 
 use bevyhavior_simulator::{
     ball::BallResource,
-    game_controller::GameControllerCommand,
+    game_controller::{GameController, GameControllerCommand},
     robot::Robot,
     time::{Ticks, TicksTime},
 };
@@ -38,14 +38,15 @@ fn startup(
 
 fn update(
     time: Res<Time<Ticks>>,
+    game_controller: Res<GameController>,
     mut exit: EventWriter<AppExit>,
     mut ball: ResMut<BallResource>,
     mut robots: Query<&mut Robot>,
 ) {
     if time.ticks() == 4500 {
         ball.state = Some(SimulatorBallState {
-            position: point!(2.25, 0.0),
-            velocity: vector![-3.0, -1.0],
+            position: point!(1.25, 2.0),
+            velocity: vector![1.0, -3.0],
         });
     }
     if time.ticks() > 4500 && time.ticks() <= 4700 {
@@ -56,7 +57,7 @@ fn update(
                     match motion_command {
                         MotionCommand::Stand { .. } => {}
                         _ => {
-                            println!("Defenders moved unnecessarily");
+                            println!("Defenders moved unnecessarily because of the following command at tick {}: \n{:?}", time.ticks(), motion_command);
                             exit.send(AppExit::from_code(1));
                         }
                     }
@@ -65,8 +66,12 @@ fn update(
             }
         }
     }
+    if game_controller.state.hulks_team.score > 0 {
+        println!("Done. Goal was scored.");
+        exit.send(AppExit::Success);
+    }
     if time.ticks() >= 8_000 {
-        println!("Done");
+        println!("Done. But no goal was scored");
         exit.send(AppExit::Success);
     }
 }
diff --git a/crates/bevyhavior_simulator/src/bin/standing_searcher.rs b/crates/bevyhavior_simulator/src/bin/standing_searcher.rs
@@ -0,0 +1,115 @@
+use std::time::Duration;
+
+use bevy::prelude::*;
+
+use linear_algebra::{point, vector};
+use scenario::scenario;
+use spl_network_messages::{GameState, Penalty, PlayerNumber, Team};
+
+use bevyhavior_simulator::{
+    ball::BallResource,
+    game_controller::GameControllerCommand,
+    robot::Robot,
+    time::{Ticks, TicksTime},
+};
+use types::{ball_position::SimulatorBallState, motion_command::MotionCommand};
+
+#[scenario]
+fn standing_searcher(app: &mut App) {
+    app.add_systems(Startup, startup);
+    app.add_systems(Update, update);
+}
+
+fn startup(
+    mut commands: Commands,
+    mut game_controller_commands: EventWriter<GameControllerCommand>,
+) {
+    for number in [
+        PlayerNumber::One,
+        PlayerNumber::Two,
+        PlayerNumber::Three,
+        PlayerNumber::Four,
+        PlayerNumber::Five,
+        PlayerNumber::Seven,
+    ] {
+        commands.spawn(Robot::new(number));
+    }
+    game_controller_commands.send(GameControllerCommand::SetGameState(GameState::Ready));
+}
+
+fn update(
+    time: Res<Time<Ticks>>,
+    mut ball: ResMut<BallResource>,
+    mut exit: EventWriter<AppExit>,
+    mut robots: Query<&mut Robot>,
+    mut game_controller_commands: EventWriter<GameControllerCommand>,
+) {
+    if time.ticks() == 4150 {
+        game_controller_commands.send(GameControllerCommand::Penalize(
+            PlayerNumber::Two,
+            Penalty::Manual {
+                remaining: Duration::from_secs(1),
+            },
+            Team::Hulks,
+        ));
+    }
+
+    if time.ticks() == 4155 {
+        game_controller_commands.send(GameControllerCommand::Unpenalize(
+            PlayerNumber::Two,
+            Team::Hulks,
+        ));
+    }
+
+    if time.ticks() == 4200 {
+        ball.state = None;
+    }
+
+    if time.ticks() == 4660 {
+        if let MotionCommand::Stand { .. } = robots
+            .iter_mut()
+            .find(|robot| robot.parameters.player_number == PlayerNumber::Two)
+            .unwrap()
+            .database
+            .main_outputs
+            .motion_command
+        {
+            println!("Standing searcher at penalty walk-in");
+            exit.send(AppExit::from_code(1));
+        }
+        if let MotionCommand::Walk { .. } = robots
+            .iter_mut()
+            .find(|robot| robot.parameters.player_number == PlayerNumber::Three)
+            .unwrap()
+            .database
+            .main_outputs
+            .motion_command
+        {
+            println!("Moving searcher after ball loss");
+            exit.send(AppExit::from_code(1));
+        }
+    }
+
+    if time.ticks() == 5500 {
+        ball.state = Some(SimulatorBallState {
+            position: point![-2.7, -0.2],
+            velocity: vector![0.0, 0.0],
+        });
+    }
+    if time.ticks() == 6000 {
+        ball.state = None;
+    }
+    if time.ticks() == 6750 {
+        ball.state = Some(SimulatorBallState {
+            position: point![4.0, -0.2],
+            velocity: vector![0.0, 0.0],
+        });
+    }
+    if time.ticks() == 7200 {
+        ball.state = None;
+    }
+    if time.ticks() >= 10_000 {
+        println!("Done");
+        exit.send(AppExit::Success);
+    }
+}
diff --git a/crates/control/src/behavior/node.rs b/crates/control/src/behavior/node.rs
@@ -1,4 +1,4 @@
-use std::time::SystemTime;
+use std::time::{SystemTime, UNIX_EPOCH};
 
 use color_eyre::Result;
 use serde::{Deserialize, Serialize};
@@ -45,7 +45,9 @@ pub struct Behavior {
     last_known_ball_position: Point2<Field>,
     active_since: Option<SystemTime>,
     previous_role: Role,
+    previous_cycle_role: Role,
     last_defender_mode: DefendMode,
+    last_time_role_changed: SystemTime,
 }
 
 #[context]
@@ -99,6 +101,8 @@ impl Behavior {
             last_known_ball_position: point![0.0, 0.0],
             active_since: None,
             previous_role: Role::Searcher,
+            previous_cycle_role: Role::Searcher,
+            last_time_role_changed: UNIX_EPOCH,
             last_defender_mode: DefendMode::Passive,
         })
     }
@@ -125,12 +129,17 @@ impl Behavior {
             (Some(_), _) => self.active_since = None,
         }
 
-        if self.previous_role != context.world_state.robot.role
-            && context.world_state.robot.role != Role::Searcher
-            && context.world_state.robot.role != Role::Loser
-            && self.previous_role != Role::Keeper
-        {
-            self.previous_role = context.world_state.robot.role;
+        if self.previous_cycle_role != context.world_state.robot.role {
+            match self.previous_cycle_role {
+                Role::DefenderLeft | Role::DefenderRight => {
+                    self.last_time_role_changed = now;
+                }
+                _ => {}
+            }
+            if self.previous_role != Role::Keeper {
+                self.previous_role = self.previous_cycle_role;
+            }
+            self.previous_cycle_role = context.world_state.robot.role;
         }
 
         let mut actions = vec![
@@ -397,6 +406,8 @@ impl Behavior {
                         &context.parameters.search,
                         &mut context.path_obstacles_output,
                         self.previous_role,
+                        self.last_time_role_changed,
+                        self.last_known_ball_position,
                         *context.search_walk_speed,
                         context
                             .parameters
diff --git a/crates/control/src/behavior/search.rs b/crates/control/src/behavior/search.rs
@@ -75,6 +75,8 @@ pub fn execute(
     parameters: &SearchParameters,
     path_obstacles_output: &mut AdditionalOutput<Vec<PathObstacle>>,
     previous_role: Role,
+    last_time_role_changed: SystemTime,
+    last_known_ball_position: Point2<Field>,
     walk_speed: WalkSpeed,
     distance_to_be_aligned: f32,
     cycle_start_time: SystemTime,
@@ -103,6 +105,22 @@ pub fn execute(
         },
         None => HeadMotion::SearchForLostBall,
     };
+    let distance_to_ball = (ground_to_field.inverse() * last_known_ball_position)
+        .coords()
+        .norm();
+    let estimated_ball_arrival_time = distance_to_ball / parameters.estimated_ball_speed;
+    let near_own_penalty_box = ground_to_field.as_pose().position().y().abs()
+        < field_dimensions.penalty_area_width / 2.0
+        && ground_to_field.as_pose().position().x() < 0.0;
+    if world_state
+        .now
+        .duration_since(last_time_role_changed)
+        .expect("time went backwards")
+        < Duration::from_secs_f32(estimated_ball_arrival_time)
+        && near_own_penalty_box
+    {
+        return Some(MotionCommand::Stand { head });
+    };
     if let Some(SearchRole::Goal) = search_role {
         let goal_pose = Pose2::from(search_position);
         walk_and_stand.execute(
diff --git a/crates/control/src/world_state_composer.rs b/crates/control/src/world_state_composer.rs
@@ -9,6 +9,7 @@ use spl_network_messages::PlayerNumber;
 use types::{
     ball_position::HypotheticalBallPosition,
     calibration::CalibrationCommand,
+    cycle_time::CycleTime,
     fall_state::FallState,
     filtered_game_controller_state::FilteredGameControllerState,
     kick_decision::KickDecision,
@@ -51,6 +52,7 @@ pub struct CycleContext {
     position_of_interest: Input<Point2<Ground>, "position_of_interest">,
     calibration_command: Input<Option<CalibrationCommand>, "calibration_command?">,
     stand_up_count: CyclerState<u32, "stand_up_count">,
+    cycle_time: Input<CycleTime, "cycle_time">,
 }
 
 #[context]
@@ -89,6 +91,7 @@ impl WorldStateComposer {
             filtered_game_controller_state: context.filtered_game_controller_state.cloned(),
             hypothetical_ball_positions: context.hypothetical_ball_position.clone(),
             calibration_command: context.calibration_command.copied(),
+            now: context.cycle_time.start_time,
         };
 
         Ok(MainOutputs {
diff --git a/crates/types/src/parameters.rs b/crates/types/src/parameters.rs
@@ -107,6 +107,7 @@ pub struct SearchParameters {
     pub rotation_per_step: f32,
     pub stand_secs: f32,
     pub turn_secs: f32,
+    pub estimated_ball_speed: f32,
 }
 
 #[derive(
diff --git a/crates/types/src/world_state.rs b/crates/types/src/world_state.rs
@@ -15,7 +15,7 @@ use crate::{
     roles::Role, rule_obstacles::RuleObstacle,
 };
 
-#[derive(Clone, Debug, Default, Serialize, Deserialize, PathSerialize, PathIntrospect)]
+#[derive(Clone, Debug, Serialize, Deserialize, PathSerialize, PathIntrospect)]
 pub struct WorldState {
     pub ball: Option<BallState>,
     pub rule_ball: Option<BallState>,
@@ -29,6 +29,27 @@ pub struct WorldState {
     pub instant_kick_decisions: Option<Vec<KickDecision>>,
     pub robot: RobotState,
     pub calibration_command: Option<CalibrationCommand>,
+    pub now: SystemTime,
+}
+
+impl Default for WorldState {
+    fn default() -> Self {
+        Self {
+            ball: Default::default(),
+            rule_ball: Default::default(),
+            hypothetical_ball_positions: Default::default(),
+            filtered_game_controller_state: Default::default(),
+            obstacles: Default::default(),
+            rule_obstacles: Default::default(),
+            position_of_interest: Point2::origin(),
+            suggested_search_position: Default::default(),
+            kick_decisions: Default::default(),
+            instant_kick_decisions: Default::default(),
+            robot: Default::default(),
+            calibration_command: Default::default(),
+            now: UNIX_EPOCH,
+        }
+    }
 }
 
 #[derive(
diff --git a/etc/parameters/default.json b/etc/parameters/default.json
@@ -1287,7 +1287,8 @@
       "position_reached_distance": 0.4,
       "rotation_per_step": 0.4,
       "turn_secs": 3.0,
-      "stand_secs": 1.0
+      "stand_secs": 1.0,
+      "estimated_ball_speed": 0.75
     },
     "look_action": {
       "angle_threshold": 0.95,

Original file line number	Diff line number	Diff line change
`@@ -107,6 +107,7 @@ pub struct SearchParameters {`
`107`	`107`	`pub rotation_per_step: f32,`
`108`	`108`	`pub stand_secs: f32,`
`109`	`109`	`pub turn_secs: f32,`
	`110`	`+ pub estimated_ball_speed: f32,`
`110`	`111`	`}`
`111`	`112`
`112`	`113`	`#[derive(`