using ClumsyCore; using MDCSToolBox.Clumsy.Pilot.MultiWheel; using Newtonsoft.Json; namespace MultiWheelC; public class PilotConfig : MultiWheelPilotConfig { [FieldMember(desc = "[sync] steering angle acceleration(deg/s^2)")] public float SyncThAccPerSec = 30f; [FieldMember(desc = "[sync] fleet member distance(mm)")] public float TestCarSyncDistance = 2400f; [FieldMember(desc = "[sync] fleet layout bias angle(deg)")] public float TestCarSyncTh = 0f; // Fleet manual remote IO values are normalized joystick ratios. Keep all speed/angle scaling here. [FieldMember(desc = "[sync] fleet manual max linear speed(m/s)")] public float FleetManualMaxSpeed = 0.3f; [FieldMember(desc = "[sync] fleet manual normal-mode full-stick steering angle(deg)")] public float FleetManualMaxSteerAngleDeg = 45f; [FieldMember(desc = "[sync] fleet manual crab-mode full-stick steering angle(deg)")] public float FleetManualMaxCrabAngleDeg = 60f; [FieldMember(desc = "[sync] fleet manual rotate-mode full-stick angular speed(deg/s)")] public float FleetManualMaxRotateOmegaDegPerSec = 45f; [FieldMember(desc = "[sync] 蟹行舵角上限(deg，应与Medulla舵轮角度限制匹配，默认120)")] public float MultiVehicleCrabSteerLimitDeg = 120f; [FieldMember(desc = "[sync] 检测中心偏移(mm)")] public float DeltaDetectCenter = 350f; // 仅控制"车队内姿态纠正"(POS 补偿)是否使用 Detour 的 SLAM 位姿，不影响"整个车队姿态的计算"。 // 默认 false：定位不参与车队内姿态纠正（各车按编队几何/互识别保持队形，不做 SLAM 逐车纠偏）。 // 为 true：额外用 getCartLocation() 反推每台车相对编队中心的偏差并做 POS 补偿。 // 注意：无论该开关如何，自动模式下整队姿态(反推/广播车队中心、SLAM 间距、自动安全门)始终依赖 Detour 全局定位； // 主车自动模式必调用 getCartLocation()，若无有效全局定位该调用会阻塞 → 联动线程阻塞不下发速度（安全停车）。 [FieldMember(desc = "[sync] 定位是否参与车队内姿态纠正(不影响整队姿态计算)")] public bool MultiVehicleSyncUseDetour = false; // 手动外部遥控联动默认只走 2 腿检测/几何同步，避免 Detour getCartLocation 阻塞导致遥控和检测可视化变慢。 [FieldMember(desc = "[sync] 手动联动是否启用定位姿态纠正(默认关闭)")] public bool MultiVehicleManualUseDetourCorrection = false; [FieldMember(desc = "多车联动：总车数")] public int MultiVehicleFleetNum = 2; [FieldMember(desc = "联动线程周期(ms)")] public int MultiVehicleSyncInterval = 50; [FieldMember(desc = "多车联动：主车端点 ip:port，/ 表示本车为主车")] public string MultiVehicleMasterEndpoint = "/"; [FieldMember(desc = "多车联动：本车同步 IP")] public string SimpleIp = "127.0.0.1"; [FieldMember(desc = "多车联动：本车回连端点 ip:port，供主车 notify 回连，空=127.0.0.1:本车port")] public string MultiVehicleSelfEndpoint = ""; [JsonProperty("MultiVehicleMasterIp")] private string LegacyMasterIpSetter { set { if (string.IsNullOrEmpty(value) || value == "/") return; if (MultiVehicleMasterEndpoint == "/") MultiVehicleMasterEndpoint = value.Contains(":") ? value : $"{value}:8008"; } } [FieldMember(desc = "多车联动：启用互识别纠正")] public bool MultiVehicleUseDetect = false; // B: 自动速度命令新鲜度(ms)。主车超过此时长未从路径控制器收到新速度命令(路径结束/早退/卡顿)， // 即视为失效并清零下发速度，避免车队按末速度滑行。0 表示自动取 max(200, interval*4)。 [FieldMember(desc = "多车联动：自动速度命令超时(ms，0=auto)")] public int MultiVehicleAutoCmdTimeoutMs = 0; // C: fleet 成员存活 TTL(ms)。主车剔除超过此时长未 register/刷新的从车；编队就绪要求所有成员新鲜。 // 0 表示自动取 max(500, interval*6)。 [FieldMember(desc = "多车联动：成员存活TTL(ms，0=auto)")] public int MultiVehicleMemberTtlMs = 0; // D: 自动模式下用主车路径控制器的理想车队中心(idealPos/idealAngle)作为各车 layout 目标， // 弧线路径上做 per-car 前馈而非仅共用 frontTh/rearTh 事后纠偏。 [FieldMember(desc = "多车联动：自动模式按理想中心前馈(弧线)")] public bool MultiVehicleAutoUseIdealCenter = true; // H: 自动模式必须有有效车队中心(SLAM 可反推)，全程定位丢失时停车，避免纯 SLAM 下盲跑。 [FieldMember(desc = "多车联动：自动模式要求有效车队中心")] public bool MultiVehicleAutoRequireFleetCenter = true; [FieldMember(desc = "多车联动：SLAM X补偿系数")] public float MultiVehiclePosBiasXFac = 0.5f; [FieldMember(desc = "多车联动：SLAM Y补偿系数")] public float MultiVehiclePosBiasYFac = 0.5f; [FieldMember(desc = "多车联动：SLAM Th补偿系数")] public float MultiVehiclePosBiasThFac = 0.5f; [FieldMember(desc = "多车联动：X补偿阈值(mm)")] public float MultiVehiclePosBiasXThreshold = 50f; [FieldMember(desc = "多车联动：Y补偿阈值(mm)")] public float MultiVehiclePosBiasYThreshold = 50f; [FieldMember(desc = "多车联动：Th补偿阈值(deg)")] public float MultiVehiclePosBiasThThreshold = 5f; [FieldMember(desc = "多车联动：互识别 X补偿系数")] public float MultiVehicleDetectBiasXFac = 0.5f; [FieldMember(desc = "多车联动：互识别 Y补偿系数")] public float MultiVehicleDetectBiasYFac = 0.5f; [FieldMember(desc = "多车联动：互识别 Th补偿系数")] public float MultiVehicleDetectBiasThFac = 0.5f; [FieldMember(desc = "多车联动：互识别 X补偿阈值(mm)")] public float MultiVehicleDetectBiasXThreshold = 50f; [FieldMember(desc = "多车联动：互识别 Y补偿阈值(mm)")] public float MultiVehicleDetectBiasYThreshold = 50f; [FieldMember(desc = "多车联动：互识别 Th补偿阈值(deg)")] public float MultiVehicleDetectBiasThThreshold = 5f; // 原地旋转(mode2)闭环纠偏(PI)：把"本车应移动到的位置(dx,dy,mm)/应转角(dth,deg)"作为误差， // 用 PI 控制器换算成车体系修正速度叠加到绕队心旋转上。纯 P 对抗恒定横向滑移扰动有稳态残差， // 加积分项把稳态误差拉到 0；积分带限幅(抗 windup)，总输出限幅在 Max 内防过冲/振荡。 // Fac=比例增益(mm/s per mm、deg/s per deg)；IFac=积分增益(mm/s per mm·s、deg/s per deg·s)；Max=总输出上限。 [FieldMember(desc = "原地旋转纠偏：平移比例增益P(mm/s per mm)")] public float MultiVehicleRotateCompXyFac = 1.2f; [FieldMember(desc = "原地旋转纠偏：平移积分增益I(mm/s per mm·s)")] public float MultiVehicleRotateCompXyIFac = 0.8f; [FieldMember(desc = "原地旋转纠偏：平移速度上限(mm/s)")] public float MultiVehicleRotateCompXyMax = 150f; [FieldMember(desc = "原地旋转纠偏：转向比例增益P(deg/s per deg)")] public float MultiVehicleRotateCompThFac = 0.8f; [FieldMember(desc = "原地旋转纠偏：转向积分增益I(deg/s per deg·s)")] public float MultiVehicleRotateCompThIFac = 0.8f; [FieldMember(desc = "原地旋转纠偏：转向速度上限(deg/s)")] public float MultiVehicleRotateCompThMax = 15f; // 仅当车队实际被指令旋转(|fleetOmega|超过此阈值)时才运行纠偏 PI；否则清零并复位积分， // 避免松开摇杆后积分残留持续驱动车辆"自行旋转停不下来"。 [FieldMember(desc = "原地旋转纠偏：生效的最小角速度阈值(deg/s)")] public float MultiVehicleRotateActiveOmega = 0.5f; // 安全网：每轮纠偏速度幅值 <= 该比例 * 本轮旋转切向速度，限制合速度相对纯切向的最大偏角。 // 旧配置若仍为 <0，运行时按安全默认 0.10 处理；确需放宽时可在主车显式调大并同步给从车。 [FieldMember(desc = "原地旋转纠偏：纠偏/旋转切向比例硬上限，<0使用安全默认0.10")] public float MultiVehicleRotateCompTangentFrac = 0.10f; [FieldMember(desc = "单车同步 xy 精度(mm)")] public float SingleCarSyncPrecisionXy = 10f; [FieldMember(desc = "单车同步 th 精度(deg)")] public float SingleCarSyncPrecisionTh = 0.2f; [FieldMember(desc = "Playground WebAPI 基地址")] public string PlaygroundWebApiUrl = "http://localhost:18090"; [FieldMember(desc = "MultiVehicle rotate pose WebAPI diagnostics (simulation only)")] public bool MultiVehicleRotatePoseWebApiDiagEnabled = false; [FieldMember(desc = "Playground 小车名称（场景 robots[].name）")] public string PlaygroundRobotName = "agv_multi_1"; [FieldMember(desc = "Playground 邻车名称（仅主车用于原地旋转位姿诊断）")] public string PlaygroundNeighborRobotName = "agv_multi_2"; [FieldMember(desc = "WebAPI 平移测试：平移距离(mm)")] public float WebApiTranslateMm = 100f; [FieldMember(desc = "WebAPI 旋转测试：旋转角度(deg)")] public float WebApiRotateDeg = 5f; [FieldMember(desc = "原地旋转：目标朝向(世界坐标系, deg)")] public float InPlaceRotateTargetWorldDeg = 90f; [FieldMember(desc = "原地旋转：旋转角速度(deg/s)")] public float InPlaceRotateSpeed = 30f; [FieldMember(desc = "原地旋转：到位角度精度(deg)")] public float InPlaceRotateArriveDeg = 1f; [FieldMember(desc = "原地旋转：起转前舵轮对齐精度(deg)")] public float InPlaceRotateWheelAlignDeg = 2f; [FieldMember(desc = "原地旋转：旋转过程中舵轮偏差重对齐阈值(deg)")] public float InPlaceRotateActiveWheelAlignDeg = 10f; // ===== 车队联动-原地旋转动作（FleetRotateInPlace / 对应 FleetRemote 原地旋转模式）===== // 通过 Clumsy 内部脚本字段驱动 TickMultiVehicle 的 mode2 旋转（绕车队中心 + PI 纠偏），需主车运行。 [FieldMember(desc = "车队原地旋转：角速度大小(deg/s，方向由目标角符号决定)")] public float FleetRotateOmega = 15f; [FieldMember(desc = "车队原地旋转：目标相对转角(deg，+逆时针)")] public float FleetRotateTargetDeltaDeg = 90f; [FieldMember(desc = "车队原地旋转：到位角度精度(deg)")] public float FleetRotateArriveDeg = 1.5f; [FieldMember(desc = "车队原地旋转：减速区宽度(deg)，抑制收尾惯性超调")] public float FleetRotateSlowDeg = 25f; [FieldMember(desc = "车队原地旋转：减速区末段最小角速度(deg/s)")] public float FleetRotateMinOmega = 3f; [FieldMember(desc = "车队原地旋转：起步缓启动角加速度(deg/s²，<=0关闭)")] public float FleetRotateAccel = 20f; [FieldMember(desc = "车队原地旋转：到位后安定时长(s)")] public float FleetRotateSettleSec = 0.5f; // 与 MultiVehicleSyncUseDetour 解耦：转到指定角度需航向反馈，默认 true 读主车 SLAM 航向闭环判停。 // false 时退化为按估算时长开环停止（实际转速≠指令时不精确，易出现"没转到目标就停"）。 [FieldMember(desc = "车队原地旋转：用Detour主车航向闭环判停(默认true，false=按时长开环)")] public bool FleetRotateUseDetourHeading = true; // ===== 车队联动-自动蟹行（FleetCrabWalk）===== // 以当前车队中心为起点，构造一条直线路径；MovementTest 中车身保持启动朝向追踪该路径。 // 动作侧参考几何控制器的路径跟踪思路，直接写入 MultiVehicleAuto... 字段，不再复用脚本手动链路。 [FieldMember(desc = "车队蟹行：路径方向相对启动时车队朝向夹角(deg，逆时针为正；路径在车右侧x度时填-x)")] public float FleetCrabAngleDeg = 45f; [FieldMember(desc = "车队蟹行：AGV入口使用的车队世界系目标朝向(deg)")] public float FleetCrabBodyWorldHeadingDeg = 0f; [FieldMember(desc = "车队蟹行：路径长度(mm)")] public float FleetCrabLengthMm = 2000f; [FieldMember(desc = "车队蟹行：行驶速度(m/s)")] public float FleetCrabSpeed = 0.2f; [FieldMember(desc = "车队蟹行：速度命令加速度限制(m/s^2，<=0表示不限制)")] public float FleetCrabAccel = 0.2f; [FieldMember(desc = "车队蟹行：预对齐后正式下发速度前5秒加速度(m/s^2，<=0表示不限制)")] public float FleetCrabStartAccel = 0.01f; [FieldMember(desc = "车队蟹行：末端开始减速距离(mm)")] public float FleetCrabSlowDistance = 2000f; [FieldMember(desc = "车队蟹行：完成距离(mm)，低于该剩余距离结束动作")] public float FleetCrabFinishDistance = 20f; [FieldMember(desc = "车队蟹行：末端最低速度(m/s)")] public float FleetCrabFinishSpeed = 0.02f; [FieldMember(desc = "车队蟹行：末端减速曲线指数")] public float FleetCrabSlowingPow = 0.8f; [FieldMember(desc = "车队蟹行：GCP舵角修正上限(deg)")] public float FleetCrabGcpThetaThreshold = 95f; [FieldMember(desc = "车队蟹行：headingErr角度纠偏比例系数")] public float FleetCrabDthLinearFac = 1f; [FieldMember(desc = "车队蟹行：headingErr角度纠偏舵角限幅(deg)")] public float FleetCrabDthLinearThreshold = 10f; [FieldMember(desc = "FleetCrab startup sync timeout(s)")] public float FleetCrabStartSyncTimeoutSec = 8f; [FieldMember(desc = "FleetCrab startup wheel alignment tolerance(deg)")] public float FleetCrabStartWheelAlignDeg = 2f; // ===== Fleet linked Bezier curve walk ===== [FieldMember(desc = "FleetCurve MovementTest Bezier control point count")] public int FleetCurveTestControlPointCount = 4; [FieldMember(desc = "FleetCurve speed(m/s)")] public float FleetCurveSpeed = 0.2f; [FieldMember(desc = "FleetCurve slow distance(mm)")] public float FleetCurveSlowDistance = 2000f; [FieldMember(desc = "FleetCurve finish distance(mm)")] public float FleetCurveFinishDistance = 20f; [FieldMember(desc = "FleetCurve finish speed(m/s)")] public float FleetCurveFinishSpeed = 0.02f; [FieldMember(desc = "FleetCurve slowing curve exponent")] public float FleetCurveSlowingPow = 0.8f; // ===== 2腿检测（单线雷达识别两腿托盘 / 轮胎）===== [FieldMember(desc = "2腿检测：雷达名（逗号分隔可多个）")] public string TwoLegLidarName = "rear_left_lidar_1,rear_right_lidar_1"; [FieldMember(desc = "2腿检测：初始猜测X(mm, 车体坐标系)")] public float TwoLegGuessX = 2000f; [FieldMember(desc = "2腿检测：两腿间距(mm)")] public float TwoLegWidth = 800f; [FieldMember(desc = "2腿检测：两腿间距允许误差(mm)")] public float TwoLegWidthErr = 100f; [FieldMember(desc = "2腿检测：聚类点间距(mm)")] public float TwoLegBlobDist = 100f; [FieldMember(desc = "2腿检测：聚类尺寸(mm)")] public float TwoLegBlobSize = 200f; [FieldMember(desc = "2腿检测：聚类最小点数")] public int TwoLegBlobPtCount = 5; [FieldMember(desc = "2腿检测：聚类 padding")] public int TwoLegPadding = 5; [FieldMember(desc = "2腿检测：腿柱搜索范围")] public int TwoLegPillarFindingScope = 20; [FieldMember(desc = "2腿检测：方向符号(±1)")] public int TwoLegSgnDir = 1; [FieldMember(desc = "2腿检测：中心X偏移(mm)")] public float TwoLegCenterChangeX = 0f; [FieldMember(desc = "2腿检测：输出X补偿(mm)")] public float TwoLegOutputBiasX = 0f; [FieldMember(desc = "2腿检测：输出Y补偿(mm)")] public float TwoLegOutputBiasY = 0f; [FieldMember(desc = "2腿检测：ROI滤波框长(mm)")] public float TwoLegFilterLength = 1800f; [FieldMember(desc = "2腿检测：ROI滤波框宽(mm)")] public float TwoLegFilterWidth = 600f; #region [FieldMember(desc = "轮胎识别:识别框长")] public float TireFilterLength = 1800f; [FieldMember(desc = "轮胎识别:识别框宽")] public float TireFilterWidth = 600f; [FieldMember(desc = "轮胎识别:轮胎间距")] public float TireTwoLegWidth = 800f; [FieldMember(desc = "轮胎识别:轮胎识别允许误差")] public float TireTwoLegWidthErr = 100f; [FieldMember(desc = "轮胎识别:轮胎聚类最小点云数")] public int TireTwoLegBlobPtCount = 15; [FieldMember(desc = "轮胎识别:前雷达参数")] public float TireFrontTwoLegBlobDist = 100f; [FieldMember(desc = "轮胎识别:前雷达参数")] public float TireFrontTwoLegBlobSize = 200f; [FieldMember(desc = "轮胎识别:前雷达参数")] public int TireFrontPadding = 5; [FieldMember(desc = "轮胎识别:前雷达参数")] public int TireFrontTwoLegPillarFindingScope = 20; [FieldMember(desc = "轮胎识别:前雷达参数")] public int TireFrontTwoLegSgnDir = 1; [FieldMember(desc = "轮胎识别:前雷达参数")] public float TireFrontTwoLegCenterChangeX = 0; [FieldMember(desc = "轮胎识别:后雷达参数")] public float TireBackTwoLegBlobDist = 100f; [FieldMember(desc = "轮胎识别:后雷达参数")] public float TireBackTwoLegBlobSize = 200f; [FieldMember(desc = "轮胎识别:后雷达参数")] public int TireBackPadding = 5; [FieldMember(desc = "轮胎识别:后雷达参数")] public int TireBackTwoLegPillarFindingScope = 20; [FieldMember(desc = "轮胎识别:后雷达参数")] public int TireBackTwoLegSgnDir = 1; [FieldMember(desc = "轮胎识别:后雷达参数")] public float TireBackTwoLegCenterChangeX = 0; [FieldMember(desc = "抱夹控制pid:Kp")] public float ClampControlKp = 0.1f; [FieldMember(desc = "抱夹控制pid:Ki")] public float ClampControlKi = 0f; [FieldMember(desc = "抱夹控制pid:Kd")] public float ClampControlKd = 0f; [FieldMember(desc = "抱夹控制pid:MaxI")] public float ClampControlMaxI = 0f; [FieldMember(desc = "抱夹控制pid:Acc")] public float ClampControlSpeedAcc = 1f; [FieldMember(desc = "抱夹控制pid:Thresh")] public float ClampControlThresh = 0.2f; [FieldMember(desc = "抱夹控制pid:DeadZone")] public float ClampControlDeadZone = 5f; [FieldMember(desc = "抱夹最大速度")] public float MaxClampSpeed = 1.5f; [FieldMember(desc = "直线行走距离")] public float LineTrackDistance = 1000f; [FieldMember(desc = "直线行走最大速度")] public float LineTrackMaxSpeed = 0.3f; [FieldMember(desc = "直线行走Kp")] public float LineTrackKp = 0.2f; [FieldMember(desc = "直线行走Ki")] public float LineTrackKi = 0f; [FieldMember(desc = "直线行走Kd")] public float LineTrackKd = 0f; [FieldMember(desc = "直线行走DeadZone")] public float LineTrackDeadZone = 50f; [FieldMember(desc = "轮胎跟踪:切换至盲走距离")] public float TireFollowingWalkBlindSwitchingDistance = 1200f; [FieldMember(desc = "轮胎跟踪:识别第一对轮胎的初始距离")] public float TireFollowingStage1GuessX = 2000f; [FieldMember(desc = "轮胎跟踪:识别第二对轮胎的初始距离")] public float TireFollowingStage2GuessX = 2475f; [FieldMember(desc = "轮胎跟踪:盲走停止距离")] public float TireFollowingWalkBlindFinishDistance = 10f; [FieldMember(desc = "轮胎跟踪:减速距离")] public float TireFollowingSlowDistance = 200f; [FieldMember(desc = "轮胎跟踪:最大速度")] public float TireFollowingMaxSpeed = 0.2f; [FieldMember(desc = "轮胎跟踪:前雷达识别路径偏移X")] public float TireFollowingFrontLidarPathTransformationX = 253f; [FieldMember(desc = "轮胎跟踪:前雷达识别路径偏移Y")] public float TireFollowingFrontLidarPathTransformationY = 13f; [FieldMember(desc = "轮胎跟踪:前雷达识别路径偏移Th")] public float TireFollowingFrontLidarWalkBlindTh = -1f; [FieldMember(desc = "轮胎跟踪:后雷达识别路径偏移X")] public float TireFollowingBackLidarPathTransformationX = 148f; [FieldMember(desc = "轮胎跟踪:后雷达识别路径偏移Y")] public float TireFollowingBackLidarPathTransformationY = 2f; [FieldMember(desc = "轮胎跟踪:后雷达识别路径偏移Th")] public float TireFollowingBackLidarWalkBlindTh = 0f; [FieldMember(desc = "轮胎跟踪:离车时后雷达识别路径偏移X")] public float TireFollowingLeaveCarBackLidarPathTransformationX = 1500f; [FieldMember(desc = "轮胎跟踪:离车时切换至盲走距离")] public float TireFollowingLeaveCarWalkBlindSwitchingDistance = 1200f; [FieldMember(desc = "轮胎跟踪:测试钻轮胎数量")] public int TireFollowingTireNum = 1; [FieldMember(desc = "轮胎跟踪:过近距离")] public float TireFollowingCloseDistance = 1400; [FieldMember(desc = "轮胎跟踪:距离过近角度忽略阈值")] public float TireFollowingAngleIgnoreThr = 0.2f; [FieldMember(desc = "轮胎跟踪:Y最大平均数")] public int TireFollowingYAverageFrameCount = 5; [FieldMember(desc = "终点跟踪:速度")] public float DstTrackerMaxSpeed = 0.3f; [FieldMember(desc = "轮胎跟踪:释放锁点距离")] public float TireFollowingReleaseDistance = 1600; #endregion [FieldMember(desc = "轮胎跟踪:角度调整kp")] public float TireFollowingThkp = 0.05f; [FieldMember(desc = "轮胎跟踪:角度调整ki")] public float TireFollowingThki = 0.01f; [FieldMember(desc = "轮胎跟踪:角度调整kd")] public float TireFollowingThkd = 0f; [FieldMember(desc = "轮胎跟踪:角度调整SpeedAcc")] public float TireFollowingThSpeedAccPerSec = 1f; [FieldMember(desc = "轮胎跟踪:角度调整Thresh")] public float TireFollowingThThresh = 0.1f; [FieldMember(desc = "轮胎跟踪:角度调整DeadZone")] public float TireFollowingThDeadZone = 5f; [FieldMember(desc = "轮胎跟踪:角度调整MaxI")] public float TireFollowingThMaxI = 0.01f; }