Initial commit

srcs/RobotNet10/RobotApp/RobotNet10.RobotApp/SLAM/Cartographer/Helpers/TrajectoryHelper.cs

@@ -0,0 +1,48 @@
+using CartographerSharp.Mapping;
+using RobotNet10.RobotApp.SLAM.Cartographer;
+
+namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
+
+/// <summary>
+/// Helper class for trajectory-related operations
+/// </summary>
+public static class TrajectoryHelper
+{
+    
+
+    /// <summary>
+    /// Build sensor IDs from configuration
+    /// </summary>
+    /// <param name="config">Cartographer configuration</param>
+    /// <returns>HashSet of sensor IDs</returns>
+    public static HashSet<ITrajectoryBuilder.SensorId> BuildSensorIds(CartographerConfiguration config)
+    {
+        var sensorIds = new HashSet<ITrajectoryBuilder.SensorId>();
+
+        // Add Lidar sensor IDs
+        foreach (var lidarConfig in config.Sensors.Lidars.Where(l => l.Enabled))
+        {
+            sensorIds.Add(new ITrajectoryBuilder.SensorId(
+                ITrajectoryBuilder.SensorId.SensorType.Range,
+                lidarConfig.DeviceId));
+        }
+
+        // Add IMU sensor ID
+        if (config.Sensors.Imu.Enabled && !string.IsNullOrEmpty(config.Sensors.Imu.DeviceId))
+        {
+            sensorIds.Add(new ITrajectoryBuilder.SensorId(
+                ITrajectoryBuilder.SensorId.SensorType.Imu,
+                config.Sensors.Imu.DeviceId));
+        }
+
+        // Add Odometry sensor ID
+        if (config.UseOdometry)
+        {
+            sensorIds.Add(new ITrajectoryBuilder.SensorId(
+                ITrajectoryBuilder.SensorId.SensorType.Odometry,
+                "odometry"));
+        }
+
+        return sensorIds;
+    }
+}