834 lines
34 KiB
C#
834 lines
34 KiB
C#
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);
|
||
}
|