update 增加停车机器人钻车等代码 实现停车机器人能力的瘦身代码
This commit is contained in:
shuai.li
@@ -1,9 +1,282 @@
|
|||||||
|
using ClumsyCore;
|
||||||
|
using FundamentalLib;
|
||||||
using MDCSToolBox.Clumsy.AgvInterfaces;
|
using MDCSToolBox.Clumsy.AgvInterfaces;
|
||||||
using MDCSToolBox.Clumsy.MotionControllers;
|
using MDCSToolBox.Clumsy.MotionControllers;
|
||||||
|
using MDCSToolBox.Clumsy.Tracks;
|
||||||
|
using MDCSToolBox.Commons.Controllers;
|
||||||
|
using Newtonsoft.Json;
|
||||||
|
using OpenCvSharp.Dnn;
|
||||||
|
using OpenCvSharp.XFeatures2D;
|
||||||
|
using System;
|
||||||
|
using System.Collections.Generic;
|
||||||
|
using System.Net.Http;
|
||||||
|
using System.Numerics;
|
||||||
|
using System.Threading;
|
||||||
|
using System.Threading.Tasks;
|
||||||
|
using static ClumsyCore.DTools.Painter;
|
||||||
|
using static OpenCvSharp.ConnectedComponents;
|
||||||
|
|
||||||
namespace MultiWheelC;
|
namespace MultiWheelC
|
||||||
|
|
||||||
public class AGV : BasicInterface
|
|
||||||
{
|
{
|
||||||
public override AbstractGeometricController GetController() => new ChassisController().Get();
|
public class SetLocationRes
|
||||||
|
{
|
||||||
|
public float x, y, th;
|
||||||
|
public int l_step;
|
||||||
|
public long tick;
|
||||||
|
public string error;
|
||||||
|
}
|
||||||
|
public class AGV : MultiWheelInterface
|
||||||
|
{
|
||||||
|
public override AbstractGeometricController GetController()
|
||||||
|
{
|
||||||
|
return new ChassisController().Get();
|
||||||
|
}
|
||||||
|
|
||||||
|
public override MultiWheelMagTracker GetMagController()
|
||||||
|
{
|
||||||
|
return new MultiWheelMagTracker();
|
||||||
|
}
|
||||||
|
|
||||||
|
public override NaiveMagnetController GetNaiveMagnetController()
|
||||||
|
{
|
||||||
|
return new NaiveMagnetController();
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Sleep(float s)
|
||||||
|
{
|
||||||
|
new DriveTask(new Sleep() { Second = s }.Get()).Wait();
|
||||||
|
}
|
||||||
|
|
||||||
|
public void ControlChargePort(bool open)
|
||||||
|
{
|
||||||
|
DLog.Log($"call ControlChargePort({open})");
|
||||||
|
PilotDefinition.Self.OpenChargeByClumsy = open;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void SwitchLidarArea(int area)
|
||||||
|
{
|
||||||
|
DLog.Log($"call SwitchLidarArea({area})");
|
||||||
|
PilotDefinition.Self.AreaChoose = area;
|
||||||
|
}
|
||||||
|
|
||||||
|
public void SwitchIoArea(int area)
|
||||||
|
{
|
||||||
|
if (area != -1)
|
||||||
|
{
|
||||||
|
PilotDefinition.Self.IOObstacleArea = area;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
//参数1：tireNum 需要钻过的轮胎对数量
|
||||||
|
//参数2：frontLidarDetect true:前雷达识别 false:后雷达识别
|
||||||
|
public void TireFollowing(int tireNum, bool frontLidarDetect, int srcId, int dstId)
|
||||||
|
{
|
||||||
|
while (!TryLock(dstId))
|
||||||
|
{
|
||||||
|
Thread.Sleep(50);
|
||||||
|
}
|
||||||
|
DLog.Log($"锁点{dstId}完成", "TireFollowing");
|
||||||
|
var lidarName = frontLidarDetect ? "前雷达" : "后雷达";
|
||||||
|
DLog.Log($"开始钻车动作，通过{lidarName}识别结果钻{tireNum}对轮胎", "TireFollowing");
|
||||||
|
if (tireNum != 1 && tireNum != 2)
|
||||||
|
{
|
||||||
|
DLog.Log($"TireNum必须是1或2 (当前输入:{tireNum})", "TireFollowing");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (PilotDefinition.Self.GhostMode)
|
||||||
|
{
|
||||||
|
while (!TryLock(dstId))
|
||||||
|
{
|
||||||
|
Console.WriteLine("等待锁取货点中...");
|
||||||
|
Thread.Sleep(200);
|
||||||
|
}
|
||||||
|
Console.WriteLine($"锁点{dstId}完成");
|
||||||
|
Thread.Sleep(1000);
|
||||||
|
Console.WriteLine($"开始钻车动作，通过{lidarName}识别结果钻{tireNum}对轮胎");
|
||||||
|
Thread.Sleep(1000);
|
||||||
|
Leave(srcId);
|
||||||
|
Console.WriteLine($"开始第一段盲走，此时释放预取货点{srcId}");
|
||||||
|
Thread.Sleep(2000);
|
||||||
|
//Leave(dstId);
|
||||||
|
//Console.WriteLine($"结束第一段盲走，此时释放取货点{dstId}");
|
||||||
|
Thread.Sleep(2000);
|
||||||
|
Console.WriteLine($"结束钻车动作");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
var detectors = new List<TireFollowing.DetectorDefinition>()
|
||||||
|
{
|
||||||
|
new TireFollowing.DetectorDefinition()
|
||||||
|
{
|
||||||
|
DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, frontLidarDetect),
|
||||||
|
StartGuessingX = frontLidarDetect ? 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>(
|
||||||
|
frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationX : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationX,
|
||||||
|
frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationY : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
|
||||||
|
0),
|
||||||
|
LeaveSrcFunction = Leave,
|
||||||
|
SrcId = srcId,
|
||||||
|
DstId = dstId,
|
||||||
|
},
|
||||||
|
new TireFollowing.DetectorDefinition()
|
||||||
|
{
|
||||||
|
DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, frontLidarDetect),
|
||||||
|
StartGuessingX = frontLidarDetect ? 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>(
|
||||||
|
frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationX : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationX,
|
||||||
|
frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarPathTransformationY : PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
|
||||||
|
0)
|
||||||
|
},
|
||||||
|
};
|
||||||
|
DLog.Log($"检测器数量为{detectors.Count}", "TireFollowing");
|
||||||
|
var following = new TireFollowing()
|
||||||
|
{
|
||||||
|
GetController = () => new ChassisController().Get(),
|
||||||
|
GuessRangeX = PilotDefinition.Conf.TireFilterLength / 2,
|
||||||
|
GuessRangeY = PilotDefinition.Conf.TireFilterWidth / 2,
|
||||||
|
detectors = detectors,
|
||||||
|
SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
|
||||||
|
MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
|
||||||
|
TireNum = detectors.Count,
|
||||||
|
CarDirection = frontLidarDetect ? 0f : 180f,
|
||||||
|
WalkBlindTh = frontLidarDetect ? PilotDefinition.Conf.TireFollowingFrontLidarWalkBlindTh : PilotDefinition.Conf.TireFollowingBackLidarWalkBlindTh,
|
||||||
|
};
|
||||||
|
var _dt = new DriveTask(following.Get());
|
||||||
|
_dt.Wait();
|
||||||
|
DLog.Log("钻车动作结束", "TireFollowing");
|
||||||
|
}
|
||||||
|
|
||||||
|
//离车一定是后雷达识别一个轮胎
|
||||||
|
public void LeaveCar(int srcId, int dstId)
|
||||||
|
{
|
||||||
|
while (!TryLock(dstId))
|
||||||
|
{
|
||||||
|
Thread.Sleep(50);
|
||||||
|
}
|
||||||
|
DLog.Log($"锁点{dstId}完成", "TireFollowing");
|
||||||
|
DLog.Log($"开始钻车动作，通过后雷达识别结果钻1对轮胎", "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.TireFollowingWalkBlindSwitchingDistance,
|
||||||
|
FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
|
||||||
|
PathTransformation = new Tuple<float, float, float>(
|
||||||
|
PilotDefinition.Conf.TireFollowingLeaveCarBackLidarPathTransformationX,
|
||||||
|
PilotDefinition.Conf.TireFollowingBackLidarPathTransformationY,
|
||||||
|
0),
|
||||||
|
LeaveSrcFunction = Leave,
|
||||||
|
SrcId = srcId,
|
||||||
|
DstId = dstId,
|
||||||
|
},
|
||||||
|
},
|
||||||
|
CarDirection = 180f,
|
||||||
|
SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
|
||||||
|
MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
|
||||||
|
WalkBlindTh = 0,
|
||||||
|
TireNum = 1
|
||||||
|
};
|
||||||
|
var _dt = new DriveTask(following.Get());
|
||||||
|
_dt.Wait();
|
||||||
|
DLog.Log("钻车动作结束", "TireFollowing");
|
||||||
|
}
|
||||||
|
|
||||||
|
//驱动器上使能
|
||||||
|
public void DriverAble()
|
||||||
|
{
|
||||||
|
var dl = new DriveTask(new DriverAble() { }.Get());
|
||||||
|
dl.Wait();
|
||||||
|
DLog.Log("驱动器上使能完成", "TireFollowing");
|
||||||
|
}
|
||||||
|
|
||||||
|
//驱动器下使能
|
||||||
|
public void DriverDisable()
|
||||||
|
{
|
||||||
|
var dl = new DriveTask(new DriverDisable() { }.Get());
|
||||||
|
dl.Wait();
|
||||||
|
DLog.Log("驱动器下使能完成", "TireFollowing");
|
||||||
|
}
|
||||||
|
|
||||||
|
//夹抱 close为true时夹抱，否则为还原
|
||||||
|
public void ClamptoTarget(bool close)
|
||||||
|
{
|
||||||
|
if (PilotDefinition.Self.GhostMode)
|
||||||
|
{
|
||||||
|
Thread.Sleep(2000);
|
||||||
|
Console.WriteLine("夹抱完成");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
new DriveTask(new ClampToTarget()
|
||||||
|
{
|
||||||
|
LeftClampTarget = close ? PilotDefinition.Self.LeftArmUpperPos : PilotDefinition.Self.LeftArmLowerPos,
|
||||||
|
RightClampTarget = close ? PilotDefinition.Self.RightArmUpperPos : PilotDefinition.Self.RightArmLowerPos
|
||||||
|
}.Get()).Wait();
|
||||||
|
}
|
||||||
|
|
||||||
|
public void LineTracking(int srcId, int dstId, float LineDistance)
|
||||||
|
{
|
||||||
|
while (!TryLock(dstId))
|
||||||
|
{
|
||||||
|
Thread.Sleep(50);
|
||||||
|
}
|
||||||
|
DLog.Log($"锁点{dstId}完成", "TireFollowing");
|
||||||
|
new DriveTask(new LineTracking()
|
||||||
|
{
|
||||||
|
Target = LineDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
|
||||||
|
LeaveSrcFunction = Leave,
|
||||||
|
SrcId = srcId,
|
||||||
|
}.Get()).Wait();
|
||||||
|
}
|
||||||
|
|
||||||
|
public void ChangeAvoidanceDistance(float stopDistance, float slowDistance)
|
||||||
|
{
|
||||||
|
DLog.Log($"call ChangeAvoidanceDistance({stopDistance},{slowDistance})");
|
||||||
|
PilotDefinition.Self.SlowDistance = slowDistance;
|
||||||
|
PilotDefinition.Self.StopDistance = stopDistance;
|
||||||
|
}
|
||||||
|
public void ChangeAvoidanceParam(float length = -1, float width = -1)
|
||||||
|
{
|
||||||
|
|
||||||
|
PilotDefinition.Self.CarLength = length;
|
||||||
|
PilotDefinition.Self.CarWidth = width;
|
||||||
|
|
||||||
|
}
|
||||||
|
public void SetLocation(float x, float y, float th)
|
||||||
|
{
|
||||||
|
DLog.Log($"call SetLocation({x},{y},{th})");
|
||||||
|
Console.WriteLine($"call SetLocation({x},{y},{th})");
|
||||||
|
Queue(() =>
|
||||||
|
{
|
||||||
|
while (true)
|
||||||
|
{
|
||||||
|
var str1 = new HttpClient()
|
||||||
|
.GetStringAsync(
|
||||||
|
$"http://127.0.0.1:4321/setLocation?x={x}&y={y}&th={th}")
|
||||||
|
.Result;
|
||||||
|
Thread.Sleep(500);
|
||||||
|
Console.WriteLine($"SetLocation str={str1}");
|
||||||
|
var setLocationRes = JsonConvert.DeserializeObject<SetLocationRes>(str1);
|
||||||
|
Console.WriteLine(setLocationRes.l_step);
|
||||||
|
if (setLocationRes != null && setLocationRes.l_step == 2) break;
|
||||||
|
}
|
||||||
|
});
|
||||||
|
|
||||||
|
}
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -0,0 +1,368 @@
|
|||||||
|
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 OpenCvSharp.Dnn;
|
||||||
|
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()
|
||||||
|
{
|
||||||
|
throw new NotImplementedException();
|
||||||
|
}
|
||||||
|
|
||||||
|
public override void Test()
|
||||||
|
{
|
||||||
|
new DriveTask(new ClampToTarget()
|
||||||
|
{
|
||||||
|
LeftClampTarget = PilotDefinition.Self.LeftArmUpperPos,
|
||||||
|
RightClampTarget = PilotDefinition.Self.RightArmUpperPos
|
||||||
|
}.Get()).Wait();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
[MovementTest(name = "抱夹打开")]
|
||||||
|
public class ClampTest2 : MovementTest
|
||||||
|
{
|
||||||
|
public override void TestStop()
|
||||||
|
{
|
||||||
|
throw new NotImplementedException();
|
||||||
|
}
|
||||||
|
|
||||||
|
public override void Test()
|
||||||
|
{
|
||||||
|
new DriveTask(new ClampToTarget()
|
||||||
|
{
|
||||||
|
LeftClampTarget = PilotDefinition.Self.LeftArmLowerPos,
|
||||||
|
RightClampTarget = PilotDefinition.Self.RightArmLowerPos
|
||||||
|
}.Get()).Wait();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
[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 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();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
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);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
@@ -0,0 +1,245 @@
|
|||||||
|
using ClumsyCore;
|
||||||
|
using ClumsyCore.DTools;
|
||||||
|
using ClumsyCore.Interfaces;
|
||||||
|
using ClumsyCore.Pilot;
|
||||||
|
using ClumsyCore.Sensors;
|
||||||
|
using ClumsyCore.Utilities;
|
||||||
|
using ClumsyDance.ClumsyWalk.Detectors;
|
||||||
|
using ClumsyDance.Sensors;
|
||||||
|
using CommonUsage.Chassis;
|
||||||
|
using FundamentalLib;
|
||||||
|
using MDCSToolBox.Clumsy.Calibration;
|
||||||
|
using MDCSToolBox.Clumsy.HighLevelSecurity;
|
||||||
|
using MDCSToolBox.Clumsy.Movements;
|
||||||
|
using MDCSToolBox.Clumsy.Pilot;
|
||||||
|
using MDCSToolBox.Clumsy.Tracks;
|
||||||
|
using MDCSToolBox.Commons;
|
||||||
|
using MDCSToolBox.Commons.Controllers;
|
||||||
|
using Newtonsoft.Json;
|
||||||
|
using OpenCvSharp.Dnn;
|
||||||
|
using System;
|
||||||
|
using System.Collections.Generic;
|
||||||
|
using System.Drawing;
|
||||||
|
using System.Linq;
|
||||||
|
using System.Net.Http;
|
||||||
|
using System.Numerics;
|
||||||
|
using System.Reflection;
|
||||||
|
using System.Text;
|
||||||
|
using System.Threading;
|
||||||
|
|
||||||
|
namespace MultiWheelC
|
||||||
|
{
|
||||||
|
public class MultiWheelRotateInPlace : MovementDefinition
|
||||||
|
{
|
||||||
|
/// <summary>
|
||||||
|
/// 旋转目标角度
|
||||||
|
/// </summary>
|
||||||
|
public float AngleTarget;
|
||||||
|
|
||||||
|
public float MaxSpeed;
|
||||||
|
|
||||||
|
public Func<float> ThetaReader = () => (float)DetourInterface.getCartLocation().th;
|
||||||
|
|
||||||
|
public MultiWheelChassis Chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||||||
|
|
||||||
|
public Func<PIDParams> PidparamsRead = () => new PIDParams() { };
|
||||||
|
|
||||||
|
public PIDController thPid;
|
||||||
|
|
||||||
|
private static float RangeAngle(float theta)
|
||||||
|
{
|
||||||
|
return (float)(theta - Math.Round(theta / 360.0f) * 360);
|
||||||
|
}
|
||||||
|
|
||||||
|
public override IEnumerable<bool> Get()
|
||||||
|
{
|
||||||
|
var targetAngle = RangeAngle(AngleTarget);
|
||||||
|
var p = PidparamsRead();
|
||||||
|
thPid = new PIDController(ThetaReader, p.Kp);
|
||||||
|
thPid.ChangeParameters(p.Kp, p.Ki, p.Kd, p.MaxI, p.DeadZone, p.OutputUpperThreshold, p.SpeedAccPerSec);
|
||||||
|
|
||||||
|
DateTime lastTime = DateTime.Now;
|
||||||
|
while (true)
|
||||||
|
{
|
||||||
|
var s = thPid.GetResponse(targetAngle, true);
|
||||||
|
Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}");
|
||||||
|
Chassis.SendXYThSpeed(0, 0, s);
|
||||||
|
lastTime = DateTime.Now;
|
||||||
|
if (thPid.IsArrived()) break;
|
||||||
|
yield return true;
|
||||||
|
}
|
||||||
|
Chassis.SendXYThSpeed(0, 0, 0);
|
||||||
|
Console.WriteLine($"final rotate to {targetAngle}");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
public class ClampToTarget : MovementDefinition
|
||||||
|
{
|
||||||
|
public float LeftClampTarget;
|
||||||
|
public float RightClampTarget;
|
||||||
|
|
||||||
|
public float MaxClampSpeed = PilotDefinition.Conf.MaxClampSpeed;
|
||||||
|
public float ClampKp = PilotDefinition.Conf.ClampControlKp;
|
||||||
|
public float ClampKi = PilotDefinition.Conf.ClampControlKi;
|
||||||
|
public float ClampKd = PilotDefinition.Conf.ClampControlKd;
|
||||||
|
public float ClampDeadZone = PilotDefinition.Conf.ClampControlDeadZone;
|
||||||
|
private PIDController leftpid, rightpid;
|
||||||
|
|
||||||
|
public override IEnumerable<bool> Get()
|
||||||
|
{
|
||||||
|
leftpid = new PIDController(() => PilotDefinition.Self.ActualPosLeftArm, ClampKp, ClampKi, ClampKd, 0,
|
||||||
|
ClampDeadZone, MaxClampSpeed)
|
||||||
|
{ SpeedAccPerSec = MaxClampSpeed / 2f };
|
||||||
|
|
||||||
|
rightpid = new PIDController(() => PilotDefinition.Self.ActualPosRightArm, ClampKp, ClampKi, ClampKd, 0,
|
||||||
|
ClampDeadZone, MaxClampSpeed)
|
||||||
|
{ SpeedAccPerSec = MaxClampSpeed / 2f };
|
||||||
|
|
||||||
|
while (true)
|
||||||
|
{
|
||||||
|
var leftspeed = leftpid.GetResponse(LeftClampTarget);
|
||||||
|
var rightspeed = rightpid.GetResponse(RightClampTarget);
|
||||||
|
Console.WriteLine($"left arm speed:{leftspeed} right arm speed:{rightspeed}");
|
||||||
|
PilotDefinition.Self.SpeedLeftArm = leftspeed;
|
||||||
|
PilotDefinition.Self.SpeedRightArm = rightspeed;
|
||||||
|
if (leftpid.IsArrived()) PilotDefinition.Self.SpeedLeftArm = 0;
|
||||||
|
if (rightpid.IsArrived()) PilotDefinition.Self.SpeedRightArm = 0;
|
||||||
|
//if(Math.Abs(PilotDefinition.Self.LeftArmActualPos - PilotDefinition.Self.RightArmActualPos) > PilotDefinition.Conf.ClampOutOfSyncThr)
|
||||||
|
//{
|
||||||
|
// PilotDefinition.Self.ClampOutOfSync = true;
|
||||||
|
// Console.WriteLine($"左夹臂位置{PilotDefinition.Self.LeftArmActualPos} 右夹臂位置{PilotDefinition.Self.RightArmActualPos}");
|
||||||
|
// Console.WriteLine($"两个夹臂的位置差了{Math.Abs(PilotDefinition.Self.LeftArmActualPos - PilotDefinition.Self.RightArmActualPos)}");
|
||||||
|
// break;
|
||||||
|
//}
|
||||||
|
|
||||||
|
if (leftpid.IsArrived() && rightpid.IsArrived()) break;
|
||||||
|
yield return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
PilotDefinition.Self.SpeedLeftArm = 0;
|
||||||
|
PilotDefinition.Self.SpeedRightArm = 0;
|
||||||
|
Console.WriteLine($"left clamp to target:{LeftClampTarget} right clamp to target:{RightClampTarget}");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public class Sleep : MovementDefinition
|
||||||
|
{
|
||||||
|
public float Second = 2;
|
||||||
|
public override IEnumerable<bool> Get()
|
||||||
|
{
|
||||||
|
var start = DateTime.Now;
|
||||||
|
while ((DateTime.Now-start).TotalSeconds<Second)
|
||||||
|
{
|
||||||
|
yield return true;
|
||||||
|
Thread.Sleep(1000);
|
||||||
|
Console.WriteLine("Sleep");
|
||||||
|
}
|
||||||
|
|
||||||
|
yield return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
//直线行走基于detour
|
||||||
|
public class LineTracking1 : MovementDefinition
|
||||||
|
{
|
||||||
|
public float LineDistance = 1000f;
|
||||||
|
public int SrcId = -1;
|
||||||
|
public int DstId = -1;
|
||||||
|
public Action<int> LeaveSrcFunction = null;
|
||||||
|
public Painter painter = UI.GetPainter("Line", false);
|
||||||
|
public override IEnumerable<bool> Get()
|
||||||
|
{
|
||||||
|
var curpose = DetourInterface.getCartLocation();
|
||||||
|
Console.WriteLine($"curpose.th:{curpose.th}");
|
||||||
|
var src = new Vector2((float)curpose.x, (float)curpose.y);
|
||||||
|
var dst = new Vector2((float)curpose.x + LineDistance * (float)Math.Cos(curpose.th),
|
||||||
|
(float)curpose.y + LineDistance * (float)Math.Sin(curpose.th));
|
||||||
|
Console.WriteLine($"src:{src.X} {src.Y}");
|
||||||
|
Console.WriteLine($"dst:{dst.X} {dst.Y}");
|
||||||
|
painter.DrawLine(Color.Green, src.X, src.Y, dst.X, dst.Y, width: 3);
|
||||||
|
|
||||||
|
var tracker = new ChassisController().Get();
|
||||||
|
var linePath = new LineTrack(src, dst) { CarDirectionBias = LineDistance > 0 ? 0 : 180 };
|
||||||
|
tracker.AddTrack(linePath);
|
||||||
|
var _dt = new DriveTask(tracker.Track());
|
||||||
|
_dt.Wait();
|
||||||
|
if (SrcId != -1 && LeaveSrcFunction != null)
|
||||||
|
{
|
||||||
|
LeaveSrcFunction(SrcId);
|
||||||
|
DLog.Log($"释放放车点{SrcId}", "TireFollowing");
|
||||||
|
}
|
||||||
|
yield return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
//直线行走基于轮里程
|
||||||
|
public class LineTracking : MovementDefinition
|
||||||
|
{
|
||||||
|
public float Target;
|
||||||
|
public float MaxSpeed = PilotDefinition.Conf.LineTrackMaxSpeed;
|
||||||
|
public float Kp = PilotDefinition.Conf.LineTrackKp;
|
||||||
|
public float Ki = PilotDefinition.Conf.LineTrackKi;
|
||||||
|
public float Kd = PilotDefinition.Conf.LineTrackKd;
|
||||||
|
public float DeadZone = PilotDefinition.Conf.LineTrackDeadZone;
|
||||||
|
public int SrcId = -1;
|
||||||
|
public int DstId = -1;
|
||||||
|
public Action<int> LeaveSrcFunction = null;
|
||||||
|
private PIDController pid;
|
||||||
|
|
||||||
|
public override IEnumerable<bool> Get()
|
||||||
|
{
|
||||||
|
pid = new PIDController(() => (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2, Kp, Ki, Kd, 0,
|
||||||
|
DeadZone, MaxSpeed)
|
||||||
|
{ SpeedAccPerSec = MaxSpeed / 2f };
|
||||||
|
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||||||
|
while (true)
|
||||||
|
{
|
||||||
|
var speed = pid.GetResponse(Target);
|
||||||
|
Console.WriteLine($"output: {speed} current: {(PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2}");
|
||||||
|
var current = (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2;
|
||||||
|
chassis.SendXYThSpeed(speed, 0, 0);
|
||||||
|
//if (Math.Abs(current - Target) < pid.DeadZone)
|
||||||
|
//{
|
||||||
|
// chassis.SendXYThSpeed(0f, 0f, 0f);
|
||||||
|
// Console.WriteLine($"调整退出:当前({current:f2}) ,目标:({Target:f2})");
|
||||||
|
// break;
|
||||||
|
//}
|
||||||
|
if (pid.IsArrived()) break;
|
||||||
|
yield return true;
|
||||||
|
}
|
||||||
|
if (SrcId != -1 && LeaveSrcFunction != null)
|
||||||
|
{
|
||||||
|
LeaveSrcFunction(SrcId);
|
||||||
|
DLog.Log($"释放放车点{SrcId}", "TireFollowing");
|
||||||
|
}
|
||||||
|
yield return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public class DriverAble : MovementDefinition
|
||||||
|
{
|
||||||
|
public override IEnumerable<bool> Get()
|
||||||
|
{
|
||||||
|
Console.WriteLine("驱动器上使能");
|
||||||
|
PilotDefinition.Self.ResetFromC = true;
|
||||||
|
Thread.Sleep(100);
|
||||||
|
PilotDefinition.Self.ResetFromC = false;
|
||||||
|
Console.WriteLine("驱动器上使能完成");
|
||||||
|
yield return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
public class DriverDisable : MovementDefinition
|
||||||
|
{
|
||||||
|
public override IEnumerable<bool> Get()
|
||||||
|
{
|
||||||
|
Console.WriteLine("驱动器下使能");
|
||||||
|
PilotDefinition.Self.DisableFromC = true;
|
||||||
|
Thread.Sleep(100);
|
||||||
|
PilotDefinition.Self.DisableFromC = false;
|
||||||
|
Console.WriteLine("驱动器下使能完成");
|
||||||
|
yield return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -10,6 +10,8 @@
|
|||||||
|
|
||||||
<ItemGroup>
|
<ItemGroup>
|
||||||
<PackageReference Include="Newtonsoft.Json" Version="13.0.3" />
|
<PackageReference Include="Newtonsoft.Json" Version="13.0.3" />
|
||||||
|
<PackageReference Include="OpenCvSharp4" Version="4.10.0.20240615" />
|
||||||
|
<PackageReference Include="OpenCvSharp4.Extensions" Version="4.10.0.20240615" />
|
||||||
<PackageReference Include="System.Numerics.Vectors" Version="4.6.1" />
|
<PackageReference Include="System.Numerics.Vectors" Version="4.6.1" />
|
||||||
</ItemGroup>
|
</ItemGroup>
|
||||||
|
|
||||||
|
|||||||
@@ -131,4 +131,59 @@ public class PilotConfig : MultiWheelPilotConfig
|
|||||||
|
|
||||||
[FieldMember(desc = "2腿检测：ROI滤波框宽(mm)")]
|
[FieldMember(desc = "2腿检测：ROI滤波框宽(mm)")]
|
||||||
public float TwoLegFilterWidth = 600f;
|
public float TwoLegFilterWidth = 600f;
|
||||||
|
|
||||||
|
#region
|
||||||
|
[FieldMember(desc = "轮胎识别:识别框长")] public float TireFilterLength = 1800f;
|
||||||
|
[FieldMember(desc = "轮胎识别:识别框宽")] public float TireFilterWidth = 600f;
|
||||||
|
[FieldMember(desc = "轮胎识别:轮胎间距")] public float TireTwoLegWidth = 800f;
|
||||||
|
[FieldMember(desc = "轮胎识别:轮胎识别允许误差")] public float TireTwoLegWidthErr = 100f;
|
||||||
|
[FieldMember(desc = "轮胎识别:轮胎聚类最小点云数")] public int TireTwoLegBlobPtCount = 15;
|
||||||
|
|
||||||
|
[FieldMember(desc = "轮胎识别:前雷达参数")] public float TireFrontTwoLegBlobDist = 100f;
|
||||||
|
[FieldMember(desc = "轮胎识别:前雷达参数")] public float TireFrontTwoLegBlobSize = 200f;
|
||||||
|
[FieldMember(desc = "轮胎识别:前雷达参数")] public int TireFrontPadding = 5;
|
||||||
|
[FieldMember(desc = "轮胎识别:前雷达参数")] public int TireFrontTwoLegPillarFindingScope = 20;
|
||||||
|
[FieldMember(desc = "轮胎识别:前雷达参数")] public int TireFrontTwoLegSgnDir = 1;
|
||||||
|
[FieldMember(desc = "轮胎识别:前雷达参数")] public float TireFrontTwoLegCenterChangeX = 0;
|
||||||
|
[FieldMember(desc = "轮胎识别:后雷达参数")] public float TireBackTwoLegBlobDist = 100f;
|
||||||
|
[FieldMember(desc = "轮胎识别:后雷达参数")] public float TireBackTwoLegBlobSize = 200f;
|
||||||
|
[FieldMember(desc = "轮胎识别:后雷达参数")] public int TireBackPadding = 5;
|
||||||
|
[FieldMember(desc = "轮胎识别:后雷达参数")] public int TireBackTwoLegPillarFindingScope = 20;
|
||||||
|
[FieldMember(desc = "轮胎识别:后雷达参数")] public int TireBackTwoLegSgnDir = 1;
|
||||||
|
[FieldMember(desc = "轮胎识别:后雷达参数")] public float TireBackTwoLegCenterChangeX = 0;
|
||||||
|
|
||||||
|
[FieldMember(desc = "抱夹控制pid:Kp")] public float ClampControlKp = 0.1f;
|
||||||
|
[FieldMember(desc = "抱夹控制pid:Ki")] public float ClampControlKi = 0f;
|
||||||
|
[FieldMember(desc = "抱夹控制pid:Kd")] public float ClampControlKd = 0f;
|
||||||
|
[FieldMember(desc = "抱夹控制pid:MaxI")] public float ClampControlMaxI = 0f;
|
||||||
|
[FieldMember(desc = "抱夹控制pid:Acc")] public float ClampControlSpeedAcc = 1f;
|
||||||
|
[FieldMember(desc = "抱夹控制pid:Thresh")] public float ClampControlThresh = 0.2f;
|
||||||
|
[FieldMember(desc = "抱夹控制pid:DeadZone")] public float ClampControlDeadZone = 5f;
|
||||||
|
[FieldMember(desc = "抱夹最大速度")] public float MaxClampSpeed = 1.5f;
|
||||||
|
|
||||||
|
[FieldMember(desc = "直线行走距离")] public float LineTrackDistance = 1000f;
|
||||||
|
[FieldMember(desc = "直线行走最大速度")] public float LineTrackMaxSpeed = 0.3f;
|
||||||
|
[FieldMember(desc = "直线行走Kp")] public float LineTrackKp = 0.2f;
|
||||||
|
[FieldMember(desc = "直线行走Ki")] public float LineTrackKi = 0f;
|
||||||
|
[FieldMember(desc = "直线行走Kd")] public float LineTrackKd = 0f;
|
||||||
|
[FieldMember(desc = "直线行走DeadZone")] public float LineTrackDeadZone = 50f;
|
||||||
|
|
||||||
|
[FieldMember(desc = "轮胎跟踪:切换至盲走距离")] public float TireFollowingWalkBlindSwitchingDistance = 1200f;
|
||||||
|
[FieldMember(desc = "轮胎跟踪:识别第一对轮胎的初始距离")] public float TireFollowingStage1GuessX = 2000f;
|
||||||
|
[FieldMember(desc = "轮胎跟踪:识别第二对轮胎的初始距离")] public float TireFollowingStage2GuessX = 2475f;
|
||||||
|
[FieldMember(desc = "轮胎跟踪:盲走停止距离")] public float TireFollowingWalkBlindFinishDistance = 10f;
|
||||||
|
[FieldMember(desc = "轮胎跟踪:减速距离")] public float TireFollowingSlowDistance = 200f;
|
||||||
|
[FieldMember(desc = "轮胎跟踪:最大速度")] public float TireFollowingMaxSpeed = 0.2f;
|
||||||
|
[FieldMember(desc = "轮胎跟踪:前雷达识别路径偏移X")] public float TireFollowingFrontLidarPathTransformationX = 253f;
|
||||||
|
[FieldMember(desc = "轮胎跟踪:前雷达识别路径偏移Y")] public float TireFollowingFrontLidarPathTransformationY = 13f;
|
||||||
|
[FieldMember(desc = "轮胎跟踪:前雷达识别路径偏移Th")] public float TireFollowingFrontLidarWalkBlindTh = -1f;
|
||||||
|
[FieldMember(desc = "轮胎跟踪:后雷达识别路径偏移X")] public float TireFollowingBackLidarPathTransformationX = 148f;
|
||||||
|
[FieldMember(desc = "轮胎跟踪:后雷达识别路径偏移Y")] public float TireFollowingBackLidarPathTransformationY = 2f;
|
||||||
|
[FieldMember(desc = "轮胎跟踪:后雷达识别路径偏移Th")] public float TireFollowingBackLidarWalkBlindTh = 0f;
|
||||||
|
[FieldMember(desc = "轮胎跟踪:离车时后雷达识别路径偏移X")] public float TireFollowingLeaveCarBackLidarPathTransformationX = 1500f;
|
||||||
|
[FieldMember(desc = "轮胎跟踪:离车时切换至盲走距离")] public float TireFollowingLeaveCarWalkBlindSwitchingDistance = 1200f;
|
||||||
|
|
||||||
|
[FieldMember(desc = "轮胎跟踪:测试钻轮胎数量")] public int TireFollowingTireNum = 1;
|
||||||
|
[FieldMember(desc = "轮胎跟踪:距离过近角度忽略阈值")] public float TireFollowingAngleIgnoreThr = 0.2f;
|
||||||
|
#endregion
|
||||||
}
|
}
|
||||||
|
|||||||
@@ -55,6 +55,35 @@ public class PilotDefinition : MultiWheelPilotDefinition<PilotConfig, PilotDefin
|
|||||||
|
|
||||||
[AsLowerIO(desc = "车号")] public int CarNum = 1;
|
[AsLowerIO(desc = "车号")] public int CarNum = 1;
|
||||||
|
|
||||||
|
#region 夹臂变量
|
||||||
|
[AsUpperIO(desc = "左夹臂下发速度")] public float SpeedLeftArm;
|
||||||
|
|
||||||
|
[AsUpperIO(desc = "右夹臂下发速度")] public float SpeedRightArm;
|
||||||
|
|
||||||
|
[AsLowerIO(desc = "左夹臂实际位置")] public float ActualPosLeftArm;
|
||||||
|
|
||||||
|
[AsLowerIO(desc = "右夹臂实际位置")] public float ActualPosRightArm;
|
||||||
|
|
||||||
|
[AsUpperIO(desc = "夹臂不同步报警")] public bool ClampOutOfSync = false;
|
||||||
|
#endregion
|
||||||
|
|
||||||
|
[AsLowerIO(desc = "左前左轮实际位置")] public float LFLActualPos;
|
||||||
|
[AsLowerIO(desc = "左前右轮实际位置")] public float LFRActualPos;
|
||||||
|
[AsLowerIO(desc = "右前左轮实际位置")] public float RFLActualPos;
|
||||||
|
[AsLowerIO(desc = "右前右轮实际位置")] public float RFRActualPos;
|
||||||
|
[AsLowerIO(desc = "左后左轮实际位置")] public float LRLActualPos;
|
||||||
|
[AsLowerIO(desc = "左后右轮实际位置")] public float LRRActualPos;
|
||||||
|
[AsLowerIO(desc = "右后左轮实际位置")] public float RRLActualPos;
|
||||||
|
[AsLowerIO(desc = "右后右轮实际位置")] public float RRRActualPos;
|
||||||
|
|
||||||
|
[AsLowerIO(desc = "左夹臂低限位")] public float LeftArmLowerPos;
|
||||||
|
[AsLowerIO(desc = "左夹臂高限位")] public float LeftArmUpperPos;
|
||||||
|
[AsLowerIO(desc = "右夹臂低限位")] public float RightArmLowerPos;
|
||||||
|
[AsLowerIO(desc = "右夹臂高限位")] public float RightArmUpperPos;
|
||||||
|
|
||||||
|
[AsUpperIO(desc = "从C往驱动器下使能")] public bool DisableFromC = false;
|
||||||
|
[AsUpperIO(desc = "从C上复位")] public bool ResetFromC = false;
|
||||||
|
|
||||||
private float _multiVehicleAccumulateTh;
|
private float _multiVehicleAccumulateTh;
|
||||||
private DateTime _multiVehicleLastThTime = DateTime.Now;
|
private DateTime _multiVehicleLastThTime = DateTime.Now;
|
||||||
private readonly object _multiVehicleNotificationLock = new();
|
private readonly object _multiVehicleNotificationLock = new();
|
||||||
|
|||||||
@@ -0,0 +1,467 @@
|
|||||||
|
using System;
|
||||||
|
using System.Collections.Generic;
|
||||||
|
using System.Drawing;
|
||||||
|
using System.Linq;
|
||||||
|
using System.Numerics;
|
||||||
|
using System.Reflection;
|
||||||
|
using System.Text;
|
||||||
|
using System.Threading;
|
||||||
|
using ClumsyCore;
|
||||||
|
using ClumsyCore.DTools;
|
||||||
|
using ClumsyCore.Interfaces;
|
||||||
|
using ClumsyCore.Pilot;
|
||||||
|
using ClumsyCore.Utilities;
|
||||||
|
using ClumsyDance.ClumsyWalk.Detectors;
|
||||||
|
using CommonUsage.Chassis;
|
||||||
|
using FundamentalLib;
|
||||||
|
using MDCSToolBox;
|
||||||
|
using MDCSToolBox.Clumsy.Calibration;
|
||||||
|
using MDCSToolBox.Clumsy.MotionControllers;
|
||||||
|
using MDCSToolBox.Clumsy.Movements;
|
||||||
|
using MDCSToolBox.Clumsy.Pilot;
|
||||||
|
using MDCSToolBox.Clumsy.Tracks;
|
||||||
|
using MDCSToolBox.Commons.Controllers;
|
||||||
|
using static ClumsyCore.DTools.Painter;
|
||||||
|
using LineSegment = ClumsyCore.Utilities.LineSegment;
|
||||||
|
|
||||||
|
namespace MultiWheelC
|
||||||
|
{
|
||||||
|
public class TireFollowing : MovementDefinition
|
||||||
|
{
|
||||||
|
public Func<AbstractGeometricController> GetController;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 车辆方向
|
||||||
|
/// </summary>
|
||||||
|
public float CarDirection = 0;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 停止距离
|
||||||
|
/// </summary>
|
||||||
|
//public float FinishDistance = 1000;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 减速距离
|
||||||
|
/// </summary>
|
||||||
|
public float SlowDistance = 1000;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 最大速度
|
||||||
|
/// </summary>
|
||||||
|
public float MaxSpeed = 0.3f;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 钻轮胎数量
|
||||||
|
/// </summary>
|
||||||
|
public int TireNum = 1;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 盲走角度偏移
|
||||||
|
/// </summary>
|
||||||
|
public float WalkBlindTh = -1f;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 是否检测到目标
|
||||||
|
/// </summary>
|
||||||
|
public bool NoTarget = false;
|
||||||
|
|
||||||
|
public float GuessRangeX;
|
||||||
|
|
||||||
|
public float GuessRangeY;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 检测器定义
|
||||||
|
/// </summary>
|
||||||
|
public class DetectorDefinition
|
||||||
|
{
|
||||||
|
/// <summary>
|
||||||
|
/// 开始检测距离
|
||||||
|
/// </summary>
|
||||||
|
public float StartGuessingX;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 开始检测距离
|
||||||
|
/// </summary>
|
||||||
|
public float StartGuessingY;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 检测函数
|
||||||
|
/// </summary>
|
||||||
|
public Func<float, float, List<DetectFilter>, LineSegment> DetectFunction = null;
|
||||||
|
|
||||||
|
public Action<int> LeaveSrcFunction = null;
|
||||||
|
|
||||||
|
public int SrcId = -1;
|
||||||
|
public int DstId = -1;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 路径偏移
|
||||||
|
/// </summary>
|
||||||
|
public Tuple<float, float, float> PathTransformation = Tuple.Create(0f, 0f, 0f);
|
||||||
|
|
||||||
|
public float PathTransformationAnchorDistance = 0f;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 切换条件
|
||||||
|
/// </summary>
|
||||||
|
public Func<float, bool> SwitchWalkBlindCondition = null;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 盲走停止距离
|
||||||
|
/// </summary>
|
||||||
|
public Func<float, bool> FinishWalkBlindCondition = null;
|
||||||
|
}
|
||||||
|
|
||||||
|
public Func<bool> FinishCondition;
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
/// 多个检测器列表
|
||||||
|
/// </summary>
|
||||||
|
public List<DetectorDefinition> detectors = null;
|
||||||
|
|
||||||
|
private Painter _painter;
|
||||||
|
private List<float> _remainDistanceList = new List<float>();
|
||||||
|
private List<float> _remainAngleList = new List<float>();
|
||||||
|
|
||||||
|
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 - GuessRangeX, guessCenterY - GuessRangeY),
|
||||||
|
new (guessCenterX + GuessRangeX, guessCenterY - GuessRangeY),
|
||||||
|
new (guessCenterX + GuessRangeX, guessCenterY + GuessRangeY),
|
||||||
|
new (guessCenterX - GuessRangeX, guessCenterY + GuessRangeY),
|
||||||
|
};
|
||||||
|
|
||||||
|
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))),
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
public void Stop()
|
||||||
|
{
|
||||||
|
_dt?.Stop();
|
||||||
|
}
|
||||||
|
|
||||||
|
/// <summary>
|
||||||
|
///计算车体中心的位移和角度增量
|
||||||
|
/// </summary>
|
||||||
|
/// <param name="a">a轮在车体坐标系下位置</param>
|
||||||
|
/// <param name="va">a轮在车体坐标系下位移增量</param>
|
||||||
|
/// <param name="b">b轮在车体坐标系下位置</param>
|
||||||
|
/// <param name="vb">b轮在车体坐标系下位移增量</param>
|
||||||
|
/// <returns></returns>
|
||||||
|
private static (float, float, float) CenterMoveFromPoints(Vector2 a,
|
||||||
|
Vector2 aDelta,
|
||||||
|
Vector2 b,
|
||||||
|
Vector2 bDelta)
|
||||||
|
{
|
||||||
|
float th_x = 0, th_y = 0, th = 0, x = 0, y = 0;
|
||||||
|
var eps = 0.0000001;
|
||||||
|
if (Math.Abs(a.Y - b.Y) > eps)
|
||||||
|
{
|
||||||
|
th_x = (aDelta.X - bDelta.X) / (b.Y - a.Y);
|
||||||
|
}
|
||||||
|
if (Math.Abs(a.X - b.X) > eps)
|
||||||
|
{
|
||||||
|
th_y = (aDelta.Y - bDelta.Y) / (a.X - b.X);
|
||||||
|
}
|
||||||
|
th = th_x == 0 ? th_y : th_x;
|
||||||
|
|
||||||
|
x = (aDelta.X + bDelta.X) / 2 - (a.Y - b.Y) / 2 * th;
|
||||||
|
|
||||||
|
y = (aDelta.Y + bDelta.Y) / 2 + (a.X - b.X) / 2 * th;
|
||||||
|
|
||||||
|
return (x, y, th);
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
public override IEnumerable<bool> Get()
|
||||||
|
{
|
||||||
|
_painter = UI.GetPainter("GeneralFollowing", false);
|
||||||
|
var lastDetectX = detectors[0].StartGuessingX;
|
||||||
|
var lastDetectY = detectors[0].StartGuessingY;
|
||||||
|
var detectorIndex = 0;
|
||||||
|
|
||||||
|
var controller = (MultiWheelGeometricController)GetController.Invoke();
|
||||||
|
controller.BaseSpeed = MaxSpeed;
|
||||||
|
controller.FinishDistance = float.MinValue;
|
||||||
|
controller.FirstThAccuracy = 999;
|
||||||
|
_dt = new DriveTask(controller.Track(true, CoordinateSystem.Car2D));
|
||||||
|
|
||||||
|
float WalkBlindCarPathDstX = -1f, WalkBlindCarPathDstY = -1f, WalkBlindCarPathDstTh = -1f;
|
||||||
|
bool WalkBlindStage1 = false, WalkBlindStage2 = false;
|
||||||
|
var angle2target = -1f;
|
||||||
|
float _lastLFLEncoder = -1, _lastLFREncoder = -1, _lastRFLEncoder = -1, _lastRFREncoder = -1;
|
||||||
|
float _lastLRLEncoder = -1, _lastLRREncoder = -1, _lastRRLEncoder = -1, _lastRRREncoder = -1;
|
||||||
|
|
||||||
|
(float, float, float) GetCurrentPos2Dst(float lastX, float lastY, float lastTh)
|
||||||
|
{
|
||||||
|
// Read current encoders
|
||||||
|
var curLFLEncoder = PilotDefinition.Self.LFLActualPos;
|
||||||
|
var curLFREncoder = PilotDefinition.Self.LFRActualPos;
|
||||||
|
var curRFLEncoder = PilotDefinition.Self.RFLActualPos;
|
||||||
|
var curRFREncoder = PilotDefinition.Self.RFRActualPos;
|
||||||
|
var curLRLEncoder = PilotDefinition.Self.LRLActualPos;
|
||||||
|
var curLRREncoder = PilotDefinition.Self.LRRActualPos;
|
||||||
|
var curRRLEncoder = PilotDefinition.Self.RRLActualPos;
|
||||||
|
var curRRREncoder = PilotDefinition.Self.RRRActualPos;
|
||||||
|
|
||||||
|
// Average delta per wheel pair (LF, LR, RF, RR)
|
||||||
|
var lfDelta = (curLFLEncoder - _lastLFLEncoder + curLFREncoder - _lastLFREncoder) / 2;
|
||||||
|
var lrDelta = (curLRLEncoder - _lastLRLEncoder + curLRREncoder - _lastLRREncoder) / 2;
|
||||||
|
var rfDelta = (curRFLEncoder - _lastRFLEncoder + curRFREncoder - _lastRFREncoder) / 2;
|
||||||
|
var rrDelta = (curRRLEncoder - _lastRRLEncoder + curRRREncoder - _lastRRREncoder) / 2;
|
||||||
|
var deltaList = new List<float> { lfDelta, lrDelta, rfDelta, rrDelta };
|
||||||
|
|
||||||
|
var xs = new List<float>();
|
||||||
|
var ys = new List<float>();
|
||||||
|
var ths = new List<float>();
|
||||||
|
var chassis = (MultiWheelChassis)BasicPilotBase.Chassis;
|
||||||
|
var steerWheels = chassis.GetSteerWheels();
|
||||||
|
for (var i = 0; i < steerWheels.Count; ++i)
|
||||||
|
{
|
||||||
|
var sw1 = steerWheels[i];
|
||||||
|
var a = sw1.Position;
|
||||||
|
var tha = sw1.ReadAngle() / 180 * (float)Math.PI;
|
||||||
|
var deltaa = deltaList[i];
|
||||||
|
var va = new Vector2(deltaa * (float)Math.Cos(tha), deltaa * (float)Math.Sin(tha));
|
||||||
|
for (var j = i + 1; j < steerWheels.Count; ++j)
|
||||||
|
{
|
||||||
|
var sw2 = steerWheels[j];
|
||||||
|
var b = sw2.Position;
|
||||||
|
var thb = sw2.ReadAngle() / 180 * (float)Math.PI;
|
||||||
|
var deltab = deltaList[j];
|
||||||
|
var vb = new Vector2(deltab * (float)Math.Cos(thb), deltab * (float)Math.Sin(thb));
|
||||||
|
var (tempx, tempy, tempth) = CenterMoveFromPoints(a, va, b, vb);
|
||||||
|
Hedingben.ToastText($"{tempx:f2} {tempy:f2} {tempth / Math.PI * 180:f2} ", $"{i}_{j}");
|
||||||
|
xs.Add(tempx);
|
||||||
|
ys.Add(tempy);
|
||||||
|
ths.Add(tempth);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
var x = xs.Average();
|
||||||
|
var y = ys.Average();
|
||||||
|
var Th = ths.Average() / (float)Math.PI * 180;
|
||||||
|
var moveTup = Tuple.Create(x, y, Th);
|
||||||
|
var moved = MathTools.SolveTransform2D(MathTools.SolveTransform2D(Tuple.Create(lastX, lastY, lastTh), moveTup), Tuple.Create(0f, 0f, 0f));
|
||||||
|
|
||||||
|
_lastLFLEncoder = curLFLEncoder;
|
||||||
|
_lastLFREncoder = curLFREncoder;
|
||||||
|
_lastRFLEncoder = curRFLEncoder;
|
||||||
|
_lastRFREncoder = curRFREncoder;
|
||||||
|
_lastLRLEncoder = curLRLEncoder;
|
||||||
|
_lastLRREncoder = curLRREncoder;
|
||||||
|
_lastRRLEncoder = curRRLEncoder;
|
||||||
|
_lastRRREncoder = curRRREncoder;
|
||||||
|
return (moved.Item1, moved.Item2, moved.Item3);
|
||||||
|
}
|
||||||
|
|
||||||
|
while (true)
|
||||||
|
{
|
||||||
|
if (detectorIndex > detectors.Count - 1)
|
||||||
|
throw new Exception("detector index out of range!");
|
||||||
|
|
||||||
|
_painter.Clear();
|
||||||
|
if (WalkBlindStage1 || WalkBlindStage2)
|
||||||
|
{
|
||||||
|
//第二次盲走时或只钻一个轮胎时
|
||||||
|
if (WalkBlindStage2 || detectors.Count == 1 || TireNum == 1)
|
||||||
|
{
|
||||||
|
controller.SlowDistance = SlowDistance;
|
||||||
|
controller.SlowingPow = 0.7f;
|
||||||
|
}
|
||||||
|
(WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh) = GetCurrentPos2Dst(WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh);
|
||||||
|
var walkBlindPathEnd = Tuple.Create(WalkBlindCarPathDstX, WalkBlindCarPathDstY, WalkBlindCarPathDstTh);
|
||||||
|
var walkBlindPathStart = LessMath.Transform2D(walkBlindPathEnd, Tuple.Create(CarDirection == 0 ? -3000f : 3000f, 0f, 0f));
|
||||||
|
var walkBlindPathDst = new Vector2(WalkBlindCarPathDstX, WalkBlindCarPathDstY);
|
||||||
|
var walkBlindPathSrc = new Vector2(walkBlindPathStart.Item1, walkBlindPathStart.Item2);
|
||||||
|
var walkBlindPath = new LineSegment(walkBlindPathSrc, walkBlindPathDst);
|
||||||
|
|
||||||
|
DLog.Log($"盲走目标点:{walkBlindPath.Src.X:F2} {walkBlindPath.Src.Y:F2} {walkBlindPath.Dst.X:F2} {walkBlindPath.Dst.Y:F2}", "TireFollowing");
|
||||||
|
_painter.DrawDot(Color.Purple, walkBlindPathDst, sz: 3);
|
||||||
|
_painter.DrawLine(Color.GreenYellow, walkBlindPath.Src, walkBlindPath.Dst, endArrow: true, width: 2);
|
||||||
|
|
||||||
|
var track = new LineTrack(walkBlindPath.Src, walkBlindPath.Dst);
|
||||||
|
track.CarDirectionBias = CarDirection;
|
||||||
|
controller.UpdateTracks(new List<AbstractTrack> { track });
|
||||||
|
var rd = (float)LessMath.PerpendicularPosition(0, 0, walkBlindPath.Dst.X, walkBlindPath.Dst.Y,
|
||||||
|
walkBlindPath.Src.X, walkBlindPath.Src.Y);
|
||||||
|
_remainDistanceList.Add(rd);
|
||||||
|
while (_remainDistanceList.Count > 3) _remainDistanceList.RemoveAt(0);
|
||||||
|
rd = _remainDistanceList.Average();
|
||||||
|
DLog.Log($"盲走投影点剩余距离:{rd:0.0} ", "TireFollowing");
|
||||||
|
// 检查是否达到盲走结束条件
|
||||||
|
if (detectors[detectorIndex].FinishWalkBlindCondition(rd))
|
||||||
|
{
|
||||||
|
if(WalkBlindStage1)
|
||||||
|
{
|
||||||
|
DLog.Log("达到第一次盲走停止距离，停下或开始钻第二对轮胎", "TireFollowing");
|
||||||
|
//if (detectors[detectorIndex].DstId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
|
||||||
|
//{
|
||||||
|
// detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].DstId);
|
||||||
|
// DLog.Log($"释放取车点{detectors[detectorIndex].DstId}", "TireFollowing");
|
||||||
|
//}
|
||||||
|
WalkBlindStage1 = false;
|
||||||
|
_remainAngleList.Clear();
|
||||||
|
_remainDistanceList.Clear();
|
||||||
|
detectorIndex++;
|
||||||
|
if (detectors.Count == 1 || TireNum == 1)
|
||||||
|
{
|
||||||
|
_dt.Stop();
|
||||||
|
yield return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if(WalkBlindStage2)
|
||||||
|
{
|
||||||
|
DLog.Log("达到第二对轮胎处，停止移动", "TireFollowing");
|
||||||
|
_dt.Stop();
|
||||||
|
yield return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
yield return true;
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
var target = detectors[detectorIndex].DetectFunction(CarDirection, lastDetectX,
|
||||||
|
SetFilters(lastDetectX, lastDetectY));
|
||||||
|
|
||||||
|
if(target == null)
|
||||||
|
{
|
||||||
|
DLog.Log("无目标，等待下一帧", "TireFollowing");
|
||||||
|
controller.FirstRotateMaxSpeed = 0;
|
||||||
|
yield return true;
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
else controller.FirstRotateMaxSpeed = 5;
|
||||||
|
|
||||||
|
var targetAngle = CalculateAngle2YAxis(target.Src, target.Dst);
|
||||||
|
var targetPos = new Vector2((target.Src.X + target.Dst.X) / 2, (target.Src.Y + target.Dst.Y) / 2);
|
||||||
|
var dis2target = (float)Math.Sqrt(Math.Pow(targetPos.X, 2) + Math.Pow(targetPos.Y, 2));
|
||||||
|
//距离较近以后角度容易跳变
|
||||||
|
if (dis2target < 1400 && Math.Abs(targetAngle) > PilotDefinition.Conf.TireFollowingAngleIgnoreThr)
|
||||||
|
{
|
||||||
|
yield return true;
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
else _remainAngleList.Add(targetAngle);
|
||||||
|
while (_remainAngleList.Count > 10) _remainAngleList.RemoveAt(0);
|
||||||
|
angle2target = _remainAngleList.Average();
|
||||||
|
var distanceLabelPos = targetPos / 2;
|
||||||
|
_painter.DrawLine(Color.Cyan, Vector2.Zero, targetPos, width: 2);
|
||||||
|
_painter.DrawText(Color.Yellow, $"{dis2target:F3}", distanceLabelPos.X, distanceLabelPos.Y);
|
||||||
|
|
||||||
|
var path = DetectorHelper.GetApproachPath(target, CoordinateSystem.Car2D, pathLen:3000,
|
||||||
|
bias: detectors[detectorIndex].PathTransformation,
|
||||||
|
biasAnchorDistance: detectors[detectorIndex].PathTransformationAnchorDistance);
|
||||||
|
|
||||||
|
if (path == null)
|
||||||
|
{
|
||||||
|
DLog.Log("no path!", "TireFollowing");
|
||||||
|
NoTarget = true;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
lastDetectX = ((target.Src + target.Dst) / 2).X;
|
||||||
|
lastDetectY = ((target.Src + target.Dst) / 2).Y;
|
||||||
|
_painter.DrawLine(Color.GreenYellow, path.CarPath.Src, path.CarPath.Dst, endArrow: true);
|
||||||
|
|
||||||
|
var rd = (float)LessMath.PerpendicularPosition(0, 0, path.CarPath.Dst.X, path.CarPath.Dst.Y,
|
||||||
|
path.CarPath.Src.X, path.CarPath.Src.Y);
|
||||||
|
_remainDistanceList.Add(rd);
|
||||||
|
while (_remainDistanceList.Count > 3) _remainDistanceList.RemoveAt(0);
|
||||||
|
rd = _remainDistanceList.Average();
|
||||||
|
_painter.DrawText(Color.Green, $"{rd:F3}", distanceLabelPos.X, distanceLabelPos.Y - 200);
|
||||||
|
|
||||||
|
if (detectorIndex < detectors.Count - 1)
|
||||||
|
{
|
||||||
|
controller.SlowDistance = 1;
|
||||||
|
if (detectors[detectorIndex].SwitchWalkBlindCondition(rd))
|
||||||
|
{
|
||||||
|
WalkBlindStage1 = true;
|
||||||
|
if (detectors[detectorIndex].SrcId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
|
||||||
|
{
|
||||||
|
detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId);
|
||||||
|
DLog.Log($"释放预取车点{detectors[detectorIndex].SrcId}", "TireFollowing");
|
||||||
|
}
|
||||||
|
WalkBlindCarPathDstX = path.CarPath.Dst.X;
|
||||||
|
WalkBlindCarPathDstY = path.CarPath.Dst.Y;
|
||||||
|
WalkBlindCarPathDstTh = angle2target + WalkBlindTh;
|
||||||
|
DLog.Log($"切换至第一次盲走时刻目标点:{WalkBlindCarPathDstX:F2} " +
|
||||||
|
$"{WalkBlindCarPathDstY:F2} " +
|
||||||
|
$"{WalkBlindCarPathDstTh:F2}", "TireFollowing");
|
||||||
|
_lastLFLEncoder = PilotDefinition.Self.LFLActualPos;
|
||||||
|
_lastLFREncoder = PilotDefinition.Self.LFRActualPos;
|
||||||
|
_lastRFLEncoder = PilotDefinition.Self.RFLActualPos;
|
||||||
|
_lastRFREncoder = PilotDefinition.Self.RFRActualPos;
|
||||||
|
_lastLRLEncoder = PilotDefinition.Self.LRLActualPos;
|
||||||
|
_lastLRREncoder = PilotDefinition.Self.LRRActualPos;
|
||||||
|
_lastRRLEncoder = PilotDefinition.Self.RRLActualPos;
|
||||||
|
_lastRRREncoder = PilotDefinition.Self.RRRActualPos;
|
||||||
|
_remainDistanceList.Clear();
|
||||||
|
lastDetectX = detectors[detectorIndex+1].StartGuessingX;
|
||||||
|
lastDetectY = detectors[detectorIndex+1].StartGuessingY;
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if (detectorIndex == detectors.Count - 1)
|
||||||
|
{
|
||||||
|
if (detectors[detectorIndex].SwitchWalkBlindCondition(rd))
|
||||||
|
{
|
||||||
|
WalkBlindStage2 = true;
|
||||||
|
WalkBlindCarPathDstX = path.CarPath.Dst.X;
|
||||||
|
WalkBlindCarPathDstY = path.CarPath.Dst.Y;
|
||||||
|
WalkBlindCarPathDstTh = angle2target + WalkBlindTh;
|
||||||
|
DLog.Log($"切换至最后一次盲走时刻目标点:{WalkBlindCarPathDstX:F2} " +
|
||||||
|
$"{WalkBlindCarPathDstY:F2} " +
|
||||||
|
$"{WalkBlindCarPathDstTh:F2}", "TireFollowing");
|
||||||
|
if(detectors.Count == 1)
|
||||||
|
{
|
||||||
|
if (detectors[detectorIndex].SrcId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
|
||||||
|
{
|
||||||
|
detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId);
|
||||||
|
DLog.Log($"释放预取车点{detectors[detectorIndex].SrcId}", "TireFollowing");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
_lastLFLEncoder = PilotDefinition.Self.LFLActualPos;
|
||||||
|
_lastLFREncoder = PilotDefinition.Self.LFRActualPos;
|
||||||
|
_lastRFLEncoder = PilotDefinition.Self.RFLActualPos;
|
||||||
|
_lastRFREncoder = PilotDefinition.Self.RFRActualPos;
|
||||||
|
_lastLRLEncoder = PilotDefinition.Self.LRLActualPos;
|
||||||
|
_lastLRREncoder = PilotDefinition.Self.LRRActualPos;
|
||||||
|
_lastRRLEncoder = PilotDefinition.Self.RRLActualPos;
|
||||||
|
_lastRRREncoder = PilotDefinition.Self.RRRActualPos;
|
||||||
|
_remainDistanceList.Clear();
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
DLog.Log($"投影点剩余距离:{rd:F2}", "TireFollowing");
|
||||||
|
var track = new LineTrack(path.CarPath.Src, path.CarPath.Dst);
|
||||||
|
track.CarDirectionBias = CarDirection;
|
||||||
|
controller.UpdateTracks(new List<AbstractTrack> { track });
|
||||||
|
NoTarget = false;
|
||||||
|
}
|
||||||
|
yield return true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
private static float CalculateAngle2YAxis(Vector2 point1, Vector2 point2)
|
||||||
|
{
|
||||||
|
return -(float)(Math.Atan((point1.X - point2.X) / (point1.Y - point2.Y)) * 180 / Math.PI);
|
||||||
|
}
|
||||||
|
|
||||||
|
private DriveTask _dt;
|
||||||
|
}
|
||||||
|
}
|
||||||
Reference in New Issue
Block a user