diag test + Head pose fix

include/RobotManager.h

@@ -47,7 +47,8 @@ class RobotManager {
         void bindMujoco(MujocoContext* mujContext);
 
         std::shared_ptr<Robot> create(const std::string& name, const std::string& type, uint8_t number, const Eigen::Vector3d& pos,
-                                      const Eigen::Vector3d& ori, const std::string& colorName, const std::shared_ptr<Team> team);
+                                      const Eigen::Vector3d& ori, const std::string& colorName, const std::shared_ptr<Team> team,
+                                      const std::string& role = "Striker");
 
         void startContainers(const std::string& fwkCfgPath = spqr::frameworkConfigPath, const std::string& pathsCfgPath = spqr::pathsConfigPath);
 

include/frontend/tools_panel/ToolsPanelGrid.h

@@ -485,7 +485,7 @@ class ToolsPanelGrid : public QWidget {
                                     auto it = sensors.find("joints");
                                     if (it != sensors.end()) {
                                         Sensor* jointsSensor = it->second;
-                                        Eigen::Vector3d position = dynamic_cast<Joints*>(jointsSensor)->getPosition();
+                                        Eigen::VectorXd position = dynamic_cast<Joints*>(jointsSensor)->getPosition();
                                         plot->addDataPoint("head_yaw", position(0), simTime);
                                         plot->addDataPoint("head_pitch", position(1), simTime);
                                         plot->addDataPoint("shoulder_left_pitch", position(2), simTime);
@@ -514,7 +514,7 @@ class ToolsPanelGrid : public QWidget {
                                     auto it = sensors.find("joints");
                                     if (it != sensors.end()) {
                                         Sensor* jointsSensor = it->second;
-                                        Eigen::Vector3d velocity = dynamic_cast<Joints*>(jointsSensor)->getVelocity();
+                                        Eigen::VectorXd velocity = dynamic_cast<Joints*>(jointsSensor)->getVelocity();
                                         plot->addDataPoint("head_yaw", velocity(0), simTime);
                                         plot->addDataPoint("head_pitch", velocity(1), simTime);
                                         plot->addDataPoint("shoulder_left_pitch", velocity(2), simTime);
@@ -543,7 +543,7 @@ class ToolsPanelGrid : public QWidget {
                                     auto it = sensors.find("joints");
                                     if (it != sensors.end()) {
                                         Sensor* jointsSensor = it->second;
-                                        Eigen::Vector3d acceleration = dynamic_cast<Joints*>(jointsSensor)->getAcceleration();
+                                        Eigen::VectorXd acceleration = dynamic_cast<Joints*>(jointsSensor)->getAcceleration();
                                         plot->addDataPoint("head_yaw", acceleration(0), simTime);
                                         plot->addDataPoint("head_pitch", acceleration(1), simTime);
                                         plot->addDataPoint("shoulder_left_pitch", acceleration(2), simTime);
@@ -572,7 +572,7 @@ class ToolsPanelGrid : public QWidget {
                                     auto it = sensors.find("joints");
                                     if (it != sensors.end()) {
                                         Sensor* jointsSensor = it->second;
-                                        Eigen::Vector3d torque = dynamic_cast<Joints*>(jointsSensor)->getTorque();
+                                        Eigen::VectorXd torque = dynamic_cast<Joints*>(jointsSensor)->getTorque();
                                         plot->addDataPoint("head_yaw", torque(0), simTime);
                                         plot->addDataPoint("head_pitch", torque(1), simTime);
                                         plot->addDataPoint("shoulder_left_pitch", torque(2), simTime);
@@ -655,9 +655,13 @@ class ToolsPanelGrid : public QWidget {
                                             int height = depthCamera->getHeight();
 
                                             if (!depthData.empty() && width > 0 && height > 0) {
+                                                // Depth is uint16 millimetres; map the near 10 m to the
+                                                // full 8-bit range so the preview keeps usable contrast
+                                                constexpr uint32_t kPreviewRangeMm = 10000;
                                                 std::vector<uint8_t> depthImage(depthData.size());
                                                 for (size_t i = 0; i < depthData.size(); ++i) {
-                                                    depthImage[i] = static_cast<uint8_t>(depthData[i] / 256);  // Scale 16-bit to 8-bit
+                                                    const uint32_t scaled = static_cast<uint32_t>(depthData[i]) * 255u / kPreviewRangeMm;
+                                                    depthImage[i] = static_cast<uint8_t>(scaled > 255u ? 255u : scaled);
                                                 }
                                                 imageTool->setImage(depthImage.data(), width, height, 1);
                                             } else {

include/robots/BoosterT1.h

@@ -9,6 +9,7 @@
 
 #include <Eigen/Eigen>
 #include <cstdlib>
+#include <iostream>
 #include <memory>
 #include <msgpack.hpp>
 #include <msgpack/v3/object_fwd_decl.hpp>
@@ -19,7 +20,9 @@
 #include "MujocoContext.h"
 #include "robots/Robot.h"
 #include "sensors/CameraDepth.h"
+#include "sensors/CameraInfo.h"
 #include "sensors/CameraRGB.h"
+#include "sensors/GroundRelativePosition.h"
 #include "sensors/ImageSharedMemoryWriter.h"
 #include "sensors/Imu.h"
 #include "sensors/Joint.h"
@@ -34,11 +37,13 @@ class Team;  // Forward declaration
 class BoosterT1 : public Robot {
     public:
         Pose* pose = nullptr;
+        GroundRelativePosition* headPose = nullptr;
         Imu* imu = nullptr;
         Joints* joints = nullptr;
         Oracle* oracle = nullptr;
         CameraRGB* rgbCamera;
         CameraDepth* depthCamera;
+        CameraInfo* rgbCameraInfo = nullptr;
 
         BoosterT1(const std::string& name, const std::string& type, uint8_t number, const Eigen::Vector3d& initPosition,
                   const Eigen::Vector3d& initOrientation, const std::string& colorName, const std::shared_ptr<Team>& team)
@@ -72,6 +77,8 @@ class BoosterT1 : public Robot {
 
         void bindMujoco(MujocoContext* mujCtx) override {
             pose = new Pose(mujCtx->model, mujCtx->data, (name + "_position").c_str(), (name + "_orientation").c_str());
+            headPose = new GroundRelativePosition(mujCtx->model, mujCtx->data, (name + "_head_rgb_cam_site").c_str(),
+                                                  GroundRelativePosition::TargetType::Site, pose);
             imu = new Imu(mujCtx->model, mujCtx->data, (name + "_linear-acceleration").c_str(), (name + "_angular-velocity").c_str());
             joints = new Joints(mujCtx->model, mujCtx->data, joint_map);
 
@@ -103,12 +110,14 @@ class BoosterT1 : public Robot {
             // Use RGB viewpoint for simulated depth to provide aligned depth-to-color.
             // This avoids parallax between rgb_cam and depth_cam when unprojecting RGB detections.
             depthCamera = new CameraDepth(mujCtx, (name + "_rgb_cam").c_str());
+            rgbCameraInfo = new CameraInfo(mujCtx->model, (name + "_rgb_cam").c_str());
 
             // Configure the writer for the shared memory file
             const int width = rgbCamera->getWidth();
             const int height = rgbCamera->getHeight();
             rgb_writer_.configure(shmFilePath_("rgb"), width, height, 3);
-            depth_writer_.configure(shmFilePath_("depth"), width, height, 1);
+            // Depth is 16-bit (16UC1): two bytes per pixel, published without precision loss
+            depth_writer_.configure(shmFilePath_("depth"), width, height, 2);
 
             // Create Oracle with the pose and all robots
             oracle = new Oracle(mujCtx->model, mujCtx->data, name, pose);
@@ -142,24 +151,28 @@ class BoosterT1 : public Robot {
             std::map<std::string, msgpack::object> msg;
             msg["robot_name"] = msgpack::object(name, buffer_zone_);
             msg["pose"] = pose->serialize(buffer_zone_);
+            msg["head_pose"] = headPose->serialize(buffer_zone_);
             msg["imu"] = imu->serialize(buffer_zone_);
             msg["joints"] = joints->serialize(buffer_zone_);
             msg["oracle"] = oracle->serialize(buffer_zone_);
+            msg["camera_info"] = rgbCameraInfo->serialize(buffer_zone_);
 
             // Write in the shared file the information
             rgb_writer_.write(rgbCamera->getImage());
-            depth_writer_.write(depthCamera->getDepth8bit());
+            depth_writer_.write(depthCamera->getDepth16UC1());
 
             return msg;
         }
 
         std::map<std::string, Sensor*> getSensors() override {
             std::map<std::string, Sensor*> sensors;
             sensors["pose"] = pose;
+            sensors["head_pose"] = headPose;
             sensors["imu"] = imu;
             sensors["joints"] = joints;
             sensors["rgb_camera"] = rgbCamera;
             sensors["depth_camera"] = depthCamera;
+            sensors["camera_info"] = rgbCameraInfo;
             return sensors;
         }
 
@@ -170,11 +183,13 @@ class BoosterT1 : public Robot {
 
         void update() override {
             pose->update();
+            headPose->update();
             imu->update();
             joints->update();
             oracle->update();
             rgbCamera->update();
             depthCamera->update();
+            rgbCameraInfo->update();
         }
 
         ~BoosterT1() = default;

include/robots/Robot.h

@@ -26,8 +26,16 @@ class Team;  // Forward declaration
 class Robot {
     public:
         Robot(const std::string& name, const std::string& type, uint8_t number, const Eigen::Vector3d& initPosition,
-              const Eigen::Vector3d& initOrientation, const std::string& colorName, const std::shared_ptr<Team>& team)
-            : name(name), type(type), number(number), initPosition(initPosition), initOrientation(initOrientation), colorName(colorName), team(team) {
+              const Eigen::Vector3d& initOrientation, const std::string& colorName, const std::shared_ptr<Team>& team,
+              const std::string& role = "Striker")
+            : name(name),
+              type(type),
+              number(number),
+              initPosition(initPosition),
+              initOrientation(initOrientation),
+              colorName(colorName),
+              team(team),
+              role(role) {
             if (colorName == "red") {
                 color = {130, 36, 51};
             } else if (colorName == "blue") {
@@ -50,6 +58,7 @@ class Robot {
         Eigen::Vector3d initPosition;
         Eigen::Vector3d initOrientation;  // Euler angles
         std::string colorName;
+        std::string role;
         std::tuple<int, int, int> color;
         std::unique_ptr<Container> container;
         std::shared_ptr<Team> team;

include/sensors/CameraDepth.h

@@ -88,7 +88,9 @@ class CameraDepth : public Sensor {
             const float extent = static_cast<float>(mujContext->model->stat.extent);
             const float znear = static_cast<float>(mujContext->model->vis.map.znear) * extent;
             const float zfar = static_cast<float>(mujContext->model->vis.map.zfar) * extent;
-            constexpr float kDepthMaxMeters = 10.0f;  // Keep in sync with SimBridge mono8 decoding.
+            // Depth is stored as uint16 millimetres, the ROS 16UC1 convention consumers
+            // expect (raw / 1000 = metres). Saturates at 65535 mm ~= 65 m.
+            constexpr float kMillimetresPerMetre = 1000.0f;
             float max_u16 = static_cast<float>(std::numeric_limits<uint16_t>::max());
 
             // Resample offscreen depth to camera resolution and convert to metric depth.
@@ -104,8 +106,8 @@ class CameraDepth : public Sensor {
                     float z_converted = (znear * zfar) / (zfar - z_raw * (zfar - znear));
                     depthNormalized[dstRow + x] = z_converted;
 
-                    float normalizedDepth = std::clamp(z_converted / kDepthMaxMeters, 0.0f, 1.0f);
-                    depth[dstRow + x] = static_cast<uint16_t>(normalizedDepth * max_u16);
+                    const float depthMillimetres = std::clamp(z_converted * kMillimetresPerMetre, 0.0f, max_u16);
+                    depth[dstRow + x] = static_cast<uint16_t>(depthMillimetres);
                 }
             }
 
@@ -134,13 +136,14 @@ class CameraDepth : public Sensor {
             return depth;
         }
 
-        std::vector<unsigned char> getDepth8bit() const {
+        std::vector<unsigned char> getDepth16UC1() const {
             std::lock_guard<std::mutex> lock(depthMutex_);
-            std::vector<unsigned char> depth8(depth.size());
+            std::vector<unsigned char> bytes(depth.size() * sizeof(uint16_t));
             for (size_t i = 0; i < depth.size(); ++i) {
-                depth8[i] = static_cast<unsigned char>(depth[i] >> 8);  // Convert uint16 to uint8
+                bytes[i * 2 + 0] = static_cast<unsigned char>(depth[i] & 0xFF);
+                bytes[i * 2 + 1] = static_cast<unsigned char>((depth[i] >> 8) & 0xFF);
             }
-            return depth8;
+            return bytes;
         }
 
         int getWidth() const {

include/sensors/CameraInfo.h

@@ -0,0 +1,85 @@
+#pragma once
+
+#include <mujoco/mujoco.h>
+
+#include <cmath>
+#include <map>
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+#include "sensors/Sensor.h"
+
+namespace spqr {
+
+// Intrinsics parameters of MuJoCo camera.
+// Values are derived once from cam_fovy/cam_resolution (ideal pinhole, no distortion,
+// since MuJoCo's OpenGL renderer doesn't model lens distortion) and never change at runtime.
+class CameraInfo : public Sensor {
+    public:
+        CameraInfo(mjModel* mujModel, const char* cameraName) {
+            int camId = mj_name2id(mujModel, mjOBJ_CAMERA, cameraName);
+            if (camId < 0)
+                throw std::runtime_error(std::string("Camera not found: ") + cameraName);
+
+            width_ = mujModel->cam_resolution[2 * camId + 0];
+            height_ = mujModel->cam_resolution[2 * camId + 1];
+
+            double fovyRad = mujModel->cam_fovy[camId] * M_PI / 180.0;
+
+            fx_ = (height_ / 2.0) / std::tan(fovyRad / 2.0);
+            fy_ = fx_;
+            cx_ = width_ / 2.0;
+            cy_ = height_ / 2.0;
+        }
+
+        void doUpdate() override {}
+
+        msgpack::object doSerialize(msgpack::zone& z) override {
+            std::map<std::string, msgpack::object> data;
+            data["height"] = msgpack::object(height_, z);
+            data["width"] = msgpack::object(width_, z);
+            data["distortion_model"] = msgpack::object(std::string("plumb_bob"), z);
+            data["d"] = msgpack::object(std::vector<double>{0.0, 0.0, 0.0, 0.0, 0.0}, z);
+            data["k"] = msgpack::object(std::vector<double>{fx_, 0.0, cx_, 0.0, fy_, cy_, 0.0, 0.0, 1.0}, z);
+            data["r"] = msgpack::object(std::vector<double>{1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}, z);
+            data["p"] = msgpack::object(std::vector<double>{fx_, 0.0, cx_, 0.0, 0.0, fy_, cy_, 0.0, 0.0, 0.0, 1.0, 0.0}, z);
+            data["binning_x"] = msgpack::object(0, z);
+            data["binning_y"] = msgpack::object(0, z);
+
+            std::map<std::string, msgpack::object> roi;
+            roi["x_offset"] = msgpack::object(0, z);
+            roi["y_offset"] = msgpack::object(0, z);
+            roi["height"] = msgpack::object(0, z);
+            roi["width"] = msgpack::object(0, z);
+            roi["do_rectify"] = msgpack::object(false, z);
+            data["roi"] = msgpack::object(roi, z);
+
+            return msgpack::object(data, z);
+        }
+
+        int getWidth() const {
+            return width_;
+        }
+        int getHeight() const {
+            return height_;
+        }
+        double getFx() const {
+            return fx_;
+        }
+        double getFy() const {
+            return fy_;
+        }
+        double getCx() const {
+            return cx_;
+        }
+        double getCy() const {
+            return cy_;
+        }
+
+    private:
+        int width_, height_;
+        double fx_, fy_, cx_, cy_;
+};
+
+}  // namespace spqr

include/sensors/GroundRelativePosition.h

@@ -0,0 +1,67 @@
+#pragma once
+
+#include <mujoco/mujoco.h>
+
+#include <Eigen/Eigen>
+#include <cmath>
+
+#include "sensors/Pose.h"
+#include "sensors/Sensor.h"
+
+namespace spqr {
+
+// Position of a body or site relative to the ground projection of a reference Pose.
+// Reference frame: (trunk_x, trunk_y, 0) with yaw-only rotation of the trunk.
+class GroundRelativePosition : public Sensor {
+    public:
+        enum class TargetType { Body, Site };
+
+        GroundRelativePosition(mjModel* mujModel, mjData* mujData, const char* name, TargetType type, Pose* referencePose)
+            : mujData_(mujData), type_(type), referencePose_(referencePose) {
+            if (type_ == TargetType::Site)
+                targetId_ = mj_name2id(mujModel, mjOBJ_SITE, name);
+            else
+                targetId_ = mj_name2id(mujModel, mjOBJ_BODY, name);
+        }
+
+        void doUpdate() override {
+            Eigen::Vector3d targetPos;
+            if (type_ == TargetType::Site)
+                targetPos = Eigen::Vector3d(mujData_->site_xpos[3 * targetId_], mujData_->site_xpos[3 * targetId_ + 1],
+                                            mujData_->site_xpos[3 * targetId_ + 2]);
+            else
+                targetPos = Eigen::Vector3d(mujData_->xpos[3 * targetId_], mujData_->xpos[3 * targetId_ + 1], mujData_->xpos[3 * targetId_ + 2]);
+
+            Eigen::Vector4d refQuatVec = referencePose_->getQuatOrientation();
+            Eigen::Quaterniond refQuat(refQuatVec(0), refQuatVec(1), refQuatVec(2), refQuatVec(3));
+            Eigen::Vector3d refPos = referencePose_->getPosition();
+
+            double w = refQuat.w(), x = refQuat.x(), y = refQuat.y(), z = refQuat.z();
+            double yaw = std::atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z));
+            Eigen::Quaterniond groundQuat(Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()));
+            Eigen::Vector3d groundPos(refPos.x(), refPos.y(), 0.0);
+
+            position_ = groundQuat.conjugate() * (targetPos - groundPos);
+        }
+
+        msgpack::object doSerialize(msgpack::zone& z) override {
+            std::vector<double> position_vec = {position_(0), position_(1), position_(2)};
+            std::map<std::string, msgpack::object> data;
+            data["position"] = msgpack::object(position_vec, z);
+            return msgpack::object(data, z);
+        }
+
+        Eigen::Vector3d getPosition() const {
+            return position_;
+        }
+
+    private:
+        mjData* mujData_;
+        int targetId_;
+        TargetType type_;
+        Pose* referencePose_;
+
+        Eigen::Vector3d position_;
+};
+
+}  // namespace spqr