diff --git a/ClumsyPilot/ClumsyPilot.csproj b/ClumsyPilot/ClumsyPilot.csproj index 28ab17a..ceb2716 100644 --- a/ClumsyPilot/ClumsyPilot.csproj +++ b/ClumsyPilot/ClumsyPilot.csproj @@ -15,6 +15,11 @@ + + + + ref\CommonUsage.dll diff --git a/ClumsyPilot/MovementTests.cs b/ClumsyPilot/MovementTests.cs index 123a689..7cd300f 100644 --- a/ClumsyPilot/MovementTests.cs +++ b/ClumsyPilot/MovementTests.cs @@ -72,30 +72,55 @@ namespace MultiWheelC [MovementTest(name = "底盘旋转测试")] public class RotateToAngleTest : MovementTest { - // 底盘旋转测试不支持停止操作。 + private DriveTask _dt; + + // 停止当前正在执行的底盘原地旋转任务。 public override void TestStop() { - throw new NotImplementedException(); + _dt?.Stop(); } // 交互输入目标角度后执行底盘原地旋转测试。 public override void Test() { - var target = UI.GetInput("输入旋转角度："); - new DriveTask(new MultiWheelRotateInPlace() + var input = UI.GetInput("输入旋转角度："); + if (!float.TryParse(input, out var angleTarget)) { - AngleTarget = float.Parse(target), - PidparamsRead = () => new PIDParams() + Console.WriteLine( + $"旋转测试输入无效：{input}"); + return; + } + // 防止重复启动测试时，上一项旋转任务仍在运行。 + _dt?.Stop(); + var task = new DriveTask( + new MultiWheelRotateInPlace { - Kp = PilotDefinition.Conf.InPlaceRotateKp, - Ki = PilotDefinition.Conf.InPlaceRotateKi, - Kd = PilotDefinition.Conf.InPlaceRotateKd, - DeadZone = PilotDefinition.Conf.InPlaceRotateArriveDeg, - SpeedAccPerSec = PilotDefinition.Conf.InPlaceRotateAcc, - OutputUpperThreshold = PilotDefinition.Conf.InPlaceRotateMaxSpeed, - MaxI = PilotDefinition.Conf.InPlaceRotateMaxI, + AngleTarget = angleTarget, + + PidparamsRead = () => new PIDParams + { + Kp = PilotDefinition.Conf.InPlaceRotateKp, + Ki = PilotDefinition.Conf.InPlaceRotateKi, + 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(); + } } } } diff --git a/ClumsyPilot/Movements.cs b/ClumsyPilot/Movements.cs index 05fbaac..e42c5f5 100644 --- a/ClumsyPilot/Movements.cs +++ b/ClumsyPilot/Movements.cs @@ -124,4 +124,5 @@ namespace MultiWheelC } } + } diff --git a/ClumsyPilot/PilotConfig.cs b/ClumsyPilot/PilotConfig.cs index 5ec4874..fc10a29 100644 --- a/ClumsyPilot/PilotConfig.cs +++ b/ClumsyPilot/PilotConfig.cs @@ -60,7 +60,7 @@ public class PilotConfig : MultiWheelPilotConfig #endregion -#if false + #region 单车-钻车与夹抱 [FieldMember(desc = "2腿检测：雷达名（逗号分隔可多个）")] @@ -165,10 +165,8 @@ public class PilotConfig : MultiWheelPilotConfig [FieldMember(desc = "抱夹控制pid:Thresh")] public float ClampControlThresh = 0.2f; [FieldMember(desc = "抱夹控制pid:DeadZone")] public float ClampControlDeadZone = 5f; [FieldMember(desc = "抱夹最大速度")] public float MaxClampSpeed = 1.5f; - - #endregion - +#if false #region 多车-编队与遥控 [FieldMember(desc = "联动时转向角爬升加速度")] public float SyncThAccPerSec = 30f; [FieldMember(desc = "两车间距 (mm)")] public float TestCarSyncDistance = 2400f; @@ -330,24 +328,6 @@ public class PilotConfig : MultiWheelPilotConfig #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 } diff --git a/ClumsyPilot/PilotDefinition.cs b/ClumsyPilot/PilotDefinition.cs index 07239b9..188fa94 100644 --- a/ClumsyPilot/PilotDefinition.cs +++ b/ClumsyPilot/PilotDefinition.cs @@ -11,4 +11,34 @@ public class PilotDefinition : MultiWheelPilotDefinition cart.LeftArmErrorCode != 0); + AddAlarm("右夹臂驱动报警", 2, () => cart.RightArmErrorCode != 0); } } } \ No newline at end of file diff --git a/MedullaAdapter/DiverCartDefinition.cs b/MedullaAdapter/DiverCartDefinition.cs index b427d3d..85a76d2 100644 --- a/MedullaAdapter/DiverCartDefinition.cs +++ b/MedullaAdapter/DiverCartDefinition.cs @@ -6,6 +6,7 @@ using Medulla.Types; using System; using System.Collections.Generic; using System.Threading; +using MyParking.Shared; namespace MedullaAdapter { @@ -26,11 +27,16 @@ namespace MedullaAdapter } internal ManualControlMode TransmitterControlMode = ManualControlMode.Normal; internal DateTime TransmitterLastTime = DateTime.Now; // 物理遥控器计算两次实体遥控器指令之间的时间间隔 + private ManualControlMode? _pendingManualMode; + private ManualControlMode? _activeManualMode; #endregion #region AsUpperIO [AsUpperIO(desc = "从C上复位")] public bool ResetFromC; [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 #region AsLowerIO @@ -42,6 +48,16 @@ namespace MedullaAdapter [AsLowerIO(desc = "左后右轮实际位置")] public float LRRActualPos; [AsLowerIO(desc = "右后左轮实际位置")] public float RRLActualPos; [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 float SOH; [AsInitParam(desc = "车号")][AsLowerIO] public int CarNum = 1; @@ -51,6 +67,12 @@ namespace MedullaAdapter [AsInitParam(desc = "MCU端口号")] public string MCUPort = "COM4"; [AsInitParam(desc = "遥控器速度上限")] public float TransmitterSpeedUpperLimit = 1.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 @@ -75,6 +97,8 @@ namespace MedullaAdapter [IOObjectMonitor(desc = "左后右驱动器远程帧706")] public byte LRRRemoteCode = 0; [IOObjectMonitor(desc = "右后左驱动器远程帧707")] public byte RRLRemoteCode = 0; [IOObjectMonitor(desc = "右后右驱动器远程帧708")] public byte RRRRemoteCode = 0; + [IOObjectMonitor(desc = "左夹臂驱动器远程帧709")] public byte LArmRemoteCode = 0; + [IOObjectMonitor(desc = "右夹臂驱动器远程帧70A")] public byte RArmRemoteCode = 0; #endregion #region 操作按钮 @@ -188,43 +212,193 @@ namespace MedullaAdapter TimeSpan? interval = null) { if (Chassis == null) return; + + var adapter = GetChassisAdapter(); + if (adapter == null) return; + + // 模式变化时先停车并下发舵轮准备角度； + // 在实际舵角到位之前，不开放驱动速度。 + if (!EnsureManualModeReady(mode, interval)) + { + adapter.StopImmediately(); + return; + } + var speed = speedThreshold * y; - var thPow = (float)Math.Pow(Math.Abs(x), ManualThetaPow) * Math.Sign(x); - var frontTh = -thPow * MaxManualTheta; - var rearTh = thPow * MaxManualTheta; + var omega = CalculateManualOmega(speed, x); ManualMode = (int)mode; + switch (mode) { case ManualControlMode.Normal: - SetChassisDirection(frontDirection); - Chassis.SendMotion(speed, frontTh, rearTh, interval); + SendBodyCommand(vx: speed, vy: 0.0, omegaRadiansPerSecond: omega, interval); break; case ManualControlMode.Crab: - SetChassisDirection(90); - Chassis.SendMotion(speed, frontTh, rearTh, interval); + SendBodyCommand(vx: 0.0, vy: speed, omegaRadiansPerSecond: omega, interval); break; 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; default: ManualMode = -1; - Chassis.PredefinedDriveStop(); + adapter.StopImmediately(); break; } } - private void SetChassisDirection(float direction) + // 停车后切换模式：先预转舵轮，实际角度到位后才允许发送运动命令。 + private bool EnsureManualModeReady( + ManualControlMode mode, + TimeSpan? interval) { - var origin = Chassis.GetOriginBias(); - // 方向没有变化时不重复计算轮组几何关系。 - if (Math.Abs(origin.Z - direction) < 0.001f) + var adapter = GetChassisAdapter(); + if (adapter == null) + 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; - Chassis.SetOriginBias( - origin.X, - origin.Y, - direction); - // 坐标系改变后重新等待四个舵轮对齐。 - Chassis.AfterDirectionChanged(); + + var command = new ChassisCommand( + CarNum, + new Twist2D(vx, vy, omegaRadiansPerSecond)); + + 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兼容参数（暂保留原硬件协议） @@ -237,65 +411,6 @@ namespace MedullaAdapter [AsUpperIO(desc = "多车灯光同步兼容值，-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 } diff --git a/MedullaAdapter/MedullaAdapter.csproj b/MedullaAdapter/MedullaAdapter.csproj index dec728d..fbac02d 100644 --- a/MedullaAdapter/MedullaAdapter.csproj +++ b/MedullaAdapter/MedullaAdapter.csproj @@ -17,7 +17,6 @@ ref\RefCartActivator.dll false - ref\RefMedullaCore.dll false @@ -44,4 +43,15 @@ + + + + + + + + diff --git a/MedullaAdapter/MotorRoutine.cs b/MedullaAdapter/MotorRoutine.cs index 9725a05..6e5b980 100644 --- a/MedullaAdapter/MotorRoutine.cs +++ b/MedullaAdapter/MotorRoutine.cs @@ -56,6 +56,7 @@ namespace MedullaAdapter cart.TransmitterSpeed, DateTime.Now - cart.TransmitterLastTime); + StopClampArms(); cart.TransmitterLastTime = DateTime.Now; } @@ -95,6 +96,7 @@ namespace MedullaAdapter 0, 0, 0, cart.TransmitterSpeed, interval); + StopClampArms(); return; } // SA关闭后立即停车。 @@ -105,6 +107,7 @@ namespace MedullaAdapter 0, 0, 0, cart.TransmitterSpeed, interval); + StopClampArms(); return; } // 限制实体遥控器的最大速度。 @@ -116,6 +119,9 @@ namespace MedullaAdapter // SD的Mode0作为底盘驾驶档。 if (cart.Transmitter_SD == TransmitterState.Mode0) { + // 底盘驾驶档不允许保留上一周期的夹臂速度。 + StopClampArms(); + cart.ManualControl( cart.TransmitterControlMode, cart.TransmitterLeftJoystickValX, @@ -126,12 +132,37 @@ namespace MedullaAdapter 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.TransmitterControlMode, 0, 0, 0, cart.TransmitterSpeed, interval); + StopClampArms(); + } + + // M层单车夹臂安全：清除物理遥控器留下的左右夹臂速度命令。 + private void StopClampArms() + { + cart.SpeedLeftArm = 0; + cart.SpeedRightArm = 0; } // M层单车底盘：根据四个舵轮的目标角度和实际角度修正8个驱动电机速度。 @@ -328,4 +359,4 @@ namespace MedullaAdapter } } -} \ No newline at end of file +} diff --git a/MedullaAdapter/Remote.cs b/MedullaAdapter/Remote.cs index 842d426..85ceb6e 100644 --- a/MedullaAdapter/Remote.cs +++ b/MedullaAdapter/Remote.cs @@ -1,15 +1,86 @@ // Medulla虚拟遥控器和夹臂控制 using MDCSToolBox.Medulla.Chassis.MultiWheel; +using CartActivator; namespace MedullaAdapter { // M层单车虚拟遥控器：使用父类提供的底盘控制界面。 public class Remote : MultiWheelRemote { + // 将父类虚拟遥控器界面输入统一转发到本车的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() { cart.Chassis?.PredefinedDriveStop(); } } -} \ No newline at end of file +} diff --git a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll index 8f7e644..ef41c12 100644 Binary files a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll and b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.dll differ diff --git a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb index 367927c..28ef8cc 100644 Binary files a/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb and b/MedullaAdapter/build/Medulla/plugins/MedullaAdapter.pdb differ diff --git a/Shared/ChassisCommand.cs b/Shared/ChassisCommand.cs new file mode 100644 index 0000000..5f073fd --- /dev/null +++ b/Shared/ChassisCommand.cs @@ -0,0 +1,183 @@ +// 纯数据层：只描述坐标、速度和命令 +// 定义二维坐标、位姿、速度、车队布局和单车底盘命令。 +// Shared层统一使用SI单位：位置m、线速度m/s、角度rad、角速度rad/s。 +// 车体坐标系采用右手系：X向前、Y向左、逆时针角度和角速度为正。 +// 命名约定：XxxInYyy表示Xxx在Yyy坐标系中的表达。 + +namespace MyParking.Shared +{ + /// + /// 二维坐标点，X、Y单位均为米。 + /// + 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); + } + + /// + /// 二维局部坐标系在父坐标系中的位姿。 + /// 位置单位为米，朝向单位为弧度，逆时针为正。 + /// 具体父子关系由变量名称说明，例如RadarPoseInBody。 + /// + 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); + } + + /// + /// 二维刚体速度。 + /// 线速度单位为m/s，角速度单位为rad/s。 + /// 速度所属坐标系由持有该Twist2D的外层类型或变量名称确定。 + /// + 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); + } + + /// + /// 发送给单辆车的车体坐标系速度命令。 + /// + public readonly struct ChassisCommand + { + public ChassisCommand( + int vehicleId, + Twist2D bodyTwist) + { + VehicleId = vehicleId; + BodyTwist = bodyTwist; + } + + public int VehicleId { get; } + + /// + /// 单车车体坐标系速度：X向前、Y向左、逆时针旋转为正。 + /// + public Twist2D BodyTwist { get; } + + /// + /// 创建指定车辆的停止命令。 + /// + public static ChassisCommand Stop(int vehicleId) + { + return new ChassisCommand( + vehicleId, + Twist2D.Zero); + } + } + + /// + /// 单辆车的车体坐标系在车队坐标系中的位姿。 + /// + public readonly struct VehicleLayout + { + public VehicleLayout( + int vehicleId, + Pose2D poseInFleet) + { + VehicleId = vehicleId; + PoseInFleet = poseInFleet; + } + + public int VehicleId { get; } + + public Pose2D PoseInFleet { get; } + } + + /// + /// 车队整体运动命令，速度分量均在车队坐标系中表达。 + /// + public readonly struct FleetMotionCommand + { + public FleetMotionCommand( + Point2D referencePointInFleet, + Twist2D twistAtReferencePoint) + { + ReferencePointInFleet = referencePointInFleet; + TwistAtReferencePoint = twistAtReferencePoint; + } + + /// + /// 速度命令对应的参考点，也可作为自定义旋转中心。 + /// + public Point2D ReferencePointInFleet { get; } + + /// + /// 参考点处的车队速度。 + /// + public Twist2D TwistAtReferencePoint { get; } + + /// + /// 创建绕指定中心原地旋转的车队命令。 + /// + public static FleetMotionCommand RotateAround( + Point2D rotationCenterInFleet, + double omegaRadiansPerSecond) + { + return new FleetMotionCommand( + rotationCenterInFleet, + new Twist2D( + 0.0, + 0.0, + omegaRadiansPerSecond)); + } + + /// + /// 创建车队停止命令。 + /// + public static FleetMotionCommand Stop() + { + return new FleetMotionCommand( + Point2D.Zero, + Twist2D.Zero); + } + } + + +} \ No newline at end of file diff --git a/Shared/FleetKinematics.cs b/Shared/FleetKinematics.cs new file mode 100644 index 0000000..46b6899 --- /dev/null +++ b/Shared/FleetKinematics.cs @@ -0,0 +1 @@ +// 把车队整体速度分解为每辆车的局部速度 diff --git a/Shared/FrameTransform2D.cs b/Shared/FrameTransform2D.cs new file mode 100644 index 0000000..367c7bf --- /dev/null +++ b/Shared/FrameTransform2D.cs @@ -0,0 +1,177 @@ +// 车体、运动、车队坐标系之间的转换 +using System; + +namespace MyParking.Shared +{ + /// + /// 提供二维刚体坐标系之间的点、向量、位姿和速度变换。 + /// 坐标系采用X向前、Y向左、逆时针为正的右手系。 + /// + public static class FrameTransform2D + { + private const double TwoPi = 2.0 * Math.PI; + /// + /// 将角度归一化到[-π, π)范围。 + /// + 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; + + } + + /// + /// 计算从current到target的最短角度差。 + /// 返回正值表示逆时针旋转。 + /// + public static double ShortestAngleDifference( + double targetRadians, + double currentRadians) + { + return NormalizeAngle(targetRadians - currentRadians); + } + + /// + /// 将源坐标系中的点变换到目标坐标系。 + /// sourcePoseInTarget表示源坐标系在目标坐标系中的位姿。 + /// + 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); + } + + /// + /// 将目标坐标系中的点反向变换到源坐标系。 + /// + 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); + } + + /// + /// 将源坐标系中的向量旋转到目标坐标系。 + /// 向量没有位置，因此不叠加平移量。 + /// + 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); + } + + /// + /// 组合两级坐标变换。 + /// parentFromMiddle表示middle在parent中的位姿； + /// middleFromChild表示child在middle中的位姿； + /// 返回child在parent中的位姿。 + /// + 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)); + } + + /// + /// 对坐标变换求逆。 + /// 输入child在parent中的位姿，返回parent在child中的位姿。 + /// + 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)); + } + + + /// + /// 将源坐标系中的位姿变换到目标坐标系。 + /// + public static Pose2D TransformPose( + Pose2D sourcePoseInTarget, + Pose2D poseInSource) + { + return Compose(sourcePoseInTarget, poseInSource); + } + + /// + /// 转换同一物理参考点处的速度表达坐标系。 + /// 只旋转线速度，角速度保持不变。 + /// + 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); + } + } +} \ No newline at end of file diff --git a/Shared/MultiWheelChassisAdapter.cs b/Shared/MultiWheelChassisAdapter.cs new file mode 100644 index 0000000..0af8468 --- /dev/null +++ b/Shared/MultiWheelChassisAdapter.cs @@ -0,0 +1,289 @@ +// 将统一命令转换为原 Chassis API 调用 +using System; +using CommonUsage.Chassis; + +namespace MyParking.Shared +{ + /// + /// 将统一的单车车体速度命令转换为旧版MultiWheelChassis调用。 + /// 车体坐标系固定为X向前、Y向左、逆时针为正。 + /// + public sealed class MultiWheelChassisAdapter + { + #region 辅助内容 + private const double RadiansToDegrees = 180.0 / Math.PI; + private const float BiasTolerance = 0.001f; + private readonly MultiWheelChassis _chassis; + /// + /// 当前适配器对应的车辆编号。 + /// + public int VehicleId { get; } + + /// + /// 检查旧底盘是否仍处于无偏置的真实车体坐标系。 + /// + 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控制蟹行。"); + } + /// + /// 检查底盘命令是否包含无效数值。 + /// + private static void ValidateTwist(Twist2D twist) + { + ValidateFinite( + twist.VxMetersPerSecond, + nameof(twist.VxMetersPerSecond)); + + ValidateFinite( + twist.VyMetersPerSecond, + nameof(twist.VyMetersPerSecond)); + + ValidateFinite( + twist.OmegaRadiansPerSecond, + nameof(twist.OmegaRadiansPerSecond)); + } + /// + /// 检查数值是否为有限值。 + /// + 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可表示范围。"); + } + } + + /// + /// 获取最近一次底盘运动分解失败原因。 + /// + public string LastFailureReason => + _chassis.LastMotionDecomposeFailureReason; + #endregion + + /// + /// 将旧底盘的原点偏置恢复为真实单车车体坐标系。 + /// + 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(); + } + + + /// + /// 将车体坐标系速度命令发送给多舵轮底盘。 + /// + 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; + } + /// + /// 按底盘减速度配置平滑停车，需要在控制周期中持续调用。 + /// + public void RampStop(TimeSpan? interval = null) + { + _chassis.RampStop(interval); + } + /// + /// 立即将所有驱动轮速度下发为零。 + /// + public void StopImmediately() + { + _chassis.PredefinedDriveStop(); + } + + /// + /// 停车并将所有舵轮转到指定的车体角度。 + /// 只调整舵轮角度，不产生车辆线速度。 + /// + 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; + } + + /// + /// 检查所有舵轮是否已经对准给定方向。 + /// + 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; + } + + /// + /// 停车并将舵轮预对齐到原地自转方向。 + /// 返回是否成功生成舵轮目标。 + /// + public bool PrepareSpin( + TimeSpan? interval = null) + { + EnsureBodyFrameIsActive(); + _chassis.PredefinedDriveStop(); + + var success = + _chassis.SendRotateMotion( + 0.0f, + interval); + + if (!success) + { + _chassis.PredefinedDriveStop(); + } + return success; + } + /// + /// 所有舵轮是否已对齐到原地自转方向。 + /// + public bool AreSpinWheelsAligned => _chassis.LastRotateAligned; + + + + } +} diff --git a/Shared/SteeringConstraintManager.cs b/Shared/SteeringConstraintManager.cs new file mode 100644 index 0000000..33a2890 --- /dev/null +++ b/Shared/SteeringConstraintManager.cs @@ -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° 蟹行姿态跟踪，而不是一直逼近机械硬限位。 \ No newline at end of file diff --git a/Simulation/Commands/BuiltInSimulationActions.cs b/Simulation/Commands/BuiltInSimulationActions.cs new file mode 100644 index 0000000..c5009a2 --- /dev/null +++ b/Simulation/Commands/BuiltInSimulationActions.cs @@ -0,0 +1,121 @@ +using MyParking.Simulation.Core; + +namespace MyParking.Simulation.Commands; + +/// +/// 内置离线测试动作；新增带特性的方法后网页会自动生成按钮。 +/// +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; + } +} diff --git a/Simulation/Commands/MySimulationTests.cs b/Simulation/Commands/MySimulationTests.cs new file mode 100644 index 0000000..1e127e5 --- /dev/null +++ b/Simulation/Commands/MySimulationTests.cs @@ -0,0 +1,31 @@ +using MyParking.Shared; +using MyParking.Simulation.Core; + +namespace MyParking.Simulation.Commands; + +/// +/// 我自己增加的停车机器人仿真测试动作。 +/// +public static class MySimulationTests +{ + /// + /// 测试车辆以0.2m/s向车体左侧运动。 + /// + // [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); + // } +} \ No newline at end of file diff --git a/Simulation/Commands/SimulationActionAttribute.cs b/Simulation/Commands/SimulationActionAttribute.cs new file mode 100644 index 0000000..a583439 --- /dev/null +++ b/Simulation/Commands/SimulationActionAttribute.cs @@ -0,0 +1,38 @@ +namespace MyParking.Simulation.Commands; + +/// +/// 将一个静态仿真测试方法自动注册为网页按钮。 +/// 方法签名必须为bool Xxx(SimulationVehicle vehicle)。 +/// +[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; } +} + +/// +/// 网页生成测试按钮所需的命令元数据。 +/// +public sealed record SimulationActionDescriptor( + string Key, + string DisplayName, + string Group, + int Order); diff --git a/Simulation/Commands/SimulationCommandDispatcher.cs b/Simulation/Commands/SimulationCommandDispatcher.cs new file mode 100644 index 0000000..57c6406 --- /dev/null +++ b/Simulation/Commands/SimulationCommandDispatcher.cs @@ -0,0 +1,150 @@ +using System.Reflection; +using MyParking.Simulation.Core; + +namespace MyParking.Simulation.Commands; + +/// +/// 自动发现带SimulationAction特性的测试方法并分发网页命令。 +/// +public sealed class SimulationCommandDispatcher +{ + private readonly SimulationWorld _world; + private readonly IReadOnlyDictionary _actions; + + public SimulationCommandDispatcher( + SimulationWorld world) + { + _world = world; + _actions = DiscoverActions(); + } + + /// + /// 返回网页动态生成按钮所需的全部命令。 + /// + public IReadOnlyList GetActions() + { + return _actions.Values + .Select(action => action.Descriptor) + .OrderBy(action => action.Group) + .ThenBy(action => action.Order) + .ToArray(); + } + + /// + /// 对指定车辆执行一个已注册的测试方法。 + /// + 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 + DiscoverActions() + { + var actions = new Dictionary( + 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(); + + if (attribute == null) + continue; + + ValidateMethod(method, attribute); + + var handler = + (Func)method.CreateDelegate( + typeof(Func)); + + 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 Handler); +} + +/// +/// 网页测试命令的执行结果。 +/// +public sealed record CommandResult( + bool Success, + string Message); diff --git a/Simulation/Core/SimulationClock.cs b/Simulation/Core/SimulationClock.cs new file mode 100644 index 0000000..cf64a77 --- /dev/null +++ b/Simulation/Core/SimulationClock.cs @@ -0,0 +1,38 @@ +using System.Diagnostics; + +namespace MyParking.Simulation.Core; + +/// +/// 以固定周期推进离线仿真世界。 +/// +public sealed class SimulationClock( + SimulationWorld world, + ILogger 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); + } + } +} diff --git a/Simulation/Core/SimulationVehicle.cs b/Simulation/Core/SimulationVehicle.cs new file mode 100644 index 0000000..af04c3e --- /dev/null +++ b/Simulation/Core/SimulationVehicle.cs @@ -0,0 +1,382 @@ +using MyParking.Shared; +using MyParking.Simulation.Models; + +namespace MyParking.Simulation.Core; + +/// +/// 保存单辆四舵轮停车机器人的离线仿真状态。 +/// +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 _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); + + /// + /// 停车并切换舵轮准备模式。 + /// + 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; + } + + /// + /// 按当前模式发送前进或后退命令。 + /// + 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)); + } + + /// + /// 在当前运动模式下增加逆时针或顺时针转动。 + /// + 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)); + } + + /// + /// 将网页虚拟遥控器的油门和转向组合为连续车体速度命令。 + /// + 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)); + } + + /// + /// 应用统一车体速度命令并分解为四个舵轮速度向量。 + /// + 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; + } + + /// + /// 将车辆目标速度设置为零并保持当前舵轮角度。 + /// + public void Stop() + { + _targetBodyTwist = Twist2D.Zero; + foreach (var wheel in _wheels) + wheel.Stop(); + } + + /// + /// 更新舵轮反馈和车辆世界位姿。 + /// + 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); + } + + /// + /// 恢复车辆初始位置和舵轮状态。 + /// + 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(); + } + + /// + /// 创建供网页读取的不可变状态快照。 + /// + 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)); + } +} diff --git a/Simulation/Core/SimulationWorld.cs b/Simulation/Core/SimulationWorld.cs new file mode 100644 index 0000000..c7197aa --- /dev/null +++ b/Simulation/Core/SimulationWorld.cs @@ -0,0 +1,205 @@ +using MyParking.Shared; +using MyParking.Simulation.Models; + +namespace MyParking.Simulation.Core; + +/// +/// 管理离线仿真车辆、车队中心和成员布局。 +/// +public sealed class SimulationWorld +{ + private readonly object _syncRoot = new(); + private Dictionary _vehicles = new(); + private SimulationConfigurationDto _configuration; + + public SimulationWorld() + { + _configuration = CreateDefaultConfiguration(); + ApplyConfigurationCore(_configuration); + } + + public T WithVehicle( + int vehicleId, + Func 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 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()); + } +} diff --git a/Simulation/Core/VirtualSteerWheel.cs b/Simulation/Core/VirtualSteerWheel.cs new file mode 100644 index 0000000..3852696 --- /dev/null +++ b/Simulation/Core/VirtualSteerWheel.cs @@ -0,0 +1,174 @@ +namespace MyParking.Simulation.Core; + +/// +/// 模拟单个舵轮的转向和驱动响应，不包含真实电机物理模型。 +/// +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; + + /// + /// 设置期望轮胎速度向量，并在机械限位内选择等价舵角。 + /// + 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); + } + + /// + /// 停车时设置舵轮预对齐方向。 + /// + public bool PrepareDirection(double targetAngleDegrees) + { + TargetSpeedMetersPerSecond = 0.0; + return SetDirectionAndSpeed( + targetAngleDegrees, + 0.0); + } + + /// + /// 将驱动目标设置为零，并保持当前舵轮方向。 + /// + public void Stop() + { + TargetSpeedMetersPerSecond = 0.0; + } + + /// + /// 按固定转向速度和驱动加速度更新虚拟反馈。 + /// + public void Step(double deltaTimeSeconds) + { + ActualAngleDegrees = MoveTowards( + ActualAngleDegrees, + TargetAngleDegrees, + MaximumSteeringRateDegreesPerSecond * + deltaTimeSeconds); + + var allowedTargetSpeed = + IsAligned ? TargetSpeedMetersPerSecond : 0.0; + + ActualSpeedMetersPerSecond = MoveTowards( + ActualSpeedMetersPerSecond, + allowedTargetSpeed, + MaximumDriveAccelerationMetersPerSecondSquared * + deltaTimeSeconds); + } + + /// + /// 恢复舵轮初始状态。 + /// + 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; + } +} diff --git a/Simulation/Models/ManualControlInputDto.cs b/Simulation/Models/ManualControlInputDto.cs new file mode 100644 index 0000000..2e527c9 --- /dev/null +++ b/Simulation/Models/ManualControlInputDto.cs @@ -0,0 +1,10 @@ +namespace MyParking.Simulation.Models; + +/// +/// 网页虚拟遥控器输入，所有输入范围均为-1到1。 +/// +public sealed record ManualControlInputDto( + double Throttle, + double Steering, + double SpeedScale, + double SteeringScale); diff --git a/Simulation/Models/SimulationConfigurationDto.cs b/Simulation/Models/SimulationConfigurationDto.cs new file mode 100644 index 0000000..a60e1b4 --- /dev/null +++ b/Simulation/Models/SimulationConfigurationDto.cs @@ -0,0 +1,26 @@ +namespace MyParking.Simulation.Models; + +/// +/// 离线仿真的车辆数量、车队中心和成员相对布局。 +/// +public sealed record SimulationConfigurationDto( + int VehicleCount, + FleetCenterDto FleetCenter, + IReadOnlyList Vehicles); + +/// +/// 车队中心在世界坐标系中的位姿。 +/// +public sealed record FleetCenterDto( + double XMeters, + double YMeters, + double YawRadians); + +/// +/// 单车车体坐标系在车队中心坐标系中的位姿。 +/// +public sealed record VehicleLayoutDto( + int VehicleId, + double XMeters, + double YMeters, + double YawRadians); diff --git a/Simulation/Models/VehicleStateDto.cs b/Simulation/Models/VehicleStateDto.cs new file mode 100644 index 0000000..660afd2 --- /dev/null +++ b/Simulation/Models/VehicleStateDto.cs @@ -0,0 +1,25 @@ +namespace MyParking.Simulation.Models; + +/// +/// 网页绘制单辆停车机器人所需的状态快照。 +/// +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 Wheels); + +/// +/// 网页显示的二维车体速度快照。 +/// +public sealed record TwistStateDto( + double VxMetersPerSecond, + double VyMetersPerSecond, + double OmegaRadiansPerSecond); diff --git a/Simulation/Models/WheelStateDto.cs b/Simulation/Models/WheelStateDto.cs new file mode 100644 index 0000000..fda10cf --- /dev/null +++ b/Simulation/Models/WheelStateDto.cs @@ -0,0 +1,14 @@ +namespace MyParking.Simulation.Models; + +/// +/// 网页绘制单个舵轮所需的状态快照。 +/// +public sealed record WheelStateDto( + string Name, + double XMeters, + double YMeters, + double TargetAngleDegrees, + double ActualAngleDegrees, + double TargetSpeedMetersPerSecond, + double ActualSpeedMetersPerSecond, + bool IsAligned); diff --git a/Simulation/MyParking.Simulation.csproj b/Simulation/MyParking.Simulation.csproj new file mode 100644 index 0000000..221b2fd --- /dev/null +++ b/Simulation/MyParking.Simulation.csproj @@ -0,0 +1,17 @@ + + + + net8.0 + enable + enable + MyParking.Simulation + + + + + + + + diff --git a/Simulation/Program.cs b/Simulation/Program.cs new file mode 100644 index 0000000..c45638f --- /dev/null +++ b/Simulation/Program.cs @@ -0,0 +1,97 @@ +using MyParking.Simulation.Commands; +using MyParking.Simulation.Core; + +var builder = WebApplication.CreateBuilder(args); + +builder.Services.AddSingleton(); +builder.Services.AddSingleton(); +builder.Services.AddHostedService(); + +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(); diff --git a/Simulation/Properties/launchSettings.json b/Simulation/Properties/launchSettings.json new file mode 100644 index 0000000..1ba4702 --- /dev/null +++ b/Simulation/Properties/launchSettings.json @@ -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" + } + } + } +} diff --git a/Simulation/appsettings.Development.json b/Simulation/appsettings.Development.json new file mode 100644 index 0000000..0c208ae --- /dev/null +++ b/Simulation/appsettings.Development.json @@ -0,0 +1,8 @@ +{ + "Logging": { + "LogLevel": { + "Default": "Information", + "Microsoft.AspNetCore": "Warning" + } + } +} diff --git a/Simulation/appsettings.json b/Simulation/appsettings.json new file mode 100644 index 0000000..10f68b8 --- /dev/null +++ b/Simulation/appsettings.json @@ -0,0 +1,9 @@ +{ + "Logging": { + "LogLevel": { + "Default": "Information", + "Microsoft.AspNetCore": "Warning" + } + }, + "AllowedHosts": "*" +} diff --git a/Simulation/wwwroot/index.html b/Simulation/wwwroot/index.html new file mode 100644 index 0000000..6fd3506 --- /dev/null +++ b/Simulation/wwwroot/index.html @@ -0,0 +1,144 @@ + + + + + + 停车机器人离线测试台 + + + +
+
+
+

MY PARKING · OFFLINE LAB

+

停车机器人离线测试台

+
+
+ + 正在连接仿真后端 +
+
+ +
+ + +
+
+ 世界视图 + X 向右 · Y 向上 · 逆时针为正 +
+ +
+ + +
+
+ + + + diff --git a/Simulation/wwwroot/viewer.css b/Simulation/wwwroot/viewer.css new file mode 100644 index 0000000..b36b8ce --- /dev/null +++ b/Simulation/wwwroot/viewer.css @@ -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; + } +} diff --git a/Simulation/wwwroot/viewer.js b/Simulation/wwwroot/viewer.js new file mode 100644 index 0000000..aeee9ac --- /dev/null +++ b/Simulation/wwwroot/viewer.js @@ -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 => ` + `) + .join(""); + + return ` +
+

${group}

+
${buttons}
+
`; + }).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 => + ``) + .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 ` + `; + }).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 = ` +
+
+ 车体 1472 × 948 mm · 实际DLL轮位 X±750 / Y±500 mm +
+ ${vehicle.wheels.map(wheel => { + const actualPercent = + (wheel.actualAngleDegrees + 120) / 240 * 100; + const targetPercent = + (wheel.targetAngleDegrees + 120) / 240 * 100; + return ` +
+
+ ${wheel.name} + 实际 ${signed(wheel.actualAngleDegrees)}° · 目标 ${signed(wheel.targetAngleDegrees)}° +
+
+ + + +
+
+ 轮速 ${wheel.actualSpeedMetersPerSecond.toFixed(2)} m/s +
+
`; + }).join("")} +
`; +} + +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 ` +
+ CAR ${id} + + + +
`; + }).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);