Send ground truth for other robots

controllers/nugus_controller/nugus_controller.cpp

@@ -298,8 +298,7 @@ class PlayerServer {
                 // Convert angle-axis to rotation matrix
                 Eigen::Matrix3d Rft_mat =
                     Eigen::AngleAxisd(Rft[3], Eigen::Vector3d(Rft[0], Rft[1], Rft[2])).toRotationMatrix();
-                // Multiply by a rotation about the z axis by PI radians if the robot is on the negative side of the
-                // field
+                // Multiply by a rotation about the z axis by PI radians if the robot is on the negative side of the field
                 if (x_side < 0) {
                     Rft_mat = Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ()).toRotationMatrix() * Rft_mat;
                 }
@@ -846,7 +845,57 @@ class PlayerServer {
         rbww->set_z(rBWw(2, 0));
         vision_ground_truth->set_allocated_rbww(rbww);
 
+        // Export all robot's ground truth position
+        RobotsGroundTruth* robots_ground_truth = new RobotsGroundTruth();
+        robots_ground_truth->set_exists(true);
+        // Loop over both teams
+        for (const char* team_name : {"RED", "BLUE"}) {
+            for (int i = 1; i <= 4; i++) {
+                std::string robot_name = std::string(team_name) + "_" + std::to_string(i);
+                webots::Node* node = robot->getFromDef(robot_name);
+                if (node != nullptr) {
+                    RobotGroundTruth* robot_ground_truth = robots_ground_truth->add_robots();
+                    robot_ground_truth->set_id(robot_name);
+                    robot_ground_truth->set_team(team_name);
+                    robot_ground_truth->set_player_number(i);
+
+                    // Get position in field space
+                    const double* translation = node->getField("translation")->getSFVec3f();
+                    const double* rotation = node->getField("rotation")->getSFRotation();
+                    const double* velocity = node->getVelocity();
+
+                    // Convert position from field space to world space
+                    Eigen::Vector3d rRFf = Eigen::Vector3d(x_side * translation[0], x_side * translation[1], translation[2]);
+                    Eigen::Vector3d rRWw = Hfw.inverse() * rRFf;
+
+                    vec3* position = new vec3();
+                    position->set_x(rRWw.x());
+                    position->set_y(rRWw.y());
+                    position->set_z(rRWw.z());
+                    robot_ground_truth->set_allocated_rrww(position);
+
+                    // Convert velocity from field space to world space
+                    Eigen::Vector3d vRf = Eigen::Vector3d(x_side * velocity[0], x_side * velocity[1], velocity[2]);
+                    Eigen::Vector3d vRw = Hfw.linear().transpose() * vRf;
+
+                    vec3* v = new vec3();
+                    v->set_x(vRw.x());
+                    v->set_y(vRw.y());
+                    v->set_z(vRw.z());
+                    robot_ground_truth->set_allocated_vrw(v);
+
+                    // Extract yaw angle from rotation
+                    Eigen::AngleAxisd rot_aa(rotation[3], Eigen::Vector3d(rotation[0], rotation[1], rotation[2]));
+                    Eigen::Matrix3d rot_matrix = rot_aa.toRotationMatrix();
+                    Eigen::Matrix3d world_rot_matrix = Hfw.linear().transpose() * rot_matrix;
+                    float yaw = atan2(world_rot_matrix(1, 0), world_rot_matrix(0, 0));
+                    robot_ground_truth->set_yaw(yaw);
+                }
+            }
+        }
+
         sensor_measurements.set_allocated_vision_ground_truth(vision_ground_truth);
+        sensor_measurements.set_allocated_robots_ground_truth(robots_ground_truth);
     }
 
     void updateDevices() {

shared/message/messages.proto

@@ -111,6 +111,31 @@ message VisionGroundTruth {
     fvec3 rBWw = 2;
 }
 
+/// Ground truth for a single robot
+message RobotGroundTruth {
+    /// Robot identifier (e.g., "RED_1", "BLUE_2")
+    string id = 1;
+    /// Team color ("RED" or "BLUE")
+    string team = 2;
+    /// Player number within team
+    int32 player_number = 3;
+    /// Position of robot in world space
+    vec3 rRWw = 4;
+    /// Velocity of robot in world space
+    vec3 vRw = 5;
+    /// Rotation of robot (yaw angle in radians)
+    float yaw = 6;
+}
+
+/// Message containing ground truth data for all robots (opponents and teammates)
+message RobotsGroundTruth {
+    /// Indicates if this message exists
+    bool exists = 1;
+    /// Robot ground truth data
+    repeated RobotGroundTruth robots = 2;
+}
+
+
 message mat4 {
     vec4 x = 1;
     vec4 y = 2;
@@ -148,9 +173,10 @@ message SensorMeasurements {
     // NUbots-specific data. Set to 100 in case other data is added in by the RoboCup TC
     RobotPoseGroundTruth    robot_pose_ground_truth = 100;
     VisionGroundTruth       vision_ground_truth       = 101;
+    RobotsGroundTruth       robots_ground_truth         = 102;
     // Optimisation
-    OptimisationRobotPosition robot_position = 102;
-    bool                      reset_done     = 103;
+    OptimisationRobotPosition robot_position = 103;
+    bool                      reset_done     = 104;
 }
 
 message MotorPosition {