Initial commit
This commit is contained in:
@@ -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);
|
||||
}
|
||||
Reference in New Issue
Block a user