Feat/16bit depth

Runtime/Tx/Depth16bitCameraTx.cs

@@ -0,0 +1,124 @@
+using System;
+using ProBridge.Tx;
+using UnityEngine;
+using UnityEngine.UI;
+using UnitySensors.Data.PointCloud;
+using UnitySensors.Sensor.Camera;
+
+[AddComponentMenu("ProBridge/Tx/sensor_msgs/Depth16bit Camera")]
+public class Depth16bitCameraTx : ProBridgeTxStamped<sensor_msgs.msg.Image>
+{
+    public Camera renderCamera;
+    public float _minRange = 0.05f;
+    public float _maxRange = 100.0f;
+    public float _gaussianNoiseSigma = 0.0f;
+    public int fov = 30;
+    public int textureWidth = 1024;
+    public int textureHeight = 1024;
+    public RawImage _rawImage;
+    [Range(1, 100)] public int CompressionQuality = 90;
+    public Shader depthShader;
+    public bool getPointCloud = false;
+
+    public PointCloud<PointXYZ> pointCloud
+    {
+        get => _cameraSensor.pointCloud;
+    }
+
+    public Depth16bitCameraSensor _cameraSensor { get; private set; }
+    private bool sensorReady = false;
+
+    private int H;
+    private int W;
+
+    protected override void AfterEnable()
+    {
+
+        _cameraSensor = renderCamera.gameObject.AddComponent<Depth16bitCameraSensor>();
+        _cameraSensor.mat = new Material(depthShader);
+        _cameraSensor._camera = renderCamera;
+        _cameraSensor.onSensorUpdated += OnSensorUpdated;
+        _cameraSensor._minRange = _minRange;
+        _cameraSensor._maxRange = _maxRange;
+        _cameraSensor._gaussianNoiseSigma = _gaussianNoiseSigma;
+        _cameraSensor._fov = fov;
+        _cameraSensor._frequency_inv = sendRate;
+        _cameraSensor._resolution.x = textureWidth;
+        _cameraSensor._resolution.y = textureHeight;
+        _cameraSensor.getPointCloud = getPointCloud;
+        _cameraSensor.Init();
+
+        W = _cameraSensor.Width;
+        H = _cameraSensor.Height;
+
+        data.height = (uint)H;
+        data.width = (uint)W;
+        data.encoding = "16UC1";
+        data.is_bigendian = 0;
+        data.step = (uint)(W * 2);
+    }
+
+    protected override void AfterDisable()
+    {
+        if (_cameraSensor != null)
+            _cameraSensor.DisposeSensor();
+    }
+
+    private void OnSensorUpdated()
+    {
+        sensorReady = true;
+    }
+
+    protected override ProBridge.ProBridge.Msg GetMsg(TimeSpan ts)
+    {
+        if (!sensorReady) return null;
+        sensorReady = false;
+
+        if (W <= 0 || H <= 0) return null;
+
+        // Packed R16 UNorm from the sensor (length = W*H*2)
+        ReadOnlySpan<byte> src = _cameraSensor.LatestR16Bytes();
+        if (src.Length != W * H * 2) return null;
+
+        float farM = _cameraSensor.m_camera.farClipPlane;
+
+        // Reuse these as fields if you want to avoid GC each frame
+        ushort[] dstU16 = new ushort[W * H];
+
+        // Convert: R16 UNorm -> distanceNorm [0..1] -> depth meters -> mm (uint16)
+        // Also flip vertically (Unity bottom-left -> ROS top-left)
+        int rowBytes = W * 2;
+        for (int y = 0; y < H; y++)
+        {
+            int srcY = H - 1 - y;                 // flip
+            int srcRow = srcY * rowBytes;
+            int dstRow = y * W;
+
+            for (int x = 0; x < W; x++)
+            {
+                // read uint16 little-endian
+                int b0 = src[srcRow + (x << 1) + 0];
+                int b1 = src[srcRow + (x << 1) + 1];
+                int u16 = (b1 << 8) | b0;
+
+                float distanceNorm = u16 * (1.0f / 65535.0f);      // 0..1
+                float depthM = (1.0f - distanceNorm) * farM;       // meters (undo shader)
+                int mm = (int)Mathf.Round(depthM * 1000.0f);       // to mm
+
+                if (mm < 0) mm = 0;
+                else if (mm > 65535) mm = 65535;
+
+                dstU16[dstRow + x] = (ushort)mm;
+            }
+        }
+
+        // Pack to bytes
+        byte[] bytes = new byte[W * H * 2];
+        Buffer.BlockCopy(dstU16, 0, bytes, 0, bytes.Length);
+
+        data.data = bytes;
+
+        return base.GetMsg(ts);
+    }
+
+}

Runtime/Tx/DepthCameraTx.cs

@@ -28,7 +28,7 @@ public PointCloud<PointXYZ> pointCloud
         get => _cameraSensor.pointCloud;
     }
 
-    private DepthCameraSensor _cameraSensor;
+    public DepthCameraSensor _cameraSensor { get; private set; }
     private bool sensorReady = false;
 
     // Reuse the compressor to avoid per-frame allocations.
@@ -75,14 +75,12 @@ protected override ProBridge.ProBridge.Msg GetMsg(TimeSpan ts)
         if (!sensorReady)
             throw new Exception("Sensor is not ready");
 
-        var tex = _cameraSensor.texture0;
-        if (tex == null)
-            throw new Exception("Depth sensor texture is not a Texture2D.");
+        sensorReady = false;
 
-        if (_rawImage != null)
-            _rawImage.texture = tex;
+        var tex = _cameraSensor.texture0;
+        if (tex == null) return null;
 
-        sensorReady = false;
+        if (_rawImage != null) _rawImage.texture = tex;
 
         NativeArray<byte> raw = tex.GetRawTextureData<byte>();
         byte[] rgbaBytes = raw.ToArray();
@@ -98,7 +96,7 @@ protected override ProBridge.ProBridge.Msg GetMsg(TimeSpan ts)
 
         data.format = "jpeg";
         data.data = jpg;
-
         return base.GetMsg(ts);
     }
+
 }

Runtime/Tx/DepthPointCloudTx.cs

@@ -0,0 +1,150 @@
+using System;
+using ProBridge.Tx;
+using sensor_msgs.msg;
+using UnityEngine;
+using UnitySensors.Sensor.Camera;
+using UnitySensors.Data.PointCloud;
+using ProBridge.Utils;
+
+[AddComponentMenu("ProBridge/Tx/sensor_msgs/PointCloud2 (Depth)")]
+public class DepthPointCloudTx : ProBridgeTxStamped<PointCloud2>
+{
+
+    public bool skipNaN = false;          // true => drop NaN points (changes width/height semantics)
+    public bool isSource16bit;
+
+    private bool sensorReady;
+
+    private Depth16bitCameraTx depth16Tx;
+    private DepthCameraTx depthTx;
+    private Depth16bitCameraSensor sensor_16;
+    private DepthCameraSensor sensor;
+    protected override void AfterEnable()
+    {
+        if (isSource16bit)
+            depth16Tx = GetComponent<Depth16bitCameraTx>();
+        else
+            depthTx = GetComponent<DepthCameraTx>();
+    }
+
+    protected override void AfterDisable()
+    {
+        if (sensor != null)
+            sensor.onSensorUpdated -= OnSensorUpdated;
+    }
+
+    private void OnSensorUpdated() => sensorReady = true;
+
+    protected override ProBridge.ProBridge.Msg GetMsg(TimeSpan ts)
+    {
+        if (isSource16bit)
+        {
+            if (sensor_16 == null && depth16Tx)
+            {
+                sensor_16 = depth16Tx._cameraSensor;
+                sensor_16.onSensorUpdated += OnSensorUpdated;
+                return null; 
+            }
+        }
+        else
+        {
+            if (sensor == null && depthTx)
+            {
+                sensor = depthTx._cameraSensor;
+                sensor.onSensorUpdated += OnSensorUpdated;
+                return null; 
+            }
+        }
+
+        if (isSource16bit ? sensor_16 == null : sensor == null)
+            return null;
+
+
+        if (!sensorReady) return null;
+        sensorReady = false;
+
+        PointCloud<PointXYZ> pc;
+        int W, H;
+
+        if (isSource16bit)
+        {
+            if (!sensor_16.getPointCloud) return null;
+            pc = sensor_16.pointCloud;
+            W = sensor_16.Width;
+            H = sensor_16.Height;
+        }
+        else
+        {
+            if (!sensor.getPointCloud) return null; 
+            pc = sensor.pointCloud;
+            W = sensor._resolution.x; 
+            H = sensor._resolution.y; 
+        }
+
+        if (pc.points.Length == 0) 
+        {
+            Debug.Log("empty pcd!");
+            return null;
+        }
+
+        int N = pc.points.Length;
+        // Build PointCloud2 fields: x,y,z float32
+        var fields = new PointField[3];
+        fields[0] = new PointField { name = "x", offset = 0, datatype = PointField.FLOAT32, count = 1 };
+        fields[1] = new PointField { name = "y", offset = 4, datatype = PointField.FLOAT32, count = 1 };
+        fields[2] = new PointField { name = "z", offset = 8, datatype = PointField.FLOAT32, count = 1 };
+
+        int pointStep = 12;                    // 3 * float32
+        byte[] bytes;
+
+        if (!skipNaN)
+        {
+            bytes = new byte[N * pointStep];
+            var tmp = new float[3];
+
+            for (int i = 0; i < N; i++)
+            {
+                var p= (Vector3)pc.points[i].position;
+                var pROS = p.ToRos();
+                tmp[0] = (float) pROS.x; tmp[1] = (float)pROS.y; tmp[2] = (float)pROS.z;
+                Buffer.BlockCopy(tmp, 0, bytes, i * pointStep, pointStep);
+            }
+
+            data.height = (uint)H;
+            data.width = (uint)W;
+            data.is_dense = false;
+        }
+        else
+        {
+            var list = new System.Collections.Generic.List<byte>(N * pointStep);
+            var tmp = new float[3];
+
+            for (int i = 0; i < N; i++)
+            {
+                var p = (Vector3)pc.points[i].position;
+                var pROS = p.ToRos();
+
+                if (double.IsNaN(pROS.x) || double.IsNaN(pROS.y) || double.IsNaN(pROS.z)) continue;
+
+                tmp[0] = (float)pROS.x; tmp[1] = (float)pROS.y; tmp[2] = (float)pROS.z;
+                var row = new byte[pointStep];
+                Buffer.BlockCopy(tmp, 0, row, 0, pointStep);
+                list.AddRange(row);
+            }
+            bytes = list.ToArray();
+
+            data.height = 1;
+            data.width = (uint)(bytes.Length / pointStep);
+            data.is_dense = true;
+        }
+
+
+        data.fields = fields;
+        data.is_bigendian = false;
+        data.point_step = (uint)pointStep;
+        data.row_step = (uint)(pointStep * (int)data.width);
+        data.data = bytes;
+
+        return base.GetMsg(ts);               
+    }
+}