First_Dev_Dept/Tutorial
12
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

增加dstTracker

Browse Source
This commit is contained in:
shuai.li
2026-07-04 14:58:03 +08:00
parent fc7c8e243b
commit 22670d916b
4 changed files with 221 additions and 96 deletions
Download Patch File Download Diff File Expand all files Collapse all files
MultiWheel/MultiWheelC/AGV.cs
+97 -58
View File
@@ -1,5 +1,7 @@
using ClumsyCore;
using ClumsyCore.Interfaces;
using ClumsyCore.Sensors;
using CommonUsage.Chassis;
using FundamentalLib;
using MDCSToolBox.Clumsy.AgvInterfaces;
using MDCSToolBox.Clumsy.MotionControllers;
@@ -10,6 +12,7 @@ using System;
using System.Collections.Generic;
using System.Net.Http;
using System.Numerics;
using System.Security.Cryptography;
using System.Threading;
using System.Threading.Tasks;
using static ClumsyCore.DTools.Painter;
@@ -65,20 +68,20 @@ namespace MultiWheelC
}
}
//参数1：tireNum 需要钻过的轮胎对数量
//参数2：frontLidarDetect true:前雷达识别 false:后雷达识别
//参数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");
DLog.Log($"锁点{dstId}完成", "TireFollowing");
var lidarName = frontLidarDetect ? "前雷达" : "后雷达";
DLog.Log($"开始钻车动作，通过{lidarName}识别结果钻{tireNum}对轮胎", "TireFollowing");
if (tireNum != 1 && tireNum != 2)
{
DLog.Log($"TireNum必须是1或2 (当前输入:{tireNum})", "TireFollowing");
DLog.Log($"TireNum必须是1或2 (当前输入:{tireNum})", "TireFollowing");
return;
}
@@ -86,20 +89,20 @@ namespace MultiWheelC
{
while (!TryLock(dstId))
{
Console.WriteLine("等待锁取货点中...");
Console.WriteLine("等待锁取货点中...");
Thread.Sleep(200);
}
Console.WriteLine($"锁点{dstId}完成");
Console.WriteLine($"锁点{dstId}完成");
Thread.Sleep(1000);
Console.WriteLine($"开始钻车动作，通过{lidarName}识别结果钻{tireNum}对轮胎");
Console.WriteLine($"开始钻车动作，通过{lidarName}识别结果钻{tireNum}对轮胎");
Thread.Sleep(1000);
Leave(srcId);
Console.WriteLine($"开始第一段盲走，此时释放预取货点{srcId}");
Console.WriteLine($"开始第一段盲走，此时释放预取货点{srcId}");
Thread.Sleep(2000);
//Leave(dstId);
//Console.WriteLine($"结束第一段盲走，此时释放取货点{dstId}");
//Console.WriteLine($"结束第一段盲走，此时释放取货点{dstId}");
Thread.Sleep(2000);
Console.WriteLine($"结束钻车动作");
Console.WriteLine($"结束钻车动作");
return;
}
@@ -133,7 +136,7 @@ namespace MultiWheelC
0)
},
};
DLog.Log($"钻胎为{tireNum}", "TireFollowing");
DLog.Log($"钻胎为{tireNum}", "TireFollowing");
var following = new TireFollowing()
{
GetController = () => new ChassisController().Get(),
@@ -148,18 +151,18 @@ namespace MultiWheelC
};
var _dt = new DriveTask(following.Get());
_dt.Wait();
DLog.Log("钻车动作结束", "TireFollowing");
DLog.Log("钻车动作结束", "TireFollowing");
}
//离车一定是后雷达识别一个轮胎
//离车一定是后雷达识别一个轮胎
public void LeaveCar(int srcId, float srcX, float srcY, int dstId, float dstX, float dstY)
{
while (!TryLock(dstId))
{
Thread.Sleep(50);
}
DLog.Log($"锁点{dstId}完成", "TireFollowing");
DLog.Log($"开始钻车动作，通过后雷达识别结果钻1对轮胎", "TireFollowing");
DLog.Log($"锁点{dstId}完成", "TireFollowing");
DLog.Log($"开始钻车动作，通过后雷达识别结果钻1对轮胎", "TireFollowing");
var following = new TireFollowing()
{
@@ -173,7 +176,7 @@ namespace MultiWheelC
DetectFunction = (_, lastDetectX, filters) => TireDetect.Detect(lastDetectX, filters, false),
StartGuessingX = -PilotDefinition.Conf.TireFollowingStage2GuessX,
StartGuessingY = 0,
SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindSwitchingDistance,
SwitchWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingLeaveCarWalkBlindSwitchingDistance,
FinishWalkBlindCondition = rd => rd <= PilotDefinition.Conf.TireFollowingWalkBlindFinishDistance,
PathTransformation = new Tuple<float, float, float>(
PilotDefinition.Conf.TireFollowingLeaveCarBackLidarPathTransformationX,
@@ -190,39 +193,97 @@ namespace MultiWheelC
WalkBlindTh = 0,
TireNum = 1
};
var _dt = new DriveTask(following.Get());
_dt.Wait();
new DriveTask(new DstTracker()
IEnumerable<bool> LeaveThenFollow()
{
Src = new Vector2(srcX, srcY),
Dst = new Vector2(dstX, dstY),
}.Get()); Wait();
DLog.Log("钻车动作结束", "TireFollowing");
foreach (var running in following.Get())
{
if (!running) break;
yield return true;
}
DLog.Log($"释放锁点{srcId}完成", "TireFollowing");
DLog.Log("离车TireFollowing结束，开始DstTracker", "TireFollowing");
foreach (var running in new DstTracker()
{
Src = new Vector2(srcX, srcY),
Dst = new Vector2(dstX, dstY),
CarDirectionBias = 180f,
}.Get())
{
if (!running) break;
yield return true;
}
yield return false;
}
var _dt = new DriveTask(LeaveThenFollow());
_dt.Wait();
DLog.Log("离车动作1结束", "TireFollowing");
}
//驱动器上使能
public void LineTracking(int srcId, float srcX, float srcY, int dstId, float dstX, float dstY)
{
while (!TryLock(dstId))
{
Thread.Sleep(50);
}
DLog.Log($"锁点{dstId}完成", "TireFollowing");
IEnumerable<bool> TrackThenFollow()
{
foreach (var running in new LineTracking()
{
Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
LeaveSrcFunction = Leave,
SrcId = srcId,
}.Get())
{
if (!running) break;
yield return true;
}
DLog.Log("离车LineTracking结束，开始DstTracker", "TireFollowing");
foreach (var running in new DstTracker()
{
Src = new Vector2(srcX, srcY),
Dst = new Vector2(dstX, dstY),
}.Get())
{
if (!running) break;
yield return true;
}
yield return false;
}
var _dt = new DriveTask(TrackThenFollow());
_dt.Wait();
//Leave(srcId);
DLog.Log($"释放锁点{srcId}完成", "TireFollowing");
DLog.Log("离车动作2结束", "TireFollowing");
}
//驱动器上使能
public void DriverAble()
{
var dl = new DriveTask(new DriverAble() { }.Get());
dl.Wait();
DLog.Log("驱动器上使能完成", "TireFollowing");
DLog.Log("驱动器上使能完成", "TireFollowing");
}
//驱动器下使能
//驱动器下使能
public void DriverDisable()
{
var dl = new DriveTask(new DriverDisable() { }.Get());
dl.Wait();
DLog.Log("驱动器下使能完成", "TireFollowing");
DLog.Log("驱动器下使能完成", "TireFollowing");
}
// 夹抱：close 为 true 时关闭夹抱，否则打开夹抱。
// 夹抱：close 为 true 时关闭夹抱，否则打开夹抱。
public void ClamptoTarget(bool close)
{
if (PilotDefinition.Self.GhostMode)
{
Thread.Sleep(2000);
Console.WriteLine("夹抱完成");
Console.WriteLine("夹抱完成");
return;
}
new DriveTask(new ClampToTarget()
@@ -231,8 +292,8 @@ namespace MultiWheelC
RightClampTarget = close ? PilotDefinition.Self.RightArmUpperPos : PilotDefinition.Self.RightArmLowerPos
}.Get()).Wait();
}
// 车队联动-自动蟹行：供调度按绝对起终点触发。
// CarDirectionBias 表示车队方向相对路径方向的夹角，逆时针为正。
// 车队联动-自动蟹行：供调度按绝对起终点触发。
// CarDirectionBias 表示车队方向相对路径方向的夹角，逆时针为正。
public void FleetCrabWalk(float srcX, float srcY, int srcId, float dstX, float dstY, int dstId,
float speed, float CarDirectionBias)
{
@@ -256,7 +317,7 @@ namespace MultiWheelC
{
Thread.Sleep(50);
}
DLog.Log($"锁点{dstId}完成", "FleetCrabDbg");
DLog.Log($"锁点{dstId}完成", "FleetCrabDbg");
}
var action = new MultiWheelC.FleetCrabWalk
@@ -286,33 +347,11 @@ namespace MultiWheelC
if (srcId != -1)
{
Leave(srcId);
DLog.Log($"释放放车点{srcId}", "FleetCrabDbg");
DLog.Log($"释放放车点{srcId}", "FleetCrabDbg");
}
}
}
public void LineTracking(int srcId, float srcX, float srcY, int dstId, float dstX, float dstY)
{
while (!TryLock(dstId))
{
Thread.Sleep(50);
}
DLog.Log($"锁点{dstId}完成", "TireFollowing");
new DriveTask(new LineTracking()
{
Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
//LeaveSrcFunction = Leave,
//SrcId = srcId,
}.Get()).Wait();
new DriveTask(new DstTracker()
{
Src = new Vector2(srcX, srcY),
Dst = new Vector2(dstX, dstY),
}.Get());Wait();
Leave(srcId);
DLog.Log($"释放锁点{srcId}完成", "TireFollowing");
}
public void ChangeAvoidanceDistance(float stopDistance, float slowDistance)
{