693 lines
29 KiB
C#
693 lines
29 KiB
C#
using LessokajiWeaverUtilities.Utilities;
|
||
using Newtonsoft.Json;
|
||
using SimpleLite;
|
||
using SimpleLite.RCS;
|
||
using SimpleLite.RCS.CarTypes;
|
||
using SimpleLite.CADTools;
|
||
using SimpleLite.Props;
|
||
using SimpleLite.UI;
|
||
using SimpleCore;
|
||
using SimpleCore.Compiler;
|
||
using SimpleCore.Extras;
|
||
using SimpleCore.Library;
|
||
using SimpleCore.PropType;
|
||
using System;
|
||
using System.Collections.Generic;
|
||
using System.Diagnostics;
|
||
using System.Drawing;
|
||
using System.Drawing.Drawing2D;
|
||
using System.Linq;
|
||
using System.Net.Http;
|
||
using System.Numerics;
|
||
using System.Reflection;
|
||
using System.Threading;
|
||
using System.Threading.Tasks;
|
||
using System.Windows.Forms;
|
||
using StandardScene.Model;
|
||
using StandardScene.Coders;
|
||
using StandardScene.Magnetic.Coders;
|
||
using Track = SimpleCore.PropType.Track;
|
||
|
||
namespace StandardScene.CarTypes
|
||
{
|
||
// MagneticTrackCoder 已物理迁出至 StandardScene.Magnetic.Coders（scene.mag 平台）。
|
||
// Kiva 系字段袋（KivaCarFields/KivaSiteFields/KivaTrackFields/KivaPlanFields）下沉至
|
||
// 基座 StandardScene.Core\CarTypes\KivaFields.cs（ArmCar 等跨平台车型继承它们）。
|
||
|
||
//标定轮里程计
|
||
[TemplateTrackCoderSettings(
|
||
priority = 10,
|
||
useVerb = "track.CalibrateWheelEncoder && track.ReverseDst != dst.id",
|
||
blockVerb = "true",
|
||
templateString = "agv.CalibrateWheelEncoder(${src.x},${src.y},${src.id},${dst.x},${dst.y},${dst.id});",
|
||
siteFields = typeof(KivaSiteFields),
|
||
trackFields = typeof(KivaTrackFields))]
|
||
|
||
// 通用避障/IO/纠偏 coder 已抽离为 StandardScene.Coders.*（见类前 [ProgramTrackCoderSettings] 引用）
|
||
|
||
// 避障区尺寸(2参)切换 coder 已抽离为 StandardScene.Coders.AvoidanceParamLWCoder（方案 B；见下方 [ProgramTrackCoderSettings] 引用）
|
||
[ProgramTrackCoderSettings(priority = 20, program = typeof(AvoidanceParamLWCoder))]
|
||
|
||
//plan.SegN 是指 当前路径所有点线的数量
|
||
//plan.SegN-2 就是终点倒数第二个站点
|
||
//[TemplateTrackCoderSettings(
|
||
// priority = 1,
|
||
// templateString = "agv.BasicGo(${src.x},${src.y},${src.id},${dst.x},${dst.y},${dst.id}," +
|
||
// "${track.id},${track.Speed},${track.Reverse||(track.ReverseDst==dst.id)}," +
|
||
// "${track.CarDirectionBias},${track.EnableCarAbsoluteDirection},${track.CarAbsoluteDirection},${track.typeInfo});",
|
||
// siteFields = typeof(KivaSiteFields),
|
||
// trackFields = typeof(KivaTrackFields),
|
||
// planFields = typeof(KivaPlanFields))]
|
||
|
||
[TemplateTrackCoderSettings(
|
||
priority = 30,
|
||
useVerb = "dst.tag>0 && src.tag>0",
|
||
templateString = "agv.QrGo(${src.x},${src.y},${src.id},${src.tag},${dst.x},${dst.y},${dst.id},${dst.tag},${track.id}," +
|
||
"${track.Speed},${track.Reverse || (track.ReverseDst == dst.id)}," +
|
||
"${track.CarDirectionBias},${track.EnableCarAbsoluteDirection},${track.CarAbsoluteDirection}," +
|
||
"${track.typeInfo});",
|
||
blockVerb = "true",
|
||
siteFields = typeof(KivaSiteFields),
|
||
trackFields = typeof(KivaTrackFields),
|
||
planFields = typeof(KivaPlanFields))]
|
||
//[TemplateTrackCoderSettings(
|
||
// priority = 2,
|
||
// useVerb = "track.ReverseDst==dst.id",
|
||
// templateString = "agv.BasicGo(${src.x},${src.y},${src.id},${dst.x},${dst.y},${dst.id},${track.id},${track.speed},0,true);",
|
||
// blockVerb = "true",
|
||
// siteFields = typeof(KivaSiteFields),
|
||
// trackFields = typeof(KivaTrackFields))]
|
||
|
||
[TemplateTrackCoderSettings(
|
||
priority = 20,
|
||
useVerb = "plan.action=='fetch' && dst.Shelf&&plan.curSeg==plan.segN-2",
|
||
templateString = "agv.Wait();" +
|
||
"agv.Fetch(${src.x},${src.y},${src.id},${dst.x},${dst.y},${dst.id}," +
|
||
"${dst.FetchSpeed},${dst.FetchLidarArea},${dst.FetchIOArea},${dst.FetchReverse}," +
|
||
"${dst.FetchBlindMoveDist},${dst.FetchLiftDownTarget},${dst.FetchLiftUpTarget}," +
|
||
"${dst.FetchUseQr},${dst.FetchQrMode},${dst.FetchIsUpQr},${dst.FetchUseDetector},${dst.FetchDetector},${dst.FetchDetectWidth},${dst.FetchDetectDepth}," +
|
||
"${plan.CarLength},${plan.CarWidth},${dst.FetchLeaveSrcEarly},${dst.FetchShieldObstacleDist});" +
|
||
"agv.Wait();",
|
||
blockVerb = "true",
|
||
siteFields = typeof(KivaSiteFields),
|
||
trackFields = typeof(KivaTrackFields),
|
||
planFields = typeof(KivaPlanFields))]
|
||
|
||
[TemplateTrackCoderSettings(
|
||
priority = 20,
|
||
useVerb = "plan.action=='put' && dst.Shelf&&plan.curSeg==plan.segN-2",
|
||
templateString = "agv.Wait();" +
|
||
"agv.Put(${src.x},${src.y},${src.id},${dst.x},${dst.y},${dst.id}," +
|
||
"${dst.PutSpeed},${dst.PutLidarArea},${dst.PutIOArea},${dst.PutReverse},${dst.PutSyncRotate}," +
|
||
"${dst.PutBlindMoveDist},${dst.PutLiftDownTarget},${dst.PutLiftUpTarget}," +
|
||
"${dst.PutUseQr},${dst.PutQrMode},${dst.PutIsUpQr},${dst.PutUseDetector},${dst.PutDetector}," +
|
||
"${dst.PutDetectWidth},${dst.PutDetectDepth},${dst.PutLeaveSrcEarly},${dst.PutShieldObstacleDist});" +
|
||
"agv.Wait();",
|
||
blockVerb = "true",
|
||
siteFields = typeof(KivaSiteFields),
|
||
trackFields = typeof(KivaTrackFields),
|
||
planFields = typeof(KivaPlanFields))]
|
||
|
||
[TemplateTrackCoderSettings(
|
||
priority = 22,
|
||
useVerb = "src.Shelf&&plan.curSeg==1",
|
||
templateString = "agv.LeaveShelf(${src.x},${src.y},${src.id},${dst.x},${dst.y},${dst.id}," +
|
||
"${src.LeaveShelfSpeed},${src.LeaveShelfLidarArea},${src.LeaveShelfIOArea},${src.LeaveShelfReverse},${src.LeaveShelfSyncRotate}," +
|
||
"${src.LeaveShelfBlindMoveDist},${src.LeaveShelfLiftDownTarget},${src.LeaveShelfRecoveryObstacleDist},);",
|
||
blockVerb = "true",
|
||
siteFields = typeof(KivaSiteFields),
|
||
trackFields = typeof(KivaTrackFields),
|
||
planFields = typeof(KivaPlanFields))]
|
||
|
||
[TemplateSiteCoderSettings(
|
||
priority = 25,
|
||
useVerb = "plan.action=='fetch'&& plan.segN == 1 &&dst.Shelf",
|
||
templateString =
|
||
"agv.FetchInPlace(${dst.FetchUseQr},${dst.FetchQrMode},${dst.FetchIsUpQr},${dst.FetchLiftUpTarget},${plan.CarLength},${plan.CarWidth});" +
|
||
"agv.Wait();",
|
||
blockVerb = "true",
|
||
siteFields = typeof(KivaSiteFields),
|
||
trackFields = typeof(KivaTrackFields),
|
||
planFields = typeof(KivaPlanFields))]
|
||
//[TemplateSiteCoderSettings(
|
||
// priority = 100,
|
||
// useVerb = "plan.curSeg == 0",
|
||
// templateString = "agv.StartingMission();agv.Wait();",
|
||
// blockVerb = "false",
|
||
// siteFields = typeof(KivaSiteFields),
|
||
// trackFields = typeof(KivaTrackFields),
|
||
// planFields = typeof(KivaPlanFields))]
|
||
|
||
|
||
|
||
[ProgramTrackCoderSettings(priority = 5, program = typeof(KivaCarTrackCoder))]
|
||
[ProgramTrackCoderSettings(priority = 19, program = typeof(MagneticTrackCoder))]
|
||
[ProgramTrackCoderSettings(priority = 17, program = typeof(LidarAreaSwitchCoder))]
|
||
[ProgramTrackCoderSettings(priority = 20, program = typeof(AvoidanceDistanceCoder))]
|
||
[ProgramTrackCoderSettings(priority = 20, program = typeof(IoAreaSwitchCoder))]
|
||
[ProgramTrackCoderSettings(priority = 20, program = typeof(TrackingErrThreshCoder))]
|
||
[CarType(Name = "Kiva", editor = typeof(Kiva))]
|
||
[I18N.DocumentTranslation(Name = "Kiva", locale = "en")]
|
||
[EnvelopConfig(centerX = 0, centerY = 0, lengthX = 950, lengthY = 650)]
|
||
public class Kiva : GhostCar, IScriptErrorRecoverable
|
||
{
|
||
/// <summary>脚本异常自恢复（AbstractLoopMission 经 IScriptErrorRecoverable 调用），转调 newReset。</summary>
|
||
public void RecoverReset(int resetSiteId = 0) => newReset(resetSiteId);
|
||
|
||
[FieldMember] public float CarLength = 2000;
|
||
[FieldMember] public float CarWidth = 1400;
|
||
public class RotateSiteEnvelope : SiteEnvelopeDefinition<Kiva>
|
||
{
|
||
public override bool Use() => true;
|
||
|
||
public override void Prompt()
|
||
{
|
||
if (curSeg == 0)
|
||
{
|
||
return;
|
||
}
|
||
|
||
if (plan.codeArr == null)
|
||
{
|
||
return;
|
||
}
|
||
|
||
var carConfig = usingCar.GetType().GetCustomAttribute<EnvelopConfigAttribute>();
|
||
var carLength = carConfig.lengthX;
|
||
var carWidth = carConfig.lengthY;
|
||
var code = plan.codeArr[curSeg - 1];
|
||
if (code.Contains("Fetch"))
|
||
{
|
||
var lengthX = plan.fields.TryGetValue("MaterialLength", out var materialLength)
|
||
? (int.TryParse(materialLength, out var lengthResult) ? lengthResult > 0 ? lengthResult : carLength : carLength)
|
||
: carLength;
|
||
var lengthY = plan.fields.TryGetValue("MaterialWidth", out var materialWidth)
|
||
? (int.TryParse(materialWidth, out var widthResult) ? widthResult > 0 ? widthResult : carWidth : carWidth)
|
||
: carWidth;
|
||
reshape(lengthX, lengthY, 0, 0);
|
||
}
|
||
else if (code.Contains("Put"))
|
||
{
|
||
reshape(carLength, carWidth, 0, 0);
|
||
}
|
||
}
|
||
|
||
public override bool Block() => false;
|
||
|
||
public override int priority => 1;
|
||
}
|
||
|
||
public static async Task<Kiva> Create()
|
||
{
|
||
var kiva = new Kiva()
|
||
{
|
||
lstatus = "连接中",
|
||
address = "127.0.0.1",
|
||
name = $"Kiva",
|
||
haveCoordination = true,
|
||
speed = 1,
|
||
};
|
||
return kiva;
|
||
}
|
||
|
||
private const float DrawWidth = 950, DrawLength = 650;
|
||
private readonly Pen orientPen = new Pen(Color.White, 3);
|
||
private bool twinkle = false;
|
||
|
||
protected override void draw(Graphics eGraphics)
|
||
{
|
||
try
|
||
{
|
||
var DrawWidth = CarLength;
|
||
var DrawLength = CarWidth;
|
||
var lineCap = new AdjustableArrowCap(5, 5, true);
|
||
orientPen.CustomEndCap = lineCap;
|
||
orientPen.StartCap = LineCap.RoundAnchor;
|
||
var alarmLevel = Commons.GetCarStatus(this, "AlarmLevel");
|
||
var soc = Commons.CarValue((Car)this, "Soc");
|
||
var electricCurrent = Commons.CarValue((Car)this, "ElectricCurrent");
|
||
var thresSpeed = Commons.CarValue(this, "ThresSpeed");
|
||
LadderLogic.FlipFlop(ref twinkle, 500, true, false);
|
||
Rectangle destRect = new Rectangle(
|
||
(int)((int)(-DrawWidth / 2)),
|
||
(int)((int)(-DrawLength / 2)),
|
||
(int)((int)DrawWidth),
|
||
(int)((int)DrawLength)
|
||
);
|
||
if (int.TryParse(alarmLevel, out var alarm) && alarm > 0)
|
||
{
|
||
eGraphics.FillRectangle(twinkle ? Brushes.DarkRed : Brushes.Green, destRect);
|
||
}
|
||
else if (alarmLevel == "-1" || this.siteID == -1)
|
||
eGraphics.FillRectangle(Brushes.LightSeaGreen, destRect);
|
||
else if (electricCurrent > 1)
|
||
eGraphics.FillRectangle(Brushes.YellowGreen, destRect);
|
||
else if (thresSpeed <= 0)
|
||
{
|
||
eGraphics.FillRectangle(twinkle ? Brushes.YellowGreen : Brushes.Green, destRect);
|
||
}
|
||
else if (soc < 25)
|
||
{
|
||
eGraphics.FillRectangle(twinkle ? Brushes.DarkRed : Brushes.LightSeaGreen, destRect);
|
||
}
|
||
else
|
||
eGraphics.FillRectangle(Brushes.Green, destRect);
|
||
|
||
eGraphics.DrawEllipse(Pens.Yellow, -160, -160, 320, 320);
|
||
eGraphics.DrawLine(orientPen, 0, 0, 480, 0);
|
||
|
||
}
|
||
catch (Exception e)
|
||
{
|
||
Diagnosis.Post("绘制小车异常" + ExceptionFormatter.FormatEx(e), "绘制小车异常", true);
|
||
}
|
||
}
|
||
|
||
private bool LoopTestRunning = false;
|
||
[MethodMember(Name = "循环测试")]
|
||
[I18N.DocumentTranslation(Name = "Loop Test", locale = "en")]
|
||
public async void LoopTest()
|
||
{
|
||
LoopTestRunning = true;
|
||
|
||
var a = SimpleLib.GetAllSites().First(s => s.name == "A");
|
||
var b = SimpleLib.GetAllSites().First(s => s.name == "B");
|
||
|
||
var plan1 = new SegmentPlan() { usingCar = this };
|
||
plan1.fields["action"] = "fetch";
|
||
plan1.FindRoute(SimpleLib.GetSite(GetLastSite()), a);
|
||
await plan1.Compile("fetch").Queue();
|
||
|
||
var plan2 = new SegmentPlan() { usingCar = this };
|
||
plan2.fields["action"] = "go";
|
||
plan2.FindRoute(SimpleLib.GetSite(GetLastSite()), b);
|
||
await plan2.Compile("go").Queue();
|
||
|
||
var plan3 = new SegmentPlan() { usingCar = this };
|
||
plan3.fields["action"] = "put";
|
||
plan3.FindRoute(SimpleLib.GetSite(GetLastSite()), a);
|
||
await plan3.Compile("put").Queue();
|
||
|
||
var plan4 = new SegmentPlan() { usingCar = this };
|
||
plan4.fields["action"] = "go";
|
||
plan4.FindRoute(SimpleLib.GetSite(GetLastSite()), b);
|
||
await plan4.Compile("go").Queue();
|
||
|
||
}
|
||
|
||
[MethodMember(Name = "循环停止测试")]
|
||
[I18N.DocumentTranslation(Name = "Stop Loop Test", locale = "en")]
|
||
public void LoopTestStop()
|
||
{
|
||
LoopTestRunning = false;
|
||
}
|
||
public override string SetDisplayInfo()
|
||
{
|
||
try
|
||
{
|
||
var str = $"{name}({id})\n";
|
||
status.enums.TryGetValue("ChassisMode", out var manual);
|
||
var ms = manual == "0" ? "自动" : "手动";
|
||
status.enums.TryGetValue("Soc", out var soc);
|
||
str = $"{str}{ms}|soc:{soc}";
|
||
var alarmStr = Commons.GetCarStatus(this, "AlarmInfo");
|
||
if (alarmStr != "")
|
||
str = $"{str}|{alarmStr}";
|
||
//if (status.enums.TryGetValue("mAlarm", out var mAlarm) && mAlarm != "")
|
||
// str = $"{str}|M:{mAlarm}";
|
||
//var hasC = status.enums.TryGetValue("cAlarm", out var cAlarm) && cAlarm != "";
|
||
//if (mAlarm != "" && hasC)
|
||
// str = $"{str}|";
|
||
//if (hasC)
|
||
// str = $"{str}C:{cAlarm}";
|
||
//var L_step = status.enums.TryGetValue("ls", out var ls) && ls != "";
|
||
//if (L_step)
|
||
//{
|
||
// str = $"{str}|ls:{ls}";
|
||
//}
|
||
if (status.enums.TryGetValue("ElectricCurrent", out var electricCurrent) &&
|
||
!string.IsNullOrEmpty(electricCurrent))
|
||
{
|
||
var charging = float.Parse(electricCurrent) * 0.01f > 0 ? "充电中" : "未充电";
|
||
str = $"{str}|charge:{charging}";
|
||
}
|
||
|
||
var thresSpeed = Commons.CarValue(this, "ThresSpeed");
|
||
str = $"{str}{((thresSpeed <= 0) ? "|避障触发" : "")}";
|
||
return str;
|
||
}
|
||
catch (Exception e)
|
||
{
|
||
Console.WriteLine(e);
|
||
return "bad car";
|
||
}
|
||
}
|
||
|
||
[MethodMember(Name = "进入小车远程", Description = "进入小车远程桌面")]
|
||
public void Mstsc()
|
||
{
|
||
var ip = this.address;
|
||
// 启动mstsc并传递IP地址
|
||
Process.Start(
|
||
new ProcessStartInfo
|
||
{
|
||
FileName = "mstsc",
|
||
Arguments = $"/v:{ip}",
|
||
UseShellExecute = false,
|
||
CreateNoWindow = true
|
||
}
|
||
);
|
||
}
|
||
|
||
[MethodMember(Name = "显示车辆监控", Description = "打开车辆状态监控窗口")]
|
||
public void ShowVehicleMonitor()
|
||
{
|
||
VehicleMonitor.ShowMonitor();
|
||
}
|
||
|
||
[MethodMember(Name = "模拟电量", Description = "/")]
|
||
public void SimarSoc()
|
||
{
|
||
if (InputBox.ShowDialog("模拟电量", "模拟电量Soc", "0", InputBox.Buttons.OkCancel) == SimpleLite.DialogResult.Cancel) return;
|
||
string value = InputBox.ResultValue;
|
||
try
|
||
{
|
||
status.enums["Soc"] = (60 + int.Parse(value)).ToString();
|
||
status.enums["Voltage"] = (50 + int.Parse(value)).ToString();
|
||
status.enums["ElectricCurrent"] = (40 + int.Parse(value)).ToString();
|
||
|
||
}
|
||
catch (Exception)
|
||
{
|
||
|
||
|
||
}
|
||
|
||
|
||
}
|
||
|
||
|
||
|
||
[MethodMember(Name = "立即强制结束new", Description = "立刻终止小车运行，重启C，并让小车下线")]
|
||
[I18N.DocumentTranslation(Name = "Force Stop new", Description = "Stop car & reset clumsy", locale = "en")]
|
||
public new async void ForceStop()
|
||
{
|
||
try
|
||
{
|
||
NoSchedule(true);
|
||
siteID = -1;
|
||
Commons.ClearTags(tags);
|
||
new Thread(() =>
|
||
{
|
||
try
|
||
{
|
||
AppendDebug("restarting clumsy");
|
||
Get("reset");
|
||
AppendDebug("wait for any pending task to flush.");
|
||
try
|
||
{
|
||
status.programs.task.Wait();
|
||
}
|
||
catch
|
||
{
|
||
}
|
||
|
||
AppendDebug("Clumsy Restarted");
|
||
NoSchedule(true);
|
||
siteID = -1;
|
||
Commons.DeleteTag(tags, "occupied");
|
||
}
|
||
catch (Exception ex)
|
||
{
|
||
MessageBox.Show(
|
||
$@"未能立即强制结束小车{name}({id})，原因：{ExceptionFormatter.FormatEx(ex)}"
|
||
);
|
||
}
|
||
})
|
||
{
|
||
Name = $"ForceStop:{name}({id})"
|
||
}.Start();
|
||
}
|
||
catch (Exception e)
|
||
{
|
||
Console.WriteLine(e);
|
||
throw;
|
||
}
|
||
}
|
||
|
||
[MethodMember(Name = "返厂检修new", Description = "不再刷新小车状态，当然也不再被调度")]
|
||
[I18N.DocumentTranslation(Name = "Blown new", Description = "Car no longer refresh status and scheduling", locale = "en")]
|
||
public new void Blown()
|
||
{
|
||
AppendDebug("ui-blown");
|
||
Diagnosis.Post($"Car {name}({id}) blown");
|
||
NoSchedule();
|
||
siteID = -1;
|
||
tags.Clear();
|
||
status.usage.AddUsage(
|
||
"base",
|
||
new CarUsage.CarUsageInfo { scheduling = false, refreshing = false }
|
||
);
|
||
lstatus = "返厂检修";
|
||
}
|
||
|
||
[MethodMember(Name = "现场检修new ", Description = "让小车不再被调度，但仍然刷新状态，并且当前点不再使用")]
|
||
[I18N.DocumentTranslation(Name = "Repair new", Description = "Car no longer scheduling but still refresh status", locale = "en")]
|
||
public new void Repair()
|
||
{
|
||
AppendDebug("ui-repair");
|
||
Diagnosis.Post($"Repair Car {name}({id})");
|
||
NoSchedule();
|
||
siteID = -1;
|
||
Commons.DeleteTag(tags, "occupied");
|
||
|
||
lstatus = "现场检修";
|
||
}
|
||
|
||
public void newReset(int resetId = 0)
|
||
{
|
||
//if (!status.programs.task.IsCompleted &&
|
||
// MessageBox.Show($"小车{name}({id})当前有任务：{status.programs.printStatus()}，确认初始化？", "确认",
|
||
// MessageBoxButtons.YesNo) == DialogResult.No) return;
|
||
try
|
||
{
|
||
if (GetLastSite() != -1 && Commons.GetVehicleStatus((Car)this)!= VehicleStatus.NeedInit)
|
||
{
|
||
Diagnosis.Post($"Car{id}初始化失败：已初始化");
|
||
return;
|
||
}
|
||
var debug = "ui invoke reset. ";
|
||
|
||
// todo: 找点和线段的最近点。。。
|
||
// lock
|
||
UISite site1 = null;
|
||
float dist = float.MaxValue;
|
||
List<(float, Site, Site)> trackList = new List<(float, Site, Site)>();
|
||
if (resetId == 0)
|
||
{
|
||
|
||
(float bias, Vector2 hPnt, float fd) Project2DLine(
|
||
Vector2 pnt,
|
||
Vector2 segSt,
|
||
Vector2 segEnd
|
||
)
|
||
{
|
||
var dir = Vector2.Normalize(segEnd - segSt);
|
||
var fd = Vector2.Dot(pnt - segSt, dir);
|
||
var hPnt = segSt + fd * dir;
|
||
var bias = dir.X * (pnt.Y - segSt.Y) - (pnt.X - segSt.X) * dir.Y;
|
||
return (bias, hPnt, fd);
|
||
}
|
||
|
||
foreach (var allTrack in SimpleLib.GetAllTracks())
|
||
{
|
||
var s = SimpleLib.GetSite(allTrack.siteA);
|
||
var e = SimpleLib.GetSite(allTrack.siteB);
|
||
var (b, h, fd) = Project2DLine(
|
||
new Vector2(x, y),
|
||
new Vector2(s.x, s.y),
|
||
new Vector2(e.x, e.y)
|
||
);
|
||
b = Math.Abs(b);
|
||
if (fd < 0 || fd > 1)
|
||
continue;
|
||
if (dist > b)
|
||
{
|
||
UISite ss;
|
||
if (fd < 0.5)
|
||
ss = (UISite)s;
|
||
else
|
||
ss = (UISite)e;
|
||
|
||
foreach (var ccar in SimpleLib.GetAllCars())
|
||
{
|
||
if (ccar == this)
|
||
continue;
|
||
if (ccar.status.holdingLocks.Contains(s.id))
|
||
goto next;
|
||
}
|
||
|
||
dist = b;
|
||
site1 = ss;
|
||
next:
|
||
;
|
||
}
|
||
}
|
||
float radius = 5000;
|
||
//根据小车为圆心找到半径radius内的站点
|
||
var surroundSites = SimpleLib.GetAllSites().ToList().FindAll(p => LessMath.dist(x, y, p.x, p.y) <= radius);
|
||
foreach (var allTrack in SimpleLib.GetAllTracks())
|
||
{
|
||
var s = SimpleLib.GetSite(allTrack.siteA);
|
||
var e = SimpleLib.GetSite(allTrack.siteB);
|
||
if (!surroundSites.Contains(s) && !surroundSites.Contains(e) && surroundSites.Count != 0) continue;
|
||
|
||
var (b, h, fd) = Project2DLine(new Vector2(x, y), new Vector2(s.x, s.y), new Vector2(e.x, e.y));
|
||
b = Math.Abs(b);
|
||
double pathAngle = Double.NaN;
|
||
if (allTrack.direction == 1)
|
||
pathAngle = Math.Atan2((e.y - s.y), (e.x - s.x)) / Math.PI * 180;
|
||
else if (allTrack.direction == 2)
|
||
pathAngle = Math.Atan2((e.y - s.y), (e.x - s.x)) / Math.PI * 180 + 180;
|
||
else
|
||
trackList.Add((b, s, e));
|
||
if (Math.Abs(this.th - pathAngle) < 30)
|
||
trackList.Add((b, s, e));
|
||
foreach (var ccar in SimpleLib.GetAllCars())
|
||
{
|
||
if (ccar == this) continue;
|
||
if (ccar.status.holdingLocks.Contains(s.id)) goto next;
|
||
}
|
||
next:;
|
||
|
||
}
|
||
//var sites = trackList.OrderBy(p => p.Item1).ToArray();
|
||
List<(double, Site)> distList = new List<(double, Site)>();
|
||
foreach (var site in trackList)
|
||
{
|
||
if (site.Item2.fields.ContainsKey("no_reset") || site.Item3.fields.ContainsKey("no_reset"))
|
||
continue;
|
||
foreach (var ccar in SimpleLib.GetAllCars())
|
||
{
|
||
if (ccar == this) continue;
|
||
if (ccar.status.holdingLocks.Contains(site.Item2.id) || ccar.status.holdingLocks.Contains(site.Item3.id)) goto next;
|
||
}
|
||
distList.Add((LessMath.dist(site.Item2.x, site.Item2.y, x, y), site.Item2));
|
||
distList.Add((LessMath.dist(site.Item3.x, site.Item3.y, x, y), site.Item3));
|
||
next:;
|
||
}
|
||
|
||
site1 = (UISite)distList.OrderBy(p => p.Item1).First().Item2;
|
||
|
||
//foreach (var site in SimpleLib.GetAllSites())
|
||
//{
|
||
// if (site.fields.ContainsKey("no_reset"))
|
||
// continue;
|
||
// foreach (var ccar in SimpleLib.GetAllCars())
|
||
// {
|
||
// if (ccar == this)
|
||
// continue;
|
||
// if (ccar.status.holdingLocks.Contains(site.id))
|
||
// goto next;
|
||
// }
|
||
|
||
// var d = LessMath.dist(site.x, site.y, x, y);
|
||
// if (d < dist)
|
||
// {
|
||
// dist = (float)d;
|
||
// site1 = (UISite)site;
|
||
// }
|
||
// next:
|
||
// ;
|
||
//}
|
||
TrafficReset(site1, true, strict: false);
|
||
}
|
||
else
|
||
{
|
||
TrafficReset(SimpleLib.GetSite(resetId), true, strict: false);
|
||
}
|
||
|
||
|
||
siteID = site1.id;
|
||
|
||
lstatus = "上线";
|
||
debug += $"site={site1.id}";
|
||
AppendDebug(debug);
|
||
G.pushStatus($"Car {name}({id}) reset to Site {site1.id}");
|
||
}
|
||
catch (Exception e)
|
||
{
|
||
Diagnosis.Post($"{this.name}---初始化失败--{e.Message}", $"{this.name}", true);
|
||
}
|
||
}
|
||
|
||
HttpClient hc = new HttpClient() { Timeout = TimeSpan.FromMilliseconds(300) };
|
||
// [MethodMember(name = "小车暂停", desc = "小车暂停")]
|
||
public void EmergencyStop(string reason)
|
||
{
|
||
Diagnosis.Post($"car{name}:EmergencyStop");
|
||
hc.GetStringAsync($"http://{address}:8008/setValue?FieldName=ISRelease&Value=1");
|
||
}
|
||
|
||
//[MethodMember(name = "小车暂停恢复", desc = "小车暂停恢复")]
|
||
public void EmergencyRelease()
|
||
{
|
||
Diagnosis.Post($"car{name}:EmergencyStop release");
|
||
hc.GetStringAsync($"http://{address}:8008/setValue?FieldName=ISRelease&Value=0");
|
||
}
|
||
//[MethodMember(name = "小车重启C", desc = "小车重启Clumsy")]
|
||
public void ResetClumsy()
|
||
{
|
||
Diagnosis.Post($"car{name}-{address}:ResetClumsy");
|
||
hc.GetStringAsync($"http://{address}:8008/reset");
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// 专门用于控制turn的问题
|
||
/// </summary>
|
||
public class KivaCarTrackCoder : ITrackCoder
|
||
{
|
||
public bool toBlock()
|
||
{
|
||
return false;
|
||
}
|
||
/// <summary>
|
||
/// 用于记录上一路径的角度
|
||
/// </summary>
|
||
public static double lastTh = -1;
|
||
|
||
public bool Code(SegmentPlan plan, Track track, Site src, Site dst, int i)
|
||
{
|
||
var trackInfo = StringDictConvert<KivaTrackFields>.Convert(track.fields);
|
||
var srcInfo = StringDictConvert<KivaSiteFields>.Convert(src.fields);
|
||
Console.WriteLine($@"srcInfo.Turn:{srcInfo.Turn},i={1}");
|
||
if (!srcInfo.Turn) //不包含旋转指令，直接跳过
|
||
{
|
||
return true;
|
||
}
|
||
if (i == 1)
|
||
{
|
||
return true;
|
||
}
|
||
var lastSite = plan.segments[i - 3] as Site;
|
||
if (lastSite == null)
|
||
{
|
||
return true;
|
||
}
|
||
var lastPathDir = Math.Round(Math.Atan2(src.y - lastSite.y, src.x - lastSite.x) / Math.PI * 2) * 90;
|
||
var curPathDir = Math.Round(Math.Atan2(dst.y - src.y, dst.x - src.x) / Math.PI * 2) * 90;
|
||
if (Math.Abs(curPathDir - lastPathDir) < 45)
|
||
{
|
||
return true;
|
||
}
|
||
var angleTarget = trackInfo.ReverseDst == dst.id || trackInfo.Reverse ? curPathDir + 180 : curPathDir;
|
||
|
||
plan.codeArr[i] += $"agv.Wait(); agv.SyncRotate({angleTarget}); agv.Wait(); ";
|
||
|
||
return true;
|
||
}
|
||
}
|
||
|
||
}
|