Searcher Standing Delay

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

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);
+    }
+}

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

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(

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 {

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(

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(

etc/parameters/default.json

@@ -1286,7 +1286,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,