From c0dabbb0cb7702a8d026f7a7a670ddf3a7ef9fb4 Mon Sep 17 00:00:00 2001 From: A-Herfel Date: Sat, 5 Jul 2025 15:59:23 +1000 Subject: [PATCH 1/3] wip --- .../nugus_controller/nugus_controller.cpp | 50 +++++++++++++++++++ shared/message/messages.proto | 17 +++++++ 2 files changed, 67 insertions(+) diff --git a/controllers/nugus_controller/nugus_controller.cpp b/controllers/nugus_controller/nugus_controller.cpp index 2fa2cc13b..10ff1067a 100644 --- a/controllers/nugus_controller/nugus_controller.cpp +++ b/controllers/nugus_controller/nugus_controller.cpp @@ -847,6 +847,56 @@ class PlayerServer { rbww->set_z(rBWw(2, 0)); vision_ground_truth->set_allocated_rbww(rbww); + // Get opponent robot ground truth data (RED team for BLUE players, BLUE team for RED players) + std::string opponent_team = (team == RED) ? "BLUE" : "RED"; + for (int i = 1; i <= 4; i++) { + std::string opponent_name = opponent_team + "_" + std::to_string(i); + webots::Node* opponent_node = robot->getFromDef(opponent_name); + + if (opponent_node != nullptr) { + RobotGroundTruth* robot_ground_truth = vision_ground_truth->add_robots(); + robot_ground_truth->set_id(opponent_name); + robot_ground_truth->set_team(opponent_team); + robot_ground_truth->set_player_number(i); + + // Get opponent position in field space + const double* opponent_translation = opponent_node->getField("translation")->getSFVec3f(); + const double* opponent_rotation = opponent_node->getField("rotation")->getSFRotation(); + const double* opponent_velocity = opponent_node->getVelocity(); + + // Convert position from field space to world space + Eigen::Vector3d rOFf = Eigen::Vector3d(x_side * opponent_translation[0], x_side * opponent_translation[1], opponent_translation[2]); + Eigen::Vector3d rOWw = Hfw.inverse() * rOFf; + + fvec3* position = new fvec3(); + position->set_x(rOWw.x()); + position->set_y(rOWw.y()); + position->set_z(rOWw.z()); + robot_ground_truth->set_allocated_rrww(position); + + // Convert velocity from field space to world space + Eigen::Vector3d vOFf = Eigen::Vector3d(x_side * opponent_velocity[0], x_side * opponent_velocity[1], opponent_velocity[2]); + Eigen::Vector3d vOWw = Hfw.linear().transpose() * vOFf; + + fvec3* velocity = new fvec3(); + velocity->set_x(vOWw.x()); + velocity->set_y(vOWw.y()); + velocity->set_z(vOWw.z()); + robot_ground_truth->set_allocated_vrw(velocity); + + // Extract yaw angle from rotation + Eigen::AngleAxisd opponent_rotation_aa(opponent_rotation[3], Eigen::Vector3d(opponent_rotation[0], opponent_rotation[1], opponent_rotation[2])); + Eigen::Matrix3d opponent_rotation_matrix = opponent_rotation_aa.toRotationMatrix(); + + // Apply field-to-world transformation to rotation + Eigen::Matrix3d world_rotation_matrix = Hfw.linear().transpose() * opponent_rotation_matrix; + + // Extract yaw angle (rotation around Z-axis) + float yaw = atan2(world_rotation_matrix(1, 0), world_rotation_matrix(0, 0)); + robot_ground_truth->set_yaw(yaw); + } + } + sensor_measurements.set_allocated_vision_ground_truth(vision_ground_truth); } diff --git a/shared/message/messages.proto b/shared/message/messages.proto index a0a63635a..636ab4756 100644 --- a/shared/message/messages.proto +++ b/shared/message/messages.proto @@ -115,6 +115,23 @@ message VisionGroundTruth { bool exists = 1; /// Vector from the world to the ball in world space. fvec3 rBWw = 2; + /// Robot ground truth data + repeated RobotGroundTruth robots = 3; +} + +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 + fvec3 rRWw = 4; + /// Velocity of robot in world space + fvec3 vRw = 5; + /// Rotation of robot (yaw angle in radians) + float yaw = 6; } message mat4 { From a492de7e6bbc95774c655fd581d8eb4bbae4f990 Mon Sep 17 00:00:00 2001 From: A-Herfel Date: Tue, 8 Jul 2025 15:37:02 +1000 Subject: [PATCH 2/3] organise better --- .../nugus_controller/nugus_controller.cpp | 97 +++++++++---------- shared/message/messages.proto | 17 +++- 2 files changed, 61 insertions(+), 53 deletions(-) diff --git a/controllers/nugus_controller/nugus_controller.cpp b/controllers/nugus_controller/nugus_controller.cpp index bd80f8fc1..d648d5ac3 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,57 +845,57 @@ class PlayerServer { rbww->set_z(rBWw(2, 0)); vision_ground_truth->set_allocated_rbww(rbww); - // Get opponent robot ground truth data (RED team for BLUE players, BLUE team for RED players) - std::string opponent_team = (team == RED) ? "BLUE" : "RED"; - for (int i = 1; i <= 4; i++) { - std::string opponent_name = opponent_team + "_" + std::to_string(i); - webots::Node* opponent_node = robot->getFromDef(opponent_name); - - if (opponent_node != nullptr) { - RobotGroundTruth* robot_ground_truth = vision_ground_truth->add_robots(); - robot_ground_truth->set_id(opponent_name); - robot_ground_truth->set_team(opponent_team); - robot_ground_truth->set_player_number(i); - - // Get opponent position in field space - const double* opponent_translation = opponent_node->getField("translation")->getSFVec3f(); - const double* opponent_rotation = opponent_node->getField("rotation")->getSFRotation(); - const double* opponent_velocity = opponent_node->getVelocity(); - - // Convert position from field space to world space - Eigen::Vector3d rOFf = Eigen::Vector3d(x_side * opponent_translation[0], x_side * opponent_translation[1], opponent_translation[2]); - Eigen::Vector3d rOWw = Hfw.inverse() * rOFf; - - fvec3* position = new fvec3(); - position->set_x(rOWw.x()); - position->set_y(rOWw.y()); - position->set_z(rOWw.z()); - robot_ground_truth->set_allocated_rrww(position); - - // Convert velocity from field space to world space - Eigen::Vector3d vOFf = Eigen::Vector3d(x_side * opponent_velocity[0], x_side * opponent_velocity[1], opponent_velocity[2]); - Eigen::Vector3d vOWw = Hfw.linear().transpose() * vOFf; - - fvec3* velocity = new fvec3(); - velocity->set_x(vOWw.x()); - velocity->set_y(vOWw.y()); - velocity->set_z(vOWw.z()); - robot_ground_truth->set_allocated_vrw(velocity); - - // Extract yaw angle from rotation - Eigen::AngleAxisd opponent_rotation_aa(opponent_rotation[3], Eigen::Vector3d(opponent_rotation[0], opponent_rotation[1], opponent_rotation[2])); - Eigen::Matrix3d opponent_rotation_matrix = opponent_rotation_aa.toRotationMatrix(); - - // Apply field-to-world transformation to rotation - Eigen::Matrix3d world_rotation_matrix = Hfw.linear().transpose() * opponent_rotation_matrix; - - // Extract yaw angle (rotation around Z-axis) - float yaw = atan2(world_rotation_matrix(1, 0), world_rotation_matrix(0, 0)); - robot_ground_truth->set_yaw(yaw); + // 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; + + fvec3* position = new fvec3(); + 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; + + fvec3* v = new fvec3(); + 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 14244e719..8b5faa064 100644 --- a/shared/message/messages.proto +++ b/shared/message/messages.proto @@ -109,10 +109,9 @@ message VisionGroundTruth { bool exists = 1; /// Vector from the world to the ball in world space. fvec3 rBWw = 2; - /// Robot ground truth data - repeated RobotGroundTruth robots = 3; } +/// Ground truth for a single robot message RobotGroundTruth { /// Robot identifier (e.g., "RED_1", "BLUE_2") string id = 1; @@ -128,6 +127,15 @@ message RobotGroundTruth { 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; @@ -165,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 { From 284c858381911cf249d25d09192b6e0c2b3d21a3 Mon Sep 17 00:00:00 2001 From: A-Herfel Date: Tue, 15 Jul 2025 22:17:14 +1000 Subject: [PATCH 3/3] change from float to double --- controllers/nugus_controller/nugus_controller.cpp | 4 ++-- shared/message/messages.proto | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/controllers/nugus_controller/nugus_controller.cpp b/controllers/nugus_controller/nugus_controller.cpp index d648d5ac3..d41dc19e5 100644 --- a/controllers/nugus_controller/nugus_controller.cpp +++ b/controllers/nugus_controller/nugus_controller.cpp @@ -868,7 +868,7 @@ class PlayerServer { Eigen::Vector3d rRFf = Eigen::Vector3d(x_side * translation[0], x_side * translation[1], translation[2]); Eigen::Vector3d rRWw = Hfw.inverse() * rRFf; - fvec3* position = new fvec3(); + vec3* position = new vec3(); position->set_x(rRWw.x()); position->set_y(rRWw.y()); position->set_z(rRWw.z()); @@ -878,7 +878,7 @@ class PlayerServer { Eigen::Vector3d vRf = Eigen::Vector3d(x_side * velocity[0], x_side * velocity[1], velocity[2]); Eigen::Vector3d vRw = Hfw.linear().transpose() * vRf; - fvec3* v = new fvec3(); + vec3* v = new vec3(); v->set_x(vRw.x()); v->set_y(vRw.y()); v->set_z(vRw.z()); diff --git a/shared/message/messages.proto b/shared/message/messages.proto index 8b5faa064..928ef26c0 100644 --- a/shared/message/messages.proto +++ b/shared/message/messages.proto @@ -120,9 +120,9 @@ message RobotGroundTruth { /// Player number within team int32 player_number = 3; /// Position of robot in world space - fvec3 rRWw = 4; + vec3 rRWw = 4; /// Velocity of robot in world space - fvec3 vRw = 5; + vec3 vRw = 5; /// Rotation of robot (yaw angle in radians) float yaw = 6; }