First_Dev_Dept/ParkingRobot
10
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

控制器测试横向跟踪，增加记录

Browse Source
This commit is contained in:
yuxiang.shen
2026-08-10 10:10:21 +08:00
parent 88f651e0c2
commit 2f9e4285d3
22 changed files with 769 additions and 92 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheelC/Experiments/NewControllerTrackingTests.cs
+210 -19
View File
@@ -1,5 +1,6 @@
using System;
using System.Drawing;
using System.Globalization;
using System.Numerics;
using System.Threading;
using ClumsyCore;
@@ -13,6 +14,82 @@ 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>
@@ -27,7 +104,7 @@ namespace MultiWheelC
private DriveTask _task;
private TrackingExperimentRecorder _recorder;
private DetourVehicleStateProvider _stateProvider;
private IVehicleStateProvider _stateProvider;
/// <summary>
/// 获取或设置本次测试编号，用于区分重复实验CSV。
@@ -37,7 +114,7 @@ namespace MultiWheelC
/// <summary>
/// 获取或设置4m直线的巡航参考速度，单位为m/s。
/// </summary>
public double CruiseSpeedMetersPerSecond = 0.30;
public double CruiseSpeedMetersPerSecond = 0.40;
/// <summary>
/// 获取或设置参考速度加速度，单位为m/s²。
@@ -66,6 +143,13 @@ namespace MultiWheelC
return;
}
if (!TrajectoryExperimentInput
.TryReadLateralOffsetMeters(
out var lateralOffsetMeters))
{
return;
}
if (!MovementTestPreparation.AreWheelsForward())
{
return;
@@ -80,21 +164,30 @@ namespace MultiWheelC
return;
}
_stateProvider =
var detourStateProvider =
new DetourVehicleStateProvider();
if (!_stateProvider.TryGetState(
if (!detourStateProvider.TryGetState(
out var initialState))
{
Console.WriteLine(
"无法读取有效Detour起点位姿：" +
_stateProvider.LastFailureReason);
detourStateProvider.LastFailureReason);
_stateProvider = null;
return;
}
_stateProvider =
new WheelFeedbackVehicleStateProvider(
detourStateProvider,
chassis);
var trajectoryStartPose =
TrajectoryExperimentInput.OffsetPoseLaterally(
initialState.PoseInWorld,
lateralOffsetMeters);
var trajectory =
TestTrajectoryFactory.CreateStraight4Meters(
initialState.PoseInWorld,
trajectoryStartPose,
CruiseSpeedMetersPerSecond,
AccelerationMetersPerSecondSquared,
DecelerationMetersPerSecondSquared,
@@ -109,7 +202,10 @@ namespace MultiWheelC
var recorder =
new TrackingExperimentRecorder(
controllerName: "NewStanleyPid",
trajectoryName: "ProfiledStraight4m",
trajectoryName:
TrajectoryExperimentInput.BuildTrajectoryName(
"ProfiledStraight4m",
lateralOffsetMeters),
trialNumber: TrialNumber,
referenceStart: referenceStart,
referenceEnd: referenceEnd,
@@ -119,7 +215,8 @@ namespace MultiWheelC
referenceAccelerationMetersPerSecondSquared:
(float)AccelerationMetersPerSecondSquared,
referenceDecelerationMetersPerSecondSquared:
(float)DecelerationMetersPerSecondSquared);
(float)DecelerationMetersPerSecondSquared,
diagnosticChassis: chassis);
_recorder = recorder;
var controlPointRadiusMeters =
@@ -130,12 +227,15 @@ namespace MultiWheelC
{
Trajectory = trajectory,
StateProvider = _stateProvider,
StanleyUsesActualSpeed = true,
MaximumCommandSpeedMetersPerSecond = 0.50,
CycleObserver = controller =>
RecordControlCycle(
recorder,
controller,
controlPointRadiusMeters)
controlPointRadiusMeters,
_stateProvider as
WheelFeedbackVehicleStateProvider)
};
recorder.Start();
@@ -239,7 +339,8 @@ namespace MultiWheelC
private static void RecordControlCycle(
TrackingExperimentRecorder recorder,
ParkingGeometricController controller,
double controlPointRadiusMeters)
double controlPointRadiusMeters,
WheelFeedbackVehicleStateProvider stateProvider)
{
if (controller.LastVehicleState.HasValue)
{
@@ -247,6 +348,10 @@ namespace MultiWheelC
controller.LastVehicleState.Value);
}
UpdateVelocityDiagnostics(
recorder,
stateProvider);
if (!controller.LastCommand.HasValue)
{
return;
@@ -267,6 +372,9 @@ namespace MultiWheelC
}
var command = controller.LastCommand.Value;
recorder.UpdateGcpCommand(
command.FrontAngleRadians,
command.RearAngleRadians);
var curvaturePerMeter = Math.Tan(
command.FrontAngleRadians) /
controlPointRadiusMeters;
@@ -279,6 +387,32 @@ namespace MultiWheelC
(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>
@@ -293,6 +427,7 @@ namespace MultiWheelC
poseInWorld.YMeters *
MillimetersPerMeter));
}
}
/// <summary>
@@ -310,7 +445,7 @@ namespace MultiWheelC
private DriveTask _task;
private TrackingExperimentRecorder _recorder;
private DetourVehicleStateProvider _stateProvider;
private IVehicleStateProvider _stateProvider;
/// <summary>
/// 获取或设置本次测试编号，用于区分重复实验CSV。
@@ -369,6 +504,13 @@ namespace MultiWheelC
return;
}
if (!TrajectoryExperimentInput
.TryReadLateralOffsetMeters(
out var lateralOffsetMeters))
{
return;
}
if (!MovementTestPreparation.AreWheelsForward())
{
return;
@@ -383,22 +525,31 @@ namespace MultiWheelC
return;
}
_stateProvider =
var detourStateProvider =
new DetourVehicleStateProvider();
if (!_stateProvider.TryGetState(
if (!detourStateProvider.TryGetState(
out var initialState))
{
Console.WriteLine(
"无法读取有效Detour起点位姿：" +
_stateProvider.LastFailureReason);
detourStateProvider.LastFailureReason);
_stateProvider = null;
return;
}
_stateProvider =
new WheelFeedbackVehicleStateProvider(
detourStateProvider,
chassis);
var trajectoryStartPose =
TrajectoryExperimentInput.OffsetPoseLaterally(
initialState.PoseInWorld,
lateralOffsetMeters);
var trajectory =
TestTrajectoryFactory
.CreateStraightLeftSemicircleStraight(
initialState.PoseInWorld,
trajectoryStartPose,
StraightLengthMeters,
TurnRadiusMeters,
CurvatureTransitionLengthMeters,
@@ -418,7 +569,9 @@ namespace MultiWheelC
new TrackingExperimentRecorder(
controllerName: "NewStanleyPid",
trajectoryName:
"ProfiledStraightSmoothLeftTurnStraight",
TrajectoryExperimentInput.BuildTrajectoryName(
"ProfiledStraightSmoothLeftTurnStraight",
lateralOffsetMeters),
trialNumber: TrialNumber,
referenceStart: referenceStart,
referenceEnd: referenceEnd,
@@ -428,7 +581,8 @@ namespace MultiWheelC
referenceAccelerationMetersPerSecondSquared:
(float)AccelerationMetersPerSecondSquared,
referenceDecelerationMetersPerSecondSquared:
(float)DecelerationMetersPerSecondSquared);
(float)DecelerationMetersPerSecondSquared,
diagnosticChassis: chassis);
_recorder = recorder;
var controlPointRadiusMeters =
@@ -439,13 +593,16 @@ namespace MultiWheelC
{
Trajectory = trajectory,
StateProvider = _stateProvider,
StanleyUsesActualSpeed = true,
MaximumCommandSpeedMetersPerSecond =
StraightMaximumSpeedMetersPerSecond,
CycleObserver = controller =>
RecordControlCycle(
recorder,
controller,
controlPointRadiusMeters)
controlPointRadiusMeters,
_stateProvider as
WheelFeedbackVehicleStateProvider)
};
recorder.Start();
@@ -549,7 +706,8 @@ namespace MultiWheelC
private static void RecordControlCycle(
TrackingExperimentRecorder recorder,
ParkingGeometricController controller,
double controlPointRadiusMeters)
double controlPointRadiusMeters,
WheelFeedbackVehicleStateProvider stateProvider)
{
if (controller.LastVehicleState.HasValue)
{
@@ -557,6 +715,10 @@ namespace MultiWheelC
controller.LastVehicleState.Value);
}
UpdateVelocityDiagnostics(
recorder,
stateProvider);
if (!controller.LastCommand.HasValue)
{
return;
@@ -577,6 +739,9 @@ namespace MultiWheelC
}
var command = controller.LastCommand.Value;
recorder.UpdateGcpCommand(
command.FrontAngleRadians,
command.RearAngleRadians);
var curvaturePerMeter = Math.Tan(
command.FrontAngleRadians) /
controlPointRadiusMeters;
@@ -589,6 +754,32 @@ namespace MultiWheelC
(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>