新增1.0和2.0两种协议车型车型
This commit is contained in:
ykkokluo
@@ -0,0 +1,588 @@
|
||||
using SimpleCore;
|
||||
using SimpleCore.PropType;
|
||||
using StandardScene.Magnetic.Protocol;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Threading;
|
||||
using System.Threading.Tasks;
|
||||
|
||||
namespace StandardScene.Magnetic.Tasking
|
||||
{
|
||||
public sealed class Fass2TaskCallbacks
|
||||
{
|
||||
public Func<int, int, double, Fass2TaskPlan> BuildPlan { get; set; }
|
||||
public Action<Fass2NodeMessage[], ulong> SendNodes { get; set; }
|
||||
public Action<Fass2NodeMessage, ulong> SendAction { get; set; }
|
||||
public Action<byte, ushort> SendControl { get; set; }
|
||||
public Func<int, ushort> ResolveNodeId { get; set; }
|
||||
public Func<ushort, Site> ResolveSite { get; set; }
|
||||
public Action<string> Log { get; set; }
|
||||
public Action<Fass2TaskContext> Persist { get; set; }
|
||||
public Action ClearPersisted { get; set; }
|
||||
public Func<ulong> AllocateTaskId { get; set; }
|
||||
public Func<ulong> AllocateActionId { get; set; }
|
||||
}
|
||||
|
||||
public sealed class Fass2TaskTickResult
|
||||
{
|
||||
public Fass2TaskPhase Phase { get; set; }
|
||||
public bool Dispatched { get; set; }
|
||||
public bool Rebuilt { get; set; }
|
||||
public bool ActionSent { get; set; }
|
||||
public bool Advanced { get; set; }
|
||||
public bool Completed { get; set; }
|
||||
public bool Faulted { get; set; }
|
||||
public string Message { get; set; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// FASS 2.0 任务状态机：规划 → 滑动窗口下发 → UDP/TCP 状态驱动推进 → 动作闭环。
|
||||
/// </summary>
|
||||
public sealed class Fass2TaskStateMachine
|
||||
{
|
||||
private readonly Fass2TaskCallbacks _callbacks;
|
||||
private readonly object _syncRoot = new object();
|
||||
private TaskCompletionSource<int> _completionSource;
|
||||
|
||||
public Fass2TaskStateMachine(Fass2TaskCallbacks callbacks)
|
||||
{
|
||||
_callbacks = callbacks ?? throw new ArgumentNullException(nameof(callbacks));
|
||||
Context = new Fass2TaskContext();
|
||||
}
|
||||
|
||||
public Fass2TaskContext Context { get; }
|
||||
|
||||
public int LockCount { get; set; } = 4;
|
||||
|
||||
public int ResendIntervalMs { get; set; } = 500;
|
||||
|
||||
public int ActionRetryIntervalMs { get; set; } = 1000;
|
||||
|
||||
public bool StartBeforeMove { get; set; } = true;
|
||||
|
||||
public ushort HeadingAngle { get; set; }
|
||||
|
||||
public bool IsIdle
|
||||
{
|
||||
get
|
||||
{
|
||||
lock (_syncRoot)
|
||||
{
|
||||
return Context.Phase is Fass2TaskPhase.Idle or Fass2TaskPhase.Complete or Fass2TaskPhase.Cancelled;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public bool IsRunning
|
||||
{
|
||||
get
|
||||
{
|
||||
lock (_syncRoot)
|
||||
{
|
||||
return Context.Phase is Fass2TaskPhase.Planning or Fass2TaskPhase.Dispatching
|
||||
or Fass2TaskPhase.Moving or Fass2TaskPhase.AtStation or Fass2TaskPhase.SegmentDone;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public event Action<Fass2TaskContext> Completed;
|
||||
public event Action<Fass2TaskContext, string> Faulted;
|
||||
|
||||
public void Begin(int startSiteId, int goalSiteId, double defaultSpeed = -1)
|
||||
{
|
||||
lock (_syncRoot)
|
||||
{
|
||||
if (!IsIdle)
|
||||
{
|
||||
throw new InvalidOperationException($"task state machine busy, phase={Context.Phase}");
|
||||
}
|
||||
|
||||
ResetCompletionSource();
|
||||
Context.Phase = Fass2TaskPhase.Planning;
|
||||
Context.StartSiteId = startSiteId;
|
||||
Context.GoalSiteId = goalSiteId;
|
||||
Context.CurrentIndex = 0;
|
||||
Context.DefaultSpeed = defaultSpeed;
|
||||
Context.StartedAt = DateTime.Now;
|
||||
Context.FaultReason = null;
|
||||
Context.Plan = null;
|
||||
Context.FieldsSignature = string.Empty;
|
||||
Context.LastDispatchAt = DateTime.MinValue;
|
||||
Context.LastActionSentAt = DateTime.MinValue;
|
||||
Context.LastActionId = 0;
|
||||
Log($"task begin start={startSiteId}, goal={goalSiteId}, speed={defaultSpeed}");
|
||||
Persist();
|
||||
}
|
||||
}
|
||||
|
||||
public bool TryRestore(Fass2TaskContext saved)
|
||||
{
|
||||
if (saved == null)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
lock (_syncRoot)
|
||||
{
|
||||
if (IsRunning)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
ResetCompletionSource();
|
||||
Context.Phase = saved.Phase;
|
||||
Context.StartSiteId = saved.StartSiteId;
|
||||
Context.GoalSiteId = saved.GoalSiteId;
|
||||
Context.CurrentIndex = saved.CurrentIndex;
|
||||
Context.TaskId = saved.TaskId;
|
||||
Context.DefaultSpeed = saved.DefaultSpeed;
|
||||
Context.FieldsSignature = saved.FieldsSignature ?? string.Empty;
|
||||
Context.StartedAt = DateTime.Now;
|
||||
Context.LastDispatchAt = DateTime.MinValue;
|
||||
Context.LastActionSentAt = DateTime.MinValue;
|
||||
Context.Plan = null;
|
||||
Context.FaultReason = null;
|
||||
|
||||
if (Context.Phase == Fass2TaskPhase.Planning)
|
||||
{
|
||||
Log($"task restored phase={Context.Phase}, goal={Context.GoalSiteId}, index={Context.CurrentIndex}");
|
||||
Persist();
|
||||
return true;
|
||||
}
|
||||
|
||||
Context.Phase = Fass2TaskPhase.Planning;
|
||||
Log($"task restored and normalized to Planning, goal={Context.GoalSiteId}, index={Context.CurrentIndex}");
|
||||
Persist();
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
public Fass2TaskTickResult Tick(Fass2StateReport report)
|
||||
{
|
||||
lock (_syncRoot)
|
||||
{
|
||||
if (Context.Phase is Fass2TaskPhase.Idle or Fass2TaskPhase.Complete
|
||||
or Fass2TaskPhase.Cancelled or Fass2TaskPhase.Fault)
|
||||
{
|
||||
return new Fass2TaskTickResult { Phase = Context.Phase };
|
||||
}
|
||||
|
||||
var result = new Fass2TaskTickResult { Phase = Context.Phase };
|
||||
|
||||
if (report?.Alarm != 0)
|
||||
{
|
||||
return Fault($"alarm=0x{report.Alarm:X}", result);
|
||||
}
|
||||
|
||||
if (report?.State == 3)
|
||||
{
|
||||
return Fault("vehicle emergency stop", result);
|
||||
}
|
||||
|
||||
if (report?.State == 4)
|
||||
{
|
||||
return Fault("vehicle fault", result);
|
||||
}
|
||||
|
||||
switch (Context.Phase)
|
||||
{
|
||||
case Fass2TaskPhase.Planning:
|
||||
return PlanAndDispatch(report, result);
|
||||
case Fass2TaskPhase.Dispatching:
|
||||
Context.Phase = Fass2TaskPhase.Moving;
|
||||
result.Phase = Context.Phase;
|
||||
return result;
|
||||
case Fass2TaskPhase.Moving:
|
||||
return HandleMoving(report, result);
|
||||
case Fass2TaskPhase.AtStation:
|
||||
return HandleAtStation(report, result);
|
||||
case Fass2TaskPhase.SegmentDone:
|
||||
return HandleSegmentDone(report, result);
|
||||
default:
|
||||
return result;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public void Cancel(string reason)
|
||||
{
|
||||
lock (_syncRoot)
|
||||
{
|
||||
if (IsIdle)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
Context.Phase = Fass2TaskPhase.Cancelled;
|
||||
Context.FaultReason = reason;
|
||||
Log($"task cancelled: {reason}");
|
||||
ClearPersisted();
|
||||
_completionSource?.TrySetException(new OperationCanceledException(reason));
|
||||
}
|
||||
}
|
||||
|
||||
public async Task WaitAsync(int timeoutMs, Func<Fass2StateReport> poll, int pollIntervalMs,
|
||||
CancellationToken cancellationToken = default)
|
||||
{
|
||||
if (poll == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(poll));
|
||||
}
|
||||
|
||||
TaskCompletionSource<int> waiter;
|
||||
lock (_syncRoot)
|
||||
{
|
||||
waiter = _completionSource ?? ResetCompletionSource();
|
||||
}
|
||||
|
||||
var timeoutTask = Task.Delay(timeoutMs, cancellationToken);
|
||||
var pollTask = Task.Run(async () =>
|
||||
{
|
||||
while (!cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
Fass2StateReport report;
|
||||
lock (_syncRoot)
|
||||
{
|
||||
if (!IsRunning)
|
||||
{
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
report = poll();
|
||||
Tick(report);
|
||||
|
||||
lock (_syncRoot)
|
||||
{
|
||||
if (!IsRunning)
|
||||
{
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
await Task.Delay(Math.Max(20, pollIntervalMs), cancellationToken);
|
||||
}
|
||||
}, cancellationToken);
|
||||
|
||||
var finished = await Task.WhenAny(waiter.Task, timeoutTask);
|
||||
if (finished == timeoutTask)
|
||||
{
|
||||
Cancel("wait timeout");
|
||||
throw new TimeoutException(
|
||||
$"FASS2 task timeout after {timeoutMs}ms, phase={Context.Phase}, index={Context.CurrentIndex}, goal={Context.GoalSiteId}");
|
||||
}
|
||||
|
||||
await waiter.Task;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// UDP 模式由 Hub 回调驱动 Tick，此处只等待完成信号，避免与 OnUdpStateReceived 争用锁导致死锁。
|
||||
/// </summary>
|
||||
public async Task WaitForCompletionAsync(int timeoutMs, CancellationToken cancellationToken = default)
|
||||
{
|
||||
TaskCompletionSource<int> waiter;
|
||||
lock (_syncRoot)
|
||||
{
|
||||
if (Context.Phase is Fass2TaskPhase.Complete or Fass2TaskPhase.Cancelled or Fass2TaskPhase.Fault)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
waiter = _completionSource ?? ResetCompletionSource();
|
||||
}
|
||||
|
||||
var timeoutTask = Task.Delay(timeoutMs, cancellationToken);
|
||||
var finished = await Task.WhenAny(waiter.Task, timeoutTask);
|
||||
if (finished == timeoutTask)
|
||||
{
|
||||
Cancel("wait timeout");
|
||||
throw new TimeoutException(
|
||||
$"FASS2 task timeout after {timeoutMs}ms, phase={Context.Phase}, index={Context.CurrentIndex}, goal={Context.GoalSiteId}");
|
||||
}
|
||||
|
||||
await waiter.Task;
|
||||
}
|
||||
|
||||
private Fass2TaskTickResult PlanAndDispatch(Fass2StateReport report, Fass2TaskTickResult result)
|
||||
{
|
||||
var startSiteId = ResolveCurrentSiteId(report);
|
||||
Context.Plan = _callbacks.BuildPlan(startSiteId, Context.GoalSiteId, Context.DefaultSpeed);
|
||||
if (Context.Plan == null || Context.Plan.Nodes.Count == 0)
|
||||
{
|
||||
return Fault($"empty task plan from {startSiteId} to {Context.GoalSiteId}", result);
|
||||
}
|
||||
|
||||
if (Context.CurrentIndex >= Context.Plan.Nodes.Count)
|
||||
{
|
||||
Context.CurrentIndex = Math.Max(0, Context.Plan.Nodes.Count - 1);
|
||||
}
|
||||
|
||||
Context.FieldsSignature = Context.Plan.FieldsSignature;
|
||||
Context.TaskId = _callbacks.AllocateTaskId();
|
||||
|
||||
if (StartBeforeMove && Context.CurrentIndex == 0)
|
||||
{
|
||||
_callbacks.SendControl(Fass2Protocol.CmdStart, HeadingAngle);
|
||||
}
|
||||
|
||||
DispatchCurrentWindow();
|
||||
Context.Phase = Fass2TaskPhase.Moving;
|
||||
result.Phase = Context.Phase;
|
||||
result.Dispatched = true;
|
||||
result.Message = $"planned nodes={Context.Plan.Nodes.Count}, task={Context.TaskId}";
|
||||
Log(result.Message);
|
||||
Persist();
|
||||
return result;
|
||||
}
|
||||
|
||||
private Fass2TaskTickResult HandleMoving(Fass2StateReport report, Fass2TaskTickResult result)
|
||||
{
|
||||
if (TryRebuildForFieldsChange(result))
|
||||
{
|
||||
return result;
|
||||
}
|
||||
|
||||
if (ShouldResend())
|
||||
{
|
||||
DispatchCurrentWindow();
|
||||
result.Dispatched = true;
|
||||
}
|
||||
|
||||
var expected = GetExpectedNode();
|
||||
if (!Fass2ActionResolver.IsAtNode(report, expected.Node))
|
||||
{
|
||||
result.Message = $"moving node={report.Node.Node}, expect={expected.Node}";
|
||||
return result;
|
||||
}
|
||||
|
||||
if (Fass2ActionResolver.IsVehicleMoving(report.State))
|
||||
{
|
||||
result.Message = $"at node={expected.Node}, still running";
|
||||
return result;
|
||||
}
|
||||
|
||||
Context.Phase = Fass2TaskPhase.AtStation;
|
||||
result.Phase = Context.Phase;
|
||||
result.Message = $"arrived site index={Context.CurrentIndex}, node={expected.Node}";
|
||||
Log(result.Message);
|
||||
Persist();
|
||||
return result;
|
||||
}
|
||||
|
||||
private Fass2TaskTickResult HandleAtStation(Fass2StateReport report, Fass2TaskTickResult result)
|
||||
{
|
||||
if (TryRebuildForFieldsChange(result))
|
||||
{
|
||||
return result;
|
||||
}
|
||||
|
||||
if (ShouldResend())
|
||||
{
|
||||
DispatchCurrentWindow();
|
||||
result.Dispatched = true;
|
||||
}
|
||||
|
||||
var expected = GetExpectedNode();
|
||||
if (!Fass2ActionResolver.IsAtNode(report, expected.Node))
|
||||
{
|
||||
Context.Phase = Fass2TaskPhase.Moving;
|
||||
result.Phase = Context.Phase;
|
||||
result.Message = $"left station node={report.Node.Node}, expect={expected.Node}";
|
||||
return result;
|
||||
}
|
||||
|
||||
if (!Fass2ActionResolver.IsStationActionComplete(expected, report.Node, report.State))
|
||||
{
|
||||
if (ShouldSendActionPatch())
|
||||
{
|
||||
var patch = Fass2ActionResolver.BuildActionPatch(expected, report.Node);
|
||||
if (patch != null)
|
||||
{
|
||||
Context.LastActionId = _callbacks.AllocateActionId();
|
||||
Context.LastActionSentAt = DateTime.Now;
|
||||
_callbacks.SendAction(patch, Context.LastActionId);
|
||||
result.ActionSent = true;
|
||||
result.Message =
|
||||
$"action pending [{Fass2ActionResolver.DescribePending(expected, report.Node)}], sent 0xA1={Context.LastActionId}";
|
||||
Log(result.Message);
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
Context.Phase = Fass2TaskPhase.SegmentDone;
|
||||
result.Phase = Context.Phase;
|
||||
result.Message = $"station done index={Context.CurrentIndex}, node={expected.Node}";
|
||||
Log(result.Message);
|
||||
Persist();
|
||||
return result;
|
||||
}
|
||||
|
||||
private Fass2TaskTickResult HandleSegmentDone(Fass2StateReport report, Fass2TaskTickResult result)
|
||||
{
|
||||
Context.CurrentIndex++;
|
||||
result.Advanced = true;
|
||||
|
||||
if (Context.Plan == null || Context.CurrentIndex >= Context.Plan.Nodes.Count)
|
||||
{
|
||||
Context.Phase = Fass2TaskPhase.Complete;
|
||||
result.Phase = Context.Phase;
|
||||
result.Completed = true;
|
||||
result.Message = $"task complete goal={Context.GoalSiteId}";
|
||||
Log(result.Message);
|
||||
ClearPersisted();
|
||||
Completed?.Invoke(Context);
|
||||
_completionSource?.TrySetResult(1);
|
||||
return result;
|
||||
}
|
||||
|
||||
Context.Phase = Fass2TaskPhase.Dispatching;
|
||||
DispatchCurrentWindow();
|
||||
Context.Phase = Fass2TaskPhase.Moving;
|
||||
result.Phase = Context.Phase;
|
||||
result.Dispatched = true;
|
||||
result.Message = $"segment advanced index={Context.CurrentIndex}";
|
||||
Log(result.Message);
|
||||
Persist();
|
||||
return result;
|
||||
}
|
||||
|
||||
private bool TryRebuildForFieldsChange(Fass2TaskTickResult result)
|
||||
{
|
||||
if (Context.Plan?.SiteIds == null || Context.Plan.SiteIds.Count == 0)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
var newSignature = Fass2SiteFieldReader.BuildFieldsSignature(
|
||||
Context.Plan.SiteIds, Context.CurrentIndex, SimpleLib.GetSite);
|
||||
if (string.Equals(newSignature, Context.FieldsSignature, StringComparison.Ordinal))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
var currentSiteId = Context.Plan.SiteIds[Context.CurrentIndex];
|
||||
Log($"fields changed at index={Context.CurrentIndex}, rebuild from site={currentSiteId}");
|
||||
Context.Plan = _callbacks.BuildPlan(currentSiteId, Context.GoalSiteId, Context.DefaultSpeed);
|
||||
if (Context.Plan == null || Context.Plan.Nodes.Count == 0)
|
||||
{
|
||||
Fault($"rebuild failed from {currentSiteId} to {Context.GoalSiteId}", result);
|
||||
return true;
|
||||
}
|
||||
|
||||
Context.CurrentIndex = 0;
|
||||
Context.FieldsSignature = Context.Plan.FieldsSignature;
|
||||
Context.TaskId = _callbacks.AllocateTaskId();
|
||||
DispatchCurrentWindow();
|
||||
Context.Phase = Fass2TaskPhase.Moving;
|
||||
result.Phase = Context.Phase;
|
||||
result.Rebuilt = true;
|
||||
result.Dispatched = true;
|
||||
result.Message = $"rebuilt nodes={Context.Plan.Nodes.Count}, task={Context.TaskId}";
|
||||
Persist();
|
||||
return true;
|
||||
}
|
||||
|
||||
private void DispatchCurrentWindow()
|
||||
{
|
||||
var window = BuildDispatchWindow();
|
||||
if (window.Length == 0)
|
||||
{
|
||||
throw new InvalidOperationException("dispatch window is empty");
|
||||
}
|
||||
|
||||
_callbacks.SendNodes(window, Context.TaskId);
|
||||
Context.LastDispatchAt = DateTime.Now;
|
||||
Log($"dispatch window count={window.Length}, fromIndex={Context.CurrentIndex}, task={Context.TaskId}");
|
||||
}
|
||||
|
||||
private Fass2NodeMessage[] BuildDispatchWindow()
|
||||
{
|
||||
var nodes = Context.Plan.Nodes;
|
||||
var start = Context.CurrentIndex;
|
||||
var remaining = nodes.Count - start;
|
||||
var windowSize = Math.Max(1, Math.Min(LockCount, remaining));
|
||||
windowSize = Math.Min(windowSize, 10);
|
||||
var window = new Fass2NodeMessage[windowSize];
|
||||
for (var i = 0; i < windowSize; i++)
|
||||
{
|
||||
window[i] = nodes[start + i];
|
||||
}
|
||||
|
||||
return window;
|
||||
}
|
||||
|
||||
private Fass2NodeMessage GetExpectedNode()
|
||||
{
|
||||
return Context.Plan.Nodes[Context.CurrentIndex];
|
||||
}
|
||||
|
||||
private int ResolveCurrentSiteId(Fass2StateReport report)
|
||||
{
|
||||
if (Context.CurrentIndex > 0 && Context.Plan?.SiteIds != null &&
|
||||
Context.CurrentIndex < Context.Plan.SiteIds.Count)
|
||||
{
|
||||
return Context.Plan.SiteIds[Context.CurrentIndex];
|
||||
}
|
||||
|
||||
if (report?.Node != null && report.Node.Node != 0)
|
||||
{
|
||||
var site = _callbacks.ResolveSite(report.Node.Node);
|
||||
if (site != null)
|
||||
{
|
||||
return site.id;
|
||||
}
|
||||
}
|
||||
|
||||
return Context.StartSiteId;
|
||||
}
|
||||
|
||||
private bool ShouldResend()
|
||||
{
|
||||
return Context.LastDispatchAt == DateTime.MinValue ||
|
||||
(DateTime.Now - Context.LastDispatchAt).TotalMilliseconds >= ResendIntervalMs;
|
||||
}
|
||||
|
||||
private bool ShouldSendActionPatch()
|
||||
{
|
||||
return Context.LastActionSentAt == DateTime.MinValue ||
|
||||
(DateTime.Now - Context.LastActionSentAt).TotalMilliseconds >= ActionRetryIntervalMs;
|
||||
}
|
||||
|
||||
private Fass2TaskTickResult Fault(string reason, Fass2TaskTickResult result)
|
||||
{
|
||||
Context.Phase = Fass2TaskPhase.Fault;
|
||||
Context.FaultReason = reason;
|
||||
result.Phase = Context.Phase;
|
||||
result.Faulted = true;
|
||||
result.Message = reason;
|
||||
Log($"task fault: {reason}");
|
||||
Persist();
|
||||
Faulted?.Invoke(Context, reason);
|
||||
_completionSource?.TrySetException(new InvalidOperationException(reason));
|
||||
return result;
|
||||
}
|
||||
|
||||
private void Persist()
|
||||
{
|
||||
_callbacks.Persist?.Invoke(Context);
|
||||
}
|
||||
|
||||
private void ClearPersisted()
|
||||
{
|
||||
_callbacks.ClearPersisted?.Invoke();
|
||||
}
|
||||
|
||||
private void Log(string message)
|
||||
{
|
||||
_callbacks.Log?.Invoke(message);
|
||||
}
|
||||
|
||||
private TaskCompletionSource<int> ResetCompletionSource()
|
||||
{
|
||||
_completionSource = new TaskCompletionSource<int>(TaskCreationOptions.RunContinuationsAsynchronously);
|
||||
return _completionSource;
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user