初始化 shenyuxiang 分支
This commit is contained in:
+6
-519
@@ -1,534 +1,21 @@
|
||||
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();
|
||||
}
|
||||
|
||||
=> new ChassisController().Get();
|
||||
public override MultiWheelMagTracker GetMagController()
|
||||
{
|
||||
return new MultiWheelMagTracker();
|
||||
}
|
||||
|
||||
=> new MultiWheelMagTracker();
|
||||
public override NaiveMagnetController GetNaiveMagnetController()
|
||||
=> new NaiveMagnetController();
|
||||
|
||||
public void Sleep(float seconds)
|
||||
{
|
||||
return new NaiveMagnetController();
|
||||
new DriveTask(new Sleep { Second = seconds }.Get()).Wait();
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,9 +1,5 @@
|
||||
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;
|
||||
@@ -14,8 +10,7 @@ public class ChassisController : MovementDefinition<MultiWheelGeometricControlle
|
||||
{
|
||||
public float BaseSpeed = Configuration.conf.basicSpeed;
|
||||
|
||||
private DateTime _sendMotionDbgLast = DateTime.MinValue;
|
||||
|
||||
// 创建单车几何跟踪控制器（直接控本车底盘，不走多车 Auto 通道）
|
||||
public override MultiWheelGeometricController Get()
|
||||
{
|
||||
return new MultiWheelGeometricController
|
||||
@@ -45,56 +40,6 @@ public class ChassisController : MovementDefinition<MultiWheelGeometricControlle
|
||||
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
|
||||
};
|
||||
}
|
||||
};
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -2,14 +2,16 @@
|
||||
|
||||
<PropertyGroup>
|
||||
<TargetFramework>netstandard2.0</TargetFramework>
|
||||
<LangVersion>10</LangVersion>
|
||||
<AllowUnsafeBlocks>true</AllowUnsafeBlocks>
|
||||
<AppendTargetFrameworkToOutputPath>false</AppendTargetFrameworkToOutputPath>
|
||||
<OutputPath>..\..\build\Clumsy\</OutputPath>
|
||||
<LangVersion>10</LangVersion>
|
||||
<AllowUnsafeBlocks>true</AllowUnsafeBlocks>
|
||||
<AssemblyName>ClumsyPilot</AssemblyName>
|
||||
<RootNamespace>MultiWheelC</RootNamespace>
|
||||
<AppendTargetFrameworkToOutputPath>false</AppendTargetFrameworkToOutputPath>
|
||||
<OutputPath>build\Clumsy\</OutputPath>
|
||||
</PropertyGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<PackageReference Include="Newtonsoft.Json" Version="13.0.4" />
|
||||
<PackageReference Include="Newtonsoft.Json" Version="13.0.3" />
|
||||
<PackageReference Include="System.Numerics.Vectors" Version="4.6.1" />
|
||||
</ItemGroup>
|
||||
|
||||
@@ -33,9 +35,5 @@
|
||||
<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>
|
||||
|
||||
@@ -1,526 +0,0 @@
|
||||
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");
|
||||
}
|
||||
}
|
||||
@@ -1,411 +0,0 @@
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -1,606 +0,0 @@
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -1,136 +0,0 @@
|
||||
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))),
|
||||
};
|
||||
}
|
||||
}
|
||||
+77
-584
@@ -1,608 +1,101 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Numerics;
|
||||
using ClumsyCore;
|
||||
using ClumsyCore.DTools;
|
||||
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;
|
||||
using MDCSToolBox.Commons.Controllers;
|
||||
using System;
|
||||
using System.Numerics;
|
||||
|
||||
namespace MultiWheelC;
|
||||
|
||||
public class MultiForwardTest : MovementDefinition
|
||||
namespace MultiWheelC
|
||||
{
|
||||
public float Speed = 0.2f;
|
||||
public float DurationSeconds = 2f;
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
public abstract class DstTrackerTestBase : MovementTest
|
||||
{
|
||||
var chassis = (MultiWheelChassis)BasicPilotBase.Chassis;
|
||||
chassis.SetOriginBias(0, 0, 0);
|
||||
var end = DateTime.Now.AddSeconds(DurationSeconds);
|
||||
while (DateTime.Now < end)
|
||||
public bool UseInteractivePick = true;
|
||||
public float srcX;
|
||||
public float srcY;
|
||||
public float dstX;
|
||||
public float dstY;
|
||||
public float carDirectionBias;
|
||||
|
||||
private readonly Painter _painter = UI.GetPainter("DstTrackerTest");
|
||||
private DriveTask _dt;
|
||||
|
||||
protected DstTrackerTestBase(float defaultCarDirectionBias)
|
||||
{
|
||||
chassis.SendMotion(Speed, 0, 0);
|
||||
yield return true;
|
||||
carDirectionBias = defaultCarDirectionBias;
|
||||
}
|
||||
|
||||
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)
|
||||
public override void TestStop()
|
||||
{
|
||||
chassis.SendRotateMotion(0);
|
||||
if (WheelsAligned(chassis, WheelAlignDeg)) break;
|
||||
yield return true;
|
||||
_dt?.Stop();
|
||||
_painter?.Clear();
|
||||
}
|
||||
|
||||
// 阶段二：旋转到目标世界朝向，到位即停。
|
||||
var target = CommonMath.RoundTh(TargetWorldDeg);
|
||||
while (true)
|
||||
public override void Test()
|
||||
{
|
||||
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))
|
||||
Vector2 p1;
|
||||
Vector2 p2;
|
||||
if (UseInteractivePick)
|
||||
{
|
||||
centerLastX = centerStartX;
|
||||
centerLastY = centerStartY;
|
||||
centerLastTh = centerStartTh;
|
||||
centerMaxDrift = 0;
|
||||
centerTracking = true;
|
||||
p1 = UI.GetPoint("point1");
|
||||
p2 = UI.GetPoint("point2");
|
||||
}
|
||||
}
|
||||
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)
|
||||
else
|
||||
{
|
||||
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;
|
||||
p1 = new Vector2(srcX, srcY);
|
||||
p2 = new Vector2(dstX, dstY);
|
||||
}
|
||||
|
||||
remaining = targetMag - Math.Abs(accumulated);
|
||||
if (remaining <= ArriveDeg) { stopReason = "arrived"; break; }
|
||||
_painter.Clear();
|
||||
_dt = new DriveTask(new DstTracker
|
||||
{
|
||||
Src = p1,
|
||||
Dst = p2,
|
||||
CarDirectionBias = carDirectionBias,
|
||||
}.Get());
|
||||
_dt.Wait();
|
||||
}
|
||||
}
|
||||
|
||||
// 减速区：剩余角度 < SlowDeg 时，目标角速度按剩余比例线性降到 MinOmega，
|
||||
// 使切断指令瞬间残余动量足够小，抑制惯性滑行造成的超调。宽度直观、便于现场调试。
|
||||
desiredMag = remaining < slowDeg
|
||||
? Math.Max(minOmega, maxOmega * (remaining / slowDeg))
|
||||
: maxOmega;
|
||||
[MovementTest(name = "测试终点跟踪动作-前进")]
|
||||
public sealed class DstTrackerForward : DstTrackerTestBase
|
||||
{
|
||||
public DstTrackerForward() : base(0f) { }
|
||||
}
|
||||
|
||||
if (centerTracking &&
|
||||
self.TryGetFleetCenterFromPose((float)carPos.x, (float)carPos.y, (float)carPos.th,
|
||||
out centerLastX, out centerLastY, out centerLastTh))
|
||||
[MovementTest(name = "测试终点跟踪动作-后退")]
|
||||
public sealed class DstTrackerBackward : DstTrackerTestBase
|
||||
{
|
||||
public DstTrackerBackward() : base(180f) { }
|
||||
}
|
||||
|
||||
[MovementTest(name = "底盘旋转测试")]
|
||||
public class RotateToAngleTest : MovementTest
|
||||
{
|
||||
// 底盘旋转测试不支持停止操作。
|
||||
public override void TestStop()
|
||||
{
|
||||
throw new NotImplementedException();
|
||||
}
|
||||
|
||||
// 交互输入目标角度后执行底盘原地旋转测试。
|
||||
public override void Test()
|
||||
{
|
||||
var target = UI.GetInput("输入旋转角度：");
|
||||
new DriveTask(new MultiWheelRotateInPlace()
|
||||
{
|
||||
AngleTarget = float.Parse(target),
|
||||
PidparamsRead = () => new PIDParams()
|
||||
{
|
||||
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");
|
||||
}
|
||||
Kp = PilotDefinition.Conf.InPlaceRotateKp,
|
||||
Ki = PilotDefinition.Conf.InPlaceRotateKi,
|
||||
Kd = PilotDefinition.Conf.InPlaceRotateKd,
|
||||
DeadZone = PilotDefinition.Conf.InPlaceRotateArriveDeg,
|
||||
SpeedAccPerSec = PilotDefinition.Conf.InPlaceRotateAcc,
|
||||
OutputUpperThreshold = PilotDefinition.Conf.InPlaceRotateMaxSpeed,
|
||||
MaxI = PilotDefinition.Conf.InPlaceRotateMaxI,
|
||||
}
|
||||
}
|
||||
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;
|
||||
}.Get()).Wait();
|
||||
}
|
||||
|
||||
// 到位：角速度先归零，保持脚本使能让 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()
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
+75
-274
@@ -1,34 +1,77 @@
|
||||
using ClumsyCore;
|
||||
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 DstTracker : MovementDefinition
|
||||
{
|
||||
public Vector2 Src;
|
||||
public Vector2 Dst;
|
||||
public float CarDirectionBias = 0f;
|
||||
public Painter Painter = UI.GetPainter("DstTracker");
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
var chassis = (MultiWheelChassis)PilotDefinition.Chassis;
|
||||
DriveTask task = null;
|
||||
try
|
||||
{
|
||||
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();
|
||||
var linePath = new LineTrack(Src, Dst)
|
||||
{
|
||||
CarDirectionBias = CarDirectionBias,
|
||||
Speed = PilotDefinition.Conf.DstTrackerMaxSpeed
|
||||
};
|
||||
tracker.AddTrack(linePath);
|
||||
task = new DriveTask(tracker.Track());
|
||||
task.Wait();
|
||||
yield return false;
|
||||
}
|
||||
finally
|
||||
{
|
||||
task?.Stop();
|
||||
chassis.SendXYThSpeed(0f, 0f, 0f);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
public class Sleep : MovementDefinition
|
||||
{
|
||||
public float Second = 2f;
|
||||
|
||||
public override IEnumerable<bool> Get()
|
||||
{
|
||||
if (Second <= 0)
|
||||
{
|
||||
yield return false;
|
||||
yield break;
|
||||
}
|
||||
|
||||
var endTime = DateTime.UtcNow.AddSeconds(Second);
|
||||
while (DateTime.UtcNow < endTime)
|
||||
{
|
||||
Thread.Sleep(50);
|
||||
yield return true;
|
||||
}
|
||||
|
||||
yield return false;
|
||||
}
|
||||
}
|
||||
|
||||
public class MultiWheelRotateInPlace : MovementDefinition
|
||||
{
|
||||
/// <summary>
|
||||
@@ -46,281 +89,39 @@ namespace MultiWheelC
|
||||
|
||||
public PIDController thPid;
|
||||
|
||||
// 将角度归一化到零到三百六十度范围内。
|
||||
private static float RangeAngle(float theta)
|
||||
{
|
||||
return (float)(theta - Math.Round(theta / 360.0f) * 360);
|
||||
}
|
||||
|
||||
// 使用 PID 控制原地旋转到目标角度。
|
||||
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)
|
||||
try
|
||||
{
|
||||
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}");
|
||||
}
|
||||
}
|
||||
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);
|
||||
|
||||
|
||||
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))
|
||||
while (true)
|
||||
{
|
||||
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 无缝接管
|
||||
var s = thPid.GetResponse(targetAngle, true);
|
||||
Console.WriteLine($"s:{s} AngleTarget:{AngleTarget}");
|
||||
Chassis.SendXYThSpeed(0, 0, s);
|
||||
if (thPid.IsArrived()) break;
|
||||
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;
|
||||
Console.WriteLine($"final rotate to {targetAngle}");
|
||||
}
|
||||
if (SrcId != -1 && LeaveSrcFunction != null)
|
||||
finally
|
||||
{
|
||||
LeaveSrcFunction(SrcId);
|
||||
DLog.Log($"释放放车点{SrcId}", "TireFollowing");
|
||||
Chassis.SendXYThSpeed(0, 0, 0);
|
||||
}
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
+223
-204
@@ -6,108 +6,19 @@ 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;
|
||||
#region 单车-轨迹跟踪
|
||||
|
||||
[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 = "直线行走距离")] 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 = "多车联动：本车回连端点 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 = "终点跟踪:速度")] public float DstTrackerMaxSpeed = 0.3f;
|
||||
#endregion
|
||||
|
||||
#region 单车-原地旋转
|
||||
[FieldMember(desc = "原地旋转：目标朝向(世界坐标系, deg)")]
|
||||
public float InPlaceRotateTargetWorldDeg = 90f;
|
||||
|
||||
@@ -123,102 +34,35 @@ public class PilotConfig : MultiWheelPilotConfig
|
||||
[FieldMember(desc = "原地旋转：旋转过程中舵轮偏差重对齐阈值(deg)")]
|
||||
public float InPlaceRotateActiveWheelAlignDeg = 10f;
|
||||
|
||||
// ===== 车队联动-原地旋转动作（FleetRotateInPlace / 对应 FleetRemote 原地旋转模式）=====
|
||||
// 通过 Clumsy 内部脚本字段驱动 TickMultiVehicle 的 mode2 旋转（绕车队中心 + PI 纠偏），需主车运行。
|
||||
[FieldMember(desc = "车队原地旋转：角速度大小(deg/s，方向由目标角符号决定)")]
|
||||
public float FleetRotateOmega = 15f;
|
||||
#endregion
|
||||
|
||||
[FieldMember(desc = "车队原地旋转：目标相对转角(deg，+逆时针)")]
|
||||
public float FleetRotateTargetDeltaDeg = 90f;
|
||||
#region 单车-临时
|
||||
[FieldMember(desc = "原地旋转Kp")]
|
||||
public float InPlaceRotateKp = 0.05f;
|
||||
|
||||
[FieldMember(desc = "车队原地旋转：到位角度精度(deg)")]
|
||||
public float FleetRotateArriveDeg = 1.5f;
|
||||
[FieldMember(desc = "原地旋转Ki")]
|
||||
public float InPlaceRotateKi = 0.01f;
|
||||
|
||||
[FieldMember(desc = "车队原地旋转：减速区宽度(deg)，抑制收尾惯性超调")]
|
||||
public float FleetRotateSlowDeg = 25f;
|
||||
[FieldMember(desc = "原地旋转Kd")]
|
||||
public float InPlaceRotateKd = 0f;
|
||||
|
||||
[FieldMember(desc = "车队原地旋转：减速区末段最小角速度(deg/s)")]
|
||||
public float FleetRotateMinOmega = 3f;
|
||||
[FieldMember(desc = "原地旋转积分限幅")]
|
||||
public float InPlaceRotateMaxI = 0.01f;
|
||||
|
||||
[FieldMember(desc = "车队原地旋转：起步缓启动角加速度(deg/s²，<=0关闭)")]
|
||||
public float FleetRotateAccel = 20f;
|
||||
[FieldMember(desc = "原地旋转最大角速度(deg/s)")]
|
||||
public float InPlaceRotateMaxSpeed = 30f;
|
||||
|
||||
[FieldMember(desc = "车队原地旋转：到位后安定时长(s)")]
|
||||
public float FleetRotateSettleSec = 0.5f;
|
||||
[FieldMember(desc = "原地旋转角加速度(deg/s²)")]
|
||||
public float InPlaceRotateAcc = 30f;
|
||||
|
||||
// 与 MultiVehicleSyncUseDetour 解耦：转到指定角度需航向反馈，默认 true 读主车 SLAM 航向闭环判停。
|
||||
// false 时退化为按估算时长开环停止（实际转速≠指令时不精确，易出现"没转到目标就停"）。
|
||||
[FieldMember(desc = "车队原地旋转：用Detour主车航向闭环判停(默认true，false=按时长开环)")]
|
||||
public bool FleetRotateUseDetourHeading = true;
|
||||
[FieldMember(desc = "原地旋转超时(s)")]
|
||||
public float InPlaceRotateTimeoutSec = 15f;
|
||||
#endregion
|
||||
|
||||
// ===== 车队联动-自动蟹行（FleetCrabWalk）=====
|
||||
// 以当前车队中心为起点，构造一条直线路径；MovementTest 中车身保持启动朝向追踪该路径。
|
||||
// 动作侧参考几何控制器的路径跟踪思路，直接写入 MultiVehicleAuto... 字段，不再复用脚本手动链路。
|
||||
[FieldMember(desc = "车队蟹行：路径方向相对启动时车队朝向夹角(deg，逆时针为正；路径在车右侧x度时填-x)")]
|
||||
public float FleetCrabAngleDeg = 45f;
|
||||
|
||||
[FieldMember(desc = "车队蟹行：AGV入口使用的车队世界系目标朝向(deg)")]
|
||||
public float FleetCrabBodyWorldHeadingDeg = 0f;
|
||||
#if false
|
||||
|
||||
[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腿检测（单线雷达识别两腿托盘 / 轮胎）=====
|
||||
#region 单车-钻车与夹抱
|
||||
[FieldMember(desc = "2腿检测：雷达名（逗号分隔可多个）")]
|
||||
public string TwoLegLidarName = "rear_left_lidar_1,rear_right_lidar_1";
|
||||
|
||||
@@ -264,7 +108,6 @@ public class PilotConfig : MultiWheelPilotConfig
|
||||
[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;
|
||||
@@ -284,22 +127,6 @@ public class PilotConfig : MultiWheelPilotConfig
|
||||
[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;
|
||||
@@ -320,9 +147,7 @@ public class PilotConfig : MultiWheelPilotConfig
|
||||
[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;
|
||||
@@ -331,4 +156,198 @@ public class PilotConfig : MultiWheelPilotConfig
|
||||
[FieldMember(desc = "轮胎跟踪:角度调整Thresh")] public float TireFollowingThThresh = 0.1f;
|
||||
[FieldMember(desc = "轮胎跟踪:角度调整DeadZone")] public float TireFollowingThDeadZone = 5f;
|
||||
[FieldMember(desc = "轮胎跟踪:角度调整MaxI")] public float TireFollowingThMaxI = 0.01f;
|
||||
|
||||
[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;
|
||||
|
||||
|
||||
#endregion
|
||||
|
||||
#region 多车-编队与遥控
|
||||
[FieldMember(desc = "联动时转向角爬升加速度")] public float SyncThAccPerSec = 30f;
|
||||
[FieldMember(desc = "两车间距 (mm)")] public float TestCarSyncDistance = 2400f;
|
||||
[FieldMember(desc = "编队排布偏角")] public float TestCarSyncTh = 0f;
|
||||
// Fleet manual remote IO values are normalized joystick ratios. Keep all speed/angle scaling here.
|
||||
[FieldMember(desc = "车队遥控最大线速度")] public float FleetManualMaxSpeed = 0.3f;
|
||||
[FieldMember(desc = "常规模式满杆舵角")] public float FleetManualMaxSteerAngleDeg = 45f;
|
||||
[FieldMember(desc = "蟹行满杆舵角")] public float FleetManualMaxCrabAngleDeg = 60f;
|
||||
[FieldMember(desc = "旋转满杆角速度")] public float FleetManualMaxRotateOmegaDegPerSec = 45f;
|
||||
[FieldMember(desc = "蟹行舵角上限（对齐 ±120）")] public float MultiVehicleCrabSteerLimitDeg = 120f;
|
||||
[FieldMember(desc = "互识别检测中心偏移")] public float DeltaDetectCenter = 350f;
|
||||
#endregion
|
||||
|
||||
#region 多车-通信
|
||||
[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 = "";
|
||||
[FieldMember(desc = "多车联动：自动速度命令超时(ms，0=auto)")] public int MultiVehicleAutoCmdTimeoutMs = 0;
|
||||
[FieldMember(desc = "多车联动：成员存活TTL(ms，0=auto)")] public int MultiVehicleMemberTtlMs = 0;
|
||||
|
||||
[JsonProperty("MultiVehicleMasterIp")]
|
||||
private string LegacyMasterIpSetter
|
||||
{
|
||||
set
|
||||
{
|
||||
if (string.IsNullOrEmpty(value) || value == "/") return;
|
||||
if (MultiVehicleMasterEndpoint == "/")
|
||||
MultiVehicleMasterEndpoint = value.Contains(":") ? value : $"{value}:8008";
|
||||
}
|
||||
}
|
||||
|
||||
#endregion
|
||||
|
||||
#region 多车-队形补偿
|
||||
[FieldMember(desc = "定位是否参与车队内姿态纠正(不影响整队姿态计算)")] public bool MultiVehicleSyncUseDetour = false;
|
||||
[FieldMember(desc = "手动联动是否启用定位姿态纠正(默认关闭)")] public bool MultiVehicleManualUseDetourCorrection = false;
|
||||
[FieldMember(desc = "多车联动：启用互识别纠正")] public bool MultiVehicleUseDetect = false;
|
||||
[FieldMember(desc = "多车联动：自动模式按理想中心前馈(弧线)")] public bool MultiVehicleAutoUseIdealCenter = true;
|
||||
[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;
|
||||
#endregion
|
||||
|
||||
#region 多车-旋转补偿
|
||||
[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;
|
||||
[FieldMember(desc = "原地旋转纠偏：生效的最小角速度阈值(deg/s)")] public float MultiVehicleRotateActiveOmega = 0.5f;
|
||||
[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;
|
||||
#endregion
|
||||
|
||||
#region 多车-联动动作
|
||||
[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;
|
||||
|
||||
[FieldMember(desc = "车队原地旋转：用Detour主车航向闭环判停(默认true，false=按时长开环)")]
|
||||
public bool FleetRotateUseDetourHeading = true;
|
||||
|
||||
[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;
|
||||
|
||||
[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;
|
||||
#endregion
|
||||
|
||||
#endif
|
||||
|
||||
#region 仿真-Playground
|
||||
[FieldMember(desc = "Playground WebAPI 基地址")]
|
||||
public string PlaygroundWebApiUrl = "http://localhost:18090";
|
||||
|
||||
[FieldMember(desc = "Playground 小车名称（场景 robots[].name）")]
|
||||
public string PlaygroundRobotName = "agv_multi_1";
|
||||
|
||||
[FieldMember(desc = "Playground 邻车名称（仅主车用于原地旋转位姿诊断）")]
|
||||
public string PlaygroundNeighborRobotName = "agv_multi_2";
|
||||
|
||||
[FieldMember(desc = "旋转位姿诊断开关(simulation only)")]
|
||||
public bool MultiVehicleRotatePoseWebApiDiagEnabled = false;
|
||||
[FieldMember(desc = "WebAPI 平移测试：平移距离(mm)")]
|
||||
public float WebApiTranslateMm = 100f;
|
||||
|
||||
[FieldMember(desc = "WebAPI 旋转测试：旋转角度(deg)")]
|
||||
public float WebApiRotateDeg = 5f;
|
||||
#endregion
|
||||
|
||||
|
||||
}
|
||||
|
||||
+1
-2115
File diff suppressed because it is too large
Load Diff
@@ -1,81 +0,0 @@
|
||||
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();
|
||||
}
|
||||
}
|
||||
@@ -1,511 +0,0 @@
|
||||
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;
|
||||
}
|
||||
}
|
||||
@@ -1,277 +0,0 @@
|
||||
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.");
|
||||
}
|
||||
}
|
||||
@@ -1,75 +0,0 @@
|
||||
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,163 @@
|
||||
{
|
||||
"runtimeTarget": {
|
||||
"name": ".NETStandard,Version=v2.0/",
|
||||
"signature": ""
|
||||
},
|
||||
"compilationOptions": {},
|
||||
"targets": {
|
||||
".NETStandard,Version=v2.0": {},
|
||||
".NETStandard,Version=v2.0/": {
|
||||
"ClumsyPilot/1.0.0": {
|
||||
"dependencies": {
|
||||
"NETStandard.Library": "2.0.3",
|
||||
"Newtonsoft.Json": "13.0.3",
|
||||
"System.Numerics.Vectors": "4.6.1",
|
||||
"CommonUsage": "1.0.0.0",
|
||||
"LessokajiWeaverUtilities": "1.0.0.0",
|
||||
"MDCSToolBox": "1.0.0.0",
|
||||
"RefClumsyCore": "0.0.0.0",
|
||||
"RefClumsyDance": "0.0.0.0",
|
||||
"RefFundamentalLib": "0.0.0.0"
|
||||
},
|
||||
"runtime": {
|
||||
"ClumsyPilot.dll": {}
|
||||
}
|
||||
},
|
||||
"Microsoft.NETCore.Platforms/1.1.0": {},
|
||||
"NETStandard.Library/2.0.3": {
|
||||
"dependencies": {
|
||||
"Microsoft.NETCore.Platforms": "1.1.0"
|
||||
}
|
||||
},
|
||||
"Newtonsoft.Json/13.0.3": {
|
||||
"runtime": {
|
||||
"lib/netstandard2.0/Newtonsoft.Json.dll": {
|
||||
"assemblyVersion": "13.0.0.0",
|
||||
"fileVersion": "13.0.3.27908"
|
||||
}
|
||||
}
|
||||
},
|
||||
"System.Numerics.Vectors/4.6.1": {
|
||||
"runtime": {
|
||||
"lib/netstandard2.0/System.Numerics.Vectors.dll": {
|
||||
"assemblyVersion": "4.1.3.0",
|
||||
"fileVersion": "4.600.125.16908"
|
||||
}
|
||||
}
|
||||
},
|
||||
"CommonUsage/1.0.0.0": {
|
||||
"runtime": {
|
||||
"CommonUsage.dll": {
|
||||
"assemblyVersion": "1.0.0.0",
|
||||
"fileVersion": "1.0.0.0"
|
||||
}
|
||||
}
|
||||
},
|
||||
"LessokajiWeaverUtilities/1.0.0.0": {
|
||||
"runtime": {
|
||||
"LessokajiWeaverUtilities.dll": {
|
||||
"assemblyVersion": "1.0.0.0",
|
||||
"fileVersion": "1.0.0.0"
|
||||
}
|
||||
}
|
||||
},
|
||||
"MDCSToolBox/1.0.0.0": {
|
||||
"runtime": {
|
||||
"MDCSToolBox.dll": {
|
||||
"assemblyVersion": "1.0.0.0",
|
||||
"fileVersion": "1.0.0.0"
|
||||
}
|
||||
}
|
||||
},
|
||||
"RefClumsyCore/0.0.0.0": {
|
||||
"runtime": {
|
||||
"RefClumsyCore.dll": {
|
||||
"assemblyVersion": "0.0.0.0",
|
||||
"fileVersion": "0.0.0.0"
|
||||
}
|
||||
}
|
||||
},
|
||||
"RefClumsyDance/0.0.0.0": {
|
||||
"runtime": {
|
||||
"RefClumsyDance.dll": {
|
||||
"assemblyVersion": "0.0.0.0",
|
||||
"fileVersion": "0.0.0.0"
|
||||
}
|
||||
}
|
||||
},
|
||||
"RefFundamentalLib/0.0.0.0": {
|
||||
"runtime": {
|
||||
"RefFundamentalLib.dll": {
|
||||
"assemblyVersion": "0.0.0.0",
|
||||
"fileVersion": "0.0.0.0"
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
},
|
||||
"libraries": {
|
||||
"ClumsyPilot/1.0.0": {
|
||||
"type": "project",
|
||||
"serviceable": false,
|
||||
"sha512": ""
|
||||
},
|
||||
"Microsoft.NETCore.Platforms/1.1.0": {
|
||||
"type": "package",
|
||||
"serviceable": true,
|
||||
"sha512": "sha512-kz0PEW2lhqygehI/d6XsPCQzD7ff7gUJaVGPVETX611eadGsA3A877GdSlU0LRVMCTH/+P3o2iDTak+S08V2+A==",
|
||||
"path": "microsoft.netcore.platforms/1.1.0",
|
||||
"hashPath": "microsoft.netcore.platforms.1.1.0.nupkg.sha512"
|
||||
},
|
||||
"NETStandard.Library/2.0.3": {
|
||||
"type": "package",
|
||||
"serviceable": true,
|
||||
"sha512": "sha512-st47PosZSHrjECdjeIzZQbzivYBJFv6P2nv4cj2ypdI204DO+vZ7l5raGMiX4eXMJ53RfOIg+/s4DHVZ54Nu2A==",
|
||||
"path": "netstandard.library/2.0.3",
|
||||
"hashPath": "netstandard.library.2.0.3.nupkg.sha512"
|
||||
},
|
||||
"Newtonsoft.Json/13.0.3": {
|
||||
"type": "package",
|
||||
"serviceable": true,
|
||||
"sha512": "sha512-HrC5BXdl00IP9zeV+0Z848QWPAoCr9P3bDEZguI+gkLcBKAOxix/tLEAAHC+UvDNPv4a2d18lOReHMOagPa+zQ==",
|
||||
"path": "newtonsoft.json/13.0.3",
|
||||
"hashPath": "newtonsoft.json.13.0.3.nupkg.sha512"
|
||||
},
|
||||
"System.Numerics.Vectors/4.6.1": {
|
||||
"type": "package",
|
||||
"serviceable": true,
|
||||
"sha512": "sha512-sQxefTnhagrhoq2ReR0D/6K0zJcr9Hrd6kikeXsA1I8kOCboTavcUC4r7TSfpKFeE163uMuxZcyfO1mGO3EN8Q==",
|
||||
"path": "system.numerics.vectors/4.6.1",
|
||||
"hashPath": "system.numerics.vectors.4.6.1.nupkg.sha512"
|
||||
},
|
||||
"CommonUsage/1.0.0.0": {
|
||||
"type": "reference",
|
||||
"serviceable": false,
|
||||
"sha512": ""
|
||||
},
|
||||
"LessokajiWeaverUtilities/1.0.0.0": {
|
||||
"type": "reference",
|
||||
"serviceable": false,
|
||||
"sha512": ""
|
||||
},
|
||||
"MDCSToolBox/1.0.0.0": {
|
||||
"type": "reference",
|
||||
"serviceable": false,
|
||||
"sha512": ""
|
||||
},
|
||||
"RefClumsyCore/0.0.0.0": {
|
||||
"type": "reference",
|
||||
"serviceable": false,
|
||||
"sha512": ""
|
||||
},
|
||||
"RefClumsyDance/0.0.0.0": {
|
||||
"type": "reference",
|
||||
"serviceable": false,
|
||||
"sha512": ""
|
||||
},
|
||||
"RefFundamentalLib/0.0.0.0": {
|
||||
"type": "reference",
|
||||
"serviceable": false,
|
||||
"sha512": ""
|
||||
}
|
||||
}
|
||||
}
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -1,23 +1,23 @@
|
||||
{
|
||||
"format": 1,
|
||||
"restore": {
|
||||
"D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\ClumsyPilot.csproj": {}
|
||||
"D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\ClumsyPilot.csproj": {}
|
||||
},
|
||||
"projects": {
|
||||
"D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\ClumsyPilot.csproj": {
|
||||
"D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\ClumsyPilot.csproj": {
|
||||
"version": "1.0.0",
|
||||
"restore": {
|
||||
"projectUniqueName": "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\ClumsyPilot.csproj",
|
||||
"projectUniqueName": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\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\\",
|
||||
"projectPath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\ClumsyPilot.csproj",
|
||||
"packagesPath": "C:\\Users\\admin\\.nuget\\packages\\",
|
||||
"outputPath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\obj\\",
|
||||
"projectStyle": "PackageReference",
|
||||
"fallbackFolders": [
|
||||
"D:\\VS2022\\Shared\\NuGetPackages"
|
||||
"C:\\Program Files (x86)\\Microsoft Visual Studio\\Shared\\NuGetPackages"
|
||||
],
|
||||
"configFilePaths": [
|
||||
"C:\\Users\\Fairyland\\AppData\\Roaming\\NuGet\\NuGet.Config",
|
||||
"C:\\Users\\admin\\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"
|
||||
],
|
||||
@@ -58,7 +58,7 @@
|
||||
},
|
||||
"Newtonsoft.Json": {
|
||||
"target": "Package",
|
||||
"version": "[13.0.4, )"
|
||||
"version": "[13.0.3, )"
|
||||
},
|
||||
"System.Numerics.Vectors": {
|
||||
"target": "Package",
|
||||
@@ -76,7 +76,7 @@
|
||||
],
|
||||
"assetTargetFallback": true,
|
||||
"warn": true,
|
||||
"runtimeIdentifierGraphPath": "C:\\Program Files\\dotnet\\sdk\\9.0.308\\RuntimeIdentifierGraph.json"
|
||||
"runtimeIdentifierGraphPath": "C:\\Program Files\\dotnet\\sdk\\9.0.316\\RuntimeIdentifierGraph.json"
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -5,12 +5,12 @@
|
||||
<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>
|
||||
<NuGetPackageFolders Condition=" '$(NuGetPackageFolders)' == '' ">C:\Users\admin\.nuget\packages\;C:\Program Files (x86)\Microsoft Visual Studio\Shared\NuGetPackages</NuGetPackageFolders>
|
||||
<NuGetProjectStyle Condition=" '$(NuGetProjectStyle)' == '' ">PackageReference</NuGetProjectStyle>
|
||||
<NuGetToolVersion Condition=" '$(NuGetToolVersion)' == '' ">6.14.2</NuGetToolVersion>
|
||||
<NuGetToolVersion Condition=" '$(NuGetToolVersion)' == '' ">6.14.3</NuGetToolVersion>
|
||||
</PropertyGroup>
|
||||
<ItemGroup Condition=" '$(ExcludeRestorePackageImports)' != 'true' ">
|
||||
<SourceRoot Include="C:\Users\Fairyland\.nuget\packages\" />
|
||||
<SourceRoot Include="D:\VS2022\Shared\NuGetPackages\" />
|
||||
<SourceRoot Include="C:\Users\admin\.nuget\packages\" />
|
||||
<SourceRoot Include="C:\Program Files (x86)\Microsoft Visual Studio\Shared\NuGetPackages\" />
|
||||
</ItemGroup>
|
||||
</Project>
|
||||
@@ -1,10 +1,9 @@
|
||||
//------------------------------------------------------------------------------
|
||||
// <auto-generated>
|
||||
// 此代码由工具生成。
|
||||
// 运行时版本:4.0.30319.42000
|
||||
// This code was generated by a tool.
|
||||
//
|
||||
// 对此文件的更改可能会导致不正确的行为，并且如果
|
||||
// 重新生成代码，这些更改将会丢失。
|
||||
// Changes to this file may cause incorrect behavior and will be lost if
|
||||
// the code is regenerated.
|
||||
// </auto-generated>
|
||||
//------------------------------------------------------------------------------
|
||||
|
||||
|
||||
@@ -1,6 +1,6 @@
|
||||
is_global = true
|
||||
build_property.RootNamespace = ClumsyPilot
|
||||
build_property.ProjectDir = D:\@FariyLandTask\@FRLD-GitProject\ParkingRobot\ClumsyPilot\
|
||||
build_property.RootNamespace = MultiWheelC
|
||||
build_property.ProjectDir = d:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\
|
||||
build_property.EnableComHosting =
|
||||
build_property.EnableGeneratedComInterfaceComImportInterop =
|
||||
build_property.CsWinRTUseWindowsUIXamlProjections = false
|
||||
|
||||
Binary file not shown.
Binary file not shown.
@@ -1 +1 @@
|
||||
622a214d5f25f6580d0c98fb28e188cba923d74f280b70d75d03c4009c18c55b
|
||||
cbba0ac806b0617cc6f7209f71f12565d8a56471fa82abdd118fca6c967d6a5a
|
||||
|
||||
@@ -1,11 +1,34 @@
|
||||
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
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\ClumsyPilot.deps.json
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\ClumsyPilot.dll
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\ClumsyPilot.pdb
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\CommonUsage.dll
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\LessokajiWeaverUtilities.dll
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\MDCSToolBox.dll
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\RefClumsyCore.dll
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\RefClumsyDance.dll
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\build\Clumsy\RefFundamentalLib.dll
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\obj\Debug\ClumsyPilot.csproj.AssemblyReference.cache
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\obj\Debug\ClumsyPilot.GeneratedMSBuildEditorConfig.editorconfig
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\obj\Debug\ClumsyPilot.AssemblyInfoInputs.cache
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\obj\Debug\ClumsyPilot.AssemblyInfo.cs
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\obj\Debug\ClumsyPilot.csproj.CoreCompileInputs.cache
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\obj\Debug\ClumsyPi.5EF10E9F.Up2Date
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\obj\Debug\ClumsyPilot.dll
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\MultiWheelC\obj\Debug\ClumsyPilot.pdb
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\build\Clumsy\ClumsyPilot.deps.json
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\build\Clumsy\ClumsyPilot.dll
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\build\Clumsy\ClumsyPilot.pdb
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\build\Clumsy\CommonUsage.dll
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\build\Clumsy\LessokajiWeaverUtilities.dll
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\build\Clumsy\MDCSToolBox.dll
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\build\Clumsy\RefClumsyCore.dll
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\build\Clumsy\RefClumsyDance.dll
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\build\Clumsy\RefFundamentalLib.dll
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\obj\Debug\ClumsyPilot.csproj.AssemblyReference.cache
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\obj\Debug\ClumsyPilot.GeneratedMSBuildEditorConfig.editorconfig
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\obj\Debug\ClumsyPilot.AssemblyInfoInputs.cache
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\obj\Debug\ClumsyPilot.AssemblyInfo.cs
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\obj\Debug\ClumsyPilot.csproj.CoreCompileInputs.cache
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\obj\Debug\ClumsyPi.5EF10E9F.Up2Date
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\obj\Debug\ClumsyPilot.dll
|
||||
D:\Users\Desktop\入职培训\停车机器人\MyParking\ClumsyPilot\obj\Debug\ClumsyPilot.pdb
|
||||
|
||||
Binary file not shown.
Binary file not shown.
@@ -1,4 +0,0 @@
|
||||
// <autogenerated />
|
||||
using System;
|
||||
using System.Reflection;
|
||||
[assembly: global::System.Runtime.Versioning.TargetFrameworkAttribute(".NETStandard,Version=v2.0", FrameworkDisplayName = ".NET Standard 2.0")]
|
||||
@@ -1,23 +0,0 @@
|
||||
//------------------------------------------------------------------------------
|
||||
// <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 类生成。
|
||||
|
||||
@@ -1 +0,0 @@
|
||||
5d77794fa0720c6591db5b06ac60427413c18989a6f7b64420ccb07d122d85bc
|
||||
-8
@@ -1,8 +0,0 @@
|
||||
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.
@@ -1 +0,0 @@
|
||||
66346c5ecc38869ab6b6f447e1f54c24aba69f81ee10b5736dee140cd5a474b8
|
||||
@@ -1,5 +0,0 @@
|
||||
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
|
||||
@@ -26,7 +26,7 @@
|
||||
"build/netstandard2.0/NETStandard.Library.targets": {}
|
||||
}
|
||||
},
|
||||
"Newtonsoft.Json/13.0.4": {
|
||||
"Newtonsoft.Json/13.0.3": {
|
||||
"type": "package",
|
||||
"compile": {
|
||||
"lib/netstandard2.0/Newtonsoft.Json.dll": {
|
||||
@@ -199,10 +199,10 @@
|
||||
"netstandard.library.nuspec"
|
||||
]
|
||||
},
|
||||
"Newtonsoft.Json/13.0.4": {
|
||||
"sha512": "pdgNNMai3zv51W5aq268sujXUyx7SNdE2bj1wZcWjAQrKMFZV260lbqYop1d2GM67JI1huLRwxo9ZqnfF/lC6A==",
|
||||
"Newtonsoft.Json/13.0.3": {
|
||||
"sha512": "HrC5BXdl00IP9zeV+0Z848QWPAoCr9P3bDEZguI+gkLcBKAOxix/tLEAAHC+UvDNPv4a2d18lOReHMOagPa+zQ==",
|
||||
"type": "package",
|
||||
"path": "newtonsoft.json/13.0.4",
|
||||
"path": "newtonsoft.json/13.0.3",
|
||||
"files": [
|
||||
".nupkg.metadata",
|
||||
".signature.p7s",
|
||||
@@ -224,7 +224,7 @@
|
||||
"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.13.0.3.nupkg.sha512",
|
||||
"newtonsoft.json.nuspec",
|
||||
"packageIcon.png"
|
||||
]
|
||||
@@ -254,28 +254,28 @@
|
||||
"projectFileDependencyGroups": {
|
||||
".NETStandard,Version=v2.0": [
|
||||
"NETStandard.Library >= 2.0.3",
|
||||
"Newtonsoft.Json >= 13.0.4",
|
||||
"Newtonsoft.Json >= 13.0.3",
|
||||
"System.Numerics.Vectors >= 4.6.1"
|
||||
]
|
||||
},
|
||||
"packageFolders": {
|
||||
"C:\\Users\\Fairyland\\.nuget\\packages\\": {},
|
||||
"D:\\VS2022\\Shared\\NuGetPackages": {}
|
||||
"C:\\Users\\admin\\.nuget\\packages\\": {},
|
||||
"C:\\Program Files (x86)\\Microsoft Visual Studio\\Shared\\NuGetPackages": {}
|
||||
},
|
||||
"project": {
|
||||
"version": "1.0.0",
|
||||
"restore": {
|
||||
"projectUniqueName": "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\ClumsyPilot.csproj",
|
||||
"projectUniqueName": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\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\\",
|
||||
"projectPath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\ClumsyPilot.csproj",
|
||||
"packagesPath": "C:\\Users\\admin\\.nuget\\packages\\",
|
||||
"outputPath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\ClumsyPilot\\obj\\",
|
||||
"projectStyle": "PackageReference",
|
||||
"fallbackFolders": [
|
||||
"D:\\VS2022\\Shared\\NuGetPackages"
|
||||
"C:\\Program Files (x86)\\Microsoft Visual Studio\\Shared\\NuGetPackages"
|
||||
],
|
||||
"configFilePaths": [
|
||||
"C:\\Users\\Fairyland\\AppData\\Roaming\\NuGet\\NuGet.Config",
|
||||
"C:\\Users\\admin\\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"
|
||||
],
|
||||
@@ -316,7 +316,7 @@
|
||||
},
|
||||
"Newtonsoft.Json": {
|
||||
"target": "Package",
|
||||
"version": "[13.0.4, )"
|
||||
"version": "[13.0.3, )"
|
||||
},
|
||||
"System.Numerics.Vectors": {
|
||||
"target": "Package",
|
||||
@@ -334,7 +334,7 @@
|
||||
],
|
||||
"assetTargetFallback": true,
|
||||
"warn": true,
|
||||
"runtimeIdentifierGraphPath": "C:\\Program Files\\dotnet\\sdk\\9.0.308\\RuntimeIdentifierGraph.json"
|
||||
"runtimeIdentifierGraphPath": "C:\\Program Files\\dotnet\\sdk\\9.0.316\\RuntimeIdentifierGraph.json"
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,13 +1,13 @@
|
||||
{
|
||||
"version": 2,
|
||||
"dgSpecHash": "gHGYwM2i+So=",
|
||||
"dgSpecHash": "YBvImiCcgSo=",
|
||||
"success": true,
|
||||
"projectFilePath": "D:\\@FariyLandTask\\@FRLD-GitProject\\ParkingRobot\\ClumsyPilot\\ClumsyPilot.csproj",
|
||||
"projectFilePath": "D:\\Users\\Desktop\\入职培训\\停车机器人\\MyParking\\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"
|
||||
"C:\\Users\\admin\\.nuget\\packages\\microsoft.netcore.platforms\\1.1.0\\microsoft.netcore.platforms.1.1.0.nupkg.sha512",
|
||||
"C:\\Users\\admin\\.nuget\\packages\\netstandard.library\\2.0.3\\netstandard.library.2.0.3.nupkg.sha512",
|
||||
"C:\\Users\\admin\\.nuget\\packages\\newtonsoft.json\\13.0.3\\newtonsoft.json.13.0.3.nupkg.sha512",
|
||||
"C:\\Users\\admin\\.nuget\\packages\\system.numerics.vectors\\4.6.1\\system.numerics.vectors.4.6.1.nupkg.sha512"
|
||||
],
|
||||
"logs": []
|
||||
}
|
||||
Reference in New Issue
Block a user