diff --git a/crates/bevyhavior_simulator/src/bin/opponent_kickoff.rs b/crates/bevyhavior_simulator/src/bin/opponent_kickoff.rs new file mode 100644 index 0000000000..71a079b0de --- /dev/null +++ b/crates/bevyhavior_simulator/src/bin/opponent_kickoff.rs @@ -0,0 +1,253 @@ +//! Regression for approaching the ball during the opponent's restricted kickoff. +//! Run with `cargo run -p bevyhavior_simulator --bin opponent_kickoff` for the viewer, +//! or `cargo test -p bevyhavior_simulator --bin opponent_kickoff` headlessly. +use std::time::{Duration, SystemTime}; + +use bevy::prelude::*; +use bevyhavior_simulator::behavior_tree_simulator::{ + BehaviorTreeSimulatorSet, SimulatedBall, SimulatorAutoReferee, SimulatorBall, SimulatorClock, + SimulatorFieldDimensions, SimulatorGameState, SimulatorRobotBundle, SimulatorRobotFrames, + SimulatorRobotId, SimulatorTimelineMarkers, SimulatorWorldStates, default_behavior_parameters, +}; +use coordinate_systems::Field; +use eframe::egui::Color32; +use geometry::{circle::Circle, rectangle::Rectangle}; +use hsl_network_messages::{GameState, PlayerNumber, Team}; +use linear_algebra::{Isometry2, Point2, point, vector}; +use scenario::scenario; +use types::{ + field_dimensions::Side, filtered_game_state::FilteredGameState, motion_command::MotionCommand, + primary_state::PrimaryState, rule_obstacles::RuleObstacle, +}; + +const PLAYER: PlayerNumber = PlayerNumber::Two; +const SET_DURATION: Duration = Duration::from_secs(3); +const RESTRICTED_DURATION: Duration = Duration::from_secs(10); +const FAILURE_TAIL_DURATION: Duration = Duration::from_secs(3); + +#[derive(Clone, Copy, PartialEq)] +enum Phase { + Ready, + Set, + Restricted, + Released, +} + +#[derive(Resource)] +struct KickoffScenario { + phase: Phase, + since: SystemTime, + ready_position: Point2, + waiting_position: Option>, + walked_in_ready: bool, + failed_at: Option, +} + +/// AI generated scenario to test that the robot does not kick the ball when the +/// opponent has kickoff. +#[scenario] +fn opponent_kickoff(app: &mut App) { + app.add_systems(Startup, startup) + .add_systems( + Update, + supply_kickoff_restriction.in_set(BehaviorTreeSimulatorSet::AfterWorldState), + ) + .add_systems( + Update, + check_and_advance.in_set(BehaviorTreeSimulatorSet::Scenario), + ); +} + +fn startup( + mut commands: Commands, + clock: Res, + mut referee: ResMut, + mut game: ResMut, + mut ball: ResMut, + mut markers: ResMut, +) { + // This scenario owns transitions: Ready ends only after the robot arrives. + referee.rules.clear(); + game.set_kicking_team(Some(Team::Opponent)); + game.set_game_state(GameState::Ready, clock.now); + let parameters = default_behavior_parameters().expect("failed to load behavior parameters"); + let ready_position = parameters.kickoff.standard_positions[PLAYER].position; + commands.insert_resource(KickoffScenario { + phase: Phase::Ready, + since: clock.now, + ready_position, + waiting_position: None, + walked_in_ready: false, + failed_at: None, + }); + commands.spawn( + SimulatorRobotBundle::new( + Team::Hulks, + PLAYER, + Isometry2::from_parts(vector![-4.0, -2.0], 0.0), + parameters, + ) + .expect("failed to create robot") + .with_primary_state(PrimaryState::Ready), + ); + // The opponent owns kickoff but does not take it; the ball stays at center. + ball.state = Some(SimulatedBall { + position: point![0.0, 0.0], + velocity: vector![0.0, 0.0], + field_side: Side::Left, + }); + markers.add(clock.now, Color32::YELLOW, "Opponent kickoff: Ready"); +} + +fn supply_kickoff_restriction( + scenario: Res, + dimensions: Res, + mut worlds: ResMut, +) { + if !matches!(scenario.phase, Phase::Restricted | Phase::Released) { + return; + } + // The simulator's simplified GC conversion always frees the ball in Playing. + // Supply the filtered state seen in recover2.mcap before behavior ticks. + let world = worlds + .0 + .get_mut(&SimulatorRobotId::new(Team::Hulks, PLAYER)) + .expect("scenario robot must have a world state"); + let filtered = world + .filtered_game_controller_state + .as_mut() + .expect("scenario must have a game controller"); + filtered.game_state = FilteredGameState::Playing { + ball_is_free: scenario.phase == Phase::Released, + kick_off: true, + }; + filtered.opponent_game_state = FilteredGameState::Playing { + ball_is_free: true, + kick_off: true, + }; + if scenario.phase == Phase::Restricted { + // Include the center-circle and opponent-half obstacles present in the log. + world.rule_obstacles = vec![ + RuleObstacle::Circle(Circle::new( + Point2::origin(), + dimensions.0.center_circle_diameter / 2.0 + 0.2, + )), + RuleObstacle::Rectangle(Rectangle { + min: point![0.0, -dimensions.0.width / 2.0], + max: point![dimensions.0.length / 2.0, dimensions.0.width / 2.0], + }), + ]; + } +} + +fn check_and_advance( + clock: Res, + frames: Res, + ball: Res, + mut scenario: ResMut, + mut game: ResMut, + mut markers: ResMut, + mut exit: MessageWriter, +) { + // Keep behavior and physics running in the phase that failed for the viewer. + if let Some(failed_at) = scenario.failed_at { + if clock + .now + .duration_since(failed_at) + .expect("monotonic simulation time") + >= FAILURE_TAIL_DURATION + { + exit.write(AppExit::from_code(1)); + } + return; + } + let frame = frames + .0 + .get(&SimulatorRobotId::new(Team::Hulks, PLAYER)) + .expect("scenario robot must produce a behavior frame"); + let pose = frame + .world_state + .robot + .ground_to_field + .expect("robot must be localized"); + let position = pose.translation(); + let elapsed = clock + .now + .duration_since(scenario.since) + .expect("monotonic simulation time"); + let is_standing = matches!(frame.motion_command, MotionCommand::Stand { .. }); + + let failure = match scenario.phase { + Phase::Ready => { + scenario.walked_in_ready |= matches!(frame.motion_command, MotionCommand::Walk { .. }); + if is_standing && (position - scenario.ready_position).norm() < 0.2 { + if !scenario.walked_in_ready { + Some("robot never walked to its Ready position") + } else { + scenario.waiting_position = Some(position); + scenario.phase = Phase::Set; + scenario.since = clock.now; + game.set_game_state(GameState::Set, clock.now); + markers.add(clock.now, Color32::YELLOW, "Ready position reached: Set"); + None + } + } else if elapsed > Duration::from_secs(45) { + Some("robot did not reach its Ready position within 45 seconds") + } else { + None + } + } + Phase::Set | Phase::Restricted => { + let waiting_position = scenario.waiting_position.expect("Ready position recorded"); + if !is_standing { + Some("robot commanded motion during Set or restricted opponent kickoff") + } else if (position - waiting_position).norm() > 0.01 { + Some("robot moved from its waiting position") + } else if ball + .state + .is_none_or(|ball| ball.position.coords().norm() > 0.001) + { + Some("ball moved before kickoff was released") + } else if scenario.phase == Phase::Set && elapsed >= SET_DURATION { + scenario.phase = Phase::Restricted; + scenario.since = clock.now; + game.set_game_state(GameState::Playing, clock.now); + markers.add(clock.now, Color32::LIGHT_RED, "Playing: ball is NOT free"); + None + } else if scenario.phase == Phase::Restricted && elapsed >= RESTRICTED_DURATION { + scenario.phase = Phase::Released; + scenario.since = clock.now; + markers.add(clock.now, Color32::LIGHT_GREEN, "Playing: ball is free"); + None + } else { + None + } + } + Phase::Released => { + if matches!(frame.motion_command, MotionCommand::VisualKick { .. }) { + println!( + "ok: reached Ready pose, waited through Set and 10s restricted kickoff, resumed kicking" + ); + markers.add( + clock.now, + Color32::LIGHT_GREEN, + "PASS: kicking after release", + ); + exit.write(AppExit::Success); + None + } else if elapsed > Duration::from_secs(15) { + Some("robot did not resume kicking within 15 seconds after release") + } else { + None + } + } + }; + if let Some(reason) = failure { + eprintln!( + "opponent kickoff failed: {reason}; motion={:?}", + frame.motion_command + ); + markers.add(clock.now, Color32::RED, format!("FAIL: {reason}")); + scenario.failed_at = Some(clock.now); + } +} diff --git a/crates/nodes/behavior_node/src/conditions.rs b/crates/nodes/behavior_node/src/conditions.rs index f534b8c714..d4d296a753 100644 --- a/crates/nodes/behavior_node/src/conditions.rs +++ b/crates/nodes/behavior_node/src/conditions.rs @@ -4,7 +4,7 @@ use hsl_network_messages::Team; use linear_algebra::{point, vector}; use types::{ controller_input::Button, filtered_game_controller_state::FilteredGameControllerState, - primary_state::PrimaryState, + filtered_game_state::FilteredGameState, primary_state::PrimaryState, }; use voronoi::Ownership; @@ -197,6 +197,29 @@ pub fn hulks_is_kicking_team(blackboard: &mut Blackboard) -> bool { ) } +pub fn is_ball_free(blackboard: &mut Blackboard) -> bool { + matches!( + blackboard.world_state.filtered_game_controller_state, + Some(FilteredGameControllerState { + game_state: FilteredGameState::Playing { + ball_is_free: true, + .. + }, + .. + }) + ) +} + +pub fn is_kickoff(blackboard: &mut Blackboard) -> bool { + matches!( + blackboard.world_state.filtered_game_controller_state, + Some(FilteredGameControllerState { + game_state: FilteredGameState::Playing { kick_off: true, .. }, + .. + }) + ) +} + pub fn is_last_hulk_standing(blackboard: &mut Blackboard) -> bool { let own_player_number = blackboard.world_state.robot.player_number; diff --git a/crates/nodes/behavior_node/src/tree.rs b/crates/nodes/behavior_node/src/tree.rs index c3a5a57e67..bae6cca590 100644 --- a/crates/nodes/behavior_node/src/tree.rs +++ b/crates/nodes/behavior_node/src/tree.rs @@ -10,10 +10,10 @@ use crate::{ behavior_tree::Node, condition, conditions::{ - has_ball_position, hulks_is_kicking_team, is_ball_interception_candidate, is_close_to_ball, - is_closest_to_ball, is_controller_connected, is_fallen, is_goalkeeper, - is_last_hulk_standing, is_primary_state, is_remote_control_enabled, is_remote_kick_mode, - is_simple, + has_ball_position, hulks_is_kicking_team, is_ball_free, is_ball_interception_candidate, + is_close_to_ball, is_closest_to_ball, is_controller_connected, is_fallen, is_goalkeeper, + is_kickoff, is_last_hulk_standing, is_primary_state, is_remote_control_enabled, + is_remote_kick_mode, is_simple, }, goalkeeper::goalkeeper_subtree, head::{look_around, look_at_ball_subtree, look_straight_ahead, search_for_lost_ball_subtree}, @@ -92,6 +92,13 @@ fn ready_subtree() -> Node { fn playing_subtree() -> Node { selection!( + sequence!( + condition!(is_kickoff), + negation!(condition!(is_ball_free)), + negation!(condition!(hulks_is_kicking_team)), + subtree!(look_at_ball_subtree), + action!(stand) + ), sequence!( condition!(is_penalty_shootout), subtree!(penalty_shootout_subtree)