1424 lines
54 KiB
C#
1424 lines
54 KiB
C#
using LessokajiWeaverUtilities.Utilities;
|
||
using Simple3;
|
||
using Simple3.RCS;
|
||
using Simple3.RCS.CarTypes;
|
||
using Simple3.Props;
|
||
using Simple3.UI;
|
||
using SimpleCore;
|
||
using SimpleCore.Compiler;
|
||
using SimpleCore.Library;
|
||
using SimpleCore.PropType;
|
||
using SimpleCore.Traffic;
|
||
using System;
|
||
using System.Collections.Generic;
|
||
using System.Diagnostics;
|
||
using System.Drawing;
|
||
using System.Drawing.Drawing2D;
|
||
using System.Globalization;
|
||
using System.IO;
|
||
using System.Linq;
|
||
using System.Net.Sockets;
|
||
using System.Threading;
|
||
using System.Threading.Tasks;
|
||
using SimpleCore.BasicProps;
|
||
|
||
namespace StandardScene.CarTypes
|
||
{
|
||
/// <summary>
|
||
/// 基于 FASS 1.0 小车 TCP 协议实现的磁导航小车类型。
|
||
/// 该类型直接按 32 字节协议帧与现场 AGV 服务端通讯，通过 <see cref="MagCarInterface"/> 执行调度脚本。
|
||
/// 默认使用长连接发送命令和读取状态；现场服务端不支持长连接时，可关闭 <see cref="UsePersistentConnection"/> 回退到短连接兼容模式。
|
||
/// </summary>
|
||
[TemplateTrackCoderSettings(
|
||
priority = 0,
|
||
templateString = "agv.MagCarGo(${src.id},${dst.id},${track.Speed});",
|
||
trackFields = typeof(BasicTrackFields))]
|
||
[CarType(Name = "MagCar")]
|
||
[I18N.DocumentTranslation(Name = "MagCar", locale = "en")]
|
||
[EnvelopConfig(lengthX = 1200, lengthY = 800, centerX = 0, centerY = 0)]
|
||
public class MagCar : GhostCar
|
||
{
|
||
/// <summary>
|
||
/// FASS 1.0 协议帧起始字节，所有发送和接收帧第 0 字节均应为 0xBB。
|
||
/// </summary>
|
||
private const byte Begin = 0xBB;
|
||
|
||
/// <summary>
|
||
/// FASS 1.0 协议帧结束字节，所有发送和接收帧第 31 字节均应为 0xEE。
|
||
/// </summary>
|
||
private const byte End = 0xEE;
|
||
|
||
/// <summary>
|
||
/// 查询 AGV 当前状态的命令码，响应中会带回节点、报警、状态、电量、速度、角度等信息。
|
||
/// </summary>
|
||
private const byte CmdQueryState = 0x00;
|
||
|
||
/// <summary>
|
||
/// 启动或解除停止状态的命令码，数据区通常携带当前车头角度。
|
||
/// </summary>
|
||
private const byte CmdStart = 0x01;
|
||
|
||
/// <summary>
|
||
/// 停止命令码，数据区可携带停止保持秒数；当前实现通常传 0 表示立即停止。
|
||
/// </summary>
|
||
private const byte CmdStop = 0x02;
|
||
|
||
/// <summary>
|
||
/// 设置速度百分比的命令码，数据区第一个字节为 0-100 的速度百分比。
|
||
/// </summary>
|
||
private const byte CmdSpeed = 0x03;
|
||
|
||
/// <summary>
|
||
/// 设置车头角度的命令码，数据区携带角度值。
|
||
/// </summary>
|
||
private const byte CmdAngle = 0x04;
|
||
|
||
/// <summary>
|
||
/// 旧版节点下发命令码，只下发目标节点，用于兼容不支持 0xA1 任务命令的现场服务端。
|
||
/// </summary>
|
||
private const byte CmdLegacyNode = 0x05;
|
||
|
||
/// <summary>
|
||
/// FASS 1.0 任务下发命令码，数据区携带起点节点、终点节点和任务动作。
|
||
/// </summary>
|
||
private const byte CmdTask = 0xA1;
|
||
|
||
/// <summary>
|
||
/// TCP 发送、接收和长连接重连的同步锁。
|
||
/// 多线程调度、keepAlive 和人工方法可能同时触发通讯，统一加锁可避免多个线程同时写同一个 Socket。
|
||
/// </summary>
|
||
private readonly object _syncRoot = new object();
|
||
|
||
/// <summary>
|
||
/// 长连接模式下复用的 TCP 客户端。
|
||
/// 发送或读取失败时会关闭并置空，下一次命令会自动重新连接。
|
||
/// </summary>
|
||
private TcpClient _persistentClient;
|
||
|
||
/// <summary>
|
||
/// 最近一次 keepAlive 实际查询小车状态的时间。
|
||
/// 外部框架可能高频调用 keepAlive，本字段用于按 <see cref="PollIntervalMs"/> 节流实际 TCP 查询频率。
|
||
/// </summary>
|
||
private DateTime _lastPollTime = DateTime.MinValue;
|
||
|
||
/// <summary>
|
||
/// AGV 服务端监听端口，默认 5000。
|
||
/// </summary>
|
||
[FieldMember] public int Port = 5000;
|
||
|
||
/// <summary>
|
||
/// 协议中的车辆编号，写入发送帧第 2-3 字节，使用大端字节序。
|
||
/// 多车共用同一服务端时用于区分车辆。
|
||
/// </summary>
|
||
[FieldMember] public ushort VehicleCode = 0;
|
||
|
||
/// <summary>
|
||
/// TCP 建连超时时间，单位毫秒。
|
||
/// 长连接首次连接或重连、短连接每次命令连接时都会使用该值。
|
||
/// </summary>
|
||
[FieldMember] public int ConnectTimeoutMs = 500;
|
||
|
||
/// <summary>
|
||
/// TCP 发送超时时间，单位毫秒。
|
||
/// </summary>
|
||
[FieldMember] public int SendTimeoutMs = 500;
|
||
|
||
/// <summary>
|
||
/// TCP 接收超时时间，单位毫秒。
|
||
/// 读取响应时会一直读满 32 字节，超过该时间仍未收到完整帧则抛出异常。
|
||
/// </summary>
|
||
[FieldMember] public int ReceiveTimeoutMs = 500;
|
||
|
||
/// <summary>
|
||
/// keepAlive 中实际查询 AGV 状态的最小间隔，单位毫秒。
|
||
/// 默认 1000，表示最多每秒向小车查询一次状态。
|
||
/// </summary>
|
||
[FieldMember] public int PollIntervalMs = 1000;
|
||
|
||
/// <summary>
|
||
/// 移动任务的预留超时时间，单位秒。
|
||
/// 当前 MagCar 不下发脚本，保留该字段用于后续任务调度或界面配置兼容。
|
||
/// </summary>
|
||
[FieldMember] public int MoveTimeoutSeconds = 120;
|
||
|
||
/// <summary>
|
||
/// 是否使用 0xA1 任务命令下发起点和终点。
|
||
/// 为 false 时改用 0x05 旧版目标节点命令，适配早期或定制 AGV 服务端。
|
||
/// </summary>
|
||
[FieldMember] public bool UseTaskCommand = true;
|
||
|
||
/// <summary>
|
||
/// 移动前是否先发送启动命令。
|
||
/// 当前文件保留该配置以兼容调度策略，具体移动流程可在后续路径执行逻辑中使用。
|
||
/// </summary>
|
||
[FieldMember] public bool StartBeforeMove = true;
|
||
|
||
/// <summary>
|
||
/// 是否按接口文档使用 ushort 大端字段写入角度、节点、停止秒数等数据。
|
||
/// 为 false 时只写入低 1 字节，用于兼容旧版 0x05 单字节节点协议。
|
||
/// </summary>
|
||
[FieldMember] public bool UseDocumentUShortFields = true;
|
||
|
||
/// <summary>
|
||
/// 是否使用站点字段 TagValue 作为 AGV 节点号。
|
||
/// 为 true 时，收到的 Node 会先匹配站点 fields["TagValue"]；为 false 时直接用站点 id 匹配。
|
||
/// </summary>
|
||
[FieldMember] public bool UseTagValueAsNode = false;
|
||
|
||
/// <summary>
|
||
/// 是否启用 TCP 长连接。
|
||
/// 默认 true；若现场 AGV 服务端不支持同一连接多次收发，可设为 false 回退到每条命令新建一次短连接。
|
||
/// </summary>
|
||
[FieldMember] public bool UsePersistentConnection = true;
|
||
|
||
/// <summary>
|
||
/// 是否输出 MagCar 详细日志。
|
||
/// 打开后会记录连接、发送、接收、状态解析、站点映射和异常信息。
|
||
/// </summary>
|
||
[FieldMember] public bool EnableDetailLog = true;
|
||
|
||
/// <summary>
|
||
/// 是否在详细日志中输出原始 32 字节协议帧。
|
||
/// 调试协议字段时建议打开；现场日志量过大时可关闭。
|
||
/// </summary>
|
||
[FieldMember] public bool LogRawFrame = true;
|
||
|
||
/// <summary>
|
||
/// 小车绘制长度，单位与场景坐标一致，默认 1200。
|
||
/// 仅影响画面显示，不参与 AGV 协议通讯。
|
||
/// </summary>
|
||
[FieldMember] public float CarLength = 1200;
|
||
|
||
/// <summary>
|
||
/// 小车绘制宽度，单位与场景坐标一致，默认 800。
|
||
/// 仅影响画面显示，不参与 AGV 协议通讯。
|
||
/// </summary>
|
||
[FieldMember] public float CarWidth = 800;
|
||
|
||
/// <summary>
|
||
/// 最近一次从 AGV 返回帧中解析出的完整状态快照。
|
||
/// UI 显示、状态枚举和站点定位均以该对象为基础。
|
||
/// </summary>
|
||
public MagCarReport LastReport { get; private set; } = new MagCarReport();
|
||
|
||
/// <summary>
|
||
/// 创建 MagCar 默认实例，供 StandardScene 通过车型注册机制实例化。
|
||
/// 默认地址为 127.0.0.1，实际现场地址应在车属性中配置。
|
||
/// </summary>
|
||
/// <returns>初始化后的 MagCar 实例。</returns>
|
||
public new static async Task<MagCar> Create()
|
||
{
|
||
return new MagCar
|
||
{
|
||
lstatus = "连接中",
|
||
address = "127.0.0.1",
|
||
name = "MagCar",
|
||
haveCoordination = true,
|
||
speed = 1
|
||
};
|
||
}
|
||
|
||
/// <summary>
|
||
/// 在场景画布中绘制小车外形和朝向箭头。
|
||
/// 绘制尺寸来自 <see cref="CarLength"/> 和 <see cref="CarWidth"/>。
|
||
/// </summary>
|
||
/// <param name="eGraphics">场景传入的绘图对象，坐标系已由基类转换到小车本地坐标。</param>
|
||
protected override void draw(Graphics eGraphics)
|
||
{
|
||
var halfLength = CarLength / 2;
|
||
var halfWidth = CarWidth / 2;
|
||
eGraphics.FillRectangle(Brushes.Gray, -halfLength, -halfWidth, CarLength, CarWidth);
|
||
eGraphics.DrawRectangle(Pens.White, -halfLength, -halfWidth, CarLength, CarWidth);
|
||
|
||
using (var orientPen = new Pen(Color.White, 3))
|
||
{
|
||
orientPen.CustomEndCap = new AdjustableArrowCap(6, 6, true);
|
||
orientPen.StartCap = LineCap.RoundAnchor;
|
||
eGraphics.DrawLine(orientPen, 0, 0, halfLength, 0);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 生成场景中小车悬浮或列表展示的简要状态文本。
|
||
/// 内容包括车辆名称、站点、电量、速度和报警摘要。
|
||
/// </summary>
|
||
/// <returns>用于界面显示的多行状态字符串。</returns>
|
||
public override string SetDisplayInfo()
|
||
{
|
||
try
|
||
{
|
||
var report = LastReport;
|
||
var state = StateText(report.State);
|
||
var alarm = report.Alarm == 0 ? "" : $"|alarm:0x{report.Alarm:X}";
|
||
return $"{name}({id})\n{state}|site:{siteID}|soc:{report.Charge}|speed:{report.Speed}{alarm}";
|
||
}
|
||
catch
|
||
{
|
||
return $"{name}({id})\n状态未知";
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// StandardScene 周期调用的保活入口。
|
||
/// 方法内部按 <see cref="PollIntervalMs"/> 节流，实际执行时发送 0x00 查询状态命令，
|
||
/// 并将返回的 Node、状态、电量、报警等字段同步到车对象和 status.enums。
|
||
/// </summary>
|
||
public override void keepAlive()
|
||
{
|
||
if ((DateTime.Now - _lastPollTime).TotalMilliseconds < PollIntervalMs)
|
||
{
|
||
return;
|
||
}
|
||
|
||
_lastPollTime = DateTime.Now;
|
||
try
|
||
{
|
||
DetailLog($"keepAlive poll begin, interval={PollIntervalMs}ms, endpoint={address}:{Port}");
|
||
var report = RefreshState();
|
||
haveCoordination = true;
|
||
DetailLog($"keepAlive poll ok, node={report.Node}, state={StateText(report.State)}, soc={report.Charge}, speed={report.Speed}, alarm=0x{report.Alarm:X}");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
lstatus = IsBenignSocketFault(ex) ? lstatus : "离线";
|
||
if (!IsBenignSocketFault(ex))
|
||
{
|
||
DetailLog($"keepAlive poll failed: {ex.Message}");
|
||
}
|
||
}
|
||
}
|
||
|
||
private bool _running;
|
||
|
||
/// <summary>
|
||
/// 最近一次已同步到交通管控层的站点 id。
|
||
/// keepAlive 仅更新 x/y 时，holdingLocks 不会自动跟随，需要显式同步。
|
||
/// </summary>
|
||
private int _lastSyncedTrafficSiteId = -1;
|
||
|
||
/// <summary>
|
||
/// 执行调度编译出的移动脚本。
|
||
/// 脚本中的 <c>agv.MagCarGo</c> 会通过 FASS 1.0 协议下发移动任务，并轮询状态直到到达目标节点。
|
||
/// </summary>
|
||
/// <param name="script">调度层编译出的脚本文本。</param>
|
||
public override async Task actualSendScript(string script)
|
||
{
|
||
if (_running)
|
||
{
|
||
throw new InvalidOperationException($"MagCar {id} already running script");
|
||
}
|
||
|
||
_running = true;
|
||
try
|
||
{
|
||
DetailLog($"script begin, length={(script ?? string.Empty).Length}");
|
||
var agv = new MagCarInterface(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 1.0协议查询AGV状态")]
|
||
/// <summary>
|
||
/// 人工查询 AGV 状态的界面方法。
|
||
/// 调用后立即发送 0x00 查询命令，并用返回帧刷新 <see cref="LastReport"/>、站点位置和 status.enums。
|
||
/// </summary>
|
||
public void QueryState()
|
||
{
|
||
RefreshState();
|
||
}
|
||
|
||
[MethodMember(Name = "启动", Description = "发送启动指令")]
|
||
/// <summary>
|
||
/// 人工启动小车的界面方法。
|
||
/// 按当前车头角度发送 0x01 启动命令。
|
||
/// </summary>
|
||
public void StartMagCar()
|
||
{
|
||
SendStart((ushort)Math.Round(th));
|
||
}
|
||
|
||
[MethodMember(Name = "停止", Description = "发送停止指令")]
|
||
/// <summary>
|
||
/// 人工停止小车的界面方法。
|
||
/// 发送 0x02 停止命令，当前停止秒数传 0。
|
||
/// </summary>
|
||
public void StopMagCar()
|
||
{
|
||
SendStop(0);
|
||
}
|
||
|
||
[MethodMember(Name = "进入小车远程", Description = "进入小车远程桌面")]
|
||
[I18N.DocumentTranslation(Name = "Open remote desktop", Description = "Open the car's remote desktop", locale = "en")]
|
||
/// <summary>
|
||
/// 打开 Windows 远程桌面客户端并连接到当前小车地址。
|
||
/// 该方法只用于运维进入车载电脑，不参与 AGV 协议通讯。
|
||
/// </summary>
|
||
public void Mstsc()
|
||
{
|
||
Process.Start(
|
||
new ProcessStartInfo
|
||
{
|
||
FileName = "mstsc",
|
||
Arguments = $"/v:{address}",
|
||
UseShellExecute = false,
|
||
CreateNoWindow = true
|
||
}
|
||
);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 调度系统触发的急停入口。
|
||
/// 记录急停原因后发送 0x02 停止命令。
|
||
/// </summary>
|
||
/// <param name="reason">触发急停的上层原因，写入诊断日志便于追查。</param>
|
||
public void EmergencyStop(string reason)
|
||
{
|
||
Diagnosis.Post($"car{name}:MagCar EmergencyStop {reason}");
|
||
SendStop(0);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 调度系统触发的急停释放入口。
|
||
/// 记录释放日志后按当前车头角度发送 0x01 启动命令。
|
||
/// </summary>
|
||
public void EmergencyRelease()
|
||
{
|
||
Diagnosis.Post($"car{name}:MagCar EmergencyRelease");
|
||
SendStart((ushort)Math.Round(th));
|
||
}
|
||
|
||
[MethodMember(Name = "重置TCP连接", Description = "关闭当前长连接，下次命令自动重连")]
|
||
/// <summary>
|
||
/// 人工重置长连接的界面方法。
|
||
/// 关闭当前 TCP 长连接；下一次 keepAlive 或命令发送会自动重新连接。
|
||
/// </summary>
|
||
public void ResetTcpConnection()
|
||
{
|
||
lock (_syncRoot)
|
||
{
|
||
ClosePersistentConnection("manual reset");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 发送 0x01 启动命令。
|
||
/// </summary>
|
||
/// <param name="angle">启动时同步给 AGV 的车头角度，按协议大端 ushort 写入数据区。</param>
|
||
public void SendStart(ushort angle)
|
||
{
|
||
SendCommand(CmdStart, BuildUShortOrLegacyByte(angle));
|
||
}
|
||
|
||
/// <summary>
|
||
/// 发送 0x02 停止命令。
|
||
/// </summary>
|
||
/// <param name="seconds">停止保持秒数；当前现场通常传 0 表示立即停止。</param>
|
||
public void SendStop(ushort seconds)
|
||
{
|
||
SendCommand(CmdStop, BuildUShortOrLegacyByte(seconds));
|
||
}
|
||
|
||
/// <summary>
|
||
/// 发送 0x03 速度设置命令。
|
||
/// </summary>
|
||
/// <param name="percent">速度百分比，推荐范围 0-100。</param>
|
||
public void SendSpeedPercent(byte percent)
|
||
{
|
||
SendCommand(CmdSpeed, percent);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 发送 0x04 角度设置命令。
|
||
/// </summary>
|
||
/// <param name="angle">目标车头角度，单位度，按协议大端 ushort 写入数据区。</param>
|
||
public void SendAngle(ushort angle)
|
||
{
|
||
SendCommand(CmdAngle, BuildUShortOrLegacyByte(angle));
|
||
}
|
||
|
||
/// <summary>
|
||
/// 下发移动任务。
|
||
/// 默认使用 0xA1 命令同时发送起点和终点；当 <see cref="UseTaskCommand"/> 为 false 时，
|
||
/// 退回 0x05 旧版目标节点命令。
|
||
/// </summary>
|
||
/// <param name="fromNode">起点节点号，通常由当前站点或路径起点转换得到。</param>
|
||
/// <param name="toNode">终点节点号，通常由目标站点转换得到。</param>
|
||
public void SendMoveTask(ushort fromNode, ushort toNode)
|
||
{
|
||
if (UseTaskCommand)
|
||
{
|
||
DetailLog($"send move task by 0xA1, fromNode={fromNode}, toNode={toNode}");
|
||
SendCommand(CmdTask, Combine(
|
||
GetBigEndian(fromNode),
|
||
GetBigEndian(toNode),
|
||
new[] { (byte)0x03 }));
|
||
return;
|
||
}
|
||
|
||
DetailLog($"send legacy node by 0x05, toNode={toNode}");
|
||
SendCommand(CmdLegacyNode, BuildUShortOrLegacyByte(toNode));
|
||
}
|
||
|
||
/// <summary>
|
||
/// 主动查询并刷新 AGV 状态。
|
||
/// 成功后会解析响应帧并调用 <see cref="ApplyReport(MagCarReport)"/> 更新车辆运行状态。
|
||
/// </summary>
|
||
/// <returns>本次查询解析出的状态报告。</returns>
|
||
public MagCarReport RefreshState()
|
||
{
|
||
var response = SendCommand(CmdQueryState);
|
||
var report = MagCarReport.Parse(response);
|
||
ApplyReport(report);
|
||
return report;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 构建并发送一条 FASS 1.0 命令帧。
|
||
/// 该方法统一负责日志、线程加锁、发送接收、响应解析和状态同步。
|
||
/// </summary>
|
||
/// <param name="command">协议命令码。</param>
|
||
/// <param name="data">需要写入数据区的协议字段，最多拷贝 26 字节。</param>
|
||
/// <returns>AGV 返回的完整 32 字节响应帧。</returns>
|
||
private byte[] SendCommand(byte command, params byte[] data)
|
||
{
|
||
var request = BuildMessage(command, data);
|
||
DetailLog($"TX {CommandName(command)}, dataLen={(data == null ? 0 : data.Length)}");
|
||
if (LogRawFrame)
|
||
{
|
||
DetailLog($"TX raw {ToHex(request)}");
|
||
}
|
||
|
||
byte[] response;
|
||
lock (_syncRoot)
|
||
{
|
||
response = SendAndReceive(request);
|
||
}
|
||
|
||
DetailLog($"RX {CommandName(command)}, bytes={response.Length}");
|
||
if (LogRawFrame)
|
||
{
|
||
DetailLog($"RX raw {ToHex(response)}");
|
||
}
|
||
|
||
if (response.Length >= 32)
|
||
{
|
||
ApplyReport(MagCarReport.Parse(response));
|
||
}
|
||
|
||
return response;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 根据连接模式发送请求并读取响应。
|
||
/// 长连接和短连接都必须返回完整 32 字节响应帧。
|
||
/// </summary>
|
||
/// <param name="request">已构建好的 32 字节请求帧。</param>
|
||
/// <returns>完整 32 字节响应帧。</returns>
|
||
private byte[] SendAndReceive(byte[] request)
|
||
{
|
||
if (UsePersistentConnection)
|
||
{
|
||
return SendAndReceivePersistent(request);
|
||
}
|
||
|
||
return SendAndReceiveShort(request);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 使用长连接发送请求并读取响应。
|
||
/// 发送或读取过程中出现异常时会关闭长连接，确保下一次命令重新建连。
|
||
/// </summary>
|
||
/// <param name="request">已构建好的 32 字节请求帧。</param>
|
||
/// <returns>完整 32 字节响应帧。</returns>
|
||
private byte[] SendAndReceivePersistent(byte[] request)
|
||
{
|
||
try
|
||
{
|
||
EnsurePersistentConnection();
|
||
var stream = _persistentClient.GetStream();
|
||
stream.Write(request, 0, request.Length);
|
||
stream.Flush();
|
||
return ReadExact(stream, 32);
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
ClosePersistentConnection($"persistent send/read failed: {ex.Message}");
|
||
throw;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 确保长连接客户端已经连接。
|
||
/// 若当前连接为空或已断开，会先清理旧连接再按配置超时时间重新连接 AGV 服务端。
|
||
/// </summary>
|
||
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;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 判断当前长连接对象是否存在且 TcpClient 仍处于 Connected 状态。
|
||
/// 该检查只作为发送前的快速判断，真正断链仍可能在写入或读取时暴露。
|
||
/// </summary>
|
||
/// <returns>当前长连接可复用时返回 true。</returns>
|
||
private bool IsPersistentConnected()
|
||
{
|
||
try
|
||
{
|
||
return _persistentClient != null && _persistentClient.Connected;
|
||
}
|
||
catch
|
||
{
|
||
return false;
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 关闭并释放当前长连接对象。
|
||
/// 该方法不会抛出关闭异常，保证故障恢复路径不会被二次异常打断。
|
||
/// </summary>
|
||
/// <param name="reason">关闭连接的原因，写入详细日志。</param>
|
||
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
|
||
{
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 使用短连接兼容模式发送请求。
|
||
/// 每条命令都会新建 TCP 连接、发送、读满响应后立即释放连接。
|
||
/// </summary>
|
||
/// <param name="request">已构建好的 32 字节请求帧。</param>
|
||
/// <returns>完整 32 字节响应帧。</returns>
|
||
private byte[] SendAndReceiveShort(byte[] request)
|
||
{
|
||
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, 32);
|
||
}
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 使用 <see cref="TcpClient.ConnectAsync(string, int, CancellationToken)"/> 实现带超时的 TCP 连接。
|
||
/// 避免 BeginConnect 超时后直接 Close 触发“已中止 I/O 操作”的 SocketException。
|
||
/// </summary>
|
||
/// <param name="client">待连接的 TcpClient 实例。</param>
|
||
private void ConnectWithTimeout(TcpClient client)
|
||
{
|
||
if (string.IsNullOrWhiteSpace(address))
|
||
{
|
||
throw new InvalidOperationException("MagCar 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);
|
||
}
|
||
}
|
||
|
||
/// <summary>超时或取消建连时安静关闭客户端，吞掉预期的套接字中止异常。</summary>
|
||
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
|
||
{
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 判断是否为连接关闭/超时取消导致的预期套接字异常（无需打印完整堆栈）。
|
||
/// </summary>
|
||
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)
|
||
{
|
||
var message = current.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;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 从网络流中精确读取指定长度的数据。
|
||
/// TCP 是字节流，不能假设一次 Read 就能拿到完整 32 字节协议包，因此这里循环读取直到读满。
|
||
/// </summary>
|
||
/// <param name="stream">已连接的网络流。</param>
|
||
/// <param name="length">期望读取的字节数，MagCar 响应固定为 32。</param>
|
||
/// <returns>长度等于 <paramref name="length"/> 的字节数组。</returns>
|
||
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 MagCar response");
|
||
}
|
||
offset += read;
|
||
}
|
||
return buffer;
|
||
}
|
||
catch (Exception ex) when (IsBenignSocketFault(ex))
|
||
{
|
||
throw new IOException("remote closed before full MagCar response", ex);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 按 FASS 1.0 格式构造 32 字节发送帧。
|
||
/// 帧结构为：起始字节、命令码、车辆编号、数据区、保留字节和结束字节。
|
||
/// </summary>
|
||
/// <param name="command">协议命令码。</param>
|
||
/// <param name="data">数据区内容，最多写入第 4-29 字节。</param>
|
||
/// <returns>完整 32 字节请求帧。</returns>
|
||
private byte[] BuildMessage(byte command, byte[] data)
|
||
{
|
||
var bytes = new byte[32];
|
||
bytes[0] = Begin;
|
||
bytes[1] = command;
|
||
var code = GetBigEndian(VehicleCode);
|
||
bytes[2] = code[0];
|
||
bytes[3] = code[1];
|
||
|
||
if (data != null && data.Length > 0)
|
||
{
|
||
Array.Copy(data, 0, bytes, 4, Math.Min(data.Length, 26));
|
||
}
|
||
|
||
bytes[30] = 0x00;
|
||
bytes[31] = End;
|
||
return bytes;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 根据兼容开关构造数值字段。
|
||
/// 标准文档模式返回 ushort 大端两字节；旧版兼容模式只返回低 1 字节。
|
||
/// </summary>
|
||
/// <param name="value">待写入协议数据区的数值。</param>
|
||
/// <returns>符合当前兼容模式的字节数组。</returns>
|
||
private byte[] BuildUShortOrLegacyByte(ushort value)
|
||
{
|
||
if (UseDocumentUShortFields)
|
||
{
|
||
return GetBigEndian(value);
|
||
}
|
||
|
||
return new[] { (byte)Clamp(value, 0, 255) };
|
||
}
|
||
|
||
/// <summary>
|
||
/// 轮询 AGV 状态，直到车辆到达指定节点或超时。
|
||
/// </summary>
|
||
/// <param name="targetNode">目标节点号。</param>
|
||
/// <param name="timeoutMs">超时时间，单位毫秒。</param>
|
||
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 == targetNode && report.State != 1)
|
||
{
|
||
DetailLog($"arrived node={targetNode}");
|
||
return;
|
||
}
|
||
|
||
Thread.Sleep(Math.Max(50, Math.Min(PollIntervalMs, 500)));
|
||
}
|
||
|
||
throw new TimeoutException($"MagCar {name}({id}) move timeout, target node={targetNode}, current={LastReport.Node}");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将 AGV 返回状态应用到车对象。
|
||
/// 该方法会更新 <see cref="LastReport"/>、在线状态、车头角度、status.enums，
|
||
/// 并把返回的 Node 映射为场景站点后同步 <see cref="Car.siteID"/>、坐标 x/y。
|
||
/// </summary>
|
||
/// <param name="report">已解析的 AGV 状态报告。</param>
|
||
private void ApplyReport(MagCarReport report)
|
||
{
|
||
LastReport = report;
|
||
lstatus = report.Alarm != 0 ? "报警" : report.State == 0 ? "未准备" : "上线";
|
||
th = report.Angle;
|
||
|
||
status.enums["Protocol"] = "FASS1.0";
|
||
status.enums["CurrentNode"] = report.Node.ToString();
|
||
status.enums["Alarm"] = $"0x{report.Alarm:X}";
|
||
status.enums["State"] = StateText(report.State);
|
||
status.enums["Soc"] = report.Charge.ToString();
|
||
status.enums["ElectricCurrent"] = report.Current.ToString("0.###");
|
||
status.enums["Voltage"] = report.Voltage.ToString("0.###");
|
||
status.enums["Speed"] = report.Speed.ToString();
|
||
status.enums["Angle"] = report.Angle.ToString();
|
||
status.enums["Task"] = report.Task.ToString();
|
||
status.enums["Lift"] = report.Lift.ToString();
|
||
status.enums["Roll"] = report.Roll.ToString();
|
||
|
||
var site = ResolveSite(report.Node);
|
||
if (site == null)
|
||
{
|
||
haveCoordination = false;
|
||
DetailLog($"report applied without mapped site, node={report.Node}, state={StateText(report.State)}, alarm=0x{report.Alarm:X}");
|
||
return;
|
||
}
|
||
|
||
haveCoordination = true;
|
||
siteID = site.id;
|
||
x = site.x;
|
||
y = site.y;
|
||
SyncTrafficFromReport(site, report);
|
||
DetailLog($"report applied, node={report.Node}, site={siteID}, x={x:0.###}, y={y:0.###}, th={th:0.###}, state={StateText(report.State)}, soc={report.Charge}, current={report.Current:0.###}, voltage={report.Voltage:0.###}, speed={report.Speed}, task={report.Task}, lift={report.Lift}, roll={report.Roll}, alarm=0x{report.Alarm:X}");
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将 AGV 上报的当前地标同步到交通锁（holdingLocks）。
|
||
/// GhostCar 调度依赖 holdingLocks/GetLastSite，仅改 x/y/siteID 会导致“画面在动、初始化占点不动”。
|
||
/// </summary>
|
||
private void SyncTrafficFromReport(Site site, MagCarReport report)
|
||
{
|
||
if (_running || site == null)
|
||
{
|
||
return;
|
||
}
|
||
|
||
var lockedSite = status.holdingLocks.Length > 0 ? status.holdingLocks[0] : -1;
|
||
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}, state={StateText(report.State)}");
|
||
TrafficReset(site, true, strict: false);
|
||
_lastSyncedTrafficSiteId = site.id;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
DetailLog($"traffic sync failed: {ex.Message}");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将 AGV 返回的节点号解析为场景站点。
|
||
/// 根据 <see cref="UseTagValueAsNode"/> 决定优先匹配站点 TagValue 还是直接匹配站点 id。
|
||
/// </summary>
|
||
/// <param name="node">AGV 协议返回的当前节点号。</param>
|
||
/// <returns>匹配到的站点；未匹配或节点为 0 时返回 null。</returns>
|
||
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);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将场景站点 id 转换为 AGV 节点号。
|
||
/// 当 <see cref="UseTagValueAsNode"/> 为 true 且站点配置了可解析的 TagValue 时使用 TagValue，
|
||
/// 否则直接使用站点 id 并限制在 ushort 范围内。
|
||
/// </summary>
|
||
/// <param name="siteId">场景站点 id。</param>
|
||
/// <returns>下发给 AGV 的节点号。</returns>
|
||
private 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);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将路径或车辆配置中的速度换算为协议需要的百分比。
|
||
/// 输入小于等于 1 时按比例值处理；大于 1 时按已经是百分比处理。
|
||
/// </summary>
|
||
/// <param name="trackSpeed">路径速度配置；小于等于 0 时回退到车辆自身 speed 字段。</param>
|
||
/// <returns>0-100 范围内的速度百分比。</returns>
|
||
private byte SpeedToPercent(double trackSpeed)
|
||
{
|
||
if (trackSpeed <= 0)
|
||
{
|
||
return (byte)Clamp((int)Math.Round(speed * 100), 0, 100);
|
||
}
|
||
|
||
if (trackSpeed <= 1)
|
||
{
|
||
return (byte)Clamp((int)Math.Round(trackSpeed * 100), 0, 100);
|
||
}
|
||
|
||
return (byte)Clamp((int)Math.Round(trackSpeed), 0, 100);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 根据起点和终点坐标计算车头方向角。
|
||
/// 支持反向行驶、角度偏置和绝对角度覆盖。
|
||
/// </summary>
|
||
/// <param name="srcX">起点 X 坐标。</param>
|
||
/// <param name="srcY">起点 Y 坐标。</param>
|
||
/// <param name="dstX">终点 X 坐标。</param>
|
||
/// <param name="dstY">终点 Y 坐标。</param>
|
||
/// <param name="reverse">是否按反向行驶计算，true 时角度增加 180 度。</param>
|
||
/// <param name="bias">额外角度偏置，单位度。</param>
|
||
/// <param name="enableAbs">是否直接使用绝对角度。</param>
|
||
/// <param name="absDir">启用绝对角度时使用的方向角。</param>
|
||
/// <returns>归一化到 0-359 的方向角。</returns>
|
||
private static ushort DirectionAngle(double srcX, double srcY, double dstX, double dstY, bool reverse, double bias, bool enableAbs, double absDir)
|
||
{
|
||
var angle = enableAbs ? absDir : Math.Atan2(dstY - srcY, dstX - srcX) / Math.PI * 180.0;
|
||
if (reverse)
|
||
{
|
||
angle += 180;
|
||
}
|
||
angle += bias;
|
||
angle %= 360;
|
||
if (angle < 0)
|
||
{
|
||
angle += 360;
|
||
}
|
||
return (ushort)Math.Round(angle);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将 ushort 转换为大端字节序。
|
||
/// FASS 1.0 协议中的车辆编号、节点号和角度字段均按大端写入。
|
||
/// </summary>
|
||
/// <param name="value">待转换的 ushort 值。</param>
|
||
/// <returns>长度为 2 的大端字节数组。</returns>
|
||
private static byte[] GetBigEndian(ushort value)
|
||
{
|
||
var bytes = BitConverter.GetBytes(value);
|
||
if (BitConverter.IsLittleEndian)
|
||
{
|
||
Array.Reverse(bytes);
|
||
}
|
||
return bytes;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 顺序拼接多个字节数组。
|
||
/// 主要用于组装 0xA1 任务命令的数据区。
|
||
/// </summary>
|
||
/// <param name="arrays">需要按顺序拼接的字节数组集合。</param>
|
||
/// <returns>拼接后的新字节数组。</returns>
|
||
private static byte[] Combine(params byte[][] arrays)
|
||
{
|
||
var result = new byte[arrays.Sum(array => array.Length)];
|
||
var offset = 0;
|
||
foreach (var array in arrays)
|
||
{
|
||
Buffer.BlockCopy(array, 0, result, offset, array.Length);
|
||
offset += array.Length;
|
||
}
|
||
return result;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将整数限制在指定闭区间内。
|
||
/// </summary>
|
||
/// <param name="value">原始值。</param>
|
||
/// <param name="min">允许的最小值。</param>
|
||
/// <param name="max">允许的最大值。</param>
|
||
/// <returns>限制后的值。</returns>
|
||
private static int Clamp(int value, int min, int max)
|
||
{
|
||
if (value < min)
|
||
{
|
||
return min;
|
||
}
|
||
return value > max ? max : value;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将 ushort 值限制在指定闭区间内。
|
||
/// </summary>
|
||
/// <param name="value">原始 ushort 值。</param>
|
||
/// <param name="min">允许的最小值。</param>
|
||
/// <param name="max">允许的最大值。</param>
|
||
/// <returns>限制后的值。</returns>
|
||
private static int Clamp(ushort value, int min, int max)
|
||
{
|
||
return Clamp((int)value, min, max);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将协议状态字节转换为便于日志和界面阅读的中文文本。
|
||
/// </summary>
|
||
/// <param name="state">AGV 返回帧第 14 字节的状态码。</param>
|
||
/// <returns>状态文本；未知状态会带出原始状态码。</returns>
|
||
private static string StateText(byte state)
|
||
{
|
||
switch (state)
|
||
{
|
||
case 0x00:
|
||
return "未准备";
|
||
case 0x01:
|
||
return "运行中";
|
||
case 0x02:
|
||
return "停止中";
|
||
case 0x03:
|
||
return "充电中";
|
||
default:
|
||
return $"未知({state})";
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 输出 MagCar 专用详细日志。
|
||
/// 日志同时写入车辆调试信息和 Diagnosis 通道，便于现场排查通讯问题。
|
||
/// </summary>
|
||
/// <param name="message">需要记录的日志正文。</param>
|
||
private void DetailLog(string message)
|
||
{
|
||
if (!EnableDetailLog)
|
||
{
|
||
return;
|
||
}
|
||
|
||
var text = $"[MagCar:{name}({id})] {DateTime.Now:HH:mm:ss.fff} {message}";
|
||
AppendDebug(text);
|
||
Diagnosis.Post(text, "MagCar", true);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将协议命令码转换为带十六进制值的命令名称。
|
||
/// </summary>
|
||
/// <param name="command">协议命令码。</param>
|
||
/// <returns>用于日志输出的命令名称。</returns>
|
||
private static string CommandName(byte command)
|
||
{
|
||
switch (command)
|
||
{
|
||
case CmdQueryState:
|
||
return "QueryState(0x00)";
|
||
case CmdStart:
|
||
return "Start(0x01)";
|
||
case CmdStop:
|
||
return "Stop(0x02)";
|
||
case CmdSpeed:
|
||
return "Speed(0x03)";
|
||
case CmdAngle:
|
||
return "Angle(0x04)";
|
||
case CmdLegacyNode:
|
||
return "LegacyNode(0x05)";
|
||
case CmdTask:
|
||
return "Task(0xA1)";
|
||
default:
|
||
return $"Unknown(0x{command:X2})";
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 将字节数组格式化为空格分隔的十六进制字符串。
|
||
/// 用于输出原始协议帧日志。
|
||
/// </summary>
|
||
/// <param name="bytes">待格式化的字节数组。</param>
|
||
/// <returns>十六进制字符串；输入为 null 时返回空字符串。</returns>
|
||
private static string ToHex(byte[] bytes)
|
||
{
|
||
return bytes == null ? string.Empty : BitConverter.ToString(bytes).Replace("-", " ");
|
||
}
|
||
|
||
/// <summary>
|
||
/// MagCar 调度脚本执行接口，将路径段转换为 FASS 1.0 移动任务。
|
||
/// </summary>
|
||
internal sealed class MagCarInterface : AGVInterface
|
||
{
|
||
private readonly MagCar _car;
|
||
|
||
public MagCarInterface(int id)
|
||
{
|
||
_car = (MagCar)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 MagCarGo(int srcId, int dstId, double speed = -1)
|
||
{
|
||
var promise = new TaskCompletionSource<int>();
|
||
Queue(async () =>
|
||
{
|
||
while (!TryLock(dstId))
|
||
{
|
||
await Task.Delay(50);
|
||
}
|
||
|
||
var fromNode = _car.ResolveNodeId(srcId);
|
||
var toNode = _car.ResolveNodeId(dstId);
|
||
_car.DetailLog($"MagCarGo src={srcId}, dst={dstId}, fromNode={fromNode}, toNode={toNode}, speed={speed}");
|
||
|
||
if (_car.StartBeforeMove)
|
||
{
|
||
_car.SendStart((ushort)Math.Round(_car.th));
|
||
}
|
||
|
||
_car.SendSpeedPercent(_car.SpeedToPercent(speed));
|
||
_car.SendMoveTask(fromNode, toNode);
|
||
_car.WaitUntilArrived(toNode, _car.MoveTimeoutSeconds * 1000);
|
||
promise.SetResult(1);
|
||
}, async () =>
|
||
{
|
||
await promise.Task;
|
||
Leave(srcId);
|
||
});
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// AGV 返回的 32 字节状态帧解析结果。
|
||
/// 字段位置遵循 FASS 1.0 协议：Node 在第 4-5 字节，Alarm 在第 6-13 字节，
|
||
/// State 在第 14 字节，Charge 在第 15 字节，Current/Voltage 为大端 float。
|
||
/// </summary>
|
||
public class MagCarReport
|
||
{
|
||
/// <summary>
|
||
/// 响应帧中的命令码，通常与请求命令码一致，用于确认本次返回对应的命令类型。
|
||
/// </summary>
|
||
public byte Command { get; private set; }
|
||
|
||
/// <summary>
|
||
/// 响应帧中的车辆编号，来自第 2-3 字节，按大端 ushort 解析。
|
||
/// </summary>
|
||
public ushort Code { get; private set; }
|
||
|
||
/// <summary>
|
||
/// AGV 当前所在节点号，来自第 4-5 字节。
|
||
/// <see cref="ApplyReport(MagCarReport)"/> 会将该值映射到场景站点并更新车辆坐标。
|
||
/// </summary>
|
||
public ushort Node { get; private set; }
|
||
|
||
/// <summary>
|
||
/// AGV 报警位图，来自第 6-13 字节。
|
||
/// 值为 0 表示无报警，非 0 时车辆在线状态会显示为报警。
|
||
/// </summary>
|
||
public ulong Alarm { get; private set; }
|
||
|
||
/// <summary>
|
||
/// AGV 运行状态码，来自第 14 字节。
|
||
/// 当前按 0 未准备、1 运行中、2 停止中、3 充电中展示。
|
||
/// </summary>
|
||
public byte State { get; private set; }
|
||
|
||
/// <summary>
|
||
/// 电量百分比，来自第 15 字节，也会写入 status.enums["Soc"]。
|
||
/// </summary>
|
||
public byte Charge { get; private set; }
|
||
|
||
/// <summary>
|
||
/// 当前电流，来自第 16-19 字节，按大端 IEEE754 float 解析。
|
||
/// </summary>
|
||
public float Current { get; private set; }
|
||
|
||
/// <summary>
|
||
/// 当前电压，来自第 20-23 字节，按大端 IEEE754 float 解析。
|
||
/// </summary>
|
||
public float Voltage { get; private set; }
|
||
|
||
/// <summary>
|
||
/// AGV 当前速度百分比或协议返回速度值，来自第 24 字节。
|
||
/// </summary>
|
||
public byte Speed { get; private set; }
|
||
|
||
/// <summary>
|
||
/// AGV 当前车头角度，来自第 25-26 字节，按大端 ushort 解析。
|
||
/// 该值会同步到车辆 th 字段。
|
||
/// </summary>
|
||
public ushort Angle { get; private set; }
|
||
|
||
/// <summary>
|
||
/// 当前任务状态或任务编号，来自第 27 字节。
|
||
/// </summary>
|
||
public byte Task { get; private set; }
|
||
|
||
/// <summary>
|
||
/// 顶升机构状态，来自第 28 字节。
|
||
/// </summary>
|
||
public byte Lift { get; private set; }
|
||
|
||
/// <summary>
|
||
/// 滚筒机构状态，来自第 29 字节。
|
||
/// </summary>
|
||
public byte Roll { get; private set; }
|
||
|
||
/// <summary>
|
||
/// 将 AGV 返回的原始 32 字节帧解析为 <see cref="MagCarReport"/>。
|
||
/// 方法会校验帧长、起始字节和结束字节，字段按 FASS 1.0 大端格式读取。
|
||
/// </summary>
|
||
/// <param name="bytes">AGV 返回的原始响应帧。</param>
|
||
/// <returns>解析后的状态报告。</returns>
|
||
public static MagCarReport Parse(byte[] bytes)
|
||
{
|
||
if (bytes == null || bytes.Length < 32)
|
||
{
|
||
throw new ArgumentException("MagCar response must be 32 bytes");
|
||
}
|
||
if (bytes[0] != Begin || bytes[31] != End)
|
||
{
|
||
throw new ArgumentException($"invalid MagCar frame: begin=0x{bytes[0]:X2}, end=0x{bytes[31]:X2}");
|
||
}
|
||
|
||
return new MagCarReport
|
||
{
|
||
Command = bytes[1],
|
||
Code = ReadUInt16(bytes, 2),
|
||
Node = ReadUInt16(bytes, 4),
|
||
Alarm = ReadUInt64(bytes, 6),
|
||
State = bytes[14],
|
||
Charge = bytes[15],
|
||
Current = ReadSingle(bytes, 16),
|
||
Voltage = ReadSingle(bytes, 20),
|
||
Speed = bytes[24],
|
||
Angle = ReadUInt16(bytes, 25),
|
||
Task = bytes[27],
|
||
Lift = bytes[28],
|
||
Roll = bytes[29]
|
||
};
|
||
}
|
||
|
||
/// <summary>
|
||
/// 从指定偏移读取大端 ushort。
|
||
/// </summary>
|
||
/// <param name="bytes">来源字节数组。</param>
|
||
/// <param name="offset">字段起始偏移。</param>
|
||
/// <returns>解析出的 ushort 值。</returns>
|
||
private static ushort ReadUInt16(byte[] bytes, int offset)
|
||
{
|
||
return (ushort)((bytes[offset] << 8) | bytes[offset + 1]);
|
||
}
|
||
|
||
/// <summary>
|
||
/// 从指定偏移读取大端 ulong。
|
||
/// 报警位图占用 8 字节，因此使用该方法解析 Alarm 字段。
|
||
/// </summary>
|
||
/// <param name="bytes">来源字节数组。</param>
|
||
/// <param name="offset">字段起始偏移。</param>
|
||
/// <returns>解析出的 ulong 值。</returns>
|
||
private static ulong ReadUInt64(byte[] bytes, int offset)
|
||
{
|
||
var result = 0UL;
|
||
for (var i = 0; i < 8; i++)
|
||
{
|
||
result = (result << 8) | bytes[offset + i];
|
||
}
|
||
return result;
|
||
}
|
||
|
||
/// <summary>
|
||
/// 从指定偏移读取大端单精度浮点数。
|
||
/// 当前用于解析电流和电压字段。
|
||
/// </summary>
|
||
/// <param name="bytes">来源字节数组。</param>
|
||
/// <param name="offset">字段起始偏移。</param>
|
||
/// <returns>解析出的 float 值。</returns>
|
||
private static float ReadSingle(byte[] bytes, int offset)
|
||
{
|
||
var buffer = new byte[4];
|
||
Buffer.BlockCopy(bytes, offset, buffer, 0, 4);
|
||
if (BitConverter.IsLittleEndian)
|
||
{
|
||
Array.Reverse(buffer);
|
||
}
|
||
return BitConverter.ToSingle(buffer, 0);
|
||
}
|
||
}
|
||
}
|
||
}
|