Initial commit from MyParking project
This commit is contained in:
@@ -0,0 +1,21 @@
|
||||
using ClumsyCore;
|
||||
using MDCSToolBox.Clumsy.AgvInterfaces;
|
||||
using MDCSToolBox.Clumsy.MotionControllers;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
public class AGV : MultiWheelInterface
|
||||
{
|
||||
public override AbstractGeometricController GetController()
|
||||
=> new ChassisController().Get();
|
||||
public override MultiWheelMagTracker GetMagController()
|
||||
=> new MultiWheelMagTracker();
|
||||
public override NaiveMagnetController GetNaiveMagnetController()
|
||||
=> new NaiveMagnetController();
|
||||
|
||||
public void Sleep(float seconds)
|
||||
{
|
||||
new DriveTask(new Sleep { Second = seconds }.Get()).Wait();
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,45 @@
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.Pilot;
|
||||
using MDCSToolBox.Clumsy.MotionControllers;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
public class ChassisController : MovementDefinition<MultiWheelGeometricController>
|
||||
{
|
||||
public float BaseSpeed = Configuration.conf.basicSpeed;
|
||||
|
||||
// 创建单车几何跟踪控制器（直接控本车底盘，不走多车 Auto 通道）
|
||||
public override MultiWheelGeometricController Get()
|
||||
{
|
||||
return new MultiWheelGeometricController
|
||||
{
|
||||
Chassis = BasicPilotBase.Chassis,
|
||||
BaseSpeed = BaseSpeed,
|
||||
SlowDistance = PilotDefinition.Conf.SlowDistance,
|
||||
SlowingPow = PilotDefinition.Conf.SlowingPow,
|
||||
FinishDistance = PilotDefinition.Conf.FinishDistance,
|
||||
FinishSpeed = PilotDefinition.Conf.FinishSpeed,
|
||||
FirstThAccuracy = PilotDefinition.Conf.FirstThAccuracy,
|
||||
FirstRotateSpeedFac = PilotDefinition.Conf.FirstRotateSpeedFac,
|
||||
FirstRotateMaxSpeed = PilotDefinition.Conf.FirstRotateMaxSpeed,
|
||||
NotContinuousAngle = PilotDefinition.Conf.NotContinuousAngle,
|
||||
DebugMode = PilotDefinition.Conf.MotionDebugPrint,
|
||||
DebugCurvature = PilotDefinition.Conf.DebugCurvature,
|
||||
PowerSteeringLookAhead = PilotDefinition.Conf.PowerSteeringLookAhead,
|
||||
SpeedLookAhead = PilotDefinition.Conf.SpeedLookAhead,
|
||||
SpeedLookAheadCurveDiff = PilotDefinition.Conf.SpeedLookAheadCurveDiff,
|
||||
SpeedLookBackCurveDiff = PilotDefinition.Conf.SpeedLookBackCurveDiff,
|
||||
SpeedLimitCurveDiffMin = PilotDefinition.Conf.SpeedLimitCurveDiffMin,
|
||||
SpeedLimitCurveMin = PilotDefinition.Conf.SpeedLimitCurveMin,
|
||||
MaxRotateSpeed = PilotDefinition.Conf.MaxRotateSpeedCurveLimit,
|
||||
MaxRotateAcc = PilotDefinition.Conf.MaxRotateAccCurveLimit,
|
||||
GcpThetaThreshold = PilotDefinition.Conf.GcpThetaThreshold,
|
||||
DthLinearFac = PilotDefinition.Conf.DthLinearFac,
|
||||
DthLinearThreshold = PilotDefinition.Conf.DthLinearThreshold,
|
||||
BiasFac = PilotDefinition.Conf.BiasFac,
|
||||
BiasThreshold = PilotDefinition.Conf.BiasThreshold,
|
||||
};
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,817 @@
|
||||
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 enum ChassisCommandBackend
|
||||
{
|
||||
SendXYThSpeed = 0,
|
||||
SendMotion = 1
|
||||
}
|
||||
|
||||
public ReferencePathKind PathKind;
|
||||
public ChassisCommandBackend CommandBackend =
|
||||
ChassisCommandBackend.SendMotion;
|
||||
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 =
|
||||
AngleMath.DegreesToRadians(30.0);
|
||||
public double MaximumVirtualSteeringRadians =
|
||||
AngleMath.DegreesToRadians(30.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);
|
||||
adapter.ResetToBodyFrame();
|
||||
|
||||
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,
|
||||
AngleMath.DegreesToRadians(
|
||||
WheelAlignmentToleranceDegrees));
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
if (CommandBackend ==
|
||||
ChassisCommandBackend.SendMotion)
|
||||
{
|
||||
// 舵轮已按真实机械角度完成预对齐；
|
||||
// 现在由Shared适配层激活SendMotion虚拟运动坐标系。
|
||||
adapter.ActivateMotionFrame(
|
||||
MotionFrameYawInBodyRadians);
|
||||
}
|
||||
|
||||
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 =
|
||||
AngleMath.DegreesToRadians(location.th);
|
||||
|
||||
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 =
|
||||
AngleMath.ShortestDifferenceRadians(
|
||||
desiredBodyYaw,
|
||||
currentBodyYaw);
|
||||
var omega =
|
||||
speed * referenceCurvature +
|
||||
HeadingGainPerSecond * headingError;
|
||||
omega = Limit(
|
||||
omega,
|
||||
MaximumAngularSpeedRadiansPerSecond);
|
||||
|
||||
var now = DateTime.Now;
|
||||
var interval = now - lastCommandTime;
|
||||
lastCommandTime = now;
|
||||
|
||||
bool commandAccepted;
|
||||
Twist2D bodyTwist;
|
||||
if (CommandBackend ==
|
||||
ChassisCommandBackend.SendMotion)
|
||||
{
|
||||
// 运动坐标系相对车体系旋转+90°：
|
||||
// 运动系正向速度会转换成车体系+Y速度。
|
||||
bodyTwist =
|
||||
FrameTransform2D
|
||||
.TransformTwistAtSamePoint(
|
||||
new Pose2D(
|
||||
0.0,
|
||||
0.0,
|
||||
MotionFrameYawInBodyRadians),
|
||||
new Twist2D(
|
||||
vxInMotion,
|
||||
vyInMotion,
|
||||
omega));
|
||||
|
||||
// 将运动坐标系原点和前后几何控制点处的速度，
|
||||
// 转换为SendMotion需要的前后轴方向。
|
||||
var controlPointRadiusMeters =
|
||||
Math.Max(
|
||||
chassis.ControlPointRadius /
|
||||
1000.0,
|
||||
0.001);
|
||||
var frontVelocityY =
|
||||
vyInMotion +
|
||||
omega *
|
||||
controlPointRadiusMeters;
|
||||
var rearVelocityY =
|
||||
vyInMotion -
|
||||
omega *
|
||||
controlPointRadiusMeters;
|
||||
var frontSteeringRadians =
|
||||
Math.Atan2(
|
||||
frontVelocityY,
|
||||
vxInMotion);
|
||||
var rearSteeringRadians =
|
||||
Math.Atan2(
|
||||
rearVelocityY,
|
||||
vxInMotion);
|
||||
|
||||
// 蟹行测试绕过M层ManualControl并直接调用SendMotion，
|
||||
// 因此需要在C层同步应用蟹行虚拟几何比例和转向符号。
|
||||
if (IsCrabMotionFrame())
|
||||
{
|
||||
var geometryRatio =
|
||||
adapter.HalfTrackWidthMeters /
|
||||
adapter.HalfWheelBaseMeters;
|
||||
|
||||
frontSteeringRadians =
|
||||
ConvertToCrabSteering(
|
||||
frontSteeringRadians,
|
||||
geometryRatio);
|
||||
rearSteeringRadians =
|
||||
ConvertToCrabSteering(
|
||||
rearSteeringRadians,
|
||||
geometryRatio);
|
||||
}
|
||||
|
||||
var frontThetaDegrees =
|
||||
(float)AngleMath.RadiansToDegrees(
|
||||
frontSteeringRadians);
|
||||
var rearThetaDegrees =
|
||||
(float)AngleMath.RadiansToDegrees(
|
||||
rearSteeringRadians);
|
||||
var motionSpeed =
|
||||
(float)Math.Sqrt(
|
||||
vxInMotion * vxInMotion +
|
||||
vyInMotion * vyInMotion);
|
||||
|
||||
commandAccepted =
|
||||
chassis.SendMotion(
|
||||
motionSpeed,
|
||||
frontThetaDegrees,
|
||||
rearThetaDegrees,
|
||||
interval);
|
||||
}
|
||||
else if (CommandBackend ==
|
||||
ChassisCommandBackend
|
||||
.SendXYThSpeed)
|
||||
{
|
||||
// 安全XYTh后端根据舵角误差统一压低驱动轮速。
|
||||
bodyTwist =
|
||||
FrameTransform2D
|
||||
.TransformTwistAtSamePoint(
|
||||
new Pose2D(
|
||||
0.0,
|
||||
0.0,
|
||||
MotionFrameYawInBodyRadians),
|
||||
new Twist2D(
|
||||
vxInMotion,
|
||||
vyInMotion,
|
||||
omega));
|
||||
var command = new ChassisCommand(
|
||||
PilotDefinition.Self.CarNum,
|
||||
bodyTwist);
|
||||
commandAccepted =
|
||||
adapter.Send(
|
||||
command,
|
||||
interval);
|
||||
}
|
||||
else
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
$"不支持的底盘命令后端：{CommandBackend}。");
|
||||
}
|
||||
|
||||
if (!commandAccepted)
|
||||
throw new InvalidOperationException(
|
||||
"运动坐标系轨迹底盘解算失败：" +
|
||||
chassis
|
||||
.LastMotionDecomposeFailureReason);
|
||||
|
||||
CommandObserver?.Invoke(
|
||||
(float)bodyTwist.VxMetersPerSecond,
|
||||
(float)bodyTwist.VyMetersPerSecond,
|
||||
(float)bodyTwist
|
||||
.OmegaRadiansPerSecond);
|
||||
|
||||
yield return true;
|
||||
}
|
||||
}
|
||||
finally
|
||||
{
|
||||
adapter.StopImmediately();
|
||||
if (CommandBackend ==
|
||||
ChassisCommandBackend.SendMotion)
|
||||
{
|
||||
// 测试退出后恢复真实车体坐标系，避免影响后续测试。
|
||||
adapter.ResetToBodyFrame();
|
||||
}
|
||||
CommandObserver?.Invoke(0f, 0f, 0f);
|
||||
}
|
||||
|
||||
yield return false;
|
||||
}
|
||||
|
||||
// 判断当前运动坐标系是否为车体左侧朝前的蟹行坐标系。
|
||||
private bool IsCrabMotionFrame()
|
||||
{
|
||||
return Math.Abs(
|
||||
AngleMath.ShortestDifferenceRadians(
|
||||
Math.PI / 2.0,
|
||||
MotionFrameYawInBodyRadians)) <
|
||||
1e-6;
|
||||
}
|
||||
|
||||
// 按车体几何比例缩小蟹行转角。
|
||||
// +90°运动坐标系已经完成方向映射，此处不能再次反号。
|
||||
private double ConvertToCrabSteering(
|
||||
double normalSteeringRadians,
|
||||
double geometryRatio)
|
||||
{
|
||||
var crabSteeringRadians =
|
||||
Math.Atan(
|
||||
geometryRatio *
|
||||
Math.Tan(
|
||||
normalSteeringRadians));
|
||||
|
||||
return Limit(
|
||||
crabSteeringRadians,
|
||||
MaximumVirtualSteeringRadians);
|
||||
}
|
||||
|
||||
// 计算当前点在直线或圆弧上的参考点、切线和剩余距离。
|
||||
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 =
|
||||
AngleMath.NormalizeRadians(
|
||||
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) ||
|
||||
MaximumVirtualSteeringRadians <= 0.0 ||
|
||||
MaximumVirtualSteeringRadians >=
|
||||
Math.PI / 2.0 ||
|
||||
!IsFinite(
|
||||
MaximumVirtualSteeringRadians))
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,949 @@
|
||||
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.Collections.Generic;
|
||||
using System.Numerics;
|
||||
using System.Threading;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
internal static class MovementTestPreparation
|
||||
{
|
||||
// 在测试正式开始前，将四个舵轮稳定回正到车体前向。
|
||||
public static bool AlignWheelsForward(
|
||||
ref DriveTask activeTask)
|
||||
{
|
||||
var preparation = new PrepareWheelsForward();
|
||||
var task = new DriveTask(preparation.Get());
|
||||
activeTask = task;
|
||||
|
||||
try
|
||||
{
|
||||
task.Wait();
|
||||
return preparation.Completed;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Console.WriteLine(
|
||||
$"测试前舵轮回正失败：{ex.Message}");
|
||||
return false;
|
||||
}
|
||||
finally
|
||||
{
|
||||
task.Stop();
|
||||
if (ReferenceEquals(activeTask, task))
|
||||
activeTask = null;
|
||||
}
|
||||
}
|
||||
|
||||
// 只读取实际舵角，检查四个舵轮是否已与车头方向一致。
|
||||
public static bool AreWheelsForward(
|
||||
float toleranceDegrees = 2f)
|
||||
{
|
||||
var chassis =
|
||||
PilotDefinition.Chassis as MultiWheelChassis;
|
||||
if (chassis == null)
|
||||
{
|
||||
Console.WriteLine(
|
||||
"当前底盘不是MultiWheelChassis，无法检查舵轮方向。");
|
||||
return false;
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
var adapter = new MultiWheelChassisAdapter(
|
||||
chassis,
|
||||
PilotDefinition.Self.CarNum);
|
||||
var toleranceRadians =
|
||||
AngleMath.DegreesToRadians(toleranceDegrees);
|
||||
|
||||
if (adapter.AreParallelWheelsAligned(
|
||||
0.0,
|
||||
toleranceRadians))
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
Console.WriteLine(
|
||||
"四个舵轮尚未与车头方向一致，请先执行“准备：四个舵轮与车头方向一致”。");
|
||||
return false;
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
Console.WriteLine(
|
||||
$"检查舵轮方向失败：{ex.Message}");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "准备：四个舵轮与车头方向一致")]
|
||||
public class AlignWheelsForwardTest : MovementTest
|
||||
{
|
||||
private DriveTask _task;
|
||||
|
||||
// 单独将四个舵轮转到车体前向0°并等待实际反馈稳定到位。
|
||||
public override void Test()
|
||||
{
|
||||
MovementTestPreparation.AlignWheelsForward(
|
||||
ref _task);
|
||||
}
|
||||
|
||||
// 停止正在执行的舵轮回正任务并清零底盘运动命令。
|
||||
public override void TestStop()
|
||||
{
|
||||
_task?.Stop();
|
||||
_task = null;
|
||||
}
|
||||
}
|
||||
|
||||
[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();
|
||||
}
|
||||
}
|
||||
|
||||
public abstract class InPlaceRotateTestBase : MovementTest
|
||||
{
|
||||
public float RelativeAngleDegrees; // 相对当前航向的旋转角度，逆时针为正。
|
||||
public float MaxAngularSpeedDegreesPerSecond = 20f; // PID输出的最大角速度。
|
||||
public int TrialNumber = 1; // 重复实验编号。
|
||||
|
||||
private DriveTask _task;
|
||||
private TrackingExperimentRecorder _recorder;
|
||||
private readonly string _trajectoryName;
|
||||
|
||||
protected InPlaceRotateTestBase(
|
||||
float relativeAngleDegrees,
|
||||
string trajectoryName)
|
||||
{
|
||||
RelativeAngleDegrees =
|
||||
relativeAngleDegrees;
|
||||
_trajectoryName =
|
||||
trajectoryName;
|
||||
}
|
||||
|
||||
// 从当前Detour航向开始，原地相对旋转指定角度并记录实验数据。
|
||||
public override void Test()
|
||||
{
|
||||
if (float.IsNaN(RelativeAngleDegrees) ||
|
||||
float.IsInfinity(RelativeAngleDegrees) ||
|
||||
float.IsNaN(MaxAngularSpeedDegreesPerSecond) ||
|
||||
float.IsInfinity(MaxAngularSpeedDegreesPerSecond) ||
|
||||
MaxAngularSpeedDegreesPerSecond <= 0f)
|
||||
{
|
||||
Console.WriteLine("原地旋转测试参数无效。");
|
||||
return;
|
||||
}
|
||||
|
||||
var location = DetourInterface.getCartLocation();
|
||||
if (double.IsNaN(location.x) ||
|
||||
double.IsInfinity(location.x) ||
|
||||
double.IsNaN(location.y) ||
|
||||
double.IsInfinity(location.y) ||
|
||||
double.IsNaN(location.th) ||
|
||||
double.IsInfinity(location.th))
|
||||
{
|
||||
Console.WriteLine(
|
||||
"Detour当前位姿无效，取消原地旋转测试。");
|
||||
return;
|
||||
}
|
||||
|
||||
var rotationCenter =
|
||||
new Vector2((float)location.x, (float)location.y);
|
||||
var targetWorldAngle =
|
||||
(float)AngleMath.NormalizeDegrees(
|
||||
location.th + RelativeAngleDegrees);
|
||||
|
||||
_recorder = new TrackingExperimentRecorder(
|
||||
controllerName: "InPlaceRotatePID",
|
||||
trajectoryName: _trajectoryName,
|
||||
trialNumber: TrialNumber,
|
||||
referenceStart: rotationCenter,
|
||||
referenceEnd: rotationCenter,
|
||||
referenceSpeed: 0f,
|
||||
referenceAngularSpeed:
|
||||
(float)AngleMath.DegreesToRadians(
|
||||
MaxAngularSpeedDegreesPerSecond));
|
||||
_recorder.Start();
|
||||
|
||||
try
|
||||
{
|
||||
_task = new DriveTask(
|
||||
new MultiWheelRotateInPlace
|
||||
{
|
||||
// MultiWheelRotateInPlace接收世界坐标系绝对航向。
|
||||
AngleTarget = targetWorldAngle,
|
||||
PidparamsRead = () => new PIDParams
|
||||
{
|
||||
Kp =
|
||||
PilotDefinition.Conf.InPlaceRotateKp,
|
||||
Ki =
|
||||
PilotDefinition.Conf.InPlaceRotateKi,
|
||||
Kd =
|
||||
PilotDefinition.Conf.InPlaceRotateKd,
|
||||
DeadZone =
|
||||
PilotDefinition.Conf
|
||||
.InPlaceRotateArriveDeg,
|
||||
SpeedAccPerSec =
|
||||
PilotDefinition.Conf.InPlaceRotateAcc,
|
||||
OutputUpperThreshold =
|
||||
MaxAngularSpeedDegreesPerSecond,
|
||||
MaxI =
|
||||
PilotDefinition.Conf.InPlaceRotateMaxI
|
||||
},
|
||||
CommandAngularSpeedObserver =
|
||||
commandAngularSpeed =>
|
||||
_recorder?.UpdateCommand(
|
||||
0f,
|
||||
(float)AngleMath.DegreesToRadians(
|
||||
commandAngularSpeed))
|
||||
}.Get());
|
||||
|
||||
_task.Wait();
|
||||
|
||||
// 保留少量停止后的样本，用于观察角速度是否回到零。
|
||||
Thread.Sleep(300);
|
||||
}
|
||||
finally
|
||||
{
|
||||
_task?.Stop();
|
||||
_recorder?.UpdateCommand(0f, 0f);
|
||||
_recorder?.StopAndSave();
|
||||
_task = null;
|
||||
_recorder = null;
|
||||
}
|
||||
}
|
||||
|
||||
// 停止原地旋转并保存当前已经采集的实验数据。
|
||||
public override void TestStop()
|
||||
{
|
||||
_task?.Stop();
|
||||
_recorder?.UpdateCommand(0f, 0f);
|
||||
_recorder?.StopAndSave();
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
[MovementTest(name = "SendXYThSpeed：原地自转90°")]
|
||||
public sealed class TestRotate90 :
|
||||
InPlaceRotateTestBase
|
||||
{
|
||||
public TestRotate90()
|
||||
: base(90f, "Rotate90")
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "SendXYThSpeed：原地自转180°")]
|
||||
public sealed class TestRotate180 :
|
||||
InPlaceRotateTestBase
|
||||
{
|
||||
public TestRotate180()
|
||||
: base(180f, "Rotate180")
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
[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();
|
||||
}
|
||||
}
|
||||
|
||||
#region 蟹行运动测试
|
||||
[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);
|
||||
}
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region 夹臂运动测试
|
||||
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;
|
||||
}
|
||||
#endregion
|
||||
}
|
||||
@@ -0,0 +1,629 @@
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.DTools;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using CommonUsage.Chassis;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Tracks;
|
||||
using MDCSToolBox.Commons.Controllers;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Numerics;
|
||||
using System.Threading;
|
||||
using FundamentalLib;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
// C层测试准备：停车并等待四个舵轮稳定回到车体前向0°。
|
||||
public class PrepareWheelsForward : MovementDefinition
|
||||
{
|
||||
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 =
|
||||
PilotDefinition.Chassis as MultiWheelChassis;
|
||||
if (chassis == null)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"当前底盘不是MultiWheelChassis，无法执行舵轮回正。");
|
||||
}
|
||||
|
||||
var adapter = new MultiWheelChassisAdapter(
|
||||
chassis,
|
||||
PilotDefinition.Self.CarNum);
|
||||
adapter.ResetToBodyFrame();
|
||||
var toleranceRadians =
|
||||
AngleMath.DegreesToRadians(ToleranceDegrees);
|
||||
var startTime = DateTime.UtcNow;
|
||||
DateTime? alignedSince = null;
|
||||
|
||||
Completed = false;
|
||||
if (!adapter.PrepareParallelDirection(0.0))
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"无法将所有舵轮下发到车体前向0°。");
|
||||
}
|
||||
|
||||
try
|
||||
{
|
||||
while (true)
|
||||
{
|
||||
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
|
||||
{
|
||||
// 只清零驱动速度，保留已经下发的0°舵角。
|
||||
adapter.StopImmediately();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#region 功能项
|
||||
public class Sleep : MovementDefinition
|
||||
{
|
||||
public float Second = 2f;
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
if (Second <= 0)
|
||||
{
|
||||
yield return false;
|
||||
yield break;
|
||||
}
|
||||
|
||||
var endTime = DateTime.UtcNow.AddSeconds(Second);
|
||||
while (DateTime.UtcNow < endTime)
|
||||
{
|
||||
Thread.Sleep(50);
|
||||
yield return true;
|
||||
}
|
||||
|
||||
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.PredefinedDriveStop();
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
//直线行走基于轮里程
|
||||
// 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 =
|
||||
AngleMath.DegreesToRadians(curpose.th);
|
||||
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>
|
||||
/// 旋转目标角度
|
||||
/// </summary>
|
||||
public float AngleTarget;
|
||||
|
||||
public Func<float> ThetaReader = () => (float)DetourInterface.getCartLocation().th;
|
||||
|
||||
public MultiWheelChassis Chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||||
|
||||
public Func<PIDParams> PidparamsRead = () => new PIDParams() { };
|
||||
|
||||
public PIDController thPid;
|
||||
|
||||
// 将本周期PID角速度输出提供给实验记录器，单位deg/s。
|
||||
public Action<float> CommandAngularSpeedObserver;
|
||||
|
||||
// 自转前舵轮实际角度允许误差，单位deg。
|
||||
public float WheelAlignmentToleranceDegrees = 2f;
|
||||
|
||||
// 自转舵轮连续保持到位的时间，单位s。
|
||||
public float WheelAlignmentStableSeconds = 0.3f;
|
||||
|
||||
// 自转舵轮准备超时时间，单位s。
|
||||
public float WheelAlignmentTimeoutSeconds = 10f;
|
||||
|
||||
// 先准备自转舵角，再通过安全版SendXYThSpeed闭环旋转到目标角度。
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
if (Chassis == null)
|
||||
throw new InvalidOperationException(
|
||||
"当前底盘不是MultiWheelChassis，无法执行原地自转。");
|
||||
|
||||
var adapter = new MultiWheelChassisAdapter(
|
||||
Chassis,
|
||||
PilotDefinition.Self.CarNum);
|
||||
adapter.ResetToBodyFrame();
|
||||
|
||||
try
|
||||
{
|
||||
var alignmentStarted = DateTime.Now;
|
||||
DateTime? alignedSince = null;
|
||||
while (true)
|
||||
{
|
||||
if (!adapter.PrepareSpin())
|
||||
throw new InvalidOperationException(
|
||||
"无法生成原地自转舵轮目标：" +
|
||||
adapter.LastFailureReason);
|
||||
|
||||
if (adapter.AreSpinWheelsAligned)
|
||||
{
|
||||
if (alignedSince == null)
|
||||
alignedSince = DateTime.Now;
|
||||
|
||||
if ((DateTime.Now - alignedSince.Value)
|
||||
.TotalSeconds >=
|
||||
WheelAlignmentStableSeconds)
|
||||
break;
|
||||
}
|
||||
else
|
||||
{
|
||||
alignedSince = null;
|
||||
}
|
||||
|
||||
if ((DateTime.Now - alignmentStarted)
|
||||
.TotalSeconds >
|
||||
WheelAlignmentTimeoutSeconds)
|
||||
throw new TimeoutException(
|
||||
"原地自转舵轮在限定时间内未稳定到位。");
|
||||
|
||||
yield return true;
|
||||
}
|
||||
|
||||
var targetAngle =
|
||||
(float)AngleMath.NormalizeDegrees(AngleTarget);
|
||||
var p = PidparamsRead();
|
||||
thPid = new PIDController(ThetaReader, p.Kp);
|
||||
thPid.ChangeParameters(p.Kp, p.Ki, p.Kd, p.MaxI, p.DeadZone,
|
||||
p.OutputUpperThreshold, p.SpeedAccPerSec);
|
||||
var lastCommandTime = DateTime.Now;
|
||||
|
||||
while (true)
|
||||
{
|
||||
var s = thPid.GetResponse(targetAngle, true);
|
||||
Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}");
|
||||
CommandAngularSpeedObserver?.Invoke(s);
|
||||
var now = DateTime.Now;
|
||||
var interval = now - lastCommandTime;
|
||||
lastCommandTime = now;
|
||||
|
||||
// PID输出s为deg/s，Shared命令统一使用rad/s。
|
||||
// adapter.Send最终调用普通安全版SendXYThSpeed。
|
||||
var omegaRadiansPerSecond =
|
||||
(float)AngleMath.DegreesToRadians(s);
|
||||
if (!adapter.Send(
|
||||
new ChassisCommand(
|
||||
PilotDefinition.Self.CarNum,
|
||||
new Twist2D(
|
||||
0.0,
|
||||
0.0,
|
||||
omegaRadiansPerSecond)),
|
||||
interval))
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"安全XYTh原地旋转底盘解算失败：" +
|
||||
adapter.LastFailureReason);
|
||||
}
|
||||
if (thPid.IsArrived()) break;
|
||||
yield return true;
|
||||
}
|
||||
|
||||
Console.WriteLine($"final rotate to {targetAngle}");
|
||||
}
|
||||
finally
|
||||
{
|
||||
CommandAngularSpeedObserver?.Invoke(0f);
|
||||
adapter.StopImmediately();
|
||||
}
|
||||
}
|
||||
}
|
||||
#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
|
||||
}
|
||||
@@ -0,0 +1,43 @@
|
||||
<Project Sdk="Microsoft.NET.Sdk">
|
||||
|
||||
<PropertyGroup>
|
||||
<TargetFramework>netstandard2.0</TargetFramework>
|
||||
<LangVersion>10</LangVersion>
|
||||
<AssemblyName>MultiWheelC</AssemblyName>
|
||||
<RootNamespace>MultiWheelC</RootNamespace>
|
||||
<AppendTargetFrameworkToOutputPath>false</AppendTargetFrameworkToOutputPath>
|
||||
<OutputPath>build\Clumsy\</OutputPath>
|
||||
</PropertyGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<PackageReference Include="Newtonsoft.Json" Version="13.0.3" />
|
||||
<PackageReference Include="System.Numerics.Vectors" Version="4.6.1" />
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<Compile Include="..\Shared\**\*.cs"
|
||||
Link="Shared\%(RecursiveDir)%(Filename)%(Extension)" />
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<Reference Include="LessokajiWeaverUtilities">
|
||||
<HintPath>ref\LessokajiWeaverUtilities.dll</HintPath>
|
||||
</Reference>
|
||||
<Reference Include="MDCSToolBox">
|
||||
<HintPath>ref\MDCSToolBox.dll</HintPath>
|
||||
</Reference>
|
||||
<Reference Include="ClumsyCore">
|
||||
<HintPath>ref\RefClumsyCore.dll</HintPath>
|
||||
</Reference>
|
||||
<Reference Include="ClumsyDance">
|
||||
<HintPath>ref\RefClumsyDance.dll</HintPath>
|
||||
</Reference>
|
||||
<Reference Include="FundamentalLib">
|
||||
<HintPath>ref\RefFundamentalLib.dll</HintPath>
|
||||
</Reference>
|
||||
<Reference Include="CommonUsage">
|
||||
<HintPath>..\ref\CommonUsage.dll</HintPath>
|
||||
</Reference>
|
||||
</ItemGroup>
|
||||
|
||||
</Project>
|
||||
@@ -0,0 +1,335 @@
|
||||
using ClumsyCore;
|
||||
using MDCSToolBox.Clumsy.Pilot.MultiWheel;
|
||||
using Newtonsoft.Json;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
public class PilotConfig : MultiWheelPilotConfig
|
||||
{
|
||||
#region 单车-轨迹跟踪 LineTracking
|
||||
|
||||
[FieldMember(desc = "直线行走距离")] public float LineTrackDistance = 1000f;
|
||||
[FieldMember(desc = "直线行走最大速度")] public float LineTrackMaxSpeed = 0.3f;
|
||||
[FieldMember(desc = "直线行走Kp")] public float LineTrackKp = 0.2f;
|
||||
[FieldMember(desc = "直线行走Ki")] public float LineTrackKi = 0f;
|
||||
[FieldMember(desc = "直线行走Kd")] public float LineTrackKd = 0f;
|
||||
[FieldMember(desc = "直线行走DeadZone")] public float LineTrackDeadZone = 50f;
|
||||
|
||||
[FieldMember(desc = "终点跟踪:速度")] public float DstTrackerMaxSpeed = 0.3f;
|
||||
#endregion
|
||||
|
||||
#region 单车-原地旋转 暂时没用上
|
||||
[FieldMember(desc = "原地旋转：目标朝向(世界坐标系, deg)")]
|
||||
public float InPlaceRotateTargetWorldDeg = 90f;
|
||||
|
||||
[FieldMember(desc = "原地旋转：旋转角速度(deg/s)")]
|
||||
public float InPlaceRotateSpeed = 30f;
|
||||
|
||||
[FieldMember(desc = "原地旋转：到位角度精度(deg)")]
|
||||
public float InPlaceRotateArriveDeg = 1f;
|
||||
|
||||
[FieldMember(desc = "原地旋转：起转前舵轮对齐精度(deg)")]
|
||||
public float InPlaceRotateWheelAlignDeg = 2f;
|
||||
|
||||
[FieldMember(desc = "原地旋转：旋转过程中舵轮偏差重对齐阈值(deg)")]
|
||||
public float InPlaceRotateActiveWheelAlignDeg = 10f;
|
||||
|
||||
#endregion
|
||||
|
||||
#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;
|
||||
|
||||
[FieldMember(desc = "原地旋转积分限幅")]
|
||||
public float InPlaceRotateMaxI = 0.01f;
|
||||
|
||||
[FieldMember(desc = "原地旋转最大角速度(deg/s)")]
|
||||
public float InPlaceRotateMaxSpeed = 30f;
|
||||
|
||||
[FieldMember(desc = "原地旋转角加速度(deg/s²)")]
|
||||
public float InPlaceRotateAcc = 30f;
|
||||
|
||||
[FieldMember(desc = "原地旋转超时(s)")]
|
||||
public float InPlaceRotateTimeoutSec = 15f;
|
||||
#endregion
|
||||
|
||||
|
||||
|
||||
|
||||
#region 单车-钻车与夹抱
|
||||
[FieldMember(desc = "2腿检测：雷达名（逗号分隔可多个）")]
|
||||
public string TwoLegLidarName = "rear_left_lidar_1,rear_right_lidar_1";
|
||||
|
||||
[FieldMember(desc = "2腿检测：初始猜测X(mm, 车体坐标系)")]
|
||||
public float TwoLegGuessX = 2000f;
|
||||
|
||||
[FieldMember(desc = "2腿检测：两腿间距(mm)")]
|
||||
public float TwoLegWidth = 800f;
|
||||
|
||||
[FieldMember(desc = "2腿检测：两腿间距允许误差(mm)")]
|
||||
public float TwoLegWidthErr = 100f;
|
||||
|
||||
[FieldMember(desc = "2腿检测：聚类点间距(mm)")]
|
||||
public float TwoLegBlobDist = 100f;
|
||||
|
||||
[FieldMember(desc = "2腿检测：聚类尺寸(mm)")]
|
||||
public float TwoLegBlobSize = 200f;
|
||||
|
||||
[FieldMember(desc = "2腿检测：聚类最小点数")]
|
||||
public int TwoLegBlobPtCount = 5;
|
||||
|
||||
[FieldMember(desc = "2腿检测：聚类 padding")]
|
||||
public int TwoLegPadding = 5;
|
||||
|
||||
[FieldMember(desc = "2腿检测：腿柱搜索范围")]
|
||||
public int TwoLegPillarFindingScope = 20;
|
||||
|
||||
[FieldMember(desc = "2腿检测：方向符号(±1)")]
|
||||
public int TwoLegSgnDir = 1;
|
||||
|
||||
[FieldMember(desc = "2腿检测：中心X偏移(mm)")]
|
||||
public float TwoLegCenterChangeX = 0f;
|
||||
|
||||
[FieldMember(desc = "2腿检测：输出X补偿(mm)")]
|
||||
public float TwoLegOutputBiasX = 0f;
|
||||
|
||||
[FieldMember(desc = "2腿检测：输出Y补偿(mm)")]
|
||||
public float TwoLegOutputBiasY = 0f;
|
||||
|
||||
[FieldMember(desc = "2腿检测：ROI滤波框长(mm)")]
|
||||
public float TwoLegFilterLength = 1800f;
|
||||
|
||||
[FieldMember(desc = "2腿检测：ROI滤波框宽(mm)")]
|
||||
public float TwoLegFilterWidth = 600f;
|
||||
|
||||
[FieldMember(desc = "轮胎识别:识别框长")] public float TireFilterLength = 1800f;
|
||||
[FieldMember(desc = "轮胎识别:识别框宽")] public float TireFilterWidth = 600f;
|
||||
[FieldMember(desc = "轮胎识别:轮胎间距")] public float TireTwoLegWidth = 800f;
|
||||
[FieldMember(desc = "轮胎识别:轮胎识别允许误差")] public float TireTwoLegWidthErr = 100f;
|
||||
[FieldMember(desc = "轮胎识别:轮胎聚类最小点云数")] public int TireTwoLegBlobPtCount = 15;
|
||||
|
||||
[FieldMember(desc = "轮胎识别:前雷达参数")] public float TireFrontTwoLegBlobDist = 100f;
|
||||
[FieldMember(desc = "轮胎识别:前雷达参数")] public float TireFrontTwoLegBlobSize = 200f;
|
||||
[FieldMember(desc = "轮胎识别:前雷达参数")] public int TireFrontPadding = 5;
|
||||
[FieldMember(desc = "轮胎识别:前雷达参数")] public int TireFrontTwoLegPillarFindingScope = 20;
|
||||
[FieldMember(desc = "轮胎识别:前雷达参数")] public int TireFrontTwoLegSgnDir = 1;
|
||||
[FieldMember(desc = "轮胎识别:前雷达参数")] public float TireFrontTwoLegCenterChangeX = 0;
|
||||
[FieldMember(desc = "轮胎识别:后雷达参数")] public float TireBackTwoLegBlobDist = 100f;
|
||||
[FieldMember(desc = "轮胎识别:后雷达参数")] public float TireBackTwoLegBlobSize = 200f;
|
||||
[FieldMember(desc = "轮胎识别:后雷达参数")] public int TireBackPadding = 5;
|
||||
[FieldMember(desc = "轮胎识别:后雷达参数")] public int TireBackTwoLegPillarFindingScope = 20;
|
||||
[FieldMember(desc = "轮胎识别:后雷达参数")] public int TireBackTwoLegSgnDir = 1;
|
||||
[FieldMember(desc = "轮胎识别:后雷达参数")] public float TireBackTwoLegCenterChangeX = 0;
|
||||
|
||||
[FieldMember(desc = "轮胎跟踪:切换至盲走距离")] public float TireFollowingWalkBlindSwitchingDistance = 1200f;
|
||||
[FieldMember(desc = "轮胎跟踪:识别第一对轮胎的初始距离")] public float TireFollowingStage1GuessX = 2000f;
|
||||
[FieldMember(desc = "轮胎跟踪:识别第二对轮胎的初始距离")] public float TireFollowingStage2GuessX = 2475f;
|
||||
[FieldMember(desc = "轮胎跟踪:盲走停止距离")] public float TireFollowingWalkBlindFinishDistance = 10f;
|
||||
[FieldMember(desc = "轮胎跟踪:减速距离")] public float TireFollowingSlowDistance = 200f;
|
||||
[FieldMember(desc = "轮胎跟踪:最大速度")] public float TireFollowingMaxSpeed = 0.2f;
|
||||
[FieldMember(desc = "轮胎跟踪:前雷达识别路径偏移X")] public float TireFollowingFrontLidarPathTransformationX = 253f;
|
||||
[FieldMember(desc = "轮胎跟踪:前雷达识别路径偏移Y")] public float TireFollowingFrontLidarPathTransformationY = 13f;
|
||||
[FieldMember(desc = "轮胎跟踪:前雷达识别路径偏移Th")] public float TireFollowingFrontLidarWalkBlindTh = -1f;
|
||||
[FieldMember(desc = "轮胎跟踪:后雷达识别路径偏移X")] public float TireFollowingBackLidarPathTransformationX = 148f;
|
||||
[FieldMember(desc = "轮胎跟踪:后雷达识别路径偏移Y")] public float TireFollowingBackLidarPathTransformationY = 2f;
|
||||
[FieldMember(desc = "轮胎跟踪:后雷达识别路径偏移Th")] public float TireFollowingBackLidarWalkBlindTh = 0f;
|
||||
[FieldMember(desc = "轮胎跟踪:离车时后雷达识别路径偏移X")] public float TireFollowingLeaveCarBackLidarPathTransformationX = 1500f;
|
||||
[FieldMember(desc = "轮胎跟踪:离车时切换至盲走距离")] public float TireFollowingLeaveCarWalkBlindSwitchingDistance = 1200f;
|
||||
|
||||
[FieldMember(desc = "轮胎跟踪:测试钻轮胎数量")] public int TireFollowingTireNum = 1;
|
||||
[FieldMember(desc = "轮胎跟踪:过近距离")] public float TireFollowingCloseDistance = 1400;
|
||||
[FieldMember(desc = "轮胎跟踪:距离过近角度忽略阈值")] public float TireFollowingAngleIgnoreThr = 0.2f;
|
||||
[FieldMember(desc = "轮胎跟踪:Y最大平均数")] public int TireFollowingYAverageFrameCount = 5;
|
||||
|
||||
[FieldMember(desc = "轮胎跟踪:释放锁点距离")] public float TireFollowingReleaseDistance = 1600;
|
||||
|
||||
[FieldMember(desc = "轮胎跟踪:角度调整kp")] public float TireFollowingThkp = 0.05f;
|
||||
[FieldMember(desc = "轮胎跟踪:角度调整ki")] public float TireFollowingThki = 0.01f;
|
||||
[FieldMember(desc = "轮胎跟踪:角度调整kd")] public float TireFollowingThkd = 0f;
|
||||
[FieldMember(desc = "轮胎跟踪:角度调整SpeedAcc")] public float TireFollowingThSpeedAccPerSec = 1f;
|
||||
[FieldMember(desc = "轮胎跟踪:角度调整Thresh")] public float TireFollowingThThresh = 0.1f;
|
||||
[FieldMember(desc = "轮胎跟踪:角度调整DeadZone")] public float TireFollowingThDeadZone = 5f;
|
||||
[FieldMember(desc = "轮胎跟踪:角度调整MaxI")] public float TireFollowingThMaxI = 0.01f;
|
||||
|
||||
[FieldMember(desc = "抱夹控制pid:Kp")] public float ClampControlKp = 0.1f;
|
||||
[FieldMember(desc = "抱夹控制pid:Ki")] public float ClampControlKi = 0f;
|
||||
[FieldMember(desc = "抱夹控制pid:Kd")] public float ClampControlKd = 0f;
|
||||
[FieldMember(desc = "抱夹控制pid:MaxI")] public float ClampControlMaxI = 0f;
|
||||
[FieldMember(desc = "抱夹控制pid:Acc")] public float ClampControlSpeedAcc = 1f;
|
||||
[FieldMember(desc = "抱夹控制pid:Thresh")] public float ClampControlThresh = 0.2f;
|
||||
[FieldMember(desc = "抱夹控制pid:DeadZone")] public float ClampControlDeadZone = 5f;
|
||||
[FieldMember(desc = "抱夹最大速度")] public float MaxClampSpeed = 1.5f;
|
||||
#endregion
|
||||
#if false
|
||||
#region 多车-编队与遥控
|
||||
[FieldMember(desc = "联动时转向角爬升加速度")] public float SyncThAccPerSec = 30f;
|
||||
[FieldMember(desc = "两车间距 (mm)")] public float TestCarSyncDistance = 2400f;
|
||||
[FieldMember(desc = "编队排布偏角")] public float TestCarSyncTh = 0f;
|
||||
// Fleet manual remote IO values are normalized joystick ratios. Keep all speed/angle scaling here.
|
||||
[FieldMember(desc = "车队遥控最大线速度")] public float FleetManualMaxSpeed = 0.3f;
|
||||
[FieldMember(desc = "常规模式满杆舵角")] public float FleetManualMaxSteerAngleDeg = 45f;
|
||||
[FieldMember(desc = "蟹行满杆舵角")] public float FleetManualMaxCrabAngleDeg = 60f;
|
||||
[FieldMember(desc = "旋转满杆角速度")] public float FleetManualMaxRotateOmegaDegPerSec = 45f;
|
||||
[FieldMember(desc = "蟹行舵角上限（对齐 ±120）")] public float MultiVehicleCrabSteerLimitDeg = 120f;
|
||||
[FieldMember(desc = "互识别检测中心偏移")] public float DeltaDetectCenter = 350f;
|
||||
#endregion
|
||||
|
||||
#region 多车-通信
|
||||
[FieldMember(desc = "多车联动：总车数")] public int MultiVehicleFleetNum = 2;
|
||||
[FieldMember(desc = "联动线程周期(ms)")] public int MultiVehicleSyncInterval = 50;
|
||||
[FieldMember(desc = "多车联动：主车端点 ip:port，/ 表示本车为主车")] public string MultiVehicleMasterEndpoint = "/";
|
||||
[FieldMember(desc = "多车联动：本车同步 IP")] public string SimpleIp = "127.0.0.1";
|
||||
|
||||
[FieldMember(desc = "多车联动：本车回连端点 ip:port，供主车 notify 回连，空=127.0.0.1:本车port")] public string MultiVehicleSelfEndpoint = "";
|
||||
[FieldMember(desc = "多车联动：自动速度命令超时(ms，0=auto)")] public int MultiVehicleAutoCmdTimeoutMs = 0;
|
||||
[FieldMember(desc = "多车联动：成员存活TTL(ms，0=auto)")] public int MultiVehicleMemberTtlMs = 0;
|
||||
|
||||
[JsonProperty("MultiVehicleMasterIp")]
|
||||
private string LegacyMasterIpSetter
|
||||
{
|
||||
set
|
||||
{
|
||||
if (string.IsNullOrEmpty(value) || value == "/") return;
|
||||
if (MultiVehicleMasterEndpoint == "/")
|
||||
MultiVehicleMasterEndpoint = value.Contains(":") ? value : $"{value}:8008";
|
||||
}
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region 多车-队形补偿
|
||||
[FieldMember(desc = "定位是否参与车队内姿态纠正(不影响整队姿态计算)")] public bool MultiVehicleSyncUseDetour = false;
|
||||
[FieldMember(desc = "手动联动是否启用定位姿态纠正(默认关闭)")] public bool MultiVehicleManualUseDetourCorrection = false;
|
||||
[FieldMember(desc = "多车联动：启用互识别纠正")] public bool MultiVehicleUseDetect = false;
|
||||
[FieldMember(desc = "多车联动：自动模式按理想中心前馈(弧线)")] public bool MultiVehicleAutoUseIdealCenter = true;
|
||||
[FieldMember(desc = "多车联动：自动模式要求有效车队中心")] public bool MultiVehicleAutoRequireFleetCenter = true;
|
||||
[FieldMember(desc = "多车联动：SLAM X补偿系数")] public float MultiVehiclePosBiasXFac = 0.5f;
|
||||
[FieldMember(desc = "多车联动：SLAM Y补偿系数")] public float MultiVehiclePosBiasYFac = 0.5f;
|
||||
[FieldMember(desc = "多车联动：SLAM Th补偿系数")] public float MultiVehiclePosBiasThFac = 0.5f;
|
||||
[FieldMember(desc = "多车联动：X补偿阈值(mm)")] public float MultiVehiclePosBiasXThreshold = 50f;
|
||||
[FieldMember(desc = "多车联动：Y补偿阈值(mm)")] public float MultiVehiclePosBiasYThreshold = 50f;
|
||||
[FieldMember(desc = "多车联动：Th补偿阈值(deg)")] public float MultiVehiclePosBiasThThreshold = 5f;
|
||||
[FieldMember(desc = "多车联动：互识别 X补偿系数")] public float MultiVehicleDetectBiasXFac = 0.5f;
|
||||
[FieldMember(desc = "多车联动：互识别 Y补偿系数")] public float MultiVehicleDetectBiasYFac = 0.5f;
|
||||
[FieldMember(desc = "多车联动：互识别 Th补偿系数")] public float MultiVehicleDetectBiasThFac = 0.5f;
|
||||
[FieldMember(desc = "多车联动：互识别 X补偿阈值(mm)")] public float MultiVehicleDetectBiasXThreshold = 50f;
|
||||
[FieldMember(desc = "多车联动：互识别 Y补偿阈值(mm)")] public float MultiVehicleDetectBiasYThreshold = 50f;
|
||||
[FieldMember(desc = "多车联动：互识别 Th补偿阈值(deg)")] public float MultiVehicleDetectBiasThThreshold = 5f;
|
||||
#endregion
|
||||
|
||||
#region 多车-旋转补偿
|
||||
[FieldMember(desc = "原地旋转纠偏：平移比例增益P(mm/s per mm)")] public float MultiVehicleRotateCompXyFac = 1.2f;
|
||||
[FieldMember(desc = "原地旋转纠偏：平移积分增益I(mm/s per mm·s)")] public float MultiVehicleRotateCompXyIFac = 0.8f;
|
||||
[FieldMember(desc = "原地旋转纠偏：平移速度上限(mm/s)")] public float MultiVehicleRotateCompXyMax = 150f;
|
||||
[FieldMember(desc = "原地旋转纠偏：转向比例增益P(deg/s per deg)")] public float MultiVehicleRotateCompThFac = 0.8f;
|
||||
[FieldMember(desc = "原地旋转纠偏：转向积分增益I(deg/s per deg·s)")] public float MultiVehicleRotateCompThIFac = 0.8f;
|
||||
[FieldMember(desc = "原地旋转纠偏：转向速度上限(deg/s)")] public float MultiVehicleRotateCompThMax = 15f;
|
||||
[FieldMember(desc = "原地旋转纠偏：生效的最小角速度阈值(deg/s)")] public float MultiVehicleRotateActiveOmega = 0.5f;
|
||||
[FieldMember(desc = "原地旋转纠偏：纠偏/旋转切向比例硬上限，<0使用安全默认0.10")] public float MultiVehicleRotateCompTangentFrac = 0.10f;
|
||||
|
||||
[FieldMember(desc = "单车同步 xy 精度(mm)")] public float SingleCarSyncPrecisionXy = 10f;
|
||||
[FieldMember(desc = "单车同步 th 精度(deg)")] public float SingleCarSyncPrecisionTh = 0.2f;
|
||||
#endregion
|
||||
|
||||
#region 多车-联动动作
|
||||
[FieldMember(desc = "车队原地旋转：角速度大小(deg/s，方向由目标角符号决定)")]
|
||||
public float FleetRotateOmega = 15f;
|
||||
|
||||
[FieldMember(desc = "车队原地旋转：目标相对转角(deg，+逆时针)")]
|
||||
public float FleetRotateTargetDeltaDeg = 90f;
|
||||
|
||||
[FieldMember(desc = "车队原地旋转：到位角度精度(deg)")]
|
||||
public float FleetRotateArriveDeg = 1.5f;
|
||||
|
||||
[FieldMember(desc = "车队原地旋转：减速区宽度(deg)，抑制收尾惯性超调")]
|
||||
public float FleetRotateSlowDeg = 25f;
|
||||
|
||||
[FieldMember(desc = "车队原地旋转：减速区末段最小角速度(deg/s)")]
|
||||
public float FleetRotateMinOmega = 3f;
|
||||
|
||||
[FieldMember(desc = "车队原地旋转：起步缓启动角加速度(deg/s²，<=0关闭)")]
|
||||
public float FleetRotateAccel = 20f;
|
||||
|
||||
[FieldMember(desc = "车队原地旋转：到位后安定时长(s)")]
|
||||
public float FleetRotateSettleSec = 0.5f;
|
||||
|
||||
[FieldMember(desc = "车队原地旋转：用Detour主车航向闭环判停(默认true，false=按时长开环)")]
|
||||
public bool FleetRotateUseDetourHeading = true;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：路径方向相对启动时车队朝向夹角(deg，逆时针为正；路径在车右侧x度时填-x)")]
|
||||
public float FleetCrabAngleDeg = 45f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：AGV入口使用的车队世界系目标朝向(deg)")]
|
||||
public float FleetCrabBodyWorldHeadingDeg = 0f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：路径长度(mm)")]
|
||||
public float FleetCrabLengthMm = 2000f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：行驶速度(m/s)")]
|
||||
public float FleetCrabSpeed = 0.2f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：速度命令加速度限制(m/s^2，<=0表示不限制)")]
|
||||
public float FleetCrabAccel = 0.2f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：预对齐后正式下发速度前5秒加速度(m/s^2，<=0表示不限制)")]
|
||||
public float FleetCrabStartAccel = 0.01f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：末端开始减速距离(mm)")]
|
||||
public float FleetCrabSlowDistance = 2000f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：完成距离(mm)，低于该剩余距离结束动作")]
|
||||
public float FleetCrabFinishDistance = 20f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：末端最低速度(m/s)")]
|
||||
public float FleetCrabFinishSpeed = 0.02f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：末端减速曲线指数")]
|
||||
public float FleetCrabSlowingPow = 0.8f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：GCP舵角修正上限(deg)")]
|
||||
public float FleetCrabGcpThetaThreshold = 95f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：headingErr角度纠偏比例系数")]
|
||||
public float FleetCrabDthLinearFac = 1f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：headingErr角度纠偏舵角限幅(deg)")]
|
||||
public float FleetCrabDthLinearThreshold = 10f;
|
||||
|
||||
[FieldMember(desc = "FleetCrab startup sync timeout(s)")]
|
||||
public float FleetCrabStartSyncTimeoutSec = 8f;
|
||||
|
||||
[FieldMember(desc = "FleetCrab startup wheel alignment tolerance(deg)")]
|
||||
public float FleetCrabStartWheelAlignDeg = 2f;
|
||||
|
||||
[FieldMember(desc = "FleetCurve MovementTest Bezier control point count")]
|
||||
public int FleetCurveTestControlPointCount = 4;
|
||||
|
||||
[FieldMember(desc = "FleetCurve speed(m/s)")]
|
||||
public float FleetCurveSpeed = 0.2f;
|
||||
|
||||
[FieldMember(desc = "FleetCurve slow distance(mm)")]
|
||||
public float FleetCurveSlowDistance = 2000f;
|
||||
|
||||
[FieldMember(desc = "FleetCurve finish distance(mm)")]
|
||||
public float FleetCurveFinishDistance = 20f;
|
||||
|
||||
[FieldMember(desc = "FleetCurve finish speed(m/s)")]
|
||||
public float FleetCurveFinishSpeed = 0.02f;
|
||||
|
||||
[FieldMember(desc = "FleetCurve slowing curve exponent")]
|
||||
public float FleetCurveSlowingPow = 0.8f;
|
||||
#endregion
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
}
|
||||
@@ -0,0 +1,44 @@
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox.Clumsy.Pilot.MultiWheel;
|
||||
namespace MultiWheelC;
|
||||
|
||||
public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefinition>
|
||||
{
|
||||
public new float CarLength = 1472f;
|
||||
public new float CarWidth = 948f;
|
||||
[AsLowerIO(desc = "车号")] public int CarNum = 1;
|
||||
public override void StandardInit() { }
|
||||
|
||||
#region 夹臂变量
|
||||
[AsUpperIO(desc = "左夹臂下发速度")] public float SpeedLeftArm;
|
||||
|
||||
[AsUpperIO(desc = "右夹臂下发速度")] public float SpeedRightArm;
|
||||
|
||||
[AsLowerIO(desc = "左夹臂实际位置")] public float ActualPosLeftArm;
|
||||
|
||||
[AsLowerIO(desc = "右夹臂实际位置")] public float ActualPosRightArm;
|
||||
|
||||
[AsUpperIO(desc = "夹臂不同步报警")] public bool ClampOutOfSync = false;
|
||||
#endregion
|
||||
|
||||
[AsLowerIO(desc = "左前左轮实际位置")] public float LFLActualPos;
|
||||
[AsLowerIO(desc = "左前右轮实际位置")] public float LFRActualPos;
|
||||
[AsLowerIO(desc = "右前左轮实际位置")] public float RFLActualPos;
|
||||
[AsLowerIO(desc = "右前右轮实际位置")] public float RFRActualPos;
|
||||
[AsLowerIO(desc = "左后左轮实际位置")] public float LRLActualPos;
|
||||
[AsLowerIO(desc = "左后右轮实际位置")] public float LRRActualPos;
|
||||
[AsLowerIO(desc = "右后左轮实际位置")] public float RRLActualPos;
|
||||
[AsLowerIO(desc = "右后右轮实际位置")] public float RRRActualPos;
|
||||
|
||||
[AsLowerIO(desc = "左夹臂低限位")] public float LeftArmLowerPos;
|
||||
[AsLowerIO(desc = "左夹臂高限位")] public float LeftArmUpperPos;
|
||||
[AsLowerIO(desc = "右夹臂低限位")] public float RightArmLowerPos;
|
||||
[AsLowerIO(desc = "右夹臂高限位")] public float RightArmUpperPos;
|
||||
|
||||
[AsUpperIO(desc = "从C往驱动器下使能")] public bool DisableFromC = false;
|
||||
[AsUpperIO(desc = "从C上复位")] public bool ResetFromC = false;
|
||||
[AsLowerIO(desc = "驱动轮使能状态")] public bool WheelAbleState = true;
|
||||
}
|
||||
@@ -0,0 +1,415 @@
|
||||
using ClumsyCore.Interfaces;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Diagnostics;
|
||||
using System.Globalization;
|
||||
using System.IO;
|
||||
using System.Numerics;
|
||||
using System.Text;
|
||||
using System.Threading;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
// C层实验数据：保存一个采样时刻的定位与控制命令。
|
||||
public sealed class TrackingSample
|
||||
{
|
||||
public double ElapsedSeconds;
|
||||
|
||||
// Detour位置单位为mm，航向单位为deg。
|
||||
public double DetourX;
|
||||
public double DetourY;
|
||||
public double DetourTheta;
|
||||
|
||||
// 车体速度单位为m/s，角速度统一使用rad/s。
|
||||
public float CommandSpeed;
|
||||
public float CommandVx;
|
||||
public float CommandVy;
|
||||
public float CommandAngularSpeed;
|
||||
}
|
||||
|
||||
// C层实验工具：统一采集并保存轨迹跟踪实验数据。
|
||||
public sealed class TrackingExperimentRecorder
|
||||
{
|
||||
private readonly string _controllerName;
|
||||
private readonly string _trajectoryName;
|
||||
private readonly int _trialNumber;
|
||||
private readonly Vector2 _referenceStart;
|
||||
private readonly Vector2 _referenceEnd;
|
||||
private readonly float _referenceSpeed;
|
||||
private readonly float _referenceAngularSpeed;
|
||||
private readonly float _referenceMotionFrameYawDegrees;
|
||||
private readonly int _sampleIntervalMs;
|
||||
|
||||
private readonly List<TrackingSample> _samples =
|
||||
new List<TrackingSample>();
|
||||
|
||||
private readonly object _sampleSyncRoot =
|
||||
new object();
|
||||
|
||||
private readonly object _commandSyncRoot =
|
||||
new object();
|
||||
|
||||
private readonly Stopwatch _stopwatch =
|
||||
new Stopwatch();
|
||||
|
||||
private Thread _worker;
|
||||
private volatile bool _running;
|
||||
private int _started;
|
||||
private int _saved;
|
||||
|
||||
private bool _hasExternalCommand;
|
||||
private float _externalCommandSpeed;
|
||||
private float _externalCommandVx;
|
||||
private float _externalCommandVy;
|
||||
private float _externalCommandAngularSpeed;
|
||||
|
||||
public TrackingExperimentRecorder(
|
||||
string controllerName,
|
||||
string trajectoryName,
|
||||
int trialNumber,
|
||||
Vector2 referenceStart,
|
||||
Vector2 referenceEnd,
|
||||
float referenceSpeed,
|
||||
float referenceAngularSpeed = 0f,
|
||||
int sampleIntervalMs = 50,
|
||||
float referenceMotionFrameYawDegrees = 0f)
|
||||
{
|
||||
if (string.IsNullOrWhiteSpace(controllerName))
|
||||
throw new ArgumentException(
|
||||
"控制器名称不能为空。",
|
||||
nameof(controllerName));
|
||||
|
||||
if (string.IsNullOrWhiteSpace(trajectoryName))
|
||||
throw new ArgumentException(
|
||||
"轨迹名称不能为空。",
|
||||
nameof(trajectoryName));
|
||||
|
||||
if (sampleIntervalMs <= 0)
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(sampleIntervalMs),
|
||||
"采样周期必须大于零。");
|
||||
|
||||
_controllerName = controllerName;
|
||||
_trajectoryName = trajectoryName;
|
||||
_trialNumber = trialNumber;
|
||||
_referenceStart = referenceStart;
|
||||
_referenceEnd = referenceEnd;
|
||||
_referenceSpeed = referenceSpeed;
|
||||
_referenceAngularSpeed = referenceAngularSpeed;
|
||||
_referenceMotionFrameYawDegrees =
|
||||
referenceMotionFrameYawDegrees;
|
||||
_sampleIntervalMs = sampleIntervalMs;
|
||||
}
|
||||
|
||||
// 保存成功后的CSV绝对路径；尚未保存时为空。
|
||||
public string SavedFilePath { get; private set; }
|
||||
|
||||
// 启动后台采样线程。
|
||||
public void Start()
|
||||
{
|
||||
if (Interlocked.Exchange(ref _started, 1) != 0)
|
||||
return;
|
||||
|
||||
_stopwatch.Restart();
|
||||
_running = true;
|
||||
|
||||
// 立即保存起点静止状态，避免第一帧被后台线程延迟。
|
||||
CaptureSample();
|
||||
|
||||
_worker = new Thread(SamplingLoop)
|
||||
{
|
||||
IsBackground = true,
|
||||
Name = "TrackingExperimentRecorder"
|
||||
};
|
||||
_worker.Start();
|
||||
}
|
||||
|
||||
// 供Stanley/LQR控制器主动写入本周期最终速度命令。
|
||||
// 调用后优先记录该命令，不再使用底盘反解值。
|
||||
public void UpdateCommand(
|
||||
float commandSpeed,
|
||||
float commandAngularSpeed)
|
||||
{
|
||||
lock (_commandSyncRoot)
|
||||
{
|
||||
_externalCommandSpeed = commandSpeed;
|
||||
_externalCommandVx = commandSpeed;
|
||||
_externalCommandVy = 0f;
|
||||
_externalCommandAngularSpeed =
|
||||
commandAngularSpeed;
|
||||
_hasExternalCommand = true;
|
||||
}
|
||||
}
|
||||
|
||||
// 供全向、蟹行和曲线控制器写入完整车体速度命令。
|
||||
public void UpdateBodyCommand(
|
||||
float commandVx,
|
||||
float commandVy,
|
||||
float commandAngularSpeed)
|
||||
{
|
||||
lock (_commandSyncRoot)
|
||||
{
|
||||
_externalCommandVx = commandVx;
|
||||
_externalCommandVy = commandVy;
|
||||
_externalCommandSpeed =
|
||||
(float)Math.Sqrt(
|
||||
commandVx * commandVx +
|
||||
commandVy * commandVy);
|
||||
_externalCommandAngularSpeed =
|
||||
commandAngularSpeed;
|
||||
_hasExternalCommand = true;
|
||||
}
|
||||
}
|
||||
|
||||
// 停止采样并将本次实验保存为CSV；重复调用只保存一次。
|
||||
public void StopAndSave()
|
||||
{
|
||||
if (Volatile.Read(ref _started) == 0)
|
||||
return;
|
||||
|
||||
if (Interlocked.Exchange(ref _saved, 1) != 0)
|
||||
return;
|
||||
|
||||
try
|
||||
{
|
||||
_running = false;
|
||||
|
||||
if (_worker != null &&
|
||||
_worker != Thread.CurrentThread)
|
||||
{
|
||||
_worker.Join(
|
||||
Math.Max(1000, _sampleIntervalMs * 4));
|
||||
}
|
||||
|
||||
// 保存停止时刻的最后一帧。
|
||||
CaptureSample();
|
||||
_stopwatch.Stop();
|
||||
SaveCsv();
|
||||
|
||||
Console.WriteLine(
|
||||
$"轨迹实验数据已保存：{SavedFilePath}");
|
||||
}
|
||||
catch
|
||||
{
|
||||
// 保存失败后允许调用者再次尝试。
|
||||
Interlocked.Exchange(ref _saved, 0);
|
||||
throw;
|
||||
}
|
||||
}
|
||||
|
||||
// 按固定周期采集Detour位姿和控制命令。
|
||||
private void SamplingLoop()
|
||||
{
|
||||
while (_running)
|
||||
{
|
||||
Thread.Sleep(_sampleIntervalMs);
|
||||
|
||||
if (!_running)
|
||||
break;
|
||||
|
||||
CaptureSample();
|
||||
}
|
||||
}
|
||||
|
||||
// 采集一帧Detour位姿和控制命令。
|
||||
private void CaptureSample()
|
||||
{
|
||||
try
|
||||
{
|
||||
var location =
|
||||
DetourInterface.getCartLocation();
|
||||
|
||||
float commandSpeed;
|
||||
float commandVx;
|
||||
float commandVy;
|
||||
float commandAngularSpeed;
|
||||
|
||||
lock (_commandSyncRoot)
|
||||
{
|
||||
if (_hasExternalCommand)
|
||||
{
|
||||
commandSpeed =
|
||||
_externalCommandSpeed;
|
||||
commandVx =
|
||||
_externalCommandVx;
|
||||
commandVy =
|
||||
_externalCommandVy;
|
||||
commandAngularSpeed =
|
||||
_externalCommandAngularSpeed;
|
||||
}
|
||||
else
|
||||
{
|
||||
var command =
|
||||
PilotDefinition.Chassis
|
||||
.GetCarSpeed(false);
|
||||
|
||||
commandVx = command.Vx;
|
||||
commandVy = command.Vy;
|
||||
// CommonUsage.GetCarSpeed().Vw的单位为deg/s，
|
||||
// 记录器内部统一转换为rad/s。
|
||||
commandAngularSpeed =
|
||||
(float)AngleMath.DegreesToRadians(
|
||||
command.Vw);
|
||||
commandSpeed = (float)Math.Sqrt(
|
||||
commandVx * commandVx +
|
||||
commandVy * commandVy);
|
||||
}
|
||||
}
|
||||
|
||||
var sample = new TrackingSample
|
||||
{
|
||||
ElapsedSeconds =
|
||||
_stopwatch.Elapsed.TotalSeconds,
|
||||
DetourX = location.x,
|
||||
DetourY = location.y,
|
||||
DetourTheta = location.th,
|
||||
CommandSpeed = commandSpeed,
|
||||
CommandVx = commandVx,
|
||||
CommandVy = commandVy,
|
||||
CommandAngularSpeed =
|
||||
commandAngularSpeed
|
||||
};
|
||||
|
||||
lock (_sampleSyncRoot)
|
||||
{
|
||||
_samples.Add(sample);
|
||||
}
|
||||
}
|
||||
catch (Exception ex)
|
||||
{
|
||||
// 单帧读取失败不应终止车辆控制或整个记录线程。
|
||||
Console.WriteLine(
|
||||
$"轨迹实验采样失败：{ex.Message}");
|
||||
}
|
||||
}
|
||||
|
||||
// 将内存中的采样数据写入CSV。
|
||||
private void SaveCsv()
|
||||
{
|
||||
List<TrackingSample> snapshot;
|
||||
|
||||
lock (_sampleSyncRoot)
|
||||
{
|
||||
snapshot =
|
||||
new List<TrackingSample>(_samples);
|
||||
}
|
||||
|
||||
var outputDirectory = Path.Combine(
|
||||
AppContext.BaseDirectory,
|
||||
"TrackingExperiments");
|
||||
|
||||
Directory.CreateDirectory(outputDirectory);
|
||||
|
||||
var fileName =
|
||||
$"{DateTime.Now:yyyyMMdd_HHmmss_fff}_" +
|
||||
$"{SanitizeFileName(_controllerName)}_" +
|
||||
$"{SanitizeFileName(_trajectoryName)}_" +
|
||||
$"Trial{_trialNumber}.csv";
|
||||
|
||||
SavedFilePath = Path.Combine(
|
||||
outputDirectory,
|
||||
fileName);
|
||||
|
||||
using (var writer = new StreamWriter(
|
||||
SavedFilePath,
|
||||
false,
|
||||
new UTF8Encoding(true)))
|
||||
{
|
||||
writer.WriteLine(
|
||||
"ElapsedSeconds," +
|
||||
"ControllerName," +
|
||||
"TrajectoryName," +
|
||||
"TrialNumber," +
|
||||
"DetourX," +
|
||||
"DetourY," +
|
||||
"DetourTheta," +
|
||||
"CommandSpeed," +
|
||||
// 保留旧列（deg/s）供历史Python脚本兼容。
|
||||
"CommandAngularSpeed," +
|
||||
"CommandAngularSpeedRadPerSecond," +
|
||||
"CommandVx," +
|
||||
"CommandVy," +
|
||||
"ReferenceStartX," +
|
||||
"ReferenceStartY," +
|
||||
"ReferenceEndX," +
|
||||
"ReferenceEndY," +
|
||||
"ReferenceSpeed," +
|
||||
"ReferenceAngularSpeedRadPerSecond," +
|
||||
"ReferenceMotionFrameYawDegrees");
|
||||
|
||||
foreach (var sample in snapshot)
|
||||
{
|
||||
writer.WriteLine(string.Join(
|
||||
",",
|
||||
Format(sample.ElapsedSeconds),
|
||||
EscapeCsv(_controllerName),
|
||||
EscapeCsv(_trajectoryName),
|
||||
_trialNumber.ToString(
|
||||
CultureInfo.InvariantCulture),
|
||||
Format(sample.DetourX),
|
||||
Format(sample.DetourY),
|
||||
Format(sample.DetourTheta),
|
||||
Format(sample.CommandSpeed),
|
||||
Format(
|
||||
AngleMath.RadiansToDegrees(
|
||||
sample.CommandAngularSpeed)),
|
||||
Format(sample.CommandAngularSpeed),
|
||||
Format(sample.CommandVx),
|
||||
Format(sample.CommandVy),
|
||||
Format(_referenceStart.X),
|
||||
Format(_referenceStart.Y),
|
||||
Format(_referenceEnd.X),
|
||||
Format(_referenceEnd.Y),
|
||||
Format(_referenceSpeed),
|
||||
Format(_referenceAngularSpeed),
|
||||
Format(_referenceMotionFrameYawDegrees)));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 将文件名中的非法字符替换为下划线。
|
||||
private static string SanitizeFileName(string value)
|
||||
{
|
||||
var result = value;
|
||||
|
||||
foreach (var invalidCharacter in
|
||||
Path.GetInvalidFileNameChars())
|
||||
{
|
||||
result = result.Replace(
|
||||
invalidCharacter,
|
||||
'_');
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
// 按固定小数格式输出数值，避免系统区域设置改变CSV格式。
|
||||
private static string Format(double value)
|
||||
{
|
||||
return value.ToString(
|
||||
"0.######",
|
||||
CultureInfo.InvariantCulture);
|
||||
}
|
||||
|
||||
// 对CSV文本字段进行引号和逗号转义。
|
||||
private static string EscapeCsv(string value)
|
||||
{
|
||||
if (value == null)
|
||||
return string.Empty;
|
||||
|
||||
if (!value.Contains(",") &&
|
||||
!value.Contains("\"") &&
|
||||
!value.Contains("\r") &&
|
||||
!value.Contains("\n"))
|
||||
{
|
||||
return value;
|
||||
}
|
||||
|
||||
return
|
||||
"\"" +
|
||||
value.Replace("\"", "\"\"") +
|
||||
"\"";
|
||||
}
|
||||
}
|
||||
}
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,163 @@
|
||||
{
|
||||
"runtimeTarget": {
|
||||
"name": ".NETStandard,Version=v2.0/",
|
||||
"signature": ""
|
||||
},
|
||||
"compilationOptions": {},
|
||||
"targets": {
|
||||
".NETStandard,Version=v2.0": {},
|
||||
".NETStandard,Version=v2.0/": {
|
||||
"MultiWheelC/1.0.0": {
|
||||
"dependencies": {
|
||||
"NETStandard.Library": "2.0.3",
|
||||
"Newtonsoft.Json": "13.0.3",
|
||||
"System.Numerics.Vectors": "4.6.1",
|
||||
"CommonUsage": "1.0.0.0",
|
||||
"LessokajiWeaverUtilities": "1.0.0.0",
|
||||
"MDCSToolBox": "1.0.0.0",
|
||||
"RefClumsyCore": "0.0.0.0",
|
||||
"RefClumsyDance": "0.0.0.0",
|
||||
"RefFundamentalLib": "0.0.0.0"
|
||||
},
|
||||
"runtime": {
|
||||
"MultiWheelC.dll": {}
|
||||
}
|
||||
},
|
||||
"Microsoft.NETCore.Platforms/1.1.0": {},
|
||||
"NETStandard.Library/2.0.3": {
|
||||
"dependencies": {
|
||||
"Microsoft.NETCore.Platforms": "1.1.0"
|
||||
}
|
||||
},
|
||||
"Newtonsoft.Json/13.0.3": {
|
||||
"runtime": {
|
||||
"lib/netstandard2.0/Newtonsoft.Json.dll": {
|
||||
"assemblyVersion": "13.0.0.0",
|
||||
"fileVersion": "13.0.3.27908"
|
||||
}
|
||||
}
|
||||
},
|
||||
"System.Numerics.Vectors/4.6.1": {
|
||||
"runtime": {
|
||||
"lib/netstandard2.0/System.Numerics.Vectors.dll": {
|
||||
"assemblyVersion": "4.1.3.0",
|
||||
"fileVersion": "4.600.125.16908"
|
||||
}
|
||||
}
|
||||
},
|
||||
"CommonUsage/1.0.0.0": {
|
||||
"runtime": {
|
||||
"CommonUsage.dll": {
|
||||
"assemblyVersion": "1.0.0.0",
|
||||
"fileVersion": "1.0.0.0"
|
||||
}
|
||||
}
|
||||
},
|
||||
"LessokajiWeaverUtilities/1.0.0.0": {
|
||||
"runtime": {
|
||||
"LessokajiWeaverUtilities.dll": {
|
||||
"assemblyVersion": "1.0.0.0",
|
||||
"fileVersion": "1.0.0.0"
|
||||
}
|
||||
}
|
||||
},
|
||||
"MDCSToolBox/1.0.0.0": {
|
||||
"runtime": {
|
||||
"MDCSToolBox.dll": {
|
||||
"assemblyVersion": "1.0.0.0",
|
||||
"fileVersion": "1.0.0.0"
|
||||
}
|
||||
}
|
||||
},
|
||||
"RefClumsyCore/0.0.0.0": {
|
||||
"runtime": {
|
||||
"RefClumsyCore.dll": {
|
||||
"assemblyVersion": "0.0.0.0",
|
||||
"fileVersion": "0.0.0.0"
|
||||
}
|
||||
}
|
||||
},
|
||||
"RefClumsyDance/0.0.0.0": {
|
||||
"runtime": {
|
||||
"RefClumsyDance.dll": {
|
||||
"assemblyVersion": "0.0.0.0",
|
||||
"fileVersion": "0.0.0.0"
|
||||
}
|
||||
}
|
||||
},
|
||||
"RefFundamentalLib/0.0.0.0": {
|
||||
"runtime": {
|
||||
"RefFundamentalLib.dll": {
|
||||
"assemblyVersion": "0.0.0.0",
|
||||
"fileVersion": "0.0.0.0"
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
},
|
||||
"libraries": {
|
||||
"MultiWheelC/1.0.0": {
|
||||
"type": "project",
|
||||
"serviceable": false,
|
||||
"sha512": ""
|
||||
},
|
||||
"Microsoft.NETCore.Platforms/1.1.0": {
|
||||
"type": "package",
|
||||
"serviceable": true,
|
||||
"sha512": "sha512-kz0PEW2lhqygehI/d6XsPCQzD7ff7gUJaVGPVETX611eadGsA3A877GdSlU0LRVMCTH/+P3o2iDTak+S08V2+A==",
|
||||
"path": "microsoft.netcore.platforms/1.1.0",
|
||||
"hashPath": "microsoft.netcore.platforms.1.1.0.nupkg.sha512"
|
||||
},
|
||||
"NETStandard.Library/2.0.3": {
|
||||
"type": "package",
|
||||
"serviceable": true,
|
||||
"sha512": "sha512-st47PosZSHrjECdjeIzZQbzivYBJFv6P2nv4cj2ypdI204DO+vZ7l5raGMiX4eXMJ53RfOIg+/s4DHVZ54Nu2A==",
|
||||
"path": "netstandard.library/2.0.3",
|
||||
"hashPath": "netstandard.library.2.0.3.nupkg.sha512"
|
||||
},
|
||||
"Newtonsoft.Json/13.0.3": {
|
||||
"type": "package",
|
||||
"serviceable": true,
|
||||
"sha512": "sha512-HrC5BXdl00IP9zeV+0Z848QWPAoCr9P3bDEZguI+gkLcBKAOxix/tLEAAHC+UvDNPv4a2d18lOReHMOagPa+zQ==",
|
||||
"path": "newtonsoft.json/13.0.3",
|
||||
"hashPath": "newtonsoft.json.13.0.3.nupkg.sha512"
|
||||
},
|
||||
"System.Numerics.Vectors/4.6.1": {
|
||||
"type": "package",
|
||||
"serviceable": true,
|
||||
"sha512": "sha512-sQxefTnhagrhoq2ReR0D/6K0zJcr9Hrd6kikeXsA1I8kOCboTavcUC4r7TSfpKFeE163uMuxZcyfO1mGO3EN8Q==",
|
||||
"path": "system.numerics.vectors/4.6.1",
|
||||
"hashPath": "system.numerics.vectors.4.6.1.nupkg.sha512"
|
||||
},
|
||||
"CommonUsage/1.0.0.0": {
|
||||
"type": "reference",
|
||||
"serviceable": false,
|
||||
"sha512": ""
|
||||
},
|
||||
"LessokajiWeaverUtilities/1.0.0.0": {
|
||||
"type": "reference",
|
||||
"serviceable": false,
|
||||
"sha512": ""
|
||||
},
|
||||
"MDCSToolBox/1.0.0.0": {
|
||||
"type": "reference",
|
||||
"serviceable": false,
|
||||
"sha512": ""
|
||||
},
|
||||
"RefClumsyCore/0.0.0.0": {
|
||||
"type": "reference",
|
||||
"serviceable": false,
|
||||
"sha512": ""
|
||||
},
|
||||
"RefClumsyDance/0.0.0.0": {
|
||||
"type": "reference",
|
||||
"serviceable": false,
|
||||
"sha512": ""
|
||||
},
|
||||
"RefFundamentalLib/0.0.0.0": {
|
||||
"type": "reference",
|
||||
"serviceable": false,
|
||||
"sha512": ""
|
||||
}
|
||||
}
|
||||
}
|
||||
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.
Binary file not shown.
Binary file not shown.
Reference in New Issue
Block a user