49 lines
1.5 KiB
C#
49 lines
1.5 KiB
C#
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;
|
|
}
|
|
}
|