264 lines
11 KiB
C#
264 lines
11 KiB
C#
using ClumsyCore;
|
||
using FundamentalLib;
|
||
using MDCSToolBox.Clumsy.AgvInterfaces;
|
||
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.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($"检测器数量为{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");
|
||
}
|
||
|
||
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;
|
||
}
|
||
});
|
||
|
||
}
|
||
}
|
||
}
|