2.0协议完善加简单交管配置

This commit is contained in:
ykkokluo
2026-08-14 09:49:23 +08:00
parent a28bc68e23
commit 1e780c2b65
51 changed files with 4013 additions and 393 deletions
@@ -7,15 +7,15 @@ namespace StandardScene.Magnetic.Tasking
{
/// <summary>
/// 比对期望节点与上报节点,判定到站与动作是否完成(对齐 backend <c>CarResponseService</c>)。
/// 默认只闭环机构类动作;轨迹字段(Speed/Orientation 等)需显式开启。
/// </summary>
public static class Fass2ActionResolver
{
private static readonly (string Name, Func<Fass2NodeMessage, byte> Get, Action<Fass2NodeMessage, byte> Set)[] ActionFields =
/// <summary>是否在停车站比对 Speed/Orientation/Direction/Byroad 等轨迹字段。</summary>
public static bool VerifyTrajectoryFields { get; set; }
private static readonly (string Name, Func<Fass2NodeMessage, byte> Get, Action<Fass2NodeMessage, byte> Set)[] MechanismFields =
{
("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),
@@ -28,6 +28,13 @@ namespace StandardScene.Magnetic.Tasking
("Shutdown", n => n.Shutdown, (n, v) => n.Shutdown = v)
};
private static readonly (string Name, Func<Fass2NodeMessage, byte> Get, Action<Fass2NodeMessage, byte> Set)[] TrajectoryFields =
{
("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)
};
public static bool IsAtNode(Fass2StateReport report, ushort targetNode)
{
return report?.Node != null && report.Node.Node == targetNode;
@@ -35,7 +42,7 @@ namespace StandardScene.Magnetic.Tasking
public static bool IsVehicleMoving(byte vehicleState)
{
return vehicleState == 1;
return vehicleState is 1 or 5;
}
public static bool RequiresActionWait(Fass2NodeMessage expected)
@@ -45,12 +52,28 @@ namespace StandardScene.Magnetic.Tasking
return false;
}
if (expected.StartStop is Fass2TaskBuilder.StartStopStop or Fass2TaskBuilder.StartStopPrecision)
if (expected.StartStop is Fass2TaskBuilder.StartStopStop
or Fass2TaskBuilder.StartStopPrecision
or Fass2TaskBuilder.StartStopControlStop
or Fass2TaskBuilder.StartStopControlStart)
{
return true;
}
foreach (var field in ActionFields)
foreach (var field in MechanismFields)
{
if (field.Get(expected) != 0)
{
return true;
}
}
if (!VerifyTrajectoryFields)
{
return false;
}
foreach (var field in TrajectoryFields)
{
if (field.Get(expected) != 0)
{
@@ -83,12 +106,16 @@ namespace StandardScene.Magnetic.Tasking
return true;
}
// 过站(StartStop=1) 只要求到点且非运行态,不要求 Orientation 等轨迹字段到位。
if (expected.StartStop == Fass2TaskBuilder.StartStopPass)
{
return true;
}
if (expected.StartStop == Fass2TaskBuilder.StartStopControlStop)
{
return false;
}
return ListPendingFields(expected, actual).Count == 0;
}
@@ -105,18 +132,25 @@ namespace StandardScene.Magnetic.Tasking
pending.Add("StartStop");
}
if (expected.Speed != 0 && actual.Speed != expected.Speed)
if (VerifyTrajectoryFields)
{
pending.Add("Speed");
}
foreach (var field in ActionFields)
{
if (field.Name == "StartStop")
if (expected.Speed != 0 && actual.Speed != expected.Speed)
{
continue;
pending.Add("Speed");
}
foreach (var field in TrajectoryFields)
{
var expectedValue = field.Get(expected);
if (expectedValue != 0 && field.Get(actual) != expectedValue)
{
pending.Add(field.Name);
}
}
}
foreach (var field in MechanismFields)
{
var expectedValue = field.Get(expected);
if (expectedValue != 0 && field.Get(actual) != expectedValue)
{
@@ -143,19 +177,27 @@ namespace StandardScene.Magnetic.Tasking
hasPatch = true;
}
if (expected.Speed != 0 && (actual == null || actual.Speed != expected.Speed))
if (VerifyTrajectoryFields)
{
patch.Speed = expected.Speed;
hasPatch = true;
}
foreach (var field in ActionFields)
{
if (field.Name == "StartStop")
if (expected.Speed != 0 && (actual == null || actual.Speed != expected.Speed))
{
continue;
patch.Speed = expected.Speed;
hasPatch = true;
}
foreach (var field in TrajectoryFields)
{
var expectedValue = field.Get(expected);
if (expectedValue != 0 && (actual == null || field.Get(actual) != expectedValue))
{
field.Set(patch, expectedValue);
hasPatch = true;
}
}
}
foreach (var field in MechanismFields)
{
var expectedValue = field.Get(expected);
if (expectedValue != 0 && (actual == null || field.Get(actual) != expectedValue))
{
@@ -0,0 +1,214 @@
using StandardScene.Magnetic.Protocol;
using System;
using System.Collections.Generic;
using System.Globalization;
namespace StandardScene.Magnetic.Tasking
{
/// <summary>
/// 管控停止(StartStop=12)按车放行:地图站点保持 12,仅改本车任务报文。
/// </summary>
public sealed class Fass2ControlAreaInfo
{
public int SiteId { get; set; }
public string AreaId { get; set; } = string.Empty;
public int[] AreaSiteIds { get; set; } = Array.Empty<int>();
public int Capacity { get; set; } = 1;
public byte ReleaseStartStop { get; set; } = Fass2TaskBuilder.StartStopControlStart;
}
public sealed class Fass2ControlOccupancy
{
public Fass2ControlOccupancy(int carId, IReadOnlyList<int> occupiedSiteIds)
{
CarId = carId;
OccupiedSiteIds = occupiedSiteIds ?? Array.Empty<int>();
}
public int CarId { get; }
public IReadOnlyList<int> OccupiedSiteIds { get; }
}
public sealed class Fass2ControlReleaseCheck
{
public bool CanRelease { get; set; }
public string AreaId { get; set; } = string.Empty;
public int Capacity { get; set; } = 1;
public int OthersInArea { get; set; }
public byte ReleaseStartStop { get; set; } = Fass2TaskBuilder.StartStopControlStart;
public string Reason { get; set; } = string.Empty;
}
public static class Fass2ControlAreaGate
{
private static readonly char[] SiteIdSeparators = { ',', ';', '|', ' ', '\t', '[', ']', '(', ')', '{', '}' };
public static bool IsControlStop(byte startStop)
{
return startStop == Fass2TaskBuilder.StartStopControlStop;
}
public static bool AllowsLeave(byte startStop)
{
return startStop == Fass2TaskBuilder.StartStopPass
|| startStop == Fass2TaskBuilder.StartStopControlStart;
}
public static Fass2ControlAreaInfo Resolve(Fass2SiteActionData data, int siteId)
{
data ??= new Fass2SiteActionData();
var areaSites = ParseSiteIds(data.ControlArea, siteId);
var capacity = data.ControlCapacity.GetValueOrDefault(1);
if (capacity < 1)
{
capacity = 1;
}
var release = data.ControlReleaseStartStop ?? Fass2TaskBuilder.StartStopControlStart;
if (release != Fass2TaskBuilder.StartStopPass &&
release != Fass2TaskBuilder.StartStopControlStart)
{
release = Fass2TaskBuilder.StartStopControlStart;
}
return new Fass2ControlAreaInfo
{
SiteId = siteId,
AreaSiteIds = areaSites,
AreaId = string.Join(",", areaSites),
Capacity = capacity,
ReleaseStartStop = release
};
}
public static int[] ParseSiteIds(string text, int fallbackSiteId)
{
var ids = new List<int>();
if (!string.IsNullOrWhiteSpace(text))
{
var parts = text.Split(SiteIdSeparators, StringSplitOptions.RemoveEmptyEntries);
foreach (var part in parts)
{
if (int.TryParse(part.Trim(), NumberStyles.Integer, CultureInfo.InvariantCulture, out var id) &&
id > 0 && !ids.Contains(id))
{
ids.Add(id);
}
}
}
if (ids.Count == 0 && fallbackSiteId > 0)
{
ids.Add(fallbackSiteId);
}
return ids.ToArray();
}
public static int LimitWindowCount(
IReadOnlyList<Fass2NodeMessage> nodes,
int startIndex,
int count,
int releasedIndex)
{
if (nodes == null || count <= 0 || startIndex < 0 || startIndex >= nodes.Count)
{
return 0;
}
var limited = Math.Min(count, nodes.Count - startIndex);
for (var i = 0; i < limited; i++)
{
var node = nodes[startIndex + i];
if (node != null && IsControlStop(node.StartStop) && releasedIndex != startIndex + i)
{
return i + 1;
}
}
return limited;
}
public static Fass2ControlReleaseCheck Evaluate(
int selfCarId,
Fass2ControlAreaInfo area,
IReadOnlyList<Fass2ControlOccupancy> occupancies)
{
area ??= new Fass2ControlAreaInfo();
if (area.AreaSiteIds == null || area.AreaSiteIds.Length == 0)
{
area.AreaSiteIds = area.SiteId > 0 ? new[] { area.SiteId } : Array.Empty<int>();
}
if (string.IsNullOrEmpty(area.AreaId))
{
area.AreaId = string.Join(",", area.AreaSiteIds);
}
if (area.Capacity < 1)
{
area.Capacity = 1;
}
var others = 0;
if (occupancies != null)
{
foreach (var occupancy in occupancies)
{
if (occupancy == null || occupancy.CarId == selfCarId)
{
continue;
}
if (OccupiesListedSites(occupancy.OccupiedSiteIds, area.AreaSiteIds))
{
others++;
}
}
}
var canRelease = others < area.Capacity;
return new Fass2ControlReleaseCheck
{
CanRelease = canRelease,
AreaId = area.AreaId,
Capacity = area.Capacity,
OthersInArea = others,
ReleaseStartStop = area.ReleaseStartStop == 0
? Fass2TaskBuilder.StartStopControlStart
: area.ReleaseStartStop,
Reason = canRelease
? "area clear"
: $"area occupied cars={others}, capacity={area.Capacity}"
};
}
public static bool OccupiesListedSites(IReadOnlyList<int> occupiedSiteIds, IReadOnlyList<int> areaSiteIds)
{
if (occupiedSiteIds == null || occupiedSiteIds.Count == 0 ||
areaSiteIds == null || areaSiteIds.Count == 0)
{
return false;
}
for (var i = 0; i < occupiedSiteIds.Count; i++)
{
var occupied = occupiedSiteIds[i];
if (occupied <= 0)
{
continue;
}
for (var j = 0; j < areaSiteIds.Count; j++)
{
if (areaSiteIds[j] == occupied)
{
return true;
}
}
}
return false;
}
}
}
@@ -0,0 +1,79 @@
using SimpleCore;
using SimpleCore.PropType;
using SimpleLite.RCS.CarTypes;
using System;
using System.Collections.Generic;
using System.Linq;
namespace StandardScene.Magnetic.Tasking
{
/// <summary>
/// 按 holdingLocks + 物理 siteID 统计管控站点列表上的他车占用。
/// </summary>
internal static class Fass2ControlAreaOccupancy
{
public static Fass2ControlReleaseCheck Check(Car self, int controlSiteId)
{
var site = SimpleLib.GetSite(controlSiteId);
var area = Fass2ControlAreaGate.Resolve(Fass2SiteFieldReader.Read(site), controlSiteId);
var occupancies = new List<Fass2ControlOccupancy>();
IEnumerable<AbstractCar> cars;
try
{
cars = SimpleLib.GetAllCars();
}
catch (Exception ex)
{
return new Fass2ControlReleaseCheck
{
CanRelease = false,
AreaId = area.AreaId,
Capacity = area.Capacity,
ReleaseStartStop = area.ReleaseStartStop,
Reason = "car list unavailable: " + ex.Message
};
}
if (cars != null)
{
foreach (var other in cars.OfType<Car>())
{
if (other == null)
{
continue;
}
occupancies.Add(new Fass2ControlOccupancy(other.id, CollectOccupiedSiteIds(other)));
}
}
return Fass2ControlAreaGate.Evaluate(self?.id ?? 0, area, occupancies);
}
private static IReadOnlyList<int> CollectOccupiedSiteIds(Car car)
{
var ids = new List<int>();
if (car.siteID > 0)
{
ids.Add(car.siteID);
}
var holding = car.status?.holdingLocks;
if (holding == null)
{
return ids;
}
foreach (var lockId in holding)
{
if (lockId > 0 && !ids.Contains(lockId))
{
ids.Add(lockId);
}
}
return ids;
}
}
}
@@ -5,7 +5,7 @@ namespace StandardScene.Magnetic.Tasking
{
internal static class Fass2PathFinder
{
public static List<int> GetSitesBetween(int startSiteId, int endSiteId)
public static List<int> GetSitesBetweenBfs(int startSiteId, int endSiteId)
{
if (startSiteId == endSiteId)
{
@@ -0,0 +1,127 @@
using System.Collections.Generic;
using System.Linq;
namespace StandardScene.Magnetic.Tasking
{
/// <summary>
/// 断线重连后,用当前占点匹配 tasklist 链路的结果。
/// </summary>
public sealed class Fass2ChainMatch
{
public int TaskId { get; set; }
public int TaskPriority { get; set; }
public int CurrentSiteId { get; set; }
public int TargetSiteId { get; set; }
public int IndexInPath { get; set; }
public int DistanceToTarget { get; set; }
public bool IsAtStartPoint { get; set; }
public bool IsAtEndPoint { get; set; }
public IReadOnlyList<int> FullPath { get; set; } = new List<int>();
public IReadOnlyList<int> RemainingPath { get; set; } = new List<int>();
public override string ToString()
{
var position = IsAtStartPoint ? "start" : (IsAtEndPoint ? "end" : "mid");
return $"task={TaskId} {CurrentSiteId}->{TargetSiteId} pos={position} remain={DistanceToTarget} prio={TaskPriority}";
}
}
/// <summary>
/// 一站多链路时的固定裁决:原 goal 仍在剩余路径 → 起点 → 优先级 → 距目标更近。
/// </summary>
public static class Fass2ReconnectChainSelector
{
public static bool TrySelect(
IReadOnlyList<Fass2ChainMatch> matches,
int? preferGoalSiteId,
out Fass2ChainMatch selected,
out string reason)
{
selected = null;
reason = "no chain";
if (matches == null || matches.Count == 0)
{
return false;
}
var candidates = matches.Where(m => m != null && m.TargetSiteId > 0).ToList();
if (candidates.Count == 0)
{
return false;
}
if (preferGoalSiteId != null && preferGoalSiteId.Value > 0)
{
var keepGoal = candidates
.Where(m => RemainingContains(m, preferGoalSiteId.Value))
.ToList();
if (keepGoal.Count == 1)
{
selected = keepGoal[0];
reason = $"preferGoal={preferGoalSiteId.Value}";
return true;
}
if (keepGoal.Count > 1)
{
candidates = keepGoal;
reason = $"preferGoal={preferGoalSiteId.Value} narrowed={keepGoal.Count}";
}
}
var ranked = candidates
.OrderByDescending(m => m.IsAtStartPoint ? 1 : 0)
.ThenByDescending(m => m.TaskPriority)
.ThenBy(m => m.DistanceToTarget)
.ThenBy(m => m.TaskId)
.ToList();
if (ranked.Count == 0)
{
return false;
}
var best = ranked[0];
if (ranked.Count > 1)
{
var second = ranked[1];
var tied = best.IsAtStartPoint == second.IsAtStartPoint &&
best.TaskPriority == second.TaskPriority &&
best.DistanceToTarget == second.DistanceToTarget;
if (tied && best.TargetSiteId != second.TargetSiteId)
{
reason = $"ambiguous {best} vs {second}";
return false;
}
}
selected = best;
if (string.IsNullOrEmpty(reason) || reason == "no chain")
{
reason = $"selected {best}";
}
else
{
reason = $"{reason}; selected {best}";
}
return true;
}
private static bool RemainingContains(Fass2ChainMatch match, int siteId)
{
if (match.RemainingPath != null)
{
for (var i = 0; i < match.RemainingPath.Count; i++)
{
if (match.RemainingPath[i] == siteId)
{
return true;
}
}
}
return match.TargetSiteId == siteId;
}
}
}
@@ -0,0 +1,210 @@
using SimpleCore;
using SimpleCore.Compiler;
using SimpleCore.Library;
using SimpleCore.PropType;
using System;
using System.Collections.Generic;
namespace StandardScene.Magnetic.Tasking
{
/// <summary>
/// 从站点序列构建 <see cref="SegmentPlan"/>,供交管 Forecast 与任务下发共用同一路径。
/// </summary>
internal static class Fass2RouteHelper
{
public static List<int> GetSitesBetween(int startSiteId, int endSiteId, AbstractCar car)
{
if (startSiteId == endSiteId)
{
return new List<int> { startSiteId };
}
if (car != null)
{
var startSite = SimpleLib.GetSite(startSiteId);
var endSite = SimpleLib.GetSite(endSiteId);
if (startSite != null && endSite != null)
{
var plan = new SegmentPlan { usingCar = car };
plan.fields["allow_destination_on_route"] = "true";
plan.FindRoute(startSite, endSite);
var sites = ExtractSiteIds(plan);
if (sites.Count > 0)
{
return sites;
}
}
}
return Fass2PathFinder.GetSitesBetweenBfs(startSiteId, endSiteId);
}
public static void ForecastTrafficSequence(AbstractCar car, IReadOnlyList<int> siteIds, int fromIndex)
{
if (car == null || siteIds == null || siteIds.Count == 0)
{
throw new ArgumentException("traffic forecast requires car and siteIds");
}
fromIndex = Math.Max(0, Math.Min(fromIndex, siteIds.Count - 1));
if (fromIndex >= siteIds.Count - 1)
{
return;
}
var routeSiteIds = new List<int>();
for (var i = fromIndex; i < siteIds.Count; i++)
{
routeSiteIds.Add(siteIds[i]);
}
try
{
var plan = BuildSegmentPlan(car, routeSiteIds);
var program = plan.Compile("fass2-traffic", false);
program.Forecast();
}
catch (Exception ex) when (IsRecoverableForecastFailure(ex))
{
// 双车 Multi-car search Exhausted 时降级:只建 pending/seqScope,交由 TryLock 现场占点
ApplyManualTrafficSequence(car, routeSiteIds);
Diagnosis.Post(
$"Mag2Car forecast fallback manual sequence car={car.id}: {ex.Message}",
"Mag2Car",
true);
}
}
/// <summary>
/// 不走 SimpleCore 多车 Forecast,直接写入交管序列(持锁起点已由 TrafficReset 保证)。
/// </summary>
public static void ApplyManualTrafficSequence(AbstractCar car, IReadOnlyList<int> routeSiteIds)
{
if (car == null || routeSiteIds == null || routeSiteIds.Count == 0)
{
return;
}
var scope = new int[routeSiteIds.Count];
for (var i = 0; i < routeSiteIds.Count; i++)
{
scope[i] = routeSiteIds[i];
}
var pending = new int[Math.Max(0, scope.Length - 1)];
for (var i = 0; i < pending.Length; i++)
{
pending[i] = scope[i + 1];
}
car.status.seqScope = scope;
car.status.pendingLocks = pending;
car.status.seqPtr = scope.Length > 1 ? 1 : 0;
car.status.escape = Array.Empty<int>();
}
private static bool IsRecoverableForecastFailure(Exception ex)
{
for (var cur = ex; cur != null; cur = cur.InnerException)
{
var msg = cur.Message ?? string.Empty;
if (msg.IndexOf("Multi-car traffic search", StringComparison.OrdinalIgnoreCase) >= 0 ||
msg.IndexOf("Exhausted", StringComparison.OrdinalIgnoreCase) >= 0 ||
msg.IndexOf("BadForecast", StringComparison.OrdinalIgnoreCase) >= 0 ||
cur.GetType().Name.IndexOf("Forecast", StringComparison.OrdinalIgnoreCase) >= 0)
{
return true;
}
}
return false;
}
public static SegmentPlan BuildSegmentPlan(AbstractCar car, IReadOnlyList<int> siteIds)
{
if (car == null || siteIds == null || siteIds.Count == 0)
{
throw new ArgumentException("segment plan requires car and siteIds");
}
var plan = new SegmentPlan { usingCar = car };
plan.fields["allow_destination_on_route"] = "true";
var firstSite = SimpleLib.GetSite(siteIds[0]);
if (firstSite == null)
{
throw new InvalidOperationException($"site {siteIds[0]} not found");
}
plan.segments.Add(firstSite);
for (var i = 0; i < siteIds.Count - 1; i++)
{
var dstSite = SimpleLib.GetSite(siteIds[i + 1]);
if (dstSite == null)
{
throw new InvalidOperationException($"site {siteIds[i + 1]} not found");
}
var track = FindTrack(siteIds[i], siteIds[i + 1]);
if (track == null)
{
throw new InvalidOperationException($"no track from site {siteIds[i]} to {siteIds[i + 1]}");
}
plan.segments.Add(track);
plan.segments.Add(dstSite);
}
return plan;
}
public static List<int> ExtractSiteIds(SegmentPlan plan)
{
var sites = new List<int>();
if (plan?.segments == null)
{
return sites;
}
foreach (var segment in plan.segments)
{
if (segment is Site site)
{
sites.Add(site.id);
}
}
return sites;
}
public 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;
}
}
}
@@ -31,6 +31,15 @@ namespace StandardScene.Magnetic.Tasking
data.Shutdown = TryReadByte(site.fields, Fass2SiteFields.Shutdown);
data.PrecisionStop = TryReadBool(site.fields, Fass2SiteFields.PrecisionStop);
data.WaitMode = TryReadString(site.fields, Fass2SiteFields.WaitMode);
data.ControlArea = TryReadString(site.fields, Fass2SiteFields.ControlArea);
data.ControlCapacity = TryReadInt(site.fields, Fass2SiteFields.ControlCapacity);
data.ControlReleaseStartStop = TryReadByte(site.fields, Fass2SiteFields.ControlReleaseStartStop);
if (!data.ControlCapacity.HasValue)
{
TryFillRegionCapacityFallback(site.fields, data);
}
return data;
}
@@ -99,5 +108,40 @@ namespace StandardScene.Magnetic.Tasking
{
return fields.TryGetValue(key, out var text) ? text : null;
}
private static int? TryReadInt(Dictionary<string, string> fields, string key)
{
if (!fields.TryGetValue(key, out var text) || string.IsNullOrWhiteSpace(text))
{
return null;
}
if (int.TryParse(text, NumberStyles.Integer, CultureInfo.InvariantCulture, out var value))
{
return value;
}
return null;
}
private static void TryFillRegionCapacityFallback(Dictionary<string, string> fields, Fass2SiteActionData data)
{
foreach (var pair in fields)
{
if (string.IsNullOrEmpty(pair.Key) ||
!pair.Key.StartsWith("Region", StringComparison.OrdinalIgnoreCase))
{
continue;
}
if (int.TryParse(pair.Value, NumberStyles.Integer, CultureInfo.InvariantCulture, out var maxCount) &&
maxCount > 0)
{
data.ControlCapacity = maxCount;
}
return;
}
}
}
}
@@ -23,6 +23,9 @@ namespace StandardScene.Magnetic.Tasking
public const string Shutdown = "Fass2_Shutdown";
public const string PrecisionStop = "Fass2_PrecisionStop";
public const string WaitMode = "Fass2_WaitMode";
public const string ControlArea = "Fass2_ControlArea";
public const string ControlCapacity = "Fass2_ControlCapacity";
public const string ControlReleaseStartStop = "Fass2_ControlReleaseStartStop";
}
/// <summary>
@@ -46,5 +49,8 @@ namespace StandardScene.Magnetic.Tasking
public byte? Shutdown { get; set; }
public bool PrecisionStop { get; set; }
public string WaitMode { get; set; }
public string ControlArea { get; set; }
public int? ControlCapacity { get; set; }
public byte? ControlReleaseStartStop { get; set; }
}
}
@@ -3,22 +3,23 @@ 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> 的组包规则
/// 对齐 backend <c>CarRequestService.GetSendNodes</c>:边属性挂在节点<b>出边</b>上
/// </summary>
public static class Fass2TaskBuilder
{
public const byte StartStopPass = 1;
public const byte StartStopStop = 2;
public const byte StartStopControlStart = 11;
public const byte StartStopControlStop = 12;
public const byte StartStopPrecision = 22;
public static Fass2TaskPlan BuildPath(int startSiteId, int goalSiteId, Fass2TaskBuildOptions options,
Func<int, ushort> resolveNodeId)
Func<int, ushort> resolveNodeId, AbstractCar routingCar = null)
{
if (resolveNodeId == null)
{
@@ -26,7 +27,7 @@ namespace StandardScene.Magnetic.Tasking
}
options ??= new Fass2TaskBuildOptions();
var siteIds = Fass2PathFinder.GetSitesBetween(startSiteId, goalSiteId);
var siteIds = Fass2RouteHelper.GetSitesBetween(startSiteId, goalSiteId, routingCar);
if (siteIds.Count == 0)
{
throw new InvalidOperationException($"no path from site {startSiteId} to {goalSiteId}");
@@ -45,9 +46,17 @@ namespace StandardScene.Magnetic.Tasking
}
public static Fass2NodeMessage[] BuildSegment(int srcSiteId, int dstSiteId, Fass2TaskBuildOptions options,
Func<int, ushort> resolveNodeId)
Func<int, ushort> resolveNodeId, AbstractCar routingCar = null)
{
return BuildPath(srcSiteId, dstSiteId, options, resolveNodeId).Nodes.ToArray();
var plan = BuildPath(srcSiteId, dstSiteId, options, resolveNodeId, routingCar);
var nodes = plan.Nodes;
var array = new Fass2NodeMessage[nodes.Count];
for (var i = 0; i < nodes.Count; i++)
{
array[i] = nodes[i];
}
return array;
}
public static IReadOnlyList<Fass2NodeMessage[]> SplitBatches(IReadOnlyList<Fass2NodeMessage> nodes, int maxPerFrame = 10)
@@ -88,14 +97,15 @@ namespace StandardScene.Magnetic.Tasking
var site = SimpleLib.GetSite(siteId);
var siteAction = Fass2SiteFieldReader.Read(site);
var isLast = i == siteIds.Count - 1;
var prevSiteId = i > 0 ? siteIds[i - 1] : siteId;
var nextSiteId = isLast ? -1 : siteIds[i + 1];
Fass2TrackMotionData trackMotion = null;
Site prevSite = null;
if (i > 0)
Site currentSite = site;
Site nextSite = null;
if (!isLast)
{
prevSite = SimpleLib.GetSite(prevSiteId);
var track = FindTrack(prevSiteId, siteId);
nextSite = SimpleLib.GetSite(nextSiteId);
var track = Fass2RouteHelper.FindTrack(siteId, nextSiteId);
trackMotion = Fass2TrackFieldReader.Read(track);
}
@@ -103,9 +113,11 @@ namespace StandardScene.Magnetic.Tasking
{
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),
Distance = !isLast ? ComputeEdgeDistance(currentSite, nextSite) : (ushort)0,
Speed = !isLast
? SpeedToProtocol(trackMotion?.Speed ?? options.DefaultSpeed)
: (ushort)0,
Orientation = !isLast ? ResolveOrientation(currentSite, nextSite, trackMotion) : (byte)0,
Byroad = siteAction.Byroad ?? trackMotion?.Byroad ?? 0,
Direction = siteAction.Direction ?? trackMotion?.Direction ?? 0,
Lift = siteAction.Lift ?? 0,
@@ -185,42 +197,17 @@ namespace StandardScene.Magnetic.Tasking
return (ushort)Math.Min(ushort.MaxValue, Math.Max(1, Math.Round(length)));
}
private static ushort SpeedToProtocol(double trackSpeed, double carSpeed)
/// <summary>
/// 协议速度单位 0.1 m/min;地图 Speed 默认 m/min,≤1 视为旧版 m/s 兼容。
/// </summary>
public static ushort SpeedToProtocol(double speed)
{
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 (speed > 0 && speed <= 1)
{
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;
}
speed *= 60;
}
return null;
return (ushort)Math.Max(1, Math.Min(10000, Math.Round(speed * 10)));
}
}
}
@@ -36,6 +36,18 @@ namespace StandardScene.Magnetic.Tasking
public DateTime LastDispatchAt { get; set; } = DateTime.MinValue;
public ulong LastActionId { get; set; }
public DateTime LastActionSentAt { get; set; } = DateTime.MinValue;
/// <summary>交管窗口未齐、正在等锁;此期间不消耗移动超时预算。</summary>
public bool WaitingForTraffic { get; set; }
/// <summary>停在管控停止点等待按车放行;此期间不消耗移动超时预算。</summary>
public bool WaitingForRelease { get; set; }
/// <summary>本任务已对哪个路径下标发过放行报文;-1 表示尚未放行。</summary>
public int ControlReleasedIndex { get; set; } = -1;
/// <summary>最近一次规划的站点链路,供断线重连判断是否仍在原路径上。</summary>
public int[] RouteSiteIds { get; set; } = Array.Empty<int>();
}
public static class Fass2TaskPersistence
@@ -47,6 +59,7 @@ namespace StandardScene.Magnetic.Tasking
public const string TagTaskId = "Fass2Task_TaskId";
public const string TagFieldsSignature = "Fass2Task_FieldsSig";
public const string TagDefaultSpeed = "Fass2Task_DefaultSpeed";
public const string TagRouteSites = "Fass2Task_RouteSites";
public static void Save(TagSet tags, Fass2TaskContext context)
{
@@ -62,6 +75,7 @@ namespace StandardScene.Magnetic.Tasking
SetTag(tags, TagTaskId, context.TaskId.ToString());
SetTag(tags, TagFieldsSignature, context.FieldsSignature ?? string.Empty);
SetTag(tags, TagDefaultSpeed, context.DefaultSpeed.ToString(System.Globalization.CultureInfo.InvariantCulture));
SetTag(tags, TagRouteSites, FormatIntList(context.RouteSiteIds));
}
public static bool TryLoad(TagSet tags, out Fass2TaskContext context)
@@ -93,6 +107,7 @@ namespace StandardScene.Magnetic.Tasking
System.Globalization.CultureInfo.InvariantCulture,
out var defaultSpeed);
tags.TryGetValue(TagRouteSites, out var routeSitesText);
context = new Fass2TaskContext
{
Phase = phase,
@@ -101,7 +116,8 @@ namespace StandardScene.Magnetic.Tasking
CurrentIndex = currentIndex,
TaskId = taskId,
FieldsSignature = fieldsSignature ?? string.Empty,
DefaultSpeed = defaultSpeed
DefaultSpeed = defaultSpeed,
RouteSiteIds = ParseIntList(routeSitesText)
};
return true;
}
@@ -120,6 +136,7 @@ namespace StandardScene.Magnetic.Tasking
RemoveTag(tags, TagTaskId);
RemoveTag(tags, TagFieldsSignature);
RemoveTag(tags, TagDefaultSpeed);
RemoveTag(tags, TagRouteSites);
}
private static void SetTag(TagSet tags, string key, string value)
@@ -145,5 +162,30 @@ namespace StandardScene.Magnetic.Tasking
value = 0;
return tags.TryGetValue(key, out var text) && int.TryParse(text, out value);
}
private static string FormatIntList(int[] values)
{
return values == null || values.Length == 0 ? string.Empty : string.Join(",", values);
}
private static int[] ParseIntList(string text)
{
if (string.IsNullOrWhiteSpace(text))
{
return Array.Empty<int>();
}
var parts = text.Split(new[] { ',' }, StringSplitOptions.RemoveEmptyEntries);
var ids = new List<int>(parts.Length);
foreach (var part in parts)
{
if (int.TryParse(part.Trim(), out var id) && id > 0)
{
ids.Add(id);
}
}
return ids.ToArray();
}
}
}
@@ -6,7 +6,7 @@ namespace StandardScene.Magnetic.Tasking
public sealed class Fass2TaskBuildOptions
{
public bool UseTagValueAsNode { get; set; }
public double DefaultSpeed { get; set; } = 0.2;
public double DefaultSpeed { get; set; } = 12;
public double CarSpeed { get; set; } = 1;
public int MaxNodesPerFrame { get; set; } = 10;
}
File diff suppressed because it is too large Load Diff
@@ -0,0 +1,303 @@
using SimpleCore;
using SimpleCore.Compiler;
using SimpleCore.PropType;
using SimpleCore.Traffic;
using System;
using System.Collections.Generic;
using System.Linq;
namespace StandardScene.Magnetic.Tasking
{
/// <summary>
/// 方案 B′:用任务下发的 siteIds 经 SegmentPlan.Forecast 建立 seqScope/pendingLocks,再段级 TryLock/Leave。
/// </summary>
internal sealed class Fass2TrafficLocker
{
private readonly AbstractCar _car;
private readonly Action<string> _log;
public Fass2TrafficLocker(AbstractCar car, Action<string> log)
{
_car = car ?? throw new ArgumentNullException(nameof(car));
_log = log ?? (_ => { });
}
public void PrepareSequence(IReadOnlyList<int> siteIds, int fromIndex)
{
if (siteIds == null || siteIds.Count == 0)
{
throw new InvalidOperationException("traffic plan siteIds is empty");
}
fromIndex = Math.Max(0, Math.Min(fromIndex, siteIds.Count - 1));
var startSiteId = siteIds[fromIndex];
var startSite = SimpleLib.GetSite(startSiteId);
if (startSite == null)
{
throw new InvalidOperationException($"traffic start site invalid: {startSiteId}");
}
// TrafficReset 会 programs.Clear() 把 now 置空;TryLock 在真正占点前要求 programs.now != null
// 否则抛 Program obsoleted。环线拦截路径仍处在 move 脚本的 actualSendScript 中,需保留/重建锚点。
var keepProgram = _car.status.programs.now;
_log($"prepare reset start={startSiteId}, goal={siteIds[siteIds.Count - 1]}, fromIndex={fromIndex}, route=[{string.Join(",", siteIds)}]");
try
{
_car.TrafficReset(startSite, makeAvailable: true, strict: false);
}
catch (Exception ex) when (IsStartOccupiedByOtherCar(ex))
{
// 起点被他车占用时不能 Reset;若本车已持有该站则跳过 Reset 继续建序,否则交给上层等待。
if (Array.IndexOf(_car.status.holdingLocks, startSiteId) < 0)
{
throw new InvalidOperationException(
$"traffic start site {startSiteId} occupied by another car", ex);
}
_log($"prepare reset skipped, already holding start={startSiteId}, ex={ex.Message}");
}
EnsureProgramAnchor(keepProgram);
if (fromIndex >= siteIds.Count - 1)
{
LogTrafficState("prepare single-site");
return;
}
Fass2RouteHelper.ForecastTrafficSequence(_car, siteIds, fromIndex);
LogTrafficState("prepare forecast ok");
}
public void RebaseFrom(IReadOnlyList<int> siteIds, int fromIndex, int windowSize)
{
PrepareSequence(siteIds, fromIndex);
EnsureWindowLocked(siteIds, fromIndex, Math.Max(1, windowSize));
}
private static bool IsStartOccupiedByOtherCar(Exception ex)
{
for (var cur = ex; cur != null; cur = cur.InnerException)
{
var msg = cur.Message ?? string.Empty;
if (msg.IndexOf("already locks it", StringComparison.OrdinalIgnoreCase) >= 0 ||
msg.IndexOf("unavailable site", StringComparison.OrdinalIgnoreCase) >= 0)
{
return true;
}
}
return false;
}
public bool TryLockNext(int siteId)
{
if (!_car.status.usage.Get().scheduling)
{
throw new InvalidOperationException("abandoned");
}
if (Array.IndexOf(_car.status.holdingLocks, siteId) >= 0)
{
return true;
}
if (_car.status.pendingLocks.Length > 0 && _car.status.pendingLocks[0] != siteId)
{
// 窗口重入时可能再次请求已锁过的更早站点;交给 EnsureWindowLocked 跳过,避免 UDP 线程抛异常。
_log($"lock skip site={siteId}, expect pending0={_car.status.pendingLocks[0]}, holding=[{FormatInts(_car.status.holdingLocks)}]");
return false;
}
EnsureProgramAnchor(keepProgram: null);
if (TrafficControl.TryLock(_car, siteId))
{
_log($"lock ok site={siteId}, holding=[{FormatInts(_car.status.holdingLocks)}]");
return true;
}
_log($"lock wait site={siteId}, blockedBy={FormatBlocked()}, TCStat={FormatTcStat()}");
return false;
}
public void LeavePassed(int siteId)
{
if (_car.status.holdingLocks.Length <= 1)
{
_log($"leave skip site={siteId}, holding count={_car.status.holdingLocks.Length}");
return;
}
if (Array.IndexOf(_car.status.holdingLocks, siteId) < 0)
{
_log($"leave skip site={siteId}, not in holding");
return;
}
EnsureProgramAnchor(keepProgram: null);
TrafficControl.Leave(_car, siteId);
_log($"leave site={siteId}, holding=[{FormatInts(_car.status.holdingLocks)}]");
}
/// <summary>
/// 释放路径上严格位于 keepFromIndex 之前的持锁站点(当前站及前方窗口保留)。
/// </summary>
public void ReleaseLocksBehind(IReadOnlyList<int> siteIds, int keepFromIndex)
{
if (siteIds == null || siteIds.Count == 0 || _car.status.holdingLocks.Length <= 1)
{
return;
}
keepFromIndex = Math.Max(0, Math.Min(keepFromIndex, siteIds.Count - 1));
var holdingSnapshot = _car.status.holdingLocks.ToArray();
foreach (var siteId in holdingSnapshot)
{
if (_car.status.holdingLocks.Length <= 1)
{
break;
}
var index = IndexOfSite(siteIds, siteId);
if (index < 0 || index >= keepFromIndex)
{
continue;
}
LeavePassed(siteId);
}
}
private static int IndexOfSite(IReadOnlyList<int> siteIds, int siteId)
{
for (var i = 0; i < siteIds.Count; i++)
{
if (siteIds[i] == siteId)
{
return i;
}
}
return -1;
}
public int EnsureWindowLocked(IReadOnlyList<int> siteIds, int fromIndex, int windowSize)
{
if (siteIds == null || siteIds.Count == 0)
{
return 0;
}
fromIndex = Math.Max(0, Math.Min(fromIndex, siteIds.Count - 1));
windowSize = Math.Max(1, Math.Min(windowSize, 10));
var maxIndex = Math.Min(siteIds.Count - 1, fromIndex + windowSize - 1);
// 返回值必须是从 fromIndex 起的连续已占/新锁站数,供 0xB1 窗口切片使用。
// UDP Tick 会反复进入:已在 holding 的站直接计数,禁止再次对已消费的 pending 站 TryLock。
var lockedCount = 0;
for (var i = fromIndex; i <= maxIndex; i++)
{
var siteId = siteIds[i];
if (Array.IndexOf(_car.status.holdingLocks, siteId) >= 0)
{
lockedCount++;
continue;
}
if (_car.status.pendingLocks.Length == 0 || _car.status.pendingLocks[0] != siteId)
{
_log(
$"window gap site={siteId}, pending0={(_car.status.pendingLocks.Length > 0 ? _car.status.pendingLocks[0].ToString() : "-")}, holding=[{FormatInts(_car.status.holdingLocks)}]");
break;
}
if (!TryLockNext(siteId))
{
break;
}
lockedCount++;
}
if (lockedCount == 0)
{
lockedCount = 1;
_log($"window warn: no holding overlap index={fromIndex}, site={siteIds[fromIndex]}, holding=[{FormatInts(_car.status.holdingLocks)}]");
}
_log($"window locked={lockedCount}/{windowSize}, fromIndex={fromIndex}, holding=[{FormatInts(_car.status.holdingLocks)}], pending=[{FormatInts(_car.status.pendingLocks)}]");
return lockedCount;
}
public void FinalizeAtSite(int siteId)
{
_log($"finalize site={siteId}, holding=[{FormatInts(_car.status.holdingLocks)}], pending=[{FormatInts(_car.status.pendingLocks)}]");
}
/// <summary>
/// SimpleCore.TryLock 在写锁前检查 programs.now;为空则抛 Program obsoleted。
/// </summary>
private void EnsureProgramAnchor(CarProgram keepProgram)
{
if (_car.status.programs.now != null)
{
return;
}
if (keepProgram != null)
{
_car.status.programs.now = keepProgram;
_log($"restore programs.now after TrafficReset, name={keepProgram.name}, state={keepProgram.status.state}");
return;
}
var dummy = new CarProgram
{
name = $"fass2-traffic-anchor:{_car.id}",
plans = new[]
{
new SegmentPlan { usingCar = _car }
}
};
dummy.status.state = CarProgram.StatusEnum.Dummy;
_car.status.programs.now = dummy;
_log($"install dummy programs.now for TryLock, name={dummy.name}");
}
private void LogTrafficState(string prefix)
{
_log(
$"{prefix}, holding=[{FormatInts(_car.status.holdingLocks)}], pending=[{FormatInts(_car.status.pendingLocks)}], seqScope=[{FormatInts(_car.status.seqScope)}], seqPtr={_car.status.seqPtr}");
}
private string FormatBlocked()
{
if (_car.status.blockedBy == null || _car.status.blockedBy.Length == 0)
{
return "-";
}
return string.Join(",", _car.status.blockedBy.Select(p => $"{p.Item1}:{p.Item2}"));
}
private string FormatTcStat()
{
try
{
var tc = _car.status.TCStat;
return string.IsNullOrWhiteSpace(tc) ? "-" : tc;
}
catch
{
return "-";
}
}
private static string FormatInts(int[] values)
{
return values == null || values.Length == 0 ? string.Empty : string.Join(",", values);
}
}
}
@@ -6,9 +6,9 @@ using System.Text;
namespace StandardScene.Magnetic.Tasking
{
/// <summary>
/// MagFass2Car 本地文件日志,按协议车号分目录:logs/car{VehicleCode}/magfass2_yyyyMMdd.log
/// Mag2Car 本地文件日志,按协议车号分目录:logs/car{VehicleCode}/mag2_yyyyMMdd.log
/// </summary>
public static class MagFass2CarFileLogger
public static class Mag2CarFileLogger
{
private static readonly ConcurrentDictionary<ushort, object> CarLocks = new ConcurrentDictionary<ushort, object>();
private static string _baseDirectory = "logs";
@@ -27,7 +27,7 @@ namespace StandardScene.Magnetic.Tasking
public static string GetCurrentLogFilePath(ushort vehicleCode)
{
var fileName = $"magfass2_{DateTime.Now:yyyyMMdd}.log";
var fileName = $"mag2_{DateTime.Now:yyyyMMdd}.log";
return Path.Combine(GetCarLogDirectory(vehicleCode), fileName);
}