501 lines
21 KiB
C#
501 lines
21 KiB
C#
using Appccelerate.StateMachine;
|
|
using Appccelerate.StateMachine.Machine;
|
|
using CartographerSharp.Mapping;
|
|
using CartographerSharp.Models.Mapping;
|
|
using CartographerSharp.Sensor;
|
|
using Microsoft.AspNetCore.SignalR;
|
|
using Microsoft.Extensions.Options;
|
|
using System.Collections.Concurrent;
|
|
using System.Diagnostics;
|
|
using System.Globalization;
|
|
using RobotNet10.RobotApp.Devices;
|
|
using RobotNet10.RobotApp.Hubs;
|
|
using RobotNet10.RobotApp.Motion;
|
|
using RobotNet10.RobotApp.Shared;
|
|
using RobotNet10.RobotApp.Shared.Enums;
|
|
using RobotNet10.RobotApp.SLAM.Cartographer.Enums;
|
|
using RobotNet10.RobotApp.SLAM.Cartographer.Geometry;
|
|
using RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
|
|
using RobotNet10.RobotApp.SLAM.Cartographer.Mcl;
|
|
using RobotNet10.Shared.Geometry;
|
|
using RobotNet10.Shared.Localization;
|
|
using RobotNet10.Shared.Numbers;
|
|
using SkiaSharp;
|
|
|
|
namespace RobotNet10.RobotApp.SLAM.Cartographer;
|
|
|
|
/// <summary>
|
|
/// Main service for Cartographer SLAM integration.
|
|
/// Manages state machine, sensor subscriptions, and handles both localization and scan mapping.
|
|
/// Implements IHostedService to start automatically when application starts.
|
|
///
|
|
/// Split into partial classes:
|
|
/// CartographerService.cs - Fields, properties, constructor
|
|
/// CartographerService.StateMachine.cs - State machine definition and helpers
|
|
/// CartographerService.StateHandlers.cs- State entry/exit handlers
|
|
/// CartographerService.SensorProcessing.cs - Sensor data callbacks and MCL processing
|
|
/// CartographerService.ApiMethods.cs - ISLAMService method implementations
|
|
/// CartographerService.MapManagement.cs- Map CRUD, loading, wall alignment
|
|
/// CartographerService.Lifecycle.cs - IHostedService, state persistence, IDisposable
|
|
/// </summary>
|
|
public partial class CartographerService : ISLAMService, IHostedService, IDisposable
|
|
{
|
|
#region Fields
|
|
|
|
private readonly PassiveStateMachine<SLAMState, CartographerTrigger> _stateMachine;
|
|
private readonly CartographerConfiguration _config;
|
|
private readonly IDeviceProvider _deviceProvider;
|
|
private readonly SensorPipeline? _sensorPipeline;
|
|
private readonly IHubContext<SLAMHub> _hubContext;
|
|
private readonly ILogger<CartographerService> _logger;
|
|
private readonly Lock _lock = new();
|
|
private readonly Lock _stateMachineLock = new();
|
|
|
|
// State tracking (synchronized with state machine in ExecuteOnEntry handlers)
|
|
private SLAMState _currentState = SLAMState.Idle;
|
|
|
|
// Error tracking
|
|
private string? _lastError;
|
|
private Exception? _lastException;
|
|
|
|
// Unified MapBuilder and TrajectoryBuilder for both ScanMapping and Localization
|
|
private IMapBuilder? _mapBuilder;
|
|
private ITrajectoryBuilder? _trajectoryBuilder;
|
|
private int _trajectoryId = -1;
|
|
private string? _currentMapName; // Store current map name for both ScanMapping and Localization
|
|
private Pose? _wallAlignedPose; // Store wall-aligned pose calculated during ScanMapping entry for use when saving
|
|
private readonly Lock _wallAlignedPoseLock = new(); // Synchronization for _wallAlignedPose
|
|
private CancellationTokenSource? _savingMapCts; // CancellationTokenSource for save map operation
|
|
|
|
// Covariance computation throttle (Fix 3)
|
|
private int _covarianceComputing; // 0 = idle, 1 = computing
|
|
|
|
// Pose sync: Stopwatch for periodic pose saving during Localizing/ScanMapping
|
|
private readonly Stopwatch _poseSyncStopwatch = new();
|
|
|
|
// Pose snapshot: lock-free cached pose + confidence (updated by background thread or MCL)
|
|
// Access via Volatile.Read/Write for memory ordering guarantees
|
|
private PoseSnapshot _poseSnapshot = PoseSnapshot.Empty;
|
|
|
|
// Background pose extrapolation thread (runs at 20Hz for ScanMapping, 100Hz for Localizing)
|
|
private Thread? _poseThread;
|
|
private volatile bool _poseThreadRunning;
|
|
|
|
// Background trigger thread: handles deferred state machine triggers from sensor processing
|
|
private Thread? _triggerThread;
|
|
private volatile bool _triggerThreadRunning;
|
|
|
|
// Confidence metrics: written by sensor callback, read by pose thread
|
|
private Matrix3x3? _poseCovariance;
|
|
private int _constraintCount = 0;
|
|
private double _averageConstraintQuality = 0.0;
|
|
private double _lastMatchingScore = -1.0; // ScanMapping: PoseConfidence from scan matcher (use Volatile.Read/Write)
|
|
|
|
// Localization: Constraint caching for performance
|
|
private readonly TrajectoryConstraintCache _constraintCache = new(TimeSpan.FromMilliseconds(100));
|
|
|
|
// MCL (Monte Carlo Localization): when Enabled, SetInitialPoseAsync runs MCL until convergence then adds trajectory (xloc flow). Created internally, only used here.
|
|
private readonly MclService? _mcl;
|
|
private readonly MclProcessor? _mclProcessor;
|
|
|
|
// Pending state machine trigger: used to defer FireStateMachine calls from lidar processing thread
|
|
// This prevents deadlock when MCL converges and tries to stop the lidar thread from within itself
|
|
// Uses int because volatile cannot be used with nullable enums; -1 = no pending, >= 0 = trigger value
|
|
private int _pendingTrigger = -1;
|
|
private readonly ManualResetEventSlim _pendingTriggerEvent = new(false);
|
|
|
|
// Sample point cloud in map/global frame from AddSensorData SamplePointCloudGlobal; returned by GetAggregatedSamplePointCloud
|
|
private List<Vector3> _completedAccumulatedSamplePointClouds = [];
|
|
private readonly Lock _pointCloudLock = new();
|
|
private readonly Stopwatch _pointCloudUpdateStopwatch = Stopwatch.StartNew();
|
|
|
|
// Reusable buffer for MCL scan conversion (Fix 6a)
|
|
private List<Vector2>? _mclScanBuffer;
|
|
|
|
// Pending localization data (set by StartLocalization, consumed by state handlers)
|
|
private readonly Lock _localizationLock = new();
|
|
private string? _pendingLocalizationMapName;
|
|
private Pose? _pendingLocalizationInitialPose;
|
|
private bool _isSetInitialPoseFlow; // Flag to differentiate SetInitialPose from StartLocalization
|
|
|
|
// Helper processors
|
|
private readonly SlamResultProcessor? _slamResultProcessor;
|
|
private readonly OccupancyGridManager _occupancyGridManager;
|
|
private readonly MapSaveProcessor? _mapSaveProcessor;
|
|
|
|
// Drift detection for Localizing state
|
|
private readonly DriftDetector _driftDetector;
|
|
|
|
// Map processing status tracking
|
|
private readonly ConcurrentDictionary<string, bool> _mapProcessingStatus = new();
|
|
|
|
private readonly IOdometryEstimator? _odometryEstimator;
|
|
|
|
// Dispose tracking
|
|
private volatile bool _disposed = false;
|
|
|
|
#endregion
|
|
|
|
#region Properties
|
|
|
|
/// <summary>
|
|
/// Gets the MapBuilder instance (internal - for OccupancyGridProvider).
|
|
/// Works for both ScanMapping and Localization modes.
|
|
/// </summary>
|
|
internal IMapBuilder? MapBuilder => _mapBuilder;
|
|
|
|
/// <summary>
|
|
/// Gets the current trajectory ID (internal).
|
|
/// Works for both ScanMapping and Localization modes.
|
|
/// </summary>
|
|
internal int TrajectoryId => _trajectoryId;
|
|
|
|
/// <summary>
|
|
/// Gets the current pose in global (map) frame.
|
|
/// Lock-free: returns cached snapshot updated by background pose thread (20Hz ScanMapping, 100Hz Localizing)
|
|
/// or directly by MCL during Relocalizing.
|
|
/// </summary>
|
|
public Pose CurrentPose => Volatile.Read(ref _poseSnapshot).Pose;
|
|
|
|
/// <summary>
|
|
/// Gets the covariance of pose estimate (only available during localization).
|
|
/// Lock-free: reads from cached snapshot.
|
|
/// </summary>
|
|
public Matrix3x3? PoseCovariance => Volatile.Read(ref _poseSnapshot).Covariance;
|
|
|
|
/// <summary>
|
|
/// Gets the current map name (available during Localization or ScanMapping)
|
|
/// </summary>
|
|
public string? CurrentMap
|
|
{
|
|
get
|
|
{
|
|
lock (_lock)
|
|
{
|
|
return _currentMapName;
|
|
}
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets the confidence score. Meaning varies by state:
|
|
/// ScanMapping: PoseConfidence from scan matcher.
|
|
/// Localizing: LocalizationScore from covariance/constraints/scanMatchScore.
|
|
/// Relocalizing: MCL reliability.
|
|
/// Lock-free: reads from cached snapshot.
|
|
/// </summary>
|
|
public double? LocalizationScore => Volatile.Read(ref _poseSnapshot).Score;
|
|
|
|
/// <summary>
|
|
/// Gets the current drift detection metrics (only meaningful during Localizing state).
|
|
/// Provides detailed breakdown of localization quality factors including scan match score,
|
|
/// odometry residual, and drift status.
|
|
/// </summary>
|
|
public DriftDetector.DriftMetrics DriftMetrics => _driftDetector.GetLatestMetrics();
|
|
|
|
/// <summary>
|
|
/// Gets the current drift status (only meaningful during Localizing state).
|
|
/// Returns Stable, Warning, Critical, or Lost based on combined metrics analysis.
|
|
/// </summary>
|
|
public DriftDetector.DriftStatus DriftStatus => _driftDetector.GetLatestMetrics().Status;
|
|
|
|
/// <summary>
|
|
/// Gets the current state (synchronized with state machine)
|
|
/// </summary>
|
|
public SLAMState State
|
|
{
|
|
get
|
|
{
|
|
lock (_lock)
|
|
{
|
|
return _currentState;
|
|
}
|
|
}
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Pose Thread
|
|
|
|
/// <summary>
|
|
/// Starts the background pose extrapolation thread.
|
|
/// </summary>
|
|
/// <param name="intervalMs">Polling interval: 50ms for ScanMapping (20Hz), 10ms for Localizing (100Hz)</param>
|
|
private void StartPoseThread(int intervalMs)
|
|
{
|
|
if (_poseThreadRunning) return;
|
|
_poseThreadRunning = true;
|
|
_poseThread = new Thread(() => PoseThreadProc(intervalMs))
|
|
{
|
|
Name = "PoseExtrapolator",
|
|
IsBackground = true,
|
|
Priority = ThreadPriority.AboveNormal
|
|
};
|
|
_poseThread.Start();
|
|
}
|
|
|
|
/// <summary>
|
|
/// Stops the background pose extrapolation thread and waits for it to finish.
|
|
/// </summary>
|
|
private void StopPoseThread()
|
|
{
|
|
if (!_poseThreadRunning) return;
|
|
_poseThreadRunning = false;
|
|
_poseThread?.Join(500);
|
|
if (_poseThread?.IsAlive == true)
|
|
_logger.LogWarning("CartographerService: Pose thread did not stop within timeout");
|
|
_poseThread = null;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Background thread loop: polls TryGetExtrapolatedPoseFilter, applies localToGlobal transform,
|
|
/// and publishes global-frame PoseSnapshot via volatile write.
|
|
/// </summary>
|
|
private void PoseThreadProc(int intervalMs)
|
|
{
|
|
Thread.BeginThreadAffinity();
|
|
try
|
|
{
|
|
while (_poseThreadRunning && !_disposed)
|
|
{
|
|
try
|
|
{
|
|
// Read references without lock (tolerate stale by 1 iteration)
|
|
var tb = _trajectoryBuilder;
|
|
var mb = _mapBuilder;
|
|
var trajId = _trajectoryId;
|
|
|
|
if (tb != null)
|
|
{
|
|
var extrapolated = tb.TryGetExtrapolatedPose(DateTime.UtcNow.Ticks);
|
|
if (extrapolated.HasValue)
|
|
{
|
|
Pose globalPose;
|
|
if (mb != null && trajId >= 0)
|
|
{
|
|
var localToGlobal = mb.PoseGraph.GetLocalToGlobalTransform(trajId);
|
|
globalPose = PoseConverter.ToPose(localToGlobal * extrapolated.Value);
|
|
}
|
|
else
|
|
{
|
|
globalPose = PoseConverter.ToPose(extrapolated.Value);
|
|
}
|
|
|
|
// Build confidence-aware snapshot based on current state
|
|
var state = _currentState;
|
|
PoseSnapshot snapshot;
|
|
|
|
if (state == SLAMState.ScanMapping)
|
|
{
|
|
var matchScore = Volatile.Read(ref _lastMatchingScore);
|
|
double? score = null;
|
|
if (matchScore >= 0)
|
|
{
|
|
// Normalize to [0, 1] range if input is in [0, 100] range
|
|
// PoseConfidence from Cartographer returns 0-100 (percentage)
|
|
score = matchScore > 1.0 ? matchScore / 100.0 : matchScore;
|
|
score = Math.Clamp(score.Value, 0.0, 1.0);
|
|
}
|
|
snapshot = new PoseSnapshot(globalPose, score: score);
|
|
}
|
|
else
|
|
{
|
|
// Localizing: read covariance fields and compute score with scan match
|
|
Matrix3x3? cov;
|
|
int cc;
|
|
double cq;
|
|
lock (_lock)
|
|
{
|
|
cov = _poseCovariance;
|
|
cc = _constraintCount;
|
|
cq = _averageConstraintQuality;
|
|
}
|
|
|
|
// Include scan match score for drift detection
|
|
var matchScore = Volatile.Read(ref _lastMatchingScore);
|
|
double? scanMatchScore = matchScore >= 0 ? matchScore : null;
|
|
|
|
// Get odometry pose for cross-validation (helps distinguish drift vs dynamic obstacles)
|
|
Pose? odometryPose = _odometryEstimator?.CurrentPose;
|
|
|
|
// Update drift detector with all available metrics
|
|
var driftMetrics = _driftDetector.Update(
|
|
cartographerPose: globalPose,
|
|
odometryPose: odometryPose,
|
|
scanMatchScore: scanMatchScore ?? 0.5,
|
|
covariance: cov,
|
|
constraintCount: cc,
|
|
constraintQuality: cq,
|
|
mclPose: null, // MCL not running during Localizing
|
|
mclReliability: null);
|
|
|
|
// Use drift detector's combined score as the localization score
|
|
// This includes scan match score with proper weighting
|
|
var locScore = driftMetrics.CombinedScore;
|
|
|
|
snapshot = new PoseSnapshot(
|
|
globalPose,
|
|
cov,
|
|
locScore,
|
|
scanMatchScore,
|
|
driftMetrics.Status);
|
|
}
|
|
|
|
Volatile.Write(ref _poseSnapshot, snapshot);
|
|
}
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogDebug(ex, "CartographerService: PoseThread iteration failed");
|
|
}
|
|
|
|
Thread.Sleep(intervalMs);
|
|
}
|
|
}
|
|
finally
|
|
{
|
|
Thread.EndThreadAffinity();
|
|
}
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Trigger Thread
|
|
|
|
/// <summary>
|
|
/// Queues a state machine trigger to be fired from a dedicated thread.
|
|
/// This prevents deadlock when MCL converges inside the lidar processing thread
|
|
/// and tries to stop the same thread via state transition.
|
|
/// </summary>
|
|
private void QueueStateMachineTrigger(CartographerTrigger trigger)
|
|
{
|
|
Interlocked.Exchange(ref _pendingTrigger, (int)trigger);
|
|
_pendingTriggerEvent.Set();
|
|
}
|
|
|
|
/// <summary>
|
|
/// Starts the background trigger thread that handles deferred state machine transitions.
|
|
/// </summary>
|
|
private void StartTriggerThread()
|
|
{
|
|
if (_triggerThreadRunning) return;
|
|
_triggerThreadRunning = true;
|
|
_triggerThread = new Thread(TriggerThreadProc)
|
|
{
|
|
Name = "StateMachineTrigger",
|
|
IsBackground = true,
|
|
Priority = ThreadPriority.AboveNormal
|
|
};
|
|
_triggerThread.Start();
|
|
}
|
|
|
|
/// <summary>
|
|
/// Stops the background trigger thread.
|
|
/// </summary>
|
|
private void StopTriggerThread()
|
|
{
|
|
if (!_triggerThreadRunning) return;
|
|
_triggerThreadRunning = false;
|
|
_pendingTriggerEvent.Set(); // Wake thread to exit
|
|
_triggerThread?.Join(1000);
|
|
if (_triggerThread?.IsAlive == true)
|
|
_logger.LogWarning("CartographerService: Trigger thread did not stop within timeout");
|
|
_triggerThread = null;
|
|
}
|
|
|
|
/// <summary>
|
|
/// Background thread that processes deferred state machine triggers.
|
|
/// </summary>
|
|
private void TriggerThreadProc()
|
|
{
|
|
while (_triggerThreadRunning && !_disposed)
|
|
{
|
|
try
|
|
{
|
|
_pendingTriggerEvent.Wait(100); // Wait with timeout to check running flag
|
|
_pendingTriggerEvent.Reset();
|
|
|
|
var triggerValue = Interlocked.Exchange(ref _pendingTrigger, -1);
|
|
if (triggerValue >= 0)
|
|
{
|
|
var trigger = (CartographerTrigger)triggerValue;
|
|
_logger.LogDebug("CartographerService: Firing deferred trigger {Trigger}", trigger);
|
|
FireStateMachine(trigger);
|
|
}
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "CartographerService: Error in TriggerThreadProc");
|
|
}
|
|
}
|
|
}
|
|
|
|
#endregion
|
|
|
|
#region Constructor
|
|
|
|
public CartographerService(
|
|
IOptions<CartographerConfiguration> configuration,
|
|
IDeviceProvider deviceProvider,
|
|
IHubContext<SLAMHub> hubContext,
|
|
ILogger<CartographerService> logger,
|
|
IOdometryEstimator odometryEstimator,
|
|
ILoggerFactory loggerFactory)
|
|
{
|
|
_config = configuration?.Value ?? throw new ArgumentNullException(nameof(configuration));
|
|
_deviceProvider = deviceProvider ?? throw new ArgumentNullException(nameof(deviceProvider));
|
|
_hubContext = hubContext ?? throw new ArgumentNullException(nameof(hubContext));
|
|
_logger = logger ?? throw new ArgumentNullException(nameof(logger));
|
|
_odometryEstimator = odometryEstimator;
|
|
|
|
// Initialize components based on Enable flag
|
|
if (_config.Enable)
|
|
{
|
|
// Full SLAM mode: create all components
|
|
_mcl = _config.Mcl.Enabled ? new MclService(configuration) : null;
|
|
_sensorPipeline = new SensorPipeline(
|
|
deviceProvider,
|
|
odometryEstimator,
|
|
configuration,
|
|
logger,
|
|
OnAddRangeData,
|
|
OnAddImuData,
|
|
OnAddOdometryData);
|
|
_mclProcessor = _config.Mcl.Enabled ? new MclProcessor(_config, loggerFactory.CreateLogger<MclProcessor>()) : null;
|
|
_slamResultProcessor = new SlamResultProcessor(loggerFactory.CreateLogger<SlamResultProcessor>());
|
|
|
|
// Wire up MclProcessor odometry event if enabled
|
|
if (_mclProcessor != null && _mcl != null)
|
|
{
|
|
_mclProcessor.OnOdometryProcessed += (dt, vx, vy, vtheta) => _mcl.OnOdom(dt, vx, vy, vtheta);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// Localization-only mode: minimal components
|
|
_mcl = null;
|
|
_sensorPipeline = null;
|
|
_mclProcessor = null;
|
|
_slamResultProcessor = null;
|
|
|
|
_logger.LogInformation("CartographerService: Running in localization-only mode (Enable = false). Only StartLocalization, SetInitialPose, and StopLocalization are available.");
|
|
}
|
|
|
|
// Always create OccupancyGridManager (needed for both modes to load occupancy grid from file)
|
|
_occupancyGridManager = new OccupancyGridManager(_config, loggerFactory.CreateLogger<OccupancyGridManager>());
|
|
|
|
// Always create MapSaveProcessor (needed for both modes to transform maps and save metadata)
|
|
_mapSaveProcessor = new MapSaveProcessor(_config, loggerFactory.CreateLogger<MapSaveProcessor>());
|
|
|
|
// Always create DriftDetector for monitoring localization quality
|
|
_driftDetector = new DriftDetector();
|
|
|
|
// Build state machine
|
|
_stateMachine = BuildStateMachine();
|
|
// Note: State machine will be started in StartAsync (IHostedService)
|
|
}
|
|
|
|
#endregion
|
|
}
|