Initial commit

srcs/RobotNet10/Commons/RobotNet10.NavigationTune/Execution/LocalizationAdapter.cs

@@ -0,0 +1,16 @@
+namespace RobotNet10.NavigationTune.Execution;
+
+/// <summary>
+/// Adapter to wrap ILocalization from RobotApp
+/// This will be implemented in the application layer that has access to RobotApp
+/// </summary>
+public class LocalizationAdapter(Func<double> getX, Func<double> getY, Func<double> getTheta) : ILocalizationProvider
+{
+    private readonly Func<double> _getX = getX;
+    private readonly Func<double> _getY = getY;
+    private readonly Func<double> _getTheta = getTheta;
+
+    public double X => _getX();
+    public double Y => _getY();
+    public double Theta => _getTheta(); // radians
+}

srcs/RobotNet10/Commons/RobotNet10.NavigationTune/Execution/TestExecutor.cs

@@ -0,0 +1,833 @@
+using Microsoft.Extensions.Logging;
+using RobotNet10.Common;
+using RobotNet10.NavigationTune.Navigation.Core;
+using RobotNet10.NavigationTune.Services;
+using RobotNet10.NavigationTune.Shared.Interfaces;
+using RobotNet10.NavigationTune.Shared.Models;
+
+namespace RobotNet10.NavigationTune.Execution;
+
+/// <summary>
+/// Navigation phases for state machine
+/// </summary>
+public enum NavigationPhase
+{
+    InitialRotation,    // Rotating to face initial lookahead point
+    PathFollowing,      // Following path using Pure Pursuit
+    FinalRotation,      // Rotating to final goal heading
+    Completed           // Navigation complete
+}
+
+/// <summary>
+/// Test executor implementation
+/// Executes test scenarios using simplified navigation controllers; control loop runs on WatchThread (50Hz).
+/// </summary>
+public class TestExecutor : ITestExecutor
+{
+    private const int ControlLoopFrequency = 50; // 50Hz
+    private const double Dt = 1.0 / ControlLoopFrequency;
+    private static readonly int ControlLoopIntervalMs = 1000 / ControlLoopFrequency;
+
+    private readonly ILocalizationProvider _localization;
+    private readonly IVelocityProvider _velocityProvider;
+    private readonly SafetyMonitor _safetyMonitor;
+    private readonly SafetyConfig _safetyConfig;
+    private readonly ILogger<TestExecutor>? _logger;
+
+    private TestStatus _status = TestStatus.Preparing;
+    private TestScenario? _currentScenario;
+    private NavigationParameterSet? _currentParameters;
+    private ReferencePath? _referencePath;
+    private List<TelemetryData> _telemetryData = new();
+    private CancellationTokenSource? _cancellationTokenSource;
+
+    // Controllers
+    private PID? _movePid;
+    private PID? _rotatePid;
+    private PurePursuitSimplified? _purePursuit;
+    private StanleySimplified? _stanley;
+    private VelocityEstimatorSimplified? _velocityEstimator;
+
+    // State
+    private Pose2D _currentPose;
+    private Twist2D _currentTwist;
+    private double _linearVelocityCommand = 0;
+    private double _angularVelocityCommand = 0;
+    private DateTime _startTime;
+    private double _totalDistanceTraveled = 0;
+    private Pose2D _lastPose;
+    private int _telemetryCreateFrequency = 15;
+    private int _telemetryCreateCount = 0;
+
+    // Navigation phase state machine
+    private NavigationPhase _navigationPhase = NavigationPhase.InitialRotation;
+    private double _targetHeading = 0; // Target heading for rotation phases
+
+    // WatchThread control loop
+    private WatchThread<TestExecutor>? _controlLoopThread;
+    private TaskCompletionSource<TestExecutionResult>? _tcs;
+    private volatile bool _controlLoopDone;
+    private double _oldDistanceToGoal;
+    private int _stallCounter;
+
+    public TestExecutor(
+        ILocalizationProvider localization,
+        IVelocityProvider velocityProvider,
+        SafetyConfig? safetyConfig = null,
+        ILogger<TestExecutor>? logger = null)
+    {
+        _localization = localization;
+        _velocityProvider = velocityProvider;
+        _safetyConfig = safetyConfig ?? new SafetyConfig();
+        _safetyMonitor = new SafetyMonitor(_safetyConfig);
+        _logger = logger;
+    }
+
+    public async Task<TestExecutionResult> ExecuteAsync(
+        TestScenario scenario,
+        NavigationParameterSet parameters,
+        Action<TelemetryData>? onTelemetryUpdate = null,
+        CancellationToken cancellationToken = default,
+        Action<TestExecutionResult>? onComplete = null)
+    {
+        if (_status == TestStatus.Running)
+            throw new InvalidOperationException("Test is already running");
+
+        _currentScenario = scenario;
+        _currentParameters = parameters;
+        _status = TestStatus.Preparing;
+        _telemetryData.Clear();
+        _safetyMonitor.Reset();
+        _controlLoopDone = false;
+        _oldDistanceToGoal = 0;
+        _stallCounter = 0;
+        _navigationPhase = NavigationPhase.InitialRotation;
+
+        // Generate reference path
+        var pathPoints = scenario.GenerateReferencePath();
+        _referencePath = new ReferencePath
+        {
+            Points = pathPoints,
+            TotalLength = pathPoints.Count > 0 ? pathPoints[^1].DistanceFromStart : 0
+        };
+
+        // Initialize controllers
+        _movePid = new PID(parameters.MovePidConfig);
+        _rotatePid = new PID(parameters.RotatePidConfig);
+
+        // Initialize path following controller based on selected type
+        if (parameters.ControllerType == PathFollowingController.Stanley)
+        {
+            _stanley = new StanleySimplified(parameters.StanleyConfig);
+            _stanley.SetPath(pathPoints);
+            Console.WriteLine("Using Stanley Controller for path following");
+        }
+        else
+        {
+            _purePursuit = new PurePursuitSimplified(parameters.PurePursuitConfig, parameters.StanleyConfig);
+            _purePursuit.SetPath(pathPoints);
+            Console.WriteLine("Using Pure Pursuit Controller for path following");
+        }
+
+        _velocityEstimator = new VelocityEstimatorSimplified(
+            parameters.EstimatorConfig,
+            parameters.SignalConfig,
+            parameters.MotorDynamicsConfig
+        );
+
+        // Initialize pose from localization
+        _currentPose = new Pose2D(_localization.X, _localization.Y, _localization.Theta);
+        _lastPose = _currentPose;
+        _startTime = DateTime.UtcNow;
+
+        // Calculate initial target heading for initial rotation phase
+        _targetHeading = CalculateInitialTargetHeading();
+
+        _cancellationTokenSource = CancellationTokenSource.CreateLinkedTokenSource(cancellationToken);
+        _status = TestStatus.Running;
+
+        if (_currentScenario != null)
+        {
+            var goalPose = _currentScenario.GetGoalPose();
+            Console.WriteLine($"Starting test execution towards goal at ({goalPose.X}, {goalPose.Y})");
+        }
+
+        _velocityProvider.SetAcceleration(parameters.NavigationConfig.Acceleration);
+        _velocityProvider.SetDeceleration(parameters.NavigationConfig.Deceleration);
+
+        if (onComplete != null)
+        {
+            // Fire-and-forget: run control loop on WatchThread, return immediately with Running
+            var thread = new WatchThread<TestExecutor>(
+                ControlLoopIntervalMs,
+                () =>
+                {
+                    if (_controlLoopDone) return;
+                    try
+                    {
+                        if (!RunOneControlTick(onTelemetryUpdate))
+                        {
+                            _controlLoopDone = true;
+                            var result = BuildFinalResult();
+                            var t = _controlLoopThread;
+                            _controlLoopThread = null;
+                            onComplete(result);
+                            _ = Task.Run(() =>
+                            {
+                                Thread.Sleep(50);
+                                t?.Stop();
+                            });
+                        }
+                    }
+                    catch (OperationCanceledException)
+                    {
+                        _status = TestStatus.Aborted;
+                        _velocityProvider.SetVelocity(0, 0);
+                        _controlLoopDone = true;
+                        var result = BuildFinalResult();
+                        var t = _controlLoopThread;
+                        _controlLoopThread = null;
+                        onComplete(result);
+                        _ = Task.Run(() => { Thread.Sleep(50); t?.Stop(); });
+                    }
+                    catch (Exception ex)
+                    {
+                        _logger?.LogError(ex, "Control loop error");
+                        _status = TestStatus.Error;
+                        _velocityProvider.SetVelocity(0, 0);
+                        _controlLoopDone = true;
+                        var result = BuildFinalResult();
+                        result.ErrorMessage = ex.Message;
+                        var t = _controlLoopThread;
+                        _controlLoopThread = null;
+                        onComplete(result);
+                        _ = Task.Run(() => { Thread.Sleep(50); t?.Stop(); });
+                    }
+                },
+                _logger);
+            _controlLoopThread = thread;
+            thread.Start();
+            return new TestExecutionResult
+            {
+                TestRunId = Guid.Empty,
+                Status = TestStatus.Running,
+                StartTime = _startTime
+            };
+        }
+
+        // Await mode: run on WatchThread and wait for TCS
+        _tcs = new TaskCompletionSource<TestExecutionResult>(TaskCreationOptions.RunContinuationsAsynchronously);
+        var watchThread = new WatchThread<TestExecutor>(
+            ControlLoopIntervalMs,
+            () =>
+            {
+                if (_controlLoopDone) return;
+                try
+                {
+                    if (!RunOneControlTick(onTelemetryUpdate))
+                    {
+                        _controlLoopDone = true;
+                        var result = BuildFinalResult();
+                        _tcs?.TrySetResult(result);
+                        var t = _controlLoopThread;
+                        _controlLoopThread = null;
+                        _ = Task.Run(() => { Thread.Sleep(50); t?.Stop(); });
+                    }
+                }
+                catch (OperationCanceledException)
+                {
+                    _status = TestStatus.Aborted;
+                    _velocityProvider.SetVelocity(0, 0);
+                    _controlLoopDone = true;
+                    _tcs?.TrySetResult(BuildFinalResult());
+                    var t = _controlLoopThread;
+                    _controlLoopThread = null;
+                    _ = Task.Run(() => { Thread.Sleep(50); t?.Stop(); });
+                }
+                catch (Exception ex)
+                {
+                    _logger?.LogError(ex, "Control loop error");
+                    _status = TestStatus.Error;
+                    _velocityProvider.SetVelocity(0, 0);
+                    _controlLoopDone = true;
+                    var errResult = BuildFinalResult();
+                    errResult.ErrorMessage = ex.Message;
+                    _tcs?.TrySetResult(errResult);
+                    var t = _controlLoopThread;
+                    _controlLoopThread = null;
+                    _ = Task.Run(() => { Thread.Sleep(50); t?.Stop(); });
+                }
+            },
+            _logger);
+        _controlLoopThread = watchThread;
+        watchThread.Start();
+
+        try
+        {
+            return await _tcs.Task;
+        }
+        finally
+        {
+            _controlLoopThread?.Stop();
+            _controlLoopThread = null;
+        }
+    }
+
+    /// <summary>
+    /// One tick of the control loop (50Hz). Returns false when done (goal/cancel/error/stall/safety).
+    /// Implements state machine: InitialRotation -> PathFollowing -> FinalRotation -> Completed
+    /// </summary>
+    private bool RunOneControlTick(Action<TelemetryData>? onTelemetryUpdate)
+    {
+        if (_currentScenario is null) return false;
+        if (_cancellationTokenSource?.Token.IsCancellationRequested == true)
+        {
+            _status = TestStatus.Aborted;
+            _velocityProvider.SetVelocity(0, 0);
+            return false;
+        }
+
+        var goalPose = _currentScenario.GetGoalPose();
+
+        // Read current state
+        _currentPose = new Pose2D(_localization.X, _localization.Y, _localization.Theta);
+        var (linearVel, angularVel) = _velocityProvider.GetActualVelocity();
+
+        var distanceToGoal = Pose2D.Distance(_currentPose, goalPose);
+
+        // Estimate hybrid velocity
+        var vHybrid = _velocityEstimator!.EstimateVelocity(
+            _linearVelocityCommand,
+            linearVel,
+            Dt);
+        var confidence = _velocityEstimator.GetConfidence();
+        _currentTwist = new Twist2D(vHybrid, angularVel);
+
+        // Create telemetry and check safety
+        var commandTwist = new Twist2D(_linearVelocityCommand, _angularVelocityCommand);
+        var telemetry = CreateTelemetryData(_currentPose, _currentTwist, commandTwist, distanceToGoal);
+        if (!_safetyMonitor.CheckSafety(telemetry, _referencePath!))
+        {
+            _status = TestStatus.EmergencyStopped;
+            _velocityProvider.SetVelocity(0, 0);
+            Console.WriteLine("Emergency stop triggered due to safety violation.");
+            return false;
+        }
+
+        if (_telemetryCreateCount++ >= (ControlLoopFrequency / _telemetryCreateFrequency))
+        {
+            _telemetryData.Add(telemetry);
+            _telemetryCreateCount = 0;
+        }
+
+        var reachedRadius = _currentParameters?.NavigationConfig.ReachedRadius ?? 0.015f;
+
+        // State machine logic
+        switch (_navigationPhase)
+        {
+            case NavigationPhase.InitialRotation:
+                {
+                    // Check if initial rotation is needed
+                    double headingError = NavigationMath.NormalizeAngle(_targetHeading - _currentPose.Theta);
+                    double initialRotationThresholdRad = (_currentParameters?.NavigationConfig.InitialRotationThreshold ?? 5.0f) * Math.PI / 180.0;
+
+                    if (Math.Abs(headingError) < initialRotationThresholdRad)
+                    {
+                        // Heading is good enough, skip to path following
+                        Console.WriteLine($"Initial heading acceptable ({Math.Abs(headingError) * 180 / Math.PI:F2}°), skipping initial rotation");
+                        _navigationPhase = NavigationPhase.PathFollowing;
+                        _rotatePid!.Reset();
+                    }
+                    else
+                    {
+                        // Perform initial rotation
+                        if (PerformInitialRotation())
+                        {
+                            // Rotation complete, move to path following
+                            _navigationPhase = NavigationPhase.PathFollowing;
+                            _rotatePid!.Reset();
+                        }
+                    }
+
+                    onTelemetryUpdate?.Invoke(telemetry);
+                    _totalDistanceTraveled += (float)Pose2D.Distance(_lastPose, _currentPose);
+                    _lastPose = _currentPose;
+                    return true;
+                }
+
+            case NavigationPhase.PathFollowing:
+                {
+                    if (distanceToGoal <= reachedRadius)
+                    {
+                        // Position reached, move to final rotation
+                        _targetHeading = CalculateFinalTargetHeading();
+                        _navigationPhase = NavigationPhase.FinalRotation;
+                        _rotatePid!.Reset();
+                        Console.WriteLine($"Position reached, starting final rotation to {_targetHeading * 180 / Math.PI:F2}°. Current pose: [{_currentPose.X} - {_currentPose.Y}], Goal: [{goalPose.X} - {goalPose.Y}], Distance: {distanceToGoal}");
+
+                        onTelemetryUpdate?.Invoke(telemetry);
+                        _totalDistanceTraveled += (float)Pose2D.Distance(_lastPose, _currentPose);
+                        _lastPose = _currentPose;
+                        return true;
+                    }
+
+                    // Stall detection (only during path following)
+                    if (_oldDistanceToGoal >= distanceToGoal) _stallCounter = 0;
+                    else if (distanceToGoal < 0.3)
+                    {
+                        _stallCounter++;
+                        if (_stallCounter >= ControlLoopFrequency * 0.1)
+                        {
+                            _status = TestStatus.Error;
+                            _velocityProvider.SetVelocity(0, 0);
+                            Console.WriteLine("Test execution stalled: no progress towards goal.");
+
+                            _targetHeading = CalculateFinalTargetHeading();
+                            _navigationPhase = NavigationPhase.FinalRotation;
+                            _rotatePid!.Reset();
+                            return true;
+                        }
+                    }
+                    _oldDistanceToGoal = distanceToGoal;
+
+                    // Calculate max linear velocity with PID deceleration
+                    double maxLinearVel;
+                    if (distanceToGoal > 5.0)
+                        maxLinearVel = _currentParameters!.NavigationConfig.MaxLinearVelocity;
+                    else
+                    {
+                        var pidOutput = _movePid!.PID_step(distanceToGoal,
+                            _currentParameters!.NavigationConfig.MaxLinearVelocity,
+                            _currentParameters.NavigationConfig.MinLinearVelocity,
+                            Dt);
+                        maxLinearVel = pidOutput;
+                    }
+
+                    // Path following using selected controller
+                    double linearVelCmd, angularVelCmd;
+
+                    if (_currentParameters!.ControllerType == PathFollowingController.Stanley)
+                    {
+                        // Stanley Controller
+                        (linearVelCmd, angularVelCmd) = _stanley!.CalculateVelocity(
+                            _currentPose.X,
+                            _currentPose.Y,
+                            _currentPose.Theta,
+                            vHybrid,
+                            maxLinearVel);
+                    }
+                    else
+                    {
+                        // Pure Pursuit Controller
+                        (linearVelCmd, angularVelCmd) = _purePursuit!.CalculateAngularVelocity(
+                            _currentPose.X,
+                            _currentPose.Y,
+                            _currentPose.Theta,
+                            vHybrid,
+                            maxLinearVel);
+                    }
+
+                    var linearVelSign = Math.Sign(linearVelCmd);
+                    _linearVelocityCommand = (float)Math.Clamp(
+                        Math.Abs(linearVelCmd),
+                        _currentParameters!.NavigationConfig.MinLinearVelocity,
+                        _currentParameters.NavigationConfig.MaxLinearVelocity);
+                    _angularVelocityCommand = (float)Math.Clamp(
+                        angularVelCmd,
+                        -_currentParameters.NavigationConfig.MaxAngularVelocity,
+                        _currentParameters.NavigationConfig.MaxAngularVelocity);
+
+                    _velocityProvider.SetVelocity(linearVelSign * _linearVelocityCommand, _angularVelocityCommand);
+
+                    onTelemetryUpdate?.Invoke(telemetry);
+                    _totalDistanceTraveled += (float)Pose2D.Distance(_lastPose, _currentPose);
+                    _lastPose = _currentPose;
+                    return true;
+                }
+
+            case NavigationPhase.FinalRotation:
+                {
+                    // Perform final rotation to goal heading
+                    if (PerformFinalRotation())
+                    {
+                        // Final rotation complete, navigation done
+                        _navigationPhase = NavigationPhase.Completed;
+                        _status = TestStatus.Completed;
+                        _velocityProvider.SetVelocity(0, 0);
+                        Console.WriteLine("Navigation complete!");
+                        return false;
+                    }
+
+                    onTelemetryUpdate?.Invoke(telemetry);
+                    _totalDistanceTraveled += (float)Pose2D.Distance(_lastPose, _currentPose);
+                    _lastPose = _currentPose;
+                    return true;
+                }
+
+            case NavigationPhase.Completed:
+                {
+                    _status = TestStatus.Completed;
+                    _velocityProvider.SetVelocity(0, 0);
+                    return false;
+                }
+
+            default:
+                return false;
+        }
+    }
+
+    private TestExecutionResult BuildFinalResult()
+    {
+        var endTime = DateTime.UtcNow;
+        var duration = (endTime - _startTime).TotalSeconds;
+        return new TestExecutionResult
+        {
+            TestRunId = Guid.NewGuid(),
+            Status = _status,
+            TelemetryData = _telemetryData,
+            SafetyViolations = _safetyMonitor.GetViolations(),
+            StartTime = _startTime,
+            EndTime = endTime,
+            Duration = duration
+        };
+    }
+
+    private TelemetryData CreateTelemetryData(Pose2D pose, Twist2D twist, Twist2D commandTwist, double distanceToGoal)
+    {
+        var closestPoint = _referencePath!.GetClosestPoint(pose);
+        var cte = closestPoint != null
+            ? Math.Sqrt(Math.Pow(pose.X - closestPoint.X, 2) + Math.Pow(pose.Y - closestPoint.Y, 2))
+            : 0;
+
+        // Calculate heading error from Direction
+        // Reference heading is calculated from direction of movement (tangent to path)
+        double headingError = 0;
+        double refTheta = 0;
+        if (closestPoint != null)
+        {
+            // Find next point to determine direction
+            int closestIndex = _referencePath.Points.IndexOf(closestPoint);
+            if (closestIndex >= 0 && closestIndex < _referencePath.Points.Count - 1)
+            {
+                var nextPoint = _referencePath.Points[closestIndex + 1];
+                double dx = nextPoint.X - closestPoint.X;
+                double dy = nextPoint.Y - closestPoint.Y;
+                refTheta = Math.Atan2(dy, dx);
+            }
+            else if (closestIndex > 0)
+            {
+                // Use previous point
+                var prevPoint = _referencePath.Points[closestIndex - 1];
+                double dx = closestPoint.X - prevPoint.X;
+                double dy = closestPoint.Y - prevPoint.Y;
+                refTheta = Math.Atan2(dy, dx);
+            }
+            // Adjust for backward direction
+            if (closestPoint.Direction == RobotDirection.BACKWARD)
+            {
+                refTheta = NavigationMath.NormalizeAngle(refTheta + Math.PI);
+            }
+            refTheta = NavigationMath.NormalizeAngle(refTheta);
+            headingError = NavigationMath.NormalizeAngle(pose.Theta - refTheta);
+        }
+
+        // Lookahead distance only applicable for Pure Pursuit
+        var lookaheadDistance = 0.0;
+        if (_currentParameters?.ControllerType == PathFollowingController.PurePursuit)
+        {
+            lookaheadDistance = _purePursuit?.GetCurrentLookahead(
+                twist.Linear,
+                _velocityEstimator?.GetConfidence() ?? 1.0
+            ) ?? 0;
+        }
+
+        var refPose = closestPoint != null
+            ? new Pose2D(closestPoint.X, closestPoint.Y, refTheta)
+            : pose;
+
+        return new TelemetryData
+        {
+            TimestampMs = DateTimeOffset.UtcNow.ToUnixTimeMilliseconds(),
+            RobotPose = pose,
+            RobotTwist = twist,
+            CommandTwist = commandTwist,
+            ReferencePose = refPose,
+            CrossTrackError = cte,
+            HeadingError = headingError,
+            LookaheadDistance = lookaheadDistance,
+            ModelConfidence = _velocityEstimator?.GetConfidence() ?? 1.0,
+            DistanceToGoal = distanceToGoal,
+            Phase = DetermineCurrentPhase(distanceToGoal)
+        };
+    }
+
+    private TelemetryPhase DetermineCurrentPhase(double distanceToGoal)
+    {
+        return _navigationPhase switch
+        {
+            NavigationPhase.InitialRotation => TelemetryPhase.InitialRotation,
+            NavigationPhase.PathFollowing => DeterminePathFollowingSubPhase(distanceToGoal),
+            NavigationPhase.FinalRotation => TelemetryPhase.FinalRotation,
+            NavigationPhase.Completed => TelemetryPhase.Completed,
+            _ => TelemetryPhase.PathFollowing
+        };
+    }
+
+    private TelemetryPhase DeterminePathFollowingSubPhase(double distanceToGoal)
+    {
+        if (_purePursuit != null && _purePursuit.IsInFinalApproach)
+            return TelemetryPhase.FinalApproach;
+        if (_stanley != null && _currentParameters != null &&
+            distanceToGoal < _currentParameters.StanleyConfig.GoalApproachDistance)
+            return TelemetryPhase.FinalApproach;
+        return TelemetryPhase.PathFollowing;
+    }
+
+
+    public void Pause()
+    {
+        if (_status == TestStatus.Running)
+        {
+            _status = TestStatus.Paused;
+            _velocityProvider.SetVelocity(0, 0);
+        }
+    }
+
+    public void Resume()
+    {
+        if (_status == TestStatus.Paused)
+        {
+            _status = TestStatus.Running;
+        }
+    }
+
+    public void Stop()
+    {
+        _cancellationTokenSource?.Cancel();
+        _velocityProvider.SetVelocity(0, 0);
+        _status = TestStatus.Aborted;
+        Console.WriteLine("Test execution stopped by user.");
+    }
+
+    public void EmergencyStop()
+    {
+        _cancellationTokenSource?.Cancel();
+        _velocityProvider.SetVelocity(0, 0);
+        _status = TestStatus.EmergencyStopped;
+    }
+
+    public TestStatus GetStatus() => _status;
+
+    public TestProgress GetProgress()
+    {
+        if (_currentScenario == null || _referencePath == null)
+            return new TestProgress();
+
+        var goalPose = _currentScenario.GetGoalPose();
+        var distanceToGoal = Pose2D.Distance(_currentPose, goalPose);
+        var totalDistance = _referencePath.TotalLength;
+        var progressPercent = totalDistance > 0
+            ? 1.0 - (distanceToGoal / totalDistance)
+            : 0;
+
+        var elapsedTime = (DateTime.UtcNow - _startTime).TotalSeconds;
+        var estimatedTimeRemaining = progressPercent > 0.01f
+            ? elapsedTime / progressPercent - elapsedTime
+            : 0;
+
+        return new TestProgress
+        {
+            ProgressPercent = Math.Clamp(progressPercent, 0, 1),
+            DistanceTraveled = _totalDistanceTraveled,
+            DistanceToGoal = distanceToGoal,
+            ElapsedTime = elapsedTime,
+            EstimatedTimeRemaining = estimatedTimeRemaining
+        };
+    }
+
+    /// <summary>
+    /// Calculate initial target heading to first lookahead point
+    /// Returns the angle from current position to the lookahead point on the path
+    /// </summary>
+    private double CalculateInitialTargetHeading()
+    {
+        if (_referencePath == null || _referencePath.Points.Count < 2)
+            return _currentPose.Theta;
+
+        // Get closest point on path
+        var closestPoint = _referencePath.GetClosestPoint(_currentPose);
+        if (closestPoint == null)
+            return _currentPose.Theta;
+
+        int closestIndex = _referencePath.Points.IndexOf(closestPoint);
+        if (closestIndex < 0)
+            return _currentPose.Theta;
+
+        // Calculate a simple lookahead distance (use minimum lookahead)
+        double lookaheadDistance = (_currentParameters?.PurePursuitConfig.LookaheadMin + _currentParameters?.PurePursuitConfig.LookaheadMax ) / 2 ?? 1;
+
+        // Find target point at lookahead distance
+        PathPoint? targetPoint = FindTargetPointAtDistance(closestIndex, lookaheadDistance);
+        if (targetPoint == null)
+            targetPoint = _referencePath.Points[^1]; // Use goal if no target found
+
+        // Calculate angle to target point
+        double dx = targetPoint.X - _currentPose.X;
+        double dy = targetPoint.Y - _currentPose.Y;
+        double heading = Math.Atan2(dy, dx);
+
+        // Adjust for backward direction
+        if (targetPoint.Direction == RobotDirection.BACKWARD)
+        {
+            heading = NavigationMath.NormalizeAngle(heading + Math.PI);
+        }
+
+        return NavigationMath.NormalizeAngle(heading);
+    }
+
+    /// <summary>
+    /// Calculate final target heading at goal (from second-to-last waypoint to goal)
+    /// Similar to PurePursuitSimplified.CalculateGoalHeading()
+    /// </summary>
+    private double CalculateFinalTargetHeading()
+    {
+        if (_referencePath == null || _referencePath.Points.Count < 2)
+            return _currentPose.Theta;
+
+        var goalPoint = _referencePath.Points[^1];
+        var secondToLast = _referencePath.Points[^2];
+
+        double dx = goalPoint.X - secondToLast.X;
+        double dy = goalPoint.Y - secondToLast.Y;
+        double heading = Math.Atan2(dy, dx);
+
+        // Adjust for backward direction
+        if (goalPoint.Direction == RobotDirection.BACKWARD)
+        {
+            heading = NavigationMath.NormalizeAngle(heading + Math.PI);
+        }
+
+        return NavigationMath.NormalizeAngle(heading);
+    }
+
+    /// <summary>
+    /// Find target point at lookahead distance from current position
+    /// Simplified version for initial rotation calculation
+    /// </summary>
+    private PathPoint? FindTargetPointAtDistance(int startIndex, double lookaheadDistance)
+    {
+        if (_referencePath == null || startIndex >= _referencePath.Points.Count - 1)
+            return null;
+
+        double accumulatedDistance = 0;
+
+        for (int i = startIndex; i < _referencePath.Points.Count - 1; i++)
+        {
+            double dx = _referencePath.Points[i + 1].X - _referencePath.Points[i].X;
+            double dy = _referencePath.Points[i + 1].Y - _referencePath.Points[i].Y;
+            double segmentLength = Math.Sqrt(dx * dx + dy * dy);
+
+            if (accumulatedDistance + segmentLength >= lookaheadDistance)
+            {
+                // Interpolate within this segment
+                double t = (lookaheadDistance - accumulatedDistance) / segmentLength;
+                return new PathPoint
+                {
+                    X = _referencePath.Points[i].X + t * (_referencePath.Points[i + 1].X - _referencePath.Points[i].X),
+                    Y = _referencePath.Points[i].Y + t * (_referencePath.Points[i + 1].Y - _referencePath.Points[i].Y),
+                    Direction = _referencePath.Points[i].Direction
+                };
+            }
+
+            accumulatedDistance += segmentLength;
+        }
+
+        return null;
+    }
+
+    /// <summary>
+    /// Perform initial rotation to face the initial lookahead point
+    /// Returns true if rotation is complete, false if still rotating
+    /// </summary>
+    private bool PerformInitialRotation()
+    {
+        // Calculate heading error
+        double headingError = NavigationMath.NormalizeAngle(_targetHeading - _currentPose.Theta);
+
+        // Check if rotation is complete (within heading tolerance)
+        double headingToleranceRad = (_currentParameters?.PurePursuitConfig.HeadingTolerance ?? 3.0f) * Math.PI / 180.0;
+        if (Math.Abs(headingError) < headingToleranceRad)
+        {
+            _velocityProvider.SetVelocity(0, 0);
+            Console.WriteLine($"Initial rotation complete. Heading error: {Math.Abs(headingError) * 180 / Math.PI:F2}<7D>");
+            return true;
+        }
+
+        // Use PID to control rotation
+        double angularVelCmd = _rotatePid!.PID_step(
+            headingError,
+            _currentParameters!.NavigationConfig.RotateAngularVelocity,
+            -_currentParameters!.NavigationConfig.RotateAngularVelocity,
+            Dt);
+
+        _angularVelocityCommand = (float)angularVelCmd;
+        _velocityProvider.SetVelocity(0, _angularVelocityCommand);
+
+        Console.WriteLine($"Initial rotation: HeadingError={headingError * 180 / Math.PI:F2}<7D>, AngVel={_angularVelocityCommand:F3}");
+        return false;
+    }
+
+    /// <summary>
+    /// Perform final rotation to face the goal heading
+    /// Returns true if rotation is complete, false if still rotating
+    /// </summary>
+    private bool PerformFinalRotation()
+    {
+        // Calculate heading error
+        double headingError = NavigationMath.NormalizeAngle(_targetHeading - _currentPose.Theta);
+
+        // Check if rotation is complete (within heading tolerance)
+        double headingToleranceRad = (_currentParameters?.PurePursuitConfig.HeadingTolerance ?? 3.0f) * Math.PI / 180.0;
+        if (Math.Abs(headingError) < headingToleranceRad)
+        {
+            _velocityProvider.SetVelocity(0, 0);
+            Console.WriteLine($"Final rotation complete. Heading error: {Math.Abs(headingError) * 180 / Math.PI:F2}<7D>");
+            return true;
+        }
+
+        // Use PID to control rotation
+        double angularVelCmd = _rotatePid!.PID_step(
+            headingError,
+            _currentParameters!.NavigationConfig.RotateAngularVelocity,
+            -_currentParameters!.NavigationConfig.RotateAngularVelocity,
+            Dt);
+
+        _angularVelocityCommand = (float)angularVelCmd;
+        _velocityProvider.SetVelocity(0, _angularVelocityCommand);
+
+        Console.WriteLine($"Final rotation: HeadingError={headingError * 180 / Math.PI:F2}<7D>, AngVel={_angularVelocityCommand:F3}");
+        return false;
+    }
+}
+
+/// <summary>
+/// Interface for localization provider (abstraction for ILocalization)
+/// </summary>
+public interface ILocalizationProvider
+{
+    double X { get; }
+    double Y { get; }
+    double Theta { get; } // radians
+}
+
+/// <summary>
+/// Interface for velocity provider (abstraction for IVelocityController)
+/// </summary>
+public interface IVelocityProvider
+{
+    (double Linear, double Angular) GetActualVelocity();
+    void SetVelocity(double linearVel, double angularVel);
+    double GetModelConfidence();
+    void SetAcceleration(double acc);
+    void SetDeceleration(double dec);
+}

srcs/RobotNet10/Commons/RobotNet10.NavigationTune/Execution/TuningNavigation.cs

@@ -0,0 +1,257 @@
+using System.Threading.Channels;
+using Microsoft.AspNetCore.SignalR;
+using RobotNet10.NavigationTune.Hubs;
+using RobotNet10.NavigationTune.Shared.Hubs;
+using RobotNet10.NavigationTune.Shared.Interfaces;
+using RobotNet10.NavigationTune.Shared.Models;
+using RobotNet10.NavigationTune.Services;
+
+namespace RobotNet10.NavigationTune.Execution;
+
+/// <summary>
+/// Tuning Navigation wrapper implementation.
+/// Navigation algorithm runs in its own loop; telemetry is published on a dedicated publisher loop.
+/// </summary>
+public class TuningNavigation(
+    ITestExecutor testExecutor,
+    IMetricsCalculator metricsCalculator,
+    IHubContext<TuningHub>? hubContext = null) : ITuningNavigation
+{
+    private readonly ITestExecutor _testExecutor = testExecutor;
+    private readonly IMetricsCalculator _metricsCalculator = metricsCalculator;
+    private readonly IHubContext<TuningHub>? _hubContext = hubContext;
+
+    private TestScenario? _currentScenario;
+    private NavigationParameterSet? _currentParameters;
+    private TestExecutionResult? _currentResult;
+    private Guid _currentTestRunId; // Set at start of run so telemetry can be pushed during execution
+    private Task<TestExecutionResult>? _executionTask;
+    private Task? _telemetryPublisherTask;
+    private Channel<TelemetryData>? _telemetryChannel;
+
+    public event Action<TelemetryData>? OnTelemetryUpdate;
+    public event Action<SafetyViolation>? OnSafetyViolation;
+    public event Action<TestStatus>? OnStatusChanged;
+
+    // Safety violation handler
+    private void HandleSafetyViolation(SafetyViolation violation)
+    {
+        OnSafetyViolation?.Invoke(violation);
+        PublishSafetyEvent(violation);
+    }
+
+    public bool IsTestRunning => _executionTask != null && !_executionTask.IsCompleted;
+
+    public async Task<TestExecutionResult> ExecuteTestAsync(
+        TestScenario scenario,
+        NavigationParameterSet parameters,
+        Guid? testRunId = null,
+        CancellationToken cancellationToken = default,
+        Action<TestExecutionResult>? onComplete = null)
+    {
+        if (IsTestRunning)
+            throw new InvalidOperationException("Test is already running");
+
+        _currentScenario = scenario;
+        _currentParameters = parameters;
+        _currentTestRunId = testRunId ?? Guid.Empty;
+
+        // Dedicated channel for telemetry: navigation loop only enqueues (no I/O in algorithm thread)
+        _telemetryChannel = Channel.CreateUnbounded<TelemetryData>(new UnboundedChannelOptions
+        {
+            SingleReader = true,
+            SingleWriter = true
+        });
+
+        // Start dedicated publisher loop (runs independently from navigation algorithm)
+        _telemetryPublisherTask = RunTelemetryPublisherLoopAsync(cancellationToken);
+
+        if (onComplete != null)
+        {
+            // Fire-and-forget: control loop runs on WatchThread; return immediately with Running
+            _ = _testExecutor.ExecuteAsync(
+                scenario,
+                parameters,
+                telemetry => _telemetryChannel.Writer.TryWrite(telemetry),
+                cancellationToken,
+                onComplete: result =>
+                {
+                    FinishExecution(result);
+                    _telemetryChannel?.Writer.Complete();
+                    _executionTask = null;
+                    onComplete(result);
+                });
+            return new TestExecutionResult
+            {
+                TestRunId = _currentTestRunId,
+                Status = TestStatus.Running,
+                StartTime = DateTime.UtcNow
+            };
+        }
+
+        // Await mode
+        _executionTask = _testExecutor.ExecuteAsync(
+            scenario,
+            parameters,
+            telemetry => _telemetryChannel.Writer.TryWrite(telemetry),
+            cancellationToken
+        );
+
+        try
+        {
+            _currentResult = await _executionTask;
+            FinishExecution(_currentResult);
+            return _currentResult;
+        }
+        finally
+        {
+            _telemetryChannel?.Writer.Complete();
+            if (_telemetryPublisherTask != null)
+            {
+                try
+                {
+                    await _telemetryPublisherTask;
+                }
+                catch (OperationCanceledException) { }
+            }
+            _telemetryPublisherTask = null;
+            _telemetryChannel = null;
+            _executionTask = null;
+        }
+    }
+
+    private void FinishExecution(TestExecutionResult result)
+    {
+        if (_currentTestRunId != Guid.Empty)
+            result.TestRunId = _currentTestRunId;
+
+        if (result.TelemetryData.Count > 0 && _currentScenario != null)
+        {
+            var pathPoints = _currentScenario.GenerateReferencePath();
+            var referencePath = new ReferencePath
+            {
+                Points = pathPoints,
+                TotalLength = pathPoints.Count > 0 ? pathPoints[^1].DistanceFromStart : 0
+            };
+            result.Metrics = _metricsCalculator.CalculateMetrics(result.TelemetryData, referencePath);
+        }
+
+        OnStatusChanged?.Invoke(result.Status);
+        PublishTestStatus(result.Status, result.TestRunId);
+    }
+
+    /// <summary>
+    /// Dedicated loop for publishing telemetry (and raising events). Runs independently from navigation algorithm.
+    /// </summary>
+    private async Task RunTelemetryPublisherLoopAsync(CancellationToken cancellationToken)
+    {
+        if (_telemetryChannel == null) return;
+
+        var reader = _telemetryChannel.Reader;
+        try
+        {
+            await foreach (var telemetry in reader.ReadAllAsync(cancellationToken))
+            {
+                OnTelemetryUpdate?.Invoke(telemetry);
+                PublishTelemetryUpdate(telemetry);
+            }
+        }
+        catch (OperationCanceledException) { }
+    }
+
+    public TestProgress GetProgress()
+    {
+        return _testExecutor.GetProgress();
+    }
+
+    public void Pause()
+    {
+        _testExecutor.Pause();
+        OnStatusChanged?.Invoke(TestStatus.Paused);
+        PublishTestStatus(TestStatus.Paused, _currentTestRunId != Guid.Empty ? _currentTestRunId : _currentResult?.TestRunId ?? Guid.Empty);
+    }
+
+    public void Resume()
+    {
+        _testExecutor.Resume();
+        OnStatusChanged?.Invoke(TestStatus.Running);
+        PublishTestStatus(TestStatus.Running, _currentTestRunId != Guid.Empty ? _currentTestRunId : _currentResult?.TestRunId ?? Guid.Empty);
+    }
+
+    public void Stop()
+    {
+        _testExecutor.Stop();
+        OnStatusChanged?.Invoke(TestStatus.Aborted);
+        PublishTestStatus(TestStatus.Aborted, _currentTestRunId != Guid.Empty ? _currentTestRunId : _currentResult?.TestRunId ?? Guid.Empty);
+    }
+
+    public void EmergencyStop()
+    {
+        _testExecutor.EmergencyStop();
+        OnStatusChanged?.Invoke(TestStatus.EmergencyStopped);
+        PublishTestStatus(TestStatus.EmergencyStopped, _currentTestRunId != Guid.Empty ? _currentTestRunId : _currentResult?.TestRunId ?? Guid.Empty);
+    }
+
+    private void PublishTelemetryUpdate(TelemetryData telemetry)
+    {
+        if (_hubContext == null) return;
+
+        var groupId = _currentTestRunId != Guid.Empty ? _currentTestRunId : _currentResult?.TestRunId ?? Guid.Empty;
+        if (groupId == Guid.Empty) return;
+
+        var dto = new TelemetryUpdateDto
+        {
+            TimestampMs = telemetry.TimestampMs,
+            X = telemetry.RobotPose.X,
+            Y = telemetry.RobotPose.Y,
+            Theta = telemetry.RobotPose.Theta,
+            LinearVelocity = telemetry.RobotTwist.Linear,
+            AngularVelocity = telemetry.RobotTwist.Angular,
+            CrossTrackError = telemetry.CrossTrackError,
+            HeadingError = telemetry.HeadingError,
+            DistanceToGoal = telemetry.DistanceToGoal,
+            CommandedLinearVelocity = telemetry.CommandTwist.Linear,
+            CommandedAngularVelocity = telemetry.CommandTwist.Angular
+        };
+
+        _ = _hubContext.Clients.Group($"test_{groupId}")
+            .SendAsync("ReceiveTelemetry", dto);
+    }
+
+    private void PublishTestStatus(TestStatus status, Guid testRunId)
+    {
+        if (_hubContext == null) return;
+
+        var progress = GetProgress();
+        var dto = new TestStatusUpdateDto
+        {
+            TestRunId = testRunId,
+            Status = status,
+            ProgressPercent = progress.ProgressPercent,
+            Message = status.ToString()
+        };
+
+        _ = _hubContext.Clients.Group($"test_{testRunId}")
+            .SendAsync("ReceiveTestStatus", dto);
+    }
+
+    private void PublishSafetyEvent(SafetyViolation violation)
+    {
+        if (_hubContext == null) return;
+
+        var groupId = _currentTestRunId != Guid.Empty ? _currentTestRunId : _currentResult?.TestRunId ?? Guid.Empty;
+        if (groupId == Guid.Empty) return;
+
+        var dto = new SafetyEventDto
+        {
+            TestRunId = groupId,
+            Type = violation.Type,
+            Severity = violation.Severity,
+            Message = violation.Message,
+            Timestamp = violation.Timestamp
+        };
+
+        _ = _hubContext.Clients.Group($"test_{groupId}")
+            .SendAsync("ReceiveSafetyEvent", dto);
+    }
+}

srcs/RobotNet10/Commons/RobotNet10.NavigationTune/Execution/VelocityControllerAdapter.cs

@@ -0,0 +1,44 @@
+namespace RobotNet10.NavigationTune.Execution;
+
+/// <summary>
+/// Adapter to wrap IVelocityController from RobotApp
+/// This will be implemented in the application layer that has access to RobotApp
+/// </summary>
+public class VelocityControllerAdapter(
+    Func<(double Linear, double Angular)> getActualVelocity,
+    Action<double, double> setVelocity,
+    Func<double> getModelConfidence,
+    Action<double>? setAcceleration = null,
+    Action<double>? setDeceleration = null) : IVelocityProvider
+{
+    private readonly Func<(double Linear, double Angular)> _getActualVelocity = getActualVelocity;
+    private readonly Action<double, double> _setVelocity = setVelocity;
+    private readonly Func<double> _getModelConfidence = getModelConfidence;
+    private readonly Action<double>? _setAcceleration = setAcceleration;
+    private readonly Action<double>? _setDeceleration = setDeceleration;
+
+    public (double Linear, double Angular) GetActualVelocity()
+    {
+        return _getActualVelocity();
+    }
+
+    public void SetVelocity(double linearVel, double angularVel)
+    {
+        _setVelocity(linearVel, angularVel);
+    }
+
+    public double GetModelConfidence()
+    {
+        return _getModelConfidence();
+    }
+
+    public void SetAcceleration(double acc)
+    {
+        _setAcceleration?.Invoke(acc);
+    }
+
+    public void SetDeceleration(double dec)
+    {
+        _setDeceleration?.Invoke(dec);
+    }
+}