298 lines
9.9 KiB
C#
298 lines
9.9 KiB
C#
using System;
|
|
using System.Drawing;
|
|
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>
|
|
/// 从当前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 DetourVehicleStateProvider _stateProvider;
|
|
|
|
/// <summary>
|
|
/// 获取或设置本次测试编号，用于区分重复实验CSV。
|
|
/// </summary>
|
|
public int TrialNumber = 1;
|
|
|
|
/// <summary>
|
|
/// 获取或设置4m直线的巡航参考速度，单位为m/s。
|
|
/// </summary>
|
|
public double CruiseSpeedMetersPerSecond = 0.30;
|
|
|
|
/// <summary>
|
|
/// 获取或设置参考速度加速度，单位为m/s²。
|
|
/// </summary>
|
|
public double AccelerationMetersPerSecondSquared = 0.20;
|
|
|
|
/// <summary>
|
|
/// 获取或设置参考速度减速度，单位为m/s²。
|
|
/// </summary>
|
|
public double DecelerationMetersPerSecondSquared = 0.20;
|
|
|
|
/// <summary>
|
|
/// 获取或设置离散轨迹点间距，单位为m。
|
|
/// </summary>
|
|
public double PointSpacingMeters = 0.02;
|
|
|
|
/// <summary>
|
|
/// 读取当前位姿、绘制离散轨迹并启动新版轨迹跟踪动作。
|
|
/// </summary>
|
|
public override void Test()
|
|
{
|
|
if (_task != null)
|
|
{
|
|
Console.WriteLine(
|
|
"新版4m直线轨迹测试已经在运行，请先停止当前测试。");
|
|
return;
|
|
}
|
|
|
|
if (!MovementTestPreparation.AreWheelsForward())
|
|
{
|
|
return;
|
|
}
|
|
|
|
var chassis =
|
|
PilotDefinition.Chassis as MultiWheelChassis;
|
|
if (chassis == null)
|
|
{
|
|
Console.WriteLine(
|
|
"当前底盘不是MultiWheelChassis，无法执行新版轨迹测试。");
|
|
return;
|
|
}
|
|
|
|
_stateProvider =
|
|
new DetourVehicleStateProvider();
|
|
if (!_stateProvider.TryGetState(
|
|
out var initialState))
|
|
{
|
|
Console.WriteLine(
|
|
"无法读取有效Detour起点位姿：" +
|
|
_stateProvider.LastFailureReason);
|
|
_stateProvider = null;
|
|
return;
|
|
}
|
|
|
|
var trajectory =
|
|
TestTrajectoryFactory.CreateStraight4Meters(
|
|
initialState.PoseInWorld,
|
|
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: "ProfiledStraight4m",
|
|
trialNumber: TrialNumber,
|
|
referenceStart: referenceStart,
|
|
referenceEnd: referenceEnd,
|
|
referenceSpeed:
|
|
(float)CruiseSpeedMetersPerSecond,
|
|
sampleIntervalMs: 50,
|
|
referenceAccelerationMetersPerSecondSquared:
|
|
(float)AccelerationMetersPerSecondSquared,
|
|
referenceDecelerationMetersPerSecondSquared:
|
|
(float)DecelerationMetersPerSecondSquared);
|
|
_recorder = recorder;
|
|
|
|
var controlPointRadiusMeters =
|
|
chassis.ControlPointRadius /
|
|
MillimetersPerMeter;
|
|
var movement =
|
|
new TrajectoryTrackingMovement
|
|
{
|
|
Trajectory = trajectory,
|
|
StateProvider = _stateProvider,
|
|
MaximumCommandSpeedMetersPerSecond = 0.50,
|
|
CycleObserver = controller =>
|
|
RecordControlCycle(
|
|
recorder,
|
|
controller,
|
|
controlPointRadiusMeters)
|
|
};
|
|
|
|
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)
|
|
{
|
|
if (controller.LastVehicleState.HasValue)
|
|
{
|
|
recorder.UpdateProcessedState(
|
|
controller.LastVehicleState.Value);
|
|
}
|
|
|
|
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;
|
|
var curvaturePerMeter = Math.Tan(
|
|
command.FrontAngleRadians) /
|
|
controlPointRadiusMeters;
|
|
var angularSpeedRadiansPerSecond =
|
|
command.SpeedMetersPerSecond *
|
|
curvaturePerMeter;
|
|
|
|
recorder.UpdateCommand(
|
|
(float)command.SpeedMetersPerSecond,
|
|
(float)angularSpeedRadiansPerSecond);
|
|
}
|
|
|
|
/// <summary>
|
|
/// 将Shared世界坐标系米制位姿转换为Clumsy绘图和旧记录器使用的毫米坐标。
|
|
/// </summary>
|
|
private static Vector2 ToMillimeterVector(
|
|
Pose2D poseInWorld)
|
|
{
|
|
return new Vector2(
|
|
(float)(
|
|
poseInWorld.XMeters *
|
|
MillimetersPerMeter),
|
|
(float)(
|
|
poseInWorld.YMeters *
|
|
MillimetersPerMeter));
|
|
}
|
|
}
|
|
}
|