init commit
This commit is contained in:
@@ -0,0 +1,49 @@
|
||||
using System;
|
||||
using System.Linq;
|
||||
using System.Windows.Forms;
|
||||
using Newtonsoft.Json;
|
||||
using SimpleCore;
|
||||
using SimpleLite.CADTools;
|
||||
using SimpleLite.RCS.CarTypes;
|
||||
using StandardScene;
|
||||
using StandardScene.Model;
|
||||
|
||||
namespace StandardScene.QrLidar.Cad
|
||||
{
|
||||
/// <summary>
|
||||
/// 扫描全图带 tag 的站点生成二维码地图(码值 → 坐标/角度),写入 <see cref="ApiController.QrMap"/>
|
||||
/// 供 <c>/api/QrMap</c> 下发,并通知带 QrCar 标记的在线车辆刷新。
|
||||
/// 自 Core StandardCADTool.cs 迁入(scene.qrlidar 平台专属能力)。
|
||||
/// </summary>
|
||||
[CADToolDescriptor(name = "同步二维码地图到小车")]
|
||||
public class SyncQrMap : CADTool
|
||||
{
|
||||
public override async void Invoke()
|
||||
{
|
||||
var tagSites = SimpleLib.GetAllSites().Where(p => p.fields.ContainsKey("tag"));
|
||||
ApiController.QrMap.Clear();
|
||||
foreach (var tagSite in tagSites)
|
||||
{
|
||||
var tag = int.Parse(tagSite.fields["tag"]);
|
||||
var tagX = tagSite.x;
|
||||
var tagY = tagSite.y;
|
||||
var tagTh = tagSite.fields.TryGetValue("th", out var field) ? float.Parse(field) : 0f;
|
||||
if (ApiController.QrMap.ContainsKey(tag))
|
||||
{
|
||||
MessageBox.Show(($@"存在重复码值:{tag} 站点id为:{tagSite.id}"));
|
||||
return;
|
||||
}
|
||||
|
||||
ApiController.QrMap[tag] = (tagX, tagY, tagTh);
|
||||
}
|
||||
|
||||
ApiController.QrMapJson = JsonConvert.SerializeObject(ApiController.QrMap);
|
||||
var qrCars = SimpleLib.GetAllCars().Where(p => p.fields.ContainsKey("QrCar"));
|
||||
foreach (var qrCar in qrCars)
|
||||
{
|
||||
if(Commons.GetVehicleStatus((Car)qrCar) is VehicleStatus.Normal or VehicleStatus.NeedInit)
|
||||
((ClumsyCar)qrCar).ImmediateCommand("pilot.GetUpdateQrMap()");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,261 @@
|
||||
using LessokajiWeaverUtilities.Utilities;
|
||||
using SimpleLite.RCS;
|
||||
using SimpleLite.RCS.CarTypes;
|
||||
using SimpleLite.CADTools;
|
||||
using SimpleLite.Props;
|
||||
using SimpleLite.UI;
|
||||
using SimpleCore;
|
||||
using SimpleCore.Compiler;
|
||||
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.Text;
|
||||
using System.Threading;
|
||||
using System.Threading.Tasks;
|
||||
|
||||
namespace StandardScene.CarTypes
|
||||
{
|
||||
class ArmCarSiteFields:KivaSiteFields
|
||||
{
|
||||
public bool putNeedTurn = false;
|
||||
}
|
||||
|
||||
class ArmCarTrackFields:KivaTrackFields
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
class ArmCarPlanFields:KivaPlanFields
|
||||
{
|
||||
public bool charge = false;
|
||||
}
|
||||
|
||||
// 自 VDA5050Car.cs 下沉至 Core:ArmCar 的 siteFields 引用它(见下方 [TemplateTrackCoderSettings])。
|
||||
// VDA5050 拆为独立 dll 后 ArmCar 留 Core,不能反依赖 VDA dll,故该字段类置于 Core。
|
||||
class VDA5050SiteField : BasicSiteFields
|
||||
{
|
||||
public bool DI41 = false;
|
||||
public int SleepTime = 0;
|
||||
public bool workPlace = false;
|
||||
}
|
||||
//[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))]
|
||||
//[TemplateSiteCoderSettings(
|
||||
// priority = 5,
|
||||
// useVerb = "plan.action=='fetch'&&plan.curSeg==plan.segN-1",
|
||||
// blockVerb = "true",
|
||||
// templateString = "agv.Wait();agv.Fetch(\"${dst.name}\");agv.Wait();",
|
||||
// siteFields = typeof(ArmCarSiteFields),
|
||||
// trackFields = typeof(ArmCarTrackFields),
|
||||
// planFields = typeof(ArmCarPlanFields))]
|
||||
//[TemplateSiteCoderSettings(
|
||||
// priority = 5,
|
||||
// useVerb = "plan.action=='put'&&plan.curSeg==plan.segN-1",
|
||||
// blockVerb = "true",
|
||||
// templateString = "agv.Wait();agv.Put(${dst.putNeedTurn},\"${dst.name}\");agv.Wait();",
|
||||
// siteFields = typeof(ArmCarSiteFields),
|
||||
// trackFields = typeof(ArmCarTrackFields),
|
||||
// planFields = typeof(ArmCarPlanFields))]
|
||||
//[TemplateSiteCoderSettings(
|
||||
// priority = 5,
|
||||
// useVerb = "plan.action=='putEmpty'&&plan.curSeg==plan.segN-1",
|
||||
// blockVerb = "true",
|
||||
// templateString = "agv.Wait();agv.PutEmpty(\"${dst.name}\");agv.Wait();",
|
||||
// siteFields = typeof(ArmCarSiteFields),
|
||||
// trackFields = typeof(ArmCarTrackFields),
|
||||
// planFields = typeof(ArmCarPlanFields))]
|
||||
[TemplateSiteCoderSettings(
|
||||
priority = 5,
|
||||
useVerb = "plan.action=='pickFull'&&plan.curSeg==plan.segN-1",
|
||||
blockVerb = "true",
|
||||
templateString = "agv.Wait();agv.PickFull(\"${dst.name}\");agv.Wait();",
|
||||
siteFields = typeof(ArmCarSiteFields),
|
||||
trackFields = typeof(ArmCarTrackFields),
|
||||
planFields = typeof(ArmCarPlanFields))]
|
||||
[TemplateSiteCoderSettings(
|
||||
priority = 5,
|
||||
useVerb = "plan.action=='pickEmpty'&&plan.curSeg==plan.segN-1",
|
||||
blockVerb = "true",
|
||||
templateString = "agv.Wait();agv.PickEmpty(\"${dst.name}\");agv.Wait();",
|
||||
siteFields = typeof(ArmCarSiteFields),
|
||||
trackFields = typeof(ArmCarTrackFields),
|
||||
planFields = typeof(ArmCarPlanFields))]
|
||||
[TemplateSiteCoderSettings(
|
||||
priority = 5,
|
||||
useVerb = "plan.action=='putFull'&&plan.curSeg==plan.segN-1",
|
||||
blockVerb = "true",
|
||||
templateString = "agv.Wait();agv.PutFull(\"${dst.name}\");agv.Wait();",
|
||||
siteFields = typeof(ArmCarSiteFields),
|
||||
trackFields = typeof(ArmCarTrackFields),
|
||||
planFields = typeof(ArmCarPlanFields))]
|
||||
[TemplateSiteCoderSettings(
|
||||
priority = 5,
|
||||
useVerb = "plan.action=='putEmpty'&&plan.curSeg==plan.segN-1",
|
||||
blockVerb = "true",
|
||||
templateString = "agv.Wait();agv.PutEmpty(\"${dst.name}\");agv.Wait();",
|
||||
siteFields = typeof(ArmCarSiteFields),
|
||||
trackFields = typeof(ArmCarTrackFields),
|
||||
planFields = typeof(ArmCarPlanFields))]
|
||||
[TemplateSiteCoderSettings(
|
||||
priority = 5,
|
||||
useVerb = "plan.action=='MoveArm'&&plan.curSeg==plan.segN-1",
|
||||
blockVerb = "true",
|
||||
templateString = "agv.Wait();agv.MoveArm(${dst.MoveDirection});agv.Wait();",
|
||||
siteFields = typeof(ArmCarSiteFields),
|
||||
trackFields = typeof(ArmCarTrackFields),
|
||||
planFields = typeof(ArmCarPlanFields))]
|
||||
[TemplateSiteCoderSettings(
|
||||
priority = 5,
|
||||
useVerb = "dst.name=='charge'",
|
||||
templateString = "agv.Wait();agv.ControlChargePort(true);agv.Wait();",
|
||||
siteFields = typeof(ArmCarSiteFields),
|
||||
trackFields = typeof(ArmCarTrackFields),
|
||||
planFields = typeof(ArmCarPlanFields))]
|
||||
|
||||
[TemplateSiteCoderSettings(
|
||||
priority = 5,
|
||||
useVerb = "dst.name=='charge'&&plan.curSeg==0",
|
||||
templateString = "agv.Wait();agv.ControlChargePort(false);",
|
||||
siteFields = typeof(VDA5050SiteField))]
|
||||
|
||||
[CarType(Name = "ArmCar")]
|
||||
[I18N.DocumentTranslation(Name = "ArmCar", locale = "en")]
|
||||
public class ArmCar : GhostCar
|
||||
{
|
||||
public static async Task<ArmCar> Create()
|
||||
{
|
||||
var car = new ArmCar()
|
||||
{
|
||||
lstatus = "连接中",
|
||||
address = "127.0.0.1",
|
||||
name = $"Kiva",
|
||||
haveCoordination = true,
|
||||
speed = 0.3f,
|
||||
};
|
||||
return car;
|
||||
}
|
||||
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
|
||||
{
|
||||
float scale = 1;
|
||||
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) * scale),
|
||||
(int)((int)(-DrawLength / 2) * scale),
|
||||
(int)((int)DrawWidth * scale),
|
||||
(int)((int)DrawLength * scale)
|
||||
);
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
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 = "进入小车远程桌面")]
|
||||
[I18N.DocumentTranslation(Name = "Open remote desktop", Description = "Open the car's remote desktop", locale = "en")]
|
||||
public void Mstsc()
|
||||
{
|
||||
var ip = this.address;
|
||||
// 启动mstsc并传递IP地址
|
||||
Process.Start(
|
||||
new ProcessStartInfo
|
||||
{
|
||||
FileName = "mstsc",
|
||||
Arguments = $"/v:{ip}",
|
||||
UseShellExecute = false,
|
||||
CreateNoWindow = true
|
||||
}
|
||||
);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,105 @@
|
||||
using LessokajiWeaverUtilities.Utilities;
|
||||
using Newtonsoft.Json;
|
||||
using SimpleLite.RCS;
|
||||
using SimpleLite.RCS.CarTypes;
|
||||
using SimpleCore;
|
||||
using SimpleCore.Compiler;
|
||||
using SimpleCore.PropType;
|
||||
using System;
|
||||
using System.Diagnostics;
|
||||
using System.Linq;
|
||||
using System.Threading;
|
||||
using System.Threading.Tasks;
|
||||
using System.Windows.Forms;
|
||||
|
||||
namespace StandardScene.CarTypes
|
||||
{
|
||||
[CarType(Name = "锂电双举升")]
|
||||
[I18N.DocumentTranslation(Name = "Dual Lift Car", locale = "en")]
|
||||
public class DualLiftingCar : GhostCar
|
||||
{
|
||||
public static async Task<DualLiftingCar> Create()
|
||||
{
|
||||
var car = new DualLiftingCar()
|
||||
{
|
||||
lstatus = "连接中",
|
||||
address = "127.0.0.1",
|
||||
name = "锂电双举升"
|
||||
};
|
||||
return car;
|
||||
}
|
||||
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("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 = "进入小车远程桌面")]
|
||||
[I18N.DocumentTranslation(Name = "Open remote desktop", Description = "Open the car's remote desktop", locale = "en")]
|
||||
public void Mstsc()
|
||||
{
|
||||
var ip = this.address;
|
||||
// 启动mstsc并传递IP地址
|
||||
Process.Start(
|
||||
new ProcessStartInfo
|
||||
{
|
||||
FileName = "mstsc",
|
||||
Arguments = $"/v:{ip}",
|
||||
UseShellExecute = false,
|
||||
CreateNoWindow = true
|
||||
}
|
||||
);
|
||||
}
|
||||
|
||||
private bool _running;
|
||||
[MethodMember(Name = "测试")]
|
||||
[I18N.DocumentTranslation(Name = "Start test", locale = "en")]
|
||||
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 = "停止测试")]
|
||||
[I18N.DocumentTranslation(Name = "Stop test", locale = "en")]
|
||||
public void Stop()
|
||||
{
|
||||
_running = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,435 @@
|
||||
using LessokajiWeaverUtilities.Utilities;
|
||||
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.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.Threading.Tasks;
|
||||
using StandardScene.Model;
|
||||
using StandardScene.Coders;
|
||||
|
||||
namespace StandardScene.CarTypes
|
||||
{
|
||||
class ForkliftSiteFields : BasicSiteFields
|
||||
{
|
||||
public int AngleTarget = 0;
|
||||
public bool Turn = false;
|
||||
|
||||
public float FetchSpeed = 0;
|
||||
public int FetchLidarArea = -2;
|
||||
public int FetchIOArea = -1;
|
||||
public float FetchBlindMoveDist = 0;
|
||||
public float FetchLiftDownTarget = -1;
|
||||
public float FetchLiftUpTarget = -1;
|
||||
public bool FetchUseDetector = false;
|
||||
public int FetchDetector = 0;
|
||||
public float FetchDetectWidth = -1;
|
||||
public float FetchDetectDepth = -1;
|
||||
public bool FetchLeaveSrcEarly = false;
|
||||
public float FetchShieldObstacleDist = -1;
|
||||
|
||||
public float PutSpeed = 0;
|
||||
public int PutLidarArea = -1;
|
||||
public int PutIOArea = -1;
|
||||
public float PutBlindMoveDist = 0;
|
||||
public float PutLiftDownTarget = -1;
|
||||
public float PutLiftUpTarget = -1;
|
||||
public bool PutUseDetector = false;
|
||||
public int PutDetector = 0;
|
||||
public float PutDetectWidth = -1;
|
||||
public float PutDetectDepth = -1;
|
||||
public bool PutLeaveSrcEarly = false;
|
||||
public float PutShieldObstacleDist = -1;
|
||||
|
||||
public float LeaveShelfSpeed = 0;
|
||||
public int LeaveShelfLidarArea = -1;
|
||||
public int LeaveShelfIOArea = -1;
|
||||
public float LeaveShelfBlindMoveDist = 0;
|
||||
public float LeaveShelfLiftDownTarget = -1;
|
||||
public float LeaveShelfRecoveryObstacleDist = -1;
|
||||
}
|
||||
|
||||
class ForkliftTrackFields : BasicTrackFields
|
||||
{
|
||||
public int ManeuverDir = 0;
|
||||
public int ForwardDst = 0;
|
||||
public int ForwardObChooseDst = -2;
|
||||
public float BlindMoveDist = 0;
|
||||
public bool UseDetector = false;
|
||||
public int DetectorMode = -1;
|
||||
public float DetectWidth = -1;
|
||||
public float DetectDepth = -1;
|
||||
public bool LeaveSrcEarly = false;
|
||||
public float ShieldObstacleDist = -1;
|
||||
}
|
||||
|
||||
class ForkliftPlanFields : BasicPlanFields
|
||||
{
|
||||
public bool reverse = false;
|
||||
public int level = 0;
|
||||
}
|
||||
|
||||
|
||||
// 通用避障/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 = 20,
|
||||
useVerb = "plan.action=='fetch' && dst.Shelf",
|
||||
templateString = "agv.Wait();" +
|
||||
"agv.Fetch(${src.x},${src.y},${src.id},${dst.x},${dst.y},${dst.id}," +
|
||||
"${dst.FetchSpeed},${dst.FetchLidarArea},${dst.FetchIOArea}," +
|
||||
"${dst.FetchBlindMoveDist},${dst.FetchLiftDownTarget},${dst.FetchLiftUpTarget}," +
|
||||
"${dst.FetchUseDetector},${dst.FetchDetector},${dst.FetchDetectWidth},${dst.FetchDetectDepth}," +
|
||||
"${plan.CarLength},${plan.CarWidth},${dst.FetchLeaveSrcEarly},${dst.FetchShieldObstacleDist});" +
|
||||
"agv.Wait();",
|
||||
blockVerb = "true",
|
||||
siteFields = typeof(ForkliftSiteFields),
|
||||
trackFields = typeof(ForkliftTrackFields),
|
||||
planFields = typeof(ForkliftPlanFields))]
|
||||
|
||||
[TemplateTrackCoderSettings(
|
||||
priority = 20,
|
||||
useVerb = "plan.action=='put' && dst.Shelf",
|
||||
templateString = "agv.Wait();" +
|
||||
"agv.Put(${src.x},${src.y},${src.id},${dst.x},${dst.y},${dst.id}," +
|
||||
"${dst.PutSpeed},${dst.PutLidarArea},${dst.PutIOArea}," +
|
||||
"${dst.PutBlindMoveDist},${dst.PutLiftDownTarget},${dst.PutLiftUpTarget}," +
|
||||
"${dst.PutUseDetector},${dst.PutDetector}," +
|
||||
"${dst.PutDetectWidth},${dst.PutDetectDepth},${dst.PutLeaveSrcEarly},${dst.PutShieldObstacleDist});" +
|
||||
"agv.Wait();",
|
||||
blockVerb = "true",
|
||||
siteFields = typeof(ForkliftSiteFields),
|
||||
trackFields = typeof(ForkliftTrackFields),
|
||||
planFields = typeof(ForkliftPlanFields))]
|
||||
|
||||
[TemplateTrackCoderSettings(
|
||||
priority = 22,
|
||||
useVerb = "src.Shelf",
|
||||
templateString = "agv.LeaveShelf(${src.x},${src.y},${src.id},${dst.x},${dst.y},${dst.id}," +
|
||||
"${src.LeaveShelfSpeed},${src.LeaveShelfLidarArea},${src.LeaveShelfIOArea}," +
|
||||
"${src.LeaveShelfBlindMoveDist},${src.LeaveShelfLiftDownTarget},${src.LeaveShelfRecoveryObstacleDist},);" +
|
||||
"agv.Wait();",
|
||||
blockVerb = "true",
|
||||
siteFields = typeof(ForkliftSiteFields),
|
||||
trackFields = typeof(ForkliftTrackFields),
|
||||
planFields = typeof(ForkliftPlanFields))]
|
||||
|
||||
[TemplateSiteCoderSettings(
|
||||
priority = 25,
|
||||
useVerb = "plan.action=='fetch'&&plan.segN==1&&dst.Shelf",
|
||||
templateString =
|
||||
"agv.FetchInPlace(${dst.FetchLiftUpTarget},${plan.CarLength},${plan.CarWidth});" +
|
||||
"agv.Wait();",
|
||||
blockVerb = "true",
|
||||
siteFields = typeof(ForkliftSiteFields),
|
||||
trackFields = typeof(ForkliftTrackFields),
|
||||
planFields = typeof(ForkliftPlanFields))]
|
||||
// 纠偏阈值 coder 已抽离为 StandardScene.Coders.TrackingErrThreshCoder
|
||||
|
||||
[ProgramTrackCoderSettings(priority = 20, program = typeof(LidarAreaSwitchCoder))]
|
||||
[ProgramTrackCoderSettings(priority = 20, program = typeof(IoAreaSwitchCoder))]
|
||||
[ProgramTrackCoderSettings(priority = 20, program = typeof(TrackingErrThreshCoder))]
|
||||
[CarType(Name = "叉车", editor = typeof(Forklift))]
|
||||
[I18N.DocumentTranslation(Name = "ForkLift", locale = "en")]
|
||||
[EnvelopConfig(centerX = -900, centerY = 0, lengthX = 3000, lengthY = 1500)]
|
||||
public class Forklift : GhostCar
|
||||
{
|
||||
public static async Task<Forklift> Create() // boilerplate
|
||||
{
|
||||
var fl = new Forklift()
|
||||
{
|
||||
lstatus = "连接中",
|
||||
address = "127.0.0.1",
|
||||
name = $"叉车",
|
||||
haveCoordination = true,
|
||||
speed = 1,
|
||||
};
|
||||
|
||||
return fl;
|
||||
}
|
||||
|
||||
private const float DrawWidth = 1100, DrawLength = 600, DrawForkLen = 1532;
|
||||
private readonly Pen _orientPen = new Pen(Color.White, 3);
|
||||
|
||||
protected override void draw(Graphics eGraphics)
|
||||
{
|
||||
var lineCap = new AdjustableArrowCap(6, 6, true);
|
||||
_orientPen.CustomEndCap = lineCap;
|
||||
_orientPen.StartCap = LineCap.RoundAnchor;
|
||||
|
||||
eGraphics.FillRectangle(Brushes.DimGray, -DrawLength / 2, -DrawWidth / 2, DrawForkLen, 200);
|
||||
eGraphics.FillRectangle(Brushes.DimGray, -DrawLength / 2, DrawWidth / 2 - 200, DrawForkLen, 200);
|
||||
eGraphics.FillRectangle(Brushes.Gray, DrawForkLen - DrawLength / 2, -DrawWidth / 2, DrawLength, DrawWidth);
|
||||
|
||||
eGraphics.DrawEllipse(Pens.White, -160, -160, 320, 320);
|
||||
eGraphics.DrawLine(_orientPen, 0, 0, 480, 0);
|
||||
}
|
||||
[MethodMember(Name = "进入小车远程", Description = "进入小车远程桌面")]
|
||||
[I18N.DocumentTranslation(Name = "Open remote desktop", Description = "Open the car's remote desktop", locale = "en")]
|
||||
public void Mstsc()
|
||||
{
|
||||
var ip = this.address;
|
||||
// 启动mstsc并传递IP地址
|
||||
Process.Start(
|
||||
new ProcessStartInfo
|
||||
{
|
||||
FileName = "mstsc",
|
||||
Arguments = $"/v:{ip}",
|
||||
UseShellExecute = false,
|
||||
CreateNoWindow = true
|
||||
}
|
||||
);
|
||||
}
|
||||
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("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";
|
||||
}
|
||||
}
|
||||
|
||||
HttpClient hc = new HttpClient() { Timeout = TimeSpan.FromMilliseconds(300) };
|
||||
[MethodMember(Name = "小车暂停", Description = "小车暂停")]
|
||||
[I18N.DocumentTranslation(Name = "EmergencyStop", Description = "EmergencyStop", locale = "en")]
|
||||
public void EmergencyStop(string reason)
|
||||
{
|
||||
Diagnosis.Post($"car{name}:EmergencyStop");
|
||||
hc.GetStringAsync($"http://{address}:8008/setValue?FieldName=ISRelease&Value=1");
|
||||
}
|
||||
|
||||
[MethodMember(Name = "小车暂停恢复", Description = "小车暂停恢复")]
|
||||
[I18N.DocumentTranslation(Name = "EmergencyRelease", Description = "EmergencyRelease", locale = "en")]
|
||||
public void EmergencyRelease()
|
||||
{
|
||||
Diagnosis.Post($"car{name}:EmergencyStop release");
|
||||
hc.GetStringAsync($"http://{address}:8008/setValue?FieldName=ISRelease&Value=0");
|
||||
}
|
||||
[MethodMember(Name = "小车重启C", Description = "小车重启Clumsy")]
|
||||
[I18N.DocumentTranslation(Name = "ResetClumsy", Description = "ResetClumsy", locale = "en")]
|
||||
public void ResetClumsy()
|
||||
{
|
||||
Diagnosis.Post($"car{name}-{address}:ResetClumsy");
|
||||
hc.GetStringAsync($"http://{address}:8008/reset");
|
||||
}
|
||||
|
||||
[MethodMember(Name = "显示车辆监控", Description = "打开车辆状态监控窗口")]
|
||||
public void ShowVehicleMonitor()
|
||||
{
|
||||
VehicleMonitor.ShowMonitor();
|
||||
}
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
[MethodMember(Name = "测试", Description = "循环")]
|
||||
[I18N.DocumentTranslation(Name = "Test", Description = "Loop", locale = "en")]
|
||||
public async void Test()
|
||||
{
|
||||
var siteA = SimpleLib.GetAllSites().First(s => s.name == "A");
|
||||
var siteB = SimpleLib.GetAllSites().First(s => s.name == "B");
|
||||
while (true)
|
||||
{
|
||||
var plan = new SegmentPlan() { usingCar = this };
|
||||
plan.FindRoute(SimpleLib.GetSite(this.GetLastSite()), siteA);
|
||||
await plan.Compile("GoA").Queue();
|
||||
var planB = new SegmentPlan() { usingCar = this };
|
||||
planB.FindRoute(SimpleLib.GetSite(this.GetLastSite()), siteB);
|
||||
await planB.Compile("GoB").Queue();
|
||||
await Task.Delay(1000);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,255 @@
|
||||
using System.Drawing;
|
||||
using System.Linq;
|
||||
using System.Threading;
|
||||
using System.Threading.Tasks;
|
||||
|
||||
using SimpleLite.RCS;
|
||||
|
||||
using SimpleLite.RCS.CarTypes;
|
||||
using SimpleCore;
|
||||
using SimpleCore.Compiler;
|
||||
using SimpleCore.Library;
|
||||
using SimpleCore.PropType;
|
||||
using StandardScene.Coders;
|
||||
|
||||
namespace StandardScene.CarTypes
|
||||
{
|
||||
//钻车
|
||||
[TemplateTrackCoderSettings(
|
||||
priority = 18,
|
||||
useVerb = "(dst.name=='FetchSite1'||dst.name=='FetchSite2')&&(plan.action=='fetch')",
|
||||
templateString = "agv.Wait();agv.TireFollowing(${plan.TireNum},${plan.FrontLidarDetect},${src.id},${dst.id},${plan.FirstTire});agv.Wait();",
|
||||
blockVerb = "true",
|
||||
siteFields = typeof(MultiWheelLifterSiteFields),
|
||||
trackFields = typeof(MultiWheelLifterTrackFields),
|
||||
planFields = typeof(MultiWheelLifterPlanFields))]
|
||||
|
||||
[TemplateTrackCoderSettings(
|
||||
priority = 18,
|
||||
useVerb = "(dst.name=='PutSite1'||dst.name=='PutSite2')&&(plan.action=='leave')",
|
||||
templateString = "agv.Wait();agv.TireFollowing(${plan.TireNum},${plan.FrontLidarDetect},${src.id},${dst.id},${plan.FirstTire});agv.Wait();",
|
||||
blockVerb = "true",
|
||||
siteFields = typeof(MultiWheelLifterSiteFields),
|
||||
trackFields = typeof(MultiWheelLifterTrackFields),
|
||||
planFields = typeof(MultiWheelLifterPlanFields))]
|
||||
|
||||
//离开时钻车(后出的那台车)
|
||||
[TemplateTrackCoderSettings(
|
||||
priority = 18,
|
||||
useVerb = "dst.name=='ReadyFetchSite'&&plan.action=='leave'&&plan.Reverse==true",
|
||||
templateString = "agv.Wait();agv.TireFollowing(${plan.TireNum},${plan.FrontLidarDetect},${src.id},${dst.id},${plan.FirstTire});agv.Wait();" +
|
||||
"agv.LineTracking(${src.id},${dst.id},${plan.LineDistance});agv.Wait()",
|
||||
blockVerb = "true",
|
||||
siteFields = typeof(MultiWheelLifterSiteFields),
|
||||
trackFields = typeof(MultiWheelLifterTrackFields),
|
||||
planFields = typeof(MultiWheelLifterPlanFields))]
|
||||
|
||||
//离开时直线行走(先出的那台车)
|
||||
[TemplateTrackCoderSettings(
|
||||
priority = 18,
|
||||
useVerb = "(dst.name=='LeaveSite'||dst.name=='ReadyFetchSite')&&plan.action=='leave'&&plan.Reverse==false",
|
||||
templateString = "agv.Wait();agv.LineTracking(${src.id},${dst.id},${plan.LineDistance});agv.Wait();",
|
||||
blockVerb = "true",
|
||||
siteFields = typeof(MultiWheelLifterSiteFields),
|
||||
trackFields = typeof(MultiWheelLifterTrackFields),
|
||||
planFields = typeof(MultiWheelLifterPlanFields))]
|
||||
|
||||
//下使能+夹抱+上使能
|
||||
[TemplateSiteCoderSettings(
|
||||
priority = 18,
|
||||
useVerb = "((dst.name=='FetchSite1'||dst.name=='FetchSite2')&&plan.action=='clamp')",
|
||||
templateString = "agv.Wait();agv.DriverDisable();agv.Wait();agv.ClamptoTarget(${dst.ClampClose});agv.Wait();agv.DriverAble();",
|
||||
blockVerb = "true",
|
||||
siteFields = typeof(MultiWheelLifterSiteFields),
|
||||
trackFields = typeof(MultiWheelLifterTrackFields),
|
||||
planFields = typeof(MultiWheelLifterPlanFields))]
|
||||
|
||||
|
||||
//抱夹打开
|
||||
[TemplateSiteCoderSettings(
|
||||
priority = 18,
|
||||
useVerb = "(dst.name=='PutSite1'||dst.name=='PutSite2')&&plan.action=='put'",
|
||||
templateString = "agv.Wait();agv.DriverDisable();agv.Wait();agv.ClamptoTarget(${dst.ClampClose});agv.Wait();agv.DriverAble();",
|
||||
blockVerb = "true",
|
||||
siteFields = typeof(MultiWheelLifterSiteFields),
|
||||
trackFields = typeof(MultiWheelLifterTrackFields),
|
||||
planFields = typeof(MultiWheelLifterPlanFields))]
|
||||
|
||||
|
||||
//临时夹抱动作
|
||||
[TemplateSiteCoderSettings(
|
||||
priority = 18,
|
||||
useVerb = "plan.action=='templateclamp'||plan.action=='templateput'",
|
||||
templateString = "agv.Wait();agv.DriverDisable();agv.Wait();agv.ClamptoTarget(${plan.ClampClose});agv.Wait();agv.DriverAble();",
|
||||
blockVerb = "true",
|
||||
siteFields = typeof(MultiWheelLifterSiteFields),
|
||||
trackFields = typeof(MultiWheelLifterTrackFields),
|
||||
planFields = typeof(MultiWheelLifterPlanFields))]
|
||||
|
||||
[TemplateTrackCoderSettings(
|
||||
priority = 2,
|
||||
useVerb = "plan.action=='fetch'",
|
||||
templateString = "agv.BasicGo(${src.x},${src.y},${src.id},${dst.x},${dst.y},${dst.id}," +
|
||||
"${track.id},${track.Speed},${track.Reverse||plan.Reverse}," +
|
||||
"${track.CarDirectionBias},${track.EnableCarAbsoluteDirection},${track.CarAbsoluteDirection},${track.MagnetChoose},${track.MultiVehicleSync},${track.typeInfo});",
|
||||
blockVerb = "true",
|
||||
siteFields = typeof(MultiWheelLifterSiteFields),
|
||||
trackFields = typeof(MultiWheelLifterTrackFields),
|
||||
planFields = typeof(MultiWheelLifterPlanFields))]
|
||||
|
||||
//[TemplateTrackCoderSettings(
|
||||
// priority = 3,
|
||||
// useVerb = "plan.action=='leave'&&(dst.name=='KeepDirectionSite1'||dst.name=='KeepDirectionSite2')",
|
||||
// templateString = "agv.BasicGo(${src.x},${src.y},${src.id},${dst.x},${dst.y},${dst.id}," +
|
||||
// "${track.id},${track.Speed},${track.Reverse||plan.Reverse}," +
|
||||
// "${track.CarDirectionBias},${track.EnableCarAbsoluteDirection},${track.CarAbsoluteDirection},${track.MagnetChoose},${track.MultiVehicleSync},${track.typeInfo});",
|
||||
// blockVerb = "true",
|
||||
// siteFields = typeof(MultiWheelLifterSiteFields),
|
||||
// trackFields = typeof(MultiWheelLifterTrackFields),
|
||||
// planFields = typeof(MultiWheelLifterPlanFields))]
|
||||
|
||||
[TemplateTrackCoderSettings(
|
||||
priority = 3,
|
||||
useVerb = "plan.action=='leave'",
|
||||
templateString = "agv.BasicGo(${src.x},${src.y},${src.id},${dst.x},${dst.y},${dst.id}," +
|
||||
"${track.id},${track.Speed},${track.Reverse||plan.Reverse}," +
|
||||
"${track.CarDirectionBias},${track.EnableCarAbsoluteDirection},${track.CarAbsoluteDirection},${track.MagnetChoose},${track.MultiVehicleSync},${track.typeInfo});",
|
||||
blockVerb = "true",
|
||||
siteFields = typeof(MultiWheelLifterSiteFields),
|
||||
trackFields = typeof(MultiWheelLifterTrackFields),
|
||||
planFields = typeof(MultiWheelLifterPlanFields))]
|
||||
|
||||
[TemplateTrackCoderSettings(
|
||||
priority = 19,
|
||||
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(MultiWheelLifterSiteFields),
|
||||
trackFields = typeof(MultiWheelLifterTrackFields),
|
||||
planFields = typeof(MultiWheelLifterPlanFields))]
|
||||
|
||||
// 通用避障/IO coder 已抽离为 StandardScene.Coders.*(见类前 [ProgramTrackCoderSettings] 引用)
|
||||
|
||||
// 避障区尺寸(4参,含中心点)切换 coder 已抽离为 StandardScene.Coders.AvoidanceParamCoder(见类前 [ProgramTrackCoderSettings] 引用)
|
||||
|
||||
// 纠偏阈值 coder 已抽离为 StandardScene.Coders.TrackingErrThreshCoder
|
||||
|
||||
[ProgramTrackCoderSettings(priority = 27, program = typeof(LidarAreaSwitchCoder))]
|
||||
[ProgramTrackCoderSettings(priority = 20, program = typeof(AvoidanceDistanceCoder))]
|
||||
[ProgramTrackCoderSettings(priority = 20, program = typeof(IoAreaSwitchCoder))]
|
||||
[ProgramTrackCoderSettings(priority = 20, program = typeof(TrackingErrThreshCoder))]
|
||||
[ProgramTrackCoderSettings(priority = 20, program = typeof(AvoidanceParamCoder))]
|
||||
[EnvelopConfig(lengthX = 2000, lengthY = 2000, centerX = 0, centerY = 0)]
|
||||
|
||||
[CarType(Name = "多车联动AGV")]
|
||||
public class MultiVehicleCar : GhostCar
|
||||
{
|
||||
[FieldMember] public float CarLength = 2000;
|
||||
[FieldMember] public float CarWidth = 1400;
|
||||
public static async Task<MultiVehicleCar> Create()
|
||||
{
|
||||
var car = new MultiVehicleCar()
|
||||
{
|
||||
lstatus = "连接中",
|
||||
address = "127.0.0.1",
|
||||
name = "联动AGV"
|
||||
};
|
||||
return car;
|
||||
}
|
||||
|
||||
protected override void draw(Graphics eGraphics)
|
||||
{
|
||||
var HalfCarLength = CarLength / 2;
|
||||
var HalfCarWidth = CarWidth / 2;
|
||||
eGraphics.FillRectangle(Brushes.Gray, -HalfCarLength, -HalfCarWidth, HalfCarLength * 2, HalfCarWidth * 2);
|
||||
eGraphics.DrawRectangle(Pens.White, -HalfCarLength, -HalfCarWidth, HalfCarLength * 2, HalfCarWidth * 2);
|
||||
eGraphics.DrawLine(Pens.White, HalfCarLength - HalfCarWidth, -HalfCarWidth, HalfCarLength, 0);
|
||||
eGraphics.DrawLine(Pens.White, HalfCarLength - HalfCarWidth, HalfCarWidth, HalfCarLength, 0);
|
||||
}
|
||||
|
||||
[MethodMember(Name = "模拟车联动初始化")]
|
||||
public void GhostMultiAgvSyncInit()
|
||||
{
|
||||
var car1 = (GhostCar)SimpleLib.GetAllCars().First(cc => cc.name == "sync-1");
|
||||
while (true)
|
||||
{
|
||||
car1.ManualTeleport();
|
||||
car1.x = 1770;
|
||||
car1.y = 0;
|
||||
car1.th = 0;
|
||||
Thread.Sleep(100);
|
||||
if (LessMath.dist(car1.x, car1.y, 1770, 0) < 1 && car1.th == 0) break;
|
||||
}
|
||||
car1.NoSchedule();
|
||||
car1.siteID = -1;
|
||||
car1.tags.Clear();
|
||||
car1.status.usage.AddUsage("base", new CarUsage.CarUsageInfo() { scheduling = false, refreshing = false });
|
||||
car1.fields["MultiVehicleSync"] = "True";
|
||||
|
||||
var car2 = (GhostCar)SimpleLib.GetAllCars().First(cc => cc.name == "sync-2");
|
||||
while (true)
|
||||
{
|
||||
//car2.ManualTeleport(-750, 0, 180);
|
||||
car2.ManualTeleport();
|
||||
car2.x = -750;
|
||||
car2.y = 0;
|
||||
car2.th = 180;
|
||||
Thread.Sleep(100);
|
||||
if (LessMath.dist(car2.x, car2.y, -750, 0) < 1 && car2.th == 180) break;
|
||||
}
|
||||
var car2Site = SimpleLib.GetAllSites().OrderBy(ss => LessMath.dist(ss.x, ss.y, car2.x, car2.y)).First();
|
||||
var syncInitSite = SimpleLib.GetSite(int.Parse(car2Site.fields["SyncInitId"]));
|
||||
car2.TrafficReset(syncInitSite);
|
||||
car2.siteID = syncInitSite.id;
|
||||
car2.fields["MultiVehicleSync"] = "True";
|
||||
}
|
||||
|
||||
[MethodMember(Name = "进入联动状态")]
|
||||
public void StartMultiAgvSync()
|
||||
{
|
||||
var car1 = (Car)SimpleLib.GetAllCars().First(cc => cc.name == "sync-1");
|
||||
car1.NoSchedule();
|
||||
car1.siteID = -1;
|
||||
car1.tags.Clear();
|
||||
car1.status.usage.AddUsage("base", new CarUsage.CarUsageInfo() { scheduling = false, refreshing = false });
|
||||
car1.fields["MultiVehicleSync"] = "True";
|
||||
|
||||
var car2 = (Car)SimpleLib.GetAllCars().First(cc => cc.name == "sync-2");
|
||||
var car2Site = SimpleLib.GetAllSites().OrderBy(ss => LessMath.dist(ss.x, ss.y, car2.x, car2.y)).First();
|
||||
var syncInitSite = SimpleLib.GetSite(int.Parse(car2Site.fields["SyncInitId"]));
|
||||
car2.TrafficReset(syncInitSite);
|
||||
car2.siteID = syncInitSite.id;
|
||||
car2.fields["MultiVehicleSync"] = "True";
|
||||
}
|
||||
|
||||
[MethodMember(Name = "退出联动状态")]
|
||||
public void ExitMultiAgvSync()
|
||||
{
|
||||
var car1 = (Car)SimpleLib.GetAllCars().First(cc => cc.name == "sync-1");
|
||||
car1.fields["MultiVehicleSync"] = "False";
|
||||
car1.Reset();
|
||||
var car2 = (Car)SimpleLib.GetAllCars().First(cc => cc.name == "sync-2");
|
||||
car2.fields["MultiVehicleSync"] = "False";
|
||||
car2.Reset();
|
||||
}
|
||||
|
||||
//[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 = "返厂检修";
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,156 @@
|
||||
using LessokajiWeaverUtilities.Utilities;
|
||||
using SimpleLite.RCS;
|
||||
using SimpleLite.RCS.CarTypes;
|
||||
using SimpleCore;
|
||||
using SimpleCore.Compiler;
|
||||
using SimpleCore.PropType;
|
||||
using StandardScene.Model;
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Linq;
|
||||
using System.Text;
|
||||
using System.Threading.Tasks;
|
||||
using static System.Windows.Forms.VisualStyles.VisualStyleElement;
|
||||
|
||||
namespace StandardScene.CarTypes
|
||||
{
|
||||
class SiteFields
|
||||
{
|
||||
public float FetchSpeed = 0;
|
||||
public float FetchLiftDownTarget = -1;
|
||||
public float FetchLiftUpTarget = -1;
|
||||
public float PutSpeed = 0;
|
||||
public float PutLiftDownTarget = -1;
|
||||
public float PutLiftUpTarget = -1;
|
||||
public float LeaveShelfSpeed = 0;
|
||||
public float LeaveShelfLiftDownTarget = -1;
|
||||
public bool Shelf = false;
|
||||
}
|
||||
|
||||
class TrackFields
|
||||
{
|
||||
public int ReverseDst = -1;
|
||||
}
|
||||
|
||||
class PlanFields
|
||||
{
|
||||
public string action = "/";
|
||||
}
|
||||
|
||||
//[TemplateTrackCoderSettings(
|
||||
// priority = 20,
|
||||
// useVerb = "src.Shelf&&plan.curSeg==1",
|
||||
// templateString = "agv.Wait();agv.LeaveShelf(${src.x},${src.y},${src.id},${dst.x},${dst.y},${dst.id},${src.LeaveShelfSpeed},${src.LeaveShelfLiftDownTarget});agv.Wait();",
|
||||
// blockVerb = "true",
|
||||
// siteFields = typeof(SiteFields),
|
||||
// trackFields = typeof(TrackFields),
|
||||
// planFields = typeof(PlanFields))]
|
||||
[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.FetchLiftDownTarget},${dst.FetchLiftUpTarget});agv.Wait();",
|
||||
blockVerb = "true",
|
||||
siteFields = typeof(SiteFields),
|
||||
trackFields = typeof(TrackFields),
|
||||
planFields = typeof(PlanFields))]
|
||||
[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.FetchSpeed},${dst.FetchLiftDownTarget},${dst.FetchLiftUpTarget});agv.Wait();",
|
||||
blockVerb = "true",
|
||||
siteFields = typeof(SiteFields),
|
||||
trackFields = typeof(TrackFields),
|
||||
planFields = typeof(PlanFields))]
|
||||
|
||||
|
||||
[CarType(Name = "多舵轮叉车", editor = typeof(MultiWheelForkLifter))]
|
||||
[I18N.DocumentTranslation(Name = "MultiWheel ForkLift Car", locale = "en")]
|
||||
public class MultiWheelForkLifter : GhostCar
|
||||
{
|
||||
public static async Task<MultiWheelForkLifter> Create()
|
||||
{
|
||||
var car = new MultiWheelForkLifter()
|
||||
{
|
||||
lstatus = "连接中",
|
||||
address = "127.0.0.1",
|
||||
name = "多舵轮叉车",
|
||||
haveCoordination = true,
|
||||
speed = 1
|
||||
};
|
||||
return car;
|
||||
}
|
||||
public static string GetCarStatus(Car car, string key)
|
||||
{
|
||||
string value = "0";
|
||||
if (car == null) return value;
|
||||
if (car.status.enums.TryGetValue(key, out var valueStr))
|
||||
value = valueStr;
|
||||
return value;
|
||||
}
|
||||
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 = GetCarStatus(this, "AlarmInfo");
|
||||
if (alarmStr != "")
|
||||
str = $"{str}|{alarmStr}";
|
||||
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";
|
||||
}
|
||||
}
|
||||
|
||||
private bool _running = false;
|
||||
[MethodMember(Name = "test")]
|
||||
public async void Test()
|
||||
{
|
||||
_running = true;
|
||||
var siteA = SimpleLib.GetAllSites().First(s => s.name == "A");
|
||||
var siteB = SimpleLib.GetAllSites().First(s => s.name == "B");
|
||||
var siteC = SimpleLib.GetAllSites().First(s => s.name == "C");
|
||||
while (_running)
|
||||
{
|
||||
var fetchPlan = new SegmentPlan() { usingCar = this };
|
||||
fetchPlan.fields["action"] = "fetch";
|
||||
fetchPlan.FindRoute(SimpleLib.GetSite(GetLastSite()), siteA);
|
||||
await fetchPlan.Compile("fetch").Queue();
|
||||
var putPlan = new SegmentPlan() { usingCar = this };
|
||||
putPlan.fields["action"] = "put";
|
||||
putPlan.FindRoute(SimpleLib.GetSite(GetLastSite()), siteB);
|
||||
await putPlan.Compile("put").Queue();
|
||||
var plan = new SegmentPlan() { usingCar = this };
|
||||
plan.FindRoute(SimpleLib.GetSite(GetLastSite()), siteC);
|
||||
await plan.Compile("go").Queue();
|
||||
var fetchPlan2 = new SegmentPlan() { usingCar = this };
|
||||
fetchPlan2.fields["action"] = "fetch";
|
||||
fetchPlan2.FindRoute(SimpleLib.GetSite(GetLastSite()), siteB);
|
||||
await fetchPlan2.Compile("fetch").Queue();
|
||||
var putPlan2 = new SegmentPlan() { usingCar = this };
|
||||
putPlan2.fields["action"] = "put";
|
||||
putPlan2.FindRoute(SimpleLib.GetSite(GetLastSite()), siteA);
|
||||
await putPlan2.Compile("put").Queue();
|
||||
var plan2 = new SegmentPlan() { usingCar = this };
|
||||
plan2.FindRoute(SimpleLib.GetSite(GetLastSite()), siteC);
|
||||
await plan2.Compile("go").Queue();
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,37 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using SimpleCore.Navigation;
|
||||
using StandardScene.CarTypes;
|
||||
|
||||
namespace StandardScene.QrLidar
|
||||
{
|
||||
/// <summary>
|
||||
/// scene.qrlidar 平台画像:激光 + 二维码融合导航场景插件。
|
||||
/// <para>激光(SLAM 坐标导航)由内核 GhostCar 的 BasicGo 兜底提供;二维码 QrGo 按轨道两端
|
||||
/// tag 字段逐段触发——同一台车同一条路线可全激光、全二维码或混合(融合 / 单独使用均可)。</para>
|
||||
/// </summary>
|
||||
public sealed class QrLidarSceneProfile : NavigationProfileBase
|
||||
{
|
||||
public override NavKind Kind => NavKind.Laser;
|
||||
|
||||
public override IReadOnlyList<NavKind> Kinds => new[] { NavKind.Laser, NavKind.QrCode };
|
||||
|
||||
public override string SceneId => "scene.qrlidar";
|
||||
|
||||
public override string DisplayName => "激光+二维码平台";
|
||||
|
||||
public override IReadOnlyList<Type> CarTypes => new[]
|
||||
{
|
||||
typeof(Forklift),
|
||||
typeof(MultiWheelForkLifter),
|
||||
typeof(DualLiftingCar),
|
||||
typeof(MultiVehicleCar),
|
||||
typeof(ArmCar),
|
||||
};
|
||||
|
||||
public override void OnActivate(ISceneContext context)
|
||||
{
|
||||
context.Log($"{DisplayName} 已激活(车型:叉车 / 多舵轮叉车 / 锂电双举升 / 多车联动 / ArmCar)");
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,58 @@
|
||||
<Project Sdk="Microsoft.NET.Sdk">
|
||||
|
||||
<PropertyGroup>
|
||||
<TargetFramework>net8.0-windows</TargetFramework>
|
||||
<OutputType>Library</OutputType>
|
||||
<UseWindowsForms>true</UseWindowsForms>
|
||||
<RootNamespace>StandardScene.QrLidar</RootNamespace>
|
||||
<AssemblyName>StandardScene.QrLidar</AssemblyName>
|
||||
<LangVersion>latest</LangVersion>
|
||||
<AllowUnsafeBlocks>true</AllowUnsafeBlocks>
|
||||
<Platforms>AnyCPU;x64</Platforms>
|
||||
<PlatformTarget>x64</PlatformTarget>
|
||||
<Deterministic>true</Deterministic>
|
||||
<GenerateAssemblyInfo>false</GenerateAssemblyInfo>
|
||||
<ImplicitUsings>disable</ImplicitUsings>
|
||||
<Nullable>disable</Nullable>
|
||||
<AppendTargetFrameworkToOutputPath>false</AppendTargetFrameworkToOutputPath>
|
||||
<NoWarn>$(NoWarn);NU1701;CS0618;CS0612;MSB3277;CA1416</NoWarn>
|
||||
<AssemblySearchPaths>{HintPathFromItem};{TargetFrameworkDirectory};{RawFileName};{GAC}</AssemblySearchPaths>
|
||||
</PropertyGroup>
|
||||
|
||||
<!-- 插件清单:随 dll 输出,供 SimpleLite plugins 选择性加载(约定 <dll>.scene.json) -->
|
||||
<ItemGroup>
|
||||
<None Update="StandardScene.QrLidar.scene.json" CopyToOutputDirectory="PreserveNewest" />
|
||||
</ItemGroup>
|
||||
|
||||
<!-- 基座:字段袋 / 通用 coder / 任务调度等留 Core;本 dll 为 scene.qrlidar 平台
|
||||
(激光坐标导航 BasicGo 由内核 GhostCar 提供,二维码 QrGo 按轨道 tag 逐段触发,可融合可单用) -->
|
||||
<ItemGroup>
|
||||
<ProjectReference Include="..\StandardScene.Core\StandardScene.Core.csproj" />
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<Reference Include="SimpleLite">
|
||||
<HintPath>E:\Work\Core\Simple-FR\Simple\SimpleLite\bin\Debug\SimpleLite.dll</HintPath>
|
||||
</Reference>
|
||||
<Reference Include="SimpleCore">
|
||||
<HintPath>E:\Work\Core\Simple-FR\Simple\SimpleCore\bin\Debug\netstandard2.0\SimpleCore.dll</HintPath>
|
||||
</Reference>
|
||||
<Reference Include="CommonUsage">
|
||||
<HintPath>D:\MDCS\Dependencies\Commons\CommonUsage.dll</HintPath>
|
||||
</Reference>
|
||||
<Reference Include="MDCSToolBox">
|
||||
<HintPath>D:\MDCS\Dependencies\Commons\MDCSToolBox.dll</HintPath>
|
||||
</Reference>
|
||||
<Reference Include="Topaz">
|
||||
<HintPath>E:\Work\Core\Simple-FR\Simple\tools\Topaz.dll</HintPath>
|
||||
</Reference>
|
||||
<Reference Include="LessokajiWeaverUtilities">
|
||||
<HintPath>E:\Work\Core\Simple-FR\Simple\SimpleLite\bin\Debug\LessokajiWeaverUtilities.dll</HintPath>
|
||||
</Reference>
|
||||
</ItemGroup>
|
||||
|
||||
<ItemGroup>
|
||||
<PackageReference Include="Newtonsoft.Json" Version="13.0.4" />
|
||||
</ItemGroup>
|
||||
|
||||
</Project>
|
||||
@@ -0,0 +1,13 @@
|
||||
{
|
||||
"id": "scene.qrlidar",
|
||||
"displayName": "激光+二维码平台",
|
||||
"navKind": "laser",
|
||||
"navKinds": [ "laser", "qrcode" ],
|
||||
"assembly": "StandardScene.QrLidar.dll",
|
||||
"coreVersion": ">=1.0.0",
|
||||
"requiresCore": "StandardScene.dll",
|
||||
"provides": {
|
||||
"carTypes": [ "Forklift", "MultiWheelForkLifter", "DualLiftingCar", "MultiVehicleCar", "ArmCar" ],
|
||||
"missionTypes": []
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user