408 lines
17 KiB
C#
408 lines
17 KiB
C#
using ClumsyCore;
|
||
using ClumsyCore.Interfaces;
|
||
using ClumsyCore.Sensors;
|
||
using CommonUsage.Chassis;
|
||
using CommonUsage.Mathematics;
|
||
using FundamentalLib;
|
||
using MDCSToolBox.Clumsy.AgvInterfaces;
|
||
using MDCSToolBox.Clumsy.Calibration;
|
||
using MDCSToolBox.Clumsy.MotionControllers;
|
||
using MDCSToolBox.Clumsy.Tracks;
|
||
using MDCSToolBox.Commons.Controllers;
|
||
using Newtonsoft.Json;
|
||
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;
|
||
|
||
namespace MultiWheelC
|
||
{
|
||
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($"钻胎为{tireNum}", "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 = tireNum,
|
||
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, float srcX, float srcY, int dstId, float dstX, float dstY)
|
||
{
|
||
while (!TryLock(dstId))
|
||
{
|
||
Thread.Sleep(50);
|
||
}
|
||
DLog.Log($"锁点{dstId}完成", "TireFollowing");
|
||
DLog.Log($"开始钻车动作，通过后雷达识别结果钻1对轮胎", "TireFollowing");
|
||
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||
chassis.SetOriginBias(0, 0, 0);
|
||
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),
|
||
LeaveSrcFunction = Leave,
|
||
SrcId = srcId,
|
||
DstId = dstId,
|
||
},
|
||
},
|
||
CarDirection = 180f,
|
||
SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
|
||
MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
|
||
WalkBlindTh = 0,
|
||
TireNum = 1
|
||
};
|
||
|
||
IEnumerable<bool> LeaveThenFollow()
|
||
{
|
||
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);
|
||
}
|
||
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||
chassis.SetOriginBias(0, 0, 0);
|
||
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");
|
||
}
|
||
|
||
//驱动器下使能
|
||
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();
|
||
}
|
||
// Fleet crab walk: convert scheduler src/dst into the same relative crab-walk path used by MovementTest.
|
||
public void FleetCrabWalk(float srcX, float srcY, int srcId, float dstX, float dstY, int dstId,
|
||
float speed)
|
||
{
|
||
var dx = dstX - srcX;
|
||
var dy = dstY - srcY;
|
||
var pathLength = (float)Math.Sqrt(dx * dx + dy * dy);
|
||
|
||
if (pathLength <= 1f)
|
||
{
|
||
DLog.Log("FleetCrabWalk abort: path length is too short.", "FleetCrabDbg");
|
||
return;
|
||
}
|
||
|
||
var self = PilotDefinition.Self;
|
||
if (!self.TryGetFleetCenterFromMembers(out var centerX, out var centerY, out var centerTh) &&
|
||
!self.TryGetFleetCenterFromSlam(out centerX, out centerY, out centerTh))
|
||
{
|
||
DLog.Log("FleetCrabWalk abort: failed to read fleet center.", "FleetCrabDbg");
|
||
Hedingben.ToastText("FleetCrab requires master localization", "FleetCrab");
|
||
return;
|
||
}
|
||
|
||
var pathAngle = (float)CommonMath.RoundTh((float)(Math.Atan2(dy, dx) / Math.PI * 180.0));
|
||
var crabAngle = (float)CommonMath.ThDiff(pathAngle, centerTh);
|
||
var targetBodyWorldHeading = (float)CommonMath.RoundTh(PilotDefinition.Conf.FleetCrabBodyWorldHeadingDeg);
|
||
var bodyToPathAngle = (float)CommonMath.ThDiff(pathAngle, targetBodyWorldHeading);
|
||
DLog.Log(
|
||
$"call FleetCrabWalk(src=({srcX:0},{srcY:0},id:{srcId}), dst=({dstX:0},{dstY:0},id:{dstId}), " +
|
||
$"len={pathLength:0.0}, speed={speed:0.000}, pathAngle={pathAngle:0.0}, " +
|
||
$"center=({centerX:0},{centerY:0},{centerTh:0.0}), crabAngle={crabAngle:0.0}, " +
|
||
$"targetBodyWorld={targetBodyWorldHeading:0.0}, bodyToPath={bodyToPathAngle:0.0})",
|
||
"FleetCrabDbg");
|
||
|
||
if (dstId != -1)
|
||
{
|
||
while (!TryLock(dstId))
|
||
{
|
||
Thread.Sleep(50);
|
||
}
|
||
DLog.Log($"锁点{dstId}完成", "FleetCrabDbg");
|
||
}
|
||
|
||
var action = new MultiWheelC.FleetCrabWalk
|
||
{
|
||
CrabAngleDeg = crabAngle,
|
||
BodyToPathAngleDeg = bodyToPathAngle,
|
||
CrabLengthMm = pathLength,
|
||
CrabSpeed = speed,
|
||
FleetCrabAccel = PilotDefinition.Conf.FleetCrabAccel,
|
||
FleetCrabStartAccel = PilotDefinition.Conf.FleetCrabStartAccel,
|
||
FleetCrabSlowDistance = PilotDefinition.Conf.FleetCrabSlowDistance,
|
||
FleetCrabFinishDistance = PilotDefinition.Conf.FleetCrabFinishDistance,
|
||
FleetCrabFinishSpeed = PilotDefinition.Conf.FleetCrabFinishSpeed,
|
||
FleetCrabSlowingPow = PilotDefinition.Conf.FleetCrabSlowingPow,
|
||
GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold
|
||
};
|
||
|
||
try
|
||
{
|
||
new DriveTask(action.Get()).Wait();
|
||
}
|
||
finally
|
||
{
|
||
if (srcId != -1)
|
||
{
|
||
Leave(srcId);
|
||
DLog.Log($"释放放车点{srcId}", "FleetCrabDbg");
|
||
}
|
||
}
|
||
}
|
||
|
||
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;
|
||
}
|
||
});
|
||
}
|
||
public float baseSpeed = 0;
|
||
}
|
||
}
|