拆分MultiWheelC并新增轨迹投影、Detour状态估计与Stanley跟踪控制
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Cursor
@@ -0,0 +1 @@
|
||||
// 所有横向控制器的统一接口
|
||||
@@ -0,0 +1 @@
|
||||
// 统一纵向控制接口
|
||||
@@ -0,0 +1 @@
|
||||
// 横向控制器的输出
|
||||
@@ -0,0 +1 @@
|
||||
// 保存一次控制周期需要的完整输入
|
||||
@@ -0,0 +1 @@
|
||||
// 车体中心命令曲率转换成前后GCP方向
|
||||
@@ -0,0 +1,9 @@
|
||||
// 表示发送给底盘前的中间命令
|
||||
// public readonly struct GcpMotionCommand
|
||||
// {
|
||||
// public double SpeedMetersPerSecond { get; }
|
||||
|
||||
// public double FrontAngleRadians { get; }
|
||||
|
||||
// public double RearAngleRadians { get; }
|
||||
// }
|
||||
@@ -0,0 +1 @@
|
||||
// 负责把纯数学命令转换成现有底盘调用
|
||||
@@ -0,0 +1 @@
|
||||
// 负责组织一个控制周期
|
||||
@@ -0,0 +1,90 @@
|
||||
|
||||
|
||||
using System;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.Pilot;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
public abstract class ClampMovementTestBase : MovementTest
|
||||
{
|
||||
public float TimeoutSeconds = 30f; // 动作超时时间，单位s。
|
||||
|
||||
private DriveTask _task;
|
||||
protected abstract bool Close { get; }
|
||||
|
||||
// 根据派生测试类型驱动左右夹臂同步夹紧或打开。
|
||||
public override void Test()
|
||||
{
|
||||
var leftTarget = Close
|
||||
? PilotDefinition.Self.LeftArmUpperPos
|
||||
: PilotDefinition.Self.LeftArmLowerPos;
|
||||
var rightTarget = Close
|
||||
? PilotDefinition.Self.RightArmUpperPos
|
||||
: PilotDefinition.Self.RightArmLowerPos;
|
||||
|
||||
if (float.IsNaN(leftTarget) ||
|
||||
float.IsInfinity(leftTarget) ||
|
||||
float.IsNaN(rightTarget) ||
|
||||
float.IsInfinity(rightTarget))
|
||||
{
|
||||
Console.WriteLine(
|
||||
"夹臂目标位置无效，取消夹臂运动测试。");
|
||||
return;
|
||||
}
|
||||
|
||||
// 防止重复点击时上一项夹臂任务仍在运行。
|
||||
TestStop();
|
||||
Console.WriteLine(
|
||||
$"开始夹臂{(Close ? "夹紧" : "打开")}测试：" +
|
||||
$"左目标={leftTarget}，右目标={rightTarget}");
|
||||
|
||||
var task = new DriveTask(
|
||||
new ClampToTarget
|
||||
{
|
||||
LeftClampTarget = leftTarget,
|
||||
RightClampTarget = rightTarget,
|
||||
TimeoutSeconds = TimeoutSeconds
|
||||
}.Get());
|
||||
_task = task;
|
||||
|
||||
try
|
||||
{
|
||||
task.Wait();
|
||||
}
|
||||
finally
|
||||
{
|
||||
PilotDefinition.Self.SpeedLeftArm = 0f;
|
||||
PilotDefinition.Self.SpeedRightArm = 0f;
|
||||
if (ReferenceEquals(_task, task))
|
||||
_task = null;
|
||||
}
|
||||
}
|
||||
|
||||
// 停止夹臂任务并立即清零左右夹臂下发速度。
|
||||
public override void TestStop()
|
||||
{
|
||||
_task?.Stop();
|
||||
_task = null;
|
||||
PilotDefinition.Self.SpeedLeftArm = 0f;
|
||||
PilotDefinition.Self.SpeedRightArm = 0f;
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "夹臂关闭测试")]
|
||||
public sealed class TestClampCloseMovement
|
||||
: ClampMovementTestBase
|
||||
{
|
||||
protected override bool Close => false;
|
||||
}
|
||||
|
||||
[MovementTest(name = "夹臂启动测试")]
|
||||
public sealed class TestClampOpenMovement
|
||||
: ClampMovementTestBase
|
||||
{
|
||||
protected override bool Close => true;
|
||||
}
|
||||
}
|
||||
@@ -1,108 +1,20 @@
|
||||
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;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
using MDCSToolBox.Clumsy.Tracks;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
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
|
||||
{
|
||||
@@ -179,148 +91,6 @@ namespace MultiWheelC
|
||||
}
|
||||
}
|
||||
|
||||
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
|
||||
{
|
||||
@@ -449,7 +219,6 @@ namespace MultiWheelC
|
||||
}
|
||||
}
|
||||
|
||||
#region 蟹行运动测试
|
||||
[MovementTest(name = "SendMotion：蟹行直线4m")]
|
||||
public class TestCrabForward4m : MovementTest
|
||||
{
|
||||
@@ -863,87 +632,4 @@ namespace MultiWheelC
|
||||
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,158 @@
|
||||
|
||||
|
||||
using System;
|
||||
using System.Numerics;
|
||||
using System.Threading;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
using MDCSToolBox.Commons.Controllers;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
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")
|
||||
{
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,103 @@
|
||||
|
||||
|
||||
using System;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.Pilot;
|
||||
using CommonUsage.Chassis;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,629 +0,0 @@
|
||||
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,91 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using ClumsyCore.Pilot;
|
||||
using MDCSToolBox.Commons.Controllers;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
+2
-2
@@ -794,8 +794,8 @@ namespace MultiWheelC
|
||||
Math.PI / 2.0 ||
|
||||
!IsFinite(
|
||||
MaximumVirtualSteeringRadians))
|
||||
throw new ArgumentOutOfRangeException(
|
||||
"蟹行轨迹测试参数无效。");
|
||||
throw new ArgumentOutOfRangeException(
|
||||
"蟹行轨迹测试参数无效。");
|
||||
}
|
||||
|
||||
private static double Limit(
|
||||
@@ -0,0 +1,123 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Threading;
|
||||
using ClumsyCore.Pilot;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,55 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Numerics;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.DTools;
|
||||
using ClumsyCore.Pilot;
|
||||
using CommonUsage.Chassis;
|
||||
using MDCSToolBox.Clumsy.Tracks;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
//在世界坐标系下，从路径起点追踪到终点并停车
|
||||
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();
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,178 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Numerics;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.DTools;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using CommonUsage.Chassis;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox.Clumsy.Tracks;
|
||||
using MDCSToolBox.Commons.Controllers;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,92 @@
|
||||
|
||||
|
||||
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using ClumsyCore.Pilot;
|
||||
using CommonUsage.Chassis;
|
||||
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();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,134 @@
|
||||
|
||||
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using CommonUsage.Chassis;
|
||||
using MDCSToolBox.Commons.Controllers;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC
|
||||
{
|
||||
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();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,509 @@
|
||||
using System;
|
||||
using System.Diagnostics;
|
||||
using ClumsyCore.Interfaces;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.StateEstimation
|
||||
{
|
||||
/// <summary>
|
||||
/// 读取Detour位姿，忽略重复或明显异常的观测，并估算车辆二维速度。
|
||||
/// </summary>
|
||||
public sealed class DetourVehicleStateProvider
|
||||
: IVehicleStateProvider
|
||||
{
|
||||
public const double DefaultMaximumLinearSpeedMetersPerSecond =
|
||||
1.20;
|
||||
public const double DefaultMaximumAngularSpeedRadiansPerSecond =
|
||||
Math.PI / 4.0;
|
||||
public const double DefaultPositionJumpMarginMeters =
|
||||
0.03;
|
||||
public const double DefaultHeadingJumpMarginRadians =
|
||||
5.0 * Math.PI / 180.0;
|
||||
public const double DefaultVelocityPositionResidualMeters =
|
||||
0.04;
|
||||
public const double DefaultVelocityHeadingResidualRadians =
|
||||
5.0 * Math.PI / 180.0;
|
||||
public const double DefaultStationaryConfirmationSeconds =
|
||||
0.35;
|
||||
|
||||
private const double MillimetersPerMeter = 1000.0;
|
||||
private const double PositionEqualityToleranceMeters = 1e-9;
|
||||
private const double HeadingEqualityToleranceRadians = 1e-8;
|
||||
|
||||
private readonly object _syncRoot = new object();
|
||||
private readonly Stopwatch _clock = Stopwatch.StartNew();
|
||||
private readonly VelocityEstimator2D _velocityEstimator;
|
||||
private readonly double _maximumLinearSpeedMetersPerSecond;
|
||||
private readonly double _maximumAngularSpeedRadiansPerSecond;
|
||||
private readonly double _positionJumpMarginMeters;
|
||||
private readonly double _headingJumpMarginRadians;
|
||||
private readonly double _velocityPositionResidualMeters;
|
||||
private readonly double _velocityHeadingResidualRadians;
|
||||
private readonly double _stationaryConfirmationSeconds;
|
||||
|
||||
private bool _hasAcceptedPose;
|
||||
private Pose2D _acceptedPoseInWorld;
|
||||
private double _acceptedTimestampSeconds;
|
||||
private VehicleState _latestState;
|
||||
private bool _stationaryHoldActive;
|
||||
|
||||
/// <summary>
|
||||
/// 创建使用停车机器人默认物理边界和速度滤波参数的Detour状态源。
|
||||
/// </summary>
|
||||
public DetourVehicleStateProvider()
|
||||
: this(
|
||||
new VelocityEstimator2D(),
|
||||
DefaultMaximumLinearSpeedMetersPerSecond,
|
||||
DefaultMaximumAngularSpeedRadiansPerSecond,
|
||||
DefaultPositionJumpMarginMeters,
|
||||
DefaultHeadingJumpMarginRadians,
|
||||
DefaultVelocityPositionResidualMeters,
|
||||
DefaultVelocityHeadingResidualRadians,
|
||||
DefaultStationaryConfirmationSeconds)
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 创建使用指定物理边界、静止确认时间和速度估计器的Detour状态源。
|
||||
/// </summary>
|
||||
public DetourVehicleStateProvider(
|
||||
VelocityEstimator2D velocityEstimator,
|
||||
double maximumLinearSpeedMetersPerSecond,
|
||||
double maximumAngularSpeedRadiansPerSecond,
|
||||
double positionJumpMarginMeters,
|
||||
double headingJumpMarginRadians,
|
||||
double velocityPositionResidualMeters,
|
||||
double velocityHeadingResidualRadians,
|
||||
double stationaryConfirmationSeconds)
|
||||
{
|
||||
_velocityEstimator = velocityEstimator ??
|
||||
throw new ArgumentNullException(
|
||||
nameof(velocityEstimator));
|
||||
|
||||
EnsureFinitePositive(
|
||||
maximumLinearSpeedMetersPerSecond,
|
||||
nameof(maximumLinearSpeedMetersPerSecond));
|
||||
EnsureFinitePositive(
|
||||
maximumAngularSpeedRadiansPerSecond,
|
||||
nameof(maximumAngularSpeedRadiansPerSecond));
|
||||
EnsureFiniteNonNegative(
|
||||
positionJumpMarginMeters,
|
||||
nameof(positionJumpMarginMeters));
|
||||
EnsureFiniteNonNegative(
|
||||
headingJumpMarginRadians,
|
||||
nameof(headingJumpMarginRadians));
|
||||
EnsureFinitePositive(
|
||||
velocityPositionResidualMeters,
|
||||
nameof(velocityPositionResidualMeters));
|
||||
EnsureFinitePositive(
|
||||
velocityHeadingResidualRadians,
|
||||
nameof(velocityHeadingResidualRadians));
|
||||
EnsureFinitePositive(
|
||||
stationaryConfirmationSeconds,
|
||||
nameof(stationaryConfirmationSeconds));
|
||||
|
||||
_maximumLinearSpeedMetersPerSecond =
|
||||
maximumLinearSpeedMetersPerSecond;
|
||||
_maximumAngularSpeedRadiansPerSecond =
|
||||
maximumAngularSpeedRadiansPerSecond;
|
||||
_positionJumpMarginMeters =
|
||||
positionJumpMarginMeters;
|
||||
_headingJumpMarginRadians =
|
||||
headingJumpMarginRadians;
|
||||
_velocityPositionResidualMeters =
|
||||
velocityPositionResidualMeters;
|
||||
_velocityHeadingResidualRadians =
|
||||
velocityHeadingResidualRadians;
|
||||
_stationaryConfirmationSeconds =
|
||||
stationaryConfirmationSeconds;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取最近一次读取失败或异常观测被忽略的原因，正常时为空字符串。
|
||||
/// </summary>
|
||||
public string LastFailureReason { get; private set; } = "";
|
||||
|
||||
/// <summary>
|
||||
/// 尝试读取Detour；重复帧保留最近状态，明显异常帧只忽略本次观测。
|
||||
/// </summary>
|
||||
public bool TryGetState(out VehicleState state)
|
||||
{
|
||||
lock (_syncRoot)
|
||||
{
|
||||
try
|
||||
{
|
||||
var poseInWorld =
|
||||
ReadDetourPoseInWorld();
|
||||
var timestampSeconds =
|
||||
_clock.Elapsed.TotalSeconds;
|
||||
|
||||
if (!_hasAcceptedPose)
|
||||
{
|
||||
state = AcceptPoseAfterReset(
|
||||
poseInWorld,
|
||||
timestampSeconds);
|
||||
LastFailureReason = "";
|
||||
return true;
|
||||
}
|
||||
|
||||
if (ArePosesEquivalent(
|
||||
poseInWorld,
|
||||
_acceptedPoseInWorld))
|
||||
{
|
||||
state = HandleRepeatedPose(
|
||||
timestampSeconds);
|
||||
LastFailureReason = "";
|
||||
return true;
|
||||
}
|
||||
|
||||
// 静止保持后出现新定位时重新建立差分基准，
|
||||
// 避免用很长的静止时间稀释第一次运动速度。
|
||||
if (_stationaryHoldActive)
|
||||
{
|
||||
state = AcceptPoseAfterReset(
|
||||
poseInWorld,
|
||||
timestampSeconds);
|
||||
LastFailureReason = "";
|
||||
return true;
|
||||
}
|
||||
|
||||
var elapsedSeconds =
|
||||
timestampSeconds -
|
||||
_acceptedTimestampSeconds;
|
||||
|
||||
if (!IsMotionPlausible(
|
||||
_acceptedPoseInWorld,
|
||||
poseInWorld,
|
||||
elapsedSeconds))
|
||||
{
|
||||
// 单帧异常不进入差分器，也不中断调用方；下一次
|
||||
// 正常观测仍相对最近有效位姿和真实时间差计算。
|
||||
state = _latestState;
|
||||
LastFailureReason =
|
||||
"Detour位姿变化超过车辆绝对运动边界，本次观测已忽略。";
|
||||
return true;
|
||||
}
|
||||
|
||||
if (IsVelocityInnovationAbnormal(
|
||||
poseInWorld,
|
||||
elapsedSeconds))
|
||||
{
|
||||
state = AcceptPoseAfterVelocityRebase(
|
||||
poseInWorld,
|
||||
timestampSeconds);
|
||||
LastFailureReason =
|
||||
"Detour位姿偏离上一速度预测，本次只更新位姿基准并保留滤波速度。";
|
||||
return true;
|
||||
}
|
||||
|
||||
state = AcceptContinuousPose(
|
||||
poseInWorld,
|
||||
timestampSeconds);
|
||||
LastFailureReason = "";
|
||||
return true;
|
||||
}
|
||||
catch (Exception exception)
|
||||
{
|
||||
state = default;
|
||||
LastFailureReason =
|
||||
"Detour车辆状态读取失败：" +
|
||||
exception.Message;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 清除Detour位姿历史和速度估计状态。
|
||||
/// </summary>
|
||||
public void Reset()
|
||||
{
|
||||
lock (_syncRoot)
|
||||
{
|
||||
_velocityEstimator.Reset();
|
||||
_hasAcceptedPose = false;
|
||||
_acceptedPoseInWorld = Pose2D.Identity;
|
||||
_acceptedTimestampSeconds = 0.0;
|
||||
_latestState = default;
|
||||
_stationaryHoldActive = false;
|
||||
LastFailureReason = "";
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 读取Detour毫米和角度数据并转换为世界坐标SI位姿。
|
||||
/// </summary>
|
||||
private static Pose2D ReadDetourPoseInWorld()
|
||||
{
|
||||
var location =
|
||||
DetourInterface.getCartLocation();
|
||||
|
||||
EnsureFinite(location.x, "DetourX");
|
||||
EnsureFinite(location.y, "DetourY");
|
||||
EnsureFinite(location.th, "DetourTheta");
|
||||
|
||||
return new Pose2D(
|
||||
location.x / MillimetersPerMeter,
|
||||
location.y / MillimetersPerMeter,
|
||||
AngleMath.NormalizeRadians(
|
||||
AngleMath.DegreesToRadians(
|
||||
location.th)));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 接受连续有效定位并更新速度估计和差分基准。
|
||||
/// </summary>
|
||||
private VehicleState AcceptContinuousPose(
|
||||
Pose2D poseInWorld,
|
||||
double timestampSeconds)
|
||||
{
|
||||
_latestState =
|
||||
_velocityEstimator.Update(
|
||||
poseInWorld,
|
||||
timestampSeconds);
|
||||
_acceptedPoseInWorld = poseInWorld;
|
||||
_acceptedTimestampSeconds =
|
||||
timestampSeconds;
|
||||
_stationaryHoldActive = false;
|
||||
return _latestState;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 接受跳变后的新位姿基准，但不让该位移进入速度差分和低通滤波器。
|
||||
/// </summary>
|
||||
private VehicleState AcceptPoseAfterVelocityRebase(
|
||||
Pose2D poseInWorld,
|
||||
double timestampSeconds)
|
||||
{
|
||||
_latestState =
|
||||
_velocityEstimator
|
||||
.RebasePreservingVelocity(
|
||||
poseInWorld,
|
||||
timestampSeconds);
|
||||
_acceptedPoseInWorld = poseInWorld;
|
||||
_acceptedTimestampSeconds =
|
||||
timestampSeconds;
|
||||
_stationaryHoldActive = false;
|
||||
return _latestState;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 接受首帧或静止后的首个新位姿并重新建立零速差分基准。
|
||||
/// </summary>
|
||||
private VehicleState AcceptPoseAfterReset(
|
||||
Pose2D poseInWorld,
|
||||
double timestampSeconds)
|
||||
{
|
||||
_latestState =
|
||||
_velocityEstimator.Reset(
|
||||
poseInWorld,
|
||||
timestampSeconds);
|
||||
_acceptedPoseInWorld = poseInWorld;
|
||||
_acceptedTimestampSeconds =
|
||||
timestampSeconds;
|
||||
_hasAcceptedPose = true;
|
||||
_stationaryHoldActive = false;
|
||||
return _latestState;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 对重复Detour观测保留最近状态，并在持续不变后将速度归零。
|
||||
/// </summary>
|
||||
private VehicleState HandleRepeatedPose(
|
||||
double timestampSeconds)
|
||||
{
|
||||
var unchangedSeconds =
|
||||
timestampSeconds -
|
||||
_acceptedTimestampSeconds;
|
||||
|
||||
if (!_stationaryHoldActive &&
|
||||
unchangedSeconds >=
|
||||
_stationaryConfirmationSeconds)
|
||||
{
|
||||
_latestState =
|
||||
new VehicleState(
|
||||
timestampSeconds,
|
||||
_acceptedPoseInWorld,
|
||||
Twist2D.Zero,
|
||||
true);
|
||||
_stationaryHoldActive = true;
|
||||
}
|
||||
|
||||
return _latestState;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判断两次有效Detour观测之间的变化是否超过车辆绝对运动能力。
|
||||
/// </summary>
|
||||
private bool IsMotionPlausible(
|
||||
Pose2D startPoseInWorld,
|
||||
Pose2D endPoseInWorld,
|
||||
double deltaTimeSeconds)
|
||||
{
|
||||
if (!IsFinite(deltaTimeSeconds) ||
|
||||
deltaTimeSeconds <= 0.0)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
var deltaX =
|
||||
endPoseInWorld.XMeters -
|
||||
startPoseInWorld.XMeters;
|
||||
var deltaY =
|
||||
endPoseInWorld.YMeters -
|
||||
startPoseInWorld.YMeters;
|
||||
var displacementMeters =
|
||||
Math.Sqrt(
|
||||
deltaX * deltaX +
|
||||
deltaY * deltaY);
|
||||
var headingChangeRadians =
|
||||
Math.Abs(
|
||||
AngleMath.ShortestDifferenceRadians(
|
||||
endPoseInWorld.YawRadians,
|
||||
startPoseInWorld.YawRadians));
|
||||
|
||||
var maximumDisplacementMeters =
|
||||
_maximumLinearSpeedMetersPerSecond *
|
||||
deltaTimeSeconds +
|
||||
_positionJumpMarginMeters;
|
||||
var maximumHeadingChangeRadians =
|
||||
_maximumAngularSpeedRadiansPerSecond *
|
||||
deltaTimeSeconds +
|
||||
_headingJumpMarginRadians;
|
||||
|
||||
return displacementMeters <=
|
||||
maximumDisplacementMeters &&
|
||||
headingChangeRadians <=
|
||||
maximumHeadingChangeRadians;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判断新位姿是否明显偏离上一滤波速度给出的恒速预测。
|
||||
/// </summary>
|
||||
private bool IsVelocityInnovationAbnormal(
|
||||
Pose2D poseInWorld,
|
||||
double deltaTimeSeconds)
|
||||
{
|
||||
if (!_latestState.HasValidVelocityEstimate)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
var predictedX =
|
||||
_acceptedPoseInWorld.XMeters +
|
||||
_latestState.TwistInWorld
|
||||
.VxMetersPerSecond *
|
||||
deltaTimeSeconds;
|
||||
var predictedY =
|
||||
_acceptedPoseInWorld.YMeters +
|
||||
_latestState.TwistInWorld
|
||||
.VyMetersPerSecond *
|
||||
deltaTimeSeconds;
|
||||
var predictedYaw =
|
||||
AngleMath.NormalizeRadians(
|
||||
_acceptedPoseInWorld.YawRadians +
|
||||
_latestState.TwistInWorld
|
||||
.OmegaRadiansPerSecond *
|
||||
deltaTimeSeconds);
|
||||
|
||||
var positionResidualX =
|
||||
poseInWorld.XMeters - predictedX;
|
||||
var positionResidualY =
|
||||
poseInWorld.YMeters - predictedY;
|
||||
var positionResidualMeters =
|
||||
Math.Sqrt(
|
||||
positionResidualX * positionResidualX +
|
||||
positionResidualY * positionResidualY);
|
||||
var headingResidualRadians =
|
||||
Math.Abs(
|
||||
AngleMath.ShortestDifferenceRadians(
|
||||
poseInWorld.YawRadians,
|
||||
predictedYaw));
|
||||
|
||||
return positionResidualMeters >
|
||||
_velocityPositionResidualMeters ||
|
||||
headingResidualRadians >
|
||||
_velocityHeadingResidualRadians;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判断两次读取是否为Detour保持输出的同一数值帧。
|
||||
/// </summary>
|
||||
private static bool ArePosesEquivalent(
|
||||
Pose2D firstPose,
|
||||
Pose2D secondPose)
|
||||
{
|
||||
return Math.Abs(
|
||||
firstPose.XMeters -
|
||||
secondPose.XMeters) <=
|
||||
PositionEqualityToleranceMeters &&
|
||||
Math.Abs(
|
||||
firstPose.YMeters -
|
||||
secondPose.YMeters) <=
|
||||
PositionEqualityToleranceMeters &&
|
||||
Math.Abs(
|
||||
AngleMath.ShortestDifferenceRadians(
|
||||
firstPose.YawRadians,
|
||||
secondPose.YawRadians)) <=
|
||||
HeadingEqualityToleranceRadians;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查数值是否为正有限值。
|
||||
/// </summary>
|
||||
private static void EnsureFinitePositive(
|
||||
double value,
|
||||
string parameterName)
|
||||
{
|
||||
EnsureFinite(value, parameterName);
|
||||
|
||||
if (value <= 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"状态源参数必须是正有限值。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查数值是否为非负有限值。
|
||||
/// </summary>
|
||||
private static void EnsureFiniteNonNegative(
|
||||
double value,
|
||||
string parameterName)
|
||||
{
|
||||
EnsureFinite(value, parameterName);
|
||||
|
||||
if (value < 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"状态源参数必须是非负有限值。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查数值是否为有限值。
|
||||
/// </summary>
|
||||
private static void EnsureFinite(
|
||||
double value,
|
||||
string parameterName)
|
||||
{
|
||||
if (!IsFinite(value))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"状态源参数和Detour位姿必须是有限值。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判断数值是否可用于状态估计。
|
||||
/// </summary>
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) &&
|
||||
!double.IsInfinity(value);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,142 @@
|
||||
using System;
|
||||
|
||||
namespace MultiWheelC.StateEstimation
|
||||
{
|
||||
/// <summary>
|
||||
/// 使用真实采样时间间隔对单个连续量执行在线一阶低通滤波。
|
||||
/// </summary>
|
||||
public sealed class FirstOrderLowPassFilter
|
||||
{
|
||||
private readonly double _timeConstantSeconds;
|
||||
private bool _isInitialized;
|
||||
private double _value;
|
||||
|
||||
/// <summary>
|
||||
/// 创建使用指定时间常数的一阶低通滤波器。
|
||||
/// </summary>
|
||||
public FirstOrderLowPassFilter(
|
||||
double timeConstantSeconds)
|
||||
{
|
||||
EnsureFinitePositive(
|
||||
timeConstantSeconds,
|
||||
nameof(timeConstantSeconds));
|
||||
|
||||
_timeConstantSeconds =
|
||||
timeConstantSeconds;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取滤波时间常数，单位为s；数值越大，滤波越强但响应越慢。
|
||||
/// </summary>
|
||||
public double TimeConstantSeconds =>
|
||||
_timeConstantSeconds;
|
||||
|
||||
/// <summary>
|
||||
/// 获取滤波器是否已经接收过有效初值。
|
||||
/// </summary>
|
||||
public bool IsInitialized =>
|
||||
_isInitialized;
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前滤波输出；尚未初始化时读取会抛出异常。
|
||||
/// </summary>
|
||||
public double Value
|
||||
{
|
||||
get
|
||||
{
|
||||
if (!_isInitialized)
|
||||
{
|
||||
throw new InvalidOperationException(
|
||||
"一阶低通滤波器尚未初始化。");
|
||||
}
|
||||
|
||||
return _value;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 使用当前输入和真实采样间隔更新滤波结果。
|
||||
/// </summary>
|
||||
public double Update(
|
||||
double input,
|
||||
double deltaTimeSeconds)
|
||||
{
|
||||
EnsureFinite(
|
||||
input,
|
||||
nameof(input));
|
||||
EnsureFinitePositive(
|
||||
deltaTimeSeconds,
|
||||
nameof(deltaTimeSeconds));
|
||||
|
||||
if (!_isInitialized)
|
||||
{
|
||||
_value = input;
|
||||
_isInitialized = true;
|
||||
return _value;
|
||||
}
|
||||
|
||||
var alpha =
|
||||
deltaTimeSeconds /
|
||||
(_timeConstantSeconds +
|
||||
deltaTimeSeconds);
|
||||
|
||||
_value += alpha * (input - _value);
|
||||
return _value;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 清除历史输出，使下一次有效输入直接成为新的初值。
|
||||
/// </summary>
|
||||
public void Reset()
|
||||
{
|
||||
_value = 0.0;
|
||||
_isInitialized = false;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 将滤波器立即重置到指定的有限初值。
|
||||
/// </summary>
|
||||
public void Reset(double initialValue)
|
||||
{
|
||||
EnsureFinite(
|
||||
initialValue,
|
||||
nameof(initialValue));
|
||||
|
||||
_value = initialValue;
|
||||
_isInitialized = true;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查数值是否为正有限值。
|
||||
/// </summary>
|
||||
private static void EnsureFinitePositive(
|
||||
double value,
|
||||
string parameterName)
|
||||
{
|
||||
EnsureFinite(value, parameterName);
|
||||
|
||||
if (value <= 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"滤波时间常数和采样间隔必须是正有限值。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查数值是否为有限值。
|
||||
/// </summary>
|
||||
private static void EnsureFinite(
|
||||
double value,
|
||||
string parameterName)
|
||||
{
|
||||
if (double.IsNaN(value) ||
|
||||
double.IsInfinity(value))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"滤波输入必须是有限值。");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,14 @@
|
||||
namespace MultiWheelC.StateEstimation
|
||||
{
|
||||
/// <summary>
|
||||
/// 为轨迹控制器提供与具体定位来源无关的统一车辆状态读取接口。
|
||||
/// </summary>
|
||||
public interface IVehicleStateProvider
|
||||
{
|
||||
/// <summary>
|
||||
/// 尝试读取当前有效车辆状态；定位不可用或过期时返回false。
|
||||
/// </summary>
|
||||
bool TryGetState(out VehicleState state);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,138 @@
|
||||
using System;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.StateEstimation
|
||||
{
|
||||
/// <summary>
|
||||
/// 保存一次经过校验的车辆位姿和速度估计快照，统一使用SI单位。
|
||||
/// </summary>
|
||||
public readonly struct VehicleState
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建车辆状态，并将世界坐标速度同步转换到车体坐标系。
|
||||
/// </summary>
|
||||
public VehicleState(
|
||||
double sampleTimestampSeconds,
|
||||
Pose2D poseInWorld,
|
||||
Twist2D twistInWorld,
|
||||
bool hasValidVelocityEstimate)
|
||||
{
|
||||
EnsureFiniteNonNegative(
|
||||
sampleTimestampSeconds,
|
||||
nameof(sampleTimestampSeconds));
|
||||
EnsureFinitePose(
|
||||
poseInWorld,
|
||||
nameof(poseInWorld));
|
||||
EnsureFiniteTwist(
|
||||
twistInWorld,
|
||||
nameof(twistInWorld));
|
||||
|
||||
SampleTimestampSeconds =
|
||||
sampleTimestampSeconds;
|
||||
PoseInWorld = new Pose2D(
|
||||
poseInWorld.XMeters,
|
||||
poseInWorld.YMeters,
|
||||
AngleMath.NormalizeRadians(
|
||||
poseInWorld.YawRadians));
|
||||
HasValidVelocityEstimate =
|
||||
hasValidVelocityEstimate;
|
||||
|
||||
// 第一帧或定位重置后的速度不可用于闭环控制，
|
||||
// 此时显式置零，避免调用方误用残留速度。
|
||||
TwistInWorld = hasValidVelocityEstimate
|
||||
? twistInWorld
|
||||
: Twist2D.Zero;
|
||||
|
||||
var worldPoseInBody =
|
||||
FrameTransform2D.Inverse(
|
||||
PoseInWorld);
|
||||
TwistInBody =
|
||||
FrameTransform2D.TransformTwistAtSamePoint(
|
||||
worldPoseInBody,
|
||||
TwistInWorld);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取状态源单调时钟中的采样时刻，单位为s。
|
||||
/// </summary>
|
||||
public double SampleTimestampSeconds { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取车体中心在Detour世界坐标系中的位姿，单位为m和rad。
|
||||
/// </summary>
|
||||
public Pose2D PoseInWorld { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取在世界坐标系中表达的车辆速度，单位为m/s和rad/s。
|
||||
/// </summary>
|
||||
public Twist2D TwistInWorld { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取在车体坐标系中表达的车辆速度，X向前、Y向左、逆时针为正。
|
||||
/// </summary>
|
||||
public Twist2D TwistInBody { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取当前速度是否已由至少两个连续有效定位样本估算得到。
|
||||
/// </summary>
|
||||
public bool HasValidVelocityEstimate { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 检查位姿是否由有限数值组成。
|
||||
/// </summary>
|
||||
private static void EnsureFinitePose(
|
||||
Pose2D pose,
|
||||
string parameterName)
|
||||
{
|
||||
if (!IsFinite(pose.XMeters) ||
|
||||
!IsFinite(pose.YMeters) ||
|
||||
!IsFinite(pose.YawRadians))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"车辆位姿必须由有限数值组成。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查速度是否由有限数值组成。
|
||||
/// </summary>
|
||||
private static void EnsureFiniteTwist(
|
||||
Twist2D twist,
|
||||
string parameterName)
|
||||
{
|
||||
if (!IsFinite(twist.VxMetersPerSecond) ||
|
||||
!IsFinite(twist.VyMetersPerSecond) ||
|
||||
!IsFinite(twist.OmegaRadiansPerSecond))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"车辆速度必须由有限数值组成。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查数值是否为非负有限值。
|
||||
/// </summary>
|
||||
private static void EnsureFiniteNonNegative(
|
||||
double value,
|
||||
string parameterName)
|
||||
{
|
||||
if (!IsFinite(value) || value < 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"采样时刻必须是非负有限值。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判断数值是否可用于车辆状态计算。
|
||||
/// </summary>
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) &&
|
||||
!double.IsInfinity(value);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,275 @@
|
||||
using System;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.StateEstimation
|
||||
{
|
||||
/// <summary>
|
||||
/// 根据连续有效的Detour世界位姿和真实时间差估算车辆二维速度。
|
||||
/// </summary>
|
||||
public sealed class VelocityEstimator2D
|
||||
{
|
||||
public const double DefaultLinearFilterTimeConstantSeconds =
|
||||
0.15;
|
||||
public const double DefaultAngularFilterTimeConstantSeconds =
|
||||
0.20;
|
||||
|
||||
private readonly FirstOrderLowPassFilter
|
||||
_worldVelocityXFilter;
|
||||
private readonly FirstOrderLowPassFilter
|
||||
_worldVelocityYFilter;
|
||||
private readonly FirstOrderLowPassFilter
|
||||
_angularVelocityFilter;
|
||||
|
||||
private bool _hasPreviousSample;
|
||||
private Pose2D _previousPoseInWorld;
|
||||
private double _previousTimestampSeconds;
|
||||
|
||||
/// <summary>
|
||||
/// 创建使用默认0.15s线速度和0.20s角速度时间常数的估计器。
|
||||
/// </summary>
|
||||
public VelocityEstimator2D()
|
||||
: this(
|
||||
DefaultLinearFilterTimeConstantSeconds,
|
||||
DefaultAngularFilterTimeConstantSeconds)
|
||||
{
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 创建使用指定线速度和角速度滤波时间常数的估计器。
|
||||
/// </summary>
|
||||
public VelocityEstimator2D(
|
||||
double linearFilterTimeConstantSeconds,
|
||||
double angularFilterTimeConstantSeconds)
|
||||
{
|
||||
_worldVelocityXFilter =
|
||||
new FirstOrderLowPassFilter(
|
||||
linearFilterTimeConstantSeconds);
|
||||
_worldVelocityYFilter =
|
||||
new FirstOrderLowPassFilter(
|
||||
linearFilterTimeConstantSeconds);
|
||||
_angularVelocityFilter =
|
||||
new FirstOrderLowPassFilter(
|
||||
angularFilterTimeConstantSeconds);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取是否已经保存了可用于下一次差分的位姿基准。
|
||||
/// </summary>
|
||||
public bool HasPreviousSample =>
|
||||
_hasPreviousSample;
|
||||
|
||||
/// <summary>
|
||||
/// 使用一个新的有效定位样本更新并返回车辆状态。
|
||||
/// </summary>
|
||||
public VehicleState Update(
|
||||
Pose2D poseInWorld,
|
||||
double sampleTimestampSeconds)
|
||||
{
|
||||
EnsureFinitePose(
|
||||
poseInWorld,
|
||||
nameof(poseInWorld));
|
||||
EnsureFiniteNonNegative(
|
||||
sampleTimestampSeconds,
|
||||
nameof(sampleTimestampSeconds));
|
||||
|
||||
var normalizedPoseInWorld =
|
||||
new Pose2D(
|
||||
poseInWorld.XMeters,
|
||||
poseInWorld.YMeters,
|
||||
AngleMath.NormalizeRadians(
|
||||
poseInWorld.YawRadians));
|
||||
|
||||
if (!_hasPreviousSample)
|
||||
{
|
||||
return Reset(
|
||||
normalizedPoseInWorld,
|
||||
sampleTimestampSeconds);
|
||||
}
|
||||
|
||||
var deltaTimeSeconds =
|
||||
sampleTimestampSeconds -
|
||||
_previousTimestampSeconds;
|
||||
|
||||
if (deltaTimeSeconds <= 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(sampleTimestampSeconds),
|
||||
"新定位样本的单调时间戳必须严格大于上一帧。");
|
||||
}
|
||||
|
||||
var rawVelocityXInWorld =
|
||||
(normalizedPoseInWorld.XMeters -
|
||||
_previousPoseInWorld.XMeters) /
|
||||
deltaTimeSeconds;
|
||||
var rawVelocityYInWorld =
|
||||
(normalizedPoseInWorld.YMeters -
|
||||
_previousPoseInWorld.YMeters) /
|
||||
deltaTimeSeconds;
|
||||
var rawAngularVelocity =
|
||||
AngleMath.ShortestDifferenceRadians(
|
||||
normalizedPoseInWorld.YawRadians,
|
||||
_previousPoseInWorld.YawRadians) /
|
||||
deltaTimeSeconds;
|
||||
|
||||
var filteredTwistInWorld =
|
||||
new Twist2D(
|
||||
_worldVelocityXFilter.Update(
|
||||
rawVelocityXInWorld,
|
||||
deltaTimeSeconds),
|
||||
_worldVelocityYFilter.Update(
|
||||
rawVelocityYInWorld,
|
||||
deltaTimeSeconds),
|
||||
_angularVelocityFilter.Update(
|
||||
rawAngularVelocity,
|
||||
deltaTimeSeconds));
|
||||
|
||||
_previousPoseInWorld =
|
||||
normalizedPoseInWorld;
|
||||
_previousTimestampSeconds =
|
||||
sampleTimestampSeconds;
|
||||
|
||||
return new VehicleState(
|
||||
sampleTimestampSeconds,
|
||||
normalizedPoseInWorld,
|
||||
filteredTwistInWorld,
|
||||
true);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 更新位姿差分基准但保留当前滤波速度，避免定位跳变形成虚假速度尖峰。
|
||||
/// </summary>
|
||||
public VehicleState RebasePreservingVelocity(
|
||||
Pose2D poseInWorld,
|
||||
double sampleTimestampSeconds)
|
||||
{
|
||||
EnsureFinitePose(
|
||||
poseInWorld,
|
||||
nameof(poseInWorld));
|
||||
EnsureFiniteNonNegative(
|
||||
sampleTimestampSeconds,
|
||||
nameof(sampleTimestampSeconds));
|
||||
|
||||
var normalizedPoseInWorld =
|
||||
new Pose2D(
|
||||
poseInWorld.XMeters,
|
||||
poseInWorld.YMeters,
|
||||
AngleMath.NormalizeRadians(
|
||||
poseInWorld.YawRadians));
|
||||
|
||||
_previousPoseInWorld =
|
||||
normalizedPoseInWorld;
|
||||
_previousTimestampSeconds =
|
||||
sampleTimestampSeconds;
|
||||
_hasPreviousSample = true;
|
||||
|
||||
var hasValidVelocityEstimate =
|
||||
_worldVelocityXFilter.IsInitialized &&
|
||||
_worldVelocityYFilter.IsInitialized &&
|
||||
_angularVelocityFilter.IsInitialized;
|
||||
|
||||
var retainedTwistInWorld =
|
||||
hasValidVelocityEstimate
|
||||
? new Twist2D(
|
||||
_worldVelocityXFilter.Value,
|
||||
_worldVelocityYFilter.Value,
|
||||
_angularVelocityFilter.Value)
|
||||
: Twist2D.Zero;
|
||||
|
||||
return new VehicleState(
|
||||
sampleTimestampSeconds,
|
||||
normalizedPoseInWorld,
|
||||
retainedTwistInWorld,
|
||||
hasValidVelocityEstimate);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 使用当前定位重新建立差分基准，并返回速度无效的零速状态。
|
||||
/// </summary>
|
||||
public VehicleState Reset(
|
||||
Pose2D poseInWorld,
|
||||
double sampleTimestampSeconds)
|
||||
{
|
||||
EnsureFinitePose(
|
||||
poseInWorld,
|
||||
nameof(poseInWorld));
|
||||
EnsureFiniteNonNegative(
|
||||
sampleTimestampSeconds,
|
||||
nameof(sampleTimestampSeconds));
|
||||
|
||||
_previousPoseInWorld =
|
||||
new Pose2D(
|
||||
poseInWorld.XMeters,
|
||||
poseInWorld.YMeters,
|
||||
AngleMath.NormalizeRadians(
|
||||
poseInWorld.YawRadians));
|
||||
_previousTimestampSeconds =
|
||||
sampleTimestampSeconds;
|
||||
_hasPreviousSample = true;
|
||||
|
||||
_worldVelocityXFilter.Reset();
|
||||
_worldVelocityYFilter.Reset();
|
||||
_angularVelocityFilter.Reset();
|
||||
|
||||
return new VehicleState(
|
||||
sampleTimestampSeconds,
|
||||
_previousPoseInWorld,
|
||||
Twist2D.Zero,
|
||||
false);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 清除差分基准和全部滤波历史，使下一帧重新初始化估计器。
|
||||
/// </summary>
|
||||
public void Reset()
|
||||
{
|
||||
_hasPreviousSample = false;
|
||||
_previousPoseInWorld = Pose2D.Identity;
|
||||
_previousTimestampSeconds = 0.0;
|
||||
|
||||
_worldVelocityXFilter.Reset();
|
||||
_worldVelocityYFilter.Reset();
|
||||
_angularVelocityFilter.Reset();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查位姿是否由有限数值组成。
|
||||
/// </summary>
|
||||
private static void EnsureFinitePose(
|
||||
Pose2D pose,
|
||||
string parameterName)
|
||||
{
|
||||
if (!IsFinite(pose.XMeters) ||
|
||||
!IsFinite(pose.YMeters) ||
|
||||
!IsFinite(pose.YawRadians))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"速度估计使用的车辆位姿必须由有限数值组成。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查数值是否为非负有限值。
|
||||
/// </summary>
|
||||
private static void EnsureFiniteNonNegative(
|
||||
double value,
|
||||
string parameterName)
|
||||
{
|
||||
if (!IsFinite(value) || value < 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"速度估计使用的采样时刻必须是非负有限值。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 判断数值是否可用于速度估计。
|
||||
/// </summary>
|
||||
private static bool IsFinite(double value)
|
||||
{
|
||||
return !double.IsNaN(value) &&
|
||||
!double.IsInfinity(value);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1 @@
|
||||
// 兼容现有 MDCS 的 AbstractTrack
|
||||
@@ -0,0 +1,172 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Collections.ObjectModel;
|
||||
using System.Linq;
|
||||
|
||||
namespace MultiWheelC.Trajectory
|
||||
{
|
||||
/// <summary>
|
||||
/// 保存一条经过基本合法性检查的只读二维参考轨迹。
|
||||
/// </summary>
|
||||
public sealed class Trajectory2D
|
||||
{
|
||||
private const double StartArcLengthToleranceMeters = 1e-9;
|
||||
private const double MinimumSegmentLengthMeters = 1e-6;
|
||||
|
||||
private readonly TrajectoryPoint[] _points;
|
||||
private readonly ReadOnlyCollection<TrajectoryPoint> _readOnlyPoints;
|
||||
|
||||
/// <summary>
|
||||
/// 复制并验证按累计弧长升序排列的参考轨迹点。
|
||||
/// </summary>
|
||||
public Trajectory2D(
|
||||
IEnumerable<TrajectoryPoint> points)
|
||||
{
|
||||
if (points == null)
|
||||
{
|
||||
throw new ArgumentNullException(
|
||||
nameof(points));
|
||||
}
|
||||
|
||||
_points = points.ToArray();
|
||||
|
||||
if (_points.Length < 2)
|
||||
{
|
||||
throw new ArgumentException(
|
||||
"二维轨迹至少需要两个轨迹点。",
|
||||
nameof(points));
|
||||
}
|
||||
|
||||
if (Math.Abs(_points[0].ArcLengthMeters) >
|
||||
StartArcLengthToleranceMeters)
|
||||
{
|
||||
throw new ArgumentException(
|
||||
"二维轨迹起点的累计弧长必须为0m。",
|
||||
nameof(points));
|
||||
}
|
||||
|
||||
for (var index = 1;
|
||||
index < _points.Length;
|
||||
index++)
|
||||
{
|
||||
ValidateSegment(
|
||||
_points[index - 1],
|
||||
_points[index],
|
||||
index,
|
||||
nameof(points));
|
||||
}
|
||||
|
||||
_readOnlyPoints =
|
||||
Array.AsReadOnly(_points);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取轨迹点数量。
|
||||
/// </summary>
|
||||
public int Count => _points.Length;
|
||||
|
||||
/// <summary>
|
||||
/// 获取指定索引处的轨迹点。
|
||||
/// </summary>
|
||||
public TrajectoryPoint this[int index] =>
|
||||
_points[index];
|
||||
|
||||
/// <summary>
|
||||
/// 获取不可修改的有序轨迹点集合。
|
||||
/// </summary>
|
||||
public IReadOnlyList<TrajectoryPoint> Points =>
|
||||
_readOnlyPoints;
|
||||
|
||||
/// <summary>
|
||||
/// 获取轨迹起点。
|
||||
/// </summary>
|
||||
public TrajectoryPoint StartPoint =>
|
||||
_points[0];
|
||||
|
||||
/// <summary>
|
||||
/// 获取轨迹终点。
|
||||
/// </summary>
|
||||
public TrajectoryPoint EndPoint =>
|
||||
_points[_points.Length - 1];
|
||||
|
||||
/// <summary>
|
||||
/// 获取轨迹总弧长，单位为m。
|
||||
/// </summary>
|
||||
public double TotalLengthMeters =>
|
||||
EndPoint.ArcLengthMeters;
|
||||
|
||||
/// <summary>
|
||||
/// 根据当前累计弧长计算到轨迹终点的剩余距离。
|
||||
/// </summary>
|
||||
public double GetRemainingDistanceMeters(
|
||||
double arcLengthMeters)
|
||||
{
|
||||
EnsureFinite(
|
||||
arcLengthMeters,
|
||||
nameof(arcLengthMeters));
|
||||
|
||||
if (arcLengthMeters <= 0.0)
|
||||
return TotalLengthMeters;
|
||||
|
||||
if (arcLengthMeters >= TotalLengthMeters)
|
||||
return 0.0;
|
||||
|
||||
return TotalLengthMeters - arcLengthMeters;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查相邻轨迹点是否构成有效的非零长度有序线段。
|
||||
/// </summary>
|
||||
private static void ValidateSegment(
|
||||
TrajectoryPoint previous,
|
||||
TrajectoryPoint current,
|
||||
int currentIndex,
|
||||
string parameterName)
|
||||
{
|
||||
if (current.ArcLengthMeters <=
|
||||
previous.ArcLengthMeters)
|
||||
{
|
||||
throw new ArgumentException(
|
||||
$"轨迹点{currentIndex}的累计弧长必须严格大于前一个点。",
|
||||
parameterName);
|
||||
}
|
||||
|
||||
var deltaX =
|
||||
current.PoseInWorld.XMeters -
|
||||
previous.PoseInWorld.XMeters;
|
||||
var deltaY =
|
||||
current.PoseInWorld.YMeters -
|
||||
previous.PoseInWorld.YMeters;
|
||||
var segmentLengthSquared =
|
||||
deltaX * deltaX +
|
||||
deltaY * deltaY;
|
||||
var minimumLengthSquared =
|
||||
MinimumSegmentLengthMeters *
|
||||
MinimumSegmentLengthMeters;
|
||||
|
||||
if (segmentLengthSquared <
|
||||
minimumLengthSquared)
|
||||
{
|
||||
throw new ArgumentException(
|
||||
$"轨迹点{currentIndex}与前一个点的位置过近，无法构成有效投影线段。",
|
||||
parameterName);
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查数值是否为有限值。
|
||||
/// </summary>
|
||||
private static void EnsureFinite(
|
||||
double value,
|
||||
string parameterName)
|
||||
{
|
||||
if (double.IsNaN(value) ||
|
||||
double.IsInfinity(value))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"轨迹弧长必须是有限值。");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,102 @@
|
||||
using System;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.Trajectory
|
||||
{
|
||||
/// <summary>
|
||||
/// 描述按弧长参数化的车体中心参考轨迹点，统一使用SI单位。
|
||||
/// </summary>
|
||||
public readonly struct TrajectoryPoint
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建包含中心位姿、曲率和速度信息的参考轨迹点。
|
||||
/// </summary>
|
||||
public TrajectoryPoint(
|
||||
double arcLengthMeters,
|
||||
Pose2D poseInWorld,
|
||||
double curvaturePerMeter,
|
||||
double referenceSpeedMetersPerSecond)
|
||||
{
|
||||
EnsureFiniteNonNegative(
|
||||
arcLengthMeters,
|
||||
nameof(arcLengthMeters));
|
||||
EnsureFinite(
|
||||
poseInWorld.XMeters,
|
||||
nameof(poseInWorld));
|
||||
EnsureFinite(
|
||||
poseInWorld.YMeters,
|
||||
nameof(poseInWorld));
|
||||
EnsureFinite(
|
||||
poseInWorld.YawRadians,
|
||||
nameof(poseInWorld));
|
||||
EnsureFinite(
|
||||
curvaturePerMeter,
|
||||
nameof(curvaturePerMeter));
|
||||
EnsureFinite(
|
||||
referenceSpeedMetersPerSecond,
|
||||
nameof(referenceSpeedMetersPerSecond));
|
||||
ArcLengthMeters = arcLengthMeters;
|
||||
PoseInWorld = new Pose2D(
|
||||
poseInWorld.XMeters,
|
||||
poseInWorld.YMeters,
|
||||
AngleMath.NormalizeRadians(
|
||||
poseInWorld.YawRadians));
|
||||
CurvaturePerMeter = curvaturePerMeter;
|
||||
ReferenceSpeedMetersPerSecond =
|
||||
referenceSpeedMetersPerSecond;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取从轨迹起点累计到当前点的弧长，单位为m。
|
||||
/// </summary>
|
||||
public double ArcLengthMeters { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取车体中心参考坐标系在世界坐标系中的位姿。
|
||||
/// </summary>
|
||||
public Pose2D PoseInWorld { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取车体中心参考轨迹曲率，单位为1/m，左转为正。
|
||||
/// </summary>
|
||||
public double CurvaturePerMeter { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取沿轨迹切线方向的有符号参考速度，单位为m/s。
|
||||
/// </summary>
|
||||
public double ReferenceSpeedMetersPerSecond { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 检查数值是否为有限值。
|
||||
/// </summary>
|
||||
private static void EnsureFinite(
|
||||
double value,
|
||||
string parameterName)
|
||||
{
|
||||
if (double.IsNaN(value) ||
|
||||
double.IsInfinity(value))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"轨迹点参数必须是有限值。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查数值是否为非负有限值。
|
||||
/// </summary>
|
||||
private static void EnsureFiniteNonNegative(
|
||||
double value,
|
||||
string parameterName)
|
||||
{
|
||||
EnsureFinite(value, parameterName);
|
||||
|
||||
if (value < 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"轨迹累计弧长不能为负数。");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,123 @@
|
||||
using System;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.Trajectory
|
||||
{
|
||||
/// <summary>
|
||||
/// 保存车体中心投影到二维参考轨迹后得到的只读结果。
|
||||
/// </summary>
|
||||
public readonly struct TrajectoryProjection
|
||||
{
|
||||
/// <summary>
|
||||
/// 创建包含轨迹进度、参考状态和跟踪误差的投影结果。
|
||||
/// </summary>
|
||||
public TrajectoryProjection(
|
||||
int segmentStartIndex,
|
||||
TrajectoryPoint referencePoint,
|
||||
double lateralErrorMeters,
|
||||
double headingErrorRadians,
|
||||
double distanceToTrajectoryMeters,
|
||||
double remainingDistanceMeters)
|
||||
{
|
||||
if (segmentStartIndex < 0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
nameof(segmentStartIndex),
|
||||
"投影线段起点索引不能为负数。");
|
||||
}
|
||||
|
||||
EnsureFinite(
|
||||
lateralErrorMeters,
|
||||
nameof(lateralErrorMeters));
|
||||
EnsureFinite(
|
||||
headingErrorRadians,
|
||||
nameof(headingErrorRadians));
|
||||
EnsureFiniteNonNegative(
|
||||
distanceToTrajectoryMeters,
|
||||
nameof(distanceToTrajectoryMeters));
|
||||
EnsureFiniteNonNegative(
|
||||
remainingDistanceMeters,
|
||||
nameof(remainingDistanceMeters));
|
||||
|
||||
SegmentStartIndex = segmentStartIndex;
|
||||
ReferencePoint = referencePoint;
|
||||
LateralErrorMeters = lateralErrorMeters;
|
||||
HeadingErrorRadians =
|
||||
AngleMath.NormalizeRadians(
|
||||
headingErrorRadians);
|
||||
DistanceToTrajectoryMeters =
|
||||
distanceToTrajectoryMeters;
|
||||
RemainingDistanceMeters =
|
||||
remainingDistanceMeters;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 获取投影所在轨迹线段的起点索引，线段终点索引为该值加1。
|
||||
/// </summary>
|
||||
public int SegmentStartIndex { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取投影位置插值得到的车体中心参考轨迹点。
|
||||
/// </summary>
|
||||
public TrajectoryPoint ReferencePoint { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取有符号横向误差，单位为m，参考轨迹位于车辆左侧时为正。
|
||||
/// </summary>
|
||||
public double LateralErrorMeters { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取参考航向减实际车体航向的最短角差，单位为rad，逆时针为正。
|
||||
/// </summary>
|
||||
public double HeadingErrorRadians { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取车体中心到投影点的欧氏距离，单位为m。
|
||||
/// </summary>
|
||||
public double DistanceToTrajectoryMeters { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取投影位置沿轨迹到终点的剩余弧长，单位为m。
|
||||
/// </summary>
|
||||
public double RemainingDistanceMeters { get; }
|
||||
|
||||
/// <summary>
|
||||
/// 获取投影位置从轨迹起点累计的弧长，单位为m。
|
||||
/// </summary>
|
||||
public double ArcLengthMeters =>
|
||||
ReferencePoint.ArcLengthMeters;
|
||||
|
||||
/// <summary>
|
||||
/// 检查数值是否为有限值。
|
||||
/// </summary>
|
||||
private static void EnsureFinite(
|
||||
double value,
|
||||
string parameterName)
|
||||
{
|
||||
if (double.IsNaN(value) ||
|
||||
double.IsInfinity(value))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"轨迹投影参数必须是有限值。");
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查数值是否为非负有限值。
|
||||
/// </summary>
|
||||
private static void EnsureFiniteNonNegative(
|
||||
double value,
|
||||
string parameterName)
|
||||
{
|
||||
EnsureFinite(value, parameterName);
|
||||
|
||||
if (value < 0.0)
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"轨迹投影距离不能为负数。");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,219 @@
|
||||
using System;
|
||||
using MyParking.Shared;
|
||||
|
||||
namespace MultiWheelC.Trajectory
|
||||
{
|
||||
/// <summary>
|
||||
/// 将Detour给出的实际车体中心位姿投影到二维离散参考轨迹。
|
||||
/// </summary>
|
||||
public static class TrajectoryProjector
|
||||
{
|
||||
/// <summary>
|
||||
/// 在整条轨迹上查找距离实际车体中心最近的线段投影结果。
|
||||
/// </summary>
|
||||
public static TrajectoryProjection Project(
|
||||
Trajectory2D trajectory,
|
||||
Pose2D vehiclePoseInWorld)
|
||||
{
|
||||
if (trajectory == null)
|
||||
{
|
||||
throw new ArgumentNullException(
|
||||
nameof(trajectory));
|
||||
}
|
||||
|
||||
EnsureFinitePose(
|
||||
vehiclePoseInWorld,
|
||||
nameof(vehiclePoseInWorld));
|
||||
|
||||
var bestSegmentStartIndex = 0;
|
||||
var bestInterpolationRatio = 0.0;
|
||||
var bestProjectedX = 0.0;
|
||||
var bestProjectedY = 0.0;
|
||||
var bestDistanceSquared =
|
||||
double.PositiveInfinity;
|
||||
|
||||
for (var segmentStartIndex = 0;
|
||||
segmentStartIndex < trajectory.Count - 1;
|
||||
segmentStartIndex++)
|
||||
{
|
||||
var segmentStart =
|
||||
trajectory[segmentStartIndex];
|
||||
var segmentEnd =
|
||||
trajectory[segmentStartIndex + 1];
|
||||
|
||||
var segmentX =
|
||||
segmentEnd.PoseInWorld.XMeters -
|
||||
segmentStart.PoseInWorld.XMeters;
|
||||
var segmentY =
|
||||
segmentEnd.PoseInWorld.YMeters -
|
||||
segmentStart.PoseInWorld.YMeters;
|
||||
var segmentLengthSquared =
|
||||
segmentX * segmentX +
|
||||
segmentY * segmentY;
|
||||
|
||||
var vehicleFromSegmentStartX =
|
||||
vehiclePoseInWorld.XMeters -
|
||||
segmentStart.PoseInWorld.XMeters;
|
||||
var vehicleFromSegmentStartY =
|
||||
vehiclePoseInWorld.YMeters -
|
||||
segmentStart.PoseInWorld.YMeters;
|
||||
|
||||
var interpolationRatio =
|
||||
InterpolationMath.Clamp01(
|
||||
(vehicleFromSegmentStartX * segmentX +
|
||||
vehicleFromSegmentStartY * segmentY) /
|
||||
segmentLengthSquared);
|
||||
|
||||
var projectedX =
|
||||
InterpolationMath.Lerp(
|
||||
segmentStart.PoseInWorld.XMeters,
|
||||
segmentEnd.PoseInWorld.XMeters,
|
||||
interpolationRatio);
|
||||
var projectedY =
|
||||
InterpolationMath.Lerp(
|
||||
segmentStart.PoseInWorld.YMeters,
|
||||
segmentEnd.PoseInWorld.YMeters,
|
||||
interpolationRatio);
|
||||
|
||||
var projectionErrorX =
|
||||
projectedX -
|
||||
vehiclePoseInWorld.XMeters;
|
||||
var projectionErrorY =
|
||||
projectedY -
|
||||
vehiclePoseInWorld.YMeters;
|
||||
var distanceSquared =
|
||||
projectionErrorX * projectionErrorX +
|
||||
projectionErrorY * projectionErrorY;
|
||||
|
||||
if (distanceSquared >= bestDistanceSquared)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
bestSegmentStartIndex =
|
||||
segmentStartIndex;
|
||||
bestInterpolationRatio =
|
||||
interpolationRatio;
|
||||
bestProjectedX = projectedX;
|
||||
bestProjectedY = projectedY;
|
||||
bestDistanceSquared = distanceSquared;
|
||||
}
|
||||
|
||||
return BuildProjection(
|
||||
trajectory,
|
||||
vehiclePoseInWorld,
|
||||
bestSegmentStartIndex,
|
||||
bestInterpolationRatio,
|
||||
bestProjectedX,
|
||||
bestProjectedY,
|
||||
bestDistanceSquared);
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 根据最近线段和插值比例生成控制器使用的完整投影结果。
|
||||
/// </summary>
|
||||
private static TrajectoryProjection BuildProjection(
|
||||
Trajectory2D trajectory,
|
||||
Pose2D vehiclePoseInWorld,
|
||||
int segmentStartIndex,
|
||||
double interpolationRatio,
|
||||
double projectedX,
|
||||
double projectedY,
|
||||
double distanceSquared)
|
||||
{
|
||||
var segmentStart =
|
||||
trajectory[segmentStartIndex];
|
||||
var segmentEnd =
|
||||
trajectory[segmentStartIndex + 1];
|
||||
|
||||
var referenceYawRadians =
|
||||
AngleMath.LerpRadians(
|
||||
segmentStart.PoseInWorld.YawRadians,
|
||||
segmentEnd.PoseInWorld.YawRadians,
|
||||
interpolationRatio);
|
||||
var referenceArcLengthMeters =
|
||||
InterpolationMath.Lerp(
|
||||
segmentStart.ArcLengthMeters,
|
||||
segmentEnd.ArcLengthMeters,
|
||||
interpolationRatio);
|
||||
var referenceCurvaturePerMeter =
|
||||
InterpolationMath.Lerp(
|
||||
segmentStart.CurvaturePerMeter,
|
||||
segmentEnd.CurvaturePerMeter,
|
||||
interpolationRatio);
|
||||
var referenceSpeedMetersPerSecond =
|
||||
InterpolationMath.Lerp(
|
||||
segmentStart.ReferenceSpeedMetersPerSecond,
|
||||
segmentEnd.ReferenceSpeedMetersPerSecond,
|
||||
interpolationRatio);
|
||||
|
||||
var referencePoint =
|
||||
new TrajectoryPoint(
|
||||
referenceArcLengthMeters,
|
||||
new Pose2D(
|
||||
projectedX,
|
||||
projectedY,
|
||||
referenceYawRadians),
|
||||
referenceCurvaturePerMeter,
|
||||
referenceSpeedMetersPerSecond);
|
||||
|
||||
var segmentX =
|
||||
segmentEnd.PoseInWorld.XMeters -
|
||||
segmentStart.PoseInWorld.XMeters;
|
||||
var segmentY =
|
||||
segmentEnd.PoseInWorld.YMeters -
|
||||
segmentStart.PoseInWorld.YMeters;
|
||||
var segmentLength =
|
||||
Math.Sqrt(
|
||||
segmentX * segmentX +
|
||||
segmentY * segmentY);
|
||||
|
||||
// 以轨迹线段的前进方向判断左右：
|
||||
// 从车辆指向参考轨迹的向量位于轨迹左侧时为正。
|
||||
var vehicleToProjectionX =
|
||||
projectedX -
|
||||
vehiclePoseInWorld.XMeters;
|
||||
var vehicleToProjectionY =
|
||||
projectedY -
|
||||
vehiclePoseInWorld.YMeters;
|
||||
var lateralErrorMeters =
|
||||
(segmentX * vehicleToProjectionY -
|
||||
segmentY * vehicleToProjectionX) /
|
||||
segmentLength;
|
||||
|
||||
var headingErrorRadians =
|
||||
AngleMath.ShortestDifferenceRadians(
|
||||
referenceYawRadians,
|
||||
vehiclePoseInWorld.YawRadians);
|
||||
|
||||
return new TrajectoryProjection(
|
||||
segmentStartIndex,
|
||||
referencePoint,
|
||||
lateralErrorMeters,
|
||||
headingErrorRadians,
|
||||
Math.Sqrt(distanceSquared),
|
||||
trajectory.GetRemainingDistanceMeters(
|
||||
referenceArcLengthMeters));
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 检查用于投影的实际车体中心位姿是否包含有限数值。
|
||||
/// </summary>
|
||||
private static void EnsureFinitePose(
|
||||
Pose2D pose,
|
||||
string parameterName)
|
||||
{
|
||||
if (double.IsNaN(pose.XMeters) ||
|
||||
double.IsInfinity(pose.XMeters) ||
|
||||
double.IsNaN(pose.YMeters) ||
|
||||
double.IsInfinity(pose.YMeters) ||
|
||||
double.IsNaN(pose.YawRadians) ||
|
||||
double.IsInfinity(pose.YawRadians))
|
||||
{
|
||||
throw new ArgumentOutOfRangeException(
|
||||
parameterName,
|
||||
"用于轨迹投影的车体位姿必须是有限值。");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user