337 lines
18 KiB
C#
337 lines
18 KiB
C#
using ClumsyCore;
|
||
using MDCSToolBox.Clumsy.Pilot.MultiWheel;
|
||
using Newtonsoft.Json;
|
||
|
||
namespace MultiWheelC;
|
||
|
||
public class PilotConfig : MultiWheelPilotConfig
|
||
{
|
||
#region 单车-轨迹跟踪 LineTracking
|
||
|
||
[FieldMember(desc = "直线行走距离")] public float LineTrackDistance = 1000f;
|
||
[FieldMember(desc = "直线行走最大速度")] public float LineTrackMaxSpeed = 0.3f;
|
||
[FieldMember(desc = "直线行走Kp")] public float LineTrackKp = 0.2f;
|
||
[FieldMember(desc = "直线行走Ki")] public float LineTrackKi = 0f;
|
||
[FieldMember(desc = "直线行走Kd")] public float LineTrackKd = 0f;
|
||
[FieldMember(desc = "直线行走DeadZone")] public float LineTrackDeadZone = 50f;
|
||
|
||
[FieldMember(desc = "终点跟踪:速度")] public float DstTrackerMaxSpeed = 0.3f;
|
||
#endregion
|
||
|
||
#region 单车-原地旋转 暂时没用上
|
||
[FieldMember(desc = "原地旋转：目标朝向(世界坐标系, deg)")]
|
||
public float InPlaceRotateTargetWorldDeg = 90f;
|
||
|
||
[FieldMember(desc = "原地旋转：旋转角速度(deg/s)")]
|
||
public float InPlaceRotateSpeed = 30f;
|
||
|
||
[FieldMember(desc = "原地旋转：到位角度精度(deg)")]
|
||
public float InPlaceRotateArriveDeg = 1.5f;
|
||
|
||
[FieldMember(desc = "原地旋转：起转前舵轮对齐精度(deg)")]
|
||
public float InPlaceRotateWheelAlignDeg = 2f;
|
||
|
||
[FieldMember(desc = "原地旋转：旋转过程中舵轮偏差重对齐阈值(deg)")]
|
||
public float InPlaceRotateActiveWheelAlignDeg = 10f;
|
||
|
||
#endregion
|
||
|
||
#region 单车-临时
|
||
[FieldMember(desc = "原地旋转Kp")]
|
||
public float InPlaceRotateKp = 1.1f;
|
||
|
||
[FieldMember(desc = "原地旋转Ki")]
|
||
public float InPlaceRotateKi = 0f;
|
||
|
||
[FieldMember(desc = "原地旋转Kd")]
|
||
public float InPlaceRotateKd = 0f;
|
||
|
||
[FieldMember(desc = "原地旋转积分限幅")]
|
||
public float InPlaceRotateMaxI = 0f;
|
||
|
||
[FieldMember(desc = "原地旋转最小有效角速度(deg/s)")]
|
||
public float InPlaceRotateMinimumSpeed = 1f;
|
||
|
||
[FieldMember(desc = "原地旋转最大角速度(deg/s)")]
|
||
public float InPlaceRotateMaxSpeed = 47.5f;
|
||
|
||
[FieldMember(desc = "原地旋转角加速度(deg/s²)")]
|
||
public float InPlaceRotateAcc = 60f;
|
||
|
||
[FieldMember(desc = "原地旋转超时(s)")]
|
||
public float InPlaceRotateTimeoutSec = 15f;
|
||
#endregion
|
||
|
||
|
||
|
||
|
||
#region 单车-钻车与夹抱
|
||
[FieldMember(desc = "2腿检测：雷达名（逗号分隔可多个）")]
|
||
public string TwoLegLidarName = "rear_left_lidar_1,rear_right_lidar_1";
|
||
|
||
[FieldMember(desc = "2腿检测：初始猜测X(mm, 车体坐标系)")]
|
||
public float TwoLegGuessX = 2000f;
|
||
|
||
[FieldMember(desc = "2腿检测：两腿间距(mm)")]
|
||
public float TwoLegWidth = 800f;
|
||
|
||
[FieldMember(desc = "2腿检测：两腿间距允许误差(mm)")]
|
||
public float TwoLegWidthErr = 100f;
|
||
|
||
[FieldMember(desc = "2腿检测：聚类点间距(mm)")]
|
||
public float TwoLegBlobDist = 100f;
|
||
|
||
[FieldMember(desc = "2腿检测：聚类尺寸(mm)")]
|
||
public float TwoLegBlobSize = 200f;
|
||
|
||
[FieldMember(desc = "2腿检测：聚类最小点数")]
|
||
public int TwoLegBlobPtCount = 5;
|
||
|
||
[FieldMember(desc = "2腿检测：聚类 padding")]
|
||
public int TwoLegPadding = 5;
|
||
|
||
[FieldMember(desc = "2腿检测：腿柱搜索范围")]
|
||
public int TwoLegPillarFindingScope = 20;
|
||
|
||
[FieldMember(desc = "2腿检测：方向符号(±1)")]
|
||
public int TwoLegSgnDir = 1;
|
||
|
||
[FieldMember(desc = "2腿检测：中心X偏移(mm)")]
|
||
public float TwoLegCenterChangeX = 0f;
|
||
|
||
[FieldMember(desc = "2腿检测：输出X补偿(mm)")]
|
||
public float TwoLegOutputBiasX = 0f;
|
||
|
||
[FieldMember(desc = "2腿检测：输出Y补偿(mm)")]
|
||
public float TwoLegOutputBiasY = 0f;
|
||
|
||
[FieldMember(desc = "2腿检测：ROI滤波框长(mm)")]
|
||
public float TwoLegFilterLength = 1800f;
|
||
|
||
[FieldMember(desc = "2腿检测：ROI滤波框宽(mm)")]
|
||
public float TwoLegFilterWidth = 600f;
|
||
|
||
[FieldMember(desc = "轮胎识别:识别框长")] public float TireFilterLength = 1800f;
|
||
[FieldMember(desc = "轮胎识别:识别框宽")] public float TireFilterWidth = 600f;
|
||
[FieldMember(desc = "轮胎识别:轮胎间距")] public float TireTwoLegWidth = 800f;
|
||
[FieldMember(desc = "轮胎识别:轮胎识别允许误差")] public float TireTwoLegWidthErr = 100f;
|
||
[FieldMember(desc = "轮胎识别:轮胎聚类最小点云数")] public int TireTwoLegBlobPtCount = 15;
|
||
|
||
[FieldMember(desc = "轮胎识别:前雷达参数")] public float TireFrontTwoLegBlobDist = 100f;
|
||
[FieldMember(desc = "轮胎识别:前雷达参数")] public float TireFrontTwoLegBlobSize = 200f;
|
||
[FieldMember(desc = "轮胎识别:前雷达参数")] public int TireFrontPadding = 5;
|
||
[FieldMember(desc = "轮胎识别:前雷达参数")] public int TireFrontTwoLegPillarFindingScope = 20;
|
||
[FieldMember(desc = "轮胎识别:前雷达参数")] public int TireFrontTwoLegSgnDir = 1;
|
||
[FieldMember(desc = "轮胎识别:前雷达参数")] public float TireFrontTwoLegCenterChangeX = 0;
|
||
[FieldMember(desc = "轮胎识别:后雷达参数")] public float TireBackTwoLegBlobDist = 100f;
|
||
[FieldMember(desc = "轮胎识别:后雷达参数")] public float TireBackTwoLegBlobSize = 200f;
|
||
[FieldMember(desc = "轮胎识别:后雷达参数")] public int TireBackPadding = 5;
|
||
[FieldMember(desc = "轮胎识别:后雷达参数")] public int TireBackTwoLegPillarFindingScope = 20;
|
||
[FieldMember(desc = "轮胎识别:后雷达参数")] public int TireBackTwoLegSgnDir = 1;
|
||
[FieldMember(desc = "轮胎识别:后雷达参数")] public float TireBackTwoLegCenterChangeX = 0;
|
||
|
||
[FieldMember(desc = "轮胎跟踪:切换至盲走距离")] public float TireFollowingWalkBlindSwitchingDistance = 1200f;
|
||
[FieldMember(desc = "轮胎跟踪:识别第一对轮胎的初始距离")] public float TireFollowingStage1GuessX = 2000f;
|
||
[FieldMember(desc = "轮胎跟踪:识别第二对轮胎的初始距离")] public float TireFollowingStage2GuessX = 2475f;
|
||
[FieldMember(desc = "轮胎跟踪:盲走停止距离")] public float TireFollowingWalkBlindFinishDistance = 10f;
|
||
[FieldMember(desc = "轮胎跟踪:减速距离")] public float TireFollowingSlowDistance = 200f;
|
||
[FieldMember(desc = "轮胎跟踪:最大速度")] public float TireFollowingMaxSpeed = 0.2f;
|
||
[FieldMember(desc = "轮胎跟踪:前雷达识别路径偏移X")] public float TireFollowingFrontLidarPathTransformationX = 253f;
|
||
[FieldMember(desc = "轮胎跟踪:前雷达识别路径偏移Y")] public float TireFollowingFrontLidarPathTransformationY = 13f;
|
||
[FieldMember(desc = "轮胎跟踪:前雷达识别路径偏移Th")] public float TireFollowingFrontLidarWalkBlindTh = -1f;
|
||
[FieldMember(desc = "轮胎跟踪:后雷达识别路径偏移X")] public float TireFollowingBackLidarPathTransformationX = 148f;
|
||
[FieldMember(desc = "轮胎跟踪:后雷达识别路径偏移Y")] public float TireFollowingBackLidarPathTransformationY = 2f;
|
||
[FieldMember(desc = "轮胎跟踪:后雷达识别路径偏移Th")] public float TireFollowingBackLidarWalkBlindTh = 0f;
|
||
[FieldMember(desc = "轮胎跟踪:离车时后雷达识别路径偏移X")] public float TireFollowingLeaveCarBackLidarPathTransformationX = 1500f;
|
||
[FieldMember(desc = "轮胎跟踪:离车时切换至盲走距离")] public float TireFollowingLeaveCarWalkBlindSwitchingDistance = 1200f;
|
||
|
||
[FieldMember(desc = "轮胎跟踪:测试钻轮胎数量")] public int TireFollowingTireNum = 1;
|
||
[FieldMember(desc = "轮胎跟踪:过近距离")] public float TireFollowingCloseDistance = 1400;
|
||
[FieldMember(desc = "轮胎跟踪:距离过近角度忽略阈值")] public float TireFollowingAngleIgnoreThr = 0.2f;
|
||
[FieldMember(desc = "轮胎跟踪:Y最大平均数")] public int TireFollowingYAverageFrameCount = 5;
|
||
|
||
[FieldMember(desc = "轮胎跟踪:释放锁点距离")] public float TireFollowingReleaseDistance = 1600;
|
||
|
||
[FieldMember(desc = "轮胎跟踪:角度调整kp")] public float TireFollowingThkp = 0.05f;
|
||
[FieldMember(desc = "轮胎跟踪:角度调整ki")] public float TireFollowingThki = 0.01f;
|
||
[FieldMember(desc = "轮胎跟踪:角度调整kd")] public float TireFollowingThkd = 0f;
|
||
[FieldMember(desc = "轮胎跟踪:角度调整SpeedAcc")] public float TireFollowingThSpeedAccPerSec = 1f;
|
||
[FieldMember(desc = "轮胎跟踪:角度调整Thresh")] public float TireFollowingThThresh = 0.1f;
|
||
[FieldMember(desc = "轮胎跟踪:角度调整DeadZone")] public float TireFollowingThDeadZone = 5f;
|
||
[FieldMember(desc = "轮胎跟踪:角度调整MaxI")] public float TireFollowingThMaxI = 0.01f;
|
||
|
||
[FieldMember(desc = "抱夹控制pid:Kp")] public float ClampControlKp = 0.1f;
|
||
[FieldMember(desc = "抱夹控制pid:Ki")] public float ClampControlKi = 0f;
|
||
[FieldMember(desc = "抱夹控制pid:Kd")] public float ClampControlKd = 0f;
|
||
[FieldMember(desc = "抱夹控制pid:MaxI")] public float ClampControlMaxI = 0f;
|
||
[FieldMember(desc = "抱夹控制pid:Acc")] public float ClampControlSpeedAcc = 1f;
|
||
[FieldMember(desc = "抱夹控制pid:Thresh")] public float ClampControlThresh = 0.2f;
|
||
[FieldMember(desc = "抱夹控制pid:DeadZone")] public float ClampControlDeadZone = 5f;
|
||
[FieldMember(desc = "抱夹最大速度")] public float MaxClampSpeed = 1.5f;
|
||
#endregion
|
||
#if false
|
||
#region 多车-编队与遥控
|
||
[FieldMember(desc = "联动时转向角爬升加速度")] public float SyncThAccPerSec = 30f;
|
||
[FieldMember(desc = "两车间距 (mm)")] public float TestCarSyncDistance = 2400f;
|
||
[FieldMember(desc = "编队排布偏角")] public float TestCarSyncTh = 0f;
|
||
// Fleet manual remote IO values are normalized joystick ratios. Keep all speed/angle scaling here.
|
||
[FieldMember(desc = "车队遥控最大线速度")] public float FleetManualMaxSpeed = 0.3f;
|
||
[FieldMember(desc = "常规模式满杆舵角")] public float FleetManualMaxSteerAngleDeg = 45f;
|
||
[FieldMember(desc = "蟹行满杆舵角")] public float FleetManualMaxCrabAngleDeg = 60f;
|
||
[FieldMember(desc = "旋转满杆角速度")] public float FleetManualMaxRotateOmegaDegPerSec = 45f;
|
||
[FieldMember(desc = "蟹行舵角上限（对齐 ±120）")] public float MultiVehicleCrabSteerLimitDeg = 120f;
|
||
[FieldMember(desc = "互识别检测中心偏移")] public float DeltaDetectCenter = 350f;
|
||
#endregion
|
||
|
||
#region 多车-通信
|
||
[FieldMember(desc = "多车联动：总车数")] public int MultiVehicleFleetNum = 2;
|
||
[FieldMember(desc = "联动线程周期(ms)")] public int MultiVehicleSyncInterval = 50;
|
||
[FieldMember(desc = "多车联动：主车端点 ip:port，/ 表示本车为主车")] public string MultiVehicleMasterEndpoint = "/";
|
||
[FieldMember(desc = "多车联动：本车同步 IP")] public string SimpleIp = "127.0.0.1";
|
||
|
||
[FieldMember(desc = "多车联动：本车回连端点 ip:port，供主车 notify 回连，空=127.0.0.1:本车port")] public string MultiVehicleSelfEndpoint = "";
|
||
[FieldMember(desc = "多车联动：自动速度命令超时(ms，0=auto)")] public int MultiVehicleAutoCmdTimeoutMs = 0;
|
||
[FieldMember(desc = "多车联动：成员存活TTL(ms，0=auto)")] public int MultiVehicleMemberTtlMs = 0;
|
||
|
||
[JsonProperty("MultiVehicleMasterIp")]
|
||
private string LegacyMasterIpSetter
|
||
{
|
||
set
|
||
{
|
||
if (string.IsNullOrEmpty(value) || value == "/") return;
|
||
if (MultiVehicleMasterEndpoint == "/")
|
||
MultiVehicleMasterEndpoint = value.Contains(":") ? value : $"{value}:8008";
|
||
}
|
||
}
|
||
|
||
#endregion
|
||
|
||
#region 多车-队形补偿
|
||
[FieldMember(desc = "定位是否参与车队内姿态纠正(不影响整队姿态计算)")] public bool MultiVehicleSyncUseDetour = false;
|
||
[FieldMember(desc = "手动联动是否启用定位姿态纠正(默认关闭)")] public bool MultiVehicleManualUseDetourCorrection = false;
|
||
[FieldMember(desc = "多车联动：启用互识别纠正")] public bool MultiVehicleUseDetect = false;
|
||
[FieldMember(desc = "多车联动：自动模式按理想中心前馈(弧线)")] public bool MultiVehicleAutoUseIdealCenter = true;
|
||
[FieldMember(desc = "多车联动：自动模式要求有效车队中心")] public bool MultiVehicleAutoRequireFleetCenter = true;
|
||
[FieldMember(desc = "多车联动：SLAM X补偿系数")] public float MultiVehiclePosBiasXFac = 0.5f;
|
||
[FieldMember(desc = "多车联动：SLAM Y补偿系数")] public float MultiVehiclePosBiasYFac = 0.5f;
|
||
[FieldMember(desc = "多车联动：SLAM Th补偿系数")] public float MultiVehiclePosBiasThFac = 0.5f;
|
||
[FieldMember(desc = "多车联动：X补偿阈值(mm)")] public float MultiVehiclePosBiasXThreshold = 50f;
|
||
[FieldMember(desc = "多车联动：Y补偿阈值(mm)")] public float MultiVehiclePosBiasYThreshold = 50f;
|
||
[FieldMember(desc = "多车联动：Th补偿阈值(deg)")] public float MultiVehiclePosBiasThThreshold = 5f;
|
||
[FieldMember(desc = "多车联动：互识别 X补偿系数")] public float MultiVehicleDetectBiasXFac = 0.5f;
|
||
[FieldMember(desc = "多车联动：互识别 Y补偿系数")] public float MultiVehicleDetectBiasYFac = 0.5f;
|
||
[FieldMember(desc = "多车联动：互识别 Th补偿系数")] public float MultiVehicleDetectBiasThFac = 0.5f;
|
||
[FieldMember(desc = "多车联动：互识别 X补偿阈值(mm)")] public float MultiVehicleDetectBiasXThreshold = 50f;
|
||
[FieldMember(desc = "多车联动：互识别 Y补偿阈值(mm)")] public float MultiVehicleDetectBiasYThreshold = 50f;
|
||
[FieldMember(desc = "多车联动：互识别 Th补偿阈值(deg)")] public float MultiVehicleDetectBiasThThreshold = 5f;
|
||
#endregion
|
||
|
||
#region 多车-旋转补偿
|
||
[FieldMember(desc = "原地旋转纠偏：平移比例增益P(mm/s per mm)")] public float MultiVehicleRotateCompXyFac = 1.2f;
|
||
[FieldMember(desc = "原地旋转纠偏：平移积分增益I(mm/s per mm·s)")] public float MultiVehicleRotateCompXyIFac = 0.8f;
|
||
[FieldMember(desc = "原地旋转纠偏：平移速度上限(mm/s)")] public float MultiVehicleRotateCompXyMax = 150f;
|
||
[FieldMember(desc = "原地旋转纠偏：转向比例增益P(deg/s per deg)")] public float MultiVehicleRotateCompThFac = 0.8f;
|
||
[FieldMember(desc = "原地旋转纠偏：转向积分增益I(deg/s per deg·s)")] public float MultiVehicleRotateCompThIFac = 0.8f;
|
||
[FieldMember(desc = "原地旋转纠偏：转向速度上限(deg/s)")] public float MultiVehicleRotateCompThMax = 15f;
|
||
[FieldMember(desc = "原地旋转纠偏：生效的最小角速度阈值(deg/s)")] public float MultiVehicleRotateActiveOmega = 0.5f;
|
||
[FieldMember(desc = "原地旋转纠偏：纠偏/旋转切向比例硬上限，<0使用安全默认0.10")] public float MultiVehicleRotateCompTangentFrac = 0.10f;
|
||
|
||
[FieldMember(desc = "单车同步 xy 精度(mm)")] public float SingleCarSyncPrecisionXy = 10f;
|
||
[FieldMember(desc = "单车同步 th 精度(deg)")] public float SingleCarSyncPrecisionTh = 0.2f;
|
||
#endregion
|
||
|
||
#region 多车-联动动作
|
||
[FieldMember(desc = "车队原地旋转：角速度大小(deg/s，方向由目标角符号决定)")]
|
||
public float FleetRotateOmega = 15f;
|
||
|
||
[FieldMember(desc = "车队原地旋转：目标相对转角(deg，+逆时针)")]
|
||
public float FleetRotateTargetDeltaDeg = 90f;
|
||
|
||
[FieldMember(desc = "车队原地旋转：到位角度精度(deg)")]
|
||
public float FleetRotateArriveDeg = 1.5f;
|
||
|
||
[FieldMember(desc = "车队原地旋转：减速区宽度(deg)，抑制收尾惯性超调")]
|
||
public float FleetRotateSlowDeg = 25f;
|
||
|
||
[FieldMember(desc = "车队原地旋转：减速区末段最小角速度(deg/s)")]
|
||
public float FleetRotateMinOmega = 3f;
|
||
|
||
[FieldMember(desc = "车队原地旋转：起步缓启动角加速度(deg/s²，<=0关闭)")]
|
||
public float FleetRotateAccel = 20f;
|
||
|
||
[FieldMember(desc = "车队原地旋转：到位后安定时长(s)")]
|
||
public float FleetRotateSettleSec = 0.5f;
|
||
|
||
[FieldMember(desc = "车队原地旋转：用Detour主车航向闭环判停(默认true，false=按时长开环)")]
|
||
public bool FleetRotateUseDetourHeading = true;
|
||
|
||
[FieldMember(desc = "车队蟹行：路径方向相对启动时车队朝向夹角(deg，逆时针为正；路径在车右侧x度时填-x)")]
|
||
public float FleetCrabAngleDeg = 45f;
|
||
|
||
[FieldMember(desc = "车队蟹行：AGV入口使用的车队世界系目标朝向(deg)")]
|
||
public float FleetCrabBodyWorldHeadingDeg = 0f;
|
||
|
||
[FieldMember(desc = "车队蟹行：路径长度(mm)")]
|
||
public float FleetCrabLengthMm = 2000f;
|
||
|
||
[FieldMember(desc = "车队蟹行：行驶速度(m/s)")]
|
||
public float FleetCrabSpeed = 0.2f;
|
||
|
||
[FieldMember(desc = "车队蟹行：速度命令加速度限制(m/s^2，<=0表示不限制)")]
|
||
public float FleetCrabAccel = 0.2f;
|
||
|
||
[FieldMember(desc = "车队蟹行：预对齐后正式下发速度前5秒加速度(m/s^2，<=0表示不限制)")]
|
||
public float FleetCrabStartAccel = 0.01f;
|
||
|
||
[FieldMember(desc = "车队蟹行：末端开始减速距离(mm)")]
|
||
public float FleetCrabSlowDistance = 2000f;
|
||
|
||
[FieldMember(desc = "车队蟹行：完成距离(mm)，低于该剩余距离结束动作")]
|
||
public float FleetCrabFinishDistance = 20f;
|
||
|
||
[FieldMember(desc = "车队蟹行：末端最低速度(m/s)")]
|
||
public float FleetCrabFinishSpeed = 0.02f;
|
||
|
||
[FieldMember(desc = "车队蟹行：末端减速曲线指数")]
|
||
public float FleetCrabSlowingPow = 0.8f;
|
||
|
||
[FieldMember(desc = "车队蟹行：GCP舵角修正上限(deg)")]
|
||
public float FleetCrabGcpThetaThreshold = 95f;
|
||
|
||
[FieldMember(desc = "车队蟹行：headingErr角度纠偏比例系数")]
|
||
public float FleetCrabDthLinearFac = 1f;
|
||
|
||
[FieldMember(desc = "车队蟹行：headingErr角度纠偏舵角限幅(deg)")]
|
||
public float FleetCrabDthLinearThreshold = 10f;
|
||
|
||
[FieldMember(desc = "FleetCrab startup sync timeout(s)")]
|
||
public float FleetCrabStartSyncTimeoutSec = 8f;
|
||
|
||
[FieldMember(desc = "FleetCrab startup wheel alignment tolerance(deg)")]
|
||
public float FleetCrabStartWheelAlignDeg = 2f;
|
||
|
||
[FieldMember(desc = "FleetCurve MovementTest Bezier control point count")]
|
||
public int FleetCurveTestControlPointCount = 4;
|
||
|
||
[FieldMember(desc = "FleetCurve speed(m/s)")]
|
||
public float FleetCurveSpeed = 0.2f;
|
||
|
||
[FieldMember(desc = "FleetCurve slow distance(mm)")]
|
||
public float FleetCurveSlowDistance = 2000f;
|
||
|
||
[FieldMember(desc = "FleetCurve finish distance(mm)")]
|
||
public float FleetCurveFinishDistance = 20f;
|
||
|
||
[FieldMember(desc = "FleetCurve finish speed(m/s)")]
|
||
public float FleetCurveFinishSpeed = 0.02f;
|
||
|
||
[FieldMember(desc = "FleetCurve slowing curve exponent")]
|
||
public float FleetCurveSlowingPow = 0.8f;
|
||
#endregion
|
||
|
||
#endif
|
||
|
||
|
||
|
||
}
|