1898 lines
66 KiB
C#
1898 lines
66 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.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 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 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.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 (!_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("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;
|
|
});
|
|
}
|
|
}
|
|
}
|
|
}
|