Introduce Calibration state (#87)

Co-authored-by: Gijs de Jong <[email protected]>

crates/control/src/behavior/calibrate.rs

@@ -0,0 +1,11 @@
+use types::{MotionCommand, PrimaryState, WorldState};
+
+pub fn execute(world_state: &WorldState) -> Option<MotionCommand> {
+    match world_state.robot.primary_state {
+        PrimaryState::Calibration => Some(MotionCommand::Stand {
+            head: types::HeadMotion::ZeroAngles,
+            is_energy_saving: false,
+        }),
+        _ => None,
+    }
+}

crates/control/src/behavior/mod.rs

@@ -1,3 +1,4 @@
+mod calibrate;
 mod defend;
 mod dribble;
 mod fall_safely;

crates/control/src/behavior/node.rs

@@ -12,6 +12,7 @@ use types::{
 };
 
 use super::{
+    calibrate,
     defend::Defend,
     dribble, fall_safely,
     head::LookAction,
@@ -97,6 +98,7 @@ impl Behavior {
             Action::FallSafely,
             Action::StandUp,
             Action::Stand,
+            Action::Calibrate,
         ];
 
         if let Some(active_since) = self.active_since {
@@ -184,6 +186,7 @@ impl Behavior {
                     Action::StandUp => stand_up::execute(world_state),
                     Action::Stand => stand::execute(world_state, context.field_dimensions),
                     Action::LookAround => look_around::execute(world_state),
+                    Action::Calibrate => calibrate::execute(world_state),
                     Action::DefendGoal => defend.goal(&mut context.path_obstacles),
                     Action::DefendKickOff => defend.kick_off(&mut context.path_obstacles),
                     Action::DefendLeft => defend.left(&mut context.path_obstacles),

crates/control/src/localization.rs

@@ -529,6 +529,10 @@ impl Localization {
                 );
                 Some(*context.robot_to_field)
             }
+            (PrimaryState::Calibration, _) => {
+                *context.robot_to_field = Isometry2::identity();
+                Some(*context.robot_to_field)
+            }
             _ => None,
         };
         Ok(MainOutputs {

crates/types/src/action.rs

@@ -11,6 +11,7 @@ pub enum Action {
     StandUp,
     Stand,
     LookAround,
+    Calibrate,
     Dribble,
     DefendGoal,
     DefendKickOff,

etc/configuration/head.P0000074A03S83I00027.json

@@ -1,4 +1,10 @@
 {
-  "disable_communication_acceptor": true,
-  "player_number": "Two"
+  "camera_matrix_parameters": {
+    "vision_top": {
+      "extrinsic_rotations": [
+        0.7900000214576721, -3.119999885559082, 0.7400000095367432
+      ]
+    }
+  },
+  "player_number": "One"
 }

etc/configuration/head.P0000074A04S8C700011.json

@@ -1,4 +1,10 @@
 {
-  "disable_communication_acceptor": true,
-  "player_number": "Three"
+  "camera_matrix_parameters": {
+    "vision_top": {
+      "extrinsic_rotations": [
+        0.03999999910593033, -3.009999990463257, 1.2999999523162842
+      ]
+    }
+  },
+  "player_number": "Two"
 }