Add Clumsy plugins and dual-vehicle fleet sync demo.
Restructure into DiffWheel/MultiWheel folders, add DiffWheelC and MultiWheelC with MovementTests, FleetRemote multi-vehicle manual sync, deploy templates, and documentation. Update dependency paths to D:\MDCS\Release and mirror motor feedback in MotorRoutine for simulation. Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
Cursor
@@ -0,0 +1,30 @@
|
||||
using System;
|
||||
using MDCSToolBox.Clumsy.AgvInterfaces;
|
||||
using MDCSToolBox.Clumsy.MotionControllers;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
public class AGV : MultiWheelInterface
|
||||
{
|
||||
public override AbstractGeometricController GetController()
|
||||
{
|
||||
return new ChassisController().Get();
|
||||
}
|
||||
|
||||
public override MultiWheelMagTracker GetMagController()
|
||||
{
|
||||
throw new NotImplementedException("Tutorial MultiWheelC does not implement magnetic tracking.");
|
||||
}
|
||||
|
||||
public override NaiveMagnetController GetNaiveMagnetController()
|
||||
{
|
||||
throw new NotImplementedException("Tutorial MultiWheelC does not implement naive magnetic tracking.");
|
||||
}
|
||||
|
||||
public void FleetGo(float srcX, float srcY, int srcId, float dstX, float dstY, int dstId, int trackId,
|
||||
float speed = -1, bool reverse = false, float carDirectionBias = 0)
|
||||
{
|
||||
BasicGo(srcX, srcY, srcId, dstX, dstY, dstId, trackId, speed, reverse, carDirectionBias,
|
||||
multiVehicleSync: true);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,70 @@
|
||||
using System;
|
||||
using System.Numerics;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using MDCSToolBox.Clumsy.MotionControllers;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
public class ChassisController : MovementDefinition<MultiWheelGeometricController>
|
||||
{
|
||||
public float BaseSpeed = Configuration.conf.basicSpeed;
|
||||
|
||||
public override MultiWheelGeometricController Get()
|
||||
{
|
||||
return new MultiWheelGeometricController
|
||||
{
|
||||
Chassis = BasicPilotBase.Chassis,
|
||||
BaseSpeed = BaseSpeed,
|
||||
SlowDistance = PilotDefinition.Conf.SlowDistance,
|
||||
SlowingPow = PilotDefinition.Conf.SlowingPow,
|
||||
FinishDistance = PilotDefinition.Conf.FinishDistance,
|
||||
FinishSpeed = PilotDefinition.Conf.FinishSpeed,
|
||||
FirstThAccuracy = PilotDefinition.Conf.FirstThAccuracy,
|
||||
FirstRotateSpeedFac = PilotDefinition.Conf.FirstRotateSpeedFac,
|
||||
FirstRotateMaxSpeed = PilotDefinition.Conf.FirstRotateMaxSpeed,
|
||||
NotContinuousAngle = PilotDefinition.Conf.NotContinuousAngle,
|
||||
DebugMode = PilotDefinition.Conf.MotionDebugPrint,
|
||||
DebugCurvature = PilotDefinition.Conf.DebugCurvature,
|
||||
PowerSteeringLookAhead = PilotDefinition.Conf.PowerSteeringLookAhead,
|
||||
SpeedLookAhead = PilotDefinition.Conf.SpeedLookAhead,
|
||||
SpeedLookAheadCurveDiff = PilotDefinition.Conf.SpeedLookAheadCurveDiff,
|
||||
SpeedLookBackCurveDiff = PilotDefinition.Conf.SpeedLookBackCurveDiff,
|
||||
SpeedLimitCurveDiffMin = PilotDefinition.Conf.SpeedLimitCurveDiffMin,
|
||||
SpeedLimitCurveMin = PilotDefinition.Conf.SpeedLimitCurveMin,
|
||||
MaxRotateSpeed = PilotDefinition.Conf.MaxRotateSpeedCurveLimit,
|
||||
MaxRotateAcc = PilotDefinition.Conf.MaxRotateAccCurveLimit,
|
||||
GcpThetaThreshold = PilotDefinition.Conf.GcpThetaThreshold,
|
||||
DthLinearFac = PilotDefinition.Conf.DthLinearFac,
|
||||
DthLinearThreshold = PilotDefinition.Conf.DthLinearThreshold,
|
||||
BiasFac = PilotDefinition.Conf.BiasFac,
|
||||
BiasThreshold = PilotDefinition.Conf.BiasThreshold,
|
||||
|
||||
MultiVehicleSendMotion = (speed, frontTh, rearTh, idealPos, idealAngle) =>
|
||||
{
|
||||
PilotDefinition.Self.MultiVehicleAutoEnabled = true;
|
||||
lock (PilotDefinition.Self.MultiVehicleFleet)
|
||||
{
|
||||
if (PilotDefinition.Self.MultiVehicleFleet.Count != PilotDefinition.Conf.MultiVehicleFleetNum)
|
||||
return;
|
||||
}
|
||||
|
||||
PilotDefinition.Self.MultiVehicleAutoVx = speed;
|
||||
PilotDefinition.Self.MultiVehicleAutoFrontTh = frontTh;
|
||||
PilotDefinition.Self.MultiVehicleAutoRearTh = rearTh;
|
||||
},
|
||||
|
||||
MultiVehicleGetFleetPos = () => new Location
|
||||
{
|
||||
x = PilotDefinition.Self.CenterX,
|
||||
y = PilotDefinition.Self.CenterY,
|
||||
th = PilotDefinition.Self.CenterTh,
|
||||
l_step = 1,
|
||||
tick = DateTime.Now.Ticks
|
||||
}
|
||||
};
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,123 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Linq;
|
||||
using System.Numerics;
|
||||
using System.Threading;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.DTools;
|
||||
using ClumsyCore.Pilot;
|
||||
using ClumsyCore.Utilities;
|
||||
using ClumsyDance.ClumsyDance.Detectors;
|
||||
using ClumsyDance.ClumsyWalk.Detectors;
|
||||
using FundamentalLib;
|
||||
using LineSegment = ClumsyCore.Utilities.LineSegment;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
/// <summary>
|
||||
/// 2腿检测：用单线激光雷达识别两腿托盘/轮胎，按上一帧结果作为下一帧猜测做闭环检测。
|
||||
/// 从 StandardMultiWheelLifter 移植；参数全部走 PilotConfig（Fields 面板），雷达选择改为配置项而非阻塞输入。
|
||||
/// </summary>
|
||||
[MovementTest(name = "多舵轮-2腿检测")]
|
||||
public class TwoLegDetect : MovementTest
|
||||
{
|
||||
private Painter _painter;
|
||||
private bool _running = true;
|
||||
|
||||
public override void TestStop() => _running = false;
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
_running = true;
|
||||
_painter = UI.GetPainter("MultiWheelTwoLegDetect", false);
|
||||
_painter.Clear();
|
||||
|
||||
var lidar = PilotDefinition.Conf.TwoLegLidarName;
|
||||
var lastDetectX = PilotDefinition.Conf.TwoLegGuessX;
|
||||
var lastDetectY = 0f;
|
||||
|
||||
while (_running)
|
||||
{
|
||||
var ld = Detect(lidar, lastDetectX, SetFilters(lastDetectX, lastDetectY));
|
||||
if (ld == null)
|
||||
{
|
||||
Thread.Sleep(100);
|
||||
continue;
|
||||
}
|
||||
|
||||
var center = (ld.Src + ld.Dst) / 2;
|
||||
var distanceToCarOrigin = Vector2.Distance(Vector2.Zero, center);
|
||||
|
||||
_painter.Clear();
|
||||
_painter.DrawLine(Color.Cyan, Vector2.Zero, center, width: 2);
|
||||
_painter.DrawText(Color.Yellow, $"{distanceToCarOrigin:F1}", center.X / 2, center.Y / 2);
|
||||
|
||||
Hedingben.ToastText(
|
||||
$"[Test检测] lidar:{lidar} guess x:{lastDetectX:F0} y:{lastDetectY:F0} | " +
|
||||
$"中心 x:{center.X:F0} y:{center.Y:F0} dist:{distanceToCarOrigin:F0}",
|
||||
"MultiWheelTwoLegDetect-test");
|
||||
|
||||
// 用本帧中心作为下一帧猜测，实现闭环跟踪
|
||||
lastDetectX = center.X;
|
||||
lastDetectY = center.Y;
|
||||
Thread.Sleep(100);
|
||||
}
|
||||
|
||||
_painter.Clear();
|
||||
}
|
||||
|
||||
/// <summary>在车体坐标系下，按猜测位置检测两腿，返回连接两腿的线段（车体系）。</summary>
|
||||
public static LineSegment Detect(string lidarName, float guessX, List<DetectFilter> filters)
|
||||
{
|
||||
var conf = PilotDefinition.Conf;
|
||||
#pragma warning disable CS0612, CS0618
|
||||
var detector = new Lidar2dDetect2LegTray
|
||||
{
|
||||
BlobDist = conf.TwoLegBlobDist,
|
||||
BlobPtCount = conf.TwoLegBlobPtCount,
|
||||
BlobSize = conf.TwoLegBlobSize,
|
||||
CenterChange = Tuple.Create(conf.TwoLegCenterChangeX, 0f, 0f),
|
||||
LegWidth = conf.TwoLegWidth,
|
||||
LegWidthErr = conf.TwoLegWidthErr,
|
||||
Padding = conf.TwoLegPadding,
|
||||
PillarFindingScope = conf.TwoLegPillarFindingScope,
|
||||
SgnDir = conf.TwoLegSgnDir,
|
||||
};
|
||||
#pragma warning restore CS0612, CS0618
|
||||
|
||||
return detector.DetectWithGuess(
|
||||
lidarName,
|
||||
new LineSegment(new Vector2(guessX, 0), Vector2.Zero),
|
||||
guessCoordinateSystem: CoordinateSystem.Car2D,
|
||||
outCoordinateSystem: CoordinateSystem.Car2D,
|
||||
filters);
|
||||
}
|
||||
|
||||
/// <summary>在猜测中心周围构造一个矩形 ROI，过滤掉框外点云，降低误识别。</summary>
|
||||
public static List<DetectFilter> SetFilters(float guessCenterX, float guessCenterY)
|
||||
{
|
||||
var conf = PilotDefinition.Conf;
|
||||
var painter = UI.GetPainter("MultiWheelTwoLegDetect.Filter", false);
|
||||
painter.Clear();
|
||||
|
||||
var box = new[]
|
||||
{
|
||||
new Vector2(guessCenterX - conf.TwoLegFilterLength / 2, guessCenterY - conf.TwoLegFilterWidth / 2),
|
||||
new Vector2(guessCenterX + conf.TwoLegFilterLength / 2, guessCenterY - conf.TwoLegFilterWidth / 2),
|
||||
new Vector2(guessCenterX + conf.TwoLegFilterLength / 2, guessCenterY + conf.TwoLegFilterWidth / 2),
|
||||
new Vector2(guessCenterX - conf.TwoLegFilterLength / 2, guessCenterY + conf.TwoLegFilterWidth / 2),
|
||||
};
|
||||
|
||||
for (var i = 0; i < box.Length; ++i)
|
||||
painter.DrawLine(Color.DarkOliveGreen, box[i], box[(i + 1) % 4]);
|
||||
|
||||
// 点云滤波在车体坐标系下进行
|
||||
return new List<DetectFilter>
|
||||
{
|
||||
new(CoordinateSystem.Car2D,
|
||||
p => LessMath.IsPointInPolygon4(
|
||||
box.Select(v => new PointF(v.X, v.Y)).ToArray(), new PointF(p.X, p.Y))),
|
||||
};
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,241 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Numerics;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using FundamentalLib;
|
||||
using CommonUsage.Chassis;
|
||||
using CommonUsage.Mathematics;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
public class MultiForwardTest : MovementDefinition
|
||||
{
|
||||
public float Speed = 0.2f;
|
||||
public float DurationSeconds = 2f;
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var chassis = (MultiWheelChassis)BasicPilotBase.Chassis;
|
||||
chassis.SetOriginBias(0, 0, 0);
|
||||
var end = DateTime.Now.AddSeconds(DurationSeconds);
|
||||
while (DateTime.Now < end)
|
||||
{
|
||||
chassis.SendMotion(Speed, 0, 0);
|
||||
yield return true;
|
||||
}
|
||||
|
||||
chassis.SendMotion(0, 0, 0);
|
||||
}
|
||||
}
|
||||
|
||||
// 原地旋转到指定世界坐标系朝向：先把舵轮打到旋转所需角度，对齐后再旋转，按目标角度停止（非固定时长）。
|
||||
public class MultiRotateToWorldAngle : MovementDefinition
|
||||
{
|
||||
/// <summary>目标朝向（世界坐标系，单位 deg）。</summary>
|
||||
public float TargetWorldDeg;
|
||||
|
||||
/// <summary>旋转角速度（deg/s，逆时针为正）。</summary>
|
||||
public float RotSpeed = 30f;
|
||||
|
||||
/// <summary>到位角度精度（deg）。</summary>
|
||||
public float ArriveDeg = 1f;
|
||||
|
||||
/// <summary>起转前舵轮对齐精度（deg）。</summary>
|
||||
public float WheelAlignDeg = 2f;
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var chassis = (MultiWheelChassis)BasicPilotBase.Chassis;
|
||||
chassis.SetOriginBias(0, 0, 0);
|
||||
|
||||
// 阶段一：仅把舵轮打到原地旋转所需角度（下发 0 速度，只对齐不旋转）。
|
||||
while (true)
|
||||
{
|
||||
chassis.SendRotateMotion(0);
|
||||
if (WheelsAligned(chassis, WheelAlignDeg)) break;
|
||||
yield return true;
|
||||
}
|
||||
|
||||
// 阶段二：旋转到目标世界朝向，到位即停。
|
||||
var target = CommonMath.RoundTh(TargetWorldDeg);
|
||||
while (true)
|
||||
{
|
||||
var cur = CommonMath.RoundTh((float)DetourInterface.getCartLocation().th);
|
||||
var diff = CommonMath.ThDiff(target, cur); // 逆时针为正
|
||||
if (Math.Abs(diff) <= ArriveDeg) break;
|
||||
chassis.SendRotateMotion(Math.Sign(diff) * RotSpeed);
|
||||
yield return true;
|
||||
}
|
||||
|
||||
chassis.PredefinedDriveStop();
|
||||
}
|
||||
|
||||
private static bool WheelsAligned(MultiWheelChassis chassis, float tolDeg)
|
||||
{
|
||||
#pragma warning disable CS0612, CS0618
|
||||
var wheels = chassis.GetSteerWheels();
|
||||
#pragma warning restore CS0612, CS0618
|
||||
foreach (var sw in wheels)
|
||||
if (Math.Abs(CommonMath.ThDiff(sw.ReadAngle(), sw.GetSendAngle())) > tolDeg)
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "多舵轮-前进2秒")]
|
||||
public class MultiForwardMovementTest : MovementTest
|
||||
{
|
||||
private DriveTask _task;
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
_task = new DriveTask(new MultiForwardTest().Get());
|
||||
_task.Wait();
|
||||
}
|
||||
|
||||
public override void TestStop() => _task?.Stop();
|
||||
}
|
||||
|
||||
[MovementTest(name = "多舵轮-原地旋转到目标角度")]
|
||||
public class MultiRotateMovementTest : MovementTest
|
||||
{
|
||||
private DriveTask _task;
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
_task = new DriveTask(new MultiRotateToWorldAngle
|
||||
{
|
||||
TargetWorldDeg = PilotDefinition.Conf.InPlaceRotateTargetWorldDeg,
|
||||
RotSpeed = PilotDefinition.Conf.InPlaceRotateSpeed,
|
||||
ArriveDeg = PilotDefinition.Conf.InPlaceRotateArriveDeg,
|
||||
WheelAlignDeg = PilotDefinition.Conf.InPlaceRotateWheelAlignDeg
|
||||
}.Get());
|
||||
_task.Wait();
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
_task?.Stop();
|
||||
((MultiWheelChassis)BasicPilotBase.Chassis).PredefinedDriveStop();
|
||||
}
|
||||
}
|
||||
|
||||
// ===== 调用 Playground WebAPI 瞬移小车（前移 / 左移 / 旋转）=====
|
||||
// 平移/旋转量在 Fields 面板配置：WebApiTranslateMm（默认100mm）、WebApiRotateDeg（默认5度）。
|
||||
|
||||
[MovementTest(name = "多舵轮-WebAPI前移")]
|
||||
public class WebApiForwardMoveTest : MovementTest
|
||||
{
|
||||
public override void Test()
|
||||
{
|
||||
var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
|
||||
var name = PilotDefinition.Conf.PlaygroundRobotName;
|
||||
var d = PilotDefinition.Conf.WebApiTranslateMm;
|
||||
|
||||
var pose = PlaygroundWebApi.GetPose(url, name);
|
||||
// 车体系前向 (d, 0) 变换到世界系：车头方向即朝向 yaw
|
||||
var dst = CommonMath.Transform2D(new Vector2(pose.X, pose.Y), pose.YawDeg, new Vector2(d, 0));
|
||||
PlaygroundWebApi.Move(url, name, dst.X, dst.Y, pose.YawDeg);
|
||||
Hedingben.ToastText($"前移 {d:f0}mm -> ({dst.X:f0},{dst.Y:f0})", "WebApiForward");
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "多舵轮-WebAPI左移")]
|
||||
public class WebApiLeftMoveTest : MovementTest
|
||||
{
|
||||
public override void Test()
|
||||
{
|
||||
var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
|
||||
var name = PilotDefinition.Conf.PlaygroundRobotName;
|
||||
var d = PilotDefinition.Conf.WebApiTranslateMm;
|
||||
|
||||
var pose = PlaygroundWebApi.GetPose(url, name);
|
||||
// 车体系左向 (0, d) 变换到世界系（车体 +Y 即左侧）
|
||||
var dst = CommonMath.Transform2D(new Vector2(pose.X, pose.Y), pose.YawDeg, new Vector2(0, d));
|
||||
PlaygroundWebApi.Move(url, name, dst.X, dst.Y, pose.YawDeg);
|
||||
Hedingben.ToastText($"左移 {d:f0}mm -> ({dst.X:f0},{dst.Y:f0})", "WebApiLeft");
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "多舵轮-WebAPI旋转")]
|
||||
public class WebApiRotateTest : MovementTest
|
||||
{
|
||||
public override void Test()
|
||||
{
|
||||
var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
|
||||
var name = PilotDefinition.Conf.PlaygroundRobotName;
|
||||
var deg = PilotDefinition.Conf.WebApiRotateDeg;
|
||||
|
||||
var pose = PlaygroundWebApi.GetPose(url, name);
|
||||
var ny = pose.YawDeg + deg; // 逆时针为正
|
||||
PlaygroundWebApi.Move(url, name, pose.X, pose.Y, ny);
|
||||
Hedingben.ToastText($"旋转 {deg:f1}° -> {ny:f1}°", "WebApiRotate");
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "多舵轮-WebAPI恢复运动")]
|
||||
public class WebApiMotionResumeTest : MovementTest
|
||||
{
|
||||
public override void Test()
|
||||
{
|
||||
var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
|
||||
PlaygroundWebApi.ResumeMotion(url);
|
||||
Hedingben.ToastText("已恢复车辆运动", "WebApiMotion");
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "多舵轮-WebAPI暂停运动")]
|
||||
public class WebApiMotionPauseTest : MovementTest
|
||||
{
|
||||
public override void Test()
|
||||
{
|
||||
var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
|
||||
PlaygroundWebApi.PauseMotion(url); // 默认 zero 模式：反馈归零
|
||||
Hedingben.ToastText("已暂停车辆运动 (zero)", "WebApiMotion");
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "多舵轮-车队直线1m")]
|
||||
public class FleetStraightMovementTest : MovementTest
|
||||
{
|
||||
public override void Test()
|
||||
{
|
||||
if (PilotDefinition.Conf.MultiVehicleMasterEndpoint != "/")
|
||||
{
|
||||
Hedingben.ToastText("仅主车可发起车队轨迹", "FleetGo");
|
||||
return;
|
||||
}
|
||||
|
||||
var pos = DetourInterface.getCartLocation();
|
||||
var agv = new AGV();
|
||||
agv.FleetGo((float)pos.x, (float)pos.y, 1, (float)pos.x + 1000f, (float)pos.y, 2, 10001, 0.2f);
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,45 @@
|
||||
<Project Sdk="Microsoft.NET.Sdk">
|
||||
|
||||
<PropertyGroup>
|
||||
<TargetFramework>netstandard2.0</TargetFramework>
|
||||
<LangVersion>10</LangVersion>
|
||||
<AllowUnsafeBlocks>true</AllowUnsafeBlocks>
|
||||
<AppendTargetFrameworkToOutputPath>false</AppendTargetFrameworkToOutputPath>
|
||||
<OutputPath>..\..\build\Clumsy\</OutputPath>
|
||||
</PropertyGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<PackageReference Include="Newtonsoft.Json" Version="13.0.3" />
|
||||
<PackageReference Include="System.Numerics.Vectors" Version="4.6.1" />
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<Reference Include="ClumsyCore">
|
||||
<HintPath>D:\MDCS\Release\Clumsy\RefClumsyCore.dll</HintPath>
|
||||
<Private>false</Private>
|
||||
</Reference>
|
||||
<Reference Include="ClumsyDance">
|
||||
<HintPath>D:\MDCS\Release\Clumsy\RefClumsyDance.dll</HintPath>
|
||||
<Private>false</Private>
|
||||
</Reference>
|
||||
<Reference Include="CommonUsage">
|
||||
<HintPath>D:\MDCS\Release\CommonUsage.dll</HintPath>
|
||||
</Reference>
|
||||
<Reference Include="FundamentalLib">
|
||||
<HintPath>D:\MDCS\Release\deps\RefFundamentalLib.dll</HintPath>
|
||||
<Private>false</Private>
|
||||
</Reference>
|
||||
<Reference Include="LessokajiWeaverUtilities">
|
||||
<HintPath>D:\MDCS\Release\deps\LessokajiWeaverUtilities.dll</HintPath>
|
||||
<Private>false</Private>
|
||||
</Reference>
|
||||
<Reference Include="MDCSToolBox">
|
||||
<HintPath>D:\MDCS\Release\MDCSToolBox.dll</HintPath>
|
||||
</Reference>
|
||||
</ItemGroup>
|
||||
|
||||
<Target Name="PostBuild" AfterTargets="PostBuildEvent">
|
||||
<Exec Command="if not exist $(SolutionDir)build\Clumsy mkdir $(SolutionDir)build\Clumsy
if not exist $(SolutionDir)build\Clumsy_AGV2 mkdir $(SolutionDir)build\Clumsy_AGV2
copy /Y D:\MDCS\Release\Clumsy\ClumsyLite.exe $(SolutionDir)build\Clumsy\
copy /Y D:\MDCS\Release\Clumsy\ClumsyLite.deps.json $(SolutionDir)build\Clumsy\
copy /Y D:\MDCS\Release\Clumsy\ClumsyLite.runtimeconfig.json $(SolutionDir)build\Clumsy\
copy /Y D:\MDCS\Release\Clumsy\RefClumsyCore.dll $(SolutionDir)build\Clumsy\
copy /Y D:\MDCS\Release\Clumsy\RefClumsyDance.dll $(SolutionDir)build\Clumsy\
copy /Y D:\MDCS\Release\MDCSToolBox.dll $(SolutionDir)build\Clumsy\
copy /Y D:\MDCS\Release\CommonUsage.dll $(SolutionDir)build\Clumsy\
copy /Y D:\MDCS\Release\Clumsy\ClumsyLite.exe $(SolutionDir)build\Clumsy_AGV2\
copy /Y D:\MDCS\Release\Clumsy\ClumsyLite.deps.json $(SolutionDir)build\Clumsy_AGV2\
copy /Y D:\MDCS\Release\Clumsy\ClumsyLite.runtimeconfig.json $(SolutionDir)build\Clumsy_AGV2\
copy /Y D:\MDCS\Release\Clumsy\RefClumsyCore.dll $(SolutionDir)build\Clumsy_AGV2\
copy /Y D:\MDCS\Release\Clumsy\RefClumsyDance.dll $(SolutionDir)build\Clumsy_AGV2\
copy /Y D:\MDCS\Release\MDCSToolBox.dll $(SolutionDir)build\Clumsy_AGV2\
copy /Y D:\MDCS\Release\CommonUsage.dll $(SolutionDir)build\Clumsy_AGV2\
copy /Y $(TargetDir)$(TargetName).dll $(SolutionDir)build\Clumsy_AGV2\
copy /Y $(TargetDir)$(TargetName).pdb $(SolutionDir)build\Clumsy_AGV2\" />
|
||||
</Target>
|
||||
|
||||
</Project>
|
||||
@@ -0,0 +1,116 @@
|
||||
using ClumsyCore;
|
||||
using MDCSToolBox.Clumsy.Pilot.MultiWheel;
|
||||
using Newtonsoft.Json;
|
||||
|
||||
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;
|
||||
[FieldMember(desc = "[sync] 手动Vth系数")] public float ManualCarSyncVthFac = 1f;
|
||||
[FieldMember(desc = "[sync] 检测中心偏移(mm)")] public float DeltaDetectCenter = 350f;
|
||||
[FieldMember(desc = "[sync] 是否启用Detour定位")] public bool PosAvailable = true;
|
||||
|
||||
[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";
|
||||
|
||||
[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;
|
||||
[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;
|
||||
|
||||
[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 = "Playground 小车名称（场景 robots[].name）")]
|
||||
public string PlaygroundRobotName = "agv_multi_1";
|
||||
|
||||
[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;
|
||||
|
||||
// ===== 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腿检测：ROI滤波框长(mm)")]
|
||||
public float TwoLegFilterLength = 1800f;
|
||||
|
||||
[FieldMember(desc = "2腿检测：ROI滤波框宽(mm)")]
|
||||
public float TwoLegFilterWidth = 600f;
|
||||
}
|
||||
@@ -0,0 +1,773 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Linq;
|
||||
using System.Net.Http;
|
||||
using System.Numerics;
|
||||
using System.Threading;
|
||||
using System.Threading.Tasks;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.DTools;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using ClumsyCore.Utilities;
|
||||
using CommonUsage;
|
||||
using CommonUsage.Chassis;
|
||||
using FundamentalLib;
|
||||
using FundamentalLib.Utilities;
|
||||
using MDCSToolBox.Clumsy.Pilot.MultiWheel;
|
||||
using Newtonsoft.Json;
|
||||
using Vector = ClumsyCore.Utilities.Vector;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefinition>
|
||||
{
|
||||
public new float CarLength = 1472f;
|
||||
public new float CarWidth = 948f;
|
||||
|
||||
public override void StandardInit()
|
||||
{
|
||||
InitMultiVehicleCoordination();
|
||||
}
|
||||
|
||||
#region MultiVehicleCoordination (从 MDCSToolBox 内联回 Tutorial)
|
||||
|
||||
[AsLowerIO(desc = "启用（手动）多车联动")] public bool MultiVehicleManualEnabled;
|
||||
[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;
|
||||
|
||||
[AsUpperIO(desc = "多车联动：自动驾驶", timeOutReset = true)] public bool MultiVehicleAutoEnabled;
|
||||
[FieldMember(desc = "多车联动：自动驾驶Vx")] public float MultiVehicleAutoVx;
|
||||
[FieldMember(desc = "多车联动：自动驾驶FrontTh")] public float MultiVehicleAutoFrontTh;
|
||||
[FieldMember(desc = "多车联动：自动驾驶RearTh")] public float MultiVehicleAutoRearTh;
|
||||
|
||||
public Dictionary<int, VehicleSyncInfo> MultiVehicleFleet = new();
|
||||
public VehicleSyncNotification MultiVehicleNotification;
|
||||
|
||||
[FieldMember(desc = "多车联动：车队姿态x")] public float CenterX;
|
||||
[FieldMember(desc = "多车联动：车队姿态y")] public float CenterY;
|
||||
[FieldMember(desc = "多车联动：车队姿态th")] public float CenterTh;
|
||||
|
||||
[AsLowerIO(desc = "车号")] public int CarNum = 1;
|
||||
|
||||
private float _multiVehicleAccumulateTh;
|
||||
private DateTime _multiVehicleLastThTime = DateTime.Now;
|
||||
private readonly object _multiVehicleNotificationLock = new();
|
||||
private DateTime _multiVehicleLastNotifyTime = DateTime.MinValue;
|
||||
private bool _multiVehicleSyncInitialized;
|
||||
|
||||
// 诊断日志：节流计时 + 最近一次检测几何（中心/朝向/距离），用于定位剧烈运动来源
|
||||
private DateTime _mvDbgLastLog = DateTime.MinValue;
|
||||
private float _mvLastDetCenterX, _mvLastDetCenterY, _mvLastDetDir, _mvLastDetDist;
|
||||
|
||||
// 写盘诊断日志（Clumsy 侧）。文件落在 Clumsy 工作目录，复现后离线分析。
|
||||
// 每次进程启动清空一次（_fleetDiagInit），同一次运行内追加，避免跨多次运行累积。
|
||||
private DateTime _mvDiskLastLog = DateTime.MinValue;
|
||||
private static bool _fleetDiagInit;
|
||||
private void FleetDiag(string msg)
|
||||
{
|
||||
try
|
||||
{
|
||||
const string path = "fleet_diag_clumsy.log";
|
||||
var line = $"{DateTime.Now:HH:mm:ss.fff} car{CarNum} {msg}{Environment.NewLine}";
|
||||
if (!_fleetDiagInit)
|
||||
{
|
||||
_fleetDiagInit = true;
|
||||
System.IO.File.WriteAllText(path,
|
||||
$"=== session start {DateTime.Now:yyyy-MM-dd HH:mm:ss} ==={Environment.NewLine}" + line);
|
||||
}
|
||||
else
|
||||
System.IO.File.AppendAllText(path, line);
|
||||
}
|
||||
catch
|
||||
{
|
||||
// 诊断日志失败不影响主流程
|
||||
}
|
||||
}
|
||||
|
||||
// 互识别：邻车两腿检测的滑动窗口（最近 1s，最多 10 帧），用于平滑抖动
|
||||
private readonly object _neighborDetectLock = new();
|
||||
private readonly List<(DateTime Time, Vector2 Src, Vector2 Dst)> _neighborDetects = new();
|
||||
|
||||
// 互识别：上一帧检测中心，用作下一帧猜测（闭环跟踪），与「多舵轮-2腿检测」MovementTest 一致
|
||||
private Vector2 _neighborGuess;
|
||||
private bool _neighborGuessValid;
|
||||
private DateTime _neighborGuessTime = DateTime.MinValue;
|
||||
|
||||
/// <summary>
|
||||
/// 互识别钩子：用单线雷达识别邻车的两腿/两轮，换算成本车体坐标系下相对“理应对齐参考点”的偏差。
|
||||
/// 全对齐时返回 (true, 0, 0, 0)；未检测到时返回 (false, ...)。
|
||||
/// detectDistance = 编队车间距 - 检测中心偏移（即邻车检测中心到本车原点的标称距离，恒为正值，方向由 TwoLegGuessX 符号决定）。
|
||||
/// </summary>
|
||||
private (bool Valid, float Dx, float Dy, float Dth, float NeighborDist) DetectNeighborBias(float detectDistance)
|
||||
{
|
||||
// 检测猜测点与「多舵轮-2腿检测」MovementTest 完全一致：首帧 / 跟丢回落都用 Conf.TwoLegGuessX
|
||||
// （方向与距离都由它一处决定），跟到后用上一帧中心闭环跟踪。
|
||||
// detectDistance 只用于计算编队对齐偏差/间距，不参与“去哪里找邻车”，避免猜测点偏离导致 ROI 框漏掉真实邻车。
|
||||
var nominal = new Vector2(Conf.TwoLegGuessX, 0f);
|
||||
|
||||
var now = DateTime.Now;
|
||||
// 闭环跟踪：1s 内检到过就用上一帧中心作猜测，使绿色 ROI 框收敛到真实邻车（与 MovementTest 行为一致）；
|
||||
// 跟丢超时则回落到编队标称位置，避免框漂走。
|
||||
var guess = (_neighborGuessValid && (now - _neighborGuessTime).TotalSeconds < 1) ? _neighborGuess : nominal;
|
||||
var seg = TwoLegDetect.Detect(Conf.TwoLegLidarName, guess.X, TwoLegDetect.SetFilters(guess.X, guess.Y));
|
||||
|
||||
Vector2 avgSrc, avgDst;
|
||||
lock (_neighborDetectLock)
|
||||
{
|
||||
if (seg != null)
|
||||
{
|
||||
_neighborDetects.Add((now, seg.Src, seg.Dst));
|
||||
_neighborGuess = (seg.Src + seg.Dst) / 2f;
|
||||
_neighborGuessValid = true;
|
||||
_neighborGuessTime = now;
|
||||
}
|
||||
_neighborDetects.RemoveAll(r => (now - r.Time).TotalSeconds > 1);
|
||||
|
||||
var cnt = _neighborDetects.Count;
|
||||
if (cnt == 0)
|
||||
{
|
||||
_neighborGuessValid = false;
|
||||
Hedingben.ToastText($"邻车未检到 (guess x:{guess.X:F0} y:{guess.Y:F0})", $"MultiVehicle{CarNum}-detect");
|
||||
return (false, 0f, 0f, 0f, 0f);
|
||||
}
|
||||
if (cnt > 10) _neighborDetects.RemoveRange(0, cnt - 10);
|
||||
|
||||
avgSrc = new Vector2(_neighborDetects.Average(r => r.Src.X), _neighborDetects.Average(r => r.Src.Y));
|
||||
avgDst = new Vector2(_neighborDetects.Average(r => r.Dst.X), _neighborDetects.Average(r => r.Dst.Y));
|
||||
}
|
||||
|
||||
// 两腿连线中点为邻车检测中心；连线的垂线方向即邻车朝向
|
||||
var dirVec = avgDst - avgSrc;
|
||||
var center = (avgSrc + avgDst) / 2f;
|
||||
var dir = (float)LessMath.RoundTh(Math.Atan2(dirVec.Y, dirVec.X) / Math.PI * 180 + 90);
|
||||
|
||||
// 从检测中心沿邻车朝向外推 detectDistance，得到“本车理应所在的参考点”（车体系）
|
||||
var target = LessMath.Transform2D(center, dir, detectDistance, 0);
|
||||
|
||||
var dx = target.X;
|
||||
var dy = target.Y;
|
||||
var dth = (float)LessMath.ThDiff(dir, 0);
|
||||
var neighborDist = center.Length();
|
||||
|
||||
_mvLastDetCenterX = center.X;
|
||||
_mvLastDetCenterY = center.Y;
|
||||
_mvLastDetDir = dir;
|
||||
_mvLastDetDist = neighborDist;
|
||||
|
||||
var painter = UI.GetPainter("MultiVehicleDetectBias", false);
|
||||
painter.Clear();
|
||||
painter.DrawLine(Color.DeepPink, center, target, width: 2);
|
||||
painter.DrawText(Color.Yellow, $"dx:{dx:F0} dy:{dy:F0} dth:{dth:F2}", center.X, center.Y);
|
||||
|
||||
Hedingben.ToastText(
|
||||
$"[联动检测] lidar:{Conf.TwoLegLidarName} guess x:{guess.X:F0} y:{guess.Y:F0} | " +
|
||||
$"中心 x:{center.X:F0} y:{center.Y:F0} dir:{dir:F1} dist:{neighborDist:F0} | 偏差 dx:{dx:F0} dy:{dy:F0} dth:{dth:F2}",
|
||||
$"MultiVehicle{CarNum}-detect");
|
||||
|
||||
return (true, dx, dy, dth, neighborDist);
|
||||
}
|
||||
|
||||
private void InitMultiVehicleCoordination()
|
||||
{
|
||||
if (_multiVehicleSyncInitialized) return;
|
||||
_multiVehicleSyncInitialized = true;
|
||||
|
||||
var hc = new HttpClient { Timeout = TimeSpan.FromSeconds(2) };
|
||||
|
||||
PicoHttpServer.AddGetHandler("/multi-vehicle-register", new { CarNum = 0, Info = "" }, query =>
|
||||
{
|
||||
try
|
||||
{
|
||||
var infoJson = Uri.UnescapeDataString(query.Info ?? "");
|
||||
var info = JsonConvert.DeserializeObject<VehicleSyncInfo>(infoJson)
|
||||
?? throw new Exception("VehicleSyncInfo is null");
|
||||
lock (MultiVehicleFleet)
|
||||
MultiVehicleFleet[query.CarNum] = info;
|
||||
return JsonConvert.SerializeObject(new { code = 200, message = "ok" });
|
||||
}
|
||||
catch (Exception e)
|
||||
{
|
||||
DLog.Log($"/multi-vehicle-register error: {e.FormatEx()}", "MultiVehicle");
|
||||
return JsonConvert.SerializeObject(new { code = 500, message = e.Message });
|
||||
}
|
||||
});
|
||||
|
||||
PicoHttpServer.AddGetHandler("/multi-vehicle-notify", new { Notification = "" }, query =>
|
||||
{
|
||||
try
|
||||
{
|
||||
var notifyJson = Uri.UnescapeDataString(query.Notification ?? "");
|
||||
var notification = JsonConvert.DeserializeObject<VehicleSyncNotification>(notifyJson)
|
||||
?? throw new Exception("notification is null");
|
||||
MultiVehicleAligned = notification.Aligned;
|
||||
lock (MultiVehicleFleet)
|
||||
MultiVehicleFleet = notification.Fleet ?? new Dictionary<int, VehicleSyncInfo>();
|
||||
lock (_multiVehicleNotificationLock)
|
||||
{
|
||||
MultiVehicleNotification = notification;
|
||||
_multiVehicleLastNotifyTime = DateTime.Now;
|
||||
}
|
||||
return JsonConvert.SerializeObject(new { code = 200, message = "ok" });
|
||||
}
|
||||
catch (Exception e)
|
||||
{
|
||||
DLog.Log($"/multi-vehicle-notify error: {e.FormatEx()}", "MultiVehicle");
|
||||
return JsonConvert.SerializeObject(new { code = 500, message = e.Message });
|
||||
}
|
||||
});
|
||||
|
||||
new Thread(() => MultiVehicleLoop(hc)) { Name = "MultiVehicle", IsBackground = true }.Start();
|
||||
}
|
||||
|
||||
private void MultiVehicleLoop(HttpClient hc)
|
||||
{
|
||||
var carLength = CarLength;
|
||||
var carWidth = CarWidth;
|
||||
var contour = new List<Vector2>
|
||||
{
|
||||
new(carLength / 2f, carWidth / 2f),
|
||||
new(-carLength / 2f, carWidth / 2f),
|
||||
new(-carLength / 2f, -carWidth / 2f),
|
||||
new(carLength / 2f, -carWidth / 2f),
|
||||
};
|
||||
var chassis = (MultiWheelChassis)Chassis;
|
||||
var sendMotionPainter = UI.GetPainter("MultiWheelChassis-SendMotionVis", false);
|
||||
chassis.SendMotionVisualizer = new Visualizer
|
||||
{
|
||||
LineAction = (color, src, dst, startArrow, endArrow, width) =>
|
||||
sendMotionPainter.DrawLine(color, src, dst, startArrow, endArrow, width),
|
||||
TextAction = (color, str, pos) => sendMotionPainter.DrawText(color, str, pos),
|
||||
Clear = () => sendMotionPainter.Clear()
|
||||
};
|
||||
|
||||
while (true)
|
||||
{
|
||||
try
|
||||
{
|
||||
TickMultiVehicle(hc, chassis, contour, sendMotionPainter);
|
||||
}
|
||||
catch (Exception e)
|
||||
{
|
||||
DLog.Log($"MultiVehicle loop error: {e.FormatEx()}", "MultiVehicle");
|
||||
}
|
||||
|
||||
Thread.Sleep(Conf.MultiVehicleSyncInterval);
|
||||
}
|
||||
}
|
||||
|
||||
private void TickMultiVehicle(HttpClient hc, MultiWheelChassis chassis, List<Vector2> contour, Painter sendMotionPainter)
|
||||
{
|
||||
var isMaster = IsMultiVehicleMaster();
|
||||
var autoEnabled = false;
|
||||
var manualEnabled = MultiVehicleManualEnabled;
|
||||
|
||||
if (isMaster)
|
||||
autoEnabled = MultiVehicleAutoEnabled;
|
||||
else
|
||||
{
|
||||
// 从车：自动/手动联动均可由主车广播解锁（无需各自再拨开关）。
|
||||
// 仅在 notification 新鲜时认账，主车停发后超时即自动停车，避免用旧指令跑飞。
|
||||
lock (_multiVehicleNotificationLock)
|
||||
{
|
||||
var fresh = (DateTime.Now - _multiVehicleLastNotifyTime).TotalMilliseconds <
|
||||
Math.Max(300, Conf.MultiVehicleSyncInterval * 5);
|
||||
if (fresh && MultiVehicleNotification != null)
|
||||
{
|
||||
autoEnabled = MultiVehicleNotification.AutoEnabled;
|
||||
manualEnabled = manualEnabled || MultiVehicleNotification.ManualEnabled;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 入口诊断（节流 ~300ms）：记录从 Medulla 收到的原始 IO 值与门控判定，
|
||||
// 用于确认遥控指令是否真的传到了 Clumsy，以及为何提前 return。
|
||||
if ((DateTime.Now - _mvDiskLastLog).TotalMilliseconds >= 300)
|
||||
{
|
||||
_mvDiskLastLog = DateTime.Now;
|
||||
int fleetCnt;
|
||||
lock (MultiVehicleFleet) fleetCnt = MultiVehicleFleet.Count;
|
||||
var notifFresh = (DateTime.Now - _multiVehicleLastNotifyTime).TotalMilliseconds <
|
||||
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} " +
|
||||
$"| gate: manualEnabled={manualEnabled} autoEnabled={autoEnabled} notifFresh={notifFresh} " +
|
||||
$"notifManualEn={(MultiVehicleNotification != null ? MultiVehicleNotification.ManualEnabled.ToString() : "null")} " +
|
||||
$"fleetCnt={fleetCnt}/{Conf.MultiVehicleFleetNum} useDetect={Conf.MultiVehicleUseDetect} pos={Conf.PosAvailable}");
|
||||
}
|
||||
|
||||
if (!manualEnabled && !autoEnabled)
|
||||
{
|
||||
UI.GetPainter("MultiVehicleFleet-vis", false).Clear();
|
||||
sendMotionPainter.Clear();
|
||||
lock (MultiVehicleFleet)
|
||||
MultiVehicleFleet.Clear();
|
||||
MultiVehicleAutoEnabled = false;
|
||||
_multiVehicleAccumulateTh = 0f;
|
||||
_multiVehicleLastThTime = DateTime.Now;
|
||||
|
||||
if (!isMaster)
|
||||
FireAndForgetRegister(hc, BuildSelfInfo(false, false, 0, 0, 0, 0, 0, 0, false));
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
if (isMaster && autoEnabled && !MultiVehicleManualEnabled && !FleetHasPosAvailable())
|
||||
{
|
||||
MultiVehicleAutoEnabled = false;
|
||||
Hedingben.ToastText("自动多车联动需要至少一台车有 Detour 定位", "MultiVehicle-auto-gate");
|
||||
return;
|
||||
}
|
||||
|
||||
var posAvailable = Conf.PosAvailable;
|
||||
float selfX = 0, selfY = 0, selfTh = 0;
|
||||
if (posAvailable)
|
||||
{
|
||||
var carPos = DetourInterface.getCartLocation();
|
||||
selfX = (float)carPos.x;
|
||||
selfY = (float)carPos.y;
|
||||
selfTh = (float)carPos.th;
|
||||
}
|
||||
|
||||
var syncTh = Conf.TestCarSyncTh;
|
||||
var syncDistance = Conf.TestCarSyncDistance;
|
||||
var deltaDetectCenter = Conf.DeltaDetectCenter;
|
||||
float fleetVx = 0, fleetFrontTh = 0, fleetRearTh = 0;
|
||||
|
||||
CenterX = CenterY = CenterTh = 0;
|
||||
|
||||
if (isMaster)
|
||||
{
|
||||
if (MultiVehicleManualEnabled)
|
||||
{
|
||||
syncTh = 0;
|
||||
fleetVx = MultiVehicleManualVx * Conf.ManualCarSyncVxFac;
|
||||
var targetTh = MultiVehicleManualVth * Conf.ManualCarSyncVthFac;
|
||||
var now = DateTime.Now;
|
||||
var dt = (float)Math.Min(0.2, Math.Max(0, (now - _multiVehicleLastThTime).TotalSeconds));
|
||||
_multiVehicleLastThTime = now;
|
||||
var sign = Math.Sign(targetTh - _multiVehicleAccumulateTh);
|
||||
_multiVehicleAccumulateTh += sign * Math.Min(Math.Abs(targetTh - _multiVehicleAccumulateTh),
|
||||
Conf.SyncThAccPerSec * dt);
|
||||
fleetFrontTh = _multiVehicleAccumulateTh;
|
||||
fleetRearTh = -fleetFrontTh;
|
||||
}
|
||||
else
|
||||
{
|
||||
fleetVx = MultiVehicleAutoVx;
|
||||
fleetFrontTh = MultiVehicleAutoFrontTh;
|
||||
fleetRearTh = MultiVehicleAutoRearTh;
|
||||
}
|
||||
}
|
||||
else if (MultiVehicleNotification != null)
|
||||
{
|
||||
VehicleSyncNotification notification;
|
||||
lock (_multiVehicleNotificationLock)
|
||||
notification = MultiVehicleNotification;
|
||||
|
||||
syncTh = notification.SyncTh;
|
||||
syncDistance = notification.SyncDistance;
|
||||
deltaDetectCenter = notification.DeltaDetectCenter;
|
||||
CenterX = notification.CenterX;
|
||||
CenterY = notification.CenterY;
|
||||
CenterTh = notification.CenterTh;
|
||||
posAvailable = notification.PosAvailable;
|
||||
MultiVehicleAutoEnabled = notification.AutoEnabled;
|
||||
fleetVx = notification.FleetVx;
|
||||
fleetFrontTh = notification.FleetFrontTh;
|
||||
fleetRearTh = notification.FleetRearTh;
|
||||
}
|
||||
|
||||
var (layoutX, layoutY, layoutTh) = GetLayoutPose(syncTh, syncDistance);
|
||||
|
||||
if (isMaster)
|
||||
{
|
||||
lock (MultiVehicleFleet)
|
||||
MultiVehicleFleet[CarNum] = BuildSelfInfo(true, posAvailable, selfX, selfY, selfTh,
|
||||
layoutX, layoutY, layoutTh, true);
|
||||
|
||||
if (TryInferFleetCenter(out var cx, out var cy, out var cth))
|
||||
{
|
||||
CenterX = cx;
|
||||
CenterY = cy;
|
||||
CenterTh = cth;
|
||||
}
|
||||
}
|
||||
|
||||
var selfAligned = !Conf.MultiVehicleUseDetect;
|
||||
var detectValid = false;
|
||||
float detectDx = 0, detectDy = 0, detectDth = 0;
|
||||
var currentSpacing = float.NaN;
|
||||
if (Conf.MultiVehicleUseDetect)
|
||||
{
|
||||
var (valid, dx, dy, dth, neighborDist) = DetectNeighborBias(syncDistance - deltaDetectCenter);
|
||||
detectValid = valid;
|
||||
if (valid)
|
||||
{
|
||||
detectDx = dx;
|
||||
detectDy = dy;
|
||||
detectDth = dth;
|
||||
// 检测中心到本车原点距离 + 检测中心偏移 = 两车参考点当前间距
|
||||
currentSpacing = neighborDist + deltaDetectCenter;
|
||||
selfAligned = Math.Abs(dx) < Conf.SingleCarSyncPrecisionXy &&
|
||||
Math.Abs(dy) < Conf.SingleCarSyncPrecisionXy &&
|
||||
Math.Abs(dth) < Conf.SingleCarSyncPrecisionTh;
|
||||
}
|
||||
else selfAligned = false;
|
||||
}
|
||||
|
||||
// 检测不可用（关闭/跟丢）时回落到 SLAM 世界坐标计算间距（两台车都需有定位）
|
||||
if (float.IsNaN(currentSpacing) && posAvailable)
|
||||
currentSpacing = TryGetSlamSpacing(selfX, selfY);
|
||||
|
||||
if (float.IsNaN(currentSpacing))
|
||||
Hedingben.ToastText($"间距 当前:N/A 目标:{syncDistance:F0}mm", $"MultiVehicle{CarNum}-spacing");
|
||||
else
|
||||
Hedingben.ToastText(
|
||||
$"间距 当前:{currentSpacing:F0}mm 目标:{syncDistance:F0}mm 差:{currentSpacing - syncDistance:F0}mm",
|
||||
$"MultiVehicle{CarNum}-spacing");
|
||||
|
||||
lock (MultiVehicleFleet)
|
||||
{
|
||||
MultiVehicleAligned = MultiVehicleFleet.Count == Conf.MultiVehicleFleetNum &&
|
||||
MultiVehicleFleet.Values.All(v => v.Aligned);
|
||||
}
|
||||
|
||||
// 安全门：开启互识别时，本车或任一其它车检测不到邻车则整队停车（速度置零）。
|
||||
// ownDetectOk 是本轮新鲜值；其它车的 DetectOk 来自其上报/主车下发（滑动窗口已给 1s 去抖）。
|
||||
var ownDetectOk = !Conf.MultiVehicleUseDetect || detectValid;
|
||||
bool othersDetectOk;
|
||||
lock (MultiVehicleFleet)
|
||||
othersDetectOk = MultiVehicleFleet.Where(kv => kv.Key != CarNum).All(kv => kv.Value.DetectOk);
|
||||
var canMove = !Conf.MultiVehicleUseDetect || (ownDetectOk && othersDetectOk);
|
||||
if (!canMove)
|
||||
{
|
||||
fleetVx = 0;
|
||||
fleetFrontTh = 0;
|
||||
fleetRearTh = 0;
|
||||
Hedingben.ToastText(
|
||||
$"车队停车：{(!ownDetectOk ? "本车" : "其它车")}2腿检测丢失（速度已置零）",
|
||||
$"MultiVehicle{CarNum}-stop");
|
||||
}
|
||||
else
|
||||
{
|
||||
Hedingben.ToastText("车队检测正常", $"MultiVehicle{CarNum}-stop");
|
||||
}
|
||||
|
||||
VisualizeFleet(contour, layoutX, layoutY, layoutTh);
|
||||
|
||||
var fleetReady = false;
|
||||
var fleetCount = 0;
|
||||
lock (MultiVehicleFleet)
|
||||
{
|
||||
fleetCount = MultiVehicleFleet.Count;
|
||||
fleetReady = fleetCount == Conf.MultiVehicleFleetNum;
|
||||
}
|
||||
|
||||
// 补偿量提到块外，便于诊断日志统一记录三类来源（检测/SLAM）的贡献。
|
||||
float xDetectCompensate = 0, yDetectCompensate = 0, thDetectCompensate = 0;
|
||||
float xPosCompensate = 0, yPosCompensate = 0, thPosCompensate = 0;
|
||||
float posBiasX = 0, posBiasY = 0, posBiasTh = 0;
|
||||
|
||||
if (fleetReady)
|
||||
{
|
||||
chassis.SetOriginBias(layoutX, layoutY, layoutTh);
|
||||
chassis.ControlPointRadius = syncDistance / 2f;
|
||||
|
||||
if (canMove && Conf.MultiVehicleUseDetect)
|
||||
{
|
||||
xDetectCompensate = Math.Abs(detectDx) > Conf.SingleCarSyncPrecisionXy
|
||||
? detectDx * Conf.MultiVehicleDetectBiasXFac : 0;
|
||||
yDetectCompensate = Math.Abs(detectDy) > Conf.SingleCarSyncPrecisionXy
|
||||
? detectDy * Conf.MultiVehicleDetectBiasYFac : 0;
|
||||
thDetectCompensate = Math.Abs(detectDth) > Conf.SingleCarSyncPrecisionTh
|
||||
? detectDth * Conf.MultiVehicleDetectBiasThFac : 0;
|
||||
|
||||
xDetectCompensate = ClampBias(xDetectCompensate, Conf.MultiVehicleDetectBiasXThreshold);
|
||||
yDetectCompensate = ClampBias(yDetectCompensate, Conf.MultiVehicleDetectBiasYThreshold);
|
||||
thDetectCompensate = ClampBias(thDetectCompensate, Conf.MultiVehicleDetectBiasThThreshold);
|
||||
}
|
||||
|
||||
if (canMove && posAvailable && TryInferFleetCenter(out _, out _, out _))
|
||||
{
|
||||
var supposedPos = LessMath.Transform2D(
|
||||
Tuple.Create(CenterX, CenterY, CenterTh),
|
||||
Tuple.Create(layoutX, layoutY, layoutTh));
|
||||
var posBias = LessMath.SolveTransform2D(Tuple.Create(selfX, selfY, selfTh), supposedPos);
|
||||
posBiasX = (float)posBias.Item1;
|
||||
posBiasY = (float)posBias.Item2;
|
||||
posBiasTh = (float)posBias.Item3;
|
||||
|
||||
xPosCompensate = Math.Abs(posBias.Item1) > Conf.SingleCarSyncPrecisionXy
|
||||
? ClampBias(posBias.Item1 * Conf.MultiVehiclePosBiasXFac, Conf.MultiVehiclePosBiasXThreshold)
|
||||
: 0;
|
||||
yPosCompensate = Math.Abs(posBias.Item2) > Conf.SingleCarSyncPrecisionXy
|
||||
? ClampBias(posBias.Item2 * Conf.MultiVehiclePosBiasYFac, Conf.MultiVehiclePosBiasYThreshold)
|
||||
: 0;
|
||||
thPosCompensate = Math.Abs(posBias.Item3) > Conf.SingleCarSyncPrecisionTh
|
||||
? ClampBias(posBias.Item3 * Conf.MultiVehiclePosBiasThFac, Conf.MultiVehiclePosBiasThThreshold)
|
||||
: 0;
|
||||
}
|
||||
|
||||
sendMotionPainter.Clear();
|
||||
chassis.SendMotion(fleetVx, fleetFrontTh, fleetRearTh, localControlRadius: 510,
|
||||
localCompensateX: xDetectCompensate + xPosCompensate,
|
||||
localCompensateY: yDetectCompensate + yPosCompensate,
|
||||
localCompensateTh: thDetectCompensate + thPosCompensate);
|
||||
}
|
||||
|
||||
// === 诊断日志（节流 ~300ms）===
|
||||
// 用于定位“剧烈运动”来源：BASE=遥控基础速度；DETECT=2腿检测补偿；POS=SLAM编队补偿；SEND=最终下发。
|
||||
// 若 BASE≈0 但 SEND 持续非零，说明是补偿在驱动；再看 DETECT/POS 哪一路的 comp 持续非零即定位到来源。
|
||||
// 同时落 DLog(tag MultiVehicleDbg) 与屏幕 ToastText(tag MultiVehicle{CarNum}-dbg)，后者无需开启磁盘转储即可直接观察。
|
||||
if ((DateTime.Now - _mvDbgLastLog).TotalMilliseconds >= 300)
|
||||
{
|
||||
_mvDbgLastLog = DateTime.Now;
|
||||
var spacingStr = float.IsNaN(currentSpacing) ? "NaN" : currentSpacing.ToString("F0");
|
||||
var cx = xDetectCompensate + xPosCompensate;
|
||||
var cy = yDetectCompensate + yPosCompensate;
|
||||
var cth = thDetectCompensate + thPosCompensate;
|
||||
|
||||
var dbg =
|
||||
$"car{CarNum} master:{isMaster} manual:{manualEnabled} auto:{autoEnabled} pos:{posAvailable} " +
|
||||
$"ready:{fleetReady}({fleetCount}/{Conf.MultiVehicleFleetNum}) canMove:{canMove} useDetect:{Conf.MultiVehicleUseDetect} " +
|
||||
$"| BASE vx:{fleetVx:F3} fTh:{fleetFrontTh:F2} rTh:{fleetRearTh:F2} " +
|
||||
$"| DETECT valid:{detectValid} center({_mvLastDetCenterX:F0},{_mvLastDetCenterY:F0}) dir:{_mvLastDetDir:F1} ndist:{_mvLastDetDist:F0} " +
|
||||
$"dx:{detectDx:F0} dy:{detectDy:F0} dth:{detectDth:F2} spacing:{spacingStr}/{syncDistance:F0} delta:{deltaDetectCenter:F0} guessX:{Conf.TwoLegGuessX:F0} " +
|
||||
$"-> comp x:{xDetectCompensate:F1} y:{yDetectCompensate:F1} th:{thDetectCompensate:F2} " +
|
||||
$"| POS self({selfX:F0},{selfY:F0},{selfTh:F1}) center({CenterX:F0},{CenterY:F0},{CenterTh:F1}) " +
|
||||
$"bias({posBiasX:F0},{posBiasY:F0},{posBiasTh:F1}) -> comp x:{xPosCompensate:F1} y:{yPosCompensate:F1} th:{thPosCompensate:F2} " +
|
||||
$"| LAYOUT({layoutX:F0},{layoutY:F0},{layoutTh:F0}) R:{syncDistance / 2f:F0} " +
|
||||
$"| SEND vx:{fleetVx:F3} fTh:{fleetFrontTh:F2} rTh:{fleetRearTh:F2} cx:{cx:F1} cy:{cy:F1} cth:{cth:F2}";
|
||||
|
||||
DLog.Log(dbg, "MultiVehicleDbg");
|
||||
FleetDiag(dbg);
|
||||
|
||||
// 屏幕分两行显示，便于直接观察（无需开启 DLog 磁盘转储）
|
||||
Hedingben.ToastText(
|
||||
$"BASE vx{fleetVx:F3} fTh{fleetFrontTh:F1} | SEND vx{fleetVx:F3} c({cx:F0},{cy:F0},{cth:F1}) " +
|
||||
$"| ready{fleetReady} canMove{canMove}",
|
||||
$"MultiVehicle{CarNum}-dbg");
|
||||
Hedingben.ToastText(
|
||||
$"DET v{detectValid} dx{detectDx:F0} dy{detectDy:F0} dth{detectDth:F1} cmp({xDetectCompensate:F0},{yDetectCompensate:F0},{thDetectCompensate:F1}) " +
|
||||
$"| POS bias({posBiasX:F0},{posBiasY:F0},{posBiasTh:F1}) cmp({xPosCompensate:F0},{yPosCompensate:F0},{thPosCompensate:F1})",
|
||||
$"MultiVehicle{CarNum}-dbg2");
|
||||
}
|
||||
|
||||
if (isMaster)
|
||||
{
|
||||
lock (MultiVehicleFleet)
|
||||
{
|
||||
MultiVehicleFleet[CarNum] = BuildSelfInfo(true, posAvailable, selfX, selfY, selfTh,
|
||||
layoutX, layoutY, layoutTh, selfAligned, ownDetectOk);
|
||||
}
|
||||
|
||||
if (fleetReady)
|
||||
{
|
||||
VehicleSyncNotification notification;
|
||||
lock (MultiVehicleFleet)
|
||||
{
|
||||
notification = new VehicleSyncNotification
|
||||
{
|
||||
PosAvailable = posAvailable,
|
||||
CenterX = CenterX,
|
||||
CenterY = CenterY,
|
||||
CenterTh = CenterTh,
|
||||
Aligned = MultiVehicleAligned,
|
||||
Fleet = new Dictionary<int, VehicleSyncInfo>(MultiVehicleFleet),
|
||||
FleetVx = fleetVx,
|
||||
FleetFrontTh = fleetFrontTh,
|
||||
FleetRearTh = fleetRearTh,
|
||||
AutoEnabled = MultiVehicleAutoEnabled,
|
||||
ManualEnabled = MultiVehicleManualEnabled,
|
||||
SyncTh = syncTh,
|
||||
SyncDistance = syncDistance,
|
||||
DeltaDetectCenter = deltaDetectCenter
|
||||
};
|
||||
}
|
||||
|
||||
foreach (var kv in notification.Fleet)
|
||||
{
|
||||
var ip = kv.Value.Ip;
|
||||
var port = kv.Value.Port > 0 ? kv.Value.Port : WebAPI.port;
|
||||
if (string.IsNullOrWhiteSpace(ip) || kv.Key == CarNum)
|
||||
continue;
|
||||
FireAndForgetNotify(hc, ip, port, notification);
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
FireAndForgetRegister(hc, BuildSelfInfo(false, posAvailable, selfX, selfY, selfTh,
|
||||
layoutX, layoutY, layoutTh, selfAligned, ownDetectOk));
|
||||
}
|
||||
}
|
||||
|
||||
private bool IsMultiVehicleMaster() => Conf.MultiVehicleMasterEndpoint == "/";
|
||||
|
||||
private bool FleetHasPosAvailable()
|
||||
{
|
||||
lock (MultiVehicleFleet)
|
||||
return MultiVehicleFleet.Values.Any(v => v.PosAvailable);
|
||||
}
|
||||
|
||||
// 基于 SLAM 世界坐标的最近邻车间距（mm）；无可用定位邻车时返回 NaN。
|
||||
private float TryGetSlamSpacing(float selfX, float selfY)
|
||||
{
|
||||
List<VehicleSyncInfo> others;
|
||||
lock (MultiVehicleFleet)
|
||||
others = MultiVehicleFleet
|
||||
.Where(kv => kv.Key != CarNum && kv.Value.PosAvailable)
|
||||
.Select(kv => kv.Value).ToList();
|
||||
|
||||
if (others.Count == 0) return float.NaN;
|
||||
return others.Min(o => (float)Math.Sqrt((o.X - selfX) * (o.X - selfX) + (o.Y - selfY) * (o.Y - selfY)));
|
||||
}
|
||||
|
||||
private bool TryInferFleetCenter(out float centerX, out float centerY, out float centerTh)
|
||||
{
|
||||
centerX = centerY = centerTh = 0;
|
||||
List<VehicleSyncInfo> positioned;
|
||||
lock (MultiVehicleFleet)
|
||||
positioned = MultiVehicleFleet.Values.Where(v => v.PosAvailable).ToList();
|
||||
|
||||
if (positioned.Count == 0) return false;
|
||||
|
||||
var centers = positioned.Select(InferFleetCenterFromCar).ToList();
|
||||
centerX = centers.Average(c => c.Item1);
|
||||
centerY = centers.Average(c => c.Item2);
|
||||
var avgSin = centers.Average(c => Math.Sin(c.Item3 * Math.PI / 180.0));
|
||||
var avgCos = centers.Average(c => Math.Cos(c.Item3 * Math.PI / 180.0));
|
||||
centerTh = (float)(Math.Atan2(avgSin, avgCos) * 180.0 / Math.PI);
|
||||
return true;
|
||||
}
|
||||
|
||||
private static (float, float, float) InferFleetCenterFromCar(VehicleSyncInfo car)
|
||||
{
|
||||
// 由 carWorld = Transform2D(center, layout) 反推 center = carWorld ∘ layout⁻¹。
|
||||
// 注意 layout⁻¹ 是真正的 SE(2) 逆，而非逐分量取负 (-layout)：
|
||||
// 当 layoutTh≠0（如从车 180°）时，逐分量取负会把平移方向算错，导致车队中心偏移 → 位姿补偿跑飞。
|
||||
var layout = Tuple.Create((double)car.LayoutX, (double)car.LayoutY, (double)car.LayoutTh);
|
||||
var layoutInv = LessMath.SolveTransform2D(layout, Tuple.Create(0.0, 0.0, 0.0));
|
||||
var center = LessMath.Transform2D(
|
||||
Tuple.Create((double)car.X, (double)car.Y, (double)car.Th),
|
||||
layoutInv);
|
||||
return ((float)center.Item1, (float)center.Item2, (float)center.Item3);
|
||||
}
|
||||
|
||||
private (float, float, float) GetLayoutPose(float syncTh, float syncDistance)
|
||||
{
|
||||
// 双车编队布局由 TestCarSyncDistance(=syncDistance) 与 TestCarSyncTh(=syncTh) 唯一确定：
|
||||
// 车体相对车队中心沿编队方向 ±syncDistance/2 对称分布，从车额外朝向翻转 180°。
|
||||
var rad = syncTh / 180f * Math.PI;
|
||||
var sign = CarNum == 1 ? 1f : -1f;
|
||||
var xx = (float)(Math.Cos(rad) * syncDistance / 2 * sign);
|
||||
var yy = (float)(Math.Sin(rad) * syncDistance / 2 * sign);
|
||||
return (xx, yy, syncTh + (CarNum == 1 ? 0 : 180));
|
||||
}
|
||||
|
||||
private VehicleSyncInfo BuildSelfInfo(bool master, bool posAvailable, float x, float y, float th,
|
||||
float layoutX, float layoutY, float layoutTh, bool aligned, bool detectOk = true)
|
||||
{
|
||||
return new VehicleSyncInfo
|
||||
{
|
||||
Master = master,
|
||||
Ip = Conf.SimpleIp,
|
||||
Port = WebAPI.port,
|
||||
PosAvailable = posAvailable,
|
||||
X = x,
|
||||
Y = y,
|
||||
Th = th,
|
||||
LayoutX = layoutX,
|
||||
LayoutY = layoutY,
|
||||
LayoutTh = layoutTh,
|
||||
Aligned = aligned,
|
||||
DetectOk = detectOk
|
||||
};
|
||||
}
|
||||
|
||||
private void VisualizeFleet(List<Vector2> contour, float egoLayoutX, float egoLayoutY, float egoLayoutTh)
|
||||
{
|
||||
// 画在本车车体系：global=false，渲染时由 ClumsyLite 叠加本车真实位姿。
|
||||
// 每个成员按“相对本车 layout 的位姿”绘制（memberInEgo = egoLayout⁻¹ ∘ memberLayout），
|
||||
// 这样手动模式无 SLAM 定位也能正确显示编队相对关系；避免之前用车队系 layout 坐标叠加本车位姿造成的整体平移。
|
||||
var fleetPainter = UI.GetPainter("MultiVehicleFleet-vis", false);
|
||||
Dictionary<int, VehicleSyncInfo> fleet;
|
||||
lock (MultiVehicleFleet)
|
||||
fleet = new Dictionary<int, VehicleSyncInfo>(MultiVehicleFleet);
|
||||
|
||||
var egoLayout = Tuple.Create((double)egoLayoutX, (double)egoLayoutY, (double)egoLayoutTh);
|
||||
|
||||
Tuple<double, double, double> MemberInEgo(float mx, float my, float mth)
|
||||
=> LessMath.SolveTransform2D(egoLayout, Tuple.Create((double)mx, (double)my, (double)mth));
|
||||
|
||||
void DrawContour(Color color, Tuple<double, double, double> rel)
|
||||
{
|
||||
var transformed = contour
|
||||
.Select(pp => LessMath.Transform2D((float)rel.Item1, (float)rel.Item2, (float)rel.Item3, pp))
|
||||
.ToList();
|
||||
for (var i = 0; i < 4; ++i)
|
||||
fleetPainter.DrawLine(color, transformed[i], transformed[(i + 1) % 4]);
|
||||
// 对角线便于辨认朝向
|
||||
fleetPainter.DrawLine(color, transformed[0], transformed[2]);
|
||||
fleetPainter.DrawLine(color, transformed[1], transformed[3]);
|
||||
}
|
||||
|
||||
fleetPainter.Clear();
|
||||
foreach (var kvp in fleet)
|
||||
{
|
||||
var color = kvp.Value.Master ? Color.Red : Color.Orange;
|
||||
var rel = MemberInEgo(kvp.Value.LayoutX, kvp.Value.LayoutY, kvp.Value.LayoutTh);
|
||||
DrawContour(color, rel);
|
||||
fleetPainter.DrawText(color, $"{kvp.Key}", new Vector((float)rel.Item1, (float)rel.Item2));
|
||||
}
|
||||
}
|
||||
|
||||
private void FireAndForgetRegister(HttpClient hc, VehicleSyncInfo info)
|
||||
{
|
||||
ParseMasterEndpoint(out var masterIp, out var masterPort);
|
||||
var infoJson = JsonConvert.SerializeObject(info);
|
||||
var url =
|
||||
$"http://{masterIp}:{masterPort}/multi-vehicle-register?CarNum={CarNum}&Info={Uri.EscapeDataString(infoJson)}";
|
||||
_ = hc.GetStringAsync(url).ContinueWith(t =>
|
||||
{
|
||||
if (t.IsFaulted)
|
||||
DLog.Log($"register failed: {t.Exception?.GetBaseException().Message}", "MultiVehicle");
|
||||
}, TaskScheduler.Default);
|
||||
}
|
||||
|
||||
private static void FireAndForgetNotify(HttpClient hc, string ip, int port, VehicleSyncNotification notification)
|
||||
{
|
||||
var notifyJson = JsonConvert.SerializeObject(notification);
|
||||
var url = $"http://{ip}:{port}/multi-vehicle-notify?Notification={Uri.EscapeDataString(notifyJson)}";
|
||||
_ = hc.GetStringAsync(url).ContinueWith(t =>
|
||||
{
|
||||
if (t.IsFaulted)
|
||||
DLog.Log($"notify {ip}:{port} failed: {t.Exception?.GetBaseException().Message}", "MultiVehicle");
|
||||
}, TaskScheduler.Default);
|
||||
}
|
||||
|
||||
private void ParseMasterEndpoint(out string ip, out int port)
|
||||
{
|
||||
ip = "127.0.0.1";
|
||||
port = 8008;
|
||||
var endpoint = Conf.MultiVehicleMasterEndpoint ?? "/";
|
||||
if (endpoint == "/") return;
|
||||
var parts = endpoint.Split(':');
|
||||
if (parts.Length >= 1 && !string.IsNullOrWhiteSpace(parts[0]))
|
||||
ip = parts[0];
|
||||
if (parts.Length >= 2 && int.TryParse(parts[1], out var p))
|
||||
port = p;
|
||||
}
|
||||
|
||||
private static float ClampBias(float value, float threshold)
|
||||
=> Math.Sign(value) * Math.Min(Math.Abs(value), threshold);
|
||||
|
||||
#endregion
|
||||
}
|
||||
@@ -0,0 +1,81 @@
|
||||
using System;
|
||||
using System.Net.Http;
|
||||
using System.Text;
|
||||
using Newtonsoft.Json;
|
||||
using Newtonsoft.Json.Linq;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
/// <summary>
|
||||
/// Playground 仿真器 HTTP Web API 轻量客户端：查询小车位姿、瞬移小车。
|
||||
/// 服务端实现见 Playground/Web/PlaygroundWebApi.cs，默认监听 http://localhost:18090。
|
||||
/// 坐标单位 mm，朝向 yawDeg 单位为度，世界坐标系与场景 JSON 一致。
|
||||
/// </summary>
|
||||
public static class PlaygroundWebApi
|
||||
{
|
||||
// 禁用系统代理：本机 Playground 走 localhost，若经系统代理(如 127.0.0.1:7890)会连接失败。
|
||||
private static readonly HttpClient Http = new HttpClient(new HttpClientHandler { UseProxy = false })
|
||||
{
|
||||
Timeout = TimeSpan.FromSeconds(3)
|
||||
};
|
||||
|
||||
public struct Pose
|
||||
{
|
||||
public float X;
|
||||
public float Y;
|
||||
public float YawDeg;
|
||||
}
|
||||
|
||||
/// <summary>查询单台小车的世界位姿。GET /api/robots/{name}。</summary>
|
||||
public static Pose GetPose(string baseUrl, string robotName)
|
||||
{
|
||||
var url = $"{baseUrl.TrimEnd('/')}/api/robots/{Uri.EscapeDataString(robotName)}";
|
||||
var json = Http.GetStringAsync(url).GetAwaiter().GetResult();
|
||||
var o = JObject.Parse(json);
|
||||
return new Pose
|
||||
{
|
||||
X = o.Value<float>("x"),
|
||||
Y = o.Value<float>("y"),
|
||||
YawDeg = o.Value<float>("yawDeg")
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>将小车瞬移到目标世界位姿。POST /api/robots/{name}/move。</summary>
|
||||
public static void Move(string baseUrl, string robotName, float x, float y, float yawDeg)
|
||||
{
|
||||
var url = $"{baseUrl.TrimEnd('/')}/api/robots/{Uri.EscapeDataString(robotName)}/move";
|
||||
var body = JsonConvert.SerializeObject(new { x, y, yaw = yawDeg, stop = true });
|
||||
using var content = new StringContent(body, Encoding.UTF8, "application/json");
|
||||
var resp = Http.PostAsync(url, content).GetAwaiter().GetResult();
|
||||
resp.EnsureSuccessStatusCode();
|
||||
}
|
||||
|
||||
/// <summary>查询车辆运动是否启用（暂停时为 false）。GET /api/motion。</summary>
|
||||
public static bool MotionEnabled(string baseUrl)
|
||||
{
|
||||
var url = $"{baseUrl.TrimEnd('/')}/api/motion";
|
||||
var json = Http.GetStringAsync(url).GetAwaiter().GetResult();
|
||||
return JObject.Parse(json).Value<bool>("motionEnabled");
|
||||
}
|
||||
|
||||
/// <summary>恢复车辆运动。POST /api/motion/resume。</summary>
|
||||
public static void ResumeMotion(string baseUrl)
|
||||
{
|
||||
var url = $"{baseUrl.TrimEnd('/')}/api/motion/resume";
|
||||
var resp = Http.PostAsync(url, null).GetAwaiter().GetResult();
|
||||
resp.EnsureSuccessStatusCode();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 暂停车辆运动（仅冻结运动，不停止仿真；传感器继续扫描）。POST /api/motion/pause。
|
||||
/// feedback: "zero"(默认,反馈归零) / "none"(不上报) / "hold"(保留暂停瞬间值)。
|
||||
/// </summary>
|
||||
public static void PauseMotion(string baseUrl, string feedback = "zero")
|
||||
{
|
||||
var url = $"{baseUrl.TrimEnd('/')}/api/motion/pause";
|
||||
var body = JsonConvert.SerializeObject(new { feedback });
|
||||
using var content = new StringContent(body, Encoding.UTF8, "application/json");
|
||||
var resp = Http.PostAsync(url, content).GetAwaiter().GetResult();
|
||||
resp.EnsureSuccessStatusCode();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,40 @@
|
||||
using System.Collections.Generic;
|
||||
using ClumsyCore;
|
||||
using Newtonsoft.Json;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
public class VehicleSyncInfo
|
||||
{
|
||||
[JsonProperty("Master")] public bool Master { get; set; }
|
||||
[JsonProperty("Ip")] public string Ip { get; set; } = "";
|
||||
[JsonProperty("Port")] public int Port { get; set; } = 8008;
|
||||
[JsonProperty("PosAvailable")] public bool PosAvailable { get; set; }
|
||||
[JsonProperty("X")] public float X { get; set; }
|
||||
[JsonProperty("Y")] public float Y { get; set; }
|
||||
[JsonProperty("Th")] public float Th { get; set; }
|
||||
[JsonProperty("LayoutX")] public float LayoutX { get; set; }
|
||||
[JsonProperty("LayoutY")] public float LayoutY { get; set; }
|
||||
[JsonProperty("LayoutTh")] public float LayoutTh { get; set; }
|
||||
[JsonProperty("Aligned")] public bool Aligned { get; set; }
|
||||
// 本车本轮是否成功识别到邻车（关闭互识别时恒为 true）。任一车为 false 则整队停车。
|
||||
[JsonProperty("DetectOk")] public bool DetectOk { get; set; }
|
||||
}
|
||||
|
||||
public class VehicleSyncNotification
|
||||
{
|
||||
[JsonProperty("PosAvailable")] public bool PosAvailable { get; set; }
|
||||
[JsonProperty("CenterX")] public float CenterX { get; set; }
|
||||
[JsonProperty("CenterY")] public float CenterY { get; set; }
|
||||
[JsonProperty("CenterTh")] public float CenterTh { get; set; }
|
||||
[JsonProperty("Aligned")] public bool Aligned { get; set; }
|
||||
[JsonProperty("Fleet")] public Dictionary<int, VehicleSyncInfo> Fleet { get; set; } = new();
|
||||
[JsonProperty("FleetVx")] public float FleetVx { get; set; }
|
||||
[JsonProperty("FleetFrontTh")] public float FleetFrontTh { get; set; }
|
||||
[JsonProperty("FleetRearTh")] public float FleetRearTh { get; set; }
|
||||
[JsonProperty("AutoEnabled")] public bool AutoEnabled { get; set; }
|
||||
[JsonProperty("ManualEnabled")] public bool ManualEnabled { get; set; }
|
||||
[JsonProperty("SyncTh")] public float SyncTh { get; set; }
|
||||
[JsonProperty("SyncDistance")] public float SyncDistance { get; set; }
|
||||
[JsonProperty("DeltaDetectCenter")] public float DeltaDetectCenter { get; set; }
|
||||
}
|
||||
Reference in New Issue
Block a user