412 lines
15 KiB
C#
412 lines
15 KiB
C#
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;
|
|
}
|
|
}
|
|
}
|