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

完善蟹行虚拟阿克曼与SendMotion运动坐标系，并添加轮速诊断日志

Browse Source 
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
yuxiang.shen
2026-07-30 18:10:22 +08:00
co-authored by Cursor
parent 7e05ff098e
commit c1fe73caec
92 changed files with 1794 additions and 776 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ClumsyPilot/MovementTests.cs
+49 -445
View File
@@ -103,7 +103,7 @@ namespace MultiWheelC
}
}
[MovementTest(name = "旧版SendMotion：连续前进4m")]
[MovementTest(name = "SendMotion：连续前进4m")]
public class TestForward4m : MovementTest
{
public float DistanceMillimeters = 4000f; // 测试距离，单位mm。
@@ -178,15 +178,25 @@ namespace MultiWheelC
}
}
[MovementTest(name = "测试原地自转90°")]
public class TestRotate90 : MovementTest
public abstract class InPlaceRotateTestBase : MovementTest
{
public float RelativeAngleDegrees = 90f; // 相对当前航向的旋转角度，逆时针为正。
public float RelativeAngleDegrees; // 相对当前航向的旋转角度，逆时针为正。
public float MaxAngularSpeedDegreesPerSecond = 20f; // PID输出的最大角速度。
public int TrialNumber = 1; // 重复实验编号。
private DriveTask _task;
private TrackingExperimentRecorder _recorder;
private readonly string _trajectoryName;
protected InPlaceRotateTestBase(
float relativeAngleDegrees,
string trajectoryName)
{
RelativeAngleDegrees =
relativeAngleDegrees;
_trajectoryName =
trajectoryName;
}
// 从当前Detour航向开始，原地相对旋转指定角度并记录实验数据。
public override void Test()
@@ -222,7 +232,7 @@ namespace MultiWheelC
_recorder = new TrackingExperimentRecorder(
controllerName: "InPlaceRotatePID",
trajectoryName: "Rotate90",
trajectoryName: _trajectoryName,
trialNumber: TrialNumber,
referenceStart: rotationCenter,
referenceEnd: rotationCenter,
@@ -297,7 +307,27 @@ namespace MultiWheelC
}
}
[MovementTest(name = "旧版SendMotion：左转90°圆弧")]
[MovementTest(name = "SendXYThSpeed：原地自转90°")]
public sealed class TestRotate90 :
InPlaceRotateTestBase
{
public TestRotate90()
: base(90f, "Rotate90")
{
}
}
[MovementTest(name = "SendXYThSpeed：原地自转180°")]
public sealed class TestRotate180 :
InPlaceRotateTestBase
{
public TestRotate180()
: base(180f, "Rotate180")
{
}
}
[MovementTest(name = "SendMotion：左转90°半径2m圆弧")]
public class TestArcMovement : MovementTest
{
public float RadiusMillimeters = 2000f; // 左转圆的半径，单位mm。
@@ -425,7 +455,7 @@ namespace MultiWheelC
}
}
[MovementTest(name = "测试蟹行前进4m")]
[MovementTest(name = "SendMotion：蟹行直线4m")]
public class TestCrabForward4m : MovementTest
{
public float DistanceMillimeters = 4000f;
@@ -455,6 +485,10 @@ namespace MultiWheelC
var tracker = new CrabMotionFrameTracker
{
CommandBackend =
CrabMotionFrameTracker
.ChassisCommandBackend
.SendMotion,
PathKind =
CrabMotionFrameTracker
.ReferencePathKind.Straight,
@@ -468,7 +502,7 @@ namespace MultiWheelC
_recorder = new TrackingExperimentRecorder(
controllerName:
"CrabMotionFrameTracker",
"CrabSendMotionTracker",
trajectoryName:
"CrabStraight4m",
trialNumber: TrialNumber,
@@ -543,7 +577,7 @@ namespace MultiWheelC
}
}
[MovementTest(name = "测试蟹行左转90°圆弧")]
[MovementTest(name = "SendMotion：蟹行左转90°半径2m圆弧")]
public class TestCrabLeftArc90 : MovementTest
{
public float RadiusMillimeters = 2000f;
@@ -577,6 +611,10 @@ namespace MultiWheelC
location.th * Math.PI / 180.0;
var tracker = new CrabMotionFrameTracker
{
CommandBackend =
CrabMotionFrameTracker
.ChassisCommandBackend
.SendMotion,
PathKind =
CrabMotionFrameTracker
.ReferencePathKind.LeftArc,
@@ -593,7 +631,7 @@ namespace MultiWheelC
_recorder = new TrackingExperimentRecorder(
controllerName:
"CrabMotionFrameTracker",
"CrabSendMotionTracker",
trajectoryName:
$"CrabLeftArc90_R{RadiusMillimeters:0}mm",
trialNumber: TrialNumber,
@@ -639,131 +677,7 @@ namespace MultiWheelC
}
}
[MovementTest(name = "测试蟹行4m S型曲线")]
public class TestCrabSCurve4m : MovementTest
{
public float LengthMillimeters = 4000f;
public float LateralOffsetMillimeters = 400f;
public float CruiseSpeed = 0.2f;
public int TrialNumber = 1;
private DriveTask _task;
private TrackingExperimentRecorder _recorder;
// 将车体左侧作为运动前向，跟踪与普通测试参数一致的4m S型曲线。
public override void Test()
{
if (float.IsNaN(LengthMillimeters) ||
float.IsInfinity(LengthMillimeters) ||
LengthMillimeters <= 0f ||
float.IsNaN(
LateralOffsetMillimeters) ||
float.IsInfinity(
LateralOffsetMillimeters) ||
LateralOffsetMillimeters <= 0f ||
float.IsNaN(CruiseSpeed) ||
float.IsInfinity(CruiseSpeed) ||
CruiseSpeed <= 0f)
{
Console.WriteLine(
"蟹行S型曲线测试参数无效。");
return;
}
var location =
DetourInterface.getCartLocation();
if (double.IsNaN(location.x) ||
double.IsInfinity(location.x) ||
double.IsNaN(location.y) ||
double.IsInfinity(location.y) ||
double.IsNaN(location.th) ||
double.IsInfinity(location.th))
{
Console.WriteLine(
"Detour当前位姿无效，取消蟹行S型曲线测试。");
return;
}
var source = new Vector2(
(float)location.x,
(float)location.y);
var bodyYawRadians =
location.th * Math.PI / 180.0;
var initialMotionYaw =
bodyYawRadians + Math.PI / 2.0;
var destination = new Vector2(
source.X +
LengthMillimeters *
(float)Math.Cos(
initialMotionYaw),
source.Y +
LengthMillimeters *
(float)Math.Sin(
initialMotionYaw));
var tracker = new CrabMotionFrameTracker
{
PathKind =
CrabMotionFrameTracker
.ReferencePathKind.SCurve,
StartPosition = source,
InitialBodyYawRadians =
bodyYawRadians,
LengthMillimeters =
LengthMillimeters,
SCurveLateralOffsetMillimeters =
LateralOffsetMillimeters,
CruiseSpeed = CruiseSpeed
};
_recorder = new TrackingExperimentRecorder(
controllerName:
"CrabMotionFrameTracker",
trajectoryName:
$"CrabSCurve4m_A{LateralOffsetMillimeters:0}mm",
trialNumber: TrialNumber,
referenceStart: source,
referenceEnd: destination,
referenceSpeed: CruiseSpeed,
referenceMotionFrameYawDegrees: 90f);
tracker.CommandObserver =
(vx, vy, omega) =>
_recorder?.UpdateBodyCommand(
vx,
vy,
omega);
_recorder.Start();
try
{
_task = new DriveTask(tracker.Get());
_task.Wait();
Thread.Sleep(300);
}
finally
{
_task?.Stop();
_recorder?.UpdateBodyCommand(
0f,
0f,
0f);
_recorder?.StopAndSave();
_task = null;
_recorder = null;
}
}
public override void TestStop()
{
_task?.Stop();
_recorder?.UpdateBodyCommand(
0f,
0f,
0f);
_recorder?.StopAndSave();
}
}
[MovementTest(name = "旧版SendMotion：4m S型曲线")]
[MovementTest(name = "SendMotion：4m S型曲线")]
public class TestSCurve4m : MovementTest
{
public float LengthMillimeters = 4000f; // S型曲线纵向长度，单位mm。
@@ -955,193 +869,6 @@ namespace MultiWheelC
}
}
// C层单车测试：统一使用车体速度命令和SendXYThSpeed跟踪普通模式轨迹。
public abstract class XYThNormalTrajectoryTestBase : MovementTest
{
public float LengthMillimeters = 4000f;
public float RadiusMillimeters = 2000f;
public float LateralOffsetMillimeters = 400f;
public float CruiseSpeed = 0.3f;
public int TrialNumber = 1;
private DriveTask _task;
private TrackingExperimentRecorder _recorder;
protected abstract CrabMotionFrameTracker.ReferencePathKind ReferencePath { get; }
protected abstract string TrajectoryName { get; }
// C层单车测试：读取Detour起点并执行普通模式SendXYThSpeed轨迹。
public override void Test()
{
ValidateParameters();
if (!TryReadDetourPose(
out var source,
out var initialBodyYawRadians))
{
throw new InvalidOperationException(
"Detour当前位置或航向无效，无法开始新版SendXYThSpeed测试。");
}
var tracker = new CrabMotionFrameTracker
{
PathKind = ReferencePath,
// 普通模式的运动坐标系与车体坐标系重合。
MotionFrameYawInBodyRadians = 0.0,
StartPosition = source,
InitialBodyYawRadians = initialBodyYawRadians,
LengthMillimeters = LengthMillimeters,
RadiusMillimeters = RadiusMillimeters,
ArcSweepRadians = Math.PI / 2.0,
SCurveLateralOffsetMillimeters =
LateralOffsetMillimeters,
CruiseSpeed = CruiseSpeed,
};
var destination = ReferencePath ==
CrabMotionFrameTracker.ReferencePathKind
.LeftArc
? tracker.GetArcDestination()
: new Vector2(
source.X +
LengthMillimeters *
(float)Math.Cos(initialBodyYawRadians),
source.Y +
LengthMillimeters *
(float)Math.Sin(initialBodyYawRadians));
_recorder = new TrackingExperimentRecorder(
controllerName: "UnifiedXYThTracker",
trajectoryName: TrajectoryName,
trialNumber: TrialNumber,
referenceStart: source,
referenceEnd: destination,
referenceSpeed: CruiseSpeed);
tracker.CommandObserver =
(vx, vy, omegaRadiansPerSecond) =>
_recorder?.UpdateBodyCommand(
vx,
vy,
omegaRadiansPerSecond);
_recorder.Start();
_task = new DriveTask(tracker.Get());
try
{
_task.Wait();
}
finally
{
_recorder?.UpdateBodyCommand(0f, 0f, 0f);
_recorder?.StopAndSave();
_recorder = null;
_task = null;
}
}
// C层单车测试：停止新版SendXYThSpeed轨迹并保存已有记录。
public override void TestStop()
{
_task?.Stop();
_recorder?.UpdateBodyCommand(0f, 0f, 0f);
_recorder?.StopAndSave();
}
// C层单车测试：检查新版轨迹的长度、半径、偏移和速度参数。
private void ValidateParameters()
{
if (!IsPositiveFinite(LengthMillimeters))
throw new ArgumentOutOfRangeException(
nameof(LengthMillimeters),
"轨迹长度必须是正有限值。");
if (!IsPositiveFinite(RadiusMillimeters))
throw new ArgumentOutOfRangeException(
nameof(RadiusMillimeters),
"圆弧半径必须是正有限值。");
if (!IsPositiveFinite(LateralOffsetMillimeters))
throw new ArgumentOutOfRangeException(
nameof(LateralOffsetMillimeters),
"S型曲线横向偏移必须是正有限值。");
if (!IsPositiveFinite(CruiseSpeed))
throw new ArgumentOutOfRangeException(
nameof(CruiseSpeed),
"巡航速度必须是正有限值。");
}
// C层单车测试：读取并验证Detour毫米坐标和角度制航向。
private static bool TryReadDetourPose(
out Vector2 position,
out double yawRadians)
{
var location = DetourInterface.getCartLocation();
var x = location.x;
var y = location.y;
var thetaDegrees = location.th;
position = new Vector2((float)x, (float)y);
yawRadians = thetaDegrees * Math.PI / 180.0;
return IsFinite(x) &&
IsFinite(y) &&
IsFinite(thetaDegrees);
}
// C层单车测试：判断浮点参数是否为有限值。
private static bool IsFinite(double value)
{
return !double.IsNaN(value) &&
!double.IsInfinity(value);
}
// C层单车测试：判断浮点参数是否为正有限值。
private static bool IsPositiveFinite(double value)
{
return IsFinite(value) && value > 0.0;
}
}
[MovementTest(name = "新版SendXYThSpeed：连续前进4m")]
public sealed class TestXYThForward4m :
XYThNormalTrajectoryTestBase
{
protected override CrabMotionFrameTracker.ReferencePathKind
ReferencePath =>
CrabMotionFrameTracker.ReferencePathKind.Straight;
protected override string TrajectoryName =>
"XYThStraight4m";
}
[MovementTest(name = "新版SendXYThSpeed：左转90°圆弧")]
public sealed class TestXYThLeftArc90 :
XYThNormalTrajectoryTestBase
{
protected override CrabMotionFrameTracker.ReferencePathKind
ReferencePath =>
CrabMotionFrameTracker.ReferencePathKind.LeftArc;
protected override string TrajectoryName =>
$"XYThLeftArc90_R{RadiusMillimeters:0}mm";
}
[MovementTest(name = "新版SendXYThSpeed：4m S型曲线")]
public sealed class TestXYThSCurve4m :
XYThNormalTrajectoryTestBase
{
protected override CrabMotionFrameTracker.ReferencePathKind
ReferencePath =>
CrabMotionFrameTracker.ReferencePathKind.SCurve;
protected override string TrajectoryName =>
$"XYThSCurve4m_A{LateralOffsetMillimeters:0}mm";
}
public abstract class ClampMovementTestBase : MovementTest
{
public float TimeoutSeconds = 30f; // 动作超时时间，单位s。
@@ -1223,126 +950,3 @@ namespace MultiWheelC
}
#region 旧版测试
// public abstract class DstTrackerTestBase : MovementTest
// {
// public bool UseInteractivePick = true;
// public float srcX;
// public float srcY;
// public float dstX;
// public float dstY;
// public float carDirectionBias;
// private readonly Painter _painter = UI.GetPainter("DstTrackerTest");
// private DriveTask _dt;
// protected DstTrackerTestBase(float defaultCarDirectionBias)
// {
// carDirectionBias = defaultCarDirectionBias;
// }
// public override void TestStop()
// {
// _dt?.Stop();
// _painter?.Clear();
// }
// public override void Test()
// {
// Vector2 p1;
// Vector2 p2;
// if (UseInteractivePick)
// {
// p1 = UI.GetPoint("point1");
// p2 = UI.GetPoint("point2");
// }
// else
// {
// p1 = new Vector2(srcX, srcY);
// p2 = new Vector2(dstX, dstY);
// }
// _painter.Clear();
// _dt = new DriveTask(new DstTracker
// {
// Src = p1,
// Dst = p2,
// CarDirectionBias = carDirectionBias,
// }.Get());
// _dt.Wait();
// }
// }
// [MovementTest(name = "测试终点跟踪动作-前进")]
// public sealed class DstTrackerForward : DstTrackerTestBase
// {
// public DstTrackerForward() : base(0f) { }
// }
// [MovementTest(name = "测试终点跟踪动作-后退")]
// public sealed class DstTrackerBackward : DstTrackerTestBase
// {
// public DstTrackerBackward() : base(180f) { }
// }
// [MovementTest(name = "底盘旋转测试")]
// public class RotateToAngleTest : MovementTest
// {
// private DriveTask _dt;
// // 停止当前正在执行的底盘原地旋转任务。
// public override void TestStop()
// {
// _dt?.Stop();
// }
// // 交互输入目标角度后执行底盘原地旋转测试。
// public override void Test()
// {
// var input = UI.GetInput("输入旋转角度：");
// if (!float.TryParse(input, out var angleTarget))
// {
// Console.WriteLine(
// $"旋转测试输入无效：{input}");
// return;
// }
// // 防止重复启动测试时，上一项旋转任务仍在运行。
// _dt?.Stop();
// var task = new DriveTask(
// new MultiWheelRotateInPlace
// {
// AngleTarget = angleTarget,
// PidparamsRead = () => new PIDParams
// {
// Kp = PilotDefinition.Conf.InPlaceRotateKp,
// Ki = PilotDefinition.Conf.InPlaceRotateKi,
// Kd = PilotDefinition.Conf.InPlaceRotateKd,
// DeadZone = PilotDefinition.Conf.InPlaceRotateArriveDeg,
// SpeedAccPerSec = PilotDefinition.Conf.InPlaceRotateAcc,
// OutputUpperThreshold = PilotDefinition.Conf.InPlaceRotateMaxSpeed,
// MaxI = PilotDefinition.Conf.InPlaceRotateMaxI,
// }
// }.Get());
// _dt = task;
// try
// {
// task.Wait();
// }
// finally
// {
// // 防止旧任务结束时，错误清除后来启动的新任务。
// if (ReferenceEquals(_dt, task))
// {
// _dt = null;
// }
// }
// }
// }
#endregion