新增1.0和2.0两种协议车型车型
This commit is contained in:
ykkokluo
@@ -0,0 +1,192 @@
|
||||
using StandardScene.Magnetic.Protocol;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Text;
|
||||
|
||||
namespace StandardScene.Magnetic.Tasking
|
||||
{
|
||||
/// <summary>
|
||||
/// 比对期望节点与上报节点，判定到站与动作是否完成（对齐 backend <c>CarResponseService</c>）。
|
||||
/// </summary>
|
||||
public static class Fass2ActionResolver
|
||||
{
|
||||
private static readonly (string Name, Func<Fass2NodeMessage, byte> Get, Action<Fass2NodeMessage, byte> Set)[] ActionFields =
|
||||
{
|
||||
("StartStop", n => n.StartStop, (n, v) => n.StartStop = v),
|
||||
("Direction", n => n.Direction, (n, v) => n.Direction = v),
|
||||
("Orientation", n => n.Orientation, (n, v) => n.Orientation = v),
|
||||
("Byroad", n => n.Byroad, (n, v) => n.Byroad = v),
|
||||
("Obstacle", n => n.Obstacle, (n, v) => n.Obstacle = v),
|
||||
("Audio", n => n.Audio, (n, v) => n.Audio = v),
|
||||
("Light", n => n.Light, (n, v) => n.Light = v),
|
||||
("Charge", n => n.Charge, (n, v) => n.Charge = v),
|
||||
("Rest", n => n.Rest, (n, v) => n.Rest = v),
|
||||
("Lift", n => n.Lift, (n, v) => n.Lift = v),
|
||||
("Clamp", n => n.Clamp, (n, v) => n.Clamp = v),
|
||||
("Tray", n => n.Tray, (n, v) => n.Tray = v),
|
||||
("Roll", n => n.Roll, (n, v) => n.Roll = v),
|
||||
("Shutdown", n => n.Shutdown, (n, v) => n.Shutdown = v)
|
||||
};
|
||||
|
||||
public static bool IsAtNode(Fass2StateReport report, ushort targetNode)
|
||||
{
|
||||
return report?.Node != null && report.Node.Node == targetNode;
|
||||
}
|
||||
|
||||
public static bool IsVehicleMoving(byte vehicleState)
|
||||
{
|
||||
return vehicleState == 1;
|
||||
}
|
||||
|
||||
public static bool RequiresActionWait(Fass2NodeMessage expected)
|
||||
{
|
||||
if (expected == null)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (expected.StartStop is Fass2TaskBuilder.StartStopStop or Fass2TaskBuilder.StartStopPrecision)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
foreach (var field in ActionFields)
|
||||
{
|
||||
if (field.Get(expected) != 0)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return expected.Speed != 0;
|
||||
}
|
||||
|
||||
public static bool IsStationActionComplete(Fass2NodeMessage expected, Fass2NodeMessage actual, byte vehicleState)
|
||||
{
|
||||
if (expected == null || actual == null)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (actual.Node != expected.Node)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (IsVehicleMoving(vehicleState))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!RequiresActionWait(expected))
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
// 过站(StartStop=1) 只要求到点且非运行态，不要求 Orientation 等轨迹字段到位。
|
||||
if (expected.StartStop == Fass2TaskBuilder.StartStopPass)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
return ListPendingFields(expected, actual).Count == 0;
|
||||
}
|
||||
|
||||
public static IReadOnlyList<string> ListPendingFields(Fass2NodeMessage expected, Fass2NodeMessage actual)
|
||||
{
|
||||
var pending = new List<string>();
|
||||
if (expected == null || actual == null)
|
||||
{
|
||||
return pending;
|
||||
}
|
||||
|
||||
if (expected.StartStop != 0 && actual.StartStop != expected.StartStop)
|
||||
{
|
||||
pending.Add("StartStop");
|
||||
}
|
||||
|
||||
if (expected.Speed != 0 && actual.Speed != expected.Speed)
|
||||
{
|
||||
pending.Add("Speed");
|
||||
}
|
||||
|
||||
foreach (var field in ActionFields)
|
||||
{
|
||||
if (field.Name == "StartStop")
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
var expectedValue = field.Get(expected);
|
||||
if (expectedValue != 0 && field.Get(actual) != expectedValue)
|
||||
{
|
||||
pending.Add(field.Name);
|
||||
}
|
||||
}
|
||||
|
||||
return pending;
|
||||
}
|
||||
|
||||
public static Fass2NodeMessage BuildActionPatch(Fass2NodeMessage expected, Fass2NodeMessage actual)
|
||||
{
|
||||
if (expected == null)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
var patch = new Fass2NodeMessage { Node = expected.Node };
|
||||
var hasPatch = false;
|
||||
|
||||
if (expected.StartStop != 0 && (actual == null || actual.StartStop != expected.StartStop))
|
||||
{
|
||||
patch.StartStop = expected.StartStop;
|
||||
hasPatch = true;
|
||||
}
|
||||
|
||||
if (expected.Speed != 0 && (actual == null || actual.Speed != expected.Speed))
|
||||
{
|
||||
patch.Speed = expected.Speed;
|
||||
hasPatch = true;
|
||||
}
|
||||
|
||||
foreach (var field in ActionFields)
|
||||
{
|
||||
if (field.Name == "StartStop")
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
var expectedValue = field.Get(expected);
|
||||
if (expectedValue != 0 && (actual == null || field.Get(actual) != expectedValue))
|
||||
{
|
||||
field.Set(patch, expectedValue);
|
||||
hasPatch = true;
|
||||
}
|
||||
}
|
||||
|
||||
return hasPatch ? patch : null;
|
||||
}
|
||||
|
||||
public static string DescribePending(Fass2NodeMessage expected, Fass2NodeMessage actual)
|
||||
{
|
||||
var pending = ListPendingFields(expected, actual);
|
||||
if (pending.Count == 0)
|
||||
{
|
||||
return string.Empty;
|
||||
}
|
||||
|
||||
var builder = new StringBuilder();
|
||||
for (var i = 0; i < pending.Count; i++)
|
||||
{
|
||||
if (i > 0)
|
||||
{
|
||||
builder.Append(',');
|
||||
}
|
||||
|
||||
builder.Append(pending[i]);
|
||||
}
|
||||
|
||||
return builder.ToString();
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,90 @@
|
||||
using SimpleCore;
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace StandardScene.Magnetic.Tasking
|
||||
{
|
||||
internal static class Fass2PathFinder
|
||||
{
|
||||
public static List<int> GetSitesBetween(int startSiteId, int endSiteId)
|
||||
{
|
||||
if (startSiteId == endSiteId)
|
||||
{
|
||||
return new List<int> { startSiteId };
|
||||
}
|
||||
|
||||
var adjacency = BuildSiteAdjacency();
|
||||
var visited = new HashSet<int> { startSiteId };
|
||||
var queue = new Queue<List<int>>();
|
||||
queue.Enqueue(new List<int> { startSiteId });
|
||||
|
||||
while (queue.Count > 0)
|
||||
{
|
||||
var path = queue.Dequeue();
|
||||
var current = path[path.Count - 1];
|
||||
if (current == endSiteId)
|
||||
{
|
||||
return path;
|
||||
}
|
||||
|
||||
if (!adjacency.TryGetValue(current, out var neighbors))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
foreach (var neighbor in neighbors)
|
||||
{
|
||||
if (visited.Contains(neighbor))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
visited.Add(neighbor);
|
||||
var nextPath = new List<int>(path) { neighbor };
|
||||
queue.Enqueue(nextPath);
|
||||
}
|
||||
}
|
||||
|
||||
return new List<int>();
|
||||
}
|
||||
|
||||
private static Dictionary<int, HashSet<int>> BuildSiteAdjacency()
|
||||
{
|
||||
var adjacency = new Dictionary<int, HashSet<int>>();
|
||||
foreach (var track in SimpleLib.GetAllTracks())
|
||||
{
|
||||
var siteA = track.siteA;
|
||||
var siteB = track.siteB;
|
||||
switch (track.direction)
|
||||
{
|
||||
case 0:
|
||||
AddAdjacency(adjacency, siteA, siteB);
|
||||
AddAdjacency(adjacency, siteB, siteA);
|
||||
break;
|
||||
case 1:
|
||||
AddAdjacency(adjacency, siteA, siteB);
|
||||
break;
|
||||
case 2:
|
||||
AddAdjacency(adjacency, siteB, siteA);
|
||||
break;
|
||||
default:
|
||||
AddAdjacency(adjacency, siteA, siteB);
|
||||
AddAdjacency(adjacency, siteB, siteA);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
return adjacency;
|
||||
}
|
||||
|
||||
private static void AddAdjacency(Dictionary<int, HashSet<int>> adjacency, int from, int to)
|
||||
{
|
||||
if (!adjacency.TryGetValue(from, out var set))
|
||||
{
|
||||
set = new HashSet<int>();
|
||||
adjacency[from] = set;
|
||||
}
|
||||
|
||||
set.Add(to);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,103 @@
|
||||
using SimpleCore.PropType;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Globalization;
|
||||
|
||||
namespace StandardScene.Magnetic.Tasking
|
||||
{
|
||||
internal static class Fass2SiteFieldReader
|
||||
{
|
||||
public static Fass2SiteActionData Read(Site site)
|
||||
{
|
||||
var data = new Fass2SiteActionData();
|
||||
if (site?.fields == null)
|
||||
{
|
||||
return data;
|
||||
}
|
||||
|
||||
data.StartStop = TryReadByte(site.fields, Fass2SiteFields.StartStop);
|
||||
data.Byroad = TryReadByte(site.fields, Fass2SiteFields.Byroad);
|
||||
data.Direction = TryReadByte(site.fields, Fass2SiteFields.Direction);
|
||||
data.Orientation = TryReadByte(site.fields, Fass2SiteFields.Orientation);
|
||||
data.Lift = TryReadByte(site.fields, Fass2SiteFields.Lift);
|
||||
data.Roll = TryReadByte(site.fields, Fass2SiteFields.Roll);
|
||||
data.Charge = TryReadByte(site.fields, Fass2SiteFields.Charge);
|
||||
data.Obstacle = TryReadByte(site.fields, Fass2SiteFields.Obstacle);
|
||||
data.Audio = TryReadByte(site.fields, Fass2SiteFields.Audio);
|
||||
data.Light = TryReadByte(site.fields, Fass2SiteFields.Light);
|
||||
data.Rest = TryReadByte(site.fields, Fass2SiteFields.Rest);
|
||||
data.Clamp = TryReadByte(site.fields, Fass2SiteFields.Clamp);
|
||||
data.Tray = TryReadByte(site.fields, Fass2SiteFields.Tray);
|
||||
data.Shutdown = TryReadByte(site.fields, Fass2SiteFields.Shutdown);
|
||||
data.PrecisionStop = TryReadBool(site.fields, Fass2SiteFields.PrecisionStop);
|
||||
data.WaitMode = TryReadString(site.fields, Fass2SiteFields.WaitMode);
|
||||
return data;
|
||||
}
|
||||
|
||||
public static string BuildFieldsSignature(IReadOnlyList<int> siteIds, int fromIndex, Func<int, Site> getSite)
|
||||
{
|
||||
if (siteIds == null || siteIds.Count == 0)
|
||||
{
|
||||
return string.Empty;
|
||||
}
|
||||
|
||||
var parts = new List<string>(siteIds.Count - fromIndex);
|
||||
for (var i = Math.Max(0, fromIndex); i < siteIds.Count; i++)
|
||||
{
|
||||
var site = getSite(siteIds[i]);
|
||||
if (site?.fields == null)
|
||||
{
|
||||
parts.Add($"{siteIds[i]}:");
|
||||
continue;
|
||||
}
|
||||
|
||||
var keys = new List<string>();
|
||||
foreach (var pair in site.fields)
|
||||
{
|
||||
if (pair.Key.StartsWith("Fass2_", StringComparison.OrdinalIgnoreCase) ||
|
||||
pair.Key.Equals(Fass2SiteFields.TagValue, StringComparison.OrdinalIgnoreCase))
|
||||
{
|
||||
keys.Add($"{pair.Key}={pair.Value}");
|
||||
}
|
||||
}
|
||||
|
||||
keys.Sort(StringComparer.OrdinalIgnoreCase);
|
||||
parts.Add($"{siteIds[i]}:{string.Join("|", keys)}");
|
||||
}
|
||||
|
||||
return string.Join(";", parts);
|
||||
}
|
||||
|
||||
private static byte? TryReadByte(Dictionary<string, string> fields, string key)
|
||||
{
|
||||
if (!fields.TryGetValue(key, out var text) || string.IsNullOrWhiteSpace(text))
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
if (byte.TryParse(text, NumberStyles.Integer, CultureInfo.InvariantCulture, out var value))
|
||||
{
|
||||
return value;
|
||||
}
|
||||
|
||||
return null;
|
||||
}
|
||||
|
||||
private static bool TryReadBool(Dictionary<string, string> fields, string key)
|
||||
{
|
||||
if (!fields.TryGetValue(key, out var text) || string.IsNullOrWhiteSpace(text))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
return bool.TryParse(text, out var value) && value
|
||||
|| text == "1"
|
||||
|| string.Equals(text, "true", StringComparison.OrdinalIgnoreCase);
|
||||
}
|
||||
|
||||
private static string TryReadString(Dictionary<string, string> fields, string key)
|
||||
{
|
||||
return fields.TryGetValue(key, out var text) ? text : null;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,50 @@
|
||||
namespace StandardScene.Magnetic.Tasking
|
||||
{
|
||||
/// <summary>
|
||||
/// FASS 2.0 站点 <c>fields</c> 键名常量，用于地图编辑器配置站点动作。
|
||||
/// </summary>
|
||||
public static class Fass2SiteFields
|
||||
{
|
||||
public const string TagValue = "TagValue";
|
||||
|
||||
public const string StartStop = "Fass2_StartStop";
|
||||
public const string Byroad = "Fass2_Byroad";
|
||||
public const string Direction = "Fass2_Direction";
|
||||
public const string Orientation = "Fass2_Orientation";
|
||||
public const string Lift = "Fass2_Lift";
|
||||
public const string Roll = "Fass2_Roll";
|
||||
public const string Charge = "Fass2_Charge";
|
||||
public const string Obstacle = "Fass2_Obstacle";
|
||||
public const string Audio = "Fass2_Audio";
|
||||
public const string Light = "Fass2_Light";
|
||||
public const string Rest = "Fass2_Rest";
|
||||
public const string Clamp = "Fass2_Clamp";
|
||||
public const string Tray = "Fass2_Tray";
|
||||
public const string Shutdown = "Fass2_Shutdown";
|
||||
public const string PrecisionStop = "Fass2_PrecisionStop";
|
||||
public const string WaitMode = "Fass2_WaitMode";
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 从站点 fields 解析出的动作参数（映射到 25B 节点块）。
|
||||
/// </summary>
|
||||
public sealed class Fass2SiteActionData
|
||||
{
|
||||
public byte? StartStop { get; set; }
|
||||
public byte? Byroad { get; set; }
|
||||
public byte? Direction { get; set; }
|
||||
public byte? Orientation { get; set; }
|
||||
public byte? Lift { get; set; }
|
||||
public byte? Roll { get; set; }
|
||||
public byte? Charge { get; set; }
|
||||
public byte? Obstacle { get; set; }
|
||||
public byte? Audio { get; set; }
|
||||
public byte? Light { get; set; }
|
||||
public byte? Rest { get; set; }
|
||||
public byte? Clamp { get; set; }
|
||||
public byte? Tray { get; set; }
|
||||
public byte? Shutdown { get; set; }
|
||||
public bool PrecisionStop { get; set; }
|
||||
public string WaitMode { get; set; }
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,226 @@
|
||||
using SimpleCore;
|
||||
using SimpleCore.PropType;
|
||||
using StandardScene.Magnetic.Protocol;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
|
||||
namespace StandardScene.Magnetic.Tasking
|
||||
{
|
||||
/// <summary>
|
||||
/// 将站点路径与站点/边 fields 合并为 FASS 2.0 <c>0xB1</c> 节点序列。
|
||||
/// 对齐 backend <c>CarRequestService.GetSendNodes</c> 的组包规则。
|
||||
/// </summary>
|
||||
public static class Fass2TaskBuilder
|
||||
{
|
||||
public const byte StartStopPass = 1;
|
||||
public const byte StartStopStop = 2;
|
||||
public const byte StartStopPrecision = 22;
|
||||
|
||||
public static Fass2TaskPlan BuildPath(int startSiteId, int goalSiteId, Fass2TaskBuildOptions options,
|
||||
Func<int, ushort> resolveNodeId)
|
||||
{
|
||||
if (resolveNodeId == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(resolveNodeId));
|
||||
}
|
||||
|
||||
options ??= new Fass2TaskBuildOptions();
|
||||
var siteIds = Fass2PathFinder.GetSitesBetween(startSiteId, goalSiteId);
|
||||
if (siteIds.Count == 0)
|
||||
{
|
||||
throw new InvalidOperationException($"no path from site {startSiteId} to {goalSiteId}");
|
||||
}
|
||||
|
||||
var nodes = BuildNodesForSites(siteIds, options, resolveNodeId);
|
||||
return new Fass2TaskPlan
|
||||
{
|
||||
StartSiteId = startSiteId,
|
||||
GoalSiteId = goalSiteId,
|
||||
SiteIds = siteIds,
|
||||
Nodes = nodes,
|
||||
Batches = SplitBatches(nodes, options.MaxNodesPerFrame),
|
||||
FieldsSignature = Fass2SiteFieldReader.BuildFieldsSignature(siteIds, 0, SimpleLib.GetSite)
|
||||
};
|
||||
}
|
||||
|
||||
public static Fass2NodeMessage[] BuildSegment(int srcSiteId, int dstSiteId, Fass2TaskBuildOptions options,
|
||||
Func<int, ushort> resolveNodeId)
|
||||
{
|
||||
return BuildPath(srcSiteId, dstSiteId, options, resolveNodeId).Nodes.ToArray();
|
||||
}
|
||||
|
||||
public static IReadOnlyList<Fass2NodeMessage[]> SplitBatches(IReadOnlyList<Fass2NodeMessage> nodes, int maxPerFrame = 10)
|
||||
{
|
||||
if (nodes == null || nodes.Count == 0)
|
||||
{
|
||||
return Array.Empty<Fass2NodeMessage[]>();
|
||||
}
|
||||
|
||||
if (maxPerFrame <= 0 || maxPerFrame > 10)
|
||||
{
|
||||
maxPerFrame = 10;
|
||||
}
|
||||
|
||||
var batches = new List<Fass2NodeMessage[]>();
|
||||
for (var offset = 0; offset < nodes.Count; offset += maxPerFrame)
|
||||
{
|
||||
var count = Math.Min(maxPerFrame, nodes.Count - offset);
|
||||
var batch = new Fass2NodeMessage[count];
|
||||
for (var i = 0; i < count; i++)
|
||||
{
|
||||
batch[i] = nodes[offset + i];
|
||||
}
|
||||
|
||||
batches.Add(batch);
|
||||
}
|
||||
|
||||
return batches;
|
||||
}
|
||||
|
||||
private static List<Fass2NodeMessage> BuildNodesForSites(IReadOnlyList<int> siteIds, Fass2TaskBuildOptions options,
|
||||
Func<int, ushort> resolveNodeId)
|
||||
{
|
||||
var nodes = new List<Fass2NodeMessage>(siteIds.Count);
|
||||
for (var i = 0; i < siteIds.Count; i++)
|
||||
{
|
||||
var siteId = siteIds[i];
|
||||
var site = SimpleLib.GetSite(siteId);
|
||||
var siteAction = Fass2SiteFieldReader.Read(site);
|
||||
var isLast = i == siteIds.Count - 1;
|
||||
var prevSiteId = i > 0 ? siteIds[i - 1] : siteId;
|
||||
|
||||
Fass2TrackMotionData trackMotion = null;
|
||||
Site prevSite = null;
|
||||
if (i > 0)
|
||||
{
|
||||
prevSite = SimpleLib.GetSite(prevSiteId);
|
||||
var track = FindTrack(prevSiteId, siteId);
|
||||
trackMotion = Fass2TrackFieldReader.Read(track);
|
||||
}
|
||||
|
||||
var node = new Fass2NodeMessage
|
||||
{
|
||||
Node = resolveNodeId(siteId),
|
||||
StartStop = ResolveStartStop(siteAction, isLast),
|
||||
Distance = i > 0 ? ComputeEdgeDistance(prevSite, site) : (ushort)0,
|
||||
Speed = SpeedToProtocol(trackMotion?.Speed ?? options.DefaultSpeed, options.CarSpeed),
|
||||
Orientation = ResolveOrientation(prevSite, site, trackMotion),
|
||||
Byroad = siteAction.Byroad ?? trackMotion?.Byroad ?? 0,
|
||||
Direction = siteAction.Direction ?? trackMotion?.Direction ?? 0,
|
||||
Lift = siteAction.Lift ?? 0,
|
||||
Roll = siteAction.Roll ?? 0,
|
||||
Charge = siteAction.Charge ?? 0,
|
||||
Obstacle = siteAction.Obstacle ?? 0,
|
||||
Audio = siteAction.Audio ?? 0,
|
||||
Light = siteAction.Light ?? 0,
|
||||
Rest = siteAction.Rest ?? 0,
|
||||
Clamp = siteAction.Clamp ?? 0,
|
||||
Tray = siteAction.Tray ?? 0,
|
||||
Shutdown = siteAction.Shutdown ?? 0
|
||||
};
|
||||
|
||||
if (siteAction.Orientation.HasValue)
|
||||
{
|
||||
node.Orientation = siteAction.Orientation.Value;
|
||||
}
|
||||
|
||||
nodes.Add(node);
|
||||
}
|
||||
|
||||
return nodes;
|
||||
}
|
||||
|
||||
private static byte ResolveStartStop(Fass2SiteActionData siteAction, bool isLast)
|
||||
{
|
||||
if (siteAction.StartStop.HasValue)
|
||||
{
|
||||
return siteAction.StartStop.Value;
|
||||
}
|
||||
|
||||
if (isLast)
|
||||
{
|
||||
return siteAction.PrecisionStop ? StartStopPrecision : StartStopStop;
|
||||
}
|
||||
|
||||
return StartStopPass;
|
||||
}
|
||||
|
||||
private static byte ResolveOrientation(Site src, Site dst, Fass2TrackMotionData trackMotion)
|
||||
{
|
||||
if (trackMotion?.Orientation != null)
|
||||
{
|
||||
return trackMotion.Orientation.Value;
|
||||
}
|
||||
|
||||
if (src == null || dst == null)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
var angle = trackMotion != null && trackMotion.EnableCarAbsoluteDirection
|
||||
? trackMotion.CarAbsoluteDirection
|
||||
: Math.Atan2(dst.y - src.y, dst.x - src.x) / Math.PI * 180.0;
|
||||
|
||||
if (trackMotion?.Reverse == true)
|
||||
{
|
||||
angle += 180;
|
||||
}
|
||||
|
||||
angle += trackMotion?.CarDirectionBias ?? 0;
|
||||
angle %= 360;
|
||||
if (angle < 0)
|
||||
{
|
||||
angle += 360;
|
||||
}
|
||||
|
||||
return (byte)((int)Math.Round(angle / 360.0 * 256) % 256);
|
||||
}
|
||||
|
||||
private static ushort ComputeEdgeDistance(Site src, Site dst)
|
||||
{
|
||||
var dx = dst.x - src.x;
|
||||
var dy = dst.y - src.y;
|
||||
var length = Math.Sqrt(dx * dx + dy * dy);
|
||||
return (ushort)Math.Min(ushort.MaxValue, Math.Max(1, Math.Round(length)));
|
||||
}
|
||||
|
||||
private static ushort SpeedToProtocol(double trackSpeed, double carSpeed)
|
||||
{
|
||||
var value = trackSpeed <= 0 ? carSpeed : trackSpeed;
|
||||
var protocolSpeed = value <= 1
|
||||
? (int)Math.Round(value * 600)
|
||||
: (int)Math.Round(value * 10);
|
||||
return (ushort)Math.Max(1, Math.Min(10000, protocolSpeed));
|
||||
}
|
||||
|
||||
private static Track FindTrack(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;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,149 @@
|
||||
using SimpleCore.PropType;
|
||||
using StandardScene.Magnetic.Protocol;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace StandardScene.Magnetic.Tasking
|
||||
{
|
||||
public enum Fass2TaskPhase
|
||||
{
|
||||
Idle,
|
||||
Planning,
|
||||
Dispatching,
|
||||
Moving,
|
||||
AtStation,
|
||||
SegmentDone,
|
||||
Complete,
|
||||
Fault,
|
||||
Cancelled
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 一次 FASS 2.0 任务执行上下文，供状态机推进与重启恢复使用。
|
||||
/// </summary>
|
||||
public sealed class Fass2TaskContext
|
||||
{
|
||||
public Fass2TaskPhase Phase { get; set; } = Fass2TaskPhase.Idle;
|
||||
public int StartSiteId { get; set; }
|
||||
public int GoalSiteId { get; set; }
|
||||
public int CurrentIndex { get; set; }
|
||||
public ulong TaskId { get; set; }
|
||||
public Fass2TaskPlan Plan { get; set; }
|
||||
public string FieldsSignature { get; set; } = string.Empty;
|
||||
public double DefaultSpeed { get; set; } = -1;
|
||||
public string FaultReason { get; set; }
|
||||
public DateTime StartedAt { get; set; }
|
||||
public DateTime LastDispatchAt { get; set; } = DateTime.MinValue;
|
||||
public ulong LastActionId { get; set; }
|
||||
public DateTime LastActionSentAt { get; set; } = DateTime.MinValue;
|
||||
}
|
||||
|
||||
public static class Fass2TaskPersistence
|
||||
{
|
||||
public const string TagPhase = "Fass2Task_Phase";
|
||||
public const string TagStartSite = "Fass2Task_StartSite";
|
||||
public const string TagGoalSite = "Fass2Task_GoalSite";
|
||||
public const string TagCurrentIndex = "Fass2Task_CurrentIndex";
|
||||
public const string TagTaskId = "Fass2Task_TaskId";
|
||||
public const string TagFieldsSignature = "Fass2Task_FieldsSig";
|
||||
public const string TagDefaultSpeed = "Fass2Task_DefaultSpeed";
|
||||
|
||||
public static void Save(TagSet tags, Fass2TaskContext context)
|
||||
{
|
||||
if (tags == null || context == null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
SetTag(tags, TagPhase, context.Phase.ToString());
|
||||
SetTag(tags, TagStartSite, context.StartSiteId.ToString());
|
||||
SetTag(tags, TagGoalSite, context.GoalSiteId.ToString());
|
||||
SetTag(tags, TagCurrentIndex, context.CurrentIndex.ToString());
|
||||
SetTag(tags, TagTaskId, context.TaskId.ToString());
|
||||
SetTag(tags, TagFieldsSignature, context.FieldsSignature ?? string.Empty);
|
||||
SetTag(tags, TagDefaultSpeed, context.DefaultSpeed.ToString(System.Globalization.CultureInfo.InvariantCulture));
|
||||
}
|
||||
|
||||
public static bool TryLoad(TagSet tags, out Fass2TaskContext context)
|
||||
{
|
||||
context = null;
|
||||
if (tags == null || !tags.TryGetValue(TagPhase, out var phaseText) ||
|
||||
!Enum.TryParse(phaseText, out Fass2TaskPhase phase))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (phase is Fass2TaskPhase.Idle or Fass2TaskPhase.Complete or Fass2TaskPhase.Cancelled)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!TryReadInt(tags, TagStartSite, out var startSite) ||
|
||||
!TryReadInt(tags, TagGoalSite, out var goalSite) ||
|
||||
!TryReadInt(tags, TagCurrentIndex, out var currentIndex))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
tags.TryGetValue(TagFieldsSignature, out var fieldsSignature);
|
||||
ulong.TryParse(tags.TryGetValue(TagTaskId, out var taskIdText) ? taskIdText : "0", out var taskId);
|
||||
double.TryParse(
|
||||
tags.TryGetValue(TagDefaultSpeed, out var speedText) ? speedText : "-1",
|
||||
System.Globalization.NumberStyles.Float,
|
||||
System.Globalization.CultureInfo.InvariantCulture,
|
||||
out var defaultSpeed);
|
||||
|
||||
context = new Fass2TaskContext
|
||||
{
|
||||
Phase = phase,
|
||||
StartSiteId = startSite,
|
||||
GoalSiteId = goalSite,
|
||||
CurrentIndex = currentIndex,
|
||||
TaskId = taskId,
|
||||
FieldsSignature = fieldsSignature ?? string.Empty,
|
||||
DefaultSpeed = defaultSpeed
|
||||
};
|
||||
return true;
|
||||
}
|
||||
|
||||
public static void Clear(TagSet tags)
|
||||
{
|
||||
if (tags == null)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
RemoveTag(tags, TagPhase);
|
||||
RemoveTag(tags, TagStartSite);
|
||||
RemoveTag(tags, TagGoalSite);
|
||||
RemoveTag(tags, TagCurrentIndex);
|
||||
RemoveTag(tags, TagTaskId);
|
||||
RemoveTag(tags, TagFieldsSignature);
|
||||
RemoveTag(tags, TagDefaultSpeed);
|
||||
}
|
||||
|
||||
private static void SetTag(TagSet tags, string key, string value)
|
||||
{
|
||||
if (tags.Contains(key))
|
||||
{
|
||||
tags.Remove(key);
|
||||
}
|
||||
|
||||
tags.Add(key, value);
|
||||
}
|
||||
|
||||
private static void RemoveTag(TagSet tags, string key)
|
||||
{
|
||||
if (tags.Contains(key))
|
||||
{
|
||||
tags.Remove(key);
|
||||
}
|
||||
}
|
||||
|
||||
private static bool TryReadInt(TagSet tags, string key, out int value)
|
||||
{
|
||||
value = 0;
|
||||
return tags.TryGetValue(key, out var text) && int.TryParse(text, out value);
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,26 @@
|
||||
using StandardScene.Magnetic.Protocol;
|
||||
using System.Collections.Generic;
|
||||
|
||||
namespace StandardScene.Magnetic.Tasking
|
||||
{
|
||||
public sealed class Fass2TaskBuildOptions
|
||||
{
|
||||
public bool UseTagValueAsNode { get; set; }
|
||||
public double DefaultSpeed { get; set; } = 0.2;
|
||||
public double CarSpeed { get; set; } = 1;
|
||||
public int MaxNodesPerFrame { get; set; } = 10;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// 一次 FASS 2.0 路径任务计划：站点序列、节点报文、分批结果。
|
||||
/// </summary>
|
||||
public sealed class Fass2TaskPlan
|
||||
{
|
||||
public int StartSiteId { get; set; }
|
||||
public int GoalSiteId { get; set; }
|
||||
public IReadOnlyList<int> SiteIds { get; set; } = new List<int>();
|
||||
public IReadOnlyList<Fass2NodeMessage> Nodes { get; set; } = new List<Fass2NodeMessage>();
|
||||
public IReadOnlyList<Fass2NodeMessage[]> Batches { get; set; } = new List<Fass2NodeMessage[]>();
|
||||
public string FieldsSignature { get; set; } = string.Empty;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,588 @@
|
||||
using SimpleCore;
|
||||
using SimpleCore.PropType;
|
||||
using StandardScene.Magnetic.Protocol;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Threading;
|
||||
using System.Threading.Tasks;
|
||||
|
||||
namespace StandardScene.Magnetic.Tasking
|
||||
{
|
||||
public sealed class Fass2TaskCallbacks
|
||||
{
|
||||
public Func<int, int, double, Fass2TaskPlan> BuildPlan { get; set; }
|
||||
public Action<Fass2NodeMessage[], ulong> SendNodes { get; set; }
|
||||
public Action<Fass2NodeMessage, ulong> SendAction { get; set; }
|
||||
public Action<byte, ushort> SendControl { get; set; }
|
||||
public Func<int, ushort> ResolveNodeId { get; set; }
|
||||
public Func<ushort, Site> ResolveSite { get; set; }
|
||||
public Action<string> Log { get; set; }
|
||||
public Action<Fass2TaskContext> Persist { get; set; }
|
||||
public Action ClearPersisted { get; set; }
|
||||
public Func<ulong> AllocateTaskId { get; set; }
|
||||
public Func<ulong> AllocateActionId { get; set; }
|
||||
}
|
||||
|
||||
public sealed class Fass2TaskTickResult
|
||||
{
|
||||
public Fass2TaskPhase Phase { get; set; }
|
||||
public bool Dispatched { get; set; }
|
||||
public bool Rebuilt { get; set; }
|
||||
public bool ActionSent { get; set; }
|
||||
public bool Advanced { get; set; }
|
||||
public bool Completed { get; set; }
|
||||
public bool Faulted { get; set; }
|
||||
public string Message { get; set; }
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// FASS 2.0 任务状态机：规划 → 滑动窗口下发 → UDP/TCP 状态驱动推进 → 动作闭环。
|
||||
/// </summary>
|
||||
public sealed class Fass2TaskStateMachine
|
||||
{
|
||||
private readonly Fass2TaskCallbacks _callbacks;
|
||||
private readonly object _syncRoot = new object();
|
||||
private TaskCompletionSource<int> _completionSource;
|
||||
|
||||
public Fass2TaskStateMachine(Fass2TaskCallbacks callbacks)
|
||||
{
|
||||
_callbacks = callbacks ?? throw new ArgumentNullException(nameof(callbacks));
|
||||
Context = new Fass2TaskContext();
|
||||
}
|
||||
|
||||
public Fass2TaskContext Context { get; }
|
||||
|
||||
public int LockCount { get; set; } = 4;
|
||||
|
||||
public int ResendIntervalMs { get; set; } = 500;
|
||||
|
||||
public int ActionRetryIntervalMs { get; set; } = 1000;
|
||||
|
||||
public bool StartBeforeMove { get; set; } = true;
|
||||
|
||||
public ushort HeadingAngle { get; set; }
|
||||
|
||||
public bool IsIdle
|
||||
{
|
||||
get
|
||||
{
|
||||
lock (_syncRoot)
|
||||
{
|
||||
return Context.Phase is Fass2TaskPhase.Idle or Fass2TaskPhase.Complete or Fass2TaskPhase.Cancelled;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public bool IsRunning
|
||||
{
|
||||
get
|
||||
{
|
||||
lock (_syncRoot)
|
||||
{
|
||||
return Context.Phase is Fass2TaskPhase.Planning or Fass2TaskPhase.Dispatching
|
||||
or Fass2TaskPhase.Moving or Fass2TaskPhase.AtStation or Fass2TaskPhase.SegmentDone;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public event Action<Fass2TaskContext> Completed;
|
||||
public event Action<Fass2TaskContext, string> Faulted;
|
||||
|
||||
public void Begin(int startSiteId, int goalSiteId, double defaultSpeed = -1)
|
||||
{
|
||||
lock (_syncRoot)
|
||||
{
|
||||
if (!IsIdle)
|
||||
{
|
||||
throw new InvalidOperationException($"task state machine busy, phase={Context.Phase}");
|
||||
}
|
||||
|
||||
ResetCompletionSource();
|
||||
Context.Phase = Fass2TaskPhase.Planning;
|
||||
Context.StartSiteId = startSiteId;
|
||||
Context.GoalSiteId = goalSiteId;
|
||||
Context.CurrentIndex = 0;
|
||||
Context.DefaultSpeed = defaultSpeed;
|
||||
Context.StartedAt = DateTime.Now;
|
||||
Context.FaultReason = null;
|
||||
Context.Plan = null;
|
||||
Context.FieldsSignature = string.Empty;
|
||||
Context.LastDispatchAt = DateTime.MinValue;
|
||||
Context.LastActionSentAt = DateTime.MinValue;
|
||||
Context.LastActionId = 0;
|
||||
Log($"task begin start={startSiteId}, goal={goalSiteId}, speed={defaultSpeed}");
|
||||
Persist();
|
||||
}
|
||||
}
|
||||
|
||||
public bool TryRestore(Fass2TaskContext saved)
|
||||
{
|
||||
if (saved == null)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
lock (_syncRoot)
|
||||
{
|
||||
if (IsRunning)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
ResetCompletionSource();
|
||||
Context.Phase = saved.Phase;
|
||||
Context.StartSiteId = saved.StartSiteId;
|
||||
Context.GoalSiteId = saved.GoalSiteId;
|
||||
Context.CurrentIndex = saved.CurrentIndex;
|
||||
Context.TaskId = saved.TaskId;
|
||||
Context.DefaultSpeed = saved.DefaultSpeed;
|
||||
Context.FieldsSignature = saved.FieldsSignature ?? string.Empty;
|
||||
Context.StartedAt = DateTime.Now;
|
||||
Context.LastDispatchAt = DateTime.MinValue;
|
||||
Context.LastActionSentAt = DateTime.MinValue;
|
||||
Context.Plan = null;
|
||||
Context.FaultReason = null;
|
||||
|
||||
if (Context.Phase == Fass2TaskPhase.Planning)
|
||||
{
|
||||
Log($"task restored phase={Context.Phase}, goal={Context.GoalSiteId}, index={Context.CurrentIndex}");
|
||||
Persist();
|
||||
return true;
|
||||
}
|
||||
|
||||
Context.Phase = Fass2TaskPhase.Planning;
|
||||
Log($"task restored and normalized to Planning, goal={Context.GoalSiteId}, index={Context.CurrentIndex}");
|
||||
Persist();
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
public Fass2TaskTickResult Tick(Fass2StateReport report)
|
||||
{
|
||||
lock (_syncRoot)
|
||||
{
|
||||
if (Context.Phase is Fass2TaskPhase.Idle or Fass2TaskPhase.Complete
|
||||
or Fass2TaskPhase.Cancelled or Fass2TaskPhase.Fault)
|
||||
{
|
||||
return new Fass2TaskTickResult { Phase = Context.Phase };
|
||||
}
|
||||
|
||||
var result = new Fass2TaskTickResult { Phase = Context.Phase };
|
||||
|
||||
if (report?.Alarm != 0)
|
||||
{
|
||||
return Fault($"alarm=0x{report.Alarm:X}", result);
|
||||
}
|
||||
|
||||
if (report?.State == 3)
|
||||
{
|
||||
return Fault("vehicle emergency stop", result);
|
||||
}
|
||||
|
||||
if (report?.State == 4)
|
||||
{
|
||||
return Fault("vehicle fault", result);
|
||||
}
|
||||
|
||||
switch (Context.Phase)
|
||||
{
|
||||
case Fass2TaskPhase.Planning:
|
||||
return PlanAndDispatch(report, result);
|
||||
case Fass2TaskPhase.Dispatching:
|
||||
Context.Phase = Fass2TaskPhase.Moving;
|
||||
result.Phase = Context.Phase;
|
||||
return result;
|
||||
case Fass2TaskPhase.Moving:
|
||||
return HandleMoving(report, result);
|
||||
case Fass2TaskPhase.AtStation:
|
||||
return HandleAtStation(report, result);
|
||||
case Fass2TaskPhase.SegmentDone:
|
||||
return HandleSegmentDone(report, result);
|
||||
default:
|
||||
return result;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public void Cancel(string reason)
|
||||
{
|
||||
lock (_syncRoot)
|
||||
{
|
||||
if (IsIdle)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
Context.Phase = Fass2TaskPhase.Cancelled;
|
||||
Context.FaultReason = reason;
|
||||
Log($"task cancelled: {reason}");
|
||||
ClearPersisted();
|
||||
_completionSource?.TrySetException(new OperationCanceledException(reason));
|
||||
}
|
||||
}
|
||||
|
||||
public async Task WaitAsync(int timeoutMs, Func<Fass2StateReport> poll, int pollIntervalMs,
|
||||
CancellationToken cancellationToken = default)
|
||||
{
|
||||
if (poll == null)
|
||||
{
|
||||
throw new ArgumentNullException(nameof(poll));
|
||||
}
|
||||
|
||||
TaskCompletionSource<int> waiter;
|
||||
lock (_syncRoot)
|
||||
{
|
||||
waiter = _completionSource ?? ResetCompletionSource();
|
||||
}
|
||||
|
||||
var timeoutTask = Task.Delay(timeoutMs, cancellationToken);
|
||||
var pollTask = Task.Run(async () =>
|
||||
{
|
||||
while (!cancellationToken.IsCancellationRequested)
|
||||
{
|
||||
Fass2StateReport report;
|
||||
lock (_syncRoot)
|
||||
{
|
||||
if (!IsRunning)
|
||||
{
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
report = poll();
|
||||
Tick(report);
|
||||
|
||||
lock (_syncRoot)
|
||||
{
|
||||
if (!IsRunning)
|
||||
{
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
await Task.Delay(Math.Max(20, pollIntervalMs), cancellationToken);
|
||||
}
|
||||
}, cancellationToken);
|
||||
|
||||
var finished = await Task.WhenAny(waiter.Task, timeoutTask);
|
||||
if (finished == timeoutTask)
|
||||
{
|
||||
Cancel("wait timeout");
|
||||
throw new TimeoutException(
|
||||
$"FASS2 task timeout after {timeoutMs}ms, phase={Context.Phase}, index={Context.CurrentIndex}, goal={Context.GoalSiteId}");
|
||||
}
|
||||
|
||||
await waiter.Task;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// UDP 模式由 Hub 回调驱动 Tick，此处只等待完成信号，避免与 OnUdpStateReceived 争用锁导致死锁。
|
||||
/// </summary>
|
||||
public async Task WaitForCompletionAsync(int timeoutMs, CancellationToken cancellationToken = default)
|
||||
{
|
||||
TaskCompletionSource<int> waiter;
|
||||
lock (_syncRoot)
|
||||
{
|
||||
if (Context.Phase is Fass2TaskPhase.Complete or Fass2TaskPhase.Cancelled or Fass2TaskPhase.Fault)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
waiter = _completionSource ?? ResetCompletionSource();
|
||||
}
|
||||
|
||||
var timeoutTask = Task.Delay(timeoutMs, cancellationToken);
|
||||
var finished = await Task.WhenAny(waiter.Task, timeoutTask);
|
||||
if (finished == timeoutTask)
|
||||
{
|
||||
Cancel("wait timeout");
|
||||
throw new TimeoutException(
|
||||
$"FASS2 task timeout after {timeoutMs}ms, phase={Context.Phase}, index={Context.CurrentIndex}, goal={Context.GoalSiteId}");
|
||||
}
|
||||
|
||||
await waiter.Task;
|
||||
}
|
||||
|
||||
private Fass2TaskTickResult PlanAndDispatch(Fass2StateReport report, Fass2TaskTickResult result)
|
||||
{
|
||||
var startSiteId = ResolveCurrentSiteId(report);
|
||||
Context.Plan = _callbacks.BuildPlan(startSiteId, Context.GoalSiteId, Context.DefaultSpeed);
|
||||
if (Context.Plan == null || Context.Plan.Nodes.Count == 0)
|
||||
{
|
||||
return Fault($"empty task plan from {startSiteId} to {Context.GoalSiteId}", result);
|
||||
}
|
||||
|
||||
if (Context.CurrentIndex >= Context.Plan.Nodes.Count)
|
||||
{
|
||||
Context.CurrentIndex = Math.Max(0, Context.Plan.Nodes.Count - 1);
|
||||
}
|
||||
|
||||
Context.FieldsSignature = Context.Plan.FieldsSignature;
|
||||
Context.TaskId = _callbacks.AllocateTaskId();
|
||||
|
||||
if (StartBeforeMove && Context.CurrentIndex == 0)
|
||||
{
|
||||
_callbacks.SendControl(Fass2Protocol.CmdStart, HeadingAngle);
|
||||
}
|
||||
|
||||
DispatchCurrentWindow();
|
||||
Context.Phase = Fass2TaskPhase.Moving;
|
||||
result.Phase = Context.Phase;
|
||||
result.Dispatched = true;
|
||||
result.Message = $"planned nodes={Context.Plan.Nodes.Count}, task={Context.TaskId}";
|
||||
Log(result.Message);
|
||||
Persist();
|
||||
return result;
|
||||
}
|
||||
|
||||
private Fass2TaskTickResult HandleMoving(Fass2StateReport report, Fass2TaskTickResult result)
|
||||
{
|
||||
if (TryRebuildForFieldsChange(result))
|
||||
{
|
||||
return result;
|
||||
}
|
||||
|
||||
if (ShouldResend())
|
||||
{
|
||||
DispatchCurrentWindow();
|
||||
result.Dispatched = true;
|
||||
}
|
||||
|
||||
var expected = GetExpectedNode();
|
||||
if (!Fass2ActionResolver.IsAtNode(report, expected.Node))
|
||||
{
|
||||
result.Message = $"moving node={report.Node.Node}, expect={expected.Node}";
|
||||
return result;
|
||||
}
|
||||
|
||||
if (Fass2ActionResolver.IsVehicleMoving(report.State))
|
||||
{
|
||||
result.Message = $"at node={expected.Node}, still running";
|
||||
return result;
|
||||
}
|
||||
|
||||
Context.Phase = Fass2TaskPhase.AtStation;
|
||||
result.Phase = Context.Phase;
|
||||
result.Message = $"arrived site index={Context.CurrentIndex}, node={expected.Node}";
|
||||
Log(result.Message);
|
||||
Persist();
|
||||
return result;
|
||||
}
|
||||
|
||||
private Fass2TaskTickResult HandleAtStation(Fass2StateReport report, Fass2TaskTickResult result)
|
||||
{
|
||||
if (TryRebuildForFieldsChange(result))
|
||||
{
|
||||
return result;
|
||||
}
|
||||
|
||||
if (ShouldResend())
|
||||
{
|
||||
DispatchCurrentWindow();
|
||||
result.Dispatched = true;
|
||||
}
|
||||
|
||||
var expected = GetExpectedNode();
|
||||
if (!Fass2ActionResolver.IsAtNode(report, expected.Node))
|
||||
{
|
||||
Context.Phase = Fass2TaskPhase.Moving;
|
||||
result.Phase = Context.Phase;
|
||||
result.Message = $"left station node={report.Node.Node}, expect={expected.Node}";
|
||||
return result;
|
||||
}
|
||||
|
||||
if (!Fass2ActionResolver.IsStationActionComplete(expected, report.Node, report.State))
|
||||
{
|
||||
if (ShouldSendActionPatch())
|
||||
{
|
||||
var patch = Fass2ActionResolver.BuildActionPatch(expected, report.Node);
|
||||
if (patch != null)
|
||||
{
|
||||
Context.LastActionId = _callbacks.AllocateActionId();
|
||||
Context.LastActionSentAt = DateTime.Now;
|
||||
_callbacks.SendAction(patch, Context.LastActionId);
|
||||
result.ActionSent = true;
|
||||
result.Message =
|
||||
$"action pending [{Fass2ActionResolver.DescribePending(expected, report.Node)}], sent 0xA1={Context.LastActionId}";
|
||||
Log(result.Message);
|
||||
}
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
Context.Phase = Fass2TaskPhase.SegmentDone;
|
||||
result.Phase = Context.Phase;
|
||||
result.Message = $"station done index={Context.CurrentIndex}, node={expected.Node}";
|
||||
Log(result.Message);
|
||||
Persist();
|
||||
return result;
|
||||
}
|
||||
|
||||
private Fass2TaskTickResult HandleSegmentDone(Fass2StateReport report, Fass2TaskTickResult result)
|
||||
{
|
||||
Context.CurrentIndex++;
|
||||
result.Advanced = true;
|
||||
|
||||
if (Context.Plan == null || Context.CurrentIndex >= Context.Plan.Nodes.Count)
|
||||
{
|
||||
Context.Phase = Fass2TaskPhase.Complete;
|
||||
result.Phase = Context.Phase;
|
||||
result.Completed = true;
|
||||
result.Message = $"task complete goal={Context.GoalSiteId}";
|
||||
Log(result.Message);
|
||||
ClearPersisted();
|
||||
Completed?.Invoke(Context);
|
||||
_completionSource?.TrySetResult(1);
|
||||
return result;
|
||||
}
|
||||
|
||||
Context.Phase = Fass2TaskPhase.Dispatching;
|
||||
DispatchCurrentWindow();
|
||||
Context.Phase = Fass2TaskPhase.Moving;
|
||||
result.Phase = Context.Phase;
|
||||
result.Dispatched = true;
|
||||
result.Message = $"segment advanced index={Context.CurrentIndex}";
|
||||
Log(result.Message);
|
||||
Persist();
|
||||
return result;
|
||||
}
|
||||
|
||||
private bool TryRebuildForFieldsChange(Fass2TaskTickResult result)
|
||||
{
|
||||
if (Context.Plan?.SiteIds == null || Context.Plan.SiteIds.Count == 0)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
var newSignature = Fass2SiteFieldReader.BuildFieldsSignature(
|
||||
Context.Plan.SiteIds, Context.CurrentIndex, SimpleLib.GetSite);
|
||||
if (string.Equals(newSignature, Context.FieldsSignature, StringComparison.Ordinal))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
var currentSiteId = Context.Plan.SiteIds[Context.CurrentIndex];
|
||||
Log($"fields changed at index={Context.CurrentIndex}, rebuild from site={currentSiteId}");
|
||||
Context.Plan = _callbacks.BuildPlan(currentSiteId, Context.GoalSiteId, Context.DefaultSpeed);
|
||||
if (Context.Plan == null || Context.Plan.Nodes.Count == 0)
|
||||
{
|
||||
Fault($"rebuild failed from {currentSiteId} to {Context.GoalSiteId}", result);
|
||||
return true;
|
||||
}
|
||||
|
||||
Context.CurrentIndex = 0;
|
||||
Context.FieldsSignature = Context.Plan.FieldsSignature;
|
||||
Context.TaskId = _callbacks.AllocateTaskId();
|
||||
DispatchCurrentWindow();
|
||||
Context.Phase = Fass2TaskPhase.Moving;
|
||||
result.Phase = Context.Phase;
|
||||
result.Rebuilt = true;
|
||||
result.Dispatched = true;
|
||||
result.Message = $"rebuilt nodes={Context.Plan.Nodes.Count}, task={Context.TaskId}";
|
||||
Persist();
|
||||
return true;
|
||||
}
|
||||
|
||||
private void DispatchCurrentWindow()
|
||||
{
|
||||
var window = BuildDispatchWindow();
|
||||
if (window.Length == 0)
|
||||
{
|
||||
throw new InvalidOperationException("dispatch window is empty");
|
||||
}
|
||||
|
||||
_callbacks.SendNodes(window, Context.TaskId);
|
||||
Context.LastDispatchAt = DateTime.Now;
|
||||
Log($"dispatch window count={window.Length}, fromIndex={Context.CurrentIndex}, task={Context.TaskId}");
|
||||
}
|
||||
|
||||
private Fass2NodeMessage[] BuildDispatchWindow()
|
||||
{
|
||||
var nodes = Context.Plan.Nodes;
|
||||
var start = Context.CurrentIndex;
|
||||
var remaining = nodes.Count - start;
|
||||
var windowSize = Math.Max(1, Math.Min(LockCount, remaining));
|
||||
windowSize = Math.Min(windowSize, 10);
|
||||
var window = new Fass2NodeMessage[windowSize];
|
||||
for (var i = 0; i < windowSize; i++)
|
||||
{
|
||||
window[i] = nodes[start + i];
|
||||
}
|
||||
|
||||
return window;
|
||||
}
|
||||
|
||||
private Fass2NodeMessage GetExpectedNode()
|
||||
{
|
||||
return Context.Plan.Nodes[Context.CurrentIndex];
|
||||
}
|
||||
|
||||
private int ResolveCurrentSiteId(Fass2StateReport report)
|
||||
{
|
||||
if (Context.CurrentIndex > 0 && Context.Plan?.SiteIds != null &&
|
||||
Context.CurrentIndex < Context.Plan.SiteIds.Count)
|
||||
{
|
||||
return Context.Plan.SiteIds[Context.CurrentIndex];
|
||||
}
|
||||
|
||||
if (report?.Node != null && report.Node.Node != 0)
|
||||
{
|
||||
var site = _callbacks.ResolveSite(report.Node.Node);
|
||||
if (site != null)
|
||||
{
|
||||
return site.id;
|
||||
}
|
||||
}
|
||||
|
||||
return Context.StartSiteId;
|
||||
}
|
||||
|
||||
private bool ShouldResend()
|
||||
{
|
||||
return Context.LastDispatchAt == DateTime.MinValue ||
|
||||
(DateTime.Now - Context.LastDispatchAt).TotalMilliseconds >= ResendIntervalMs;
|
||||
}
|
||||
|
||||
private bool ShouldSendActionPatch()
|
||||
{
|
||||
return Context.LastActionSentAt == DateTime.MinValue ||
|
||||
(DateTime.Now - Context.LastActionSentAt).TotalMilliseconds >= ActionRetryIntervalMs;
|
||||
}
|
||||
|
||||
private Fass2TaskTickResult Fault(string reason, Fass2TaskTickResult result)
|
||||
{
|
||||
Context.Phase = Fass2TaskPhase.Fault;
|
||||
Context.FaultReason = reason;
|
||||
result.Phase = Context.Phase;
|
||||
result.Faulted = true;
|
||||
result.Message = reason;
|
||||
Log($"task fault: {reason}");
|
||||
Persist();
|
||||
Faulted?.Invoke(Context, reason);
|
||||
_completionSource?.TrySetException(new InvalidOperationException(reason));
|
||||
return result;
|
||||
}
|
||||
|
||||
private void Persist()
|
||||
{
|
||||
_callbacks.Persist?.Invoke(Context);
|
||||
}
|
||||
|
||||
private void ClearPersisted()
|
||||
{
|
||||
_callbacks.ClearPersisted?.Invoke();
|
||||
}
|
||||
|
||||
private void Log(string message)
|
||||
{
|
||||
_callbacks.Log?.Invoke(message);
|
||||
}
|
||||
|
||||
private TaskCompletionSource<int> ResetCompletionSource()
|
||||
{
|
||||
_completionSource = new TaskCompletionSource<int>(TaskCreationOptions.RunContinuationsAsynchronously);
|
||||
return _completionSource;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,84 @@
|
||||
using SimpleCore.PropType;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Globalization;
|
||||
|
||||
namespace StandardScene.Magnetic.Tasking
|
||||
{
|
||||
internal sealed class Fass2TrackMotionData
|
||||
{
|
||||
public float Speed { get; set; } = 0.2f;
|
||||
public bool Reverse { get; set; }
|
||||
public float CarDirectionBias { get; set; }
|
||||
public bool EnableCarAbsoluteDirection { get; set; }
|
||||
public float CarAbsoluteDirection { get; set; }
|
||||
public byte? Byroad { get; set; }
|
||||
public byte? Direction { get; set; }
|
||||
public byte? Orientation { get; set; }
|
||||
}
|
||||
|
||||
internal static class Fass2TrackFieldReader
|
||||
{
|
||||
public static Fass2TrackMotionData Read(Track track)
|
||||
{
|
||||
var data = new Fass2TrackMotionData();
|
||||
if (track?.fields == null)
|
||||
{
|
||||
return data;
|
||||
}
|
||||
|
||||
if (track.fields.TryGetValue("Speed", out var speedText) &&
|
||||
float.TryParse(speedText, NumberStyles.Float, CultureInfo.InvariantCulture, out var speed))
|
||||
{
|
||||
data.Speed = speed;
|
||||
}
|
||||
|
||||
if (track.fields.TryGetValue("Reverse", out var reverseText) &&
|
||||
bool.TryParse(reverseText, out var reverse))
|
||||
{
|
||||
data.Reverse = reverse;
|
||||
}
|
||||
|
||||
if (track.fields.TryGetValue("CarDirectionBias", out var biasText) &&
|
||||
float.TryParse(biasText, NumberStyles.Float, CultureInfo.InvariantCulture, out var bias))
|
||||
{
|
||||
data.CarDirectionBias = bias;
|
||||
}
|
||||
|
||||
if (track.fields.TryGetValue("EnableCarAbsoluteDirection", out var enableAbsText) &&
|
||||
bool.TryParse(enableAbsText, out var enableAbs))
|
||||
{
|
||||
data.EnableCarAbsoluteDirection = enableAbs;
|
||||
}
|
||||
|
||||
if (track.fields.TryGetValue("CarAbsoluteDirection", out var absText) &&
|
||||
float.TryParse(absText, NumberStyles.Float, CultureInfo.InvariantCulture, out var absDir))
|
||||
{
|
||||
data.CarAbsoluteDirection = absDir;
|
||||
}
|
||||
|
||||
data.Byroad = TryReadByte(track.fields, "Fass2_Byroad");
|
||||
if (data.Byroad == null && track.fields.TryGetValue("magSelect", out var magSelect) &&
|
||||
byte.TryParse(magSelect, NumberStyles.Integer, CultureInfo.InvariantCulture, out var magValue))
|
||||
{
|
||||
data.Byroad = magValue;
|
||||
}
|
||||
|
||||
data.Direction = TryReadByte(track.fields, "Fass2_Direction");
|
||||
data.Orientation = TryReadByte(track.fields, "Fass2_Orientation");
|
||||
return data;
|
||||
}
|
||||
|
||||
private static byte? TryReadByte(Dictionary<string, string> fields, string key)
|
||||
{
|
||||
if (!fields.TryGetValue(key, out var text) || string.IsNullOrWhiteSpace(text))
|
||||
{
|
||||
return null;
|
||||
}
|
||||
|
||||
return byte.TryParse(text, NumberStyles.Integer, CultureInfo.InvariantCulture, out var value)
|
||||
? value
|
||||
: (byte?)null;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,26 @@
|
||||
using System;
|
||||
using System.Threading;
|
||||
using System.Threading.Tasks;
|
||||
|
||||
namespace StandardScene.Magnetic.Tasking
|
||||
{
|
||||
/// <summary>
|
||||
/// FASS 2.0 车型与 LoopMission 衔接接口。
|
||||
/// LoopMission 仍通过 <c>goalSite</c> 标签分配目标；车型侧负责一次全程任务执行。
|
||||
/// </summary>
|
||||
public interface IFass2LoopCar
|
||||
{
|
||||
bool IsTaskIdle { get; }
|
||||
|
||||
bool HasGoalSite { get; }
|
||||
|
||||
int? GoalSiteId { get; }
|
||||
|
||||
/// <summary>执行 <c>goalSite</c>（或指定目标）对应的全程 FASS 2.0 任务。</summary>
|
||||
Task ExecuteGoalTaskAsync(int? goalSiteId = null, double defaultSpeed = -1,
|
||||
CancellationToken cancellationToken = default);
|
||||
|
||||
/// <summary>任务到达终点后触发，参数为到达站 ID。</summary>
|
||||
event Action<int> TaskCompleted;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,61 @@
|
||||
using System;
|
||||
using System.Collections.Concurrent;
|
||||
using System.IO;
|
||||
using System.Text;
|
||||
|
||||
namespace StandardScene.Magnetic.Tasking
|
||||
{
|
||||
/// <summary>
|
||||
/// MagFass2Car 本地文件日志，按协议车号分目录：logs/car{VehicleCode}/magfass2_yyyyMMdd.log
|
||||
/// </summary>
|
||||
public static class MagFass2CarFileLogger
|
||||
{
|
||||
private static readonly ConcurrentDictionary<ushort, object> CarLocks = new ConcurrentDictionary<ushort, object>();
|
||||
private static string _baseDirectory = "logs";
|
||||
private static bool _enabled = true;
|
||||
|
||||
public static void Configure(string baseDirectory, bool enabled)
|
||||
{
|
||||
_baseDirectory = string.IsNullOrWhiteSpace(baseDirectory) ? "logs" : baseDirectory.Trim();
|
||||
_enabled = enabled;
|
||||
}
|
||||
|
||||
public static string GetCarLogDirectory(ushort vehicleCode)
|
||||
{
|
||||
return Path.Combine(ResolveBaseDirectory(), $"car{vehicleCode}");
|
||||
}
|
||||
|
||||
public static string GetCurrentLogFilePath(ushort vehicleCode)
|
||||
{
|
||||
var fileName = $"magfass2_{DateTime.Now:yyyyMMdd}.log";
|
||||
return Path.Combine(GetCarLogDirectory(vehicleCode), fileName);
|
||||
}
|
||||
|
||||
public static void Write(ushort vehicleCode, string message)
|
||||
{
|
||||
if (!_enabled || vehicleCode == 0 || string.IsNullOrWhiteSpace(message))
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
var carLock = CarLocks.GetOrAdd(vehicleCode, _ => new object());
|
||||
lock (carLock)
|
||||
{
|
||||
var directory = GetCarLogDirectory(vehicleCode);
|
||||
Directory.CreateDirectory(directory);
|
||||
var path = GetCurrentLogFilePath(vehicleCode);
|
||||
File.AppendAllText(
|
||||
path,
|
||||
$"[{DateTime.Now:yyyy-MM-dd HH:mm:ss.fff}] {message}{Environment.NewLine}",
|
||||
new UTF8Encoding(encoderShouldEmitUTF8Identifier: false));
|
||||
}
|
||||
}
|
||||
|
||||
private static string ResolveBaseDirectory()
|
||||
{
|
||||
return Path.IsPathRooted(_baseDirectory)
|
||||
? _baseDirectory
|
||||
: Path.Combine(AppContext.BaseDirectory, _baseDirectory);
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user