Initial commit

srcs/RobotNet10/RobotApp/Communication/CartographerSharp/Sensor/TimedPointCloudData.cs

@@ -0,0 +1,147 @@
+/*
+ * Copyright 2017 The Cartographer Authors
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+using  RobotNet10.Shared.Numbers;
+using CartographerSharp.Models.Sensor;
+using CartographerSharp.Models.Transform;
+
+namespace CartographerSharp.Sensor;
+
+/// <summary>
+/// Timed point cloud data structure.
+/// </summary>
+public struct TimedPointCloudData
+{
+    public long Time { get; set; }
+    public Vector3 Origin { get; set; }
+    public TimedPointCloud Ranges { get; set; }
+    
+    /// <summary>
+    /// 'intensities' has to be same size as 'ranges', or empty.
+    /// </summary>
+    public List<double> Intensities { get; set; }
+
+    public TimedPointCloudData(long time, Vector3 origin, TimedPointCloud ranges, List<double>? intensities = null)
+    {
+        Time = time;
+        Origin = origin;
+        Ranges = ranges;
+        Intensities = intensities ?? [];
+        
+        if (Intensities.Count > 0 && Intensities.Count != Ranges.Count)
+        {
+            throw new ArgumentException("Intensities must have the same size as ranges, or be empty.");
+        }
+    }
+}
+
+/// <summary>
+/// Timed point cloud origin data structure.
+/// </summary>
+public struct TimedPointCloudOriginData(long time, List<Vector3> origins, List<TimedPointCloudOriginData.RangeMeasurement> ranges)
+{
+    /// <summary>
+    /// Range measurement with point time, intensity, and origin index.
+    /// </summary>
+    public struct RangeMeasurement(TimedRangefinderPoint pointTime, double intensity, int originIndex)
+    {
+        public TimedRangefinderPoint PointTime { get; set; } = pointTime;
+        public double Intensity { get; set; } = intensity;
+        public int OriginIndex { get; set; } = originIndex;
+    }
+
+    public long Time { get; set; } = time;
+    public List<Vector3> Origins { get; set; } = origins;
+    public List<RangeMeasurement> Ranges { get; set; } = ranges;
+}
+
+/// <summary>
+/// Operations on TimedPointCloudData.
+/// </summary>
+public static class TimedPointCloudDataOperations
+{
+    /// <summary>
+    /// Converts 'timed_point_cloud_data' to a proto::TimedPointCloudData.
+    /// Note: Time is already in Universal Time Scale ticks (long), so no conversion needed.
+    /// </summary>
+    public static Models.Sensor.TimedPointCloudData ToProto(TimedPointCloudData timedPointCloudData)
+    {
+        var pointData = new List<Models.Sensor.TimedRangefinderPoint>(timedPointCloudData.Ranges.Count);
+        foreach (var range in timedPointCloudData.Ranges)
+        {
+            pointData.Add(new Models.Sensor.TimedRangefinderPoint(
+                new Vector3f(range.Position.X, range.Position.Y, range.Position.Z),
+                range.Time
+            ));
+        }
+
+        return new Models.Sensor.TimedPointCloudData(
+            timedPointCloudData.Time, // Already in Universal Time Scale ticks
+            new Vector3f(timedPointCloudData.Origin.X, timedPointCloudData.Origin.Y, timedPointCloudData.Origin.Z),
+            pointDataLegacy: null,
+            pointData: pointData,
+            intensities: timedPointCloudData.Intensities
+        );
+    }
+
+    /// <summary>
+    /// Converts 'proto' to TimedPointCloudData.
+    /// Note: Time is already in Universal Time Scale ticks (long), so no conversion needed.
+    /// </summary>
+    public static TimedPointCloudData FromProto(Models.Sensor.TimedPointCloudData proto)
+    {
+        var timedPointCloud = new TimedPointCloud();
+        
+        // Use point_data if available, otherwise fall back to point_data_legacy
+        if (proto.PointData != null && proto.PointData.Count > 0)
+        {
+            timedPointCloud.Capacity = proto.PointData.Count;
+            foreach (var protoPoint in proto.PointData)
+            {
+                timedPointCloud.Add(new TimedRangefinderPoint(
+                    new Vector3(protoPoint.Position.X, protoPoint.Position.Y, protoPoint.Position.Z),
+                    protoPoint.Time
+                ));
+            }
+        }
+        else if (proto.PointDataLegacy != null && proto.PointDataLegacy.Count > 0)
+        {
+            // Legacy format: Vector4f where T component is time
+            timedPointCloud.Capacity = proto.PointDataLegacy.Count;
+            foreach (var point4 in proto.PointDataLegacy)
+            {
+                timedPointCloud.Add(new TimedRangefinderPoint(
+                    new Vector3(point4.X, point4.Y, point4.Z),
+                    point4.T
+                ));
+            }
+        }
+
+        var intensities = new List<double>(proto.Intensities ?? []);
+        
+        if (intensities.Count > 0 && intensities.Count != timedPointCloud.Count)
+        {
+            throw new ArgumentException("Intensities size must match ranges size, or be empty.");
+        }
+
+        return new TimedPointCloudData(
+            proto.Timestamp, // Already in Universal Time Scale ticks
+            new Vector3(proto.Origin.X, proto.Origin.Y, proto.Origin.Z),
+            timedPointCloud,
+            intensities
+        );
+    }
+}