完善轨迹跟踪测试并添加实验数据记录与绘图分析工具
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Cursor
@@ -88,3 +88,5 @@ _ReSharper*/
|
||||
*.db
|
||||
*.sqlite
|
||||
*.sqlite3
|
||||
|
||||
*.csv
|
||||
+589
-95
@@ -2,125 +2,619 @@ 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.Numerics;
|
||||
using System.Threading;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
public abstract class DstTrackerTestBase : MovementTest
|
||||
internal static class MovementTestPreparation
|
||||
{
|
||||
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)
|
||||
// 在测试正式开始前，将四个舵轮稳定回正到车体前向。
|
||||
public static bool AlignWheelsForward(
|
||||
ref DriveTask activeTask)
|
||||
{
|
||||
carDirectionBias = defaultCarDirectionBias;
|
||||
}
|
||||
var preparation = new PrepareWheelsForward();
|
||||
var task = new DriveTask(preparation.Get());
|
||||
activeTask = task;
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
_dt?.Stop();
|
||||
_painter?.Clear();
|
||||
}
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
Vector2 p1;
|
||||
Vector2 p2;
|
||||
if (UseInteractivePick)
|
||||
try
|
||||
{
|
||||
p1 = UI.GetPoint("point1");
|
||||
p2 = UI.GetPoint("point2");
|
||||
task.Wait();
|
||||
return preparation.Completed;
|
||||
}
|
||||
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))
|
||||
catch (Exception ex)
|
||||
{
|
||||
Console.WriteLine(
|
||||
$"旋转测试输入无效：{input}");
|
||||
$"测试前舵轮回正失败：{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 = "测试连续前进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;
|
||||
}
|
||||
// 防止重复启动测试时，上一项旋转任务仍在运行。
|
||||
_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,
|
||||
}
|
||||
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: "Stanley",
|
||||
trajectoryName: "Straight4m",
|
||||
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());
|
||||
_dt = task;
|
||||
_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:
|
||||
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,
|
||||
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();
|
||||
}
|
||||
|
||||
// 将世界航向归一化到大约[-180°,180°]。
|
||||
private static float NormalizeDegrees(float angleDegrees)
|
||||
{
|
||||
return (float)(
|
||||
angleDegrees -
|
||||
Math.Round(angleDegrees / 360.0) * 360.0);
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "测试左转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
|
||||
};
|
||||
|
||||
if (!controller.AddTrack(arc, "LeftArc90Degrees"))
|
||||
{
|
||||
Console.WriteLine(
|
||||
"左转90°圆弧轨迹添加失败，取消测试。");
|
||||
return;
|
||||
}
|
||||
|
||||
_recorder = new TrackingExperimentRecorder(
|
||||
controllerName: "GeometricController",
|
||||
trajectoryName:
|
||||
$"LeftArc90_R{RadiusMillimeters:0}mm",
|
||||
trialNumber: TrialNumber,
|
||||
referenceStart: source,
|
||||
referenceEnd: source,
|
||||
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();
|
||||
}
|
||||
}
|
||||
|
||||
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
|
||||
{
|
||||
// 防止旧任务结束时，错误清除后来启动的新任务。
|
||||
if (ReferenceEquals(_dt, task))
|
||||
{
|
||||
_dt = null;
|
||||
}
|
||||
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
|
||||
|
||||
+457
-23
@@ -11,44 +11,92 @@ using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Numerics;
|
||||
using System.Threading;
|
||||
using FundamentalLib;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
public class DstTracker : MovementDefinition
|
||||
// C层测试准备：停车并等待四个舵轮稳定回到车体前向0°。
|
||||
public class PrepareWheelsForward : MovementDefinition
|
||||
{
|
||||
public Vector2 Src;
|
||||
public Vector2 Dst;
|
||||
public float CarDirectionBias = 0f;
|
||||
public Painter Painter = UI.GetPainter("DstTracker");
|
||||
public float ToleranceDegrees = 2f;
|
||||
public float StableSeconds = 0.3f;
|
||||
public float TimeoutSeconds = 10f;
|
||||
public bool Completed { get; private set; }
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||||
DriveTask task = null;
|
||||
var chassis =
|
||||
PilotDefinition.Chassis as MultiWheelChassis;
|
||||
if (chassis == null)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"当前底盘不是MultiWheelChassis，无法执行舵轮回正。");
|
||||
}
|
||||
|
||||
var adapter = new MultiWheelChassisAdapter(
|
||||
chassis,
|
||||
PilotDefinition.Self.CarNum);
|
||||
var toleranceRadians =
|
||||
ToleranceDegrees * Math.PI / 180.0;
|
||||
var startTime = DateTime.UtcNow;
|
||||
DateTime? alignedSince = null;
|
||||
|
||||
Completed = false;
|
||||
if (!adapter.PrepareParallelDirection(0.0))
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"无法将所有舵轮下发到车体前向0°。");
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
Console.WriteLine($"DstTracker src:({Src.X:F2}, {Src.Y:F2}) dst:({Dst.X:F2}, {Dst.Y:F2})");
|
||||
Painter.DrawLine(Color.Cyan, Src.X, Src.Y, Dst.X, Dst.Y, width: 3);
|
||||
|
||||
var tracker = new ChassisController().Get();
|
||||
var linePath = new LineTrack(Src, Dst)
|
||||
while (true)
|
||||
{
|
||||
CarDirectionBias = CarDirectionBias,
|
||||
Speed = PilotDefinition.Conf.DstTrackerMaxSpeed
|
||||
};
|
||||
tracker.AddTrack(linePath);
|
||||
task = new DriveTask(tracker.Track());
|
||||
task.Wait();
|
||||
yield return false;
|
||||
var aligned =
|
||||
adapter.AreParallelWheelsAligned(
|
||||
0.0,
|
||||
toleranceRadians);
|
||||
|
||||
if (aligned)
|
||||
{
|
||||
if (!alignedSince.HasValue)
|
||||
alignedSince = DateTime.UtcNow;
|
||||
|
||||
if ((DateTime.UtcNow -
|
||||
alignedSince.Value).TotalSeconds >=
|
||||
StableSeconds)
|
||||
{
|
||||
Completed = true;
|
||||
yield break;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
alignedSince = null;
|
||||
}
|
||||
|
||||
if (TimeoutSeconds > 0f &&
|
||||
(DateTime.UtcNow - startTime).TotalSeconds >
|
||||
TimeoutSeconds)
|
||||
{
|
||||
throw new TimeoutException(
|
||||
$"舵轮回正超过{TimeoutSeconds:F1}s，" +
|
||||
"测试已经取消。");
|
||||
}
|
||||
|
||||
yield return true;
|
||||
}
|
||||
}
|
||||
finally
|
||||
{
|
||||
task?.Stop();
|
||||
chassis.SendXYThSpeed(0f, 0f, 0f);
|
||||
// 只清零驱动速度，保留已经下发的0°舵角。
|
||||
adapter.StopImmediately();
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#region 功能项
|
||||
public class Sleep : MovementDefinition
|
||||
{
|
||||
public float Second = 2f;
|
||||
@@ -71,7 +119,303 @@ namespace MultiWheelC
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
public class DriverAble : MovementDefinition
|
||||
{
|
||||
public int WaitTimeoutMs = 2000;
|
||||
public int PollIntervalMs = 50;
|
||||
|
||||
// C层单车硬件：请求全部驱动轮复位并恢复使能。
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
PilotDefinition.Self.ResetFromC = true;
|
||||
|
||||
try
|
||||
{
|
||||
var start = DateTime.Now;
|
||||
var timeoutMs = Math.Max(0, WaitTimeoutMs);
|
||||
var pollMs = Math.Max(1, PollIntervalMs);
|
||||
|
||||
// 至少保留一个调度周期，确保M层能收到复位请求。
|
||||
yield return true;
|
||||
|
||||
while (!PilotDefinition.Self.WheelAbleState &&
|
||||
(DateTime.Now - start).TotalMilliseconds < timeoutMs)
|
||||
{
|
||||
Thread.Sleep(pollMs);
|
||||
yield return true;
|
||||
}
|
||||
}
|
||||
finally
|
||||
{
|
||||
PilotDefinition.Self.ResetFromC = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
public class DriverDisable : MovementDefinition
|
||||
{
|
||||
public int WaitTimeoutMs = 3000;
|
||||
public int PollIntervalMs = 20;
|
||||
|
||||
// C层单车硬件：请求驱动轮退出使能，并等待M层状态反馈。
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var timeoutMs = Math.Max(0, WaitTimeoutMs);
|
||||
var pollMs = Math.Max(1, PollIntervalMs);
|
||||
var startTime = DateTime.UtcNow;
|
||||
var success = false;
|
||||
|
||||
PilotDefinition.Self.DisableFromC = true;
|
||||
|
||||
try
|
||||
{
|
||||
// 至少保持一个C层调度周期，确保M层能收到下使能请求。
|
||||
yield return true;
|
||||
|
||||
success = !PilotDefinition.Self.WheelAbleState;
|
||||
|
||||
while (!success &&
|
||||
(DateTime.UtcNow - startTime).TotalMilliseconds <
|
||||
timeoutMs)
|
||||
{
|
||||
Thread.Sleep(pollMs);
|
||||
|
||||
success =
|
||||
!PilotDefinition.Self.WheelAbleState;
|
||||
|
||||
if (!success)
|
||||
{
|
||||
yield return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
finally
|
||||
{
|
||||
// 无论正常完成、超时、异常还是任务被停止，都撤销请求。
|
||||
PilotDefinition.Self.DisableFromC = false;
|
||||
}
|
||||
if (success)
|
||||
{
|
||||
Console.WriteLine(
|
||||
$"驱动器下使能完成，" +
|
||||
$"WheelAbleState=" +
|
||||
$"{PilotDefinition.Self.WheelAbleState}");
|
||||
}
|
||||
else
|
||||
{
|
||||
Console.WriteLine(
|
||||
$"驱动器下使能超时，" +
|
||||
$"WheelAbleState=" +
|
||||
$"{PilotDefinition.Self.WheelAbleState}，" +
|
||||
$"等待{timeoutMs}ms");
|
||||
}
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
#endregion
|
||||
|
||||
#region 直线运动
|
||||
//在世界坐标系下，从路径起点追踪到终点并停车
|
||||
public class DstTracker : MovementDefinition
|
||||
{
|
||||
public Vector2 Src;
|
||||
public Vector2 Dst;
|
||||
// 本次轨迹的巡航速度上限，单位m/s。
|
||||
public float MaxSpeed = PilotDefinition.Conf.DstTrackerMaxSpeed;
|
||||
public float CarDirectionBias = 0f;
|
||||
public Painter Painter = UI.GetPainter("DstTracker");
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||||
DriveTask task = null;
|
||||
try
|
||||
{
|
||||
Console.WriteLine($"DstTracker src:({Src.X:F2}, {Src.Y:F2}) dst:({Dst.X:F2}, {Dst.Y:F2})");
|
||||
Painter.DrawLine(Color.Cyan, Src.X, Src.Y, Dst.X, Dst.Y, width: 3);
|
||||
|
||||
var tracker = new ChassisController
|
||||
{
|
||||
BaseSpeed = MaxSpeed
|
||||
}.Get();
|
||||
// 要求路径末端速度下降到零。
|
||||
tracker.FinishSpeed = 0f;
|
||||
var linePath = new LineTrack(Src, Dst)
|
||||
{
|
||||
CarDirectionBias = CarDirectionBias,
|
||||
Speed = MaxSpeed
|
||||
};
|
||||
tracker.AddTrack(linePath);
|
||||
task = new DriveTask(tracker.Track());
|
||||
task.Wait();
|
||||
yield return false;
|
||||
}
|
||||
finally
|
||||
{
|
||||
task?.Stop();
|
||||
chassis.SendXYThSpeed(0f, 0f, 0f);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
//直线行走基于轮里程
|
||||
// C层单车底盘：按照车轮里程行驶指定的相对距离。
|
||||
public class LineTracking : MovementDefinition
|
||||
{
|
||||
// 相对动作启动位置的行驶距离，单位mm。
|
||||
// 正数表示前进，负数表示后退。
|
||||
public float TargetDistance;
|
||||
public float MaxSpeed = PilotDefinition.Conf.LineTrackMaxSpeed;
|
||||
public float Kp = PilotDefinition.Conf.LineTrackKp;
|
||||
public float Ki = PilotDefinition.Conf.LineTrackKi;
|
||||
public float Kd = PilotDefinition.Conf.LineTrackKd;
|
||||
public float DeadZone = PilotDefinition.Conf.LineTrackDeadZone;
|
||||
public int SrcId = -1;
|
||||
public int DstId = -1;
|
||||
public Action<int> LeaveSrcFunction;
|
||||
// 接近目标后是否保留速度，交给下一个动作接管。
|
||||
public bool EnableHandover;
|
||||
// 进入动作衔接的剩余距离，单位mm。
|
||||
public float HandoverDistance = 80f;
|
||||
// HandoverSpeed小于0时，使用MaxSpeed的此比例。
|
||||
public float HandoverSpeedRatio = 0.5f;
|
||||
// 大于等于0时，直接作为衔接速度，单位m/s。
|
||||
public float HandoverSpeed = -1f;
|
||||
public float MinHandoverSpeed = 0.05f;
|
||||
private PIDController _pid;
|
||||
// 读取当前单车直线行驶里程，单位mm。
|
||||
private static float ReadPosition()
|
||||
{
|
||||
return
|
||||
(PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2f;
|
||||
}
|
||||
|
||||
// 根据动作启动位置和目标距离执行直线里程闭环。
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
if (float.IsNaN(TargetDistance) || float.IsInfinity(TargetDistance))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(TargetDistance),
|
||||
"目标行驶距离必须是有限值。");
|
||||
}
|
||||
|
||||
if (float.IsNaN(MaxSpeed) || float.IsInfinity(MaxSpeed) || MaxSpeed <= 0f)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(MaxSpeed),
|
||||
"最大速度必须是大于零的有限值。");
|
||||
}
|
||||
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||||
// 每次启动动作时重新读取起始编码器位置。
|
||||
var startPosition = ReadPosition();
|
||||
// PID仍然控制绝对编码器位置，但绝对目标由动作自动计算。
|
||||
var targetPosition = startPosition + TargetDistance;
|
||||
_pid = new PIDController(ReadPosition, Kp, Ki, Kd, 0, DeadZone, MaxSpeed)
|
||||
{
|
||||
SpeedAccPerSec = Math.Abs(MaxSpeed) / 2f
|
||||
};
|
||||
var handoverRequested = false;
|
||||
var keepHandoverSpeed = false;
|
||||
DLog.Log(
|
||||
$"直线里程动作：" +
|
||||
$"起点={startPosition:F1}mm，" +
|
||||
$"距离={TargetDistance:F1}mm，" +
|
||||
$"目标={targetPosition:F1}mm",
|
||||
"straight_line");
|
||||
try
|
||||
{
|
||||
while (true)
|
||||
{
|
||||
var currentPosition = ReadPosition();
|
||||
var remainingDistance = targetPosition - currentPosition;
|
||||
// 接近目标后，保留一定速度交给后续动作。
|
||||
if (EnableHandover && Math.Abs(remainingDistance) <= Math.Max(1f, HandoverDistance))
|
||||
{
|
||||
var direction = Math.Sign(remainingDistance);
|
||||
if (direction == 0)
|
||||
{
|
||||
direction = Math.Sign(TargetDistance);
|
||||
}
|
||||
var requestedSpeed = HandoverSpeed >= 0f ? Math.Abs(HandoverSpeed) : Math.Abs(MaxSpeed) * HandoverSpeedRatio;
|
||||
var maximumSpeed = Math.Abs(MaxSpeed);
|
||||
var minimumSpeed = Math.Min(Math.Abs(MinHandoverSpeed), maximumSpeed);
|
||||
var limitedSpeed = Math.Max(minimumSpeed, Math.Min(requestedSpeed, maximumSpeed));
|
||||
var handoverSpeed = limitedSpeed * direction;
|
||||
chassis.SendXYThSpeed(handoverSpeed, 0f, 0f);
|
||||
handoverRequested = true;
|
||||
// 保持一个调度周期，让速度命令实际生效。
|
||||
yield return true;
|
||||
break;
|
||||
}
|
||||
var speed = _pid.GetResponse(targetPosition);
|
||||
chassis.SendXYThSpeed(speed, 0f, 0f);
|
||||
if (_pid.IsArrived())
|
||||
{
|
||||
break;
|
||||
}
|
||||
yield return true;
|
||||
}
|
||||
if (SrcId != -1 &&
|
||||
LeaveSrcFunction != null)
|
||||
{
|
||||
LeaveSrcFunction(SrcId);
|
||||
DLog.Log($"释放放车点{SrcId}", "straight_line");
|
||||
}
|
||||
// 只有正常完成动作衔接时才允许保留非零速度。
|
||||
keepHandoverSpeed = handoverRequested;
|
||||
}
|
||||
finally
|
||||
{
|
||||
// 普通完成、人工停止或异常退出时都必须停车。
|
||||
if (!keepHandoverSpeed)
|
||||
{
|
||||
chassis.SendXYThSpeed(0f, 0f, 0f);
|
||||
}
|
||||
}
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
//直线行走基于detour
|
||||
public class LineTracking_based_detour : MovementDefinition
|
||||
{
|
||||
public float LineDistance = 1000f;
|
||||
public int SrcId = -1;
|
||||
public int DstId = -1;
|
||||
public Action<int> LeaveSrcFunction = null;
|
||||
public Painter painter = UI.GetPainter("Line", false);
|
||||
// C层单车轨迹：执行早期版本的两点直线跟踪动作。
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var curpose = DetourInterface.getCartLocation();
|
||||
Console.WriteLine($"curpose.th:{curpose.th}");
|
||||
var src = new Vector2((float)curpose.x, (float)curpose.y);
|
||||
var headingRadians = curpose.th * Math.PI / 180.0;
|
||||
var dst = new Vector2(
|
||||
(float)(curpose.x +
|
||||
LineDistance * Math.Cos(headingRadians)),
|
||||
(float)(curpose.y +
|
||||
LineDistance * Math.Sin(headingRadians)));
|
||||
// var dst = new Vector2((float)curpose.x + LineDistance * (float)Math.Cos(curpose.th),
|
||||
// (float)curpose.y + LineDistance * (float)Math.Sin(curpose.th));
|
||||
Console.WriteLine($"src:{src.X} {src.Y}");
|
||||
Console.WriteLine($"dst:{dst.X} {dst.Y}");
|
||||
painter.DrawLine(Color.Green, src.X, src.Y, dst.X, dst.Y, width: 3);
|
||||
|
||||
var tracker = new ChassisController().Get();
|
||||
var linePath = new LineTrack(src, dst) { CarDirectionBias = LineDistance > 0 ? 0 : 180 };
|
||||
tracker.AddTrack(linePath);
|
||||
var _dt = new DriveTask(tracker.Track());
|
||||
_dt.Wait();
|
||||
if (SrcId != -1 && LeaveSrcFunction != null)
|
||||
{
|
||||
LeaveSrcFunction(SrcId);
|
||||
DLog.Log($"释放放车点{SrcId}", "straight_line");
|
||||
}
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
#endregion
|
||||
|
||||
#region 旋转运动
|
||||
public class MultiWheelRotateInPlace : MovementDefinition
|
||||
{
|
||||
/// <summary>
|
||||
@@ -89,7 +433,10 @@ namespace MultiWheelC
|
||||
|
||||
public PIDController thPid;
|
||||
|
||||
// 将角度归一化到零到三百六十度范围内。
|
||||
// 将本周期PID角速度输出提供给实验记录器，单位deg/s。
|
||||
public Action<float> CommandAngularSpeedObserver;
|
||||
|
||||
// 归一化到大约 [-180°, 180°]
|
||||
private static float RangeAngle(float theta)
|
||||
{
|
||||
return (float)(theta - Math.Round(theta / 360.0f) * 360);
|
||||
@@ -110,6 +457,7 @@ namespace MultiWheelC
|
||||
{
|
||||
var s = thPid.GetResponse(targetAngle, true);
|
||||
Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}");
|
||||
CommandAngularSpeedObserver?.Invoke(s);
|
||||
Chassis.SendXYThSpeed(0, 0, s);
|
||||
if (thPid.IsArrived()) break;
|
||||
yield return true;
|
||||
@@ -119,10 +467,96 @@ namespace MultiWheelC
|
||||
}
|
||||
finally
|
||||
{
|
||||
CommandAngularSpeedObserver?.Invoke(0f);
|
||||
Chassis.SendXYThSpeed(0, 0, 0);
|
||||
}
|
||||
}
|
||||
}
|
||||
#endregion
|
||||
|
||||
#region 夹臂运动
|
||||
public class ClampToTarget : MovementDefinition
|
||||
{
|
||||
public float LeftClampTarget;
|
||||
public float RightClampTarget;
|
||||
public float MaxClampSpeed = PilotDefinition.Conf.MaxClampSpeed;
|
||||
public float ClampKp = PilotDefinition.Conf.ClampControlKp;
|
||||
public float ClampKi = PilotDefinition.Conf.ClampControlKi;
|
||||
public float ClampKd = PilotDefinition.Conf.ClampControlKd;
|
||||
public float ClampMaxI = PilotDefinition.Conf.ClampControlMaxI;
|
||||
public float ClampSpeedAcc = PilotDefinition.Conf.ClampControlSpeedAcc;
|
||||
public float ClampDeadZone = PilotDefinition.Conf.ClampControlDeadZone;
|
||||
public float TimeoutSeconds = 30f;
|
||||
private PIDController leftpid, rightpid;
|
||||
|
||||
// C层单车业务：驱动左右夹臂运动到夹紧或松开目标。
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
try
|
||||
{
|
||||
leftpid = new PIDController(
|
||||
() => PilotDefinition.Self.ActualPosLeftArm,
|
||||
ClampKp, ClampKi, ClampKd, ClampMaxI,
|
||||
ClampDeadZone, MaxClampSpeed)
|
||||
{
|
||||
SpeedAccPerSec = ClampSpeedAcc
|
||||
};
|
||||
|
||||
rightpid = new PIDController(
|
||||
() => PilotDefinition.Self.ActualPosRightArm,
|
||||
ClampKp, ClampKi, ClampKd, ClampMaxI,
|
||||
ClampDeadZone, MaxClampSpeed)
|
||||
{
|
||||
SpeedAccPerSec = ClampSpeedAcc
|
||||
};
|
||||
|
||||
var startTime = DateTime.UtcNow;
|
||||
while (true)
|
||||
{
|
||||
if (TimeoutSeconds > 0f &&
|
||||
(DateTime.UtcNow - startTime).TotalSeconds >
|
||||
TimeoutSeconds)
|
||||
{
|
||||
Console.WriteLine(
|
||||
$"夹臂运动超时({TimeoutSeconds:F1}s)，" +
|
||||
"停止左右夹臂。");
|
||||
yield break;
|
||||
}
|
||||
|
||||
var leftspeed =
|
||||
leftpid.GetResponse(LeftClampTarget);
|
||||
var rightspeed =
|
||||
rightpid.GetResponse(RightClampTarget);
|
||||
Console.WriteLine(
|
||||
$"left arm speed:{leftspeed} " +
|
||||
$"right arm speed:{rightspeed}");
|
||||
|
||||
PilotDefinition.Self.SpeedLeftArm = leftspeed;
|
||||
PilotDefinition.Self.SpeedRightArm = rightspeed;
|
||||
|
||||
var leftArrived = leftpid.IsArrived();
|
||||
var rightArrived = rightpid.IsArrived();
|
||||
if (leftArrived)
|
||||
PilotDefinition.Self.SpeedLeftArm = 0f;
|
||||
if (rightArrived)
|
||||
PilotDefinition.Self.SpeedRightArm = 0f;
|
||||
|
||||
if (leftArrived && rightArrived)
|
||||
break;
|
||||
|
||||
yield return true;
|
||||
}
|
||||
|
||||
Console.WriteLine(
|
||||
$"left clamp to target:{LeftClampTarget} " +
|
||||
$"right clamp to target:{RightClampTarget}");
|
||||
}
|
||||
finally
|
||||
{
|
||||
PilotDefinition.Self.SpeedLeftArm = 0f;
|
||||
PilotDefinition.Self.SpeedRightArm = 0f;
|
||||
}
|
||||
}
|
||||
}
|
||||
#endregion
|
||||
}
|
||||
|
||||
@@ -38,7 +38,7 @@ public class PilotConfig : MultiWheelPilotConfig
|
||||
|
||||
#region 单车-临时
|
||||
[FieldMember(desc = "原地旋转Kp")]
|
||||
public float InPlaceRotateKp = 0.05f;
|
||||
public float InPlaceRotateKp = 0.2f;
|
||||
|
||||
[FieldMember(desc = "原地旋转Ki")]
|
||||
public float InPlaceRotateKi = 0.01f;
|
||||
|
||||
@@ -0,0 +1,394 @@
|
||||
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;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
// C层实验数据：保存一个采样时刻的定位与控制命令。
|
||||
public sealed class TrackingSample
|
||||
{
|
||||
public double ElapsedSeconds;
|
||||
|
||||
// Detour位置单位为mm，航向单位为deg。
|
||||
public double DetourX;
|
||||
public double DetourY;
|
||||
public double DetourTheta;
|
||||
|
||||
// 车体速度单位为m/s，角速度单位为deg/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 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,
|
||||
int sampleIntervalMs = 50)
|
||||
{
|
||||
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;
|
||||
_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;
|
||||
commandAngularSpeed = 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," +
|
||||
"CommandAngularSpeed," +
|
||||
"CommandVx," +
|
||||
"CommandVy," +
|
||||
"ReferenceStartX," +
|
||||
"ReferenceStartY," +
|
||||
"ReferenceEndX," +
|
||||
"ReferenceEndY," +
|
||||
"ReferenceSpeed");
|
||||
|
||||
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(sample.CommandAngularSpeed),
|
||||
Format(sample.CommandVx),
|
||||
Format(sample.CommandVy),
|
||||
Format(_referenceStart.X),
|
||||
Format(_referenceStart.Y),
|
||||
Format(_referenceEnd.X),
|
||||
Format(_referenceEnd.Y),
|
||||
Format(_referenceSpeed)));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 将文件名中的非法字符替换为下划线。
|
||||
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("\"", "\"\"") +
|
||||
"\"";
|
||||
}
|
||||
}
|
||||
}
|
||||
Binary file not shown.
Binary file not shown.
@@ -13,7 +13,7 @@ using System.Reflection;
|
||||
[assembly: System.Reflection.AssemblyCompanyAttribute("ClumsyPilot")]
|
||||
[assembly: System.Reflection.AssemblyConfigurationAttribute("Debug")]
|
||||
[assembly: System.Reflection.AssemblyFileVersionAttribute("1.0.0.0")]
|
||||
[assembly: System.Reflection.AssemblyInformationalVersionAttribute("1.0.0+580a936a830dcb7a25ef327cf553341405264033")]
|
||||
[assembly: System.Reflection.AssemblyInformationalVersionAttribute("1.0.0+e6b99c45b352f24ff58092f7855d0eadd8828d42")]
|
||||
[assembly: System.Reflection.AssemblyProductAttribute("ClumsyPilot")]
|
||||
[assembly: System.Reflection.AssemblyTitleAttribute("ClumsyPilot")]
|
||||
[assembly: System.Reflection.AssemblyVersionAttribute("1.0.0.0")]
|
||||
|
||||
@@ -1 +1 @@
|
||||
038d57c5b1714403d308c9343b385ef762951607b63a9fb275f4d7d2b8fd29cb
|
||||
3e5507bf56e38facdc245d5ff407e181c060d8094f38b94ad743b8c3305523f2
|
||||
|
||||
@@ -1 +1 @@
|
||||
3920568398d269aea7b71fb4581ad111777c15ca3f2f2bb272ee9e0a989215c4
|
||||
e5afef3021b83202643b31f468b83ac62f224cd367fe52dc0a1cbb98570c91e1
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
BIN
Binary file not shown.
|
After Width: | Height: | Size: 183 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 285 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 247 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 116 KiB |
@@ -0,0 +1,100 @@
|
||||
"""绘制控制器下发角速度命令曲线。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
from pathlib import Path
|
||||
|
||||
import matplotlib
|
||||
matplotlib.use("Agg")
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
|
||||
from plot_trajectory_comparison import (
|
||||
configure_matplotlib,
|
||||
discover_csv_files,
|
||||
load_and_resample,
|
||||
output_path,
|
||||
shade_localization_jump_windows,
|
||||
)
|
||||
|
||||
|
||||
def plot_angular_command(
|
||||
csv_path: Path,
|
||||
frequency_hz: float,
|
||||
filter_window_seconds: float,
|
||||
output_directory: str | None,
|
||||
show: bool,
|
||||
) -> Path:
|
||||
"""生成单份CSV的命令角速度曲线。"""
|
||||
frame, metadata = load_and_resample(
|
||||
csv_path,
|
||||
frequency_hz,
|
||||
filter_window_seconds,
|
||||
)
|
||||
time = frame["TimeSeconds"].to_numpy(dtype=float)
|
||||
angular_command = frame[
|
||||
"CommandAngularSpeedDegPerSec"
|
||||
].to_numpy(dtype=float)
|
||||
maximum = float(np.max(angular_command))
|
||||
minimum = float(np.min(angular_command))
|
||||
|
||||
fig, ax = plt.subplots(figsize=(10.0, 5.5))
|
||||
ax.plot(
|
||||
time,
|
||||
angular_command,
|
||||
color="tab:red",
|
||||
linewidth=1.6,
|
||||
label="CommandAngularSpeed",
|
||||
)
|
||||
ax.axhline(0.0, color="black", linewidth=0.8)
|
||||
shade_localization_jump_windows(ax, metadata)
|
||||
ax.set_xlabel("时间 / s")
|
||||
ax.set_ylabel("命令角速度 / (°/s)")
|
||||
ax.set_title(
|
||||
f"角速度指令曲线\n"
|
||||
f"{metadata['controller_name']} - "
|
||||
f"{metadata['trajectory_name']}，"
|
||||
f"范围=[{minimum:.3f}, {maximum:.3f}]°/s"
|
||||
)
|
||||
ax.grid(True, alpha=0.3)
|
||||
ax.legend()
|
||||
fig.tight_layout()
|
||||
|
||||
destination = output_path(
|
||||
csv_path,
|
||||
output_directory,
|
||||
"angular_command",
|
||||
)
|
||||
fig.savefig(destination, dpi=300, bbox_inches="tight")
|
||||
if show:
|
||||
plt.show()
|
||||
plt.close(fig)
|
||||
return destination
|
||||
|
||||
|
||||
def main() -> None:
|
||||
configure_matplotlib()
|
||||
parser = argparse.ArgumentParser(
|
||||
description="绘制控制器下发角速度命令曲线。"
|
||||
)
|
||||
parser.add_argument("files", nargs="*", help="一个或多个CSV文件")
|
||||
parser.add_argument("--frequency", type=float, default=20.0)
|
||||
parser.add_argument("--window", type=float, default=0.55)
|
||||
parser.add_argument("--output-dir")
|
||||
parser.add_argument("--show", action="store_true")
|
||||
args = parser.parse_args()
|
||||
|
||||
for csv_path in discover_csv_files(args.files):
|
||||
destination = plot_angular_command(
|
||||
csv_path,
|
||||
args.frequency,
|
||||
args.window,
|
||||
args.output_dir,
|
||||
args.show,
|
||||
)
|
||||
print(f"已生成：{destination}")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,177 @@
|
||||
"""绘制控制器参考速度与Detour差分实际速度对比图。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
from pathlib import Path
|
||||
|
||||
import matplotlib
|
||||
matplotlib.use("Agg")
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
|
||||
from plot_trajectory_comparison import (
|
||||
configure_matplotlib,
|
||||
discover_csv_files,
|
||||
load_and_resample,
|
||||
output_path,
|
||||
segmented_savgol,
|
||||
shade_localization_jump_windows,
|
||||
)
|
||||
|
||||
|
||||
def calculate_actual_speed_mps(
|
||||
frame,
|
||||
filter_window_seconds: float,
|
||||
) -> np.ndarray:
|
||||
"""使用Savitzky-Golay求位置导数并计算Detour实际合速度。"""
|
||||
time = frame["TimeSeconds"].to_numpy(dtype=float)
|
||||
dt = float(np.median(np.diff(time)))
|
||||
# 直接对固定频率重采样后的位置做SG求导，避免“先平滑再求导”
|
||||
# 造成两次滤波和过度削弱速度峰值。
|
||||
x_mm = frame["DetourXRawMm"].to_numpy(dtype=float)
|
||||
y_mm = frame["DetourYRawMm"].to_numpy(dtype=float)
|
||||
vx_mm_per_second = segmented_savgol(
|
||||
x_mm,
|
||||
dt,
|
||||
filter_window_seconds,
|
||||
derivative=1,
|
||||
)
|
||||
vy_mm_per_second = segmented_savgol(
|
||||
y_mm,
|
||||
dt,
|
||||
filter_window_seconds,
|
||||
derivative=1,
|
||||
)
|
||||
|
||||
speed = np.hypot(
|
||||
vx_mm_per_second,
|
||||
vy_mm_per_second,
|
||||
) / 1000.0
|
||||
speed[
|
||||
frame["InvalidNearLocalizationJump"].to_numpy(dtype=bool)
|
||||
] = np.nan
|
||||
return speed
|
||||
|
||||
|
||||
def plot_speed(
|
||||
csv_path: Path,
|
||||
frequency_hz: float,
|
||||
filter_window_seconds: float,
|
||||
output_directory: str | None,
|
||||
show: bool,
|
||||
) -> Path:
|
||||
"""生成单份CSV的参考/实际速度响应图。"""
|
||||
frame, metadata = load_and_resample(
|
||||
csv_path,
|
||||
frequency_hz,
|
||||
filter_window_seconds,
|
||||
)
|
||||
time = frame["TimeSeconds"].to_numpy(dtype=float)
|
||||
command_speed = frame["CommandSpeedMps"].to_numpy(dtype=float)
|
||||
actual_speed = calculate_actual_speed_mps(
|
||||
frame,
|
||||
filter_window_seconds,
|
||||
)
|
||||
is_in_place_rotation = (
|
||||
str(metadata["trajectory_name"])
|
||||
.lower()
|
||||
.startswith("rotate")
|
||||
)
|
||||
# 原地自转CSV中的ReferenceSpeed历史上保存的是角速度上限deg/s，
|
||||
# 不能作为线速度m/s使用；其参考线速度应为0。
|
||||
configured_speed = (
|
||||
0.0
|
||||
if is_in_place_rotation
|
||||
else float(metadata["reference_speed_mps"])
|
||||
)
|
||||
|
||||
moving = (
|
||||
(command_speed > max(0.02, configured_speed * 0.1)) &
|
||||
np.isfinite(actual_speed)
|
||||
)
|
||||
if np.any(moving):
|
||||
speed_rmse = float(
|
||||
np.sqrt(
|
||||
np.mean(
|
||||
(actual_speed[moving] - command_speed[moving]) ** 2
|
||||
)
|
||||
)
|
||||
)
|
||||
else:
|
||||
speed_rmse = float("nan")
|
||||
|
||||
fig, ax = plt.subplots(figsize=(10.0, 5.8))
|
||||
ax.plot(
|
||||
time,
|
||||
command_speed,
|
||||
linewidth=1.8,
|
||||
label="控制器参考/下发线速度",
|
||||
)
|
||||
ax.plot(
|
||||
time,
|
||||
actual_speed,
|
||||
linewidth=1.5,
|
||||
label="Detour差分实际线速度（SG求导）",
|
||||
)
|
||||
ax.axhline(
|
||||
configured_speed,
|
||||
linestyle=":",
|
||||
linewidth=1.3,
|
||||
color="tab:green",
|
||||
label=(
|
||||
"原地自转参考线速度 0 m/s"
|
||||
if is_in_place_rotation
|
||||
else f"配置巡航速度 {configured_speed:.3f} m/s"
|
||||
),
|
||||
)
|
||||
shade_localization_jump_windows(ax, metadata)
|
||||
ax.set_xlabel("时间 / s")
|
||||
ax.set_ylabel("线速度 / (m/s)")
|
||||
ax.set_title(
|
||||
f"参考速度与实际速度对比\n"
|
||||
f"{metadata['controller_name']} - "
|
||||
f"{metadata['trajectory_name']}，"
|
||||
f"运动段RMSE={speed_rmse:.4f} m/s"
|
||||
)
|
||||
ax.grid(True, alpha=0.3)
|
||||
ax.legend()
|
||||
fig.tight_layout()
|
||||
|
||||
destination = output_path(
|
||||
csv_path,
|
||||
output_directory,
|
||||
"speed_response",
|
||||
)
|
||||
fig.savefig(destination, dpi=300, bbox_inches="tight")
|
||||
if show:
|
||||
plt.show()
|
||||
plt.close(fig)
|
||||
return destination
|
||||
|
||||
|
||||
def main() -> None:
|
||||
configure_matplotlib()
|
||||
parser = argparse.ArgumentParser(
|
||||
description="绘制参考速度与Detour差分实际速度对比图。"
|
||||
)
|
||||
parser.add_argument("files", nargs="*", help="一个或多个CSV文件")
|
||||
parser.add_argument("--frequency", type=float, default=20.0)
|
||||
parser.add_argument("--window", type=float, default=0.55)
|
||||
parser.add_argument("--output-dir")
|
||||
parser.add_argument("--show", action="store_true")
|
||||
args = parser.parse_args()
|
||||
|
||||
for csv_path in discover_csv_files(args.files):
|
||||
destination = plot_speed(
|
||||
csv_path,
|
||||
args.frequency,
|
||||
args.window,
|
||||
args.output_dir,
|
||||
args.show,
|
||||
)
|
||||
print(f"已生成：{destination}")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,172 @@
|
||||
"""绘制横向误差和航向误差随时间变化图。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
from pathlib import Path
|
||||
|
||||
import matplotlib
|
||||
matplotlib.use("Agg")
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
|
||||
from plot_trajectory_comparison import (
|
||||
build_reference,
|
||||
configure_matplotlib,
|
||||
discover_csv_files,
|
||||
load_and_resample,
|
||||
output_path,
|
||||
shade_localization_jump_windows,
|
||||
)
|
||||
|
||||
|
||||
def plot_errors(
|
||||
csv_path: Path,
|
||||
frequency_hz: float,
|
||||
filter_window_seconds: float,
|
||||
output_directory: str | None,
|
||||
show: bool,
|
||||
) -> Path:
|
||||
"""生成单份CSV的横向/航向误差图。"""
|
||||
frame, metadata = load_and_resample(
|
||||
csv_path,
|
||||
frequency_hz,
|
||||
filter_window_seconds,
|
||||
)
|
||||
reference = build_reference(frame, metadata)
|
||||
time = frame["TimeSeconds"].to_numpy()
|
||||
lateral = np.asarray(reference["lateral_error_mm"])
|
||||
heading = np.asarray(reference["heading_error_degrees"])
|
||||
invalid = frame[
|
||||
"InvalidNearLocalizationJump"
|
||||
].to_numpy(dtype=bool)
|
||||
lateral_for_statistics = lateral.copy()
|
||||
heading_for_statistics = heading.copy()
|
||||
lateral_for_statistics[invalid] = np.nan
|
||||
heading_for_statistics[invalid] = np.nan
|
||||
|
||||
lateral_rmse = float(
|
||||
np.sqrt(np.nanmean(lateral_for_statistics**2))
|
||||
)
|
||||
heading_rmse = float(
|
||||
np.sqrt(np.nanmean(heading_for_statistics**2))
|
||||
)
|
||||
lateral_max = float(
|
||||
np.nanmax(np.abs(lateral_for_statistics))
|
||||
)
|
||||
heading_max = float(
|
||||
np.nanmax(np.abs(heading_for_statistics))
|
||||
)
|
||||
is_in_place_rotation = (
|
||||
reference["kind"] == "in_place_rotation"
|
||||
)
|
||||
|
||||
fig, axes = plt.subplots(
|
||||
2,
|
||||
1,
|
||||
figsize=(10.0, 7.0),
|
||||
sharex=True,
|
||||
)
|
||||
axes[0].plot(time, lateral, linewidth=1.5)
|
||||
axes[0].axhline(0.0, color="black", linewidth=0.8)
|
||||
if is_in_place_rotation:
|
||||
axes[0].set_ylabel("旋转中心位置漂移 / mm")
|
||||
axes[0].set_title(
|
||||
f"原地自转位置漂移：RMS={lateral_rmse:.2f} mm，"
|
||||
f"最大值={lateral_max:.2f} mm"
|
||||
)
|
||||
else:
|
||||
axes[0].set_ylabel("横向误差 / mm")
|
||||
axes[0].set_title(
|
||||
f"横向误差：RMSE={lateral_rmse:.2f} mm，"
|
||||
f"最大绝对值={lateral_max:.2f} mm"
|
||||
)
|
||||
shade_localization_jump_windows(axes[0], metadata)
|
||||
axes[0].grid(True, alpha=0.3)
|
||||
|
||||
axes[1].plot(
|
||||
time,
|
||||
heading,
|
||||
color="tab:orange",
|
||||
linewidth=1.5,
|
||||
)
|
||||
axes[1].axhline(0.0, color="black", linewidth=0.8)
|
||||
axes[1].set_xlabel("时间 / s")
|
||||
axes[1].set_ylabel(
|
||||
"目标角度剩余误差 / °"
|
||||
if is_in_place_rotation
|
||||
else "航向误差 / °"
|
||||
)
|
||||
axes[1].set_title(
|
||||
(
|
||||
f"目标角度剩余误差：RMSE={heading_rmse:.2f}°，"
|
||||
f"最大绝对值={heading_max:.2f}°"
|
||||
)
|
||||
if is_in_place_rotation
|
||||
else (
|
||||
f"航向误差：RMSE={heading_rmse:.2f}°，"
|
||||
f"最大绝对值={heading_max:.2f}°"
|
||||
)
|
||||
)
|
||||
shade_localization_jump_windows(axes[1], metadata)
|
||||
axes[1].grid(True, alpha=0.3)
|
||||
if metadata["localization_jump_events"]:
|
||||
axes[1].legend(loc="best")
|
||||
|
||||
fig.suptitle(
|
||||
f"横向/航向误差随时间变化\n"
|
||||
f"{metadata['controller_name']} - "
|
||||
f"{metadata['trajectory_name']}"
|
||||
)
|
||||
fig.tight_layout()
|
||||
|
||||
destination = output_path(
|
||||
csv_path,
|
||||
output_directory,
|
||||
"tracking_errors",
|
||||
)
|
||||
fig.savefig(destination, dpi=300, bbox_inches="tight")
|
||||
if show:
|
||||
plt.show()
|
||||
plt.close(fig)
|
||||
|
||||
if is_in_place_rotation:
|
||||
print(
|
||||
f"{csv_path.name}: position drift RMS="
|
||||
f"{lateral_rmse:.3f} mm, "
|
||||
f"target-angle error RMS={heading_rmse:.3f} deg"
|
||||
)
|
||||
else:
|
||||
print(
|
||||
f"{csv_path.name}: lateral RMSE="
|
||||
f"{lateral_rmse:.3f} mm, "
|
||||
f"heading RMSE={heading_rmse:.3f} deg"
|
||||
)
|
||||
return destination
|
||||
|
||||
|
||||
def main() -> None:
|
||||
configure_matplotlib()
|
||||
parser = argparse.ArgumentParser(
|
||||
description="绘制横向误差和航向误差随时间变化图。"
|
||||
)
|
||||
parser.add_argument("files", nargs="*", help="一个或多个CSV文件")
|
||||
parser.add_argument("--frequency", type=float, default=20.0)
|
||||
parser.add_argument("--window", type=float, default=0.55)
|
||||
parser.add_argument("--output-dir")
|
||||
parser.add_argument("--show", action="store_true")
|
||||
args = parser.parse_args()
|
||||
|
||||
for csv_path in discover_csv_files(args.files):
|
||||
destination = plot_errors(
|
||||
csv_path,
|
||||
args.frequency,
|
||||
args.window,
|
||||
args.output_dir,
|
||||
args.show,
|
||||
)
|
||||
print(f"已生成：{destination}")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,706 @@
|
||||
"""绘制理想轨迹与Detour实际轨迹对比图。
|
||||
|
||||
不传CSV路径时，默认处理本脚本目录下的全部CSV文件。
|
||||
本文件也提供其余三个绘图脚本共用的数据预处理函数。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import re
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import matplotlib
|
||||
matplotlib.use("Agg")
|
||||
import matplotlib.pyplot as plt
|
||||
import numpy as np
|
||||
import pandas as pd
|
||||
from scipy.signal import savgol_filter
|
||||
|
||||
|
||||
SCRIPT_DIR = Path(__file__).resolve().parent
|
||||
REQUIRED_COLUMNS = {
|
||||
"ElapsedSeconds",
|
||||
"TrajectoryName",
|
||||
"DetourX",
|
||||
"DetourY",
|
||||
"DetourTheta",
|
||||
"CommandSpeed",
|
||||
"CommandAngularSpeed",
|
||||
"ReferenceStartX",
|
||||
"ReferenceStartY",
|
||||
"ReferenceEndX",
|
||||
"ReferenceEndY",
|
||||
"ReferenceSpeed",
|
||||
}
|
||||
|
||||
|
||||
def configure_matplotlib() -> None:
|
||||
"""配置中文字体和图片输出风格。"""
|
||||
matplotlib.rcParams["font.sans-serif"] = [
|
||||
"Microsoft YaHei",
|
||||
"SimHei",
|
||||
"Arial Unicode MS",
|
||||
"DejaVu Sans",
|
||||
]
|
||||
matplotlib.rcParams["axes.unicode_minus"] = False
|
||||
matplotlib.rcParams["figure.dpi"] = 120
|
||||
|
||||
|
||||
def _odd_window_length(
|
||||
sample_count: int,
|
||||
sample_interval: float,
|
||||
window_seconds: float,
|
||||
polynomial_order: int = 2,
|
||||
) -> int | None:
|
||||
"""计算不超过数据长度的Savitzky-Golay奇数窗口。"""
|
||||
requested = max(
|
||||
polynomial_order + 2,
|
||||
int(round(window_seconds / sample_interval)),
|
||||
)
|
||||
if requested % 2 == 0:
|
||||
requested += 1
|
||||
|
||||
maximum = sample_count if sample_count % 2 == 1 else sample_count - 1
|
||||
window = min(requested, maximum)
|
||||
minimum = polynomial_order + 2
|
||||
if minimum % 2 == 0:
|
||||
minimum += 1
|
||||
|
||||
return window if window >= minimum else None
|
||||
|
||||
|
||||
def wrap_degrees(angle_degrees: np.ndarray) -> np.ndarray:
|
||||
"""将角度差归一化到[-180°, 180°)。"""
|
||||
return (angle_degrees + 180.0) % 360.0 - 180.0
|
||||
|
||||
|
||||
def segmented_savgol(
|
||||
values: np.ndarray,
|
||||
sample_interval: float,
|
||||
window_seconds: float,
|
||||
derivative: int = 0,
|
||||
polynomial_order: int = 2,
|
||||
) -> np.ndarray:
|
||||
"""对含NaN断点的数据逐段执行SG滤波或求导。"""
|
||||
values = np.asarray(values, dtype=float)
|
||||
result = np.full_like(values, np.nan)
|
||||
finite_indices = np.flatnonzero(np.isfinite(values))
|
||||
if finite_indices.size == 0:
|
||||
return result
|
||||
|
||||
breaks = np.flatnonzero(np.diff(finite_indices) > 1)
|
||||
starts = np.r_[0, breaks + 1]
|
||||
ends = np.r_[breaks + 1, finite_indices.size]
|
||||
|
||||
for start_index, end_index in zip(starts, ends):
|
||||
indices = finite_indices[start_index:end_index]
|
||||
segment = values[indices]
|
||||
window = _odd_window_length(
|
||||
len(segment),
|
||||
sample_interval,
|
||||
window_seconds,
|
||||
polynomial_order,
|
||||
)
|
||||
if window is not None:
|
||||
result[indices] = savgol_filter(
|
||||
segment,
|
||||
window,
|
||||
polynomial_order,
|
||||
deriv=derivative,
|
||||
delta=sample_interval,
|
||||
mode="interp",
|
||||
)
|
||||
elif derivative == 0:
|
||||
result[indices] = segment
|
||||
elif len(segment) >= 2:
|
||||
result[indices] = np.gradient(segment, sample_interval)
|
||||
|
||||
return result
|
||||
|
||||
|
||||
def shade_localization_jump_windows(
|
||||
axis,
|
||||
metadata: dict[str, Any],
|
||||
) -> None:
|
||||
"""在时间曲线中标记不应参与车辆动力学评价的定位跳变窗口。"""
|
||||
for index, (start, end) in enumerate(
|
||||
metadata["jump_exclusion_windows"]
|
||||
):
|
||||
axis.axvspan(
|
||||
start,
|
||||
end,
|
||||
color="tab:red",
|
||||
alpha=0.12,
|
||||
label="Detour定位跳变排除窗口" if index == 0 else None,
|
||||
)
|
||||
|
||||
|
||||
def load_and_resample(
|
||||
csv_path: Path,
|
||||
frequency_hz: float = 20.0,
|
||||
filter_window_seconds: float = 0.55,
|
||||
) -> tuple[pd.DataFrame, dict[str, Any]]:
|
||||
"""压缩Detour保持帧，检测定位跳变，再分段重采样和平滑。"""
|
||||
if not np.isfinite(frequency_hz) or frequency_hz <= 0.0:
|
||||
raise ValueError("重采样频率必须是正有限值。")
|
||||
|
||||
raw = pd.read_csv(csv_path)
|
||||
missing = REQUIRED_COLUMNS.difference(raw.columns)
|
||||
if missing:
|
||||
raise ValueError(
|
||||
f"{csv_path.name}缺少列：{', '.join(sorted(missing))}"
|
||||
)
|
||||
|
||||
numeric_columns = [
|
||||
"ElapsedSeconds",
|
||||
"DetourX",
|
||||
"DetourY",
|
||||
"DetourTheta",
|
||||
"CommandSpeed",
|
||||
"CommandAngularSpeed",
|
||||
"ReferenceStartX",
|
||||
"ReferenceStartY",
|
||||
"ReferenceEndX",
|
||||
"ReferenceEndY",
|
||||
"ReferenceSpeed",
|
||||
]
|
||||
for column in numeric_columns:
|
||||
raw[column] = pd.to_numeric(raw[column], errors="coerce")
|
||||
|
||||
raw = (
|
||||
raw.dropna(subset=[
|
||||
"ElapsedSeconds",
|
||||
"DetourX",
|
||||
"DetourY",
|
||||
"DetourTheta",
|
||||
])
|
||||
.sort_values("ElapsedSeconds")
|
||||
.drop_duplicates("ElapsedSeconds", keep="last")
|
||||
.reset_index(drop=True)
|
||||
)
|
||||
if len(raw) < 5:
|
||||
raise ValueError(f"{csv_path.name}有效数据不足5行。")
|
||||
|
||||
time_raw = raw["ElapsedSeconds"].to_numpy(dtype=float)
|
||||
time_raw = time_raw - time_raw[0]
|
||||
raw["ElapsedSeconds"] = time_raw
|
||||
duration = float(time_raw[-1])
|
||||
sample_interval = 1.0 / frequency_hz
|
||||
time_uniform = np.arange(
|
||||
0.0,
|
||||
duration + sample_interval * 0.5,
|
||||
sample_interval,
|
||||
)
|
||||
|
||||
def interpolate_command(column: str) -> np.ndarray:
|
||||
values = raw[column].to_numpy(dtype=float)
|
||||
return np.interp(time_uniform, time_raw, values)
|
||||
|
||||
# 记录器频率高于Detour更新频率，会得到A,A,B,B形式的保持帧。
|
||||
# 速度估计前先保留真正发生位姿更新的样本。
|
||||
x_all = raw["DetourX"].to_numpy(dtype=float)
|
||||
y_all = raw["DetourY"].to_numpy(dtype=float)
|
||||
theta_all = raw["DetourTheta"].to_numpy(dtype=float)
|
||||
position_change = np.hypot(np.diff(x_all), np.diff(y_all))
|
||||
heading_change = np.abs(wrap_degrees(np.diff(theta_all)))
|
||||
update_mask = np.r_[
|
||||
True,
|
||||
(position_change > 1e-6) | (heading_change > 1e-6),
|
||||
]
|
||||
updates = raw.loc[update_mask].copy().reset_index(drop=True)
|
||||
if len(updates) < 3:
|
||||
raise ValueError(f"{csv_path.name}有效Detour更新点不足3个。")
|
||||
|
||||
update_time = updates["ElapsedSeconds"].to_numpy(dtype=float)
|
||||
update_x = updates["DetourX"].to_numpy(dtype=float)
|
||||
update_y = updates["DetourY"].to_numpy(dtype=float)
|
||||
update_theta = updates["DetourTheta"].to_numpy(dtype=float)
|
||||
update_command_speed = np.abs(
|
||||
updates["CommandSpeed"].to_numpy(dtype=float)
|
||||
)
|
||||
update_command_angular = np.abs(
|
||||
updates["CommandAngularSpeed"].to_numpy(dtype=float)
|
||||
)
|
||||
|
||||
# 自适应跳变阈值：正常移动允许达到参考位移的3倍并保留15mm余量；
|
||||
# 低速阶段仍至少允许30mm，防止把普通定位噪声误判为跳变。
|
||||
update_dt = np.diff(update_time)
|
||||
update_distance = np.hypot(np.diff(update_x), np.diff(update_y))
|
||||
expected_distance = (
|
||||
0.5 *
|
||||
(update_command_speed[1:] + update_command_speed[:-1]) *
|
||||
update_dt *
|
||||
1000.0
|
||||
)
|
||||
distance_threshold = np.maximum(
|
||||
30.0,
|
||||
expected_distance * 3.0 + 15.0,
|
||||
)
|
||||
update_heading_delta = np.abs(
|
||||
wrap_degrees(np.diff(update_theta))
|
||||
)
|
||||
expected_heading_delta = (
|
||||
0.5 *
|
||||
(update_command_angular[1:] + update_command_angular[:-1]) *
|
||||
update_dt
|
||||
)
|
||||
heading_threshold = np.maximum(
|
||||
5.0,
|
||||
expected_heading_delta * 3.0 + 2.0,
|
||||
)
|
||||
jump_before_current = (
|
||||
(update_distance > distance_threshold) |
|
||||
(update_heading_delta > heading_threshold)
|
||||
)
|
||||
jump_at_update = np.r_[False, jump_before_current]
|
||||
segment_ids = np.cumsum(jump_at_update.astype(int))
|
||||
|
||||
jump_events: list[dict[str, float]] = []
|
||||
for current_index in np.flatnonzero(jump_at_update):
|
||||
previous_index = current_index - 1
|
||||
jump_events.append({
|
||||
"time_seconds": float(update_time[current_index]),
|
||||
"distance_mm": float(update_distance[previous_index]),
|
||||
"heading_change_degrees":
|
||||
float(update_heading_delta[previous_index]),
|
||||
"before_x_mm": float(update_x[previous_index]),
|
||||
"before_y_mm": float(update_y[previous_index]),
|
||||
"after_x_mm": float(update_x[current_index]),
|
||||
"after_y_mm": float(update_y[current_index]),
|
||||
})
|
||||
|
||||
# 不跨越定位跳变插值。跳变前后之间保留NaN，使轨迹图自然断线，
|
||||
# 也防止SG滤波把坐标修正涂抹成车辆高速运动。
|
||||
x_resampled = np.full_like(time_uniform, np.nan)
|
||||
y_resampled = np.full_like(time_uniform, np.nan)
|
||||
theta_resampled = np.full_like(time_uniform, np.nan)
|
||||
update_theta_unwrapped = np.rad2deg(
|
||||
np.unwrap(np.deg2rad(update_theta))
|
||||
)
|
||||
maximum_segment_id = int(segment_ids[-1])
|
||||
for segment_id in range(maximum_segment_id + 1):
|
||||
segment_mask = segment_ids == segment_id
|
||||
segment_time = update_time[segment_mask]
|
||||
if segment_time.size == 0:
|
||||
continue
|
||||
|
||||
interval_start = (
|
||||
0.0 if segment_id == 0 else float(segment_time[0])
|
||||
)
|
||||
interval_end = (
|
||||
duration
|
||||
if segment_id == maximum_segment_id
|
||||
else float(segment_time[-1])
|
||||
)
|
||||
uniform_mask = (
|
||||
(time_uniform >= interval_start) &
|
||||
(time_uniform <= interval_end)
|
||||
)
|
||||
x_resampled[uniform_mask] = np.interp(
|
||||
time_uniform[uniform_mask],
|
||||
segment_time,
|
||||
update_x[segment_mask],
|
||||
)
|
||||
y_resampled[uniform_mask] = np.interp(
|
||||
time_uniform[uniform_mask],
|
||||
segment_time,
|
||||
update_y[segment_mask],
|
||||
)
|
||||
theta_resampled[uniform_mask] = np.interp(
|
||||
time_uniform[uniform_mask],
|
||||
segment_time,
|
||||
update_theta_unwrapped[segment_mask],
|
||||
)
|
||||
|
||||
x_filtered = segmented_savgol(
|
||||
x_resampled,
|
||||
sample_interval,
|
||||
filter_window_seconds,
|
||||
)
|
||||
y_filtered = segmented_savgol(
|
||||
y_resampled,
|
||||
sample_interval,
|
||||
filter_window_seconds,
|
||||
)
|
||||
theta_filtered = segmented_savgol(
|
||||
theta_resampled,
|
||||
sample_interval,
|
||||
filter_window_seconds,
|
||||
)
|
||||
|
||||
exclusion_half_width = max(
|
||||
0.30,
|
||||
filter_window_seconds * 0.5,
|
||||
)
|
||||
jump_exclusion_windows = [
|
||||
(
|
||||
max(0.0, event["time_seconds"] - exclusion_half_width),
|
||||
min(duration, event["time_seconds"] + exclusion_half_width),
|
||||
)
|
||||
for event in jump_events
|
||||
]
|
||||
invalid_near_jump = np.zeros(len(time_uniform), dtype=bool)
|
||||
for start, end in jump_exclusion_windows:
|
||||
invalid_near_jump |= (
|
||||
(time_uniform >= start) & (time_uniform <= end)
|
||||
)
|
||||
|
||||
frame = pd.DataFrame({
|
||||
"TimeSeconds": time_uniform,
|
||||
"DetourXRawMm": x_resampled,
|
||||
"DetourYRawMm": y_resampled,
|
||||
"DetourXFilteredMm": x_filtered,
|
||||
"DetourYFilteredMm": y_filtered,
|
||||
"DetourThetaUnwrappedDeg": theta_filtered,
|
||||
"DetourThetaDeg": wrap_degrees(theta_filtered),
|
||||
"CommandSpeedMps": interpolate_command("CommandSpeed"),
|
||||
"CommandAngularSpeedDegPerSec":
|
||||
interpolate_command("CommandAngularSpeed"),
|
||||
"InvalidNearLocalizationJump": invalid_near_jump,
|
||||
})
|
||||
|
||||
first = raw.iloc[0]
|
||||
metadata: dict[str, Any] = {
|
||||
"csv_path": csv_path,
|
||||
"trajectory_name": str(first["TrajectoryName"]),
|
||||
"controller_name": str(first.get("ControllerName", "")),
|
||||
"trial_number": str(first.get("TrialNumber", "")),
|
||||
"reference_start_mm": np.array(
|
||||
[first["ReferenceStartX"], first["ReferenceStartY"]],
|
||||
dtype=float,
|
||||
),
|
||||
"reference_end_mm": np.array(
|
||||
[first["ReferenceEndX"], first["ReferenceEndY"]],
|
||||
dtype=float,
|
||||
),
|
||||
"reference_speed_mps": float(first["ReferenceSpeed"]),
|
||||
# 圆弧构造时使用了测试开始处Detour航向，因此这里取首帧航向。
|
||||
"start_heading_degrees": float(first["DetourTheta"]),
|
||||
"sample_interval_seconds": sample_interval,
|
||||
"filter_window_seconds": filter_window_seconds,
|
||||
"raw_sample_count": len(raw),
|
||||
"detour_update_count": len(updates),
|
||||
"held_sample_count": int(len(raw) - len(updates)),
|
||||
"localization_jump_events": jump_events,
|
||||
"jump_exclusion_windows": jump_exclusion_windows,
|
||||
}
|
||||
return frame, metadata
|
||||
|
||||
|
||||
def build_reference(
|
||||
frame: pd.DataFrame,
|
||||
metadata: dict[str, Any],
|
||||
) -> dict[str, np.ndarray | float | str]:
|
||||
"""根据CSV元数据建立直线、圆弧或原地自转参考及误差。"""
|
||||
trajectory_name = str(metadata["trajectory_name"])
|
||||
start = np.asarray(metadata["reference_start_mm"], dtype=float)
|
||||
end = np.asarray(metadata["reference_end_mm"], dtype=float)
|
||||
actual = frame[
|
||||
["DetourXFilteredMm", "DetourYFilteredMm"]
|
||||
].to_numpy(dtype=float)
|
||||
actual_heading = frame["DetourThetaUnwrappedDeg"].to_numpy(dtype=float)
|
||||
|
||||
radius_match = re.search(
|
||||
r"LeftArc(?P<sweep>[0-9.]+)_R(?P<radius>[0-9.]+)mm",
|
||||
trajectory_name,
|
||||
flags=re.IGNORECASE,
|
||||
)
|
||||
if radius_match:
|
||||
radius = float(radius_match.group("radius"))
|
||||
sweep_degrees = float(radius_match.group("sweep"))
|
||||
start_heading = float(metadata["start_heading_degrees"])
|
||||
heading_radians = np.deg2rad(start_heading)
|
||||
center = start + radius * np.array(
|
||||
[-np.sin(heading_radians), np.cos(heading_radians)]
|
||||
)
|
||||
start_radial_degrees = start_heading - 90.0
|
||||
|
||||
radial = actual - center
|
||||
distance_to_center = np.linalg.norm(radial, axis=1)
|
||||
radial_angle_degrees = np.rad2deg(
|
||||
np.arctan2(radial[:, 1], radial[:, 0])
|
||||
)
|
||||
radial_angle_radians = np.deg2rad(radial_angle_degrees)
|
||||
reference_points = center + radius * np.column_stack([
|
||||
np.cos(radial_angle_radians),
|
||||
np.sin(radial_angle_radians),
|
||||
])
|
||||
# 对逆时针圆弧，正横向误差表示车辆位于轨迹左侧（圆内侧）。
|
||||
lateral_error = radius - distance_to_center
|
||||
reference_heading = radial_angle_degrees + 90.0
|
||||
heading_error = wrap_degrees(
|
||||
actual_heading - reference_heading
|
||||
)
|
||||
|
||||
plot_angles = np.deg2rad(
|
||||
np.linspace(
|
||||
start_radial_degrees,
|
||||
start_radial_degrees + sweep_degrees,
|
||||
361,
|
||||
)
|
||||
)
|
||||
ideal_plot = center + radius * np.column_stack([
|
||||
np.cos(plot_angles),
|
||||
np.sin(plot_angles),
|
||||
])
|
||||
return {
|
||||
"kind": "left_arc",
|
||||
"ideal_plot_mm": ideal_plot,
|
||||
"reference_points_mm": reference_points,
|
||||
"reference_heading_degrees": reference_heading,
|
||||
"lateral_error_mm": lateral_error,
|
||||
"heading_error_degrees": heading_error,
|
||||
"center_mm": center,
|
||||
"radius_mm": radius,
|
||||
}
|
||||
|
||||
line = end - start
|
||||
length = float(np.linalg.norm(line))
|
||||
if length <= 1e-6:
|
||||
rotation_match = re.search(
|
||||
r"Rotate(?P<angle>[+-]?[0-9.]+)",
|
||||
trajectory_name,
|
||||
flags=re.IGNORECASE,
|
||||
)
|
||||
if rotation_match:
|
||||
relative_angle_degrees = float(
|
||||
rotation_match.group("angle")
|
||||
)
|
||||
target_heading_degrees = (
|
||||
float(metadata["start_heading_degrees"]) +
|
||||
relative_angle_degrees
|
||||
)
|
||||
reference_points = np.repeat(
|
||||
start[np.newaxis, :],
|
||||
len(frame),
|
||||
axis=0,
|
||||
)
|
||||
position_drift = np.linalg.norm(
|
||||
actual - start,
|
||||
axis=1,
|
||||
)
|
||||
reference_heading = np.full(
|
||||
len(frame),
|
||||
target_heading_degrees,
|
||||
)
|
||||
heading_error = wrap_degrees(
|
||||
actual_heading - reference_heading
|
||||
)
|
||||
ideal_plot = np.repeat(
|
||||
start[np.newaxis, :],
|
||||
2,
|
||||
axis=0,
|
||||
)
|
||||
return {
|
||||
"kind": "in_place_rotation",
|
||||
"ideal_plot_mm": ideal_plot,
|
||||
"reference_points_mm": reference_points,
|
||||
"reference_heading_degrees": reference_heading,
|
||||
# 对原地自转，该字段表示偏离初始旋转中心的距离。
|
||||
"lateral_error_mm": position_drift,
|
||||
"heading_error_degrees": heading_error,
|
||||
"rotation_center_mm": start,
|
||||
"relative_angle_degrees": relative_angle_degrees,
|
||||
"target_heading_degrees": target_heading_degrees,
|
||||
}
|
||||
|
||||
raise ValueError(
|
||||
f"{trajectory_name}无法识别为圆弧，且参考直线长度为0。"
|
||||
)
|
||||
|
||||
tangent = line / length
|
||||
left_normal = np.array([-tangent[1], tangent[0]])
|
||||
displacement = actual - start
|
||||
progress = np.clip(displacement @ tangent, 0.0, length)
|
||||
reference_points = start + np.outer(progress, tangent)
|
||||
lateral_error = (actual - reference_points) @ left_normal
|
||||
reference_heading_scalar = np.rad2deg(
|
||||
np.arctan2(tangent[1], tangent[0])
|
||||
)
|
||||
reference_heading = np.full(len(frame), reference_heading_scalar)
|
||||
heading_error = wrap_degrees(
|
||||
actual_heading - reference_heading
|
||||
)
|
||||
ideal_plot = np.linspace(start, end, 361)
|
||||
return {
|
||||
"kind": "line",
|
||||
"ideal_plot_mm": ideal_plot,
|
||||
"reference_points_mm": reference_points,
|
||||
"reference_heading_degrees": reference_heading,
|
||||
"lateral_error_mm": lateral_error,
|
||||
"heading_error_degrees": heading_error,
|
||||
}
|
||||
|
||||
|
||||
def discover_csv_files(arguments: list[str]) -> list[Path]:
|
||||
"""解析命令行CSV；未指定时使用脚本目录下全部CSV。"""
|
||||
if arguments:
|
||||
files = [Path(item).expanduser().resolve() for item in arguments]
|
||||
else:
|
||||
files = sorted(SCRIPT_DIR.glob("*.csv"))
|
||||
if not files:
|
||||
raise FileNotFoundError("没有找到可处理的CSV文件。")
|
||||
return files
|
||||
|
||||
|
||||
def output_path(
|
||||
csv_path: Path,
|
||||
output_directory: str | None,
|
||||
suffix: str,
|
||||
) -> Path:
|
||||
"""构造图片输出路径并创建目录。"""
|
||||
directory = (
|
||||
Path(output_directory).expanduser().resolve()
|
||||
if output_directory
|
||||
else csv_path.parent / "plots"
|
||||
)
|
||||
directory.mkdir(parents=True, exist_ok=True)
|
||||
return directory / f"{csv_path.stem}_{suffix}.png"
|
||||
|
||||
|
||||
def plot_trajectory(
|
||||
csv_path: Path,
|
||||
frequency_hz: float,
|
||||
filter_window_seconds: float,
|
||||
output_directory: str | None,
|
||||
show: bool,
|
||||
) -> Path:
|
||||
"""生成单份CSV的理想/实际轨迹对比图。"""
|
||||
frame, metadata = load_and_resample(
|
||||
csv_path,
|
||||
frequency_hz,
|
||||
filter_window_seconds,
|
||||
)
|
||||
reference = build_reference(frame, metadata)
|
||||
|
||||
actual_x_m = frame["DetourXFilteredMm"].to_numpy() / 1000.0
|
||||
actual_y_m = frame["DetourYFilteredMm"].to_numpy() / 1000.0
|
||||
ideal_m = np.asarray(reference["ideal_plot_mm"]) / 1000.0
|
||||
|
||||
fig, ax = plt.subplots(figsize=(8.0, 7.0))
|
||||
ax.plot(
|
||||
ideal_m[:, 0],
|
||||
ideal_m[:, 1],
|
||||
"--",
|
||||
linewidth=2.2,
|
||||
label="理想轨迹",
|
||||
)
|
||||
ax.plot(
|
||||
actual_x_m,
|
||||
actual_y_m,
|
||||
linewidth=1.8,
|
||||
label="Detour实际轨迹（滤波后）",
|
||||
)
|
||||
if reference["kind"] == "in_place_rotation":
|
||||
ax.scatter(
|
||||
[ideal_m[0, 0]],
|
||||
[ideal_m[0, 1]],
|
||||
marker="*",
|
||||
s=100,
|
||||
label="理想旋转中心",
|
||||
zorder=5,
|
||||
)
|
||||
else:
|
||||
ax.scatter(
|
||||
[ideal_m[0, 0]],
|
||||
[ideal_m[0, 1]],
|
||||
marker="o",
|
||||
s=55,
|
||||
label="起点",
|
||||
zorder=5,
|
||||
)
|
||||
ax.scatter(
|
||||
[ideal_m[-1, 0]],
|
||||
[ideal_m[-1, 1]],
|
||||
marker="x",
|
||||
s=65,
|
||||
label="终点",
|
||||
zorder=5,
|
||||
)
|
||||
for event_index, event in enumerate(
|
||||
metadata["localization_jump_events"]
|
||||
):
|
||||
before = np.array([
|
||||
event["before_x_mm"],
|
||||
event["before_y_mm"],
|
||||
]) / 1000.0
|
||||
after = np.array([
|
||||
event["after_x_mm"],
|
||||
event["after_y_mm"],
|
||||
]) / 1000.0
|
||||
ax.scatter(
|
||||
[before[0], after[0]],
|
||||
[before[1], after[1]],
|
||||
marker="x",
|
||||
color="tab:red",
|
||||
s=55,
|
||||
zorder=6,
|
||||
label="Detour定位跳变前/后"
|
||||
if event_index == 0 else None,
|
||||
)
|
||||
ax.annotate(
|
||||
f"定位跳变 {event['distance_mm']:.1f} mm\n"
|
||||
f"t={event['time_seconds']:.2f} s",
|
||||
xy=(after[0], after[1]),
|
||||
xytext=(8, 8),
|
||||
textcoords="offset points",
|
||||
color="tab:red",
|
||||
fontsize=9,
|
||||
)
|
||||
ax.set_aspect("equal", adjustable="box")
|
||||
ax.set_xlabel("世界坐标 X / m")
|
||||
ax.set_ylabel("世界坐标 Y / m")
|
||||
ax.set_title(
|
||||
f"理想轨迹与实际轨迹对比\n"
|
||||
f"{metadata['controller_name']} - "
|
||||
f"{metadata['trajectory_name']}"
|
||||
)
|
||||
ax.grid(True, alpha=0.3)
|
||||
ax.legend()
|
||||
fig.tight_layout()
|
||||
|
||||
destination = output_path(
|
||||
csv_path,
|
||||
output_directory,
|
||||
"trajectory_comparison",
|
||||
)
|
||||
fig.savefig(destination, dpi=300, bbox_inches="tight")
|
||||
if show:
|
||||
plt.show()
|
||||
plt.close(fig)
|
||||
print(
|
||||
f"{csv_path.name}: 原始采样{metadata['raw_sample_count']}帧，"
|
||||
f"有效Detour更新{metadata['detour_update_count']}帧，"
|
||||
f"保持重复{metadata['held_sample_count']}帧，"
|
||||
f"定位跳变{len(metadata['localization_jump_events'])}次"
|
||||
)
|
||||
return destination
|
||||
|
||||
|
||||
def main() -> None:
|
||||
configure_matplotlib()
|
||||
parser = argparse.ArgumentParser(
|
||||
description="绘制理想轨迹与Detour实际轨迹对比图。"
|
||||
)
|
||||
parser.add_argument("files", nargs="*", help="一个或多个CSV文件")
|
||||
parser.add_argument("--frequency", type=float, default=20.0)
|
||||
parser.add_argument("--window", type=float, default=0.55)
|
||||
parser.add_argument("--output-dir")
|
||||
parser.add_argument("--show", action="store_true")
|
||||
args = parser.parse_args()
|
||||
|
||||
for csv_path in discover_csv_files(args.files):
|
||||
destination = plot_trajectory(
|
||||
csv_path,
|
||||
args.frequency,
|
||||
args.window,
|
||||
args.output_dir,
|
||||
args.show,
|
||||
)
|
||||
print(f"已生成：{destination}")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
Binary file not shown.
|
After Width: | Height: | Size: 244 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 274 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 316 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 92 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 168 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 268 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 276 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 256 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 246 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 282 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 316 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 92 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 156 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 294 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 279 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 307 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 164 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 296 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 284 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 168 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 170 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 283 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 305 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 159 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 122 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 283 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 293 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 89 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 156 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 268 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 264 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 292 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 161 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 296 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 292 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 180 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 120 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 287 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 286 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 94 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 154 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 278 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 274 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 293 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 165 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 290 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 298 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 186 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 119 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 281 KiB |
Binary file not shown.
|
After Width: | Height: | Size: 309 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 96 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 168 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 264 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 282 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 256 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 164 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 305 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 291 KiB |
BIN
Binary file not shown.
|
After Width: | Height: | Size: 181 KiB |
@@ -0,0 +1,4 @@
|
||||
numpy>=1.26
|
||||
pandas>=2.2
|
||||
matplotlib>=3.8
|
||||
scipy>=1.12
|
||||
@@ -0,0 +1,191 @@
|
||||
"""一次运行四个轨迹实验绘图脚本。"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import os
|
||||
import subprocess
|
||||
import sys
|
||||
from concurrent.futures import ThreadPoolExecutor, as_completed
|
||||
from pathlib import Path
|
||||
|
||||
|
||||
SCRIPT_NAMES = (
|
||||
"plot_trajectory_comparison.py",
|
||||
"plot_tracking_errors.py",
|
||||
"plot_speed_response.py",
|
||||
"plot_angular_command.py",
|
||||
)
|
||||
|
||||
|
||||
def build_command(
|
||||
script_path: Path,
|
||||
files: list[str],
|
||||
frequency_hz: float,
|
||||
filter_window_seconds: float,
|
||||
output_directory: str | None,
|
||||
show: bool,
|
||||
) -> list[str]:
|
||||
"""为一个绘图脚本构造与统一入口一致的命令行参数。"""
|
||||
command = [
|
||||
sys.executable,
|
||||
str(script_path),
|
||||
*files,
|
||||
"--frequency",
|
||||
str(frequency_hz),
|
||||
"--window",
|
||||
str(filter_window_seconds),
|
||||
]
|
||||
|
||||
if output_directory:
|
||||
command.extend(["--output-dir", output_directory])
|
||||
|
||||
if show:
|
||||
command.append("--show")
|
||||
|
||||
return command
|
||||
|
||||
|
||||
def run_script(
|
||||
script_path: Path,
|
||||
files: list[str],
|
||||
frequency_hz: float,
|
||||
filter_window_seconds: float,
|
||||
output_directory: str | None,
|
||||
show: bool,
|
||||
) -> tuple[str, int, str, str]:
|
||||
"""运行一个绘图脚本并返回名称、退出码及标准输出和错误。"""
|
||||
result = subprocess.run(
|
||||
build_command(
|
||||
script_path,
|
||||
files,
|
||||
frequency_hz,
|
||||
filter_window_seconds,
|
||||
output_directory,
|
||||
show,
|
||||
),
|
||||
cwd=script_path.parent,
|
||||
capture_output=True,
|
||||
text=True,
|
||||
encoding="utf-8",
|
||||
errors="replace",
|
||||
env={
|
||||
**os.environ,
|
||||
"PYTHONIOENCODING": "utf-8",
|
||||
},
|
||||
check=False,
|
||||
)
|
||||
|
||||
return (
|
||||
script_path.name,
|
||||
result.returncode,
|
||||
result.stdout.strip(),
|
||||
result.stderr.strip(),
|
||||
)
|
||||
|
||||
|
||||
def main() -> None:
|
||||
"""并行执行四类实验图的生成任务。"""
|
||||
parser = argparse.ArgumentParser(
|
||||
description="一次生成轨迹、误差、速度响应和角速度指令四类图。"
|
||||
)
|
||||
parser.add_argument(
|
||||
"files",
|
||||
nargs="*",
|
||||
help="一个或多个CSV文件；省略时处理data_process目录下全部CSV。",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--frequency",
|
||||
type=float,
|
||||
default=20.0,
|
||||
help="固定重采样频率，默认20 Hz。",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--window",
|
||||
type=float,
|
||||
default=0.55,
|
||||
help="Savitzky-Golay滤波窗口，默认0.55 s。",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--output-dir",
|
||||
help="图片输出目录；省略时由各绘图脚本使用默认目录。",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--show",
|
||||
action="store_true",
|
||||
help="生成后请求显示图片。",
|
||||
)
|
||||
args = parser.parse_args()
|
||||
|
||||
if args.frequency <= 0.0:
|
||||
parser.error("--frequency必须大于0。")
|
||||
|
||||
if args.window <= 0.0:
|
||||
parser.error("--window必须大于0。")
|
||||
|
||||
script_directory = Path(__file__).resolve().parent
|
||||
script_paths = [
|
||||
script_directory / name
|
||||
for name in SCRIPT_NAMES
|
||||
]
|
||||
missing_scripts = [
|
||||
str(path)
|
||||
for path in script_paths
|
||||
if not path.is_file()
|
||||
]
|
||||
if missing_scripts:
|
||||
parser.error(
|
||||
"缺少绘图脚本：" + "，".join(missing_scripts)
|
||||
)
|
||||
|
||||
print("开始并行生成四类实验图……")
|
||||
failures: list[str] = []
|
||||
|
||||
with ThreadPoolExecutor(
|
||||
max_workers=len(script_paths)
|
||||
) as executor:
|
||||
futures = [
|
||||
executor.submit(
|
||||
run_script,
|
||||
script_path,
|
||||
args.files,
|
||||
args.frequency,
|
||||
args.window,
|
||||
args.output_dir,
|
||||
args.show,
|
||||
)
|
||||
for script_path in script_paths
|
||||
]
|
||||
|
||||
for future in as_completed(futures):
|
||||
script_name, return_code, stdout, stderr = (
|
||||
future.result()
|
||||
)
|
||||
print(f"\n[{script_name}]")
|
||||
if stdout:
|
||||
print(stdout)
|
||||
if stderr:
|
||||
print(stderr, file=sys.stderr)
|
||||
|
||||
if return_code == 0:
|
||||
print("执行成功。")
|
||||
else:
|
||||
failures.append(script_name)
|
||||
print(
|
||||
f"执行失败，退出码={return_code}。",
|
||||
file=sys.stderr,
|
||||
)
|
||||
|
||||
if failures:
|
||||
print(
|
||||
"\n以下脚本执行失败：" +
|
||||
"，".join(failures),
|
||||
file=sys.stderr,
|
||||
)
|
||||
raise SystemExit(1)
|
||||
|
||||
print("\n四类实验图均已生成。")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,35 @@
|
||||
单个停车机器人轨迹测试方案
|
||||
### 测试对象
|
||||
单台停车机器人（+50kg负载）
|
||||
### 测试曲线
|
||||
1. 直线：前进/后退2m、速度0.3m/s、起点终点静止
|
||||
2. 转弯：左转/右转组合前进/后退、转弯半径1m、曲率1.0、速度0.3m/s
|
||||
3. 原地自转：±90°/±180°、角速度10°/s、20°/s、起点终点静止
|
||||
|
||||
### 评价指标
|
||||
横向误差 RMSE、最大横向误差；航向误差 RMSE、最大航向误差；速度误差 RMSE、最大速度偏差；
|
||||
角速度或转角指令的变化曲线；最终位置误差、最终航向误差
|
||||
### 展示形式
|
||||
理想轨迹与实际轨迹对比图、横向/航向误差随时间变化图、参考速度与实际速度对比图、角速度指令曲线
|
||||
|
||||
### LQR调参策略
|
||||
归一化状态/控制量：一般Q、R初始选择对应控制量最大值的平方的倒数
|
||||
或者使用Bryson’s Rule来给 Q、R 一个很好的初始猜测
|
||||
贝叶斯优化在仿真中自动调节Q、R参数
|
||||
ALQR：在线估计最新参数实时重新求解
|
||||
----------------------------------------
|
||||
不调参：学习式
|
||||
|
||||
### 杂项
|
||||
1. M层获取实际小车的速度信息与位置信息并保存、使用python可视化来量化跟踪误差
|
||||
2. 自行车模型改动、过于局限于阿克曼小车的运动学限制
|
||||
3.
|
||||
|
||||
|
||||
|
||||
先用 Bryson’s Rule + 贝叶斯优化在仿真里把 Q/R 调到一个不错的基准。
|
||||
上实车时采用自适应 LQR：在线估计关键参数（尤其是轮胎刚度），实时更新 K。
|
||||
C# 实现的话：
|
||||
矩阵运算继续用 Math.NET
|
||||
贝叶斯优化可以调 Python 库，或者自己写简单版本
|
||||
在线参数估计（RLS）用 C# 写很轻松
|
||||
@@ -0,0 +1,20 @@
|
||||
还可以把底盘的失败原因暴露出来：
|
||||
/// <summary>
|
||||
/// 获取最近一次底盘运动分解失败原因。
|
||||
/// </summary>
|
||||
public string LastFailureReason =>
|
||||
_chassis.LastMotionDecomposeFailureReason;
|
||||
这样调用方可以打印：
|
||||
if (!adapter.Send(command))
|
||||
{
|
||||
Console.WriteLine(
|
||||
$"底盘命令执行失败：{adapter.LastFailureReason}");
|
||||
}
|
||||
|
||||
|
||||
需要注意，Detour 差分速度会有噪声，建议在 Python 中：
|
||||
按固定频率重新采样。
|
||||
对位置做轻微滤波或使用 Savitzky–Golay 求导。
|
||||
再计算速度，避免直接逐点差分产生尖峰。
|
||||
Stanley 和 LQR 必须使用相同的滤波和采样参数。
|
||||
|
||||
Reference in New Issue
Block a user