zhaowei.huang
a0dc1e6cd0
将 StandardScene 各插件的配置/监控窗体从 WinForms 迁移到 CycleGUI（删除 .Designer.cs/.resx，重写为 PanelBuilder 立即模式 UI，新增 CycleUiHelper 统一对话框）。 同时修复代码审核中的问题： - 后台文件写入加锁 + try/catch（ButtonBoxManager / DoorManager，对齐 LoopViewer.SaveTasks 模式） - CoderFieldsMetadata.cs 启用 #nullable enable，消除 CS8632 警告 - DummyCar 移除已废弃的 rightClickAction()/SetPosition() - CarRemoteHelper.OpenVehicleWebPage 的 Process.Start 加 try/catch - 重命名名不副实的 Mstsc()（现为打开网页） - 统一弃元命名为 _ - TrafficInterlockViewer 改用稳定 Id（GUID）做选择/编辑，替代行索引 - csproj 改用 $(CGUILibDir) 解析 CycleGUI，绝对路径收敛到 Directory.Build.props 构建：dotnet build StandardScene.sln → 0 错误，30 警告（均为历史遗留）。 注：static 单例状态重构（审核第 8 项）暂未处理，留待单独任务。
1067 lines
41 KiB
C#
1067 lines
41 KiB
C#
using Acornima.Ast;
|
||
using CommonUsage.Protocols.VDA5050.Messages;
|
||
using CommonUsage.Protocols.VDA5050.Objects;
|
||
using LessokajiWeaverUtilities.Utilities;
|
||
using MQTTnet;
|
||
using MQTTnet.Client;
|
||
using MQTTnet.Extensions.ManagedClient;
|
||
using MQTTnet.Protocol;
|
||
using Nancy.Routing;
|
||
using Newtonsoft.Json;
|
||
using SimpleLite;
|
||
using SimpleLite.RCS;
|
||
using SimpleLite.RCS.CarTypes;
|
||
using SimpleLite.RCS.Signal;
|
||
using SimpleLite.CADTools;
|
||
using SimpleLite.Props;
|
||
using SimpleLite.UI;
|
||
using SimpleCore;
|
||
using SimpleCore.Compiler;
|
||
using SimpleCore.Library;
|
||
using SimpleCore.PropType;
|
||
using StandardScene.Chained;
|
||
using System;
|
||
using System.Collections.Generic;
|
||
using System.ComponentModel;
|
||
using System.Drawing;
|
||
using System.Linq;
|
||
using System.Net.Http;
|
||
using System.Security.Policy;
|
||
using System.Threading;
|
||
using System.Threading.Tasks;
|
||
using static SimpleLite.RCS.CarTypes.ClumsyCar;
|
||
using Site = SimpleCore.PropType.Site;
|
||
|
||
namespace StandardScene.CarTypes
|
||
{
|
||
class VDA5050TrackField
|
||
{
|
||
public float Speed = -1;
|
||
public bool Reverse = false;
|
||
public int ReverseDst = -1;
|
||
public float[] typeInfo = [];
|
||
}
|
||
|
||
// VDA5050SiteField 已下沉至 Core（ArmCar.cs）：ArmCar 引用它而 ArmCar 留 Core，
|
||
// 故拆 VDA dll 时该字段类保留于 Core，本文件不再定义（仅注释中曾引用）。
|
||
|
||
|
||
[TemplateTrackCoderSettings(
|
||
priority = 0,
|
||
templateString = "agv.Go(${src.id},${dst.id},${track.id},${track.Speed},${track.Reverse || track.ReverseDst == dst.id},[${track.typeInfo}]);",
|
||
trackFields = typeof(VDA5050TrackField))]
|
||
|
||
//[TemplateSiteCoderSettings(
|
||
// priority = 1,
|
||
// useVerb = "dst.DI41",
|
||
// templateString = "agv.Wait();agv.ChangeDI41Signal();agv.WaitAO3Signal();agv.Wait();agv.Sleep(${dst.SleepTime});",
|
||
// siteFields = typeof(VDA5050SiteField))]
|
||
|
||
[CarType(Name = "VDA5050标准车")]
|
||
[I18N.DocumentTranslation(Name = "VDA5050 Car", locale = "en")]
|
||
public class VDA5050Car : Car
|
||
{
|
||
private static readonly MasterMQTTCommunication _mqttCommunication = new MasterMQTTCommunication();
|
||
|
||
[FieldMember] public string conf = "";
|
||
|
||
[FieldMember] public string SerialNumber = "";
|
||
|
||
/// <summary>
|
||
/// Total length of base routes sent from master control to AGV.
|
||
/// </summary>
|
||
[FieldMember] public float BaseLength = 5000;
|
||
|
||
/// <summary>
|
||
/// Total length of horizon routes sent from mater control to AGV.
|
||
/// </summary>
|
||
[FieldMember] public float HorizonLength = 5000;
|
||
|
||
private string _connectionStatus = "Offline"; // Default to "Offline"
|
||
private const string OrderTopic = "vda5050/orders";
|
||
private VDA5050Interface currentAgv;
|
||
HttpClient hc = new HttpClient();
|
||
|
||
public bool IsPaused = false;
|
||
public int AO3 = 0;
|
||
|
||
// Property for connection status
|
||
public string ConnectionStatus
|
||
{
|
||
get => _connectionStatus;
|
||
set
|
||
{
|
||
_connectionStatus = value;
|
||
|
||
// Update lstatus based on the connection status
|
||
if (_connectionStatus == "ONLINE")
|
||
{
|
||
lstatus = "Online";
|
||
}
|
||
else if (_connectionStatus == "OFFLINE")
|
||
{
|
||
lstatus = "Offline";
|
||
}
|
||
else if (_connectionStatus == "CONNECTIONBROKEN")
|
||
{
|
||
lstatus = "Connection Broken";
|
||
}
|
||
}
|
||
}
|
||
|
||
public static async Task<VDA5050Car> Create()
|
||
{
|
||
return new VDA5050Car()
|
||
{
|
||
lstatus = "Not Connected",
|
||
address = "127.0.0.1",
|
||
name = $"VDA5050标准车",
|
||
haveCoordination = true,
|
||
};
|
||
}
|
||
|
||
[JsonIgnore]public bool Listened = false;
|
||
public override void keepAlive()
|
||
{
|
||
// MasterMQTTCommunication _mqttCommunication = new MasterMQTTCommunication();
|
||
|
||
// _mqttCommunication.SubscribeTo<stateMessage>(UpdateState,"vda5050/state").GetAwaiter().GetResult();
|
||
// _mqttCommunication.SubscribeTo<stateMessage>(UpdatePosition, "vda5050/visualization").GetAwaiter().GetResult();
|
||
// var test = new MasterControlMQTTTest();
|
||
// _mqttCommunication.TestPublishTo();
|
||
// if(ConnectionStatus == "Online")
|
||
// _mqttCommunication.SubscribeToVisualizationTopic();
|
||
if (!Listened)
|
||
{
|
||
SetupListeners();
|
||
Listened = true;
|
||
}
|
||
// StartStateProcessing();
|
||
// SetupInstActionListeners();
|
||
lock(RouteCache) ProcessCacheAndSendOrderMessage();
|
||
|
||
//GetVDA5050StateFromC();
|
||
// else Diagnosis.Post(" WARNING!! AGV is offline. Orders will not be sent");
|
||
}
|
||
|
||
public async Task GetVDA5050StateFromC()
|
||
{
|
||
try
|
||
{
|
||
//string jsonResponse1 = await hc.GetStringAsync($"http://{this.address}:8008/getStat");
|
||
//var data1 = JsonConvert.DeserializeObject<System.Collections.Generic.Dictionary<string, string>>(jsonResponse1);
|
||
string jsonResponse1 = await hc.GetStringAsync($"http://{this.address}:8008/getStat");
|
||
var data1 = JsonConvert.DeserializeObject<System.Collections.Generic.Dictionary<string, string>>(jsonResponse1);
|
||
if (data1.ContainsKey("AO3"))
|
||
{
|
||
string AO3Str = data1["AO3"];
|
||
AO3 = int.Parse(AO3Str);
|
||
}
|
||
else Console.WriteLine("AO3 key not found in the response.");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
Diagnosis.Log($"GetVDA5050StateFromC 失败: {ex.Message}", "VDA5050", true);
|
||
}
|
||
}
|
||
|
||
// public override Task actualSendScript(string script)
|
||
// {
|
||
// var tsk = new Task<object>(() =>
|
||
// {
|
||
// SelfEvaluating(new VDA5050Interface(id), script);
|
||
// return null;
|
||
// });
|
||
// tsk.Start();
|
||
// return tsk;
|
||
// }
|
||
public bool running = false;
|
||
public override async Task actualSendScript(string script)
|
||
{
|
||
script += "\nagv.Wait(1);\n";
|
||
Diagnosis.Log($"{script}", "script", true);
|
||
if (running)
|
||
throw new Exception($"dummy car {id} already running script");
|
||
running = true;
|
||
|
||
try
|
||
{
|
||
route = new int[0];
|
||
AppendDebug($"VDA5050 car {id} use jint for simulation");
|
||
lock(RouteCache)currentAgv = new VDA5050Interface(id);
|
||
var finish = false;
|
||
var tcs = new TaskCompletionSource<int>();
|
||
new Thread(() => {
|
||
try
|
||
{
|
||
Diagnosis.Log($"start evaluate","script",true);
|
||
SelfEvaluating(currentAgv, script);
|
||
Diagnosis.Log($"end evaluate", "script", true);
|
||
tcs.SetResult(1);
|
||
finish = true;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
tcs.SetException(ex);
|
||
}
|
||
}
|
||
)
|
||
{ Name = $"eva_{name}({id}):{status.programs.now.name}" }.Start();
|
||
async void monitor()
|
||
{
|
||
await Task.Run(() =>
|
||
{
|
||
while (true)
|
||
{
|
||
if (status.holdingLocks.Length > 0 && status.holdingLocks.Last() == GetLastSite() || finish)
|
||
{
|
||
Diagnosis.Log($"{script} {status.holdingLocks.Last()} {GetLastSite()} {finish}","script",true);
|
||
tcs.SetResult(1);
|
||
break;
|
||
}
|
||
Thread.Sleep(50);
|
||
}
|
||
});
|
||
}
|
||
|
||
// monitor();
|
||
await tcs.Task;
|
||
// await currentAgv.WaitAsync();
|
||
|
||
Console.WriteLine($"{name}({id}) self evaluating script completed");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
AppendDebug($"simulation error:{ExceptionFormatter.FormatEx(ex)}");
|
||
Console.WriteLine($"* {id} evaluating script failed, ex:{ExceptionFormatter.FormatEx(ex)}");
|
||
running = false;
|
||
throw;
|
||
}
|
||
|
||
running = false;
|
||
}
|
||
// public bool Restarting = false;
|
||
private void ProcessCacheAndSendOrderMessage()
|
||
{
|
||
try
|
||
{
|
||
int currentBase;
|
||
bool generateNewOrder = false;
|
||
|
||
lock (RouteCache)
|
||
{
|
||
Console.WriteLine("RouteCache.Count: " + RouteCache.Count);
|
||
if (RouteCache.Count==0)
|
||
{
|
||
// Diagnosis.Post($"RouteCache.Count :{0}");
|
||
return;
|
||
};
|
||
lock (_receivedLastConfirmedRoute)
|
||
{
|
||
Console.WriteLine("_receivedOrderId: " + _receivedOrderId + " OrderId: " + OrderId);
|
||
Console.WriteLine(_receivedOrderId == OrderId);
|
||
if (_receivedOrderId == OrderId)
|
||
{
|
||
if (_receivedLastConfirmedRoute.Count == 0)
|
||
{
|
||
if (RouteCache.Count>0 && _receivedLastNodeSequenceId == RouteCache.Last().Item.sequenceId)
|
||
_toTraverseSequenceId = RouteCache.Count;
|
||
else
|
||
{
|
||
_toTraverseSequenceId = _receivedLastNodeSequenceId;
|
||
// _toTraverseSequenceId = 0;
|
||
}
|
||
}
|
||
else _toTraverseSequenceId = (int)_receivedLastConfirmedRoute[0].sequenceId;
|
||
|
||
Console.WriteLine("_receivedLastConfirmedRoute.Count: " + _receivedLastConfirmedRoute.Count);
|
||
Console.WriteLine("_toTraverseSequenceId: " + _toTraverseSequenceId);
|
||
for (var i = 0; i < _toTraverseSequenceId; ++i)//完成的路径，节点
|
||
RouteCache[i].State = VDA5050Segment.SegState.Executed;
|
||
|
||
foreach (var seg in RouteCache.Where(seg =>
|
||
seg.State == VDA5050Segment.SegState.Executed))
|
||
{
|
||
var shouldTriggerFinish = false;
|
||
|
||
if (seg.Item.sequenceId == RouteCache.Count - 1) shouldTriggerFinish = true;
|
||
else if (seg.IsNode() && !seg.FinishTriggered && seg.Item.sequenceId + 1<RouteCache.Count&& RouteCache[(int)(seg.Item.sequenceId + 1)].State == VDA5050Segment.SegState.Executed)
|
||
shouldTriggerFinish = true;
|
||
|
||
if (shouldTriggerFinish && !seg.FinishTriggered)
|
||
{
|
||
seg.FinishToken.SetResult(1);
|
||
seg.FinishTriggered = true;
|
||
}
|
||
}
|
||
|
||
foreach (var item in _receivedLastConfirmedRoute)
|
||
{
|
||
Console.WriteLine("item.released: " + item.released +
|
||
" item.sequenceId:" + item.sequenceId);
|
||
if (item.released && RouteCache[(int)item.sequenceId].State ==
|
||
VDA5050Segment.SegState.BaseSending)
|
||
RouteCache[(int)item.sequenceId].State =
|
||
VDA5050Segment.SegState.BaseAcknowledged;
|
||
}
|
||
}
|
||
|
||
currentBase = RouteCache.FindLastIndex(rr =>
|
||
rr.State == VDA5050Segment.SegState.BaseSending ||
|
||
rr.State == VDA5050Segment.SegState.BaseAcknowledged||rr.State == VDA5050Segment.SegState.Executed);
|
||
|
||
var accumulatedHorizon = 0f;
|
||
for (var currentHorizon = currentBase + 1; currentHorizon < RouteCache.Count; ++currentHorizon)
|
||
{
|
||
RouteCache[currentHorizon].State = VDA5050Segment.SegState.HorizonSending;
|
||
|
||
if (RouteCache[currentHorizon].IsNode()) continue;
|
||
accumulatedHorizon += RouteCache[currentHorizon].GetTrackLength();
|
||
if (accumulatedHorizon >= HorizonLength) break;
|
||
}
|
||
|
||
foreach (var item in _receivedLastConfirmedRoute)
|
||
{
|
||
if (item.released) continue;
|
||
|
||
if (RouteCache[(int)item.sequenceId].State ==
|
||
VDA5050Segment.SegState.HorizonSending)
|
||
RouteCache[(int)item.sequenceId].State =
|
||
VDA5050Segment.SegState.HorizonAcknowledged;
|
||
}
|
||
}
|
||
|
||
if (currentBase > _lastBaseNodeSequenceId)
|
||
{
|
||
Diagnosis.Log($"\n{RouteCache.Display()}", "Interface", true);
|
||
Diagnosis.Log($"lastBase:{_lastBaseNodeSequenceId} curBase:{currentBase}","Interface",true);
|
||
_taskUpdateId++;
|
||
generateNewOrder = true;
|
||
}
|
||
}
|
||
|
||
RouteCacheViewer?.UpdateText($"{DateTime.Now:HH:mm:ss-fff}\n\n{RouteCache.Display()}");
|
||
|
||
void PrintOrder(orderMessage om)
|
||
{
|
||
Diagnosis.Post($"order{om.orderId}-{om.orderUpdateId}:\n" +
|
||
$"nodes: {string.Join(" ", om.nodes.Select(nn => $"{nn.nodeId}({nn.released})"))}\n" +
|
||
$"edges: {string.Join(" ", om.edges.Select(ee => $"{ee.edgeId}({ee.released})"))}",
|
||
"orders", true);
|
||
}
|
||
var startTime = DateTime.Now;
|
||
if (generateNewOrder)
|
||
{
|
||
var activeItems = RouteCache.Skip(_lastBaseNodeSequenceId)
|
||
.Where(rr => rr.State != VDA5050Segment.SegState.Waiting).Select(rr => rr.Item).ToList();
|
||
|
||
var newOrder = new orderMessage()
|
||
{
|
||
orderId = OrderId,
|
||
orderUpdateId = (uint)_taskUpdateId,
|
||
nodes = activeItems.OfType<node>().ToArray(),
|
||
edges = activeItems.OfType<edge>().ToArray()
|
||
};
|
||
var newContent = JsonConvert.SerializeObject(newOrder, Formatting.Indented);
|
||
// Diagnosis.Post($"add order:", "orders", true);
|
||
// PrintOrder(newOrder);
|
||
_orderList.Add((newContent, DateTime.MinValue));
|
||
startTime = DateTime.Now;
|
||
}
|
||
|
||
_lastBaseNodeSequenceId = currentBase;
|
||
|
||
Console.WriteLine("_orderList.Count: " + _orderList.Count);
|
||
if (_orderList.Count == 0) return;
|
||
|
||
var pendingOrder = _orderList[0];
|
||
if ((DateTime.Now - pendingOrder.SendTime).TotalSeconds < 0.5) return;
|
||
var (content, _) = pendingOrder;
|
||
var order = JsonConvert.DeserializeObject<orderMessage>(content);
|
||
|
||
var acknowledged = true;
|
||
lock (RouteCache)
|
||
{
|
||
// if (generateNewOrder) Console.WriteLine($"~~~哈哈 get Lock ,interval:{(DateTime.Now - startTime).TotalSeconds}s");
|
||
foreach (var item in order.nodes)
|
||
{
|
||
var state = RouteCache[(int)item.sequenceId].State;
|
||
Diagnosis.Post($"node: {item.nodeId}({item.sequenceId}), {state}");
|
||
if (item.released && state != VDA5050Segment.SegState.BaseAcknowledged &&
|
||
state != VDA5050Segment.SegState.Executed) acknowledged = false;
|
||
if (!item.released && state != VDA5050Segment.SegState.HorizonAcknowledged &&
|
||
state != VDA5050Segment.SegState.BaseSending) acknowledged = false;
|
||
}
|
||
foreach (var item in order.edges)
|
||
{
|
||
var state = RouteCache[(int)item.sequenceId].State;
|
||
Diagnosis.Post($"edge: {item.edgeId}({item.sequenceId}), {state}");
|
||
if (item.released && state != VDA5050Segment.SegState.BaseAcknowledged &&
|
||
state != VDA5050Segment.SegState.Executed) acknowledged = false;
|
||
if (!item.released && state != VDA5050Segment.SegState.HorizonAcknowledged &&
|
||
state != VDA5050Segment.SegState.BaseSending) acknowledged = false;
|
||
}
|
||
}
|
||
|
||
if (acknowledged) _orderList.RemoveAt(0);
|
||
else
|
||
{
|
||
//Diagnosis.Post($"order-{order.orderUpdateId} resend");
|
||
|
||
Diagnosis.Post("-------Sending order to AGV");
|
||
PrintOrder(order);
|
||
_mqttCommunication.PublishTo(content).GetAwaiter().GetResult();
|
||
// Post("order", content);
|
||
|
||
_orderList[0] = (content, DateTime.Now);//todo 这里可能会有时间差？车端还没反应好（或者处理对不上了），在发了一次？
|
||
}
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
// 打印异常信息和堆栈跟踪，帮助定位报错位置
|
||
Console.WriteLine("ProcessCacheAndSendOrderMessage 发生异常：");
|
||
Console.WriteLine("异常信息：" + ex.Message);
|
||
Console.WriteLine("堆栈跟踪：" + ex.StackTrace);
|
||
// throw;
|
||
}
|
||
|
||
|
||
}
|
||
|
||
private int _lastBaseNodeSequenceId = -1;
|
||
|
||
private List<(string Content, DateTime SendTime)> _orderList = new();
|
||
|
||
/// <summary>
|
||
/// Route state stored by master control.
|
||
/// </summary>
|
||
internal VDA5050SegmentsCache RouteCache = new();
|
||
|
||
public void VDAReset()
|
||
{
|
||
lock (RouteCache)
|
||
lock (_receivedLastConfirmedRoute)
|
||
{
|
||
RouteCache = new();
|
||
_receivedLastNodeSequenceId = -1;
|
||
_toTraverseSequenceId = -1;
|
||
_receivedLastConfirmedRoute = [];
|
||
_lastBaseNodeSequenceId = -1;
|
||
TaskId = 0;
|
||
running = false;
|
||
_orderList = new();
|
||
}
|
||
}
|
||
|
||
internal class VDA5050SegmentsCache
|
||
{
|
||
public void Add(VDA5050Segment segment)
|
||
{
|
||
segment.Item.sequenceId = _sequenceId++;
|
||
_segments.Add(segment);
|
||
}
|
||
|
||
public VDA5050Segment this[int index]
|
||
{
|
||
get => _segments[index];
|
||
set => _segments[index] = value;
|
||
}
|
||
|
||
public VDA5050Segment Last()
|
||
{
|
||
return _segments.Last();
|
||
}
|
||
|
||
public int FindLastIndex(Predicate<VDA5050Segment> func)
|
||
{
|
||
return _segments.FindLastIndex(func);
|
||
}
|
||
|
||
public bool Any(Func<VDA5050Segment, bool> func)
|
||
{
|
||
return _segments.Any(func);
|
||
}
|
||
|
||
public IEnumerable<VDA5050Segment> Where(Func<VDA5050Segment, bool> func)
|
||
{
|
||
return _segments.Where(func);
|
||
}
|
||
|
||
public IEnumerable<VDA5050Segment> Skip(int count)
|
||
{
|
||
return _segments.Skip(count);
|
||
}
|
||
|
||
public int Count => _segments.Count;
|
||
|
||
public string Display()
|
||
{
|
||
return string.Join("\n",
|
||
_segments.Select(seg =>
|
||
{
|
||
var id = "";
|
||
if (seg.Item is node nn) id = nn.nodeId;
|
||
else if (seg.Item is edge ee) id = ee.edgeId;
|
||
return $"{seg.Item.sequenceId}\t{id}\t{seg.Item.GetType().Name}\t{seg.State}";
|
||
}));
|
||
}
|
||
|
||
private uint _sequenceId = 0;
|
||
|
||
private List<VDA5050Segment> _segments = new();
|
||
}
|
||
|
||
private int _taskId;
|
||
|
||
private int _taskUpdateId = -1;
|
||
|
||
internal int TaskId
|
||
{
|
||
get => _taskId;
|
||
set
|
||
{
|
||
_taskUpdateId = -1;
|
||
_lastBaseNodeSequenceId = -1;
|
||
_taskId = value;
|
||
}
|
||
}
|
||
|
||
internal string OrderId => $"order-{_taskId}";
|
||
|
||
internal uint OrderUpdateId => (uint)_taskUpdateId;
|
||
|
||
/// <summary>
|
||
/// Route state stored by AGV.
|
||
/// </summary>
|
||
private List<sequenceItem> _receivedLastConfirmedRoute = [];
|
||
|
||
private string _receivedOrderId = "";
|
||
// Add these fields at the top of the VDA5050Car class (with your other field declarations)
|
||
private volatile stateMessage _latestStateMessage = null;
|
||
private CancellationTokenSource _stateProcessingCts;
|
||
|
||
|
||
private uint _receivedOrderUpdateId = 0;
|
||
|
||
private int _receivedLastNodeSequenceId = -1;
|
||
|
||
public void ReseData()
|
||
{
|
||
RouteCache = new();
|
||
_receivedLastNodeSequenceId = -1;
|
||
_toTraverseSequenceId = -1;
|
||
_receivedLastConfirmedRoute = [];
|
||
_lastBaseNodeSequenceId = -1;
|
||
// TaskId = 0;
|
||
_orderList = new();
|
||
}
|
||
|
||
|
||
private int _toTraverseSequenceId = -1;
|
||
|
||
public int TotalStateMessageNumber = 0;
|
||
|
||
public void UpdateState(stateMessage msg)
|
||
{
|
||
|
||
// lock (RouteCache)
|
||
lock (_receivedLastConfirmedRoute)
|
||
{
|
||
var segments = new List<sequenceItem>();
|
||
|
||
int ii = 0, jj = 0;
|
||
while (ii < msg.edgeStates.Length && jj < msg.nodeStates.Length)
|
||
{
|
||
var edge = msg.edgeStates[ii];
|
||
var node = msg.nodeStates[jj];
|
||
var takeEdge = edge.sequenceId < node.sequenceId;
|
||
|
||
if (takeEdge)
|
||
{
|
||
segments.Add(edge);
|
||
ii++;
|
||
}
|
||
else
|
||
{
|
||
segments.Add(node);
|
||
jj++;
|
||
}
|
||
}
|
||
for (var i = ii; i < msg.edgeStates.Length; ++i) segments.Add(msg.edgeStates[i]);
|
||
for (var j = jj; j < msg.nodeStates.Length; ++j) segments.Add(msg.nodeStates[j]);
|
||
|
||
if (segments.Count > 1)
|
||
for (var i = 1; i < segments.Count; i++)
|
||
if (segments[i - 1].sequenceId + 1 != segments[i].sequenceId)
|
||
throw new Exception("stateMessage not continuous!");
|
||
|
||
_receivedOrderId = msg.orderId;
|
||
_receivedOrderUpdateId = msg.orderUpdatedId;
|
||
_receivedLastNodeSequenceId = (int)msg.lastNodeSequenceId;
|
||
_receivedLastConfirmedRoute = segments;
|
||
var s = $"{DateTime.Now:HH:mm:ss-fff}\n\n" +
|
||
$"{string.Join("\n", _receivedLastConfirmedRoute.Select(item => $"{(item is nodeState ? "*" + ((nodeState)item).nodeId : ((edgeState)item).edgeId)}\t{item.sequenceId}\t{item.released}"))}" +
|
||
$"\n_receivedLastNodeSequenceId:{_receivedLastNodeSequenceId}\n" +
|
||
$"{string.Join("\n",msg.errors.Select(e=>$"{e.errorType}：{e.errorLevel}"))}" +
|
||
$"{string.Join("\n",msg.actionStates.Select(a=>$"{a.actionDescription}：{a.actionStatus} {a.actionId}"))}";
|
||
StateMessageViewer?.UpdateText(s);
|
||
// Diagnosis.Log(s,"stateMsg",true);
|
||
}
|
||
}
|
||
|
||
public void UpdatePosition(visualizationMessage msg)
|
||
{
|
||
haveCoordination = true;
|
||
x = (float)msg.agvPosition.x;
|
||
y = (float)msg.agvPosition.y;
|
||
th = (float)(msg.agvPosition.theta / Math.PI * 180);
|
||
}
|
||
|
||
/// <summary>
|
||
/// Called from the MQTT callback to store the latest state message.
|
||
/// </summary>
|
||
public void SetLatestState(stateMessage msg)
|
||
{
|
||
_latestStateMessage = msg;
|
||
}
|
||
|
||
public void SetupListeners()
|
||
{
|
||
_mqttCommunication.SubsribeToConnectionTopic();
|
||
//_mqttCommunication.SubsribeToFactSheetTopic();
|
||
//_mqttCommunication.SubscribeToRegularState();
|
||
_mqttCommunication.SubscribeToState();
|
||
_mqttCommunication.SubscribeToVisualization();
|
||
//_mqttCommunication.SubscribeToVisualization();
|
||
}
|
||
|
||
// public void SetupInstActionListeners()
|
||
// {
|
||
// InstanceActionPause();
|
||
// InstanceActionResume();
|
||
// InstanceActionCancel();
|
||
// }
|
||
// todo: should not inherit form ClumsyCarStatus
|
||
public class VDA5050CarStatus : ClumsyCar.ClumsyCarStatus
|
||
{
|
||
|
||
}
|
||
public override CarStatus status { get; set; } = new VDA5050CarStatus();
|
||
|
||
[MethodMember(Name = "Request fact sheet")]
|
||
public void InstantActionFactSheet()
|
||
{
|
||
var instantMsg = VDA5050Commons.CreateInstanceAction(
|
||
actionId: Guid.NewGuid().ToString(),
|
||
actionType: "factSheetRequest");
|
||
_mqttCommunication.PublishInstantActions(JsonConvert.SerializeObject(instantMsg));
|
||
}
|
||
|
||
[MethodMember(Name = "Pause")]
|
||
public void InstanceActionPause()
|
||
{
|
||
var instantMsg = VDA5050Commons.CreateInstanceAction(
|
||
actionId: Guid.NewGuid().ToString(),
|
||
actionType: "startPause"
|
||
);
|
||
|
||
_mqttCommunication.PublishInstantActions(JsonConvert.SerializeObject(instantMsg));
|
||
|
||
|
||
// Post("instanceAction", JsonConvert.SerializeObject(instanceMsg));
|
||
}
|
||
[MethodMember(Name = "Resume")]
|
||
public void InstanceActionResume()
|
||
{
|
||
|
||
var instantMsg = VDA5050Commons.CreateInstanceAction(
|
||
actionId: Guid.NewGuid().ToString(),
|
||
actionType: "stopPause"
|
||
);
|
||
|
||
_mqttCommunication.PublishInstantActions(JsonConvert.SerializeObject(instantMsg));
|
||
|
||
// Post("instanceAction", JsonConvert.SerializeObject(instanceMsg));
|
||
|
||
}
|
||
[MethodMember(Name = "Cancel")]
|
||
public void InstanceActionCancel()
|
||
{
|
||
|
||
var instantMsg = VDA5050Commons.CreateInstanceAction(
|
||
actionId: Guid.NewGuid().ToString(),
|
||
actionType: "cancelOrder"
|
||
);
|
||
_mqttCommunication.PublishInstantActions(JsonConvert.SerializeObject(instantMsg));
|
||
|
||
//_mqttCommunication.RestartClient();
|
||
|
||
//ForceStop();
|
||
|
||
|
||
// Post("instanceAction", JsonConvert.SerializeObject(instanceMsg));
|
||
}
|
||
|
||
[MethodMember(Name = "显示RouteCache")]
|
||
public void DisplayRouteCache()
|
||
{
|
||
RouteCacheViewer = new TextViewer();
|
||
RouteCacheViewer.Show();
|
||
}
|
||
|
||
[MethodMember(Name = "立即强制结束")]
|
||
public void ForceStop()
|
||
{
|
||
AppendDebug("Clumsy Restarted");
|
||
NoSchedule(true);
|
||
siteID = -1;
|
||
VDAReset();
|
||
Get("reset");
|
||
Commons.DeleteTag(tags, "occupied");
|
||
AppendDebug("restarting clumsy");
|
||
AppendDebug(
|
||
"wait for any pending task to flush."
|
||
);
|
||
status.programs.task = Task.CompletedTask;
|
||
|
||
AppendDebug("Clumsy Restarted");
|
||
NoSchedule(true);
|
||
siteID = -1;
|
||
Commons.DeleteTag(tags, "occupied");
|
||
// Reset();
|
||
}
|
||
|
||
[MethodMember(Name = "新现场检修")]
|
||
public new void Repair()
|
||
{
|
||
AppendDebug("ui-repair");
|
||
Diagnosis.Post($"Repair Car {this.name}({this.id})");
|
||
NoSchedule(makeUnavailable: false);
|
||
siteID = -1;
|
||
base.tags.Clear();
|
||
lstatus = "现场检修";
|
||
}
|
||
[MethodMember(Name = "显示上报消息")]
|
||
public void DisplayLatestMessage()
|
||
{
|
||
StateMessageViewer = new TextViewer();
|
||
StateMessageViewer.Show();
|
||
}
|
||
|
||
public TextViewer StateMessageViewer;
|
||
|
||
public TextViewer RouteCacheViewer;
|
||
|
||
public override void rightClickAction(float mouseX, float mouseY)
|
||
{
|
||
try
|
||
{
|
||
Site site1 = null;
|
||
float dist = float.MaxValue;
|
||
foreach (var site in SimpleLib.GetAllSites())
|
||
{
|
||
var d = LessMath.dist(site.x, site.y, mouseX, mouseY);
|
||
if (d < dist)
|
||
{
|
||
dist = (float)d;
|
||
site1 = site;
|
||
}
|
||
}
|
||
|
||
Console.WriteLine($"cart {id} goto {site1.id}");
|
||
if (dist < 20)
|
||
{
|
||
_ = Task.Factory.StartNew(() =>
|
||
{
|
||
var siteDst = site1;
|
||
if (siteID == -1)
|
||
{
|
||
dist = float.MaxValue;
|
||
foreach (var site in SimpleLib.GetAllSites())
|
||
{
|
||
var d = LessMath.dist(site.x, site.y, x, y);
|
||
if (d < dist)
|
||
{
|
||
dist = (float)d;
|
||
site1 = site;
|
||
}
|
||
}
|
||
}
|
||
//else site1 = SimpleLib.GetSite(siteID);
|
||
else site1 = SimpleLib.GetSite(GetLastSite());
|
||
|
||
try
|
||
{
|
||
var plan = new SegmentPlan() { usingCar = this };
|
||
plan.FindRoute(site1, siteDst);
|
||
// var code = $"{plan.Code()}; agv.Wait();";
|
||
G.pushStatus($"向{name}({id})下发行走任务");
|
||
Console.WriteLine($"right click, directs AGV{name}({id}) from {site1.id} to {siteDst.id}");
|
||
tags.Add("occupied", "SimplyMove");
|
||
tags.Add("dest", siteID.ToString());
|
||
var tsk = plan.Compile("walk").Queue(false,false);//接续任务
|
||
// var tsk = plan.Compile("walk").Queue();
|
||
G.pushStatus($"AGV:{name}({id})开始执行任务");
|
||
tsk.Wait();
|
||
G.pushStatus($"AGV:{name}({id})执行任务完毕, siteID={siteDst.id}");
|
||
siteID = siteDst.id;
|
||
tags.Remove("occupied");
|
||
tags.Remove("dest");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
Console.WriteLine($"Go route exception:{ExceptionFormatter.FormatEx(ex)}");
|
||
}
|
||
});
|
||
}
|
||
}
|
||
catch
|
||
{
|
||
//... ignored
|
||
}
|
||
}
|
||
|
||
private bool _running;
|
||
[MethodMember(Name = "测试")]
|
||
public async void Test()
|
||
{
|
||
_running = true;
|
||
var siteA = SimpleLib.GetAllSites().First(s => s.name == "A");
|
||
var siteB = SimpleLib.GetAllSites().First(s => s.name == "B");
|
||
while (_running)
|
||
{
|
||
var planA = new SegmentPlan() { usingCar = this };
|
||
planA.FindRoute(SimpleLib.GetSite(GetLastSite()), siteA);
|
||
await planA.Compile("A").Queue();
|
||
var planB = new SegmentPlan() { usingCar = this };
|
||
planB.FindRoute(siteA, siteB);
|
||
await planB.Compile("B").Queue();
|
||
Thread.Sleep(1000);
|
||
}
|
||
}
|
||
|
||
[MethodMember(Name = "多点任务测试")]
|
||
public async void MultiPointTest()
|
||
{
|
||
_running = true;
|
||
var siteNumResult = InputBox.ShowDialog("请按顺序输入当前多点任务的站点id:");
|
||
string sitesIDStr = InputBox.ResultValue;
|
||
Console.WriteLine("sitesIDStr: " + sitesIDStr);
|
||
int[] sitesID = Array.ConvertAll(sitesIDStr.Split(' '), int.Parse);
|
||
for (var i = 0; i < sitesID.Length; i++)
|
||
{
|
||
Console.WriteLine(sitesID[i]);
|
||
}
|
||
Site[] sites = new Site[sitesID.Length];
|
||
SimpleLite.Point[] pts = new SimpleLite.Point[sitesID.Length];
|
||
|
||
//for (var i = 0; i < sitesID.Length; i++)
|
||
//{
|
||
|
||
//}
|
||
|
||
for (var i = 0; i < sitesID.Length; i++)
|
||
{
|
||
Console.WriteLine("-------------------当前点为：" + sitesID[i]);
|
||
sites[i] = SimpleLib.GetSite(sitesID[i]);
|
||
Commons.AddOrUpdateSiteField(sites[i], "codeArrive", "agv.Wait();agv.ChangeDI41Signal();agv.WaitAO3Signal();agv.Wait();");
|
||
var plan = new SegmentPlan() { usingCar = this };
|
||
plan.FindRoute(SimpleLib.GetSite(GetLastSite()), sites[i]);
|
||
G.pushStatus($"下发行走任务from {GetLastSite()} to {sites[i]}");
|
||
await plan.Compile($"plan").Queue();
|
||
//Thread.Sleep(200);
|
||
Commons.DeleteSiteField(sites[i], "codeArrive");
|
||
Console.WriteLine("-----------------------开始下一段任务");
|
||
}
|
||
|
||
//完成以上任务后返回待命点
|
||
G.pushStatus($"返回待命点");
|
||
var targetPlan = Commons.GetNearestPlan(
|
||
this,
|
||
site =>
|
||
site.fields.ContainsKey("standby")
|
||
&& site.fields["standby"] == "true"
|
||
);
|
||
if (targetPlan != null)
|
||
{
|
||
Commons.GoSite(this, targetPlan.Destination, 1);
|
||
}
|
||
|
||
}
|
||
|
||
|
||
[MethodMember(Name = "停止测试")]
|
||
public void Stop()
|
||
{
|
||
_running = false;
|
||
}
|
||
|
||
|
||
[MethodMember(Name = "测试修改DI41信号")]
|
||
public void ChangeDI41SignalTest()
|
||
{
|
||
try
|
||
{
|
||
Console.WriteLine("car address: " + this.address);
|
||
hc.GetStringAsync($"http://{this.address}:8008/setValue?FieldName=DI41&Value=True");
|
||
Console.WriteLine("将DI41置为True");
|
||
Thread.Sleep(1000);
|
||
hc.GetStringAsync($"http://{this.address}:8008/setValue?FieldName=DI41&Value=False");
|
||
Console.WriteLine("1秒后将DI41置为False");
|
||
}
|
||
catch
|
||
{
|
||
Console.WriteLine("错误");
|
||
};
|
||
|
||
}
|
||
|
||
[MethodMember(Name = "测试修改AO3信号")]
|
||
public void ChangeAO3SignalTest()
|
||
{
|
||
try
|
||
{
|
||
hc.GetStringAsync($"http://{this.address}:8008/setValue?FieldName=AO3&Value=1");
|
||
//hc.GetStringAsync($"http://192.168.2.1:8008/setValue?FieldName=AO3&Value=1");
|
||
Console.WriteLine("将AO3置为1");
|
||
Thread.Sleep(1000);
|
||
hc.GetStringAsync($"http://{this.address}:8008/setValue?FieldName=AO3&Value=0");
|
||
//hc.GetStringAsync($"http://192.168.2.1:8008/setValue?FieldName=AO3&Value=0");
|
||
Console.WriteLine("1秒后将AO3置为0");
|
||
}
|
||
catch
|
||
{
|
||
Console.WriteLine("错误");
|
||
};
|
||
}
|
||
|
||
[MethodMember(Name = "返回待命点")]
|
||
public void GoStandBySite()
|
||
{
|
||
try
|
||
{
|
||
if (this != null)
|
||
{
|
||
var carGroup = this.fields.TryGetValue("group", out var value)
|
||
? value
|
||
: "all";
|
||
var targetPlan = Commons.GetNearestPlan(
|
||
this,
|
||
site =>
|
||
site.fields.ContainsKey("standby")
|
||
&& carGroup.Equals(
|
||
site.fields.TryGetValue("group", out var value) ? value : "all"
|
||
)
|
||
&& site.fields["standby"] == "true"
|
||
);
|
||
if (targetPlan != null)
|
||
{
|
||
Commons.GoSite(this, targetPlan.Destination, 1);
|
||
}
|
||
}
|
||
}
|
||
catch (Exception)
|
||
{
|
||
Console.WriteLine($"调用小车去待命点失败");
|
||
}
|
||
}
|
||
|
||
protected override void draw(Graphics eGraphics)
|
||
{
|
||
eGraphics.FillRectangle(Brushes.Gray, -160, -120, 320, 240);
|
||
eGraphics.DrawRectangle(Pens.White, -160, -120, 320, 240);
|
||
eGraphics.DrawLine(Pens.White, 0, -120, 160, 0);
|
||
eGraphics.DrawLine(Pens.White, 0, 120, 160, 0);
|
||
}
|
||
|
||
|
||
public void Post1(string api, string content, int port = 0)
|
||
{
|
||
}
|
||
|
||
public void Post(string api, string content, int port = 0)
|
||
{
|
||
// Console.WriteLine($"Content of the order message in http: " + content);
|
||
var addresses = address.Split(',').OrderBy(p => ((VDA5050CarStatus)status).apiStat.Contains(p) ? 0 : 1);
|
||
foreach (var addr in addresses)
|
||
{
|
||
var call = $"{addr}:{(port != 0 ? port : GetConf("cport", 8008))}/{api}";
|
||
try
|
||
{
|
||
GetHC().Post($"http://{call}", content);
|
||
}
|
||
catch
|
||
{
|
||
|
||
}
|
||
}
|
||
}
|
||
public string Get(string api, int port = 0, bool outputIfFail = false)
|
||
{
|
||
var addrls = address.Split(',');
|
||
var exs = new string[addrls.Length];
|
||
int j = 0;
|
||
foreach (var addr in addrls.OrderBy(p => ((ClumsyCarStatus)status).apiStat.Contains(p) ? 0 : 1))
|
||
{
|
||
for (int i = 0; i < 8; ++i)
|
||
{
|
||
var call = $"{addr}:{(port != 0 ? port : GetConf("cport", 8008))}/{api}";
|
||
try
|
||
{
|
||
((ClumsyCarStatus)status).apiStat = $"[try{i}]{call}";
|
||
var str = GetHC().GetString($"http://{call}");
|
||
((ClumsyCarStatus)status).apiStat = $"[fin{i}]{call}";
|
||
// seqAPIFail = 0;
|
||
return str;
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
((ClumsyCarStatus)status).apiStat = $"[bad{i}]{call}, ex={ex.Message}";
|
||
exs[j] = ex.Message;
|
||
// seqAPIFail += 1;
|
||
}
|
||
}
|
||
|
||
j += 1;
|
||
}
|
||
|
||
var exstr = $"Get API {api} failed, addresses={address} all not available({string.Join(",", exs)})";
|
||
AppendDebug(exstr);
|
||
throw new Exception(exstr);
|
||
}
|
||
private HttpClient2 GetHC()
|
||
{
|
||
lock (this)
|
||
return _carHc ??= new HttpClient2(TimeSpan.FromSeconds(GetTimeout()), this);
|
||
}
|
||
|
||
private int GetTimeout()
|
||
{
|
||
var timeout = 5;
|
||
if (conf.Contains("timeout"))
|
||
timeout = Convert.ToInt32(conf.Substring(conf.IndexOf("timeout") + 8).Split(',')[0]);
|
||
return timeout;
|
||
}
|
||
|
||
private T GetConf<T>(string name, T defVal)
|
||
{
|
||
var timeout = defVal;
|
||
if (conf.Contains(name))
|
||
{
|
||
var ll = conf.Split(',').Where(ss => ss.Contains(name)).ToList();
|
||
if (ll.Count > 0)
|
||
{
|
||
var str = ll[0];
|
||
return (T)TypeDescriptor.GetConverter(typeof(T)).ConvertFromString(str.Split(':')[1]);
|
||
}
|
||
}
|
||
return timeout;
|
||
}
|
||
|
||
// mqtt controller
|
||
|
||
|
||
private HttpClient2 _carHc;
|
||
}
|
||
}
|