Initial commit
This commit is contained in:
@@ -0,0 +1,500 @@
|
||||
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
|
||||
}
|
||||
Reference in New Issue
Block a user