1349 lines
45 KiB
C#
1349 lines
45 KiB
C#
using ClumsyCore;
|
||
using ClumsyCore.DTools;
|
||
using ClumsyCore.Interfaces;
|
||
using ClumsyCore.Pilot;
|
||
using CommonUsage.Chassis;
|
||
using MDCSToolBox.Commons.Controllers;
|
||
using MDCSToolBox.Clumsy.Tracks;
|
||
using MyParking.Shared;
|
||
using System;
|
||
using System.Collections.Generic;
|
||
using System.Numerics;
|
||
using System.Threading;
|
||
|
||
namespace MultiWheelC
|
||
{
|
||
internal static class MovementTestPreparation
|
||
{
|
||
// 在测试正式开始前，将四个舵轮稳定回正到车体前向。
|
||
public static bool AlignWheelsForward(
|
||
ref DriveTask activeTask)
|
||
{
|
||
var preparation = new PrepareWheelsForward();
|
||
var task = new DriveTask(preparation.Get());
|
||
activeTask = task;
|
||
|
||
try
|
||
{
|
||
task.Wait();
|
||
return preparation.Completed;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
Console.WriteLine(
|
||
$"测试前舵轮回正失败：{ex.Message}");
|
||
return false;
|
||
}
|
||
finally
|
||
{
|
||
task.Stop();
|
||
if (ReferenceEquals(activeTask, task))
|
||
activeTask = null;
|
||
}
|
||
}
|
||
|
||
// 只读取实际舵角，检查四个舵轮是否已与车头方向一致。
|
||
public static bool AreWheelsForward(
|
||
float toleranceDegrees = 2f)
|
||
{
|
||
var chassis =
|
||
PilotDefinition.Chassis as MultiWheelChassis;
|
||
if (chassis == null)
|
||
{
|
||
Console.WriteLine(
|
||
"当前底盘不是MultiWheelChassis，无法检查舵轮方向。");
|
||
return false;
|
||
}
|
||
|
||
try
|
||
{
|
||
var adapter = new MultiWheelChassisAdapter(
|
||
chassis,
|
||
PilotDefinition.Self.CarNum);
|
||
var toleranceRadians =
|
||
toleranceDegrees * Math.PI / 180.0;
|
||
|
||
if (adapter.AreParallelWheelsAligned(
|
||
0.0,
|
||
toleranceRadians))
|
||
{
|
||
return true;
|
||
}
|
||
|
||
Console.WriteLine(
|
||
"四个舵轮尚未与车头方向一致，请先执行“准备：四个舵轮与车头方向一致”。");
|
||
return false;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
Console.WriteLine(
|
||
$"检查舵轮方向失败：{ex.Message}");
|
||
return false;
|
||
}
|
||
}
|
||
}
|
||
|
||
[MovementTest(name = "准备：四个舵轮与车头方向一致")]
|
||
public class AlignWheelsForwardTest : MovementTest
|
||
{
|
||
private DriveTask _task;
|
||
|
||
// 单独将四个舵轮转到车体前向0°并等待实际反馈稳定到位。
|
||
public override void Test()
|
||
{
|
||
MovementTestPreparation.AlignWheelsForward(
|
||
ref _task);
|
||
}
|
||
|
||
// 停止正在执行的舵轮回正任务并清零底盘运动命令。
|
||
public override void TestStop()
|
||
{
|
||
_task?.Stop();
|
||
_task = null;
|
||
}
|
||
}
|
||
|
||
[MovementTest(name = "旧版SendMotion：连续前进4m")]
|
||
public class TestForward4m : MovementTest
|
||
{
|
||
public float DistanceMillimeters = 4000f; // 测试距离，单位mm。
|
||
public float CruiseSpeed = 0.3f; // 巡航速度上限，单位m/s。
|
||
public int TrialNumber = 1; // 重复实验编号。
|
||
private DriveTask _task;
|
||
private TrackingExperimentRecorder _recorder;
|
||
// 从当前Detour位置沿车头方向生成4m连续直线并记录测试数据。
|
||
public override void Test()
|
||
{
|
||
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测试。");
|
||
return;
|
||
}
|
||
var source = new Vector2((float)location.x, (float)location.y);
|
||
// Detour航向单位是度，三角函数需要弧度。
|
||
var headingRadians = location.th * Math.PI / 180.0;
|
||
var destination = new Vector2(
|
||
source.X + DistanceMillimeters * (float)Math.Cos(headingRadians),
|
||
source.Y + DistanceMillimeters * (float)Math.Sin(headingRadians));
|
||
_recorder =
|
||
new TrackingExperimentRecorder(
|
||
controllerName: "LegacyGeometricController",
|
||
trajectoryName: "LegacyStraight4m",
|
||
trialNumber: TrialNumber,
|
||
referenceStart: source,
|
||
referenceEnd: destination,
|
||
referenceSpeed: CruiseSpeed);
|
||
_recorder.Start();
|
||
try
|
||
{
|
||
_task = new DriveTask(
|
||
new DstTracker
|
||
{
|
||
Src = source,
|
||
Dst = destination,
|
||
CarDirectionBias = 0f,
|
||
MaxSpeed = CruiseSpeed
|
||
}.Get());
|
||
_task.Wait();
|
||
// 保留少量停车后数据，便于观察速度是否回到零。
|
||
Thread.Sleep(300);
|
||
}
|
||
finally
|
||
{
|
||
_task?.Stop();
|
||
_recorder?.UpdateCommand(0f, 0f);
|
||
_recorder?.StopAndSave();
|
||
_task = null;
|
||
_recorder = null;
|
||
}
|
||
}
|
||
|
||
public override void TestStop()
|
||
{
|
||
_task?.Stop();
|
||
_recorder?.UpdateCommand(0f, 0f);
|
||
_recorder?.StopAndSave();
|
||
}
|
||
}
|
||
|
||
[MovementTest(name = "测试原地自转90°")]
|
||
public class TestRotate90 : MovementTest
|
||
{
|
||
public float RelativeAngleDegrees = 90f; // 相对当前航向的旋转角度，逆时针为正。
|
||
public float MaxAngularSpeedDegreesPerSecond = 20f; // PID输出的最大角速度。
|
||
public int TrialNumber = 1; // 重复实验编号。
|
||
|
||
private DriveTask _task;
|
||
private TrackingExperimentRecorder _recorder;
|
||
|
||
// 从当前Detour航向开始，原地相对旋转指定角度并记录实验数据。
|
||
public override void Test()
|
||
{
|
||
if (float.IsNaN(RelativeAngleDegrees) ||
|
||
float.IsInfinity(RelativeAngleDegrees) ||
|
||
float.IsNaN(MaxAngularSpeedDegreesPerSecond) ||
|
||
float.IsInfinity(MaxAngularSpeedDegreesPerSecond) ||
|
||
MaxAngularSpeedDegreesPerSecond <= 0f)
|
||
{
|
||
Console.WriteLine("原地旋转测试参数无效。");
|
||
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当前位姿无效，取消原地旋转测试。");
|
||
return;
|
||
}
|
||
|
||
var rotationCenter =
|
||
new Vector2((float)location.x, (float)location.y);
|
||
var targetWorldAngle =
|
||
NormalizeDegrees(
|
||
(float)location.th + RelativeAngleDegrees);
|
||
|
||
_recorder = new TrackingExperimentRecorder(
|
||
controllerName: "InPlaceRotatePID",
|
||
trajectoryName: "Rotate90",
|
||
trialNumber: TrialNumber,
|
||
referenceStart: rotationCenter,
|
||
referenceEnd: rotationCenter,
|
||
referenceSpeed: 0f,
|
||
referenceAngularSpeed:
|
||
MaxAngularSpeedDegreesPerSecond *
|
||
(float)Math.PI / 180f);
|
||
_recorder.Start();
|
||
|
||
try
|
||
{
|
||
_task = new DriveTask(
|
||
new MultiWheelRotateInPlace
|
||
{
|
||
// MultiWheelRotateInPlace接收世界坐标系绝对航向。
|
||
AngleTarget = targetWorldAngle,
|
||
PidparamsRead = () => new PIDParams
|
||
{
|
||
Kp =
|
||
PilotDefinition.Conf.InPlaceRotateKp,
|
||
Ki =
|
||
PilotDefinition.Conf.InPlaceRotateKi,
|
||
Kd =
|
||
PilotDefinition.Conf.InPlaceRotateKd,
|
||
DeadZone =
|
||
PilotDefinition.Conf
|
||
.InPlaceRotateArriveDeg,
|
||
SpeedAccPerSec =
|
||
PilotDefinition.Conf.InPlaceRotateAcc,
|
||
OutputUpperThreshold =
|
||
MaxAngularSpeedDegreesPerSecond,
|
||
MaxI =
|
||
PilotDefinition.Conf.InPlaceRotateMaxI
|
||
},
|
||
CommandAngularSpeedObserver =
|
||
commandAngularSpeed =>
|
||
_recorder?.UpdateCommand(
|
||
0f,
|
||
commandAngularSpeed *
|
||
(float)Math.PI / 180f)
|
||
}.Get());
|
||
|
||
_task.Wait();
|
||
|
||
// 保留少量停止后的样本，用于观察角速度是否回到零。
|
||
Thread.Sleep(300);
|
||
}
|
||
finally
|
||
{
|
||
_task?.Stop();
|
||
_recorder?.UpdateCommand(0f, 0f);
|
||
_recorder?.StopAndSave();
|
||
_task = null;
|
||
_recorder = null;
|
||
}
|
||
}
|
||
|
||
// 停止原地旋转并保存当前已经采集的实验数据。
|
||
public override void TestStop()
|
||
{
|
||
_task?.Stop();
|
||
_recorder?.UpdateCommand(0f, 0f);
|
||
_recorder?.StopAndSave();
|
||
}
|
||
|
||
// 将世界航向归一化到大约[-180°,180°]。
|
||
private static float NormalizeDegrees(float angleDegrees)
|
||
{
|
||
return (float)(
|
||
angleDegrees -
|
||
Math.Round(angleDegrees / 360.0) * 360.0);
|
||
}
|
||
}
|
||
|
||
[MovementTest(name = "旧版SendMotion：左转90°圆弧")]
|
||
public class TestArcMovement : MovementTest
|
||
{
|
||
public float RadiusMillimeters = 2000f; // 左转圆的半径，单位mm。
|
||
public float CruiseSpeed = 0.3f; // 圆周运动速度上限，单位m/s。
|
||
public int TrialNumber = 1; // 重复实验编号。
|
||
|
||
private DriveTask _task;
|
||
private TrackingExperimentRecorder _recorder;
|
||
|
||
// 从当前位姿开始，沿半径2m的圆弧向左转弯90°。
|
||
public override void Test()
|
||
{
|
||
if (float.IsNaN(RadiusMillimeters) ||
|
||
float.IsInfinity(RadiusMillimeters) ||
|
||
RadiusMillimeters <= 0f ||
|
||
float.IsNaN(CruiseSpeed) ||
|
||
float.IsInfinity(CruiseSpeed) ||
|
||
CruiseSpeed <= 0f)
|
||
{
|
||
Console.WriteLine("圆弧运动测试参数无效。");
|
||
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当前位姿无效，取消圆弧运动测试。");
|
||
return;
|
||
}
|
||
|
||
var source =
|
||
new Vector2((float)location.x, (float)location.y);
|
||
var headingRadians =
|
||
location.th * Math.PI / 180.0;
|
||
|
||
// 根据世界航向求车体左法向，左转圆心位于车辆左侧。
|
||
var center = new Vector2(
|
||
source.X -
|
||
RadiusMillimeters *
|
||
(float)Math.Sin(headingRadians),
|
||
source.Y +
|
||
RadiusMillimeters *
|
||
(float)Math.Cos(headingRadians));
|
||
|
||
// 从圆心指向车辆起点的极角，比车辆切线航向小90°。
|
||
var startRadialAngleDegrees =
|
||
(float)location.th - 90f;
|
||
|
||
var controller = new ChassisController
|
||
{
|
||
BaseSpeed = CruiseSpeed
|
||
}.Get();
|
||
controller.FinishSpeed = 0f;
|
||
|
||
var arc = new CircularArcTrack(
|
||
center,
|
||
RadiusMillimeters,
|
||
startRadialAngleDegrees,
|
||
startRadialAngleDegrees + 90f,
|
||
direction: 1)
|
||
{
|
||
Speed = CruiseSpeed,
|
||
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(
|
||
"左转90°圆弧轨迹添加失败，取消测试。");
|
||
return;
|
||
}
|
||
|
||
_recorder = new TrackingExperimentRecorder(
|
||
controllerName: "LegacyGeometricController",
|
||
trajectoryName:
|
||
$"LegacyLeftArc90_R{RadiusMillimeters: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;
|
||
}
|
||
}
|
||
|
||
// 停止圆弧运动并保存当前已经采集的实验数据。
|
||
public override void TestStop()
|
||
{
|
||
_task?.Stop();
|
||
_recorder?.UpdateCommand(0f, 0f);
|
||
_recorder?.StopAndSave();
|
||
}
|
||
}
|
||
|
||
[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。
|
||
|
||
private DriveTask _task;
|
||
protected abstract bool Close { get; }
|
||
|
||
// 根据派生测试类型驱动左右夹臂同步夹紧或打开。
|
||
public override void Test()
|
||
{
|
||
var leftTarget = Close
|
||
? PilotDefinition.Self.LeftArmUpperPos
|
||
: PilotDefinition.Self.LeftArmLowerPos;
|
||
var rightTarget = Close
|
||
? PilotDefinition.Self.RightArmUpperPos
|
||
: PilotDefinition.Self.RightArmLowerPos;
|
||
|
||
if (float.IsNaN(leftTarget) ||
|
||
float.IsInfinity(leftTarget) ||
|
||
float.IsNaN(rightTarget) ||
|
||
float.IsInfinity(rightTarget))
|
||
{
|
||
Console.WriteLine(
|
||
"夹臂目标位置无效，取消夹臂运动测试。");
|
||
return;
|
||
}
|
||
|
||
// 防止重复点击时上一项夹臂任务仍在运行。
|
||
TestStop();
|
||
Console.WriteLine(
|
||
$"开始夹臂{(Close ? "夹紧" : "打开")}测试：" +
|
||
$"左目标={leftTarget}，右目标={rightTarget}");
|
||
|
||
var task = new DriveTask(
|
||
new ClampToTarget
|
||
{
|
||
LeftClampTarget = leftTarget,
|
||
RightClampTarget = rightTarget,
|
||
TimeoutSeconds = TimeoutSeconds
|
||
}.Get());
|
||
_task = task;
|
||
|
||
try
|
||
{
|
||
task.Wait();
|
||
}
|
||
finally
|
||
{
|
||
PilotDefinition.Self.SpeedLeftArm = 0f;
|
||
PilotDefinition.Self.SpeedRightArm = 0f;
|
||
if (ReferenceEquals(_task, task))
|
||
_task = null;
|
||
}
|
||
}
|
||
|
||
// 停止夹臂任务并立即清零左右夹臂下发速度。
|
||
public override void TestStop()
|
||
{
|
||
_task?.Stop();
|
||
_task = null;
|
||
PilotDefinition.Self.SpeedLeftArm = 0f;
|
||
PilotDefinition.Self.SpeedRightArm = 0f;
|
||
}
|
||
}
|
||
|
||
[MovementTest(name = "夹臂关闭测试")]
|
||
public sealed class TestClampOpenMovement
|
||
: ClampMovementTestBase
|
||
{
|
||
protected override bool Close => false;
|
||
}
|
||
|
||
[MovementTest(name = "夹臂启动测试")]
|
||
public sealed class TestClampCloseMovement
|
||
: ClampMovementTestBase
|
||
{
|
||
protected override bool Close => true;
|
||
}
|
||
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
#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
|