Lost ball behavior (#651)

Fyor · MachaMeijer · oxkitsune · web-flow · commit d909466747d4 · 2025-07-18T21:30:18.000+02:00
Co-authored-by: machameijer &lt;machameijer@gmail.com&gt;
Co-authored-by: Gijs de Jong &lt;berichtaangijs@gmail.com&gt;
diff --git a/yggdrasil/src/behavior/behaviors/lost_ball_search.rs b/yggdrasil/src/behavior/behaviors/lost_ball_search.rs
@@ -0,0 +1,121 @@
+use bevy::prelude::*;
+use serde::{Deserialize, Serialize};
+use serde_with::serde_as;
+use std::time::Instant;
+
+use crate::{
+    behavior::{
+        BehaviorConfig,
+        engine::{Behavior, BehaviorState, in_behavior},
+    },
+    motion::walking_engine::{StandingHeight, step::Step, step_context::StepContext},
+    nao::{NaoManager, Priority},
+};
+use nidhogg::types::{FillExt, HeadJoints};
+
+const ROTATION_STIFFNESS: f32 = 0.3;
+
+/// Config struct containing parameters for the initial behavior.
+#[serde_as]
+#[derive(Resource, Serialize, Deserialize, Debug, Clone)]
+#[serde(deny_unknown_fields)]
+pub struct LostBallSearchBehaviorConfig {
+    // Controls how fast the robot moves its head back and forth while looking around
+    pub head_rotation_speed: f32,
+    // Controls how far to the left and right the robot looks while looking around, in radians.
+    // If this value is one, the robot will look one radian to the left and one radian to the
+    // right.
+    pub head_pitch_max: f32,
+    // Controls how far to the bottom the robot looks while looking around, in radians
+    pub head_yaw_max: f32,
+}
+
+#[derive(Resource, Deref)]
+struct LostBallSearchStartingTime(Instant);
+
+/// This behavior makes the robot look around with a sinusoidal head movement with an optional step.
+/// With this behavior, the robot can observe its surroundings while standing still or turning.
+#[derive(Resource, Default)]
+pub struct LostBallSearch {
+    pub step: Option<Step>,
+}
+
+impl LostBallSearch {
+    #[must_use]
+    pub fn with_turning(turn: f32) -> Self {
+        LostBallSearch {
+            step: Some(Step {
+                turn,
+                ..Default::default()
+            }),
+        }
+    }
+}
+
+impl Behavior for LostBallSearch {
+    const STATE: BehaviorState = BehaviorState::LostBallSearch;
+}
+
+pub struct LostBallSearchBehaviorPlugin;
+
+impl Plugin for LostBallSearchBehaviorPlugin {
+    fn build(&self, app: &mut App) {
+        app.add_systems(Update, observe.run_if(in_behavior::<LostBallSearch>))
+            .add_systems(
+                OnEnter(BehaviorState::Observe),
+                reset_lost_ball_search_starting_time,
+            )
+            .insert_resource(LostBallSearchStartingTime(Instant::now()));
+    }
+}
+
+fn reset_lost_ball_search_starting_time(
+    mut lost_ball_search_starting_time: ResMut<LostBallSearchStartingTime>,
+) {
+    lost_ball_search_starting_time.0 = Instant::now();
+}
+
+fn observe(
+    mut nao_manager: ResMut<NaoManager>,
+    behavior_config: Res<BehaviorConfig>,
+    observe: Res<LostBallSearch>,
+    observe_starting_time: Res<LostBallSearchStartingTime>,
+    mut step_context: ResMut<StepContext>,
+) {
+    let observe_config = &behavior_config.observe;
+    look_around(
+        &mut nao_manager,
+        **observe_starting_time,
+        observe_config.head_rotation_speed,
+        observe_config.head_yaw_max,
+        observe_config.head_pitch_max,
+    );
+
+    if let Some(step) = observe.step {
+        step_context.request_walk(step);
+    } else {
+        step_context.request_stand_with_height(StandingHeight::MAX);
+    }
+}
+
+fn look_around(
+    nao_manager: &mut NaoManager,
+    starting_time: Instant,
+    rotation_speed: f32,
+    yaw_multiplier: f32,
+    pitch_multiplier: f32,
+) {
+    // Used to parameterize the yaw and pitch angles, multiplying with a large
+    // rotation speed will make the rotation go faster.
+    let movement_progress = starting_time.elapsed().as_secs_f32() * rotation_speed;
+    let yaw = (movement_progress).sin() * yaw_multiplier;
+    let pitch = (movement_progress * 2.0 + std::f32::consts::FRAC_PI_2)
+        .sin()
+        .max(0.0)
+        * pitch_multiplier;
+
+    let position = HeadJoints { yaw, pitch };
+    let stiffness = HeadJoints::fill(ROTATION_STIFFNESS);
+
+    nao_manager.set_head(position, stiffness, Priority::default());
+}
diff --git a/yggdrasil/src/behavior/behaviors/mod.rs b/yggdrasil/src/behavior/behaviors/mod.rs
@@ -1,4 +1,5 @@
 mod catchfall;
+mod lost_ball_search;
 mod observe;
 mod rl_striker_search;
 mod sitting;
@@ -13,6 +14,7 @@ mod walk_to_ball;
 mod walk_to_set;
 
 pub use catchfall::*;
+pub use lost_ball_search::*;
 pub use observe::*;
 pub use rl_striker_search::*;
 pub use sitting::*;
diff --git a/yggdrasil/src/behavior/engine.rs b/yggdrasil/src/behavior/engine.rs
@@ -8,6 +8,7 @@ use ml::{
 use nalgebra::Point2;
 
 use crate::{
+    behavior::roles::LostBallSearchTimer,
     core::config::showtime::PlayerConfig,
     motion::walking_engine::Gait,
     nao::{NaoManager, Priority, RobotInfo},
@@ -17,18 +18,20 @@ use crate::{
 
 use super::{
     behaviors::{
-        CatchFall, CatchFallBehaviorPlugin, ObserveBehaviorPlugin, RlStrikerSearchBehaviorPlugin,
-        Sitting, SittingBehaviorPlugin, Stand, StandBehaviorPlugin, StandLookAt,
-        StandLookAtBehaviorPlugin, Standup, StandupBehaviorPlugin, StartUpBehaviorPlugin,
-        VisualReferee, VisualRefereeBehaviorPlugin, WalkBehaviorPlugin, WalkToBallBehaviorPlugin,
-        WalkToBehaviorPlugin, WalkToSet, WalkToSetBehaviorPlugin,
+        CatchFall, CatchFallBehaviorPlugin, LostBallSearchBehaviorPlugin, ObserveBehaviorPlugin,
+        RlStrikerSearchBehaviorPlugin, Sitting, SittingBehaviorPlugin, Stand, StandBehaviorPlugin,
+        StandLookAt, StandLookAtBehaviorPlugin, Standup, StandupBehaviorPlugin,
+        StartUpBehaviorPlugin, VisualReferee, VisualRefereeBehaviorPlugin, WalkBehaviorPlugin,
+        WalkToBallBehaviorPlugin, WalkToBehaviorPlugin, WalkToSet, WalkToSetBehaviorPlugin,
     },
     primary_state::PrimaryState,
     roles::{
         Defender, DefenderRolePlugin, Goalkeeper, GoalkeeperRolePlugin, Striker, StrikerRolePlugin,
     },
 };
 
+use std::time::Duration;
+
 const FORWARD_LEANING_THRESHOLD: f32 = 0.2;
 const BACKWARD_LEANING_THRESHOLD: f32 = -0.2;
 
@@ -57,6 +60,7 @@ impl Plugin for BehaviorEnginePlugin {
                 WalkToBallBehaviorPlugin,
                 WalkToBehaviorPlugin,
                 WalkToSetBehaviorPlugin,
+                LostBallSearchBehaviorPlugin,
             ))
             .add_systems(PostUpdate, role_base);
     }
@@ -70,6 +74,20 @@ fn broken_robot_behavior_override(robot_info: Res<RobotInfo>, mut nao_manager: R
     }
 }
 
+#[derive(Resource)]
+pub struct DefenderSwitchTimer {
+    timer: Timer,
+}
+
+impl DefenderSwitchTimer {
+    #[must_use]
+    pub fn new(duration: Duration) -> Self {
+        DefenderSwitchTimer {
+            timer: Timer::new(duration, TimerMode::Once),
+        }
+    }
+}
+
 pub trait RlBehaviorInput<T> {
     fn to_input(&self) -> T;
 }
@@ -111,6 +129,7 @@ pub enum BehaviorState {
     WalkToSet,
     WalkToBall,
     RlStrikerSearchBehavior,
+    LostBallSearch,
 }
 
 #[must_use]
@@ -173,13 +192,38 @@ impl RoleState {
         commands: &mut Commands,
         player_number: u8,
         possible_ball_distance: Option<f32>,
+        role_state: Res<State<RoleState>>,
+        defender_switch_timer: Option<ResMut<DefenderSwitchTimer>>,
+        time: Res<Time>,
     ) {
         if let Some(distance) = possible_ball_distance {
             if distance < 3.0 {
                 commands.set_role(Striker);
                 return;
             }
         }
+
+        // check if the current role is striker
+        if *role_state == RoleState::Striker && (player_number != 4 && player_number != 5) {
+            if let Some(mut timer) = defender_switch_timer {
+                timer.timer.tick(time.delta());
+                if timer.timer.finished() {
+                    commands.remove_resource::<DefenderSwitchTimer>();
+                    if player_number == 1 {
+                        commands.set_role(Goalkeeper);
+                    } else {
+                        commands.set_role(Defender);
+                    }
+                } else {
+                    commands.set_role(Striker);
+                }
+                return;
+            }
+            commands.insert_resource(DefenderSwitchTimer::new(Duration::from_secs(9)));
+            commands.set_role(Striker);
+            return;
+        }
+
         // TODO: Check if robots have been penalized, or which robot is closed to the ball etc.
         Self::by_player_number(commands, player_number);
     }
@@ -216,6 +260,9 @@ pub fn role_base(
     game_controller_message: Option<Res<GameControllerMessage>>,
     imu_values: Res<IMUValues>,
     ball_tracker: Res<BallTracker>,
+    role_state: Res<State<RoleState>>,
+    defender_switch_timer: Option<ResMut<DefenderSwitchTimer>>,
+    time: Res<Time>,
 ) {
     commands.disable_role();
     let behavior = behavior_state.get();
@@ -278,6 +325,9 @@ pub fn role_base(
     match *primary_state {
         PrimaryState::Sitting => commands.set_behavior(Sitting),
         PrimaryState::Penalized => {
+            // reset all timers
+            commands.remove_resource::<DefenderSwitchTimer>();
+            commands.remove_resource::<LostBallSearchTimer>();
             commands.set_behavior(Stand);
         }
         PrimaryState::Standby => {
@@ -308,6 +358,9 @@ pub fn role_base(
                 &mut commands,
                 player_config.player_number,
                 possible_ball_distance,
+                role_state,
+                defender_switch_timer,
+                time,
             );
         }
     }
diff --git a/yggdrasil/src/behavior/roles/mod.rs b/yggdrasil/src/behavior/roles/mod.rs
@@ -6,4 +6,4 @@ mod striker;
 
 pub use defender::{Defender, DefenderRolePlugin};
 pub use goalkeeper::{Goalkeeper, GoalkeeperRolePlugin};
-pub use striker::{Striker, StrikerRolePlugin};
+pub use striker::{LostBallSearchTimer, Striker, StrikerRolePlugin};
diff --git a/yggdrasil/src/behavior/roles/striker.rs b/yggdrasil/src/behavior/roles/striker.rs
@@ -5,7 +5,10 @@ use nidhogg::types::{FillExt, RightEye, color};
 
 use crate::{
     behavior::{
-        behaviors::{LookMode, RlStrikerSearchBehavior, StandLookAt, Walk, WalkTo, WalkToBall},
+        behaviors::{
+            LookMode, LostBallSearch, RlStrikerSearchBehavior, StandLookAt, Walk, WalkTo,
+            WalkToBall,
+        },
         engine::{BehaviorState, CommandsBehaviorExt, RoleState, Roles, in_role},
         primary_state::PrimaryState,
     },
@@ -19,6 +22,8 @@ use crate::{
     vision::ball_detection::ball_tracker::BallTracker,
 };
 
+use std::time::Duration;
+
 const WALK_WITH_BALL_ANGLE: f32 = 0.3;
 const ALIGN_WITH_BALL_DISTANCE: f32 = 0.3;
 
@@ -69,6 +74,22 @@ fn in_set_play(
 #[derive(Resource, Default, Debug)]
 pub struct Striker;
 
+#[derive(Resource)]
+pub struct LostBallSearchTimer {
+    timer: Timer,
+    last_ball: Point2<f32>,
+}
+
+impl LostBallSearchTimer {
+    #[must_use]
+    pub fn new(duration: Duration, last_ball: Point2<f32>) -> Self {
+        LostBallSearchTimer {
+            timer: Timer::new(duration, TimerMode::Once),
+            last_ball,
+        }
+    }
+}
+
 impl Roles for Striker {
     const STATE: RoleState = RoleState::Striker;
 }
@@ -83,18 +104,49 @@ pub fn striker_role(
     layout_config: Res<LayoutConfig>,
     ball_tracker: Res<BallTracker>,
     mut nao_manager: ResMut<NaoManager>,
+    lost_ball_timer: Option<ResMut<LostBallSearchTimer>>,
+    time: Res<Time>,
 ) {
     let Some(relative_ball) = ball_tracker.stationary_ball() else {
-        nao_manager.set_right_eye_led(RightEye::fill(color::f32::GREEN), Priority::default());
+        if let Some(mut timer) = lost_ball_timer {
+            timer.timer.tick(time.delta()); // <- tick the timer
 
-        commands.set_behavior(RlStrikerSearchBehavior);
+            if timer.timer.finished() {
+                commands.remove_resource::<LostBallSearchTimer>();
+            } else {
+                nao_manager
+                    .set_right_eye_led(RightEye::fill(color::f32::BLUE), Priority::default()); // TEMP LED
+
+                // determine the side we need to turn to by using timer.last_ball
+                let relative_last_ball = &timer.last_ball;
+                commands.set_behavior(LostBallSearch::with_turning(
+                    relative_last_ball.y.signum() * 0.6, //TODO test
+                ));
+            }
+        } else {
+            nao_manager.set_right_eye_led(RightEye::fill(color::f32::GREEN), Priority::default());
+            commands.set_behavior(RlStrikerSearchBehavior);
+        }
         return;
     };
+    let absolute_ball: nalgebra::OPoint<f32, nalgebra::Const<2>> =
+        pose.robot_to_world(&relative_ball);
+
+    if ball_tracker.timestamp.elapsed().as_secs_f32() > 0.5 {
+        if lost_ball_timer.is_none() {
+            commands.insert_resource(LostBallSearchTimer::new(
+                Duration::from_secs(9),
+                relative_ball,
+            ));
+        }
+    } else {
+        commands.remove_resource::<LostBallSearchTimer>();
+    }
 
-    let absolute_ball = pose.robot_to_world(&relative_ball);
     let ball_angle = pose.angle_to(&absolute_ball);
     let ball_distance = relative_ball.coords.norm();
-    let ball_target = Point3::new(absolute_ball.x, absolute_ball.y, 0.2);
+    let ball_target: nalgebra::OPoint<f32, nalgebra::Const<3>> =
+        Point3::new(absolute_ball.x, absolute_ball.y, 0.2);
 
     let relative_goalpost_left =
         pose.world_to_robot(&Point2::new(layout_config.field.length / 2., 0.8));
