Keep track of balls relative to the robot (#570)

Co-authored-by: Gijs de Jong <14833076+oxkitsune@users.noreply.github.com>

Author: YukumoHunter

deploy/config/ball_detection.toml

@@ -74,7 +74,7 @@ confidence_threshold = 0.96
 time_budget = 1500
 
 # Process noise parameter for position prediction in the Kalman filter
-prediction_noise = 0.0006 # ~2.45 cm per cycle or ~1.96 m per second
+prediction_noise = 0.0018 # ~3.5 m per second
 
 # Measurement noise parameter for the Kalman filter
 measurement_noise = 0.01 # ~10cm per measurement

deploy/config/scan_lines.toml

@@ -16,6 +16,6 @@ max_black_luminance = 60.0
 max_black_saturation = 160.0
 
 # Green chromaticity threshold
-blue_chromaticity_threshold = 0.3    # not fine tuned
-green_chromaticity_threshold = 0.400 # tuned
-red_chromaticity_threshold = 0.3     # not fine tuned
+blue_chromaticity_threshold = 0.3   # not fine tuned
+green_chromaticity_threshold = 0.36 # tuned
+red_chromaticity_threshold = 0.3    # not fine tuned

yggdrasil/src/behavior/behaviors/walk_to_ball.rs

@@ -38,7 +38,10 @@ fn walk_to_ball(
     mut nao_manager: ResMut<NaoManager>,
     ball_tracker: Res<BallTracker>,
 ) {
-    let Some(ball) = ball_tracker.stationary_ball() else {
+    let Some(ball) = ball_tracker
+        .stationary_ball()
+        .map(|ball| pose.robot_to_world(&ball))
+    else {
         return;
     };
 

yggdrasil/src/behavior/engine.rs

@@ -9,7 +9,6 @@ use nalgebra::Point2;
 
 use crate::{
     core::config::showtime::PlayerConfig,
-    localization::RobotPose,
     motion::walking_engine::Gait,
     sensor::{button::HeadButtons, falling::FallState, imu::IMUValues},
     vision::ball_detection::ball_tracker::BallTracker,
@@ -205,7 +204,6 @@ pub fn role_base(
     game_controller_message: Option<Res<GameControllerMessage>>,
     imu_values: Res<IMUValues>,
     ball_tracker: Res<BallTracker>,
-    pose: Res<RobotPose>,
 ) {
     commands.disable_role();
     let behavior = behavior_state.get();
@@ -285,7 +283,7 @@ pub fn role_base(
         PrimaryState::Playing { .. } => {
             let possible_ball_distance = ball_tracker
                 .stationary_ball()
-                .map(|ball| pose.distance_to(&ball));
+                .map(|ball| ball.coords.norm());
 
             RoleState::assign_role(
                 &mut commands,

yggdrasil/src/behavior/roles/striker.rs

@@ -41,15 +41,15 @@ pub fn striker_role(
     ball_tracker: Res<BallTracker>,
     mut nao_manager: ResMut<NaoManager>,
 ) {
-    let Some(ball) = ball_tracker.stationary_ball() else {
+    let Some(relative_ball) = ball_tracker.stationary_ball() else {
         commands.set_behavior(RlStrikerSearchBehavior);
         return;
     };
 
-    let relative_ball = pose.world_to_robot(&ball);
-    let ball_angle = pose.angle_to(&ball);
-    let ball_distance = pose.distance_to(&ball);
-    let ball_target = Point3::new(ball.x, ball.y, 0.2);
+    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 relative_goalpost_left =
         pose.world_to_robot(&Point2::new(layout_config.field.length / 2., 0.8));
@@ -89,7 +89,7 @@ pub fn striker_role(
     } else if ball_angle.abs() > WALK_WITH_BALL_ANGLE {
         nao_manager.set_right_eye_led(RightEye::fill(color::f32::PURPLE), Priority::default());
 
-        if relative_ball.y < 0. {
+        if relative_ball.y.is_sign_negative() {
             commands.set_behavior(Walk {
                 step: Step::RIGHT,
                 look_target: Some(ball_target),

yggdrasil/src/game_controller/transmit.rs

@@ -67,7 +67,7 @@ pub fn send_message(
         return;
     }
 
-    let (ball_age, ball_pos) = balls_to_game_controller_ball(&ball_tracker, &robot_pose);
+    let (ball_age, ball_pos) = balls_to_game_controller_ball(&ball_tracker);
     let return_message = GameControllerReturnMessage::new(
         player_config.player_number,
         player_config.team_number,
@@ -94,17 +94,16 @@ fn robot_pose_to_game_controller_pose(robot_pose: &RobotPose) -> [f32; 3] {
     ]
 }
 
-fn balls_to_game_controller_ball(ball_tracker: &BallTracker, pose: &RobotPose) -> (f32, [f32; 2]) {
+fn balls_to_game_controller_ball(ball_tracker: &BallTracker) -> (f32, [f32; 2]) {
     let Some(ball) = ball_tracker.stationary_ball() else {
         return NO_BALL_DETECTED_DATA;
     };
 
-    let relative_ball = pose.world_to_robot(&ball);
     (
         ball_tracker.timestamp.elapsed().as_secs_f32(),
         [
-            relative_ball.x * MILLIMETERS_PER_METER,
-            relative_ball.y * MILLIMETERS_PER_METER,
+            ball.x * MILLIMETERS_PER_METER,
+            ball.y * MILLIMETERS_PER_METER,
         ],
     )
 }

yggdrasil/src/vision/ball_detection/ball_tracker.rs

@@ -4,14 +4,15 @@ use bevy::prelude::*;
 use filter::{CovarianceMatrix, StateTransform, StateVector, UnscentedKalmanFilter};
 use nalgebra::{point, Point2};
 
-use crate::nao::Cycle;
+use crate::{localization::odometry::Odometry, nao::Cycle};
 
 #[derive(Debug)]
 pub enum BallHypothesis {
     Moving(f32),
     Stationary(f32),
 }
 
+/// Filtered ball position in robot coordinates
 #[derive(Resource, Deref, DerefMut)]
 pub struct BallTracker {
     #[deref]
@@ -51,16 +52,21 @@ impl BallTracker {
         }
     }
 
-    pub fn predict(&mut self) {
-        let f = |p: BallPosition| p;
-        if let Err(err) = self.position_kf.predict(f, self.prediction_noise) {
+    pub fn predict(&mut self, odometry: &Odometry) {
+        // update last ball state with odometry, as we're moving relative to the ball.
+        if let Err(err) = self.position_kf.predict(
+            |p: BallPosition| BallPosition(odometry.offset_to_last.inverse() * p.0),
+            self.prediction_noise,
+        ) {
             error!("failed to predict ball position: {err:?}");
         }
     }
 
     pub fn measurement_update(&mut self, measurement: BallPosition) {
-        let h = |p: BallPosition| p;
-        if let Err(err) = self.position_kf.update(h, measurement, self.sensor_noise) {
+        if let Err(err) =
+            self.position_kf
+                .update(std::convert::identity, measurement, self.sensor_noise)
+        {
             error!("failed to do measurement update: {err:?}");
         }
 

yggdrasil/src/vision/ball_detection/classifier.rs

@@ -14,8 +14,8 @@ use serde::{Deserialize, Serialize};
 use serde_with::{serde_as, DurationMicroSeconds};
 
 use crate::core::debug::DebugContext;
-use crate::localization::RobotPose;
 
+use crate::localization::odometry::Odometry;
 use crate::nao::Cycle;
 use crate::vision::camera::init_camera;
 use crate::vision::referee::detect::VisualRefereeDetectionStatus;
@@ -146,8 +146,8 @@ impl<T: CameraLocation> Clone for Ball<T> {
 }
 
 /// System that runs the prediction step for the UKF backing the ball tracker.
-fn update_ball_tracker(mut ball_tracker: ResMut<BallTracker>) {
-    ball_tracker.predict();
+fn update_ball_tracker(mut ball_tracker: ResMut<BallTracker>, odometry: Res<Odometry>) {
+    ball_tracker.predict(&odometry);
 }
 
 #[allow(clippy::too_many_arguments)]
@@ -160,7 +160,6 @@ fn classify_balls<T: CameraLocation>(
     mut ball_tracker: ResMut<BallTracker>,
     camera_matrix: Res<CameraMatrix<T>>,
     config: Res<BallDetectionConfig>,
-    robot_pose: Res<RobotPose>,
 ) {
     let classifier = &config.classifier;
     let start = Instant::now();
@@ -207,7 +206,7 @@ fn classify_balls<T: CameraLocation>(
             continue;
         };
 
-        let position = BallPosition(robot_pose.robot_to_world(&Point2::from(robot_to_ball.xy())));
+        let position = BallPosition(robot_to_ball.xy());
 
         confident_balls.push((position, confidence, proposal.clone()));
 

yggdrasil/src/vision/ball_detection/mod.rs

@@ -19,6 +19,7 @@ use serde_with::{serde_as, DurationMilliSeconds};
 
 use crate::{
     core::debug::DebugContext,
+    localization::RobotPose,
     nao::{Cycle, NaoManager, Priority},
     prelude::*,
 };
@@ -135,13 +136,14 @@ fn setup_3d_ball_debug_logging(dbg: DebugContext) {
 fn log_3d_balls(
     dbg: DebugContext,
     ball_tracker: Res<BallTracker>,
+    robot_pose: Res<RobotPose>,
     mut last_logged: Local<Option<Cycle>>,
 ) {
     let last_ball_tracker_update = ball_tracker.cycle;
     let state = ball_tracker.cutoff();
 
     if let BallHypothesis::Stationary(max_variance) = state {
-        let pos = ball_tracker.state();
+        let pos = robot_pose.robot_to_world(&ball_tracker.state());
         if last_logged.is_none_or(|last_logged_cycle| last_ball_tracker_update > last_logged_cycle)
         {
             *last_logged = Some(last_ball_tracker_update);