535 lines
22 KiB
C#
535 lines
22 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;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
public void RotateToTarget(float target)
|
|||
|
|
{
|
|||
|
|
//if (!needrotate) return;
|
|||
|
|
var dl = new DriveTask(new MultiWheelRotateInPlace()
|
|||
|
|
{
|
|||
|
|
AngleTarget = target,
|
|||
|
|
PidparamsRead = () => new PIDParams()
|
|||
|
|
{
|
|||
|
|
Kp = PilotDefinition.Conf.TireFollowingThkp,
|
|||
|
|
Ki = PilotDefinition.Conf.TireFollowingThki,
|
|||
|
|
Kd = PilotDefinition.Conf.TireFollowingThkd,
|
|||
|
|
DeadZone = PilotDefinition.Conf.TireFollowingThDeadZone,
|
|||
|
|
SpeedAccPerSec = PilotDefinition.Conf.TireFollowingThSpeedAccPerSec,
|
|||
|
|
OutputUpperThreshold = PilotDefinition.Conf.TireFollowingThThresh,
|
|||
|
|
MaxI = PilotDefinition.Conf.TireFollowingThMaxI,
|
|||
|
|
}
|
|||
|
|
}.Get());
|
|||
|
|
dl.Wait();
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//参数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 = 0.25f,
|
|||
|
|
EnableHandover = true,
|
|||
|
|
HandoverDistance = 200f,
|
|||
|
|
HandoverSpeed = 0.3f,
|
|||
|
|
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,
|
|||
|
|
InitialSendSpeed = 0.3f
|
|||
|
|
}.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,
|
|||
|
|
EnableHandover = true,
|
|||
|
|
HandoverDistance = 200,
|
|||
|
|
HandoverSpeed = 0.3f,
|
|||
|
|
}.Get())
|
|||
|
|
{
|
|||
|
|
if (!running) break;
|
|||
|
|
yield return true;
|
|||
|
|
}
|
|||
|
|
DLog.Log($"释放锁点{srcId}完成", "TireFollowing");
|
|||
|
|
DLog.Log("离车LineTracking结束，开始DstTracker", "TireFollowing");
|
|||
|
|
while (!TryLock(426))
|
|||
|
|
{
|
|||
|
|
Thread.Sleep(20);
|
|||
|
|
}
|
|||
|
|
Leave(dstId);
|
|||
|
|
DLog.Log($"释放锁点{dstId}完成", "TireFollowing");
|
|||
|
|
foreach (var running in new DstTracker()
|
|||
|
|
{
|
|||
|
|
Src = new Vector2(srcX, srcY),
|
|||
|
|
Dst = new Vector2(dstX, dstY),
|
|||
|
|
InitialSendSpeed = 0.3f
|
|||
|
|
}.Get())
|
|||
|
|
{
|
|||
|
|
if (!running) break;
|
|||
|
|
yield return true;
|
|||
|
|
}
|
|||
|
|
yield return false;
|
|||
|
|
}
|
|||
|
|
var _dt = new DriveTask(TrackThenFollow());
|
|||
|
|
_dt.Wait();
|
|||
|
|
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 FleetCurveWalk(float srcX, float srcY, int srcId, float dstX, float dstY, int dstId,
|
|||
|
|
float speed, params float[] trackTypeInfo)
|
|||
|
|
{
|
|||
|
|
if (trackTypeInfo == null || trackTypeInfo.Length < 2)
|
|||
|
|
{
|
|||
|
|
DLog.Log("FleetCurveWalk abort: invalid trackTypeInfo, expected Bezier type info.", "FleetCurveDbg");
|
|||
|
|
Hedingben.ToastText("FleetCurve invalid trackTypeInfo", "FleetCurve");
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
var trackType = (int)trackTypeInfo[0];
|
|||
|
|
if (trackType != 2)
|
|||
|
|
{
|
|||
|
|
DLog.Log($"FleetCurveWalk abort: unsupported trackType={trackType}, only Bezier(type=2) is supported.",
|
|||
|
|
"FleetCurveDbg");
|
|||
|
|
Hedingben.ToastText("FleetCurve only supports Bezier trackType=2", "FleetCurve");
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
var controlPointNum = (int)trackTypeInfo[1];
|
|||
|
|
var expectedLength = 2 + controlPointNum * 2;
|
|||
|
|
if (controlPointNum < 3 || trackTypeInfo.Length < expectedLength)
|
|||
|
|
{
|
|||
|
|
DLog.Log(
|
|||
|
|
$"FleetCurveWalk abort: invalid Bezier trackTypeInfo. controlPointNum={controlPointNum}, " +
|
|||
|
|
$"length={trackTypeInfo.Length}, expected>={expectedLength}.",
|
|||
|
|
"FleetCurveDbg");
|
|||
|
|
Hedingben.ToastText("FleetCurve invalid Bezier trackTypeInfo", "FleetCurve");
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
BezierTrack track;
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
track = ProcessTrackTypeInfo(srcX, srcY, dstX, dstY, trackTypeInfo) as BezierTrack;
|
|||
|
|
}
|
|||
|
|
catch (Exception ex)
|
|||
|
|
{
|
|||
|
|
DLog.Log($"FleetCurveWalk abort: failed to process trackTypeInfo. {ex.Message}", "FleetCurveDbg");
|
|||
|
|
Hedingben.ToastText("FleetCurve failed to process track", "FleetCurve");
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
if (track == null)
|
|||
|
|
{
|
|||
|
|
DLog.Log("FleetCurveWalk abort: ProcessTrackTypeInfo did not return BezierTrack.", "FleetCurveDbg");
|
|||
|
|
Hedingben.ToastText("FleetCurve requires BezierTrack", "FleetCurve");
|
|||
|
|
return;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
track.Speed = speed;
|
|||
|
|
track.CarDirectionBias = 0f;
|
|||
|
|
|
|||
|
|
DLog.Log(
|
|||
|
|
$"call FleetCurveWalk(src=({srcX:0},{srcY:0},id:{srcId}), dst=({dstX:0},{dstY:0},id:{dstId}), " +
|
|||
|
|
$"speed={speed:0.000}, trackType={trackType}, controls={controlPointNum}, track={track.GetType().Name}, " +
|
|||
|
|
$"carDirectionBias=0.0)",
|
|||
|
|
"FleetCurveDbg");
|
|||
|
|
|
|||
|
|
if (dstId != -1)
|
|||
|
|
{
|
|||
|
|
while (!TryLock(dstId))
|
|||
|
|
{
|
|||
|
|
Thread.Sleep(50);
|
|||
|
|
}
|
|||
|
|
DLog.Log($"閿佺偣{dstId}瀹屾垚", "FleetCurveDbg");
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
var action = new MultiWheelC.FleetCurveWalk
|
|||
|
|
{
|
|||
|
|
Track = track,
|
|||
|
|
CurveSpeed = speed,
|
|||
|
|
CarDirectionBias = 0f,
|
|||
|
|
SlowDistance = PilotDefinition.Conf.FleetCurveSlowDistance,
|
|||
|
|
FinishDistance = PilotDefinition.Conf.FleetCurveFinishDistance,
|
|||
|
|
FinishSpeed = PilotDefinition.Conf.FleetCurveFinishSpeed,
|
|||
|
|
SlowingPow = PilotDefinition.Conf.FleetCurveSlowingPow,
|
|||
|
|
GcpThetaThreshold = PilotDefinition.Conf.FleetCrabGcpThetaThreshold,
|
|||
|
|
StartSyncTimeoutSec = PilotDefinition.Conf.FleetCrabStartSyncTimeoutSec
|
|||
|
|
};
|
|||
|
|
|
|||
|
|
try
|
|||
|
|
{
|
|||
|
|
new DriveTask(action.Get()).Wait();
|
|||
|
|
}
|
|||
|
|
finally
|
|||
|
|
{
|
|||
|
|
if (srcId != -1)
|
|||
|
|
{
|
|||
|
|
Leave(srcId);
|
|||
|
|
DLog.Log($"release srcId={srcId}", "FleetCurveDbg");
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
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;
|
|||
|
|
}
|
|||
|
|
}
|