607 lines
24 KiB
C#
607 lines
24 KiB
C#
|
using ClumsyCore;
|
||
|
|
using ClumsyCore.DTools;
|
||
|
|
using ClumsyCore.Interfaces;
|
||
|
|
using ClumsyCore.Pilot;
|
||
|
|
using ClumsyCore.Utilities;
|
||
|
|
using ClumsyDance.ClumsyDance.Detectors;
|
||
|
|
using ClumsyDance.ClumsyWalk.Detectors;
|
||
|
|
using CommonUsage.Chassis;
|
||
|
|
using CommonUsage.Mathematics;
|
||
|
|
using FundamentalLib;
|
||
|
|
using MDCSToolBox.Clumsy.Calibration;
|
||
|
|
using MDCSToolBox.Clumsy.Tracks;
|
||
|
|
using MDCSToolBox.Commons.Controllers;
|
||
|
|
using System;
|
||
|
|
using System.Collections.Generic;
|
||
|
|
using System.Drawing;
|
||
|
|
using System.Linq;
|
||
|
|
using System.Numerics;
|
||
|
|
using System.Security.Cryptography;
|
||
|
|
using System.Threading;
|
||
|
|
using LineSegment = ClumsyCore.Utilities.LineSegment;
|
||
|
|
|
||
|
|
namespace MultiWheelC
|
||
|
|
{
|
||
|
|
[MovementTest(name = "轮胎检测")]
|
||
|
|
public class TireDetect : MovementTest
|
||
|
|
{
|
||
|
|
public override void TestStop()
|
||
|
|
{
|
||
|
|
_running = false;
|
||
|
|
}
|
||
|
|
|
||
|
|
public override void Test()
|
||
|
|
{
|
||
|
|
_painter = UI.GetPainter("TwoLegDetectTest", false);
|
||
|
|
_painter.Clear();
|
||
|
|
|
||
|
|
var frontlidar = UI.GetInput("1是用前雷达识别，2是用后雷达识别");
|
||
|
|
var result = int.Parse(frontlidar.ToString());
|
||
|
|
var lastDetectX = result == 1 ? PilotDefinition.Conf.TireFollowingStage1GuessX : -PilotDefinition.Conf.TireFollowingStage1GuessX;
|
||
|
|
var lastDetectY = 0f;
|
||
|
|
while (_running)
|
||
|
|
{
|
||
|
|
var ld = Detect(lastDetectX, SetFilters(lastDetectX, lastDetectY), result == 1 ? true : false);
|
||
|
|
if(ld == null)
|
||
|
|
{
|
||
|
|
//Console.WriteLine("ld == null");
|
||
|
|
continue;
|
||
|
|
}
|
||
|
|
_painter.Clear();
|
||
|
|
var center = (ld.Src + ld.Dst) / 2;
|
||
|
|
var distanceToCarOrigin = Vector2.Distance(Vector2.Zero, center);
|
||
|
|
var distanceLabelPos = center / 2;
|
||
|
|
_painter.DrawLine(Color.Cyan, Vector2.Zero, center, width: 2);
|
||
|
|
_painter.DrawText(Color.Yellow, $"{distanceToCarOrigin:F3}", distanceLabelPos.X, distanceLabelPos.Y);
|
||
|
|
lastDetectX = center.X;
|
||
|
|
lastDetectY = center.Y;
|
||
|
|
Thread.Sleep(100);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
public static LineSegment Detect(float guessX, List<DetectFilter> filters, bool frontlidar)
|
||
|
|
{
|
||
|
|
return new Lidar2dDetect2LegTray()
|
||
|
|
{
|
||
|
|
BlobDist = frontlidar ? PilotDefinition.Conf.TireFrontTwoLegBlobDist : PilotDefinition.Conf.TireBackTwoLegBlobDist,
|
||
|
|
BlobPtCount = frontlidar ? PilotDefinition.Conf.TireTwoLegBlobPtCount : PilotDefinition.Conf.TireTwoLegBlobPtCount,
|
||
|
|
BlobSize = frontlidar ? PilotDefinition.Conf.TireFrontTwoLegBlobSize : PilotDefinition.Conf.TireBackTwoLegBlobSize,
|
||
|
|
CenterChange = Tuple.Create(frontlidar ? PilotDefinition.Conf.TireFrontTwoLegCenterChangeX : PilotDefinition.Conf.TireBackTwoLegCenterChangeX, 0f, 0f),
|
||
|
|
LegWidth = PilotDefinition.Conf.TireTwoLegWidth,
|
||
|
|
LegWidthErr = frontlidar ? PilotDefinition.Conf.TireTwoLegWidthErr : PilotDefinition.Conf.TireTwoLegWidthErr,
|
||
|
|
Padding = frontlidar ? PilotDefinition.Conf.TireFrontPadding : PilotDefinition.Conf.TireBackPadding,
|
||
|
|
PillarFindingScope = frontlidar ? PilotDefinition.Conf.TireFrontTwoLegPillarFindingScope : PilotDefinition.Conf.TireBackTwoLegPillarFindingScope,
|
||
|
|
SgnDir = PilotDefinition.Conf.TwoLegSgnDir,
|
||
|
|
}.DetectWithGuess(frontlidar ? "frontlidar" : "leftlidar,rightlidar", new LineSegment(new Vector2(guessX, 0), Vector2.Zero),
|
||
|
|
guessCoordinateSystem: CoordinateSystem.Car2D, outCoordinateSystem: CoordinateSystem.Car2D, filters);
|
||
|
|
}
|
||
|
|
|
||
|
|
private List<DetectFilter> SetFilters(float guessCenterX, float guessCenterY)
|
||
|
|
{
|
||
|
|
var painter = UI.GetPainter("GeneralFollowing.SetFilters", false);
|
||
|
|
painter.Clear();
|
||
|
|
painter.Clear(3000);
|
||
|
|
|
||
|
|
var box = new Vector2[]
|
||
|
|
{
|
||
|
|
new (guessCenterX - PilotDefinition.Conf.TireFilterLength / 2, guessCenterY - PilotDefinition.Conf.TireFilterWidth / 2),
|
||
|
|
new (guessCenterX + PilotDefinition.Conf.TireFilterLength / 2, guessCenterY - PilotDefinition.Conf.TireFilterWidth / 2),
|
||
|
|
new (guessCenterX + PilotDefinition.Conf.TireFilterLength / 2, guessCenterY + PilotDefinition.Conf.TireFilterWidth / 2),
|
||
|
|
new (guessCenterX - PilotDefinition.Conf.TireFilterLength / 2, guessCenterY + PilotDefinition.Conf.TireFilterWidth / 2),
|
||
|
|
};
|
||
|
|
|
||
|
|
for (var i = 0; i < box.Length; ++i)
|
||
|
|
painter.DrawLine(Color.DarkOliveGreen, box[i], box[(i + 1) % 4]);
|
||
|
|
|
||
|
|
// PC filter in car coordinate frame
|
||
|
|
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))),
|
||
|
|
};
|
||
|
|
}
|
||
|
|
|
||
|
|
private Painter _painter;
|
||
|
|
private bool _running = true;
|
||
|
|
}
|
||
|
|
|
||
|
|
[MovementTest(name = "钻车测试")]
|
||
|
|
public class FollowTire : MovementTest
|
||
|
|
{
|
||
|
|
public override void TestStop()
|
||
|
|
{
|
||
|
|
_dt?.Stop();
|
||
|
|
}
|
||
|
|
|
||
|
|
public override void Test()
|
||
|
|
{
|
||
|
|
var front = UI.GetInput("1是用前雷达识别，2是用后雷达识别");
|
||
|
|
var result = int.Parse(front.ToString());
|
||
|
|
var lidarname = result == 1 ? "前雷达" : "后雷达";
|
||
|
|
DLog.Log($"开始钻车测试，用{lidarname}识别", "TireFollowing");
|
||
|
|
|
||
|
|
var following = new TireFollowing()
|
||
|
|
{
|
||
|
|
GetController = () => new ChassisController().Get(),
|
||
|
|
GuessRangeX = PilotDefinition.Conf.TireFilterLength / 2,
|
||
|
|
GuessRangeY = PilotDefinition.Conf.TireFilterWidth / 2,
|
||
|
|
detectors = new List<TireFollowing.DetectorDefinition>()
|
||
|
|
{
|
||
|
|
new TireFollowing.DetectorDefinition()
|
||
|
|
{
|
||
|
|
DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, result == 1 ? true : false),
|
||
|
|
StartGuessingX = result == 1 ? PilotDefinition.Conf.TireFollowingStage1GuessX : -PilotDefinition.Conf.TireFollowingStage1GuessX,
|
||
|
|
StartGuessingY = 0,
|
||
|
|
SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
|
||
|
|
FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
|
||
|
|
PathTransformation = new Tuple<float, float, float>(
|
||
|
|
result == 1 ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationX : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationX,
|
||
|
|
result == 1 ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationY : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
|
||
|
|
0)
|
||
|
|
},
|
||
|
|
new TireFollowing.DetectorDefinition()
|
||
|
|
{
|
||
|
|
DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, result == 1 ? true : false),
|
||
|
|
StartGuessingX = result == 1 ? PilotDefinition.Conf.TireFollowingStage2GuessX : -PilotDefinition.Conf.TireFollowingStage2GuessX,
|
||
|
|
StartGuessingY = 0,
|
||
|
|
SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
|
||
|
|
FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
|
||
|
|
PathTransformation = new Tuple<float, float, float>(
|
||
|
|
result == 1 ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationX : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationX,
|
||
|
|
result == 1 ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationY : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
|
||
|
|
0)
|
||
|
|
},
|
||
|
|
},
|
||
|
|
CarDirection = result == 1 ? 0f : 180f,
|
||
|
|
SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
|
||
|
|
MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
|
||
|
|
TireNum = PilotDefinition.Conf.TireFollowingTireNum,
|
||
|
|
WalkBlindTh = result == 1 ? PilotDefinition.Conf.TireFollowingFrontLidarWalkBlindTh : PilotDefinition.Conf.TireFollowingBackLidarWalkBlindTh,
|
||
|
|
};
|
||
|
|
_dt = new DriveTask(following.Get());
|
||
|
|
_dt.Wait();
|
||
|
|
DLog.Log($"结束钻车测试", "TireFollowing");
|
||
|
|
}
|
||
|
|
private DriveTask _dt;
|
||
|
|
}
|
||
|
|
|
||
|
|
[MovementTest(name = "离车测试")]
|
||
|
|
public class LeaveCar : MovementTest
|
||
|
|
{
|
||
|
|
public override void TestStop()
|
||
|
|
{
|
||
|
|
_dt?.Stop();
|
||
|
|
}
|
||
|
|
|
||
|
|
public override void Test()
|
||
|
|
{
|
||
|
|
DLog.Log($"开始离车测试，用后雷达识别", "TireFollowing");
|
||
|
|
var following = new TireFollowing()
|
||
|
|
{
|
||
|
|
GetController = () => new ChassisController().Get(),
|
||
|
|
GuessRangeX = PilotDefinition.Conf.TireFilterLength / 2,
|
||
|
|
GuessRangeY = PilotDefinition.Conf.TireFilterWidth / 2,
|
||
|
|
detectors = new List<TireFollowing.DetectorDefinition>()
|
||
|
|
{
|
||
|
|
new TireFollowing.DetectorDefinition()
|
||
|
|
{
|
||
|
|
DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, false),
|
||
|
|
StartGuessingX = -PilotDefinition.Conf.TireFollowingStage2GuessX,
|
||
|
|
StartGuessingY = 0,
|
||
|
|
SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingLeaveCarWalkBlindSwitchingDistance,
|
||
|
|
FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
|
||
|
|
PathTransformation = new Tuple<float, float, float>(
|
||
|
|
PilotDefinition.Conf.TireFollowingLeaveCarBackLidarPathTransformationX,
|
||
|
|
PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
|
||
|
|
0)
|
||
|
|
},
|
||
|
|
},
|
||
|
|
CarDirection = 180f,
|
||
|
|
SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
|
||
|
|
MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
|
||
|
|
WalkBlindTh = 0,
|
||
|
|
TireNum = 1
|
||
|
|
};
|
||
|
|
_dt = new DriveTask(following.Get());
|
||
|
|
_dt.Wait();
|
||
|
|
DLog.Log($"结束离车测试", "TireFollowing");
|
||
|
|
}
|
||
|
|
private DriveTask _dt;
|
||
|
|
}
|
||
|
|
|
||
|
|
[MovementTest(name = "抱夹关闭")]
|
||
|
|
public class ClampTest1 : MovementTest
|
||
|
|
{
|
||
|
|
public override void TestStop()
|
||
|
|
{
|
||
|
|
_dt?.Stop();
|
||
|
|
PilotDefinition.Self.SpeedLeftArm = 0;
|
||
|
|
PilotDefinition.Self.SpeedRightArm = 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
public override void Test()
|
||
|
|
{
|
||
|
|
_dt = new DriveTask(new ClampToTarget()
|
||
|
|
{
|
||
|
|
LeftClampTarget = PilotDefinition.Self.LeftArmUpperPos,
|
||
|
|
RightClampTarget = PilotDefinition.Self.RightArmUpperPos
|
||
|
|
}.Get());
|
||
|
|
_dt.Wait();
|
||
|
|
}
|
||
|
|
|
||
|
|
private DriveTask _dt;
|
||
|
|
}
|
||
|
|
|
||
|
|
[MovementTest(name = "抱夹打开")]
|
||
|
|
public class ClampTest2 : MovementTest
|
||
|
|
{
|
||
|
|
public override void TestStop()
|
||
|
|
{
|
||
|
|
_dt?.Stop();
|
||
|
|
PilotDefinition.Self.SpeedLeftArm = 0;
|
||
|
|
PilotDefinition.Self.SpeedRightArm = 0;
|
||
|
|
}
|
||
|
|
|
||
|
|
public override void Test()
|
||
|
|
{
|
||
|
|
_dt = new DriveTask(new ClampToTarget()
|
||
|
|
{
|
||
|
|
LeftClampTarget = PilotDefinition.Self.LeftArmLowerPos,
|
||
|
|
RightClampTarget = PilotDefinition.Self.RightArmLowerPos
|
||
|
|
}.Get());
|
||
|
|
_dt.Wait();
|
||
|
|
}
|
||
|
|
|
||
|
|
private DriveTask _dt;
|
||
|
|
}
|
||
|
|
|
||
|
|
[MovementTest(name = "测试前进基于轮里程")]
|
||
|
|
public class LineTrackingTest : MovementTest
|
||
|
|
{
|
||
|
|
public override void TestStop()
|
||
|
|
{
|
||
|
|
_dt?.Stop();
|
||
|
|
}
|
||
|
|
public override void Test()
|
||
|
|
{
|
||
|
|
_dt = new DriveTask(new LineTracking()
|
||
|
|
{
|
||
|
|
Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
|
||
|
|
}.Get());
|
||
|
|
_dt.Wait();
|
||
|
|
}
|
||
|
|
private DriveTask _dt;
|
||
|
|
}
|
||
|
|
|
||
|
|
[MovementTest(name = "测试后退基于轮里程")]
|
||
|
|
public class ReverseLineTrackingTest : MovementTest
|
||
|
|
{
|
||
|
|
public override void TestStop()
|
||
|
|
{
|
||
|
|
_dt?.Stop();
|
||
|
|
}
|
||
|
|
public override void Test()
|
||
|
|
{
|
||
|
|
_dt = new DriveTask(new LineTracking()
|
||
|
|
{
|
||
|
|
Target = -PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
|
||
|
|
}.Get());
|
||
|
|
_dt.Wait();
|
||
|
|
}
|
||
|
|
private DriveTask _dt;
|
||
|
|
}
|
||
|
|
|
||
|
|
[MovementTest(name = "测试终点跟踪动作-前进")]
|
||
|
|
public class DstTrackerForward : MovementTest
|
||
|
|
{
|
||
|
|
public bool UseInteractivePick = true;
|
||
|
|
public float srcX;
|
||
|
|
public float srcY;
|
||
|
|
public float dstX;
|
||
|
|
public float dstY;
|
||
|
|
public float carDirectionBias = 0f;
|
||
|
|
private readonly Painter _painter = UI.GetPainter("DstTrackerTest");
|
||
|
|
|
||
|
|
public override void TestStop()
|
||
|
|
{
|
||
|
|
_dt?.Stop();
|
||
|
|
_painter?.Clear();
|
||
|
|
}
|
||
|
|
public override void Test()
|
||
|
|
{
|
||
|
|
var p1 = UI.GetPoint("point1");
|
||
|
|
var p2 = UI.GetPoint("point2");
|
||
|
|
|
||
|
|
_painter.Clear();
|
||
|
|
_dt = new DriveTask(new DstTracker()
|
||
|
|
{
|
||
|
|
Src = p1,
|
||
|
|
Dst = p2,
|
||
|
|
CarDirectionBias = carDirectionBias,
|
||
|
|
}.Get());
|
||
|
|
_dt.Wait();
|
||
|
|
}
|
||
|
|
|
||
|
|
private DriveTask _dt;
|
||
|
|
}
|
||
|
|
|
||
|
|
[MovementTest(name = "测试终点跟踪动作-后退")]
|
||
|
|
public class DstTrackerhoutui : MovementTest
|
||
|
|
{
|
||
|
|
public bool UseInteractivePick = true;
|
||
|
|
public float srcX;
|
||
|
|
public float srcY;
|
||
|
|
public float dstX;
|
||
|
|
public float dstY;
|
||
|
|
public float carDirectionBias = 180f;
|
||
|
|
private readonly Painter _painter = UI.GetPainter("DstTrackerTest");
|
||
|
|
|
||
|
|
public override void TestStop()
|
||
|
|
{
|
||
|
|
_dt?.Stop();
|
||
|
|
_painter?.Clear();
|
||
|
|
}
|
||
|
|
public override void Test()
|
||
|
|
{
|
||
|
|
var p1 = UI.GetPoint("point1");
|
||
|
|
var p2 = UI.GetPoint("point2");
|
||
|
|
|
||
|
|
_painter.Clear();
|
||
|
|
_dt = new DriveTask(new DstTracker()
|
||
|
|
{
|
||
|
|
Src = p1,
|
||
|
|
Dst = p2,
|
||
|
|
CarDirectionBias = carDirectionBias,
|
||
|
|
}.Get());
|
||
|
|
_dt.Wait();
|
||
|
|
}
|
||
|
|
|
||
|
|
private DriveTask _dt;
|
||
|
|
}
|
||
|
|
|
||
|
|
[MovementTest(name = "测试先直行再终点跟踪")]
|
||
|
|
public class LineTrackThenDstTrackerTest : MovementTest
|
||
|
|
{
|
||
|
|
public float carDirectionBias = 0f;
|
||
|
|
private readonly Painter _painter = UI.GetPainter("LineTrackThenDstTrackerTest");
|
||
|
|
public override void TestStop()
|
||
|
|
{
|
||
|
|
_dt?.Stop();
|
||
|
|
_painter?.Clear();
|
||
|
|
}
|
||
|
|
public override void Test()
|
||
|
|
{
|
||
|
|
var src = UI.GetPoint("请在上位机选择起点(src)");
|
||
|
|
var dst = UI.GetPoint("请在上位机选择终点(dst)");
|
||
|
|
_painter.Clear();
|
||
|
|
_painter.DrawLine(Color.Cyan, src.X, src.Y, dst.X, dst.Y, width: 3);
|
||
|
|
_painter.DrawCircle(Color.LimeGreen, src.X, src.Y, 80f);
|
||
|
|
_painter.DrawCircle(Color.OrangeRed, dst.X, dst.Y, 80f);
|
||
|
|
_painter.DrawText(Color.LimeGreen, "src", src.X + 80f, src.Y + 80f);
|
||
|
|
_painter.DrawText(Color.OrangeRed, "dst", dst.X + 80f, dst.Y + 80f);
|
||
|
|
IEnumerable<bool> TrackThenFollow()
|
||
|
|
{
|
||
|
|
foreach (var running in new LineTracking()
|
||
|
|
{
|
||
|
|
Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
|
||
|
|
EnableHandover = true,
|
||
|
|
HandoverDistance = 200f,
|
||
|
|
HandoverSpeed = 0.3f,
|
||
|
|
}.Get())
|
||
|
|
{
|
||
|
|
if (!running) break;
|
||
|
|
yield return true;
|
||
|
|
}
|
||
|
|
foreach (var running in new DstTracker()
|
||
|
|
{
|
||
|
|
Src = src,
|
||
|
|
Dst = dst,
|
||
|
|
CarDirectionBias = carDirectionBias,
|
||
|
|
InitialSendSpeed = 0.3f
|
||
|
|
}.Get())
|
||
|
|
{
|
||
|
|
if (!running) break;
|
||
|
|
yield return true;
|
||
|
|
}
|
||
|
|
yield return false;
|
||
|
|
}
|
||
|
|
_dt = new DriveTask(TrackThenFollow());
|
||
|
|
_dt.Wait();
|
||
|
|
}
|
||
|
|
private DriveTask _dt;
|
||
|
|
}
|
||
|
|
|
||
|
|
[MovementTest(name = "测试先离车再终点跟踪")]
|
||
|
|
public class LeaveCarThenDstTrackerTest : MovementTest
|
||
|
|
{
|
||
|
|
private readonly Painter _painter = UI.GetPainter("LeaveCarThenDstTrackerTest");
|
||
|
|
|
||
|
|
public override void TestStop()
|
||
|
|
{
|
||
|
|
_dt?.Stop();
|
||
|
|
_painter?.Clear();
|
||
|
|
}
|
||
|
|
|
||
|
|
public override void Test()
|
||
|
|
{
|
||
|
|
var src = UI.GetPoint("请在上位机选择离车后起点(src)");
|
||
|
|
var dst = UI.GetPoint("请在上位机选择终点(dst)");
|
||
|
|
|
||
|
|
_painter.Clear();
|
||
|
|
_painter.DrawLine(Color.Cyan, src.X, src.Y, dst.X, dst.Y, width: 3);
|
||
|
|
_painter.DrawCircle(Color.LimeGreen, src.X, src.Y, 80f);
|
||
|
|
_painter.DrawCircle(Color.OrangeRed, dst.X, dst.Y, 80f);
|
||
|
|
_painter.DrawText(Color.LimeGreen, "src", src.X + 80f, src.Y + 80f);
|
||
|
|
_painter.DrawText(Color.OrangeRed, "dst", dst.X + 80f, dst.Y + 80f);
|
||
|
|
|
||
|
|
IEnumerable<bool> LeaveThenFollow()
|
||
|
|
{
|
||
|
|
var following = new TireFollowing()
|
||
|
|
{
|
||
|
|
GetController = () => new ChassisController().Get(),
|
||
|
|
GuessRangeX = PilotDefinition.Conf.TireFilterLength / 2,
|
||
|
|
GuessRangeY = PilotDefinition.Conf.TireFilterWidth / 2,
|
||
|
|
detectors = new List<TireFollowing.DetectorDefinition>()
|
||
|
|
{
|
||
|
|
new TireFollowing.DetectorDefinition()
|
||
|
|
{
|
||
|
|
DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, false),
|
||
|
|
StartGuessingX = -PilotDefinition.Conf.TireFollowingStage2GuessX,
|
||
|
|
StartGuessingY = 0,
|
||
|
|
SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
|
||
|
|
FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
|
||
|
|
PathTransformation = new Tuple<float, float, float>(
|
||
|
|
PilotDefinition.Conf.TireFollowingLeaveCarBackLidarPathTransformationX,
|
||
|
|
PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
|
||
|
|
0),
|
||
|
|
},
|
||
|
|
},
|
||
|
|
CarDirection = 180f,
|
||
|
|
SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
|
||
|
|
MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
|
||
|
|
WalkBlindTh = 0,
|
||
|
|
TireNum = 1
|
||
|
|
};
|
||
|
|
|
||
|
|
foreach (var running in following.Get())
|
||
|
|
{
|
||
|
|
if (!running) break;
|
||
|
|
yield return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
foreach (var running in new DstTracker()
|
||
|
|
{
|
||
|
|
Src = src,
|
||
|
|
Dst = dst,
|
||
|
|
CarDirectionBias = 180f,
|
||
|
|
}.Get())
|
||
|
|
{
|
||
|
|
if (!running) break;
|
||
|
|
yield return true;
|
||
|
|
}
|
||
|
|
|
||
|
|
yield return false;
|
||
|
|
}
|
||
|
|
|
||
|
|
_dt = new DriveTask(LeaveThenFollow());
|
||
|
|
_dt.Wait();
|
||
|
|
}
|
||
|
|
|
||
|
|
private DriveTask _dt;
|
||
|
|
}
|
||
|
|
|
||
|
|
|
||
|
|
[MovementTest(name = "驱动器下使能测试")]
|
||
|
|
public class DriverDisableTest : MovementTest
|
||
|
|
{
|
||
|
|
public override void TestStop()
|
||
|
|
{
|
||
|
|
throw new NotImplementedException();
|
||
|
|
}
|
||
|
|
|
||
|
|
public override void Test()
|
||
|
|
{
|
||
|
|
new DriveTask(new DriverDisable(){ }.Get()).Wait();
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
[MovementTest(name = "驱动器复位测试")]
|
||
|
|
public class DriverAbleTest : MovementTest
|
||
|
|
{
|
||
|
|
public override void TestStop()
|
||
|
|
{
|
||
|
|
throw new NotImplementedException();
|
||
|
|
}
|
||
|
|
|
||
|
|
public override void Test()
|
||
|
|
{
|
||
|
|
new DriveTask(new DriverAble(){ }.Get()).Wait();
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
[MovementTest(name = "底盘旋转测试")]
|
||
|
|
public class RotateToAngleTest : MovementTest
|
||
|
|
{
|
||
|
|
public override void TestStop()
|
||
|
|
{
|
||
|
|
throw new NotImplementedException();
|
||
|
|
}
|
||
|
|
|
||
|
|
public override void Test()
|
||
|
|
{
|
||
|
|
var target = UI.GetInput("输入旋转角度：");
|
||
|
|
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||
|
|
new DriveTask(new MultiWheelRotateInPlace()
|
||
|
|
{
|
||
|
|
AngleTarget = float.Parse(target),
|
||
|
|
PidparamsRead = () => new PIDParams()
|
||
|
|
{
|
||
|
|
Kp = PilotDefinition.Conf.TireFollowingThkp,
|
||
|
|
Ki = PilotDefinition.Conf.TireFollowingThki,
|
||
|
|
Kd = PilotDefinition.Conf.TireFollowingThkd,
|
||
|
|
DeadZone = PilotDefinition.Conf.TireFollowingThDeadZone,
|
||
|
|
SpeedAccPerSec = PilotDefinition.Conf.TireFollowingThSpeedAccPerSec,
|
||
|
|
OutputUpperThreshold = PilotDefinition.Conf.TireFollowingThThresh,
|
||
|
|
MaxI = PilotDefinition.Conf.TireFollowingThMaxI,
|
||
|
|
}
|
||
|
|
}.Get()).Wait();
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
public class utils
|
||
|
|
{
|
||
|
|
public static List<(float x, float y, float th)> RemoveOutliers(List<(float x, float y, float th)> data, float threshold = 2.0f)
|
||
|
|
{
|
||
|
|
var means = CalculateMean(data);
|
||
|
|
var stdDevs = CalculateStandardDeviation(data, means);
|
||
|
|
|
||
|
|
return data.Where(point =>
|
||
|
|
Math.Abs(point.x - means.x) <= threshold * stdDevs.x &&
|
||
|
|
Math.Abs(point.y - means.y) <= threshold * stdDevs.y &&
|
||
|
|
AngularDistance(point.th, means.th) <= threshold * stdDevs.th
|
||
|
|
).ToList();
|
||
|
|
}
|
||
|
|
|
||
|
|
public static (float x, float y, float th) CalculateMean(List<(float x, float y, float th)> data)
|
||
|
|
{
|
||
|
|
float meanX = data.Average(point => point.x);
|
||
|
|
float meanY = data.Average(point => point.y);
|
||
|
|
float sinSum = data.Sum(point => (float)Math.Sin(DegreeToRadian(point.th)));
|
||
|
|
float cosSum = data.Sum(point => (float)Math.Cos(DegreeToRadian(point.th)));
|
||
|
|
float meanTh = RadianToDegree((float)Math.Atan2(sinSum, cosSum));
|
||
|
|
|
||
|
|
return (meanX, meanY, meanTh);
|
||
|
|
}
|
||
|
|
|
||
|
|
public static (float x, float y, float th) CalculateStandardDeviation(List<(float x, float y, float th)> data, (float x, float y, float th) means)
|
||
|
|
{
|
||
|
|
float varianceX = data.Average(point => (point.x - means.x) * (point.x - means.x));
|
||
|
|
float varianceY = data.Average(point => (point.y - means.y) * (point.y - means.y));
|
||
|
|
|
||
|
|
// 计算角度的方差
|
||
|
|
float varianceTh = data.Average(point => AngularDistance(point.th, means.th) * AngularDistance(point.th, means.th));
|
||
|
|
|
||
|
|
return ((float)Math.Sqrt(varianceX), (float)Math.Sqrt(varianceY), (float)Math.Sqrt(varianceTh));
|
||
|
|
}
|
||
|
|
|
||
|
|
public static float DegreeToRadian(float degree)
|
||
|
|
{
|
||
|
|
return (float)(degree * Math.PI / 180.0);
|
||
|
|
}
|
||
|
|
|
||
|
|
public static float RadianToDegree(float radian)
|
||
|
|
{
|
||
|
|
return (float)(radian * 180.0 / Math.PI);
|
||
|
|
}
|
||
|
|
|
||
|
|
public static float AngularDistance(float angle1, float angle2)
|
||
|
|
{
|
||
|
|
return CommonMath.ThDiff(angle1, angle2);
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
}
|
||
|
|
|
||
|
|
|