436 lines
16 KiB
C#
436 lines
16 KiB
C#
using System;
|
|
using System.Diagnostics;
|
|
using MultiWheelC.Control.Abstractions;
|
|
using MultiWheelC.Control.Allocation;
|
|
using MultiWheelC.StateEstimation;
|
|
using MultiWheelC.Trajectory;
|
|
using MyParking.Shared;
|
|
|
|
namespace MultiWheelC.Control.Execution
|
|
{
|
|
// 单周期轨迹控制结果。
|
|
public enum ParkingControlCycleResult
|
|
{
|
|
Inactive = 0,
|
|
CommandSent = 1,
|
|
Completed = 2,
|
|
StateUnavailable = 3,
|
|
Faulted = 4
|
|
}
|
|
|
|
// 单周期各阶段耗时及定位帧更新状态，用于区分控制计算与底层通信延迟。
|
|
public readonly struct ParkingControlCycleTiming
|
|
{
|
|
public ParkingControlCycleTiming(
|
|
long cycleIndex,
|
|
double cycleIntervalMilliseconds,
|
|
double stateReadMilliseconds,
|
|
double projectionMilliseconds,
|
|
double controllerComputeMilliseconds,
|
|
double commandSendMilliseconds,
|
|
double totalCycleMilliseconds,
|
|
bool hasStateTimestamp,
|
|
double stateTimestampSeconds,
|
|
bool stateTimestampChanged,
|
|
ParkingControlCycleResult result)
|
|
{
|
|
CycleIndex = cycleIndex;
|
|
CycleIntervalMilliseconds =
|
|
cycleIntervalMilliseconds;
|
|
StateReadMilliseconds = stateReadMilliseconds;
|
|
ProjectionMilliseconds = projectionMilliseconds;
|
|
ControllerComputeMilliseconds =
|
|
controllerComputeMilliseconds;
|
|
CommandSendMilliseconds = commandSendMilliseconds;
|
|
TotalCycleMilliseconds = totalCycleMilliseconds;
|
|
HasStateTimestamp = hasStateTimestamp;
|
|
StateTimestampSeconds = stateTimestampSeconds;
|
|
StateTimestampChanged = stateTimestampChanged;
|
|
Result = result;
|
|
}
|
|
|
|
public long CycleIndex { get; }
|
|
public double CycleIntervalMilliseconds { get; }
|
|
public double StateReadMilliseconds { get; }
|
|
public double ProjectionMilliseconds { get; }
|
|
public double ControllerComputeMilliseconds { get; }
|
|
public double CommandSendMilliseconds { get; }
|
|
public double TotalCycleMilliseconds { get; }
|
|
public bool HasStateTimestamp { get; }
|
|
public double StateTimestampSeconds { get; }
|
|
public bool StateTimestampChanged { get; }
|
|
public ParkingControlCycleResult Result { get; }
|
|
}
|
|
|
|
// 负责单车状态读取、公共轨迹计算和真实底盘命令发送。
|
|
public sealed class ParkingGeometricController
|
|
{
|
|
private readonly IVehicleStateProvider _stateProvider;
|
|
private readonly GcpCommandExecutor _commandExecutor;
|
|
private readonly PathTrackingCore _trackingCore;
|
|
|
|
private long _cycleIndex;
|
|
private bool _hasPreviousStateTimestamp;
|
|
private double _previousStateTimestampSeconds;
|
|
|
|
// 创建具有终点判定、轨迹偏离保护和曲率前馈预瞄的单车轨迹控制器。
|
|
public ParkingGeometricController(
|
|
IVehicleStateProvider stateProvider,
|
|
ILateralController lateralController,
|
|
ILongitudinalController longitudinalController,
|
|
GcpCommandAllocator gcpAllocator,
|
|
GcpCommandExecutor commandExecutor,
|
|
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)
|
|
{
|
|
_stateProvider = stateProvider ??
|
|
throw new ArgumentNullException(
|
|
nameof(stateProvider));
|
|
_commandExecutor = commandExecutor ??
|
|
throw new ArgumentNullException(
|
|
nameof(commandExecutor));
|
|
_trackingCore = new PathTrackingCore(
|
|
lateralController,
|
|
longitudinalController,
|
|
gcpAllocator,
|
|
finishDistanceMeters,
|
|
finishSpeedMetersPerSecond,
|
|
finishHeadingToleranceRadians,
|
|
maximumDistanceToTrajectoryMeters,
|
|
terminalBrakingPreviewMeters,
|
|
terminalApproachDistanceMeters,
|
|
terminalApproachGainPerSecond,
|
|
maximumTerminalApproachSpeedMetersPerSecond,
|
|
curvaturePreviewSeconds,
|
|
maximumCurvaturePreviewMeters,
|
|
motionDirectionInBodyRadians);
|
|
}
|
|
|
|
// 以下控制参数由公共轨迹核心统一持有。
|
|
public double FinishDistanceMeters =>
|
|
_trackingCore.FinishDistanceMeters;
|
|
|
|
public double FinishSpeedMetersPerSecond =>
|
|
_trackingCore.FinishSpeedMetersPerSecond;
|
|
|
|
public double FinishHeadingToleranceRadians =>
|
|
_trackingCore.FinishHeadingToleranceRadians;
|
|
|
|
public double MaximumDistanceToTrajectoryMeters =>
|
|
_trackingCore.MaximumDistanceToTrajectoryMeters;
|
|
|
|
public double TerminalBrakingPreviewMeters =>
|
|
_trackingCore.TerminalBrakingPreviewMeters;
|
|
|
|
public double TerminalApproachDistanceMeters =>
|
|
_trackingCore.TerminalApproachDistanceMeters;
|
|
|
|
public double TerminalApproachGainPerSecond =>
|
|
_trackingCore.TerminalApproachGainPerSecond;
|
|
|
|
public double MaximumTerminalApproachSpeedMetersPerSecond =>
|
|
_trackingCore.MaximumTerminalApproachSpeedMetersPerSecond;
|
|
|
|
public double CurvaturePreviewSeconds =>
|
|
_trackingCore.CurvaturePreviewSeconds;
|
|
|
|
public double MaximumCurvaturePreviewMeters =>
|
|
_trackingCore.MaximumCurvaturePreviewMeters;
|
|
|
|
public bool IsActive => _trackingCore.IsActive;
|
|
|
|
public bool IsCompleted => _trackingCore.IsCompleted;
|
|
|
|
public string LastFailureReason =>
|
|
_trackingCore.LastFailureReason;
|
|
|
|
public Exception LastException =>
|
|
_trackingCore.LastException;
|
|
|
|
// 最近一次有效车辆状态仍由单车外层保存。
|
|
public VehicleState? LastVehicleState { get; private set; }
|
|
|
|
public TrajectoryProjection? LastProjection =>
|
|
_trackingCore.LastProjection;
|
|
|
|
public GcpMotionCommand? LastRequestedCommand =>
|
|
_trackingCore.LastRequestedCommand;
|
|
|
|
// 经过GCP角速度限制后实际发送给底盘的最近一次命令。
|
|
public GcpMotionCommand? LastCommand { get; private set; }
|
|
|
|
public double? LastControlReferenceSpeedMetersPerSecond =>
|
|
_trackingCore.LastControlReferenceSpeedMetersPerSecond;
|
|
|
|
public double? LastCurvaturePreviewDistanceMeters =>
|
|
_trackingCore.LastCurvaturePreviewDistanceMeters;
|
|
|
|
public double? LastFeedforwardCurvaturePerMeter =>
|
|
_trackingCore.LastFeedforwardCurvaturePerMeter;
|
|
|
|
public ParkingControlCycleTiming? LastCycleTiming { get; private set; }
|
|
|
|
// 先停车，再从轨迹起点重置公共控制核心和单车执行诊断。
|
|
public void Start(Trajectory2D trajectory)
|
|
{
|
|
_commandExecutor.Stop();
|
|
_trackingCore.Start(trajectory);
|
|
ClearExecutionDiagnostics();
|
|
}
|
|
|
|
// 读取车辆状态、调用公共核心并发送一次真实底盘命令。
|
|
public ParkingControlCycleResult ExecuteCycle(
|
|
double deltaTimeSeconds)
|
|
{
|
|
NumericGuard.EnsureFinitePositive(
|
|
deltaTimeSeconds,
|
|
nameof(deltaTimeSeconds));
|
|
|
|
if (!_trackingCore.IsActive)
|
|
{
|
|
return ParkingControlCycleResult.Inactive;
|
|
}
|
|
|
|
var cycleIndex = ++_cycleIndex;
|
|
var cycleStartTimestamp =
|
|
Stopwatch.GetTimestamp();
|
|
var stateReadMilliseconds = 0.0;
|
|
var projectionMilliseconds = 0.0;
|
|
var controllerComputeMilliseconds = 0.0;
|
|
var commandSendMilliseconds = 0.0;
|
|
var hasStateTimestamp = false;
|
|
var stateTimestampSeconds = 0.0;
|
|
var stateTimestampChanged = false;
|
|
var cycleResult =
|
|
ParkingControlCycleResult.Faulted;
|
|
|
|
try
|
|
{
|
|
var stateReadStartTimestamp =
|
|
Stopwatch.GetTimestamp();
|
|
bool stateAvailable;
|
|
VehicleState vehicleState;
|
|
try
|
|
{
|
|
stateAvailable =
|
|
_stateProvider.TryGetState(
|
|
out vehicleState);
|
|
}
|
|
finally
|
|
{
|
|
stateReadMilliseconds =
|
|
GetElapsedMilliseconds(
|
|
stateReadStartTimestamp);
|
|
}
|
|
|
|
if (!stateAvailable)
|
|
{
|
|
var stopStartTimestamp =
|
|
Stopwatch.GetTimestamp();
|
|
try
|
|
{
|
|
StopForUnavailableState();
|
|
}
|
|
finally
|
|
{
|
|
commandSendMilliseconds =
|
|
GetElapsedMilliseconds(
|
|
stopStartTimestamp);
|
|
}
|
|
|
|
cycleResult = ParkingControlCycleResult
|
|
.StateUnavailable;
|
|
return cycleResult;
|
|
}
|
|
|
|
LastVehicleState = vehicleState;
|
|
hasStateTimestamp = true;
|
|
stateTimestampSeconds =
|
|
vehicleState.SampleTimestampSeconds;
|
|
stateTimestampChanged =
|
|
!_hasPreviousStateTimestamp ||
|
|
stateTimestampSeconds !=
|
|
_previousStateTimestampSeconds;
|
|
_previousStateTimestampSeconds =
|
|
stateTimestampSeconds;
|
|
_hasPreviousStateTimestamp = true;
|
|
|
|
var output = _trackingCore.Compute(
|
|
vehicleState.PoseInWorld,
|
|
vehicleState.TwistInBody,
|
|
vehicleState.HasValidVelocityEstimate,
|
|
deltaTimeSeconds);
|
|
projectionMilliseconds =
|
|
output.ProjectionMilliseconds;
|
|
controllerComputeMilliseconds =
|
|
output.ControllerComputeMilliseconds;
|
|
|
|
if (output.Result ==
|
|
PathTrackingCycleResult.Completed)
|
|
{
|
|
commandSendMilliseconds =
|
|
StopForCompletedTrajectory();
|
|
cycleResult =
|
|
ParkingControlCycleResult.Completed;
|
|
return cycleResult;
|
|
}
|
|
|
|
if (output.Result ==
|
|
PathTrackingCycleResult.Faulted)
|
|
{
|
|
commandSendMilliseconds =
|
|
StopForTrackingFault();
|
|
cycleResult =
|
|
ParkingControlCycleResult.Faulted;
|
|
return cycleResult;
|
|
}
|
|
|
|
if (output.Result !=
|
|
PathTrackingCycleResult.CommandGenerated ||
|
|
!output.Command.HasValue)
|
|
{
|
|
cycleResult =
|
|
ParkingControlCycleResult.Inactive;
|
|
return cycleResult;
|
|
}
|
|
|
|
var commandSendStartTimestamp =
|
|
Stopwatch.GetTimestamp();
|
|
bool commandSucceeded;
|
|
try
|
|
{
|
|
commandSucceeded =
|
|
_commandExecutor.Execute(
|
|
output.Command.Value,
|
|
deltaTimeSeconds);
|
|
}
|
|
finally
|
|
{
|
|
commandSendMilliseconds =
|
|
GetElapsedMilliseconds(
|
|
commandSendStartTimestamp);
|
|
}
|
|
|
|
if (!commandSucceeded)
|
|
{
|
|
cycleResult = EnterFault(
|
|
string.IsNullOrWhiteSpace(
|
|
_commandExecutor.LastFailureReason)
|
|
? "GCP底盘命令执行失败。"
|
|
: _commandExecutor.LastFailureReason);
|
|
return cycleResult;
|
|
}
|
|
|
|
LastCommand =
|
|
_commandExecutor.LastSentCommand;
|
|
cycleResult =
|
|
ParkingControlCycleResult.CommandSent;
|
|
return cycleResult;
|
|
}
|
|
catch (Exception exception)
|
|
{
|
|
cycleResult = EnterFault(
|
|
"停车机器人轨迹控制周期异常：" +
|
|
exception.Message,
|
|
exception);
|
|
return cycleResult;
|
|
}
|
|
finally
|
|
{
|
|
LastCycleTiming =
|
|
new ParkingControlCycleTiming(
|
|
cycleIndex,
|
|
deltaTimeSeconds * 1000.0,
|
|
stateReadMilliseconds,
|
|
projectionMilliseconds,
|
|
controllerComputeMilliseconds,
|
|
commandSendMilliseconds,
|
|
GetElapsedMilliseconds(
|
|
cycleStartTimestamp),
|
|
hasStateTimestamp,
|
|
stateTimestampSeconds,
|
|
stateTimestampChanged,
|
|
cycleResult);
|
|
}
|
|
}
|
|
|
|
// 主动取消当前轨迹、立即停车并清除全部控制状态。
|
|
public void Cancel()
|
|
{
|
|
_commandExecutor.Stop();
|
|
_trackingCore.Cancel();
|
|
ClearExecutionDiagnostics();
|
|
}
|
|
|
|
// 状态不可用时停车并重置反馈历史，同时保留轨迹等待恢复。
|
|
private void StopForUnavailableState()
|
|
{
|
|
_commandExecutor.Stop();
|
|
_trackingCore.PauseForUnavailableState(
|
|
"当前无法获得有效车辆状态，底盘已停车并等待定位恢复。");
|
|
LastCommand = null;
|
|
}
|
|
|
|
// 公共核心完成轨迹后发送停车，并保留零命令供实验记录。
|
|
private double StopForCompletedTrajectory()
|
|
{
|
|
var startTimestamp = Stopwatch.GetTimestamp();
|
|
_commandExecutor.Stop();
|
|
LastCommand = new GcpMotionCommand(
|
|
0.0,
|
|
0.0,
|
|
0.0);
|
|
return GetElapsedMilliseconds(startTimestamp);
|
|
}
|
|
|
|
// 公共核心故障后只负责真实底盘停车，不覆盖核心保存的失败原因。
|
|
private double StopForTrackingFault()
|
|
{
|
|
var startTimestamp = Stopwatch.GetTimestamp();
|
|
_commandExecutor.Stop();
|
|
LastCommand = null;
|
|
return GetElapsedMilliseconds(startTimestamp);
|
|
}
|
|
|
|
// 将底盘执行或外层异常同步到公共核心，并立即停车。
|
|
private ParkingControlCycleResult EnterFault(
|
|
string reason,
|
|
Exception exception = null)
|
|
{
|
|
_commandExecutor.Stop();
|
|
_trackingCore.Fail(reason, exception);
|
|
LastCommand = null;
|
|
return ParkingControlCycleResult.Faulted;
|
|
}
|
|
|
|
// 清除只属于单车状态读取、发送和周期计时的诊断。
|
|
private void ClearExecutionDiagnostics()
|
|
{
|
|
_cycleIndex = 0;
|
|
_hasPreviousStateTimestamp = false;
|
|
_previousStateTimestampSeconds = 0.0;
|
|
LastVehicleState = null;
|
|
LastCommand = null;
|
|
LastCycleTiming = null;
|
|
}
|
|
|
|
private static double GetElapsedMilliseconds(
|
|
long startTimestamp)
|
|
{
|
|
return (Stopwatch.GetTimestamp() - startTimestamp) *
|
|
1000.0 /
|
|
Stopwatch.Frequency;
|
|
}
|
|
}
|
|
}
|