diff --git a/controllers/nugus_controller/nugus_controller.cpp b/controllers/nugus_controller/nugus_controller.cpp index e6bd489dd..d41dc19e5 100644 --- a/controllers/nugus_controller/nugus_controller.cpp +++ b/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() { diff --git a/shared/message/messages.proto b/shared/message/messages.proto index 352c77840..928ef26c0 100644 --- a/shared/message/messages.proto +++ b/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 {