794 lines
29 KiB
C#
794 lines
29 KiB
C#
using System;
|
||
using System.Diagnostics;
|
||
using MultiWheelC.Control.Abstractions;
|
||
using MultiWheelC.Control.Allocation;
|
||
using MultiWheelC.Trajectory;
|
||
using MyParking.Shared;
|
||
|
||
namespace MultiWheelC.Control.Execution
|
||
{
|
||
// 表示公共轨迹跟踪核心单周期的计算结果，不包含底盘发送结果。
|
||
public enum PathTrackingCycleResult
|
||
{
|
||
Inactive = 0,
|
||
CommandGenerated = 1,
|
||
Completed = 2,
|
||
Faulted = 3
|
||
}
|
||
|
||
// 保存公共核心生成的GCP命令、轨迹投影和分阶段计算耗时。
|
||
public readonly struct PathTrackingCycleOutput
|
||
{
|
||
public PathTrackingCycleOutput(
|
||
PathTrackingCycleResult result,
|
||
GcpMotionCommand? command,
|
||
TrajectoryProjection? projection,
|
||
double projectionMilliseconds,
|
||
double controllerComputeMilliseconds)
|
||
{
|
||
Result = result;
|
||
Command = command;
|
||
Projection = projection;
|
||
ProjectionMilliseconds = projectionMilliseconds;
|
||
ControllerComputeMilliseconds =
|
||
controllerComputeMilliseconds;
|
||
}
|
||
|
||
public PathTrackingCycleResult Result { get; }
|
||
|
||
public GcpMotionCommand? Command { get; }
|
||
|
||
public TrajectoryProjection? Projection { get; }
|
||
|
||
public double ProjectionMilliseconds { get; }
|
||
|
||
public double ControllerComputeMilliseconds { get; }
|
||
}
|
||
|
||
// 统一处理单车和虚拟车队共有的轨迹投影、速度整形及GCP命令生成。
|
||
public sealed class PathTrackingCore
|
||
{
|
||
private const double ZeroReferenceSpeedToleranceMetersPerSecond =
|
||
1e-6;
|
||
private const double StartupRegionMeters = 0.02;
|
||
private const double StartupPreviewDistanceMeters = 0.05;
|
||
private const double MaximumStartupSpeedMetersPerSecond = 0.08;
|
||
private const double ProjectionBackwardSearchDistanceMeters =
|
||
0.10;
|
||
private const double ProjectionForwardSearchDistanceMeters =
|
||
1.00;
|
||
|
||
private readonly ILateralController _lateralController;
|
||
private readonly ILongitudinalController _longitudinalController;
|
||
private readonly GcpCommandAllocator _gcpAllocator;
|
||
private readonly double _motionDirectionInBodyRadians;
|
||
|
||
private Trajectory2D _trajectory;
|
||
private double _terminalTravelDirection = 1.0;
|
||
|
||
public PathTrackingCore(
|
||
ILateralController lateralController,
|
||
ILongitudinalController longitudinalController,
|
||
GcpCommandAllocator gcpAllocator,
|
||
double finishDistanceMeters = 0.04,
|
||
double finishSpeedMetersPerSecond = 0.02,
|
||
double finishHeadingToleranceRadians =
|
||
3.0 * Math.PI / 180.0,
|
||
double maximumDistanceToTrajectoryMeters = 0.30,
|
||
double terminalBrakingPreviewMeters = 0.02,
|
||
double terminalApproachDistanceMeters = 0.10,
|
||
double terminalApproachGainPerSecond = 0.8,
|
||
double maximumTerminalApproachSpeedMetersPerSecond = 0.05,
|
||
double curvaturePreviewSeconds = 0.20,
|
||
double maximumCurvaturePreviewMeters = 0.12,
|
||
double motionDirectionInBodyRadians = 0.0)
|
||
{
|
||
_lateralController = lateralController ??
|
||
throw new ArgumentNullException(
|
||
nameof(lateralController));
|
||
_longitudinalController = longitudinalController ??
|
||
throw new ArgumentNullException(
|
||
nameof(longitudinalController));
|
||
_gcpAllocator = gcpAllocator ??
|
||
throw new ArgumentNullException(
|
||
nameof(gcpAllocator));
|
||
|
||
NumericGuard.EnsureFinitePositive(
|
||
finishDistanceMeters,
|
||
nameof(finishDistanceMeters));
|
||
NumericGuard.EnsureFiniteNonNegative(
|
||
finishSpeedMetersPerSecond,
|
||
nameof(finishSpeedMetersPerSecond));
|
||
NumericGuard.EnsureFinitePositive(
|
||
finishHeadingToleranceRadians,
|
||
nameof(finishHeadingToleranceRadians));
|
||
NumericGuard.EnsureFinitePositive(
|
||
maximumDistanceToTrajectoryMeters,
|
||
nameof(maximumDistanceToTrajectoryMeters));
|
||
NumericGuard.EnsureFiniteNonNegative(
|
||
terminalBrakingPreviewMeters,
|
||
nameof(terminalBrakingPreviewMeters));
|
||
NumericGuard.EnsureFinitePositive(
|
||
terminalApproachDistanceMeters,
|
||
nameof(terminalApproachDistanceMeters));
|
||
NumericGuard.EnsureFinitePositive(
|
||
terminalApproachGainPerSecond,
|
||
nameof(terminalApproachGainPerSecond));
|
||
NumericGuard.EnsureFinitePositive(
|
||
maximumTerminalApproachSpeedMetersPerSecond,
|
||
nameof(maximumTerminalApproachSpeedMetersPerSecond));
|
||
NumericGuard.EnsureFiniteNonNegative(
|
||
curvaturePreviewSeconds,
|
||
nameof(curvaturePreviewSeconds));
|
||
NumericGuard.EnsureFiniteNonNegative(
|
||
maximumCurvaturePreviewMeters,
|
||
nameof(maximumCurvaturePreviewMeters));
|
||
NumericGuard.EnsureFinite(
|
||
motionDirectionInBodyRadians,
|
||
nameof(motionDirectionInBodyRadians));
|
||
|
||
if (terminalApproachDistanceMeters <=
|
||
finishDistanceMeters)
|
||
{
|
||
throw new ArgumentOutOfRangeException(
|
||
nameof(terminalApproachDistanceMeters),
|
||
"终点单向逼近范围必须大于终点位置容差。");
|
||
}
|
||
|
||
FinishDistanceMeters = finishDistanceMeters;
|
||
FinishSpeedMetersPerSecond =
|
||
finishSpeedMetersPerSecond;
|
||
FinishHeadingToleranceRadians =
|
||
finishHeadingToleranceRadians;
|
||
MaximumDistanceToTrajectoryMeters =
|
||
maximumDistanceToTrajectoryMeters;
|
||
TerminalBrakingPreviewMeters =
|
||
terminalBrakingPreviewMeters;
|
||
TerminalApproachDistanceMeters =
|
||
terminalApproachDistanceMeters;
|
||
TerminalApproachGainPerSecond =
|
||
terminalApproachGainPerSecond;
|
||
MaximumTerminalApproachSpeedMetersPerSecond =
|
||
maximumTerminalApproachSpeedMetersPerSecond;
|
||
CurvaturePreviewSeconds = curvaturePreviewSeconds;
|
||
MaximumCurvaturePreviewMeters =
|
||
maximumCurvaturePreviewMeters;
|
||
_motionDirectionInBodyRadians =
|
||
AngleMath.NormalizeRadians(
|
||
motionDirectionInBodyRadians);
|
||
}
|
||
|
||
public double FinishDistanceMeters { get; }
|
||
|
||
public double FinishSpeedMetersPerSecond { get; }
|
||
|
||
public double FinishHeadingToleranceRadians { get; }
|
||
|
||
public double MaximumDistanceToTrajectoryMeters { get; }
|
||
|
||
public double TerminalBrakingPreviewMeters { get; }
|
||
|
||
public double TerminalApproachDistanceMeters { get; }
|
||
|
||
public double TerminalApproachGainPerSecond { get; }
|
||
|
||
public double MaximumTerminalApproachSpeedMetersPerSecond { get; }
|
||
|
||
public double CurvaturePreviewSeconds { get; }
|
||
|
||
public double MaximumCurvaturePreviewMeters { get; }
|
||
|
||
public bool IsActive { get; private set; }
|
||
|
||
public bool IsCompleted { get; private set; }
|
||
|
||
public string LastFailureReason { get; private set; } =
|
||
string.Empty;
|
||
|
||
public Exception LastException { get; private set; }
|
||
|
||
public TrajectoryProjection? LastProjection { get; private set; }
|
||
|
||
public GcpMotionCommand? LastRequestedCommand { get; private set; }
|
||
|
||
public double? LastControlReferenceSpeedMetersPerSecond { get; private set; }
|
||
|
||
public double? LastCurvaturePreviewDistanceMeters { get; private set; }
|
||
|
||
public double? LastFeedforwardCurvaturePerMeter { get; private set; }
|
||
|
||
// 重置跨周期状态，并从轨迹起点开始新的跟踪过程。
|
||
public void Start(Trajectory2D trajectory)
|
||
{
|
||
if (trajectory == null)
|
||
{
|
||
throw new ArgumentNullException(
|
||
nameof(trajectory));
|
||
}
|
||
|
||
var terminalTravelDirection =
|
||
ResolveTerminalTravelDirection(trajectory);
|
||
|
||
ResetFeedbackControllers();
|
||
_trajectory = trajectory;
|
||
_terminalTravelDirection =
|
||
terminalTravelDirection;
|
||
IsActive = true;
|
||
IsCompleted = false;
|
||
ClearDiagnostics();
|
||
}
|
||
|
||
// 将受控刚体的位姿和速度转换为本周期GCP命令。
|
||
public PathTrackingCycleOutput Compute(
|
||
Pose2D poseInWorld,
|
||
Twist2D actualTwistInBody,
|
||
bool hasValidVelocityEstimate,
|
||
double deltaTimeSeconds)
|
||
{
|
||
NumericGuard.EnsureFinite(
|
||
poseInWorld,
|
||
nameof(poseInWorld));
|
||
NumericGuard.EnsureFinite(
|
||
actualTwistInBody,
|
||
nameof(actualTwistInBody));
|
||
NumericGuard.EnsureFinitePositive(
|
||
deltaTimeSeconds,
|
||
nameof(deltaTimeSeconds));
|
||
|
||
if (!IsActive || _trajectory == null)
|
||
{
|
||
return new PathTrackingCycleOutput(
|
||
PathTrackingCycleResult.Inactive,
|
||
null,
|
||
LastProjection,
|
||
0.0,
|
||
0.0);
|
||
}
|
||
|
||
var projectionStartTimestamp =
|
||
Stopwatch.GetTimestamp();
|
||
var projectionCompleted = false;
|
||
var projectionMilliseconds = 0.0;
|
||
var controllerComputeStartTimestamp = 0L;
|
||
|
||
try
|
||
{
|
||
var projection = LastProjection.HasValue
|
||
? TrajectoryProjector.Project(
|
||
_trajectory,
|
||
poseInWorld,
|
||
LastProjection.Value.ArcLengthMeters,
|
||
ProjectionBackwardSearchDistanceMeters,
|
||
ProjectionForwardSearchDistanceMeters)
|
||
: TrajectoryProjector.Project(
|
||
_trajectory,
|
||
poseInWorld);
|
||
|
||
projectionMilliseconds =
|
||
GetElapsedMilliseconds(
|
||
projectionStartTimestamp);
|
||
projectionCompleted = true;
|
||
LastProjection = projection;
|
||
controllerComputeStartTimestamp =
|
||
Stopwatch.GetTimestamp();
|
||
|
||
if (projection.DistanceToTrajectoryMeters >
|
||
MaximumDistanceToTrajectoryMeters)
|
||
{
|
||
Fail(
|
||
"受控刚体距离参考轨迹" +
|
||
$"{projection.DistanceToTrajectoryMeters:F3}m，" +
|
||
"超过允许值" +
|
||
$"{MaximumDistanceToTrajectoryMeters:F3}m。");
|
||
return CreateOutput(
|
||
PathTrackingCycleResult.Faulted,
|
||
null,
|
||
projection,
|
||
projectionMilliseconds,
|
||
controllerComputeStartTimestamp);
|
||
}
|
||
|
||
if (HasReachedEnd(
|
||
poseInWorld,
|
||
actualTwistInBody,
|
||
hasValidVelocityEstimate,
|
||
projection))
|
||
{
|
||
CompleteTrajectory();
|
||
return CreateOutput(
|
||
PathTrackingCycleResult.Completed,
|
||
null,
|
||
projection,
|
||
projectionMilliseconds,
|
||
controllerComputeStartTimestamp);
|
||
}
|
||
|
||
if (HasStoppedAtUnsatisfiedTerminal(
|
||
poseInWorld,
|
||
actualTwistInBody,
|
||
hasValidVelocityEstimate,
|
||
projection,
|
||
out var terminalFailureReason))
|
||
{
|
||
Fail(terminalFailureReason);
|
||
return CreateOutput(
|
||
PathTrackingCycleResult.Faulted,
|
||
null,
|
||
projection,
|
||
projectionMilliseconds,
|
||
controllerComputeStartTimestamp);
|
||
}
|
||
|
||
var controlReferenceSpeedMetersPerSecond =
|
||
ResolveControlReferenceSpeed(
|
||
poseInWorld,
|
||
projection);
|
||
LastControlReferenceSpeedMetersPerSecond =
|
||
controlReferenceSpeedMetersPerSecond;
|
||
var curvaturePreviewDistanceMeters =
|
||
ResolveCurvaturePreviewDistanceMeters(
|
||
actualTwistInBody,
|
||
hasValidVelocityEstimate,
|
||
controlReferenceSpeedMetersPerSecond);
|
||
LastCurvaturePreviewDistanceMeters =
|
||
curvaturePreviewDistanceMeters;
|
||
var feedforwardCurvaturePerMeter =
|
||
ResolveFeedforwardCurvaturePerMeter(
|
||
projection,
|
||
curvaturePreviewDistanceMeters);
|
||
LastFeedforwardCurvaturePerMeter =
|
||
feedforwardCurvaturePerMeter;
|
||
var context = new PathTrackingContext(
|
||
actualTwistInBody,
|
||
hasValidVelocityEstimate,
|
||
projection,
|
||
controlReferenceSpeedMetersPerSecond,
|
||
feedforwardCurvaturePerMeter,
|
||
deltaTimeSeconds,
|
||
_motionDirectionInBodyRadians);
|
||
var lateralCommand =
|
||
_lateralController.Compute(context);
|
||
var commandSpeedMetersPerSecond =
|
||
_longitudinalController
|
||
.ComputeSpeedMetersPerSecond(context);
|
||
var command = _gcpAllocator.Allocate(
|
||
commandSpeedMetersPerSecond,
|
||
lateralCommand);
|
||
|
||
LastRequestedCommand = command;
|
||
LastFailureReason = string.Empty;
|
||
LastException = null;
|
||
|
||
return CreateOutput(
|
||
PathTrackingCycleResult.CommandGenerated,
|
||
command,
|
||
projection,
|
||
projectionMilliseconds,
|
||
controllerComputeStartTimestamp);
|
||
}
|
||
catch (Exception exception)
|
||
{
|
||
if (!projectionCompleted)
|
||
{
|
||
projectionMilliseconds =
|
||
GetElapsedMilliseconds(
|
||
projectionStartTimestamp);
|
||
}
|
||
|
||
Fail(
|
||
"轨迹跟踪核心计算异常：" +
|
||
exception.Message,
|
||
exception);
|
||
|
||
return CreateOutput(
|
||
PathTrackingCycleResult.Faulted,
|
||
null,
|
||
LastProjection,
|
||
projectionMilliseconds,
|
||
controllerComputeStartTimestamp);
|
||
}
|
||
}
|
||
|
||
// 状态暂不可用时重置反馈历史，但保留当前轨迹和投影进度等待恢复。
|
||
public void PauseForUnavailableState(string reason)
|
||
{
|
||
ResetFeedbackControllers();
|
||
LastRequestedCommand = null;
|
||
LastFailureReason = reason ?? string.Empty;
|
||
LastException = null;
|
||
}
|
||
|
||
// 将外层执行故障同步到公共核心，并终止当前轨迹。
|
||
public void Fail(
|
||
string reason,
|
||
Exception exception = null)
|
||
{
|
||
ResetFeedbackControllers();
|
||
_trajectory = null;
|
||
IsActive = false;
|
||
IsCompleted = false;
|
||
LastRequestedCommand = null;
|
||
LastFailureReason = reason ?? string.Empty;
|
||
LastException = exception;
|
||
}
|
||
|
||
// 取消当前轨迹并清除全部跟踪状态。
|
||
public void Cancel()
|
||
{
|
||
ResetFeedbackControllers();
|
||
_trajectory = null;
|
||
_terminalTravelDirection = 1.0;
|
||
IsActive = false;
|
||
IsCompleted = false;
|
||
ClearDiagnostics();
|
||
}
|
||
|
||
private double ResolveControlReferenceSpeed(
|
||
Pose2D poseInWorld,
|
||
TrajectoryProjection projection)
|
||
{
|
||
if (projection.RemainingDistanceMeters >
|
||
TerminalApproachDistanceMeters)
|
||
{
|
||
return ResolveReferenceSpeedForControl(
|
||
projection);
|
||
}
|
||
|
||
return ResolveTerminalApproachSpeed(
|
||
poseInWorld);
|
||
}
|
||
|
||
private double ResolveCurvaturePreviewDistanceMeters(
|
||
Twist2D actualTwistInBody,
|
||
bool hasValidVelocityEstimate,
|
||
double controlReferenceSpeedMetersPerSecond)
|
||
{
|
||
if (CurvaturePreviewSeconds <= 0.0 ||
|
||
MaximumCurvaturePreviewMeters <= 0.0)
|
||
{
|
||
return 0.0;
|
||
}
|
||
|
||
var previewSpeedMetersPerSecond =
|
||
hasValidVelocityEstimate
|
||
? CalculateActualLongitudinalSpeedMetersPerSecond(
|
||
actualTwistInBody)
|
||
: Math.Abs(
|
||
controlReferenceSpeedMetersPerSecond);
|
||
|
||
return Math.Min(
|
||
MaximumCurvaturePreviewMeters,
|
||
previewSpeedMetersPerSecond *
|
||
CurvaturePreviewSeconds);
|
||
}
|
||
|
||
private double ResolveFeedforwardCurvaturePerMeter(
|
||
TrajectoryProjection projection,
|
||
double previewDistanceMeters)
|
||
{
|
||
var previewArcLengthMeters = Math.Min(
|
||
_trajectory.TotalLengthMeters,
|
||
projection.ArcLengthMeters +
|
||
previewDistanceMeters);
|
||
|
||
return _trajectory
|
||
.SampleAtArcLength(previewArcLengthMeters)
|
||
.CurvaturePerMeter;
|
||
}
|
||
|
||
private double ResolveTerminalApproachSpeed(
|
||
Pose2D poseInWorld)
|
||
{
|
||
var distanceToEndMeters =
|
||
CalculateDistanceToEndMeters(
|
||
poseInWorld);
|
||
var headingErrorToEndRadians =
|
||
CalculateHeadingErrorToEndRadians(
|
||
poseInWorld);
|
||
|
||
if (distanceToEndMeters <=
|
||
FinishDistanceMeters &&
|
||
headingErrorToEndRadians <=
|
||
FinishHeadingToleranceRadians)
|
||
{
|
||
return 0.0;
|
||
}
|
||
|
||
var endPose = _trajectory.EndPoint.PoseInWorld;
|
||
var deltaX = endPose.XMeters -
|
||
poseInWorld.XMeters;
|
||
var deltaY = endPose.YMeters -
|
||
poseInWorld.YMeters;
|
||
var longitudinalErrorMeters =
|
||
deltaX * Math.Cos(endPose.YawRadians) +
|
||
deltaY * Math.Sin(endPose.YawRadians);
|
||
var remainingAlongTravelMeters =
|
||
_terminalTravelDirection *
|
||
longitudinalErrorMeters;
|
||
|
||
// 越过终点后不生成与原轨迹方向相反的修正速度。
|
||
if (remainingAlongTravelMeters <= 0.0)
|
||
{
|
||
return 0.0;
|
||
}
|
||
|
||
var speedMagnitudeMetersPerSecond =
|
||
Math.Min(
|
||
MaximumTerminalApproachSpeedMetersPerSecond,
|
||
TerminalApproachGainPerSecond *
|
||
remainingAlongTravelMeters);
|
||
|
||
return _terminalTravelDirection *
|
||
speedMagnitudeMetersPerSecond;
|
||
}
|
||
|
||
private double ResolveReferenceSpeedForControl(
|
||
TrajectoryProjection projection)
|
||
{
|
||
var currentReferenceSpeed =
|
||
ApplyTerminalBrakingPreview(
|
||
projection,
|
||
projection.ReferencePoint
|
||
.ReferenceSpeedMetersPerSecond);
|
||
var isInStartupRegion =
|
||
projection.ArcLengthMeters <=
|
||
StartupRegionMeters &&
|
||
projection.RemainingDistanceMeters >
|
||
FinishDistanceMeters;
|
||
|
||
if (!isInStartupRegion)
|
||
{
|
||
return currentReferenceSpeed;
|
||
}
|
||
|
||
var previewArcLengthMeters = Math.Min(
|
||
_trajectory.TotalLengthMeters,
|
||
projection.ArcLengthMeters +
|
||
StartupPreviewDistanceMeters);
|
||
var previewReferenceSpeed =
|
||
_trajectory
|
||
.SampleAtArcLength(previewArcLengthMeters)
|
||
.ReferenceSpeedMetersPerSecond;
|
||
|
||
if (Math.Abs(previewReferenceSpeed) <=
|
||
ZeroReferenceSpeedToleranceMetersPerSecond)
|
||
{
|
||
return currentReferenceSpeed;
|
||
}
|
||
|
||
var startupReleaseSpeed =
|
||
Math.Sign(previewReferenceSpeed) *
|
||
Math.Min(
|
||
Math.Abs(previewReferenceSpeed),
|
||
MaximumStartupSpeedMetersPerSecond);
|
||
|
||
if (Math.Sign(currentReferenceSpeed) ==
|
||
Math.Sign(startupReleaseSpeed) &&
|
||
Math.Abs(currentReferenceSpeed) >=
|
||
Math.Abs(startupReleaseSpeed))
|
||
{
|
||
return currentReferenceSpeed;
|
||
}
|
||
|
||
return startupReleaseSpeed;
|
||
}
|
||
|
||
private double ApplyTerminalBrakingPreview(
|
||
TrajectoryProjection projection,
|
||
double currentReferenceSpeed)
|
||
{
|
||
if (TerminalBrakingPreviewMeters <= 0.0)
|
||
{
|
||
return currentReferenceSpeed;
|
||
}
|
||
|
||
var previewArcLengthMeters = Math.Min(
|
||
_trajectory.TotalLengthMeters,
|
||
projection.ArcLengthMeters +
|
||
TerminalBrakingPreviewMeters);
|
||
var previewReferenceSpeed =
|
||
_trajectory
|
||
.SampleAtArcLength(previewArcLengthMeters)
|
||
.ReferenceSpeedMetersPerSecond;
|
||
var previewIsStop =
|
||
Math.Abs(previewReferenceSpeed) <=
|
||
ZeroReferenceSpeedToleranceMetersPerSecond;
|
||
var hasSameDirection =
|
||
Math.Sign(previewReferenceSpeed) ==
|
||
Math.Sign(currentReferenceSpeed);
|
||
var previewIsSlower =
|
||
Math.Abs(previewReferenceSpeed) <
|
||
Math.Abs(currentReferenceSpeed);
|
||
|
||
if (previewIsSlower &&
|
||
(previewIsStop || hasSameDirection))
|
||
{
|
||
return previewReferenceSpeed;
|
||
}
|
||
|
||
return currentReferenceSpeed;
|
||
}
|
||
|
||
private static double ResolveTerminalTravelDirection(
|
||
Trajectory2D trajectory)
|
||
{
|
||
for (var index = trajectory.Count - 1;
|
||
index >= 0;
|
||
index--)
|
||
{
|
||
var referenceSpeedMetersPerSecond =
|
||
trajectory[index]
|
||
.ReferenceSpeedMetersPerSecond;
|
||
|
||
if (Math.Abs(referenceSpeedMetersPerSecond) >
|
||
ZeroReferenceSpeedToleranceMetersPerSecond)
|
||
{
|
||
return Math.Sign(
|
||
referenceSpeedMetersPerSecond);
|
||
}
|
||
}
|
||
|
||
throw new ArgumentException(
|
||
"轨迹必须在终点前包含至少一个非零参考速度。",
|
||
nameof(trajectory));
|
||
}
|
||
|
||
private bool HasReachedEnd(
|
||
Pose2D poseInWorld,
|
||
Twist2D actualTwistInBody,
|
||
bool hasValidVelocityEstimate,
|
||
TrajectoryProjection projection)
|
||
{
|
||
if (!hasValidVelocityEstimate)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
return projection.RemainingDistanceMeters <=
|
||
FinishDistanceMeters &&
|
||
CalculateDistanceToEndMeters(poseInWorld) <=
|
||
FinishDistanceMeters &&
|
||
CalculateHeadingErrorToEndRadians(poseInWorld) <=
|
||
FinishHeadingToleranceRadians &&
|
||
CalculateActualLongitudinalSpeedMetersPerSecond(
|
||
actualTwistInBody) <=
|
||
FinishSpeedMetersPerSecond;
|
||
}
|
||
|
||
private bool HasStoppedAtUnsatisfiedTerminal(
|
||
Pose2D poseInWorld,
|
||
Twist2D actualTwistInBody,
|
||
bool hasValidVelocityEstimate,
|
||
TrajectoryProjection projection,
|
||
out string failureReason)
|
||
{
|
||
failureReason = string.Empty;
|
||
|
||
var isTerminalZeroSpeedReference =
|
||
projection.RemainingDistanceMeters <=
|
||
FinishDistanceMeters &&
|
||
Math.Abs(
|
||
projection.ReferencePoint
|
||
.ReferenceSpeedMetersPerSecond) <=
|
||
ZeroReferenceSpeedToleranceMetersPerSecond;
|
||
|
||
if (!isTerminalZeroSpeedReference ||
|
||
!hasValidVelocityEstimate ||
|
||
CalculateActualLongitudinalSpeedMetersPerSecond(
|
||
actualTwistInBody) >
|
||
FinishSpeedMetersPerSecond)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
var positionErrorMeters =
|
||
CalculateDistanceToEndMeters(
|
||
poseInWorld);
|
||
var headingErrorRadians =
|
||
CalculateHeadingErrorToEndRadians(
|
||
poseInWorld);
|
||
|
||
failureReason =
|
||
"受控刚体已在终点零速参考处停稳，但终点精度不满足要求：" +
|
||
$"位置误差={positionErrorMeters:F3}m，" +
|
||
"航向误差=" +
|
||
$"{AngleMath.RadiansToDegrees(headingErrorRadians):F2}°。";
|
||
return true;
|
||
}
|
||
|
||
private double CalculateDistanceToEndMeters(
|
||
Pose2D poseInWorld)
|
||
{
|
||
var endPose = _trajectory.EndPoint.PoseInWorld;
|
||
var deltaX = poseInWorld.XMeters -
|
||
endPose.XMeters;
|
||
var deltaY = poseInWorld.YMeters -
|
||
endPose.YMeters;
|
||
|
||
return Math.Sqrt(
|
||
deltaX * deltaX +
|
||
deltaY * deltaY);
|
||
}
|
||
|
||
private double CalculateHeadingErrorToEndRadians(
|
||
Pose2D poseInWorld)
|
||
{
|
||
return Math.Abs(
|
||
AngleMath.ShortestDifferenceRadians(
|
||
_trajectory.EndPoint
|
||
.PoseInWorld.YawRadians,
|
||
poseInWorld.YawRadians));
|
||
}
|
||
|
||
private double CalculateActualLongitudinalSpeedMetersPerSecond(
|
||
Twist2D actualTwistInBody)
|
||
{
|
||
return Math.Abs(
|
||
Math.Cos(_motionDirectionInBodyRadians) *
|
||
actualTwistInBody.VxMetersPerSecond +
|
||
Math.Sin(_motionDirectionInBodyRadians) *
|
||
actualTwistInBody.VyMetersPerSecond);
|
||
}
|
||
|
||
private void CompleteTrajectory()
|
||
{
|
||
ResetFeedbackControllers();
|
||
_trajectory = null;
|
||
IsActive = false;
|
||
IsCompleted = true;
|
||
LastRequestedCommand = new GcpMotionCommand(
|
||
0.0,
|
||
0.0,
|
||
0.0);
|
||
LastFailureReason = string.Empty;
|
||
LastException = null;
|
||
}
|
||
|
||
private void ResetFeedbackControllers()
|
||
{
|
||
_lateralController.Reset();
|
||
_longitudinalController.Reset();
|
||
}
|
||
|
||
private void ClearDiagnostics()
|
||
{
|
||
LastProjection = null;
|
||
LastRequestedCommand = null;
|
||
LastControlReferenceSpeedMetersPerSecond = null;
|
||
LastCurvaturePreviewDistanceMeters = null;
|
||
LastFeedforwardCurvaturePerMeter = null;
|
||
LastFailureReason = string.Empty;
|
||
LastException = null;
|
||
}
|
||
|
||
private static PathTrackingCycleOutput CreateOutput(
|
||
PathTrackingCycleResult result,
|
||
GcpMotionCommand? command,
|
||
TrajectoryProjection? projection,
|
||
double projectionMilliseconds,
|
||
long controllerComputeStartTimestamp)
|
||
{
|
||
var controllerComputeMilliseconds =
|
||
controllerComputeStartTimestamp == 0L
|
||
? 0.0
|
||
: GetElapsedMilliseconds(
|
||
controllerComputeStartTimestamp);
|
||
|
||
return new PathTrackingCycleOutput(
|
||
result,
|
||
command,
|
||
projection,
|
||
projectionMilliseconds,
|
||
controllerComputeMilliseconds);
|
||
}
|
||
|
||
private static double GetElapsedMilliseconds(
|
||
long startTimestamp)
|
||
{
|
||
return (Stopwatch.GetTimestamp() - startTimestamp) *
|
||
1000.0 /
|
||
Stopwatch.Frequency;
|
||
}
|
||
}
|
||
}
|