Feat/lidar intensity

Runtime/Tx/PointsToPointCloud2MsgJob.cs

@@ -6,21 +6,34 @@
 namespace ProBridge.Tx.Sensor
 {
     [BurstCompile]
-    public struct PointsToPointCloud2MsgJob<T> : IJobParallelFor
-        where T : struct, IPointXYZInterface
+    public struct PointsToPointCloud2MsgJob : IJobParallelFor
     {
-        [ReadOnly] public NativeArray<T> points;
+        [ReadOnly] public NativeArray<PointXYZI> points;
+        [ReadOnly] public bool _includeIntensity;
 
         public NativeArray<byte> data;
-
+        
         public void Execute(int index)
         {
-            NativeArray<float> tmp = new NativeArray<float>(3, Allocator.Temp);
+            var tmp = CreateTempArray(index);
+            var slice = new NativeSlice<float>(tmp).SliceConvert<byte>();
+            var bytesPerPoint = _includeIntensity ? 16 : 12;
+            slice.CopyTo(data.GetSubArray(index * bytesPerPoint, bytesPerPoint));
+        }
+
+        private NativeArray<float> CreateTempArray(int index)
+        {
+            var size = _includeIntensity ? 4 : 3;
+            var tmp = new NativeArray<float>(size, Allocator.Temp);
             tmp[0] = points[index].position.z;
             tmp[1] = -points[index].position.x;
             tmp[2] = points[index].position.y;
-            var slice = new NativeSlice<float>(tmp).SliceConvert<byte>();
-            slice.CopyTo(data.GetSubArray(index * 12, 12));
+            if (_includeIntensity)
+            {
+                tmp[3] = points[index].intensity;
+            }
+            return tmp;
         }
+
     }
 }

Runtime/Tx/RaycastLiDARTx.cs

@@ -20,6 +20,7 @@ public class RaycastLiDARTx : ProBridgeTxStamped<PointCloud2>
         public float _minRange = 0.5f;
         public float _maxRange = 100.0f;
         public float _gaussianNoiseSigma = 0.0f;
+        public bool _includeIntensity;
         public float _maxIntensity = 255.0f;
         public float minAzimuthAngle = 0;
         public float maxAzimuthAngle = 360f;
@@ -28,7 +29,7 @@ public class RaycastLiDARTx : ProBridgeTxStamped<PointCloud2>
 
         private RaycastLiDARSensor sensor;
 
-        private PointsToPointCloud2MsgJob<PointXYZI> _pointsToPointCloud2MsgJob;
+        private PointsToPointCloud2MsgJob _pointsToPointCloud2MsgJob;
         private FilterZeroPointsParallelJob _zeroFilterJob;
 
         private JobHandle _jobHandle;
@@ -60,7 +61,7 @@ protected override void AfterEnable()
             sensor.UpdateSensor();
 
 
-            data.fields = new PointField[3];
+            data.fields = new PointField[_includeIntensity ? 4 : 3];
             for (int i = 0; i < 3; i++)
             {
                 data.fields[i] = new PointField();
@@ -70,6 +71,15 @@ protected override void AfterEnable()
                 data.fields[i].count = 1;
             }
 
+            if (_includeIntensity)
+            {
+                data.fields[3] = new PointField();
+                data.fields[3].name = "intensity";
+                data.fields[3].offset = 0;
+                data.fields[3].datatype = PointField.FLOAT32;
+                data.fields[3].count = 1;
+            }
+
             CalculateFieldsOffset();
         }
 
@@ -155,10 +165,11 @@ protected override ProBridge.Msg GetMsg(TimeSpan ts)
             data.data = new byte[data.row_step * data.height];
             tempData = new NativeArray<byte>((int)(data.row_step * data.height), Allocator.TempJob);
             tempPointsInput = tempQueue.ToArray(Allocator.TempJob);
-            _pointsToPointCloud2MsgJob = new PointsToPointCloud2MsgJob<PointXYZI>()
+            _pointsToPointCloud2MsgJob = new PointsToPointCloud2MsgJob
             {
                 points = tempPointsInput,
-                data = tempData
+                data = tempData,
+                _includeIntensity = _includeIntensity
             };
 
             _jobHandle = _pointsToPointCloud2MsgJob.Schedule(tempQueue.Count, 12);