2.0协议完善加简单交管配置
This commit is contained in:
@@ -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);
|
||||
}
|
||||
}
|
||||
}
|
||||
+3
-3
@@ -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);
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user