Fail softly and carry a big error (#1865)

* Prevent freewheeling when channel is closed

* Add soft-errors and check if robot wants to walk into rule-obstacle

* Impl PathSerde for serde_json::Value

* Print tick number with soft error message

* Add soft error exception for penalty kick striker

* Restrict midfielders x position during penalty kick

Author: knoellle

crates/bevyhavior_simulator/src/lib.rs

@@ -17,6 +17,8 @@ pub mod robot;
 pub mod scenario;
 pub mod server;
 pub mod simulator;
+pub mod soft_error;
+pub mod test_rules;
 pub mod time;
 pub mod visual_referee;
 pub mod whistle;

crates/bevyhavior_simulator/src/server.rs

@@ -57,6 +57,7 @@ async fn timeline_server(
                         if !progress.is_finished() {
                             progress.finish();
                         }
+                        continue;
                     }
                 }
             }

crates/bevyhavior_simulator/src/simulator.rs

@@ -10,7 +10,7 @@ use bevy::{
     time::Time,
 };
 use color_eyre::{
-    eyre::{eyre, Context, ContextCompat},
+    eyre::{bail, eyre, Context, ContextCompat},
     Result,
 };
 
@@ -25,6 +25,8 @@ use crate::{
     recorder::Recording,
     robot::{cycle_robots, move_robots, Messages},
     server::Parameters,
+    soft_error::{soft_error_plugin, SoftErrorResource},
+    test_rules::check_robots_dont_walk_into_rule_obstacles,
     time::{update_time, Ticks},
     visual_referee::VisualRefereeResource,
     whistle::WhistleResource,
@@ -54,6 +56,7 @@ impl Plugin for SimulatorPlugin {
         ))
         .add_plugins(autoref_plugin)
         .add_plugins(game_controller_plugin)
+        .add_plugins(soft_error_plugin)
         .insert_resource(SimulatorFieldDimensions::from(parameters.field_dimensions))
         .insert_resource(GameController::default())
         .insert_resource(BallResource::default())
@@ -64,6 +67,12 @@ impl Plugin for SimulatorPlugin {
         .insert_resource(Time::<Ticks>::default())
         .add_systems(First, update_time)
         .add_systems(Update, cycle_robots.before(move_robots).after(autoref))
+        .add_systems(
+            Update,
+            check_robots_dont_walk_into_rule_obstacles
+                .before(move_robots)
+                .after(cycle_robots),
+        )
         .add_systems(Update, move_robots)
         .add_systems(Update, move_ball.after(move_robots));
 
@@ -94,10 +103,19 @@ impl AppExt for App {
             recording.join()?
         }
 
-        match exit {
-            AppExit::Success => Ok(()),
-            AppExit::Error(code) => Err(eyre!("Scenario exited with error code {code}")),
+        if let AppExit::Error(code) = exit {
+            return Err(eyre!("Scenario exited with error code {code}"));
         }
+
+        let soft_errors = self
+            .world_mut()
+            .get_resource_mut::<SoftErrorResource>()
+            .expect("soft error storage should exist");
+        if !soft_errors.errors.is_empty() {
+            bail!("{} soft error(s) found", soft_errors.errors.len())
+        }
+
+        Ok(())
     }
 }
 

crates/bevyhavior_simulator/src/soft_error.rs

@@ -0,0 +1,36 @@
+use bevy::{
+    app::App,
+    ecs::system::{Res, ResMut, Resource, SystemParam},
+    time::Time,
+};
+
+use crate::time::{Ticks, TicksTime};
+
+#[derive(Clone)]
+pub struct SoftError;
+
+#[derive(Default, Resource)]
+pub struct SoftErrorResource {
+    pub errors: Vec<SoftError>,
+}
+
+#[derive(SystemParam)]
+pub struct SoftErrorSender<'w> {
+    time: Res<'w, Time<Ticks>>,
+    resource: ResMut<'w, SoftErrorResource>,
+}
+
+impl SoftErrorSender<'_> {
+    pub fn send(&mut self, message: impl Into<String>) {
+        let message = message.into();
+        let tick = self.time.ticks();
+        println!("{tick} {message}");
+        if self.resource.errors.is_empty() {
+            self.resource.errors.push(SoftError);
+        }
+    }
+}
+
+pub fn soft_error_plugin(app: &mut App) {
+    app.insert_resource(SoftErrorResource::default());
+}

crates/bevyhavior_simulator/src/test_rules.rs

@@ -0,0 +1,37 @@
+use bevy::ecs::system::{Query, ResMut};
+use types::{motion_command::MotionCommand, planned_path::PathSegment, roles::Role};
+
+use crate::{game_controller::GameController, robot::Robot, soft_error::SoftErrorSender};
+
+pub fn check_robots_dont_walk_into_rule_obstacles(
+    robots: Query<&Robot>,
+    game_controller: ResMut<GameController>,
+    mut soft_error: SoftErrorSender,
+) {
+    for robot in robots.iter() {
+        let rule_obstacles = &robot.database.main_outputs.rule_obstacles;
+        let motion_command = &robot.database.main_outputs.motion_command;
+        let MotionCommand::Walk { path, .. } = motion_command else {
+            continue;
+        };
+        let Some(PathSegment::LineSegment(segment)) = path.last() else {
+            continue;
+        };
+        let destination_in_field = robot.ground_to_field() * segment.1;
+
+        if game_controller.state.sub_state == Some(spl_network_messages::SubState::PenaltyKick)
+            && robot.database.main_outputs.role == Role::Striker
+        {
+            continue;
+        }
+
+        for obstacle in rule_obstacles {
+            if obstacle.contains(destination_in_field) {
+                soft_error.send(format!(
+                    "Robot {} ran into rule obstacle",
+                    robot.parameters.player_number
+                ));
+            }
+        }
+    }
+}

crates/control/src/behavior/support.rs

@@ -95,6 +95,9 @@ fn support_pose(
         (Some(FilteredGameState::Ready), Some(Some(SubState::PenaltyKick))) => {
             supporting_position.x().max(field_dimensions.length / 4.0)
         }
+        (Some(FilteredGameState::Playing { .. }), Some(Some(SubState::GoalKick))) => {
+            supporting_position.x().min(field_dimensions.length / 4.0)
+        }
         (Some(FilteredGameState::Ready), _)
         | (
             Some(FilteredGameState::Playing {

crates/geometry/src/rectangle.rs

@@ -42,4 +42,9 @@ impl<Frame> Rectangle<Frame> {
         let dimensions = self.max - self.min;
         dimensions.x() * dimensions.y()
     }
+
+    pub fn contains(self, point: Point2<Frame>) -> bool {
+        (self.min.x()..=self.max.x()).contains(&point.x())
+            && (self.min.y()..=self.max.y()).contains(&point.y())
+    }
 }

crates/types/src/rule_obstacles.rs

@@ -1,4 +1,5 @@
 use geometry::{circle::Circle, rectangle::Rectangle};
+use linear_algebra::Point2;
 use path_serde::{PathDeserialize, PathIntrospect, PathSerialize};
 use serde::{Deserialize, Serialize};
 
@@ -11,3 +12,12 @@ pub enum RuleObstacle {
     Circle(Circle<Field>),
     Rectangle(Rectangle<Field>),
 }
+
+impl RuleObstacle {
+    pub fn contains(&self, point: Point2<Field>) -> bool {
+        match self {
+            RuleObstacle::Circle(circle) => circle.contains(point),
+            RuleObstacle::Rectangle(rectangle) => rectangle.contains(point),
+        }
+    }
+}