黄兆尉andCursor
e4a245e644
等锁超时、全路径签名、无锁窗口不下发、Fault 终态可等待、Pass+机构不再短路、Alarm 不杀单、catch-up 停在未完成站；VehicleCode=0/重复车号拒绝注册，Hub 已运行时不抢监听口；FileLogger 空目录可写。 Co-authored-by: Cursor <cursoragent@cursor.com>
1933 lines
68 KiB
C#
1933 lines
68 KiB
C#
using LessokajiWeaverUtilities.Utilities;
|
||
using SimpleCore;
|
||
using SimpleCore.Compiler;
|
||
using SimpleCore.BasicProps;
|
||
using SimpleCore.Library;
|
||
using SimpleCore.PropType;
|
||
using SimpleCore.Traffic;
|
||
using Simple3.Props;
|
||
using Simple3.RCS;
|
||
using Simple3.RCS.CarTypes;
|
||
using Simple3.UI;
|
||
using StandardScene.Magnetic.Protocol;
|
||
using StandardScene.Magnetic.Tasking;
|
||
using StandardScene;
|
||
using System;
|
||
using System.Diagnostics;
|
||
using System.Drawing;
|
||
using System.Drawing.Drawing2D;
|
||
using System.IO;
|
||
using System.Linq;
|
||
using System.Net.Sockets;
|
||
using System.Threading;
|
||
using System.Threading.Tasks;
|
||
|
||
namespace StandardScene.CarTypes
|
||
{
|
||
/// <summary>
|
||
/// 基于 FASS 2.0 通用控制接口实现的磁导航小车类型。
|
||
/// 支持 PLC/TCP（主动查询）与 PCB/UDP（被动收状态 + 0x10 应答）两种通讯模式。
|
||
/// 路径段通过 0xB1 下发，业务逻辑对齐 <c>FASS.Scheduler</c> Fairyland 调度实现。
|
||
/// </summary>
|
||
[TemplateTrackCoderSettings(
|
||
priority = 1,
|
||
templateString = "agv.Mag2Go(${src.id},${dst.id},${track.Speed});",
|
||
blockVerb = "true",
|
||
trackFields = typeof(BasicTrackFields))]
|
||
[CarType(Name = "Mag2Car")]
|
||
[I18N.DocumentTranslation(Name = "Mag2Car", locale = "en")]
|
||
[EnvelopConfig(lengthX = 1200, lengthY = 800, centerX = 0, centerY = 0)]
|
||
public class Mag2Car : GhostCar, IFass2UdpCar, IFass2LoopCar
|
||
{
|
||
private readonly object _syncRoot = new object();
|
||
private TcpClient _persistentClient;
|
||
private DateTime _lastPollTime = DateTime.MinValue;
|
||
private DateTime _lastUdpReportTime = DateTime.MinValue;
|
||
private bool _udpRegistered;
|
||
private bool _udpZeroCodeWarned;
|
||
private ushort _registeredVehicleCode = ushort.MaxValue;
|
||
private long _nextTaskId = 1;
|
||
private long _nextActionId = 1;
|
||
private Fass2TaskStateMachine _taskStateMachine;
|
||
private Fass2TrafficLocker _trafficLocker;
|
||
private bool _taskRestoreAttempted;
|
||
private bool _wasOffline;
|
||
private int _reconnectWaitSiteId = -1;
|
||
private int _lastOffPathHandledSiteId = -1;
|
||
private ushort _lastMappedNode;
|
||
private ushort _lastMappedDistance;
|
||
|
||
[FieldMember] public Fass2CommMode CommMode = Fass2CommMode.Udp;
|
||
[FieldMember] public int Port = 5000;
|
||
[FieldMember] public int ListenPort = 20103;
|
||
[FieldMember] public ushort VehicleCode = 0;
|
||
[FieldMember] public int ConnectTimeoutMs = 500;
|
||
[FieldMember] public int SendTimeoutMs = 500;
|
||
[FieldMember] public int ReceiveTimeoutMs = 500;
|
||
[FieldMember] public int PollIntervalMs = 1000;
|
||
[FieldMember] public int UdpOfflineTimeoutMs = 15000;
|
||
[FieldMember] public int MoveTimeoutSeconds = 120;
|
||
[FieldMember] public int LockCount = 4;
|
||
[FieldMember] public bool EnableTrafficControl = true;
|
||
[FieldMember] public bool VerifyTrajectoryFieldsOnStation = false;
|
||
[FieldMember] public int TaskResendIntervalMs = 500;
|
||
[FieldMember] public int ActionRetryIntervalMs = 1000;
|
||
[FieldMember] public int TrafficWaitTimeoutMs = 180000;
|
||
[FieldMember] public bool EnableTaskStateMachine = true;
|
||
[FieldMember] public bool EnableLoopTaskDrive = true;
|
||
[FieldMember] public bool StartBeforeMove = true;
|
||
[FieldMember] public bool UseTagValueAsNode = false;
|
||
[FieldMember] public bool UsePersistentConnection = true;
|
||
[FieldMember] public bool EnableDetailLog = true;
|
||
[FieldMember] public bool LogRawFrame = true;
|
||
[FieldMember] public bool EnableFileLog = true;
|
||
[FieldMember] public string LogDirectory = "logs";
|
||
[FieldMember] public float CarLength = 1200;
|
||
[FieldMember] public float CarWidth = 800;
|
||
|
||
public Fass2StateReport LastReport { get; private set; } = new Fass2StateReport();
|
||
|
||
public bool IsTaskIdle => _taskStateMachine?.IsIdle ?? true;
|
||
|
||
public Fass2TaskPhase TaskPhase => _taskStateMachine?.Context.Phase ?? Fass2TaskPhase.Idle;
|
||
|
||
public bool HasGoalSite => TryGetGoalSiteId(out _);
|
||
|
||
public int? GoalSiteId => TryGetGoalSiteId(out var goalSiteId) ? goalSiteId : null;
|
||
|
||
public event Action<int> TaskCompleted;
|
||
|
||
string IFass2UdpCar.RemoteAddress => address;
|
||
|
||
int IFass2UdpCar.RemotePort => Port;
|
||
|
||
ushort IFass2UdpCar.VehicleCode => VehicleCode;
|
||
|
||
void IFass2UdpCar.OnUdpStateReceived(Fass2StateReport report)
|
||
{
|
||
lock (_syncRoot)
|
||
{
|
||
_lastUdpReportTime = DateTime.Now;
|
||
ApplyReport(report);
|
||
TryRecoverAfterReconnectIfNeeded(report);
|
||
SyncUdpOnlineTags(true);
|
||
}
|
||
|
||
TickTaskStateMachine(report);
|
||
}
|
||
|
||
public new static async Task<Mag2Car> Create()
|
||
{
|
||
var car = new Mag2Car
|
||
{
|
||
lstatus = "连接中",
|
||
address = "127.0.0.1",
|
||
name = "Mag2Car",
|
||
haveCoordination = true,
|
||
speed = 1
|
||
};
|
||
car.EnsureTaskStateMachine();
|
||
Mag2CarFileLogger.Configure(car.LogDirectory, car.EnableFileLog);
|
||
return car;
|
||
}
|
||
|
||
protected override void draw(Graphics eGraphics)
|
||
{
|
||
var halfLength = CarLength / 2;
|
||
var halfWidth = CarWidth / 2;
|
||
eGraphics.FillRectangle(Brushes.DimGray, -halfLength, -halfWidth, CarLength, CarWidth);
|
||
eGraphics.DrawRectangle(Pens.White, -halfLength, -halfWidth, CarLength, CarWidth);
|
||
|
||
using (var orientPen = new Pen(Color.Cyan, 3))
|
||
{
|
||
orientPen.CustomEndCap = new AdjustableArrowCap(6, 6, true);
|
||
orientPen.StartCap = LineCap.RoundAnchor;
|
||
eGraphics.DrawLine(orientPen, 0, 0, halfLength, 0);
|
||
}
|
||
}
|
||
|
||
public override string SetDisplayInfo()
|
||
{
|
||
try
|
||
{
|
||
var report = LastReport;
|
||
var state = Fass2Protocol.StateText(report.State);
|
||
var alarm = report.Alarm == 0 ? "" : $"|alarm:0x{report.Alarm:X}";
|
||
return $"{name}({id})\n{state}|site:{siteID}|soc:{report.BatteryCharge}|node:{report.Node.Node}{alarm}";
|
||
}
|
||
catch
|
||
{
|
||
return $"{name}({id})\n状态未知";
|
||
}
|
||
}
|
||
|
||
public override void keepAlive()
|
||
{
|
||
if ((DateTime.Now - _lastPollTime).TotalMilliseconds < PollIntervalMs)
|
||
{
|
||
return;
|
||
}
|
||
|
||
_lastPollTime = DateTime.Now;
|
||
|
||
if (CommMode == Fass2CommMode.Udp)
|
||
{
|
||
try
|
||
{
|
||
EnsureUdpSession();
|
||
TryRestoreTaskFromTags();
|
||
if (_lastUdpReportTime != DateTime.MinValue &&
|
||
(DateTime.Now - _lastUdpReportTime).TotalMilliseconds <= UdpOfflineTimeoutMs)
|
||
{
|
||
haveCoordination = true;
|
||
SyncUdpOnlineTags(true);
|
||
TryLogLoopAssignmentToFile();
|
||
var report = LastReport;
|
||
DetailLog($"keepAlive udp ok, node={report.Node.Node}, state={Fass2Protocol.StateText(report.State)}, soc={report.BatteryCharge}, taskPhase={TaskPhase}");
|
||
}
|
||
else
|
||
{
|
||
lstatus = "离线";
|
||
_wasOffline = true;
|
||
SyncUdpOnlineTags(false);
|
||
DetailLog($"keepAlive udp waiting, listen={ListenPort}, remote={address}:{Port}");
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
lstatus = "离线";
|
||
_wasOffline = true;
|
||
SyncUdpOnlineTags(false);
|
||
DetailLog($"keepAlive udp failed: {ex.Message}");
|
||
}
|
||
|
||
return;
|
||
}
|
||
|
||
try
|
||
{
|
||
DetailLog($"keepAlive poll begin, endpoint={address}:{Port}");
|
||
var report = RefreshState();
|
||
haveCoordination = true;
|
||
DetailLog($"keepAlive ok, node={report.Node.Node}, state={Fass2Protocol.StateText(report.State)}, soc={report.BatteryCharge}");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
lstatus = IsBenignSocketFault(ex) ? lstatus : "离线";
|
||
if (!IsBenignSocketFault(ex))
|
||
{
|
||
_wasOffline = true;
|
||
DetailLog($"keepAlive failed: {ex.Message}");
|
||
}
|
||
}
|
||
}
|
||
|
||
private bool _running;
|
||
private int _lastSyncedTrafficSiteId = -1;
|
||
private int _lastFileLoggedGoalSite = -1;
|
||
|
||
public override async Task actualSendScript(string script)
|
||
{
|
||
if (_running)
|
||
{
|
||
throw new InvalidOperationException($"Mag2Car {id} already running script");
|
||
}
|
||
|
||
if (ShouldDriveLoopGoalAsTrackedTask(script))
|
||
{
|
||
_running = true;
|
||
try
|
||
{
|
||
DetailLog($"loop goal drive intercept, goal={GoalSiteId}, scriptLen={(script ?? string.Empty).Length}");
|
||
await ExecuteGoalTaskAsync();
|
||
DetailLog("loop goal drive completed");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
DetailLog($"loop goal drive failed: {ExceptionFormatter.FormatEx(ex)}");
|
||
RecoverAfterGoalDriveFailure();
|
||
throw;
|
||
}
|
||
finally
|
||
{
|
||
_running = false;
|
||
}
|
||
|
||
return;
|
||
}
|
||
|
||
_running = true;
|
||
try
|
||
{
|
||
DetailLog($"script begin, length={(script ?? string.Empty).Length}");
|
||
var agv = new Mag2CarInterface(id);
|
||
var tcs = new TaskCompletionSource<int>();
|
||
new Thread(() =>
|
||
{
|
||
try
|
||
{
|
||
SelfEvaluating(agv, script);
|
||
tcs.SetResult(1);
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
tcs.SetException(ex);
|
||
}
|
||
})
|
||
{ Name = $"eva_{name}({id}):{status.programs.now.name}" }.Start();
|
||
|
||
await tcs.Task;
|
||
await agv.WaitAsync();
|
||
DetailLog("script completed");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
DetailLog($"script failed: {ExceptionFormatter.FormatEx(ex)}");
|
||
throw;
|
||
}
|
||
finally
|
||
{
|
||
_running = false;
|
||
}
|
||
}
|
||
|
||
[MethodMember(Name = "查询状态", Description = "按FASS 2.0协议查询AGV状态")]
|
||
public void QueryState()
|
||
{
|
||
RefreshState();
|
||
}
|
||
|
||
[MethodMember(Name = "启动", Description = "发送启动指令(0x01)")]
|
||
public void StartMag2Car()
|
||
{
|
||
SendControl(Fass2Protocol.CmdStart, 0);
|
||
}
|
||
|
||
[MethodMember(Name = "停止", Description = "发送停止指令(0x02)")]
|
||
public void StopMag2Car()
|
||
{
|
||
SendControl(Fass2Protocol.CmdStop, 0);
|
||
}
|
||
|
||
[MethodMember(Name = "急停", Description = "发送急停指令(0x03)")]
|
||
public void EmergencyStopMag2Car()
|
||
{
|
||
SendControl(Fass2Protocol.CmdEmergencyStop, 0);
|
||
}
|
||
|
||
[MethodMember(Name = "进入小车远程", Description = "进入小车远程桌面")]
|
||
[I18N.DocumentTranslation(Name = "Open remote desktop", Description = "Open the car's remote desktop", locale = "en")]
|
||
public void Mstsc()
|
||
{
|
||
Process.Start(new ProcessStartInfo
|
||
{
|
||
FileName = "mstsc",
|
||
Arguments = $"/v:{address}",
|
||
UseShellExecute = false,
|
||
CreateNoWindow = true
|
||
});
|
||
}
|
||
|
||
public void EmergencyStop(string reason)
|
||
{
|
||
Diagnosis.Post($"car{name}:Mag2Car EmergencyStop {reason}");
|
||
SendControl(Fass2Protocol.CmdEmergencyStop, 0);
|
||
}
|
||
|
||
public void EmergencyRelease()
|
||
{
|
||
Diagnosis.Post($"car{name}:Mag2Car EmergencyRelease");
|
||
SendControl(Fass2Protocol.CmdStart, 0);
|
||
}
|
||
|
||
[MethodMember(Name = "重置UDP监听", Description = "重新注册 UDP 会话（PCB 模式）")]
|
||
public void ResetUdpSession()
|
||
{
|
||
lock (_syncRoot)
|
||
{
|
||
if (_udpRegistered)
|
||
{
|
||
Fass2UdpHub.Unregister(_registeredVehicleCode, this);
|
||
_udpRegistered = false;
|
||
_registeredVehicleCode = ushort.MaxValue;
|
||
}
|
||
|
||
_lastUdpReportTime = DateTime.MinValue;
|
||
}
|
||
|
||
EnsureUdpSession();
|
||
DetailLog($"UDP session reset, listen={ListenPort}, remote={address}:{Port}");
|
||
}
|
||
|
||
[MethodMember(Name = "重置TCP连接", Description = "关闭当前长连接，下次命令自动重连（PLC 模式）")]
|
||
public void ResetTcpConnection()
|
||
{
|
||
lock (_syncRoot)
|
||
{
|
||
ClosePersistentConnection("manual reset");
|
||
}
|
||
}
|
||
|
||
[MethodMember(Name = "重置环线联调", Description = "清空 Loop/FASS2 标签、交管锁和脚本错误，恢复可再分配")]
|
||
public void ResetLoopDriveState()
|
||
{
|
||
lock (_syncRoot)
|
||
{
|
||
_running = false;
|
||
_lastSyncedTrafficSiteId = -1;
|
||
_taskRestoreAttempted = false;
|
||
_wasOffline = false;
|
||
_reconnectWaitSiteId = -1;
|
||
_lastOffPathHandledSiteId = -1;
|
||
|
||
if (_taskStateMachine != null && !_taskStateMachine.IsIdle)
|
||
{
|
||
_taskStateMachine.Cancel("manual loop reset");
|
||
}
|
||
|
||
ClearTaskPersistence();
|
||
ClearLoopDriveTags();
|
||
_lastFileLoggedGoalSite = -1;
|
||
|
||
var site = ResolveSite(LastReport.Node.Node);
|
||
if (site == null && GetLastSite() > 0)
|
||
{
|
||
site = SimpleLib.GetSite(GetLastSite());
|
||
}
|
||
|
||
if (site == null && siteID > 0)
|
||
{
|
||
site = SimpleLib.GetSite(siteID);
|
||
}
|
||
|
||
if (site != null)
|
||
{
|
||
TrafficReset(site, true, strict: false);
|
||
siteID = site.id;
|
||
x = site.x;
|
||
y = site.y;
|
||
_lastSyncedTrafficSiteId = site.id;
|
||
haveCoordination = true;
|
||
}
|
||
|
||
status.programs.task = Task.CompletedTask;
|
||
SyncUdpOnlineTags(IsUdpSessionOnline());
|
||
DetailLog(
|
||
$"loop drive state reset, site={(site?.id.ToString() ?? "-")}, holding=[{string.Join(",", status.holdingLocks)}], pending=[{string.Join(",", status.pendingLocks)}]");
|
||
}
|
||
}
|
||
|
||
private void ClearLoopDriveTags()
|
||
{
|
||
if (tags == null)
|
||
{
|
||
return;
|
||
}
|
||
|
||
Commons.DeleteTag(tags, "goalSite");
|
||
Commons.DeleteTag(tags, "occupied");
|
||
Commons.DeleteTag(tags, "loopAssigned");
|
||
Commons.DeleteTag(tags, "dest");
|
||
Commons.DeleteTag(tags, "deliver");
|
||
Commons.DeleteTag(tags, "changePriority");
|
||
Commons.DeleteTag(tags, "priority");
|
||
Commons.DeleteTag(tags, "currentWorkStep");
|
||
Fass2TaskPersistence.Clear(tags);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 任务失败/超时后清 pending、复位本站交管锁，去掉 occupied，保留 goalSite 供环线重派。
|
||
/// </summary>
|
||
private void RecoverAfterGoalDriveFailure()
|
||
{
|
||
try
|
||
{
|
||
if (_taskStateMachine != null && !_taskStateMachine.IsIdle)
|
||
{
|
||
_taskStateMachine.Cancel("recover after failure");
|
||
}
|
||
}
|
||
catch
|
||
{
|
||
// ignore
|
||
}
|
||
|
||
try
|
||
{
|
||
ClearTaskPersistence();
|
||
Commons.DeleteTag(tags, "occupied");
|
||
|
||
var site = ResolveSite(LastReport.Node.Node);
|
||
if (site == null && siteID > 0)
|
||
{
|
||
site = SimpleLib.GetSite(siteID);
|
||
}
|
||
|
||
if (site == null && GetLastSite() > 0)
|
||
{
|
||
site = SimpleLib.GetSite(GetLastSite());
|
||
}
|
||
|
||
if (site != null)
|
||
{
|
||
TrafficReset(site, true, strict: false);
|
||
siteID = site.id;
|
||
_lastSyncedTrafficSiteId = site.id;
|
||
haveCoordination = true;
|
||
}
|
||
else
|
||
{
|
||
status.pendingLocks = Array.Empty<int>();
|
||
status.seqScope = Array.Empty<int>();
|
||
status.seqPtr = 0;
|
||
}
|
||
|
||
SyncUdpOnlineTags(IsUdpSessionOnline());
|
||
DetailLog(
|
||
$"goal drive recover done, keepGoal={GoalSiteId}, site={(site?.id.ToString() ?? "-")}, holding=[{string.Join(",", status.holdingLocks)}], pending=[{string.Join(",", status.pendingLocks)}]");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
DetailLog($"goal drive recover failed: {ex.Message}");
|
||
}
|
||
}
|
||
|
||
private bool IsUdpSessionOnline()
|
||
{
|
||
return CommMode == Fass2CommMode.Udp &&
|
||
_lastUdpReportTime != DateTime.MinValue &&
|
||
(DateTime.Now - _lastUdpReportTime).TotalMilliseconds <= UdpOfflineTimeoutMs;
|
||
}
|
||
|
||
public Fass2StateReport RefreshState()
|
||
{
|
||
if (CommMode == Fass2CommMode.Udp)
|
||
{
|
||
EnsureUdpSession();
|
||
if (_lastUdpReportTime == DateTime.MinValue ||
|
||
(DateTime.Now - _lastUdpReportTime).TotalMilliseconds > UdpOfflineTimeoutMs)
|
||
{
|
||
throw new TimeoutException(
|
||
$"Mag2Car {name}({id}) UDP state timeout ({UdpOfflineTimeoutMs}ms), listen={ListenPort}");
|
||
}
|
||
|
||
return LastReport;
|
||
}
|
||
|
||
var request = Fass2Protocol.BuildControl(Fass2Protocol.CmdQuery, VehicleCode, 0);
|
||
var response = SendRequest(request, Fass2Protocol.StateFrameLength, "Query(0x00)");
|
||
var report = Fass2Protocol.ParseState(response);
|
||
ApplyReport(report);
|
||
TryRecoverAfterReconnectIfNeeded(report);
|
||
return report;
|
||
}
|
||
|
||
public void SendControl(byte command, ushort param)
|
||
{
|
||
var request = Fass2Protocol.BuildControl(command, VehicleCode, param);
|
||
SendRequest(request, Fass2Protocol.StateFrameLength, CommandName(command));
|
||
}
|
||
|
||
public void SendPathNodes(Fass2NodeMessage[] nodes)
|
||
{
|
||
var taskId = (ulong)Interlocked.Increment(ref _nextTaskId);
|
||
SendPathNodes(nodes, taskId);
|
||
}
|
||
|
||
public void SendPathNodes(Fass2NodeMessage[] nodes, ulong taskId)
|
||
{
|
||
var request = Fass2Protocol.BuildNodes(VehicleCode, taskId, nodes);
|
||
DetailLog($"send nodes 0xB1, task={taskId}, count={nodes.Length}");
|
||
SendRequest(request, Fass2Protocol.StateFrameLength, "Nodes(0xB1)");
|
||
}
|
||
|
||
public Fass2TaskPlan BuildTaskPlan(int startSiteId, int goalSiteId, double defaultSpeed = -1)
|
||
{
|
||
return Fass2TaskBuilder.BuildPath(startSiteId, goalSiteId, CreateTaskBuildOptions(defaultSpeed), ResolveNodeId, this);
|
||
}
|
||
|
||
public void SendTaskPlan(Fass2TaskPlan plan)
|
||
{
|
||
if (plan == null || plan.Nodes == null || plan.Nodes.Count == 0)
|
||
{
|
||
throw new ArgumentException("Fass2TaskPlan requires at least one node");
|
||
}
|
||
|
||
var taskId = (ulong)Interlocked.Increment(ref _nextTaskId);
|
||
DetailLog(
|
||
$"send task plan 0xB1, task={taskId}, sites={plan.SiteIds.Count}, nodes={plan.Nodes.Count}, batches={plan.Batches.Count}, sig={plan.FieldsSignature}");
|
||
|
||
foreach (var batch in plan.Batches)
|
||
{
|
||
SendPathNodes(batch, taskId);
|
||
}
|
||
}
|
||
|
||
public void BeginTrackedTask(int startSiteId, int goalSiteId, double defaultSpeed = -1)
|
||
{
|
||
EnsureTaskStateMachine();
|
||
_taskStateMachine.LockCount = LockCount;
|
||
_taskStateMachine.ResendIntervalMs = TaskResendIntervalMs;
|
||
_taskStateMachine.ActionRetryIntervalMs = ActionRetryIntervalMs;
|
||
_taskStateMachine.TrafficWaitTimeoutMs = TrafficWaitTimeoutMs;
|
||
_taskStateMachine.StartBeforeMove = StartBeforeMove;
|
||
Fass2ActionResolver.VerifyTrajectoryFields = VerifyTrajectoryFieldsOnStation;
|
||
_taskStateMachine.Begin(startSiteId, goalSiteId, defaultSpeed);
|
||
|
||
var tick = _taskStateMachine.Tick(LastReport);
|
||
DetailLog($"tracked task bootstrap tick: {tick.Message}");
|
||
}
|
||
|
||
public async Task WaitTrackedTaskAsync(int timeoutMs)
|
||
{
|
||
EnsureTaskStateMachine();
|
||
var waitMs = timeoutMs <= 0 ? MoveTimeoutSeconds * 1000 : timeoutMs;
|
||
if (CommMode == Fass2CommMode.Udp)
|
||
{
|
||
await _taskStateMachine.WaitForCompletionAsync(waitMs);
|
||
return;
|
||
}
|
||
|
||
await _taskStateMachine.WaitAsync(
|
||
waitMs,
|
||
RefreshState,
|
||
Math.Max(50, Math.Min(PollIntervalMs, 500)));
|
||
}
|
||
|
||
async Task IFass2LoopCar.ExecuteGoalTaskAsync(int? goalSiteId, double defaultSpeed,
|
||
CancellationToken cancellationToken)
|
||
{
|
||
await ExecuteGoalTaskAsync(goalSiteId, defaultSpeed, cancellationToken);
|
||
}
|
||
|
||
public async Task ExecuteGoalTaskAsync(int? goalSiteId = null, double defaultSpeed = -1,
|
||
CancellationToken cancellationToken = default)
|
||
{
|
||
if (!EnableTaskStateMachine)
|
||
{
|
||
throw new InvalidOperationException("ExecuteGoalTaskAsync requires EnableTaskStateMachine=true");
|
||
}
|
||
|
||
var goalId = goalSiteId ?? GoalSiteId;
|
||
if (goalId == null || goalId.Value <= 0)
|
||
{
|
||
throw new InvalidOperationException("Mag2Car goalSite is not set");
|
||
}
|
||
|
||
var startId = ResolvePhysicalSiteId();
|
||
if (startId <= 0)
|
||
{
|
||
throw new InvalidOperationException($"Mag2Car {name}({id}) cannot resolve start site for loop task");
|
||
}
|
||
|
||
var lockedSite = status.holdingLocks.Length > 0 ? status.holdingLocks[0] : -1;
|
||
if (lockedSite > 0 && lockedSite != startId)
|
||
{
|
||
DetailLog($"ExecuteGoalTaskAsync normalize start, holding={lockedSite}, reportStart={startId}");
|
||
}
|
||
|
||
DetailLog($"ExecuteGoalTaskAsync start={startId}, goal={goalId.Value}, speed={defaultSpeed}, traffic={EnableTrafficControl}");
|
||
|
||
BeginTrackedTask(startId, goalId.Value, defaultSpeed);
|
||
await WaitTrackedTaskAsync(MoveTimeoutSeconds * 1000);
|
||
TaskCompleted?.Invoke(goalId.Value);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 环线联调以 UDP 上报节点映射的站点为准，避免 FindRoute/forecast 污染 holdingLocks 后 GetLastSite 变成目标站。
|
||
/// </summary>
|
||
private int ResolvePhysicalSiteId()
|
||
{
|
||
if (LastReport.Node.Node != 0)
|
||
{
|
||
var fromReport = ResolveSite(LastReport.Node.Node);
|
||
if (fromReport != null)
|
||
{
|
||
return fromReport.id;
|
||
}
|
||
}
|
||
|
||
if (GetLastSite() > 0)
|
||
{
|
||
return GetLastSite();
|
||
}
|
||
|
||
return siteID > 0 ? siteID : -1;
|
||
}
|
||
|
||
private bool ShouldDriveLoopGoalAsTrackedTask(string script)
|
||
{
|
||
if (!EnableLoopTaskDrive || !EnableTaskStateMachine || !HasGoalSite)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
return !string.IsNullOrEmpty(script) &&
|
||
script.Contains("Mag2Go", StringComparison.Ordinal);
|
||
}
|
||
|
||
private bool TryGetGoalSiteId(out int goalSiteId)
|
||
{
|
||
goalSiteId = 0;
|
||
if (tags == null || !tags.TryGetValue("goalSite", out var goalText) ||
|
||
!int.TryParse(goalText, out goalSiteId))
|
||
{
|
||
return false;
|
||
}
|
||
|
||
return goalSiteId > 0;
|
||
}
|
||
|
||
private void EnsureTaskStateMachine()
|
||
{
|
||
if (_taskStateMachine != null)
|
||
{
|
||
return;
|
||
}
|
||
|
||
_trafficLocker = new Fass2TrafficLocker(this, message => DetailLog(message));
|
||
|
||
_taskStateMachine = new Fass2TaskStateMachine(new Fass2TaskCallbacks
|
||
{
|
||
BuildPlan = BuildTaskPlan,
|
||
SendNodes = (nodes, taskId) => SendPathNodes(nodes, taskId),
|
||
SendAction = SendAction,
|
||
SendControl = SendControl,
|
||
ResolveNodeId = ResolveNodeId,
|
||
ResolveSite = ResolveSite,
|
||
AllocateTaskId = () => (ulong)Interlocked.Increment(ref _nextTaskId),
|
||
AllocateActionId = () => (ulong)Interlocked.Increment(ref _nextActionId),
|
||
Persist = PersistTaskContext,
|
||
ClearPersisted = ClearTaskPersistence,
|
||
Log = message => DetailLog($"task-sm {message}"),
|
||
PrepareTraffic = (siteIds, fromIndex) =>
|
||
{
|
||
if (!EnableTrafficControl)
|
||
{
|
||
return;
|
||
}
|
||
|
||
_trafficLocker.PrepareSequence(siteIds, fromIndex);
|
||
},
|
||
EnsureTrafficWindow = (siteIds, fromIndex, wantWindow) =>
|
||
{
|
||
if (!EnableTrafficControl)
|
||
{
|
||
return wantWindow;
|
||
}
|
||
|
||
return _trafficLocker.EnsureWindowLocked(siteIds, fromIndex, wantWindow);
|
||
},
|
||
LeaveTraffic = siteId =>
|
||
{
|
||
if (!EnableTrafficControl)
|
||
{
|
||
return;
|
||
}
|
||
|
||
_trafficLocker.LeavePassed(siteId);
|
||
},
|
||
ReleaseTrafficBehind = (siteIds, keepFromIndex) =>
|
||
{
|
||
if (!EnableTrafficControl)
|
||
{
|
||
return;
|
||
}
|
||
|
||
_trafficLocker.ReleaseLocksBehind(siteIds, keepFromIndex);
|
||
},
|
||
RebaseTraffic = (siteIds, fromIndex) =>
|
||
{
|
||
if (!EnableTrafficControl)
|
||
{
|
||
return;
|
||
}
|
||
|
||
_trafficLocker.RebaseFrom(siteIds, fromIndex, LockCount);
|
||
},
|
||
FinalizeTraffic = siteId =>
|
||
{
|
||
if (!EnableTrafficControl)
|
||
{
|
||
return;
|
||
}
|
||
|
||
_trafficLocker.FinalizeAtSite(siteId);
|
||
},
|
||
CheckControlRelease = siteId =>
|
||
{
|
||
try
|
||
{
|
||
return Fass2ControlAreaOccupancy.Check(this, siteId);
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
DetailLog($"control occupancy failed: site={siteId}, {ex.Message}");
|
||
return new Fass2ControlReleaseCheck
|
||
{
|
||
CanRelease = false,
|
||
Reason = ex.Message
|
||
};
|
||
}
|
||
}
|
||
});
|
||
}
|
||
|
||
private void TickTaskStateMachine(Fass2StateReport report)
|
||
{
|
||
if (!EnableTaskStateMachine || _taskStateMachine == null || !_taskStateMachine.IsRunning)
|
||
{
|
||
return;
|
||
}
|
||
|
||
var tick = _taskStateMachine.Tick(report);
|
||
if (!string.IsNullOrEmpty(tick.Message) &&
|
||
(tick.Dispatched || tick.Rebuilt || tick.ActionSent || tick.Advanced || tick.Completed || tick.Faulted))
|
||
{
|
||
DetailLog($"task-sm tick phase={tick.Phase}, {tick.Message}");
|
||
}
|
||
|
||
UpdateTaskStatusEnums();
|
||
}
|
||
|
||
private void TryRestoreTaskFromTags()
|
||
{
|
||
if (!EnableTaskStateMachine || _taskRestoreAttempted || tags == null)
|
||
{
|
||
return;
|
||
}
|
||
|
||
_taskRestoreAttempted = true;
|
||
if (!Fass2TaskPersistence.TryLoad(tags, out var saved))
|
||
{
|
||
return;
|
||
}
|
||
|
||
EnsureTaskStateMachine();
|
||
if (_taskStateMachine.TryRestore(saved))
|
||
{
|
||
DetailLog(
|
||
$"task restored from tags, phase={saved.Phase}, goal={saved.GoalSiteId}, index={saved.CurrentIndex}, task={saved.TaskId}");
|
||
UpdateTaskStatusEnums();
|
||
}
|
||
}
|
||
|
||
private void TryRecoverAfterReconnectIfNeeded(Fass2StateReport report)
|
||
{
|
||
var site = report?.Node != null ? ResolveSite(report.Node.Node) : null;
|
||
if (site == null)
|
||
{
|
||
if (_wasOffline || _reconnectWaitSiteId > 0)
|
||
{
|
||
DetailLog($"reconnect skip, unmapped node={report?.Node.Node}");
|
||
}
|
||
|
||
return;
|
||
}
|
||
|
||
EnsureTaskStateMachine();
|
||
var running = _taskStateMachine != null && _taskStateMachine.IsRunning;
|
||
var onRoute = running && _taskStateMachine.TryFindIndexOnRoute(site.id, out _);
|
||
var offlineRecover = _wasOffline || _reconnectWaitSiteId > 0;
|
||
var offPathRecover = running && !onRoute && _lastOffPathHandledSiteId != site.id;
|
||
|
||
if (!offlineRecover && !offPathRecover)
|
||
{
|
||
return;
|
||
}
|
||
|
||
if (_reconnectWaitSiteId > 0 && _reconnectWaitSiteId != site.id)
|
||
{
|
||
_reconnectWaitSiteId = -1;
|
||
}
|
||
|
||
_wasOffline = false;
|
||
|
||
var holding = status.holdingLocks == null || status.holdingLocks.Length == 0
|
||
? string.Empty
|
||
: string.Join(",", status.holdingLocks);
|
||
var routeText = FormatRouteSites();
|
||
DetailLog(
|
||
$"reconnect site={site.id}, onRoute={onRoute}, offPath={offPathRecover}, taskPhase={TaskPhase}, holding=[{holding}], goal={GoalSiteId?.ToString() ?? "-"}, route=[{routeText}]");
|
||
|
||
if (!running)
|
||
{
|
||
_reconnectWaitSiteId = -1;
|
||
DetailLog("reconnect idle, keep SyncTrafficFromReport");
|
||
return;
|
||
}
|
||
|
||
if (!TryClaimReportedSite(site))
|
||
{
|
||
_reconnectWaitSiteId = site.id;
|
||
DetailLog($"reconnect wait, site={site.id} held by another car");
|
||
return;
|
||
}
|
||
|
||
_reconnectWaitSiteId = -1;
|
||
|
||
try
|
||
{
|
||
if (onRoute)
|
||
{
|
||
if (_taskStateMachine.IsAtRouteEnd(site.id))
|
||
{
|
||
TrafficResetToReportedSite(site);
|
||
_taskStateMachine.TryCompleteAtSite(site.id);
|
||
DetailLog($"reconnect rebase complete at end site={site.id}");
|
||
return;
|
||
}
|
||
|
||
if (_taskStateMachine.RebaseToSite(site.id))
|
||
{
|
||
_lastSyncedTrafficSiteId = site.id;
|
||
DetailLog($"reconnect rebase site={site.id}");
|
||
}
|
||
|
||
return;
|
||
}
|
||
|
||
_lastOffPathHandledSiteId = site.id;
|
||
RecoverOffOriginalRoute(site);
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
DetailLog($"reconnect recover failed: {ex.Message}");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 上报站不在当前任务 Plan/RouteSiteIds 上：按 tasklist 归属链路重下，不再用原 goal 续跑。
|
||
/// </summary>
|
||
private void RecoverOffOriginalRoute(Site site)
|
||
{
|
||
var originalGoal = _taskStateMachine.Context.GoalSiteId > 0
|
||
? _taskStateMachine.Context.GoalSiteId
|
||
: GoalSiteId ?? 0;
|
||
|
||
var matches = Mag2LoopMission.FindChainsContaining(site.id);
|
||
if (originalGoal > 0 && matches.Count > 0)
|
||
{
|
||
matches = matches.Where(m => m != null && m.TargetSiteId != originalGoal).ToList();
|
||
}
|
||
|
||
if (Fass2ReconnectChainSelector.TrySelect(matches, preferGoalSiteId: null, out var chain, out var reason)
|
||
&& chain != null)
|
||
{
|
||
DetailLog($"reconnect off-path chain={chain}, reason={reason}, dropGoal={originalGoal}");
|
||
if (chain.IsAtEndPoint || chain.TargetSiteId == site.id)
|
||
{
|
||
HandOffToAutoLoop(site);
|
||
_taskStateMachine.TryCompleteAtSite(site.id);
|
||
DetailLog($"reconnect chain end site={site.id}, goal={chain.TargetSiteId}");
|
||
return;
|
||
}
|
||
|
||
SetGoalSiteTag(chain.TargetSiteId);
|
||
if (_taskStateMachine.RestartFromSite(site.id, chain.TargetSiteId))
|
||
{
|
||
TrafficResetToReportedSite(site);
|
||
_lastSyncedTrafficSiteId = site.id;
|
||
DetailLog($"reconnect restart start={site.id}, goal={chain.TargetSiteId}");
|
||
return;
|
||
}
|
||
}
|
||
|
||
DetailLog(
|
||
$"reconnect off-path unmatched site={site.id}, dropGoal={originalGoal}, matches={matches.Count}, {reason ?? "no chain"}");
|
||
|
||
if (!EnableLoopTaskDrive && originalGoal > 0 && originalGoal != site.id)
|
||
{
|
||
try
|
||
{
|
||
var probe = BuildTaskPlan(site.id, originalGoal);
|
||
if (probe?.SiteIds != null && probe.SiteIds.Count > 0)
|
||
{
|
||
SetGoalSiteTag(originalGoal);
|
||
if (_taskStateMachine.RestartFromSite(site.id, originalGoal))
|
||
{
|
||
TrafficResetToReportedSite(site);
|
||
_lastSyncedTrafficSiteId = site.id;
|
||
DetailLog($"reconnect restart keepGoal (non-loop) start={site.id}, goal={originalGoal}");
|
||
return;
|
||
}
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
DetailLog($"reconnect keepGoal plan failed: {ex.Message}");
|
||
}
|
||
}
|
||
|
||
DetailLog($"reconnect unmatched site={site.id}, handoff AutoLoop");
|
||
HandOffToAutoLoop(site);
|
||
if (!_taskStateMachine.IsIdle)
|
||
{
|
||
_taskStateMachine.Cancel("reconnect unmatched site");
|
||
}
|
||
}
|
||
|
||
private string FormatRouteSites()
|
||
{
|
||
var sites = _taskStateMachine?.Context?.Plan?.SiteIds;
|
||
if (sites == null || sites.Count == 0)
|
||
{
|
||
sites = _taskStateMachine?.Context?.RouteSiteIds;
|
||
}
|
||
|
||
if (sites == null || sites.Count == 0)
|
||
{
|
||
return string.Empty;
|
||
}
|
||
|
||
return string.Join(",", sites);
|
||
}
|
||
|
||
private bool TryClaimReportedSite(Site site)
|
||
{
|
||
if (site == null)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
var holder = FindOtherCarHoldingSite(site.id);
|
||
if (holder == null)
|
||
{
|
||
return true;
|
||
}
|
||
|
||
if (TryReleaseStaleHolder(holder, site.id))
|
||
{
|
||
DetailLog($"reconnect released stale holder car={holder.id}, site={site.id}");
|
||
return true;
|
||
}
|
||
|
||
return false;
|
||
}
|
||
|
||
private void TrafficResetToReportedSite(Site site)
|
||
{
|
||
if (site == null)
|
||
{
|
||
return;
|
||
}
|
||
|
||
try
|
||
{
|
||
TrafficReset(site, true, strict: false);
|
||
siteID = site.id;
|
||
_lastSyncedTrafficSiteId = site.id;
|
||
haveCoordination = true;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
DetailLog($"reconnect TrafficReset failed: site={site.id}, {ex.Message}");
|
||
}
|
||
}
|
||
|
||
private void HandOffToAutoLoop(Site site)
|
||
{
|
||
Commons.DeleteTag(tags, "occupied");
|
||
Commons.DeleteTag(tags, "goalSite");
|
||
Commons.DeleteTag(tags, "loopAssigned");
|
||
ClearTaskPersistence();
|
||
TrafficResetToReportedSite(site);
|
||
}
|
||
|
||
private void SetGoalSiteTag(int goalSiteId)
|
||
{
|
||
if (tags == null || goalSiteId <= 0)
|
||
{
|
||
return;
|
||
}
|
||
|
||
Commons.AddOrUpdateTag(tags, "goalSite", goalSiteId.ToString());
|
||
}
|
||
|
||
private void PersistTaskContext(Fass2TaskContext context)
|
||
{
|
||
if (tags == null || context == null)
|
||
{
|
||
return;
|
||
}
|
||
|
||
Fass2TaskPersistence.Save(tags, context);
|
||
UpdateTaskStatusEnums();
|
||
}
|
||
|
||
private void ClearTaskPersistence()
|
||
{
|
||
if (tags == null)
|
||
{
|
||
return;
|
||
}
|
||
|
||
Fass2TaskPersistence.Clear(tags);
|
||
UpdateTaskStatusEnums();
|
||
}
|
||
|
||
private void UpdateTaskStatusEnums()
|
||
{
|
||
if (_taskStateMachine == null)
|
||
{
|
||
status.enums["TaskPhase"] = Fass2TaskPhase.Idle.ToString();
|
||
return;
|
||
}
|
||
|
||
var context = _taskStateMachine.Context;
|
||
status.enums["TaskPhase"] = context.Phase.ToString();
|
||
status.enums["TaskGoal"] = context.GoalSiteId.ToString();
|
||
status.enums["TaskIndex"] = context.CurrentIndex.ToString();
|
||
status.enums["TaskFieldsSig"] = context.FieldsSignature ?? string.Empty;
|
||
if (!string.IsNullOrEmpty(context.FaultReason))
|
||
{
|
||
status.enums["TaskFault"] = context.FaultReason;
|
||
}
|
||
else
|
||
{
|
||
status.enums.Remove("TaskFault");
|
||
}
|
||
}
|
||
|
||
private Fass2TaskBuildOptions CreateTaskBuildOptions(double defaultSpeed = -1)
|
||
{
|
||
return new Fass2TaskBuildOptions
|
||
{
|
||
UseTagValueAsNode = UseTagValueAsNode,
|
||
DefaultSpeed = defaultSpeed > 0 ? defaultSpeed : 12,
|
||
CarSpeed = speed
|
||
};
|
||
}
|
||
|
||
public void SendAction(Fass2NodeMessage node, ulong actionId = 0)
|
||
{
|
||
var request = Fass2Protocol.BuildAction(VehicleCode, actionId, node);
|
||
DetailLog($"send action 0xA1, actionId={actionId}, node={node.Node}");
|
||
SendRequest(request, Fass2Protocol.ActionFrameLength, "Action(0xA1)");
|
||
}
|
||
|
||
private byte[] SendRequest(byte[] request, int responseLength, string commandName)
|
||
{
|
||
DetailLog($"TX {commandName}, len={request.Length}, mode={CommMode}");
|
||
if (LogRawFrame)
|
||
{
|
||
DetailLog($"TX raw {Fass2Protocol.ToHex(request)}");
|
||
}
|
||
|
||
if (CommMode == Fass2CommMode.Udp)
|
||
{
|
||
EnsureUdpSession();
|
||
Fass2UdpHub.SendToCar(this, request);
|
||
|
||
DetailLog($"UDP TX {commandName} done (async, no sync RX)");
|
||
return Array.Empty<byte>();
|
||
}
|
||
|
||
byte[] response;
|
||
lock (_syncRoot)
|
||
{
|
||
response = SendAndReceive(request, responseLength);
|
||
}
|
||
|
||
DetailLog($"RX {commandName}, bytes={response.Length}");
|
||
if (LogRawFrame)
|
||
{
|
||
DetailLog($"RX raw {Fass2Protocol.ToHex(response)}");
|
||
}
|
||
|
||
if (response.Length >= Fass2Protocol.StateFrameLength)
|
||
{
|
||
ApplyReport(Fass2Protocol.ParseState(response));
|
||
}
|
||
|
||
return response;
|
||
}
|
||
|
||
private void EnsureUdpSession()
|
||
{
|
||
if (string.IsNullOrWhiteSpace(address))
|
||
{
|
||
throw new InvalidOperationException("Mag2Car address is empty for UDP mode");
|
||
}
|
||
|
||
if (VehicleCode == 0)
|
||
{
|
||
if (!_udpZeroCodeWarned)
|
||
{
|
||
_udpZeroCodeWarned = true;
|
||
Diagnosis.Post(
|
||
$"Mag2Car {name}({id}) VehicleCode=0，UDP 未注册。请为每台车配置唯一车号",
|
||
"Mag2Car",
|
||
true);
|
||
DetailLog("UDP register skipped: VehicleCode=0");
|
||
}
|
||
|
||
return;
|
||
}
|
||
|
||
if (!_udpRegistered || _registeredVehicleCode != VehicleCode)
|
||
{
|
||
if (_udpRegistered && _registeredVehicleCode != VehicleCode)
|
||
{
|
||
Fass2UdpHub.Unregister(_registeredVehicleCode, this);
|
||
_udpRegistered = false;
|
||
}
|
||
|
||
if (!Fass2UdpHub.Register(this))
|
||
{
|
||
DetailLog($"UDP register rejected, VehicleCode={VehicleCode}");
|
||
return;
|
||
}
|
||
|
||
if (!Fass2UdpHub.IsRunning)
|
||
{
|
||
Fass2UdpHub.EnsureStarted(ListenPort);
|
||
}
|
||
else if (ListenPort > 0 && ListenPort != Fass2UdpHub.ListenPort)
|
||
{
|
||
var msg =
|
||
$"Mag2Car {name}({id}) ListenPort={ListenPort} 与已运行 Hub={Fass2UdpHub.ListenPort} 不一致，已忽略本车端口（配置错误）";
|
||
Diagnosis.Post(msg, "Mag2Car", true);
|
||
DetailLog(
|
||
$"UDP listen keep hub={Fass2UdpHub.ListenPort}, car ListenPort={ListenPort} ignored");
|
||
}
|
||
|
||
_udpRegistered = true;
|
||
_registeredVehicleCode = VehicleCode;
|
||
DetailLog($"UDP registered, VehicleCode={VehicleCode}, listen={Fass2UdpHub.ListenPort}, remote={address}:{Port}");
|
||
}
|
||
}
|
||
|
||
private byte[] SendAndReceive(byte[] request, int responseLength)
|
||
{
|
||
return UsePersistentConnection
|
||
? SendAndReceivePersistent(request, responseLength)
|
||
: SendAndReceiveShort(request, responseLength);
|
||
}
|
||
|
||
private byte[] SendAndReceivePersistent(byte[] request, int responseLength)
|
||
{
|
||
try
|
||
{
|
||
EnsurePersistentConnection();
|
||
var stream = _persistentClient.GetStream();
|
||
stream.Write(request, 0, request.Length);
|
||
stream.Flush();
|
||
return ReadExact(stream, responseLength);
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
ClosePersistentConnection($"persistent send/read failed: {ex.Message}");
|
||
throw;
|
||
}
|
||
}
|
||
|
||
private void EnsurePersistentConnection()
|
||
{
|
||
if (IsPersistentConnected())
|
||
{
|
||
return;
|
||
}
|
||
|
||
ClosePersistentConnection("prepare reconnect");
|
||
DetailLog($"persistent connect begin {address}:{Port}, timeout={ConnectTimeoutMs}ms");
|
||
var client = new TcpClient();
|
||
try
|
||
{
|
||
ConnectWithTimeout(client);
|
||
client.SendTimeout = SendTimeoutMs;
|
||
client.ReceiveTimeout = ReceiveTimeoutMs;
|
||
_persistentClient = client;
|
||
DetailLog($"persistent connect ok {address}:{Port}");
|
||
}
|
||
catch
|
||
{
|
||
try { client.Close(); } catch { }
|
||
_persistentClient = null;
|
||
throw;
|
||
}
|
||
}
|
||
|
||
private bool IsPersistentConnected()
|
||
{
|
||
try
|
||
{
|
||
return _persistentClient != null && _persistentClient.Connected;
|
||
}
|
||
catch
|
||
{
|
||
return false;
|
||
}
|
||
}
|
||
|
||
private void ClosePersistentConnection(string reason)
|
||
{
|
||
var client = _persistentClient;
|
||
if (client == null)
|
||
{
|
||
return;
|
||
}
|
||
|
||
_persistentClient = null;
|
||
DetailLog($"persistent connection closed, reason={reason}");
|
||
try
|
||
{
|
||
if (client.Connected)
|
||
{
|
||
client.Client?.Shutdown(SocketShutdown.Both);
|
||
}
|
||
}
|
||
catch (Exception ex) when (IsBenignSocketFault(ex)) { }
|
||
catch { }
|
||
|
||
try { client.Close(); }
|
||
catch (Exception ex) when (IsBenignSocketFault(ex)) { }
|
||
catch { }
|
||
}
|
||
|
||
private byte[] SendAndReceiveShort(byte[] request, int responseLength)
|
||
{
|
||
using (var client = new TcpClient())
|
||
{
|
||
DetailLog($"short connect begin {address}:{Port}, timeout={ConnectTimeoutMs}ms");
|
||
ConnectWithTimeout(client);
|
||
DetailLog($"short connect ok {address}:{Port}");
|
||
|
||
client.SendTimeout = SendTimeoutMs;
|
||
client.ReceiveTimeout = ReceiveTimeoutMs;
|
||
|
||
using (var stream = client.GetStream())
|
||
{
|
||
stream.Write(request, 0, request.Length);
|
||
stream.Flush();
|
||
return ReadExact(stream, responseLength);
|
||
}
|
||
}
|
||
}
|
||
|
||
private void ConnectWithTimeout(TcpClient client)
|
||
{
|
||
if (string.IsNullOrWhiteSpace(address))
|
||
{
|
||
throw new InvalidOperationException("Mag2Car address is empty");
|
||
}
|
||
|
||
using var cts = new CancellationTokenSource(ConnectTimeoutMs);
|
||
try
|
||
{
|
||
client.ConnectAsync(address, Port, cts.Token).GetAwaiter().GetResult();
|
||
}
|
||
catch (OperationCanceledException) when (cts.IsCancellationRequested)
|
||
{
|
||
AbortClientQuietly(client);
|
||
throw new TimeoutException($"connect {address}:{Port} timeout ({ConnectTimeoutMs}ms)");
|
||
}
|
||
catch (Exception ex) when (IsBenignSocketFault(ex) && cts.IsCancellationRequested)
|
||
{
|
||
AbortClientQuietly(client);
|
||
throw new TimeoutException($"connect {address}:{Port} timeout ({ConnectTimeoutMs}ms)", ex);
|
||
}
|
||
}
|
||
|
||
private static void AbortClientQuietly(TcpClient client)
|
||
{
|
||
if (client == null) return;
|
||
try { client.Client?.Close(); } catch (Exception ex) when (IsBenignSocketFault(ex)) { } catch { }
|
||
try { client.Close(); } catch (Exception ex) when (IsBenignSocketFault(ex)) { } catch { }
|
||
}
|
||
|
||
private static bool IsBenignSocketFault(Exception ex)
|
||
{
|
||
for (var current = ex; current != null; current = current.InnerException)
|
||
{
|
||
if (current is ObjectDisposedException) return true;
|
||
if (current is SocketException socketEx)
|
||
{
|
||
switch (socketEx.SocketErrorCode)
|
||
{
|
||
case SocketError.OperationAborted:
|
||
case SocketError.Interrupted:
|
||
case SocketError.ConnectionAborted:
|
||
case SocketError.ConnectionReset:
|
||
case SocketError.Shutdown:
|
||
return true;
|
||
}
|
||
}
|
||
if (current is IOException ioEx)
|
||
{
|
||
var message = ioEx.Message;
|
||
if (!string.IsNullOrEmpty(message) &&
|
||
(message.Contains("已中止 I/O", StringComparison.Ordinal) ||
|
||
message.Contains("I/O operation", StringComparison.OrdinalIgnoreCase) ||
|
||
message.Contains("operation was aborted", StringComparison.OrdinalIgnoreCase)))
|
||
{
|
||
return true;
|
||
}
|
||
}
|
||
}
|
||
return false;
|
||
}
|
||
|
||
private static byte[] ReadExact(NetworkStream stream, int length)
|
||
{
|
||
var buffer = new byte[length];
|
||
var offset = 0;
|
||
try
|
||
{
|
||
while (offset < length)
|
||
{
|
||
var read = stream.Read(buffer, offset, length - offset);
|
||
if (read == 0)
|
||
{
|
||
throw new IOException("remote closed before full FASS2 response");
|
||
}
|
||
offset += read;
|
||
}
|
||
return buffer;
|
||
}
|
||
catch (Exception ex) when (IsBenignSocketFault(ex))
|
||
{
|
||
throw new IOException("remote closed before full FASS2 response", ex);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 按 FASS2 调度器 <c>GetSendNodes</c> 规则构建 src→dst 路径段（含中间站与站点/边 fields）。
|
||
/// </summary>
|
||
internal Fass2NodeMessage[] BuildMoveNodes(int srcId, int dstId, double trackSpeed = -1)
|
||
{
|
||
return Fass2TaskBuilder.BuildSegment(srcId, dstId, CreateTaskBuildOptions(trackSpeed), ResolveNodeId, this);
|
||
}
|
||
|
||
private void WaitUntilArrived(ushort targetNode, int timeoutMs)
|
||
{
|
||
var deadline = DateTime.Now.AddMilliseconds(timeoutMs <= 0 ? MoveTimeoutSeconds * 1000 : timeoutMs);
|
||
while (DateTime.Now < deadline)
|
||
{
|
||
var report = RefreshState();
|
||
if (report.Node.Node == targetNode && report.State != 1)
|
||
{
|
||
DetailLog($"arrived node={targetNode}, state={Fass2Protocol.StateText(report.State)}");
|
||
return;
|
||
}
|
||
|
||
Thread.Sleep(Math.Max(50, Math.Min(PollIntervalMs, 500)));
|
||
}
|
||
|
||
throw new TimeoutException(
|
||
$"Mag2Car {name}({id}) move timeout, target node={targetNode}, current={LastReport.Node.Node}, state={Fass2Protocol.StateText(LastReport.State)}");
|
||
}
|
||
|
||
private void ApplyReport(Fass2StateReport report)
|
||
{
|
||
LastReport = report;
|
||
lstatus = report.Alarm != 0 ? "报警" : report.State switch
|
||
{
|
||
0 => "未准备",
|
||
4 => "故障",
|
||
3 => "急停",
|
||
_ => "上线"
|
||
};
|
||
th = report.HeadingAngle;
|
||
|
||
status.enums["Protocol"] = CommMode == Fass2CommMode.Udp ? "FASS2.0/UDP" : "FASS2.0/TCP";
|
||
status.enums["CurrentNode"] = report.Node.Node.ToString();
|
||
status.enums["Alarm"] = $"0x{report.Alarm:X}";
|
||
status.enums["State"] = Fass2Protocol.StateText(report.State);
|
||
status.enums["Soc"] = report.BatteryCharge.ToString();
|
||
status.enums["BatteryHealth"] = report.BatteryHealth.ToString();
|
||
status.enums["ElectricCurrent"] = report.BatteryCurrent.ToString();
|
||
status.enums["Voltage"] = report.BatteryVoltage.ToString();
|
||
status.enums["HeadingAngle"] = report.HeadingAngle.ToString();
|
||
status.enums["Task"] = report.Task.ToString();
|
||
status.enums["StartStop"] = report.Node.StartStop.ToString();
|
||
status.enums["Lift"] = report.Node.Lift.ToString();
|
||
status.enums["Roll"] = report.Node.Roll.ToString();
|
||
UpdateTaskStatusEnums();
|
||
|
||
var site = ResolveSite(report.Node.Node);
|
||
if (site == null)
|
||
{
|
||
haveCoordination = false;
|
||
DetailLog($"report applied without mapped site, node={report.Node.Node}, state={Fass2Protocol.StateText(report.State)}");
|
||
return;
|
||
}
|
||
|
||
haveCoordination = true;
|
||
ApplyMapCoordinates(site, report);
|
||
SyncTrafficFromReport(site, report);
|
||
DetailLog(
|
||
$"report applied, node={report.Node.Node}, site={siteID}, x={x:0.###}, y={y:0.###}, dist={report.Node.Distance}, state={Fass2Protocol.StateText(report.State)}, soc={report.BatteryCharge}, task={report.Task}");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 路段内按 Distance 在起止站点间插值，避免地图只在 node 跳变时才移动。
|
||
/// <para>
|
||
/// 协议行驶中 Node 一直是出发站、Distance 递增；到站才改 Node。
|
||
/// 若中途 0xB1 重下/task 重建把 Distance 清 0，旧逻辑会把车坐标打回出发站中心 → 看起来“闪回上一站”。
|
||
/// </para>
|
||
/// </summary>
|
||
private void ApplyMapCoordinates(Site site, Fass2StateReport report)
|
||
{
|
||
siteID = site.id;
|
||
|
||
var node = report.Node.Node;
|
||
var dist = report.Node.Distance;
|
||
var moving = Fass2ActionResolver.IsVehicleMoving(report.State);
|
||
|
||
// 同站行驶中 Distance 被清零：保留当前插值坐标，不要拽回站心。
|
||
if (dist <= 0 && moving &&
|
||
_lastMappedNode == node && _lastMappedDistance > 100)
|
||
{
|
||
DetailLog(
|
||
$"map coord keep (dist cleared while moving), node={node}, lastDist={_lastMappedDistance}, site={site.id}");
|
||
_lastMappedDistance = 0;
|
||
return;
|
||
}
|
||
|
||
if (dist <= 0 ||
|
||
!TryResolveMotionSegment(node, out var fromSite, out var toSite, out var segmentLenMm))
|
||
{
|
||
x = site.x;
|
||
y = site.y;
|
||
_lastMappedNode = node;
|
||
_lastMappedDistance = dist;
|
||
return;
|
||
}
|
||
|
||
var progress = Math.Min(1.0, dist / Math.Max(1.0, segmentLenMm));
|
||
var dx = toSite.x - fromSite.x;
|
||
var dy = toSite.y - fromSite.y;
|
||
var mapLen = Math.Sqrt(dx * dx + dy * dy);
|
||
|
||
if (mapLen >= 1.0)
|
||
{
|
||
x = (float)(fromSite.x + dx * progress);
|
||
y = (float)(fromSite.y + dy * progress);
|
||
_lastMappedNode = node;
|
||
_lastMappedDistance = dist;
|
||
return;
|
||
}
|
||
|
||
var track = FindTrackBetween(fromSite.id, toSite.id);
|
||
if (track != null)
|
||
{
|
||
var trackDx = toSite.x - fromSite.x;
|
||
var trackDy = toSite.y - fromSite.y;
|
||
if (Math.Abs(trackDx) + Math.Abs(trackDy) >= 1.0)
|
||
{
|
||
x = (float)(fromSite.x + trackDx * progress);
|
||
y = (float)(fromSite.y + trackDy * progress);
|
||
_lastMappedNode = node;
|
||
_lastMappedDistance = dist;
|
||
return;
|
||
}
|
||
}
|
||
|
||
// 地图起止站点坐标重合时，用协议距离沿 X 轴做最小可视化偏移。
|
||
x = (float)(fromSite.x + dist);
|
||
y = fromSite.y;
|
||
_lastMappedNode = node;
|
||
_lastMappedDistance = dist;
|
||
}
|
||
|
||
private bool TryResolveMotionSegment(ushort currentNode, out Site fromSite, out Site toSite, out double segmentLenMm)
|
||
{
|
||
fromSite = null;
|
||
toSite = null;
|
||
segmentLenMm = 0;
|
||
|
||
fromSite = ResolveSite(currentNode);
|
||
if (fromSite == null)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
var plan = _taskStateMachine?.Context?.Plan;
|
||
if (plan?.Nodes == null || plan.Nodes.Count < 2)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
for (var i = 0; i < plan.Nodes.Count - 1; i++)
|
||
{
|
||
if (plan.Nodes[i].Node != currentNode)
|
||
{
|
||
continue;
|
||
}
|
||
|
||
var targetNode = plan.Nodes[i + 1].Node;
|
||
toSite = ResolveSite(targetNode);
|
||
if (toSite == null)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
// Distance 挂在出发站节点上（边长），不是下一站。
|
||
segmentLenMm = plan.Nodes[i].Distance;
|
||
if (segmentLenMm <= 0)
|
||
{
|
||
var dx = toSite.x - fromSite.x;
|
||
var dy = toSite.y - fromSite.y;
|
||
segmentLenMm = Math.Max(1.0, Math.Sqrt(dx * dx + dy * dy));
|
||
}
|
||
|
||
return true;
|
||
}
|
||
|
||
return false;
|
||
}
|
||
|
||
private static Track FindTrackBetween(int fromSiteId, int toSiteId)
|
||
{
|
||
foreach (var track in SimpleLib.GetAllTracks())
|
||
{
|
||
if (track.direction == 0)
|
||
{
|
||
if ((track.siteA == fromSiteId && track.siteB == toSiteId) ||
|
||
(track.siteB == fromSiteId && track.siteA == toSiteId))
|
||
{
|
||
return track;
|
||
}
|
||
|
||
continue;
|
||
}
|
||
|
||
if (track.direction == 1 && track.siteA == fromSiteId && track.siteB == toSiteId)
|
||
{
|
||
return track;
|
||
}
|
||
|
||
if (track.direction == 2 && track.siteB == fromSiteId && track.siteA == toSiteId)
|
||
{
|
||
return track;
|
||
}
|
||
}
|
||
|
||
return null;
|
||
}
|
||
|
||
private void SyncUdpOnlineTags(bool isOnline)
|
||
{
|
||
if (CommMode != Fass2CommMode.Udp)
|
||
{
|
||
return;
|
||
}
|
||
|
||
if (isOnline)
|
||
{
|
||
Commons.AddOrUpdateTag(tags, "Online", "true");
|
||
Commons.DeleteTag(tags, "agvOffline");
|
||
return;
|
||
}
|
||
|
||
Commons.DeleteTag(tags, "Online");
|
||
}
|
||
|
||
private void SyncTrafficFromReport(Site site, Fass2StateReport report)
|
||
{
|
||
if (site == null)
|
||
{
|
||
return;
|
||
}
|
||
|
||
var taskRunning = _running || (_taskStateMachine?.IsRunning ?? false);
|
||
if (taskRunning)
|
||
{
|
||
var lockedSite = status.holdingLocks.Length > 0 ? status.holdingLocks[0] : -1;
|
||
if (lockedSite == site.id)
|
||
{
|
||
_lastSyncedTrafficSiteId = site.id;
|
||
}
|
||
else
|
||
{
|
||
DetailLog(
|
||
$"traffic sync skipped during task, lock={lockedSite}, reportSite={site.id}, node={report.Node.Node}");
|
||
}
|
||
|
||
return;
|
||
}
|
||
|
||
var idleLockedSite = status.holdingLocks.Length > 0 ? status.holdingLocks[0] : -1;
|
||
if (idleLockedSite == site.id)
|
||
{
|
||
_lastSyncedTrafficSiteId = site.id;
|
||
return;
|
||
}
|
||
|
||
if (_lastSyncedTrafficSiteId == site.id && idleLockedSite == site.id)
|
||
{
|
||
return;
|
||
}
|
||
|
||
// 双车叠在同一站时，后到车不能 TrafficReset 抢锁；离线/错站的陈旧占锁则回收后重占。
|
||
var holder = FindOtherCarHoldingSite(site.id);
|
||
if (holder != null)
|
||
{
|
||
if (TryReleaseStaleHolder(holder, site.id))
|
||
{
|
||
DetailLog(
|
||
$"traffic sync released stale holder car={holder.id}, site={site.id}, node={report.Node.Node}");
|
||
}
|
||
else
|
||
{
|
||
if (_lastSyncedTrafficSiteId != -site.id)
|
||
{
|
||
DetailLog(
|
||
$"traffic sync skip, site={site.id} held by car={holder.id}, selfLock={idleLockedSite}, node={report.Node.Node}");
|
||
_lastSyncedTrafficSiteId = -site.id;
|
||
}
|
||
|
||
return;
|
||
}
|
||
}
|
||
|
||
try
|
||
{
|
||
DetailLog($"traffic sync TrafficReset, lock={idleLockedSite}, targetSite={site.id}, node={report.Node.Node}");
|
||
TrafficReset(site, true, strict: false);
|
||
_lastSyncedTrafficSiteId = site.id;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
DetailLog($"traffic sync failed: {ex.Message}");
|
||
}
|
||
}
|
||
|
||
private Car FindOtherCarHoldingSite(int siteId)
|
||
{
|
||
foreach (var other in SimpleLib.GetAllCars().OfType<Car>())
|
||
{
|
||
if (other == null || other.id == id)
|
||
{
|
||
continue;
|
||
}
|
||
|
||
if (other.status?.holdingLocks != null &&
|
||
Array.IndexOf(other.status.holdingLocks, siteId) >= 0)
|
||
{
|
||
return other;
|
||
}
|
||
}
|
||
|
||
return null;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 他车离线，或上报站点已离开，却仍占着本站锁 → 视为陈旧占锁，允许回收。
|
||
/// </summary>
|
||
private bool TryReleaseStaleHolder(Car other, int siteId)
|
||
{
|
||
if (other == null)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
var online = other.tags?.Contains("Online") == true;
|
||
var physicalSiteId = other is Mag2Car mag2 && mag2.siteID > 0
|
||
? mag2.siteID
|
||
: other.GetLastSite();
|
||
|
||
var stale = !online || (physicalSiteId > 0 && physicalSiteId != siteId);
|
||
if (!stale)
|
||
{
|
||
return false;
|
||
}
|
||
|
||
try
|
||
{
|
||
TrafficControl.Leave(other, siteId);
|
||
return Array.IndexOf(other.status.holdingLocks ?? Array.Empty<int>(), siteId) < 0;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
DetailLog($"traffic sync release stale holder failed: car={other.id}, site={siteId}, {ex.Message}");
|
||
return false;
|
||
}
|
||
}
|
||
|
||
private Site ResolveSite(ushort node)
|
||
{
|
||
if (node == 0)
|
||
{
|
||
return null;
|
||
}
|
||
|
||
if (UseTagValueAsNode)
|
||
{
|
||
var byTagValue = SimpleLib.GetAllSites().FirstOrDefault(site =>
|
||
site.fields.TryGetValue("TagValue", out var tag) &&
|
||
int.TryParse(tag, out var tagValue) &&
|
||
tagValue == node);
|
||
if (byTagValue != null)
|
||
{
|
||
return byTagValue;
|
||
}
|
||
}
|
||
|
||
return SimpleLib.GetAllSites().FirstOrDefault(site => site.id == node);
|
||
}
|
||
|
||
internal ushort ResolveNodeId(int siteId)
|
||
{
|
||
var site = SimpleLib.GetSite(siteId);
|
||
if (UseTagValueAsNode &&
|
||
site.fields.TryGetValue("TagValue", out var tag) &&
|
||
ushort.TryParse(tag, out var tagValue))
|
||
{
|
||
return tagValue;
|
||
}
|
||
|
||
return (ushort)Clamp(siteId, 0, ushort.MaxValue);
|
||
}
|
||
|
||
private static int Clamp(int value, int min, int max)
|
||
{
|
||
if (value < min) return min;
|
||
return value > max ? max : value;
|
||
}
|
||
|
||
private void DetailLog(string message)
|
||
{
|
||
WriteFileLog(message);
|
||
|
||
if (!EnableDetailLog)
|
||
{
|
||
return;
|
||
}
|
||
|
||
var text = $"[Mag2Car:{name}({id})] {DateTime.Now:HH:mm:ss.fff} {message}";
|
||
AppendDebug(text);
|
||
Diagnosis.Post(text, "Mag2Car", true);
|
||
}
|
||
|
||
private void WriteFileLog(string message)
|
||
{
|
||
if (!EnableFileLog)
|
||
{
|
||
return;
|
||
}
|
||
|
||
var logVehicleCode = ResolveLogVehicleCode();
|
||
if (logVehicleCode == 0)
|
||
{
|
||
return;
|
||
}
|
||
|
||
Mag2CarFileLogger.Configure(LogDirectory, true);
|
||
Mag2CarFileLogger.Write(logVehicleCode, message);
|
||
}
|
||
|
||
private ushort ResolveLogVehicleCode()
|
||
{
|
||
if (VehicleCode != 0)
|
||
{
|
||
return VehicleCode;
|
||
}
|
||
|
||
if (id <= 0 || id > ushort.MaxValue)
|
||
{
|
||
return 0;
|
||
}
|
||
|
||
return (ushort)id;
|
||
}
|
||
|
||
private void TryLogLoopAssignmentToFile()
|
||
{
|
||
if (!EnableFileLog || !TryGetGoalSiteId(out var goalSiteId))
|
||
{
|
||
_lastFileLoggedGoalSite = -1;
|
||
return;
|
||
}
|
||
|
||
if (_lastFileLoggedGoalSite == goalSiteId)
|
||
{
|
||
return;
|
||
}
|
||
|
||
_lastFileLoggedGoalSite = goalSiteId;
|
||
var startSite = GetLastSite() > 0 ? GetLastSite() : siteID;
|
||
WriteFileLog($"AutoLoop: 车辆 {name} 从站点 {startSite} 自动分配到目标站点 {goalSiteId}");
|
||
}
|
||
|
||
private static string CommandName(byte command)
|
||
{
|
||
switch (command)
|
||
{
|
||
case Fass2Protocol.CmdQuery: return "Query(0x00)";
|
||
case Fass2Protocol.CmdStart: return "Start(0x01)";
|
||
case Fass2Protocol.CmdStop: return "Stop(0x02)";
|
||
case Fass2Protocol.CmdEmergencyStop: return "EmergencyStop(0x03)";
|
||
case Fass2Protocol.CmdReset: return "Reset(0x04)";
|
||
case Fass2Protocol.CmdRest: return "Rest(0x05)";
|
||
case Fass2Protocol.CmdShutdown: return "Shutdown(0x06)";
|
||
default: return $"Unknown(0x{command:X2})";
|
||
}
|
||
}
|
||
|
||
internal sealed class Mag2CarInterface : AGVInterface
|
||
{
|
||
private readonly Mag2Car _car;
|
||
|
||
public Mag2CarInterface(int id)
|
||
{
|
||
_car = (Mag2Car)SimpleLib.GetCar(id);
|
||
}
|
||
|
||
public override bool TryLock(int siteId)
|
||
{
|
||
if (!_car.status.usage.Get().scheduling)
|
||
{
|
||
throw new Exception("abandoned");
|
||
}
|
||
|
||
if (_car.status.pendingLocks.Length > 0 && _car.status.pendingLocks[0] != siteId)
|
||
{
|
||
throw new Exception("lock not according to sequence");
|
||
}
|
||
|
||
return TrafficControl.TryLock(_car, siteId);
|
||
}
|
||
|
||
public override void Leave(int siteId)
|
||
{
|
||
TrafficControl.Leave(_car, siteId);
|
||
}
|
||
|
||
public void Mag2Go(int srcId, int dstId, double speed = -1)
|
||
{
|
||
var promise = new TaskCompletionSource<int>();
|
||
Queue(async () =>
|
||
{
|
||
_car.DetailLog($"Mag2Go src={srcId}, dst={dstId}, speed={speed}, traffic={_car.EnableTrafficControl}");
|
||
|
||
if (_car.EnableTaskStateMachine)
|
||
{
|
||
_car.BeginTrackedTask(srcId, dstId, speed);
|
||
await _car.WaitTrackedTaskAsync(_car.MoveTimeoutSeconds * 1000);
|
||
}
|
||
else
|
||
{
|
||
if (_car.StartBeforeMove)
|
||
{
|
||
_car.SendControl(Fass2Protocol.CmdStart, 0);
|
||
}
|
||
|
||
var plan = _car.BuildTaskPlan(srcId, dstId, speed);
|
||
_car.DetailLog(
|
||
$"Mag2Go plan sites={plan.SiteIds.Count}, nodes={plan.Nodes.Count}, batches={plan.Batches.Count}");
|
||
_car.SendTaskPlan(plan);
|
||
_car.WaitUntilArrived(_car.ResolveNodeId(dstId), _car.MoveTimeoutSeconds * 1000);
|
||
}
|
||
|
||
promise.SetResult(1);
|
||
}, async () =>
|
||
{
|
||
await promise.Task;
|
||
});
|
||
}
|
||
}
|
||
}
|
||
}
|