Add live Webots vision error testing (goal detection)

nuwebots/controllers/nugus_controller/CMakeLists.txt

@@ -3,6 +3,8 @@ find_package(webots REQUIRED)
 find_package(Eigen3 REQUIRED)
 find_package(Protobuf REQUIRED)
 
+find_package(Eigen3 REQUIRED)
+
 # Find/list all of the source files
 set(controller_sources nugus_controller.cpp)
 

nuwebots/controllers/nugus_controller/nugus_controller.cpp

@@ -794,12 +794,22 @@ class PlayerServer {
         Eigen::Vector3d rBXx           = Eigen::Vector3d(ball_translation[0], ball_translation[1], ball_translation[2]);
         Eigen::Vector3d rBWw           = Hxw.inverse() * rBXx;
 
+        // Get field position in world space
+        const double* field_position = robot->getFromDef("FIELD")->getPosition();
+        Eigen::Vector3d rFXx         = Eigen::Vector3d(field_position[0], field_position[1], field_position[2]);
+        Eigen::Vector3d rFWw         = Hxw.inverse() * rFXx;
+
         fvec3* rbww = new fvec3();
         rbww->set_x(rBWw(0, 0));
         rbww->set_y(rBWw(1, 0));
         rbww->set_z(rBWw(2, 0));
         vision_ground_truth->set_allocated_rbww(rbww);
 
+        fvec3* rfww = new fvec3();
+        rfww->set_x(rFWw(0, 0));
+        rfww->set_y(rFWw(1, 0));
+        rfww->set_z(rFWw(2, 0));
+        vision_ground_truth->set_allocated_rfww(rfww);
         sensor_measurements.set_allocated_vision_ground_truth(vision_ground_truth);
     }
 

nuwebots/shared/message/messages.proto

@@ -103,6 +103,8 @@ message VisionGroundTruth {
     bool exists = 1;
     /// Vector from the world to the ball in world space.
     fvec3 rBWw = 2;
+    // Vector from the world to the field in world space.
+    fvec3 rFWw = 3;
 }
 
 message mat4 {