Update fleet manual remote scaling

This commit is contained in:
2026-07-15 14:01:46 +08:00
parent 68dd6fc061
commit 2ade94f196
8 changed files with 83 additions and 160 deletions
+8 -9
View File
@@ -6,15 +6,14 @@ namespace MultiWheelC;
public class PilotConfig : MultiWheelPilotConfig
{
// ===== 多车联动(已从 MDCSToolBox 内联回 Tutorial=====
[FieldMember(desc = "[sync] (deg/s^2)")] public float SyncThAccPerSec = 30f;
[FieldMember(desc = "[sync] (mm)")] public float TestCarSyncDistance = 2400f;
[FieldMember(desc = "[sync] (deg)")] public float TestCarSyncTh = 0f;
// 手动遥控 Vx 已是 m/s、Vth 已是转向角(deg),此处系数保持 1(直通),不要再次缩放。
[FieldMember(desc = "[sync] Vx系数")] public float ManualCarSyncVxFac = 1f;
// Manual crab mode: VxFac scales linear speed, VyFac maps steer stick ratio to crab steer angle in degrees.
[FieldMember(desc = "[sync] Vy系数(deg)")] public float ManualCarSyncVyFac = 60f;
[FieldMember(desc = "[sync] Vth系数")] public float ManualCarSyncVthFac = 1f;
[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] (degMedulla舵轮角度限制匹配120)")] public float MultiVehicleCrabSteerLimitDeg = 120f;
[FieldMember(desc = "[sync] (mm)")] public float DeltaDetectCenter = 350f;
// 仅控制"车队内姿态纠正"(POS 补偿)是否使用 Detour 的 SLAM 位姿,不影响"整个车队姿态的计算"。
+46 -21
View File
@@ -37,14 +37,17 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
[AsLowerIO(desc = "(手动)多车联动模式")] public int MultiVehicleManualMode;
[FieldMember(desc = "多车联动已同步")] public bool MultiVehicleAligned;
[AsLowerIO(desc = "多车联动:遥控器Vx")] public float MultiVehicleManualVx;
[AsLowerIO(desc = "多车联动:遥控器Vy")] public float MultiVehicleManualVy;
[AsLowerIO(desc = "多车联动:遥控器Vth")] public float MultiVehicleManualVth;
[AsLowerIO(desc = "多车联动:遥控器Vx比例")] public float MultiVehicleManualVx;
[AsLowerIO(desc = "多车联动:遥控器Vy比例")] public float MultiVehicleManualVy;
[AsLowerIO(desc = "多车联动:遥控器Vth比例")] public float MultiVehicleManualVth;
[AsLowerIO(desc = "多车联动:遥控暂停")] public bool MultiVehicleHold;
// ===== Clumsy 侧脚本/动作驱动的手动等价输入(不走 Medulla IO,不会被 IO 同步覆盖)=====
// 手动 IO 字段是 [AsLowerIO]Medulla→ClumsyMedulla 每周期回写),Clumsy 侧 MovementTest 写它们会被覆盖。
// 因此提供这组内部字段,让 Clumsy 侧动作(如 FleetRotateInPlace)能像 FleetRemote 一样驱动车队联动
// ScriptEnabled=使能;Mode 0=常规 1=蟹行 2=原地旋转;Vx/Vy/Vth 语义与手动遥控完全一致(m/s、m/s、deg/s
// 因此提供这组内部字段,让 Clumsy 侧动作(如 FleetRotateInPlace)能像 FleetRemote 一样驱动车队联动
// ScriptEnabled=使能;Mode 0=常规 1=蟹行 2=原地旋转。
// 注意:Medulla 遥控 IO 是归一化摇杆比例;脚本字段保持物理量,避免动作参数再被 FleetManual* 二次缩放。
// Mode0: Vx=m/s, Vth=目标舵角degMode1: Vx=m/s, Vy=蟹行舵角degMode2: Vth=角速度deg/s。
[FieldMember(desc = "多车联动:脚本驱动使能")] public bool MultiVehicleScriptEnabled;
[FieldMember(desc = "多车联动:脚本驱动模式")] public int MultiVehicleScriptMode;
[FieldMember(desc = "多车联动:脚本驱动Vx")] public float MultiVehicleScriptVx;
@@ -214,7 +217,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
DLog.Log(
$"INPUT master={isMaster} endpoint={Conf.MultiVehicleMasterEndpoint} selfEndpoint={Conf.MultiVehicleSelfEndpoint} car={CarNum} " +
$"rawEn={MultiVehicleManualEnabled} rawMode={MultiVehicleManualMode} rawVx={MultiVehicleManualVx:0.000} rawVy={MultiVehicleManualVy:0.000} rawVth={MultiVehicleManualVth:0.000} " +
$"rawEn={MultiVehicleManualEnabled} rawHold={MultiVehicleHold} rawMode={MultiVehicleManualMode} rawVx={MultiVehicleManualVx:0.000} rawVy={MultiVehicleManualVy:0.000} rawVth={MultiVehicleManualVth:0.000} " +
$"scriptOn={scriptOn} effEn={manualEnabled} effMode={manualMode} effVx={manualVx:0.000} effVy={manualVy:0.000} effVth={manualVth:0.000} " +
$"auto={autoEnabled} notifFresh={notifFresh} notifAgeMs={notifAgeMs:0} fleetCnt={fleetCnt}/{Conf.MultiVehicleFleetNum}",
"MultiVehicleRemoteDbg");
@@ -624,6 +627,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
var manualVx = scriptOn ? MultiVehicleScriptVx : MultiVehicleManualVx;
var manualVy = scriptOn ? MultiVehicleScriptVy : MultiVehicleManualVy;
var manualVth = scriptOn ? MultiVehicleScriptVth : MultiVehicleManualVth;
var manualHold = !scriptOn && MultiVehicleHold;
VehicleSyncNotification activeNotification = null;
if (isMaster)
@@ -660,7 +664,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
Math.Max(300, Conf.MultiVehicleSyncInterval * 5);
FleetDiag(
$"ENTRY master={isMaster} | IO: ManualEn={MultiVehicleManualEnabled} Mode={MultiVehicleManualMode} " +
$"Vx={MultiVehicleManualVx:0.000} Vy={MultiVehicleManualVy:0.000} Vth={MultiVehicleManualVth:0.0} AutoEn(IO)={MultiVehicleAutoEnabled} " +
$"Hold={MultiVehicleHold} Vx={MultiVehicleManualVx:0.000} Vy={MultiVehicleManualVy:0.000} Vth={MultiVehicleManualVth:0.0} AutoEn(IO)={MultiVehicleAutoEnabled} " +
$"| SCRIPT en={MultiVehicleScriptEnabled} mode={MultiVehicleScriptMode} Vx={MultiVehicleScriptVx:0.000} Vy={MultiVehicleScriptVy:0.000} Vth={MultiVehicleScriptVth:0.0} " +
$"| gate: manualEnabled={manualEnabled} autoEnabled={autoEnabled} notifFresh={notifFresh} " +
$"notifManualEn={(MultiVehicleNotification != null ? MultiVehicleNotification.ManualEnabled.ToString() : "null")} " +
@@ -681,7 +685,7 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
ResetMultiVehicleRotateCenterDrift();
LogMultiVehicleRemoteDecision(
$"RETURN_IDLE master={isMaster} rawEn={MultiVehicleManualEnabled} scriptOn={scriptOn} auto={autoEnabled} " +
$"rawMode={MultiVehicleManualMode} rawVx={MultiVehicleManualVx:0.000} rawVy={MultiVehicleManualVy:0.000} rawVth={MultiVehicleManualVth:0.000}");
$"rawHold={MultiVehicleHold} rawMode={MultiVehicleManualMode} rawVx={MultiVehicleManualVx:0.000} rawVy={MultiVehicleManualVy:0.000} rawVth={MultiVehicleManualVth:0.000}");
UI.GetPainter("MultiVehicleFleet-vis", false).Clear();
sendMotionPainter.Clear();
lock (FleetLock)
@@ -762,10 +766,13 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
{
syncTh = 0;
fleetMode = manualMode;
var remoteRatioInput = !scriptOn;
if (fleetMode == 2)
{
// 原地旋转:摇杆左右 → 绕车队中心角速度(deg/s)。底盘 SetOriginBias 已设为车队中心
fleetOmega = manualVth;
// 外部遥控: Vth 为摇杆比例;内部脚本: Vth 为实际角速度(deg/s)
fleetOmega = remoteRatioInput
? ClampFloat(manualVth, -1f, 1f) * Math.Max(0f, Conf.FleetManualMaxRotateOmegaDegPerSec)
: manualVth;
fleetVx = 0;
fleetFrontTh = 0;
fleetRearTh = 0;
@@ -774,12 +781,16 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
}
else if (fleetMode == 1)
{
// 手动蟹行:Vx 只表示线速度,Vy 表示方向摇杆比例(-1..1),由 VyFac 映射为舵角
var speed = manualVx * Conf.ManualCarSyncVxFac;
var crabRatio = Math.Max(-90f, Math.Min(90f, manualVy));
var crabAngle = crabRatio * Conf.ManualCarSyncVyFac;
// 外部遥控: Vx/Vy 为摇杆比例;内部脚本: Vx 为线速度(m/s), Vy 为蟹行舵角(deg)
var speed = remoteRatioInput
? ClampFloat(manualVx, -1f, 1f) * Math.Max(0f, Conf.FleetManualMaxSpeed)
: manualVx;
var crabRatio = remoteRatioInput ? ClampFloat(manualVy, -1f, 1f) : 0f;
var crabAngle = remoteRatioInput
? crabRatio * Math.Abs(Conf.FleetManualMaxCrabAngleDeg)
: manualVy;
crabInputVx = speed;
crabInputVy = crabRatio;
crabInputVy = remoteRatioInput ? crabRatio : crabAngle;
crabRawAngle = crabAngle;
crabSteerLimit = Math.Min(179f, Math.Max(1f, Math.Abs(Conf.MultiVehicleCrabSteerLimitDeg)));
@@ -793,8 +804,13 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
}
else
{
fleetVx = manualVx * Conf.ManualCarSyncVxFac;
var targetTh = manualVth * Conf.ManualCarSyncVthFac;
// 外部遥控: Vx/Vth 为摇杆比例;内部脚本: Vx 为线速度(m/s), Vth 为目标舵角(deg)。
fleetVx = remoteRatioInput
? ClampFloat(manualVx, -1f, 1f) * Math.Max(0f, Conf.FleetManualMaxSpeed)
: manualVx;
var targetTh = remoteRatioInput
? ClampFloat(manualVth, -1f, 1f) * Math.Abs(Conf.FleetManualMaxSteerAngleDeg)
: manualVth;
var now = DateTime.Now;
var dt = (float)Math.Min(0.2, Math.Max(0, (now - _multiVehicleLastThTime).TotalSeconds));
_multiVehicleLastThTime = now;
@@ -804,6 +820,17 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
fleetFrontTh = _multiVehicleAccumulateTh;
fleetRearTh = -fleetFrontTh;
}
if (manualHold)
{
fleetMode = 0;
fleetVx = 0;
fleetFrontTh = 0;
fleetRearTh = 0;
fleetOmega = 0;
_multiVehicleAccumulateTh = 0f;
_multiVehicleLastThTime = DateTime.Now;
}
}
else
{
@@ -830,11 +857,9 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
}
}
}
else if (MultiVehicleNotification != null)
else if (activeNotification != null)
{
VehicleSyncNotification notification;
lock (_multiVehicleNotificationLock)
notification = MultiVehicleNotification;
var notification = activeNotification;
syncTh = notification.SyncTh;
syncDistance = notification.SyncDistance;
+9 -6
View File
@@ -89,22 +89,25 @@ public partial class CartDefinition : CartActivator.CartDefinition
[AsLowerIO(desc = "(手动)多车联动模式")]
public int MultiVehicleManualMode;
[AsLowerIO(desc = "多车联动:遥控器Vx")]
[AsLowerIO(desc = "多车联动:遥控器Vx比例")]
public float MultiVehicleManualVx;
[AsLowerIO(desc = "多车联动:遥控器Vy")]
[AsLowerIO(desc = "多车联动:遥控器Vy比例")]
public float MultiVehicleManualVy;
[AsLowerIO(desc = "多车联动:遥控器Vth")]
[AsLowerIO(desc = "多车联动:遥控器Vth比例")]
public float MultiVehicleManualVth;
[AsInitParam(desc = "手动最大速度(m/s)")]
[AsLowerIO(desc = "多车联动:遥控暂停")]
public bool MultiVehicleHold;
[AsInitParam(desc = "普通手动最大速度(m/s),车队联动不使用")]
public float MaxManualSpeed = 0.3f;
[AsInitParam(desc = "手动最大自旋角速度(deg/s)")]
[AsInitParam(desc = "普通手动最大自旋角速度(deg/s),车队联动不使用")]
public float MaxManualAngularSpeed = 45f;
[AsInitParam(desc = "手动最大转向角度(deg)")]
[AsInitParam(desc = "普通手动最大转向角度(deg),车队联动不使用")]
public float MaxManualTheta = 45f;
[AsInitParam(desc = "遥控转向输入幂数")]
+9 -24
View File
@@ -26,6 +26,7 @@ public partial class CartDefinition
MultiVehicleManualVx = 0;
MultiVehicleManualVy = 0;
MultiVehicleManualVth = 0;
MultiVehicleHold = false;
}
[IOObjectUtility]
@@ -43,7 +44,6 @@ public partial class CartDefinition
var crabOn = false; // 蟹行(四轮同向平移)
var rotateOn = false; // 原地旋转(绕车队中心)
// 速度比例 0~1,作用于摇杆输出的线速度/横移/角速度。
var speedRatio = 1f;
UseGesture? manip = null;
// 0=常规(前进+转向) 1=蟹行 2=原地旋转。crab 优先于 rotate(同时打开时蟹行生效)。
@@ -87,7 +87,6 @@ public partial class CartDefinition
return;
}
var ratio = Math.Clamp(speedRatio, 0, 1);
var px = Math.Clamp(pos.X, -1, 1);
var py = Math.Clamp(pos.Y, -1, 1);
var mode = CurrentMode();
@@ -95,31 +94,27 @@ public partial class CartDefinition
if (mode == 2)
{
// 原地旋转:pos.X(左右) → 角速度(deg/s),绕车队中心。
MultiVehicleManualVx = 0;
MultiVehicleManualVy = 0;
MultiVehicleManualVth = px * MaxManualAngularSpeed * ratio;
MultiVehicleManualVth = px;
}
else if (mode == 1)
{
// 蟹行:pos.Y → 前后向线速度,pos.X → 横向线速度(m/s)。
MultiVehicleManualVx = py * MaxManualSpeed * ratio;
MultiVehicleManualVy = px * MaxManualSpeed * ratio;
MultiVehicleManualVx = py;
MultiVehicleManualVy = px;
MultiVehicleManualVth = 0;
}
else
{
// 常规:pos.Y → 线速度(m/s)pos.X → 转向角(deg)。
MultiVehicleManualVx = py * MaxManualSpeed * ratio;
MultiVehicleManualVx = py;
MultiVehicleManualVy = 0;
MultiVehicleManualVth = px * MaxManualAngularSpeed * ratio;
MultiVehicleManualVth = px;
}
if ((DateTime.Now - _fleetDiagLastStick).TotalMilliseconds >= 200)
{
_fleetDiagLastStick = DateTime.Now;
FleetDiag($"STICK mode={mode} pos=({pos.X:0.00},{pos.Y:0.00}) manip={manipulating} ratio={ratio:0.00} " +
$"-> Vx={MultiVehicleManualVx:0.000} Vy={MultiVehicleManualVy:0.000} Vth={MultiVehicleManualVth:0.0} en={MultiVehicleManualEnabled}");
FleetDiag($"STICK mode={mode} pos=({pos.X:0.00},{pos.Y:0.00}) manip={manipulating} raw " +
$"-> Vx={MultiVehicleManualVx:0.000} Vy={MultiVehicleManualVy:0.000} Vth={MultiVehicleManualVth:0.000} en={MultiVehicleManualEnabled}");
}
}
});
@@ -169,15 +164,6 @@ public partial class CartDefinition
}
});
manip.AddWidget(new UseGesture.ThrottleWidget
{
name = "fleet_speed_ratio",
text = "速度比例",
position = "50%+10px, 64%+10px",
size = "37.5%-10px, 10%-10px",
bounceBack = false,
OnValue = (val, _) => speedRatio = Math.Clamp(val, 0, 1)
});
manip.AddWidget(new UseGesture.ButtonWidget
{
@@ -218,8 +204,7 @@ public partial class CartDefinition
pb.SeparatorText("状态");
var modeName = MultiVehicleManualMode == 2 ? "原地旋转" : MultiVehicleManualMode == 1 ? "蟹行" : "常规";
pb.Label($"车队联动: {MultiVehicleManualEnabled} 模式: {modeName}");
pb.Label($"速度比例: {speedRatio:0.00}");
pb.Label($"Vx={MultiVehicleManualVx:0.000} Vy={MultiVehicleManualVy:0.000} m/s, Vth={MultiVehicleManualVth:0.0}");
pb.Label($"VxRatio={MultiVehicleManualVx:0.000} VyRatio={MultiVehicleManualVy:0.000}, VthRatio={MultiVehicleManualVth:0.000}");
pb.Label($"优先级: {CartActivator.CartDefinition.currentPriority} ({CartActivator.CartDefinition.currentPriorityDesc})");
pb.Label("请勿同时打开「手动控制」面板");
pb.Panel.Repaint();
-22
View File
@@ -1,22 +0,0 @@
using System.Threading.Tasks;
using SimpleComposer.RCS;
using SimpleCore.PropType;
namespace MultiWheelS
{
[CarType(Name = "多车联动AGV")]
public class MultiVehicleCar : GhostCar
{
public bool MultiVehicleSync = true;
public static new async Task<MultiVehicleCar> Create()
{
return new MultiVehicleCar
{
lstatus = "连接中",
address = "127.0.0.1",
name = "联动AGV"
};
}
}
}
-62
View File
@@ -1,62 +0,0 @@
<?xml version="1.0" encoding="utf-8"?>
<Project ToolsVersion="15.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
<Import Project="$(MSBuildExtensionsPath)\$(MSBuildToolsVersion)\Microsoft.Common.props" Condition="Exists('$(MSBuildExtensionsPath)\$(MSBuildToolsVersion)\Microsoft.Common.props')" />
<PropertyGroup>
<LangVersion>latest</LangVersion>
</PropertyGroup>
<PropertyGroup>
<Configuration Condition=" '$(Configuration)' == '' ">Debug</Configuration>
<Platform Condition=" '$(Platform)' == '' ">AnyCPU</Platform>
<ProjectGuid>{D1D1D1D1-E2E2-F3F3-A4A4-B5B5B5B5B5B3}</ProjectGuid>
<OutputType>Library</OutputType>
<RootNamespace>MultiWheelS</RootNamespace>
<AssemblyName>MultiWheelS</AssemblyName>
<TargetFrameworkVersion>v4.8</TargetFrameworkVersion>
<FileAlignment>512</FileAlignment>
<Deterministic>true</Deterministic>
</PropertyGroup>
<PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Debug|AnyCPU' ">
<DebugSymbols>true</DebugSymbols>
<DebugType>full</DebugType>
<Optimize>false</Optimize>
<OutputPath>bin\Debug\</OutputPath>
<DefineConstants>DEBUG;TRACE</DefineConstants>
<ErrorReport>prompt</ErrorReport>
<WarningLevel>4</WarningLevel>
</PropertyGroup>
<PropertyGroup Condition=" '$(Configuration)|$(Platform)' == 'Release|AnyCPU' ">
<DebugType>pdbonly</DebugType>
<Optimize>true</Optimize>
<OutputPath>bin\Release\</OutputPath>
<DefineConstants>TRACE</DefineConstants>
<ErrorReport>prompt</ErrorReport>
<WarningLevel>4</WarningLevel>
</PropertyGroup>
<ItemGroup>
<Reference Include="LessokajiWeaverUtilities">
<HintPath>D:\MDCS\Release\deps\LessokajiWeaverUtilities.dll</HintPath>
</Reference>
<Reference Include="RefSimpleCore">
<HintPath>D:\MDCS\Dependencies\Simple\RefSimpleCore.dll</HintPath>
</Reference>
<Reference Include="SimpleComposer">
<HintPath>D:\MDCS\Executables\Simple\SimpleComposer.exe</HintPath>
</Reference>
<Reference Include="System" />
<Reference Include="System.Core" />
</ItemGroup>
<ItemGroup>
<Compile Include="MultiVehicleCar.cs" />
</ItemGroup>
<Import Project="$(MSBuildToolsPath)\Microsoft.CSharp.targets" />
<PropertyGroup>
<PostBuildEvent>if not exist "$(SolutionDir)build\Simple\plugins" mkdir "$(SolutionDir)build\Simple\plugins"
if not exist "$(SolutionDir)build\Simple" mkdir "$(SolutionDir)build\Simple"
xcopy "$(TargetDir)$(TargetFileName)" "$(SolutionDir)build\Simple\plugins" /y
xcopy "$(TargetDir)$(TargetName).pdb" "$(SolutionDir)build\Simple\plugins" /y
copy /Y "D:\MDCS\Executables\Simple\SimpleComposer.exe" "$(SolutionDir)build\Simple\"
copy /Y "D:\MDCS\Release\MDCSToolBox.dll" "$(SolutionDir)build\Simple\"
copy /Y "D:\MDCS\Release\CommonUsage.dll" "$(SolutionDir)build\Simple\"
copy /Y "D:\MDCS\Dependencies\Simple\RefSimpleCore.dll" "$(SolutionDir)build\Simple\"</PostBuildEvent>
</PropertyGroup>
</Project>