Merge commit '0952958af7685646cdbf2a4907c6e2ae59d4d352'

2026-05-28 10:44:03 +08:00
parent 4eeaa3fef3
commit 35bdb08d11
144 changed files with 33730 additions and 392 deletions
--- a/src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandProtocol.cs
+++ b/src/Flyshot.Runtime.Fanuc/Protocol/FanucCommandProtocol.cs
@@ -0,0 +1,621 @@
+using System.Buffers.Binary;
+using System.Text;
+
+namespace Flyshot.Runtime.Fanuc.Protocol;
+
+/// <summary>
+/// 定义 FANUC TCP 10012 命令通道已经由抓包和逆向资料确认的消息号。
+/// </summary>
+public static class FanucCommandMessageIds
+{
+    /// <summary>
+    /// 获取控制器程序状态的消息号。
+    /// </summary>
+    public const uint GetProgramStatus = 0x2003;
+
+    /// <summary>
+    /// 复位控制器的消息号。
+    /// </summary>
+    public const uint ResetRobot = 0x2100;
+
+    /// <summary>
+    /// 启动控制器程序的消息号。
+    /// </summary>
+    public const uint StartProgram = 0x2102;
+
+    /// <summary>
+    /// 停止控制器程序的消息号。
+    /// </summary>
+    public const uint StopProgram = 0x2103;
+
+    /// <summary>
+    /// 读取控制器 TCP 的消息号。
+    /// </summary>
+    public const uint GetTcp = 0x2200;
+
+    /// <summary>
+    /// 设置控制器 TCP 的消息号。
+    /// </summary>
+    public const uint SetTcp = 0x2201;
+
+    /// <summary>
+    /// 读取控制器速度倍率的消息号。
+    /// </summary>
+    public const uint GetSpeedRatio = 0x2206;
+
+    /// <summary>
+    /// 设置控制器速度倍率的消息号。
+    /// </summary>
+    public const uint SetSpeedRatio = 0x2207;
+
+    /// <summary>
+    /// 读取控制器 IO 的消息号。
+    /// </summary>
+    public const uint GetIo = 0x2208;
+
+    /// <summary>
+    /// 设置控制器 IO 的消息号。
+    /// </summary>
+    public const uint SetIo = 0x2209;
+}
+
+/// <summary>
+/// 定义旧 ControllerClient 公开的 FANUC IO 类型枚举值。
+/// </summary>
+public static class FanucIoTypes
+{
+    /// <summary>
+    /// FANUC 数字输入 DI。
+    /// </summary>
+    public const uint DigitalInput = 1;
+
+    /// <summary>
+    /// FANUC 数字输出 DO。
+    /// </summary>
+    public const uint DigitalOutput = 2;
+
+    /// <summary>
+    /// FANUC 机器人输入 RI。
+    /// </summary>
+    public const uint RobotInput = 8;
+
+    /// <summary>
+    /// FANUC 机器人输出 RO。
+    /// </summary>
+    public const uint RobotOutput = 9;
+
+    /// <summary>
+    /// 将 HTTP/兼容层传入的 IO 类型字符串转换为 FANUC 命令通道枚举值。
+    /// </summary>
+    /// <param name="ioType">IO 类型字符串，例如 DI、DO、RI、RO。</param>
+    /// <returns>命令通道使用的 IO 类型数值。</returns>
+    public static uint FromName(string ioType)
+    {
+        if (string.IsNullOrWhiteSpace(ioType))
+        {
+            throw new ArgumentException("IO 类型不能为空。", nameof(ioType));
+        }
+
+        return ioType.Trim().ToUpperInvariant() switch
+        {
+            "DI" or "KIOTYPEDI" => DigitalInput,
+            "DO" or "KIOTYPEDO" => DigitalOutput,
+            "RI" or "KIOTYPERI" => RobotInput,
+            "RO" or "KIOTYPERO" => RobotOutput,
+            _ => throw new ArgumentOutOfRangeException(nameof(ioType), ioType, "未知 IO 类型。")
+        };
+    }
+}
+
+/// <summary>
+/// 表示 FANUC TCP 10012 命令通道中只携带结果码的响应。
+/// </summary>
+public sealed class FanucCommandResultResponse
+{
+    /// <summary>
+    /// 初始化命令结果响应。
+    /// </summary>
+    /// <param name="messageId">响应对应的消息号。</param>
+    /// <param name="resultCode">控制器返回的结果码。</param>
+    public FanucCommandResultResponse(uint messageId, uint resultCode)
+    {
+        MessageId = messageId;
+        ResultCode = resultCode;
+    }
+
+    /// <summary>
+    /// 获取响应对应的消息号。
+    /// </summary>
+    public uint MessageId { get; }
+
+    /// <summary>
+    /// 获取控制器返回的结果码。
+    /// </summary>
+    public uint ResultCode { get; }
+
+    /// <summary>
+    /// 获取当前响应是否表示成功。
+    /// </summary>
+    public bool IsSuccess => ResultCode == 0;
+}
+
+/// <summary>
+/// 表示 FANUC TCP 10012 速度倍率响应。
+/// </summary>
+public sealed class FanucSpeedRatioResponse
+{
+    /// <summary>
+    /// 初始化速度倍率响应。
+    /// </summary>
+    /// <param name="messageId">响应对应的消息号。</param>
+    /// <param name="ratioInt">控制器返回的整数百分比。</param>
+    /// <param name="resultCode">控制器返回的结果码。</param>
+    public FanucSpeedRatioResponse(uint messageId, uint ratioInt, uint resultCode)
+    {
+        MessageId = messageId;
+        RatioInt = ratioInt;
+        ResultCode = resultCode;
+    }
+
+    /// <summary>
+    /// 获取响应对应的消息号。
+    /// </summary>
+    public uint MessageId { get; }
+
+    /// <summary>
+    /// 获取控制器返回的整数百分比。
+    /// </summary>
+    public uint RatioInt { get; }
+
+    /// <summary>
+    /// 获取控制器返回的结果码。
+    /// </summary>
+    public uint ResultCode { get; }
+
+    /// <summary>
+    /// 获取转换后的 0.0 到 1.0 速度倍率。
+    /// </summary>
+    public double Ratio => RatioInt / 100.0;
+
+    /// <summary>
+    /// 获取当前响应是否表示成功。
+    /// </summary>
+    public bool IsSuccess => ResultCode == 0;
+}
+
+/// <summary>
+/// 表示 FANUC TCP 10012 TCP 位姿响应。
+/// </summary>
+public sealed class FanucTcpResponse
+{
+    /// <summary>
+    /// 初始化 TCP 位姿响应。
+    /// </summary>
+    /// <param name="messageId">响应对应的消息号。</param>
+    /// <param name="resultCode">控制器返回的结果码。</param>
+    /// <param name="tcpId">控制器返回的 TCP ID。</param>
+    /// <param name="pose">7 维 TCP 位姿。</param>
+    public FanucTcpResponse(uint messageId, uint resultCode, uint tcpId, IReadOnlyList<double> pose)
+    {
+        MessageId = messageId;
+        ResultCode = resultCode;
+        TcpId = tcpId;
+        Pose = pose.ToArray();
+    }
+
+    /// <summary>
+    /// 获取响应对应的消息号。
+    /// </summary>
+    public uint MessageId { get; }
+
+    /// <summary>
+    /// 获取控制器返回的结果码。
+    /// </summary>
+    public uint ResultCode { get; }
+
+    /// <summary>
+    /// 获取控制器返回的 TCP ID。
+    /// </summary>
+    public uint TcpId { get; }
+
+    /// <summary>
+    /// 获取 7 维 TCP 位姿。
+    /// </summary>
+    public IReadOnlyList<double> Pose { get; }
+
+    /// <summary>
+    /// 获取当前响应是否表示成功。
+    /// </summary>
+    public bool IsSuccess => ResultCode == 0;
+}
+
+/// <summary>
+/// 表示 FANUC TCP 10012 IO 读取响应。
+/// </summary>
+public sealed class FanucIoResponse
+{
+    /// <summary>
+    /// 初始化 IO 读取响应。
+    /// </summary>
+    /// <param name="messageId">响应对应的消息号。</param>
+    /// <param name="resultCode">控制器返回的结果码。</param>
+    /// <param name="numericValue">控制器返回的 float IO 数值。</param>
+    public FanucIoResponse(uint messageId, uint resultCode, double numericValue)
+    {
+        MessageId = messageId;
+        ResultCode = resultCode;
+        NumericValue = numericValue;
+    }
+
+    /// <summary>
+    /// 获取响应对应的消息号。
+    /// </summary>
+    public uint MessageId { get; }
+
+    /// <summary>
+    /// 获取控制器返回的结果码。
+    /// </summary>
+    public uint ResultCode { get; }
+
+    /// <summary>
+    /// 获取控制器返回的原始数值。
+    /// </summary>
+    public double NumericValue { get; }
+
+    /// <summary>
+    /// 获取按布尔 IO 解释后的值。
+    /// </summary>
+    public bool Value => Math.Abs(NumericValue) > double.Epsilon;
+
+    /// <summary>
+    /// 获取当前响应是否表示成功。
+    /// </summary>
+    public bool IsSuccess => ResultCode == 0;
+}
+
+/// <summary>
+/// 表示 FANUC TCP 10012 程序状态响应。
+/// </summary>
+public sealed class FanucProgramStatusResponse
+{
+    /// <summary>
+    /// 初始化程序状态响应。
+    /// </summary>
+    /// <param name="messageId">响应对应的消息号。</param>
+    /// <param name="resultCode">控制器返回的结果码。</param>
+    /// <param name="programStatus">控制器程序状态。</param>
+    public FanucProgramStatusResponse(uint messageId, uint resultCode, uint programStatus)
+    {
+        MessageId = messageId;
+        ResultCode = resultCode;
+        ProgramStatus = programStatus;
+    }
+
+    /// <summary>
+    /// 获取响应对应的消息号。
+    /// </summary>
+    public uint MessageId { get; }
+
+    /// <summary>
+    /// 获取控制器返回的结果码。
+    /// </summary>
+    public uint ResultCode { get; }
+
+    /// <summary>
+    /// 获取控制器程序状态值。
+    /// </summary>
+    public uint ProgramStatus { get; }
+
+    /// <summary>
+    /// 获取当前响应是否表示成功。
+    /// </summary>
+    public bool IsSuccess => ResultCode == 0;
+}
+
+/// <summary>
+/// 提供 FANUC TCP 10012 命令通道的基础封包与响应解析能力。
+/// </summary>
+public static class FanucCommandProtocol
+{
+    /// <summary>
+    /// 将无业务体命令封装为 TCP 10012 二进制帧。
+    /// </summary>
+    /// <param name="messageId">命令消息号。</param>
+    /// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
+    public static byte[] PackEmptyCommand(uint messageId)
+    {
+        return PackFrame(messageId, ReadOnlySpan<byte>.Empty);
+    }
+
+    /// <summary>
+    /// 将程序名命令封装为 TCP 10012 二进制帧。
+    /// </summary>
+    /// <param name="messageId">命令消息号。</param>
+    /// <param name="programName">控制器程序名。</param>
+    /// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
+    public static byte[] PackProgramCommand(uint messageId, string programName)
+    {
+        if (string.IsNullOrWhiteSpace(programName))
+        {
+            throw new ArgumentException("程序名不能为空。", nameof(programName));
+        }
+
+        var programNameBytes = Encoding.ASCII.GetBytes(programName);
+        var body = new byte[sizeof(uint) + programNameBytes.Length];
+        BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(0, sizeof(uint)), (uint)programNameBytes.Length);
+        programNameBytes.CopyTo(body.AsSpan(sizeof(uint)));
+
+        return PackFrame(messageId, body);
+    }
+
+    /// <summary>
+    /// 封装读取速度倍率命令。
+    /// </summary>
+    /// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
+    public static byte[] PackGetSpeedRatioCommand()
+    {
+        return PackEmptyCommand(FanucCommandMessageIds.GetSpeedRatio);
+    }
+
+    /// <summary>
+    /// 封装设置速度倍率命令，按旧系统逻辑转换为 0..100 的整数百分比。
+    /// </summary>
+    /// <param name="ratio">目标速度倍率。</param>
+    /// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
+    public static byte[] PackSetSpeedRatioCommand(double ratio)
+    {
+        if (double.IsNaN(ratio) || double.IsInfinity(ratio))
+        {
+            throw new ArgumentOutOfRangeException(nameof(ratio), "ratio 必须是有限数值。");
+        }
+
+        var ratioInt = (uint)Math.Clamp((int)(ratio * 100.0), 0, 100);
+        var body = new byte[sizeof(uint)];
+        BinaryPrimitives.WriteUInt32BigEndian(body, ratioInt);
+        return PackFrame(FanucCommandMessageIds.SetSpeedRatio, body);
+    }
+
+    /// <summary>
+    /// 封装读取 TCP 位姿命令。
+    /// </summary>
+    /// <param name="tcpId">目标 TCP ID。</param>
+    /// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
+    public static byte[] PackGetTcpCommand(uint tcpId)
+    {
+        var body = new byte[sizeof(uint)];
+        BinaryPrimitives.WriteUInt32BigEndian(body, tcpId);
+        return PackFrame(FanucCommandMessageIds.GetTcp, body);
+    }
+
+    /// <summary>
+    /// 封装设置 TCP 位姿命令。
+    /// </summary>
+    /// <param name="tcpId">目标 TCP ID。</param>
+    /// <param name="pose">7 维 TCP 位姿。</param>
+    /// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
+    public static byte[] PackSetTcpCommand(uint tcpId, IReadOnlyList<double> pose)
+    {
+        ArgumentNullException.ThrowIfNull(pose);
+        if (pose.Count != 7)
+        {
+            throw new ArgumentException("TCP 位姿必须包含 7 个数值。", nameof(pose));
+        }
+
+        var body = new byte[sizeof(uint) + sizeof(float) * 7];
+        BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(0, sizeof(uint)), tcpId);
+        for (int i = 0; i < 7; i++)
+        {
+            BinaryPrimitives.WriteSingleBigEndian(body.AsSpan(sizeof(uint) + i * sizeof(float), sizeof(float)), (float)pose[i]);
+        }
+
+        return PackFrame(FanucCommandMessageIds.SetTcp, body);
+    }
+
+    /// <summary>
+    /// 封装读取 IO 命令，字段顺序为 io_type 后接 io_index。
+    /// </summary>
+    /// <param name="ioType">IO 类型数值。</param>
+    /// <param name="ioIndex">IO 索引。</param>
+    /// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
+    public static byte[] PackGetIoCommand(uint ioType, int ioIndex)
+    {
+        if (ioIndex < 0)
+        {
+            throw new ArgumentOutOfRangeException(nameof(ioIndex), "IO 索引不能为负数。");
+        }
+
+        var body = new byte[sizeof(uint) * 2];
+        BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(0, sizeof(uint)), ioType);
+        BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(sizeof(uint), sizeof(uint)), (uint)ioIndex);
+        return PackFrame(FanucCommandMessageIds.GetIo, body);
+    }
+
+    /// <summary>
+    /// 封装设置 IO 命令，字段顺序为 io_type、io_index、float io_value。
+    /// </summary>
+    /// <param name="ioType">IO 类型数值。</param>
+    /// <param name="ioIndex">IO 索引。</param>
+    /// <param name="value">目标 IO 布尔值。</param>
+    /// <returns>可直接写入命令通道 Socket 的完整帧。</returns>
+    public static byte[] PackSetIoCommand(uint ioType, int ioIndex, bool value)
+    {
+        if (ioIndex < 0)
+        {
+            throw new ArgumentOutOfRangeException(nameof(ioIndex), "IO 索引不能为负数。");
+        }
+
+        var body = new byte[sizeof(uint) * 2 + sizeof(float)];
+        BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(0, sizeof(uint)), ioType);
+        BinaryPrimitives.WriteUInt32BigEndian(body.AsSpan(sizeof(uint), sizeof(uint)), (uint)ioIndex);
+        BinaryPrimitives.WriteSingleBigEndian(body.AsSpan(sizeof(uint) * 2, sizeof(float)), value ? 1.0f : 0.0f);
+        return PackFrame(FanucCommandMessageIds.SetIo, body);
+    }
+
+    /// <summary>
+    /// 解析只携带结果码的 TCP 10012 响应帧。
+    /// </summary>
+    /// <param name="frame">完整响应帧。</param>
+    /// <returns>命令结果响应。</returns>
+    public static FanucCommandResultResponse ParseResultResponse(ReadOnlySpan<byte> frame)
+    {
+        var messageId = ValidateAndReadMessageId(frame);
+        var body = GetBody(frame);
+        if (body.Length < sizeof(uint))
+        {
+            throw new InvalidDataException("FANUC 命令响应体长度不足。");
+        }
+
+        return new FanucCommandResultResponse(
+            messageId,
+            BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]));
+    }
+
+    /// <summary>
+    /// 解析 GetSpeedRatio 的 TCP 10012 响应帧。
+    /// </summary>
+    /// <param name="frame">完整响应帧。</param>
+    /// <returns>速度倍率响应。</returns>
+    public static FanucSpeedRatioResponse ParseSpeedRatioResponse(ReadOnlySpan<byte> frame)
+    {
+        var messageId = ValidateAndReadMessageId(frame);
+        var body = GetBody(frame);
+        if (body.Length < sizeof(uint) * 2)
+        {
+            throw new InvalidDataException("FANUC 速度倍率响应体长度不足。");
+        }
+
+        // GetSpeedRatio 的字段顺序特殊：ratio_int 在前，result_code 在后。
+        var ratioInt = BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]);
+        var resultCode = BinaryPrimitives.ReadUInt32BigEndian(body.Slice(sizeof(uint), sizeof(uint)));
+        return new FanucSpeedRatioResponse(messageId, ratioInt, resultCode);
+    }
+
+    /// <summary>
+    /// 解析 GetTCP 的 TCP 10012 响应帧。
+    /// </summary>
+    /// <param name="frame">完整响应帧。</param>
+    /// <returns>TCP 位姿响应。</returns>
+    public static FanucTcpResponse ParseTcpResponse(ReadOnlySpan<byte> frame)
+    {
+        var messageId = ValidateAndReadMessageId(frame);
+        var body = GetBody(frame);
+        if (body.Length < sizeof(uint) * 2 + sizeof(float) * 7)
+        {
+            throw new InvalidDataException("FANUC TCP 响应体长度不足。");
+        }
+
+        var resultCode = BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]);
+        var tcpId = BinaryPrimitives.ReadUInt32BigEndian(body.Slice(sizeof(uint), sizeof(uint)));
+        var pose = new double[7];
+        for (int i = 0; i < pose.Length; i++)
+        {
+            pose[i] = BinaryPrimitives.ReadSingleBigEndian(body.Slice(sizeof(uint) * 2 + i * sizeof(float), sizeof(float)));
+        }
+
+        return new FanucTcpResponse(messageId, resultCode, tcpId, pose);
+    }
+
+    /// <summary>
+    /// 解析 GetIO 的 TCP 10012 响应帧。
+    /// </summary>
+    /// <param name="frame">完整响应帧。</param>
+    /// <returns>IO 读取响应。</returns>
+    public static FanucIoResponse ParseIoResponse(ReadOnlySpan<byte> frame)
+    {
+        var messageId = ValidateAndReadMessageId(frame);
+        var body = GetBody(frame);
+        if (body.Length < sizeof(uint) + sizeof(float))
+        {
+            throw new InvalidDataException("FANUC IO 响应体长度不足。");
+        }
+
+        var resultCode = BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]);
+        var ioValue = BinaryPrimitives.ReadSingleBigEndian(body.Slice(sizeof(uint), sizeof(float)));
+        return new FanucIoResponse(messageId, resultCode, ioValue);
+    }
+
+    /// <summary>
+    /// 解析 GetProgStatus 的 TCP 10012 响应帧。
+    /// </summary>
+    /// <param name="frame">完整响应帧。</param>
+    /// <returns>程序状态响应。</returns>
+    public static FanucProgramStatusResponse ParseProgramStatusResponse(ReadOnlySpan<byte> frame)
+    {
+        var messageId = ValidateAndReadMessageId(frame);
+        var body = GetBody(frame);
+        if (body.Length < sizeof(uint) * 2)
+        {
+            throw new InvalidDataException("FANUC 程序状态响应体长度不足。");
+        }
+
+        // all-reconnect.pcap 中字段顺序为 prog_status 后接 result_code。
+        var programStatus = BinaryPrimitives.ReadUInt32BigEndian(body[..sizeof(uint)]);
+        var resultCode = BinaryPrimitives.ReadUInt32BigEndian(body.Slice(sizeof(uint), sizeof(uint)));
+        return new FanucProgramStatusResponse(messageId, resultCode, programStatus);
+    }
+
+    /// <summary>
+    /// 按 FANUC 命令通道 framing 规则封装完整帧。
+    /// </summary>
+    /// <param name="messageId">命令消息号。</param>
+    /// <param name="body">业务体。</param>
+    /// <returns>完整命令帧。</returns>
+    internal static byte[] PackFrame(uint messageId, ReadOnlySpan<byte> body)
+    {
+        var frameLength = 3 + sizeof(uint) + sizeof(uint) + body.Length + 3;
+        var frame = new byte[frameLength];
+
+        frame[0] = (byte)'d';
+        frame[1] = (byte)'o';
+        frame[2] = (byte)'z';
+        BinaryPrimitives.WriteUInt32BigEndian(frame.AsSpan(3, sizeof(uint)), (uint)frameLength);
+        BinaryPrimitives.WriteUInt32BigEndian(frame.AsSpan(7, sizeof(uint)), messageId);
+        body.CopyTo(frame.AsSpan(11));
+        frame[^3] = (byte)'z';
+        frame[^2] = (byte)'o';
+        frame[^1] = (byte)'d';
+
+        return frame;
+    }
+
+    /// <summary>
+    /// 校验完整帧并读取消息号。
+    /// </summary>
+    /// <param name="frame">完整响应帧。</param>
+    /// <returns>响应消息号。</returns>
+    private static uint ValidateAndReadMessageId(ReadOnlySpan<byte> frame)
+    {
+        if (frame.Length < 14)
+        {
+            throw new InvalidDataException("FANUC 命令帧长度不足。");
+        }
+
+        if (frame[0] != (byte)'d' || frame[1] != (byte)'o' || frame[2] != (byte)'z')
+        {
+            throw new InvalidDataException("FANUC 命令帧头 magic 不正确。");
+        }
+
+        if (frame[^3] != (byte)'z' || frame[^2] != (byte)'o' || frame[^1] != (byte)'d')
+        {
+            throw new InvalidDataException("FANUC 命令帧尾 magic 不正确。");
+        }
+
+        var declaredLength = BinaryPrimitives.ReadUInt32BigEndian(frame.Slice(3, sizeof(uint)));
+        if (declaredLength != frame.Length)
+        {
+            throw new InvalidDataException("FANUC 命令帧长度字段与实际长度不一致。");
+        }
+
+        return BinaryPrimitives.ReadUInt32BigEndian(frame.Slice(7, sizeof(uint)));
+    }
+
+    /// <summary>
+    /// 获取完整帧中的业务体切片。
+    /// </summary>
+    /// <param name="frame">完整响应帧。</param>
+    /// <returns>业务体切片。</returns>
+    private static ReadOnlySpan<byte> GetBody(ReadOnlySpan<byte> frame)
+    {
+        return frame.Slice(11, frame.Length - 14);
+    }
+}