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:06:26 +08:00
parent a59499e638
commit 33a33af710
41 changed files with 817 additions and 2654 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheelC/Experiments/ClampTests.cs
-90
View File
@@ -1,90 +0,0 @@
// using System;
// using ClumsyCore;
// using ClumsyCore.Pilot;
// using FundamentalLib;
// using MDCSToolBox.Clumsy.Movements;
// using MDCSToolBox.Clumsy.Pilot;
// namespace MultiWheelC
// {
// public abstract class ClampMovementTestBase : MovementTest
// {
// public float TimeoutSeconds = 30f; // 动作超时时间，单位s。
// private DriveTask _task;
// protected abstract bool Close { get; }
// // 根据派生测试类型驱动左右夹臂同步夹紧或打开。
// public override void Test()
// {
// var leftTarget = Close
// ? PilotDefinition.Self.LeftArmUpperPos
// : PilotDefinition.Self.LeftArmLowerPos;
// var rightTarget = Close
// ? PilotDefinition.Self.RightArmUpperPos
// : PilotDefinition.Self.RightArmLowerPos;
// if (float.IsNaN(leftTarget) ||
// float.IsInfinity(leftTarget) ||
// float.IsNaN(rightTarget) ||
// float.IsInfinity(rightTarget))
// {
// Console.WriteLine(
// "夹臂目标位置无效，取消夹臂运动测试。");
// return;
// }
// // 防止重复点击时上一项夹臂任务仍在运行。
// TestStop();
// Console.WriteLine(
// $"开始夹臂{(Close ? "夹紧" : "打开")}测试：" +
// $"左目标={leftTarget}，右目标={rightTarget}");
// var task = new DriveTask(
// new ClampToTarget
// {
// LeftClampTarget = leftTarget,
// RightClampTarget = rightTarget,
// TimeoutSeconds = TimeoutSeconds
// }.Get());
// _task = task;
// try
// {
// task.Wait();
// }
// finally
// {
// PilotDefinition.Self.SpeedLeftArm = 0f;
// PilotDefinition.Self.SpeedRightArm = 0f;
// if (ReferenceEquals(_task, task))
// _task = null;
// }
// }
// // 停止夹臂任务并立即清零左右夹臂下发速度。
// public override void TestStop()
// {
// _task?.Stop();
// _task = null;
// PilotDefinition.Self.SpeedLeftArm = 0f;
// PilotDefinition.Self.SpeedRightArm = 0f;
// }
// }
// [MovementTest(name = "夹臂关闭测试")]
// public sealed class TestClampCloseMovement
// : ClampMovementTestBase
// {
// protected override bool Close => false;
// }
// [MovementTest(name = "夹臂启动测试")]
// public sealed class TestClampOpenMovement
// : ClampMovementTestBase
// {
// protected override bool Close => true;
// }
// }
MultiWheelC/Experiments/CompositeMotionPlanTests.cs
+8 -26
View File
@@ -59,11 +59,6 @@ namespace MultiWheelC
return;
}
if (!MovementTestPreparation.AreWheelsForward())
{
return;
}
var chassis = PilotDefinition.Chassis as MultiWheelChassis;
if (chassis == null)
{
@@ -72,22 +67,18 @@ namespace MultiWheelC
return;
}
var detourStateProvider =
new DetourVehicleStateProvider();
if (!detourStateProvider.TryGetState(
var stateProvider =
ParkingVehicleStateProviderFactory.Create(
chassis);
if (!stateProvider.TryGetState(
out var initialState))
{
Console.WriteLine(
"无法读取组合运动起点位姿：" +
detourStateProvider.LastFailureReason);
"无法读取组合运动起点状态：" +
stateProvider.LastFailureReason);
return;
}
var stateProvider =
new WheelFeedbackVehicleStateProvider(
detourStateProvider,
chassis);
var planStartPose =
TrajectoryExperimentInput.OffsetPoseLaterally(
initialState.PoseInWorld,
@@ -176,15 +167,6 @@ namespace MultiWheelC
{
Segments = plan,
StateProvider = stateProvider,
ConfigureTrackingMovement = tracking =>
{
tracking.StanleyUsesActualSpeed = true;
tracking.MaximumCommandSpeedMetersPerSecond =
StraightMaximumSpeedMetersPerSecond;
tracking
.LongitudinalSpeedErrorDeadbandMetersPerSecond =
0.025;
},
SegmentStarted = (index, segment) =>
{
_recorder?.ClearControlReference();
@@ -330,14 +312,14 @@ namespace MultiWheelC
}
if (controller.LastProjection.HasValue &&
controller.LastReferenceSpeedMetersPerSecond.HasValue)
controller.LastControlReferenceSpeedMetersPerSecond.HasValue)
{
var projection =
controller.LastProjection.Value;
_recorder?.UpdateControlReference(
projection.ArcLengthMeters,
controller
.LastReferenceSpeedMetersPerSecond.Value,
.LastControlReferenceSpeedMetersPerSecond.Value,
projection.LateralErrorMeters,
projection.HeadingErrorRadians,
projection.DistanceToTrajectoryMeters,
MultiWheelC/Experiments/NewControllerTrackingTests.cs
+18 -37
View File
@@ -150,11 +150,6 @@ namespace MultiWheelC
return;
}
if (!MovementTestPreparation.AreWheelsForward())
{
return;
}
var chassis =
PilotDefinition.Chassis as MultiWheelChassis;
if (chassis == null)
@@ -164,22 +159,20 @@ namespace MultiWheelC
return;
}
var detourStateProvider =
new DetourVehicleStateProvider();
if (!detourStateProvider.TryGetState(
var stateProvider =
ParkingVehicleStateProviderFactory.Create(
chassis);
if (!stateProvider.TryGetState(
out var initialState))
{
Console.WriteLine(
"无法读取有效Detour起点位姿：" +
detourStateProvider.LastFailureReason);
"无法读取有效停车状态起点位姿：" +
stateProvider.LastFailureReason);
_stateProvider = null;
return;
}
_stateProvider =
new WheelFeedbackVehicleStateProvider(
detourStateProvider,
chassis);
_stateProvider = stateProvider;
var trajectoryStartPose =
TrajectoryExperimentInput.OffsetPoseLaterally(
@@ -227,8 +220,6 @@ namespace MultiWheelC
{
Trajectory = trajectory,
StateProvider = _stateProvider,
StanleyUsesActualSpeed = true,
MaximumCommandSpeedMetersPerSecond = 0.50,
CycleObserver = controller =>
RecordControlCycle(
recorder,
@@ -358,13 +349,13 @@ namespace MultiWheelC
}
if (controller.LastProjection.HasValue &&
controller.LastReferenceSpeedMetersPerSecond.HasValue)
controller.LastControlReferenceSpeedMetersPerSecond.HasValue)
{
var projection =
controller.LastProjection.Value;
recorder.UpdateControlReference(
projection.ArcLengthMeters,
controller.LastReferenceSpeedMetersPerSecond.Value,
controller.LastControlReferenceSpeedMetersPerSecond.Value,
projection.LateralErrorMeters,
projection.HeadingErrorRadians,
projection.DistanceToTrajectoryMeters,
@@ -511,11 +502,6 @@ namespace MultiWheelC
return;
}
if (!MovementTestPreparation.AreWheelsForward())
{
return;
}
var chassis =
PilotDefinition.Chassis as MultiWheelChassis;
if (chassis == null)
@@ -525,22 +511,20 @@ namespace MultiWheelC
return;
}
var detourStateProvider =
new DetourVehicleStateProvider();
if (!detourStateProvider.TryGetState(
var stateProvider =
ParkingVehicleStateProviderFactory.Create(
chassis);
if (!stateProvider.TryGetState(
out var initialState))
{
Console.WriteLine(
"无法读取有效Detour起点位姿：" +
detourStateProvider.LastFailureReason);
"无法读取有效停车状态起点位姿：" +
stateProvider.LastFailureReason);
_stateProvider = null;
return;
}
_stateProvider =
new WheelFeedbackVehicleStateProvider(
detourStateProvider,
chassis);
_stateProvider = stateProvider;
var trajectoryStartPose =
TrajectoryExperimentInput.OffsetPoseLaterally(
@@ -593,9 +577,6 @@ namespace MultiWheelC
{
Trajectory = trajectory,
StateProvider = _stateProvider,
StanleyUsesActualSpeed = true,
MaximumCommandSpeedMetersPerSecond =
StraightMaximumSpeedMetersPerSecond,
CycleObserver = controller =>
RecordControlCycle(
recorder,
@@ -725,13 +706,13 @@ namespace MultiWheelC
}
if (controller.LastProjection.HasValue &&
controller.LastReferenceSpeedMetersPerSecond.HasValue)
controller.LastControlReferenceSpeedMetersPerSecond.HasValue)
{
var projection =
controller.LastProjection.Value;
recorder.UpdateControlReference(
projection.ArcLengthMeters,
controller.LastReferenceSpeedMetersPerSecond.Value,
controller.LastControlReferenceSpeedMetersPerSecond.Value,
projection.LateralErrorMeters,
projection.HeadingErrorRadians,
projection.DistanceToTrajectoryMeters,
MultiWheelC/Experiments/PathTrackingTests.cs
-635
View File
@@ -1,635 +0,0 @@
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);
}
}
}
MultiWheelC/Experiments/RotationTests.cs
-24
View File
@@ -10,7 +10,6 @@ using ClumsyCore.Pilot;
using FundamentalLib;
using MDCSToolBox.Clumsy.Movements;
using MDCSToolBox.Clumsy.Pilot;
using MDCSToolBox.Commons.Controllers;
using MyParking.Shared;
namespace MultiWheelC
@@ -109,29 +108,6 @@ namespace MultiWheelC
{
// MultiWheelRotateInPlace接收世界坐标系绝对航向。
AngleTarget = targetWorldAngle,
PidparamsRead = () => new PIDParams
{
Kp =
config.InPlaceRotateKp,
Ki =
config.InPlaceRotateKi,
Kd =
config.InPlaceRotateKd,
DeadZone =
config.InPlaceRotateArriveDeg,
SpeedAccPerSec =
config.InPlaceRotateAcc,
OutputUpperThreshold =
config.InPlaceRotateMaxSpeed,
MaxI =
config.InPlaceRotateMaxI
},
MinimumAngularSpeedDegreesPerSecond =
config.InPlaceRotateMinimumSpeed,
WheelAlignmentToleranceDegrees =
config.InPlaceRotateWheelAlignDeg,
RotationTimeoutSeconds =
config.InPlaceRotateTimeoutSec,
CommandAngularSpeedObserver =
commandAngularSpeed =>
_recorder?.UpdateCommand(
MultiWheelC/Experiments/TestTrajectoryFactory.cs
+78 -57
View File
@@ -13,7 +13,7 @@ namespace MultiWheelC
private const double StraightLengthMeters = 4.0;
/// <summary>
/// 从给定车体中心位姿沿当前航向生成带梯形速度规划的4m直线轨迹。
/// 从给定车体中心位姿按速度符号沿车头或车尾方向生成带梯形速度规划的4m直线轨迹。
/// </summary>
public static Trajectory2D CreateStraight4Meters(
Pose2D startPoseInWorld,
@@ -32,7 +32,7 @@ namespace MultiWheelC
}
/// <summary>
/// 从给定车体中心位姿沿当前航向生成指定长度并在终点停车的直线轨迹。
/// 从给定车体中心位姿按速度符号沿车头或车尾方向生成指定长度并在终点停车的直线轨迹。
/// </summary>
public static Trajectory2D CreateStraight(
Pose2D startPoseInWorld,
@@ -42,22 +42,22 @@ namespace MultiWheelC
double decelerationMetersPerSecondSquared = 0.20,
double pointSpacingMeters = 0.02)
{
EnsureFinitePose(
NumericGuard.EnsureFinite(
startPoseInWorld,
nameof(startPoseInWorld));
EnsureFinitePositive(
NumericGuard.EnsureFinitePositive(
lengthMeters,
nameof(lengthMeters));
EnsureFinitePositive(
var travelDirection = GetTravelDirection(
cruiseSpeedMetersPerSecond,
nameof(cruiseSpeedMetersPerSecond));
EnsureFinitePositive(
NumericGuard.EnsureFinitePositive(
accelerationMetersPerSecondSquared,
nameof(accelerationMetersPerSecondSquared));
EnsureFinitePositive(
NumericGuard.EnsureFinitePositive(
decelerationMetersPerSecondSquared,
nameof(decelerationMetersPerSecondSquared));
EnsureFinitePositive(
NumericGuard.EnsureFinitePositive(
pointSpacingMeters,
nameof(pointSpacingMeters));
@@ -73,10 +73,10 @@ namespace MultiWheelC
pointSpacingMeters);
var points = new List<TrajectoryPoint>(
segmentCount + 1);
var directionX = Math.Cos(
startPoseInWorld.YawRadians);
var directionY = Math.Sin(
startPoseInWorld.YawRadians);
var directionX = travelDirection *
Math.Cos(startPoseInWorld.YawRadians);
var directionY = travelDirection *
Math.Sin(startPoseInWorld.YawRadians);
for (var index = 0;
index <= segmentCount;
@@ -116,7 +116,7 @@ namespace MultiWheelC
}
/// <summary>
/// 从当前位姿生成“3m直线、平滑进入半径2m左转、平滑退出、3m直线”的180°转弯轨迹。
/// 从当前位姿按速度符号生成“3m直线、沿行进方向平滑左弯180°、3m直线”的轨迹。
/// </summary>
public static Trajectory2D CreateStraightLeftSemicircleStraight(
Pose2D startPoseInWorld,
@@ -143,7 +143,7 @@ namespace MultiWheelC
}
/// <summary>
/// 生成“直线、平滑左转、直线”轨迹，并使总转向角严格等于指定角度。
/// 按共同速度符号生成“直线、沿行进方向平滑左弯、直线”轨迹，并使总转角严格等于指定角度。
/// </summary>
public static Trajectory2D CreateStraightSmoothLeftTurnStraight(
Pose2D startPoseInWorld,
@@ -157,34 +157,33 @@ namespace MultiWheelC
double decelerationMetersPerSecondSquared,
double pointSpacingMeters)
{
EnsureFinitePose(
NumericGuard.EnsureFinite(
startPoseInWorld,
nameof(startPoseInWorld));
EnsureFinitePositive(
NumericGuard.EnsureFinitePositive(
straightLengthMeters,
nameof(straightLengthMeters));
EnsureFinitePositive(
NumericGuard.EnsureFinitePositive(
turnRadiusMeters,
nameof(turnRadiusMeters));
EnsureFinitePositive(
NumericGuard.EnsureFinitePositive(
turnAngleRadians,
nameof(turnAngleRadians));
EnsureFinitePositive(
NumericGuard.EnsureFinitePositive(
curvatureTransitionLengthMeters,
nameof(curvatureTransitionLengthMeters));
EnsureFinitePositive(
var travelDirection = GetCommonTravelDirection(
straightMaximumSpeedMetersPerSecond,
nameof(straightMaximumSpeedMetersPerSecond));
EnsureFinitePositive(
nameof(straightMaximumSpeedMetersPerSecond),
turnMaximumSpeedMetersPerSecond,
nameof(turnMaximumSpeedMetersPerSecond));
EnsureFinitePositive(
NumericGuard.EnsureFinitePositive(
accelerationMetersPerSecondSquared,
nameof(accelerationMetersPerSecondSquared));
EnsureFinitePositive(
NumericGuard.EnsureFinitePositive(
decelerationMetersPerSecondSquared,
nameof(decelerationMetersPerSecondSquared));
EnsureFinitePositive(
NumericGuard.EnsureFinitePositive(
pointSpacingMeters,
nameof(pointSpacingMeters));
@@ -245,8 +244,10 @@ namespace MultiWheelC
turnStartArcLengthMeters &&
arcLengthMeters <=
turnEndArcLengthMeters
? turnMaximumSpeedMetersPerSecond
: straightMaximumSpeedMetersPerSecond;
? Math.Abs(
turnMaximumSpeedMetersPerSecond)
: Math.Abs(
straightMaximumSpeedMetersPerSecond);
}
ApplyAccelerationAndBrakingLimits(
@@ -256,6 +257,14 @@ namespace MultiWheelC
accelerationMetersPerSecondSquared,
decelerationMetersPerSecondSquared);
for (var index = 0;
index < referenceSpeeds.Length;
index++)
{
referenceSpeeds[index] *=
travelDirection;
}
var points = new List<TrajectoryPoint>(
sampleArcLengths.Count);
var startCos = Math.Cos(
@@ -296,10 +305,10 @@ namespace MultiWheelC
if (Math.Abs(segmentCurvaturePerMeter) <=
1e-12)
{
localX +=
localX += travelDirection *
Math.Cos(localYawRadians) *
segmentLengthMeters;
localY +=
localY += travelDirection *
Math.Sin(localYawRadians) *
segmentLengthMeters;
}
@@ -308,11 +317,11 @@ namespace MultiWheelC
var nextYawRadians =
localYawRadians +
segmentYawChangeRadians;
localX +=
localX += travelDirection *
(Math.Sin(nextYawRadians) -
Math.Sin(localYawRadians)) /
segmentCurvaturePerMeter;
localY +=
localY += travelDirection *
(Math.Cos(localYawRadians) -
Math.Cos(nextYawRadians)) /
segmentCurvaturePerMeter;
@@ -490,7 +499,7 @@ namespace MultiWheelC
}
/// <summary>
/// 对逐点速度上限执行前向加速约束和反向制动约束，生成连续可执行的空间速度曲线。
/// 对逐点速度幅值上限执行前向加速约束和反向制动约束，生成连续可执行的空间速度曲线。
/// </summary>
private static void ApplyAccelerationAndBrakingLimits(
IReadOnlyList<double> arcLengthsMeters,
@@ -547,7 +556,7 @@ namespace MultiWheelC
}
/// <summary>
/// 根据起步、巡航和制动能力计算指定弧长位置允许的参考速度。
/// 根据起步、巡航和制动能力计算指定弧长位置允许的有符号参考速度。
/// </summary>
private static double CalculateReferenceSpeed(
double arcLengthMeters,
@@ -566,52 +575,64 @@ namespace MultiWheelC
decelerationMetersPerSecondSquared *
Math.Max(0.0, remainingDistanceMeters));
return Math.Min(
var travelDirection = GetTravelDirection(
cruiseSpeedMetersPerSecond,
nameof(cruiseSpeedMetersPerSecond));
var speedMagnitude = Math.Min(
Math.Abs(cruiseSpeedMetersPerSecond),
Math.Min(
accelerationLimitedSpeed,
brakingLimitedSpeed));
return travelDirection * speedMagnitude;
}
/// <summary>
/// 检查世界坐标系起点位姿是否全部为有限值。
/// 获取非零有符号速度表示的前进或倒车方向。
/// </summary>
private static void EnsureFinitePose(
Pose2D pose,
private static double GetTravelDirection(
double signedSpeedMetersPerSecond,
string parameterName)
{
if (!IsFinite(pose.XMeters) ||
!IsFinite(pose.YMeters) ||
!IsFinite(pose.YawRadians))
NumericGuard.EnsureFinite(
signedSpeedMetersPerSecond,
parameterName);
if (signedSpeedMetersPerSecond == 0.0)
{
throw new ArgumentOutOfRangeException(
parameterName,
"直线测试轨迹的起点位姿必须由有限值组成。");
"测试轨迹的最大速度不能为零；正值表示前进，负值表示倒车。");
}
return Math.Sign(
signedSpeedMetersPerSecond);
}
/// <summary>
/// 检查测试轨迹参数是否为正有限值。
/// 确保直线段和转弯段速度使用相同的前进或倒车方向。
/// </summary>
private static void EnsureFinitePositive(
double value,
string parameterName)
private static double GetCommonTravelDirection(
double firstSpeedMetersPerSecond,
string firstParameterName,
double secondSpeedMetersPerSecond,
string secondParameterName)
{
if (!IsFinite(value) || value <= 0.0)
var firstDirection = GetTravelDirection(
firstSpeedMetersPerSecond,
firstParameterName);
var secondDirection = GetTravelDirection(
secondSpeedMetersPerSecond,
secondParameterName);
if (firstDirection != secondDirection)
{
throw new ArgumentOutOfRangeException(
parameterName,
"直线测试轨迹的速度、加速度和点间距必须是正有限值。");
throw new ArgumentException(
"同一条测试轨迹的直线段和转弯段速度必须同号，不能在运动中直接切换前进与倒车方向。",
firstParameterName);
}
}
/// <summary>
/// 判断数值是否可用于轨迹计算。
/// </summary>
private static bool IsFinite(double value)
{
return !double.IsNaN(value) &&
!double.IsInfinity(value);
return firstDirection;
}
}
}
MultiWheelC/Experiments/WheelPreparationTests.cs
+9 -42
View File
@@ -3,17 +3,20 @@
using System;
using ClumsyCore;
using ClumsyCore.Pilot;
using CommonUsage.Chassis;
using FundamentalLib;
using MDCSToolBox.Clumsy.Movements;
using MDCSToolBox.Clumsy.Pilot;
using MyParking.Shared;
namespace MultiWheelC
{
/// <summary>
/// 为需要显式执行舵轮回正的测试管理准备动作及其DriveTask生命周期。
/// </summary>
internal static class MovementTestPreparation
{
// 在测试正式开始前，将四个舵轮稳定回正到车体前向。
/// <summary>
/// 执行舵轮回正动作并返回四轮是否已经稳定朝向车体前方。
/// </summary>
public static bool AlignWheelsForward(
ref DriveTask activeTask)
{
@@ -40,47 +43,11 @@ namespace MultiWheelC
}
}
// 只读取实际舵角，检查四个舵轮是否已与车头方向一致。
public static bool AreWheelsForward(
float toleranceDegrees = 2f)
{
var chassis =
PilotDefinition.Chassis as MultiWheelChassis;
if (chassis == null)
{
Console.WriteLine(
"当前底盘不是MultiWheelChassis，无法检查舵轮方向。");
return false;
}
try
{
var adapter = new MultiWheelChassisAdapter(
chassis,
PilotDefinition.Self.CarNum);
var toleranceRadians =
AngleMath.DegreesToRadians(toleranceDegrees);
if (adapter.AreParallelWheelsAligned(
0.0,
toleranceRadians))
{
return true;
}
Console.WriteLine(
"四个舵轮尚未与车头方向一致，请先执行“准备：四个舵轮与车头方向一致”。");
return false;
}
catch (Exception ex)
{
Console.WriteLine(
$"检查舵轮方向失败：{ex.Message}");
return false;
}
}
}
/// <summary>
/// 提供可从测试界面单独触发的四舵轮回正动作。
/// </summary>
[MovementTest(name = "准备：四个舵轮与车头方向一致")]
public class AlignWheelsForwardTest : MovementTest
{