2004 lines
74 KiB
C#
2004 lines
74 KiB
C#
using Microsoft.AspNetCore.SignalR;
|
|
using Microsoft.Extensions.Configuration;
|
|
using Microsoft.Extensions.Hosting;
|
|
using Microsoft.Extensions.Logging;
|
|
using RobotNet10.RobotApp.Devices;
|
|
using RobotNet10.RobotApp.Hubs;
|
|
using RobotNet10.RobotApp.Motion;
|
|
using RobotNet10.RobotApp.TF3;
|
|
using RobotNet10.Shared;
|
|
using RobotNet10.Shared.Geometry;
|
|
using RobotNet10.Shared.Sensor;
|
|
using System;
|
|
using System.Collections.Concurrent;
|
|
using System.Threading;
|
|
using System.Threading.Tasks;
|
|
// using RobotNet10.Shared.Numbers;
|
|
|
|
namespace RobotNet10.RobotApp.Xloc;
|
|
|
|
/// <summary>
|
|
/// Integration service to dispatch sensor data from RobotNet to XLOC SLAM engine
|
|
/// Sends EKF filtered odometry, IMU raw data, and LIDAR scans
|
|
/// </summary>
|
|
public class XlocIntegrationService : IHostedService, IDisposable
|
|
{
|
|
private readonly XlocIntegrationConfiguration _config;
|
|
private readonly IDeviceProvider _deviceProvider;
|
|
private readonly OdometryService? _odometryService;
|
|
private readonly EKFService? _ekfService;
|
|
private readonly ILogger<XlocIntegrationService> _logger;
|
|
private readonly IHubContext<XlocPoseHub>? _poseHubContext;
|
|
private readonly TF3BufferManager? _tf3BufferManager;
|
|
private readonly object _lock = new();
|
|
private readonly object _statsLock = new(); // Lock for thread-safe statistics access
|
|
private readonly object _timestampLock = new(); // Lock for monotonic timestamp adjustment
|
|
private readonly object _sourceSampleLock = new(); // Lock for source sample deduplication
|
|
private readonly ConcurrentDictionary<string, DateTime> _latencyLogTimes = new();
|
|
private readonly ConcurrentDictionary<string, DateTime> _lastDispatchedTimestamps = new();
|
|
private readonly ConcurrentDictionary<string, DateTime> _stampLogTimes = new();
|
|
private readonly ConcurrentDictionary<string, double> _lastSourceTimestamps = new();
|
|
private readonly ConcurrentDictionary<string, uint> _lastSourceSequences = new();
|
|
private readonly SemaphoreSlim _dispatchSemaphore = new SemaphoreSlim(1, 1); // Only 1 dispatch at a time!
|
|
|
|
private XlocClient? _xlocClient;
|
|
private XlocAsyncDispatcher? _asyncDispatcher;
|
|
|
|
// Separate timers for each sensor type (multi-threading)
|
|
private Timer? _imuTimer;
|
|
private Timer? _odomTimer;
|
|
private Timer? _lidarTimer;
|
|
private Timer? _loggingTimer; // Timer for console logging of pose and diagnostics
|
|
|
|
private bool _disposed = false;
|
|
private bool _isInitialized = false;
|
|
|
|
/// <summary>
|
|
/// UTC time when <see cref="StopLocalization"/> last succeeded (API or direct). Used by
|
|
/// <see cref="XlocAutoLocalizationHostedService"/> to avoid immediately re-starting after user stop.
|
|
/// </summary>
|
|
public DateTime? LastLocalizationStopUtc { get; private set; }
|
|
|
|
// Event handler references (stored for unregistration)
|
|
private EventHandler<LidarScanDataEventArgs>? _lidar1ScanHandler;
|
|
private EventHandler<LidarScanDataEventArgs>? _lidar2ScanHandler;
|
|
private EventHandler<LidarScanDataEventArgs>? _lidar3ScanHandler;
|
|
private EventHandler<OdometryUpdatedEventArgs>? _odomUpdatedHandler;
|
|
private EventHandler<AngularVelocityChangedEventArgs>? _imuAngularVelocityHandler;
|
|
|
|
// Lidar event pump queue system
|
|
private readonly ConcurrentQueue<(int slot, LaserScan scan)> _pendingLidarQueue1 = new();
|
|
private readonly ConcurrentQueue<(int slot, LaserScan scan)> _pendingLidarQueue2 = new();
|
|
private readonly ConcurrentQueue<(int slot, LaserScan scan)> _pendingLidarQueue3 = new();
|
|
private const int MaxPendingLidarScans = 20;
|
|
private Task? _lidarEventPumpTask;
|
|
private CancellationTokenSource? _lidarEventPumpCts;
|
|
|
|
// Cached device references
|
|
private IInertialMeasurementUnit? _imuDevice;
|
|
private ILidar? _lidarDevice1;
|
|
private ILidar? _lidarDevice2; // Second lidar (right side)
|
|
private ILidar? _lidarDevice3; // Third lidar (left side)
|
|
|
|
// Cached laser scans for web visualization (thread-safe)
|
|
private LaserScan? _cachedLaserScan1 = null;
|
|
private LaserScan? _cachedLaserScan2 = null;
|
|
private LaserScan? _cachedLaserScan3 = null;
|
|
private readonly object _laserCacheLock = new();
|
|
|
|
// Statistics (accessed from multiple threads)
|
|
private long _odometryCount = 0;
|
|
private long _imuCount = 0;
|
|
private long _lidarCount = 0;
|
|
private long _lidar1DispatchCount = 0;
|
|
private long _lidar2DispatchCount = 0;
|
|
private long _lidar3DispatchCount = 0;
|
|
private uint _imuSeq = 0;
|
|
private DateTime _lastStatsLog = DateTime.UtcNow;
|
|
|
|
// Frequency snapshot tracking
|
|
private long _lastOdomCountSnapshot;
|
|
private long _lastImuCountSnapshot;
|
|
private long _lastLidar1CountSnapshot;
|
|
private long _lastLidar2CountSnapshot;
|
|
private long _lastLidar3CountSnapshot;
|
|
private DateTime _lastFreqSnapshotTime = DateTime.UtcNow;
|
|
|
|
public XlocIntegrationService(
|
|
IConfiguration configuration,
|
|
IDeviceProvider deviceProvider,
|
|
ILogger<XlocIntegrationService> logger,
|
|
TF3BufferManager? tf3BufferManager = null,
|
|
IHubContext<XlocPoseHub>? poseHubContext = null,
|
|
OdometryService? odometryService = null,
|
|
EKFService? ekfService = null)
|
|
{
|
|
_deviceProvider = deviceProvider ?? throw new ArgumentNullException(nameof(deviceProvider));
|
|
_logger = logger ?? throw new ArgumentNullException(nameof(logger));
|
|
_poseHubContext = poseHubContext;
|
|
_odometryService = odometryService;
|
|
_ekfService = ekfService;
|
|
_tf3BufferManager = tf3BufferManager;
|
|
|
|
// Load configuration
|
|
var configSection = configuration.GetSection("Xloc:Integration");
|
|
if (!configSection.Exists())
|
|
{
|
|
_logger.LogWarning("Configuration section 'Xloc:Integration' not found. Using defaults.");
|
|
_config = new XlocIntegrationConfiguration();
|
|
}
|
|
else
|
|
{
|
|
_config = new XlocIntegrationConfiguration();
|
|
configSection.Bind(_config);
|
|
}
|
|
|
|
ValidateConfiguration();
|
|
}
|
|
|
|
private void ValidateConfiguration()
|
|
{
|
|
if (_config.Enabled)
|
|
{
|
|
if (_config.UpdateRateHz <= 0 || _config.UpdateRateHz > 100)
|
|
{
|
|
_logger.LogWarning("UpdateRateHz {Rate} is out of range [1-100]. Using default 20 Hz.", _config.UpdateRateHz);
|
|
_config.UpdateRateHz = 20;
|
|
}
|
|
|
|
if (_config.EnableEKFOdometry && _ekfService == null)
|
|
{
|
|
_logger.LogWarning("EKF Odometry is enabled but EKFService is not available. Will be disabled.");
|
|
_config.EnableEKFOdometry = false;
|
|
}
|
|
|
|
if (_config.EnableRawOdometry && _odometryService == null)
|
|
{
|
|
_logger.LogWarning("Raw Odometry is enabled but OdometryService is not available. Will be disabled.");
|
|
_config.EnableRawOdometry = false;
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get the XlocClient instance for use in other services
|
|
/// </summary>
|
|
public XlocClient? XlocClient => _xlocClient;
|
|
|
|
public async Task StartAsync(CancellationToken cancellationToken)
|
|
{
|
|
// var connected = await _deviceProvider.WaitForDevicesConnectedAsync(TimeSpan.FromMinutes(5), cancellationToken);
|
|
if (!_config.Enabled)
|
|
{
|
|
_logger.LogInformation("XlocIntegrationService is disabled in configuration");
|
|
return;
|
|
}
|
|
|
|
_logger.LogInformation("Starting XlocIntegrationService...");
|
|
|
|
try
|
|
{
|
|
// Wait for devices to be connected
|
|
_logger.LogInformation("Waiting for devices to be connected...");
|
|
var connected = await _deviceProvider.WaitForDevicesConnectedAsync(TimeSpan.FromMinutes(5), cancellationToken);
|
|
|
|
if (!connected)
|
|
{
|
|
_logger.LogWarning("Timeout waiting for devices. XlocIntegrationService will retry initialization.");
|
|
_ = Task.Run(async () => await RetryInitializationAsync(cancellationToken));
|
|
return;
|
|
}
|
|
|
|
|
|
await InitializeAsync(cancellationToken);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error starting XlocIntegrationService");
|
|
}
|
|
}
|
|
|
|
private async Task RetryInitializationAsync(CancellationToken cancellationToken)
|
|
{
|
|
const int maxRetries = 60; // 5 minutes with 5 second intervals
|
|
int retryCount = 0;
|
|
|
|
while (retryCount < maxRetries && !cancellationToken.IsCancellationRequested && !_isInitialized)
|
|
{
|
|
await Task.Delay(TimeSpan.FromSeconds(5), cancellationToken);
|
|
|
|
var connected = await _deviceProvider.WaitForDevicesConnectedAsync(TimeSpan.FromSeconds(1), cancellationToken);
|
|
if (connected)
|
|
{
|
|
_logger.LogInformation("Devices are now connected. Initializing XlocIntegrationService...");
|
|
await InitializeAsync(cancellationToken);
|
|
return;
|
|
}
|
|
|
|
retryCount++;
|
|
if (retryCount % 12 == 0) // Log every minute
|
|
{
|
|
_logger.LogInformation("Still waiting for devices... (attempt {Attempt}/{MaxAttempts})",
|
|
retryCount, maxRetries);
|
|
}
|
|
}
|
|
|
|
if (retryCount >= maxRetries)
|
|
{
|
|
_logger.LogWarning("Timeout waiting for devices. XlocIntegrationService will not be initialized.");
|
|
}
|
|
}
|
|
|
|
private async Task InitializeAsync(CancellationToken cancellationToken)
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (_isInitialized)
|
|
return;
|
|
|
|
try
|
|
{
|
|
// Get device references
|
|
if (_config.EnableIMU && !string.IsNullOrEmpty(_config.ImuDeviceId))
|
|
{
|
|
var device = _deviceProvider.GetDevice(_config.ImuDeviceId);
|
|
if (device is IInertialMeasurementUnit imu && device.IsConnected)
|
|
{
|
|
_imuDevice = imu;
|
|
_logger.LogInformation("IMU device '{DeviceId}' found and connected", _config.ImuDeviceId);
|
|
}
|
|
else
|
|
{
|
|
_logger.LogWarning("IMU device '{DeviceId}' not found or not connected. IMU data will not be sent.",
|
|
_config.ImuDeviceId);
|
|
}
|
|
}
|
|
|
|
if (_config.EnableLidar1 && !string.IsNullOrEmpty(_config.Lidar1DeviceId))
|
|
{
|
|
var device = _deviceProvider.GetDevice(_config.Lidar1DeviceId);
|
|
if (device is ILidar lidar && device.IsConnected)
|
|
{
|
|
_lidarDevice1 = lidar;
|
|
_logger.LogInformation("LIDAR device '{DeviceId}' found and connected", _config.Lidar1DeviceId);
|
|
}
|
|
else
|
|
{
|
|
_logger.LogWarning("LIDAR device '{DeviceId}' not found or not connected. LIDAR data will not be sent.",
|
|
_config.Lidar1DeviceId);
|
|
}
|
|
}
|
|
|
|
// Second lidar (right side)
|
|
if (_config.EnableLidar2 && !string.IsNullOrEmpty(_config.Lidar2DeviceId))
|
|
{
|
|
var device2 = _deviceProvider.GetDevice(_config.Lidar2DeviceId);
|
|
if (device2 is ILidar lidar2 && device2.IsConnected)
|
|
{
|
|
_lidarDevice2 = lidar2;
|
|
_logger.LogInformation("LIDAR 2 device '{DeviceId}' found and connected", _config.Lidar2DeviceId);
|
|
}
|
|
else
|
|
{
|
|
_logger.LogWarning("LIDAR 2 device '{DeviceId}' not found or not connected.",
|
|
_config.Lidar2DeviceId);
|
|
}
|
|
}
|
|
|
|
// Third lidar (left side)
|
|
if (_config.EnableLidar3 && !string.IsNullOrEmpty(_config.Lidar3DeviceId))
|
|
{
|
|
var device3 = _deviceProvider.GetDevice(_config.Lidar3DeviceId);
|
|
if (device3 is ILidar lidar3 && device3.IsConnected)
|
|
{
|
|
_lidarDevice3 = lidar3;
|
|
_logger.LogInformation("LIDAR 3 device '{DeviceId}' found and connected", _config.Lidar3DeviceId);
|
|
}
|
|
else
|
|
{
|
|
_logger.LogWarning("LIDAR 3 device '{DeviceId}' not found or not connected.",
|
|
_config.Lidar3DeviceId);
|
|
}
|
|
}
|
|
|
|
// Create and initialize xloc client with TF3 buffer from TF3BufferManager
|
|
if (_tf3BufferManager == null || !_tf3BufferManager.IsInitialized)
|
|
{
|
|
throw new InvalidOperationException(
|
|
"TF3BufferManager is not available or not initialized. " +
|
|
"Ensure TF3BufferManager is registered in dependency injection and initialized before XlocIntegrationService.");
|
|
}
|
|
|
|
IntPtr tfBuffer = _tf3BufferManager.TfBuffer;
|
|
_logger.LogInformation("[XLOC] Using TF3 buffer from TF3BufferManager: {Buffer}", tfBuffer);
|
|
|
|
_xlocClient = new XlocClient(tfBuffer, _logger);
|
|
_xlocClient.Initialize();
|
|
|
|
_logger.LogInformation("XlocClient initialized successfully");
|
|
|
|
// DON'T start mapping/localization immediately - wait for sensor data first!
|
|
// This prevents crash when xloc doesn't have any data yet
|
|
|
|
if (_config.Mode == XlocMode.Localization && !string.IsNullOrEmpty(_config.MapFilePath))
|
|
{
|
|
if (_xlocClient.ActivateMap(_config.MapFilePath))
|
|
{
|
|
_logger.LogInformation("Map activated: {MapFile}", _config.MapFilePath);
|
|
}
|
|
else
|
|
{
|
|
_logger.LogWarning("Failed to activate map. Will try to start without map.");
|
|
}
|
|
}
|
|
// Create async dispatcher — single-thread model (like ROS ros::spin)
|
|
_asyncDispatcher = new XlocAsyncDispatcher(_xlocClient, queueCapacity: 300, logger: _logger);
|
|
_logger.LogInformation("[XLOC] Async dispatcher created (single-thread, capacity=300)");
|
|
|
|
// Verify timestamp conversion at startup
|
|
VerifyTimestampConversion();
|
|
|
|
// Register event-driven odometry dispatch
|
|
if (_config.EnableRawOdometry && _odometryService != null)
|
|
{
|
|
_odomUpdatedHandler = OnOdometryUpdated;
|
|
_odometryService.OdometryUpdated += _odomUpdatedHandler;
|
|
_logger.LogInformation("[XLOC] Odometry event handler registered (event-driven)");
|
|
}
|
|
|
|
// Register event-driven IMU dispatch
|
|
if (_config.EnableIMU && _imuDevice != null)
|
|
{
|
|
_imuAngularVelocityHandler = OnImuAngularVelocityChanged;
|
|
_imuDevice.AngularVelocityChanged += _imuAngularVelocityHandler;
|
|
_logger.LogInformation("[XLOC] IMU event handler registered (event-driven)");
|
|
}
|
|
|
|
// Register event-driven lidar dispatch with queue pump
|
|
RegisterLidarEventDispatch(cancellationToken);
|
|
|
|
// Start console logging timer (every 2 seconds)
|
|
_loggingTimer = new Timer(LogPoseAndDiagnostics, null,
|
|
TimeSpan.FromSeconds(2), TimeSpan.FromSeconds(2));
|
|
_logger.LogInformation("XLOC console logging started (every 2 seconds)");
|
|
|
|
_isInitialized = true;
|
|
_logger.LogInformation("XlocIntegrationService initialized successfully with event-driven sensor dispatch");
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Failed to initialize XlocIntegrationService");
|
|
throw;
|
|
}
|
|
}
|
|
|
|
await Task.CompletedTask;
|
|
}// IMU message sequence counter
|
|
|
|
public async Task StopAsync(CancellationToken cancellationToken)
|
|
{
|
|
_logger.LogInformation("Stopping XlocIntegrationService...");
|
|
|
|
try
|
|
{
|
|
// IMPORTANT: Set _isInitialized to false FIRST to signal event handlers to stop
|
|
lock (_lock)
|
|
{
|
|
_isInitialized = false;
|
|
}
|
|
|
|
// Unregister all event handlers immediately
|
|
UnregisterLidarEventDispatch();
|
|
UnregisterOdomImuEventDispatch();
|
|
|
|
// Dispose timers
|
|
_imuTimer?.Dispose();
|
|
_imuTimer = null;
|
|
_odomTimer?.Dispose();
|
|
_odomTimer = null;
|
|
_lidarTimer?.Dispose();
|
|
_lidarTimer = null;
|
|
_loggingTimer?.Dispose();
|
|
_loggingTimer = null;
|
|
|
|
// Wait for in-flight dispatches to complete
|
|
_logger.LogInformation("Waiting for in-flight dispatches to complete...");
|
|
await Task.Delay(500, cancellationToken);
|
|
|
|
// Dispose async dispatcher BEFORE stopping xloc client (prevent race conditions)
|
|
var dispatcher = _asyncDispatcher;
|
|
_asyncDispatcher = null;
|
|
dispatcher?.Dispose();
|
|
|
|
// Stop xloc
|
|
if (_xlocClient != null)
|
|
{
|
|
if (_config.Mode == XlocMode.Localization)
|
|
{
|
|
_xlocClient.StopLocalization();
|
|
}
|
|
else if (_config.Mode == XlocMode.Mapping && !string.IsNullOrEmpty(_config.SaveMapFilePath))
|
|
{
|
|
_xlocClient.StopMapping(_config.SaveMapFilePath);
|
|
_logger.LogInformation("Map saved to: {MapFile}", _config.SaveMapFilePath);
|
|
}
|
|
|
|
_xlocClient.Dispose();
|
|
_xlocClient = null;
|
|
_logger.LogInformation("XlocClient disposed successfully");
|
|
}
|
|
|
|
LogFinalStatistics();
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error stopping XlocIntegrationService");
|
|
}
|
|
|
|
await Task.CompletedTask;
|
|
}
|
|
|
|
#region Event-Driven Dispatch — Odometry, IMU, Lidar
|
|
|
|
/// <summary>
|
|
/// Event handler: called when OdometryService fires OdometryUpdated.
|
|
/// Deduplicates, adjusts timestamp, and dispatches via async dispatcher.
|
|
/// </summary>
|
|
private void OnOdometryUpdated(object? sender, OdometryUpdatedEventArgs e)
|
|
{
|
|
if (!_isInitialized || _xlocClient == null || _asyncDispatcher == null)
|
|
return;
|
|
|
|
try
|
|
{
|
|
var rawOdom = e.Odometry;
|
|
if (rawOdom.Header.FrameId == null)
|
|
return;
|
|
|
|
// Dedup by sequence + timestamp
|
|
var ts = (rawOdom.Header.Stamp - DateTime.UnixEpoch).TotalSeconds;
|
|
if (!TryAcceptSourceSample(_config.RawOdometrySensorId, rawOdom.Header.Seq, ts))
|
|
return;
|
|
|
|
// Monotonic timestamp
|
|
var stamp = GetAdjustedSensorStamp(_config.RawOdometrySensorId, DateTime.UtcNow);
|
|
|
|
var updatedOdom = new Odometry
|
|
{
|
|
Header = new Header
|
|
{
|
|
Seq = rawOdom.Header.Seq,
|
|
Stamp = stamp,
|
|
FrameId = rawOdom.Header.FrameId
|
|
},
|
|
ChildFrameId = rawOdom.ChildFrameId,
|
|
Pose = rawOdom.Pose,
|
|
Twist = rawOdom.Twist
|
|
};
|
|
|
|
var sensorId = _config.RawOdometrySensorId;
|
|
var client = _xlocClient;
|
|
var dispatcher = _asyncDispatcher;
|
|
if (client == null || dispatcher == null) return;
|
|
|
|
_ = dispatcher.EnqueueOdometryAsync(() =>
|
|
{
|
|
client.DispatchOdometry(updatedOdom, sensorId);
|
|
Interlocked.Increment(ref _odometryCount);
|
|
}, sensorId);
|
|
}
|
|
catch (TaskCanceledException) { }
|
|
catch (OperationCanceledException) { }
|
|
catch (InvalidOperationException ex) when (ex.Message.Contains("disposed"))
|
|
{
|
|
_logger.LogDebug("Odometry dispatch skipped: dispatcher disposed");
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error in OnOdometryUpdated event handler");
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Event handler: called when IMU fires AngularVelocityChanged.
|
|
/// Creates full IMU message, deduplicates, and dispatches via async dispatcher.
|
|
/// </summary>
|
|
private void OnImuAngularVelocityChanged(object? sender, AngularVelocityChangedEventArgs e)
|
|
{
|
|
if (!_isInitialized || _xlocClient == null || _imuDevice == null || _asyncDispatcher == null)
|
|
return;
|
|
|
|
try
|
|
{
|
|
var imu = CreateImuMessage(_imuDevice);
|
|
if (imu == null)
|
|
return;
|
|
|
|
var originalImu = imu.Value;
|
|
|
|
// Dedup by sequence + timestamp
|
|
var ts = (originalImu.Header.Stamp - DateTime.UnixEpoch).TotalSeconds;
|
|
if (!TryAcceptSourceSample(_config.ImuSensorId, originalImu.Header.Seq, ts))
|
|
return;
|
|
|
|
// Monotonic timestamp
|
|
var stamp = GetAdjustedSensorStamp(_config.ImuSensorId, DateTime.UtcNow);
|
|
|
|
var updatedImu = new Imu
|
|
{
|
|
Header = new Header
|
|
{
|
|
Seq = originalImu.Header.Seq,
|
|
Stamp = stamp,
|
|
FrameId = originalImu.Header.FrameId
|
|
},
|
|
Orientation = originalImu.Orientation,
|
|
OrientationCovariance = originalImu.OrientationCovariance,
|
|
AngularVelocity = originalImu.AngularVelocity,
|
|
AngularVelocityCovariance = originalImu.AngularVelocityCovariance,
|
|
LinearAcceleration = originalImu.LinearAcceleration,
|
|
LinearAccelerationCovariance = originalImu.LinearAccelerationCovariance
|
|
};
|
|
|
|
var sensorId = _config.ImuSensorId;
|
|
var client = _xlocClient;
|
|
var dispatcher = _asyncDispatcher;
|
|
if (client == null || dispatcher == null) return;
|
|
|
|
_ = dispatcher.EnqueueImuAsync(() =>
|
|
{
|
|
client.DispatchImu(updatedImu, sensorId);
|
|
Interlocked.Increment(ref _imuCount);
|
|
}, sensorId);
|
|
}
|
|
catch (TaskCanceledException) { }
|
|
catch (OperationCanceledException) { }
|
|
catch (InvalidOperationException ex) when (ex.Message.Contains("disposed"))
|
|
{
|
|
_logger.LogDebug("IMU dispatch skipped: dispatcher disposed");
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error in OnImuAngularVelocityChanged event handler");
|
|
}
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Lidar Event Pump System
|
|
|
|
/// <summary>
|
|
/// Register lidar event handlers and start the pump loop.
|
|
/// </summary>
|
|
private void RegisterLidarEventDispatch(CancellationToken ct)
|
|
{
|
|
StartLidarEventPump(ct);
|
|
|
|
if (_lidarDevice1 != null && _config.EnableLidar1)
|
|
{
|
|
_lidar1ScanHandler = (s, e) => QueuePendingLidarScan(1, e.MeasurementData);
|
|
_lidarDevice1.ScanDataReceived += _lidar1ScanHandler;
|
|
_logger.LogInformation("[XLOC] Lidar 1 event handler registered");
|
|
}
|
|
|
|
if (_lidarDevice2 != null && _config.EnableLidar2)
|
|
{
|
|
_lidar2ScanHandler = (s, e) => QueuePendingLidarScan(2, e.MeasurementData);
|
|
_lidarDevice2.ScanDataReceived += _lidar2ScanHandler;
|
|
_logger.LogInformation("[XLOC] Lidar 2 event handler registered");
|
|
}
|
|
|
|
if (_lidarDevice3 != null && _config.EnableLidar3)
|
|
{
|
|
_lidar3ScanHandler = (s, e) => QueuePendingLidarScan(3, e.MeasurementData);
|
|
_lidarDevice3.ScanDataReceived += _lidar3ScanHandler;
|
|
_logger.LogInformation("[XLOC] Lidar 3 event handler registered");
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Unregister lidar event handlers and stop the pump.
|
|
/// </summary>
|
|
private void UnregisterLidarEventDispatch()
|
|
{
|
|
if (_lidarDevice1 != null && _lidar1ScanHandler != null)
|
|
{
|
|
_lidarDevice1.ScanDataReceived -= _lidar1ScanHandler;
|
|
_lidar1ScanHandler = null;
|
|
}
|
|
if (_lidarDevice2 != null && _lidar2ScanHandler != null)
|
|
{
|
|
_lidarDevice2.ScanDataReceived -= _lidar2ScanHandler;
|
|
_lidar2ScanHandler = null;
|
|
}
|
|
if (_lidarDevice3 != null && _lidar3ScanHandler != null)
|
|
{
|
|
_lidarDevice3.ScanDataReceived -= _lidar3ScanHandler;
|
|
_lidar3ScanHandler = null;
|
|
}
|
|
|
|
StopLidarEventPump();
|
|
}
|
|
|
|
/// <summary>
|
|
/// Unregister odometry and IMU event handlers.
|
|
/// </summary>
|
|
private void UnregisterOdomImuEventDispatch()
|
|
{
|
|
if (_odometryService != null && _odomUpdatedHandler != null)
|
|
{
|
|
_odometryService.OdometryUpdated -= _odomUpdatedHandler;
|
|
_odomUpdatedHandler = null;
|
|
}
|
|
if (_imuDevice != null && _imuAngularVelocityHandler != null)
|
|
{
|
|
_imuDevice.AngularVelocityChanged -= _imuAngularVelocityHandler;
|
|
_imuAngularVelocityHandler = null;
|
|
}
|
|
}
|
|
|
|
private void QueuePendingLidarScan(int slot, LaserScan scan)
|
|
{
|
|
var queue = slot switch
|
|
{
|
|
1 => _pendingLidarQueue1,
|
|
2 => _pendingLidarQueue2,
|
|
_ => _pendingLidarQueue3
|
|
};
|
|
|
|
var queueCountBefore = queue.Count;
|
|
queue.Enqueue((slot, scan));
|
|
|
|
// Trim queue to prevent unbounded growth
|
|
int droppedCount = 0;
|
|
while (queue.Count > MaxPendingLidarScans)
|
|
{
|
|
queue.TryDequeue(out _);
|
|
droppedCount++;
|
|
}
|
|
|
|
if (droppedCount > 0)
|
|
{
|
|
// _logger.LogWarning("[XLOC-DIAG] scan_{Slot} QueuePending DROPPED {Dropped} scans (queue was {Before}, max={Max})",
|
|
// slot, droppedCount, queueCountBefore, MaxPendingLidarScans);
|
|
}
|
|
}
|
|
|
|
private void StartLidarEventPump(CancellationToken externalCt)
|
|
{
|
|
_lidarEventPumpCts = CancellationTokenSource.CreateLinkedTokenSource(externalCt);
|
|
_lidarEventPumpTask = Task.Run(() => LidarEventPumpLoopAsync(_lidarEventPumpCts.Token));
|
|
_logger.LogInformation("[XLOC] Lidar event pump started");
|
|
}
|
|
|
|
private void StopLidarEventPump()
|
|
{
|
|
try
|
|
{
|
|
_lidarEventPumpCts?.Cancel();
|
|
_lidarEventPumpTask?.Wait(TimeSpan.FromSeconds(2));
|
|
}
|
|
catch (AggregateException) { }
|
|
catch (OperationCanceledException) { }
|
|
finally
|
|
{
|
|
_lidarEventPumpCts?.Dispose();
|
|
_lidarEventPumpCts = null;
|
|
_lidarEventPumpTask = null;
|
|
|
|
// Clear queues
|
|
while (_pendingLidarQueue1.TryDequeue(out _)) { }
|
|
while (_pendingLidarQueue2.TryDequeue(out _)) { }
|
|
while (_pendingLidarQueue3.TryDequeue(out _)) { }
|
|
}
|
|
}
|
|
|
|
private async Task LidarEventPumpLoopAsync(CancellationToken ct)
|
|
{
|
|
while (!ct.IsCancellationRequested)
|
|
{
|
|
try
|
|
{
|
|
var loopStart = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
|
|
|
|
bool dispatchedAny = false;
|
|
dispatchedAny |= await DrainLidarQueueAsync(_pendingLidarQueue1, "queue1", ct);
|
|
dispatchedAny |= await DrainLidarQueueAsync(_pendingLidarQueue2, "queue2", ct);
|
|
dispatchedAny |= await DrainLidarQueueAsync(_pendingLidarQueue3, "queue3", ct);
|
|
|
|
var loopElapsed = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds() - loopStart;
|
|
if (dispatchedAny && loopElapsed > 20)
|
|
{
|
|
// _logger.LogWarning("[XLOC-DIAG] PumpLoop took {ElapsedMs}ms (q1={Q1},q2={Q2},q3={Q3})",
|
|
// loopElapsed, _pendingLidarQueue1.Count, _pendingLidarQueue2.Count, _pendingLidarQueue3.Count);
|
|
}
|
|
|
|
if (!dispatchedAny)
|
|
{
|
|
await Task.Delay(_config.LidarDispatchIntervalMs, ct);
|
|
}
|
|
}
|
|
catch (OperationCanceledException) { break; }
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "[XLOC] Error in lidar event pump loop");
|
|
}
|
|
}
|
|
}
|
|
|
|
private async Task<bool> DrainLidarQueueAsync(ConcurrentQueue<(int slot, LaserScan scan)> queue, string queueName, CancellationToken ct)
|
|
{
|
|
bool dispatched = false;
|
|
int count = 0;
|
|
var drainStart = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
|
|
|
|
while (queue.TryDequeue(out var item) && !ct.IsCancellationRequested)
|
|
{
|
|
await DispatchLidarFromEventAsync(item.slot, item.scan);
|
|
dispatched = true;
|
|
count++;
|
|
}
|
|
|
|
if (dispatched)
|
|
{
|
|
var drainElapsed = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds() - drainStart;
|
|
if (drainElapsed > 10)
|
|
{
|
|
// _logger.LogWarning("[XLOC-DIAG] Drain {Queue} took {ElapsedMs}ms for {Count} scans",
|
|
// queueName, drainElapsed, count);
|
|
}
|
|
}
|
|
return dispatched;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Dispatch a lidar scan from event queue. Deduplicates, adjusts timestamp,
|
|
/// caches for web visualization, and dispatches via async dispatcher.
|
|
/// </summary>
|
|
private async Task DispatchLidarFromEventAsync(int slot, LaserScan originalScan)
|
|
{
|
|
if (!_isInitialized || _xlocClient == null || _asyncDispatcher == null)
|
|
return;
|
|
|
|
try
|
|
{
|
|
var (sensorId, frameId) = slot switch
|
|
{
|
|
1 => (_config.Lidar1SensorId, "scan_1"),
|
|
2 => (_config.Lidar2SensorId, "scan_2"),
|
|
_ => (_config.Lidar3SensorId, "scan_3")
|
|
};
|
|
|
|
// Dedup
|
|
var ts = (originalScan.Header.Stamp - DateTime.UnixEpoch).TotalSeconds;
|
|
if (!TryAcceptSourceSample(sensorId, originalScan.Header.Seq, ts))
|
|
return;
|
|
|
|
// Monotonic timestamp
|
|
var stamp = GetAdjustedSensorStamp(sensorId, DateTime.UtcNow);
|
|
|
|
// Use sensor-provided angle parameters (QUYVN device config)
|
|
// For Lidar 3, use hardcoded values as in original code
|
|
var updatedScan = new LaserScan
|
|
{
|
|
Header = new Header
|
|
{
|
|
Seq = originalScan.Header.Seq,
|
|
Stamp = stamp,
|
|
FrameId = frameId
|
|
},
|
|
AngleMin = slot == 3 ? (float)-2.356194496154785 : originalScan.AngleMin,
|
|
AngleMax = slot == 3 ? (float)2.3557233810424805 : originalScan.AngleMax,
|
|
AngleIncrement = slot == 3 ? (float)0.00581718236207962 : originalScan.AngleIncrement,
|
|
TimeIncrement = slot == 3 ? (float)6.172839493956417e-05 : originalScan.TimeIncrement,
|
|
ScanTime = slot == 3 ? (float)0.06666667014360428 : originalScan.ScanTime,
|
|
RangeMin = slot == 3 ? 0f : originalScan.RangeMin,
|
|
RangeMax = slot == 3 ? 100f : originalScan.RangeMax,
|
|
Ranges = originalScan.Ranges,
|
|
Intensities = originalScan.Intensities ?? []
|
|
};
|
|
|
|
// Always cache latest scan for web visualization even when xloc is stopped
|
|
lock (_laserCacheLock)
|
|
{
|
|
switch (slot)
|
|
{
|
|
case 1: _cachedLaserScan1 = updatedScan; break;
|
|
case 2: _cachedLaserScan2 = updatedScan; break;
|
|
case 3: _cachedLaserScan3 = updatedScan; break;
|
|
}
|
|
}
|
|
|
|
// Dispatch to xloc via async dispatcher
|
|
var client = _xlocClient;
|
|
var dispatcher = _asyncDispatcher;
|
|
if (client == null || dispatcher == null ) return;
|
|
|
|
var lidarSlot = slot; // capture for lambda
|
|
var enqueueTs = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
|
|
await dispatcher.EnqueueLaserScanAsync(() =>
|
|
{
|
|
var asyncWaitMs = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds() - enqueueTs;
|
|
var nativeStart = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds();
|
|
|
|
client.DispatchLaserScan(updatedScan, sensorId);
|
|
|
|
var nativeElapsedMs = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds() - nativeStart;
|
|
Interlocked.Increment(ref _lidarCount);
|
|
switch (lidarSlot)
|
|
{
|
|
case 1: Interlocked.Increment(ref _lidar1DispatchCount); break;
|
|
case 2: Interlocked.Increment(ref _lidar2DispatchCount); break;
|
|
case 3: Interlocked.Increment(ref _lidar3DispatchCount); break;
|
|
}
|
|
|
|
if (asyncWaitMs > 10 || nativeElapsedMs > 10)
|
|
{
|
|
// _logger.LogWarning(
|
|
// "[XLOC-DIAG] scan_{Slot} async_wait={AsyncWaitMs}ms native_call={NativeMs}ms (sensor={SensorId})",
|
|
// lidarSlot, asyncWaitMs, nativeElapsedMs, sensorId);
|
|
}
|
|
}, sensorId);
|
|
}
|
|
catch (TaskCanceledException) { }
|
|
catch (OperationCanceledException) { }
|
|
catch (InvalidOperationException ex) when (ex.Message.Contains("disposed"))
|
|
{
|
|
_logger.LogDebug("Lidar {Slot} dispatch skipped: dispatcher disposed", slot);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error dispatching lidar {Slot} from event", slot);
|
|
}
|
|
}
|
|
|
|
#endregion
|
|
|
|
private Imu? CreateImuMessage(IInertialMeasurementUnit imu)
|
|
{
|
|
try
|
|
{
|
|
// Increment IMU sequence number
|
|
var seq = Interlocked.Increment(ref _imuSeq);
|
|
|
|
var header = new Header
|
|
{
|
|
Seq = seq,
|
|
// Use device-provided timestamp directly (do not use MonotonicTimestampManager here)
|
|
Stamp = imu.LastUpdateTime,
|
|
FrameId = _config.ImuFrameId
|
|
};
|
|
|
|
// Get orientation (raw from IMU)
|
|
var orientation = new Quaternion();
|
|
var quatData = imu.CachedQuaternion;
|
|
if (quatData.HasValue)
|
|
{
|
|
orientation = quatData.Value.Quaternion;
|
|
}
|
|
else
|
|
{
|
|
// Fallback to identity if no quaternion available
|
|
orientation = new Quaternion { X = 0, Y = 0, Z = 0, W = 1 };
|
|
}
|
|
|
|
// Get angular velocity (raw from IMU)
|
|
var angularVelocity = imu.CachedAngularVelocity.Vector;
|
|
|
|
// Get linear acceleration (raw from IMU)
|
|
var linearAcceleration = imu.CachedAcceleration.Accel.Linear;
|
|
|
|
// ===== SEND RAW IMU DATA TO XLOC (NO TRANSFORMATION) =====
|
|
// Testing to see if transformation is needed or causing issues
|
|
|
|
// Covariance matrices (row-major, 3x3)
|
|
// Based on Wheeltec N100 IMU specifications
|
|
var orientationCov = new double[9]
|
|
{
|
|
0.01, 0.0, 0.0, // Row 1: var_x, cov_xy, cov_xz
|
|
0.0, 0.01, 0.0, // Row 2: cov_yx, var_y, cov_yz
|
|
0.0, 0.0, 0.01 // Row 3: cov_zx, cov_zy, var_z
|
|
};
|
|
|
|
var angularVelocityCov = new double[9]
|
|
{
|
|
0.01, 0.0, 0.0,
|
|
0.0, 0.01, 0.0,
|
|
0.0, 0.0, 0.01
|
|
};
|
|
|
|
var linearAccelerationCov = new double[9]
|
|
{
|
|
0.05, 0.0, 0.0,
|
|
0.0, 0.05, 0.0,
|
|
0.0, 0.0, 0.05
|
|
};
|
|
|
|
return new Imu
|
|
{
|
|
Header = header,
|
|
Orientation = orientation,
|
|
OrientationCovariance = orientationCov,
|
|
AngularVelocity = angularVelocity,
|
|
AngularVelocityCovariance = angularVelocityCov,
|
|
LinearAcceleration = linearAcceleration,
|
|
LinearAccelerationCovariance = linearAccelerationCov
|
|
};
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error creating IMU message");
|
|
return null;
|
|
}
|
|
}
|
|
|
|
#region Helper Methods — Timestamp, Dedup, Delay
|
|
|
|
/// <summary>
|
|
/// Precise delay that compensates for drift instead of fixed Task.Delay.
|
|
/// Resyncs if more than one full period late.
|
|
/// Returns the updated nextCycleTime.
|
|
/// </summary>
|
|
private static async Task<DateTime> DelayUntilNextCycleAsync(int intervalMs, DateTime nextCycleTime, CancellationToken ct)
|
|
{
|
|
nextCycleTime = nextCycleTime.AddMilliseconds(intervalMs);
|
|
var now = DateTime.UtcNow;
|
|
var delay = nextCycleTime - now;
|
|
|
|
if (delay.TotalMilliseconds < -intervalMs)
|
|
{
|
|
// More than one full period late — resync to avoid burst catch-up
|
|
return now.AddMilliseconds(intervalMs);
|
|
}
|
|
|
|
if (delay > TimeSpan.Zero)
|
|
await Task.Delay(delay, ct);
|
|
|
|
return nextCycleTime;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Ensures strictly monotonic timestamps per sensor.
|
|
/// If the new stamp equals or precedes the previous one, adds 1 tick.
|
|
/// </summary>
|
|
private DateTime GetAdjustedSensorStamp(string sensorId, DateTime rawStamp)
|
|
{
|
|
lock (_timestampLock)
|
|
{
|
|
// Fallback to UtcNow if stamp is default (uninitialized)
|
|
if (rawStamp == default)
|
|
rawStamp = DateTime.UtcNow;
|
|
|
|
if (_lastDispatchedTimestamps.TryGetValue(sensorId, out var last) && rawStamp <= last)
|
|
{
|
|
rawStamp = last.AddTicks(1);
|
|
}
|
|
_lastDispatchedTimestamps[sensorId] = rawStamp;
|
|
return rawStamp;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Deduplicates source samples using dual strategy: sequence-based (preferred) + timestamp-based fallback.
|
|
/// Returns false if the sample was already processed.
|
|
/// When sequence is strictly increasing, use sequence alone.
|
|
/// When sequence is unchanged (e.g. device always sends seq=0), fall through to timestamp-based dedup.
|
|
/// </summary>
|
|
private bool TryAcceptSourceSample(string sourceId, uint sequence, double timestamp)
|
|
{
|
|
lock (_sourceSampleLock)
|
|
{
|
|
bool hasLastSeq = _lastSourceSequences.TryGetValue(sourceId, out var lastSeq);
|
|
|
|
if (hasLastSeq)
|
|
{
|
|
if (sequence > lastSeq)
|
|
{
|
|
// Sequence strictly increased — accept without timestamp check
|
|
_lastSourceSequences[sourceId] = sequence;
|
|
_lastSourceTimestamps[sourceId] = timestamp;
|
|
return true;
|
|
}
|
|
|
|
if (sequence < lastSeq)
|
|
{
|
|
// Sequence went backwards (duplicate or reorder) — reject
|
|
return false;
|
|
}
|
|
|
|
// sequence == lastSeq: device may not increment seq (e.g. always 0)
|
|
// Fall through to timestamp-based dedup
|
|
}
|
|
|
|
// Timestamp-based dedup (primary when seq is unchanged, fallback otherwise)
|
|
if (_lastSourceTimestamps.TryGetValue(sourceId, out var lastTs) && timestamp <= lastTs)
|
|
return false;
|
|
|
|
_lastSourceSequences[sourceId] = sequence;
|
|
_lastSourceTimestamps[sourceId] = timestamp;
|
|
return true;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Validates DateTime to Unix time conversion roundtrip at startup.
|
|
/// </summary>
|
|
private void VerifyTimestampConversion()
|
|
{
|
|
var now = DateTime.UtcNow;
|
|
var unixSec = (now - DateTime.UnixEpoch).TotalSeconds;
|
|
var roundtrip = DateTime.UnixEpoch.AddSeconds(unixSec);
|
|
var driftMs = Math.Abs((roundtrip - now).TotalMilliseconds);
|
|
if (driftMs > 100)
|
|
_logger.LogWarning("[XLOC] Timestamp conversion drift: {Drift:F3}ms — may cause issues", driftMs);
|
|
else
|
|
_logger.LogInformation("[XLOC] Timestamp conversion OK, drift: {Drift:F6}ms", driftMs);
|
|
}
|
|
|
|
#endregion
|
|
|
|
private void LogStatistics()
|
|
{
|
|
_logger.LogInformation(
|
|
"Xloc sensor data dispatched - Odometry: {OdomCount}, IMU: {ImuCount}, LIDAR: {LidarCount}",
|
|
_odometryCount, _imuCount, _lidarCount);
|
|
}
|
|
|
|
private void LogFinalStatistics()
|
|
{
|
|
_logger.LogInformation(
|
|
"XlocIntegrationService final statistics - Total dispatched: Odometry={OdomCount}, IMU={ImuCount}, LIDAR={LidarCount}",
|
|
_odometryCount, _imuCount, _lidarCount);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get current pose from xloc (thread-safe)
|
|
/// </summary>
|
|
public (double x, double y, double z, double qx, double qy, double qz, double qw)? GetCurrentPose()
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (!_isInitialized || _xlocClient == null)
|
|
return null;
|
|
|
|
return _xlocClient.GetCurrentPose();
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get diagnostics from xloc (thread-safe)
|
|
/// </summary>
|
|
public XlocDiagnosticsData? GetDiagnostics()
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (!_isInitialized || _xlocClient == null)
|
|
return null;
|
|
|
|
return _xlocClient.GetDiagnostics();
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get current pose as 2D (x, y, yaw) - easier to use for navigation
|
|
/// </summary>
|
|
public OccupancyGridData? GetStaticGridMap(bool reloadFromFile = false)
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (_xlocClient == null || !_isInitialized)
|
|
return null;
|
|
|
|
// Avoid calling native map getter before a map is active to prevent noisy native warnings.
|
|
var diagnostics = _xlocClient.GetDiagnostics();
|
|
if (diagnostics == null || string.IsNullOrWhiteSpace(diagnostics.CurrentActiveMap))
|
|
return null;
|
|
|
|
return _xlocClient.GetStaticGridMap(reloadFromFile);
|
|
}
|
|
}
|
|
|
|
public OccupancyGridData? GetOnlineGridMap()
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (_xlocClient == null || !_isInitialized)
|
|
return null;
|
|
|
|
return _xlocClient.GetOnlineGridMap();
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get static grid map pointer (from loaded map file)
|
|
/// Returns IntPtr to xloc_occupancy_grid_t (must be freed with xloc_free_occupancy_grid)
|
|
/// </summary>
|
|
public IntPtr GetStaticGridMapPtr(bool reloadFromFile = false)
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (_xlocClient == null || !_isInitialized)
|
|
return IntPtr.Zero;
|
|
|
|
return _xlocClient.GetStaticGridMapPtr(reloadFromFile);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get online grid map pointer (from SLAM)
|
|
/// Returns IntPtr to xloc_occupancy_grid_t (must be freed with xloc_free_occupancy_grid)
|
|
/// </summary>
|
|
public IntPtr GetOnlineGridMapPtr()
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (_xlocClient == null || !_isInitialized)
|
|
return IntPtr.Zero;
|
|
|
|
return _xlocClient.GetOnlineGridMapPtr();
|
|
}
|
|
}
|
|
|
|
public (double x, double y, double yaw)? GetCurrentPose2D()
|
|
{
|
|
var pose = GetCurrentPose();
|
|
if (!pose.HasValue)
|
|
return null;
|
|
|
|
var (x, y, z, qx, qy, qz, qw) = pose.Value;
|
|
|
|
// Convert quaternion to yaw angle
|
|
double yaw = Math.Atan2(2.0 * (qw * qz + qx * qy),
|
|
1.0 - 2.0 * (qy * qy + qz * qz));
|
|
|
|
return (x, y, yaw);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get sampled laser scan data for web visualization (1 degree intervals)
|
|
/// Returns array of (angle, range) pairs
|
|
/// Thread-safe
|
|
/// </summary>
|
|
public List<(float angle, float range)>? GetSampledLaserScan()
|
|
{
|
|
return GetSampledLaserScanInternal(_cachedLaserScan1, 360);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get sampled laser scan data from Lidar 2 (right side)
|
|
/// </summary>
|
|
public List<(float angle, float range)>? GetSampledLaserScan2()
|
|
{
|
|
return GetSampledLaserScanInternal(_cachedLaserScan2, 360);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get sampled laser scan data from Lidar 3 (left side)
|
|
/// </summary>
|
|
public List<(float angle, float range)>? GetSampledLaserScan3()
|
|
{
|
|
return GetSampledLaserScanInternal(_cachedLaserScan3, 360);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get full laser scan data (no downsampling) from Lidar 1 for web visualization.
|
|
/// </summary>
|
|
public List<(float angle, float range)>? GetFullLaserScan()
|
|
{
|
|
return GetFullLaserScanInternal(_cachedLaserScan1);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get full laser scan data (no downsampling) from Lidar 2.
|
|
/// </summary>
|
|
public List<(float angle, float range)>? GetFullLaserScan2()
|
|
{
|
|
return GetFullLaserScanInternal(_cachedLaserScan2);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Get full laser scan data (no downsampling) from Lidar 3.
|
|
/// </summary>
|
|
public List<(float angle, float range)>? GetFullLaserScan3()
|
|
{
|
|
return GetFullLaserScanInternal(_cachedLaserScan3);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Internal method to sample laser scan data
|
|
/// </summary>
|
|
private List<(float angle, float range)>? GetSampledLaserScanInternal(LaserScan? cachedScan, int maxPoints)
|
|
{
|
|
lock (_laserCacheLock)
|
|
{
|
|
if (cachedScan == null)
|
|
return null;
|
|
|
|
var scan = cachedScan.Value;
|
|
if (scan.Ranges == null || scan.Ranges.Length == 0)
|
|
return null;
|
|
|
|
var result = new List<(float angle, float range)>();
|
|
|
|
// Target: 1 degree = π/180 radians ≈ 0.01745 rad
|
|
const float targetAngleIncrement = (float)(Math.PI / 180.0);
|
|
|
|
// Calculate how many indices to skip for ~1 degree spacing
|
|
int skipCount = Math.Max(1, (int)Math.Round(targetAngleIncrement / scan.AngleIncrement));
|
|
|
|
// Sample points at ~1 degree intervals
|
|
for (int i = 0; i < scan.Ranges.Length; i += skipCount)
|
|
{
|
|
float range = (float)scan.Ranges[i];
|
|
|
|
// Only include valid ranges (within min/max limits)
|
|
if (range >= scan.RangeMin && range <= scan.RangeMax && !float.IsNaN(range) && !float.IsInfinity(range))
|
|
{
|
|
float angle = (float)(scan.AngleMin + i * scan.AngleIncrement);
|
|
result.Add((angle, range));
|
|
|
|
// Limit to maxPoints per lidar
|
|
if (result.Count >= maxPoints)
|
|
break;
|
|
}
|
|
}
|
|
|
|
return result;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Internal method to return full-resolution laser scan points.
|
|
/// </summary>
|
|
private List<(float angle, float range)>? GetFullLaserScanInternal(LaserScan? cachedScan)
|
|
{
|
|
lock (_laserCacheLock)
|
|
{
|
|
if (cachedScan == null)
|
|
return null;
|
|
|
|
var scan = cachedScan.Value;
|
|
if (scan.Ranges == null || scan.Ranges.Length == 0)
|
|
return null;
|
|
|
|
var result = new List<(float angle, float range)>(scan.Ranges.Length);
|
|
|
|
for (int i = 0; i < scan.Ranges.Length; i++)
|
|
{
|
|
float range = (float)scan.Ranges[i];
|
|
|
|
// Keep all finite points from iLidar data (no downsampling).
|
|
if (!float.IsNaN(range) && !float.IsInfinity(range))
|
|
{
|
|
float angle = (float)(scan.AngleMin + i * scan.AngleIncrement);
|
|
result.Add((angle, range));
|
|
}
|
|
}
|
|
|
|
return result;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Log XLOC pose and diagnostics to console (called by timer)
|
|
/// </summary>
|
|
private void LogPoseAndDiagnostics(object? state)
|
|
{
|
|
XlocClient? client = null;
|
|
|
|
lock (_lock)
|
|
{
|
|
if (!_isInitialized || _xlocClient == null)
|
|
return;
|
|
|
|
client = _xlocClient;
|
|
}
|
|
|
|
if (client == null)
|
|
return;
|
|
|
|
try
|
|
{
|
|
// Get and log current pose
|
|
var pose = client.GetCurrentPose();
|
|
if (pose.HasValue)
|
|
{
|
|
double yaw = Math.Atan2(
|
|
2 * (pose.Value.qw * pose.Value.qz + pose.Value.qx * pose.Value.qy),
|
|
1 - 2 * (pose.Value.qy * pose.Value.qy + pose.Value.qz * pose.Value.qz)
|
|
) * 180.0 / Math.PI;
|
|
|
|
// IMPORTANT: Publish map→odom transform based on current localized pose
|
|
// This updates the TF3 buffer with the latest localization result
|
|
// Navigation uses this transform to know where the robot is in the map frame
|
|
// client.PublishMapToOdomTransform();
|
|
}
|
|
else
|
|
{
|
|
_logger.LogWarning("XLOC POSE: No data available from xloc_get_current_pose()");
|
|
}
|
|
|
|
// Get and log diagnostics
|
|
|
|
// Log sensor dispatch frequency every ~2 seconds
|
|
var now = DateTime.UtcNow;
|
|
var elapsed = (now - _lastFreqSnapshotTime).TotalSeconds;
|
|
if (elapsed >= 2.0)
|
|
{
|
|
var odomHz = (Interlocked.Read(ref _odometryCount) - _lastOdomCountSnapshot) / elapsed;
|
|
var imuHz = (Interlocked.Read(ref _imuCount) - _lastImuCountSnapshot) / elapsed;
|
|
var l1Hz = (Interlocked.Read(ref _lidar1DispatchCount) - _lastLidar1CountSnapshot) / elapsed;
|
|
var l2Hz = (Interlocked.Read(ref _lidar2DispatchCount) - _lastLidar2CountSnapshot) / elapsed;
|
|
var l3Hz = (Interlocked.Read(ref _lidar3DispatchCount) - _lastLidar3CountSnapshot) / elapsed;
|
|
|
|
_logger.LogInformation(
|
|
"[XLOC-FREQ] Odom: {O:F1}Hz, IMU: {I:F1}Hz, L1: {L1:F1}Hz, L2: {L2:F1}Hz, L3: {L3:F1}Hz",
|
|
odomHz, imuHz, l1Hz, l2Hz, l3Hz);
|
|
|
|
_lastOdomCountSnapshot = Interlocked.Read(ref _odometryCount);
|
|
_lastImuCountSnapshot = Interlocked.Read(ref _imuCount);
|
|
_lastLidar1CountSnapshot = Interlocked.Read(ref _lidar1DispatchCount);
|
|
_lastLidar2CountSnapshot = Interlocked.Read(ref _lidar2DispatchCount);
|
|
_lastLidar3CountSnapshot = Interlocked.Read(ref _lidar3DispatchCount);
|
|
_lastFreqSnapshotTime = now;
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error logging XLOC data to console");
|
|
}
|
|
}
|
|
|
|
#region Manual Control Methods
|
|
|
|
/// <summary>
|
|
/// Manually activate a map for localization
|
|
/// </summary>
|
|
public bool ActivateMap(string mapFilePath)
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (!_isInitialized || _xlocClient == null)
|
|
{
|
|
_logger.LogWarning("Cannot activate map: XlocIntegrationService not initialized");
|
|
return false;
|
|
}
|
|
|
|
return _xlocClient.ActivateMap(mapFilePath);
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Set initial pose: Tell XLOC where the ROBOT is located in the FIXED map frame
|
|
///
|
|
/// IMPORTANT:
|
|
/// - This sets the ROBOT's position (x, y, z) and orientation (roll, pitch, yaw) IN the map frame
|
|
/// - The MAP FRAME itself NEVER moves (always fixed at origin 0,0,0)
|
|
/// - Only the robot's estimated position in the map frame changes
|
|
///
|
|
/// Parameters: (x, y, z, roll, pitch, yaw) all in map frame coordinates
|
|
/// - Position (x, y, z): meters in map frame
|
|
/// - Orientation (roll, pitch, yaw): radians (converted to quaternion internally)
|
|
/// </summary>
|
|
public bool SetInitialPose(double x, double y, double z, double roll, double pitch, double yaw)
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (!_isInitialized || _xlocClient == null)
|
|
{
|
|
_logger.LogWarning("Cannot set initial pose: XlocIntegrationService not initialized");
|
|
return false;
|
|
}
|
|
|
|
// Guard: skip SetInitialPose if XLOC is in MAPPING (0) or PROCESSING (2) state
|
|
var diagnostics = _xlocClient.GetDiagnostics();
|
|
if (diagnostics != null && diagnostics.XlocState is 0 or 2)
|
|
{
|
|
_logger.LogWarning("Skip SetInitialPose because XLOC state is {State} (0=MAPPING, 2=PROCESSING)", diagnostics.XlocState);
|
|
return false;
|
|
}
|
|
|
|
// Get current pose BEFORE setting initial pose
|
|
var poseBefore = _xlocClient.GetCurrentPose();
|
|
_logger.LogWarning("=== XLOC SetInitialPose - BEFORE ===");
|
|
if (poseBefore.HasValue)
|
|
{
|
|
var (px, py, pz, pqx, pqy, pqz, pqw) = poseBefore.Value;
|
|
// Convert quaternion to yaw for logging
|
|
double yawBefore = Math.Atan2(2.0 * (pqw * pqz + pqx * pqy), 1.0 - 2.0 * (pqy * pqy + pqz * pqz));
|
|
}
|
|
else
|
|
{
|
|
_logger.LogWarning("Current pose: NULL (no pose available)");
|
|
}
|
|
|
|
// Convert Euler angles (roll, pitch, yaw) to quaternion using full formula
|
|
var cy = Math.Cos(yaw * 0.5);
|
|
var sy = Math.Sin(yaw * 0.5);
|
|
var cp = Math.Cos(pitch * 0.5);
|
|
var sp = Math.Sin(pitch * 0.5);
|
|
var cr = Math.Cos(roll * 0.5);
|
|
var sr = Math.Sin(roll * 0.5);
|
|
|
|
var qw = cr * cp * cy + sr * sp * sy;
|
|
var qx = sr * cp * cy - cr * sp * sy;
|
|
var qy = cr * sp * cy + sr * cp * sy;
|
|
var qz = cr * cp * sy - sr * sp * cy;
|
|
|
|
var result = _xlocClient.SetInitialPose(x, y, z, qx, qy, qz, qw);
|
|
|
|
if (result)
|
|
{
|
|
// Get current pose AFTER setting initial pose
|
|
System.Threading.Thread.Sleep(50); // Small delay to let XLOC process
|
|
var poseAfter = _xlocClient.GetCurrentPose();
|
|
if (poseAfter.HasValue)
|
|
{
|
|
var (px, py, pz, pqx, pqy, pqz, pqw) = poseAfter.Value;
|
|
double yawAfter = Math.Atan2(2.0 * (pqw * pqz + pqx * pqy), 1.0 - 2.0 * (pqy * pqy + pqz * pqz));
|
|
var dx = Math.Abs(px - x);
|
|
var dy = Math.Abs(py - y);
|
|
var dz = Math.Abs(pz - z);
|
|
var dyaw = Math.Abs(yawAfter - yaw);
|
|
}
|
|
}
|
|
return result;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Change (re-anchor) the map coordinate system origin.
|
|
/// Takes an (x, y, z) offset in meters and Euler angles (roll, pitch, yaw) in radians,
|
|
/// converts them to a pose, and invokes the native XLOC change_map_origin.
|
|
/// </summary>
|
|
public bool ChangeMapOrigin(double x, double y, double z, double roll, double pitch, double yaw)
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (!_isInitialized || _xlocClient == null)
|
|
{
|
|
_logger.LogWarning("Cannot change map origin: XlocIntegrationService not initialized");
|
|
return false;
|
|
}
|
|
|
|
// Block when XLOC is MAPPING (0) or PROCESSING (2) to avoid inconsistent map state
|
|
var diagnostics = _xlocClient.GetDiagnostics();
|
|
if (diagnostics != null && diagnostics.XlocState is 0 or 2)
|
|
{
|
|
_logger.LogWarning("Skip ChangeMapOrigin because XLOC state is {State} (0=MAPPING, 2=PROCESSING)", diagnostics.XlocState);
|
|
return false;
|
|
}
|
|
|
|
var cy = Math.Cos(yaw * 0.5);
|
|
var sy = Math.Sin(yaw * 0.5);
|
|
var cp = Math.Cos(pitch * 0.5);
|
|
var sp = Math.Sin(pitch * 0.5);
|
|
var cr = Math.Cos(roll * 0.5);
|
|
var sr = Math.Sin(roll * 0.5);
|
|
|
|
var qw = cr * cp * cy + sr * sp * sy;
|
|
var qx = sr * cp * cy - cr * sp * sy;
|
|
var qy = cr * sp * cy + sr * cp * sy;
|
|
var qz = cr * cp * sy - sr * sp * cy;
|
|
|
|
_logger.LogInformation("ChangeMapOrigin -> pos=({X:F3}, {Y:F3}, {Z:F3}) yaw={Yaw:F3} rad",
|
|
x, y, z, yaw);
|
|
|
|
var result = _xlocClient.ChangeMapOrigin(x, y, z, qx, qy, qz, qw);
|
|
if (result)
|
|
{
|
|
_logger.LogInformation("Map origin changed successfully");
|
|
|
|
// Persist the new origin to the map's YAML on disk so that subsequent calls to
|
|
// /api/xloc/gridmap/static (which override the native origin with the YAML value)
|
|
// return the same origin that native XLOC now uses. Without this, the map bytes
|
|
// and the origin served to the client are desynchronized and the map renders
|
|
// in the wrong place until the user reloads the map.
|
|
try
|
|
{
|
|
var nativeGrid = _xlocClient.GetStaticGridMap(reloadFromFile: false);
|
|
var activeMapName = diagnostics?.CurrentActiveMap;
|
|
if (nativeGrid != null && !string.IsNullOrWhiteSpace(activeMapName))
|
|
{
|
|
var newYaw = QuaternionToYaw(
|
|
nativeGrid.Origin.Qx,
|
|
nativeGrid.Origin.Qy,
|
|
nativeGrid.Origin.Qz,
|
|
nativeGrid.Origin.Qw);
|
|
|
|
UpdateMapYamlOrigin(activeMapName!, nativeGrid.Origin.X, nativeGrid.Origin.Y, newYaw);
|
|
}
|
|
else
|
|
{
|
|
_logger.LogWarning("ChangeMapOrigin: cannot persist YAML origin (native grid or active map name missing)");
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "ChangeMapOrigin: failed to persist new origin to YAML");
|
|
}
|
|
}
|
|
else
|
|
{
|
|
_logger.LogWarning("Failed to change map origin");
|
|
}
|
|
return result;
|
|
}
|
|
}
|
|
|
|
private static double QuaternionToYaw(double qx, double qy, double qz, double qw)
|
|
{
|
|
// ZYX yaw: atan2(2*(qw*qz + qx*qy), 1 - 2*(qy^2 + qz^2))
|
|
return Math.Atan2(2.0 * (qw * qz + qx * qy), 1.0 - 2.0 * (qy * qy + qz * qz));
|
|
}
|
|
|
|
/// <summary>
|
|
/// Rewrite the `origin: [x, y, yaw]` line of the active map's YAML file so the
|
|
/// static-gridmap endpoint (which reads origin from YAML) returns the same value
|
|
/// that native XLOC now has in memory after xloc_change_map_origin.
|
|
/// </summary>
|
|
private void UpdateMapYamlOrigin(string activeMapName, double x, double y, double yaw)
|
|
{
|
|
var mapsDir = XlocPaths.GetMapsDirectory();
|
|
var folderName = System.IO.Path.GetFileName(activeMapName.TrimEnd(System.IO.Path.DirectorySeparatorChar, '/'));
|
|
if (string.IsNullOrEmpty(folderName))
|
|
{
|
|
_logger.LogWarning("UpdateMapYamlOrigin: active map name '{Name}' did not resolve to a folder", activeMapName);
|
|
return;
|
|
}
|
|
|
|
var mapFolder = System.IO.Path.Combine(mapsDir, folderName);
|
|
if (!System.IO.Directory.Exists(mapFolder))
|
|
{
|
|
_logger.LogWarning("UpdateMapYamlOrigin: map folder not found: {Folder}", mapFolder);
|
|
return;
|
|
}
|
|
|
|
var yamlFiles = System.IO.Directory.GetFiles(mapFolder, "*.yaml");
|
|
if (yamlFiles.Length == 0)
|
|
{
|
|
_logger.LogWarning("UpdateMapYamlOrigin: no YAML file found in {Folder}", mapFolder);
|
|
return;
|
|
}
|
|
|
|
var yamlPath = yamlFiles[0];
|
|
var ci = System.Globalization.CultureInfo.InvariantCulture;
|
|
var newOriginLine = string.Format(ci, "origin: [{0:F6}, {1:F6}, {2:F6}]", x, y, yaw);
|
|
|
|
var lines = System.IO.File.ReadAllLines(yamlPath);
|
|
bool replaced = false;
|
|
for (int i = 0; i < lines.Length; i++)
|
|
{
|
|
if (lines[i].TrimStart().StartsWith("origin:", StringComparison.Ordinal))
|
|
{
|
|
lines[i] = newOriginLine;
|
|
replaced = true;
|
|
break;
|
|
}
|
|
}
|
|
if (!replaced)
|
|
{
|
|
// Preserve the rest of the file by appending an origin line if one wasn't present.
|
|
var appended = new string[lines.Length + 1];
|
|
Array.Copy(lines, appended, lines.Length);
|
|
appended[lines.Length] = newOriginLine;
|
|
lines = appended;
|
|
}
|
|
|
|
System.IO.File.WriteAllLines(yamlPath, lines);
|
|
_logger.LogInformation("UpdateMapYamlOrigin: {Yaml} -> {Line}", yamlPath, newOriginLine);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Manually start localization mode
|
|
/// </summary>
|
|
public bool StartLocalization()
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (!_isInitialized || _xlocClient == null)
|
|
{
|
|
_logger.LogWarning("Cannot start localization: XlocIntegrationService not initialized");
|
|
return false;
|
|
}
|
|
|
|
_logger.LogInformation("Attempting to start localization...");
|
|
var result = _xlocClient.StartLocalization();
|
|
if (result)
|
|
{
|
|
_logger.LogInformation("Localization started manually");
|
|
}
|
|
else
|
|
{
|
|
_logger.LogWarning("Failed to start localization - check previous log messages for error details");
|
|
}
|
|
return result;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Manually stop localization mode
|
|
/// </summary>
|
|
public bool StopLocalization()
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (!_isInitialized || _xlocClient == null)
|
|
{
|
|
_logger.LogWarning("Cannot stop localization: XlocIntegrationService not initialized");
|
|
return false;
|
|
}
|
|
|
|
var result = _xlocClient.StopLocalization();
|
|
if (result)
|
|
{
|
|
LastLocalizationStopUtc = DateTime.UtcNow;
|
|
_logger.LogInformation("Localization stopped manually");
|
|
}
|
|
return result;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Manually start mapping mode
|
|
/// </summary>
|
|
public bool StartMapping()
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (!_isInitialized || _xlocClient == null)
|
|
{
|
|
_logger.LogWarning("Cannot start mapping: XlocIntegrationService not initialized");
|
|
return false;
|
|
}
|
|
|
|
var result = _xlocClient.StartMapping();
|
|
if (result)
|
|
{
|
|
_logger.LogInformation("Mapping started manually");
|
|
}
|
|
return result;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Manually stop mapping and save to file
|
|
/// </summary>
|
|
public bool StopMapping(string saveMapFilePath)
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (!_isInitialized || _xlocClient == null)
|
|
{
|
|
_logger.LogWarning("Cannot stop mapping: XlocIntegrationService not initialized");
|
|
return false;
|
|
}
|
|
|
|
var result = _xlocClient.StopMapping(saveMapFilePath);
|
|
if (result)
|
|
{
|
|
_logger.LogInformation("Mapping stopped and saved to: {MapFile}", saveMapFilePath);
|
|
}
|
|
return result;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Reset SLAM error state - clears previous trajectory state
|
|
/// </summary>
|
|
public bool ResetSlamError()
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (!_isInitialized || _xlocClient == null)
|
|
{
|
|
_logger.LogWarning("Cannot reset SLAM error: XlocIntegrationService not initialized");
|
|
return false;
|
|
}
|
|
|
|
_logger.LogInformation("Resetting SLAM error state...");
|
|
var result = _xlocClient.ResetSlamError();
|
|
if (result)
|
|
{
|
|
_logger.LogInformation("SLAM error reset successfully");
|
|
}
|
|
else
|
|
{
|
|
_logger.LogWarning("Failed to reset SLAM error");
|
|
}
|
|
return result;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Start updating existing map (only allowed when in localization mode)
|
|
/// </summary>
|
|
public bool StartUpdateMap()
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (!_isInitialized || _xlocClient == null)
|
|
{
|
|
_logger.LogWarning("Cannot start update map: XlocIntegrationService not initialized");
|
|
return false;
|
|
}
|
|
|
|
_logger.LogInformation("Attempting to start map update...");
|
|
var result = _xlocClient.StartUpdateMap();
|
|
if (result)
|
|
{
|
|
_logger.LogInformation("Map update started successfully");
|
|
}
|
|
else
|
|
{
|
|
_logger.LogWarning("Failed to start map update - check that robot is in localization mode");
|
|
}
|
|
return result;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Stop updating map and optionally save the updated map
|
|
/// </summary>
|
|
public bool StopUpdateMap(bool saveUpdatedMap = true)
|
|
{
|
|
lock (_lock)
|
|
{
|
|
if (!_isInitialized || _xlocClient == null)
|
|
{
|
|
_logger.LogWarning("Cannot stop update map: XlocIntegrationService not initialized");
|
|
return false;
|
|
}
|
|
|
|
_logger.LogInformation("Stopping map update (save={Save})...", saveUpdatedMap);
|
|
var result = _xlocClient.StopUpdateMap(saveUpdatedMap);
|
|
if (result)
|
|
{
|
|
_logger.LogInformation("Map update stopped successfully (save={Save})", saveUpdatedMap);
|
|
}
|
|
else
|
|
{
|
|
_logger.LogWarning("Failed to stop map update");
|
|
}
|
|
return result;
|
|
}
|
|
}
|
|
|
|
#endregion
|
|
|
|
/// <summary>
|
|
/// Broadcast current pose to all SignalR clients
|
|
/// </summary>
|
|
private async Task BroadcastPoseToClientsAsync()
|
|
{
|
|
if (_poseHubContext == null)
|
|
return;
|
|
|
|
try
|
|
{
|
|
var pose = GetCurrentPose();
|
|
if (!pose.HasValue)
|
|
return;
|
|
|
|
var (x, y, z, qx, qy, qz, qw) = pose.Value;
|
|
|
|
// Convert quaternion to yaw
|
|
double yaw = Math.Atan2(2.0 * (qw * qz + qx * qy),
|
|
1.0 - 2.0 * (qy * qy + qz * qz));
|
|
|
|
var poseData = new XlocPoseData
|
|
{
|
|
X = x,
|
|
Y = y,
|
|
Z = z,
|
|
Yaw = yaw,
|
|
QuaternionX = qx,
|
|
QuaternionY = qy,
|
|
QuaternionZ = qz,
|
|
QuaternionW = qw,
|
|
Timestamp = DateTime.UtcNow,
|
|
IsActive = _isInitialized,
|
|
XlocState = 0, // Not available without diagnostics call
|
|
MapName = string.Empty,
|
|
Reliability = 0.0,
|
|
MatchingScore = 0.0
|
|
};
|
|
|
|
await _poseHubContext.Clients.All.SendAsync("ReceivePose", poseData);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error broadcasting pose to SignalR clients");
|
|
}
|
|
}
|
|
|
|
public void Dispose()
|
|
{
|
|
if (_disposed)
|
|
return;
|
|
|
|
_disposed = true;
|
|
|
|
try
|
|
{
|
|
// Stop event handlers first
|
|
lock (_lock)
|
|
{
|
|
_isInitialized = false;
|
|
}
|
|
|
|
UnregisterLidarEventDispatch();
|
|
Thread.Sleep(500);
|
|
UnregisterOdomImuEventDispatch();
|
|
|
|
_imuTimer?.Dispose();
|
|
_odomTimer?.Dispose();
|
|
_lidarTimer?.Dispose();
|
|
_loggingTimer?.Dispose();
|
|
_asyncDispatcher?.Dispose();
|
|
_xlocClient?.Dispose();
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error disposing XlocIntegrationService");
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Xloc operation mode
|
|
/// </summary>
|
|
public enum XlocMode
|
|
{
|
|
None,
|
|
Localization,
|
|
Mapping
|
|
}
|
|
|
|
/// <summary>
|
|
/// Configuration for XlocIntegrationService
|
|
/// </summary>
|
|
public class XlocIntegrationConfiguration
|
|
{
|
|
/// <summary>
|
|
/// Enable/disable the integration service
|
|
/// </summary>
|
|
public bool Enabled { get; set; } = false;
|
|
|
|
/// <summary>
|
|
/// Operation mode: Localization or Mapping
|
|
/// </summary>
|
|
public XlocMode Mode { get; set; } = XlocMode.Localization;
|
|
|
|
/// <summary>
|
|
/// IMU data dispatch interval in milliseconds (default: 1ms = 1000Hz)
|
|
/// </summary>
|
|
public int ImuDispatchIntervalMs { get; set; } = 1;
|
|
|
|
/// <summary>
|
|
/// Odometry data dispatch interval in milliseconds (default: 1ms = 1000Hz)
|
|
/// </summary>
|
|
public int OdomDispatchIntervalMs { get; set; } = 1;
|
|
|
|
/// <summary>
|
|
/// Lidar data dispatch interval in milliseconds (default: 1ms = 1000Hz)
|
|
/// </summary>
|
|
public int LidarDispatchIntervalMs { get; set; } = 1;
|
|
|
|
/// <summary>
|
|
/// Update rate in Hz (DEPRECATED - use individual sensor intervals instead)
|
|
/// </summary>
|
|
[Obsolete("Use ImuDispatchIntervalMs, OdomDispatchIntervalMs, LidarDispatchIntervalMs instead")]
|
|
public int UpdateRateHz { get; set; } = 20;
|
|
|
|
/// <summary>
|
|
/// Path to map file for localization mode
|
|
/// </summary>
|
|
public string MapFilePath { get; set; } = string.Empty;
|
|
|
|
/// <summary>
|
|
/// Path to save map file when stopping mapping mode
|
|
/// </summary>
|
|
public string SaveMapFilePath { get; set; } = string.Empty;
|
|
|
|
/// <summary>
|
|
/// Enable dispatching EKF filtered odometry
|
|
/// </summary>
|
|
public bool EnableEKFOdometry { get; set; } = true;
|
|
|
|
/// <summary>
|
|
/// Sensor ID for EKF odometry
|
|
/// </summary>
|
|
public string EKFOdometrySensorId { get; set; } = "ekf_odometry";
|
|
|
|
/// <summary>
|
|
/// Enable dispatching raw wheel odometry (usually not needed if EKF is enabled)
|
|
/// </summary>
|
|
public bool EnableRawOdometry { get; set; } = false;
|
|
|
|
/// <summary>
|
|
/// Sensor ID for raw odometry
|
|
/// </summary>
|
|
public string RawOdometrySensorId { get; set; } = "wheel_odometry";
|
|
|
|
/// <summary>
|
|
/// Enable dispatching IMU data
|
|
/// </summary>
|
|
public bool EnableIMU { get; set; } = true;
|
|
|
|
/// <summary>
|
|
/// IMU device ID
|
|
/// </summary>
|
|
public string ImuDeviceId { get; set; } = string.Empty;
|
|
|
|
/// <summary>
|
|
/// Sensor ID for IMU
|
|
/// </summary>
|
|
public string ImuSensorId { get; set; } = "imu0";
|
|
|
|
/// <summary>
|
|
/// IMU frame ID
|
|
/// </summary>
|
|
public string ImuFrameId { get; set; } = "imu_link";
|
|
|
|
/// <summary>
|
|
/// Enable dispatching LIDAR data
|
|
/// </summary>
|
|
public bool EnableLidar1 { get; set; } = true;
|
|
|
|
public bool EnableLidar2 { get; set; } = true;
|
|
|
|
public bool EnableLidar3 { get; set; } = true;
|
|
|
|
/// <summary>
|
|
/// LIDAR device ID
|
|
/// </summary>
|
|
public string Lidar1DeviceId { get; set; } = "scan_1";
|
|
|
|
/// <summary>
|
|
/// Sensor ID for LIDAR 1 (rear center)
|
|
/// </summary>
|
|
public string Lidar1SensorId { get; set; } = "scan_1";
|
|
|
|
/// <summary>
|
|
/// LIDAR 2 device ID (right side)
|
|
/// </summary>
|
|
public string Lidar2DeviceId { get; set; } = "scan_2";
|
|
|
|
/// <summary>
|
|
/// Sensor ID for LIDAR 2 (right side)
|
|
/// </summary>
|
|
public string Lidar2SensorId { get; set; } = "scan_2";
|
|
|
|
/// <summary>
|
|
/// LIDAR 3 device ID (left side)
|
|
/// </summary>
|
|
public string Lidar3DeviceId { get; set; } = "scan_3";
|
|
|
|
/// <summary>
|
|
/// Sensor ID for LIDAR 3 (left side)
|
|
/// </summary>
|
|
public string Lidar3SensorId { get; set; } = "scan_3";
|
|
|
|
/// <summary>
|
|
/// Interval in seconds to log statistics
|
|
/// </summary>
|
|
public int StatsLogIntervalSeconds { get; set; } = 10;
|
|
|
|
/// <summary>
|
|
/// When true, <see cref="XlocAutoLocalizationHostedService"/> polls diagnostics and calls
|
|
/// StartLocalization when XLOC enters READY (3) with an active map.
|
|
/// </summary>
|
|
public bool AutoStartLocalizationOnReady { get; set; }
|
|
|
|
/// <summary>
|
|
/// How often to poll diagnostics for auto-start (milliseconds).
|
|
/// </summary>
|
|
public int AutoStartLocalizationPollIntervalMs { get; set; } = 500;
|
|
|
|
/// <summary>
|
|
/// After a successful stop, do not auto-start localization for this many milliseconds.
|
|
/// </summary>
|
|
public int AutoStartLocalizationCooldownAfterStopMs { get; set; } = 10_000;
|
|
}
|