拆分MultiWheelC并新增轨迹投影、Detour状态估计与Stanley跟踪控制
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Cursor
@@ -0,0 +1,90 @@
|
||||
|
||||
|
||||
using System;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.Pilot;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
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 TestClampCloseMovement
|
||||
: ClampMovementTestBase
|
||||
{
|
||||
protected override bool Close => false;
|
||||
}
|
||||
|
||||
[MovementTest(name = "夹臂启动测试")]
|
||||
public sealed class TestClampOpenMovement
|
||||
: ClampMovementTestBase
|
||||
{
|
||||
protected override bool Close => true;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,635 @@
|
||||
|
||||
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Numerics;
|
||||
using System.Threading;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
using MDCSToolBox.Clumsy.Tracks;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
[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 =
|
||||
AngleMath.DegreesToRadians(location.th);
|
||||
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 = "SendMotion：左转90°半径2m圆弧")]
|
||||
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 =
|
||||
AngleMath.DegreesToRadians(location.th);
|
||||
|
||||
// 根据世界航向求车体左法向，左转圆心位于车辆左侧。
|
||||
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 = "SendMotion：蟹行直线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
|
||||
{
|
||||
CommandBackend =
|
||||
CrabMotionFrameTracker
|
||||
.ChassisCommandBackend
|
||||
.SendMotion,
|
||||
PathKind =
|
||||
CrabMotionFrameTracker
|
||||
.ReferencePathKind.Straight,
|
||||
StartPosition = source,
|
||||
InitialBodyYawRadians =
|
||||
bodyYawRadians,
|
||||
LengthMillimeters =
|
||||
DistanceMillimeters,
|
||||
CruiseSpeed = CruiseSpeed
|
||||
};
|
||||
|
||||
_recorder = new TrackingExperimentRecorder(
|
||||
controllerName:
|
||||
"CrabSendMotionTracker",
|
||||
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 =
|
||||
AngleMath.DegreesToRadians(location.th);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "SendMotion：蟹行左转90°半径2m圆弧")]
|
||||
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 =
|
||||
AngleMath.DegreesToRadians(location.th);
|
||||
var tracker = new CrabMotionFrameTracker
|
||||
{
|
||||
CommandBackend =
|
||||
CrabMotionFrameTracker
|
||||
.ChassisCommandBackend
|
||||
.SendMotion,
|
||||
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:
|
||||
"CrabSendMotionTracker",
|
||||
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 = "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 =
|
||||
AngleMath.DegreesToRadians(location.th);
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,158 @@
|
||||
|
||||
|
||||
using System;
|
||||
using System.Numerics;
|
||||
using System.Threading;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
using MDCSToolBox.Commons.Controllers;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
public abstract class InPlaceRotateTestBase : MovementTest
|
||||
{
|
||||
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()
|
||||
{
|
||||
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 =
|
||||
(float)AngleMath.NormalizeDegrees(
|
||||
location.th + RelativeAngleDegrees);
|
||||
|
||||
_recorder = new TrackingExperimentRecorder(
|
||||
controllerName: "InPlaceRotatePID",
|
||||
trajectoryName: _trajectoryName,
|
||||
trialNumber: TrialNumber,
|
||||
referenceStart: rotationCenter,
|
||||
referenceEnd: rotationCenter,
|
||||
referenceSpeed: 0f,
|
||||
referenceAngularSpeed:
|
||||
(float)AngleMath.DegreesToRadians(
|
||||
MaxAngularSpeedDegreesPerSecond));
|
||||
_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,
|
||||
(float)AngleMath.DegreesToRadians(
|
||||
commandAngularSpeed))
|
||||
}.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 = "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")
|
||||
{
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,415 @@
|
||||
using ClumsyCore.Interfaces;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Diagnostics;
|
||||
using System.Globalization;
|
||||
using System.IO;
|
||||
using System.Numerics;
|
||||
using System.Text;
|
||||
using System.Threading;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
// C层实验数据：保存一个采样时刻的定位与控制命令。
|
||||
public sealed class TrackingSample
|
||||
{
|
||||
public double ElapsedSeconds;
|
||||
|
||||
// Detour位置单位为mm，航向单位为deg。
|
||||
public double DetourX;
|
||||
public double DetourY;
|
||||
public double DetourTheta;
|
||||
|
||||
// 车体速度单位为m/s，角速度统一使用rad/s。
|
||||
public float CommandSpeed;
|
||||
public float CommandVx;
|
||||
public float CommandVy;
|
||||
public float CommandAngularSpeed;
|
||||
}
|
||||
|
||||
// C层实验工具：统一采集并保存轨迹跟踪实验数据。
|
||||
public sealed class TrackingExperimentRecorder
|
||||
{
|
||||
private readonly string _controllerName;
|
||||
private readonly string _trajectoryName;
|
||||
private readonly int _trialNumber;
|
||||
private readonly Vector2 _referenceStart;
|
||||
private readonly Vector2 _referenceEnd;
|
||||
private readonly float _referenceSpeed;
|
||||
private readonly float _referenceAngularSpeed;
|
||||
private readonly float _referenceMotionFrameYawDegrees;
|
||||
private readonly int _sampleIntervalMs;
|
||||
|
||||
private readonly List<TrackingSample> _samples =
|
||||
new List<TrackingSample>();
|
||||
|
||||
private readonly object _sampleSyncRoot =
|
||||
new object();
|
||||
|
||||
private readonly object _commandSyncRoot =
|
||||
new object();
|
||||
|
||||
private readonly Stopwatch _stopwatch =
|
||||
new Stopwatch();
|
||||
|
||||
private Thread _worker;
|
||||
private volatile bool _running;
|
||||
private int _started;
|
||||
private int _saved;
|
||||
|
||||
private bool _hasExternalCommand;
|
||||
private float _externalCommandSpeed;
|
||||
private float _externalCommandVx;
|
||||
private float _externalCommandVy;
|
||||
private float _externalCommandAngularSpeed;
|
||||
|
||||
public TrackingExperimentRecorder(
|
||||
string controllerName,
|
||||
string trajectoryName,
|
||||
int trialNumber,
|
||||
Vector2 referenceStart,
|
||||
Vector2 referenceEnd,
|
||||
float referenceSpeed,
|
||||
float referenceAngularSpeed = 0f,
|
||||
int sampleIntervalMs = 50,
|
||||
float referenceMotionFrameYawDegrees = 0f)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(controllerName))
|
||||
throw new ArgumentException(
|
||||
"控制器名称不能为空。",
|
||||
nameof(controllerName));
|
||||
|
||||
if (string.IsNullOrWhiteSpace(trajectoryName))
|
||||
throw new ArgumentException(
|
||||
"轨迹名称不能为空。",
|
||||
nameof(trajectoryName));
|
||||
|
||||
if (sampleIntervalMs <= 0)
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(sampleIntervalMs),
|
||||
"采样周期必须大于零。");
|
||||
|
||||
_controllerName = controllerName;
|
||||
_trajectoryName = trajectoryName;
|
||||
_trialNumber = trialNumber;
|
||||
_referenceStart = referenceStart;
|
||||
_referenceEnd = referenceEnd;
|
||||
_referenceSpeed = referenceSpeed;
|
||||
_referenceAngularSpeed = referenceAngularSpeed;
|
||||
_referenceMotionFrameYawDegrees =
|
||||
referenceMotionFrameYawDegrees;
|
||||
_sampleIntervalMs = sampleIntervalMs;
|
||||
}
|
||||
|
||||
// 保存成功后的CSV绝对路径；尚未保存时为空。
|
||||
public string SavedFilePath { get; private set; }
|
||||
|
||||
// 启动后台采样线程。
|
||||
public void Start()
|
||||
{
|
||||
if (Interlocked.Exchange(ref _started, 1) != 0)
|
||||
return;
|
||||
|
||||
_stopwatch.Restart();
|
||||
_running = true;
|
||||
|
||||
// 立即保存起点静止状态，避免第一帧被后台线程延迟。
|
||||
CaptureSample();
|
||||
|
||||
_worker = new Thread(SamplingLoop)
|
||||
{
|
||||
IsBackground = true,
|
||||
Name = "TrackingExperimentRecorder"
|
||||
};
|
||||
_worker.Start();
|
||||
}
|
||||
|
||||
// 供Stanley/LQR控制器主动写入本周期最终速度命令。
|
||||
// 调用后优先记录该命令，不再使用底盘反解值。
|
||||
public void UpdateCommand(
|
||||
float commandSpeed,
|
||||
float commandAngularSpeed)
|
||||
{
|
||||
lock (_commandSyncRoot)
|
||||
{
|
||||
_externalCommandSpeed = commandSpeed;
|
||||
_externalCommandVx = commandSpeed;
|
||||
_externalCommandVy = 0f;
|
||||
_externalCommandAngularSpeed =
|
||||
commandAngularSpeed;
|
||||
_hasExternalCommand = true;
|
||||
}
|
||||
}
|
||||
|
||||
// 供全向、蟹行和曲线控制器写入完整车体速度命令。
|
||||
public void UpdateBodyCommand(
|
||||
float commandVx,
|
||||
float commandVy,
|
||||
float commandAngularSpeed)
|
||||
{
|
||||
lock (_commandSyncRoot)
|
||||
{
|
||||
_externalCommandVx = commandVx;
|
||||
_externalCommandVy = commandVy;
|
||||
_externalCommandSpeed =
|
||||
(float)Math.Sqrt(
|
||||
commandVx * commandVx +
|
||||
commandVy * commandVy);
|
||||
_externalCommandAngularSpeed =
|
||||
commandAngularSpeed;
|
||||
_hasExternalCommand = true;
|
||||
}
|
||||
}
|
||||
|
||||
// 停止采样并将本次实验保存为CSV；重复调用只保存一次。
|
||||
public void StopAndSave()
|
||||
{
|
||||
if (Volatile.Read(ref _started) == 0)
|
||||
return;
|
||||
|
||||
if (Interlocked.Exchange(ref _saved, 1) != 0)
|
||||
return;
|
||||
|
||||
try
|
||||
{
|
||||
_running = false;
|
||||
|
||||
if (_worker != null &&
|
||||
_worker != Thread.CurrentThread)
|
||||
{
|
||||
_worker.Join(
|
||||
Math.Max(1000, _sampleIntervalMs * 4));
|
||||
}
|
||||
|
||||
// 保存停止时刻的最后一帧。
|
||||
CaptureSample();
|
||||
_stopwatch.Stop();
|
||||
SaveCsv();
|
||||
|
||||
Console.WriteLine(
|
||||
$"轨迹实验数据已保存：{SavedFilePath}");
|
||||
}
|
||||
catch
|
||||
{
|
||||
// 保存失败后允许调用者再次尝试。
|
||||
Interlocked.Exchange(ref _saved, 0);
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
// 按固定周期采集Detour位姿和控制命令。
|
||||
private void SamplingLoop()
|
||||
{
|
||||
while (_running)
|
||||
{
|
||||
Thread.Sleep(_sampleIntervalMs);
|
||||
|
||||
if (!_running)
|
||||
break;
|
||||
|
||||
CaptureSample();
|
||||
}
|
||||
}
|
||||
|
||||
// 采集一帧Detour位姿和控制命令。
|
||||
private void CaptureSample()
|
||||
{
|
||||
try
|
||||
{
|
||||
var location =
|
||||
DetourInterface.getCartLocation();
|
||||
|
||||
float commandSpeed;
|
||||
float commandVx;
|
||||
float commandVy;
|
||||
float commandAngularSpeed;
|
||||
|
||||
lock (_commandSyncRoot)
|
||||
{
|
||||
if (_hasExternalCommand)
|
||||
{
|
||||
commandSpeed =
|
||||
_externalCommandSpeed;
|
||||
commandVx =
|
||||
_externalCommandVx;
|
||||
commandVy =
|
||||
_externalCommandVy;
|
||||
commandAngularSpeed =
|
||||
_externalCommandAngularSpeed;
|
||||
}
|
||||
else
|
||||
{
|
||||
var command =
|
||||
PilotDefinition.Chassis
|
||||
.GetCarSpeed(false);
|
||||
|
||||
commandVx = command.Vx;
|
||||
commandVy = command.Vy;
|
||||
// CommonUsage.GetCarSpeed().Vw的单位为deg/s，
|
||||
// 记录器内部统一转换为rad/s。
|
||||
commandAngularSpeed =
|
||||
(float)AngleMath.DegreesToRadians(
|
||||
command.Vw);
|
||||
commandSpeed = (float)Math.Sqrt(
|
||||
commandVx * commandVx +
|
||||
commandVy * commandVy);
|
||||
}
|
||||
}
|
||||
|
||||
var sample = new TrackingSample
|
||||
{
|
||||
ElapsedSeconds =
|
||||
_stopwatch.Elapsed.TotalSeconds,
|
||||
DetourX = location.x,
|
||||
DetourY = location.y,
|
||||
DetourTheta = location.th,
|
||||
CommandSpeed = commandSpeed,
|
||||
CommandVx = commandVx,
|
||||
CommandVy = commandVy,
|
||||
CommandAngularSpeed =
|
||||
commandAngularSpeed
|
||||
};
|
||||
|
||||
lock (_sampleSyncRoot)
|
||||
{
|
||||
_samples.Add(sample);
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
// 单帧读取失败不应终止车辆控制或整个记录线程。
|
||||
Console.WriteLine(
|
||||
$"轨迹实验采样失败：{ex.Message}");
|
||||
}
|
||||
}
|
||||
|
||||
// 将内存中的采样数据写入CSV。
|
||||
private void SaveCsv()
|
||||
{
|
||||
List<TrackingSample> snapshot;
|
||||
|
||||
lock (_sampleSyncRoot)
|
||||
{
|
||||
snapshot =
|
||||
new List<TrackingSample>(_samples);
|
||||
}
|
||||
|
||||
var outputDirectory = Path.Combine(
|
||||
AppContext.BaseDirectory,
|
||||
"TrackingExperiments");
|
||||
|
||||
Directory.CreateDirectory(outputDirectory);
|
||||
|
||||
var fileName =
|
||||
$"{DateTime.Now:yyyyMMdd_HHmmss_fff}_" +
|
||||
$"{SanitizeFileName(_controllerName)}_" +
|
||||
$"{SanitizeFileName(_trajectoryName)}_" +
|
||||
$"Trial{_trialNumber}.csv";
|
||||
|
||||
SavedFilePath = Path.Combine(
|
||||
outputDirectory,
|
||||
fileName);
|
||||
|
||||
using (var writer = new StreamWriter(
|
||||
SavedFilePath,
|
||||
false,
|
||||
new UTF8Encoding(true)))
|
||||
{
|
||||
writer.WriteLine(
|
||||
"ElapsedSeconds," +
|
||||
"ControllerName," +
|
||||
"TrajectoryName," +
|
||||
"TrialNumber," +
|
||||
"DetourX," +
|
||||
"DetourY," +
|
||||
"DetourTheta," +
|
||||
"CommandSpeed," +
|
||||
// 保留旧列（deg/s）供历史Python脚本兼容。
|
||||
"CommandAngularSpeed," +
|
||||
"CommandAngularSpeedRadPerSecond," +
|
||||
"CommandVx," +
|
||||
"CommandVy," +
|
||||
"ReferenceStartX," +
|
||||
"ReferenceStartY," +
|
||||
"ReferenceEndX," +
|
||||
"ReferenceEndY," +
|
||||
"ReferenceSpeed," +
|
||||
"ReferenceAngularSpeedRadPerSecond," +
|
||||
"ReferenceMotionFrameYawDegrees");
|
||||
|
||||
foreach (var sample in snapshot)
|
||||
{
|
||||
writer.WriteLine(string.Join(
|
||||
",",
|
||||
Format(sample.ElapsedSeconds),
|
||||
EscapeCsv(_controllerName),
|
||||
EscapeCsv(_trajectoryName),
|
||||
_trialNumber.ToString(
|
||||
CultureInfo.InvariantCulture),
|
||||
Format(sample.DetourX),
|
||||
Format(sample.DetourY),
|
||||
Format(sample.DetourTheta),
|
||||
Format(sample.CommandSpeed),
|
||||
Format(
|
||||
AngleMath.RadiansToDegrees(
|
||||
sample.CommandAngularSpeed)),
|
||||
Format(sample.CommandAngularSpeed),
|
||||
Format(sample.CommandVx),
|
||||
Format(sample.CommandVy),
|
||||
Format(_referenceStart.X),
|
||||
Format(_referenceStart.Y),
|
||||
Format(_referenceEnd.X),
|
||||
Format(_referenceEnd.Y),
|
||||
Format(_referenceSpeed),
|
||||
Format(_referenceAngularSpeed),
|
||||
Format(_referenceMotionFrameYawDegrees)));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 将文件名中的非法字符替换为下划线。
|
||||
private static string SanitizeFileName(string value)
|
||||
{
|
||||
var result = value;
|
||||
|
||||
foreach (var invalidCharacter in
|
||||
Path.GetInvalidFileNameChars())
|
||||
{
|
||||
result = result.Replace(
|
||||
invalidCharacter,
|
||||
'_');
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
// 按固定小数格式输出数值，避免系统区域设置改变CSV格式。
|
||||
private static string Format(double value)
|
||||
{
|
||||
return value.ToString(
|
||||
"0.######",
|
||||
CultureInfo.InvariantCulture);
|
||||
}
|
||||
|
||||
// 对CSV文本字段进行引号和逗号转义。
|
||||
private static string EscapeCsv(string value)
|
||||
{
|
||||
if (value == null)
|
||||
return string.Empty;
|
||||
|
||||
if (!value.Contains(",") &&
|
||||
!value.Contains("\"") &&
|
||||
!value.Contains("\r") &&
|
||||
!value.Contains("\n"))
|
||||
{
|
||||
return value;
|
||||
}
|
||||
|
||||
return
|
||||
"\"" +
|
||||
value.Replace("\"", "\"\"") +
|
||||
"\"";
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,103 @@
|
||||
|
||||
|
||||
using System;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.Pilot;
|
||||
using CommonUsage.Chassis;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
using MyParking.Shared;
|
||||
|
||||
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 =
|
||||
AngleMath.DegreesToRadians(toleranceDegrees);
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user