打通新版Stanley轨迹跟踪闭环并补充实验测试与数据分析脚本
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Cursor
@@ -0,0 +1,297 @@
|
||||
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));
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user