实现蟹行轨迹跟踪测试并优化底盘XYTh与原地旋转舵角控制
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Cursor
+738
-10
@@ -7,6 +7,7 @@ using MDCSToolBox.Commons.Controllers;
|
||||
using MDCSToolBox.Clumsy.Tracks;
|
||||
using MyParking.Shared;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Numerics;
|
||||
using System.Threading;
|
||||
|
||||
@@ -102,7 +103,7 @@ namespace MultiWheelC
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "测试连续前进4m")]
|
||||
[MovementTest(name = "旧版SendMotion：连续前进4m")]
|
||||
public class TestForward4m : MovementTest
|
||||
{
|
||||
public float DistanceMillimeters = 4000f; // 测试距离，单位mm。
|
||||
@@ -138,8 +139,8 @@ namespace MultiWheelC
|
||||
source.Y + DistanceMillimeters * (float)Math.Sin(headingRadians));
|
||||
_recorder =
|
||||
new TrackingExperimentRecorder(
|
||||
controllerName: "Stanley",
|
||||
trajectoryName: "Straight4m",
|
||||
controllerName: "LegacyGeometricController",
|
||||
trajectoryName: "LegacyStraight4m",
|
||||
trialNumber: TrialNumber,
|
||||
referenceStart: source,
|
||||
referenceEnd: destination,
|
||||
@@ -225,8 +226,10 @@ namespace MultiWheelC
|
||||
trialNumber: TrialNumber,
|
||||
referenceStart: rotationCenter,
|
||||
referenceEnd: rotationCenter,
|
||||
referenceSpeed:
|
||||
MaxAngularSpeedDegreesPerSecond);
|
||||
referenceSpeed: 0f,
|
||||
referenceAngularSpeed:
|
||||
MaxAngularSpeedDegreesPerSecond *
|
||||
(float)Math.PI / 180f);
|
||||
_recorder.Start();
|
||||
|
||||
try
|
||||
@@ -258,7 +261,8 @@ namespace MultiWheelC
|
||||
commandAngularSpeed =>
|
||||
_recorder?.UpdateCommand(
|
||||
0f,
|
||||
commandAngularSpeed)
|
||||
commandAngularSpeed *
|
||||
(float)Math.PI / 180f)
|
||||
}.Get());
|
||||
|
||||
_task.Wait();
|
||||
@@ -293,7 +297,7 @@ namespace MultiWheelC
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "测试左转90°圆弧")]
|
||||
[MovementTest(name = "旧版SendMotion：左转90°圆弧")]
|
||||
public class TestArcMovement : MovementTest
|
||||
{
|
||||
public float RadiusMillimeters = 2000f; // 左转圆的半径，单位mm。
|
||||
@@ -370,6 +374,13 @@ namespace MultiWheelC
|
||||
CarDirectionBias = 0f
|
||||
};
|
||||
|
||||
// 左转90°后，圆心到终点的径向方向等于起始车头方向。
|
||||
var destination = center + new Vector2(
|
||||
RadiusMillimeters *
|
||||
(float)Math.Cos(headingRadians),
|
||||
RadiusMillimeters *
|
||||
(float)Math.Sin(headingRadians));
|
||||
|
||||
if (!controller.AddTrack(arc, "LeftArc90Degrees"))
|
||||
{
|
||||
Console.WriteLine(
|
||||
@@ -378,12 +389,12 @@ namespace MultiWheelC
|
||||
}
|
||||
|
||||
_recorder = new TrackingExperimentRecorder(
|
||||
controllerName: "GeometricController",
|
||||
controllerName: "LegacyGeometricController",
|
||||
trajectoryName:
|
||||
$"LeftArc90_R{RadiusMillimeters:0}mm",
|
||||
$"LegacyLeftArc90_R{RadiusMillimeters:0}mm",
|
||||
trialNumber: TrialNumber,
|
||||
referenceStart: source,
|
||||
referenceEnd: source,
|
||||
referenceEnd: destination,
|
||||
referenceSpeed: CruiseSpeed);
|
||||
_recorder.Start();
|
||||
|
||||
@@ -414,6 +425,723 @@ namespace MultiWheelC
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "测试蟹行前进4m")]
|
||||
public class TestCrabForward4m : MovementTest
|
||||
{
|
||||
public float DistanceMillimeters = 4000f;
|
||||
public float CruiseSpeed = 0.2f;
|
||||
public int TrialNumber = 1;
|
||||
|
||||
private DriveTask _task;
|
||||
private TrackingExperimentRecorder _recorder;
|
||||
|
||||
// 将车体左侧作为运动前向，沿直线蟹行4m并记录Detour实验数据。
|
||||
public override void Test()
|
||||
{
|
||||
if (!TryReadStartPose(
|
||||
out var source,
|
||||
out var bodyYawRadians))
|
||||
return;
|
||||
|
||||
var motionYaw =
|
||||
bodyYawRadians + Math.PI / 2.0;
|
||||
var destination = new Vector2(
|
||||
source.X +
|
||||
DistanceMillimeters *
|
||||
(float)Math.Cos(motionYaw),
|
||||
source.Y +
|
||||
DistanceMillimeters *
|
||||
(float)Math.Sin(motionYaw));
|
||||
|
||||
var tracker = new CrabMotionFrameTracker
|
||||
{
|
||||
PathKind =
|
||||
CrabMotionFrameTracker
|
||||
.ReferencePathKind.Straight,
|
||||
StartPosition = source,
|
||||
InitialBodyYawRadians =
|
||||
bodyYawRadians,
|
||||
LengthMillimeters =
|
||||
DistanceMillimeters,
|
||||
CruiseSpeed = CruiseSpeed
|
||||
};
|
||||
|
||||
_recorder = new TrackingExperimentRecorder(
|
||||
controllerName:
|
||||
"CrabMotionFrameTracker",
|
||||
trajectoryName:
|
||||
"CrabStraight4m",
|
||||
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();
|
||||
}
|
||||
|
||||
// 读取并校验测试开始时的Detour世界位姿。
|
||||
private static bool TryReadStartPose(
|
||||
out Vector2 source,
|
||||
out double bodyYawRadians)
|
||||
{
|
||||
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当前位姿无效，取消蟹行直线测试。");
|
||||
source = Vector2.Zero;
|
||||
bodyYawRadians = 0.0;
|
||||
return false;
|
||||
}
|
||||
|
||||
source = new Vector2(
|
||||
(float)location.x,
|
||||
(float)location.y);
|
||||
bodyYawRadians =
|
||||
location.th * Math.PI / 180.0;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "测试蟹行左转90°圆弧")]
|
||||
public class TestCrabLeftArc90 : MovementTest
|
||||
{
|
||||
public float RadiusMillimeters = 2000f;
|
||||
public float CruiseSpeed = 0.2f;
|
||||
public int TrialNumber = 1;
|
||||
|
||||
private DriveTask _task;
|
||||
private TrackingExperimentRecorder _recorder;
|
||||
|
||||
// 将车体左侧作为运动前向，沿半径2m的左转圆弧运动90°。
|
||||
public override void Test()
|
||||
{
|
||||
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当前位姿无效，取消蟹行圆弧测试。");
|
||||
return;
|
||||
}
|
||||
|
||||
var source = new Vector2(
|
||||
(float)location.x,
|
||||
(float)location.y);
|
||||
var bodyYawRadians =
|
||||
location.th * Math.PI / 180.0;
|
||||
var tracker = new CrabMotionFrameTracker
|
||||
{
|
||||
PathKind =
|
||||
CrabMotionFrameTracker
|
||||
.ReferencePathKind.LeftArc,
|
||||
StartPosition = source,
|
||||
InitialBodyYawRadians =
|
||||
bodyYawRadians,
|
||||
RadiusMillimeters =
|
||||
RadiusMillimeters,
|
||||
ArcSweepRadians = Math.PI / 2.0,
|
||||
CruiseSpeed = CruiseSpeed
|
||||
};
|
||||
var destination =
|
||||
tracker.GetArcDestination();
|
||||
|
||||
_recorder = new TrackingExperimentRecorder(
|
||||
controllerName:
|
||||
"CrabMotionFrameTracker",
|
||||
trajectoryName:
|
||||
$"CrabLeftArc90_R{RadiusMillimeters: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 = "测试蟹行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型曲线")]
|
||||
public class TestSCurve4m : MovementTest
|
||||
{
|
||||
public float LengthMillimeters = 4000f; // S型曲线纵向长度，单位mm。
|
||||
public float LateralOffsetMillimeters = 400f; // S型曲线左右两侧的最大偏移，单位mm。
|
||||
public float CruiseSpeed = 0.3f; // 首次实车测试建议使用0.3m/s。
|
||||
public int TrialNumber = 1; // 重复实验编号。
|
||||
|
||||
private DriveTask _task;
|
||||
private TrackingExperimentRecorder _recorder;
|
||||
|
||||
// 从当前Detour位姿开始，沿车头方向跟踪先左偏、再右偏并最终回中的完整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;
|
||||
}
|
||||
|
||||
if (!MovementTestPreparation.AreWheelsForward())
|
||||
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当前位姿无效，取消4m S型曲线测试。");
|
||||
return;
|
||||
}
|
||||
|
||||
var source =
|
||||
new Vector2((float)location.x, (float)location.y);
|
||||
var headingRadians =
|
||||
location.th * Math.PI / 180.0;
|
||||
var length = LengthMillimeters;
|
||||
var offset = LateralOffsetMillimeters;
|
||||
|
||||
// 三段三次贝塞尔依次经过左侧峰值、中心线和右侧峰值，
|
||||
// 起点、两个峰值和终点的切线均沿初始前向，连接处没有折角。
|
||||
var firstControlPoints = new List<Vector2>
|
||||
{
|
||||
LocalToWorld(source, headingRadians, 0f, 0f),
|
||||
LocalToWorld(
|
||||
source, headingRadians,
|
||||
length / 12f, 0f),
|
||||
LocalToWorld(
|
||||
source, headingRadians,
|
||||
length / 6f, offset),
|
||||
LocalToWorld(
|
||||
source, headingRadians,
|
||||
length * 0.25f, offset)
|
||||
};
|
||||
var secondControlPoints = new List<Vector2>
|
||||
{
|
||||
LocalToWorld(
|
||||
source, headingRadians,
|
||||
length * 0.25f, offset),
|
||||
LocalToWorld(
|
||||
source, headingRadians,
|
||||
length / 3f, offset),
|
||||
LocalToWorld(
|
||||
source, headingRadians,
|
||||
length * 2f / 3f, -offset),
|
||||
LocalToWorld(
|
||||
source, headingRadians,
|
||||
length * 0.75f, -offset)
|
||||
};
|
||||
var thirdControlPoints = new List<Vector2>
|
||||
{
|
||||
LocalToWorld(
|
||||
source, headingRadians,
|
||||
length * 0.75f, -offset),
|
||||
LocalToWorld(
|
||||
source, headingRadians,
|
||||
length * 5f / 6f, -offset),
|
||||
LocalToWorld(
|
||||
source, headingRadians,
|
||||
length * 11f / 12f, 0f),
|
||||
LocalToWorld(
|
||||
source, headingRadians,
|
||||
length, 0f)
|
||||
};
|
||||
|
||||
var firstTrack = new BezierTrack(firstControlPoints)
|
||||
{
|
||||
Speed = CruiseSpeed,
|
||||
CarDirectionBias = 0f
|
||||
};
|
||||
var secondTrack = new BezierTrack(secondControlPoints)
|
||||
{
|
||||
Speed = CruiseSpeed,
|
||||
CarDirectionBias = 0f
|
||||
};
|
||||
var thirdTrack = new BezierTrack(thirdControlPoints)
|
||||
{
|
||||
Speed = CruiseSpeed,
|
||||
CarDirectionBias = 0f
|
||||
};
|
||||
|
||||
var controller = new ChassisController
|
||||
{
|
||||
BaseSpeed = CruiseSpeed
|
||||
}.Get();
|
||||
controller.FinishSpeed = 0f;
|
||||
|
||||
if (!controller.AddTrack(
|
||||
firstTrack,
|
||||
"SCurve4m-Part1") ||
|
||||
!controller.AddTrack(
|
||||
secondTrack,
|
||||
"SCurve4m-Part2") ||
|
||||
!controller.AddTrack(
|
||||
thirdTrack,
|
||||
"SCurve4m-Part3"))
|
||||
{
|
||||
Console.WriteLine(
|
||||
"4m S型曲线轨迹添加失败，取消测试。");
|
||||
return;
|
||||
}
|
||||
|
||||
var destination =
|
||||
LocalToWorld(
|
||||
source,
|
||||
headingRadians,
|
||||
length,
|
||||
0f);
|
||||
_recorder = new TrackingExperimentRecorder(
|
||||
controllerName: "LegacyGeometricController",
|
||||
trajectoryName:
|
||||
$"LegacySCurve4m_A{LateralOffsetMillimeters:0}mm",
|
||||
trialNumber: TrialNumber,
|
||||
referenceStart: source,
|
||||
referenceEnd: destination,
|
||||
referenceSpeed: CruiseSpeed);
|
||||
_recorder.Start();
|
||||
|
||||
try
|
||||
{
|
||||
_task = new DriveTask(controller.Track());
|
||||
_task.Wait();
|
||||
|
||||
// 保留少量停止后的数据，用于观察速度是否回到零。
|
||||
Thread.Sleep(300);
|
||||
}
|
||||
finally
|
||||
{
|
||||
_task?.Stop();
|
||||
_recorder?.UpdateCommand(0f, 0f);
|
||||
_recorder?.StopAndSave();
|
||||
_task = null;
|
||||
_recorder = null;
|
||||
}
|
||||
}
|
||||
|
||||
// 停止S型曲线测试并保存当前已经采集的数据。
|
||||
public override void TestStop()
|
||||
{
|
||||
_task?.Stop();
|
||||
_recorder?.UpdateCommand(0f, 0f);
|
||||
_recorder?.StopAndSave();
|
||||
}
|
||||
|
||||
// 将车体起点局部坐标转换为Detour世界坐标，X向前、Y向左。
|
||||
private static Vector2 LocalToWorld(
|
||||
Vector2 origin,
|
||||
double headingRadians,
|
||||
float localX,
|
||||
float localY)
|
||||
{
|
||||
var cos = (float)Math.Cos(headingRadians);
|
||||
var sin = (float)Math.Sin(headingRadians);
|
||||
|
||||
return new Vector2(
|
||||
origin.X + localX * cos - localY * sin,
|
||||
origin.Y + localX * sin + localY * cos);
|
||||
}
|
||||
}
|
||||
|
||||
// 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。
|
||||
|
||||
Reference in New Issue
Block a user