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

集中停车控制配置并完善终点逼近和周期诊断

Browse Source
This commit is contained in:
yuxiang.shen
2026-08-11 17:46:52 +08:00
parent 33a33af710
commit 3043febd91
30 changed files with 2946 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheelC/Old/PathTrackingTests.cs
+635
View File
@@ -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);
// }
// }
// }