update
This commit is contained in:
shuai.li
@@ -0,0 +1,534 @@
|
||||
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;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,100 @@
|
||||
using System;
|
||||
using System.Numerics;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using FundamentalLib;
|
||||
using MDCSToolBox.Clumsy.MotionControllers;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
public class ChassisController : MovementDefinition<MultiWheelGeometricController>
|
||||
{
|
||||
public float BaseSpeed = Configuration.conf.basicSpeed;
|
||||
|
||||
private DateTime _sendMotionDbgLast = DateTime.MinValue;
|
||||
|
||||
public override MultiWheelGeometricController Get()
|
||||
{
|
||||
return new MultiWheelGeometricController
|
||||
{
|
||||
Chassis = BasicPilotBase.Chassis,
|
||||
BaseSpeed = BaseSpeed,
|
||||
SlowDistance = PilotDefinition.Conf.SlowDistance,
|
||||
SlowingPow = PilotDefinition.Conf.SlowingPow,
|
||||
FinishDistance = PilotDefinition.Conf.FinishDistance,
|
||||
FinishSpeed = PilotDefinition.Conf.FinishSpeed,
|
||||
FirstThAccuracy = PilotDefinition.Conf.FirstThAccuracy,
|
||||
FirstRotateSpeedFac = PilotDefinition.Conf.FirstRotateSpeedFac,
|
||||
FirstRotateMaxSpeed = PilotDefinition.Conf.FirstRotateMaxSpeed,
|
||||
NotContinuousAngle = PilotDefinition.Conf.NotContinuousAngle,
|
||||
DebugMode = PilotDefinition.Conf.MotionDebugPrint,
|
||||
DebugCurvature = PilotDefinition.Conf.DebugCurvature,
|
||||
PowerSteeringLookAhead = PilotDefinition.Conf.PowerSteeringLookAhead,
|
||||
SpeedLookAhead = PilotDefinition.Conf.SpeedLookAhead,
|
||||
SpeedLookAheadCurveDiff = PilotDefinition.Conf.SpeedLookAheadCurveDiff,
|
||||
SpeedLookBackCurveDiff = PilotDefinition.Conf.SpeedLookBackCurveDiff,
|
||||
SpeedLimitCurveDiffMin = PilotDefinition.Conf.SpeedLimitCurveDiffMin,
|
||||
SpeedLimitCurveMin = PilotDefinition.Conf.SpeedLimitCurveMin,
|
||||
MaxRotateSpeed = PilotDefinition.Conf.MaxRotateSpeedCurveLimit,
|
||||
MaxRotateAcc = PilotDefinition.Conf.MaxRotateAccCurveLimit,
|
||||
GcpThetaThreshold = PilotDefinition.Conf.GcpThetaThreshold,
|
||||
DthLinearFac = PilotDefinition.Conf.DthLinearFac,
|
||||
DthLinearThreshold = PilotDefinition.Conf.DthLinearThreshold,
|
||||
BiasFac = PilotDefinition.Conf.BiasFac,
|
||||
BiasThreshold = PilotDefinition.Conf.BiasThreshold,
|
||||
|
||||
MultiVehicleSendMotion = (speed, frontTh, rearTh, idealPos, idealAngle) =>
|
||||
{
|
||||
var self = PilotDefinition.Self;
|
||||
self.MultiVehicleAutoEnabled = true;
|
||||
// A: 用固定锁对象（不再锁会被替换的字段引用）。
|
||||
int fleetCnt;
|
||||
lock (self.FleetLock)
|
||||
fleetCnt = self.MultiVehicleFleet.Count;
|
||||
|
||||
// 诊断（节流 ~300ms）：确认回调被调用、编队是否就绪、是否因数量不符提前 return（导致不下发速度）。
|
||||
if ((DateTime.Now - _sendMotionDbgLast).TotalMilliseconds >= 300)
|
||||
{
|
||||
_sendMotionDbgLast = DateTime.Now;
|
||||
DLog.Log(
|
||||
$"SENDMOTION speed={speed:0.000} fTh={frontTh:0.0} rTh={rearTh:0.0} " +
|
||||
$"ideal=({idealPos.X:0},{idealPos.Y:0},{idealAngle:0.0}) " +
|
||||
$"editCnt={fleetCnt}/{PilotDefinition.Conf.MultiVehicleFleetNum} " +
|
||||
$"earlyReturn={fleetCnt != PilotDefinition.Conf.MultiVehicleFleetNum}",
|
||||
"FleetCrabDbg");
|
||||
}
|
||||
|
||||
if (fleetCnt != PilotDefinition.Conf.MultiVehicleFleetNum)
|
||||
return;
|
||||
|
||||
self.MultiVehicleAutoVx = speed;
|
||||
self.MultiVehicleAutoFrontTh = frontTh;
|
||||
self.MultiVehicleAutoRearTh = rearTh;
|
||||
// D: 透传路径控制器算出的理想车队中心位姿（此前被丢弃），供各车按 layout 做前馈。
|
||||
self.MultiVehicleAutoIdealX = idealPos.X;
|
||||
self.MultiVehicleAutoIdealY = idealPos.Y;
|
||||
self.MultiVehicleAutoIdealTh = idealAngle;
|
||||
self.MultiVehicleAutoHasIdeal = true;
|
||||
// B: 标记命令新鲜度。路径结束/早退/卡顿不再刷新此时刻 → 主车超时后清零速度，避免滑行。
|
||||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||||
},
|
||||
|
||||
// G: 读取车队中心原子快照，避免跨线程读到撕裂的 x/y/th 组合。
|
||||
MultiVehicleGetFleetPos = () =>
|
||||
{
|
||||
var snap = PilotDefinition.Self.GetFleetCenterSnapshot();
|
||||
return new Location
|
||||
{
|
||||
x = snap.X,
|
||||
y = snap.Y,
|
||||
th = snap.Th,
|
||||
l_step = 1,
|
||||
tick = DateTime.Now.Ticks
|
||||
};
|
||||
}
|
||||
};
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,41 @@
|
||||
<Project Sdk="Microsoft.NET.Sdk">
|
||||
|
||||
<PropertyGroup>
|
||||
<TargetFramework>netstandard2.0</TargetFramework>
|
||||
<LangVersion>10</LangVersion>
|
||||
<AllowUnsafeBlocks>true</AllowUnsafeBlocks>
|
||||
<AppendTargetFrameworkToOutputPath>false</AppendTargetFrameworkToOutputPath>
|
||||
<OutputPath>..\..\build\Clumsy\</OutputPath>
|
||||
</PropertyGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<PackageReference Include="Newtonsoft.Json" Version="13.0.4" />
|
||||
<PackageReference Include="System.Numerics.Vectors" Version="4.6.1" />
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<Reference Include="CommonUsage">
|
||||
<HintPath>ref\CommonUsage.dll</HintPath>
|
||||
</Reference>
|
||||
<Reference Include="LessokajiWeaverUtilities">
|
||||
<HintPath>ref\LessokajiWeaverUtilities.dll</HintPath>
|
||||
</Reference>
|
||||
<Reference Include="MDCSToolBox">
|
||||
<HintPath>ref\MDCSToolBox.dll</HintPath>
|
||||
</Reference>
|
||||
<Reference Include="ClumsyCore">
|
||||
<HintPath>ref\RefClumsyCore.dll</HintPath>
|
||||
</Reference>
|
||||
<Reference Include="ClumsyDance">
|
||||
<HintPath>ref\RefClumsyDance.dll</HintPath>
|
||||
</Reference>
|
||||
<Reference Include="FundamentalLib">
|
||||
<HintPath>ref\RefFundamentalLib.dll</HintPath>
|
||||
</Reference>
|
||||
</ItemGroup>
|
||||
|
||||
<Target Name="PostBuild" AfterTargets="PostBuildEvent">
|
||||
<Exec Command="if not exist $(SolutionDir)build\Clumsy mkdir $(SolutionDir)build\Clumsy
if not exist $(SolutionDir)build\Clumsy_AGV2 mkdir $(SolutionDir)build\Clumsy_AGV2
copy /Y D:\MDCS\Release\Clumsy\ClumsyLite.exe $(SolutionDir)build\Clumsy\
copy /Y D:\MDCS\Release\Clumsy\ClumsyLite.deps.json $(SolutionDir)build\Clumsy\
copy /Y D:\MDCS\Release\Clumsy\ClumsyLite.runtimeconfig.json $(SolutionDir)build\Clumsy\
copy /Y D:\MDCS\Release\Clumsy\RefClumsyCore.dll $(SolutionDir)build\Clumsy\
copy /Y D:\MDCS\Release\Clumsy\RefClumsyDance.dll $(SolutionDir)build\Clumsy\
copy /Y D:\MDCS\Release\MDCSToolBox.dll $(SolutionDir)build\Clumsy\
copy /Y D:\MDCS\Release\CommonUsage.dll $(SolutionDir)build\Clumsy\
copy /Y D:\MDCS\Release\Clumsy\ClumsyLite.exe $(SolutionDir)build\Clumsy_AGV2\
copy /Y D:\MDCS\Release\Clumsy\ClumsyLite.deps.json $(SolutionDir)build\Clumsy_AGV2\
copy /Y D:\MDCS\Release\Clumsy\ClumsyLite.runtimeconfig.json $(SolutionDir)build\Clumsy_AGV2\
copy /Y D:\MDCS\Release\Clumsy\RefClumsyCore.dll $(SolutionDir)build\Clumsy_AGV2\
copy /Y D:\MDCS\Release\Clumsy\RefClumsyDance.dll $(SolutionDir)build\Clumsy_AGV2\
copy /Y D:\MDCS\Release\MDCSToolBox.dll $(SolutionDir)build\Clumsy_AGV2\
copy /Y D:\MDCS\Release\CommonUsage.dll $(SolutionDir)build\Clumsy_AGV2\
copy /Y $(TargetDir)$(TargetName).dll $(SolutionDir)build\Clumsy_AGV2\
copy /Y $(TargetDir)$(TargetName).pdb $(SolutionDir)build\Clumsy_AGV2\" />
|
||||
</Target>
|
||||
|
||||
</Project>
|
||||
@@ -0,0 +1,526 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Numerics;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using FundamentalLib;
|
||||
using CommonUsage.Chassis;
|
||||
using CommonUsage.Mathematics;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
// ===== 车队联动-自动蟹行动作 =====
|
||||
// 以当前车队中心为起点，构造指定方向和长度的直线路径；
|
||||
// 执行侧直接写 MultiVehicleAuto...，由 TickMultiVehicle 自动分支统一下发。
|
||||
//
|
||||
// 控制思路参考 MDCSToolbox 几何控制器，但实现收在 MultiWheelC 内：
|
||||
// 1) 读取主车 Detour 反推车队中心，计算沿直线的进度、横向偏差和车身目标朝向偏差；
|
||||
// 2) 根据横向偏差给前后 GCP 同向修正，根据车身目标朝向偏差给前后 GCP 反向修正；
|
||||
// 3) 根据终点距离减速，并发布 ideal fleet center 给从车做前馈。
|
||||
//
|
||||
// 前提：在主车(MultiVehicleMasterEndpoint=="/")运行，且主车有 Detour 定位。
|
||||
public class FleetCrabWalk : MovementDefinition
|
||||
{
|
||||
/// <summary>路径方向相对启动时车队朝向的夹角(deg，逆时针为正)。</summary>
|
||||
public float CrabAngleDeg = 45f;
|
||||
|
||||
/// <summary>路径方向相对车身目标朝向的夹角(deg，逆时针为正)。MovementTest 会设为 CrabAngleDeg，以保持启动时车身朝向。</summary>
|
||||
public float BodyToPathAngleDeg = 45f;
|
||||
|
||||
/// <summary>路径长度(mm)。</summary>
|
||||
public float CrabLengthMm = 2000f;
|
||||
|
||||
/// <summary>行驶速度(m/s)。</summary>
|
||||
public float CrabSpeed = 0.2f;
|
||||
|
||||
/// <summary>速度命令加速度限制(m/s^2)，小于等于 0 表示不限制。</summary>
|
||||
public float FleetCrabAccel = 0.2f;
|
||||
|
||||
/// <summary>预对齐后正式下发速度前 5 秒加速度限制(m/s^2)，小于等于 0 表示不限制。</summary>
|
||||
public float FleetCrabStartAccel = 0.01f;
|
||||
|
||||
/// <summary>末端开始减速距离(mm)。</summary>
|
||||
public float FleetCrabSlowDistance = 2000f;
|
||||
|
||||
/// <summary>完成距离(mm)，低于该剩余距离结束动作。</summary>
|
||||
public float FleetCrabFinishDistance = 20f;
|
||||
|
||||
/// <summary>末端最低速度(m/s)。</summary>
|
||||
public float FleetCrabFinishSpeed = 0.02f;
|
||||
|
||||
/// <summary>末端减速曲线指数。</summary>
|
||||
public float FleetCrabSlowingPow = 0.8f;
|
||||
|
||||
/// <summary>前后 GCP 舵角修正上限(deg)。</summary>
|
||||
public float GcpThetaThreshold = 95f;
|
||||
|
||||
private bool _stopping;
|
||||
|
||||
private void Cleanup()
|
||||
{
|
||||
var self = PilotDefinition.Self;
|
||||
self.MultiVehicleScriptVx = 0;
|
||||
self.MultiVehicleScriptVy = 0;
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
self.MultiVehicleScriptMode = 0;
|
||||
self.MultiVehicleScriptEnabled = false;
|
||||
self.MultiVehicleAutoVx = 0;
|
||||
self.MultiVehicleAutoFrontTh = 0;
|
||||
self.MultiVehicleAutoRearTh = 0;
|
||||
self.MultiVehicleAutoHasIdeal = false;
|
||||
self.MultiVehicleAutoEnabled = false;
|
||||
}
|
||||
|
||||
public void Stop()
|
||||
{
|
||||
_stopping = true;
|
||||
Cleanup();
|
||||
}
|
||||
|
||||
private static float Clamp(float value, float min, float max)
|
||||
{
|
||||
if (value < min) return min;
|
||||
if (value > max) return max;
|
||||
return value;
|
||||
}
|
||||
|
||||
private static float ClampAbs(float value, float limit)
|
||||
{
|
||||
var absLimit = Math.Abs(limit);
|
||||
if (absLimit <= 0) return value;
|
||||
if (value > absLimit) return absLimit;
|
||||
if (value < -absLimit) return -absLimit;
|
||||
return value;
|
||||
}
|
||||
|
||||
private static float Slew(float current, float target, float maxDelta)
|
||||
{
|
||||
if (maxDelta <= 0) return target;
|
||||
if (target > current + maxDelta) return current + maxDelta;
|
||||
if (target < current - maxDelta) return current - maxDelta;
|
||||
return target;
|
||||
}
|
||||
|
||||
private static float AverageAngle(float frontTh, float rearTh)
|
||||
{
|
||||
var diff = (float)CommonMath.ThDiff(frontTh, rearTh);
|
||||
return (float)CommonMath.RoundTh(rearTh + diff / 2f);
|
||||
}
|
||||
|
||||
private static void ResolveCrabDriveEquivalent(float speed, float rawFrontTh, float rawRearTh, float steerLimit,
|
||||
out float driveSpeed, out float frontTh, out float rearTh, out bool reverseEquivalent, out float rawBaseTh)
|
||||
{
|
||||
var limit = Math.Min(179f, Math.Max(1f, Math.Abs(steerLimit)));
|
||||
rawBaseTh = AverageAngle(rawFrontTh, rawRearTh);
|
||||
driveSpeed = speed;
|
||||
frontTh = rawFrontTh;
|
||||
rearTh = rawRearTh;
|
||||
reverseEquivalent = false;
|
||||
|
||||
if (rawBaseTh > limit)
|
||||
{
|
||||
frontTh = (float)CommonMath.RoundTh(frontTh - 180f);
|
||||
rearTh = (float)CommonMath.RoundTh(rearTh - 180f);
|
||||
driveSpeed = -driveSpeed;
|
||||
reverseEquivalent = true;
|
||||
}
|
||||
else if (rawBaseTh < -limit)
|
||||
{
|
||||
frontTh = (float)CommonMath.RoundTh(frontTh + 180f);
|
||||
rearTh = (float)CommonMath.RoundTh(rearTh + 180f);
|
||||
driveSpeed = -driveSpeed;
|
||||
reverseEquivalent = true;
|
||||
}
|
||||
|
||||
frontTh = ClampAbs(frontTh, limit);
|
||||
rearTh = ClampAbs(rearTh, limit);
|
||||
}
|
||||
|
||||
private static float ProbeSpeed(float speed)
|
||||
{
|
||||
return Math.Abs(speed) > 1e-4f ? speed : 1f;
|
||||
}
|
||||
|
||||
private static bool TryGetMotionYawSign(float frontTh, float rearTh, float driveSpeed, float controlRadius,
|
||||
out float yawSign)
|
||||
{
|
||||
yawSign = 0f;
|
||||
if (Math.Abs(CommonMath.ThDiff(frontTh, rearTh)) <= 1e-3f)
|
||||
return false;
|
||||
|
||||
var radius = Math.Max(1f, Math.Abs(controlRadius));
|
||||
Vector2 pFront = new(radius, 0), pRear = new(-radius, 0),
|
||||
normFront = CommonMath.Transform2D(pFront, frontTh + 90f, Vector2.UnitX),
|
||||
normRear = CommonMath.Transform2D(pRear, rearTh + 90f, Vector2.UnitX);
|
||||
var (intersect, center) = CommonMath.TwoLinesIntersection(pFront, normFront, pRear, normRear);
|
||||
if (!intersect)
|
||||
return false;
|
||||
|
||||
// Match MultiWheelChassis.SendMotion: the tangent side is selected by
|
||||
// rotCenter.Y > 1, and reverse-equivalent motion flips the yaw direction.
|
||||
var tangentSign = center.Y > 1f ? 1f : -1f;
|
||||
var speedSign = driveSpeed >= 0f ? 1f : -1f;
|
||||
yawSign = speedSign * tangentSign;
|
||||
return true;
|
||||
}
|
||||
|
||||
private static float GetYawSplitSign(float baseTh, float speed, float steerLimit, float controlRadius)
|
||||
{
|
||||
const float probeDth = 1f;
|
||||
ResolveCrabDriveEquivalent(ProbeSpeed(speed), baseTh + probeDth, baseTh - probeDth, steerLimit,
|
||||
out var probeSpeed, out var probeFrontTh, out var probeRearTh, out _, out _);
|
||||
return TryGetMotionYawSign(probeFrontTh, probeRearTh, probeSpeed, controlRadius, out var yawSign)
|
||||
? yawSign
|
||||
: 1f;
|
||||
}
|
||||
|
||||
private static float EstimateLateralVelocity(float bodyTh, float frontTh, float rearTh, float driveSpeed,
|
||||
Vector2 pathLeft)
|
||||
{
|
||||
var motionTh = (float)CommonMath.RoundTh(bodyTh + AverageAngle(frontTh, rearTh));
|
||||
var rad = motionTh / 180f * Math.PI;
|
||||
var dir = new Vector2((float)Math.Cos(rad), (float)Math.Sin(rad));
|
||||
if (driveSpeed < 0f)
|
||||
dir = -dir;
|
||||
return Vector2.Dot(dir, pathLeft);
|
||||
}
|
||||
|
||||
private static float ScoreBiasSign(float baseTh, float bodyTh, float speed, float steerLimit, Vector2 pathLeft,
|
||||
float lateral, float biasProbe)
|
||||
{
|
||||
ResolveCrabDriveEquivalent(ProbeSpeed(speed), baseTh + biasProbe, baseTh + biasProbe, steerLimit,
|
||||
out var probeSpeed, out var probeFrontTh, out var probeRearTh, out _, out _);
|
||||
var lateralVelocity = EstimateLateralVelocity(bodyTh, probeFrontTh, probeRearTh, probeSpeed, pathLeft);
|
||||
return -Math.Sign(lateral) * lateralVelocity;
|
||||
}
|
||||
|
||||
private static float GetLateralBiasSign(float baseTh, float bodyTh, float speed, float steerLimit, Vector2 pathLeft,
|
||||
float lateral)
|
||||
{
|
||||
if (Math.Abs(lateral) <= 1e-3f)
|
||||
return 1f;
|
||||
|
||||
const float probeBias = 1f;
|
||||
var positiveScore = ScoreBiasSign(baseTh, bodyTh, speed, steerLimit, pathLeft, lateral, probeBias);
|
||||
var negativeScore = ScoreBiasSign(baseTh, bodyTh, speed, steerLimit, pathLeft, lateral, -probeBias);
|
||||
return positiveScore >= negativeScore ? 1f : -1f;
|
||||
}
|
||||
|
||||
private static bool TryGetControlFleetCenter(PilotDefinition self, out float centerX, out float centerY,
|
||||
out float centerTh, out string source)
|
||||
{
|
||||
if (self.TryGetFleetCenterFromMembers(out centerX, out centerY, out centerTh))
|
||||
{
|
||||
source = "fleet";
|
||||
return true;
|
||||
}
|
||||
|
||||
if (self.TryGetFleetCenterFromSlam(out centerX, out centerY, out centerTh))
|
||||
{
|
||||
source = "slam";
|
||||
return true;
|
||||
}
|
||||
|
||||
source = "none";
|
||||
return false;
|
||||
}
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var self = PilotDefinition.Self;
|
||||
var conf = PilotDefinition.Conf;
|
||||
var chassis = BasicPilotBase.Chassis as MultiWheelChassis;
|
||||
if (chassis == null)
|
||||
{
|
||||
DLog.Log("ABORT: FleetCrabWalk requires MultiWheelChassis.", "FleetCrabDbg");
|
||||
yield break;
|
||||
}
|
||||
_stopping = false;
|
||||
|
||||
DLog.Log(
|
||||
$"ENTER master?={conf.MultiVehicleMasterEndpoint == "/"} endpoint={conf.MultiVehicleMasterEndpoint} " +
|
||||
$"fleetNum={conf.MultiVehicleFleetNum} useDetect={conf.MultiVehicleUseDetect} " +
|
||||
$"syncUseDetour={conf.MultiVehicleSyncUseDetour} useIdealCenter={conf.MultiVehicleAutoUseIdealCenter} " +
|
||||
$"autoFields=true pathMode=relative pathAngle={CrabAngleDeg:0.0} " +
|
||||
$"bodyToPath={BodyToPathAngleDeg:0.0} gcpLimit={GcpThetaThreshold:0.0} " +
|
||||
$"biasFac={conf.BiasFac:0.00} fleetCrabDthFac={conf.FleetCrabDthLinearFac:0.00}",
|
||||
"FleetCrabDbg");
|
||||
|
||||
if (conf.MultiVehicleMasterEndpoint != "/")
|
||||
{
|
||||
DLog.Log($"ABORT: 非主车 (endpoint={conf.MultiVehicleMasterEndpoint})", "FleetCrabDbg");
|
||||
Hedingben.ToastText("车队蟹行需在主车(主车端点=\"/\")运行", "FleetCrab");
|
||||
yield break;
|
||||
}
|
||||
|
||||
// 注意：getCartLocation() 在无有效 Detour 定位时会阻塞——若卡在这里且后面看不到 CENTER 日志，即定位未就绪。
|
||||
DLog.Log("主车校验通过，开始读取车队中心 (getCartLocation 无定位会阻塞)…", "FleetCrabDbg");
|
||||
if (!TryGetControlFleetCenter(self, out var x0, out var y0, out var theta, out var initialCenterSource))
|
||||
{
|
||||
DLog.Log("ABORT: TryGetFleetCenterFromSlam 返回 false (无定位)", "FleetCrabDbg");
|
||||
Hedingben.ToastText("车队蟹行需要主车 Detour 定位", "FleetCrab");
|
||||
yield break;
|
||||
}
|
||||
DLog.Log($"CENTER 车队中心=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg");
|
||||
|
||||
DLog.Log($"CENTER_SOURCE source={initialCenterSource} center=({x0:0},{y0:0},{theta:0.0})", "FleetCrabDbg");
|
||||
|
||||
var pathStart = new Vector2(x0, y0);
|
||||
var pathLengthMm = CrabLengthMm;
|
||||
var phi = CommonMath.RoundTh(theta + CrabAngleDeg);
|
||||
var dst = CommonMath.Transform2D(pathStart, phi, new Vector2(pathLengthMm, 0));
|
||||
var targetBodyTh = CommonMath.RoundTh(phi - BodyToPathAngleDeg);
|
||||
var phiRad = phi / 180.0 * Math.PI;
|
||||
var pathDir = new Vector2((float)Math.Cos(phiRad), (float)Math.Sin(phiRad));
|
||||
var pathLeft = new Vector2(-pathDir.Y, pathDir.X);
|
||||
|
||||
DLog.Log(
|
||||
$"START center=({x0:0},{y0:0},{theta:0.0}) pathMode=relative " +
|
||||
$"src=({pathStart.X:0},{pathStart.Y:0}) pathAngle={CrabAngleDeg:0.0} bodyToPath={BodyToPathAngleDeg:0.0} " +
|
||||
$"phi={phi:0.0} targetBody={targetBodyTh:0.0} " +
|
||||
$"len={pathLengthMm:0} dst=({dst.X:0},{dst.Y:0}) speed={CrabSpeed:0.000} startAccel={FleetCrabStartAccel:0.000} accel={FleetCrabAccel:0.000} " +
|
||||
$"slow={FleetCrabSlowDistance:0} finishDist={FleetCrabFinishDistance:0} " +
|
||||
$"finishSpeed={FleetCrabFinishSpeed:0.000} slowingPow={FleetCrabSlowingPow:0.00}",
|
||||
"FleetCrabDbg");
|
||||
|
||||
var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold));
|
||||
var controlRadius = Math.Max(1f, Math.Abs(conf.TestCarSyncDistance) / 2f);
|
||||
ResolveCrabDriveEquivalent(0f, (float)CommonMath.ThDiff(phi, theta),
|
||||
(float)CommonMath.ThDiff(phi, theta), gcpLimit, out _, out var holdFrontTh, out var holdRearTh,
|
||||
out _, out _);
|
||||
var warmStart = DateTime.Now;
|
||||
var warmSeqBaseline = self.BeginFleetMotionWarmup();
|
||||
|
||||
self.MultiVehicleScriptEnabled = false;
|
||||
self.MultiVehicleScriptMode = 0;
|
||||
self.MultiVehicleScriptVx = 0;
|
||||
self.MultiVehicleScriptVy = 0;
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
self.MultiVehicleAutoEnabled = true;
|
||||
self.MultiVehicleAutoVx = 0;
|
||||
self.MultiVehicleAutoFrontTh = holdFrontTh;
|
||||
self.MultiVehicleAutoRearTh = holdRearTh;
|
||||
self.MultiVehicleAutoIdealX = pathStart.X;
|
||||
self.MultiVehicleAutoIdealY = pathStart.Y;
|
||||
self.MultiVehicleAutoIdealTh = targetBodyTh;
|
||||
self.MultiVehicleAutoHasIdeal = true;
|
||||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||||
self.PrimeMasterAutoFromSlam();
|
||||
DLog.Log(
|
||||
$"WARMUP auto fields enabled, waiting for fleet startup sync seqBase={warmSeqBaseline} " +
|
||||
$"hold=({holdFrontTh:0.00},{holdRearTh:0.00})",
|
||||
"FleetCrabDbg");
|
||||
|
||||
var warmEnd = warmStart.AddSeconds(Math.Max(1.0f, conf.FleetCrabStartSyncTimeoutSec));
|
||||
var warmIter = 0;
|
||||
var warmReady = false;
|
||||
var warmDetail = "";
|
||||
while (!_stopping && DateTime.Now < warmEnd)
|
||||
{
|
||||
warmIter++;
|
||||
self.MultiVehicleScriptEnabled = false;
|
||||
self.MultiVehicleScriptMode = 0;
|
||||
self.MultiVehicleAutoEnabled = true;
|
||||
self.MultiVehicleAutoVx = 0;
|
||||
self.MultiVehicleAutoFrontTh = holdFrontTh;
|
||||
self.MultiVehicleAutoRearTh = holdRearTh;
|
||||
self.MultiVehicleAutoIdealX = pathStart.X;
|
||||
self.MultiVehicleAutoIdealY = pathStart.Y;
|
||||
self.MultiVehicleAutoIdealTh = targetBodyTh;
|
||||
self.MultiVehicleAutoHasIdeal = true;
|
||||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||||
self.PrimeMasterAutoFromSlam();
|
||||
var snap = self.GetFleetCenterSnapshot();
|
||||
int cnt;
|
||||
lock (self.FleetLock) cnt = self.MultiVehicleFleet.Count;
|
||||
if (warmIter % 5 == 0)
|
||||
DLog.Log(
|
||||
$"WARMUP#{warmIter} 快照=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) tick={snap.Tick} " +
|
||||
$"autoEn={self.MultiVehicleAutoEnabled} scriptEn={self.MultiVehicleScriptEnabled} cnt={cnt}/{conf.MultiVehicleFleetNum} " +
|
||||
$"detail={warmDetail}",
|
||||
"FleetCrabDbg");
|
||||
if (self.IsFleetMotionWarmupReady(warmStart, warmSeqBaseline,
|
||||
conf.TestCarSyncTh, conf.TestCarSyncDistance, out warmDetail))
|
||||
{
|
||||
warmReady = true;
|
||||
DLog.Log(
|
||||
$"WARMUP done iter={warmIter} 快照=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) cnt={cnt} detail={warmDetail}",
|
||||
"FleetCrabDbg");
|
||||
break;
|
||||
}
|
||||
yield return true;
|
||||
}
|
||||
if (!warmReady)
|
||||
{
|
||||
DLog.Log($"WARMUP timeout: fleet startup sync failed, abort action. detail={warmDetail}",
|
||||
"FleetCrabDbg");
|
||||
Hedingben.ToastText("车队蟹行启动同步超时，已取消", "FleetCrab");
|
||||
Cleanup();
|
||||
yield break;
|
||||
}
|
||||
|
||||
Hedingben.ToastText($"车队蟹行 路径{phi:0.0}° 车身夹角{BodyToPathAngleDeg:0.0}° 长度{pathLengthMm:0}mm", "FleetCrab");
|
||||
|
||||
if (warmReady && self.TryGetFleetCenterFromMembers(out var warmX, out var warmY, out var warmTh))
|
||||
{
|
||||
x0 = warmX;
|
||||
y0 = warmY;
|
||||
theta = warmTh;
|
||||
pathStart = new Vector2(x0, y0);
|
||||
phi = CommonMath.RoundTh(theta + CrabAngleDeg);
|
||||
dst = CommonMath.Transform2D(pathStart, phi, new Vector2(pathLengthMm, 0));
|
||||
targetBodyTh = CommonMath.RoundTh(phi - BodyToPathAngleDeg);
|
||||
phiRad = phi / 180.0 * Math.PI;
|
||||
pathDir = new Vector2((float)Math.Cos(phiRad), (float)Math.Sin(phiRad));
|
||||
pathLeft = new Vector2(-pathDir.Y, pathDir.X);
|
||||
self.MultiVehicleAutoIdealX = pathStart.X;
|
||||
self.MultiVehicleAutoIdealY = pathStart.Y;
|
||||
self.MultiVehicleAutoIdealTh = targetBodyTh;
|
||||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||||
DLog.Log(
|
||||
$"WARMUP_REBASE source=fleet center=({x0:0},{y0:0},{theta:0.0}) phi={phi:0.0} targetBody={targetBodyTh:0.0} dst=({dst.X:0},{dst.Y:0})",
|
||||
"FleetCrabDbg");
|
||||
}
|
||||
|
||||
var iter = 0;
|
||||
var lastLog = DateTime.MinValue;
|
||||
var finishDistance = Math.Max(0f, FleetCrabFinishDistance);
|
||||
var slowDistance = Math.Max(finishDistance + 1f, FleetCrabSlowDistance);
|
||||
var baseSpeed = Math.Abs(CrabSpeed);
|
||||
var finishSpeed = Math.Min(baseSpeed, Math.Abs(FleetCrabFinishSpeed));
|
||||
var slowingPow = Math.Max(0.01f, FleetCrabSlowingPow);
|
||||
var accel = Math.Abs(FleetCrabAccel);
|
||||
var startAccel = Math.Abs(FleetCrabStartAccel);
|
||||
var cmdSpeed = 0f;
|
||||
var lastTick = DateTime.Now;
|
||||
var speedRampStart = DateTime.Now;
|
||||
var stopReason = "done";
|
||||
|
||||
while (!_stopping)
|
||||
{
|
||||
iter++;
|
||||
|
||||
if (!TryGetControlFleetCenter(self, out var cx, out var cy, out var cth, out var centerSource))
|
||||
{
|
||||
stopReason = "fleet center invalid";
|
||||
DLog.Log("ABORT: TryGetControlFleetCenter returned false during auto crab.", "FleetCrabDbg");
|
||||
break;
|
||||
}
|
||||
var delta = new Vector2(cx - pathStart.X, cy - pathStart.Y);
|
||||
var along = Vector2.Dot(delta, pathDir);
|
||||
var lateral = Vector2.Dot(delta, pathLeft);
|
||||
var remain = pathLengthMm - along;
|
||||
if (remain <= finishDistance)
|
||||
break;
|
||||
|
||||
var targetSpeed = baseSpeed;
|
||||
var slowRatio = 1f;
|
||||
if (remain < slowDistance)
|
||||
{
|
||||
slowRatio = (float)Math.Pow(Clamp(Math.Max(0, remain) / slowDistance, 0f, 1f), slowingPow);
|
||||
targetSpeed = slowRatio * (baseSpeed - finishSpeed) + finishSpeed;
|
||||
}
|
||||
var now = DateTime.Now;
|
||||
var dt = Math.Max(0.001f, (float)(now - lastTick).TotalSeconds);
|
||||
lastTick = now;
|
||||
var rampElapsed = (now - speedRampStart).TotalSeconds;
|
||||
var activeAccel = rampElapsed < 5.0 ? startAccel : accel;
|
||||
var speed = activeAccel > 0 ? Slew(cmdSpeed, targetSpeed, activeAccel * dt) : targetSpeed;
|
||||
cmdSpeed = speed;
|
||||
|
||||
var baseCrabTh = (float)CommonMath.ThDiff(phi, cth);
|
||||
var headingErr = (float)CommonMath.ThDiff(targetBodyTh, cth);
|
||||
var headingErrReverse = (float)CommonMath.ThDiff(cth, targetBodyTh);
|
||||
var targetBodyToPath = (float)CommonMath.ThDiff(phi, targetBodyTh);
|
||||
var rawBiasMagnitude = (float)(Math.Atan(conf.BiasFac * Math.Abs(lateral) / 1000f /
|
||||
Math.Max(speed, 0.3f)) / Math.PI * 180.0);
|
||||
var biasSign = GetLateralBiasSign(baseCrabTh, cth, speed, gcpLimit, pathLeft, lateral);
|
||||
var rawBiasItem = rawBiasMagnitude * biasSign;
|
||||
var biasItem = ClampAbs(rawBiasItem, conf.BiasThreshold);
|
||||
var yawSplitSign = GetYawSplitSign(baseCrabTh + biasItem, speed, gcpLimit, controlRadius);
|
||||
var rawDthItem = conf.FleetCrabDthLinearFac * headingErr * yawSplitSign;
|
||||
var dthItem = ClampAbs(rawDthItem, conf.FleetCrabDthLinearThreshold);
|
||||
var rawFrontTh = baseCrabTh + biasItem + dthItem;
|
||||
var rawRearTh = baseCrabTh + biasItem - dthItem;
|
||||
ResolveCrabDriveEquivalent(speed, rawFrontTh, rawRearTh, gcpLimit, out var driveSpeed,
|
||||
out var frontTh, out var rearTh, out var reverseEquivalent, out var rawBaseTh);
|
||||
holdFrontTh = frontTh;
|
||||
holdRearTh = rearTh;
|
||||
var idealAlong = Clamp(along, 0f, pathLengthMm);
|
||||
var ideal = pathStart + pathDir * idealAlong;
|
||||
|
||||
self.MultiVehicleScriptEnabled = false;
|
||||
self.MultiVehicleScriptMode = 0;
|
||||
self.MultiVehicleScriptVx = 0;
|
||||
self.MultiVehicleScriptVy = 0;
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
self.MultiVehicleAutoEnabled = true;
|
||||
self.MultiVehicleAutoVx = driveSpeed;
|
||||
self.MultiVehicleAutoFrontTh = frontTh;
|
||||
self.MultiVehicleAutoRearTh = rearTh;
|
||||
self.MultiVehicleAutoIdealX = ideal.X;
|
||||
self.MultiVehicleAutoIdealY = ideal.Y;
|
||||
self.MultiVehicleAutoIdealTh = targetBodyTh;
|
||||
self.MultiVehicleAutoHasIdeal = true;
|
||||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||||
|
||||
if ((DateTime.Now - lastLog).TotalMilliseconds >= 300)
|
||||
{
|
||||
lastLog = DateTime.Now;
|
||||
var snap = self.GetFleetCenterSnapshot();
|
||||
int fleetCnt;
|
||||
lock (self.FleetLock) fleetCnt = self.MultiVehicleFleet.Count;
|
||||
DLog.Log(
|
||||
$"ITER#{iter} centerSrc={centerSource} center=({cx:0},{cy:0},{cth:0.0}) snap=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) " +
|
||||
$"along={along:0} lateral={lateral:0} remain={remain:0} headingErr={headingErr:0.0} " +
|
||||
$"baseTh={baseCrabTh:0.0} bias={biasItem:0.0} dth={dthItem:0.0} " +
|
||||
$"slowRatio={slowRatio:0.000} targetV={targetSpeed:0.000} rampT={rampElapsed:0.0} accel={activeAccel:0.000} auto=(vx:{driveSpeed:0.000},fTh:{frontTh:0.0},rTh:{rearTh:0.0}) " +
|
||||
$"ideal=({ideal.X:0},{ideal.Y:0},{targetBodyTh:0.0}) scriptEn={self.MultiVehicleScriptEnabled} " +
|
||||
$"cnt={fleetCnt}/{conf.MultiVehicleFleetNum}",
|
||||
"FleetCrabDbg");
|
||||
DLog.Log(
|
||||
$"CTRL iter={iter} centerSrc:{centerSource} phi:{phi:0.00} targetBody:{targetBodyTh:0.00} startTheta:{theta:0.00} " +
|
||||
$"cth:{cth:0.00} crabAngle:{CrabAngleDeg:0.00} bodyToPathCfg:{BodyToPathAngleDeg:0.00} " +
|
||||
$"targetBodyToPath:{targetBodyToPath:0.00} bodyToPathNow:{baseCrabTh:0.00} " +
|
||||
$"headingErr(target-current):{headingErr:0.00} reverse(current-target):{headingErrReverse:0.00} yawSign:{yawSplitSign:0} " +
|
||||
$"fleetCrabDthFac:{conf.FleetCrabDthLinearFac:0.000} rawDth:{rawDthItem:0.00} dth:{dthItem:0.00} dthLimit:{conf.FleetCrabDthLinearThreshold:0.00} " +
|
||||
$"lateral:{lateral:0.0} biasFac:{conf.BiasFac:0.000} biasSign:{biasSign:0} rawBias:{rawBiasItem:0.00} bias:{biasItem:0.00} biasLimit:{conf.BiasThreshold:0.00} " +
|
||||
$"baseTh:{baseCrabTh:0.00} rawBase:{rawBaseTh:0.00} rawOut(f:{rawFrontTh:0.00},r:{rawRearTh:0.00}) " +
|
||||
$"out(f:{frontTh:0.00},r:{rearTh:0.00}) gcpLimit:{gcpLimit:0.00} revEq:{reverseEquivalent} " +
|
||||
$"speedRaw:{speed:0.000} speed:{driveSpeed:0.000} rampT:{rampElapsed:0.0} accel:{activeAccel:0.000} along:{along:0.0} remain:{remain:0.0} ideal=({ideal.X:0.0},{ideal.Y:0.0},{targetBodyTh:0.00})",
|
||||
"FleetCrabHeadingDbg");
|
||||
}
|
||||
yield return true;
|
||||
}
|
||||
|
||||
if (_stopping)
|
||||
stopReason = "stop";
|
||||
|
||||
self.MultiVehicleAutoVx = 0;
|
||||
self.MultiVehicleAutoFrontTh = holdFrontTh;
|
||||
self.MultiVehicleAutoRearTh = holdRearTh;
|
||||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||||
DLog.Log(
|
||||
$"STOP_HOLD iter={iter} reason={stopReason} hold=(fTh:{holdFrontTh:0.0},rTh:{holdRearTh:0.0}) cmdSpeed={cmdSpeed:0.000}",
|
||||
"FleetCrabDbg");
|
||||
var settleEnd = DateTime.Now.AddMilliseconds(Math.Max(100, conf.MultiVehicleSyncInterval * 3));
|
||||
while (!_stopping && DateTime.Now < settleEnd)
|
||||
{
|
||||
self.MultiVehicleScriptEnabled = false;
|
||||
self.MultiVehicleScriptMode = 0;
|
||||
self.MultiVehicleAutoEnabled = true;
|
||||
self.MultiVehicleAutoVx = 0;
|
||||
self.MultiVehicleAutoFrontTh = holdFrontTh;
|
||||
self.MultiVehicleAutoRearTh = holdRearTh;
|
||||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||||
yield return true;
|
||||
}
|
||||
|
||||
Cleanup();
|
||||
Hedingben.ToastText("车队蟹行完成", "FleetCrab");
|
||||
DLog.Log($"DONE iter={iter} reason={stopReason}", "FleetCrabDbg");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,411 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Globalization;
|
||||
using System.Numerics;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using FundamentalLib;
|
||||
using CommonUsage.Chassis;
|
||||
using CommonUsage.Mathematics;
|
||||
using MDCSToolBox.Clumsy.MotionControllers;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
using MDCSToolBox.Clumsy.Tracks;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
public class FleetCurveWalk : MovementDefinition
|
||||
{
|
||||
public BezierTrack Track;
|
||||
public List<Vector2> ControlPoints = new();
|
||||
public float CurveSpeed = 0.2f;
|
||||
public float CarDirectionBias = 0f;
|
||||
public int BezierResolution = 100;
|
||||
public float SlowDistance = 2000f;
|
||||
public float FinishDistance = 20f;
|
||||
public float FinishSpeed = 0.02f;
|
||||
public float SlowingPow = 0.8f;
|
||||
public float GcpThetaThreshold = 95f;
|
||||
public float StartSyncTimeoutSec = 8f;
|
||||
|
||||
private bool _stopping;
|
||||
private MultiWheelGeometricController _controller;
|
||||
private MultiWheelChassis _chassis;
|
||||
private bool _savedControlPoints;
|
||||
private float _savedControlRadius;
|
||||
private Vector2 _savedGcp0;
|
||||
private Vector2 _savedGcp1;
|
||||
|
||||
public void Stop()
|
||||
{
|
||||
_stopping = true;
|
||||
if (_controller != null)
|
||||
_controller.BreakAndHold = true;
|
||||
Cleanup();
|
||||
}
|
||||
|
||||
public static bool TryParsePointList(string text, out List<Vector2> points, out string error)
|
||||
{
|
||||
points = new List<Vector2>();
|
||||
error = "";
|
||||
if (string.IsNullOrWhiteSpace(text))
|
||||
{
|
||||
error = "empty control point list";
|
||||
return false;
|
||||
}
|
||||
|
||||
var segments = text.Split(new[] { ';', '|' }, StringSplitOptions.RemoveEmptyEntries);
|
||||
for (var i = 0; i < segments.Length; i++)
|
||||
{
|
||||
var pair = segments[i].Split(new[] { ',', ' ', '\t' }, StringSplitOptions.RemoveEmptyEntries);
|
||||
if (pair.Length != 2)
|
||||
{
|
||||
error = $"invalid point #{i + 1}: {segments[i]}";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!TryParseFloat(pair[0], out var x) || !TryParseFloat(pair[1], out var y))
|
||||
{
|
||||
error = $"invalid number in point #{i + 1}: {segments[i]}";
|
||||
return false;
|
||||
}
|
||||
points.Add(new Vector2(x, y));
|
||||
}
|
||||
|
||||
if (points.Count < 3)
|
||||
{
|
||||
error = "Bezier curve requires at least 3 control points";
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
public static List<Vector2> BuildRelativeControlPoints(Vector2 start, float startTh, List<Vector2> relativePoints)
|
||||
{
|
||||
var source = relativePoints ?? new List<Vector2>();
|
||||
var normalized = new List<Vector2>();
|
||||
if (source.Count == 0 || Vector2.Distance(source[0], Vector2.Zero) > 1f)
|
||||
normalized.Add(Vector2.Zero);
|
||||
for (var i = 0; i < source.Count; i++)
|
||||
normalized.Add(source[i]);
|
||||
if (normalized.Count < 2)
|
||||
normalized.Add(new Vector2(1000f, 0f));
|
||||
if (normalized.Count < 3)
|
||||
normalized.Add(new Vector2(2000f, 0f));
|
||||
|
||||
var result = new List<Vector2>();
|
||||
for (var i = 0; i < normalized.Count; i++)
|
||||
result.Add(CommonMath.Transform2D(start, startTh, normalized[i]));
|
||||
return result;
|
||||
}
|
||||
|
||||
public static List<Vector2> BuildAgvControlPoints(float srcX, float srcY, float dstX, float dstY,
|
||||
params float[] controlPointCoords)
|
||||
{
|
||||
var src = new Vector2(srcX, srcY);
|
||||
var dst = new Vector2(dstX, dstY);
|
||||
var result = new List<Vector2>();
|
||||
if (controlPointCoords == null || controlPointCoords.Length == 0)
|
||||
{
|
||||
result.Add(src);
|
||||
result.Add((src + dst) / 2f);
|
||||
result.Add(dst);
|
||||
return result;
|
||||
}
|
||||
|
||||
if (controlPointCoords.Length % 2 != 0)
|
||||
throw new ArgumentException("FleetCurve controlPointCoords must contain x,y pairs.");
|
||||
|
||||
var supplied = new List<Vector2>();
|
||||
for (var i = 0; i < controlPointCoords.Length; i += 2)
|
||||
supplied.Add(new Vector2(controlPointCoords[i], controlPointCoords[i + 1]));
|
||||
|
||||
if (supplied.Count >= 3 &&
|
||||
Vector2.Distance(supplied[0], src) <= 10f &&
|
||||
Vector2.Distance(supplied[supplied.Count - 1], dst) <= 10f)
|
||||
return supplied;
|
||||
|
||||
result.Add(src);
|
||||
for (var i = 0; i < supplied.Count; i++)
|
||||
result.Add(supplied[i]);
|
||||
result.Add(dst);
|
||||
if (result.Count < 3)
|
||||
result.Insert(1, (src + dst) / 2f);
|
||||
return result;
|
||||
}
|
||||
|
||||
private static bool TryParseFloat(string text, out float value)
|
||||
{
|
||||
return float.TryParse(text, NumberStyles.Float, CultureInfo.InvariantCulture, out value) ||
|
||||
float.TryParse(text, out value);
|
||||
}
|
||||
|
||||
private static float ClampAbs(float value, float limit)
|
||||
{
|
||||
var absLimit = Math.Abs(limit);
|
||||
if (absLimit <= 0) return value;
|
||||
if (value > absLimit) return absLimit;
|
||||
if (value < -absLimit) return -absLimit;
|
||||
return value;
|
||||
}
|
||||
|
||||
private static bool TryGetControlFleetCenter(PilotDefinition self, out float centerX, out float centerY,
|
||||
out float centerTh, out string source)
|
||||
{
|
||||
if (self.TryGetFleetCenterFromMembers(out centerX, out centerY, out centerTh))
|
||||
{
|
||||
source = "fleet";
|
||||
return true;
|
||||
}
|
||||
|
||||
if (self.TryGetFleetCenterFromSlam(out centerX, out centerY, out centerTh))
|
||||
{
|
||||
source = "slam";
|
||||
return true;
|
||||
}
|
||||
|
||||
source = "none";
|
||||
return false;
|
||||
}
|
||||
|
||||
private void Cleanup()
|
||||
{
|
||||
var self = PilotDefinition.Self;
|
||||
self.MultiVehicleScriptVx = 0;
|
||||
self.MultiVehicleScriptVy = 0;
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
self.MultiVehicleScriptMode = 0;
|
||||
self.MultiVehicleScriptEnabled = false;
|
||||
self.MultiVehicleAutoVx = 0;
|
||||
self.MultiVehicleAutoFrontTh = 0;
|
||||
self.MultiVehicleAutoRearTh = 0;
|
||||
self.MultiVehicleAutoHasIdeal = false;
|
||||
self.MultiVehicleAutoEnabled = false;
|
||||
RestoreControlPointRadius();
|
||||
}
|
||||
|
||||
private void ApplyFleetControlPointRadius(MultiWheelChassis chassis, float radius)
|
||||
{
|
||||
if (!_savedControlPoints)
|
||||
{
|
||||
_chassis = chassis;
|
||||
_savedControlRadius = chassis.ControlPointRadius;
|
||||
var gcps = chassis.GetGeometricControlPoints();
|
||||
if (gcps.Count >= 2)
|
||||
{
|
||||
_savedGcp0 = gcps[0].Position;
|
||||
_savedGcp1 = gcps[1].Position;
|
||||
}
|
||||
_savedControlPoints = true;
|
||||
}
|
||||
|
||||
chassis.ControlPointRadius = radius;
|
||||
var points = chassis.GetGeometricControlPoints();
|
||||
if (points.Count >= 2)
|
||||
{
|
||||
points[0].Position = new Vector2(radius, 0);
|
||||
points[1].Position = new Vector2(-radius, 0);
|
||||
}
|
||||
}
|
||||
|
||||
private void RestoreControlPointRadius()
|
||||
{
|
||||
if (!_savedControlPoints || _chassis == null)
|
||||
return;
|
||||
|
||||
_chassis.ControlPointRadius = _savedControlRadius;
|
||||
var points = _chassis.GetGeometricControlPoints();
|
||||
if (points.Count >= 2)
|
||||
{
|
||||
points[0].Position = _savedGcp0;
|
||||
points[1].Position = _savedGcp1;
|
||||
}
|
||||
_savedControlPoints = false;
|
||||
}
|
||||
|
||||
private static void WriteWarmupAuto(PilotDefinition self, Vector2 idealPos, float idealTh,
|
||||
float frontTh, float rearTh)
|
||||
{
|
||||
self.MultiVehicleScriptEnabled = false;
|
||||
self.MultiVehicleScriptMode = 0;
|
||||
self.MultiVehicleScriptVx = 0;
|
||||
self.MultiVehicleScriptVy = 0;
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
self.MultiVehicleAutoEnabled = true;
|
||||
self.MultiVehicleAutoVx = 0;
|
||||
self.MultiVehicleAutoFrontTh = frontTh;
|
||||
self.MultiVehicleAutoRearTh = rearTh;
|
||||
self.MultiVehicleAutoIdealX = idealPos.X;
|
||||
self.MultiVehicleAutoIdealY = idealPos.Y;
|
||||
self.MultiVehicleAutoIdealTh = idealTh;
|
||||
self.MultiVehicleAutoHasIdeal = true;
|
||||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||||
}
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var self = PilotDefinition.Self;
|
||||
var conf = PilotDefinition.Conf;
|
||||
var chassis = BasicPilotBase.Chassis as MultiWheelChassis;
|
||||
_stopping = false;
|
||||
|
||||
if (chassis == null)
|
||||
{
|
||||
DLog.Log("ABORT: FleetCurveWalk requires MultiWheelChassis.", "FleetCurveDbg");
|
||||
yield break;
|
||||
}
|
||||
|
||||
if (conf.MultiVehicleMasterEndpoint != "/")
|
||||
{
|
||||
DLog.Log($"ABORT: FleetCurveWalk must run on master endpoint, endpoint={conf.MultiVehicleMasterEndpoint}",
|
||||
"FleetCurveDbg");
|
||||
Hedingben.ToastText("FleetCurve requires master vehicle", "FleetCurve");
|
||||
yield break;
|
||||
}
|
||||
|
||||
if (Track == null && (ControlPoints == null || ControlPoints.Count < 3))
|
||||
{
|
||||
DLog.Log("ABORT: FleetCurveWalk requires a BezierTrack or at least 3 control points.", "FleetCurveDbg");
|
||||
Hedingben.ToastText("FleetCurve requires track or >=3 control points", "FleetCurve");
|
||||
yield break;
|
||||
}
|
||||
|
||||
if (!TryGetControlFleetCenter(self, out var x0, out var y0, out var theta, out var initialCenterSource))
|
||||
{
|
||||
DLog.Log("ABORT: FleetCurveWalk failed to read fleet center.", "FleetCurveDbg");
|
||||
Hedingben.ToastText("FleetCurve requires master localization", "FleetCurve");
|
||||
yield break;
|
||||
}
|
||||
|
||||
var baseSpeed = Math.Abs(CurveSpeed);
|
||||
if (baseSpeed <= 1e-4f)
|
||||
{
|
||||
DLog.Log("ABORT: FleetCurveWalk speed is zero.", "FleetCurveDbg");
|
||||
yield break;
|
||||
}
|
||||
|
||||
var resolution = Math.Max(2, BezierResolution);
|
||||
var speedFinish = Math.Min(baseSpeed, Math.Abs(FinishSpeed));
|
||||
var gcpLimit = Math.Max(1f, Math.Abs(GcpThetaThreshold));
|
||||
var controlRadius = Math.Max(1f, Math.Abs(conf.TestCarSyncDistance) / 2f);
|
||||
|
||||
ApplyFleetControlPointRadius(chassis, controlRadius);
|
||||
|
||||
try
|
||||
{
|
||||
var track = Track;
|
||||
var trackSource = "external";
|
||||
if (track == null)
|
||||
{
|
||||
var points = new List<Vector2>(ControlPoints);
|
||||
track = new BezierTrack(points, resolution);
|
||||
trackSource = "controlPoints";
|
||||
}
|
||||
track.CarDirectionBias = CarDirectionBias;
|
||||
track.Speed = baseSpeed;
|
||||
|
||||
var center = new Vector2(x0, y0);
|
||||
var (idealPos, idealAngle, bias, pd) = track.QueryTangentPoint(center);
|
||||
var carDirection = (float)CommonMath.ThDiff(theta, CarDirectionBias);
|
||||
var holdTh = ClampAbs((float)CommonMath.ThDiff(idealAngle, carDirection), gcpLimit);
|
||||
var targetBodyTh = (float)CommonMath.RoundTh(idealAngle + CarDirectionBias);
|
||||
|
||||
DLog.Log(
|
||||
$"START center=({x0:0},{y0:0},{theta:0.0}) source={initialCenterSource} " +
|
||||
$"track={track.GetType().Name} trackSource={trackSource} controls={ControlPoints?.Count ?? 0} " +
|
||||
$"len={track.Length():0} speed={baseSpeed:0.000} bias={CarDirectionBias:0.0} " +
|
||||
$"query=({idealPos.X:0},{idealPos.Y:0}) tangent={idealAngle:0.0} targetBody={targetBodyTh:0.0} " +
|
||||
$"pathBias={bias:0.0} pd={pd:0.0} hold={holdTh:0.0} radius={controlRadius:0}",
|
||||
"FleetCurveDbg");
|
||||
|
||||
var warmStart = DateTime.Now;
|
||||
var warmSeqBaseline = self.BeginFleetMotionWarmup();
|
||||
WriteWarmupAuto(self, idealPos, targetBodyTh, holdTh, holdTh);
|
||||
self.PrimeMasterAutoFromSlam();
|
||||
|
||||
var warmEnd = warmStart.AddSeconds(Math.Max(1.0f, StartSyncTimeoutSec));
|
||||
var warmIter = 0;
|
||||
var warmReady = false;
|
||||
var warmDetail = "";
|
||||
while (!_stopping && DateTime.Now < warmEnd)
|
||||
{
|
||||
warmIter++;
|
||||
WriteWarmupAuto(self, idealPos, targetBodyTh, holdTh, holdTh);
|
||||
self.PrimeMasterAutoFromSlam();
|
||||
|
||||
if (warmIter % 5 == 0)
|
||||
{
|
||||
var snap = self.GetFleetCenterSnapshot();
|
||||
int cnt;
|
||||
lock (self.FleetLock) cnt = self.MultiVehicleFleet.Count;
|
||||
DLog.Log(
|
||||
$"WARMUP#{warmIter} snap=({snap.X:0},{snap.Y:0},{snap.Th:0.0}) " +
|
||||
$"cnt={cnt}/{conf.MultiVehicleFleetNum} detail={warmDetail}",
|
||||
"FleetCurveDbg");
|
||||
}
|
||||
|
||||
if (self.IsFleetMotionWarmupReady(warmStart, warmSeqBaseline,
|
||||
conf.TestCarSyncTh, conf.TestCarSyncDistance, out warmDetail))
|
||||
{
|
||||
warmReady = true;
|
||||
DLog.Log($"WARMUP done iter={warmIter} detail={warmDetail}", "FleetCurveDbg");
|
||||
break;
|
||||
}
|
||||
yield return true;
|
||||
}
|
||||
|
||||
if (!warmReady)
|
||||
{
|
||||
DLog.Log($"WARMUP timeout: fleet startup sync failed, abort curve action. detail={warmDetail}",
|
||||
"FleetCurveDbg");
|
||||
Hedingben.ToastText("FleetCurve startup sync timeout", "FleetCurve");
|
||||
Cleanup();
|
||||
yield break;
|
||||
}
|
||||
|
||||
_controller = new ChassisController { BaseSpeed = baseSpeed }.Get();
|
||||
_controller.MultiVehicleSync = true;
|
||||
_controller.BaseSpeed = baseSpeed;
|
||||
_controller.SlowDistance = Math.Max(FinishDistance + 1f, SlowDistance);
|
||||
_controller.FinishDistance = Math.Max(0f, FinishDistance);
|
||||
_controller.FinishSpeed = speedFinish;
|
||||
_controller.SlowingPow = Math.Max(0.01f, SlowingPow);
|
||||
_controller.GcpThetaThreshold = gcpLimit;
|
||||
_controller.AddTrack(track, "FleetCurve");
|
||||
|
||||
Hedingben.ToastText($"FleetCurve len {track.Length():0}mm speed {baseSpeed:0.00}", "FleetCurve");
|
||||
|
||||
foreach (var running in _controller.Track())
|
||||
{
|
||||
if (_stopping)
|
||||
break;
|
||||
if (!running)
|
||||
break;
|
||||
yield return true;
|
||||
}
|
||||
|
||||
if (!_stopping)
|
||||
{
|
||||
self.MultiVehicleAutoVx = 0;
|
||||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||||
var settleEnd = DateTime.Now.AddMilliseconds(Math.Max(100, conf.MultiVehicleSyncInterval * 3));
|
||||
while (!_stopping && DateTime.Now < settleEnd)
|
||||
{
|
||||
self.MultiVehicleAutoEnabled = true;
|
||||
self.MultiVehicleAutoVx = 0;
|
||||
self.MultiVehicleAutoCmdTime = DateTime.Now;
|
||||
yield return true;
|
||||
}
|
||||
}
|
||||
|
||||
DLog.Log($"DONE stopping={_stopping}", "FleetCurveDbg");
|
||||
}
|
||||
finally
|
||||
{
|
||||
Cleanup();
|
||||
_controller = null;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,606 @@
|
||||
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 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()
|
||||
{
|
||||
_dt?.Stop();
|
||||
PilotDefinition.Self.SpeedLeftArm = 0;
|
||||
PilotDefinition.Self.SpeedRightArm = 0;
|
||||
}
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
_dt = new DriveTask(new ClampToTarget()
|
||||
{
|
||||
LeftClampTarget = PilotDefinition.Self.LeftArmUpperPos,
|
||||
RightClampTarget = PilotDefinition.Self.RightArmUpperPos
|
||||
}.Get());
|
||||
_dt.Wait();
|
||||
}
|
||||
|
||||
private DriveTask _dt;
|
||||
}
|
||||
|
||||
[MovementTest(name = "抱夹打开")]
|
||||
public class ClampTest2 : MovementTest
|
||||
{
|
||||
public override void TestStop()
|
||||
{
|
||||
_dt?.Stop();
|
||||
PilotDefinition.Self.SpeedLeftArm = 0;
|
||||
PilotDefinition.Self.SpeedRightArm = 0;
|
||||
}
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
_dt = new DriveTask(new ClampToTarget()
|
||||
{
|
||||
LeftClampTarget = PilotDefinition.Self.LeftArmLowerPos,
|
||||
RightClampTarget = PilotDefinition.Self.RightArmLowerPos
|
||||
}.Get());
|
||||
_dt.Wait();
|
||||
}
|
||||
|
||||
private DriveTask _dt;
|
||||
}
|
||||
|
||||
[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 DstTrackerForward : MovementTest
|
||||
{
|
||||
public bool UseInteractivePick = true;
|
||||
public float srcX;
|
||||
public float srcY;
|
||||
public float dstX;
|
||||
public float dstY;
|
||||
public float carDirectionBias = 0f;
|
||||
private readonly Painter _painter = UI.GetPainter("DstTrackerTest");
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
_dt?.Stop();
|
||||
_painter?.Clear();
|
||||
}
|
||||
public override void Test()
|
||||
{
|
||||
var p1 = UI.GetPoint("point1");
|
||||
var p2 = UI.GetPoint("point2");
|
||||
|
||||
_painter.Clear();
|
||||
_dt = new DriveTask(new DstTracker()
|
||||
{
|
||||
Src = p1,
|
||||
Dst = p2,
|
||||
CarDirectionBias = carDirectionBias,
|
||||
}.Get());
|
||||
_dt.Wait();
|
||||
}
|
||||
|
||||
private DriveTask _dt;
|
||||
}
|
||||
|
||||
[MovementTest(name = "测试终点跟踪动作-后退")]
|
||||
public class DstTrackerhoutui : MovementTest
|
||||
{
|
||||
public bool UseInteractivePick = true;
|
||||
public float srcX;
|
||||
public float srcY;
|
||||
public float dstX;
|
||||
public float dstY;
|
||||
public float carDirectionBias = 180f;
|
||||
private readonly Painter _painter = UI.GetPainter("DstTrackerTest");
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
_dt?.Stop();
|
||||
_painter?.Clear();
|
||||
}
|
||||
public override void Test()
|
||||
{
|
||||
var p1 = UI.GetPoint("point1");
|
||||
var p2 = UI.GetPoint("point2");
|
||||
|
||||
_painter.Clear();
|
||||
_dt = new DriveTask(new DstTracker()
|
||||
{
|
||||
Src = p1,
|
||||
Dst = p2,
|
||||
CarDirectionBias = carDirectionBias,
|
||||
}.Get());
|
||||
_dt.Wait();
|
||||
}
|
||||
|
||||
private DriveTask _dt;
|
||||
}
|
||||
|
||||
[MovementTest(name = "测试先直行再终点跟踪")]
|
||||
public class LineTrackThenDstTrackerTest : MovementTest
|
||||
{
|
||||
public float carDirectionBias = 0f;
|
||||
private readonly Painter _painter = UI.GetPainter("LineTrackThenDstTrackerTest");
|
||||
public override void TestStop()
|
||||
{
|
||||
_dt?.Stop();
|
||||
_painter?.Clear();
|
||||
}
|
||||
public override void Test()
|
||||
{
|
||||
var src = UI.GetPoint("请在上位机选择起点(src)");
|
||||
var dst = UI.GetPoint("请在上位机选择终点(dst)");
|
||||
_painter.Clear();
|
||||
_painter.DrawLine(Color.Cyan, src.X, src.Y, dst.X, dst.Y, width: 3);
|
||||
_painter.DrawCircle(Color.LimeGreen, src.X, src.Y, 80f);
|
||||
_painter.DrawCircle(Color.OrangeRed, dst.X, dst.Y, 80f);
|
||||
_painter.DrawText(Color.LimeGreen, "src", src.X + 80f, src.Y + 80f);
|
||||
_painter.DrawText(Color.OrangeRed, "dst", dst.X + 80f, dst.Y + 80f);
|
||||
IEnumerable<bool> TrackThenFollow()
|
||||
{
|
||||
foreach (var running in new LineTracking()
|
||||
{
|
||||
Target = PilotDefinition.Conf.LineTrackDistance + (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2,
|
||||
EnableHandover = true,
|
||||
HandoverDistance = 200f,
|
||||
HandoverSpeed = 0.3f,
|
||||
}.Get())
|
||||
{
|
||||
if (!running) break;
|
||||
yield return true;
|
||||
}
|
||||
foreach (var running in new DstTracker()
|
||||
{
|
||||
Src = src,
|
||||
Dst = dst,
|
||||
CarDirectionBias = carDirectionBias,
|
||||
InitialSendSpeed = 0.3f
|
||||
}.Get())
|
||||
{
|
||||
if (!running) break;
|
||||
yield return true;
|
||||
}
|
||||
yield return false;
|
||||
}
|
||||
_dt = new DriveTask(TrackThenFollow());
|
||||
_dt.Wait();
|
||||
}
|
||||
private DriveTask _dt;
|
||||
}
|
||||
|
||||
[MovementTest(name = "测试先离车再终点跟踪")]
|
||||
public class LeaveCarThenDstTrackerTest : MovementTest
|
||||
{
|
||||
private readonly Painter _painter = UI.GetPainter("LeaveCarThenDstTrackerTest");
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
_dt?.Stop();
|
||||
_painter?.Clear();
|
||||
}
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
var src = UI.GetPoint("请在上位机选择离车后起点(src)");
|
||||
var dst = UI.GetPoint("请在上位机选择终点(dst)");
|
||||
|
||||
_painter.Clear();
|
||||
_painter.DrawLine(Color.Cyan, src.X, src.Y, dst.X, dst.Y, width: 3);
|
||||
_painter.DrawCircle(Color.LimeGreen, src.X, src.Y, 80f);
|
||||
_painter.DrawCircle(Color.OrangeRed, dst.X, dst.Y, 80f);
|
||||
_painter.DrawText(Color.LimeGreen, "src", src.X + 80f, src.Y + 80f);
|
||||
_painter.DrawText(Color.OrangeRed, "dst", dst.X + 80f, dst.Y + 80f);
|
||||
|
||||
IEnumerable<bool> LeaveThenFollow()
|
||||
{
|
||||
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),
|
||||
},
|
||||
},
|
||||
CarDirection = 180f,
|
||||
SlowDistance = PilotDefinition.Conf.TireFollowingSlowDistance,
|
||||
MaxSpeed = PilotDefinition.Conf.TireFollowingMaxSpeed,
|
||||
WalkBlindTh = 0,
|
||||
TireNum = 1
|
||||
};
|
||||
|
||||
foreach (var running in following.Get())
|
||||
{
|
||||
if (!running) break;
|
||||
yield return true;
|
||||
}
|
||||
|
||||
foreach (var running in new DstTracker()
|
||||
{
|
||||
Src = src,
|
||||
Dst = dst,
|
||||
CarDirectionBias = 180f,
|
||||
}.Get())
|
||||
{
|
||||
if (!running) break;
|
||||
yield return true;
|
||||
}
|
||||
|
||||
yield return false;
|
||||
}
|
||||
|
||||
_dt = new DriveTask(LeaveThenFollow());
|
||||
_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();
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "底盘旋转测试")]
|
||||
public class RotateToAngleTest : MovementTest
|
||||
{
|
||||
public override void TestStop()
|
||||
{
|
||||
throw new NotImplementedException();
|
||||
}
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
var target = UI.GetInput("输入旋转角度：");
|
||||
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||||
new DriveTask(new MultiWheelRotateInPlace()
|
||||
{
|
||||
AngleTarget = float.Parse(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()).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,136 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Drawing;
|
||||
using System.Linq;
|
||||
using System.Numerics;
|
||||
using System.Threading;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.DTools;
|
||||
using ClumsyCore.Pilot;
|
||||
using ClumsyCore.Utilities;
|
||||
using ClumsyDance.ClumsyDance.Detectors;
|
||||
using ClumsyDance.ClumsyWalk.Detectors;
|
||||
using FundamentalLib;
|
||||
using LineSegment = ClumsyCore.Utilities.LineSegment;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
/// <summary>
|
||||
/// 2腿检测：用单线激光雷达识别两腿托盘/轮胎，按上一帧结果作为下一帧猜测做闭环检测。
|
||||
/// 从 StandardMultiWheelLifter 移植；参数全部走 PilotConfig（Fields 面板），雷达选择改为配置项而非阻塞输入。
|
||||
/// </summary>
|
||||
[MovementTest(name = "多舵轮-2腿检测")]
|
||||
public class TwoLegDetect : MovementTest
|
||||
{
|
||||
private Painter _painter;
|
||||
private bool _running = true;
|
||||
|
||||
public override void TestStop() => _running = false;
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
_running = true;
|
||||
_painter = UI.GetPainter("MultiWheelTwoLegDetect", false);
|
||||
_painter.Clear();
|
||||
|
||||
var lidar = PilotDefinition.Conf.TwoLegLidarName;
|
||||
var lastDetectX = PilotDefinition.Conf.TwoLegGuessX;
|
||||
var lastDetectY = 0f;
|
||||
|
||||
while (_running)
|
||||
{
|
||||
var ld = Detect(lidar, lastDetectX, SetFilters(lastDetectX, lastDetectY));
|
||||
if (ld == null)
|
||||
{
|
||||
Thread.Sleep(100);
|
||||
continue;
|
||||
}
|
||||
|
||||
var center = (ld.Src + ld.Dst) / 2;
|
||||
var distanceToCarOrigin = Vector2.Distance(Vector2.Zero, center);
|
||||
|
||||
_painter.Clear();
|
||||
_painter.DrawLine(Color.Cyan, Vector2.Zero, center, width: 2);
|
||||
_painter.DrawText(Color.Yellow, $"{distanceToCarOrigin:F1}", center.X / 2, center.Y / 2);
|
||||
|
||||
Hedingben.ToastText(
|
||||
$"[Test检测] lidar:{lidar} guess x:{lastDetectX:F0} y:{lastDetectY:F0} | " +
|
||||
$"中心 x:{center.X:F0} y:{center.Y:F0} dist:{distanceToCarOrigin:F0}",
|
||||
"MultiWheelTwoLegDetect-test");
|
||||
|
||||
// 用本帧中心作为下一帧猜测，实现闭环跟踪
|
||||
lastDetectX = center.X;
|
||||
lastDetectY = center.Y;
|
||||
Thread.Sleep(100);
|
||||
}
|
||||
|
||||
_painter.Clear();
|
||||
}
|
||||
|
||||
/// <summary>在车体坐标系下，按猜测位置检测两腿，返回连接两腿的线段（车体系）。</summary>
|
||||
public static LineSegment Detect(string lidarName, float guessX, List<DetectFilter> filters)
|
||||
{
|
||||
var conf = PilotDefinition.Conf;
|
||||
#pragma warning disable CS0612, CS0618
|
||||
var detector = new Lidar2dDetect2LegTray
|
||||
{
|
||||
BlobDist = conf.TwoLegBlobDist,
|
||||
BlobPtCount = conf.TwoLegBlobPtCount,
|
||||
BlobSize = conf.TwoLegBlobSize,
|
||||
CenterChange = Tuple.Create(conf.TwoLegCenterChangeX, 0f, 0f),
|
||||
LegWidth = conf.TwoLegWidth,
|
||||
LegWidthErr = conf.TwoLegWidthErr,
|
||||
Padding = conf.TwoLegPadding,
|
||||
PillarFindingScope = conf.TwoLegPillarFindingScope,
|
||||
SgnDir = conf.TwoLegSgnDir,
|
||||
};
|
||||
#pragma warning restore CS0612, CS0618
|
||||
|
||||
var result = detector.DetectWithGuess(
|
||||
lidarName,
|
||||
new LineSegment(new Vector2(guessX, 0), Vector2.Zero),
|
||||
guessCoordinateSystem: CoordinateSystem.Car2D,
|
||||
outCoordinateSystem: CoordinateSystem.Car2D,
|
||||
filters);
|
||||
return ApplyOutputBias(result);
|
||||
}
|
||||
|
||||
private static LineSegment ApplyOutputBias(LineSegment result)
|
||||
{
|
||||
if (result == null) return null;
|
||||
|
||||
var conf = PilotDefinition.Conf;
|
||||
if (Math.Abs(conf.TwoLegOutputBiasX) < 1e-6f && Math.Abs(conf.TwoLegOutputBiasY) < 1e-6f)
|
||||
return result;
|
||||
|
||||
var bias = new Vector2(conf.TwoLegOutputBiasX, conf.TwoLegOutputBiasY);
|
||||
return new LineSegment(result.Src + bias, result.Dst + bias);
|
||||
}
|
||||
|
||||
/// <summary>在猜测中心周围构造一个矩形 ROI，过滤掉框外点云，降低误识别。</summary>
|
||||
public static List<DetectFilter> SetFilters(float guessCenterX, float guessCenterY)
|
||||
{
|
||||
var conf = PilotDefinition.Conf;
|
||||
var painter = UI.GetPainter("MultiWheelTwoLegDetect.Filter", false);
|
||||
painter.Clear();
|
||||
|
||||
var box = new[]
|
||||
{
|
||||
new Vector2(guessCenterX - conf.TwoLegFilterLength / 2, guessCenterY - conf.TwoLegFilterWidth / 2),
|
||||
new Vector2(guessCenterX + conf.TwoLegFilterLength / 2, guessCenterY - conf.TwoLegFilterWidth / 2),
|
||||
new Vector2(guessCenterX + conf.TwoLegFilterLength / 2, guessCenterY + conf.TwoLegFilterWidth / 2),
|
||||
new Vector2(guessCenterX - conf.TwoLegFilterLength / 2, guessCenterY + conf.TwoLegFilterWidth / 2),
|
||||
};
|
||||
|
||||
for (var i = 0; i < box.Length; ++i)
|
||||
painter.DrawLine(Color.DarkOliveGreen, box[i], box[(i + 1) % 4]);
|
||||
|
||||
// 点云滤波在车体坐标系下进行
|
||||
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))),
|
||||
};
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,608 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Numerics;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.Interfaces;
|
||||
using ClumsyCore.Pilot;
|
||||
using FundamentalLib;
|
||||
using CommonUsage.Chassis;
|
||||
using CommonUsage.Mathematics;
|
||||
using MDCSToolBox.Clumsy.Movements;
|
||||
using MDCSToolBox.Clumsy.Pilot;
|
||||
using MDCSToolBox.Clumsy.Tracks;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
public class MultiForwardTest : MovementDefinition
|
||||
{
|
||||
public float Speed = 0.2f;
|
||||
public float DurationSeconds = 2f;
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var chassis = (MultiWheelChassis)BasicPilotBase.Chassis;
|
||||
chassis.SetOriginBias(0, 0, 0);
|
||||
var end = DateTime.Now.AddSeconds(DurationSeconds);
|
||||
while (DateTime.Now < end)
|
||||
{
|
||||
chassis.SendMotion(Speed, 0, 0);
|
||||
yield return true;
|
||||
}
|
||||
|
||||
chassis.SendMotion(0, 0, 0);
|
||||
}
|
||||
}
|
||||
|
||||
// 原地旋转到指定世界坐标系朝向：先把舵轮打到旋转所需角度，对齐后再旋转，按目标角度停止（非固定时长）。
|
||||
public class MultiRotateToWorldAngle : MovementDefinition
|
||||
{
|
||||
/// <summary>目标朝向（世界坐标系，单位 deg）。</summary>
|
||||
public float TargetWorldDeg;
|
||||
|
||||
/// <summary>旋转角速度（deg/s，逆时针为正）。</summary>
|
||||
public float RotSpeed = 30f;
|
||||
|
||||
/// <summary>到位角度精度（deg）。</summary>
|
||||
public float ArriveDeg = 1f;
|
||||
|
||||
/// <summary>起转前舵轮对齐精度（deg）。</summary>
|
||||
public float WheelAlignDeg = 2f;
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var chassis = (MultiWheelChassis)BasicPilotBase.Chassis;
|
||||
chassis.SetOriginBias(0, 0, 0);
|
||||
|
||||
// 阶段一：仅把舵轮打到原地旋转所需角度（下发 0 速度，只对齐不旋转）。
|
||||
while (true)
|
||||
{
|
||||
chassis.SendRotateMotion(0);
|
||||
if (WheelsAligned(chassis, WheelAlignDeg)) break;
|
||||
yield return true;
|
||||
}
|
||||
|
||||
// 阶段二：旋转到目标世界朝向，到位即停。
|
||||
var target = CommonMath.RoundTh(TargetWorldDeg);
|
||||
while (true)
|
||||
{
|
||||
var cur = CommonMath.RoundTh((float)DetourInterface.getCartLocation().th);
|
||||
var diff = CommonMath.ThDiff(target, cur); // 逆时针为正
|
||||
if (Math.Abs(diff) <= ArriveDeg) break;
|
||||
chassis.SendRotateMotion(Math.Sign(diff) * RotSpeed);
|
||||
yield return true;
|
||||
}
|
||||
|
||||
chassis.PredefinedDriveStop();
|
||||
}
|
||||
|
||||
private static bool WheelsAligned(MultiWheelChassis chassis, float tolDeg)
|
||||
{
|
||||
#pragma warning disable CS0612, CS0618
|
||||
var wheels = chassis.GetSteerWheels();
|
||||
#pragma warning restore CS0612, CS0618
|
||||
foreach (var sw in wheels)
|
||||
if (Math.Abs(CommonMath.ThDiff(sw.ReadAngle(), sw.GetSendAngle())) > tolDeg)
|
||||
return false;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "多舵轮-前进2秒")]
|
||||
public class MultiForwardMovementTest : MovementTest
|
||||
{
|
||||
private DriveTask _task;
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
_task = new DriveTask(new MultiForwardTest().Get());
|
||||
_task.Wait();
|
||||
}
|
||||
|
||||
public override void TestStop() => _task?.Stop();
|
||||
}
|
||||
|
||||
[MovementTest(name = "多舵轮-原地旋转到目标角度")]
|
||||
public class MultiRotateMovementTest : MovementTest
|
||||
{
|
||||
private DriveTask _task;
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
_task = new DriveTask(new MultiRotateToWorldAngle
|
||||
{
|
||||
TargetWorldDeg = PilotDefinition.Conf.InPlaceRotateTargetWorldDeg,
|
||||
RotSpeed = PilotDefinition.Conf.InPlaceRotateSpeed,
|
||||
ArriveDeg = PilotDefinition.Conf.InPlaceRotateArriveDeg,
|
||||
WheelAlignDeg = PilotDefinition.Conf.InPlaceRotateWheelAlignDeg
|
||||
}.Get());
|
||||
_task.Wait();
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
_task?.Stop();
|
||||
((MultiWheelChassis)BasicPilotBase.Chassis).PredefinedDriveStop();
|
||||
}
|
||||
}
|
||||
|
||||
// ===== 车队联动-原地旋转动作 =====
|
||||
// 等价于 FleetRemote 的「原地旋转」模式（已实测可用）：FleetRemote 通过 Medulla 手动 IO
|
||||
// (MultiVehicleManualEnabled + Mode=2 + Vth) 驱动 PilotDefinition.TickMultiVehicle 绕车队中心旋转。
|
||||
// 手动 IO 是 [AsLowerIO]（Medulla→Clumsy，每周期回写），Clumsy 侧动作直接写会被覆盖；
|
||||
// 因此本动作改用 Clumsy 内部脚本字段 MultiVehicleScript*（TickMultiVehicle 已将其作为手动等价输入），
|
||||
// 不写一行底盘指令——实际的 SendRotateMotion + PI 纠偏 + 向从车广播均由 TickMultiVehicle 完成。
|
||||
//
|
||||
// 前提：在「主车」(MultiVehicleMasterEndpoint == "/") 的 Clumsy 上运行，且从车已注册(编队就绪)。
|
||||
// 停止条件：主车 SLAM 朝向累计转过 |TargetDeltaDeg|（刚体原地旋转，整车朝向变化量 == 车队转角）；
|
||||
// 无定位时退化为按 |TargetDeltaDeg| / |Omega| 估算时长；并带安全超时。
|
||||
public class FleetRotateInPlace : MovementDefinition
|
||||
{
|
||||
/// <summary>角速度大小(deg/s)；实际方向由 TargetDeltaDeg 的符号决定。</summary>
|
||||
public float Omega = 15f;
|
||||
|
||||
/// <summary>目标相对转角(deg，带符号，+ 为逆时针)。</summary>
|
||||
public float TargetDeltaDeg = 90f;
|
||||
|
||||
/// <summary>到位角度精度(deg)。</summary>
|
||||
public float ArriveDeg = 1.5f;
|
||||
|
||||
/// <summary>减速区宽度(deg)：剩余角度小于此值时，角速度按剩余比例线性降到 MinOmega，抑制惯性超调。</summary>
|
||||
public float SlowDeg = 25f;
|
||||
|
||||
/// <summary>减速区末段最小角速度(deg/s)：避免越接近目标越慢、长尾停不下/到不了位。</summary>
|
||||
public float MinOmega = 3f;
|
||||
|
||||
/// <summary>缓启动角加速度(deg/s²)：起步时角速度从 0 按此斜率爬升到巡航值，抑制起步抖动/队形骤偏。仅作用于起步加速，<=0 关闭缓启动（阶跃起步）。</summary>
|
||||
public float AccelDegPerSec2 = 20f;
|
||||
|
||||
/// <summary>
|
||||
/// 是否用 Detour 主车航向闭环判停（读 getCartLocation().th 累计实际转角，到 |TargetDeltaDeg| 停）。
|
||||
/// 与 MultiVehicleSyncUseDetour 解耦：转到指定角度需要角度反馈，故默认 true。
|
||||
/// false 时退化为按估算时长开环停止（实际转速≠指令时不精确）。注意 true 时若无有效全局定位，
|
||||
/// getCartLocation() 会阻塞（与单车 MultiRotateToWorldAngle 行为一致）。
|
||||
/// </summary>
|
||||
public bool UseDetourHeading = true;
|
||||
|
||||
// 注：不设超时上限——旋转持续到到位（或无定位时按估算时长结束），或被 Stop()/TestStop() 主动中止。
|
||||
|
||||
/// <summary>到位后保持脚本使能、角速度归零的安定时长(s)，让纠偏把队形稳住再撤离。</summary>
|
||||
public float SettleSec = 0.5f;
|
||||
|
||||
private void ClearScript()
|
||||
{
|
||||
var self = PilotDefinition.Self;
|
||||
self.MultiVehicleScriptVx = 0;
|
||||
self.MultiVehicleScriptVy = 0;
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
self.MultiVehicleScriptEnabled = false;
|
||||
}
|
||||
|
||||
public void Stop() => ClearScript();
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var self = PilotDefinition.Self;
|
||||
var conf = PilotDefinition.Conf;
|
||||
|
||||
if (conf.MultiVehicleMasterEndpoint != "/")
|
||||
{
|
||||
Hedingben.ToastText("车队原地旋转需在主车(主车端点=\"/\")运行", "FleetRotate");
|
||||
yield break;
|
||||
}
|
||||
|
||||
var dir = Math.Sign(TargetDeltaDeg);
|
||||
if (dir == 0) dir = 1;
|
||||
var maxOmega = Math.Abs(Omega);
|
||||
var minOmega = Math.Min(Math.Abs(MinOmega), maxOmega); // 最小不超过最大
|
||||
var slowDeg = Math.Max(1e-3f, SlowDeg); // 减速区宽度
|
||||
var accel = AccelDegPerSec2; // 缓启动角加速度，仅作用于起步，<=0 关闭
|
||||
var targetMag = Math.Abs(TargetDeltaDeg);
|
||||
|
||||
var hasPos = UseDetourHeading;
|
||||
var prevTh = hasPos ? (float)DetourInterface.getCartLocation().th : 0f;
|
||||
var startTh = prevTh;
|
||||
var accumulated = 0f; // 累计带符号转角(deg)
|
||||
var start = DateTime.Now;
|
||||
var lastTime = start;
|
||||
var lastLog = DateTime.MinValue;
|
||||
var lastCenterLog = DateTime.MinValue;
|
||||
var cmdMag = 0f; // 当前实际下发角速度大小(deg/s)，缓启动从 0 斜坡爬升
|
||||
var centerTracking = false;
|
||||
float centerStartX = 0, centerStartY = 0, centerStartTh = 0;
|
||||
float centerLastX = 0, centerLastY = 0, centerLastTh = 0, centerMaxDrift = 0;
|
||||
// 无定位按时长估算时，补上缓启动斜坡少转的等效时间(≈ maxOmega/(2·accel))，使时长更接近目标角。
|
||||
var estDuration = maxOmega > 1e-3 ? targetMag / maxOmega : 0;
|
||||
if (accel > 1e-3) estDuration += maxOmega / (2 * accel);
|
||||
|
||||
DLog.Log(
|
||||
$"REQUEST target={TargetDeltaDeg:0.0} dir={dir} omega={maxOmega:0.0} accel={accel:0.0} " +
|
||||
$"slowDeg={slowDeg:0.0} minOmega={minOmega:0.0} useDetourHeading={hasPos} startTh={startTh:0.00} " +
|
||||
$"estDuration={estDuration:0.00}s syncUseDetour={conf.MultiVehicleSyncUseDetour}",
|
||||
"FleetRotateDbg");
|
||||
|
||||
// 使能脚本驱动的原地旋转（mode2）。TickMultiVehicle 后台循环据此执行旋转并广播给从车。
|
||||
// 起步从 0 角速度开始，由缓启动斜坡爬升，避免阶跃下发导致队形骤偏/抖动。
|
||||
self.MultiVehicleScriptVx = 0;
|
||||
self.MultiVehicleScriptVy = 0;
|
||||
self.MultiVehicleScriptMode = 2;
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
self.MultiVehicleScriptEnabled = true;
|
||||
self.MultiVehicleRotateWheelsReady = false;
|
||||
self.MultiVehicleRotateFleetReady = false;
|
||||
|
||||
DLog.Log("WAIT_ALIGN fleet rotate wheels", "FleetRotateDbg");
|
||||
while (!self.MultiVehicleRotateFleetReady)
|
||||
{
|
||||
self.MultiVehicleScriptVx = 0;
|
||||
self.MultiVehicleScriptVy = 0;
|
||||
self.MultiVehicleScriptMode = 2;
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
self.MultiVehicleScriptEnabled = true;
|
||||
Hedingben.ToastText("车队原地旋转舵轮预对齐中", "FleetRotate");
|
||||
yield return true;
|
||||
}
|
||||
|
||||
float centerStartCarX = 0, centerStartCarY = 0, centerStartCarTh = 0;
|
||||
if (hasPos)
|
||||
{
|
||||
var startPos = DetourInterface.getCartLocation();
|
||||
centerStartCarX = (float)startPos.x;
|
||||
centerStartCarY = (float)startPos.y;
|
||||
centerStartCarTh = (float)startPos.th;
|
||||
prevTh = centerStartCarTh;
|
||||
startTh = prevTh;
|
||||
if (self.TryGetFleetCenterFromPose(centerStartCarX, centerStartCarY, centerStartCarTh,
|
||||
out centerStartX, out centerStartY, out centerStartTh))
|
||||
{
|
||||
centerLastX = centerStartX;
|
||||
centerLastY = centerStartY;
|
||||
centerLastTh = centerStartTh;
|
||||
centerMaxDrift = 0;
|
||||
centerTracking = true;
|
||||
}
|
||||
}
|
||||
accumulated = 0f;
|
||||
start = DateTime.Now;
|
||||
lastTime = start;
|
||||
lastLog = DateTime.MinValue;
|
||||
lastCenterLog = DateTime.MinValue;
|
||||
cmdMag = 0f;
|
||||
DLog.Log(
|
||||
$"START target={TargetDeltaDeg:0.0} dir={dir} omega={maxOmega:0.0} startTh={startTh:0.00} " +
|
||||
$"fleetAligned={self.MultiVehicleRotateFleetReady}",
|
||||
"FleetRotateDbg");
|
||||
if (centerTracking)
|
||||
{
|
||||
DLog.Log(
|
||||
$"START center=({centerStartX:0.0},{centerStartY:0.0},{centerStartTh:0.00}) " +
|
||||
$"car=({centerStartCarX:0.0},{centerStartCarY:0.0},{centerStartCarTh:0.00}) " +
|
||||
$"target={TargetDeltaDeg:0.0} omega={maxOmega:0.0}",
|
||||
"FleetRotateCenterDbg");
|
||||
}
|
||||
|
||||
var stopReason = "stop()";
|
||||
while (true)
|
||||
{
|
||||
var now = DateTime.Now;
|
||||
var dt = (float)Math.Min(0.2, Math.Max(0, (now - lastTime).TotalSeconds));
|
||||
lastTime = now;
|
||||
var elapsed = (now - start).TotalSeconds;
|
||||
|
||||
float desiredMag;
|
||||
float curTh = 0f, remaining = 0f, actualRate = 0f;
|
||||
if (hasPos)
|
||||
{
|
||||
var carPos = DetourInterface.getCartLocation();
|
||||
curTh = (float)carPos.th;
|
||||
var step = (float)CommonMath.ThDiff(curTh, prevTh); // 本帧实际转角(逆时针为正)
|
||||
accumulated += step;
|
||||
actualRate = dt > 1e-3 ? step / dt : 0f; // 实际角速率(deg/s)，用于对比指令
|
||||
prevTh = curTh;
|
||||
|
||||
remaining = targetMag - Math.Abs(accumulated);
|
||||
if (remaining <= ArriveDeg) { stopReason = "arrived"; break; }
|
||||
|
||||
// 减速区：剩余角度 < SlowDeg 时，目标角速度按剩余比例线性降到 MinOmega，
|
||||
// 使切断指令瞬间残余动量足够小，抑制惯性滑行造成的超调。宽度直观、便于现场调试。
|
||||
desiredMag = remaining < slowDeg
|
||||
? Math.Max(minOmega, maxOmega * (remaining / slowDeg))
|
||||
: maxOmega;
|
||||
|
||||
if (centerTracking &&
|
||||
self.TryGetFleetCenterFromPose((float)carPos.x, (float)carPos.y, (float)carPos.th,
|
||||
out centerLastX, out centerLastY, out centerLastTh))
|
||||
{
|
||||
var centerDx = centerLastX - centerStartX;
|
||||
var centerDy = centerLastY - centerStartY;
|
||||
var centerDrift = (float)Math.Sqrt(centerDx * centerDx + centerDy * centerDy);
|
||||
centerMaxDrift = Math.Max(centerMaxDrift, centerDrift);
|
||||
var centerDth = (float)CommonMath.ThDiff(centerLastTh, centerStartTh);
|
||||
if ((now - lastCenterLog).TotalMilliseconds >= 250)
|
||||
{
|
||||
lastCenterLog = now;
|
||||
DLog.Log(
|
||||
$"ACTION t={elapsed:0.00}s center=({centerLastX:0.0},{centerLastY:0.0},{centerLastTh:0.00}) " +
|
||||
$"start=({centerStartX:0.0},{centerStartY:0.0},{centerStartTh:0.00}) " +
|
||||
$"drift=({centerDx:0.0},{centerDy:0.0}) dist={centerDrift:0.0} max={centerMaxDrift:0.0} dth={centerDth:0.00} " +
|
||||
$"cmdW={dir * cmdMag:0.000} actualW={actualRate:0.000} acc={accumulated:0.0} remain={remaining:0.0} " +
|
||||
$"wheelReady={self.MultiVehicleRotateWheelsReady} fleetReady={self.MultiVehicleRotateFleetReady}",
|
||||
"FleetRotateCenterDbg");
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// 无定位：按时长估算，无法测角，目标维持巡航速度到估算时长（仅缓启动整形）。
|
||||
desiredMag = maxOmega;
|
||||
if (elapsed >= estDuration) { stopReason = "estDuration"; break; }
|
||||
}
|
||||
|
||||
// 缓启动：只对“加速(目标>当前)”按角加速度限斜率，让起步平滑爬升；
|
||||
// “减速(目标<当前)”跟随上面的减速曲线立即下调，保证及时刹车不超调。
|
||||
if (accel > 1e-3 && desiredMag > cmdMag)
|
||||
cmdMag = Math.Min(desiredMag, cmdMag + accel * dt);
|
||||
else
|
||||
cmdMag = desiredMag;
|
||||
|
||||
self.MultiVehicleScriptVth = dir * cmdMag;
|
||||
|
||||
// 落盘诊断(节流~150ms)：实际航向/累计转角/实际角速率 vs 指令角速率，定位"开环转速不足"。
|
||||
if ((now - lastLog).TotalMilliseconds >= 150)
|
||||
{
|
||||
lastLog = now;
|
||||
DLog.Log(
|
||||
hasPos
|
||||
? $"t={elapsed:0.00}s curTh={curTh:0.00} acc={accumulated:0.0} remain={remaining:0.0} " +
|
||||
$"cmdW={dir * cmdMag:0.0} actualW={actualRate:0.0} (实际/指令={(Math.Abs(cmdMag) > 1e-3 ? actualRate / (dir * cmdMag) : 0):0.00})"
|
||||
: $"t={elapsed:0.00}s/{estDuration:0.00}s (无航向反馈,开环按时长) cmdW={dir * cmdMag:0.0}",
|
||||
"FleetRotateDbg");
|
||||
}
|
||||
|
||||
Hedingben.ToastText(
|
||||
hasPos
|
||||
? $"车队原地旋转 目标{TargetDeltaDeg:0.0}° 已转{accumulated:0.0}° 余{targetMag - Math.Abs(accumulated):0.0}° ω={cmdMag:0.0}"
|
||||
: $"车队原地旋转(无定位,按时长) {elapsed:0.0}/{estDuration:0.0}s ω={cmdMag:0.0}",
|
||||
"FleetRotate");
|
||||
|
||||
yield return true;
|
||||
}
|
||||
|
||||
// 到位：角速度先归零，保持脚本使能让 TickMultiVehicle 的 PI 把队形稳住一小段时间再撤离。
|
||||
self.MultiVehicleScriptVth = 0;
|
||||
var settleEnd = DateTime.Now.AddSeconds(Math.Max(0, SettleSec));
|
||||
while (DateTime.Now < settleEnd)
|
||||
yield return true;
|
||||
|
||||
ClearScript();
|
||||
DLog.Log(
|
||||
$"DONE reason={stopReason} 累计转角={accumulated:0.0}° 目标={TargetDeltaDeg:0.0}° " +
|
||||
$"用时={(DateTime.Now - start).TotalSeconds:0.00}s useDetourHeading={hasPos}",
|
||||
"FleetRotateDbg");
|
||||
if (centerTracking)
|
||||
{
|
||||
var centerDx = centerLastX - centerStartX;
|
||||
var centerDy = centerLastY - centerStartY;
|
||||
var centerDrift = (float)Math.Sqrt(centerDx * centerDx + centerDy * centerDy);
|
||||
var centerDth = (float)CommonMath.ThDiff(centerLastTh, centerStartTh);
|
||||
DLog.Log(
|
||||
$"DONE reason={stopReason} center=({centerLastX:0.0},{centerLastY:0.0},{centerLastTh:0.00}) " +
|
||||
$"start=({centerStartX:0.0},{centerStartY:0.0},{centerStartTh:0.00}) " +
|
||||
$"drift=({centerDx:0.0},{centerDy:0.0}) dist={centerDrift:0.0} max={centerMaxDrift:0.0} dth={centerDth:0.00} " +
|
||||
$"acc={accumulated:0.0} target={TargetDeltaDeg:0.0}",
|
||||
"FleetRotateCenterDbg");
|
||||
}
|
||||
Hedingben.ToastText($"车队原地旋转完成({stopReason}) 累计{accumulated:0.0}°", "FleetRotate");
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "车队联动-原地旋转")]
|
||||
public class FleetRotateInPlaceTest : MovementTest
|
||||
{
|
||||
private FleetRotateInPlace _proc;
|
||||
private DriveTask _task;
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
_proc = new FleetRotateInPlace
|
||||
{
|
||||
Omega = PilotDefinition.Conf.FleetRotateOmega,
|
||||
TargetDeltaDeg = PilotDefinition.Conf.FleetRotateTargetDeltaDeg,
|
||||
ArriveDeg = PilotDefinition.Conf.FleetRotateArriveDeg,
|
||||
SlowDeg = PilotDefinition.Conf.FleetRotateSlowDeg,
|
||||
MinOmega = PilotDefinition.Conf.FleetRotateMinOmega,
|
||||
AccelDegPerSec2 = PilotDefinition.Conf.FleetRotateAccel,
|
||||
SettleSec = PilotDefinition.Conf.FleetRotateSettleSec,
|
||||
UseDetourHeading = PilotDefinition.Conf.FleetRotateUseDetourHeading
|
||||
};
|
||||
_task = new DriveTask(_proc.Get());
|
||||
_task.Wait();
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
_proc?.Stop();
|
||||
_task?.Stop();
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "车队联动-曲线行走")]
|
||||
public class FleetCurveWalkTest : MovementTest
|
||||
{
|
||||
private FleetCurveWalk _proc;
|
||||
private DriveTask _task;
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
var self = PilotDefinition.Self;
|
||||
if (!self.TryGetFleetCenterFromMembers(out var x, out var y, out var th) &&
|
||||
!self.TryGetFleetCenterFromSlam(out x, out y, out th))
|
||||
{
|
||||
DLog.Log("FleetCurveWalkTest abort: failed to read fleet center.", "FleetCurveDbg");
|
||||
Hedingben.ToastText("FleetCurve requires master localization", "FleetCurve");
|
||||
return;
|
||||
}
|
||||
|
||||
var pointCount = Math.Max(3, PilotDefinition.Conf.FleetCurveTestControlPointCount);
|
||||
var controlPoints = new List<Vector2>();
|
||||
for (var i = 0; i < pointCount; i++)
|
||||
controlPoints.Add(UI.GetPoint($"FleetCurve point {i + 1}/{pointCount}"));
|
||||
var fleetCenter = new Vector2(x, y);
|
||||
if (Vector2.Distance(fleetCenter, controlPoints[0]) >
|
||||
Vector2.Distance(fleetCenter, controlPoints[controlPoints.Count - 1]))
|
||||
controlPoints.Reverse();
|
||||
var track = new BezierTrack(controlPoints)
|
||||
{
|
||||
Speed = PilotDefinition.Conf.FleetCurveSpeed,
|
||||
CarDirectionBias = 0f
|
||||
};
|
||||
|
||||
_proc = new FleetCurveWalk
|
||||
{
|
||||
Track = track,
|
||||
CurveSpeed = PilotDefinition.Conf.FleetCurveSpeed,
|
||||
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
|
||||
};
|
||||
_task = new DriveTask(_proc.Get());
|
||||
_task.Wait();
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
_proc?.Stop();
|
||||
_task?.Stop();
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "车队联动-自动蟹行")]
|
||||
public class FleetCrabWalkTest : MovementTest
|
||||
{
|
||||
private FleetCrabWalk _proc;
|
||||
private DriveTask _task;
|
||||
|
||||
public override void Test()
|
||||
{
|
||||
_proc = new FleetCrabWalk
|
||||
{
|
||||
CrabAngleDeg = PilotDefinition.Conf.FleetCrabAngleDeg,
|
||||
BodyToPathAngleDeg = PilotDefinition.Conf.FleetCrabAngleDeg,
|
||||
CrabLengthMm = PilotDefinition.Conf.FleetCrabLengthMm,
|
||||
CrabSpeed = PilotDefinition.Conf.FleetCrabSpeed,
|
||||
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
|
||||
};
|
||||
_task = new DriveTask(_proc.Get());
|
||||
_task.Wait();
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
_proc?.Stop();
|
||||
_task?.Stop();
|
||||
}
|
||||
}
|
||||
|
||||
// ===== 调用 Playground WebAPI 瞬移小车（前移 / 左移 / 旋转）=====
|
||||
// 平移/旋转量在 Fields 面板配置：WebApiTranslateMm（默认100mm）、WebApiRotateDeg（默认5度）。
|
||||
|
||||
[MovementTest(name = "多舵轮-WebAPI前移")]
|
||||
public class WebApiForwardMoveTest : MovementTest
|
||||
{
|
||||
public override void Test()
|
||||
{
|
||||
var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
|
||||
var name = PilotDefinition.Conf.PlaygroundRobotName;
|
||||
var d = PilotDefinition.Conf.WebApiTranslateMm;
|
||||
|
||||
var pose = PlaygroundWebApi.GetPose(url, name);
|
||||
// 车体系前向 (d, 0) 变换到世界系：车头方向即朝向 yaw
|
||||
var dst = CommonMath.Transform2D(new Vector2(pose.X, pose.Y), pose.YawDeg, new Vector2(d, 0));
|
||||
PlaygroundWebApi.Move(url, name, dst.X, dst.Y, pose.YawDeg);
|
||||
Hedingben.ToastText($"前移 {d:f0}mm -> ({dst.X:f0},{dst.Y:f0})", "WebApiForward");
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "多舵轮-WebAPI左移")]
|
||||
public class WebApiLeftMoveTest : MovementTest
|
||||
{
|
||||
public override void Test()
|
||||
{
|
||||
var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
|
||||
var name = PilotDefinition.Conf.PlaygroundRobotName;
|
||||
var d = PilotDefinition.Conf.WebApiTranslateMm;
|
||||
|
||||
var pose = PlaygroundWebApi.GetPose(url, name);
|
||||
// 车体系左向 (0, d) 变换到世界系（车体 +Y 即左侧）
|
||||
var dst = CommonMath.Transform2D(new Vector2(pose.X, pose.Y), pose.YawDeg, new Vector2(0, d));
|
||||
PlaygroundWebApi.Move(url, name, dst.X, dst.Y, pose.YawDeg);
|
||||
Hedingben.ToastText($"左移 {d:f0}mm -> ({dst.X:f0},{dst.Y:f0})", "WebApiLeft");
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "多舵轮-WebAPI旋转")]
|
||||
public class WebApiRotateTest : MovementTest
|
||||
{
|
||||
public override void Test()
|
||||
{
|
||||
var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
|
||||
var name = PilotDefinition.Conf.PlaygroundRobotName;
|
||||
var deg = PilotDefinition.Conf.WebApiRotateDeg;
|
||||
|
||||
var pose = PlaygroundWebApi.GetPose(url, name);
|
||||
var ny = pose.YawDeg + deg; // 逆时针为正
|
||||
PlaygroundWebApi.Move(url, name, pose.X, pose.Y, ny);
|
||||
Hedingben.ToastText($"旋转 {deg:f1}° -> {ny:f1}°", "WebApiRotate");
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "多舵轮-WebAPI恢复运动")]
|
||||
public class WebApiMotionResumeTest : MovementTest
|
||||
{
|
||||
public override void Test()
|
||||
{
|
||||
var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
|
||||
PlaygroundWebApi.ResumeMotion(url);
|
||||
Hedingben.ToastText("已恢复车辆运动", "WebApiMotion");
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
[MovementTest(name = "多舵轮-WebAPI暂停运动")]
|
||||
public class WebApiMotionPauseTest : MovementTest
|
||||
{
|
||||
public override void Test()
|
||||
{
|
||||
var url = PilotDefinition.Conf.PlaygroundWebApiUrl;
|
||||
PlaygroundWebApi.PauseMotion(url); // 默认 zero 模式：反馈归零
|
||||
Hedingben.ToastText("已暂停车辆运动 (zero)", "WebApiMotion");
|
||||
}
|
||||
|
||||
public override void TestStop()
|
||||
{
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,326 @@
|
||||
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 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;
|
||||
using static ClumsyCore.DTools.Painter;
|
||||
|
||||
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 ClampMaxI = PilotDefinition.Conf.ClampControlMaxI;
|
||||
public float ClampSpeedAcc = PilotDefinition.Conf.ClampControlSpeedAcc;
|
||||
public float ClampDeadZone = PilotDefinition.Conf.ClampControlDeadZone;
|
||||
private PIDController leftpid, rightpid;
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
leftpid = new PIDController(() => PilotDefinition.Self.ActualPosLeftArm, ClampKp, ClampKi, ClampKd,
|
||||
ClampMaxI, ClampDeadZone, MaxClampSpeed)
|
||||
{ SpeedAccPerSec = ClampSpeedAcc };
|
||||
|
||||
rightpid = new PIDController(() => PilotDefinition.Self.ActualPosRightArm, ClampKp, ClampKi, ClampKd,
|
||||
ClampMaxI, ClampDeadZone, MaxClampSpeed)
|
||||
{ SpeedAccPerSec = ClampSpeedAcc };
|
||||
|
||||
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 (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 DstTracker : MovementDefinition
|
||||
{
|
||||
public Vector2 Src;
|
||||
public Vector2 Dst;
|
||||
public float CarDirectionBias = 0f;
|
||||
public Painter Painter = UI.GetPainter("DstTracker");
|
||||
|
||||
public float InitialSendSpeed = 0;
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
Console.WriteLine($"DstTracker src:({Src.X:F2}, {Src.Y:F2}) dst:({Dst.X:F2}, {Dst.Y:F2})");
|
||||
Painter.DrawLine(Color.Cyan, Src.X, Src.Y, Dst.X, Dst.Y, width: 3);
|
||||
|
||||
var tracker = new ChassisController().Get();
|
||||
|
||||
if (InitialSendSpeed != 0)
|
||||
{
|
||||
tracker.SkipInitialRotate = true;
|
||||
tracker.InitialSendSpeed = InitialSendSpeed;
|
||||
}
|
||||
|
||||
var linePath = new LineTrack(Src, Dst) { CarDirectionBias = CarDirectionBias, Speed = PilotDefinition.Conf.DstTrackerMaxSpeed };
|
||||
tracker.AddTrack(linePath);
|
||||
var task = new DriveTask(tracker.Track());
|
||||
task.Wait();
|
||||
|
||||
// 到点后兜底停车
|
||||
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||||
chassis.SendXYThSpeed(0f, 0f, 0f);
|
||||
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;
|
||||
// 末段衔接：接近目标后不再让 PID 把速度降到 0，保留一个接力速度给后续动作接管
|
||||
public bool EnableHandover = false;
|
||||
public float HandoverDistance = 80f; // mm
|
||||
public float HandoverSpeed = 0.15f; // m/s
|
||||
|
||||
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;
|
||||
//chassis.SetOriginBias(0, 0, 0);
|
||||
DLog.Log($"直线行驶距离:{Target}", "TireFollowing");
|
||||
while (true)
|
||||
{
|
||||
var current = (PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2;
|
||||
var remain = Target - current;
|
||||
if (EnableHandover && Math.Abs(remain) <= Math.Max(1f, HandoverDistance))
|
||||
{
|
||||
var handoverSign = Math.Sign(remain);
|
||||
if (handoverSign == 0) handoverSign = 1;
|
||||
var handoverSpeed = Math.Abs(HandoverSpeed) * handoverSign;
|
||||
Console.WriteLine($"handover speed: {handoverSpeed:F3}, remain: {remain:F2}");
|
||||
chassis.SendXYThSpeed(handoverSpeed, 0, 0);
|
||||
// 保留一拍接力速度，让后续 DstTracker 无缝接管
|
||||
yield return true;
|
||||
break;
|
||||
}
|
||||
var speed = pid.GetResponse(Target);
|
||||
Console.WriteLine($"output: {speed} current: {(PilotDefinition.Self.LFLActualPos + PilotDefinition.Self.LFRActualPos) / 2}");
|
||||
|
||||
chassis.SendXYThSpeed(speed, 0, 0);
|
||||
|
||||
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 int WaitTimeoutMs = 2000;
|
||||
public int PollIntervalMs = 50;
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
Console.WriteLine("驱动器上使能");
|
||||
PilotDefinition.Self.ResetFromC = true;
|
||||
|
||||
var start = DateTime.Now;
|
||||
var timeoutMs = Math.Max(0, WaitTimeoutMs);
|
||||
var pollMs = Math.Max(1, PollIntervalMs);
|
||||
var success = PilotDefinition.Self.WheelAbleState;
|
||||
while (!success && (DateTime.Now - start).TotalMilliseconds < timeoutMs)
|
||||
{
|
||||
Thread.Sleep(pollMs);
|
||||
success = PilotDefinition.Self.WheelAbleState;
|
||||
if (!success) yield return true;
|
||||
}
|
||||
|
||||
PilotDefinition.Self.ResetFromC = false;
|
||||
if (success)
|
||||
Console.WriteLine($"驱动器上使能完成，WheelAbleState={PilotDefinition.Self.WheelAbleState}");
|
||||
else
|
||||
Console.WriteLine($"驱动器上使能超时，WheelAbleState={PilotDefinition.Self.WheelAbleState}，等待{timeoutMs}ms");
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
|
||||
public class DriverDisable : MovementDefinition
|
||||
{
|
||||
public int WaitTimeoutMs = 3000;
|
||||
public int PollIntervalMs = 20;
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
Console.WriteLine("驱动器下使能");
|
||||
PilotDefinition.Self.DisableFromC = true;
|
||||
|
||||
var start = DateTime.Now;
|
||||
var timeoutMs = Math.Max(0, WaitTimeoutMs);
|
||||
var pollMs = Math.Max(1, PollIntervalMs);
|
||||
var success = !PilotDefinition.Self.WheelAbleState;
|
||||
while (!success && (DateTime.Now - start).TotalMilliseconds < timeoutMs)
|
||||
{
|
||||
Thread.Sleep(pollMs);
|
||||
success = !PilotDefinition.Self.WheelAbleState;
|
||||
if (!success) yield return true;
|
||||
}
|
||||
|
||||
PilotDefinition.Self.DisableFromC = false;
|
||||
if (success)
|
||||
Console.WriteLine($"驱动器下使能完成，WheelAbleState={PilotDefinition.Self.WheelAbleState}");
|
||||
else
|
||||
Console.WriteLine($"驱动器下使能超时，WheelAbleState={PilotDefinition.Self.WheelAbleState}，等待{timeoutMs}ms");
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,334 @@
|
||||
using ClumsyCore;
|
||||
using MDCSToolBox.Clumsy.Pilot.MultiWheel;
|
||||
using Newtonsoft.Json;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
public class PilotConfig : MultiWheelPilotConfig
|
||||
{
|
||||
[FieldMember(desc = "[sync] steering angle acceleration(deg/s^2)")] public float SyncThAccPerSec = 30f;
|
||||
[FieldMember(desc = "[sync] fleet member distance(mm)")] public float TestCarSyncDistance = 2400f;
|
||||
[FieldMember(desc = "[sync] fleet layout bias angle(deg)")] public float TestCarSyncTh = 0f;
|
||||
// Fleet manual remote IO values are normalized joystick ratios. Keep all speed/angle scaling here.
|
||||
[FieldMember(desc = "[sync] fleet manual max linear speed(m/s)")] public float FleetManualMaxSpeed = 0.3f;
|
||||
[FieldMember(desc = "[sync] fleet manual normal-mode full-stick steering angle(deg)")] public float FleetManualMaxSteerAngleDeg = 45f;
|
||||
[FieldMember(desc = "[sync] fleet manual crab-mode full-stick steering angle(deg)")] public float FleetManualMaxCrabAngleDeg = 60f;
|
||||
[FieldMember(desc = "[sync] fleet manual rotate-mode full-stick angular speed(deg/s)")] public float FleetManualMaxRotateOmegaDegPerSec = 45f;
|
||||
[FieldMember(desc = "[sync] 蟹行舵角上限(deg，应与Medulla舵轮角度限制匹配，默认120)")] public float MultiVehicleCrabSteerLimitDeg = 120f;
|
||||
[FieldMember(desc = "[sync] 检测中心偏移(mm)")] public float DeltaDetectCenter = 350f;
|
||||
// 仅控制"车队内姿态纠正"(POS 补偿)是否使用 Detour 的 SLAM 位姿，不影响"整个车队姿态的计算"。
|
||||
// 默认 false：定位不参与车队内姿态纠正（各车按编队几何/互识别保持队形，不做 SLAM 逐车纠偏）。
|
||||
// 为 true：额外用 getCartLocation() 反推每台车相对编队中心的偏差并做 POS 补偿。
|
||||
// 注意：无论该开关如何，自动模式下整队姿态(反推/广播车队中心、SLAM 间距、自动安全门)始终依赖 Detour 全局定位；
|
||||
// 主车自动模式必调用 getCartLocation()，若无有效全局定位该调用会阻塞 → 联动线程阻塞不下发速度（安全停车）。
|
||||
[FieldMember(desc = "[sync] 定位是否参与车队内姿态纠正(不影响整队姿态计算)")] public bool MultiVehicleSyncUseDetour = false;
|
||||
// 手动外部遥控联动默认只走 2 腿检测/几何同步，避免 Detour getCartLocation 阻塞导致遥控和检测可视化变慢。
|
||||
[FieldMember(desc = "[sync] 手动联动是否启用定位姿态纠正(默认关闭)")] public bool MultiVehicleManualUseDetourCorrection = false;
|
||||
|
||||
[FieldMember(desc = "多车联动：总车数")] public int MultiVehicleFleetNum = 2;
|
||||
[FieldMember(desc = "联动线程周期(ms)")] public int MultiVehicleSyncInterval = 50;
|
||||
[FieldMember(desc = "多车联动：主车端点 ip:port，/ 表示本车为主车")] public string MultiVehicleMasterEndpoint = "/";
|
||||
[FieldMember(desc = "多车联动：本车同步 IP")] public string SimpleIp = "127.0.0.1";
|
||||
|
||||
[FieldMember(desc = "多车联动：本车回连端点 ip:port，供主车 notify 回连，空=127.0.0.1:本车port")] public string MultiVehicleSelfEndpoint = "";
|
||||
|
||||
[JsonProperty("MultiVehicleMasterIp")]
|
||||
private string LegacyMasterIpSetter
|
||||
{
|
||||
set
|
||||
{
|
||||
if (string.IsNullOrEmpty(value) || value == "/") return;
|
||||
if (MultiVehicleMasterEndpoint == "/")
|
||||
MultiVehicleMasterEndpoint = value.Contains(":") ? value : $"{value}:8008";
|
||||
}
|
||||
}
|
||||
|
||||
[FieldMember(desc = "多车联动：启用互识别纠正")] public bool MultiVehicleUseDetect = false;
|
||||
|
||||
// B: 自动速度命令新鲜度(ms)。主车超过此时长未从路径控制器收到新速度命令(路径结束/早退/卡顿)，
|
||||
// 即视为失效并清零下发速度，避免车队按末速度滑行。0 表示自动取 max(200, interval*4)。
|
||||
[FieldMember(desc = "多车联动：自动速度命令超时(ms，0=auto)")] public int MultiVehicleAutoCmdTimeoutMs = 0;
|
||||
// C: fleet 成员存活 TTL(ms)。主车剔除超过此时长未 register/刷新的从车；编队就绪要求所有成员新鲜。
|
||||
// 0 表示自动取 max(500, interval*6)。
|
||||
[FieldMember(desc = "多车联动：成员存活TTL(ms，0=auto)")] public int MultiVehicleMemberTtlMs = 0;
|
||||
// D: 自动模式下用主车路径控制器的理想车队中心(idealPos/idealAngle)作为各车 layout 目标，
|
||||
// 弧线路径上做 per-car 前馈而非仅共用 frontTh/rearTh 事后纠偏。
|
||||
[FieldMember(desc = "多车联动：自动模式按理想中心前馈(弧线)")] public bool MultiVehicleAutoUseIdealCenter = true;
|
||||
// H: 自动模式必须有有效车队中心(SLAM 可反推)，全程定位丢失时停车，避免纯 SLAM 下盲跑。
|
||||
[FieldMember(desc = "多车联动：自动模式要求有效车队中心")] public bool MultiVehicleAutoRequireFleetCenter = true;
|
||||
[FieldMember(desc = "多车联动：SLAM X补偿系数")] public float MultiVehiclePosBiasXFac = 0.5f;
|
||||
[FieldMember(desc = "多车联动：SLAM Y补偿系数")] public float MultiVehiclePosBiasYFac = 0.5f;
|
||||
[FieldMember(desc = "多车联动：SLAM Th补偿系数")] public float MultiVehiclePosBiasThFac = 0.5f;
|
||||
[FieldMember(desc = "多车联动：X补偿阈值(mm)")] public float MultiVehiclePosBiasXThreshold = 50f;
|
||||
[FieldMember(desc = "多车联动：Y补偿阈值(mm)")] public float MultiVehiclePosBiasYThreshold = 50f;
|
||||
[FieldMember(desc = "多车联动：Th补偿阈值(deg)")] public float MultiVehiclePosBiasThThreshold = 5f;
|
||||
|
||||
[FieldMember(desc = "多车联动：互识别 X补偿系数")] public float MultiVehicleDetectBiasXFac = 0.5f;
|
||||
[FieldMember(desc = "多车联动：互识别 Y补偿系数")] public float MultiVehicleDetectBiasYFac = 0.5f;
|
||||
[FieldMember(desc = "多车联动：互识别 Th补偿系数")] public float MultiVehicleDetectBiasThFac = 0.5f;
|
||||
[FieldMember(desc = "多车联动：互识别 X补偿阈值(mm)")] public float MultiVehicleDetectBiasXThreshold = 50f;
|
||||
[FieldMember(desc = "多车联动：互识别 Y补偿阈值(mm)")] public float MultiVehicleDetectBiasYThreshold = 50f;
|
||||
[FieldMember(desc = "多车联动：互识别 Th补偿阈值(deg)")] public float MultiVehicleDetectBiasThThreshold = 5f;
|
||||
|
||||
// 原地旋转(mode2)闭环纠偏(PI)：把"本车应移动到的位置(dx,dy,mm)/应转角(dth,deg)"作为误差，
|
||||
// 用 PI 控制器换算成车体系修正速度叠加到绕队心旋转上。纯 P 对抗恒定横向滑移扰动有稳态残差，
|
||||
// 加积分项把稳态误差拉到 0；积分带限幅(抗 windup)，总输出限幅在 Max 内防过冲/振荡。
|
||||
// Fac=比例增益(mm/s per mm、deg/s per deg)；IFac=积分增益(mm/s per mm·s、deg/s per deg·s)；Max=总输出上限。
|
||||
[FieldMember(desc = "原地旋转纠偏：平移比例增益P(mm/s per mm)")] public float MultiVehicleRotateCompXyFac = 1.2f;
|
||||
[FieldMember(desc = "原地旋转纠偏：平移积分增益I(mm/s per mm·s)")] public float MultiVehicleRotateCompXyIFac = 0.8f;
|
||||
[FieldMember(desc = "原地旋转纠偏：平移速度上限(mm/s)")] public float MultiVehicleRotateCompXyMax = 150f;
|
||||
[FieldMember(desc = "原地旋转纠偏：转向比例增益P(deg/s per deg)")] public float MultiVehicleRotateCompThFac = 0.8f;
|
||||
[FieldMember(desc = "原地旋转纠偏：转向积分增益I(deg/s per deg·s)")] public float MultiVehicleRotateCompThIFac = 0.8f;
|
||||
[FieldMember(desc = "原地旋转纠偏：转向速度上限(deg/s)")] public float MultiVehicleRotateCompThMax = 15f;
|
||||
// 仅当车队实际被指令旋转(|fleetOmega|超过此阈值)时才运行纠偏 PI；否则清零并复位积分，
|
||||
// 避免松开摇杆后积分残留持续驱动车辆"自行旋转停不下来"。
|
||||
[FieldMember(desc = "原地旋转纠偏：生效的最小角速度阈值(deg/s)")] public float MultiVehicleRotateActiveOmega = 0.5f;
|
||||
// 安全网：每轮纠偏速度幅值 <= 该比例 * 本轮旋转切向速度，限制合速度相对纯切向的最大偏角。
|
||||
// 旧配置若仍为 <0，运行时按安全默认 0.10 处理；确需放宽时可在主车显式调大并同步给从车。
|
||||
[FieldMember(desc = "原地旋转纠偏：纠偏/旋转切向比例硬上限，<0使用安全默认0.10")] public float MultiVehicleRotateCompTangentFrac = 0.10f;
|
||||
|
||||
[FieldMember(desc = "单车同步 xy 精度(mm)")] public float SingleCarSyncPrecisionXy = 10f;
|
||||
[FieldMember(desc = "单车同步 th 精度(deg)")] public float SingleCarSyncPrecisionTh = 0.2f;
|
||||
|
||||
[FieldMember(desc = "Playground WebAPI 基地址")]
|
||||
public string PlaygroundWebApiUrl = "http://localhost:18090";
|
||||
|
||||
[FieldMember(desc = "MultiVehicle rotate pose WebAPI diagnostics (simulation only)")]
|
||||
public bool MultiVehicleRotatePoseWebApiDiagEnabled = false;
|
||||
|
||||
[FieldMember(desc = "Playground 小车名称（场景 robots[].name）")]
|
||||
public string PlaygroundRobotName = "agv_multi_1";
|
||||
|
||||
[FieldMember(desc = "Playground 邻车名称（仅主车用于原地旋转位姿诊断）")]
|
||||
public string PlaygroundNeighborRobotName = "agv_multi_2";
|
||||
|
||||
[FieldMember(desc = "WebAPI 平移测试：平移距离(mm)")]
|
||||
public float WebApiTranslateMm = 100f;
|
||||
|
||||
[FieldMember(desc = "WebAPI 旋转测试：旋转角度(deg)")]
|
||||
public float WebApiRotateDeg = 5f;
|
||||
|
||||
[FieldMember(desc = "原地旋转：目标朝向(世界坐标系, deg)")]
|
||||
public float InPlaceRotateTargetWorldDeg = 90f;
|
||||
|
||||
[FieldMember(desc = "原地旋转：旋转角速度(deg/s)")]
|
||||
public float InPlaceRotateSpeed = 30f;
|
||||
|
||||
[FieldMember(desc = "原地旋转：到位角度精度(deg)")]
|
||||
public float InPlaceRotateArriveDeg = 1f;
|
||||
|
||||
[FieldMember(desc = "原地旋转：起转前舵轮对齐精度(deg)")]
|
||||
public float InPlaceRotateWheelAlignDeg = 2f;
|
||||
|
||||
[FieldMember(desc = "原地旋转：旋转过程中舵轮偏差重对齐阈值(deg)")]
|
||||
public float InPlaceRotateActiveWheelAlignDeg = 10f;
|
||||
|
||||
// ===== 车队联动-原地旋转动作（FleetRotateInPlace / 对应 FleetRemote 原地旋转模式）=====
|
||||
// 通过 Clumsy 内部脚本字段驱动 TickMultiVehicle 的 mode2 旋转（绕车队中心 + PI 纠偏），需主车运行。
|
||||
[FieldMember(desc = "车队原地旋转：角速度大小(deg/s，方向由目标角符号决定)")]
|
||||
public float FleetRotateOmega = 15f;
|
||||
|
||||
[FieldMember(desc = "车队原地旋转：目标相对转角(deg，+逆时针)")]
|
||||
public float FleetRotateTargetDeltaDeg = 90f;
|
||||
|
||||
[FieldMember(desc = "车队原地旋转：到位角度精度(deg)")]
|
||||
public float FleetRotateArriveDeg = 1.5f;
|
||||
|
||||
[FieldMember(desc = "车队原地旋转：减速区宽度(deg)，抑制收尾惯性超调")]
|
||||
public float FleetRotateSlowDeg = 25f;
|
||||
|
||||
[FieldMember(desc = "车队原地旋转：减速区末段最小角速度(deg/s)")]
|
||||
public float FleetRotateMinOmega = 3f;
|
||||
|
||||
[FieldMember(desc = "车队原地旋转：起步缓启动角加速度(deg/s²，<=0关闭)")]
|
||||
public float FleetRotateAccel = 20f;
|
||||
|
||||
[FieldMember(desc = "车队原地旋转：到位后安定时长(s)")]
|
||||
public float FleetRotateSettleSec = 0.5f;
|
||||
|
||||
// 与 MultiVehicleSyncUseDetour 解耦：转到指定角度需航向反馈，默认 true 读主车 SLAM 航向闭环判停。
|
||||
// false 时退化为按估算时长开环停止（实际转速≠指令时不精确，易出现"没转到目标就停"）。
|
||||
[FieldMember(desc = "车队原地旋转：用Detour主车航向闭环判停(默认true，false=按时长开环)")]
|
||||
public bool FleetRotateUseDetourHeading = true;
|
||||
|
||||
// ===== 车队联动-自动蟹行（FleetCrabWalk）=====
|
||||
// 以当前车队中心为起点，构造一条直线路径；MovementTest 中车身保持启动朝向追踪该路径。
|
||||
// 动作侧参考几何控制器的路径跟踪思路，直接写入 MultiVehicleAuto... 字段，不再复用脚本手动链路。
|
||||
[FieldMember(desc = "车队蟹行：路径方向相对启动时车队朝向夹角(deg，逆时针为正；路径在车右侧x度时填-x)")]
|
||||
public float FleetCrabAngleDeg = 45f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：AGV入口使用的车队世界系目标朝向(deg)")]
|
||||
public float FleetCrabBodyWorldHeadingDeg = 0f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：路径长度(mm)")]
|
||||
public float FleetCrabLengthMm = 2000f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：行驶速度(m/s)")]
|
||||
public float FleetCrabSpeed = 0.2f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：速度命令加速度限制(m/s^2，<=0表示不限制)")]
|
||||
public float FleetCrabAccel = 0.2f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：预对齐后正式下发速度前5秒加速度(m/s^2，<=0表示不限制)")]
|
||||
public float FleetCrabStartAccel = 0.01f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：末端开始减速距离(mm)")]
|
||||
public float FleetCrabSlowDistance = 2000f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：完成距离(mm)，低于该剩余距离结束动作")]
|
||||
public float FleetCrabFinishDistance = 20f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：末端最低速度(m/s)")]
|
||||
public float FleetCrabFinishSpeed = 0.02f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：末端减速曲线指数")]
|
||||
public float FleetCrabSlowingPow = 0.8f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：GCP舵角修正上限(deg)")]
|
||||
public float FleetCrabGcpThetaThreshold = 95f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：headingErr角度纠偏比例系数")]
|
||||
public float FleetCrabDthLinearFac = 1f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：headingErr角度纠偏舵角限幅(deg)")]
|
||||
public float FleetCrabDthLinearThreshold = 10f;
|
||||
|
||||
[FieldMember(desc = "FleetCrab startup sync timeout(s)")]
|
||||
public float FleetCrabStartSyncTimeoutSec = 8f;
|
||||
|
||||
[FieldMember(desc = "FleetCrab startup wheel alignment tolerance(deg)")]
|
||||
public float FleetCrabStartWheelAlignDeg = 2f;
|
||||
|
||||
// ===== Fleet linked Bezier curve walk =====
|
||||
[FieldMember(desc = "FleetCurve MovementTest Bezier control point count")]
|
||||
public int FleetCurveTestControlPointCount = 4;
|
||||
|
||||
[FieldMember(desc = "FleetCurve speed(m/s)")]
|
||||
public float FleetCurveSpeed = 0.2f;
|
||||
|
||||
[FieldMember(desc = "FleetCurve slow distance(mm)")]
|
||||
public float FleetCurveSlowDistance = 2000f;
|
||||
|
||||
[FieldMember(desc = "FleetCurve finish distance(mm)")]
|
||||
public float FleetCurveFinishDistance = 20f;
|
||||
|
||||
[FieldMember(desc = "FleetCurve finish speed(m/s)")]
|
||||
public float FleetCurveFinishSpeed = 0.02f;
|
||||
|
||||
[FieldMember(desc = "FleetCurve slowing curve exponent")]
|
||||
public float FleetCurveSlowingPow = 0.8f;
|
||||
|
||||
// ===== 2腿检测（单线雷达识别两腿托盘 / 轮胎）=====
|
||||
[FieldMember(desc = "2腿检测：雷达名（逗号分隔可多个）")]
|
||||
public string TwoLegLidarName = "rear_left_lidar_1,rear_right_lidar_1";
|
||||
|
||||
[FieldMember(desc = "2腿检测：初始猜测X(mm, 车体坐标系)")]
|
||||
public float TwoLegGuessX = 2000f;
|
||||
|
||||
[FieldMember(desc = "2腿检测：两腿间距(mm)")]
|
||||
public float TwoLegWidth = 800f;
|
||||
|
||||
[FieldMember(desc = "2腿检测：两腿间距允许误差(mm)")]
|
||||
public float TwoLegWidthErr = 100f;
|
||||
|
||||
[FieldMember(desc = "2腿检测：聚类点间距(mm)")]
|
||||
public float TwoLegBlobDist = 100f;
|
||||
|
||||
[FieldMember(desc = "2腿检测：聚类尺寸(mm)")]
|
||||
public float TwoLegBlobSize = 200f;
|
||||
|
||||
[FieldMember(desc = "2腿检测：聚类最小点数")]
|
||||
public int TwoLegBlobPtCount = 5;
|
||||
|
||||
[FieldMember(desc = "2腿检测：聚类 padding")]
|
||||
public int TwoLegPadding = 5;
|
||||
|
||||
[FieldMember(desc = "2腿检测：腿柱搜索范围")]
|
||||
public int TwoLegPillarFindingScope = 20;
|
||||
|
||||
[FieldMember(desc = "2腿检测：方向符号(±1)")]
|
||||
public int TwoLegSgnDir = 1;
|
||||
|
||||
[FieldMember(desc = "2腿检测：中心X偏移(mm)")]
|
||||
public float TwoLegCenterChangeX = 0f;
|
||||
|
||||
[FieldMember(desc = "2腿检测：输出X补偿(mm)")]
|
||||
public float TwoLegOutputBiasX = 0f;
|
||||
|
||||
[FieldMember(desc = "2腿检测：输出Y补偿(mm)")]
|
||||
public float TwoLegOutputBiasY = 0f;
|
||||
|
||||
[FieldMember(desc = "2腿检测：ROI滤波框长(mm)")]
|
||||
public float TwoLegFilterLength = 1800f;
|
||||
|
||||
[FieldMember(desc = "2腿检测：ROI滤波框宽(mm)")]
|
||||
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 TireFollowingCloseDistance = 1400;
|
||||
[FieldMember(desc = "轮胎跟踪:距离过近角度忽略阈值")] public float TireFollowingAngleIgnoreThr = 0.2f;
|
||||
[FieldMember(desc = "轮胎跟踪:Y最大平均数")] public int TireFollowingYAverageFrameCount = 5;
|
||||
|
||||
[FieldMember(desc = "终点跟踪:速度")] public float DstTrackerMaxSpeed = 0.3f;
|
||||
[FieldMember(desc = "轮胎跟踪:释放锁点距离")] public float TireFollowingReleaseDistance = 1600;
|
||||
#endregion
|
||||
|
||||
[FieldMember(desc = "轮胎跟踪:角度调整kp")] public float TireFollowingThkp = 0.05f;
|
||||
[FieldMember(desc = "轮胎跟踪:角度调整ki")] public float TireFollowingThki = 0.01f;
|
||||
[FieldMember(desc = "轮胎跟踪:角度调整kd")] public float TireFollowingThkd = 0f;
|
||||
[FieldMember(desc = "轮胎跟踪:角度调整SpeedAcc")] public float TireFollowingThSpeedAccPerSec = 1f;
|
||||
[FieldMember(desc = "轮胎跟踪:角度调整Thresh")] public float TireFollowingThThresh = 0.1f;
|
||||
[FieldMember(desc = "轮胎跟踪:角度调整DeadZone")] public float TireFollowingThDeadZone = 5f;
|
||||
[FieldMember(desc = "轮胎跟踪:角度调整MaxI")] public float TireFollowingThMaxI = 0.01f;
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,81 @@
|
||||
using System;
|
||||
using System.Net.Http;
|
||||
using System.Text;
|
||||
using Newtonsoft.Json;
|
||||
using Newtonsoft.Json.Linq;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
/// <summary>
|
||||
/// Playground 仿真器 HTTP Web API 轻量客户端：查询小车位姿、瞬移小车。
|
||||
/// 服务端实现见 Playground/Web/PlaygroundWebApi.cs，默认监听 http://localhost:18090。
|
||||
/// 坐标单位 mm，朝向 yawDeg 单位为度，世界坐标系与场景 JSON 一致。
|
||||
/// </summary>
|
||||
public static class PlaygroundWebApi
|
||||
{
|
||||
// 禁用系统代理：本机 Playground 走 localhost，若经系统代理(如 127.0.0.1:7890)会连接失败。
|
||||
private static readonly HttpClient Http = new HttpClient(new HttpClientHandler { UseProxy = false })
|
||||
{
|
||||
Timeout = TimeSpan.FromSeconds(3)
|
||||
};
|
||||
|
||||
public struct Pose
|
||||
{
|
||||
public float X;
|
||||
public float Y;
|
||||
public float YawDeg;
|
||||
}
|
||||
|
||||
/// <summary>查询单台小车的世界位姿。GET /api/robots/{name}。</summary>
|
||||
public static Pose GetPose(string baseUrl, string robotName)
|
||||
{
|
||||
var url = $"{baseUrl.TrimEnd('/')}/api/robots/{Uri.EscapeDataString(robotName)}";
|
||||
var json = Http.GetStringAsync(url).GetAwaiter().GetResult();
|
||||
var o = JObject.Parse(json);
|
||||
return new Pose
|
||||
{
|
||||
X = o.Value<float>("x"),
|
||||
Y = o.Value<float>("y"),
|
||||
YawDeg = o.Value<float>("yawDeg")
|
||||
};
|
||||
}
|
||||
|
||||
/// <summary>将小车瞬移到目标世界位姿。POST /api/robots/{name}/move。</summary>
|
||||
public static void Move(string baseUrl, string robotName, float x, float y, float yawDeg)
|
||||
{
|
||||
var url = $"{baseUrl.TrimEnd('/')}/api/robots/{Uri.EscapeDataString(robotName)}/move";
|
||||
var body = JsonConvert.SerializeObject(new { x, y, yaw = yawDeg, stop = true });
|
||||
using var content = new StringContent(body, Encoding.UTF8, "application/json");
|
||||
var resp = Http.PostAsync(url, content).GetAwaiter().GetResult();
|
||||
resp.EnsureSuccessStatusCode();
|
||||
}
|
||||
|
||||
/// <summary>查询车辆运动是否启用（暂停时为 false）。GET /api/motion。</summary>
|
||||
public static bool MotionEnabled(string baseUrl)
|
||||
{
|
||||
var url = $"{baseUrl.TrimEnd('/')}/api/motion";
|
||||
var json = Http.GetStringAsync(url).GetAwaiter().GetResult();
|
||||
return JObject.Parse(json).Value<bool>("motionEnabled");
|
||||
}
|
||||
|
||||
/// <summary>恢复车辆运动。POST /api/motion/resume。</summary>
|
||||
public static void ResumeMotion(string baseUrl)
|
||||
{
|
||||
var url = $"{baseUrl.TrimEnd('/')}/api/motion/resume";
|
||||
var resp = Http.PostAsync(url, null).GetAwaiter().GetResult();
|
||||
resp.EnsureSuccessStatusCode();
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 暂停车辆运动（仅冻结运动，不停止仿真；传感器继续扫描）。POST /api/motion/pause。
|
||||
/// feedback: "zero"(默认,反馈归零) / "none"(不上报) / "hold"(保留暂停瞬间值)。
|
||||
/// </summary>
|
||||
public static void PauseMotion(string baseUrl, string feedback = "zero")
|
||||
{
|
||||
var url = $"{baseUrl.TrimEnd('/')}/api/motion/pause";
|
||||
var body = JsonConvert.SerializeObject(new { feedback });
|
||||
using var content = new StringContent(body, Encoding.UTF8, "application/json");
|
||||
var resp = Http.PostAsync(url, content).GetAwaiter().GetResult();
|
||||
resp.EnsureSuccessStatusCode();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,511 @@
|
||||
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;
|
||||
|
||||
// 末段衔接：接近盲走终点时给非零速度，供后续动作连续接管
|
||||
public bool EnableHandover = false;
|
||||
public float HandoverDistance = 200f; // mm
|
||||
public float HandoverSpeed = 0.2f; // m/s
|
||||
|
||||
/// <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<float> _targetYList = 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) / 2f - (a.Y - b.Y) / 2f * th;
|
||||
|
||||
y = (aDelta.Y + bDelta.Y) / 2f + (a.X - b.X) / 2f * 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));
|
||||
void HardStop()
|
||||
{
|
||||
_dt?.Stop();
|
||||
((MultiWheelChassis)PilotDefinition.Chassis).DriveStop();
|
||||
DLog.Log($"Hard Stop!", "TireFollowing");
|
||||
}
|
||||
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) / 2f;
|
||||
var lrDelta = (curLRLEncoder - _lastLRLEncoder + curLRREncoder - _lastLRREncoder) / 2f;
|
||||
var rfDelta = (curRFLEncoder - _lastRFLEncoder + curRFREncoder - _lastRFREncoder) / 2f;
|
||||
var rrDelta = (curRRLEncoder - _lastRRLEncoder + curRRREncoder - _lastRRREncoder) / 2f;
|
||||
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() / 180f * (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() / 180f * (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 * 180f: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.FinishDistance = 10f;
|
||||
controller.SlowDistance = SlowDistance;
|
||||
controller.SlowingPow = 0.7f;
|
||||
}
|
||||
if (EnableHandover)
|
||||
{
|
||||
controller.SlowDistance = float.MinValue;
|
||||
controller.FinishSpeed = 0.2f;
|
||||
controller.FinishDistance = 50;
|
||||
}
|
||||
(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) && !EnableHandover)
|
||||
{
|
||||
HardStop();
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
else if (WalkBlindStage2)
|
||||
{
|
||||
DLog.Log("达到第二对轮胎处，停止移动", "TireFollowing");
|
||||
if (!EnableHandover)
|
||||
{
|
||||
HardStop();
|
||||
}
|
||||
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) / 2f, (target.Src.Y + target.Dst.Y) / 2f);
|
||||
var dis2target = (float)Math.Sqrt(Math.Pow(targetPos.X, 2) + Math.Pow(targetPos.Y, 2));
|
||||
//距离较近以后角度容易跳变
|
||||
if (dis2target < PilotDefinition.Conf.TireFollowingCloseDistance && 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 / 2f;
|
||||
_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) / 2f).X;
|
||||
lastDetectY = ((target.Src + target.Dst) / 2f).Y;
|
||||
|
||||
var currentY = path.CarPath.Dst.Y;
|
||||
if (Math.Abs(targetAngle) < PilotDefinition.Conf.TireFollowingAngleIgnoreThr &&
|
||||
dis2target < PilotDefinition.Conf.TireFollowingCloseDistance)
|
||||
{
|
||||
_targetYList.Add(currentY);
|
||||
while (_targetYList.Count > PilotDefinition.Conf.TireFollowingYAverageFrameCount) _targetYList.RemoveAt(0);
|
||||
}
|
||||
|
||||
var trackDstY = _targetYList.Count > 0 ? _targetYList.Average() : currentY;
|
||||
Hedingben.ToastText($"target Y:{_targetYList.Count} {trackDstY}", "target Y");
|
||||
|
||||
var trackDst = new Vector2(path.CarPath.Dst.X, trackDstY);
|
||||
_painter.DrawLine(Color.GreenYellow, path.CarPath.Src, trackDst, endArrow: true);
|
||||
|
||||
var rd = (float)LessMath.PerpendicularPosition(0, 0, trackDst.X, trackDst.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(rd < PilotDefinition.Conf.TireFollowingReleaseDistance)
|
||||
{
|
||||
if (detectors[detectorIndex].SrcId != -1 && detectors[detectorIndex].LeaveSrcFunction != null)
|
||||
{
|
||||
detectors[detectorIndex].LeaveSrcFunction(detectors[detectorIndex].SrcId);
|
||||
DLog.Log($"释放预取车点{detectors[detectorIndex].SrcId}", "TireFollowing");
|
||||
}
|
||||
}
|
||||
|
||||
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 = trackDst.X;
|
||||
WalkBlindCarPathDstY = trackDst.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();
|
||||
_targetYList.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 = trackDst.X;
|
||||
WalkBlindCarPathDstY = trackDst.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();
|
||||
_targetYList.Clear();
|
||||
continue;
|
||||
}
|
||||
}
|
||||
DLog.Log($"投影点剩余距离:{rd:F2}", "TireFollowing");
|
||||
var track = new LineTrack(path.CarPath.Src, trackDst);
|
||||
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;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,277 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.IO;
|
||||
using System.Text;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
internal static class VehicleSyncBinaryCodec
|
||||
{
|
||||
private const byte Version = 2;
|
||||
private const byte RegisterType = 1;
|
||||
private const byte NotificationType = 2;
|
||||
private static readonly byte[] Magic = Encoding.ASCII.GetBytes("MVS1");
|
||||
|
||||
public static byte[] EncodeRegister(int carNum, VehicleSyncInfo info)
|
||||
{
|
||||
using var stream = new MemoryStream();
|
||||
using var writer = new BinaryWriter(stream, Encoding.UTF8);
|
||||
WriteHeader(writer, RegisterType);
|
||||
writer.Write(carNum);
|
||||
WriteInfo(writer, info);
|
||||
writer.Flush();
|
||||
return stream.ToArray();
|
||||
}
|
||||
|
||||
public static (int CarNum, VehicleSyncInfo Info) DecodeRegister(byte[] payload)
|
||||
{
|
||||
using var stream = new MemoryStream(payload ?? throw new ArgumentNullException(nameof(payload)));
|
||||
using var reader = new BinaryReader(stream, Encoding.UTF8);
|
||||
var version = ReadHeader(reader, RegisterType);
|
||||
var carNum = reader.ReadInt32();
|
||||
var info = ReadInfo(reader, version);
|
||||
EnsureFullyRead(stream);
|
||||
return (carNum, info);
|
||||
}
|
||||
|
||||
public static byte[] EncodeNotification(VehicleSyncNotification notification)
|
||||
{
|
||||
using var stream = new MemoryStream();
|
||||
using var writer = new BinaryWriter(stream, Encoding.UTF8);
|
||||
WriteHeader(writer, NotificationType);
|
||||
|
||||
writer.Write(notification.Seq);
|
||||
writer.Write(BuildNotificationFlags(notification));
|
||||
writer.Write(notification.Mode);
|
||||
writer.Write(notification.FleetStopSourceCar);
|
||||
writer.Write(notification.CenterX);
|
||||
writer.Write(notification.CenterY);
|
||||
writer.Write(notification.CenterTh);
|
||||
writer.Write(notification.FleetVx);
|
||||
writer.Write(notification.FleetFrontTh);
|
||||
writer.Write(notification.FleetRearTh);
|
||||
writer.Write(notification.FleetOmega);
|
||||
writer.Write(notification.RequestedFleetOmega);
|
||||
writer.Write(notification.SyncTh);
|
||||
writer.Write(notification.SyncDistance);
|
||||
writer.Write(notification.DeltaDetectCenter);
|
||||
writer.Write(notification.RotateActiveOmega);
|
||||
writer.Write(notification.RotateCompXyFac);
|
||||
writer.Write(notification.RotateCompXyIFac);
|
||||
writer.Write(notification.RotateCompXyMax);
|
||||
writer.Write(notification.RotateCompThFac);
|
||||
writer.Write(notification.RotateCompThIFac);
|
||||
writer.Write(notification.RotateCompThMax);
|
||||
writer.Write(notification.RotateCompTangentFrac);
|
||||
writer.Write(notification.RotateStartWheelAlignDeg);
|
||||
writer.Write(notification.RotateActiveWheelAlignDeg);
|
||||
writer.Write(notification.IdealX);
|
||||
writer.Write(notification.IdealY);
|
||||
writer.Write(notification.IdealTh);
|
||||
WriteString(writer, notification.FleetStopReason);
|
||||
|
||||
var fleet = notification.Fleet ?? new Dictionary<int, VehicleSyncInfo>();
|
||||
if (fleet.Count > ushort.MaxValue)
|
||||
throw new InvalidOperationException($"Fleet count {fleet.Count} exceeds binary protocol limit.");
|
||||
writer.Write((ushort)fleet.Count);
|
||||
foreach (var kv in fleet)
|
||||
{
|
||||
writer.Write(kv.Key);
|
||||
WriteInfo(writer, kv.Value);
|
||||
}
|
||||
|
||||
writer.Flush();
|
||||
return stream.ToArray();
|
||||
}
|
||||
|
||||
public static VehicleSyncNotification DecodeNotification(byte[] payload)
|
||||
{
|
||||
using var stream = new MemoryStream(payload ?? throw new ArgumentNullException(nameof(payload)));
|
||||
using var reader = new BinaryReader(stream, Encoding.UTF8);
|
||||
var version = ReadHeader(reader, NotificationType);
|
||||
|
||||
var notification = new VehicleSyncNotification
|
||||
{
|
||||
Seq = reader.ReadInt64()
|
||||
};
|
||||
|
||||
ApplyNotificationFlags(notification, reader.ReadUInt16());
|
||||
notification.Mode = reader.ReadInt32();
|
||||
notification.FleetStopSourceCar = reader.ReadInt32();
|
||||
notification.CenterX = reader.ReadSingle();
|
||||
notification.CenterY = reader.ReadSingle();
|
||||
notification.CenterTh = reader.ReadSingle();
|
||||
notification.FleetVx = reader.ReadSingle();
|
||||
notification.FleetFrontTh = reader.ReadSingle();
|
||||
notification.FleetRearTh = reader.ReadSingle();
|
||||
notification.FleetOmega = reader.ReadSingle();
|
||||
notification.RequestedFleetOmega = reader.ReadSingle();
|
||||
notification.SyncTh = reader.ReadSingle();
|
||||
notification.SyncDistance = reader.ReadSingle();
|
||||
notification.DeltaDetectCenter = reader.ReadSingle();
|
||||
notification.RotateActiveOmega = reader.ReadSingle();
|
||||
notification.RotateCompXyFac = reader.ReadSingle();
|
||||
notification.RotateCompXyIFac = reader.ReadSingle();
|
||||
notification.RotateCompXyMax = reader.ReadSingle();
|
||||
notification.RotateCompThFac = reader.ReadSingle();
|
||||
notification.RotateCompThIFac = reader.ReadSingle();
|
||||
notification.RotateCompThMax = reader.ReadSingle();
|
||||
notification.RotateCompTangentFrac = reader.ReadSingle();
|
||||
notification.RotateStartWheelAlignDeg = reader.ReadSingle();
|
||||
notification.RotateActiveWheelAlignDeg = reader.ReadSingle();
|
||||
notification.IdealX = reader.ReadSingle();
|
||||
notification.IdealY = reader.ReadSingle();
|
||||
notification.IdealTh = reader.ReadSingle();
|
||||
notification.FleetStopReason = ReadString(reader);
|
||||
|
||||
var fleetCount = reader.ReadUInt16();
|
||||
notification.Fleet = new Dictionary<int, VehicleSyncInfo>(fleetCount);
|
||||
for (var i = 0; i < fleetCount; ++i)
|
||||
{
|
||||
var carNum = reader.ReadInt32();
|
||||
notification.Fleet[carNum] = ReadInfo(reader, version);
|
||||
}
|
||||
|
||||
EnsureFullyRead(stream);
|
||||
return notification;
|
||||
}
|
||||
|
||||
private static void WriteHeader(BinaryWriter writer, byte type)
|
||||
{
|
||||
writer.Write(Magic);
|
||||
writer.Write(Version);
|
||||
writer.Write(type);
|
||||
writer.Write((ushort)0);
|
||||
}
|
||||
|
||||
private static byte ReadHeader(BinaryReader reader, byte expectedType)
|
||||
{
|
||||
for (var i = 0; i < Magic.Length; ++i)
|
||||
{
|
||||
if (reader.ReadByte() != Magic[i])
|
||||
throw new InvalidDataException("Invalid multi-vehicle sync binary magic.");
|
||||
}
|
||||
|
||||
var version = reader.ReadByte();
|
||||
if (version < 1 || version > Version)
|
||||
throw new InvalidDataException($"Unsupported multi-vehicle sync binary version {version}.");
|
||||
|
||||
var type = reader.ReadByte();
|
||||
if (type != expectedType)
|
||||
throw new InvalidDataException($"Unexpected multi-vehicle sync packet type {type}.");
|
||||
|
||||
var reserved = reader.ReadUInt16();
|
||||
if (reserved != 0)
|
||||
throw new InvalidDataException("Invalid multi-vehicle sync binary reserved field.");
|
||||
|
||||
return version;
|
||||
}
|
||||
|
||||
private static void WriteInfo(BinaryWriter writer, VehicleSyncInfo info)
|
||||
{
|
||||
writer.Write(BuildInfoFlags(info));
|
||||
WriteString(writer, info.Ip);
|
||||
writer.Write(info.Port);
|
||||
writer.Write(info.X);
|
||||
writer.Write(info.Y);
|
||||
writer.Write(info.Th);
|
||||
writer.Write(info.LayoutX);
|
||||
writer.Write(info.LayoutY);
|
||||
writer.Write(info.LayoutTh);
|
||||
WriteString(writer, info.MotionInfeasibleReason);
|
||||
WriteString(writer, info.RotateWheelAlignDetail);
|
||||
writer.Write(info.AppliedNotificationSeq);
|
||||
}
|
||||
|
||||
private static VehicleSyncInfo ReadInfo(BinaryReader reader, byte version)
|
||||
{
|
||||
var info = new VehicleSyncInfo();
|
||||
ApplyInfoFlags(info, reader.ReadUInt16());
|
||||
info.Ip = ReadString(reader);
|
||||
info.Port = reader.ReadInt32();
|
||||
info.X = reader.ReadSingle();
|
||||
info.Y = reader.ReadSingle();
|
||||
info.Th = reader.ReadSingle();
|
||||
info.LayoutX = reader.ReadSingle();
|
||||
info.LayoutY = reader.ReadSingle();
|
||||
info.LayoutTh = reader.ReadSingle();
|
||||
info.MotionInfeasibleReason = ReadString(reader);
|
||||
info.RotateWheelAlignDetail = ReadString(reader);
|
||||
info.AppliedNotificationSeq = version >= 2 ? reader.ReadInt64() : -1;
|
||||
return info;
|
||||
}
|
||||
|
||||
private static ushort BuildInfoFlags(VehicleSyncInfo info)
|
||||
{
|
||||
ushort flags = 0;
|
||||
if (info.Master) flags |= 1 << 0;
|
||||
if (info.PosAvailable) flags |= 1 << 1;
|
||||
if (info.Aligned) flags |= 1 << 2;
|
||||
if (info.DetectOk) flags |= 1 << 3;
|
||||
if (info.MotionFeasible) flags |= 1 << 4;
|
||||
if (info.RotateWheelsAligned) flags |= 1 << 5;
|
||||
return flags;
|
||||
}
|
||||
|
||||
private static void ApplyInfoFlags(VehicleSyncInfo info, ushort flags)
|
||||
{
|
||||
info.Master = (flags & (1 << 0)) != 0;
|
||||
info.PosAvailable = (flags & (1 << 1)) != 0;
|
||||
info.Aligned = (flags & (1 << 2)) != 0;
|
||||
info.DetectOk = (flags & (1 << 3)) != 0;
|
||||
info.MotionFeasible = (flags & (1 << 4)) != 0;
|
||||
info.RotateWheelsAligned = (flags & (1 << 5)) != 0;
|
||||
}
|
||||
|
||||
private static ushort BuildNotificationFlags(VehicleSyncNotification notification)
|
||||
{
|
||||
ushort flags = 0;
|
||||
if (notification.PosAvailable) flags |= 1 << 0;
|
||||
if (notification.Aligned) flags |= 1 << 1;
|
||||
if (notification.FleetMotionReleased) flags |= 1 << 2;
|
||||
if (notification.FleetStopActive) flags |= 1 << 3;
|
||||
if (notification.AutoEnabled) flags |= 1 << 4;
|
||||
if (notification.ManualEnabled) flags |= 1 << 5;
|
||||
if (notification.HasIdeal) flags |= 1 << 6;
|
||||
if (notification.RotateParamsValid) flags |= 1 << 7;
|
||||
if (notification.UseDetourCorrection) flags |= 1 << 8;
|
||||
return flags;
|
||||
}
|
||||
|
||||
private static void ApplyNotificationFlags(VehicleSyncNotification notification, ushort flags)
|
||||
{
|
||||
notification.PosAvailable = (flags & (1 << 0)) != 0;
|
||||
notification.Aligned = (flags & (1 << 1)) != 0;
|
||||
notification.FleetMotionReleased = (flags & (1 << 2)) != 0;
|
||||
notification.FleetStopActive = (flags & (1 << 3)) != 0;
|
||||
notification.AutoEnabled = (flags & (1 << 4)) != 0;
|
||||
notification.ManualEnabled = (flags & (1 << 5)) != 0;
|
||||
notification.HasIdeal = (flags & (1 << 6)) != 0;
|
||||
notification.RotateParamsValid = (flags & (1 << 7)) != 0;
|
||||
notification.UseDetourCorrection = (flags & (1 << 8)) != 0;
|
||||
}
|
||||
|
||||
private static void WriteString(BinaryWriter writer, string value)
|
||||
{
|
||||
var bytes = Encoding.UTF8.GetBytes(value ?? "");
|
||||
if (bytes.Length > ushort.MaxValue)
|
||||
throw new InvalidOperationException($"String payload length {bytes.Length} exceeds binary protocol limit.");
|
||||
writer.Write((ushort)bytes.Length);
|
||||
writer.Write(bytes);
|
||||
}
|
||||
|
||||
private static string ReadString(BinaryReader reader)
|
||||
{
|
||||
var length = reader.ReadUInt16();
|
||||
var bytes = reader.ReadBytes(length);
|
||||
if (bytes.Length != length)
|
||||
throw new EndOfStreamException("Truncated multi-vehicle sync string payload.");
|
||||
return Encoding.UTF8.GetString(bytes);
|
||||
}
|
||||
|
||||
private static void EnsureFullyRead(MemoryStream stream)
|
||||
{
|
||||
if (stream.Position != stream.Length)
|
||||
throw new InvalidDataException("Unexpected trailing bytes in multi-vehicle sync packet.");
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,75 @@
|
||||
using System.Collections.Generic;
|
||||
using ClumsyCore;
|
||||
using Newtonsoft.Json;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
public class VehicleSyncInfo
|
||||
{
|
||||
[JsonProperty("Master")] public bool Master { get; set; }
|
||||
[JsonProperty("Ip")] public string Ip { get; set; } = "";
|
||||
[JsonProperty("Port")] public int Port { get; set; } = 8008;
|
||||
[JsonProperty("PosAvailable")] public bool PosAvailable { get; set; }
|
||||
[JsonProperty("X")] public float X { get; set; }
|
||||
[JsonProperty("Y")] public float Y { get; set; }
|
||||
[JsonProperty("Th")] public float Th { get; set; }
|
||||
[JsonProperty("LayoutX")] public float LayoutX { get; set; }
|
||||
[JsonProperty("LayoutY")] public float LayoutY { get; set; }
|
||||
[JsonProperty("LayoutTh")] public float LayoutTh { get; set; }
|
||||
[JsonProperty("Aligned")] public bool Aligned { get; set; }
|
||||
// 本车本轮是否成功识别到邻车（关闭互识别时恒为 true）。任一车为 false 则整队停车。
|
||||
[JsonProperty("DetectOk")] public bool DetectOk { get; set; }
|
||||
[JsonProperty("MotionFeasible")] public bool MotionFeasible { get; set; } = true;
|
||||
[JsonProperty("MotionInfeasibleReason")] public string MotionInfeasibleReason { get; set; } = "";
|
||||
[JsonProperty("RotateWheelsAligned")] public bool RotateWheelsAligned { get; set; } = true;
|
||||
[JsonProperty("RotateWheelAlignDetail")] public string RotateWheelAlignDetail { get; set; } = "";
|
||||
[JsonProperty("AppliedNotificationSeq")] public long AppliedNotificationSeq { get; set; } = -1;
|
||||
}
|
||||
|
||||
public class VehicleSyncNotification
|
||||
{
|
||||
[JsonProperty("PosAvailable")] public bool PosAvailable { get; set; }
|
||||
[JsonProperty("CenterX")] public float CenterX { get; set; }
|
||||
[JsonProperty("CenterY")] public float CenterY { get; set; }
|
||||
[JsonProperty("CenterTh")] public float CenterTh { get; set; }
|
||||
[JsonProperty("Aligned")] public bool Aligned { get; set; }
|
||||
[JsonProperty("Fleet")] public Dictionary<int, VehicleSyncInfo> Fleet { get; set; } = new();
|
||||
[JsonProperty("FleetVx")] public float FleetVx { get; set; }
|
||||
[JsonProperty("FleetFrontTh")] public float FleetFrontTh { get; set; }
|
||||
[JsonProperty("FleetRearTh")] public float FleetRearTh { get; set; }
|
||||
// 联动运动模式：0=常规(前进+转向) 1=蟹行(四轮同向平移) 2=原地旋转(绕车队中心)
|
||||
[JsonProperty("Mode")] public int Mode { get; set; }
|
||||
// 原地旋转角速度(deg/s，逆时针为正)，仅 Mode==2 有效
|
||||
[JsonProperty("FleetOmega")] public float FleetOmega { get; set; }
|
||||
[JsonProperty("RequestedFleetOmega")] public float RequestedFleetOmega { get; set; }
|
||||
[JsonProperty("FleetMotionReleased")] public bool FleetMotionReleased { get; set; } = true;
|
||||
[JsonProperty("FleetStopActive")] public bool FleetStopActive { get; set; }
|
||||
[JsonProperty("FleetStopReason")] public string FleetStopReason { get; set; } = "";
|
||||
[JsonProperty("FleetStopSourceCar")] public int FleetStopSourceCar { get; set; }
|
||||
[JsonProperty("AutoEnabled")] public bool AutoEnabled { get; set; }
|
||||
[JsonProperty("ManualEnabled")] public bool ManualEnabled { get; set; }
|
||||
[JsonProperty("UseDetourCorrection")] public bool UseDetourCorrection { get; set; }
|
||||
[JsonProperty("SyncTh")] public float SyncTh { get; set; }
|
||||
[JsonProperty("SyncDistance")] public float SyncDistance { get; set; }
|
||||
[JsonProperty("DeltaDetectCenter")] public float DeltaDetectCenter { get; set; }
|
||||
// 原地旋转纠偏参数由主车广播，从车运行时使用同一套增益/限幅，避免主从补偿强度不一致。
|
||||
[JsonProperty("RotateParamsValid")] public bool RotateParamsValid { get; set; }
|
||||
[JsonProperty("RotateActiveOmega")] public float RotateActiveOmega { get; set; }
|
||||
[JsonProperty("RotateCompXyFac")] public float RotateCompXyFac { get; set; }
|
||||
[JsonProperty("RotateCompXyIFac")] public float RotateCompXyIFac { get; set; }
|
||||
[JsonProperty("RotateCompXyMax")] public float RotateCompXyMax { get; set; }
|
||||
[JsonProperty("RotateCompThFac")] public float RotateCompThFac { get; set; }
|
||||
[JsonProperty("RotateCompThIFac")] public float RotateCompThIFac { get; set; }
|
||||
[JsonProperty("RotateCompThMax")] public float RotateCompThMax { get; set; }
|
||||
[JsonProperty("RotateCompTangentFrac")] public float RotateCompTangentFrac { get; set; }
|
||||
[JsonProperty("RotateStartWheelAlignDeg")] public float RotateStartWheelAlignDeg { get; set; }
|
||||
[JsonProperty("RotateActiveWheelAlignDeg")] public float RotateActiveWheelAlignDeg { get; set; }
|
||||
// F: 单调递增序列号，从车据此丢弃乱序到达的旧 notify 包。
|
||||
[JsonProperty("Seq")] public long Seq { get; set; }
|
||||
// D: 自动模式下主车路径控制器算出的车队中心理想位姿（世界系），由 idealPos/idealAngle 透传而来。
|
||||
// HasIdeal=true 时各从车按各自 layout 推算 per-car 目标位姿做前馈+补偿，弧线路径不再只靠事后纠偏。
|
||||
[JsonProperty("HasIdeal")] public bool HasIdeal { get; set; }
|
||||
[JsonProperty("IdealX")] public float IdealX { get; set; }
|
||||
[JsonProperty("IdealY")] public float IdealY { get; set; }
|
||||
[JsonProperty("IdealTh")] public float IdealTh { get; set; }
|
||||
}
|
||||
@@ -0,0 +1,84 @@
|
||||
{
|
||||
"format": 1,
|
||||
"restore": {
|
||||
"D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\ClumsyPilot.csproj": {}
|
||||
},
|
||||
"projects": {
|
||||
"D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\ClumsyPilot.csproj": {
|
||||
"version": "1.0.0",
|
||||
"restore": {
|
||||
"projectUniqueName": "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\ClumsyPilot.csproj",
|
||||
"projectName": "ClumsyPilot",
|
||||
"projectPath": "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\ClumsyPilot.csproj",
|
||||
"packagesPath": "C:\\Users\\Fairyland\\.nuget\\packages\\",
|
||||
"outputPath": "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\obj\\",
|
||||
"projectStyle": "PackageReference",
|
||||
"fallbackFolders": [
|
||||
"D:\\VS2022\\Shared\\NuGetPackages"
|
||||
],
|
||||
"configFilePaths": [
|
||||
"C:\\Users\\Fairyland\\AppData\\Roaming\\NuGet\\NuGet.Config",
|
||||
"C:\\Program Files (x86)\\NuGet\\Config\\Microsoft.VisualStudio.FallbackLocation.config",
|
||||
"C:\\Program Files (x86)\\NuGet\\Config\\Microsoft.VisualStudio.Offline.config"
|
||||
],
|
||||
"originalTargetFrameworks": [
|
||||
"netstandard2.0"
|
||||
],
|
||||
"sources": {
|
||||
"C:\\Program Files (x86)\\Microsoft SDKs\\NuGetPackages\\": {},
|
||||
"https://api.nuget.org/v3/index.json": {}
|
||||
},
|
||||
"frameworks": {
|
||||
"netstandard2.0": {
|
||||
"targetAlias": "netstandard2.0",
|
||||
"projectReferences": {}
|
||||
}
|
||||
},
|
||||
"warningProperties": {
|
||||
"warnAsError": [
|
||||
"NU1605"
|
||||
]
|
||||
},
|
||||
"restoreAuditProperties": {
|
||||
"enableAudit": "true",
|
||||
"auditLevel": "low",
|
||||
"auditMode": "direct"
|
||||
},
|
||||
"SdkAnalysisLevel": "9.0.300"
|
||||
},
|
||||
"frameworks": {
|
||||
"netstandard2.0": {
|
||||
"targetAlias": "netstandard2.0",
|
||||
"dependencies": {
|
||||
"NETStandard.Library": {
|
||||
"suppressParent": "All",
|
||||
"target": "Package",
|
||||
"version": "[2.0.3, )",
|
||||
"autoReferenced": true
|
||||
},
|
||||
"Newtonsoft.Json": {
|
||||
"target": "Package",
|
||||
"version": "[13.0.4, )"
|
||||
},
|
||||
"System.Numerics.Vectors": {
|
||||
"target": "Package",
|
||||
"version": "[4.6.1, )"
|
||||
}
|
||||
},
|
||||
"imports": [
|
||||
"net461",
|
||||
"net462",
|
||||
"net47",
|
||||
"net471",
|
||||
"net472",
|
||||
"net48",
|
||||
"net481"
|
||||
],
|
||||
"assetTargetFallback": true,
|
||||
"warn": true,
|
||||
"runtimeIdentifierGraphPath": "C:\\Program Files\\dotnet\\sdk\\9.0.308\\RuntimeIdentifierGraph.json"
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,16 @@
|
||||
<?xml version="1.0" encoding="utf-8" standalone="no"?>
|
||||
<Project ToolsVersion="14.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
||||
<PropertyGroup Condition=" '$(ExcludeRestorePackageImports)' != 'true' ">
|
||||
<RestoreSuccess Condition=" '$(RestoreSuccess)' == '' ">True</RestoreSuccess>
|
||||
<RestoreTool Condition=" '$(RestoreTool)' == '' ">NuGet</RestoreTool>
|
||||
<ProjectAssetsFile Condition=" '$(ProjectAssetsFile)' == '' ">$(MSBuildThisFileDirectory)project.assets.json</ProjectAssetsFile>
|
||||
<NuGetPackageRoot Condition=" '$(NuGetPackageRoot)' == '' ">$(UserProfile)\.nuget\packages\</NuGetPackageRoot>
|
||||
<NuGetPackageFolders Condition=" '$(NuGetPackageFolders)' == '' ">C:\Users\Fairyland\.nuget\packages\;D:\VS2022\Shared\NuGetPackages</NuGetPackageFolders>
|
||||
<NuGetProjectStyle Condition=" '$(NuGetProjectStyle)' == '' ">PackageReference</NuGetProjectStyle>
|
||||
<NuGetToolVersion Condition=" '$(NuGetToolVersion)' == '' ">6.14.2</NuGetToolVersion>
|
||||
</PropertyGroup>
|
||||
<ItemGroup Condition=" '$(ExcludeRestorePackageImports)' != 'true' ">
|
||||
<SourceRoot Include="C:\Users\Fairyland\.nuget\packages\" />
|
||||
<SourceRoot Include="D:\VS2022\Shared\NuGetPackages\" />
|
||||
</ItemGroup>
|
||||
</Project>
|
||||
@@ -0,0 +1,6 @@
|
||||
<?xml version="1.0" encoding="utf-8" standalone="no"?>
|
||||
<Project ToolsVersion="14.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
||||
<ImportGroup Condition=" '$(ExcludeRestorePackageImports)' != 'true' ">
|
||||
<Import Project="$(NuGetPackageRoot)netstandard.library\2.0.3\build\netstandard2.0\NETStandard.Library.targets" Condition="Exists('$(NuGetPackageRoot)netstandard.library\2.0.3\build\netstandard2.0\NETStandard.Library.targets')" />
|
||||
</ImportGroup>
|
||||
</Project>
|
||||
@@ -0,0 +1,4 @@
|
||||
// <autogenerated />
|
||||
using System;
|
||||
using System.Reflection;
|
||||
[assembly: global::System.Runtime.Versioning.TargetFrameworkAttribute(".NETStandard,Version=v2.0", FrameworkDisplayName = ".NET Standard 2.0")]
|
||||
@@ -0,0 +1,23 @@
|
||||
//------------------------------------------------------------------------------
|
||||
// <auto-generated>
|
||||
// 此代码由工具生成。
|
||||
// 运行时版本:4.0.30319.42000
|
||||
//
|
||||
// 对此文件的更改可能会导致不正确的行为，并且如果
|
||||
// 重新生成代码，这些更改将会丢失。
|
||||
// </auto-generated>
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
using System;
|
||||
using System.Reflection;
|
||||
|
||||
[assembly: System.Reflection.AssemblyCompanyAttribute("ClumsyPilot")]
|
||||
[assembly: System.Reflection.AssemblyConfigurationAttribute("Debug")]
|
||||
[assembly: System.Reflection.AssemblyFileVersionAttribute("1.0.0.0")]
|
||||
[assembly: System.Reflection.AssemblyInformationalVersionAttribute("1.0.0")]
|
||||
[assembly: System.Reflection.AssemblyProductAttribute("ClumsyPilot")]
|
||||
[assembly: System.Reflection.AssemblyTitleAttribute("ClumsyPilot")]
|
||||
[assembly: System.Reflection.AssemblyVersionAttribute("1.0.0.0")]
|
||||
|
||||
// 由 MSBuild WriteCodeFragment 类生成。
|
||||
|
||||
@@ -0,0 +1 @@
|
||||
5d77794fa0720c6591db5b06ac60427413c18989a6f7b64420ccb07d122d85bc
|
||||
@@ -0,0 +1,8 @@
|
||||
is_global = true
|
||||
build_property.RootNamespace = ClumsyPilot
|
||||
build_property.ProjectDir = D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\
|
||||
build_property.EnableComHosting =
|
||||
build_property.EnableGeneratedComInterfaceComImportInterop =
|
||||
build_property.CsWinRTUseWindowsUIXamlProjections = false
|
||||
build_property.EffectiveAnalysisLevelStyle =
|
||||
build_property.EnableCodeStyleSeverity =
|
||||
Binary file not shown.
Binary file not shown.
@@ -0,0 +1 @@
|
||||
622a214d5f25f6580d0c98fb28e188cba923d74f280b70d75d03c4009c18c55b
|
||||
@@ -0,0 +1,11 @@
|
||||
D:\@FariyLandTask\@FRLD-GitProject\build\Clumsy\ClumsyPilot.deps.json
|
||||
D:\@FariyLandTask\@FRLD-GitProject\build\Clumsy\ClumsyPilot.dll
|
||||
D:\@FariyLandTask\@FRLD-GitProject\build\Clumsy\ClumsyPilot.pdb
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\ClumsyPilot.csproj.AssemblyReference.cache
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\ClumsyPilot.GeneratedMSBuildEditorConfig.editorconfig
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\ClumsyPilot.AssemblyInfoInputs.cache
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\ClumsyPilot.AssemblyInfo.cs
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\ClumsyPilot.csproj.CoreCompileInputs.cache
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\ClumsyPi.5EF10E9F.Up2Date
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\ClumsyPilot.dll
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\ClumsyPilot.pdb
|
||||
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,4 @@
|
||||
// <autogenerated />
|
||||
using System;
|
||||
using System.Reflection;
|
||||
[assembly: global::System.Runtime.Versioning.TargetFrameworkAttribute(".NETStandard,Version=v2.0", FrameworkDisplayName = ".NET Standard 2.0")]
|
||||
@@ -0,0 +1,23 @@
|
||||
//------------------------------------------------------------------------------
|
||||
// <auto-generated>
|
||||
// 此代码由工具生成。
|
||||
// 运行时版本:4.0.30319.42000
|
||||
//
|
||||
// 对此文件的更改可能会导致不正确的行为，并且如果
|
||||
// 重新生成代码，这些更改将会丢失。
|
||||
// </auto-generated>
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
using System;
|
||||
using System.Reflection;
|
||||
|
||||
[assembly: System.Reflection.AssemblyCompanyAttribute("ClumsyPilot")]
|
||||
[assembly: System.Reflection.AssemblyConfigurationAttribute("Debug")]
|
||||
[assembly: System.Reflection.AssemblyFileVersionAttribute("1.0.0.0")]
|
||||
[assembly: System.Reflection.AssemblyInformationalVersionAttribute("1.0.0")]
|
||||
[assembly: System.Reflection.AssemblyProductAttribute("ClumsyPilot")]
|
||||
[assembly: System.Reflection.AssemblyTitleAttribute("ClumsyPilot")]
|
||||
[assembly: System.Reflection.AssemblyVersionAttribute("1.0.0.0")]
|
||||
|
||||
// 由 MSBuild WriteCodeFragment 类生成。
|
||||
|
||||
@@ -0,0 +1 @@
|
||||
5d77794fa0720c6591db5b06ac60427413c18989a6f7b64420ccb07d122d85bc
|
||||
+8
@@ -0,0 +1,8 @@
|
||||
is_global = true
|
||||
build_property.RootNamespace = ClumsyPilot
|
||||
build_property.ProjectDir = D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\
|
||||
build_property.EnableComHosting =
|
||||
build_property.EnableGeneratedComInterfaceComImportInterop =
|
||||
build_property.CsWinRTUseWindowsUIXamlProjections = false
|
||||
build_property.EffectiveAnalysisLevelStyle =
|
||||
build_property.EnableCodeStyleSeverity =
|
||||
Binary file not shown.
Binary file not shown.
@@ -0,0 +1 @@
|
||||
66346c5ecc38869ab6b6f447e1f54c24aba69f81ee10b5736dee140cd5a474b8
|
||||
@@ -0,0 +1,5 @@
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\netstandard2.0\ClumsyPilot.csproj.AssemblyReference.cache
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\netstandard2.0\ClumsyPilot.GeneratedMSBuildEditorConfig.editorconfig
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\netstandard2.0\ClumsyPilot.AssemblyInfoInputs.cache
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\netstandard2.0\ClumsyPilot.AssemblyInfo.cs
|
||||
D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\obj\Debug\netstandard2.0\ClumsyPilot.csproj.CoreCompileInputs.cache
|
||||
@@ -0,0 +1,341 @@
|
||||
{
|
||||
"version": 3,
|
||||
"targets": {
|
||||
".NETStandard,Version=v2.0": {
|
||||
"Microsoft.NETCore.Platforms/1.1.0": {
|
||||
"type": "package",
|
||||
"compile": {
|
||||
"lib/netstandard1.0/_._": {}
|
||||
},
|
||||
"runtime": {
|
||||
"lib/netstandard1.0/_._": {}
|
||||
}
|
||||
},
|
||||
"NETStandard.Library/2.0.3": {
|
||||
"type": "package",
|
||||
"dependencies": {
|
||||
"Microsoft.NETCore.Platforms": "1.1.0"
|
||||
},
|
||||
"compile": {
|
||||
"lib/netstandard1.0/_._": {}
|
||||
},
|
||||
"runtime": {
|
||||
"lib/netstandard1.0/_._": {}
|
||||
},
|
||||
"build": {
|
||||
"build/netstandard2.0/NETStandard.Library.targets": {}
|
||||
}
|
||||
},
|
||||
"Newtonsoft.Json/13.0.4": {
|
||||
"type": "package",
|
||||
"compile": {
|
||||
"lib/netstandard2.0/Newtonsoft.Json.dll": {
|
||||
"related": ".xml"
|
||||
}
|
||||
},
|
||||
"runtime": {
|
||||
"lib/netstandard2.0/Newtonsoft.Json.dll": {
|
||||
"related": ".xml"
|
||||
}
|
||||
}
|
||||
},
|
||||
"System.Numerics.Vectors/4.6.1": {
|
||||
"type": "package",
|
||||
"compile": {
|
||||
"lib/netstandard2.0/System.Numerics.Vectors.dll": {
|
||||
"related": ".xml"
|
||||
}
|
||||
},
|
||||
"runtime": {
|
||||
"lib/netstandard2.0/System.Numerics.Vectors.dll": {
|
||||
"related": ".xml"
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
},
|
||||
"libraries": {
|
||||
"Microsoft.NETCore.Platforms/1.1.0": {
|
||||
"sha512": "kz0PEW2lhqygehI/d6XsPCQzD7ff7gUJaVGPVETX611eadGsA3A877GdSlU0LRVMCTH/+P3o2iDTak+S08V2+A==",
|
||||
"type": "package",
|
||||
"path": "microsoft.netcore.platforms/1.1.0",
|
||||
"files": [
|
||||
".nupkg.metadata",
|
||||
".signature.p7s",
|
||||
"ThirdPartyNotices.txt",
|
||||
"dotnet_library_license.txt",
|
||||
"lib/netstandard1.0/_._",
|
||||
"microsoft.netcore.platforms.1.1.0.nupkg.sha512",
|
||||
"microsoft.netcore.platforms.nuspec",
|
||||
"runtime.json"
|
||||
]
|
||||
},
|
||||
"NETStandard.Library/2.0.3": {
|
||||
"sha512": "st47PosZSHrjECdjeIzZQbzivYBJFv6P2nv4cj2ypdI204DO+vZ7l5raGMiX4eXMJ53RfOIg+/s4DHVZ54Nu2A==",
|
||||
"type": "package",
|
||||
"path": "netstandard.library/2.0.3",
|
||||
"files": [
|
||||
".nupkg.metadata",
|
||||
".signature.p7s",
|
||||
"LICENSE.TXT",
|
||||
"THIRD-PARTY-NOTICES.TXT",
|
||||
"build/netstandard2.0/NETStandard.Library.targets",
|
||||
"build/netstandard2.0/ref/Microsoft.Win32.Primitives.dll",
|
||||
"build/netstandard2.0/ref/System.AppContext.dll",
|
||||
"build/netstandard2.0/ref/System.Collections.Concurrent.dll",
|
||||
"build/netstandard2.0/ref/System.Collections.NonGeneric.dll",
|
||||
"build/netstandard2.0/ref/System.Collections.Specialized.dll",
|
||||
"build/netstandard2.0/ref/System.Collections.dll",
|
||||
"build/netstandard2.0/ref/System.ComponentModel.Composition.dll",
|
||||
"build/netstandard2.0/ref/System.ComponentModel.EventBasedAsync.dll",
|
||||
"build/netstandard2.0/ref/System.ComponentModel.Primitives.dll",
|
||||
"build/netstandard2.0/ref/System.ComponentModel.TypeConverter.dll",
|
||||
"build/netstandard2.0/ref/System.ComponentModel.dll",
|
||||
"build/netstandard2.0/ref/System.Console.dll",
|
||||
"build/netstandard2.0/ref/System.Core.dll",
|
||||
"build/netstandard2.0/ref/System.Data.Common.dll",
|
||||
"build/netstandard2.0/ref/System.Data.dll",
|
||||
"build/netstandard2.0/ref/System.Diagnostics.Contracts.dll",
|
||||
"build/netstandard2.0/ref/System.Diagnostics.Debug.dll",
|
||||
"build/netstandard2.0/ref/System.Diagnostics.FileVersionInfo.dll",
|
||||
"build/netstandard2.0/ref/System.Diagnostics.Process.dll",
|
||||
"build/netstandard2.0/ref/System.Diagnostics.StackTrace.dll",
|
||||
"build/netstandard2.0/ref/System.Diagnostics.TextWriterTraceListener.dll",
|
||||
"build/netstandard2.0/ref/System.Diagnostics.Tools.dll",
|
||||
"build/netstandard2.0/ref/System.Diagnostics.TraceSource.dll",
|
||||
"build/netstandard2.0/ref/System.Diagnostics.Tracing.dll",
|
||||
"build/netstandard2.0/ref/System.Drawing.Primitives.dll",
|
||||
"build/netstandard2.0/ref/System.Drawing.dll",
|
||||
"build/netstandard2.0/ref/System.Dynamic.Runtime.dll",
|
||||
"build/netstandard2.0/ref/System.Globalization.Calendars.dll",
|
||||
"build/netstandard2.0/ref/System.Globalization.Extensions.dll",
|
||||
"build/netstandard2.0/ref/System.Globalization.dll",
|
||||
"build/netstandard2.0/ref/System.IO.Compression.FileSystem.dll",
|
||||
"build/netstandard2.0/ref/System.IO.Compression.ZipFile.dll",
|
||||
"build/netstandard2.0/ref/System.IO.Compression.dll",
|
||||
"build/netstandard2.0/ref/System.IO.FileSystem.DriveInfo.dll",
|
||||
"build/netstandard2.0/ref/System.IO.FileSystem.Primitives.dll",
|
||||
"build/netstandard2.0/ref/System.IO.FileSystem.Watcher.dll",
|
||||
"build/netstandard2.0/ref/System.IO.FileSystem.dll",
|
||||
"build/netstandard2.0/ref/System.IO.IsolatedStorage.dll",
|
||||
"build/netstandard2.0/ref/System.IO.MemoryMappedFiles.dll",
|
||||
"build/netstandard2.0/ref/System.IO.Pipes.dll",
|
||||
"build/netstandard2.0/ref/System.IO.UnmanagedMemoryStream.dll",
|
||||
"build/netstandard2.0/ref/System.IO.dll",
|
||||
"build/netstandard2.0/ref/System.Linq.Expressions.dll",
|
||||
"build/netstandard2.0/ref/System.Linq.Parallel.dll",
|
||||
"build/netstandard2.0/ref/System.Linq.Queryable.dll",
|
||||
"build/netstandard2.0/ref/System.Linq.dll",
|
||||
"build/netstandard2.0/ref/System.Net.Http.dll",
|
||||
"build/netstandard2.0/ref/System.Net.NameResolution.dll",
|
||||
"build/netstandard2.0/ref/System.Net.NetworkInformation.dll",
|
||||
"build/netstandard2.0/ref/System.Net.Ping.dll",
|
||||
"build/netstandard2.0/ref/System.Net.Primitives.dll",
|
||||
"build/netstandard2.0/ref/System.Net.Requests.dll",
|
||||
"build/netstandard2.0/ref/System.Net.Security.dll",
|
||||
"build/netstandard2.0/ref/System.Net.Sockets.dll",
|
||||
"build/netstandard2.0/ref/System.Net.WebHeaderCollection.dll",
|
||||
"build/netstandard2.0/ref/System.Net.WebSockets.Client.dll",
|
||||
"build/netstandard2.0/ref/System.Net.WebSockets.dll",
|
||||
"build/netstandard2.0/ref/System.Net.dll",
|
||||
"build/netstandard2.0/ref/System.Numerics.dll",
|
||||
"build/netstandard2.0/ref/System.ObjectModel.dll",
|
||||
"build/netstandard2.0/ref/System.Reflection.Extensions.dll",
|
||||
"build/netstandard2.0/ref/System.Reflection.Primitives.dll",
|
||||
"build/netstandard2.0/ref/System.Reflection.dll",
|
||||
"build/netstandard2.0/ref/System.Resources.Reader.dll",
|
||||
"build/netstandard2.0/ref/System.Resources.ResourceManager.dll",
|
||||
"build/netstandard2.0/ref/System.Resources.Writer.dll",
|
||||
"build/netstandard2.0/ref/System.Runtime.CompilerServices.VisualC.dll",
|
||||
"build/netstandard2.0/ref/System.Runtime.Extensions.dll",
|
||||
"build/netstandard2.0/ref/System.Runtime.Handles.dll",
|
||||
"build/netstandard2.0/ref/System.Runtime.InteropServices.RuntimeInformation.dll",
|
||||
"build/netstandard2.0/ref/System.Runtime.InteropServices.dll",
|
||||
"build/netstandard2.0/ref/System.Runtime.Numerics.dll",
|
||||
"build/netstandard2.0/ref/System.Runtime.Serialization.Formatters.dll",
|
||||
"build/netstandard2.0/ref/System.Runtime.Serialization.Json.dll",
|
||||
"build/netstandard2.0/ref/System.Runtime.Serialization.Primitives.dll",
|
||||
"build/netstandard2.0/ref/System.Runtime.Serialization.Xml.dll",
|
||||
"build/netstandard2.0/ref/System.Runtime.Serialization.dll",
|
||||
"build/netstandard2.0/ref/System.Runtime.dll",
|
||||
"build/netstandard2.0/ref/System.Security.Claims.dll",
|
||||
"build/netstandard2.0/ref/System.Security.Cryptography.Algorithms.dll",
|
||||
"build/netstandard2.0/ref/System.Security.Cryptography.Csp.dll",
|
||||
"build/netstandard2.0/ref/System.Security.Cryptography.Encoding.dll",
|
||||
"build/netstandard2.0/ref/System.Security.Cryptography.Primitives.dll",
|
||||
"build/netstandard2.0/ref/System.Security.Cryptography.X509Certificates.dll",
|
||||
"build/netstandard2.0/ref/System.Security.Principal.dll",
|
||||
"build/netstandard2.0/ref/System.Security.SecureString.dll",
|
||||
"build/netstandard2.0/ref/System.ServiceModel.Web.dll",
|
||||
"build/netstandard2.0/ref/System.Text.Encoding.Extensions.dll",
|
||||
"build/netstandard2.0/ref/System.Text.Encoding.dll",
|
||||
"build/netstandard2.0/ref/System.Text.RegularExpressions.dll",
|
||||
"build/netstandard2.0/ref/System.Threading.Overlapped.dll",
|
||||
"build/netstandard2.0/ref/System.Threading.Tasks.Parallel.dll",
|
||||
"build/netstandard2.0/ref/System.Threading.Tasks.dll",
|
||||
"build/netstandard2.0/ref/System.Threading.Thread.dll",
|
||||
"build/netstandard2.0/ref/System.Threading.ThreadPool.dll",
|
||||
"build/netstandard2.0/ref/System.Threading.Timer.dll",
|
||||
"build/netstandard2.0/ref/System.Threading.dll",
|
||||
"build/netstandard2.0/ref/System.Transactions.dll",
|
||||
"build/netstandard2.0/ref/System.ValueTuple.dll",
|
||||
"build/netstandard2.0/ref/System.Web.dll",
|
||||
"build/netstandard2.0/ref/System.Windows.dll",
|
||||
"build/netstandard2.0/ref/System.Xml.Linq.dll",
|
||||
"build/netstandard2.0/ref/System.Xml.ReaderWriter.dll",
|
||||
"build/netstandard2.0/ref/System.Xml.Serialization.dll",
|
||||
"build/netstandard2.0/ref/System.Xml.XDocument.dll",
|
||||
"build/netstandard2.0/ref/System.Xml.XPath.XDocument.dll",
|
||||
"build/netstandard2.0/ref/System.Xml.XPath.dll",
|
||||
"build/netstandard2.0/ref/System.Xml.XmlDocument.dll",
|
||||
"build/netstandard2.0/ref/System.Xml.XmlSerializer.dll",
|
||||
"build/netstandard2.0/ref/System.Xml.dll",
|
||||
"build/netstandard2.0/ref/System.dll",
|
||||
"build/netstandard2.0/ref/mscorlib.dll",
|
||||
"build/netstandard2.0/ref/netstandard.dll",
|
||||
"build/netstandard2.0/ref/netstandard.xml",
|
||||
"lib/netstandard1.0/_._",
|
||||
"netstandard.library.2.0.3.nupkg.sha512",
|
||||
"netstandard.library.nuspec"
|
||||
]
|
||||
},
|
||||
"Newtonsoft.Json/13.0.4": {
|
||||
"sha512": "pdgNNMai3zv51W5aq268sujXUyx7SNdE2bj1wZcWjAQrKMFZV260lbqYop1d2GM67JI1huLRwxo9ZqnfF/lC6A==",
|
||||
"type": "package",
|
||||
"path": "newtonsoft.json/13.0.4",
|
||||
"files": [
|
||||
".nupkg.metadata",
|
||||
".signature.p7s",
|
||||
"LICENSE.md",
|
||||
"README.md",
|
||||
"lib/net20/Newtonsoft.Json.dll",
|
||||
"lib/net20/Newtonsoft.Json.xml",
|
||||
"lib/net35/Newtonsoft.Json.dll",
|
||||
"lib/net35/Newtonsoft.Json.xml",
|
||||
"lib/net40/Newtonsoft.Json.dll",
|
||||
"lib/net40/Newtonsoft.Json.xml",
|
||||
"lib/net45/Newtonsoft.Json.dll",
|
||||
"lib/net45/Newtonsoft.Json.xml",
|
||||
"lib/net6.0/Newtonsoft.Json.dll",
|
||||
"lib/net6.0/Newtonsoft.Json.xml",
|
||||
"lib/netstandard1.0/Newtonsoft.Json.dll",
|
||||
"lib/netstandard1.0/Newtonsoft.Json.xml",
|
||||
"lib/netstandard1.3/Newtonsoft.Json.dll",
|
||||
"lib/netstandard1.3/Newtonsoft.Json.xml",
|
||||
"lib/netstandard2.0/Newtonsoft.Json.dll",
|
||||
"lib/netstandard2.0/Newtonsoft.Json.xml",
|
||||
"newtonsoft.json.13.0.4.nupkg.sha512",
|
||||
"newtonsoft.json.nuspec",
|
||||
"packageIcon.png"
|
||||
]
|
||||
},
|
||||
"System.Numerics.Vectors/4.6.1": {
|
||||
"sha512": "sQxefTnhagrhoq2ReR0D/6K0zJcr9Hrd6kikeXsA1I8kOCboTavcUC4r7TSfpKFeE163uMuxZcyfO1mGO3EN8Q==",
|
||||
"type": "package",
|
||||
"path": "system.numerics.vectors/4.6.1",
|
||||
"files": [
|
||||
".nupkg.metadata",
|
||||
".signature.p7s",
|
||||
"Icon.png",
|
||||
"PACKAGE.md",
|
||||
"buildTransitive/net461/System.Numerics.Vectors.targets",
|
||||
"buildTransitive/net462/_._",
|
||||
"lib/net462/System.Numerics.Vectors.dll",
|
||||
"lib/net462/System.Numerics.Vectors.xml",
|
||||
"lib/netcoreapp2.0/_._",
|
||||
"lib/netstandard2.0/System.Numerics.Vectors.dll",
|
||||
"lib/netstandard2.0/System.Numerics.Vectors.xml",
|
||||
"lib/netstandard2.1/_._",
|
||||
"system.numerics.vectors.4.6.1.nupkg.sha512",
|
||||
"system.numerics.vectors.nuspec"
|
||||
]
|
||||
}
|
||||
},
|
||||
"projectFileDependencyGroups": {
|
||||
".NETStandard,Version=v2.0": [
|
||||
"NETStandard.Library >= 2.0.3",
|
||||
"Newtonsoft.Json >= 13.0.4",
|
||||
"System.Numerics.Vectors >= 4.6.1"
|
||||
]
|
||||
},
|
||||
"packageFolders": {
|
||||
"C:\\Users\\Fairyland\\.nuget\\packages\\": {},
|
||||
"D:\\VS2022\\Shared\\NuGetPackages": {}
|
||||
},
|
||||
"project": {
|
||||
"version": "1.0.0",
|
||||
"restore": {
|
||||
"projectUniqueName": "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\ClumsyPilot.csproj",
|
||||
"projectName": "ClumsyPilot",
|
||||
"projectPath": "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\ClumsyPilot.csproj",
|
||||
"packagesPath": "C:\\Users\\Fairyland\\.nuget\\packages\\",
|
||||
"outputPath": "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\obj\\",
|
||||
"projectStyle": "PackageReference",
|
||||
"fallbackFolders": [
|
||||
"D:\\VS2022\\Shared\\NuGetPackages"
|
||||
],
|
||||
"configFilePaths": [
|
||||
"C:\\Users\\Fairyland\\AppData\\Roaming\\NuGet\\NuGet.Config",
|
||||
"C:\\Program Files (x86)\\NuGet\\Config\\Microsoft.VisualStudio.FallbackLocation.config",
|
||||
"C:\\Program Files (x86)\\NuGet\\Config\\Microsoft.VisualStudio.Offline.config"
|
||||
],
|
||||
"originalTargetFrameworks": [
|
||||
"netstandard2.0"
|
||||
],
|
||||
"sources": {
|
||||
"C:\\Program Files (x86)\\Microsoft SDKs\\NuGetPackages\\": {},
|
||||
"https://api.nuget.org/v3/index.json": {}
|
||||
},
|
||||
"frameworks": {
|
||||
"netstandard2.0": {
|
||||
"targetAlias": "netstandard2.0",
|
||||
"projectReferences": {}
|
||||
}
|
||||
},
|
||||
"warningProperties": {
|
||||
"warnAsError": [
|
||||
"NU1605"
|
||||
]
|
||||
},
|
||||
"restoreAuditProperties": {
|
||||
"enableAudit": "true",
|
||||
"auditLevel": "low",
|
||||
"auditMode": "direct"
|
||||
},
|
||||
"SdkAnalysisLevel": "9.0.300"
|
||||
},
|
||||
"frameworks": {
|
||||
"netstandard2.0": {
|
||||
"targetAlias": "netstandard2.0",
|
||||
"dependencies": {
|
||||
"NETStandard.Library": {
|
||||
"suppressParent": "All",
|
||||
"target": "Package",
|
||||
"version": "[2.0.3, )",
|
||||
"autoReferenced": true
|
||||
},
|
||||
"Newtonsoft.Json": {
|
||||
"target": "Package",
|
||||
"version": "[13.0.4, )"
|
||||
},
|
||||
"System.Numerics.Vectors": {
|
||||
"target": "Package",
|
||||
"version": "[4.6.1, )"
|
||||
}
|
||||
},
|
||||
"imports": [
|
||||
"net461",
|
||||
"net462",
|
||||
"net47",
|
||||
"net471",
|
||||
"net472",
|
||||
"net48",
|
||||
"net481"
|
||||
],
|
||||
"assetTargetFallback": true,
|
||||
"warn": true,
|
||||
"runtimeIdentifierGraphPath": "C:\\Program Files\\dotnet\\sdk\\9.0.308\\RuntimeIdentifierGraph.json"
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,13 @@
|
||||
{
|
||||
"version": 2,
|
||||
"dgSpecHash": "gHGYwM2i+So=",
|
||||
"success": true,
|
||||
"projectFilePath": "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\ClumsyPilot.csproj",
|
||||
"expectedPackageFiles": [
|
||||
"C:\\Users\\Fairyland\\.nuget\\packages\\microsoft.netcore.platforms\\1.1.0\\microsoft.netcore.platforms.1.1.0.nupkg.sha512",
|
||||
"C:\\Users\\Fairyland\\.nuget\\packages\\netstandard.library\\2.0.3\\netstandard.library.2.0.3.nupkg.sha512",
|
||||
"C:\\Users\\Fairyland\\.nuget\\packages\\newtonsoft.json\\13.0.4\\newtonsoft.json.13.0.4.nupkg.sha512",
|
||||
"C:\\Users\\Fairyland\\.nuget\\packages\\system.numerics.vectors\\4.6.1\\system.numerics.vectors.4.6.1.nupkg.sha512"
|
||||
],
|
||||
"logs": []
|
||||
}
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Reference in New Issue
Block a user