实现蟹行轨迹跟踪测试并优化底盘XYTh与原地旋转舵角控制
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Cursor
@@ -0,0 +1,667 @@
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.DTools;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using CommonUsage.Chassis;
|
||||
using MyParking.Shared;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Numerics;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
// C层单车测试：在可配置的运动坐标系中统一跟踪直线、圆弧或S型曲线。
|
||||
public sealed class CrabMotionFrameTracker : MovementDefinition
|
||||
{
|
||||
public enum ReferencePathKind
|
||||
{
|
||||
Straight = 0,
|
||||
LeftArc = 1,
|
||||
SCurve = 2
|
||||
}
|
||||
|
||||
public ReferencePathKind PathKind;
|
||||
public Vector2 StartPosition;
|
||||
public double InitialBodyYawRadians;
|
||||
public float LengthMillimeters = 4000f;
|
||||
public float RadiusMillimeters = 2000f;
|
||||
public float SCurveLateralOffsetMillimeters = 400f;
|
||||
public double ArcSweepRadians = Math.PI / 2.0;
|
||||
public float CruiseSpeed = 0.2f;
|
||||
public float SlowDistanceMillimeters = 600f;
|
||||
public float FinishDistanceMillimeters = 30f;
|
||||
public float MinimumSpeed = 0.04f;
|
||||
public double LateralGainPerSecond = 0.8;
|
||||
public double MaximumLateralCorrection = 0.12;
|
||||
public double HeadingGainPerSecond = 1.5;
|
||||
public double MaximumAngularSpeedRadiansPerSecond =
|
||||
30.0 * Math.PI / 180.0;
|
||||
public float WheelAlignmentToleranceDegrees = 2f;
|
||||
public float WheelAlignmentStableSeconds = 0.3f;
|
||||
public float WheelAlignmentTimeoutSeconds = 10f;
|
||||
public float TrackingTimeoutSeconds = 60f;
|
||||
public Action<float, float, float> CommandObserver;
|
||||
|
||||
// 运动坐标系相对车体坐标系的朝向：普通模式为0，蟹行为π/2。
|
||||
public double MotionFrameYawInBodyRadians = Math.PI / 2.0;
|
||||
private double _lastSCurveProgress;
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
ValidateParameters();
|
||||
|
||||
var chassis =
|
||||
PilotDefinition.Chassis as MultiWheelChassis;
|
||||
if (chassis == null)
|
||||
throw new InvalidOperationException(
|
||||
"当前底盘不是MultiWheelChassis，无法执行运动坐标系轨迹测试。");
|
||||
|
||||
var adapter = new MultiWheelChassisAdapter(
|
||||
chassis,
|
||||
PilotDefinition.Self.CarNum);
|
||||
|
||||
var lastCommandTime = DateTime.Now;
|
||||
|
||||
try
|
||||
{
|
||||
// 模式切换阶段只转舵轮，驱动速度始终保持为零。
|
||||
var alignmentStarted = DateTime.Now;
|
||||
DateTime? stableSince = null;
|
||||
while (true)
|
||||
{
|
||||
if (!adapter.PrepareParallelDirection(
|
||||
MotionFrameYawInBodyRadians))
|
||||
throw new InvalidOperationException(
|
||||
"无法生成运动坐标系对应的舵轮准备姿态。");
|
||||
|
||||
var aligned =
|
||||
adapter.AreParallelWheelsAligned(
|
||||
MotionFrameYawInBodyRadians,
|
||||
WheelAlignmentToleranceDegrees *
|
||||
Math.PI / 180.0);
|
||||
|
||||
if (aligned)
|
||||
{
|
||||
if (stableSince == null)
|
||||
stableSince = DateTime.Now;
|
||||
|
||||
if ((DateTime.Now - stableSince.Value)
|
||||
.TotalSeconds >=
|
||||
WheelAlignmentStableSeconds)
|
||||
break;
|
||||
}
|
||||
else
|
||||
{
|
||||
stableSince = null;
|
||||
}
|
||||
|
||||
if ((DateTime.Now - alignmentStarted)
|
||||
.TotalSeconds >
|
||||
WheelAlignmentTimeoutSeconds)
|
||||
throw new TimeoutException(
|
||||
"舵轮在限定时间内未稳定到达运动坐标系初始方向。");
|
||||
|
||||
yield return true;
|
||||
}
|
||||
|
||||
var trackingStarted = DateTime.Now;
|
||||
while (true)
|
||||
{
|
||||
if ((DateTime.Now - trackingStarted)
|
||||
.TotalSeconds >
|
||||
TrackingTimeoutSeconds)
|
||||
throw new TimeoutException(
|
||||
"蟹行轨迹在限定时间内未完成。");
|
||||
|
||||
var location =
|
||||
DetourInterface.getCartLocation();
|
||||
if (!IsFinite(location.x) ||
|
||||
!IsFinite(location.y) ||
|
||||
!IsFinite(location.th))
|
||||
throw new InvalidOperationException(
|
||||
"蟹行轨迹测试期间Detour位姿无效。");
|
||||
|
||||
var currentPosition = new Vector2(
|
||||
(float)location.x,
|
||||
(float)location.y);
|
||||
var currentBodyYaw =
|
||||
location.th * Math.PI / 180.0;
|
||||
|
||||
CalculateReference(
|
||||
currentPosition,
|
||||
out var tangentYaw,
|
||||
out var referencePoint,
|
||||
out var remainingMillimeters,
|
||||
out var referenceCurvature);
|
||||
|
||||
if (remainingMillimeters <=
|
||||
FinishDistanceMillimeters)
|
||||
break;
|
||||
|
||||
var speed =
|
||||
CalculateSpeed(remainingMillimeters);
|
||||
var tangent = new Vector2(
|
||||
(float)Math.Cos(tangentYaw),
|
||||
(float)Math.Sin(tangentYaw));
|
||||
var leftNormal = new Vector2(
|
||||
-tangent.Y,
|
||||
tangent.X);
|
||||
var positionError =
|
||||
currentPosition - referencePoint;
|
||||
var lateralErrorMeters =
|
||||
Vector2.Dot(
|
||||
positionError,
|
||||
leftNormal) / 1000.0;
|
||||
var normalCorrection =
|
||||
Limit(
|
||||
-LateralGainPerSecond *
|
||||
lateralErrorMeters,
|
||||
MaximumLateralCorrection);
|
||||
|
||||
// 先在世界坐标中组合切向速度与横向纠偏速度。
|
||||
var worldVx =
|
||||
tangent.X * speed +
|
||||
leftNormal.X * (float)normalCorrection;
|
||||
var worldVy =
|
||||
tangent.Y * speed +
|
||||
leftNormal.Y * (float)normalCorrection;
|
||||
|
||||
// 将世界速度表达为当前蟹行运动坐标系速度。
|
||||
var motionYaw =
|
||||
currentBodyYaw +
|
||||
MotionFrameYawInBodyRadians;
|
||||
var motionCos = Math.Cos(motionYaw);
|
||||
var motionSin = Math.Sin(motionYaw);
|
||||
var vxInMotion =
|
||||
motionCos * worldVx +
|
||||
motionSin * worldVy;
|
||||
var vyInMotion =
|
||||
-motionSin * worldVx +
|
||||
motionCos * worldVy;
|
||||
|
||||
var desiredBodyYaw =
|
||||
tangentYaw -
|
||||
MotionFrameYawInBodyRadians;
|
||||
var headingError =
|
||||
FrameTransform2D
|
||||
.ShortestAngleDifference(
|
||||
desiredBodyYaw,
|
||||
currentBodyYaw);
|
||||
var omega =
|
||||
speed * referenceCurvature +
|
||||
HeadingGainPerSecond * headingError;
|
||||
omega = Limit(
|
||||
omega,
|
||||
MaximumAngularSpeedRadiansPerSecond);
|
||||
|
||||
// 运动坐标系相对车体系旋转+90°：
|
||||
// 运动系正向速度会转换成车体系+Y速度。
|
||||
var bodyTwist =
|
||||
FrameTransform2D
|
||||
.TransformTwistAtSamePoint(
|
||||
new Pose2D(
|
||||
0.0,
|
||||
0.0,
|
||||
MotionFrameYawInBodyRadians),
|
||||
new Twist2D(
|
||||
vxInMotion,
|
||||
vyInMotion,
|
||||
omega));
|
||||
|
||||
var now = DateTime.Now;
|
||||
var interval = now - lastCommandTime;
|
||||
lastCommandTime = now;
|
||||
var command = new ChassisCommand(
|
||||
PilotDefinition.Self.CarNum,
|
||||
bodyTwist);
|
||||
|
||||
if (!adapter.Send(command, interval))
|
||||
throw new InvalidOperationException(
|
||||
"蟹行轨迹底盘解算失败：" +
|
||||
adapter.LastFailureReason);
|
||||
|
||||
CommandObserver?.Invoke(
|
||||
(float)bodyTwist.VxMetersPerSecond,
|
||||
(float)bodyTwist.VyMetersPerSecond,
|
||||
(float)bodyTwist
|
||||
.OmegaRadiansPerSecond);
|
||||
|
||||
yield return true;
|
||||
}
|
||||
}
|
||||
finally
|
||||
{
|
||||
adapter.StopImmediately();
|
||||
CommandObserver?.Invoke(0f, 0f, 0f);
|
||||
}
|
||||
|
||||
yield return false;
|
||||
}
|
||||
|
||||
// 计算当前点在直线或圆弧上的参考点、切线和剩余距离。
|
||||
private void CalculateReference(
|
||||
Vector2 currentPosition,
|
||||
out double tangentYaw,
|
||||
out Vector2 referencePoint,
|
||||
out float remainingMillimeters,
|
||||
out double curvaturePerMeter)
|
||||
{
|
||||
var initialMotionYaw =
|
||||
InitialBodyYawRadians +
|
||||
MotionFrameYawInBodyRadians;
|
||||
|
||||
if (PathKind == ReferencePathKind.Straight)
|
||||
{
|
||||
var tangent = new Vector2(
|
||||
(float)Math.Cos(initialMotionYaw),
|
||||
(float)Math.Sin(initialMotionYaw));
|
||||
var relative = currentPosition - StartPosition;
|
||||
var progress =
|
||||
Vector2.Dot(relative, tangent);
|
||||
var clampedProgress =
|
||||
Math.Max(
|
||||
0f,
|
||||
Math.Min(progress, LengthMillimeters));
|
||||
|
||||
tangentYaw = initialMotionYaw;
|
||||
referencePoint =
|
||||
StartPosition +
|
||||
tangent * clampedProgress;
|
||||
remainingMillimeters =
|
||||
Math.Max(
|
||||
0f,
|
||||
LengthMillimeters - progress);
|
||||
curvaturePerMeter = 0.0;
|
||||
return;
|
||||
}
|
||||
|
||||
if (PathKind == ReferencePathKind.SCurve)
|
||||
{
|
||||
CalculateSCurveReference(
|
||||
currentPosition,
|
||||
initialMotionYaw,
|
||||
out tangentYaw,
|
||||
out referencePoint,
|
||||
out remainingMillimeters,
|
||||
out curvaturePerMeter);
|
||||
return;
|
||||
}
|
||||
|
||||
var center = GetArcCenter();
|
||||
var startRadialYaw =
|
||||
initialMotionYaw - Math.PI / 2.0;
|
||||
var radial = currentPosition - center;
|
||||
var currentRadialYaw =
|
||||
Math.Atan2(radial.Y, radial.X);
|
||||
var progressRadians =
|
||||
FrameTransform2D.NormalizeAngle(
|
||||
currentRadialYaw - startRadialYaw);
|
||||
|
||||
// 测试圆弧只有+90°，起点附近的轻微负噪声按0处理。
|
||||
if (progressRadians < 0.0)
|
||||
progressRadians = 0.0;
|
||||
|
||||
var clampedProgressRadians =
|
||||
Math.Min(
|
||||
progressRadians,
|
||||
ArcSweepRadians);
|
||||
var referenceRadialYaw =
|
||||
startRadialYaw +
|
||||
clampedProgressRadians;
|
||||
referencePoint = center + new Vector2(
|
||||
RadiusMillimeters *
|
||||
(float)Math.Cos(referenceRadialYaw),
|
||||
RadiusMillimeters *
|
||||
(float)Math.Sin(referenceRadialYaw));
|
||||
tangentYaw =
|
||||
referenceRadialYaw + Math.PI / 2.0;
|
||||
remainingMillimeters =
|
||||
(float)Math.Max(
|
||||
0.0,
|
||||
(ArcSweepRadians - progressRadians) *
|
||||
RadiusMillimeters);
|
||||
curvaturePerMeter =
|
||||
1000.0 / RadiusMillimeters;
|
||||
}
|
||||
|
||||
// 通过离散最近点和解析导数计算两段三次贝塞尔S曲线的参考状态。
|
||||
private void CalculateSCurveReference(
|
||||
Vector2 currentPosition,
|
||||
double initialMotionYaw,
|
||||
out double tangentYaw,
|
||||
out Vector2 referencePoint,
|
||||
out float remainingMillimeters,
|
||||
out double curvaturePerMeter)
|
||||
{
|
||||
const int nearestPointSamples = 200;
|
||||
var searchStart =
|
||||
Math.Max(
|
||||
0.0,
|
||||
_lastSCurveProgress - 0.02);
|
||||
var bestProgress = _lastSCurveProgress;
|
||||
var bestDistanceSquared = double.MaxValue;
|
||||
|
||||
for (var i = 0;
|
||||
i <= nearestPointSamples;
|
||||
i++)
|
||||
{
|
||||
var progress =
|
||||
searchStart +
|
||||
(1.0 - searchStart) *
|
||||
i / nearestPointSamples;
|
||||
EvaluateSCurve(
|
||||
progress,
|
||||
out var localPoint,
|
||||
out _,
|
||||
out _);
|
||||
var worldPoint =
|
||||
LocalPathPointToWorld(
|
||||
localPoint,
|
||||
initialMotionYaw);
|
||||
var distanceSquared =
|
||||
Vector2.DistanceSquared(
|
||||
currentPosition,
|
||||
worldPoint);
|
||||
|
||||
if (distanceSquared <
|
||||
bestDistanceSquared)
|
||||
{
|
||||
bestDistanceSquared =
|
||||
distanceSquared;
|
||||
bestProgress = progress;
|
||||
}
|
||||
}
|
||||
|
||||
// 轨迹进度不允许因定位噪声倒退，防止控制目标跳回上一段曲线。
|
||||
_lastSCurveProgress =
|
||||
Math.Max(
|
||||
_lastSCurveProgress,
|
||||
bestProgress);
|
||||
EvaluateSCurve(
|
||||
_lastSCurveProgress,
|
||||
out var bestLocalPoint,
|
||||
out var firstDerivative,
|
||||
out var secondDerivative);
|
||||
referencePoint =
|
||||
LocalPathPointToWorld(
|
||||
bestLocalPoint,
|
||||
initialMotionYaw);
|
||||
tangentYaw =
|
||||
initialMotionYaw +
|
||||
Math.Atan2(
|
||||
firstDerivative.Y,
|
||||
firstDerivative.X);
|
||||
|
||||
var derivativeMagnitude =
|
||||
Math.Sqrt(
|
||||
firstDerivative.X *
|
||||
firstDerivative.X +
|
||||
firstDerivative.Y *
|
||||
firstDerivative.Y);
|
||||
if (derivativeMagnitude < 1e-6)
|
||||
{
|
||||
curvaturePerMeter = 0.0;
|
||||
}
|
||||
else
|
||||
{
|
||||
// 导数单位为mm，乘1000后将曲率从1/mm转换成1/m。
|
||||
curvaturePerMeter =
|
||||
(firstDerivative.X *
|
||||
secondDerivative.Y -
|
||||
firstDerivative.Y *
|
||||
secondDerivative.X) *
|
||||
1000.0 /
|
||||
Math.Pow(
|
||||
derivativeMagnitude,
|
||||
3.0);
|
||||
}
|
||||
|
||||
remainingMillimeters =
|
||||
ApproximateSCurveRemainingLength(
|
||||
_lastSCurveProgress);
|
||||
}
|
||||
|
||||
// 计算与普通4m S型测试完全一致的三段三次贝塞尔完整S曲线。
|
||||
private void EvaluateSCurve(
|
||||
double progress,
|
||||
out Vector2 point,
|
||||
out Vector2 firstDerivative,
|
||||
out Vector2 secondDerivative)
|
||||
{
|
||||
progress =
|
||||
Math.Max(
|
||||
0.0,
|
||||
Math.Min(progress, 1.0));
|
||||
|
||||
Vector2 p0;
|
||||
Vector2 p1;
|
||||
Vector2 p2;
|
||||
Vector2 p3;
|
||||
double t;
|
||||
|
||||
if (progress <= 0.25)
|
||||
{
|
||||
t = progress * 4.0;
|
||||
p0 = new Vector2(0f, 0f);
|
||||
p1 = new Vector2(
|
||||
LengthMillimeters / 12f,
|
||||
0f);
|
||||
p2 = new Vector2(
|
||||
LengthMillimeters / 6f,
|
||||
SCurveLateralOffsetMillimeters);
|
||||
p3 = new Vector2(
|
||||
LengthMillimeters * 0.25f,
|
||||
SCurveLateralOffsetMillimeters);
|
||||
}
|
||||
else if (progress <= 0.75)
|
||||
{
|
||||
t = (progress - 0.25) * 2.0;
|
||||
p0 = new Vector2(
|
||||
LengthMillimeters * 0.25f,
|
||||
SCurveLateralOffsetMillimeters);
|
||||
p1 = new Vector2(
|
||||
LengthMillimeters / 3f,
|
||||
SCurveLateralOffsetMillimeters);
|
||||
p2 = new Vector2(
|
||||
LengthMillimeters * 2f / 3f,
|
||||
-SCurveLateralOffsetMillimeters);
|
||||
p3 = new Vector2(
|
||||
LengthMillimeters * 0.75f,
|
||||
-SCurveLateralOffsetMillimeters);
|
||||
}
|
||||
else
|
||||
{
|
||||
t = (progress - 0.75) * 4.0;
|
||||
p0 = new Vector2(
|
||||
LengthMillimeters * 0.75f,
|
||||
-SCurveLateralOffsetMillimeters);
|
||||
p1 = new Vector2(
|
||||
LengthMillimeters * 5f / 6f,
|
||||
-SCurveLateralOffsetMillimeters);
|
||||
p2 = new Vector2(
|
||||
LengthMillimeters * 11f / 12f,
|
||||
0f);
|
||||
p3 = new Vector2(
|
||||
LengthMillimeters,
|
||||
0f);
|
||||
}
|
||||
|
||||
var oneMinusT = 1.0 - t;
|
||||
point =
|
||||
p0 * (float)(
|
||||
oneMinusT *
|
||||
oneMinusT *
|
||||
oneMinusT) +
|
||||
p1 * (float)(
|
||||
3.0 *
|
||||
oneMinusT *
|
||||
oneMinusT *
|
||||
t) +
|
||||
p2 * (float)(
|
||||
3.0 *
|
||||
oneMinusT *
|
||||
t *
|
||||
t) +
|
||||
p3 * (float)(t * t * t);
|
||||
firstDerivative =
|
||||
(p1 - p0) *
|
||||
(float)(
|
||||
3.0 *
|
||||
oneMinusT *
|
||||
oneMinusT) +
|
||||
(p2 - p1) *
|
||||
(float)(
|
||||
6.0 *
|
||||
oneMinusT *
|
||||
t) +
|
||||
(p3 - p2) *
|
||||
(float)(3.0 * t * t);
|
||||
secondDerivative =
|
||||
(p2 - 2f * p1 + p0) *
|
||||
(float)(6.0 * oneMinusT) +
|
||||
(p3 - 2f * p2 + p1) *
|
||||
(float)(6.0 * t);
|
||||
}
|
||||
|
||||
// 通过分段采样估算从当前S曲线进度到终点的实际弧长。
|
||||
private float ApproximateSCurveRemainingLength(
|
||||
double startProgress)
|
||||
{
|
||||
const int lengthSamples = 100;
|
||||
EvaluateSCurve(
|
||||
startProgress,
|
||||
out var previousPoint,
|
||||
out _,
|
||||
out _);
|
||||
var length = 0f;
|
||||
|
||||
for (var i = 1;
|
||||
i <= lengthSamples;
|
||||
i++)
|
||||
{
|
||||
var progress =
|
||||
startProgress +
|
||||
(1.0 - startProgress) *
|
||||
i / lengthSamples;
|
||||
EvaluateSCurve(
|
||||
progress,
|
||||
out var point,
|
||||
out _,
|
||||
out _);
|
||||
length +=
|
||||
Vector2.Distance(
|
||||
previousPoint,
|
||||
point);
|
||||
previousPoint = point;
|
||||
}
|
||||
|
||||
return length;
|
||||
}
|
||||
|
||||
// 将以初始蟹行方向为X轴的局部路径点转换到Detour世界坐标。
|
||||
private Vector2 LocalPathPointToWorld(
|
||||
Vector2 localPoint,
|
||||
double initialMotionYaw)
|
||||
{
|
||||
var cos =
|
||||
(float)Math.Cos(initialMotionYaw);
|
||||
var sin =
|
||||
(float)Math.Sin(initialMotionYaw);
|
||||
|
||||
return StartPosition + new Vector2(
|
||||
localPoint.X * cos -
|
||||
localPoint.Y * sin,
|
||||
localPoint.X * sin +
|
||||
localPoint.Y * cos);
|
||||
}
|
||||
|
||||
// 获取蟹行左转圆弧圆心；它位于初始运动方向的左侧。
|
||||
public Vector2 GetArcCenter()
|
||||
{
|
||||
var initialMotionYaw =
|
||||
InitialBodyYawRadians +
|
||||
MotionFrameYawInBodyRadians;
|
||||
return StartPosition + new Vector2(
|
||||
-RadiusMillimeters *
|
||||
(float)Math.Sin(initialMotionYaw),
|
||||
RadiusMillimeters *
|
||||
(float)Math.Cos(initialMotionYaw));
|
||||
}
|
||||
|
||||
// 获取圆弧测试的理论终点。
|
||||
public Vector2 GetArcDestination()
|
||||
{
|
||||
var initialMotionYaw =
|
||||
InitialBodyYawRadians +
|
||||
MotionFrameYawInBodyRadians;
|
||||
var startRadialYaw =
|
||||
initialMotionYaw - Math.PI / 2.0;
|
||||
var endRadialYaw =
|
||||
startRadialYaw + ArcSweepRadians;
|
||||
var center = GetArcCenter();
|
||||
|
||||
return center + new Vector2(
|
||||
RadiusMillimeters *
|
||||
(float)Math.Cos(endRadialYaw),
|
||||
RadiusMillimeters *
|
||||
(float)Math.Sin(endRadialYaw));
|
||||
}
|
||||
|
||||
// 根据剩余路径长度生成终点减速速度。
|
||||
private float CalculateSpeed(
|
||||
float remainingMillimeters)
|
||||
{
|
||||
if (remainingMillimeters >=
|
||||
SlowDistanceMillimeters)
|
||||
return CruiseSpeed;
|
||||
|
||||
var ratio =
|
||||
remainingMillimeters /
|
||||
Math.Max(
|
||||
SlowDistanceMillimeters,
|
||||
1f);
|
||||
return Math.Max(
|
||||
MinimumSpeed,
|
||||
CruiseSpeed * ratio);
|
||||
}
|
||||
|
||||
private void ValidateParameters()
|
||||
{
|
||||
if (CruiseSpeed <= 0f ||
|
||||
!IsFinite(CruiseSpeed) ||
|
||||
LengthMillimeters <= 0f ||
|
||||
!IsFinite(LengthMillimeters) ||
|
||||
RadiusMillimeters <= 0f ||
|
||||
!IsFinite(RadiusMillimeters) ||
|
||||
SCurveLateralOffsetMillimeters <= 0f ||
|
||||
!IsFinite(
|
||||
SCurveLateralOffsetMillimeters) ||
|
||||
ArcSweepRadians <= 0.0 ||
|
||||
!IsFinite(ArcSweepRadians) ||
|
||||
SlowDistanceMillimeters <= 0f ||
|
||||
!IsFinite(SlowDistanceMillimeters) ||
|
||||
FinishDistanceMillimeters < 0f ||
|
||||
!IsFinite(FinishDistanceMillimeters) ||
|
||||
TrackingTimeoutSeconds <= 0f ||
|
||||
!IsFinite(TrackingTimeoutSeconds))
|
||||
throw new ArgumentOutOfRangeException(
|
||||
"蟹行轨迹测试参数无效。");
|
||||
}
|
||||
|
||||
private static double Limit(
|
||||
double value,
|
||||
double absoluteLimit)
|
||||
{
|
||||
return Math.Max(
|
||||
-absoluteLimit,
|
||||
Math.Min(value, absoluteLimit));
|
||||
}
|
||||
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return
|
||||
!double.IsNaN(value) &&
|
||||
!double.IsInfinity(value);
|
||||
}
|
||||
}
|
||||
}
|
||||
+738
-10
@@ -7,6 +7,7 @@ using MDCSToolBox.Commons.Controllers;
|
||||
using MDCSToolBox.Clumsy.Tracks;
|
||||
using MyParking.Shared;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Numerics;
|
||||
using System.Threading;
|
||||
|
||||
@@ -102,7 +103,7 @@ namespace MultiWheelC
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "测试连续前进4m")]
|
||||
[MovementTest(name = "旧版SendMotion：连续前进4m")]
|
||||
public class TestForward4m : MovementTest
|
||||
{
|
||||
public float DistanceMillimeters = 4000f; // 测试距离，单位mm。
|
||||
@@ -138,8 +139,8 @@ namespace MultiWheelC
|
||||
source.Y + DistanceMillimeters * (float)Math.Sin(headingRadians));
|
||||
_recorder =
|
||||
new TrackingExperimentRecorder(
|
||||
controllerName: "Stanley",
|
||||
trajectoryName: "Straight4m",
|
||||
controllerName: "LegacyGeometricController",
|
||||
trajectoryName: "LegacyStraight4m",
|
||||
trialNumber: TrialNumber,
|
||||
referenceStart: source,
|
||||
referenceEnd: destination,
|
||||
@@ -225,8 +226,10 @@ namespace MultiWheelC
|
||||
trialNumber: TrialNumber,
|
||||
referenceStart: rotationCenter,
|
||||
referenceEnd: rotationCenter,
|
||||
referenceSpeed:
|
||||
MaxAngularSpeedDegreesPerSecond);
|
||||
referenceSpeed: 0f,
|
||||
referenceAngularSpeed:
|
||||
MaxAngularSpeedDegreesPerSecond *
|
||||
(float)Math.PI / 180f);
|
||||
_recorder.Start();
|
||||
|
||||
try
|
||||
@@ -258,7 +261,8 @@ namespace MultiWheelC
|
||||
commandAngularSpeed =>
|
||||
_recorder?.UpdateCommand(
|
||||
0f,
|
||||
commandAngularSpeed)
|
||||
commandAngularSpeed *
|
||||
(float)Math.PI / 180f)
|
||||
}.Get());
|
||||
|
||||
_task.Wait();
|
||||
@@ -293,7 +297,7 @@ namespace MultiWheelC
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "测试左转90°圆弧")]
|
||||
[MovementTest(name = "旧版SendMotion：左转90°圆弧")]
|
||||
public class TestArcMovement : MovementTest
|
||||
{
|
||||
public float RadiusMillimeters = 2000f; // 左转圆的半径，单位mm。
|
||||
@@ -370,6 +374,13 @@ namespace MultiWheelC
|
||||
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(
|
||||
@@ -378,12 +389,12 @@ namespace MultiWheelC
|
||||
}
|
||||
|
||||
_recorder = new TrackingExperimentRecorder(
|
||||
controllerName: "GeometricController",
|
||||
controllerName: "LegacyGeometricController",
|
||||
trajectoryName:
|
||||
$"LeftArc90_R{RadiusMillimeters:0}mm",
|
||||
$"LegacyLeftArc90_R{RadiusMillimeters:0}mm",
|
||||
trialNumber: TrialNumber,
|
||||
referenceStart: source,
|
||||
referenceEnd: source,
|
||||
referenceEnd: destination,
|
||||
referenceSpeed: CruiseSpeed);
|
||||
_recorder.Start();
|
||||
|
||||
@@ -414,6 +425,723 @@ namespace MultiWheelC
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "测试蟹行前进4m")]
|
||||
public class TestCrabForward4m : MovementTest
|
||||
{
|
||||
public float DistanceMillimeters = 4000f;
|
||||
public float CruiseSpeed = 0.2f;
|
||||
public int TrialNumber = 1;
|
||||
|
||||
private DriveTask _task;
|
||||
private TrackingExperimentRecorder _recorder;
|
||||
|
||||
// 将车体左侧作为运动前向，沿直线蟹行4m并记录Detour实验数据。
|
||||
public override void Test()
|
||||
{
|
||||
if (!TryReadStartPose(
|
||||
out var source,
|
||||
out var bodyYawRadians))
|
||||
return;
|
||||
|
||||
var motionYaw =
|
||||
bodyYawRadians + Math.PI / 2.0;
|
||||
var destination = new Vector2(
|
||||
source.X +
|
||||
DistanceMillimeters *
|
||||
(float)Math.Cos(motionYaw),
|
||||
source.Y +
|
||||
DistanceMillimeters *
|
||||
(float)Math.Sin(motionYaw));
|
||||
|
||||
var tracker = new CrabMotionFrameTracker
|
||||
{
|
||||
PathKind =
|
||||
CrabMotionFrameTracker
|
||||
.ReferencePathKind.Straight,
|
||||
StartPosition = source,
|
||||
InitialBodyYawRadians =
|
||||
bodyYawRadians,
|
||||
LengthMillimeters =
|
||||
DistanceMillimeters,
|
||||
CruiseSpeed = CruiseSpeed
|
||||
};
|
||||
|
||||
_recorder = new TrackingExperimentRecorder(
|
||||
controllerName:
|
||||
"CrabMotionFrameTracker",
|
||||
trajectoryName:
|
||||
"CrabStraight4m",
|
||||
trialNumber: TrialNumber,
|
||||
referenceStart: source,
|
||||
referenceEnd: destination,
|
||||
referenceSpeed: CruiseSpeed,
|
||||
referenceMotionFrameYawDegrees: 90f);
|
||||
tracker.CommandObserver =
|
||||
(vx, vy, omega) =>
|
||||
_recorder?.UpdateBodyCommand(
|
||||
vx,
|
||||
vy,
|
||||
omega);
|
||||
_recorder.Start();
|
||||
|
||||
try
|
||||
{
|
||||
_task = new DriveTask(tracker.Get());
|
||||
_task.Wait();
|
||||
Thread.Sleep(300);
|
||||
}
|
||||
finally
|
||||
{
|
||||
_task?.Stop();
|
||||
_recorder?.UpdateBodyCommand(
|
||||
0f,
|
||||
0f,
|
||||
0f);
|
||||
_recorder?.StopAndSave();
|
||||
_task = null;
|
||||
_recorder = null;
|
||||
}
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
_task?.Stop();
|
||||
_recorder?.UpdateBodyCommand(
|
||||
0f,
|
||||
0f,
|
||||
0f);
|
||||
_recorder?.StopAndSave();
|
||||
}
|
||||
|
||||
// 读取并校验测试开始时的Detour世界位姿。
|
||||
private static bool TryReadStartPose(
|
||||
out Vector2 source,
|
||||
out double bodyYawRadians)
|
||||
{
|
||||
var location =
|
||||
DetourInterface.getCartLocation();
|
||||
if (double.IsNaN(location.x) ||
|
||||
double.IsInfinity(location.x) ||
|
||||
double.IsNaN(location.y) ||
|
||||
double.IsInfinity(location.y) ||
|
||||
double.IsNaN(location.th) ||
|
||||
double.IsInfinity(location.th))
|
||||
{
|
||||
Console.WriteLine(
|
||||
"Detour当前位姿无效，取消蟹行直线测试。");
|
||||
source = Vector2.Zero;
|
||||
bodyYawRadians = 0.0;
|
||||
return false;
|
||||
}
|
||||
|
||||
source = new Vector2(
|
||||
(float)location.x,
|
||||
(float)location.y);
|
||||
bodyYawRadians =
|
||||
location.th * Math.PI / 180.0;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "测试蟹行左转90°圆弧")]
|
||||
public class TestCrabLeftArc90 : MovementTest
|
||||
{
|
||||
public float RadiusMillimeters = 2000f;
|
||||
public float CruiseSpeed = 0.2f;
|
||||
public int TrialNumber = 1;
|
||||
|
||||
private DriveTask _task;
|
||||
private TrackingExperimentRecorder _recorder;
|
||||
|
||||
// 将车体左侧作为运动前向，沿半径2m的左转圆弧运动90°。
|
||||
public override void Test()
|
||||
{
|
||||
var location =
|
||||
DetourInterface.getCartLocation();
|
||||
if (double.IsNaN(location.x) ||
|
||||
double.IsInfinity(location.x) ||
|
||||
double.IsNaN(location.y) ||
|
||||
double.IsInfinity(location.y) ||
|
||||
double.IsNaN(location.th) ||
|
||||
double.IsInfinity(location.th))
|
||||
{
|
||||
Console.WriteLine(
|
||||
"Detour当前位姿无效，取消蟹行圆弧测试。");
|
||||
return;
|
||||
}
|
||||
|
||||
var source = new Vector2(
|
||||
(float)location.x,
|
||||
(float)location.y);
|
||||
var bodyYawRadians =
|
||||
location.th * Math.PI / 180.0;
|
||||
var tracker = new CrabMotionFrameTracker
|
||||
{
|
||||
PathKind =
|
||||
CrabMotionFrameTracker
|
||||
.ReferencePathKind.LeftArc,
|
||||
StartPosition = source,
|
||||
InitialBodyYawRadians =
|
||||
bodyYawRadians,
|
||||
RadiusMillimeters =
|
||||
RadiusMillimeters,
|
||||
ArcSweepRadians = Math.PI / 2.0,
|
||||
CruiseSpeed = CruiseSpeed
|
||||
};
|
||||
var destination =
|
||||
tracker.GetArcDestination();
|
||||
|
||||
_recorder = new TrackingExperimentRecorder(
|
||||
controllerName:
|
||||
"CrabMotionFrameTracker",
|
||||
trajectoryName:
|
||||
$"CrabLeftArc90_R{RadiusMillimeters:0}mm",
|
||||
trialNumber: TrialNumber,
|
||||
referenceStart: source,
|
||||
referenceEnd: destination,
|
||||
referenceSpeed: CruiseSpeed,
|
||||
referenceMotionFrameYawDegrees: 90f);
|
||||
tracker.CommandObserver =
|
||||
(vx, vy, omega) =>
|
||||
_recorder?.UpdateBodyCommand(
|
||||
vx,
|
||||
vy,
|
||||
omega);
|
||||
_recorder.Start();
|
||||
|
||||
try
|
||||
{
|
||||
_task = new DriveTask(tracker.Get());
|
||||
_task.Wait();
|
||||
Thread.Sleep(300);
|
||||
}
|
||||
finally
|
||||
{
|
||||
_task?.Stop();
|
||||
_recorder?.UpdateBodyCommand(
|
||||
0f,
|
||||
0f,
|
||||
0f);
|
||||
_recorder?.StopAndSave();
|
||||
_task = null;
|
||||
_recorder = null;
|
||||
}
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
_task?.Stop();
|
||||
_recorder?.UpdateBodyCommand(
|
||||
0f,
|
||||
0f,
|
||||
0f);
|
||||
_recorder?.StopAndSave();
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "测试蟹行4m S型曲线")]
|
||||
public class TestCrabSCurve4m : MovementTest
|
||||
{
|
||||
public float LengthMillimeters = 4000f;
|
||||
public float LateralOffsetMillimeters = 400f;
|
||||
public float CruiseSpeed = 0.2f;
|
||||
public int TrialNumber = 1;
|
||||
|
||||
private DriveTask _task;
|
||||
private TrackingExperimentRecorder _recorder;
|
||||
|
||||
// 将车体左侧作为运动前向，跟踪与普通测试参数一致的4m S型曲线。
|
||||
public override void Test()
|
||||
{
|
||||
if (float.IsNaN(LengthMillimeters) ||
|
||||
float.IsInfinity(LengthMillimeters) ||
|
||||
LengthMillimeters <= 0f ||
|
||||
float.IsNaN(
|
||||
LateralOffsetMillimeters) ||
|
||||
float.IsInfinity(
|
||||
LateralOffsetMillimeters) ||
|
||||
LateralOffsetMillimeters <= 0f ||
|
||||
float.IsNaN(CruiseSpeed) ||
|
||||
float.IsInfinity(CruiseSpeed) ||
|
||||
CruiseSpeed <= 0f)
|
||||
{
|
||||
Console.WriteLine(
|
||||
"蟹行S型曲线测试参数无效。");
|
||||
return;
|
||||
}
|
||||
|
||||
var location =
|
||||
DetourInterface.getCartLocation();
|
||||
if (double.IsNaN(location.x) ||
|
||||
double.IsInfinity(location.x) ||
|
||||
double.IsNaN(location.y) ||
|
||||
double.IsInfinity(location.y) ||
|
||||
double.IsNaN(location.th) ||
|
||||
double.IsInfinity(location.th))
|
||||
{
|
||||
Console.WriteLine(
|
||||
"Detour当前位姿无效，取消蟹行S型曲线测试。");
|
||||
return;
|
||||
}
|
||||
|
||||
var source = new Vector2(
|
||||
(float)location.x,
|
||||
(float)location.y);
|
||||
var bodyYawRadians =
|
||||
location.th * Math.PI / 180.0;
|
||||
var initialMotionYaw =
|
||||
bodyYawRadians + Math.PI / 2.0;
|
||||
var destination = new Vector2(
|
||||
source.X +
|
||||
LengthMillimeters *
|
||||
(float)Math.Cos(
|
||||
initialMotionYaw),
|
||||
source.Y +
|
||||
LengthMillimeters *
|
||||
(float)Math.Sin(
|
||||
initialMotionYaw));
|
||||
var tracker = new CrabMotionFrameTracker
|
||||
{
|
||||
PathKind =
|
||||
CrabMotionFrameTracker
|
||||
.ReferencePathKind.SCurve,
|
||||
StartPosition = source,
|
||||
InitialBodyYawRadians =
|
||||
bodyYawRadians,
|
||||
LengthMillimeters =
|
||||
LengthMillimeters,
|
||||
SCurveLateralOffsetMillimeters =
|
||||
LateralOffsetMillimeters,
|
||||
CruiseSpeed = CruiseSpeed
|
||||
};
|
||||
|
||||
_recorder = new TrackingExperimentRecorder(
|
||||
controllerName:
|
||||
"CrabMotionFrameTracker",
|
||||
trajectoryName:
|
||||
$"CrabSCurve4m_A{LateralOffsetMillimeters:0}mm",
|
||||
trialNumber: TrialNumber,
|
||||
referenceStart: source,
|
||||
referenceEnd: destination,
|
||||
referenceSpeed: CruiseSpeed,
|
||||
referenceMotionFrameYawDegrees: 90f);
|
||||
tracker.CommandObserver =
|
||||
(vx, vy, omega) =>
|
||||
_recorder?.UpdateBodyCommand(
|
||||
vx,
|
||||
vy,
|
||||
omega);
|
||||
_recorder.Start();
|
||||
|
||||
try
|
||||
{
|
||||
_task = new DriveTask(tracker.Get());
|
||||
_task.Wait();
|
||||
Thread.Sleep(300);
|
||||
}
|
||||
finally
|
||||
{
|
||||
_task?.Stop();
|
||||
_recorder?.UpdateBodyCommand(
|
||||
0f,
|
||||
0f,
|
||||
0f);
|
||||
_recorder?.StopAndSave();
|
||||
_task = null;
|
||||
_recorder = null;
|
||||
}
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
_task?.Stop();
|
||||
_recorder?.UpdateBodyCommand(
|
||||
0f,
|
||||
0f,
|
||||
0f);
|
||||
_recorder?.StopAndSave();
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "旧版SendMotion：4m S型曲线")]
|
||||
public class TestSCurve4m : MovementTest
|
||||
{
|
||||
public float LengthMillimeters = 4000f; // S型曲线纵向长度，单位mm。
|
||||
public float LateralOffsetMillimeters = 400f; // S型曲线左右两侧的最大偏移，单位mm。
|
||||
public float CruiseSpeed = 0.3f; // 首次实车测试建议使用0.3m/s。
|
||||
public int TrialNumber = 1; // 重复实验编号。
|
||||
|
||||
private DriveTask _task;
|
||||
private TrackingExperimentRecorder _recorder;
|
||||
|
||||
// 从当前Detour位姿开始，沿车头方向跟踪先左偏、再右偏并最终回中的完整S型曲线。
|
||||
public override void Test()
|
||||
{
|
||||
if (float.IsNaN(LengthMillimeters) ||
|
||||
float.IsInfinity(LengthMillimeters) ||
|
||||
LengthMillimeters <= 0f ||
|
||||
float.IsNaN(LateralOffsetMillimeters) ||
|
||||
float.IsInfinity(LateralOffsetMillimeters) ||
|
||||
LateralOffsetMillimeters <= 0f ||
|
||||
float.IsNaN(CruiseSpeed) ||
|
||||
float.IsInfinity(CruiseSpeed) ||
|
||||
CruiseSpeed <= 0f)
|
||||
{
|
||||
Console.WriteLine("S型曲线测试参数无效。");
|
||||
return;
|
||||
}
|
||||
|
||||
if (!MovementTestPreparation.AreWheelsForward())
|
||||
return;
|
||||
|
||||
var location = DetourInterface.getCartLocation();
|
||||
if (double.IsNaN(location.x) ||
|
||||
double.IsInfinity(location.x) ||
|
||||
double.IsNaN(location.y) ||
|
||||
double.IsInfinity(location.y) ||
|
||||
double.IsNaN(location.th) ||
|
||||
double.IsInfinity(location.th))
|
||||
{
|
||||
Console.WriteLine(
|
||||
"Detour当前位姿无效，取消4m S型曲线测试。");
|
||||
return;
|
||||
}
|
||||
|
||||
var source =
|
||||
new Vector2((float)location.x, (float)location.y);
|
||||
var headingRadians =
|
||||
location.th * Math.PI / 180.0;
|
||||
var length = LengthMillimeters;
|
||||
var offset = LateralOffsetMillimeters;
|
||||
|
||||
// 三段三次贝塞尔依次经过左侧峰值、中心线和右侧峰值，
|
||||
// 起点、两个峰值和终点的切线均沿初始前向，连接处没有折角。
|
||||
var firstControlPoints = new List<Vector2>
|
||||
{
|
||||
LocalToWorld(source, headingRadians, 0f, 0f),
|
||||
LocalToWorld(
|
||||
source, headingRadians,
|
||||
length / 12f, 0f),
|
||||
LocalToWorld(
|
||||
source, headingRadians,
|
||||
length / 6f, offset),
|
||||
LocalToWorld(
|
||||
source, headingRadians,
|
||||
length * 0.25f, offset)
|
||||
};
|
||||
var secondControlPoints = new List<Vector2>
|
||||
{
|
||||
LocalToWorld(
|
||||
source, headingRadians,
|
||||
length * 0.25f, offset),
|
||||
LocalToWorld(
|
||||
source, headingRadians,
|
||||
length / 3f, offset),
|
||||
LocalToWorld(
|
||||
source, headingRadians,
|
||||
length * 2f / 3f, -offset),
|
||||
LocalToWorld(
|
||||
source, headingRadians,
|
||||
length * 0.75f, -offset)
|
||||
};
|
||||
var thirdControlPoints = new List<Vector2>
|
||||
{
|
||||
LocalToWorld(
|
||||
source, headingRadians,
|
||||
length * 0.75f, -offset),
|
||||
LocalToWorld(
|
||||
source, headingRadians,
|
||||
length * 5f / 6f, -offset),
|
||||
LocalToWorld(
|
||||
source, headingRadians,
|
||||
length * 11f / 12f, 0f),
|
||||
LocalToWorld(
|
||||
source, headingRadians,
|
||||
length, 0f)
|
||||
};
|
||||
|
||||
var firstTrack = new BezierTrack(firstControlPoints)
|
||||
{
|
||||
Speed = CruiseSpeed,
|
||||
CarDirectionBias = 0f
|
||||
};
|
||||
var secondTrack = new BezierTrack(secondControlPoints)
|
||||
{
|
||||
Speed = CruiseSpeed,
|
||||
CarDirectionBias = 0f
|
||||
};
|
||||
var thirdTrack = new BezierTrack(thirdControlPoints)
|
||||
{
|
||||
Speed = CruiseSpeed,
|
||||
CarDirectionBias = 0f
|
||||
};
|
||||
|
||||
var controller = new ChassisController
|
||||
{
|
||||
BaseSpeed = CruiseSpeed
|
||||
}.Get();
|
||||
controller.FinishSpeed = 0f;
|
||||
|
||||
if (!controller.AddTrack(
|
||||
firstTrack,
|
||||
"SCurve4m-Part1") ||
|
||||
!controller.AddTrack(
|
||||
secondTrack,
|
||||
"SCurve4m-Part2") ||
|
||||
!controller.AddTrack(
|
||||
thirdTrack,
|
||||
"SCurve4m-Part3"))
|
||||
{
|
||||
Console.WriteLine(
|
||||
"4m S型曲线轨迹添加失败，取消测试。");
|
||||
return;
|
||||
}
|
||||
|
||||
var destination =
|
||||
LocalToWorld(
|
||||
source,
|
||||
headingRadians,
|
||||
length,
|
||||
0f);
|
||||
_recorder = new TrackingExperimentRecorder(
|
||||
controllerName: "LegacyGeometricController",
|
||||
trajectoryName:
|
||||
$"LegacySCurve4m_A{LateralOffsetMillimeters:0}mm",
|
||||
trialNumber: TrialNumber,
|
||||
referenceStart: source,
|
||||
referenceEnd: destination,
|
||||
referenceSpeed: CruiseSpeed);
|
||||
_recorder.Start();
|
||||
|
||||
try
|
||||
{
|
||||
_task = new DriveTask(controller.Track());
|
||||
_task.Wait();
|
||||
|
||||
// 保留少量停止后的数据，用于观察速度是否回到零。
|
||||
Thread.Sleep(300);
|
||||
}
|
||||
finally
|
||||
{
|
||||
_task?.Stop();
|
||||
_recorder?.UpdateCommand(0f, 0f);
|
||||
_recorder?.StopAndSave();
|
||||
_task = null;
|
||||
_recorder = null;
|
||||
}
|
||||
}
|
||||
|
||||
// 停止S型曲线测试并保存当前已经采集的数据。
|
||||
public override void TestStop()
|
||||
{
|
||||
_task?.Stop();
|
||||
_recorder?.UpdateCommand(0f, 0f);
|
||||
_recorder?.StopAndSave();
|
||||
}
|
||||
|
||||
// 将车体起点局部坐标转换为Detour世界坐标，X向前、Y向左。
|
||||
private static Vector2 LocalToWorld(
|
||||
Vector2 origin,
|
||||
double headingRadians,
|
||||
float localX,
|
||||
float localY)
|
||||
{
|
||||
var cos = (float)Math.Cos(headingRadians);
|
||||
var sin = (float)Math.Sin(headingRadians);
|
||||
|
||||
return new Vector2(
|
||||
origin.X + localX * cos - localY * sin,
|
||||
origin.Y + localX * sin + localY * cos);
|
||||
}
|
||||
}
|
||||
|
||||
// C层单车测试：统一使用车体速度命令和SendXYThSpeed跟踪普通模式轨迹。
|
||||
public abstract class XYThNormalTrajectoryTestBase : MovementTest
|
||||
{
|
||||
public float LengthMillimeters = 4000f;
|
||||
public float RadiusMillimeters = 2000f;
|
||||
public float LateralOffsetMillimeters = 400f;
|
||||
public float CruiseSpeed = 0.3f;
|
||||
public int TrialNumber = 1;
|
||||
|
||||
private DriveTask _task;
|
||||
private TrackingExperimentRecorder _recorder;
|
||||
|
||||
protected abstract CrabMotionFrameTracker.ReferencePathKind ReferencePath { get; }
|
||||
|
||||
protected abstract string TrajectoryName { get; }
|
||||
|
||||
// C层单车测试：读取Detour起点并执行普通模式SendXYThSpeed轨迹。
|
||||
public override void Test()
|
||||
{
|
||||
ValidateParameters();
|
||||
|
||||
if (!TryReadDetourPose(
|
||||
out var source,
|
||||
out var initialBodyYawRadians))
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"Detour当前位置或航向无效，无法开始新版SendXYThSpeed测试。");
|
||||
}
|
||||
|
||||
var tracker = new CrabMotionFrameTracker
|
||||
{
|
||||
PathKind = ReferencePath,
|
||||
// 普通模式的运动坐标系与车体坐标系重合。
|
||||
MotionFrameYawInBodyRadians = 0.0,
|
||||
StartPosition = source,
|
||||
InitialBodyYawRadians = initialBodyYawRadians,
|
||||
LengthMillimeters = LengthMillimeters,
|
||||
RadiusMillimeters = RadiusMillimeters,
|
||||
ArcSweepRadians = Math.PI / 2.0,
|
||||
SCurveLateralOffsetMillimeters =
|
||||
LateralOffsetMillimeters,
|
||||
CruiseSpeed = CruiseSpeed,
|
||||
};
|
||||
|
||||
var destination = ReferencePath ==
|
||||
CrabMotionFrameTracker.ReferencePathKind
|
||||
.LeftArc
|
||||
? tracker.GetArcDestination()
|
||||
: new Vector2(
|
||||
source.X +
|
||||
LengthMillimeters *
|
||||
(float)Math.Cos(initialBodyYawRadians),
|
||||
source.Y +
|
||||
LengthMillimeters *
|
||||
(float)Math.Sin(initialBodyYawRadians));
|
||||
|
||||
_recorder = new TrackingExperimentRecorder(
|
||||
controllerName: "UnifiedXYThTracker",
|
||||
trajectoryName: TrajectoryName,
|
||||
trialNumber: TrialNumber,
|
||||
referenceStart: source,
|
||||
referenceEnd: destination,
|
||||
referenceSpeed: CruiseSpeed);
|
||||
|
||||
tracker.CommandObserver =
|
||||
(vx, vy, omegaRadiansPerSecond) =>
|
||||
_recorder?.UpdateBodyCommand(
|
||||
vx,
|
||||
vy,
|
||||
omegaRadiansPerSecond);
|
||||
|
||||
_recorder.Start();
|
||||
_task = new DriveTask(tracker.Get());
|
||||
|
||||
try
|
||||
{
|
||||
_task.Wait();
|
||||
}
|
||||
finally
|
||||
{
|
||||
_recorder?.UpdateBodyCommand(0f, 0f, 0f);
|
||||
_recorder?.StopAndSave();
|
||||
_recorder = null;
|
||||
_task = null;
|
||||
}
|
||||
}
|
||||
|
||||
// C层单车测试：停止新版SendXYThSpeed轨迹并保存已有记录。
|
||||
public override void TestStop()
|
||||
{
|
||||
_task?.Stop();
|
||||
_recorder?.UpdateBodyCommand(0f, 0f, 0f);
|
||||
_recorder?.StopAndSave();
|
||||
}
|
||||
|
||||
// C层单车测试：检查新版轨迹的长度、半径、偏移和速度参数。
|
||||
private void ValidateParameters()
|
||||
{
|
||||
if (!IsPositiveFinite(LengthMillimeters))
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(LengthMillimeters),
|
||||
"轨迹长度必须是正有限值。");
|
||||
|
||||
if (!IsPositiveFinite(RadiusMillimeters))
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(RadiusMillimeters),
|
||||
"圆弧半径必须是正有限值。");
|
||||
|
||||
if (!IsPositiveFinite(LateralOffsetMillimeters))
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(LateralOffsetMillimeters),
|
||||
"S型曲线横向偏移必须是正有限值。");
|
||||
|
||||
if (!IsPositiveFinite(CruiseSpeed))
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(CruiseSpeed),
|
||||
"巡航速度必须是正有限值。");
|
||||
}
|
||||
|
||||
// C层单车测试：读取并验证Detour毫米坐标和角度制航向。
|
||||
private static bool TryReadDetourPose(
|
||||
out Vector2 position,
|
||||
out double yawRadians)
|
||||
{
|
||||
var location = DetourInterface.getCartLocation();
|
||||
var x = location.x;
|
||||
var y = location.y;
|
||||
var thetaDegrees = location.th;
|
||||
|
||||
position = new Vector2((float)x, (float)y);
|
||||
yawRadians = thetaDegrees * Math.PI / 180.0;
|
||||
|
||||
return IsFinite(x) &&
|
||||
IsFinite(y) &&
|
||||
IsFinite(thetaDegrees);
|
||||
}
|
||||
|
||||
// C层单车测试：判断浮点参数是否为有限值。
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) &&
|
||||
!double.IsInfinity(value);
|
||||
}
|
||||
|
||||
// C层单车测试：判断浮点参数是否为正有限值。
|
||||
private static bool IsPositiveFinite(double value)
|
||||
{
|
||||
return IsFinite(value) && value > 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "新版SendXYThSpeed：连续前进4m")]
|
||||
public sealed class TestXYThForward4m :
|
||||
XYThNormalTrajectoryTestBase
|
||||
{
|
||||
protected override CrabMotionFrameTracker.ReferencePathKind
|
||||
ReferencePath =>
|
||||
CrabMotionFrameTracker.ReferencePathKind.Straight;
|
||||
|
||||
protected override string TrajectoryName =>
|
||||
"XYThStraight4m";
|
||||
}
|
||||
|
||||
[MovementTest(name = "新版SendXYThSpeed：左转90°圆弧")]
|
||||
public sealed class TestXYThLeftArc90 :
|
||||
XYThNormalTrajectoryTestBase
|
||||
{
|
||||
protected override CrabMotionFrameTracker.ReferencePathKind
|
||||
ReferencePath =>
|
||||
CrabMotionFrameTracker.ReferencePathKind.LeftArc;
|
||||
|
||||
protected override string TrajectoryName =>
|
||||
$"XYThLeftArc90_R{RadiusMillimeters:0}mm";
|
||||
}
|
||||
|
||||
[MovementTest(name = "新版SendXYThSpeed：4m S型曲线")]
|
||||
public sealed class TestXYThSCurve4m :
|
||||
XYThNormalTrajectoryTestBase
|
||||
{
|
||||
protected override CrabMotionFrameTracker.ReferencePathKind
|
||||
ReferencePath =>
|
||||
CrabMotionFrameTracker.ReferencePathKind.SCurve;
|
||||
|
||||
protected override string TrajectoryName =>
|
||||
$"XYThSCurve4m_A{LateralOffsetMillimeters:0}mm";
|
||||
}
|
||||
|
||||
public abstract class ClampMovementTestBase : MovementTest
|
||||
{
|
||||
public float TimeoutSeconds = 30f; // 动作超时时间，单位s。
|
||||
|
||||
@@ -251,7 +251,7 @@ namespace MultiWheelC
|
||||
finally
|
||||
{
|
||||
task?.Stop();
|
||||
chassis.SendXYThSpeed(0f, 0f, 0f);
|
||||
chassis.PredefinedDriveStop();
|
||||
}
|
||||
}
|
||||
|
||||
@@ -458,7 +458,12 @@ namespace MultiWheelC
|
||||
var s = thPid.GetResponse(targetAngle, true);
|
||||
Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}");
|
||||
CommandAngularSpeedObserver?.Invoke(s);
|
||||
Chassis.SendXYThSpeed(0, 0, s);
|
||||
if (!Chassis.SendRotateMotion(s))
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"原地旋转底盘解算失败：" +
|
||||
Chassis.LastMotionDecomposeFailureReason);
|
||||
}
|
||||
if (thPid.IsArrived()) break;
|
||||
yield return true;
|
||||
}
|
||||
@@ -468,7 +473,7 @@ namespace MultiWheelC
|
||||
finally
|
||||
{
|
||||
CommandAngularSpeedObserver?.Invoke(0f);
|
||||
Chassis.SendXYThSpeed(0, 0, 0);
|
||||
Chassis.PredefinedDriveStop();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -39,9 +39,11 @@ public class PilotConfig : MultiWheelPilotConfig
|
||||
#region 单车-临时
|
||||
[FieldMember(desc = "原地旋转Kp")]
|
||||
public float InPlaceRotateKp = 0.2f;
|
||||
// public float InPlaceRotateKp = 0.2f;
|
||||
|
||||
[FieldMember(desc = "原地旋转Ki")]
|
||||
public float InPlaceRotateKi = 0.01f;
|
||||
// public float InPlaceRotateKi = 0.01f;
|
||||
|
||||
[FieldMember(desc = "原地旋转Kd")]
|
||||
public float InPlaceRotateKd = 0f;
|
||||
|
||||
@@ -20,7 +20,7 @@ namespace MultiWheelC
|
||||
public double DetourY;
|
||||
public double DetourTheta;
|
||||
|
||||
// 车体速度单位为m/s，角速度单位为deg/s。
|
||||
// 车体速度单位为m/s，角速度统一使用rad/s。
|
||||
public float CommandSpeed;
|
||||
public float CommandVx;
|
||||
public float CommandVy;
|
||||
@@ -36,6 +36,8 @@ namespace MultiWheelC
|
||||
private readonly Vector2 _referenceStart;
|
||||
private readonly Vector2 _referenceEnd;
|
||||
private readonly float _referenceSpeed;
|
||||
private readonly float _referenceAngularSpeed;
|
||||
private readonly float _referenceMotionFrameYawDegrees;
|
||||
private readonly int _sampleIntervalMs;
|
||||
|
||||
private readonly List<TrackingSample> _samples =
|
||||
@@ -68,7 +70,9 @@ namespace MultiWheelC
|
||||
Vector2 referenceStart,
|
||||
Vector2 referenceEnd,
|
||||
float referenceSpeed,
|
||||
int sampleIntervalMs = 50)
|
||||
float referenceAngularSpeed = 0f,
|
||||
int sampleIntervalMs = 50,
|
||||
float referenceMotionFrameYawDegrees = 0f)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(controllerName))
|
||||
throw new ArgumentException(
|
||||
@@ -91,6 +95,9 @@ namespace MultiWheelC
|
||||
_referenceStart = referenceStart;
|
||||
_referenceEnd = referenceEnd;
|
||||
_referenceSpeed = referenceSpeed;
|
||||
_referenceAngularSpeed = referenceAngularSpeed;
|
||||
_referenceMotionFrameYawDegrees =
|
||||
referenceMotionFrameYawDegrees;
|
||||
_sampleIntervalMs = sampleIntervalMs;
|
||||
}
|
||||
|
||||
@@ -238,7 +245,11 @@ namespace MultiWheelC
|
||||
|
||||
commandVx = command.Vx;
|
||||
commandVy = command.Vy;
|
||||
commandAngularSpeed = command.Vw;
|
||||
// CommonUsage.GetCarSpeed().Vw的单位为deg/s，
|
||||
// 记录器内部统一转换为rad/s。
|
||||
commandAngularSpeed =
|
||||
command.Vw *
|
||||
(float)Math.PI / 180f;
|
||||
commandSpeed = (float)Math.Sqrt(
|
||||
commandVx * commandVx +
|
||||
commandVy * commandVy);
|
||||
@@ -313,14 +324,18 @@ namespace MultiWheelC
|
||||
"DetourY," +
|
||||
"DetourTheta," +
|
||||
"CommandSpeed," +
|
||||
// 保留旧列（deg/s）供历史Python脚本兼容。
|
||||
"CommandAngularSpeed," +
|
||||
"CommandAngularSpeedRadPerSecond," +
|
||||
"CommandVx," +
|
||||
"CommandVy," +
|
||||
"ReferenceStartX," +
|
||||
"ReferenceStartY," +
|
||||
"ReferenceEndX," +
|
||||
"ReferenceEndY," +
|
||||
"ReferenceSpeed");
|
||||
"ReferenceSpeed," +
|
||||
"ReferenceAngularSpeedRadPerSecond," +
|
||||
"ReferenceMotionFrameYawDegrees");
|
||||
|
||||
foreach (var sample in snapshot)
|
||||
{
|
||||
@@ -335,6 +350,9 @@ namespace MultiWheelC
|
||||
Format(sample.DetourY),
|
||||
Format(sample.DetourTheta),
|
||||
Format(sample.CommandSpeed),
|
||||
Format(
|
||||
sample.CommandAngularSpeed *
|
||||
180.0 / Math.PI),
|
||||
Format(sample.CommandAngularSpeed),
|
||||
Format(sample.CommandVx),
|
||||
Format(sample.CommandVy),
|
||||
@@ -342,7 +360,9 @@ namespace MultiWheelC
|
||||
Format(_referenceStart.Y),
|
||||
Format(_referenceEnd.X),
|
||||
Format(_referenceEnd.Y),
|
||||
Format(_referenceSpeed)));
|
||||
Format(_referenceSpeed),
|
||||
Format(_referenceAngularSpeed),
|
||||
Format(_referenceMotionFrameYawDegrees)));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,9 +1,10 @@
|
||||
//------------------------------------------------------------------------------
|
||||
// <auto-generated>
|
||||
// This code was generated by a tool.
|
||||
// 此代码由工具生成。
|
||||
// 运行时版本:4.0.30319.42000
|
||||
//
|
||||
// Changes to this file may cause incorrect behavior and will be lost if
|
||||
// the code is regenerated.
|
||||
// 对此文件的更改可能会导致不正确的行为，并且如果
|
||||
// 重新生成代码，这些更改将会丢失。
|
||||
// </auto-generated>
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
|
||||
Binary file not shown.
@@ -1 +1 @@
|
||||
e972a413d047c4137a8ce86cbff54a8d2e2558806d9d974d3d6312467ee8ba4d
|
||||
1093d2ea159af831cb6cf39a28abbec1d032f5760b7f90d76a2cda9dbd80e1a4
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Reference in New Issue
Block a user