1472 lines
51 KiB
C#
1472 lines
51 KiB
C#
using LessokajiWeaverUtilities.Utilities;
|
||
using SimpleCore;
|
||
using SimpleCore.Compiler;
|
||
using SimpleCore.BasicProps;
|
||
using SimpleCore.Library;
|
||
using SimpleCore.PropType;
|
||
using SimpleCore.Traffic;
|
||
using SimpleLite.Props;
|
||
using SimpleLite.RCS;
|
||
using SimpleLite.RCS.CarTypes;
|
||
using SimpleLite.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.MagFass2Go(${src.id},${dst.id},${track.Speed});",
|
||
trackFields = typeof(BasicTrackFields))]
|
||
[CarType(Name = "MagFass2Car")]
|
||
[I18N.DocumentTranslation(Name = "MagFass2Car", locale = "en")]
|
||
[EnvelopConfig(lengthX = 1200, lengthY = 800, centerX = 0, centerY = 0)]
|
||
public class MagFass2Car : 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 ushort _registeredVehicleCode = ushort.MaxValue;
|
||
private long _nextTaskId = 1;
|
||
private long _nextActionId = 1;
|
||
private Fass2TaskStateMachine _taskStateMachine;
|
||
private bool _taskRestoreAttempted;
|
||
|
||
[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 int TaskResendIntervalMs = 500;
|
||
[FieldMember] public int ActionRetryIntervalMs = 1000;
|
||
[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);
|
||
SyncUdpOnlineTags(true);
|
||
}
|
||
|
||
TickTaskStateMachine(report);
|
||
}
|
||
|
||
public new static async Task<MagFass2Car> Create()
|
||
{
|
||
var car = new MagFass2Car
|
||
{
|
||
lstatus = "连接中",
|
||
address = "127.0.0.1",
|
||
name = "MagFass2Car",
|
||
haveCoordination = true,
|
||
speed = 1
|
||
};
|
||
car.EnsureTaskStateMachine();
|
||
MagFass2CarFileLogger.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 = "离线";
|
||
SyncUdpOnlineTags(false);
|
||
DetailLog($"keepAlive udp waiting, listen={ListenPort}, remote={address}:{Port}");
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
lstatus = "离线";
|
||
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))
|
||
{
|
||
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($"MagFass2Car {id} already running script");
|
||
}
|
||
|
||
if (ShouldDriveLoopGoalAsTrackedTask(script))
|
||
{
|
||
_running = true;
|
||
try
|
||
{
|
||
DetailLog($"loop goal drive intercept, goal={GoalSiteId}, scriptLen={(script ?? string.Empty).Length}");
|
||
PrepareForLoopGoalDrive();
|
||
await ExecuteGoalTaskAsync(skipGoalLock: true);
|
||
DetailLog("loop goal drive completed");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
DetailLog($"loop goal drive failed: {ExceptionFormatter.FormatEx(ex)}");
|
||
throw;
|
||
}
|
||
finally
|
||
{
|
||
_running = false;
|
||
}
|
||
|
||
return;
|
||
}
|
||
|
||
_running = true;
|
||
try
|
||
{
|
||
DetailLog($"script begin, length={(script ?? string.Empty).Length}");
|
||
var agv = new MagFass2CarInterface(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 StartMagFass2Car()
|
||
{
|
||
SendControl(Fass2Protocol.CmdStart, (ushort)Math.Round(th));
|
||
}
|
||
|
||
[MethodMember(Name = "停止", Description = "发送停止指令(0x02)")]
|
||
public void StopMagFass2Car()
|
||
{
|
||
SendControl(Fass2Protocol.CmdStop, 0);
|
||
}
|
||
|
||
[MethodMember(Name = "急停", Description = "发送急停指令(0x03)")]
|
||
public void EmergencyStopMagFass2Car()
|
||
{
|
||
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}:MagFass2Car EmergencyStop {reason}");
|
||
SendControl(Fass2Protocol.CmdEmergencyStop, 0);
|
||
}
|
||
|
||
public void EmergencyRelease()
|
||
{
|
||
Diagnosis.Post($"car{name}:MagFass2Car EmergencyRelease");
|
||
SendControl(Fass2Protocol.CmdStart, (ushort)Math.Round(th));
|
||
}
|
||
|
||
[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;
|
||
|
||
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);
|
||
}
|
||
|
||
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(
|
||
$"MagFass2Car {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);
|
||
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);
|
||
}
|
||
|
||
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.StartBeforeMove = StartBeforeMove;
|
||
_taskStateMachine.HeadingAngle = (ushort)Math.Round(th);
|
||
_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, skipGoalLock: false);
|
||
}
|
||
|
||
public async Task ExecuteGoalTaskAsync(int? goalSiteId = null, double defaultSpeed = -1,
|
||
CancellationToken cancellationToken = default, bool skipGoalLock = false)
|
||
{
|
||
if (!EnableTaskStateMachine)
|
||
{
|
||
throw new InvalidOperationException("ExecuteGoalTaskAsync requires EnableTaskStateMachine=true");
|
||
}
|
||
|
||
var goalId = goalSiteId ?? GoalSiteId;
|
||
if (goalId == null || goalId.Value <= 0)
|
||
{
|
||
throw new InvalidOperationException("MagFass2Car goalSite is not set");
|
||
}
|
||
|
||
var startId = ResolvePhysicalSiteId();
|
||
if (startId <= 0)
|
||
{
|
||
throw new InvalidOperationException($"MagFass2Car {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}, skipGoalLock={skipGoalLock}");
|
||
|
||
var goalLocked = false;
|
||
if (!skipGoalLock)
|
||
{
|
||
while (!TrafficControl.TryLock(this, goalId.Value))
|
||
{
|
||
cancellationToken.ThrowIfCancellationRequested();
|
||
await Task.Delay(50, cancellationToken);
|
||
}
|
||
|
||
goalLocked = true;
|
||
}
|
||
|
||
try
|
||
{
|
||
BeginTrackedTask(startId, goalId.Value, defaultSpeed);
|
||
await WaitTrackedTaskAsync(MoveTimeoutSeconds * 1000);
|
||
TaskCompleted?.Invoke(goalId.Value);
|
||
}
|
||
finally
|
||
{
|
||
if (goalLocked)
|
||
{
|
||
TrafficControl.Leave(this, goalId.Value);
|
||
}
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// Loop GoSite 在 Compile 前会通过 FindRoute 写入 seqScope/pendingLocks；
|
||
/// 全程任务改由 FASS2 状态机推进，需先清掉这段交管残留，避免 TryLock 与顺序锁冲突。
|
||
/// </summary>
|
||
private void PrepareForLoopGoalDrive()
|
||
{
|
||
var startSiteId = ResolvePhysicalSiteId();
|
||
var site = startSiteId > 0 ? SimpleLib.GetSite(startSiteId) : null;
|
||
|
||
DetailLog(
|
||
$"loop goal prepare before reset, node={LastReport.Node.Node}, start={startSiteId}, holding=[{string.Join(",", status.holdingLocks)}], pending=[{string.Join(",", status.pendingLocks)}]");
|
||
|
||
if (site != null)
|
||
{
|
||
TrafficReset(site, true, strict: false);
|
||
siteID = site.id;
|
||
_lastSyncedTrafficSiteId = site.id;
|
||
}
|
||
|
||
DetailLog(
|
||
$"loop goal prepare after reset, start={startSiteId}, holding=[{string.Join(",", status.holdingLocks)}], pending=[{string.Join(",", status.pendingLocks)}]");
|
||
}
|
||
|
||
/// <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("MagFass2Go", 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;
|
||
}
|
||
|
||
_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}")
|
||
});
|
||
}
|
||
|
||
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 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 : 0.2,
|
||
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("MagFass2Car address is empty for UDP mode");
|
||
}
|
||
|
||
if (!_udpRegistered || _registeredVehicleCode != VehicleCode)
|
||
{
|
||
if (_udpRegistered && _registeredVehicleCode != VehicleCode)
|
||
{
|
||
Fass2UdpHub.Unregister(_registeredVehicleCode, this);
|
||
_udpRegistered = false;
|
||
}
|
||
|
||
Fass2UdpHub.Register(this);
|
||
Fass2UdpHub.EnsureStarted(ListenPort);
|
||
_udpRegistered = true;
|
||
_registeredVehicleCode = VehicleCode;
|
||
DetailLog($"UDP registered, VehicleCode={VehicleCode}, listen={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("MagFass2Car 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);
|
||
}
|
||
|
||
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(
|
||
$"MagFass2Car {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 跳变时才移动。
|
||
/// </summary>
|
||
private void ApplyMapCoordinates(Site site, Fass2StateReport report)
|
||
{
|
||
siteID = site.id;
|
||
x = site.x;
|
||
y = site.y;
|
||
|
||
if (report.Node.Distance <= 0 ||
|
||
!TryResolveMotionSegment(report.Node.Node, out var fromSite, out var toSite, out var segmentLenMm))
|
||
{
|
||
return;
|
||
}
|
||
|
||
var progress = Math.Min(1.0, report.Node.Distance / 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);
|
||
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);
|
||
return;
|
||
}
|
||
}
|
||
|
||
// 地图起止站点坐标重合时，用协议距离沿 X 轴做最小可视化偏移。
|
||
x = (float)(fromSite.x + report.Node.Distance);
|
||
y = fromSite.y;
|
||
}
|
||
|
||
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;
|
||
}
|
||
|
||
segmentLenMm = plan.Nodes[i + 1].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 lockedSite = status.holdingLocks.Length > 0 ? status.holdingLocks[0] : -1;
|
||
if (_running)
|
||
{
|
||
if (lockedSite == site.id)
|
||
{
|
||
_lastSyncedTrafficSiteId = site.id;
|
||
return;
|
||
}
|
||
|
||
try
|
||
{
|
||
DetailLog(
|
||
$"traffic sync during task, lock={lockedSite}, targetSite={site.id}, node={report.Node.Node}");
|
||
TrafficReset(site, true, strict: false);
|
||
_lastSyncedTrafficSiteId = site.id;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
DetailLog($"traffic sync during task failed: {ex.Message}");
|
||
}
|
||
|
||
return;
|
||
}
|
||
|
||
if (lockedSite == site.id)
|
||
{
|
||
_lastSyncedTrafficSiteId = site.id;
|
||
return;
|
||
}
|
||
|
||
if (_lastSyncedTrafficSiteId == site.id && lockedSite == site.id)
|
||
{
|
||
return;
|
||
}
|
||
|
||
try
|
||
{
|
||
DetailLog($"traffic sync TrafficReset, lock={lockedSite}, 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 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 = $"[MagFass2Car:{name}({id})] {DateTime.Now:HH:mm:ss.fff} {message}";
|
||
AppendDebug(text);
|
||
Diagnosis.Post(text, "MagFass2Car", true);
|
||
}
|
||
|
||
private void WriteFileLog(string message)
|
||
{
|
||
if (!EnableFileLog)
|
||
{
|
||
return;
|
||
}
|
||
|
||
var logVehicleCode = ResolveLogVehicleCode();
|
||
if (logVehicleCode == 0)
|
||
{
|
||
return;
|
||
}
|
||
|
||
MagFass2CarFileLogger.Configure(LogDirectory, true);
|
||
MagFass2CarFileLogger.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 MagFass2CarInterface : AGVInterface
|
||
{
|
||
private readonly MagFass2Car _car;
|
||
|
||
public MagFass2CarInterface(int id)
|
||
{
|
||
_car = (MagFass2Car)SimpleLib.GetCar(id);
|
||
}
|
||
|
||
public override bool TryLock(int siteId)
|
||
{
|
||
if (!_car.status.usage.Get().scheduling)
|
||
{
|
||
throw new Exception("abandoned");
|
||
}
|
||
|
||
return TrafficControl.TryLock(_car, siteId);
|
||
}
|
||
|
||
public override void Leave(int siteId)
|
||
{
|
||
TrafficControl.Leave(_car, siteId);
|
||
}
|
||
|
||
public void MagFass2Go(int srcId, int dstId, double speed = -1)
|
||
{
|
||
var promise = new TaskCompletionSource<int>();
|
||
Queue(async () =>
|
||
{
|
||
while (!TryLock(dstId))
|
||
{
|
||
await Task.Delay(50);
|
||
}
|
||
|
||
var dstNode = _car.ResolveNodeId(dstId);
|
||
_car.DetailLog($"MagFass2Go src={srcId}, dst={dstId}, dstNode={dstNode}, speed={speed}");
|
||
|
||
if (_car.EnableTaskStateMachine)
|
||
{
|
||
_car.BeginTrackedTask(srcId, dstId, speed);
|
||
await _car.WaitTrackedTaskAsync(_car.MoveTimeoutSeconds * 1000);
|
||
}
|
||
else
|
||
{
|
||
if (_car.StartBeforeMove)
|
||
{
|
||
_car.SendControl(Fass2Protocol.CmdStart, (ushort)Math.Round(_car.th));
|
||
}
|
||
|
||
var plan = _car.BuildTaskPlan(srcId, dstId, speed);
|
||
_car.DetailLog(
|
||
$"MagFass2Go plan sites={plan.SiteIds.Count}, nodes={plan.Nodes.Count}, batches={plan.Batches.Count}");
|
||
_car.SendTaskPlan(plan);
|
||
_car.WaitUntilArrived(dstNode, _car.MoveTimeoutSeconds * 1000);
|
||
}
|
||
|
||
promise.SetResult(1);
|
||
}, async () =>
|
||
{
|
||
await promise.Task;
|
||
Leave(srcId);
|
||
});
|
||
}
|
||
}
|
||
}
|
||
}
|