137 lines
5.1 KiB
C#
137 lines
5.1 KiB
C#
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
|
|
|
|
var result = detector.DetectWithGuess(
|
|
lidarName,
|
|
new LineSegment(new Vector2(guessX, 0), Vector2.Zero),
|
|
guessCoordinateSystem: CoordinateSystem.Car2D,
|
|
outCoordinateSystem: CoordinateSystem.Car2D,
|
|
filters);
|
|
return ApplyOutputBias(result);
|
|
}
|
|
|
|
private static LineSegment ApplyOutputBias(LineSegment result)
|
|
{
|
|
if (result == null) return null;
|
|
|
|
var conf = PilotDefinition.Conf;
|
|
if (Math.Abs(conf.TwoLegOutputBiasX) < 1e-6f && Math.Abs(conf.TwoLegOutputBiasY) < 1e-6f)
|
|
return result;
|
|
|
|
var bias = new Vector2(conf.TwoLegOutputBiasX, conf.TwoLegOutputBiasY);
|
|
return new LineSegment(result.Src + bias, result.Dst + bias);
|
|
}
|
|
|
|
/// <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))),
|
|
};
|
|
}
|
|
}
|