Initial commit
This commit is contained in:
@@ -0,0 +1,269 @@
|
||||
using CartographerSharp.Sensor;
|
||||
using RobotNet10.RobotApp.SLAM.Cartographer;
|
||||
using RobotNet10.Shared.Geometry;
|
||||
using RobotNet10.Shared.Localization;
|
||||
|
||||
namespace RobotNet10.RobotApp.SLAM.Cartographer.Helpers;
|
||||
|
||||
/// <summary>
|
||||
/// Reason for MCL convergence
|
||||
/// </summary>
|
||||
public enum MclConvergenceReason
|
||||
{
|
||||
/// <summary>MCL has not converged yet</summary>
|
||||
NotConverged,
|
||||
/// <summary>Pose is stable and quality criteria met</summary>
|
||||
PoseStable,
|
||||
/// <summary>Timeout reached without convergence</summary>
|
||||
Timeout,
|
||||
/// <summary>Max iterations reached without convergence</summary>
|
||||
MaxIterations
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Processes MCL (Monte Carlo Localization) convergence and pose tracking
|
||||
/// </summary>
|
||||
public class MclProcessor
|
||||
{
|
||||
private readonly ILogger<MclProcessor> _logger;
|
||||
private readonly CartographerConfiguration _config;
|
||||
private readonly Lock _lock = new();
|
||||
|
||||
private bool _running;
|
||||
private string? _primaryLidarId;
|
||||
private DateTime? _stableSinceUtc;
|
||||
private DateTime _runningSinceUtc = DateTime.MinValue;
|
||||
private int _iterationCount;
|
||||
private Pose? _lastPose;
|
||||
private Pose _initialPose; // Initial pose for fallback on timeout
|
||||
private OdometryData? _lastOdometryData;
|
||||
private double _lastOdometryTimestampSec;
|
||||
|
||||
public MclProcessor(CartographerConfiguration config, ILogger<MclProcessor> logger)
|
||||
{
|
||||
_config = config ?? throw new ArgumentNullException(nameof(config));
|
||||
_logger = logger ?? throw new ArgumentNullException(nameof(logger));
|
||||
}
|
||||
|
||||
public bool IsRunning
|
||||
{
|
||||
get { lock (_lock) { return _running; } }
|
||||
}
|
||||
|
||||
public string? PrimaryLidarId
|
||||
{
|
||||
get { lock (_lock) { return _primaryLidarId; } }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Gets the initial pose used when MCL started (for fallback on timeout)
|
||||
/// </summary>
|
||||
public Pose InitialPose
|
||||
{
|
||||
get { lock (_lock) { return _initialPose; } }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Start MCL with initial seed pose
|
||||
/// </summary>
|
||||
public void Start(Pose seedPose, string effectivePrimaryLiderId, OccupancyGrid? grid)
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
_running = true;
|
||||
_primaryLidarId = effectivePrimaryLiderId;
|
||||
_iterationCount = 0;
|
||||
_lastPose = seedPose;
|
||||
_initialPose = seedPose; // Store initial pose for timeout fallback
|
||||
_stableSinceUtc = null;
|
||||
_runningSinceUtc = DateTime.UtcNow;
|
||||
_lastOdometryData = null;
|
||||
_lastOdometryTimestampSec = 0;
|
||||
}
|
||||
|
||||
_logger.LogInformation("MclProcessor: MCL started with primary lidar: {LidarId}", effectivePrimaryLiderId);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Stop MCL
|
||||
/// </summary>
|
||||
public void Stop()
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
_running = false;
|
||||
_primaryLidarId = null;
|
||||
_stableSinceUtc = null;
|
||||
_iterationCount = 0;
|
||||
_lastPose = null;
|
||||
_lastOdometryData = null;
|
||||
_lastOdometryTimestampSec = 0;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Update MCL seed pose during running state
|
||||
/// </summary>
|
||||
public void UpdateSeedPose(Pose newPose)
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
if (!_running)
|
||||
return;
|
||||
|
||||
_iterationCount = 0;
|
||||
_lastPose = null;
|
||||
_stableSinceUtc = null;
|
||||
_lastOdometryData = null;
|
||||
_lastOdometryTimestampSec = 0;
|
||||
}
|
||||
|
||||
_logger.LogInformation("MclProcessor: Seed pose updated");
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Check MCL convergence based on pose stability and quality criteria
|
||||
/// </summary>
|
||||
/// <returns>True if converged (for any reason), false otherwise</returns>
|
||||
public bool CheckConvergence(Pose currentPose, double reliability, double? mae, out int iterationCount)
|
||||
{
|
||||
return CheckConvergenceWithReason(currentPose, reliability, mae, out iterationCount) != MclConvergenceReason.NotConverged;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Check MCL convergence and return the reason
|
||||
/// </summary>
|
||||
/// <returns>Convergence reason indicating why MCL stopped or NotConverged if still running</returns>
|
||||
public MclConvergenceReason CheckConvergenceWithReason(Pose currentPose, double reliability, double? mae, out int iterationCount)
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
iterationCount = ++_iterationCount;
|
||||
|
||||
if (_iterationCount < _config.Mcl.ConvergenceMinIterations)
|
||||
{
|
||||
_lastPose = currentPose;
|
||||
return MclConvergenceReason.NotConverged;
|
||||
}
|
||||
|
||||
// Calculate pose change
|
||||
double poseDist = double.MaxValue;
|
||||
double yawDiff = double.MaxValue;
|
||||
|
||||
if (_lastPose.HasValue)
|
||||
{
|
||||
double dx = currentPose.Position.X - _lastPose.Value.Position.X;
|
||||
double dy = currentPose.Position.Y - _lastPose.Value.Position.Y;
|
||||
poseDist = Math.Sqrt(dx * dx + dy * dy);
|
||||
|
||||
double yawCur = GetYawFromPose(currentPose);
|
||||
double yawLast = GetYawFromPose(_lastPose.Value);
|
||||
double dyaw = yawCur - yawLast;
|
||||
while (dyaw > Math.PI) dyaw -= 2.0 * Math.PI;
|
||||
while (dyaw < -Math.PI) dyaw += 2.0 * Math.PI;
|
||||
yawDiff = Math.Abs(dyaw);
|
||||
}
|
||||
|
||||
_lastPose = currentPose;
|
||||
|
||||
// Check stability criteria
|
||||
bool poseStable = poseDist < _config.Mcl.ConvergencePoseChangeThresholdMeters &&
|
||||
yawDiff < _config.Mcl.ConvergenceYawChangeThresholdRad;
|
||||
|
||||
bool qualityGood = !_config.Mcl.EstimateReliability ||
|
||||
(reliability >= _config.Mcl.ConvergenceReliabilityMin &&
|
||||
(!mae.HasValue || mae.Value <= _config.Mcl.ConvergenceMaeMaxMeters));
|
||||
|
||||
var now = DateTime.UtcNow;
|
||||
double runningSec = (now - _runningSinceUtc).TotalSeconds;
|
||||
double stableDurationSec = _config.Mcl.ConvergenceStableDurationSeconds;
|
||||
double timeoutSec = _config.Mcl.ConvergenceTimeoutSeconds;
|
||||
|
||||
if (qualityGood)
|
||||
{
|
||||
if (poseStable)
|
||||
{
|
||||
if (!_stableSinceUtc.HasValue)
|
||||
_stableSinceUtc = now;
|
||||
else if ((now - _stableSinceUtc.Value).TotalSeconds >= stableDurationSec)
|
||||
return MclConvergenceReason.PoseStable;
|
||||
}
|
||||
else
|
||||
{
|
||||
_stableSinceUtc = null;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
_stableSinceUtc = null;
|
||||
if (runningSec >= timeoutSec)
|
||||
return MclConvergenceReason.Timeout;
|
||||
}
|
||||
|
||||
if (_iterationCount >= _config.Mcl.ConvergenceMaxIterations)
|
||||
return MclConvergenceReason.MaxIterations;
|
||||
|
||||
return MclConvergenceReason.NotConverged;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Process odometry data for MCL motion model
|
||||
/// </summary>
|
||||
public void ProcessOdometry(OdometryData odomData)
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
if (!_running || !_lastOdometryData.HasValue)
|
||||
{
|
||||
_lastOdometryData = odomData;
|
||||
_lastOdometryTimestampSec = odomData.Time / 10_000_000.0;
|
||||
return;
|
||||
}
|
||||
|
||||
double currentTimeSec = odomData.Time / 10_000_000.0;
|
||||
double deltaTimeSec = currentTimeSec - _lastOdometryTimestampSec;
|
||||
|
||||
if (deltaTimeSec > 0 && deltaTimeSec < 1.0)
|
||||
{
|
||||
var lastPose = _lastOdometryData.Value.Pose;
|
||||
var currentOdomPose = odomData.Pose;
|
||||
|
||||
double dx = currentOdomPose.Translation.X - lastPose.Translation.X;
|
||||
double dy = currentOdomPose.Translation.Y - lastPose.Translation.Y;
|
||||
double linearX = dx / deltaTimeSec;
|
||||
double linearY = dy / deltaTimeSec;
|
||||
|
||||
double yawLast = ((RobotNet10.Shared.Numbers.Quaternion)lastPose.Rotation).ToYawRadian();
|
||||
double yawCurrent = ((RobotNet10.Shared.Numbers.Quaternion)currentOdomPose.Rotation).ToYawRadian();
|
||||
double dyaw = yawCurrent - yawLast;
|
||||
while (dyaw > Math.PI) dyaw -= 2.0 * Math.PI;
|
||||
while (dyaw < -Math.PI) dyaw += 2.0 * Math.PI;
|
||||
double angularZ = dyaw / deltaTimeSec;
|
||||
|
||||
// Store for use by MCL (would be passed to mcl.OnOdom in actual implementation)
|
||||
OnOdometryProcessed?.Invoke(deltaTimeSec, linearX, linearY, angularZ);
|
||||
}
|
||||
|
||||
_lastOdometryData = odomData;
|
||||
_lastOdometryTimestampSec = currentTimeSec;
|
||||
}
|
||||
}
|
||||
|
||||
// Event to signal odometry processing
|
||||
public event Action<double, double, double, double>? OnOdometryProcessed;
|
||||
|
||||
private static double GetYawFromPose(Pose p)
|
||||
{
|
||||
var q = p.Orientation;
|
||||
return Math.Atan2(2.0 * (q.W * q.Z + q.X * q.Y), 1.0 - 2.0 * (q.Y * q.Y + q.Z * q.Z));
|
||||
}
|
||||
|
||||
public void Dispose()
|
||||
{
|
||||
lock (_lock)
|
||||
{
|
||||
Stop();
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user