添加单车底盘仿真平台并完善运动控制与夹臂功能
This commit is contained in:
@@ -15,6 +15,11 @@
|
|||||||
<PackageReference Include="System.Numerics.Vectors" Version="4.6.1" />
|
<PackageReference Include="System.Numerics.Vectors" Version="4.6.1" />
|
||||||
</ItemGroup>
|
</ItemGroup>
|
||||||
|
|
||||||
|
<ItemGroup>
|
||||||
|
<Compile Include="..\Shared\*.cs"
|
||||||
|
Link="Shared\%(Filename)%(Extension)" />
|
||||||
|
</ItemGroup>
|
||||||
|
|
||||||
<ItemGroup>
|
<ItemGroup>
|
||||||
<Reference Include="CommonUsage">
|
<Reference Include="CommonUsage">
|
||||||
<HintPath>ref\CommonUsage.dll</HintPath>
|
<HintPath>ref\CommonUsage.dll</HintPath>
|
||||||
|
|||||||
@@ -72,30 +72,55 @@ namespace MultiWheelC
|
|||||||
[MovementTest(name = "底盘旋转测试")]
|
[MovementTest(name = "底盘旋转测试")]
|
||||||
public class RotateToAngleTest : MovementTest
|
public class RotateToAngleTest : MovementTest
|
||||||
{
|
{
|
||||||
// 底盘旋转测试不支持停止操作。
|
private DriveTask _dt;
|
||||||
|
|
||||||
|
// 停止当前正在执行的底盘原地旋转任务。
|
||||||
public override void TestStop()
|
public override void TestStop()
|
||||||
{
|
{
|
||||||
throw new NotImplementedException();
|
_dt?.Stop();
|
||||||
}
|
}
|
||||||
|
|
||||||
// 交互输入目标角度后执行底盘原地旋转测试。
|
// 交互输入目标角度后执行底盘原地旋转测试。
|
||||||
public override void Test()
|
public override void Test()
|
||||||
{
|
{
|
||||||
var target = UI.GetInput("输入旋转角度:");
|
var input = UI.GetInput("输入旋转角度:");
|
||||||
new DriveTask(new MultiWheelRotateInPlace()
|
if (!float.TryParse(input, out var angleTarget))
|
||||||
{
|
{
|
||||||
AngleTarget = float.Parse(target),
|
Console.WriteLine(
|
||||||
PidparamsRead = () => new PIDParams()
|
$"旋转测试输入无效:{input}");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
// 防止重复启动测试时,上一项旋转任务仍在运行。
|
||||||
|
_dt?.Stop();
|
||||||
|
var task = new DriveTask(
|
||||||
|
new MultiWheelRotateInPlace
|
||||||
{
|
{
|
||||||
Kp = PilotDefinition.Conf.InPlaceRotateKp,
|
AngleTarget = angleTarget,
|
||||||
Ki = PilotDefinition.Conf.InPlaceRotateKi,
|
|
||||||
Kd = PilotDefinition.Conf.InPlaceRotateKd,
|
PidparamsRead = () => new PIDParams
|
||||||
DeadZone = PilotDefinition.Conf.InPlaceRotateArriveDeg,
|
{
|
||||||
SpeedAccPerSec = PilotDefinition.Conf.InPlaceRotateAcc,
|
Kp = PilotDefinition.Conf.InPlaceRotateKp,
|
||||||
OutputUpperThreshold = PilotDefinition.Conf.InPlaceRotateMaxSpeed,
|
Ki = PilotDefinition.Conf.InPlaceRotateKi,
|
||||||
MaxI = PilotDefinition.Conf.InPlaceRotateMaxI,
|
Kd = PilotDefinition.Conf.InPlaceRotateKd,
|
||||||
|
DeadZone = PilotDefinition.Conf.InPlaceRotateArriveDeg,
|
||||||
|
SpeedAccPerSec = PilotDefinition.Conf.InPlaceRotateAcc,
|
||||||
|
OutputUpperThreshold = PilotDefinition.Conf.InPlaceRotateMaxSpeed,
|
||||||
|
MaxI = PilotDefinition.Conf.InPlaceRotateMaxI,
|
||||||
|
}
|
||||||
|
}.Get());
|
||||||
|
_dt = task;
|
||||||
|
try
|
||||||
|
{
|
||||||
|
task.Wait();
|
||||||
|
}
|
||||||
|
finally
|
||||||
|
{
|
||||||
|
// 防止旧任务结束时,错误清除后来启动的新任务。
|
||||||
|
if (ReferenceEquals(_dt, task))
|
||||||
|
{
|
||||||
|
_dt = null;
|
||||||
}
|
}
|
||||||
}.Get()).Wait();
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -124,4 +124,5 @@ namespace MultiWheelC
|
|||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -60,7 +60,7 @@ public class PilotConfig : MultiWheelPilotConfig
|
|||||||
#endregion
|
#endregion
|
||||||
|
|
||||||
|
|
||||||
#if false
|
|
||||||
|
|
||||||
#region 单车-钻车与夹抱
|
#region 单车-钻车与夹抱
|
||||||
[FieldMember(desc = "2腿检测:雷达名(逗号分隔可多个)")]
|
[FieldMember(desc = "2腿检测:雷达名(逗号分隔可多个)")]
|
||||||
@@ -165,10 +165,8 @@ public class PilotConfig : MultiWheelPilotConfig
|
|||||||
[FieldMember(desc = "抱夹控制pid:Thresh")] public float ClampControlThresh = 0.2f;
|
[FieldMember(desc = "抱夹控制pid:Thresh")] public float ClampControlThresh = 0.2f;
|
||||||
[FieldMember(desc = "抱夹控制pid:DeadZone")] public float ClampControlDeadZone = 5f;
|
[FieldMember(desc = "抱夹控制pid:DeadZone")] public float ClampControlDeadZone = 5f;
|
||||||
[FieldMember(desc = "抱夹最大速度")] public float MaxClampSpeed = 1.5f;
|
[FieldMember(desc = "抱夹最大速度")] public float MaxClampSpeed = 1.5f;
|
||||||
|
|
||||||
|
|
||||||
#endregion
|
#endregion
|
||||||
|
#if false
|
||||||
#region 多车-编队与遥控
|
#region 多车-编队与遥控
|
||||||
[FieldMember(desc = "联动时转向角爬升加速度")] public float SyncThAccPerSec = 30f;
|
[FieldMember(desc = "联动时转向角爬升加速度")] public float SyncThAccPerSec = 30f;
|
||||||
[FieldMember(desc = "两车间距 (mm)")] public float TestCarSyncDistance = 2400f;
|
[FieldMember(desc = "两车间距 (mm)")] public float TestCarSyncDistance = 2400f;
|
||||||
@@ -330,24 +328,6 @@ public class PilotConfig : MultiWheelPilotConfig
|
|||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#region 仿真-Playground
|
|
||||||
[FieldMember(desc = "Playground WebAPI 基地址")]
|
|
||||||
public string PlaygroundWebApiUrl = "http://localhost:18090";
|
|
||||||
|
|
||||||
[FieldMember(desc = "Playground 小车名称(场景 robots[].name)")]
|
|
||||||
public string PlaygroundRobotName = "agv_multi_1";
|
|
||||||
|
|
||||||
[FieldMember(desc = "Playground 邻车名称(仅主车用于原地旋转位姿诊断)")]
|
|
||||||
public string PlaygroundNeighborRobotName = "agv_multi_2";
|
|
||||||
|
|
||||||
[FieldMember(desc = "旋转位姿诊断开关(simulation only)")]
|
|
||||||
public bool MultiVehicleRotatePoseWebApiDiagEnabled = false;
|
|
||||||
[FieldMember(desc = "WebAPI 平移测试:平移距离(mm)")]
|
|
||||||
public float WebApiTranslateMm = 100f;
|
|
||||||
|
|
||||||
[FieldMember(desc = "WebAPI 旋转测试:旋转角度(deg)")]
|
|
||||||
public float WebApiRotateDeg = 5f;
|
|
||||||
#endregion
|
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -11,4 +11,34 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
|||||||
public new float CarWidth = 948f;
|
public new float CarWidth = 948f;
|
||||||
[AsLowerIO(desc = "车号")] public int CarNum = 1;
|
[AsLowerIO(desc = "车号")] public int CarNum = 1;
|
||||||
public override void StandardInit() { }
|
public override void StandardInit() { }
|
||||||
|
|
||||||
|
#region 夹臂变量
|
||||||
|
[AsUpperIO(desc = "左夹臂下发速度")] public float SpeedLeftArm;
|
||||||
|
|
||||||
|
[AsUpperIO(desc = "右夹臂下发速度")] public float SpeedRightArm;
|
||||||
|
|
||||||
|
[AsLowerIO(desc = "左夹臂实际位置")] public float ActualPosLeftArm;
|
||||||
|
|
||||||
|
[AsLowerIO(desc = "右夹臂实际位置")] public float ActualPosRightArm;
|
||||||
|
|
||||||
|
[AsUpperIO(desc = "夹臂不同步报警")] public bool ClampOutOfSync = false;
|
||||||
|
#endregion
|
||||||
|
|
||||||
|
[AsLowerIO(desc = "左前左轮实际位置")] public float LFLActualPos;
|
||||||
|
[AsLowerIO(desc = "左前右轮实际位置")] public float LFRActualPos;
|
||||||
|
[AsLowerIO(desc = "右前左轮实际位置")] public float RFLActualPos;
|
||||||
|
[AsLowerIO(desc = "右前右轮实际位置")] public float RFRActualPos;
|
||||||
|
[AsLowerIO(desc = "左后左轮实际位置")] public float LRLActualPos;
|
||||||
|
[AsLowerIO(desc = "左后右轮实际位置")] public float LRRActualPos;
|
||||||
|
[AsLowerIO(desc = "右后左轮实际位置")] public float RRLActualPos;
|
||||||
|
[AsLowerIO(desc = "右后右轮实际位置")] public float RRRActualPos;
|
||||||
|
|
||||||
|
[AsLowerIO(desc = "左夹臂低限位")] public float LeftArmLowerPos;
|
||||||
|
[AsLowerIO(desc = "左夹臂高限位")] public float LeftArmUpperPos;
|
||||||
|
[AsLowerIO(desc = "右夹臂低限位")] public float RightArmLowerPos;
|
||||||
|
[AsLowerIO(desc = "右夹臂高限位")] public float RightArmUpperPos;
|
||||||
|
|
||||||
|
[AsUpperIO(desc = "从C往驱动器下使能")] public bool DisableFromC = false;
|
||||||
|
[AsUpperIO(desc = "从C上复位")] public bool ResetFromC = false;
|
||||||
|
[AsLowerIO(desc = "驱动轮使能状态")] public bool WheelAbleState = true;
|
||||||
}
|
}
|
||||||
|
|||||||
Binary file not shown.
Binary file not shown.
@@ -13,7 +13,7 @@ using System.Reflection;
|
|||||||
[assembly: System.Reflection.AssemblyCompanyAttribute("ClumsyPilot")]
|
[assembly: System.Reflection.AssemblyCompanyAttribute("ClumsyPilot")]
|
||||||
[assembly: System.Reflection.AssemblyConfigurationAttribute("Debug")]
|
[assembly: System.Reflection.AssemblyConfigurationAttribute("Debug")]
|
||||||
[assembly: System.Reflection.AssemblyFileVersionAttribute("1.0.0.0")]
|
[assembly: System.Reflection.AssemblyFileVersionAttribute("1.0.0.0")]
|
||||||
[assembly: System.Reflection.AssemblyInformationalVersionAttribute("1.0.0")]
|
[assembly: System.Reflection.AssemblyInformationalVersionAttribute("1.0.0+580a936a830dcb7a25ef327cf553341405264033")]
|
||||||
[assembly: System.Reflection.AssemblyProductAttribute("ClumsyPilot")]
|
[assembly: System.Reflection.AssemblyProductAttribute("ClumsyPilot")]
|
||||||
[assembly: System.Reflection.AssemblyTitleAttribute("ClumsyPilot")]
|
[assembly: System.Reflection.AssemblyTitleAttribute("ClumsyPilot")]
|
||||||
[assembly: System.Reflection.AssemblyVersionAttribute("1.0.0.0")]
|
[assembly: System.Reflection.AssemblyVersionAttribute("1.0.0.0")]
|
||||||
|
|||||||
@@ -1 +1 @@
|
|||||||
5d77794fa0720c6591db5b06ac60427413c18989a6f7b64420ccb07d122d85bc
|
038d57c5b1714403d308c9343b385ef762951607b63a9fb275f4d7d2b8fd29cb
|
||||||
|
|||||||
@@ -1,6 +1,6 @@
|
|||||||
is_global = true
|
is_global = true
|
||||||
build_property.RootNamespace = MultiWheelC
|
build_property.RootNamespace = MultiWheelC
|
||||||
build_property.ProjectDir = d:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\
|
build_property.ProjectDir = d:\MyParking\ClumsyPilot\
|
||||||
build_property.EnableComHosting =
|
build_property.EnableComHosting =
|
||||||
build_property.EnableGeneratedComInterfaceComImportInterop =
|
build_property.EnableGeneratedComInterfaceComImportInterop =
|
||||||
build_property.CsWinRTUseWindowsUIXamlProjections = false
|
build_property.CsWinRTUseWindowsUIXamlProjections = false
|
||||||
|
|||||||
Binary file not shown.
Binary file not shown.
@@ -1 +1 @@
|
|||||||
cbba0ac806b0617cc6f7209f71f12565d8a56471fa82abdd118fca6c967d6a5a
|
3920568398d269aea7b71fb4581ad111777c15ca3f2f2bb272ee9e0a989215c4
|
||||||
|
|||||||
Binary file not shown.
Binary file not shown.
@@ -8,7 +8,8 @@ namespace MedullaAdapter
|
|||||||
// M层单车安全:汇总停车机器人夹臂等自定义报警状态。
|
// M层单车安全:汇总停车机器人夹臂等自定义报警状态。
|
||||||
public override void SetOtherAlarms()
|
public override void SetOtherAlarms()
|
||||||
{
|
{
|
||||||
// 当前阶段不使用夹臂,暂时没有自定义报警。
|
AddAlarm("左夹臂驱动报警", 2, () => cart.LeftArmErrorCode != 0);
|
||||||
|
AddAlarm("右夹臂驱动报警", 2, () => cart.RightArmErrorCode != 0);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
@@ -6,6 +6,7 @@ using Medulla.Types;
|
|||||||
using System;
|
using System;
|
||||||
using System.Collections.Generic;
|
using System.Collections.Generic;
|
||||||
using System.Threading;
|
using System.Threading;
|
||||||
|
using MyParking.Shared;
|
||||||
|
|
||||||
namespace MedullaAdapter
|
namespace MedullaAdapter
|
||||||
{
|
{
|
||||||
@@ -26,11 +27,16 @@ namespace MedullaAdapter
|
|||||||
}
|
}
|
||||||
internal ManualControlMode TransmitterControlMode = ManualControlMode.Normal;
|
internal ManualControlMode TransmitterControlMode = ManualControlMode.Normal;
|
||||||
internal DateTime TransmitterLastTime = DateTime.Now; // 物理遥控器计算两次实体遥控器指令之间的时间间隔
|
internal DateTime TransmitterLastTime = DateTime.Now; // 物理遥控器计算两次实体遥控器指令之间的时间间隔
|
||||||
|
private ManualControlMode? _pendingManualMode;
|
||||||
|
private ManualControlMode? _activeManualMode;
|
||||||
#endregion
|
#endregion
|
||||||
|
|
||||||
#region AsUpperIO
|
#region AsUpperIO
|
||||||
[AsUpperIO(desc = "从C上复位")] public bool ResetFromC;
|
[AsUpperIO(desc = "从C上复位")] public bool ResetFromC;
|
||||||
[AsUpperIO(desc = "从C将驱动轮下使能")] public bool DisableFromC;
|
[AsUpperIO(desc = "从C将驱动轮下使能")] public bool DisableFromC;
|
||||||
|
[AsUpperIO(desc = "左夹臂下发速度", timeOutReset = true)] public float SpeedLeftArm;
|
||||||
|
[AsUpperIO(desc = "右夹臂下发速度", timeOutReset = true)] public float SpeedRightArm;
|
||||||
|
[AsUpperIO(desc = "夹臂不同步报警")] public bool ClampOutOfSync;
|
||||||
#endregion
|
#endregion
|
||||||
|
|
||||||
#region AsLowerIO
|
#region AsLowerIO
|
||||||
@@ -42,6 +48,16 @@ namespace MedullaAdapter
|
|||||||
[AsLowerIO(desc = "左后右轮实际位置")] public float LRRActualPos;
|
[AsLowerIO(desc = "左后右轮实际位置")] public float LRRActualPos;
|
||||||
[AsLowerIO(desc = "右后左轮实际位置")] public float RRLActualPos;
|
[AsLowerIO(desc = "右后左轮实际位置")] public float RRLActualPos;
|
||||||
[AsLowerIO(desc = "右后右轮实际位置")] public float RRRActualPos;
|
[AsLowerIO(desc = "右后右轮实际位置")] public float RRRActualPos;
|
||||||
|
[AsLowerIO(desc = "左夹臂实际速度")] public float ActualSpeedLeftArm;
|
||||||
|
[AsLowerIO(desc = "右夹臂实际速度")] public float ActualSpeedRightArm;
|
||||||
|
[AsLowerIO(desc = "左夹臂状态字")] public int LeftArmStateCode;
|
||||||
|
[AsLowerIO(desc = "右夹臂状态字")] public int RightArmStateCode;
|
||||||
|
[AsLowerIO(desc = "左夹臂错误字")] public int LeftArmErrorCode;
|
||||||
|
[AsLowerIO(desc = "右夹臂错误字")] public int RightArmErrorCode;
|
||||||
|
[AsLowerIO(desc = "左夹臂电流")] public float LeftArmElectric;
|
||||||
|
[AsLowerIO(desc = "右夹臂电流")] public float RightArmElectric;
|
||||||
|
[AsLowerIO(desc = "左夹臂实际位置")] public float ActualPosLeftArm;
|
||||||
|
[AsLowerIO(desc = "右夹臂实际位置")] public float ActualPosRightArm;
|
||||||
[AsLowerIO(desc = "驱动轮使能状态")] public bool WheelAbleState = true;
|
[AsLowerIO(desc = "驱动轮使能状态")] public bool WheelAbleState = true;
|
||||||
[AsLowerIO(desc = "电池健康状态")] public float SOH;
|
[AsLowerIO(desc = "电池健康状态")] public float SOH;
|
||||||
[AsInitParam(desc = "车号")][AsLowerIO] public int CarNum = 1;
|
[AsInitParam(desc = "车号")][AsLowerIO] public int CarNum = 1;
|
||||||
@@ -51,6 +67,12 @@ namespace MedullaAdapter
|
|||||||
[AsInitParam(desc = "MCU端口号")] public string MCUPort = "COM4";
|
[AsInitParam(desc = "MCU端口号")] public string MCUPort = "COM4";
|
||||||
[AsInitParam(desc = "遥控器速度上限")] public float TransmitterSpeedUpperLimit = 1.0f;
|
[AsInitParam(desc = "遥控器速度上限")] public float TransmitterSpeedUpperLimit = 1.0f;
|
||||||
[AsInitParam(desc = "遥控器速度下限")] public float TransmitterSpeedLowerLimit = 0.0f;
|
[AsInitParam(desc = "遥控器速度下限")] public float TransmitterSpeedLowerLimit = 0.0f;
|
||||||
|
[AsInitParam(desc = "手动控制夹臂速度系数")] public float ManualArmSpeedFac = 1.0f;
|
||||||
|
[AsInitParam(desc = "左夹臂低限位")][AsLowerIO] public int LeftArmLowerPos = -10000;
|
||||||
|
[AsInitParam(desc = "左夹臂高限位")][AsLowerIO] public int LeftArmUpperPos = 5927610;
|
||||||
|
[AsInitParam(desc = "右夹臂低限位")][AsLowerIO] public int RightArmLowerPos = -17295;
|
||||||
|
[AsInitParam(desc = "右夹臂高限位")][AsLowerIO] public int RightArmUpperPos = 5927610;
|
||||||
|
|
||||||
|
|
||||||
#endregion
|
#endregion
|
||||||
|
|
||||||
@@ -75,6 +97,8 @@ namespace MedullaAdapter
|
|||||||
[IOObjectMonitor(desc = "左后右驱动器远程帧706")] public byte LRRRemoteCode = 0;
|
[IOObjectMonitor(desc = "左后右驱动器远程帧706")] public byte LRRRemoteCode = 0;
|
||||||
[IOObjectMonitor(desc = "右后左驱动器远程帧707")] public byte RRLRemoteCode = 0;
|
[IOObjectMonitor(desc = "右后左驱动器远程帧707")] public byte RRLRemoteCode = 0;
|
||||||
[IOObjectMonitor(desc = "右后右驱动器远程帧708")] public byte RRRRemoteCode = 0;
|
[IOObjectMonitor(desc = "右后右驱动器远程帧708")] public byte RRRRemoteCode = 0;
|
||||||
|
[IOObjectMonitor(desc = "左夹臂驱动器远程帧709")] public byte LArmRemoteCode = 0;
|
||||||
|
[IOObjectMonitor(desc = "右夹臂驱动器远程帧70A")] public byte RArmRemoteCode = 0;
|
||||||
#endregion
|
#endregion
|
||||||
|
|
||||||
#region 操作按钮
|
#region 操作按钮
|
||||||
@@ -188,43 +212,193 @@ namespace MedullaAdapter
|
|||||||
TimeSpan? interval = null)
|
TimeSpan? interval = null)
|
||||||
{
|
{
|
||||||
if (Chassis == null) return;
|
if (Chassis == null) return;
|
||||||
|
|
||||||
|
var adapter = GetChassisAdapter();
|
||||||
|
if (adapter == null) return;
|
||||||
|
|
||||||
|
// 模式变化时先停车并下发舵轮准备角度;
|
||||||
|
// 在实际舵角到位之前,不开放驱动速度。
|
||||||
|
if (!EnsureManualModeReady(mode, interval))
|
||||||
|
{
|
||||||
|
adapter.StopImmediately();
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
var speed = speedThreshold * y;
|
var speed = speedThreshold * y;
|
||||||
var thPow = (float)Math.Pow(Math.Abs(x), ManualThetaPow) * Math.Sign(x);
|
var omega = CalculateManualOmega(speed, x);
|
||||||
var frontTh = -thPow * MaxManualTheta;
|
|
||||||
var rearTh = thPow * MaxManualTheta;
|
|
||||||
ManualMode = (int)mode;
|
ManualMode = (int)mode;
|
||||||
|
|
||||||
switch (mode)
|
switch (mode)
|
||||||
{
|
{
|
||||||
case ManualControlMode.Normal:
|
case ManualControlMode.Normal:
|
||||||
SetChassisDirection(frontDirection);
|
SendBodyCommand(vx: speed, vy: 0.0, omegaRadiansPerSecond: omega, interval);
|
||||||
Chassis.SendMotion(speed, frontTh, rearTh, interval);
|
|
||||||
break;
|
break;
|
||||||
case ManualControlMode.Crab:
|
case ManualControlMode.Crab:
|
||||||
SetChassisDirection(90);
|
SendBodyCommand(vx: 0.0, vy: speed, omegaRadiansPerSecond: omega, interval);
|
||||||
Chassis.SendMotion(speed, frontTh, rearTh, interval);
|
|
||||||
break;
|
break;
|
||||||
case ManualControlMode.Spin:
|
case ManualControlMode.Spin:
|
||||||
Chassis.SendRotateMotion(speed * MaxAngularSpeed, interval);
|
var spinOmega =
|
||||||
|
speed * MaxAngularSpeed *
|
||||||
|
Math.PI / 180.0;
|
||||||
|
|
||||||
|
SendBodyCommand(
|
||||||
|
vx: 0.0,
|
||||||
|
vy: 0.0,
|
||||||
|
omegaRadiansPerSecond: spinOmega,
|
||||||
|
interval);
|
||||||
break;
|
break;
|
||||||
default:
|
default:
|
||||||
ManualMode = -1;
|
ManualMode = -1;
|
||||||
Chassis.PredefinedDriveStop();
|
adapter.StopImmediately();
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
private void SetChassisDirection(float direction)
|
// 停车后切换模式:先预转舵轮,实际角度到位后才允许发送运动命令。
|
||||||
|
private bool EnsureManualModeReady(
|
||||||
|
ManualControlMode mode,
|
||||||
|
TimeSpan? interval)
|
||||||
{
|
{
|
||||||
var origin = Chassis.GetOriginBias();
|
var adapter = GetChassisAdapter();
|
||||||
// 方向没有变化时不重复计算轮组几何关系。
|
if (adapter == null)
|
||||||
if (Math.Abs(origin.Z - direction) < 0.001f)
|
return false;
|
||||||
|
|
||||||
|
// 当前模式已经完成准备,可以直接接受运动命令。
|
||||||
|
if (_activeManualMode == mode &&
|
||||||
|
_pendingManualMode == null)
|
||||||
|
{
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 第一次收到新模式时,停车并下发一次舵轮准备姿态。
|
||||||
|
if (_pendingManualMode != mode)
|
||||||
|
{
|
||||||
|
adapter.StopImmediately();
|
||||||
|
|
||||||
|
var preparationAccepted = mode switch
|
||||||
|
{
|
||||||
|
ManualControlMode.Normal =>
|
||||||
|
adapter.PrepareParallelDirection(0.0),
|
||||||
|
|
||||||
|
ManualControlMode.Crab =>
|
||||||
|
adapter.PrepareParallelDirection(
|
||||||
|
Math.PI / 2.0),
|
||||||
|
|
||||||
|
ManualControlMode.Spin =>
|
||||||
|
adapter.PrepareSpin(interval),
|
||||||
|
|
||||||
|
_ => false
|
||||||
|
};
|
||||||
|
|
||||||
|
if (!preparationAccepted)
|
||||||
|
{
|
||||||
|
_pendingManualMode = null;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
_pendingManualMode = mode;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 后续控制周期保持停车,并读取实际舵角判断是否到位。
|
||||||
|
adapter.StopImmediately();
|
||||||
|
|
||||||
|
const double toleranceRadians =
|
||||||
|
2.0 * Math.PI / 180.0;
|
||||||
|
|
||||||
|
bool aligned;
|
||||||
|
|
||||||
|
if (mode == ManualControlMode.Spin)
|
||||||
|
{
|
||||||
|
// 自转的四个舵轮目标角不同,等待期间持续刷新其目标。
|
||||||
|
var preparationAccepted =
|
||||||
|
adapter.PrepareSpin(interval);
|
||||||
|
|
||||||
|
aligned =
|
||||||
|
preparationAccepted &&
|
||||||
|
adapter.AreSpinWheelsAligned;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
var targetDirection = mode ==
|
||||||
|
ManualControlMode.Crab
|
||||||
|
? Math.PI / 2.0
|
||||||
|
: 0.0;
|
||||||
|
|
||||||
|
aligned =
|
||||||
|
adapter.AreParallelWheelsAligned(
|
||||||
|
targetDirection,
|
||||||
|
toleranceRadians);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!aligned)
|
||||||
|
return false;
|
||||||
|
|
||||||
|
_activeManualMode = mode;
|
||||||
|
_pendingManualMode = null;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
private double CalculateManualOmega(
|
||||||
|
float speed,
|
||||||
|
float steeringInput)
|
||||||
|
{
|
||||||
|
var normalizedSteering =
|
||||||
|
(float)Math.Pow(
|
||||||
|
Math.Abs(steeringInput),
|
||||||
|
ManualThetaPow) *
|
||||||
|
Math.Sign(steeringInput);
|
||||||
|
|
||||||
|
var steeringDegrees =
|
||||||
|
-normalizedSteering * MaxManualTheta;
|
||||||
|
|
||||||
|
var steeringRadians =
|
||||||
|
steeringDegrees * Math.PI / 180.0;
|
||||||
|
|
||||||
|
// CommonUsage中的ControlPointRadius单位为毫米。
|
||||||
|
var halfWheelBaseMeters =
|
||||||
|
Math.Max(
|
||||||
|
Chassis.ControlPointRadius / 1000.0,
|
||||||
|
0.01);
|
||||||
|
|
||||||
|
return speed * Math.Tan(steeringRadians) / halfWheelBaseMeters;
|
||||||
|
}
|
||||||
|
|
||||||
|
internal void SendBodyCommand(double vx, double vy, double omegaRadiansPerSecond, TimeSpan? interval = null)
|
||||||
|
{
|
||||||
|
var adapter = GetChassisAdapter();
|
||||||
|
|
||||||
|
if (adapter == null)
|
||||||
return;
|
return;
|
||||||
Chassis.SetOriginBias(
|
|
||||||
origin.X,
|
var command = new ChassisCommand(
|
||||||
origin.Y,
|
CarNum,
|
||||||
direction);
|
new Twist2D(vx, vy, omegaRadiansPerSecond));
|
||||||
// 坐标系改变后重新等待四个舵轮对齐。
|
|
||||||
Chassis.AfterDirectionChanged();
|
if (!adapter.Send(command, interval))
|
||||||
|
{
|
||||||
|
adapter.StopImmediately();
|
||||||
|
|
||||||
|
Console.WriteLine(
|
||||||
|
"底盘命令分解失败,车辆已经停车:" +
|
||||||
|
adapter.LastFailureReason);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
private MultiWheelChassisAdapter _chassisAdapter;
|
||||||
|
|
||||||
|
private MultiWheelChassisAdapter GetChassisAdapter()
|
||||||
|
{
|
||||||
|
if (Chassis == null)
|
||||||
|
return null;
|
||||||
|
|
||||||
|
if (_chassisAdapter == null ||
|
||||||
|
_chassisAdapter.VehicleId != CarNum)
|
||||||
|
{
|
||||||
|
_chassisAdapter =
|
||||||
|
new MultiWheelChassisAdapter(Chassis, CarNum);
|
||||||
|
}
|
||||||
|
|
||||||
|
return _chassisAdapter;
|
||||||
}
|
}
|
||||||
|
|
||||||
#region MCURoutine兼容参数(暂保留原硬件协议)
|
#region MCURoutine兼容参数(暂保留原硬件协议)
|
||||||
@@ -237,65 +411,6 @@ namespace MedullaAdapter
|
|||||||
[AsUpperIO(desc = "多车灯光同步兼容值,-1使用本车灯光")]
|
[AsUpperIO(desc = "多车灯光同步兼容值,-1使用本车灯光")]
|
||||||
public int MultiVehicleLightSync = -1;
|
public int MultiVehicleLightSync = -1;
|
||||||
|
|
||||||
// 以下夹臂字段用于兼容原MCU的0x209、0x20A等CAN协议;当前不使用时目标速度保持为0。
|
|
||||||
[AsUpperIO(desc = "左夹臂目标速度", timeOutReset = true)]
|
|
||||||
public float SpeedLeftArm;
|
|
||||||
|
|
||||||
[AsUpperIO(desc = "右夹臂目标速度", timeOutReset = true)]
|
|
||||||
public float SpeedRightArm;
|
|
||||||
|
|
||||||
[AsLowerIO(desc = "左夹臂实际速度")]
|
|
||||||
public float ActualSpeedLeftArm;
|
|
||||||
|
|
||||||
[AsLowerIO(desc = "右夹臂实际速度")]
|
|
||||||
public float ActualSpeedRightArm;
|
|
||||||
|
|
||||||
[AsLowerIO(desc = "左夹臂实际位置")]
|
|
||||||
public float ActualPosLeftArm;
|
|
||||||
|
|
||||||
[AsLowerIO(desc = "右夹臂实际位置")]
|
|
||||||
public float ActualPosRightArm;
|
|
||||||
|
|
||||||
[AsLowerIO(desc = "左夹臂状态字")]
|
|
||||||
public int LeftArmStateCode;
|
|
||||||
|
|
||||||
[AsLowerIO(desc = "右夹臂状态字")]
|
|
||||||
public int RightArmStateCode;
|
|
||||||
|
|
||||||
[AsLowerIO(desc = "左夹臂错误字")]
|
|
||||||
public int LeftArmErrorCode;
|
|
||||||
|
|
||||||
[AsLowerIO(desc = "右夹臂错误字")]
|
|
||||||
public int RightArmErrorCode;
|
|
||||||
|
|
||||||
[AsLowerIO(desc = "左夹臂电流")]
|
|
||||||
public float LeftArmElectric;
|
|
||||||
|
|
||||||
[AsLowerIO(desc = "右夹臂电流")]
|
|
||||||
public float RightArmElectric;
|
|
||||||
|
|
||||||
[AsInitParam(desc = "左夹臂低限位")]
|
|
||||||
[AsLowerIO]
|
|
||||||
public int LeftArmLowerPos = -10000;
|
|
||||||
|
|
||||||
[AsInitParam(desc = "左夹臂高限位")]
|
|
||||||
[AsLowerIO]
|
|
||||||
public int LeftArmUpperPos = 5927610;
|
|
||||||
|
|
||||||
[AsInitParam(desc = "右夹臂低限位")]
|
|
||||||
[AsLowerIO]
|
|
||||||
public int RightArmLowerPos = -17295;
|
|
||||||
|
|
||||||
[AsInitParam(desc = "右夹臂高限位")]
|
|
||||||
[AsLowerIO]
|
|
||||||
public int RightArmUpperPos = 5927610;
|
|
||||||
|
|
||||||
[AsLowerIO(desc = "左夹臂驱动器远程帧709")]
|
|
||||||
public byte LArmRemoteCode;
|
|
||||||
|
|
||||||
[AsLowerIO(desc = "右夹臂驱动器远程帧70A")]
|
|
||||||
public byte RArmRemoteCode;
|
|
||||||
|
|
||||||
#endregion
|
#endregion
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -17,7 +17,6 @@
|
|||||||
<HintPath>ref\RefCartActivator.dll</HintPath>
|
<HintPath>ref\RefCartActivator.dll</HintPath>
|
||||||
<Private>false</Private>
|
<Private>false</Private>
|
||||||
</Reference>
|
</Reference>
|
||||||
|
|
||||||
<Reference Include="MedullaCore">
|
<Reference Include="MedullaCore">
|
||||||
<HintPath>ref\RefMedullaCore.dll</HintPath>
|
<HintPath>ref\RefMedullaCore.dll</HintPath>
|
||||||
<Private>false</Private>
|
<Private>false</Private>
|
||||||
@@ -44,4 +43,15 @@
|
|||||||
</Reference>
|
</Reference>
|
||||||
</ItemGroup>
|
</ItemGroup>
|
||||||
|
|
||||||
|
<ItemGroup>
|
||||||
|
<Compile Include="..\Shared\ChassisCommand.cs"
|
||||||
|
Link="Shared\ChassisCommand.cs" />
|
||||||
|
|
||||||
|
<Compile Include="..\Shared\FrameTransform2D.cs"
|
||||||
|
Link="Shared\FrameTransform2D.cs" />
|
||||||
|
|
||||||
|
<Compile Include="..\Shared\MultiWheelChassisAdapter.cs"
|
||||||
|
Link="Shared\MultiWheelChassisAdapter.cs" />
|
||||||
|
</ItemGroup>
|
||||||
|
|
||||||
</Project>
|
</Project>
|
||||||
|
|||||||
@@ -56,6 +56,7 @@ namespace MedullaAdapter
|
|||||||
cart.TransmitterSpeed,
|
cart.TransmitterSpeed,
|
||||||
DateTime.Now - cart.TransmitterLastTime);
|
DateTime.Now - cart.TransmitterLastTime);
|
||||||
|
|
||||||
|
StopClampArms();
|
||||||
cart.TransmitterLastTime = DateTime.Now;
|
cart.TransmitterLastTime = DateTime.Now;
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -95,6 +96,7 @@ namespace MedullaAdapter
|
|||||||
0, 0, 0,
|
0, 0, 0,
|
||||||
cart.TransmitterSpeed,
|
cart.TransmitterSpeed,
|
||||||
interval);
|
interval);
|
||||||
|
StopClampArms();
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
// SA关闭后立即停车。
|
// SA关闭后立即停车。
|
||||||
@@ -105,6 +107,7 @@ namespace MedullaAdapter
|
|||||||
0, 0, 0,
|
0, 0, 0,
|
||||||
cart.TransmitterSpeed,
|
cart.TransmitterSpeed,
|
||||||
interval);
|
interval);
|
||||||
|
StopClampArms();
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
// 限制实体遥控器的最大速度。
|
// 限制实体遥控器的最大速度。
|
||||||
@@ -116,6 +119,9 @@ namespace MedullaAdapter
|
|||||||
// SD的Mode0作为底盘驾驶档。
|
// SD的Mode0作为底盘驾驶档。
|
||||||
if (cart.Transmitter_SD == TransmitterState.Mode0)
|
if (cart.Transmitter_SD == TransmitterState.Mode0)
|
||||||
{
|
{
|
||||||
|
// 底盘驾驶档不允许保留上一周期的夹臂速度。
|
||||||
|
StopClampArms();
|
||||||
|
|
||||||
cart.ManualControl(
|
cart.ManualControl(
|
||||||
cart.TransmitterControlMode,
|
cart.TransmitterControlMode,
|
||||||
cart.TransmitterLeftJoystickValX,
|
cart.TransmitterLeftJoystickValX,
|
||||||
@@ -126,12 +132,37 @@ namespace MedullaAdapter
|
|||||||
|
|
||||||
return;
|
return;
|
||||||
}
|
}
|
||||||
|
if (cart.Transmitter_SD == TransmitterState.Mode1)
|
||||||
|
{
|
||||||
|
// 切换到夹臂档时,先确保底盘停止。
|
||||||
|
cart.ManualControl(
|
||||||
|
cart.TransmitterControlMode,
|
||||||
|
0, 0, 0,
|
||||||
|
cart.TransmitterSpeed,
|
||||||
|
interval);
|
||||||
|
|
||||||
|
var armSpeed =
|
||||||
|
cart.TransmitterRightJoystickValX *
|
||||||
|
cart.ManualArmSpeedFac;
|
||||||
|
|
||||||
|
cart.SpeedLeftArm = armSpeed;
|
||||||
|
cart.SpeedRightArm = armSpeed;
|
||||||
|
return;
|
||||||
|
}
|
||||||
// 非驾驶档必须主动停车,防止上一条运动指令残留。
|
// 非驾驶档必须主动停车,防止上一条运动指令残留。
|
||||||
cart.ManualControl(
|
cart.ManualControl(
|
||||||
cart.TransmitterControlMode,
|
cart.TransmitterControlMode,
|
||||||
0, 0, 0,
|
0, 0, 0,
|
||||||
cart.TransmitterSpeed,
|
cart.TransmitterSpeed,
|
||||||
interval);
|
interval);
|
||||||
|
StopClampArms();
|
||||||
|
}
|
||||||
|
|
||||||
|
// M层单车夹臂安全:清除物理遥控器留下的左右夹臂速度命令。
|
||||||
|
private void StopClampArms()
|
||||||
|
{
|
||||||
|
cart.SpeedLeftArm = 0;
|
||||||
|
cart.SpeedRightArm = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
// M层单车底盘:根据四个舵轮的目标角度和实际角度修正8个驱动电机速度。
|
// M层单车底盘:根据四个舵轮的目标角度和实际角度修正8个驱动电机速度。
|
||||||
@@ -328,4 +359,4 @@ namespace MedullaAdapter
|
|||||||
}
|
}
|
||||||
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -1,15 +1,86 @@
|
|||||||
// Medulla虚拟遥控器和夹臂控制
|
// Medulla虚拟遥控器和夹臂控制
|
||||||
using MDCSToolBox.Medulla.Chassis.MultiWheel;
|
using MDCSToolBox.Medulla.Chassis.MultiWheel;
|
||||||
|
using CartActivator;
|
||||||
|
|
||||||
namespace MedullaAdapter
|
namespace MedullaAdapter
|
||||||
{
|
{
|
||||||
// M层单车虚拟遥控器:使用父类提供的底盘控制界面。
|
// M层单车虚拟遥控器:使用父类提供的底盘控制界面。
|
||||||
public class Remote : MultiWheelRemote<DiverCartDefinition>
|
public class Remote : MultiWheelRemote<DiverCartDefinition>
|
||||||
{
|
{
|
||||||
|
// 将父类虚拟遥控器界面输入统一转发到本车的ManualControl。
|
||||||
|
public override void ChassisOperation()
|
||||||
|
{
|
||||||
|
if (MultiVehicleMode.on)
|
||||||
|
{
|
||||||
|
MultiVehicleModeChassisLogic();
|
||||||
|
statusText = "单车版本不支持多车联动遥控";
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 当前单车适配层只定义Normal、Crab和Spin三种模式。
|
||||||
|
// 禁止这些旧按钮绕过适配层直接修改底盘坐标偏置。
|
||||||
|
if (AckermannMode.on ||
|
||||||
|
SwayMode.on ||
|
||||||
|
XYThMode.on)
|
||||||
|
{
|
||||||
|
cart.Chassis?.PredefinedDriveStop();
|
||||||
|
statusText = "当前单车版本暂不支持阿克曼、斜行或全向模式";
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
var mode = SpinMode.on
|
||||||
|
? DiverCartDefinition.ManualControlMode.Spin
|
||||||
|
: CrabMode.on
|
||||||
|
? DiverCartDefinition.ManualControlMode.Crab
|
||||||
|
: DiverCartDefinition.ManualControlMode.Normal;
|
||||||
|
|
||||||
|
cart.ManualControl(
|
||||||
|
mode,
|
||||||
|
SpeedPad.x,
|
||||||
|
SpeedPad.y,
|
||||||
|
FrontDirection.dval * 180,
|
||||||
|
SpeedThreshold.val);
|
||||||
|
|
||||||
|
statusText =
|
||||||
|
$"{mode}, x={SpeedPad.x:0.00}, " +
|
||||||
|
$"y={SpeedPad.y:0.00}, " +
|
||||||
|
$"speed={SpeedThreshold.val:0.00}";
|
||||||
|
}
|
||||||
|
[AsControlItem(name = "夹抱速度", LayoutRow = 0, LayoutCol = 4)]
|
||||||
|
public Throttle ArmSpeed;
|
||||||
|
|
||||||
|
[AsControlItem(name = "夹抱打开", LayoutRow = 1, LayoutCol = 0)]
|
||||||
|
public Button Open;
|
||||||
|
|
||||||
|
[AsControlItem(name = "夹抱关闭", LayoutRow = 1, LayoutCol = 2)]
|
||||||
|
public Button Close;
|
||||||
|
// M层虚拟遥控器:控制左右夹臂同步打开或关闭。
|
||||||
|
public override void CustomOperation()
|
||||||
|
{
|
||||||
|
if (Open.pressed)
|
||||||
|
{
|
||||||
|
cart.SpeedLeftArm =
|
||||||
|
-ArmSpeed.val * cart.ManualArmSpeedFac;
|
||||||
|
cart.SpeedRightArm =
|
||||||
|
-ArmSpeed.val * cart.ManualArmSpeedFac;
|
||||||
|
}
|
||||||
|
else if (Close.pressed)
|
||||||
|
{
|
||||||
|
cart.SpeedLeftArm =
|
||||||
|
ArmSpeed.val * cart.ManualArmSpeedFac;
|
||||||
|
cart.SpeedRightArm =
|
||||||
|
ArmSpeed.val * cart.ManualArmSpeedFac;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
cart.SpeedLeftArm = 0;
|
||||||
|
cart.SpeedRightArm = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
// 单车不支持多车联动,误打开多车开关时主动停车。
|
// 单车不支持多车联动,误打开多车开关时主动停车。
|
||||||
public override void MultiVehicleModeChassisLogic()
|
public override void MultiVehicleModeChassisLogic()
|
||||||
{
|
{
|
||||||
cart.Chassis?.PredefinedDriveStop();
|
cart.Chassis?.PredefinedDriveStop();
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,183 @@
|
|||||||
|
// 纯数据层:只描述坐标、速度和命令
|
||||||
|
// 定义二维坐标、位姿、速度、车队布局和单车底盘命令。
|
||||||
|
// Shared层统一使用SI单位:位置m、线速度m/s、角度rad、角速度rad/s。
|
||||||
|
// 车体坐标系采用右手系:X向前、Y向左、逆时针角度和角速度为正。
|
||||||
|
// 命名约定:XxxInYyy表示Xxx在Yyy坐标系中的表达。
|
||||||
|
|
||||||
|
namespace MyParking.Shared
|
||||||
|
{
|
||||||
|
/// <summary>
|
||||||
|
/// 二维坐标点,X、Y单位均为米。
|
||||||
|
/// </summary>
|
||||||
|
public readonly struct Point2D
|
||||||
|
{
|
||||||
|
public Point2D(double xMeters, double yMeters)
|
||||||
|
{
|
||||||
|
XMeters = xMeters;
|
||||||
|
YMeters = yMeters;
|
||||||
|
}
|
||||||
|
|
||||||
|
public double XMeters { get; }
|
||||||
|
|
||||||
|
public double YMeters { get; }
|
||||||
|
|
||||||
|
public static Point2D Zero => new Point2D(0.0, 0.0);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 二维局部坐标系在父坐标系中的位姿。
|
||||||
|
/// 位置单位为米,朝向单位为弧度,逆时针为正。
|
||||||
|
/// 具体父子关系由变量名称说明,例如RadarPoseInBody。
|
||||||
|
/// </summary>
|
||||||
|
public readonly struct Pose2D
|
||||||
|
{
|
||||||
|
public Pose2D(
|
||||||
|
double xMeters,
|
||||||
|
double yMeters,
|
||||||
|
double yawRadians)
|
||||||
|
{
|
||||||
|
XMeters = xMeters;
|
||||||
|
YMeters = yMeters;
|
||||||
|
YawRadians = yawRadians;
|
||||||
|
}
|
||||||
|
|
||||||
|
public double XMeters { get; }
|
||||||
|
|
||||||
|
public double YMeters { get; }
|
||||||
|
|
||||||
|
public double YawRadians { get; }
|
||||||
|
|
||||||
|
public Point2D Position =>
|
||||||
|
new Point2D(XMeters, YMeters);
|
||||||
|
|
||||||
|
public static Pose2D Identity =>
|
||||||
|
new Pose2D(0.0, 0.0, 0.0);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 二维刚体速度。
|
||||||
|
/// 线速度单位为m/s,角速度单位为rad/s。
|
||||||
|
/// 速度所属坐标系由持有该Twist2D的外层类型或变量名称确定。
|
||||||
|
/// </summary>
|
||||||
|
public readonly struct Twist2D
|
||||||
|
{
|
||||||
|
public Twist2D(
|
||||||
|
double vxMetersPerSecond,
|
||||||
|
double vyMetersPerSecond,
|
||||||
|
double omegaRadiansPerSecond)
|
||||||
|
{
|
||||||
|
VxMetersPerSecond = vxMetersPerSecond;
|
||||||
|
VyMetersPerSecond = vyMetersPerSecond;
|
||||||
|
OmegaRadiansPerSecond = omegaRadiansPerSecond;
|
||||||
|
}
|
||||||
|
|
||||||
|
public double VxMetersPerSecond { get; }
|
||||||
|
|
||||||
|
public double VyMetersPerSecond { get; }
|
||||||
|
|
||||||
|
public double OmegaRadiansPerSecond { get; }
|
||||||
|
|
||||||
|
public static Twist2D Zero =>
|
||||||
|
new Twist2D(0.0, 0.0, 0.0);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 发送给单辆车的车体坐标系速度命令。
|
||||||
|
/// </summary>
|
||||||
|
public readonly struct ChassisCommand
|
||||||
|
{
|
||||||
|
public ChassisCommand(
|
||||||
|
int vehicleId,
|
||||||
|
Twist2D bodyTwist)
|
||||||
|
{
|
||||||
|
VehicleId = vehicleId;
|
||||||
|
BodyTwist = bodyTwist;
|
||||||
|
}
|
||||||
|
|
||||||
|
public int VehicleId { get; }
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 单车车体坐标系速度:X向前、Y向左、逆时针旋转为正。
|
||||||
|
/// </summary>
|
||||||
|
public Twist2D BodyTwist { get; }
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 创建指定车辆的停止命令。
|
||||||
|
/// </summary>
|
||||||
|
public static ChassisCommand Stop(int vehicleId)
|
||||||
|
{
|
||||||
|
return new ChassisCommand(
|
||||||
|
vehicleId,
|
||||||
|
Twist2D.Zero);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 单辆车的车体坐标系在车队坐标系中的位姿。
|
||||||
|
/// </summary>
|
||||||
|
public readonly struct VehicleLayout
|
||||||
|
{
|
||||||
|
public VehicleLayout(
|
||||||
|
int vehicleId,
|
||||||
|
Pose2D poseInFleet)
|
||||||
|
{
|
||||||
|
VehicleId = vehicleId;
|
||||||
|
PoseInFleet = poseInFleet;
|
||||||
|
}
|
||||||
|
|
||||||
|
public int VehicleId { get; }
|
||||||
|
|
||||||
|
public Pose2D PoseInFleet { get; }
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 车队整体运动命令,速度分量均在车队坐标系中表达。
|
||||||
|
/// </summary>
|
||||||
|
public readonly struct FleetMotionCommand
|
||||||
|
{
|
||||||
|
public FleetMotionCommand(
|
||||||
|
Point2D referencePointInFleet,
|
||||||
|
Twist2D twistAtReferencePoint)
|
||||||
|
{
|
||||||
|
ReferencePointInFleet = referencePointInFleet;
|
||||||
|
TwistAtReferencePoint = twistAtReferencePoint;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 速度命令对应的参考点,也可作为自定义旋转中心。
|
||||||
|
/// </summary>
|
||||||
|
public Point2D ReferencePointInFleet { get; }
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 参考点处的车队速度。
|
||||||
|
/// </summary>
|
||||||
|
public Twist2D TwistAtReferencePoint { get; }
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 创建绕指定中心原地旋转的车队命令。
|
||||||
|
/// </summary>
|
||||||
|
public static FleetMotionCommand RotateAround(
|
||||||
|
Point2D rotationCenterInFleet,
|
||||||
|
double omegaRadiansPerSecond)
|
||||||
|
{
|
||||||
|
return new FleetMotionCommand(
|
||||||
|
rotationCenterInFleet,
|
||||||
|
new Twist2D(
|
||||||
|
0.0,
|
||||||
|
0.0,
|
||||||
|
omegaRadiansPerSecond));
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 创建车队停止命令。
|
||||||
|
/// </summary>
|
||||||
|
public static FleetMotionCommand Stop()
|
||||||
|
{
|
||||||
|
return new FleetMotionCommand(
|
||||||
|
Point2D.Zero,
|
||||||
|
Twist2D.Zero);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
@@ -0,0 +1 @@
|
|||||||
|
// 把车队整体速度分解为每辆车的局部速度
|
||||||
@@ -0,0 +1,177 @@
|
|||||||
|
// 车体、运动、车队坐标系之间的转换
|
||||||
|
using System;
|
||||||
|
|
||||||
|
namespace MyParking.Shared
|
||||||
|
{
|
||||||
|
/// <summary>
|
||||||
|
/// 提供二维刚体坐标系之间的点、向量、位姿和速度变换。
|
||||||
|
/// 坐标系采用X向前、Y向左、逆时针为正的右手系。
|
||||||
|
/// </summary>
|
||||||
|
public static class FrameTransform2D
|
||||||
|
{
|
||||||
|
private const double TwoPi = 2.0 * Math.PI;
|
||||||
|
/// <summary>
|
||||||
|
/// 将角度归一化到[-π, π)范围。
|
||||||
|
/// </summary>
|
||||||
|
public static double NormalizeAngle(double angleRadians)
|
||||||
|
{
|
||||||
|
|
||||||
|
if (double.IsNaN(angleRadians) || double.IsInfinity(angleRadians))
|
||||||
|
{
|
||||||
|
throw new ArgumentOutOfRangeException(
|
||||||
|
nameof(angleRadians),
|
||||||
|
"角度必须是有限数值。");
|
||||||
|
}
|
||||||
|
angleRadians %= TwoPi;
|
||||||
|
if (angleRadians >= Math.PI)
|
||||||
|
angleRadians -= TwoPi;
|
||||||
|
if (angleRadians < -Math.PI)
|
||||||
|
angleRadians += TwoPi;
|
||||||
|
return angleRadians;
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 计算从current到target的最短角度差。
|
||||||
|
/// 返回正值表示逆时针旋转。
|
||||||
|
/// </summary>
|
||||||
|
public static double ShortestAngleDifference(
|
||||||
|
double targetRadians,
|
||||||
|
double currentRadians)
|
||||||
|
{
|
||||||
|
return NormalizeAngle(targetRadians - currentRadians);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 将源坐标系中的点变换到目标坐标系。
|
||||||
|
/// sourcePoseInTarget表示源坐标系在目标坐标系中的位姿。
|
||||||
|
/// </summary>
|
||||||
|
public static Point2D TransformPoint(
|
||||||
|
Pose2D sourcePoseInTarget,
|
||||||
|
Point2D pointInSource)
|
||||||
|
{
|
||||||
|
var cos = Math.Cos(sourcePoseInTarget.YawRadians);
|
||||||
|
var sin = Math.Sin(sourcePoseInTarget.YawRadians);
|
||||||
|
|
||||||
|
return new Point2D(
|
||||||
|
sourcePoseInTarget.XMeters +
|
||||||
|
cos * pointInSource.XMeters -
|
||||||
|
sin * pointInSource.YMeters,
|
||||||
|
|
||||||
|
sourcePoseInTarget.YMeters +
|
||||||
|
sin * pointInSource.XMeters +
|
||||||
|
cos * pointInSource.YMeters);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 将目标坐标系中的点反向变换到源坐标系。
|
||||||
|
/// </summary>
|
||||||
|
public static Point2D InverseTransformPoint(
|
||||||
|
Pose2D sourcePoseInTarget,
|
||||||
|
Point2D pointInTarget)
|
||||||
|
{
|
||||||
|
var dx = pointInTarget.XMeters - sourcePoseInTarget.XMeters;
|
||||||
|
|
||||||
|
var dy = pointInTarget.YMeters - sourcePoseInTarget.YMeters;
|
||||||
|
|
||||||
|
var cos = Math.Cos(sourcePoseInTarget.YawRadians);
|
||||||
|
var sin = Math.Sin(sourcePoseInTarget.YawRadians);
|
||||||
|
|
||||||
|
return new Point2D(
|
||||||
|
cos * dx + sin * dy,
|
||||||
|
-sin * dx + cos * dy);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 将源坐标系中的向量旋转到目标坐标系。
|
||||||
|
/// 向量没有位置,因此不叠加平移量。
|
||||||
|
/// </summary>
|
||||||
|
public static Point2D TransformVector(
|
||||||
|
Pose2D sourcePoseInTarget,
|
||||||
|
Point2D vectorInSource)
|
||||||
|
{
|
||||||
|
var cos = Math.Cos(sourcePoseInTarget.YawRadians);
|
||||||
|
var sin = Math.Sin(sourcePoseInTarget.YawRadians);
|
||||||
|
|
||||||
|
return new Point2D(
|
||||||
|
cos * vectorInSource.XMeters -
|
||||||
|
sin * vectorInSource.YMeters,
|
||||||
|
|
||||||
|
sin * vectorInSource.XMeters +
|
||||||
|
cos * vectorInSource.YMeters);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 组合两级坐标变换。
|
||||||
|
/// parentFromMiddle表示middle在parent中的位姿;
|
||||||
|
/// middleFromChild表示child在middle中的位姿;
|
||||||
|
/// 返回child在parent中的位姿。
|
||||||
|
/// </summary>
|
||||||
|
public static Pose2D Compose(
|
||||||
|
Pose2D parentFromMiddle,
|
||||||
|
Pose2D middleFromChild)
|
||||||
|
{
|
||||||
|
var childPositionInParent = TransformPoint(
|
||||||
|
parentFromMiddle,
|
||||||
|
middleFromChild.Position);
|
||||||
|
|
||||||
|
return new Pose2D(
|
||||||
|
childPositionInParent.XMeters,
|
||||||
|
childPositionInParent.YMeters,
|
||||||
|
NormalizeAngle(
|
||||||
|
parentFromMiddle.YawRadians +
|
||||||
|
middleFromChild.YawRadians));
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 对坐标变换求逆。
|
||||||
|
/// 输入child在parent中的位姿,返回parent在child中的位姿。
|
||||||
|
/// </summary>
|
||||||
|
public static Pose2D Inverse(Pose2D childPoseInParent)
|
||||||
|
{
|
||||||
|
var cos = Math.Cos(childPoseInParent.YawRadians);
|
||||||
|
var sin = Math.Sin(childPoseInParent.YawRadians);
|
||||||
|
|
||||||
|
return new Pose2D(
|
||||||
|
-cos * childPoseInParent.XMeters -
|
||||||
|
sin * childPoseInParent.YMeters,
|
||||||
|
|
||||||
|
sin * childPoseInParent.XMeters -
|
||||||
|
cos * childPoseInParent.YMeters,
|
||||||
|
|
||||||
|
NormalizeAngle(
|
||||||
|
-childPoseInParent.YawRadians));
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 将源坐标系中的位姿变换到目标坐标系。
|
||||||
|
/// </summary>
|
||||||
|
public static Pose2D TransformPose(
|
||||||
|
Pose2D sourcePoseInTarget,
|
||||||
|
Pose2D poseInSource)
|
||||||
|
{
|
||||||
|
return Compose(sourcePoseInTarget, poseInSource);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 转换同一物理参考点处的速度表达坐标系。
|
||||||
|
/// 只旋转线速度,角速度保持不变。
|
||||||
|
/// </summary>
|
||||||
|
public static Twist2D TransformTwistAtSamePoint(
|
||||||
|
Pose2D sourcePoseInTarget,
|
||||||
|
Twist2D twistInSource)
|
||||||
|
{
|
||||||
|
var linearVelocityInTarget = TransformVector(
|
||||||
|
sourcePoseInTarget,
|
||||||
|
new Point2D(
|
||||||
|
twistInSource.VxMetersPerSecond,
|
||||||
|
twistInSource.VyMetersPerSecond));
|
||||||
|
|
||||||
|
return new Twist2D(
|
||||||
|
linearVelocityInTarget.XMeters,
|
||||||
|
linearVelocityInTarget.YMeters,
|
||||||
|
twistInSource.OmegaRadiansPerSecond);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -0,0 +1,289 @@
|
|||||||
|
// 将统一命令转换为原 Chassis API 调用
|
||||||
|
using System;
|
||||||
|
using CommonUsage.Chassis;
|
||||||
|
|
||||||
|
namespace MyParking.Shared
|
||||||
|
{
|
||||||
|
/// <summary>
|
||||||
|
/// 将统一的单车车体速度命令转换为旧版MultiWheelChassis调用。
|
||||||
|
/// 车体坐标系固定为X向前、Y向左、逆时针为正。
|
||||||
|
/// </summary>
|
||||||
|
public sealed class MultiWheelChassisAdapter
|
||||||
|
{
|
||||||
|
#region 辅助内容
|
||||||
|
private const double RadiansToDegrees = 180.0 / Math.PI;
|
||||||
|
private const float BiasTolerance = 0.001f;
|
||||||
|
private readonly MultiWheelChassis _chassis;
|
||||||
|
/// <summary>
|
||||||
|
/// 当前适配器对应的车辆编号。
|
||||||
|
/// </summary>
|
||||||
|
public int VehicleId { get; }
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 检查旧底盘是否仍处于无偏置的真实车体坐标系。
|
||||||
|
/// </summary>
|
||||||
|
private void EnsureBodyFrameIsActive()
|
||||||
|
{
|
||||||
|
var bias = _chassis.GetOriginBias();
|
||||||
|
|
||||||
|
if (Math.Abs(bias.X) <= BiasTolerance &&
|
||||||
|
Math.Abs(bias.Y) <= BiasTolerance &&
|
||||||
|
Math.Abs(bias.Z) <= BiasTolerance)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
throw new InvalidOperationException(
|
||||||
|
"MultiWheelChassis的坐标偏置在适配器创建后被修改。" +
|
||||||
|
$"当前偏置为X={bias.X}, Y={bias.Y}, Th={bias.Z}°。" +
|
||||||
|
"请不要再调用DirectionAngle或SetOriginBias控制蟹行。");
|
||||||
|
}
|
||||||
|
/// <summary>
|
||||||
|
/// 检查底盘命令是否包含无效数值。
|
||||||
|
/// </summary>
|
||||||
|
private static void ValidateTwist(Twist2D twist)
|
||||||
|
{
|
||||||
|
ValidateFinite(
|
||||||
|
twist.VxMetersPerSecond,
|
||||||
|
nameof(twist.VxMetersPerSecond));
|
||||||
|
|
||||||
|
ValidateFinite(
|
||||||
|
twist.VyMetersPerSecond,
|
||||||
|
nameof(twist.VyMetersPerSecond));
|
||||||
|
|
||||||
|
ValidateFinite(
|
||||||
|
twist.OmegaRadiansPerSecond,
|
||||||
|
nameof(twist.OmegaRadiansPerSecond));
|
||||||
|
}
|
||||||
|
/// <summary>
|
||||||
|
/// 检查数值是否为有限值。
|
||||||
|
/// </summary>
|
||||||
|
private static void ValidateFinite(
|
||||||
|
double value,
|
||||||
|
string parameterName)
|
||||||
|
{
|
||||||
|
if (double.IsNaN(value) ||
|
||||||
|
double.IsInfinity(value))
|
||||||
|
{
|
||||||
|
throw new ArgumentOutOfRangeException(
|
||||||
|
parameterName,
|
||||||
|
"底盘速度命令不能是NaN或无穷大。");
|
||||||
|
}
|
||||||
|
|
||||||
|
if (value > float.MaxValue ||
|
||||||
|
value < -float.MaxValue)
|
||||||
|
{
|
||||||
|
throw new ArgumentOutOfRangeException(
|
||||||
|
parameterName,
|
||||||
|
"底盘速度命令超过float可表示范围。");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 获取最近一次底盘运动分解失败原因。
|
||||||
|
/// </summary>
|
||||||
|
public string LastFailureReason =>
|
||||||
|
_chassis.LastMotionDecomposeFailureReason;
|
||||||
|
#endregion
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 将旧底盘的原点偏置恢复为真实单车车体坐标系。
|
||||||
|
/// </summary>
|
||||||
|
public void ResetToBodyFrame()
|
||||||
|
{
|
||||||
|
_chassis.SetOriginBias(
|
||||||
|
x: 0.0f,
|
||||||
|
y: 0.0f,
|
||||||
|
th: 0.0f);
|
||||||
|
}
|
||||||
|
public MultiWheelChassisAdapter(MultiWheelChassis chassis, int vehicleId)
|
||||||
|
{
|
||||||
|
_chassis = chassis ?? throw new ArgumentNullException(nameof(chassis));
|
||||||
|
if (vehicleId <= 0)
|
||||||
|
{
|
||||||
|
throw new ArgumentOutOfRangeException(
|
||||||
|
nameof(vehicleId),
|
||||||
|
"车辆编号必须大于零。");
|
||||||
|
}
|
||||||
|
VehicleId = vehicleId;
|
||||||
|
#pragma warning disable CS0612, CS0618
|
||||||
|
var wheels = _chassis.GetSteerWheels();
|
||||||
|
#pragma warning restore CS0612, CS0618
|
||||||
|
|
||||||
|
if (wheels.Count == 0)
|
||||||
|
{
|
||||||
|
throw new InvalidOperationException(
|
||||||
|
"MultiWheelChassis尚未完成舵轮初始化," +
|
||||||
|
"不能创建底盘适配器。");
|
||||||
|
}
|
||||||
|
// 禁用旧版DirectionAngle/ZeroDirection坐标偏置,
|
||||||
|
// 保证SendXYThSpeed直接使用真实车体坐标系。
|
||||||
|
ResetToBodyFrame();
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 将车体坐标系速度命令发送给多舵轮底盘。
|
||||||
|
/// </summary>
|
||||||
|
public bool Send(ChassisCommand command, TimeSpan? interval = null)
|
||||||
|
{
|
||||||
|
if (command.VehicleId != VehicleId)
|
||||||
|
{
|
||||||
|
throw new InvalidOperationException(
|
||||||
|
$"命令车辆编号{command.VehicleId}与适配器车辆编号" +
|
||||||
|
$"{VehicleId}不一致。");
|
||||||
|
}
|
||||||
|
ValidateTwist(command.BodyTwist);
|
||||||
|
// 防止其他旧逻辑再次调用DirectionAngle或
|
||||||
|
// SetOriginBias改变底盘坐标语义。
|
||||||
|
EnsureBodyFrameIsActive();
|
||||||
|
var vxMetersPerSecond =
|
||||||
|
(float)command.BodyTwist.VxMetersPerSecond;
|
||||||
|
var vyMetersPerSecond =
|
||||||
|
(float)command.BodyTwist.VyMetersPerSecond;
|
||||||
|
var omegaDegreesPerSecond =
|
||||||
|
(float)(
|
||||||
|
command.BodyTwist.OmegaRadiansPerSecond *
|
||||||
|
RadiansToDegrees);
|
||||||
|
var success = _chassis.SendXYThSpeed(
|
||||||
|
vxMetersPerSecond,
|
||||||
|
vyMetersPerSecond,
|
||||||
|
omegaDegreesPerSecond,
|
||||||
|
interval);
|
||||||
|
if (!success)
|
||||||
|
{
|
||||||
|
// 防止分解失败后继续执行上一条运动命令。
|
||||||
|
_chassis.PredefinedDriveStop();
|
||||||
|
}
|
||||||
|
return success;
|
||||||
|
}
|
||||||
|
/// <summary>
|
||||||
|
/// 按底盘减速度配置平滑停车,需要在控制周期中持续调用。
|
||||||
|
/// </summary>
|
||||||
|
public void RampStop(TimeSpan? interval = null)
|
||||||
|
{
|
||||||
|
_chassis.RampStop(interval);
|
||||||
|
}
|
||||||
|
/// <summary>
|
||||||
|
/// 立即将所有驱动轮速度下发为零。
|
||||||
|
/// </summary>
|
||||||
|
public void StopImmediately()
|
||||||
|
{
|
||||||
|
_chassis.PredefinedDriveStop();
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 停车并将所有舵轮转到指定的车体角度。
|
||||||
|
/// 只调整舵轮角度,不产生车辆线速度。
|
||||||
|
/// </summary>
|
||||||
|
public bool PrepareParallelDirection(
|
||||||
|
double directionRadians)
|
||||||
|
{
|
||||||
|
EnsureBodyFrameIsActive();
|
||||||
|
var targetDegrees = (float)(FrameTransform2D.NormalizeAngle(directionRadians) *
|
||||||
|
RadiansToDegrees);
|
||||||
|
|
||||||
|
#pragma warning disable CS0612, CS0618
|
||||||
|
var wheels = _chassis.GetSteerWheels();
|
||||||
|
#pragma warning restore CS0612, CS0618
|
||||||
|
|
||||||
|
// 没有舵轮时不能认为预对齐成功。
|
||||||
|
if (wheels.Count == 0)
|
||||||
|
{
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// 先检查所有舵轮能否到达目标机械角度。
|
||||||
|
foreach (var wheel in wheels)
|
||||||
|
{
|
||||||
|
if (targetDegrees < wheel.AngleLowerLimit ||
|
||||||
|
targetDegrees > wheel.AngleUpperLimit)
|
||||||
|
{
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// 模式切换前立即停止驱动轮。
|
||||||
|
_chassis.PredefinedDriveStop();
|
||||||
|
// 检查完成后再统一下发,避免只转动一部分舵轮。
|
||||||
|
foreach (var wheel in wheels)
|
||||||
|
{
|
||||||
|
wheel.WriteAngle(targetDegrees);
|
||||||
|
}
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 检查所有舵轮是否已经对准给定方向。
|
||||||
|
/// </summary>
|
||||||
|
public bool AreParallelWheelsAligned(
|
||||||
|
double directionRadians,
|
||||||
|
double toleranceRadians)
|
||||||
|
{
|
||||||
|
if (double.IsNaN(toleranceRadians) ||
|
||||||
|
double.IsInfinity(toleranceRadians) ||
|
||||||
|
toleranceRadians < 0.0)
|
||||||
|
{
|
||||||
|
throw new ArgumentOutOfRangeException(
|
||||||
|
nameof(toleranceRadians),
|
||||||
|
"舵轮到位容差必须是非负有限值。");
|
||||||
|
}
|
||||||
|
|
||||||
|
EnsureBodyFrameIsActive();
|
||||||
|
var targetDegrees = (float)(
|
||||||
|
FrameTransform2D.NormalizeAngle(directionRadians) *
|
||||||
|
180.0 / Math.PI);
|
||||||
|
|
||||||
|
var toleranceDegrees = (float)(
|
||||||
|
Math.Abs(toleranceRadians) *
|
||||||
|
180.0 / Math.PI);
|
||||||
|
|
||||||
|
#pragma warning disable CS0612, CS0618
|
||||||
|
var wheels = _chassis.GetSteerWheels();
|
||||||
|
#pragma warning restore CS0612, CS0618
|
||||||
|
|
||||||
|
foreach (var wheel in wheels)
|
||||||
|
{
|
||||||
|
var angleErrorDegrees = targetDegrees - wheel.ReadAngle();
|
||||||
|
|
||||||
|
if (Math.Abs(angleErrorDegrees) >
|
||||||
|
toleranceDegrees)
|
||||||
|
{
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 停车并将舵轮预对齐到原地自转方向。
|
||||||
|
/// 返回是否成功生成舵轮目标。
|
||||||
|
/// </summary>
|
||||||
|
public bool PrepareSpin(
|
||||||
|
TimeSpan? interval = null)
|
||||||
|
{
|
||||||
|
EnsureBodyFrameIsActive();
|
||||||
|
_chassis.PredefinedDriveStop();
|
||||||
|
|
||||||
|
var success =
|
||||||
|
_chassis.SendRotateMotion(
|
||||||
|
0.0f,
|
||||||
|
interval);
|
||||||
|
|
||||||
|
if (!success)
|
||||||
|
{
|
||||||
|
_chassis.PredefinedDriveStop();
|
||||||
|
}
|
||||||
|
return success;
|
||||||
|
}
|
||||||
|
/// <summary>
|
||||||
|
/// 所有舵轮是否已对齐到原地自转方向。
|
||||||
|
/// </summary>
|
||||||
|
public bool AreSpinWheelsAligned => _chassis.LastRotateAligned;
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -0,0 +1,225 @@
|
|||||||
|
// 然后因为我这个车的舵轮是有正负120°限制的,所以有这种情况,比如我给了+90°让它进入蟹行模式,然后我在蟹行模式我去追一个相对于车左侧方向+40°的轨迹,那这时候就超过限制了,那我这是要做一个什么措施吗,比如我能想到的就是达到这个限位要么就停车打印说明到达限位,安全措施;要么就是我在追这个轨迹的过程中知道我大概可能超过限位,然后我提前停下,自转,调整为车头朝向为轨迹的方向,也就是自转调整到我车当前模式的初始姿态方向这种;你有什么好的建议吗,其他的建议吗,或者有什么可行的方法,因为实际的过程还是需要考虑这个限位的问题的
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// 9:49
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// 这个问题必须考虑,但“目标轮角超过 +120°”不一定意味着命令无法执行。对于舵轮,首先要利用“轮角反向、轮速反向”的等价关系,然后才考虑停车和车身重定向。
|
||||||
|
// 1. +130° 不一定超过限位后就无法运动
|
||||||
|
// 你的例子是:
|
||||||
|
// 蟹行基准方向:+90°
|
||||||
|
// 轨迹又向该方向增加:+40°
|
||||||
|
// 期望运动方向:+130°
|
||||||
|
// 舵轮限制:[-120°, +120°]
|
||||||
|
// 直接使用:
|
||||||
|
// 轮角:+130°
|
||||||
|
// 轮速:+0.3 m/s
|
||||||
|
// 确实超过了 +120°。
|
||||||
|
// 但它有一个完全等价的表达:
|
||||||
|
// 轮角:-50°
|
||||||
|
// 轮速:-0.3 m/s
|
||||||
|
// 因为:
|
||||||
|
// (θ, v) ≡ (θ - 180°, -v)
|
||||||
|
// 即:
|
||||||
|
// (+130°, +0.3) ≡ (-50°, -0.3)
|
||||||
|
// 虽然轮子的机械朝向和转速符号变了,但轮胎与地面的速度向量完全相同。
|
||||||
|
// 所以第一层措施应当是:
|
||||||
|
// 先尝试期望角度
|
||||||
|
// 如果超过限位
|
||||||
|
// 尝试角度±180°
|
||||||
|
// 同时将轮速取反
|
||||||
|
// 如果等价角度满足限位
|
||||||
|
// 使用等价解
|
||||||
|
// 否则才判定无法执行
|
||||||
|
// 你当前的 CommonUsage.dll 中已经有类似的舵角反解逻辑,SendXYThSpeed() 返回 bool 也是为了告诉上层运动分解是否成功。
|
||||||
|
// 2. 你的±120°结构在静态上覆盖了所有运动方向
|
||||||
|
// 因为:
|
||||||
|
// 可转范围总宽度 = 240°
|
||||||
|
// 大于 180°。
|
||||||
|
// 只要驱动轮允许正反转,那么对于任意二维速度方向,通常都能在下面两种表达中找到一个合法值:
|
||||||
|
// (θ, +v)
|
||||||
|
// (θ ± 180°, -v)
|
||||||
|
// 因此单纯从“某一时刻目标速度方向”看,+130° 可以用 -50° 表达,不一定需要旋转车身。
|
||||||
|
// 真正困难的是动态过程:
|
||||||
|
// 当前轮角:+90°
|
||||||
|
// 新目标等价轮角:-50°
|
||||||
|
// 需要转动:-140°
|
||||||
|
// 虽然最终角度合法,但舵轮需要转过很大的角度。在舵轮没有到位前,不能继续以正常速度驱动车辆,否则实际运动方向会与轨迹命令不一致。
|
||||||
|
// 所以核心问题不是“有没有合法终态”,而是:
|
||||||
|
// 从当前轮角切换到合法目标轮角的过程是否安全、连续。
|
||||||
|
|
||||||
|
// 3. 建议采用四层处理策略
|
||||||
|
// 第一层:等价舵角选择
|
||||||
|
// 为每一个舵轮计算速度向量:
|
||||||
|
// Vix = Vx - ω·yi
|
||||||
|
// Viy = Vy + ω·xi
|
||||||
|
// 然后:
|
||||||
|
// θi = atan2(Viy, Vix)
|
||||||
|
// si = sqrt(Vix² + Viy²)
|
||||||
|
// 尝试候选解:
|
||||||
|
// 候选1:(θi, si)
|
||||||
|
// 候选2:(θi + 180°, -si)
|
||||||
|
// 候选3:(θi - 180°, -si)
|
||||||
|
// 过滤掉超过 [-120°, +120°] 的候选,再选择相对当前机械轮角转动最小的候选。
|
||||||
|
// 这里计算机械转角距离时,不要简单把差值归一化成最短圆周角,因为舵轮不能无限旋转。对于有限机械区间,应当直接比较:
|
||||||
|
// Math.Abs(candidateAngle - currentMechanicalAngle)
|
||||||
|
// 第二层:软限位
|
||||||
|
// 不要把 ±120° 当作正常工作的边界,建议设置软限位,例如:
|
||||||
|
// 物理硬限位:[-120°, +120°]
|
||||||
|
// 软件工作限位:[-105°, +105°]
|
||||||
|
// 预警区域:[105°, 120°]
|
||||||
|
// 具体余量需要实车标定,初期可以保留 10°~20°。
|
||||||
|
// 状态可以分成:
|
||||||
|
// |θ| < 100° 正常
|
||||||
|
// 100°~105° 接近限位,开始规划
|
||||||
|
// 105°~120° 限速或停止切换
|
||||||
|
// >120° 禁止下发并报警
|
||||||
|
// 软限位的意义是给控制器留出制动、舵轮响应和误差空间。
|
||||||
|
// 第三层:舵轮转向期间限制驱动速度
|
||||||
|
// 如果新的合法舵角与当前舵角相差很大,例如:
|
||||||
|
// 当前:+90°
|
||||||
|
// 目标:-50°
|
||||||
|
// 差值:140°
|
||||||
|
// 不应当马上正常行驶,应进入:
|
||||||
|
// 减速
|
||||||
|
// ↓
|
||||||
|
// 驱动速度降为0
|
||||||
|
// ↓
|
||||||
|
// 舵轮转到-50°
|
||||||
|
// ↓
|
||||||
|
// 确认所有舵轮到位
|
||||||
|
// ↓
|
||||||
|
// 轮速使用负值重新起步
|
||||||
|
// 可以设置阈值:
|
||||||
|
// 舵角误差 < 2°:允许正常起步
|
||||||
|
// 舵角误差 2°~10°:低速或保持
|
||||||
|
// 舵角误差 > 10°:驱动速度为0
|
||||||
|
// 实际阈值根据舵向响应速度标定。
|
||||||
|
// 需要注意:把所有 Vx、Vy、Omega 同比例减小,只能降低运动速度,通常不会改变目标舵轮角度。因此“减速”能让切换更安全,但不一定能让超限角度重新变得合法。
|
||||||
|
// 4. 车身重定向是更平滑的高级方案
|
||||||
|
// 你提出的:
|
||||||
|
// 提前停车,然后自转,调整车头朝向,再继续跟踪。
|
||||||
|
|
||||||
|
// 这是正确而且很实用的方案,尤其适合连续轨迹跟踪。
|
||||||
|
// 但不一定要把车头完全转到轨迹方向,应当根据当前驾驶模式选择车身朝向。
|
||||||
|
// 假设轨迹方向相对当前车体为:
|
||||||
|
// +130°
|
||||||
|
// 如果希望继续保持蟹行模式,舵轮的优选方向是 +90°,那么只需要让车体逆时针自转:
|
||||||
|
// 130° - 90° = 40°
|
||||||
|
// 车体旋转 +40° 后:
|
||||||
|
// 世界轨迹方向不变
|
||||||
|
// 轨迹在新车体坐标系中的方向:+90°
|
||||||
|
// 舵轮回到标准蟹行角度:+90°
|
||||||
|
// 不需要让车头旋转完整的 130°。
|
||||||
|
// 如果准备切换成正常前进模式,优选舵角是 0°,才需要让车头基本对准轨迹:
|
||||||
|
// 130° - 0° = 130°
|
||||||
|
// 因此可以使用下面的通用公式:
|
||||||
|
// 期望车身朝向 =
|
||||||
|
// 世界轨迹切线方向 - 当前模式的优选舵轮方向
|
||||||
|
// 其中:
|
||||||
|
// 正常模式优选角:0°
|
||||||
|
// 左蟹行优选角:+90°
|
||||||
|
// 右蟹行优选角:-90°
|
||||||
|
// 对于蟹行轨迹,车身朝向参考值为:
|
||||||
|
// yawReference =
|
||||||
|
// pathTangentWorldRadians -
|
||||||
|
// Math.PI / 2.0;
|
||||||
|
// 这样车体会逐渐调整自身朝向,使轨迹方向始终落在舵轮舒适区,而不是一直顶着 ±120° 工作。
|
||||||
|
// 5. 不建议到达硬限位才突然自转
|
||||||
|
// 更好的方案是向前预测。
|
||||||
|
// 轨迹跟踪本身通常有前视点,可以提前计算未来一段轨迹对应的目标舵角:
|
||||||
|
// 当前时刻目标轮角
|
||||||
|
// 未来0.2秒目标轮角
|
||||||
|
// 未来0.5秒目标轮角
|
||||||
|
// 未来1.0秒目标轮角
|
||||||
|
// 一旦预测到未来会进入软限位,就提前触发车身重定向:
|
||||||
|
// 正常跟踪
|
||||||
|
// ↓
|
||||||
|
// 预测舵角接近软限位
|
||||||
|
// ↓
|
||||||
|
// 逐渐降低线速度
|
||||||
|
// ↓
|
||||||
|
// 停车
|
||||||
|
// ↓
|
||||||
|
// 车身原地旋转
|
||||||
|
// ↓
|
||||||
|
// 舵轮调整到模式初始方向
|
||||||
|
// ↓
|
||||||
|
// 重新捕获轨迹
|
||||||
|
// ↓
|
||||||
|
// 继续跟踪
|
||||||
|
// 可以定义状态机:
|
||||||
|
// internal enum TrackingConstraintState
|
||||||
|
// {
|
||||||
|
// Tracking,
|
||||||
|
// ApproachingSteeringLimit,
|
||||||
|
// Stopping,
|
||||||
|
// ReorientingBody,
|
||||||
|
// AligningWheels,
|
||||||
|
// Resuming,
|
||||||
|
// Fault
|
||||||
|
// }
|
||||||
|
// 不要在一个控制周期内同时完成停车、车身旋转和恢复跟踪。
|
||||||
|
// 6. LQR和限位处理应该分层
|
||||||
|
// 以后写 LQR 时,不建议让基础 LQR 直接承担全部舵轮限位逻辑。
|
||||||
|
// 推荐结构:
|
||||||
|
// 轨迹参考
|
||||||
|
// ↓
|
||||||
|
// LQR轨迹控制器
|
||||||
|
// 输出期望 Vx、Vy、Omega
|
||||||
|
// ↓
|
||||||
|
// 舵轮约束管理器
|
||||||
|
// 等价角选择、软限位、预测、车身重定向
|
||||||
|
// ↓
|
||||||
|
// ChassisCommand
|
||||||
|
// ↓
|
||||||
|
// MultiWheelChassisAdapter
|
||||||
|
// ↓
|
||||||
|
// SendXYThSpeed
|
||||||
|
// 可以以后增加一个文件:
|
||||||
|
// Shared/SteeringConstraintManager.cs
|
||||||
|
// 它负责:
|
||||||
|
// 根据 Vx、Vy、Omega 预测每个舵轮目标角。
|
||||||
|
// 选择“舵角反向、轮速反向”的等价解。
|
||||||
|
// 检查硬限位和软限位。
|
||||||
|
// 判断是否需要停车对齐。
|
||||||
|
// 给出是否需要重新规划车身朝向。
|
||||||
|
// 记录哪个舵轮导致不可执行。
|
||||||
|
// 而 FrameTransform2D、ChassisCommand 不应该包含这些车辆机械约束。
|
||||||
|
// 7. 当前阶段建议先做到什么
|
||||||
|
// 现在先实现最小安全版本:
|
||||||
|
// 使用 SendXYThSpeed(),让底层优先选择等价舵角。
|
||||||
|
// 检查 adapter.Send(command) 的返回值。
|
||||||
|
// 返回 false 时立即停车,不能继续沿用上一条速度。
|
||||||
|
// 打印每个舵轮的当前角度、目标角度和上下限。
|
||||||
|
// 模式切换时先停车、预转舵轮、确认到位,再开放摇杆。
|
||||||
|
// 先用较低速度测试 +90° → +110° → +120° → +130° 的过程。
|
||||||
|
// 最低限度的发送逻辑应当是:
|
||||||
|
// var success = adapter.Send(command, interval);
|
||||||
|
|
||||||
|
// if (!success)
|
||||||
|
// {
|
||||||
|
// adapter.Stop(interval);
|
||||||
|
|
||||||
|
// Console.WriteLine(
|
||||||
|
// "底盘命令分解失败,已停车:" +
|
||||||
|
// "可能存在舵角限制或舵轮无法完成方向切换。");
|
||||||
|
// }
|
||||||
|
// 然后再实现预测重定向。
|
||||||
|
// 推荐的最终策略
|
||||||
|
// 对于你的停车机器人,我建议采用:
|
||||||
|
// 优先级1:使用θ±180°、轮速反向的等价解
|
||||||
|
// 优先级2:选择机械转角变化较小的等价解
|
||||||
|
// 优先级3:接近软限位时降低速度
|
||||||
|
// 优先级4:提前规划车身朝向,保持舵轮在舒适角度
|
||||||
|
// 优先级5:无法连续切换时停车、自转、重新对齐
|
||||||
|
// 优先级6:仍不可行时保持停车并报警
|
||||||
|
// 所以你的 +130° 示例不应该一开始就判故障。它可以先使用 -50° + 反向轮速。如果从当前 +90° 切换到 -50° 的动态过程不安全,再停车调整。若轨迹允许改变车身姿态,更理想的办法是让车身提前旋转 +40°,从而继续以标准 +90° 蟹行姿态跟踪,而不是一直逼近机械硬限位。
|
||||||
@@ -0,0 +1,121 @@
|
|||||||
|
using MyParking.Simulation.Core;
|
||||||
|
|
||||||
|
namespace MyParking.Simulation.Commands;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 内置离线测试动作;新增带特性的方法后网页会自动生成按钮。
|
||||||
|
/// </summary>
|
||||||
|
public static class BuiltInSimulationActions
|
||||||
|
{
|
||||||
|
[SimulationAction(
|
||||||
|
"mode-normal",
|
||||||
|
"正常",
|
||||||
|
"舵轮模式",
|
||||||
|
10)]
|
||||||
|
public static bool NormalMode(
|
||||||
|
SimulationVehicle vehicle)
|
||||||
|
{
|
||||||
|
return vehicle.SetMode("Normal");
|
||||||
|
}
|
||||||
|
|
||||||
|
[SimulationAction(
|
||||||
|
"mode-crab-left",
|
||||||
|
"左蟹行",
|
||||||
|
"舵轮模式",
|
||||||
|
20)]
|
||||||
|
public static bool CrabLeftMode(
|
||||||
|
SimulationVehicle vehicle)
|
||||||
|
{
|
||||||
|
return vehicle.SetMode("CrabLeft");
|
||||||
|
}
|
||||||
|
|
||||||
|
[SimulationAction(
|
||||||
|
"mode-crab-right",
|
||||||
|
"右蟹行",
|
||||||
|
"舵轮模式",
|
||||||
|
30)]
|
||||||
|
public static bool CrabRightMode(
|
||||||
|
SimulationVehicle vehicle)
|
||||||
|
{
|
||||||
|
return vehicle.SetMode("CrabRight");
|
||||||
|
}
|
||||||
|
|
||||||
|
[SimulationAction(
|
||||||
|
"mode-spin",
|
||||||
|
"自转",
|
||||||
|
"舵轮模式",
|
||||||
|
40)]
|
||||||
|
public static bool SpinMode(
|
||||||
|
SimulationVehicle vehicle)
|
||||||
|
{
|
||||||
|
return vehicle.SetMode("Spin");
|
||||||
|
}
|
||||||
|
|
||||||
|
[SimulationAction(
|
||||||
|
"forward",
|
||||||
|
"前进",
|
||||||
|
"运动测试",
|
||||||
|
10)]
|
||||||
|
public static bool Forward(
|
||||||
|
SimulationVehicle vehicle)
|
||||||
|
{
|
||||||
|
return vehicle.Move(1.0);
|
||||||
|
}
|
||||||
|
|
||||||
|
[SimulationAction(
|
||||||
|
"turn-left",
|
||||||
|
"左转",
|
||||||
|
"运动测试",
|
||||||
|
20)]
|
||||||
|
public static bool TurnLeft(
|
||||||
|
SimulationVehicle vehicle)
|
||||||
|
{
|
||||||
|
return vehicle.Turn(1.0);
|
||||||
|
}
|
||||||
|
|
||||||
|
[SimulationAction(
|
||||||
|
"stop",
|
||||||
|
"停止",
|
||||||
|
"运动测试",
|
||||||
|
30)]
|
||||||
|
public static bool Stop(
|
||||||
|
SimulationVehicle vehicle)
|
||||||
|
{
|
||||||
|
vehicle.Stop();
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
[SimulationAction(
|
||||||
|
"turn-right",
|
||||||
|
"右转",
|
||||||
|
"运动测试",
|
||||||
|
40)]
|
||||||
|
public static bool TurnRight(
|
||||||
|
SimulationVehicle vehicle)
|
||||||
|
{
|
||||||
|
return vehicle.Turn(-1.0);
|
||||||
|
}
|
||||||
|
|
||||||
|
[SimulationAction(
|
||||||
|
"backward",
|
||||||
|
"后退",
|
||||||
|
"运动测试",
|
||||||
|
50)]
|
||||||
|
public static bool Backward(
|
||||||
|
SimulationVehicle vehicle)
|
||||||
|
{
|
||||||
|
return vehicle.Move(-1.0);
|
||||||
|
}
|
||||||
|
|
||||||
|
[SimulationAction(
|
||||||
|
"reset",
|
||||||
|
"单车复位",
|
||||||
|
"维护",
|
||||||
|
10)]
|
||||||
|
public static bool Reset(
|
||||||
|
SimulationVehicle vehicle)
|
||||||
|
{
|
||||||
|
vehicle.Reset();
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -0,0 +1,31 @@
|
|||||||
|
using MyParking.Shared;
|
||||||
|
using MyParking.Simulation.Core;
|
||||||
|
|
||||||
|
namespace MyParking.Simulation.Commands;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 我自己增加的停车机器人仿真测试动作。
|
||||||
|
/// </summary>
|
||||||
|
public static class MySimulationTests
|
||||||
|
{
|
||||||
|
/// <summary>
|
||||||
|
/// 测试车辆以0.2m/s向车体左侧运动。
|
||||||
|
/// </summary>
|
||||||
|
// [SimulationAction(
|
||||||
|
// key: "move-left-020",
|
||||||
|
// displayName: "向左移动0.2m/s",
|
||||||
|
// group: "我的测试",
|
||||||
|
// order: 10)]
|
||||||
|
// public static bool MoveLeft(
|
||||||
|
// SimulationVehicle vehicle)
|
||||||
|
// {
|
||||||
|
// var command = new ChassisCommand(
|
||||||
|
// vehicle.VehicleId,
|
||||||
|
// new Twist2D(
|
||||||
|
// vxMetersPerSecond: 0.0,
|
||||||
|
// vyMetersPerSecond: 0.2,
|
||||||
|
// omegaRadiansPerSecond: 0.0));
|
||||||
|
|
||||||
|
// return vehicle.ApplyCommand(command);
|
||||||
|
// }
|
||||||
|
}
|
||||||
@@ -0,0 +1,38 @@
|
|||||||
|
namespace MyParking.Simulation.Commands;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 将一个静态仿真测试方法自动注册为网页按钮。
|
||||||
|
/// 方法签名必须为bool Xxx(SimulationVehicle vehicle)。
|
||||||
|
/// </summary>
|
||||||
|
[AttributeUsage(AttributeTargets.Method)]
|
||||||
|
public sealed class SimulationActionAttribute : Attribute
|
||||||
|
{
|
||||||
|
public SimulationActionAttribute(
|
||||||
|
string key,
|
||||||
|
string displayName,
|
||||||
|
string group,
|
||||||
|
int order = 0)
|
||||||
|
{
|
||||||
|
Key = key;
|
||||||
|
DisplayName = displayName;
|
||||||
|
Group = group;
|
||||||
|
Order = order;
|
||||||
|
}
|
||||||
|
|
||||||
|
public string Key { get; }
|
||||||
|
|
||||||
|
public string DisplayName { get; }
|
||||||
|
|
||||||
|
public string Group { get; }
|
||||||
|
|
||||||
|
public int Order { get; }
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 网页生成测试按钮所需的命令元数据。
|
||||||
|
/// </summary>
|
||||||
|
public sealed record SimulationActionDescriptor(
|
||||||
|
string Key,
|
||||||
|
string DisplayName,
|
||||||
|
string Group,
|
||||||
|
int Order);
|
||||||
@@ -0,0 +1,150 @@
|
|||||||
|
using System.Reflection;
|
||||||
|
using MyParking.Simulation.Core;
|
||||||
|
|
||||||
|
namespace MyParking.Simulation.Commands;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 自动发现带SimulationAction特性的测试方法并分发网页命令。
|
||||||
|
/// </summary>
|
||||||
|
public sealed class SimulationCommandDispatcher
|
||||||
|
{
|
||||||
|
private readonly SimulationWorld _world;
|
||||||
|
private readonly IReadOnlyDictionary<string, RegisteredAction> _actions;
|
||||||
|
|
||||||
|
public SimulationCommandDispatcher(
|
||||||
|
SimulationWorld world)
|
||||||
|
{
|
||||||
|
_world = world;
|
||||||
|
_actions = DiscoverActions();
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 返回网页动态生成按钮所需的全部命令。
|
||||||
|
/// </summary>
|
||||||
|
public IReadOnlyList<SimulationActionDescriptor> GetActions()
|
||||||
|
{
|
||||||
|
return _actions.Values
|
||||||
|
.Select(action => action.Descriptor)
|
||||||
|
.OrderBy(action => action.Group)
|
||||||
|
.ThenBy(action => action.Order)
|
||||||
|
.ToArray();
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 对指定车辆执行一个已注册的测试方法。
|
||||||
|
/// </summary>
|
||||||
|
public CommandResult Execute(
|
||||||
|
int vehicleId,
|
||||||
|
string command)
|
||||||
|
{
|
||||||
|
if (!_actions.TryGetValue(
|
||||||
|
command,
|
||||||
|
out var registeredAction))
|
||||||
|
{
|
||||||
|
return new CommandResult(
|
||||||
|
false,
|
||||||
|
$"未注册仿真命令:{command}。");
|
||||||
|
}
|
||||||
|
|
||||||
|
try
|
||||||
|
{
|
||||||
|
return _world.WithVehicle(vehicleId, vehicle =>
|
||||||
|
{
|
||||||
|
var success = registeredAction.Handler(vehicle);
|
||||||
|
var message = success
|
||||||
|
? $"车辆{vehicleId}已执行:{registeredAction.Descriptor.DisplayName}。"
|
||||||
|
: $"车辆{vehicleId}暂时无法执行:{registeredAction.Descriptor.DisplayName}。";
|
||||||
|
|
||||||
|
return new CommandResult(success, message);
|
||||||
|
});
|
||||||
|
}
|
||||||
|
catch (KeyNotFoundException exception)
|
||||||
|
{
|
||||||
|
return new CommandResult(
|
||||||
|
false,
|
||||||
|
exception.Message);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
private static IReadOnlyDictionary<string, RegisteredAction>
|
||||||
|
DiscoverActions()
|
||||||
|
{
|
||||||
|
var actions = new Dictionary<string, RegisteredAction>(
|
||||||
|
StringComparer.OrdinalIgnoreCase);
|
||||||
|
|
||||||
|
var methods = Assembly.GetExecutingAssembly()
|
||||||
|
.GetTypes()
|
||||||
|
.SelectMany(type => type.GetMethods(
|
||||||
|
BindingFlags.Public |
|
||||||
|
BindingFlags.NonPublic |
|
||||||
|
BindingFlags.Static));
|
||||||
|
|
||||||
|
foreach (var method in methods)
|
||||||
|
{
|
||||||
|
var attribute =
|
||||||
|
method.GetCustomAttribute<SimulationActionAttribute>();
|
||||||
|
|
||||||
|
if (attribute == null)
|
||||||
|
continue;
|
||||||
|
|
||||||
|
ValidateMethod(method, attribute);
|
||||||
|
|
||||||
|
var handler =
|
||||||
|
(Func<SimulationVehicle, bool>)method.CreateDelegate(
|
||||||
|
typeof(Func<SimulationVehicle, bool>));
|
||||||
|
|
||||||
|
var descriptor = new SimulationActionDescriptor(
|
||||||
|
attribute.Key,
|
||||||
|
attribute.DisplayName,
|
||||||
|
attribute.Group,
|
||||||
|
attribute.Order);
|
||||||
|
|
||||||
|
if (!actions.TryAdd(
|
||||||
|
attribute.Key,
|
||||||
|
new RegisteredAction(descriptor, handler)))
|
||||||
|
{
|
||||||
|
throw new InvalidOperationException(
|
||||||
|
$"仿真命令Key重复:{attribute.Key}。");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return actions;
|
||||||
|
}
|
||||||
|
|
||||||
|
private static void ValidateMethod(
|
||||||
|
MethodInfo method,
|
||||||
|
SimulationActionAttribute attribute)
|
||||||
|
{
|
||||||
|
var parameters = method.GetParameters();
|
||||||
|
|
||||||
|
if (method.ReturnType != typeof(bool) ||
|
||||||
|
parameters.Length != 1 ||
|
||||||
|
parameters[0].ParameterType !=
|
||||||
|
typeof(SimulationVehicle))
|
||||||
|
{
|
||||||
|
throw new InvalidOperationException(
|
||||||
|
$"[{nameof(SimulationActionAttribute)}]方法" +
|
||||||
|
$"{method.DeclaringType?.FullName}.{method.Name}" +
|
||||||
|
"必须是static bool Xxx(SimulationVehicle vehicle)。");
|
||||||
|
}
|
||||||
|
|
||||||
|
if (string.IsNullOrWhiteSpace(attribute.Key) ||
|
||||||
|
string.IsNullOrWhiteSpace(attribute.DisplayName) ||
|
||||||
|
string.IsNullOrWhiteSpace(attribute.Group))
|
||||||
|
{
|
||||||
|
throw new InvalidOperationException(
|
||||||
|
$"仿真命令{method.Name}的特性参数不能为空。");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
private sealed record RegisteredAction(
|
||||||
|
SimulationActionDescriptor Descriptor,
|
||||||
|
Func<SimulationVehicle, bool> Handler);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 网页测试命令的执行结果。
|
||||||
|
/// </summary>
|
||||||
|
public sealed record CommandResult(
|
||||||
|
bool Success,
|
||||||
|
string Message);
|
||||||
@@ -0,0 +1,38 @@
|
|||||||
|
using System.Diagnostics;
|
||||||
|
|
||||||
|
namespace MyParking.Simulation.Core;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 以固定周期推进离线仿真世界。
|
||||||
|
/// </summary>
|
||||||
|
public sealed class SimulationClock(
|
||||||
|
SimulationWorld world,
|
||||||
|
ILogger<SimulationClock> logger) : BackgroundService
|
||||||
|
{
|
||||||
|
private static readonly TimeSpan TickInterval =
|
||||||
|
TimeSpan.FromMilliseconds(20);
|
||||||
|
|
||||||
|
protected override async Task ExecuteAsync(
|
||||||
|
CancellationToken stoppingToken)
|
||||||
|
{
|
||||||
|
logger.LogInformation(
|
||||||
|
"停车机器人离线仿真时钟已启动,周期{Period}ms。",
|
||||||
|
TickInterval.TotalMilliseconds);
|
||||||
|
|
||||||
|
using var timer = new PeriodicTimer(TickInterval);
|
||||||
|
var stopwatch = Stopwatch.StartNew();
|
||||||
|
var previousSeconds = stopwatch.Elapsed.TotalSeconds;
|
||||||
|
|
||||||
|
while (await timer.WaitForNextTickAsync(stoppingToken))
|
||||||
|
{
|
||||||
|
var currentSeconds = stopwatch.Elapsed.TotalSeconds;
|
||||||
|
var deltaTimeSeconds = Math.Clamp(
|
||||||
|
currentSeconds - previousSeconds,
|
||||||
|
0.001,
|
||||||
|
0.1);
|
||||||
|
|
||||||
|
previousSeconds = currentSeconds;
|
||||||
|
world.Step(deltaTimeSeconds);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -0,0 +1,382 @@
|
|||||||
|
using MyParking.Shared;
|
||||||
|
using MyParking.Simulation.Models;
|
||||||
|
|
||||||
|
namespace MyParking.Simulation.Core;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 保存单辆四舵轮停车机器人的离线仿真状态。
|
||||||
|
/// </summary>
|
||||||
|
public sealed class SimulationVehicle
|
||||||
|
{
|
||||||
|
public const double BodyLengthMeters = 1.472;
|
||||||
|
public const double BodyWidthMeters = 0.948;
|
||||||
|
|
||||||
|
// 当前MDCSToolBox.dll的MultiWheelChassisInitializer运行时轮位:
|
||||||
|
// X=±750mm、Y=±500mm,舵角机械限位为±120°。
|
||||||
|
private const double WheelX = 0.75;
|
||||||
|
private const double WheelY = 0.5;
|
||||||
|
private const double MaximumBodyAcceleration = 0.6;
|
||||||
|
private const double MaximumAngularAcceleration = 0.8;
|
||||||
|
|
||||||
|
private readonly List<VirtualSteerWheel> _wheels;
|
||||||
|
private readonly double _initialX;
|
||||||
|
private readonly double _initialY;
|
||||||
|
private readonly double _initialYaw;
|
||||||
|
|
||||||
|
private Twist2D _targetBodyTwist = Twist2D.Zero;
|
||||||
|
private double _actualVx;
|
||||||
|
private double _actualVy;
|
||||||
|
private double _actualOmega;
|
||||||
|
|
||||||
|
public SimulationVehicle(
|
||||||
|
int vehicleId,
|
||||||
|
double initialX,
|
||||||
|
double initialY,
|
||||||
|
double initialYaw)
|
||||||
|
{
|
||||||
|
VehicleId = vehicleId;
|
||||||
|
_initialX = initialX;
|
||||||
|
_initialY = initialY;
|
||||||
|
_initialYaw = initialYaw;
|
||||||
|
|
||||||
|
_wheels =
|
||||||
|
[
|
||||||
|
new VirtualSteerWheel("左前", WheelX, WheelY),
|
||||||
|
new VirtualSteerWheel("右前", WheelX, -WheelY),
|
||||||
|
new VirtualSteerWheel("左后", -WheelX, WheelY),
|
||||||
|
new VirtualSteerWheel("右后", -WheelX, -WheelY)
|
||||||
|
];
|
||||||
|
|
||||||
|
Reset();
|
||||||
|
}
|
||||||
|
|
||||||
|
public int VehicleId { get; }
|
||||||
|
|
||||||
|
public string Mode { get; private set; } = "Normal";
|
||||||
|
|
||||||
|
public double XMeters { get; private set; }
|
||||||
|
|
||||||
|
public double YMeters { get; private set; }
|
||||||
|
|
||||||
|
public double YawRadians { get; private set; }
|
||||||
|
|
||||||
|
public bool ModeReady => _wheels.All(wheel => wheel.IsAligned);
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 停车并切换舵轮准备模式。
|
||||||
|
/// </summary>
|
||||||
|
public bool SetMode(string mode)
|
||||||
|
{
|
||||||
|
Stop();
|
||||||
|
|
||||||
|
var success = mode switch
|
||||||
|
{
|
||||||
|
"Normal" => PrepareParallelDirection(0.0),
|
||||||
|
"CrabLeft" => PrepareParallelDirection(90.0),
|
||||||
|
"CrabRight" => PrepareParallelDirection(-90.0),
|
||||||
|
"Spin" => PrepareSpinDirection(),
|
||||||
|
_ => false
|
||||||
|
};
|
||||||
|
|
||||||
|
if (success)
|
||||||
|
Mode = mode;
|
||||||
|
|
||||||
|
return success;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 按当前模式发送前进或后退命令。
|
||||||
|
/// </summary>
|
||||||
|
public bool Move(double directionSign)
|
||||||
|
{
|
||||||
|
if (!ModeReady)
|
||||||
|
return false;
|
||||||
|
|
||||||
|
const double linearSpeed = 0.35;
|
||||||
|
const double angularSpeed = 0.45;
|
||||||
|
|
||||||
|
var command = Mode switch
|
||||||
|
{
|
||||||
|
"Normal" => new Twist2D(
|
||||||
|
directionSign * linearSpeed, 0.0, 0.0),
|
||||||
|
"CrabLeft" => new Twist2D(
|
||||||
|
0.0, directionSign * linearSpeed, 0.0),
|
||||||
|
"CrabRight" => new Twist2D(
|
||||||
|
0.0, -directionSign * linearSpeed, 0.0),
|
||||||
|
"Spin" => new Twist2D(
|
||||||
|
0.0, 0.0, directionSign * angularSpeed),
|
||||||
|
_ => Twist2D.Zero
|
||||||
|
};
|
||||||
|
|
||||||
|
return ApplyCommand(
|
||||||
|
new ChassisCommand(VehicleId, command));
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 在当前运动模式下增加逆时针或顺时针转动。
|
||||||
|
/// </summary>
|
||||||
|
public bool Turn(double directionSign)
|
||||||
|
{
|
||||||
|
if (!ModeReady || Mode == "Spin")
|
||||||
|
return false;
|
||||||
|
|
||||||
|
var command = new Twist2D(
|
||||||
|
_targetBodyTwist.VxMetersPerSecond,
|
||||||
|
_targetBodyTwist.VyMetersPerSecond,
|
||||||
|
directionSign * 0.28);
|
||||||
|
|
||||||
|
return ApplyCommand(
|
||||||
|
new ChassisCommand(VehicleId, command));
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 将网页虚拟遥控器的油门和转向组合为连续车体速度命令。
|
||||||
|
/// </summary>
|
||||||
|
public bool ManualDrive(
|
||||||
|
double throttle,
|
||||||
|
double steering,
|
||||||
|
double speedScale,
|
||||||
|
double steeringScale)
|
||||||
|
{
|
||||||
|
if (!AreFinite(
|
||||||
|
throttle,
|
||||||
|
steering,
|
||||||
|
speedScale,
|
||||||
|
steeringScale))
|
||||||
|
{
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
throttle = Math.Clamp(throttle, -1.0, 1.0);
|
||||||
|
steering = Math.Clamp(steering, -1.0, 1.0);
|
||||||
|
speedScale = Math.Clamp(speedScale, 0.0, 1.0);
|
||||||
|
steeringScale = Math.Clamp(steeringScale, 0.0, 1.0);
|
||||||
|
|
||||||
|
if (Math.Abs(throttle) < 0.001 &&
|
||||||
|
Math.Abs(steering) < 0.001)
|
||||||
|
{
|
||||||
|
Stop();
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!ModeReady)
|
||||||
|
return false;
|
||||||
|
|
||||||
|
const double maximumLinearSpeed = 0.6;
|
||||||
|
const double maximumAngularSpeed = 0.7;
|
||||||
|
|
||||||
|
var linearSpeed =
|
||||||
|
throttle * maximumLinearSpeed * speedScale;
|
||||||
|
var angularSpeed =
|
||||||
|
steering * maximumAngularSpeed * steeringScale;
|
||||||
|
|
||||||
|
var twist = Mode switch
|
||||||
|
{
|
||||||
|
"Normal" => new Twist2D(
|
||||||
|
linearSpeed,
|
||||||
|
0.0,
|
||||||
|
angularSpeed),
|
||||||
|
"CrabLeft" => new Twist2D(
|
||||||
|
0.0,
|
||||||
|
linearSpeed,
|
||||||
|
angularSpeed),
|
||||||
|
"CrabRight" => new Twist2D(
|
||||||
|
0.0,
|
||||||
|
-linearSpeed,
|
||||||
|
angularSpeed),
|
||||||
|
"Spin" => new Twist2D(
|
||||||
|
0.0,
|
||||||
|
0.0,
|
||||||
|
angularSpeed),
|
||||||
|
_ => Twist2D.Zero
|
||||||
|
};
|
||||||
|
|
||||||
|
return ApplyCommand(
|
||||||
|
new ChassisCommand(VehicleId, twist));
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 应用统一车体速度命令并分解为四个舵轮速度向量。
|
||||||
|
/// </summary>
|
||||||
|
public bool ApplyCommand(ChassisCommand command)
|
||||||
|
{
|
||||||
|
if (command.VehicleId != VehicleId)
|
||||||
|
return false;
|
||||||
|
|
||||||
|
var twist = command.BodyTwist;
|
||||||
|
var wheelCommands = _wheels.Select(wheel =>
|
||||||
|
{
|
||||||
|
var wheelVx =
|
||||||
|
twist.VxMetersPerSecond -
|
||||||
|
twist.OmegaRadiansPerSecond * wheel.YMeters;
|
||||||
|
|
||||||
|
var wheelVy =
|
||||||
|
twist.VyMetersPerSecond +
|
||||||
|
twist.OmegaRadiansPerSecond * wheel.XMeters;
|
||||||
|
|
||||||
|
return (Wheel: wheel, Vx: wheelVx, Vy: wheelVy);
|
||||||
|
}).ToArray();
|
||||||
|
|
||||||
|
foreach (var item in wheelCommands)
|
||||||
|
{
|
||||||
|
if (!item.Wheel.SetVelocityVector(
|
||||||
|
item.Vx,
|
||||||
|
item.Vy))
|
||||||
|
{
|
||||||
|
Stop();
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
_targetBodyTwist = twist;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 将车辆目标速度设置为零并保持当前舵轮角度。
|
||||||
|
/// </summary>
|
||||||
|
public void Stop()
|
||||||
|
{
|
||||||
|
_targetBodyTwist = Twist2D.Zero;
|
||||||
|
foreach (var wheel in _wheels)
|
||||||
|
wheel.Stop();
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 更新舵轮反馈和车辆世界位姿。
|
||||||
|
/// </summary>
|
||||||
|
public void Step(double deltaTimeSeconds)
|
||||||
|
{
|
||||||
|
foreach (var wheel in _wheels)
|
||||||
|
wheel.Step(deltaTimeSeconds);
|
||||||
|
|
||||||
|
var canMove = _wheels.All(wheel => wheel.IsAligned);
|
||||||
|
var targetVx = canMove
|
||||||
|
? _targetBodyTwist.VxMetersPerSecond
|
||||||
|
: 0.0;
|
||||||
|
var targetVy = canMove
|
||||||
|
? _targetBodyTwist.VyMetersPerSecond
|
||||||
|
: 0.0;
|
||||||
|
var targetOmega = canMove
|
||||||
|
? _targetBodyTwist.OmegaRadiansPerSecond
|
||||||
|
: 0.0;
|
||||||
|
|
||||||
|
_actualVx = MoveTowards(
|
||||||
|
_actualVx,
|
||||||
|
targetVx,
|
||||||
|
MaximumBodyAcceleration * deltaTimeSeconds);
|
||||||
|
|
||||||
|
_actualVy = MoveTowards(
|
||||||
|
_actualVy,
|
||||||
|
targetVy,
|
||||||
|
MaximumBodyAcceleration * deltaTimeSeconds);
|
||||||
|
|
||||||
|
_actualOmega = MoveTowards(
|
||||||
|
_actualOmega,
|
||||||
|
targetOmega,
|
||||||
|
MaximumAngularAcceleration * deltaTimeSeconds);
|
||||||
|
|
||||||
|
var cos = Math.Cos(YawRadians);
|
||||||
|
var sin = Math.Sin(YawRadians);
|
||||||
|
|
||||||
|
var worldVx = cos * _actualVx - sin * _actualVy;
|
||||||
|
var worldVy = sin * _actualVx + cos * _actualVy;
|
||||||
|
|
||||||
|
XMeters += worldVx * deltaTimeSeconds;
|
||||||
|
YMeters += worldVy * deltaTimeSeconds;
|
||||||
|
YawRadians = FrameTransform2D.NormalizeAngle(
|
||||||
|
YawRadians + _actualOmega * deltaTimeSeconds);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 恢复车辆初始位置和舵轮状态。
|
||||||
|
/// </summary>
|
||||||
|
public void Reset()
|
||||||
|
{
|
||||||
|
XMeters = _initialX;
|
||||||
|
YMeters = _initialY;
|
||||||
|
YawRadians = _initialYaw;
|
||||||
|
Mode = "Normal";
|
||||||
|
_targetBodyTwist = Twist2D.Zero;
|
||||||
|
_actualVx = 0.0;
|
||||||
|
_actualVy = 0.0;
|
||||||
|
_actualOmega = 0.0;
|
||||||
|
|
||||||
|
foreach (var wheel in _wheels)
|
||||||
|
wheel.Reset();
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 创建供网页读取的不可变状态快照。
|
||||||
|
/// </summary>
|
||||||
|
public VehicleStateDto GetSnapshot()
|
||||||
|
{
|
||||||
|
return new VehicleStateDto(
|
||||||
|
VehicleId,
|
||||||
|
XMeters,
|
||||||
|
YMeters,
|
||||||
|
YawRadians,
|
||||||
|
BodyLengthMeters,
|
||||||
|
BodyWidthMeters,
|
||||||
|
Mode,
|
||||||
|
ModeReady,
|
||||||
|
new TwistStateDto(
|
||||||
|
_targetBodyTwist.VxMetersPerSecond,
|
||||||
|
_targetBodyTwist.VyMetersPerSecond,
|
||||||
|
_targetBodyTwist.OmegaRadiansPerSecond),
|
||||||
|
new TwistStateDto(
|
||||||
|
_actualVx,
|
||||||
|
_actualVy,
|
||||||
|
_actualOmega),
|
||||||
|
_wheels.Select(wheel =>
|
||||||
|
new WheelStateDto(
|
||||||
|
wheel.Name,
|
||||||
|
wheel.XMeters,
|
||||||
|
wheel.YMeters,
|
||||||
|
wheel.TargetAngleDegrees,
|
||||||
|
wheel.ActualAngleDegrees,
|
||||||
|
wheel.TargetSpeedMetersPerSecond,
|
||||||
|
wheel.ActualSpeedMetersPerSecond,
|
||||||
|
wheel.IsAligned)).ToArray());
|
||||||
|
}
|
||||||
|
|
||||||
|
private bool PrepareParallelDirection(double targetAngleDegrees)
|
||||||
|
{
|
||||||
|
return _wheels.All(wheel =>
|
||||||
|
wheel.PrepareDirection(targetAngleDegrees));
|
||||||
|
}
|
||||||
|
|
||||||
|
private bool PrepareSpinDirection()
|
||||||
|
{
|
||||||
|
var success = true;
|
||||||
|
|
||||||
|
foreach (var wheel in _wheels)
|
||||||
|
{
|
||||||
|
var vx = -wheel.YMeters;
|
||||||
|
var vy = wheel.XMeters;
|
||||||
|
success &= wheel.SetVelocityVector(vx, vy);
|
||||||
|
wheel.Stop();
|
||||||
|
}
|
||||||
|
|
||||||
|
return success;
|
||||||
|
}
|
||||||
|
|
||||||
|
private static double MoveTowards(
|
||||||
|
double current,
|
||||||
|
double target,
|
||||||
|
double maximumChange)
|
||||||
|
{
|
||||||
|
var difference = target - current;
|
||||||
|
if (Math.Abs(difference) <= maximumChange)
|
||||||
|
return target;
|
||||||
|
|
||||||
|
return current + Math.Sign(difference) * maximumChange;
|
||||||
|
}
|
||||||
|
|
||||||
|
private static bool AreFinite(params double[] values)
|
||||||
|
{
|
||||||
|
return values.All(value =>
|
||||||
|
!double.IsNaN(value) &&
|
||||||
|
!double.IsInfinity(value));
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -0,0 +1,205 @@
|
|||||||
|
using MyParking.Shared;
|
||||||
|
using MyParking.Simulation.Models;
|
||||||
|
|
||||||
|
namespace MyParking.Simulation.Core;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 管理离线仿真车辆、车队中心和成员布局。
|
||||||
|
/// </summary>
|
||||||
|
public sealed class SimulationWorld
|
||||||
|
{
|
||||||
|
private readonly object _syncRoot = new();
|
||||||
|
private Dictionary<int, SimulationVehicle> _vehicles = new();
|
||||||
|
private SimulationConfigurationDto _configuration;
|
||||||
|
|
||||||
|
public SimulationWorld()
|
||||||
|
{
|
||||||
|
_configuration = CreateDefaultConfiguration();
|
||||||
|
ApplyConfigurationCore(_configuration);
|
||||||
|
}
|
||||||
|
|
||||||
|
public T WithVehicle<T>(
|
||||||
|
int vehicleId,
|
||||||
|
Func<SimulationVehicle, T> action)
|
||||||
|
{
|
||||||
|
lock (_syncRoot)
|
||||||
|
{
|
||||||
|
if (!_vehicles.TryGetValue(
|
||||||
|
vehicleId,
|
||||||
|
out var vehicle))
|
||||||
|
{
|
||||||
|
throw new KeyNotFoundException(
|
||||||
|
$"不存在车辆{vehicleId}。");
|
||||||
|
}
|
||||||
|
|
||||||
|
return action(vehicle);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Step(double deltaTimeSeconds)
|
||||||
|
{
|
||||||
|
lock (_syncRoot)
|
||||||
|
{
|
||||||
|
foreach (var vehicle in _vehicles.Values)
|
||||||
|
vehicle.Step(deltaTimeSeconds);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public IReadOnlyList<VehicleStateDto> GetSnapshot()
|
||||||
|
{
|
||||||
|
lock (_syncRoot)
|
||||||
|
{
|
||||||
|
return _vehicles.Values
|
||||||
|
.OrderBy(vehicle => vehicle.VehicleId)
|
||||||
|
.Select(vehicle => vehicle.GetSnapshot())
|
||||||
|
.ToArray();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public SimulationConfigurationDto GetConfiguration()
|
||||||
|
{
|
||||||
|
lock (_syncRoot)
|
||||||
|
{
|
||||||
|
return CloneConfiguration(_configuration);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public void ApplyConfiguration(
|
||||||
|
SimulationConfigurationDto configuration)
|
||||||
|
{
|
||||||
|
ValidateConfiguration(configuration);
|
||||||
|
|
||||||
|
lock (_syncRoot)
|
||||||
|
{
|
||||||
|
_configuration = CloneConfiguration(configuration);
|
||||||
|
ApplyConfigurationCore(_configuration);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Reset()
|
||||||
|
{
|
||||||
|
lock (_syncRoot)
|
||||||
|
{
|
||||||
|
ApplyConfigurationCore(_configuration);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
private void ApplyConfigurationCore(
|
||||||
|
SimulationConfigurationDto configuration)
|
||||||
|
{
|
||||||
|
var fleetPoseInWorld = new Pose2D(
|
||||||
|
configuration.FleetCenter.XMeters,
|
||||||
|
configuration.FleetCenter.YMeters,
|
||||||
|
configuration.FleetCenter.YawRadians);
|
||||||
|
|
||||||
|
_vehicles = configuration.Vehicles
|
||||||
|
.Take(configuration.VehicleCount)
|
||||||
|
.Select(layout =>
|
||||||
|
{
|
||||||
|
var bodyPoseInFleet = new Pose2D(
|
||||||
|
layout.XMeters,
|
||||||
|
layout.YMeters,
|
||||||
|
layout.YawRadians);
|
||||||
|
|
||||||
|
var bodyPoseInWorld =
|
||||||
|
FrameTransform2D.Compose(
|
||||||
|
fleetPoseInWorld,
|
||||||
|
bodyPoseInFleet);
|
||||||
|
|
||||||
|
return new SimulationVehicle(
|
||||||
|
layout.VehicleId,
|
||||||
|
bodyPoseInWorld.XMeters,
|
||||||
|
bodyPoseInWorld.YMeters,
|
||||||
|
bodyPoseInWorld.YawRadians);
|
||||||
|
})
|
||||||
|
.ToDictionary(vehicle => vehicle.VehicleId);
|
||||||
|
}
|
||||||
|
|
||||||
|
private static void ValidateConfiguration(
|
||||||
|
SimulationConfigurationDto configuration)
|
||||||
|
{
|
||||||
|
if (configuration.VehicleCount is < 1 or > 8)
|
||||||
|
{
|
||||||
|
throw new ArgumentOutOfRangeException(
|
||||||
|
nameof(configuration.VehicleCount),
|
||||||
|
"仿真车辆数量必须在1到8之间。");
|
||||||
|
}
|
||||||
|
|
||||||
|
if (configuration.FleetCenter == null)
|
||||||
|
{
|
||||||
|
throw new ArgumentException(
|
||||||
|
"必须提供车队中心位姿。",
|
||||||
|
nameof(configuration));
|
||||||
|
}
|
||||||
|
|
||||||
|
if (configuration.Vehicles == null ||
|
||||||
|
configuration.Vehicles.Count <
|
||||||
|
configuration.VehicleCount)
|
||||||
|
{
|
||||||
|
throw new ArgumentException(
|
||||||
|
"成员布局数量不能少于车辆数量。",
|
||||||
|
nameof(configuration));
|
||||||
|
}
|
||||||
|
|
||||||
|
var selectedLayouts = configuration.Vehicles
|
||||||
|
.Take(configuration.VehicleCount)
|
||||||
|
.ToArray();
|
||||||
|
|
||||||
|
if (selectedLayouts.Any(layout =>
|
||||||
|
layout.VehicleId <= 0) ||
|
||||||
|
selectedLayouts
|
||||||
|
.Select(layout => layout.VehicleId)
|
||||||
|
.Distinct()
|
||||||
|
.Count() != selectedLayouts.Length)
|
||||||
|
{
|
||||||
|
throw new ArgumentException(
|
||||||
|
"车辆编号必须大于零且不能重复。",
|
||||||
|
nameof(configuration));
|
||||||
|
}
|
||||||
|
|
||||||
|
var values = new[]
|
||||||
|
{
|
||||||
|
configuration.FleetCenter.XMeters,
|
||||||
|
configuration.FleetCenter.YMeters,
|
||||||
|
configuration.FleetCenter.YawRadians
|
||||||
|
}.Concat(selectedLayouts.SelectMany(layout => new[]
|
||||||
|
{
|
||||||
|
layout.XMeters,
|
||||||
|
layout.YMeters,
|
||||||
|
layout.YawRadians
|
||||||
|
}));
|
||||||
|
|
||||||
|
if (values.Any(value =>
|
||||||
|
double.IsNaN(value) ||
|
||||||
|
double.IsInfinity(value)))
|
||||||
|
{
|
||||||
|
throw new ArgumentException(
|
||||||
|
"车队和车辆布局不能包含NaN或无穷大。",
|
||||||
|
nameof(configuration));
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
private static SimulationConfigurationDto
|
||||||
|
CreateDefaultConfiguration()
|
||||||
|
{
|
||||||
|
return new SimulationConfigurationDto(
|
||||||
|
1,
|
||||||
|
new FleetCenterDto(0.0, 0.0, 0.0),
|
||||||
|
new[]
|
||||||
|
{
|
||||||
|
new VehicleLayoutDto(1, 0.0, 0.0, 0.0)
|
||||||
|
});
|
||||||
|
}
|
||||||
|
|
||||||
|
private static SimulationConfigurationDto
|
||||||
|
CloneConfiguration(
|
||||||
|
SimulationConfigurationDto configuration)
|
||||||
|
{
|
||||||
|
return new SimulationConfigurationDto(
|
||||||
|
configuration.VehicleCount,
|
||||||
|
configuration.FleetCenter with { },
|
||||||
|
configuration.Vehicles
|
||||||
|
.Select(layout => layout with { })
|
||||||
|
.ToArray());
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -0,0 +1,174 @@
|
|||||||
|
namespace MyParking.Simulation.Core;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 模拟单个舵轮的转向和驱动响应,不包含真实电机物理模型。
|
||||||
|
/// </summary>
|
||||||
|
public sealed class VirtualSteerWheel
|
||||||
|
{
|
||||||
|
private const double AngleLowerLimitDegrees = -120.0;
|
||||||
|
private const double AngleUpperLimitDegrees = 120.0;
|
||||||
|
private const double MaximumSteeringRateDegreesPerSecond = 90.0;
|
||||||
|
private const double MaximumDriveAccelerationMetersPerSecondSquared = 0.8;
|
||||||
|
private const double AlignmentToleranceDegrees = 1.5;
|
||||||
|
|
||||||
|
public VirtualSteerWheel(
|
||||||
|
string name,
|
||||||
|
double xMeters,
|
||||||
|
double yMeters)
|
||||||
|
{
|
||||||
|
Name = name;
|
||||||
|
XMeters = xMeters;
|
||||||
|
YMeters = yMeters;
|
||||||
|
}
|
||||||
|
|
||||||
|
public string Name { get; }
|
||||||
|
|
||||||
|
public double XMeters { get; }
|
||||||
|
|
||||||
|
public double YMeters { get; }
|
||||||
|
|
||||||
|
public double TargetAngleDegrees { get; private set; }
|
||||||
|
|
||||||
|
public double ActualAngleDegrees { get; private set; }
|
||||||
|
|
||||||
|
public double TargetSpeedMetersPerSecond { get; private set; }
|
||||||
|
|
||||||
|
public double ActualSpeedMetersPerSecond { get; private set; }
|
||||||
|
|
||||||
|
public bool IsAligned =>
|
||||||
|
Math.Abs(TargetAngleDegrees - ActualAngleDegrees) <=
|
||||||
|
AlignmentToleranceDegrees;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 设置期望轮胎速度向量,并在机械限位内选择等价舵角。
|
||||||
|
/// </summary>
|
||||||
|
public bool SetVelocityVector(
|
||||||
|
double vxMetersPerSecond,
|
||||||
|
double vyMetersPerSecond)
|
||||||
|
{
|
||||||
|
var speed = Math.Sqrt(
|
||||||
|
vxMetersPerSecond * vxMetersPerSecond +
|
||||||
|
vyMetersPerSecond * vyMetersPerSecond);
|
||||||
|
|
||||||
|
if (speed < 1e-6)
|
||||||
|
{
|
||||||
|
TargetSpeedMetersPerSecond = 0.0;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
var desiredAngleDegrees =
|
||||||
|
Math.Atan2(vyMetersPerSecond, vxMetersPerSecond) *
|
||||||
|
180.0 / Math.PI;
|
||||||
|
|
||||||
|
return SetDirectionAndSpeed(
|
||||||
|
desiredAngleDegrees,
|
||||||
|
speed);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 停车时设置舵轮预对齐方向。
|
||||||
|
/// </summary>
|
||||||
|
public bool PrepareDirection(double targetAngleDegrees)
|
||||||
|
{
|
||||||
|
TargetSpeedMetersPerSecond = 0.0;
|
||||||
|
return SetDirectionAndSpeed(
|
||||||
|
targetAngleDegrees,
|
||||||
|
0.0);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 将驱动目标设置为零,并保持当前舵轮方向。
|
||||||
|
/// </summary>
|
||||||
|
public void Stop()
|
||||||
|
{
|
||||||
|
TargetSpeedMetersPerSecond = 0.0;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 按固定转向速度和驱动加速度更新虚拟反馈。
|
||||||
|
/// </summary>
|
||||||
|
public void Step(double deltaTimeSeconds)
|
||||||
|
{
|
||||||
|
ActualAngleDegrees = MoveTowards(
|
||||||
|
ActualAngleDegrees,
|
||||||
|
TargetAngleDegrees,
|
||||||
|
MaximumSteeringRateDegreesPerSecond *
|
||||||
|
deltaTimeSeconds);
|
||||||
|
|
||||||
|
var allowedTargetSpeed =
|
||||||
|
IsAligned ? TargetSpeedMetersPerSecond : 0.0;
|
||||||
|
|
||||||
|
ActualSpeedMetersPerSecond = MoveTowards(
|
||||||
|
ActualSpeedMetersPerSecond,
|
||||||
|
allowedTargetSpeed,
|
||||||
|
MaximumDriveAccelerationMetersPerSecondSquared *
|
||||||
|
deltaTimeSeconds);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 恢复舵轮初始状态。
|
||||||
|
/// </summary>
|
||||||
|
public void Reset()
|
||||||
|
{
|
||||||
|
TargetAngleDegrees = 0.0;
|
||||||
|
ActualAngleDegrees = 0.0;
|
||||||
|
TargetSpeedMetersPerSecond = 0.0;
|
||||||
|
ActualSpeedMetersPerSecond = 0.0;
|
||||||
|
}
|
||||||
|
|
||||||
|
private bool SetDirectionAndSpeed(
|
||||||
|
double desiredAngleDegrees,
|
||||||
|
double desiredSpeedMetersPerSecond)
|
||||||
|
{
|
||||||
|
var candidates = new[]
|
||||||
|
{
|
||||||
|
(Angle: NormalizeDegrees(desiredAngleDegrees),
|
||||||
|
Speed: desiredSpeedMetersPerSecond),
|
||||||
|
(Angle: NormalizeDegrees(desiredAngleDegrees + 180.0),
|
||||||
|
Speed: -desiredSpeedMetersPerSecond),
|
||||||
|
(Angle: NormalizeDegrees(desiredAngleDegrees - 180.0),
|
||||||
|
Speed: -desiredSpeedMetersPerSecond)
|
||||||
|
};
|
||||||
|
|
||||||
|
var validCandidates = candidates
|
||||||
|
.Where(candidate =>
|
||||||
|
candidate.Angle >= AngleLowerLimitDegrees &&
|
||||||
|
candidate.Angle <= AngleUpperLimitDegrees)
|
||||||
|
.OrderBy(candidate =>
|
||||||
|
Math.Abs(candidate.Angle - ActualAngleDegrees))
|
||||||
|
.ToArray();
|
||||||
|
|
||||||
|
if (validCandidates.Length == 0)
|
||||||
|
{
|
||||||
|
TargetSpeedMetersPerSecond = 0.0;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
var selected = validCandidates[0];
|
||||||
|
TargetAngleDegrees = selected.Angle;
|
||||||
|
TargetSpeedMetersPerSecond = selected.Speed;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
private static double MoveTowards(
|
||||||
|
double current,
|
||||||
|
double target,
|
||||||
|
double maximumChange)
|
||||||
|
{
|
||||||
|
var difference = target - current;
|
||||||
|
if (Math.Abs(difference) <= maximumChange)
|
||||||
|
return target;
|
||||||
|
|
||||||
|
return current + Math.Sign(difference) * maximumChange;
|
||||||
|
}
|
||||||
|
|
||||||
|
private static double NormalizeDegrees(double angleDegrees)
|
||||||
|
{
|
||||||
|
angleDegrees %= 360.0;
|
||||||
|
if (angleDegrees >= 180.0)
|
||||||
|
angleDegrees -= 360.0;
|
||||||
|
if (angleDegrees < -180.0)
|
||||||
|
angleDegrees += 360.0;
|
||||||
|
return angleDegrees;
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -0,0 +1,10 @@
|
|||||||
|
namespace MyParking.Simulation.Models;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 网页虚拟遥控器输入,所有输入范围均为-1到1。
|
||||||
|
/// </summary>
|
||||||
|
public sealed record ManualControlInputDto(
|
||||||
|
double Throttle,
|
||||||
|
double Steering,
|
||||||
|
double SpeedScale,
|
||||||
|
double SteeringScale);
|
||||||
@@ -0,0 +1,26 @@
|
|||||||
|
namespace MyParking.Simulation.Models;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 离线仿真的车辆数量、车队中心和成员相对布局。
|
||||||
|
/// </summary>
|
||||||
|
public sealed record SimulationConfigurationDto(
|
||||||
|
int VehicleCount,
|
||||||
|
FleetCenterDto FleetCenter,
|
||||||
|
IReadOnlyList<VehicleLayoutDto> Vehicles);
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 车队中心在世界坐标系中的位姿。
|
||||||
|
/// </summary>
|
||||||
|
public sealed record FleetCenterDto(
|
||||||
|
double XMeters,
|
||||||
|
double YMeters,
|
||||||
|
double YawRadians);
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 单车车体坐标系在车队中心坐标系中的位姿。
|
||||||
|
/// </summary>
|
||||||
|
public sealed record VehicleLayoutDto(
|
||||||
|
int VehicleId,
|
||||||
|
double XMeters,
|
||||||
|
double YMeters,
|
||||||
|
double YawRadians);
|
||||||
@@ -0,0 +1,25 @@
|
|||||||
|
namespace MyParking.Simulation.Models;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 网页绘制单辆停车机器人所需的状态快照。
|
||||||
|
/// </summary>
|
||||||
|
public sealed record VehicleStateDto(
|
||||||
|
int VehicleId,
|
||||||
|
double XMeters,
|
||||||
|
double YMeters,
|
||||||
|
double YawRadians,
|
||||||
|
double BodyLengthMeters,
|
||||||
|
double BodyWidthMeters,
|
||||||
|
string Mode,
|
||||||
|
bool ModeReady,
|
||||||
|
TwistStateDto TargetBodyTwist,
|
||||||
|
TwistStateDto ActualBodyTwist,
|
||||||
|
IReadOnlyList<WheelStateDto> Wheels);
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 网页显示的二维车体速度快照。
|
||||||
|
/// </summary>
|
||||||
|
public sealed record TwistStateDto(
|
||||||
|
double VxMetersPerSecond,
|
||||||
|
double VyMetersPerSecond,
|
||||||
|
double OmegaRadiansPerSecond);
|
||||||
@@ -0,0 +1,14 @@
|
|||||||
|
namespace MyParking.Simulation.Models;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 网页绘制单个舵轮所需的状态快照。
|
||||||
|
/// </summary>
|
||||||
|
public sealed record WheelStateDto(
|
||||||
|
string Name,
|
||||||
|
double XMeters,
|
||||||
|
double YMeters,
|
||||||
|
double TargetAngleDegrees,
|
||||||
|
double ActualAngleDegrees,
|
||||||
|
double TargetSpeedMetersPerSecond,
|
||||||
|
double ActualSpeedMetersPerSecond,
|
||||||
|
bool IsAligned);
|
||||||
@@ -0,0 +1,17 @@
|
|||||||
|
<Project Sdk="Microsoft.NET.Sdk.Web">
|
||||||
|
|
||||||
|
<PropertyGroup>
|
||||||
|
<TargetFramework>net8.0</TargetFramework>
|
||||||
|
<Nullable>enable</Nullable>
|
||||||
|
<ImplicitUsings>enable</ImplicitUsings>
|
||||||
|
<RootNamespace>MyParking.Simulation</RootNamespace>
|
||||||
|
</PropertyGroup>
|
||||||
|
|
||||||
|
<ItemGroup>
|
||||||
|
<Compile Include="..\Shared\ChassisCommand.cs"
|
||||||
|
Link="Shared\ChassisCommand.cs" />
|
||||||
|
<Compile Include="..\Shared\FrameTransform2D.cs"
|
||||||
|
Link="Shared\FrameTransform2D.cs" />
|
||||||
|
</ItemGroup>
|
||||||
|
|
||||||
|
</Project>
|
||||||
@@ -0,0 +1,97 @@
|
|||||||
|
using MyParking.Simulation.Commands;
|
||||||
|
using MyParking.Simulation.Core;
|
||||||
|
|
||||||
|
var builder = WebApplication.CreateBuilder(args);
|
||||||
|
|
||||||
|
builder.Services.AddSingleton<SimulationWorld>();
|
||||||
|
builder.Services.AddSingleton<SimulationCommandDispatcher>();
|
||||||
|
builder.Services.AddHostedService<SimulationClock>();
|
||||||
|
|
||||||
|
var app = builder.Build();
|
||||||
|
|
||||||
|
app.UseDefaultFiles();
|
||||||
|
app.UseStaticFiles();
|
||||||
|
|
||||||
|
app.MapGet("/api/vehicles", (SimulationWorld world) =>
|
||||||
|
Results.Ok(world.GetSnapshot()));
|
||||||
|
|
||||||
|
app.MapGet(
|
||||||
|
"/api/actions",
|
||||||
|
(SimulationCommandDispatcher dispatcher) =>
|
||||||
|
Results.Ok(dispatcher.GetActions()));
|
||||||
|
|
||||||
|
app.MapGet("/api/configuration", (SimulationWorld world) =>
|
||||||
|
Results.Ok(world.GetConfiguration()));
|
||||||
|
|
||||||
|
app.MapPost(
|
||||||
|
"/api/configuration",
|
||||||
|
(MyParking.Simulation.Models.SimulationConfigurationDto configuration,
|
||||||
|
SimulationWorld world) =>
|
||||||
|
{
|
||||||
|
try
|
||||||
|
{
|
||||||
|
world.ApplyConfiguration(configuration);
|
||||||
|
return Results.Ok(world.GetConfiguration());
|
||||||
|
}
|
||||||
|
catch (ArgumentException exception)
|
||||||
|
{
|
||||||
|
return Results.BadRequest(new
|
||||||
|
{
|
||||||
|
message = exception.Message
|
||||||
|
});
|
||||||
|
}
|
||||||
|
});
|
||||||
|
|
||||||
|
app.MapPost(
|
||||||
|
"/api/vehicles/{vehicleId:int}/commands/{command}",
|
||||||
|
(int vehicleId, string command, SimulationCommandDispatcher dispatcher) =>
|
||||||
|
{
|
||||||
|
var result = dispatcher.Execute(vehicleId, command);
|
||||||
|
return result.Success
|
||||||
|
? Results.Ok(result)
|
||||||
|
: Results.BadRequest(result);
|
||||||
|
});
|
||||||
|
|
||||||
|
app.MapPost(
|
||||||
|
"/api/vehicles/{vehicleId:int}/manual-control",
|
||||||
|
(int vehicleId,
|
||||||
|
MyParking.Simulation.Models.ManualControlInputDto input,
|
||||||
|
SimulationWorld world) =>
|
||||||
|
{
|
||||||
|
try
|
||||||
|
{
|
||||||
|
var success = world.WithVehicle(
|
||||||
|
vehicleId,
|
||||||
|
vehicle => vehicle.ManualDrive(
|
||||||
|
input.Throttle,
|
||||||
|
input.Steering,
|
||||||
|
input.SpeedScale,
|
||||||
|
input.SteeringScale));
|
||||||
|
|
||||||
|
var result = new CommandResult(
|
||||||
|
success,
|
||||||
|
success
|
||||||
|
? $"车辆{vehicleId}虚拟遥控输入已更新。"
|
||||||
|
: $"车辆{vehicleId}舵轮尚未到位或输入无效。");
|
||||||
|
|
||||||
|
return success
|
||||||
|
? Results.Ok(result)
|
||||||
|
: Results.BadRequest(result);
|
||||||
|
}
|
||||||
|
catch (KeyNotFoundException exception)
|
||||||
|
{
|
||||||
|
return Results.NotFound(new CommandResult(
|
||||||
|
false,
|
||||||
|
exception.Message));
|
||||||
|
}
|
||||||
|
});
|
||||||
|
|
||||||
|
app.MapPost("/api/reset", (SimulationWorld world) =>
|
||||||
|
{
|
||||||
|
world.Reset();
|
||||||
|
return Results.Ok(new { message = "全部仿真车已复位。" });
|
||||||
|
});
|
||||||
|
|
||||||
|
app.MapFallbackToFile("index.html");
|
||||||
|
|
||||||
|
app.Run();
|
||||||
@@ -0,0 +1,38 @@
|
|||||||
|
{
|
||||||
|
"$schema": "http://json.schemastore.org/launchsettings.json",
|
||||||
|
"iisSettings": {
|
||||||
|
"windowsAuthentication": false,
|
||||||
|
"anonymousAuthentication": true,
|
||||||
|
"iisExpress": {
|
||||||
|
"applicationUrl": "http://localhost:1095",
|
||||||
|
"sslPort": 44395
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"profiles": {
|
||||||
|
"http": {
|
||||||
|
"commandName": "Project",
|
||||||
|
"dotnetRunMessages": true,
|
||||||
|
"launchBrowser": true,
|
||||||
|
"applicationUrl": "http://localhost:5203",
|
||||||
|
"environmentVariables": {
|
||||||
|
"ASPNETCORE_ENVIRONMENT": "Development"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"https": {
|
||||||
|
"commandName": "Project",
|
||||||
|
"dotnetRunMessages": true,
|
||||||
|
"launchBrowser": true,
|
||||||
|
"applicationUrl": "https://localhost:7055;http://localhost:5203",
|
||||||
|
"environmentVariables": {
|
||||||
|
"ASPNETCORE_ENVIRONMENT": "Development"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"IIS Express": {
|
||||||
|
"commandName": "IISExpress",
|
||||||
|
"launchBrowser": true,
|
||||||
|
"environmentVariables": {
|
||||||
|
"ASPNETCORE_ENVIRONMENT": "Development"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -0,0 +1,8 @@
|
|||||||
|
{
|
||||||
|
"Logging": {
|
||||||
|
"LogLevel": {
|
||||||
|
"Default": "Information",
|
||||||
|
"Microsoft.AspNetCore": "Warning"
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -0,0 +1,9 @@
|
|||||||
|
{
|
||||||
|
"Logging": {
|
||||||
|
"LogLevel": {
|
||||||
|
"Default": "Information",
|
||||||
|
"Microsoft.AspNetCore": "Warning"
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"AllowedHosts": "*"
|
||||||
|
}
|
||||||
@@ -0,0 +1,144 @@
|
|||||||
|
<!doctype html>
|
||||||
|
<html lang="zh-CN">
|
||||||
|
<head>
|
||||||
|
<meta charset="utf-8">
|
||||||
|
<meta name="viewport" content="width=device-width, initial-scale=1">
|
||||||
|
<title>停车机器人离线测试台</title>
|
||||||
|
<link rel="stylesheet" href="viewer.css">
|
||||||
|
</head>
|
||||||
|
<body>
|
||||||
|
<main class="app-shell">
|
||||||
|
<header class="topbar">
|
||||||
|
<div>
|
||||||
|
<p class="eyebrow">MY PARKING · OFFLINE LAB</p>
|
||||||
|
<h1>停车机器人离线测试台</h1>
|
||||||
|
</div>
|
||||||
|
<div class="connection">
|
||||||
|
<span id="connection-dot" class="dot"></span>
|
||||||
|
<span id="connection-text">正在连接仿真后端</span>
|
||||||
|
</div>
|
||||||
|
</header>
|
||||||
|
|
||||||
|
<section class="workspace">
|
||||||
|
<aside class="panel controls-panel">
|
||||||
|
<div class="panel-heading">
|
||||||
|
<span>控制台</span>
|
||||||
|
<button id="reset-all" class="text-button">全部复位</button>
|
||||||
|
</div>
|
||||||
|
|
||||||
|
<label class="field-label" for="vehicle-select">当前车辆</label>
|
||||||
|
<select id="vehicle-select">
|
||||||
|
<option value="1">车辆 1</option>
|
||||||
|
</select>
|
||||||
|
|
||||||
|
<section class="virtual-remote">
|
||||||
|
<div class="remote-heading">
|
||||||
|
<p class="group-label">虚拟遥控器</p>
|
||||||
|
<span id="remote-state">已松开</span>
|
||||||
|
</div>
|
||||||
|
<div class="remote-pad" aria-label="虚拟遥控方向面板">
|
||||||
|
<button data-throttle="1" data-steering="1"
|
||||||
|
aria-label="前进并左转">↖</button>
|
||||||
|
<button data-throttle="1" data-steering="0"
|
||||||
|
aria-label="前进">↑</button>
|
||||||
|
<button data-throttle="1" data-steering="-1"
|
||||||
|
aria-label="前进并右转">↗</button>
|
||||||
|
<button data-throttle="0" data-steering="1"
|
||||||
|
aria-label="原地向左转">←</button>
|
||||||
|
<button id="remote-stop" class="remote-stop"
|
||||||
|
aria-label="停止">STOP</button>
|
||||||
|
<button data-throttle="0" data-steering="-1"
|
||||||
|
aria-label="原地向右转">→</button>
|
||||||
|
<button data-throttle="-1" data-steering="1"
|
||||||
|
aria-label="后退并左转">↙</button>
|
||||||
|
<button data-throttle="-1" data-steering="0"
|
||||||
|
aria-label="后退">↓</button>
|
||||||
|
<button data-throttle="-1" data-steering="-1"
|
||||||
|
aria-label="后退并右转">↘</button>
|
||||||
|
</div>
|
||||||
|
<p class="remote-help">
|
||||||
|
按住操作;键盘 W/S 前后,A/D 转向,可组合按键。
|
||||||
|
</p>
|
||||||
|
<label class="remote-slider">
|
||||||
|
<span>线速度 <output id="speed-scale-value">60%</output></span>
|
||||||
|
<input id="speed-scale" type="range"
|
||||||
|
min="0" max="100" value="60">
|
||||||
|
</label>
|
||||||
|
<label class="remote-slider">
|
||||||
|
<span>转向强度 <output id="steering-scale-value">60%</output></span>
|
||||||
|
<input id="steering-scale" type="range"
|
||||||
|
min="0" max="100" value="60">
|
||||||
|
</label>
|
||||||
|
<p class="remote-mode-note">
|
||||||
|
遥控方向遵循当前舵轮模式;正常模式下可边走边转。
|
||||||
|
</p>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<div id="action-groups"></div>
|
||||||
|
|
||||||
|
<div id="command-message" class="message">
|
||||||
|
请选择车辆和模式。
|
||||||
|
</div>
|
||||||
|
|
||||||
|
<details class="configuration">
|
||||||
|
<summary>仿真布局配置</summary>
|
||||||
|
|
||||||
|
<label class="field-label" for="vehicle-count">车辆数量</label>
|
||||||
|
<select id="vehicle-count">
|
||||||
|
<option value="1">1 辆</option>
|
||||||
|
<option value="2">2 辆</option>
|
||||||
|
<option value="3">3 辆</option>
|
||||||
|
<option value="4">4 辆</option>
|
||||||
|
<option value="5">5 辆</option>
|
||||||
|
<option value="6">6 辆</option>
|
||||||
|
<option value="7">7 辆</option>
|
||||||
|
<option value="8">8 辆</option>
|
||||||
|
</select>
|
||||||
|
|
||||||
|
<p class="group-label">车队中心(世界系)</p>
|
||||||
|
<div class="numeric-grid">
|
||||||
|
<label>X / m<input id="fleet-x" type="number" step="0.1"></label>
|
||||||
|
<label>Y / m<input id="fleet-y" type="number" step="0.1"></label>
|
||||||
|
<label>Yaw / °<input id="fleet-yaw" type="number" step="1"></label>
|
||||||
|
</div>
|
||||||
|
|
||||||
|
<p class="group-label">单车相对车队中心位姿</p>
|
||||||
|
<div id="layout-rows" class="layout-rows"></div>
|
||||||
|
<button id="apply-configuration" class="primary-button">
|
||||||
|
应用布局并复位
|
||||||
|
</button>
|
||||||
|
</details>
|
||||||
|
|
||||||
|
<div class="legend">
|
||||||
|
<div><span class="axis-swatch x-axis"></span>车体 +X(车头)</div>
|
||||||
|
<div><span class="axis-swatch y-axis"></span>车体 +Y(左侧)</div>
|
||||||
|
<div><span class="legend-line zero"></span>舵轮机械 0°</div>
|
||||||
|
<div><span class="legend-line actual"></span>实际舵角</div>
|
||||||
|
<div><span class="legend-line target"></span>目标舵角</div>
|
||||||
|
<div><span class="legend-dot ready"></span>舵轮已到位</div>
|
||||||
|
<div><span class="legend-dot aligning"></span>舵轮转向中</div>
|
||||||
|
</div>
|
||||||
|
</aside>
|
||||||
|
|
||||||
|
<section class="panel viewport-panel">
|
||||||
|
<div class="panel-heading">
|
||||||
|
<span>世界视图</span>
|
||||||
|
<span class="coordinate-note">X 向右 · Y 向上 · 逆时针为正</span>
|
||||||
|
</div>
|
||||||
|
<canvas id="world-canvas"></canvas>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<aside class="panel status-panel">
|
||||||
|
<div class="panel-heading">
|
||||||
|
<span>车辆状态</span>
|
||||||
|
<span id="update-rate">-- Hz</span>
|
||||||
|
</div>
|
||||||
|
<div id="selected-vehicle-detail"></div>
|
||||||
|
<div id="vehicle-cards" class="vehicle-cards"></div>
|
||||||
|
</aside>
|
||||||
|
</section>
|
||||||
|
</main>
|
||||||
|
|
||||||
|
<script src="viewer.js"></script>
|
||||||
|
</body>
|
||||||
|
</html>
|
||||||
@@ -0,0 +1,603 @@
|
|||||||
|
:root {
|
||||||
|
color-scheme: dark;
|
||||||
|
font-family: Inter, "Segoe UI", "Microsoft YaHei", sans-serif;
|
||||||
|
background: #050a0e;
|
||||||
|
color: #dbe9ef;
|
||||||
|
--panel: rgba(13, 24, 32, 0.92);
|
||||||
|
--border: rgba(116, 151, 168, 0.19);
|
||||||
|
--muted: #78909d;
|
||||||
|
--accent: #42dfb7;
|
||||||
|
--warning: #ffb257;
|
||||||
|
--danger: #ff6577;
|
||||||
|
}
|
||||||
|
|
||||||
|
* {
|
||||||
|
box-sizing: border-box;
|
||||||
|
}
|
||||||
|
|
||||||
|
body {
|
||||||
|
min-width: 1100px;
|
||||||
|
min-height: 100vh;
|
||||||
|
margin: 0;
|
||||||
|
background:
|
||||||
|
radial-gradient(circle at 25% 0%,
|
||||||
|
rgba(32, 117, 106, 0.16), transparent 34%),
|
||||||
|
linear-gradient(145deg, #071018, #030609 72%);
|
||||||
|
}
|
||||||
|
|
||||||
|
button,
|
||||||
|
select {
|
||||||
|
font: inherit;
|
||||||
|
}
|
||||||
|
|
||||||
|
.app-shell {
|
||||||
|
min-height: 100vh;
|
||||||
|
padding: 24px;
|
||||||
|
}
|
||||||
|
|
||||||
|
.topbar {
|
||||||
|
display: flex;
|
||||||
|
align-items: flex-end;
|
||||||
|
justify-content: space-between;
|
||||||
|
max-width: 1800px;
|
||||||
|
margin: 0 auto 18px;
|
||||||
|
}
|
||||||
|
|
||||||
|
.eyebrow {
|
||||||
|
margin: 0 0 7px;
|
||||||
|
color: var(--accent);
|
||||||
|
font-size: 11px;
|
||||||
|
font-weight: 700;
|
||||||
|
letter-spacing: 0.18em;
|
||||||
|
}
|
||||||
|
|
||||||
|
h1 {
|
||||||
|
margin: 0;
|
||||||
|
font-size: 26px;
|
||||||
|
font-weight: 650;
|
||||||
|
}
|
||||||
|
|
||||||
|
.connection {
|
||||||
|
display: flex;
|
||||||
|
gap: 9px;
|
||||||
|
align-items: center;
|
||||||
|
color: var(--muted);
|
||||||
|
font-size: 13px;
|
||||||
|
}
|
||||||
|
|
||||||
|
.dot {
|
||||||
|
width: 9px;
|
||||||
|
height: 9px;
|
||||||
|
border-radius: 50%;
|
||||||
|
background: var(--danger);
|
||||||
|
box-shadow: 0 0 12px rgba(255, 101, 119, 0.45);
|
||||||
|
}
|
||||||
|
|
||||||
|
.dot.online {
|
||||||
|
background: var(--accent);
|
||||||
|
box-shadow: 0 0 12px rgba(66, 223, 183, 0.5);
|
||||||
|
}
|
||||||
|
|
||||||
|
.workspace {
|
||||||
|
display: grid;
|
||||||
|
grid-template-columns: 320px minmax(600px, 1fr) 340px;
|
||||||
|
gap: 14px;
|
||||||
|
max-width: 1800px;
|
||||||
|
min-height: calc(100vh - 112px);
|
||||||
|
margin: 0 auto;
|
||||||
|
}
|
||||||
|
|
||||||
|
.panel {
|
||||||
|
overflow: hidden;
|
||||||
|
border: 1px solid var(--border);
|
||||||
|
border-radius: 14px;
|
||||||
|
background: var(--panel);
|
||||||
|
box-shadow: 0 20px 55px rgba(0, 0, 0, 0.25);
|
||||||
|
}
|
||||||
|
|
||||||
|
.panel-heading {
|
||||||
|
display: flex;
|
||||||
|
align-items: center;
|
||||||
|
justify-content: space-between;
|
||||||
|
min-height: 48px;
|
||||||
|
padding: 0 16px;
|
||||||
|
border-bottom: 1px solid var(--border);
|
||||||
|
color: #bcd0d9;
|
||||||
|
font-size: 13px;
|
||||||
|
font-weight: 650;
|
||||||
|
}
|
||||||
|
|
||||||
|
.controls-panel {
|
||||||
|
max-height: calc(100vh - 112px);
|
||||||
|
overflow-y: auto;
|
||||||
|
padding-bottom: 18px;
|
||||||
|
}
|
||||||
|
|
||||||
|
.controls-panel > :not(.panel-heading) {
|
||||||
|
margin-right: 16px;
|
||||||
|
margin-left: 16px;
|
||||||
|
}
|
||||||
|
|
||||||
|
.text-button {
|
||||||
|
width: auto;
|
||||||
|
padding: 4px 0;
|
||||||
|
border: 0;
|
||||||
|
background: transparent;
|
||||||
|
color: var(--accent);
|
||||||
|
cursor: pointer;
|
||||||
|
font-size: 12px;
|
||||||
|
}
|
||||||
|
|
||||||
|
.field-label,
|
||||||
|
.group-label {
|
||||||
|
display: block;
|
||||||
|
margin-top: 20px;
|
||||||
|
margin-bottom: 8px;
|
||||||
|
color: var(--muted);
|
||||||
|
font-size: 11px;
|
||||||
|
font-weight: 700;
|
||||||
|
letter-spacing: 0.08em;
|
||||||
|
}
|
||||||
|
|
||||||
|
select,
|
||||||
|
.button-grid button,
|
||||||
|
.drive-pad button {
|
||||||
|
border: 1px solid rgba(126, 164, 182, 0.23);
|
||||||
|
border-radius: 8px;
|
||||||
|
background: #101e27;
|
||||||
|
color: #dbe9ef;
|
||||||
|
}
|
||||||
|
|
||||||
|
select {
|
||||||
|
width: calc(100% - 32px);
|
||||||
|
padding: 10px;
|
||||||
|
}
|
||||||
|
|
||||||
|
.virtual-remote {
|
||||||
|
margin-top: 18px !important;
|
||||||
|
padding: 12px;
|
||||||
|
border: 1px solid var(--border);
|
||||||
|
border-radius: 10px;
|
||||||
|
background: rgba(5, 12, 17, 0.58);
|
||||||
|
}
|
||||||
|
|
||||||
|
.remote-heading {
|
||||||
|
display: flex;
|
||||||
|
align-items: center;
|
||||||
|
justify-content: space-between;
|
||||||
|
}
|
||||||
|
|
||||||
|
.remote-heading .group-label {
|
||||||
|
margin: 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
#remote-state {
|
||||||
|
color: var(--muted);
|
||||||
|
font-size: 10px;
|
||||||
|
}
|
||||||
|
|
||||||
|
#remote-state.active {
|
||||||
|
color: var(--accent);
|
||||||
|
}
|
||||||
|
|
||||||
|
.remote-pad {
|
||||||
|
display: grid;
|
||||||
|
grid-template-columns: repeat(3, 54px);
|
||||||
|
gap: 7px;
|
||||||
|
justify-content: center;
|
||||||
|
margin: 12px 0 8px;
|
||||||
|
touch-action: none;
|
||||||
|
user-select: none;
|
||||||
|
}
|
||||||
|
|
||||||
|
.remote-pad button {
|
||||||
|
width: 54px;
|
||||||
|
height: 45px;
|
||||||
|
padding: 0;
|
||||||
|
border: 1px solid rgba(126, 164, 182, 0.3);
|
||||||
|
border-radius: 9px;
|
||||||
|
background: #101e27;
|
||||||
|
color: #dbe9ef;
|
||||||
|
cursor: pointer;
|
||||||
|
font-size: 22px;
|
||||||
|
}
|
||||||
|
|
||||||
|
.remote-pad button:hover,
|
||||||
|
.remote-pad button.pressed {
|
||||||
|
border-color: var(--accent);
|
||||||
|
background: rgba(66, 223, 183, 0.18);
|
||||||
|
color: #78f0d5;
|
||||||
|
}
|
||||||
|
|
||||||
|
.remote-pad .remote-stop {
|
||||||
|
border-color: rgba(255, 101, 119, 0.45);
|
||||||
|
color: #ff98a5;
|
||||||
|
font-size: 9px;
|
||||||
|
font-weight: 800;
|
||||||
|
letter-spacing: 0.08em;
|
||||||
|
}
|
||||||
|
|
||||||
|
.remote-help,
|
||||||
|
.remote-mode-note {
|
||||||
|
margin: 7px 0 0;
|
||||||
|
color: #718a96;
|
||||||
|
font-size: 9px;
|
||||||
|
line-height: 1.45;
|
||||||
|
text-align: center;
|
||||||
|
}
|
||||||
|
|
||||||
|
.remote-slider {
|
||||||
|
display: grid;
|
||||||
|
gap: 4px;
|
||||||
|
margin-top: 10px;
|
||||||
|
color: #8fa8b4;
|
||||||
|
font-size: 10px;
|
||||||
|
}
|
||||||
|
|
||||||
|
.remote-slider span {
|
||||||
|
display: flex;
|
||||||
|
justify-content: space-between;
|
||||||
|
}
|
||||||
|
|
||||||
|
.remote-slider output {
|
||||||
|
color: var(--accent);
|
||||||
|
font-variant-numeric: tabular-nums;
|
||||||
|
}
|
||||||
|
|
||||||
|
.remote-slider input {
|
||||||
|
height: 18px;
|
||||||
|
padding: 0;
|
||||||
|
accent-color: var(--accent);
|
||||||
|
}
|
||||||
|
|
||||||
|
.action-grid {
|
||||||
|
display: grid;
|
||||||
|
grid-template-columns: 1fr 1fr;
|
||||||
|
gap: 7px;
|
||||||
|
}
|
||||||
|
|
||||||
|
.action-grid button {
|
||||||
|
min-height: 38px;
|
||||||
|
border: 1px solid rgba(126, 164, 182, 0.23);
|
||||||
|
border-radius: 8px;
|
||||||
|
background: #101e27;
|
||||||
|
color: #dbe9ef;
|
||||||
|
cursor: pointer;
|
||||||
|
transition:
|
||||||
|
border-color 120ms,
|
||||||
|
background 120ms,
|
||||||
|
transform 120ms;
|
||||||
|
}
|
||||||
|
|
||||||
|
.action-grid button:hover {
|
||||||
|
border-color: var(--accent);
|
||||||
|
background: #142c31;
|
||||||
|
transform: translateY(-1px);
|
||||||
|
}
|
||||||
|
|
||||||
|
.action-grid .danger-button {
|
||||||
|
border-color: rgba(255, 101, 119, 0.45);
|
||||||
|
color: #ff98a5;
|
||||||
|
}
|
||||||
|
|
||||||
|
.configuration {
|
||||||
|
margin-top: 18px !important;
|
||||||
|
border-top: 1px solid var(--border);
|
||||||
|
border-bottom: 1px solid var(--border);
|
||||||
|
padding: 13px 0 16px;
|
||||||
|
}
|
||||||
|
|
||||||
|
.configuration summary {
|
||||||
|
color: #bcd0d9;
|
||||||
|
cursor: pointer;
|
||||||
|
font-size: 12px;
|
||||||
|
font-weight: 650;
|
||||||
|
}
|
||||||
|
|
||||||
|
.configuration select {
|
||||||
|
width: 100%;
|
||||||
|
}
|
||||||
|
|
||||||
|
.numeric-grid {
|
||||||
|
display: grid;
|
||||||
|
grid-template-columns: repeat(3, 1fr);
|
||||||
|
gap: 6px;
|
||||||
|
}
|
||||||
|
|
||||||
|
.numeric-grid label,
|
||||||
|
.layout-row label {
|
||||||
|
display: grid;
|
||||||
|
gap: 4px;
|
||||||
|
color: var(--muted);
|
||||||
|
font-size: 9px;
|
||||||
|
}
|
||||||
|
|
||||||
|
input {
|
||||||
|
width: 100%;
|
||||||
|
min-width: 0;
|
||||||
|
padding: 7px 5px;
|
||||||
|
border: 1px solid rgba(126, 164, 182, 0.23);
|
||||||
|
border-radius: 6px;
|
||||||
|
outline: 0;
|
||||||
|
background: #0b171f;
|
||||||
|
color: #dbe9ef;
|
||||||
|
font: inherit;
|
||||||
|
font-variant-numeric: tabular-nums;
|
||||||
|
}
|
||||||
|
|
||||||
|
input:focus {
|
||||||
|
border-color: var(--accent);
|
||||||
|
}
|
||||||
|
|
||||||
|
.layout-rows {
|
||||||
|
display: grid;
|
||||||
|
gap: 7px;
|
||||||
|
max-height: 275px;
|
||||||
|
overflow-y: auto;
|
||||||
|
}
|
||||||
|
|
||||||
|
.layout-row {
|
||||||
|
display: grid;
|
||||||
|
grid-template-columns: 48px repeat(3, 1fr);
|
||||||
|
gap: 5px;
|
||||||
|
align-items: end;
|
||||||
|
padding: 8px;
|
||||||
|
border: 1px solid var(--border);
|
||||||
|
border-radius: 7px;
|
||||||
|
background: rgba(5, 12, 17, 0.6);
|
||||||
|
}
|
||||||
|
|
||||||
|
.layout-row strong {
|
||||||
|
align-self: center;
|
||||||
|
color: #9cb2bd;
|
||||||
|
font-size: 10px;
|
||||||
|
}
|
||||||
|
|
||||||
|
.primary-button {
|
||||||
|
width: 100%;
|
||||||
|
margin-top: 10px;
|
||||||
|
padding: 9px;
|
||||||
|
border: 1px solid rgba(66, 223, 183, 0.5);
|
||||||
|
border-radius: 7px;
|
||||||
|
background: rgba(66, 223, 183, 0.12);
|
||||||
|
color: #78f0d5;
|
||||||
|
cursor: pointer;
|
||||||
|
}
|
||||||
|
|
||||||
|
.message {
|
||||||
|
min-height: 56px;
|
||||||
|
margin-top: 18px !important;
|
||||||
|
padding: 11px;
|
||||||
|
border-left: 3px solid var(--accent);
|
||||||
|
border-radius: 4px;
|
||||||
|
background: rgba(66, 223, 183, 0.07);
|
||||||
|
color: #a8c4cf;
|
||||||
|
font-size: 12px;
|
||||||
|
line-height: 1.5;
|
||||||
|
}
|
||||||
|
|
||||||
|
.message.error {
|
||||||
|
border-left-color: var(--danger);
|
||||||
|
background: rgba(255, 101, 119, 0.08);
|
||||||
|
}
|
||||||
|
|
||||||
|
.legend {
|
||||||
|
display: grid;
|
||||||
|
gap: 10px;
|
||||||
|
margin-top: 22px !important;
|
||||||
|
color: var(--muted);
|
||||||
|
font-size: 11px;
|
||||||
|
}
|
||||||
|
|
||||||
|
.legend > div {
|
||||||
|
display: flex;
|
||||||
|
gap: 9px;
|
||||||
|
align-items: center;
|
||||||
|
}
|
||||||
|
|
||||||
|
.legend-line {
|
||||||
|
width: 24px;
|
||||||
|
height: 0;
|
||||||
|
border-top: 3px solid var(--accent);
|
||||||
|
}
|
||||||
|
|
||||||
|
.legend-line.target {
|
||||||
|
border-top-style: dashed;
|
||||||
|
border-top-color: var(--warning);
|
||||||
|
}
|
||||||
|
|
||||||
|
.legend-line.zero {
|
||||||
|
border-top-width: 1px;
|
||||||
|
border-top-style: dashed;
|
||||||
|
border-top-color: #56a7ff;
|
||||||
|
}
|
||||||
|
|
||||||
|
.axis-swatch {
|
||||||
|
width: 24px;
|
||||||
|
height: 3px;
|
||||||
|
background: #ff6477;
|
||||||
|
}
|
||||||
|
|
||||||
|
.axis-swatch.y-axis {
|
||||||
|
background: #54d991;
|
||||||
|
}
|
||||||
|
|
||||||
|
.legend-dot {
|
||||||
|
width: 8px;
|
||||||
|
height: 8px;
|
||||||
|
border-radius: 50%;
|
||||||
|
background: var(--accent);
|
||||||
|
}
|
||||||
|
|
||||||
|
.legend-dot.aligning {
|
||||||
|
background: var(--warning);
|
||||||
|
}
|
||||||
|
|
||||||
|
.viewport-panel {
|
||||||
|
display: flex;
|
||||||
|
min-width: 0;
|
||||||
|
flex-direction: column;
|
||||||
|
}
|
||||||
|
|
||||||
|
.coordinate-note,
|
||||||
|
#update-rate {
|
||||||
|
color: var(--muted);
|
||||||
|
font-size: 11px;
|
||||||
|
font-weight: 500;
|
||||||
|
}
|
||||||
|
|
||||||
|
#world-canvas {
|
||||||
|
display: block;
|
||||||
|
width: 100%;
|
||||||
|
height: calc(100% - 48px);
|
||||||
|
min-height: 620px;
|
||||||
|
}
|
||||||
|
|
||||||
|
.vehicle-cards {
|
||||||
|
display: grid;
|
||||||
|
gap: 9px;
|
||||||
|
padding: 12px;
|
||||||
|
}
|
||||||
|
|
||||||
|
.status-panel {
|
||||||
|
max-height: calc(100vh - 112px);
|
||||||
|
overflow-y: auto;
|
||||||
|
}
|
||||||
|
|
||||||
|
.wheel-detail {
|
||||||
|
padding: 12px;
|
||||||
|
border-bottom: 1px solid var(--border);
|
||||||
|
}
|
||||||
|
|
||||||
|
.geometry-note {
|
||||||
|
margin-bottom: 11px;
|
||||||
|
color: #8fa8b4;
|
||||||
|
font-size: 10px;
|
||||||
|
line-height: 1.5;
|
||||||
|
}
|
||||||
|
|
||||||
|
.wheel-row {
|
||||||
|
margin-top: 10px;
|
||||||
|
}
|
||||||
|
|
||||||
|
.wheel-row-title {
|
||||||
|
display: flex;
|
||||||
|
justify-content: space-between;
|
||||||
|
gap: 8px;
|
||||||
|
color: #9eb4be;
|
||||||
|
font-size: 10px;
|
||||||
|
}
|
||||||
|
|
||||||
|
.wheel-row-title span {
|
||||||
|
font-variant-numeric: tabular-nums;
|
||||||
|
}
|
||||||
|
|
||||||
|
.angle-scale {
|
||||||
|
position: relative;
|
||||||
|
height: 8px;
|
||||||
|
margin: 6px 0 3px;
|
||||||
|
border-radius: 4px;
|
||||||
|
background:
|
||||||
|
linear-gradient(90deg,
|
||||||
|
rgba(181, 140, 255, 0.42),
|
||||||
|
rgba(66, 223, 183, 0.15) 50%,
|
||||||
|
rgba(181, 140, 255, 0.42));
|
||||||
|
}
|
||||||
|
|
||||||
|
.zero-mark,
|
||||||
|
.target-mark,
|
||||||
|
.actual-mark {
|
||||||
|
position: absolute;
|
||||||
|
top: -3px;
|
||||||
|
width: 2px;
|
||||||
|
height: 14px;
|
||||||
|
transform: translateX(-1px);
|
||||||
|
}
|
||||||
|
|
||||||
|
.zero-mark {
|
||||||
|
left: 50%;
|
||||||
|
background: #56a7ff;
|
||||||
|
}
|
||||||
|
|
||||||
|
.target-mark {
|
||||||
|
background: var(--warning);
|
||||||
|
}
|
||||||
|
|
||||||
|
.actual-mark {
|
||||||
|
width: 3px;
|
||||||
|
background: var(--accent);
|
||||||
|
}
|
||||||
|
|
||||||
|
.wheel-speed {
|
||||||
|
color: #617b88;
|
||||||
|
font-size: 9px;
|
||||||
|
text-align: right;
|
||||||
|
}
|
||||||
|
|
||||||
|
.vehicle-card {
|
||||||
|
width: 100%;
|
||||||
|
padding: 12px;
|
||||||
|
border: 1px solid var(--border);
|
||||||
|
border-radius: 9px;
|
||||||
|
background: rgba(9, 18, 24, 0.78);
|
||||||
|
color: inherit;
|
||||||
|
cursor: pointer;
|
||||||
|
text-align: left;
|
||||||
|
}
|
||||||
|
|
||||||
|
.vehicle-card.selected {
|
||||||
|
border-color: rgba(66, 223, 183, 0.7);
|
||||||
|
background: rgba(36, 120, 105, 0.11);
|
||||||
|
}
|
||||||
|
|
||||||
|
.card-topline {
|
||||||
|
display: flex;
|
||||||
|
justify-content: space-between;
|
||||||
|
font-size: 12px;
|
||||||
|
}
|
||||||
|
|
||||||
|
.status-ready {
|
||||||
|
color: var(--accent);
|
||||||
|
}
|
||||||
|
|
||||||
|
.status-aligning {
|
||||||
|
color: var(--warning);
|
||||||
|
}
|
||||||
|
|
||||||
|
.mode-name {
|
||||||
|
margin: 7px 0 9px;
|
||||||
|
color: #91a9b5;
|
||||||
|
font-size: 12px;
|
||||||
|
}
|
||||||
|
|
||||||
|
dl {
|
||||||
|
display: grid;
|
||||||
|
gap: 5px;
|
||||||
|
margin: 0;
|
||||||
|
font-size: 11px;
|
||||||
|
}
|
||||||
|
|
||||||
|
dl div {
|
||||||
|
display: flex;
|
||||||
|
justify-content: space-between;
|
||||||
|
}
|
||||||
|
|
||||||
|
dt {
|
||||||
|
color: #607985;
|
||||||
|
}
|
||||||
|
|
||||||
|
dd {
|
||||||
|
margin: 0;
|
||||||
|
color: #bcd0d9;
|
||||||
|
font-variant-numeric: tabular-nums;
|
||||||
|
}
|
||||||
|
|
||||||
|
@media (max-width: 1300px) {
|
||||||
|
body {
|
||||||
|
min-width: 1100px;
|
||||||
|
}
|
||||||
|
|
||||||
|
.workspace {
|
||||||
|
grid-template-columns: 285px minmax(520px, 1fr) 305px;
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -0,0 +1,842 @@
|
|||||||
|
const canvas = document.getElementById("world-canvas");
|
||||||
|
const context = canvas.getContext("2d");
|
||||||
|
const vehicleSelect = document.getElementById("vehicle-select");
|
||||||
|
const commandMessage = document.getElementById("command-message");
|
||||||
|
const connectionDot = document.getElementById("connection-dot");
|
||||||
|
const connectionText = document.getElementById("connection-text");
|
||||||
|
const vehicleCards = document.getElementById("vehicle-cards");
|
||||||
|
const selectedVehicleDetail =
|
||||||
|
document.getElementById("selected-vehicle-detail");
|
||||||
|
const actionGroups = document.getElementById("action-groups");
|
||||||
|
const updateRate = document.getElementById("update-rate");
|
||||||
|
const vehicleCountInput = document.getElementById("vehicle-count");
|
||||||
|
const layoutRows = document.getElementById("layout-rows");
|
||||||
|
const remoteState = document.getElementById("remote-state");
|
||||||
|
const speedScaleInput = document.getElementById("speed-scale");
|
||||||
|
const steeringScaleInput =
|
||||||
|
document.getElementById("steering-scale");
|
||||||
|
const speedScaleValue =
|
||||||
|
document.getElementById("speed-scale-value");
|
||||||
|
const steeringScaleValue =
|
||||||
|
document.getElementById("steering-scale-value");
|
||||||
|
|
||||||
|
let vehicles = [];
|
||||||
|
let actions = [];
|
||||||
|
let configuration = {
|
||||||
|
vehicleCount: 1,
|
||||||
|
fleetCenter: { xMeters: 0, yMeters: 0, yawRadians: 0 },
|
||||||
|
vehicles: [
|
||||||
|
{ vehicleId: 1, xMeters: 0, yMeters: 0, yawRadians: 0 }
|
||||||
|
]
|
||||||
|
};
|
||||||
|
let lastUpdateTime = performance.now();
|
||||||
|
let successfulUpdates = 0;
|
||||||
|
let remoteThrottle = 0;
|
||||||
|
let remoteSteering = 0;
|
||||||
|
let remotePointerId = null;
|
||||||
|
let remoteVehicleId = null;
|
||||||
|
const pressedDriveKeys = new Set();
|
||||||
|
|
||||||
|
function resizeCanvas() {
|
||||||
|
const bounds = canvas.getBoundingClientRect();
|
||||||
|
const ratio = window.devicePixelRatio || 1;
|
||||||
|
canvas.width = Math.max(1, Math.floor(bounds.width * ratio));
|
||||||
|
canvas.height = Math.max(1, Math.floor(bounds.height * ratio));
|
||||||
|
context.setTransform(ratio, 0, 0, ratio, 0, 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
function getView() {
|
||||||
|
const centerX = configuration.fleetCenter.xMeters;
|
||||||
|
const centerY = configuration.fleetCenter.yMeters;
|
||||||
|
let radiusX = 2.1;
|
||||||
|
let radiusY = 1.7;
|
||||||
|
|
||||||
|
for (const vehicle of vehicles) {
|
||||||
|
radiusX = Math.max(
|
||||||
|
radiusX,
|
||||||
|
Math.abs(vehicle.xMeters - centerX) + 1.2);
|
||||||
|
radiusY = Math.max(
|
||||||
|
radiusY,
|
||||||
|
Math.abs(vehicle.yMeters - centerY) + 1.0);
|
||||||
|
}
|
||||||
|
|
||||||
|
const width = canvas.clientWidth;
|
||||||
|
const height = canvas.clientHeight;
|
||||||
|
const scale = Math.min(
|
||||||
|
width / (radiusX * 2),
|
||||||
|
height / (radiusY * 2),
|
||||||
|
155);
|
||||||
|
|
||||||
|
return { centerX, centerY, scale, width, height };
|
||||||
|
}
|
||||||
|
|
||||||
|
function worldToScreen(xMeters, yMeters, view = getView()) {
|
||||||
|
return {
|
||||||
|
x: view.width / 2 +
|
||||||
|
(xMeters - view.centerX) * view.scale,
|
||||||
|
y: view.height / 2 -
|
||||||
|
(yMeters - view.centerY) * view.scale,
|
||||||
|
scale: view.scale
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
function drawGrid(view) {
|
||||||
|
const width = view.width;
|
||||||
|
const height = view.height;
|
||||||
|
const worldOrigin = worldToScreen(0, 0, view);
|
||||||
|
const spacing = view.scale;
|
||||||
|
|
||||||
|
context.clearRect(0, 0, width, height);
|
||||||
|
context.fillStyle = "#071018";
|
||||||
|
context.fillRect(0, 0, width, height);
|
||||||
|
context.lineWidth = 1;
|
||||||
|
context.strokeStyle = "rgba(109, 143, 160, 0.12)";
|
||||||
|
|
||||||
|
for (let x = worldOrigin.x % spacing; x < width; x += spacing) {
|
||||||
|
context.beginPath();
|
||||||
|
context.moveTo(x, 0);
|
||||||
|
context.lineTo(x, height);
|
||||||
|
context.stroke();
|
||||||
|
}
|
||||||
|
|
||||||
|
for (let y = worldOrigin.y % spacing; y < height; y += spacing) {
|
||||||
|
context.beginPath();
|
||||||
|
context.moveTo(0, y);
|
||||||
|
context.lineTo(width, y);
|
||||||
|
context.stroke();
|
||||||
|
}
|
||||||
|
|
||||||
|
context.strokeStyle = "rgba(90, 125, 142, 0.35)";
|
||||||
|
context.lineWidth = 1.2;
|
||||||
|
context.beginPath();
|
||||||
|
context.moveTo(0, worldOrigin.y);
|
||||||
|
context.lineTo(width, worldOrigin.y);
|
||||||
|
context.moveTo(worldOrigin.x, 0);
|
||||||
|
context.lineTo(worldOrigin.x, height);
|
||||||
|
context.stroke();
|
||||||
|
}
|
||||||
|
|
||||||
|
function drawFleetCenter(view) {
|
||||||
|
const fleet = configuration.fleetCenter;
|
||||||
|
const point = worldToScreen(
|
||||||
|
fleet.xMeters,
|
||||||
|
fleet.yMeters,
|
||||||
|
view);
|
||||||
|
const yaw = fleet.yawRadians;
|
||||||
|
const axisLength = 55;
|
||||||
|
|
||||||
|
context.save();
|
||||||
|
context.translate(point.x, point.y);
|
||||||
|
context.rotate(-yaw);
|
||||||
|
|
||||||
|
context.fillStyle = "#b58cff";
|
||||||
|
context.beginPath();
|
||||||
|
context.moveTo(0, -8);
|
||||||
|
context.lineTo(8, 0);
|
||||||
|
context.lineTo(0, 8);
|
||||||
|
context.lineTo(-8, 0);
|
||||||
|
context.closePath();
|
||||||
|
context.fill();
|
||||||
|
|
||||||
|
drawArrow(0, 0, axisLength, 0, "#ff7285", 2);
|
||||||
|
drawArrow(0, 0, 0, -axisLength, "#66d99a", 2);
|
||||||
|
context.restore();
|
||||||
|
|
||||||
|
context.fillStyle = "#c7aaff";
|
||||||
|
context.font = "12px Inter, sans-serif";
|
||||||
|
context.fillText("FLEET CENTER", point.x + 12, point.y - 11);
|
||||||
|
}
|
||||||
|
|
||||||
|
function drawVehicle(vehicle, view) {
|
||||||
|
const point = worldToScreen(
|
||||||
|
vehicle.xMeters,
|
||||||
|
vehicle.yMeters,
|
||||||
|
view);
|
||||||
|
const selected = Number(vehicleSelect.value) === vehicle.vehicleId;
|
||||||
|
const bodyLength = vehicle.bodyLengthMeters * point.scale;
|
||||||
|
const bodyWidth = vehicle.bodyWidthMeters * point.scale;
|
||||||
|
|
||||||
|
context.save();
|
||||||
|
context.translate(point.x, point.y);
|
||||||
|
context.rotate(-vehicle.yawRadians);
|
||||||
|
|
||||||
|
context.fillStyle = selected
|
||||||
|
? "rgba(26, 180, 155, 0.2)"
|
||||||
|
: "rgba(50, 73, 88, 0.42)";
|
||||||
|
context.strokeStyle = selected ? "#39d8b7" : "#668392";
|
||||||
|
context.lineWidth = selected ? 2.5 : 1.5;
|
||||||
|
context.beginPath();
|
||||||
|
context.roundRect(
|
||||||
|
-bodyLength / 2,
|
||||||
|
-bodyWidth / 2,
|
||||||
|
bodyLength,
|
||||||
|
bodyWidth,
|
||||||
|
12);
|
||||||
|
context.fill();
|
||||||
|
context.stroke();
|
||||||
|
|
||||||
|
drawBodyAxes(bodyLength, bodyWidth);
|
||||||
|
|
||||||
|
for (const wheel of vehicle.wheels) {
|
||||||
|
drawWheel(
|
||||||
|
wheel.xMeters * point.scale,
|
||||||
|
-wheel.yMeters * point.scale,
|
||||||
|
wheel);
|
||||||
|
}
|
||||||
|
|
||||||
|
context.fillStyle = "rgba(202, 222, 230, 0.8)";
|
||||||
|
context.font = "10px Inter, sans-serif";
|
||||||
|
context.textAlign = "center";
|
||||||
|
context.fillText(
|
||||||
|
"1472 × 948 mm",
|
||||||
|
0,
|
||||||
|
bodyWidth / 2 - 8);
|
||||||
|
context.restore();
|
||||||
|
|
||||||
|
context.textAlign = "left";
|
||||||
|
context.fillStyle = "#dbe9ef";
|
||||||
|
context.font = "600 13px Inter, sans-serif";
|
||||||
|
context.fillText(
|
||||||
|
`CAR ${vehicle.vehicleId}`,
|
||||||
|
point.x - 24,
|
||||||
|
point.y - bodyWidth / 2 - 18);
|
||||||
|
|
||||||
|
context.fillStyle = vehicle.modeReady ? "#42dfb7" : "#ffb257";
|
||||||
|
context.font = "12px Inter, sans-serif";
|
||||||
|
context.fillText(
|
||||||
|
vehicle.modeReady ? "READY" : "ALIGNING",
|
||||||
|
point.x - 23,
|
||||||
|
point.y + bodyWidth / 2 + 25);
|
||||||
|
}
|
||||||
|
|
||||||
|
function drawBodyAxes(bodyLength, bodyWidth) {
|
||||||
|
const xLength = bodyLength / 2 + 30;
|
||||||
|
const yLength = bodyWidth / 2 + 27;
|
||||||
|
|
||||||
|
drawArrow(0, 0, xLength, 0, "#ff6477", 2.4);
|
||||||
|
drawArrow(0, 0, 0, -yLength, "#54d991", 2.4);
|
||||||
|
|
||||||
|
context.font = "700 11px Inter, sans-serif";
|
||||||
|
context.fillStyle = "#ff8b99";
|
||||||
|
context.fillText("+X", xLength - 2, -7);
|
||||||
|
context.fillStyle = "#7debab";
|
||||||
|
context.fillText("+Y", 6, -yLength + 2);
|
||||||
|
}
|
||||||
|
|
||||||
|
function drawWheel(x, y, wheel) {
|
||||||
|
const wheelLength = 33;
|
||||||
|
const limitRadius = 20;
|
||||||
|
context.save();
|
||||||
|
context.translate(x, y);
|
||||||
|
|
||||||
|
context.strokeStyle = "rgba(86, 167, 255, 0.72)";
|
||||||
|
context.lineWidth = 1.2;
|
||||||
|
context.setLineDash([3, 3]);
|
||||||
|
context.beginPath();
|
||||||
|
context.moveTo(-22, 0);
|
||||||
|
context.lineTo(24, 0);
|
||||||
|
context.stroke();
|
||||||
|
|
||||||
|
drawWheelLimitArc(limitRadius);
|
||||||
|
|
||||||
|
context.save();
|
||||||
|
context.rotate(-wheel.targetAngleDegrees * Math.PI / 180);
|
||||||
|
context.strokeStyle = "rgba(255, 178, 87, 0.9)";
|
||||||
|
context.lineWidth = 3;
|
||||||
|
context.setLineDash([5, 4]);
|
||||||
|
context.beginPath();
|
||||||
|
context.moveTo(-wheelLength / 2, 0);
|
||||||
|
context.lineTo(wheelLength / 2, 0);
|
||||||
|
context.stroke();
|
||||||
|
context.restore();
|
||||||
|
|
||||||
|
context.save();
|
||||||
|
context.rotate(-wheel.actualAngleDegrees * Math.PI / 180);
|
||||||
|
context.strokeStyle = wheel.isAligned ? "#42dfb7" : "#f2f6f8";
|
||||||
|
context.lineWidth = 7;
|
||||||
|
context.setLineDash([]);
|
||||||
|
context.lineCap = "round";
|
||||||
|
context.beginPath();
|
||||||
|
context.moveTo(-wheelLength / 2, 0);
|
||||||
|
context.lineTo(wheelLength / 2, 0);
|
||||||
|
context.stroke();
|
||||||
|
context.restore();
|
||||||
|
|
||||||
|
context.setLineDash([]);
|
||||||
|
context.fillStyle = wheel.isAligned ? "#7ef4dc" : "#ffca83";
|
||||||
|
context.font = "700 9px Inter, sans-serif";
|
||||||
|
context.textAlign = "center";
|
||||||
|
const sign = wheel.actualAngleDegrees >= 0 ? "+" : "";
|
||||||
|
context.fillText(
|
||||||
|
`${sign}${wheel.actualAngleDegrees.toFixed(0)}°`,
|
||||||
|
0,
|
||||||
|
-25);
|
||||||
|
context.restore();
|
||||||
|
}
|
||||||
|
|
||||||
|
function drawWheelLimitArc(radius) {
|
||||||
|
context.strokeStyle = "rgba(181, 140, 255, 0.5)";
|
||||||
|
context.lineWidth = 1;
|
||||||
|
context.setLineDash([]);
|
||||||
|
context.beginPath();
|
||||||
|
|
||||||
|
for (let angle = -120; angle <= 120; angle += 5) {
|
||||||
|
const radians = -angle * Math.PI / 180;
|
||||||
|
const x = Math.cos(radians) * radius;
|
||||||
|
const y = Math.sin(radians) * radius;
|
||||||
|
if (angle === -120) context.moveTo(x, y);
|
||||||
|
else context.lineTo(x, y);
|
||||||
|
}
|
||||||
|
context.stroke();
|
||||||
|
|
||||||
|
for (const angle of [-120, 0, 120]) {
|
||||||
|
const radians = -angle * Math.PI / 180;
|
||||||
|
context.beginPath();
|
||||||
|
context.moveTo(
|
||||||
|
Math.cos(radians) * (radius - 3),
|
||||||
|
Math.sin(radians) * (radius - 3));
|
||||||
|
context.lineTo(
|
||||||
|
Math.cos(radians) * (radius + 3),
|
||||||
|
Math.sin(radians) * (radius + 3));
|
||||||
|
context.stroke();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
function drawArrow(x1, y1, x2, y2, color, width) {
|
||||||
|
const angle = Math.atan2(y2 - y1, x2 - x1);
|
||||||
|
context.strokeStyle = color;
|
||||||
|
context.fillStyle = color;
|
||||||
|
context.lineWidth = width;
|
||||||
|
context.setLineDash([]);
|
||||||
|
context.beginPath();
|
||||||
|
context.moveTo(x1, y1);
|
||||||
|
context.lineTo(x2, y2);
|
||||||
|
context.stroke();
|
||||||
|
context.beginPath();
|
||||||
|
context.moveTo(x2, y2);
|
||||||
|
context.lineTo(
|
||||||
|
x2 - 9 * Math.cos(angle - Math.PI / 6),
|
||||||
|
y2 - 9 * Math.sin(angle - Math.PI / 6));
|
||||||
|
context.lineTo(
|
||||||
|
x2 - 9 * Math.cos(angle + Math.PI / 6),
|
||||||
|
y2 - 9 * Math.sin(angle + Math.PI / 6));
|
||||||
|
context.closePath();
|
||||||
|
context.fill();
|
||||||
|
}
|
||||||
|
|
||||||
|
function draw() {
|
||||||
|
const view = getView();
|
||||||
|
drawGrid(view);
|
||||||
|
drawFleetCenter(view);
|
||||||
|
vehicles.forEach(vehicle => drawVehicle(vehicle, view));
|
||||||
|
requestAnimationFrame(draw);
|
||||||
|
}
|
||||||
|
|
||||||
|
function renderActions() {
|
||||||
|
const preferredGroups = ["舵轮模式", "运动测试", "维护"];
|
||||||
|
const groups = [...new Set(actions.map(action => action.group))]
|
||||||
|
.sort((left, right) => {
|
||||||
|
const leftIndex = preferredGroups.indexOf(left);
|
||||||
|
const rightIndex = preferredGroups.indexOf(right);
|
||||||
|
if (leftIndex < 0 && rightIndex < 0)
|
||||||
|
return left.localeCompare(right, "zh-CN");
|
||||||
|
if (leftIndex < 0) return 1;
|
||||||
|
if (rightIndex < 0) return -1;
|
||||||
|
return leftIndex - rightIndex;
|
||||||
|
});
|
||||||
|
|
||||||
|
actionGroups.innerHTML = groups.map(group => {
|
||||||
|
const buttons = actions
|
||||||
|
.filter(action => action.group === group)
|
||||||
|
.sort((a, b) => a.order - b.order)
|
||||||
|
.map(action => `
|
||||||
|
<button data-command="${action.key}"
|
||||||
|
class="${action.key === "stop" ? "danger-button" : ""}">
|
||||||
|
${action.displayName}
|
||||||
|
</button>`)
|
||||||
|
.join("");
|
||||||
|
|
||||||
|
return `
|
||||||
|
<section class="action-group">
|
||||||
|
<p class="group-label">${group}</p>
|
||||||
|
<div class="action-grid">${buttons}</div>
|
||||||
|
</section>`;
|
||||||
|
}).join("");
|
||||||
|
|
||||||
|
actionGroups.querySelectorAll("[data-command]")
|
||||||
|
.forEach(button => {
|
||||||
|
button.addEventListener("click", () =>
|
||||||
|
sendCommand(button.dataset.command));
|
||||||
|
});
|
||||||
|
}
|
||||||
|
|
||||||
|
function renderVehicleSelector() {
|
||||||
|
const previous = Number(vehicleSelect.value);
|
||||||
|
vehicleSelect.innerHTML = vehicles
|
||||||
|
.map(vehicle =>
|
||||||
|
`<option value="${vehicle.vehicleId}">车辆 ${vehicle.vehicleId}</option>`)
|
||||||
|
.join("");
|
||||||
|
|
||||||
|
if (vehicles.some(vehicle => vehicle.vehicleId === previous))
|
||||||
|
vehicleSelect.value = String(previous);
|
||||||
|
}
|
||||||
|
|
||||||
|
function renderCards() {
|
||||||
|
vehicleCards.innerHTML = vehicles.map(vehicle => {
|
||||||
|
const actual = vehicle.actualBodyTwist;
|
||||||
|
const selected =
|
||||||
|
Number(vehicleSelect.value) === vehicle.vehicleId;
|
||||||
|
return `
|
||||||
|
<button class="vehicle-card ${selected ? "selected" : ""}"
|
||||||
|
data-vehicle="${vehicle.vehicleId}">
|
||||||
|
<div class="card-topline">
|
||||||
|
<strong>CAR ${vehicle.vehicleId}</strong>
|
||||||
|
<span class="${vehicle.modeReady ? "status-ready" : "status-aligning"}">
|
||||||
|
${vehicle.modeReady ? "READY" : "ALIGNING"}
|
||||||
|
</span>
|
||||||
|
</div>
|
||||||
|
<div class="mode-name">${formatMode(vehicle.mode)}</div>
|
||||||
|
<dl>
|
||||||
|
<div><dt>X</dt><dd>${vehicle.xMeters.toFixed(2)} m</dd></div>
|
||||||
|
<div><dt>Y</dt><dd>${vehicle.yMeters.toFixed(2)} m</dd></div>
|
||||||
|
<div><dt>Yaw</dt><dd>${radToDeg(vehicle.yawRadians).toFixed(1)}°</dd></div>
|
||||||
|
<div><dt>Vx / Vy</dt><dd>${actual.vxMetersPerSecond.toFixed(2)} / ${actual.vyMetersPerSecond.toFixed(2)}</dd></div>
|
||||||
|
</dl>
|
||||||
|
</button>`;
|
||||||
|
}).join("");
|
||||||
|
|
||||||
|
vehicleCards.querySelectorAll("[data-vehicle]").forEach(card => {
|
||||||
|
card.addEventListener("click", () => {
|
||||||
|
vehicleSelect.value = card.dataset.vehicle;
|
||||||
|
renderCards();
|
||||||
|
renderSelectedVehicleDetail();
|
||||||
|
});
|
||||||
|
});
|
||||||
|
}
|
||||||
|
|
||||||
|
function renderSelectedVehicleDetail() {
|
||||||
|
const selectedId = Number(vehicleSelect.value);
|
||||||
|
const vehicle = vehicles.find(item => item.vehicleId === selectedId);
|
||||||
|
if (!vehicle) {
|
||||||
|
selectedVehicleDetail.innerHTML = "";
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
selectedVehicleDetail.innerHTML = `
|
||||||
|
<section class="wheel-detail">
|
||||||
|
<div class="geometry-note">
|
||||||
|
车体 1472 × 948 mm · 实际DLL轮位 X±750 / Y±500 mm
|
||||||
|
</div>
|
||||||
|
${vehicle.wheels.map(wheel => {
|
||||||
|
const actualPercent =
|
||||||
|
(wheel.actualAngleDegrees + 120) / 240 * 100;
|
||||||
|
const targetPercent =
|
||||||
|
(wheel.targetAngleDegrees + 120) / 240 * 100;
|
||||||
|
return `
|
||||||
|
<div class="wheel-row">
|
||||||
|
<div class="wheel-row-title">
|
||||||
|
<strong>${wheel.name}</strong>
|
||||||
|
<span>实际 ${signed(wheel.actualAngleDegrees)}° · 目标 ${signed(wheel.targetAngleDegrees)}°</span>
|
||||||
|
</div>
|
||||||
|
<div class="angle-scale">
|
||||||
|
<span class="zero-mark"></span>
|
||||||
|
<span class="target-mark" style="left:${targetPercent}%"></span>
|
||||||
|
<span class="actual-mark" style="left:${actualPercent}%"></span>
|
||||||
|
</div>
|
||||||
|
<div class="wheel-speed">
|
||||||
|
轮速 ${wheel.actualSpeedMetersPerSecond.toFixed(2)} m/s
|
||||||
|
</div>
|
||||||
|
</div>`;
|
||||||
|
}).join("")}
|
||||||
|
</section>`;
|
||||||
|
}
|
||||||
|
|
||||||
|
function formatMode(mode) {
|
||||||
|
return {
|
||||||
|
Normal: "正常模式",
|
||||||
|
CrabLeft: "左蟹行",
|
||||||
|
CrabRight: "右蟹行",
|
||||||
|
Spin: "自转模式"
|
||||||
|
}[mode] || mode;
|
||||||
|
}
|
||||||
|
|
||||||
|
async function loadActions() {
|
||||||
|
const response = await fetch("/api/actions", { cache: "no-store" });
|
||||||
|
if (!response.ok) throw new Error(`HTTP ${response.status}`);
|
||||||
|
actions = await response.json();
|
||||||
|
renderActions();
|
||||||
|
}
|
||||||
|
|
||||||
|
async function loadConfiguration() {
|
||||||
|
const response = await fetch("/api/configuration", {
|
||||||
|
cache: "no-store"
|
||||||
|
});
|
||||||
|
if (!response.ok) throw new Error(`HTTP ${response.status}`);
|
||||||
|
configuration = await response.json();
|
||||||
|
populateConfigurationForm();
|
||||||
|
}
|
||||||
|
|
||||||
|
function populateConfigurationForm() {
|
||||||
|
vehicleCountInput.value = String(configuration.vehicleCount);
|
||||||
|
document.getElementById("fleet-x").value =
|
||||||
|
configuration.fleetCenter.xMeters;
|
||||||
|
document.getElementById("fleet-y").value =
|
||||||
|
configuration.fleetCenter.yMeters;
|
||||||
|
document.getElementById("fleet-yaw").value =
|
||||||
|
radToDeg(configuration.fleetCenter.yawRadians).toFixed(1);
|
||||||
|
renderLayoutRows(configuration.vehicleCount, configuration.vehicles);
|
||||||
|
}
|
||||||
|
|
||||||
|
function readExistingLayouts() {
|
||||||
|
return [...layoutRows.querySelectorAll(".layout-row")].map(row => ({
|
||||||
|
vehicleId: Number(row.dataset.vehicleId),
|
||||||
|
xMeters: Number(row.querySelector("[data-field='x']").value),
|
||||||
|
yMeters: Number(row.querySelector("[data-field='y']").value),
|
||||||
|
yawRadians: degToRad(
|
||||||
|
Number(row.querySelector("[data-field='yaw']").value))
|
||||||
|
}));
|
||||||
|
}
|
||||||
|
|
||||||
|
function renderLayoutRows(count, sourceLayouts = readExistingLayouts()) {
|
||||||
|
const byId = new Map(
|
||||||
|
sourceLayouts.map(layout => [layout.vehicleId, layout]));
|
||||||
|
|
||||||
|
const fallbackSpacing = 2.0;
|
||||||
|
layoutRows.innerHTML = Array.from({ length: count }, (_, index) => {
|
||||||
|
const id = index + 1;
|
||||||
|
const fallbackX =
|
||||||
|
(index - (count - 1) / 2) * fallbackSpacing;
|
||||||
|
const layout = byId.get(id) || {
|
||||||
|
vehicleId: id,
|
||||||
|
xMeters: count === 1 ? 0 : fallbackX,
|
||||||
|
yMeters: 0,
|
||||||
|
yawRadians: 0
|
||||||
|
};
|
||||||
|
|
||||||
|
return `
|
||||||
|
<div class="layout-row" data-vehicle-id="${id}">
|
||||||
|
<strong>CAR ${id}</strong>
|
||||||
|
<label>X<input data-field="x" type="number" step="0.1"
|
||||||
|
value="${layout.xMeters}"></label>
|
||||||
|
<label>Y<input data-field="y" type="number" step="0.1"
|
||||||
|
value="${layout.yMeters}"></label>
|
||||||
|
<label>Yaw°<input data-field="yaw" type="number" step="1"
|
||||||
|
value="${radToDeg(layout.yawRadians).toFixed(1)}"></label>
|
||||||
|
</div>`;
|
||||||
|
}).join("");
|
||||||
|
}
|
||||||
|
|
||||||
|
async function applyConfiguration() {
|
||||||
|
const count = Number(vehicleCountInput.value);
|
||||||
|
const layouts = readExistingLayouts().slice(0, count);
|
||||||
|
const payload = {
|
||||||
|
vehicleCount: count,
|
||||||
|
fleetCenter: {
|
||||||
|
xMeters: Number(document.getElementById("fleet-x").value),
|
||||||
|
yMeters: Number(document.getElementById("fleet-y").value),
|
||||||
|
yawRadians: degToRad(
|
||||||
|
Number(document.getElementById("fleet-yaw").value))
|
||||||
|
},
|
||||||
|
vehicles: layouts
|
||||||
|
};
|
||||||
|
|
||||||
|
const response = await fetch("/api/configuration", {
|
||||||
|
method: "POST",
|
||||||
|
headers: { "Content-Type": "application/json" },
|
||||||
|
body: JSON.stringify(payload)
|
||||||
|
});
|
||||||
|
const result = await response.json();
|
||||||
|
|
||||||
|
if (!response.ok) {
|
||||||
|
commandMessage.textContent =
|
||||||
|
result.message || "布局配置失败。";
|
||||||
|
commandMessage.classList.add("error");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
configuration = result;
|
||||||
|
commandMessage.textContent =
|
||||||
|
`已应用 ${configuration.vehicleCount} 辆车的车队布局。`;
|
||||||
|
commandMessage.classList.remove("error");
|
||||||
|
await updateVehicles();
|
||||||
|
}
|
||||||
|
|
||||||
|
async function updateVehicles() {
|
||||||
|
try {
|
||||||
|
const response = await fetch("/api/vehicles", {
|
||||||
|
cache: "no-store"
|
||||||
|
});
|
||||||
|
if (!response.ok) throw new Error(`HTTP ${response.status}`);
|
||||||
|
vehicles = await response.json();
|
||||||
|
renderVehicleSelector();
|
||||||
|
renderCards();
|
||||||
|
renderSelectedVehicleDetail();
|
||||||
|
connectionDot.classList.add("online");
|
||||||
|
connectionText.textContent = "离线仿真运行中";
|
||||||
|
successfulUpdates += 1;
|
||||||
|
} catch {
|
||||||
|
connectionDot.classList.remove("online");
|
||||||
|
connectionText.textContent = "仿真后端未连接";
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
async function sendCommand(command) {
|
||||||
|
const vehicleId = vehicleSelect.value;
|
||||||
|
try {
|
||||||
|
const response = await fetch(
|
||||||
|
`/api/vehicles/${vehicleId}/commands/${command}`,
|
||||||
|
{ method: "POST" });
|
||||||
|
const result = await response.json();
|
||||||
|
commandMessage.textContent = result.message;
|
||||||
|
commandMessage.classList.toggle("error", !result.success);
|
||||||
|
await updateVehicles();
|
||||||
|
} catch {
|
||||||
|
commandMessage.textContent =
|
||||||
|
"命令发送失败,请检查仿真后端。";
|
||||||
|
commandMessage.classList.add("error");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
function getRemoteScale(input) {
|
||||||
|
return Number(input.value) / 100;
|
||||||
|
}
|
||||||
|
|
||||||
|
function updateRemoteLabels() {
|
||||||
|
speedScaleValue.textContent = `${speedScaleInput.value}%`;
|
||||||
|
steeringScaleValue.textContent =
|
||||||
|
`${steeringScaleInput.value}%`;
|
||||||
|
}
|
||||||
|
|
||||||
|
async function sendManualControl(
|
||||||
|
throttle,
|
||||||
|
steering,
|
||||||
|
showMessage = false,
|
||||||
|
vehicleIdOverride = null) {
|
||||||
|
remoteThrottle = throttle;
|
||||||
|
remoteSteering = steering;
|
||||||
|
|
||||||
|
const active =
|
||||||
|
Math.abs(throttle) > 0.001 ||
|
||||||
|
Math.abs(steering) > 0.001;
|
||||||
|
remoteState.textContent = active
|
||||||
|
? `油门 ${signed(throttle)} · 转向 ${signed(steering)}`
|
||||||
|
: "已松开";
|
||||||
|
remoteState.classList.toggle("active", active);
|
||||||
|
|
||||||
|
try {
|
||||||
|
const vehicleId = vehicleIdOverride ??
|
||||||
|
(active
|
||||||
|
? vehicleSelect.value
|
||||||
|
: remoteVehicleId ?? vehicleSelect.value);
|
||||||
|
|
||||||
|
if (active)
|
||||||
|
remoteVehicleId = String(vehicleId);
|
||||||
|
|
||||||
|
const response = await fetch(
|
||||||
|
`/api/vehicles/${vehicleId}/manual-control`,
|
||||||
|
{
|
||||||
|
method: "POST",
|
||||||
|
headers: { "Content-Type": "application/json" },
|
||||||
|
body: JSON.stringify({
|
||||||
|
throttle,
|
||||||
|
steering,
|
||||||
|
speedScale: getRemoteScale(speedScaleInput),
|
||||||
|
steeringScale: getRemoteScale(steeringScaleInput)
|
||||||
|
})
|
||||||
|
});
|
||||||
|
const result = await response.json();
|
||||||
|
|
||||||
|
if (!response.ok || showMessage) {
|
||||||
|
commandMessage.textContent = result.message;
|
||||||
|
commandMessage.classList.toggle(
|
||||||
|
"error",
|
||||||
|
!result.success);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!active &&
|
||||||
|
String(vehicleId) === remoteVehicleId) {
|
||||||
|
remoteVehicleId = null;
|
||||||
|
}
|
||||||
|
} catch {
|
||||||
|
commandMessage.textContent =
|
||||||
|
"虚拟遥控命令发送失败,请检查仿真后端。";
|
||||||
|
commandMessage.classList.add("error");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
function stopRemote(showMessage = false) {
|
||||||
|
const vehicleId = remoteVehicleId;
|
||||||
|
pressedDriveKeys.clear();
|
||||||
|
document.querySelectorAll(".remote-pad .pressed")
|
||||||
|
.forEach(button => button.classList.remove("pressed"));
|
||||||
|
return sendManualControl(
|
||||||
|
0,
|
||||||
|
0,
|
||||||
|
showMessage,
|
||||||
|
vehicleId);
|
||||||
|
}
|
||||||
|
|
||||||
|
function driveFromKeyboard() {
|
||||||
|
const forward =
|
||||||
|
pressedDriveKeys.has("KeyW") ||
|
||||||
|
pressedDriveKeys.has("ArrowUp");
|
||||||
|
const backward =
|
||||||
|
pressedDriveKeys.has("KeyS") ||
|
||||||
|
pressedDriveKeys.has("ArrowDown");
|
||||||
|
const left =
|
||||||
|
pressedDriveKeys.has("KeyA") ||
|
||||||
|
pressedDriveKeys.has("ArrowLeft");
|
||||||
|
const right =
|
||||||
|
pressedDriveKeys.has("KeyD") ||
|
||||||
|
pressedDriveKeys.has("ArrowRight");
|
||||||
|
|
||||||
|
const throttle = Number(forward) - Number(backward);
|
||||||
|
const steering = Number(left) - Number(right);
|
||||||
|
sendManualControl(throttle, steering);
|
||||||
|
}
|
||||||
|
|
||||||
|
function isTypingTarget(target) {
|
||||||
|
return target instanceof HTMLInputElement ||
|
||||||
|
target instanceof HTMLSelectElement ||
|
||||||
|
target instanceof HTMLTextAreaElement;
|
||||||
|
}
|
||||||
|
|
||||||
|
document.querySelectorAll(
|
||||||
|
".remote-pad [data-throttle][data-steering]")
|
||||||
|
.forEach(button => {
|
||||||
|
button.addEventListener("pointerdown", event => {
|
||||||
|
event.preventDefault();
|
||||||
|
remotePointerId = event.pointerId;
|
||||||
|
button.setPointerCapture(event.pointerId);
|
||||||
|
button.classList.add("pressed");
|
||||||
|
sendManualControl(
|
||||||
|
Number(button.dataset.throttle),
|
||||||
|
Number(button.dataset.steering));
|
||||||
|
});
|
||||||
|
|
||||||
|
const release = event => {
|
||||||
|
if (remotePointerId !== event.pointerId)
|
||||||
|
return;
|
||||||
|
remotePointerId = null;
|
||||||
|
button.classList.remove("pressed");
|
||||||
|
stopRemote();
|
||||||
|
};
|
||||||
|
|
||||||
|
button.addEventListener("pointerup", release);
|
||||||
|
button.addEventListener("pointercancel", release);
|
||||||
|
button.addEventListener("lostpointercapture", release);
|
||||||
|
});
|
||||||
|
|
||||||
|
document.getElementById("remote-stop").addEventListener(
|
||||||
|
"click",
|
||||||
|
() => stopRemote(true));
|
||||||
|
|
||||||
|
window.addEventListener("keydown", event => {
|
||||||
|
if (isTypingTarget(event.target))
|
||||||
|
return;
|
||||||
|
|
||||||
|
const driveKeys = [
|
||||||
|
"KeyW", "KeyS", "KeyA", "KeyD",
|
||||||
|
"ArrowUp", "ArrowDown", "ArrowLeft", "ArrowRight"
|
||||||
|
];
|
||||||
|
if (!driveKeys.includes(event.code))
|
||||||
|
return;
|
||||||
|
|
||||||
|
event.preventDefault();
|
||||||
|
if (event.repeat)
|
||||||
|
return;
|
||||||
|
|
||||||
|
pressedDriveKeys.add(event.code);
|
||||||
|
driveFromKeyboard();
|
||||||
|
});
|
||||||
|
|
||||||
|
window.addEventListener("keyup", event => {
|
||||||
|
if (!pressedDriveKeys.has(event.code))
|
||||||
|
return;
|
||||||
|
|
||||||
|
event.preventDefault();
|
||||||
|
pressedDriveKeys.delete(event.code);
|
||||||
|
driveFromKeyboard();
|
||||||
|
});
|
||||||
|
|
||||||
|
window.addEventListener("blur", () => stopRemote());
|
||||||
|
document.addEventListener("visibilitychange", () => {
|
||||||
|
if (document.hidden)
|
||||||
|
stopRemote();
|
||||||
|
});
|
||||||
|
|
||||||
|
[speedScaleInput, steeringScaleInput].forEach(input => {
|
||||||
|
input.addEventListener("input", () => {
|
||||||
|
updateRemoteLabels();
|
||||||
|
if (remoteThrottle !== 0 || remoteSteering !== 0) {
|
||||||
|
sendManualControl(
|
||||||
|
remoteThrottle,
|
||||||
|
remoteSteering);
|
||||||
|
}
|
||||||
|
});
|
||||||
|
});
|
||||||
|
|
||||||
|
document.getElementById("reset-all").addEventListener(
|
||||||
|
"click",
|
||||||
|
async () => {
|
||||||
|
await fetch("/api/reset", { method: "POST" });
|
||||||
|
commandMessage.textContent = "全部仿真车已复位。";
|
||||||
|
commandMessage.classList.remove("error");
|
||||||
|
await updateVehicles();
|
||||||
|
});
|
||||||
|
|
||||||
|
document.getElementById("apply-configuration")
|
||||||
|
.addEventListener("click", applyConfiguration);
|
||||||
|
|
||||||
|
vehicleCountInput.addEventListener("change", () =>
|
||||||
|
renderLayoutRows(Number(vehicleCountInput.value)));
|
||||||
|
|
||||||
|
vehicleSelect.addEventListener("change", () => {
|
||||||
|
stopRemote();
|
||||||
|
renderCards();
|
||||||
|
renderSelectedVehicleDetail();
|
||||||
|
});
|
||||||
|
|
||||||
|
window.addEventListener("resize", resizeCanvas);
|
||||||
|
|
||||||
|
setInterval(updateVehicles, 100);
|
||||||
|
setInterval(() => {
|
||||||
|
const now = performance.now();
|
||||||
|
const seconds = (now - lastUpdateTime) / 1000;
|
||||||
|
updateRate.textContent =
|
||||||
|
`${(successfulUpdates / seconds).toFixed(0)} Hz`;
|
||||||
|
successfulUpdates = 0;
|
||||||
|
lastUpdateTime = now;
|
||||||
|
}, 1000);
|
||||||
|
|
||||||
|
function signed(value) {
|
||||||
|
return `${value >= 0 ? "+" : ""}${value.toFixed(1)}`;
|
||||||
|
}
|
||||||
|
|
||||||
|
function degToRad(value) {
|
||||||
|
return value * Math.PI / 180;
|
||||||
|
}
|
||||||
|
|
||||||
|
function radToDeg(value) {
|
||||||
|
return value * 180 / Math.PI;
|
||||||
|
}
|
||||||
|
|
||||||
|
async function initialize() {
|
||||||
|
try {
|
||||||
|
await Promise.all([
|
||||||
|
loadActions(),
|
||||||
|
loadConfiguration(),
|
||||||
|
updateVehicles()
|
||||||
|
]);
|
||||||
|
} catch {
|
||||||
|
connectionText.textContent = "仿真初始化失败";
|
||||||
|
commandMessage.textContent =
|
||||||
|
"无法读取命令或布局配置,请检查后端。";
|
||||||
|
commandMessage.classList.add("error");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
resizeCanvas();
|
||||||
|
updateRemoteLabels();
|
||||||
|
initialize();
|
||||||
|
requestAnimationFrame(draw);
|
||||||
Reference in New Issue
Block a user