799 lines
27 KiB
C#
799 lines
27 KiB
C#
using System;
|
|
using System.Drawing;
|
|
using System.Globalization;
|
|
using System.Numerics;
|
|
using System.Threading;
|
|
using ClumsyCore;
|
|
using ClumsyCore.DTools;
|
|
using ClumsyCore.Interfaces;
|
|
using ClumsyCore.Pilot;
|
|
using CommonUsage.Chassis;
|
|
using MultiWheelC.Control.Execution;
|
|
using MultiWheelC.StateEstimation;
|
|
using MyParking.Shared;
|
|
|
|
namespace MultiWheelC
|
|
{
|
|
/// <summary>
|
|
/// 统一读取轨迹实验的有符号横向偏移，并将车体局部偏移转换到世界坐标系。
|
|
/// </summary>
|
|
internal static class TrajectoryExperimentInput
|
|
{
|
|
private const double MaximumOffsetCentimeters = 30.0;
|
|
|
|
/// <summary>
|
|
/// 从Clumsy输入框读取车体左正右负的横向偏移，单位转换为m。
|
|
/// </summary>
|
|
public static bool TryReadLateralOffsetMeters(
|
|
out double lateralOffsetMeters)
|
|
{
|
|
lateralOffsetMeters = 0.0;
|
|
var input = UI.GetInput(
|
|
"输入轨迹横向偏移（cm，左正右负，范围-30～30）：");
|
|
var parsed = double.TryParse(
|
|
input,
|
|
NumberStyles.Float,
|
|
CultureInfo.CurrentCulture,
|
|
out var offsetCentimeters) ||
|
|
double.TryParse(
|
|
input,
|
|
NumberStyles.Float,
|
|
CultureInfo.InvariantCulture,
|
|
out offsetCentimeters);
|
|
|
|
if (!parsed ||
|
|
double.IsNaN(offsetCentimeters) ||
|
|
double.IsInfinity(offsetCentimeters) ||
|
|
Math.Abs(offsetCentimeters) >
|
|
MaximumOffsetCentimeters)
|
|
{
|
|
Console.WriteLine(
|
|
"轨迹横向偏移必须是-30～30cm之间的有限数值，测试未启动。");
|
|
return false;
|
|
}
|
|
|
|
lateralOffsetMeters =
|
|
offsetCentimeters / 100.0;
|
|
return true;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 沿初始车体左方向平移参考轨迹起点，同时保持世界坐标航向不变。
|
|
/// </summary>
|
|
public static Pose2D OffsetPoseLaterally(
|
|
Pose2D poseInWorld,
|
|
double lateralOffsetMeters)
|
|
{
|
|
var yawRadians = poseInWorld.YawRadians;
|
|
return new Pose2D(
|
|
poseInWorld.XMeters -
|
|
Math.Sin(yawRadians) *
|
|
lateralOffsetMeters,
|
|
poseInWorld.YMeters +
|
|
Math.Cos(yawRadians) *
|
|
lateralOffsetMeters,
|
|
yawRadians);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 生成带毫米偏移标识的实验轨迹名称。
|
|
/// </summary>
|
|
public static string BuildTrajectoryName(
|
|
string baseName,
|
|
double lateralOffsetMeters)
|
|
{
|
|
return baseName +
|
|
"_Offset" +
|
|
(lateralOffsetMeters * 1000.0)
|
|
.ToString("+0;-0;0", CultureInfo.InvariantCulture) +
|
|
"mm";
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 从当前Detour位姿开始执行新版控制器4m直线跟踪并保存实验数据。
|
|
/// </summary>
|
|
[MovementTest(name = "新版控制器：4m直线轨迹跟踪")]
|
|
public sealed class NewControllerStraight4mTest
|
|
: MovementTest
|
|
{
|
|
private const float MillimetersPerMeter = 1000f;
|
|
|
|
private readonly Painter _painter =
|
|
UI.GetPainter("NewControllerStraight4m");
|
|
|
|
private DriveTask _task;
|
|
private TrackingExperimentRecorder _recorder;
|
|
private IVehicleStateProvider _stateProvider;
|
|
|
|
/// <summary>
|
|
/// 获取或设置本次测试编号，用于区分重复实验CSV。
|
|
/// </summary>
|
|
public int TrialNumber = 1;
|
|
|
|
/// <summary>
|
|
/// 获取或设置4m直线的巡航参考速度，单位为m/s。
|
|
/// </summary>
|
|
public double CruiseSpeedMetersPerSecond = 0.40;
|
|
|
|
/// <summary>
|
|
/// 获取或设置参考速度加速度，单位为m/s²。
|
|
/// </summary>
|
|
public double AccelerationMetersPerSecondSquared = 0.20;
|
|
|
|
/// <summary>
|
|
/// 获取或设置参考速度减速度，单位为m/s²。
|
|
/// </summary>
|
|
public double DecelerationMetersPerSecondSquared = 0.10;
|
|
|
|
/// <summary>
|
|
/// 获取或设置离散轨迹点间距，单位为m。
|
|
/// </summary>
|
|
public double PointSpacingMeters = 0.02;
|
|
|
|
/// <summary>
|
|
/// 读取当前位姿、绘制离散轨迹并启动新版轨迹跟踪动作。
|
|
/// </summary>
|
|
public override void Test()
|
|
{
|
|
if (_task != null)
|
|
{
|
|
Console.WriteLine(
|
|
"新版4m直线轨迹测试已经在运行，请先停止当前测试。");
|
|
return;
|
|
}
|
|
|
|
if (!TrajectoryExperimentInput
|
|
.TryReadLateralOffsetMeters(
|
|
out var lateralOffsetMeters))
|
|
{
|
|
return;
|
|
}
|
|
|
|
if (!MovementTestPreparation.AreWheelsForward())
|
|
{
|
|
return;
|
|
}
|
|
|
|
var chassis =
|
|
PilotDefinition.Chassis as MultiWheelChassis;
|
|
if (chassis == null)
|
|
{
|
|
Console.WriteLine(
|
|
"当前底盘不是MultiWheelChassis，无法执行新版轨迹测试。");
|
|
return;
|
|
}
|
|
|
|
var detourStateProvider =
|
|
new DetourVehicleStateProvider();
|
|
if (!detourStateProvider.TryGetState(
|
|
out var initialState))
|
|
{
|
|
Console.WriteLine(
|
|
"无法读取有效Detour起点位姿：" +
|
|
detourStateProvider.LastFailureReason);
|
|
_stateProvider = null;
|
|
return;
|
|
}
|
|
|
|
_stateProvider =
|
|
new WheelFeedbackVehicleStateProvider(
|
|
detourStateProvider,
|
|
chassis);
|
|
|
|
var trajectoryStartPose =
|
|
TrajectoryExperimentInput.OffsetPoseLaterally(
|
|
initialState.PoseInWorld,
|
|
lateralOffsetMeters);
|
|
var trajectory =
|
|
TestTrajectoryFactory.CreateStraight4Meters(
|
|
trajectoryStartPose,
|
|
CruiseSpeedMetersPerSecond,
|
|
AccelerationMetersPerSecondSquared,
|
|
DecelerationMetersPerSecondSquared,
|
|
PointSpacingMeters);
|
|
|
|
DrawTrajectory(trajectory);
|
|
|
|
var referenceStart = ToMillimeterVector(
|
|
trajectory.StartPoint.PoseInWorld);
|
|
var referenceEnd = ToMillimeterVector(
|
|
trajectory.EndPoint.PoseInWorld);
|
|
var recorder =
|
|
new TrackingExperimentRecorder(
|
|
controllerName: "NewStanleyPid",
|
|
trajectoryName:
|
|
TrajectoryExperimentInput.BuildTrajectoryName(
|
|
"ProfiledStraight4m",
|
|
lateralOffsetMeters),
|
|
trialNumber: TrialNumber,
|
|
referenceStart: referenceStart,
|
|
referenceEnd: referenceEnd,
|
|
referenceSpeed:
|
|
(float)CruiseSpeedMetersPerSecond,
|
|
sampleIntervalMs: 50,
|
|
referenceAccelerationMetersPerSecondSquared:
|
|
(float)AccelerationMetersPerSecondSquared,
|
|
referenceDecelerationMetersPerSecondSquared:
|
|
(float)DecelerationMetersPerSecondSquared,
|
|
diagnosticChassis: chassis);
|
|
_recorder = recorder;
|
|
|
|
var controlPointRadiusMeters =
|
|
chassis.ControlPointRadius /
|
|
MillimetersPerMeter;
|
|
var movement =
|
|
new TrajectoryTrackingMovement
|
|
{
|
|
Trajectory = trajectory,
|
|
StateProvider = _stateProvider,
|
|
StanleyUsesActualSpeed = true,
|
|
MaximumCommandSpeedMetersPerSecond = 0.50,
|
|
CycleObserver = controller =>
|
|
RecordControlCycle(
|
|
recorder,
|
|
controller,
|
|
controlPointRadiusMeters,
|
|
_stateProvider as
|
|
WheelFeedbackVehicleStateProvider)
|
|
};
|
|
|
|
recorder.Start();
|
|
|
|
try
|
|
{
|
|
_task = new DriveTask(movement.Get());
|
|
_task.Wait();
|
|
|
|
// 保留少量停车后原始Detour数据，并生成一帧处理后的静止状态。
|
|
Thread.Sleep(400);
|
|
recorder.UpdateCommand(0f, 0f);
|
|
if (_stateProvider.TryGetState(
|
|
out var stoppedState))
|
|
{
|
|
recorder.UpdateProcessedState(
|
|
stoppedState);
|
|
}
|
|
}
|
|
finally
|
|
{
|
|
_task?.Stop();
|
|
recorder.UpdateCommand(0f, 0f);
|
|
recorder.StopAndSave();
|
|
_painter.Clear();
|
|
_task = null;
|
|
_recorder = null;
|
|
_stateProvider = null;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 停止正在运行的测试、保存已有数据并清除Clumsy轨迹可视化。
|
|
/// </summary>
|
|
public override void TestStop()
|
|
{
|
|
_task?.Stop();
|
|
_recorder?.UpdateCommand(0f, 0f);
|
|
_recorder?.StopAndSave();
|
|
_painter.Clear();
|
|
_task = null;
|
|
_recorder = null;
|
|
_stateProvider = null;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 将离散轨迹点和相邻线段从SI单位转换为Clumsy毫米坐标后绘制。
|
|
/// </summary>
|
|
private void DrawTrajectory(
|
|
Trajectory.Trajectory2D trajectory)
|
|
{
|
|
_painter.Clear();
|
|
|
|
for (var index = 0;
|
|
index < trajectory.Count;
|
|
index++)
|
|
{
|
|
var point = ToMillimeterVector(
|
|
trajectory[index].PoseInWorld);
|
|
_painter.DrawDot(
|
|
Color.Cyan,
|
|
point.X,
|
|
point.Y,
|
|
3f);
|
|
|
|
if (index == 0)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
var previousPoint = ToMillimeterVector(
|
|
trajectory[index - 1].PoseInWorld);
|
|
_painter.DrawLine(
|
|
Color.DeepSkyBlue,
|
|
previousPoint.X,
|
|
previousPoint.Y,
|
|
point.X,
|
|
point.Y,
|
|
width: 2);
|
|
}
|
|
|
|
var start = ToMillimeterVector(
|
|
trajectory.StartPoint.PoseInWorld);
|
|
var end = ToMillimeterVector(
|
|
trajectory.EndPoint.PoseInWorld);
|
|
_painter.DrawDot(
|
|
Color.LimeGreen,
|
|
start.X,
|
|
start.Y,
|
|
8f);
|
|
_painter.DrawDot(
|
|
Color.OrangeRed,
|
|
end.X,
|
|
end.Y,
|
|
8f);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 将控制器本周期使用的状态和最终GCP命令同步给实验记录器。
|
|
/// </summary>
|
|
private static void RecordControlCycle(
|
|
TrackingExperimentRecorder recorder,
|
|
ParkingGeometricController controller,
|
|
double controlPointRadiusMeters,
|
|
WheelFeedbackVehicleStateProvider stateProvider)
|
|
{
|
|
if (controller.LastVehicleState.HasValue)
|
|
{
|
|
recorder.UpdateProcessedState(
|
|
controller.LastVehicleState.Value);
|
|
}
|
|
|
|
UpdateVelocityDiagnostics(
|
|
recorder,
|
|
stateProvider);
|
|
|
|
if (!controller.LastCommand.HasValue)
|
|
{
|
|
return;
|
|
}
|
|
|
|
if (controller.LastProjection.HasValue &&
|
|
controller.LastReferenceSpeedMetersPerSecond.HasValue)
|
|
{
|
|
var projection =
|
|
controller.LastProjection.Value;
|
|
recorder.UpdateControlReference(
|
|
projection.ArcLengthMeters,
|
|
controller.LastReferenceSpeedMetersPerSecond.Value,
|
|
projection.LateralErrorMeters,
|
|
projection.HeadingErrorRadians,
|
|
projection.DistanceToTrajectoryMeters,
|
|
projection.RemainingDistanceMeters);
|
|
}
|
|
|
|
var command = controller.LastCommand.Value;
|
|
recorder.UpdateGcpCommand(
|
|
command.FrontAngleRadians,
|
|
command.RearAngleRadians);
|
|
var curvaturePerMeter = Math.Tan(
|
|
command.FrontAngleRadians) /
|
|
controlPointRadiusMeters;
|
|
var angularSpeedRadiansPerSecond =
|
|
command.SpeedMetersPerSecond *
|
|
curvaturePerMeter;
|
|
|
|
recorder.UpdateCommand(
|
|
(float)command.SpeedMetersPerSecond,
|
|
(float)angularSpeedRadiansPerSecond);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 将同一周期的Detour速度和轮速解算速度写入实验记录器。
|
|
/// </summary>
|
|
private static void UpdateVelocityDiagnostics(
|
|
TrackingExperimentRecorder recorder,
|
|
WheelFeedbackVehicleStateProvider stateProvider)
|
|
{
|
|
if (stateProvider == null ||
|
|
!stateProvider.TryGetLatestVelocityDiagnostics(
|
|
out var detourBodyVx,
|
|
out var detourVelocityValid,
|
|
out var rawWheelBodyVx,
|
|
out var filteredWheelBodyVx,
|
|
out var wheelVelocityValid))
|
|
{
|
|
return;
|
|
}
|
|
|
|
recorder.UpdateVelocityDiagnostics(
|
|
detourBodyVx,
|
|
detourVelocityValid,
|
|
rawWheelBodyVx,
|
|
filteredWheelBodyVx,
|
|
wheelVelocityValid);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 将Shared世界坐标系米制位姿转换为Clumsy绘图和旧记录器使用的毫米坐标。
|
|
/// </summary>
|
|
private static Vector2 ToMillimeterVector(
|
|
Pose2D poseInWorld)
|
|
{
|
|
return new Vector2(
|
|
(float)(
|
|
poseInWorld.XMeters *
|
|
MillimetersPerMeter),
|
|
(float)(
|
|
poseInWorld.YMeters *
|
|
MillimetersPerMeter));
|
|
}
|
|
|
|
}
|
|
|
|
/// <summary>
|
|
/// 从当前Detour位姿开始执行“3m直线—左半圆—3m直线”新版控制器跟踪实验。
|
|
/// </summary>
|
|
[MovementTest(name = "新版控制器：直线-左半圆-直线轨迹跟踪")]
|
|
public sealed class NewControllerStraightSemicircleStraightTest
|
|
: MovementTest
|
|
{
|
|
private const float MillimetersPerMeter = 1000f;
|
|
|
|
private readonly Painter _painter =
|
|
UI.GetPainter(
|
|
"NewControllerStraightSemicircleStraight");
|
|
|
|
private DriveTask _task;
|
|
private TrackingExperimentRecorder _recorder;
|
|
private IVehicleStateProvider _stateProvider;
|
|
|
|
/// <summary>
|
|
/// 获取或设置本次测试编号，用于区分重复实验CSV。
|
|
/// </summary>
|
|
public int TrialNumber = 1;
|
|
|
|
/// <summary>
|
|
/// 获取或设置半圆前后两段直线的长度，单位为m。
|
|
/// </summary>
|
|
public double StraightLengthMeters = 3.0;
|
|
|
|
/// <summary>
|
|
/// 获取或设置左转半圆的转弯半径，单位为m。
|
|
/// </summary>
|
|
public double TurnRadiusMeters = 2.0;
|
|
|
|
/// <summary>
|
|
/// 获取或设置直线与等曲率转弯之间的曲率过渡长度，单位为m。
|
|
/// </summary>
|
|
public double CurvatureTransitionLengthMeters = 0.70;
|
|
|
|
/// <summary>
|
|
/// 获取或设置两段直线的最大参考速度，单位为m/s。
|
|
/// </summary>
|
|
public double StraightMaximumSpeedMetersPerSecond = 0.40;
|
|
|
|
/// <summary>
|
|
/// 获取或设置半圆段的最大参考速度，单位为m/s。
|
|
/// </summary>
|
|
public double SemicircleMaximumSpeedMetersPerSecond = 0.30;
|
|
|
|
/// <summary>
|
|
/// 获取或设置参考速度加速度，单位为m/s²。
|
|
/// </summary>
|
|
public double AccelerationMetersPerSecondSquared = 0.20;
|
|
|
|
/// <summary>
|
|
/// 获取或设置参考速度减速度，单位为m/s²。
|
|
/// </summary>
|
|
public double DecelerationMetersPerSecondSquared = 0.12;
|
|
|
|
/// <summary>
|
|
/// 获取或设置离散轨迹点间距，单位为m。
|
|
/// </summary>
|
|
public double PointSpacingMeters = 0.02;
|
|
|
|
/// <summary>
|
|
/// 读取当前位姿、绘制组合轨迹并启动新版轨迹跟踪动作。
|
|
/// </summary>
|
|
public override void Test()
|
|
{
|
|
if (_task != null)
|
|
{
|
|
Console.WriteLine(
|
|
"新版直线-左半圆-直线测试已经在运行，请先停止当前测试。");
|
|
return;
|
|
}
|
|
|
|
if (!TrajectoryExperimentInput
|
|
.TryReadLateralOffsetMeters(
|
|
out var lateralOffsetMeters))
|
|
{
|
|
return;
|
|
}
|
|
|
|
if (!MovementTestPreparation.AreWheelsForward())
|
|
{
|
|
return;
|
|
}
|
|
|
|
var chassis =
|
|
PilotDefinition.Chassis as MultiWheelChassis;
|
|
if (chassis == null)
|
|
{
|
|
Console.WriteLine(
|
|
"当前底盘不是MultiWheelChassis，无法执行新版轨迹测试。");
|
|
return;
|
|
}
|
|
|
|
var detourStateProvider =
|
|
new DetourVehicleStateProvider();
|
|
if (!detourStateProvider.TryGetState(
|
|
out var initialState))
|
|
{
|
|
Console.WriteLine(
|
|
"无法读取有效Detour起点位姿：" +
|
|
detourStateProvider.LastFailureReason);
|
|
_stateProvider = null;
|
|
return;
|
|
}
|
|
|
|
_stateProvider =
|
|
new WheelFeedbackVehicleStateProvider(
|
|
detourStateProvider,
|
|
chassis);
|
|
|
|
var trajectoryStartPose =
|
|
TrajectoryExperimentInput.OffsetPoseLaterally(
|
|
initialState.PoseInWorld,
|
|
lateralOffsetMeters);
|
|
var trajectory =
|
|
TestTrajectoryFactory
|
|
.CreateStraightLeftSemicircleStraight(
|
|
trajectoryStartPose,
|
|
StraightLengthMeters,
|
|
TurnRadiusMeters,
|
|
CurvatureTransitionLengthMeters,
|
|
StraightMaximumSpeedMetersPerSecond,
|
|
SemicircleMaximumSpeedMetersPerSecond,
|
|
AccelerationMetersPerSecondSquared,
|
|
DecelerationMetersPerSecondSquared,
|
|
PointSpacingMeters);
|
|
|
|
DrawTrajectory(trajectory);
|
|
|
|
var referenceStart = ToMillimeterVector(
|
|
trajectory.StartPoint.PoseInWorld);
|
|
var referenceEnd = ToMillimeterVector(
|
|
trajectory.EndPoint.PoseInWorld);
|
|
var recorder =
|
|
new TrackingExperimentRecorder(
|
|
controllerName: "NewStanleyPid",
|
|
trajectoryName:
|
|
TrajectoryExperimentInput.BuildTrajectoryName(
|
|
"ProfiledStraightSmoothLeftTurnStraight",
|
|
lateralOffsetMeters),
|
|
trialNumber: TrialNumber,
|
|
referenceStart: referenceStart,
|
|
referenceEnd: referenceEnd,
|
|
referenceSpeed:
|
|
(float)StraightMaximumSpeedMetersPerSecond,
|
|
sampleIntervalMs: 50,
|
|
referenceAccelerationMetersPerSecondSquared:
|
|
(float)AccelerationMetersPerSecondSquared,
|
|
referenceDecelerationMetersPerSecondSquared:
|
|
(float)DecelerationMetersPerSecondSquared,
|
|
diagnosticChassis: chassis);
|
|
_recorder = recorder;
|
|
|
|
var controlPointRadiusMeters =
|
|
chassis.ControlPointRadius /
|
|
MillimetersPerMeter;
|
|
var movement =
|
|
new TrajectoryTrackingMovement
|
|
{
|
|
Trajectory = trajectory,
|
|
StateProvider = _stateProvider,
|
|
StanleyUsesActualSpeed = true,
|
|
MaximumCommandSpeedMetersPerSecond =
|
|
StraightMaximumSpeedMetersPerSecond,
|
|
CycleObserver = controller =>
|
|
RecordControlCycle(
|
|
recorder,
|
|
controller,
|
|
controlPointRadiusMeters,
|
|
_stateProvider as
|
|
WheelFeedbackVehicleStateProvider)
|
|
};
|
|
|
|
recorder.Start();
|
|
|
|
try
|
|
{
|
|
_task = new DriveTask(movement.Get());
|
|
_task.Wait();
|
|
|
|
// 保留少量停车后原始Detour数据，并生成一帧处理后的静止状态。
|
|
Thread.Sleep(400);
|
|
recorder.UpdateCommand(0f, 0f);
|
|
if (_stateProvider.TryGetState(
|
|
out var stoppedState))
|
|
{
|
|
recorder.UpdateProcessedState(
|
|
stoppedState);
|
|
}
|
|
}
|
|
finally
|
|
{
|
|
_task?.Stop();
|
|
recorder.UpdateCommand(0f, 0f);
|
|
recorder.StopAndSave();
|
|
_painter.Clear();
|
|
_task = null;
|
|
_recorder = null;
|
|
_stateProvider = null;
|
|
}
|
|
}
|
|
|
|
/// <summary>
|
|
/// 停止组合轨迹测试、保存已有数据并清除Clumsy轨迹可视化。
|
|
/// </summary>
|
|
public override void TestStop()
|
|
{
|
|
_task?.Stop();
|
|
_recorder?.UpdateCommand(0f, 0f);
|
|
_recorder?.StopAndSave();
|
|
_painter.Clear();
|
|
_task = null;
|
|
_recorder = null;
|
|
_stateProvider = null;
|
|
}
|
|
|
|
/// <summary>
|
|
/// 将组合轨迹的离散点和相邻线段转换为Clumsy毫米坐标后绘制。
|
|
/// </summary>
|
|
private void DrawTrajectory(
|
|
Trajectory.Trajectory2D trajectory)
|
|
{
|
|
_painter.Clear();
|
|
|
|
for (var index = 0;
|
|
index < trajectory.Count;
|
|
index++)
|
|
{
|
|
var point = ToMillimeterVector(
|
|
trajectory[index].PoseInWorld);
|
|
_painter.DrawDot(
|
|
Color.Cyan,
|
|
point.X,
|
|
point.Y,
|
|
3f);
|
|
|
|
if (index == 0)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
var previousPoint = ToMillimeterVector(
|
|
trajectory[index - 1].PoseInWorld);
|
|
_painter.DrawLine(
|
|
Color.DeepSkyBlue,
|
|
previousPoint.X,
|
|
previousPoint.Y,
|
|
point.X,
|
|
point.Y,
|
|
width: 2);
|
|
}
|
|
|
|
var start = ToMillimeterVector(
|
|
trajectory.StartPoint.PoseInWorld);
|
|
var end = ToMillimeterVector(
|
|
trajectory.EndPoint.PoseInWorld);
|
|
_painter.DrawDot(
|
|
Color.LimeGreen,
|
|
start.X,
|
|
start.Y,
|
|
8f);
|
|
_painter.DrawDot(
|
|
Color.OrangeRed,
|
|
end.X,
|
|
end.Y,
|
|
8f);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 将组合轨迹控制周期的状态、参考量和最终GCP命令同步给实验记录器。
|
|
/// </summary>
|
|
private static void RecordControlCycle(
|
|
TrackingExperimentRecorder recorder,
|
|
ParkingGeometricController controller,
|
|
double controlPointRadiusMeters,
|
|
WheelFeedbackVehicleStateProvider stateProvider)
|
|
{
|
|
if (controller.LastVehicleState.HasValue)
|
|
{
|
|
recorder.UpdateProcessedState(
|
|
controller.LastVehicleState.Value);
|
|
}
|
|
|
|
UpdateVelocityDiagnostics(
|
|
recorder,
|
|
stateProvider);
|
|
|
|
if (!controller.LastCommand.HasValue)
|
|
{
|
|
return;
|
|
}
|
|
|
|
if (controller.LastProjection.HasValue &&
|
|
controller.LastReferenceSpeedMetersPerSecond.HasValue)
|
|
{
|
|
var projection =
|
|
controller.LastProjection.Value;
|
|
recorder.UpdateControlReference(
|
|
projection.ArcLengthMeters,
|
|
controller.LastReferenceSpeedMetersPerSecond.Value,
|
|
projection.LateralErrorMeters,
|
|
projection.HeadingErrorRadians,
|
|
projection.DistanceToTrajectoryMeters,
|
|
projection.RemainingDistanceMeters);
|
|
}
|
|
|
|
var command = controller.LastCommand.Value;
|
|
recorder.UpdateGcpCommand(
|
|
command.FrontAngleRadians,
|
|
command.RearAngleRadians);
|
|
var curvaturePerMeter = Math.Tan(
|
|
command.FrontAngleRadians) /
|
|
controlPointRadiusMeters;
|
|
var angularSpeedRadiansPerSecond =
|
|
command.SpeedMetersPerSecond *
|
|
curvaturePerMeter;
|
|
|
|
recorder.UpdateCommand(
|
|
(float)command.SpeedMetersPerSecond,
|
|
(float)angularSpeedRadiansPerSecond);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 将同一周期的Detour速度和轮速解算速度写入实验记录器。
|
|
/// </summary>
|
|
private static void UpdateVelocityDiagnostics(
|
|
TrackingExperimentRecorder recorder,
|
|
WheelFeedbackVehicleStateProvider stateProvider)
|
|
{
|
|
if (stateProvider == null ||
|
|
!stateProvider.TryGetLatestVelocityDiagnostics(
|
|
out var detourBodyVx,
|
|
out var detourVelocityValid,
|
|
out var rawWheelBodyVx,
|
|
out var filteredWheelBodyVx,
|
|
out var wheelVelocityValid))
|
|
{
|
|
return;
|
|
}
|
|
|
|
recorder.UpdateVelocityDiagnostics(
|
|
detourBodyVx,
|
|
detourVelocityValid,
|
|
rawWheelBodyVx,
|
|
filteredWheelBodyVx,
|
|
wheelVelocityValid);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 将Shared世界坐标系米制位姿转换为Clumsy绘图和记录器使用的毫米坐标。
|
|
/// </summary>
|
|
private static Vector2 ToMillimeterVector(
|
|
Pose2D poseInWorld)
|
|
{
|
|
return new Vector2(
|
|
(float)(
|
|
poseInWorld.XMeters *
|
|
MillimetersPerMeter),
|
|
(float)(
|
|
poseInWorld.YMeters *
|
|
MillimetersPerMeter));
|
|
}
|
|
}
|
|
}
|