GetPath.cs
55.1 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
1001
1002
1003
1004
1005
1006
1007
1008
1009
1010
1011
1012
1013
1014
1015
1016
1017
1018
1019
1020
1021
1022
1023
1024
1025
1026
1027
1028
1029
1030
1031
1032
1033
1034
1035
1036
1037
1038
1039
1040
1041
1042
1043
1044
1045
1046
1047
1048
1049
1050
1051
1052
1053
1054
1055
1056
1057
1058
1059
using RCS.Dal;
using RCS.Model.Comm;
using RCS.Model.Entity;
using RCS.WinClient.Common;
using SqlSugar;
using System.Threading.Tasks;
using static RCS.Model.Comm.EnumMsg;
namespace RCS.WinClient.Service
{
public class GetPath
{
/// <summary>
/// 为子任务分配路径
/// </summary>
public static void AssignePath()
{
foreach (Base_Agv agv in App.AgvList.FindAll(a => a.IsEnable))
{
var agvPoint = App.PointList.FirstOrDefault(x => x.Barcode == agv.Barcode);
if (agvPoint == null)
{
continue;
}
////由于地图是环形单向的，所以倒车行走的点位，在地图上是没有路径方向的，需要临时加方向、删除方向锁，等路径生成完后，再加上。
//var pointDirectionDic = new Dictionary<EnumMsg.Direction, Tuple<Base_Point, List<TmpDirection>>>();
try
{
if (agv.AgvState != EnumMsg.AGVState.自动空闲 && agv.AgvState != EnumMsg.AGVState.充电中)
{
continue;
}
//if (task != null && task.SubTaskList.Count > 0)
//{
// App.ExFile.MessageLog("AssignePath", $"主任务【{task.TaskNo}】子任务【{task.SubTaskList[0].SubTaskNo}】子任务状态【{task.SubTaskList[0].SubTaskState}】");
//}
if (agv.AgvTask == null || agv.AgvTask.SubTaskList == null || agv.AgvTask.SubTaskList.Count == 0 || agv.AgvTask.SubTaskList[0].SubTaskState > EnumMsg.SubTaskState.重建路径中)
{
continue;
}
var task = agv.AgvTask;
var subTask = task.SubTaskList.First();
#region 判断任务是否能执行
//如果下个任务是倒车行走，且当前已经在下个任务的终点，就删除当前任务
if (subTask.ActionType == EnumMsg.ActionType.普通行走 && task.SubTaskList.Count > 1)
{
var nextSubTask = task.SubTaskList.Skip(1).First();
if (nextSubTask.ActionType == EnumMsg.ActionType.倒车行走 && nextSubTask.EndBarcode == agv.Barcode)
{
//更新界面
App.AppDispatcher.Invoke(new Action(() =>
{
task.SubTaskList.Remove(subTask);
agv.PathPointList.Clear();
}));
continue;
}
}
if (subTask.ActionType != EnumMsg.ActionType.普通行走 && subTask.StartBarcode != agv.Barcode)
{
task.TaskErrMsg = "AGV不在子任务的起点，不能生成路径！";
continue;
}
var sonTaskDb = new BaseDal<Base_SubTask>();
if (subTask.SubTaskState == EnumMsg.SubTaskState.等待分配终点)
{
task.TaskErrMsg = "已经取消不分配终点的任务，请检查数据！";
continue;
#region WMS出库区任务没有终点，先到720点，再请求WMS分配终点。经过沟通改为出库区任务也分配起点和终点，就暂时注释处理。
////如果没有终点，就向WMS请求获取终点
//if (task.EndPoint == null)
//{
// //请求WMS获取终点
// var getResult = WebApiClient.GetEndBarcode(task);
// if (!getResult.Success)
// {
// continue;
// }
// var endPoint = App.PointList.FirstOrDefault(x => x.Barcode == getResult.Data);
// if (endPoint == null)
// {
// agv.StopReason = ComnMethod.GetStopReason(agv.StopReason, $"WMS分配的点位【{getResult.Data}】不存在！");
// continue;
// }
// task.EndPoint = endPoint;
// task.EndBarcode = task.EndPoint.Barcode;
//}
////根据终点获取倒退点
//var backPoint = ComnMethod.GetOutAreaBackPoint(task.EndPoint);
//if (backPoint == null)
//{
// agv.StopReason = ComnMethod.GetStopReason(agv.StopReason, $"根据终点【{task.EndBarcode}】获取不到倒退点！");
// continue;
//}
////根据终点获取终点前置点
//var prePoint = ComnMethod.GetOutAreaPrePoint(task.EndPoint);
//if (prePoint == null)
//{
// agv.StopReason = ComnMethod.GetStopReason(agv.StopReason, $"根据终点【{task.EndBarcode}】获取不到终点前置点！");
// continue;
//}
////第9个子任务是行走到倒退点
//var waitSonTask = task.SubTaskList.First(x => x.SerialNo == 9);
//waitSonTask.EndPoint = backPoint;
//waitSonTask.EndBarcode = backPoint.Barcode;
//waitSonTask.SubTaskState = EnumMsg.SubTaskState.等待建立路径;
////第10个子任务是倒退到终点
//waitSonTask = task.SubTaskList.First(x => x.SerialNo == 10);
//waitSonTask.StartPoint = backPoint;
//waitSonTask.StartBarcode = backPoint.Barcode;
//waitSonTask.EndPoint = task.EndPoint;
//waitSonTask.EndBarcode = task.EndPoint.Barcode;
//waitSonTask.SubTaskState = EnumMsg.SubTaskState.等待建立路径;
////第11个子任务是货叉下降
//waitSonTask = task.SubTaskList.First(x => x.SerialNo == 11);
//waitSonTask.StartPoint = task.EndPoint;
//waitSonTask.StartBarcode = task.EndPoint.Barcode;
//waitSonTask.EndPoint = task.EndPoint;
//waitSonTask.EndBarcode = task.EndPoint.Barcode;
//waitSonTask.SubTaskState = EnumMsg.SubTaskState.等待建立路径;
////第12个子任务是前进到终点前置点
//waitSonTask = task.SubTaskList.First(x => x.SerialNo == 12);
//waitSonTask.StartPoint = task.EndPoint;
//waitSonTask.StartBarcode = task.EndPoint.Barcode;
//waitSonTask.EndPoint = prePoint;
//waitSonTask.EndBarcode = prePoint.Barcode;
//waitSonTask.SubTaskState = EnumMsg.SubTaskState.等待建立路径;
////第13个子任务是货叉升到行走高度
//waitSonTask = task.SubTaskList.First(x => x.SerialNo == 13);
//waitSonTask.StartPoint = prePoint;
//waitSonTask.StartBarcode = prePoint.Barcode;
//waitSonTask.EndPoint = prePoint;
//waitSonTask.EndBarcode = prePoint.Barcode;
//waitSonTask.SubTaskState = EnumMsg.SubTaskState.等待建立路径;
//var subTasks = task.SubTaskList.Where(x => x.SerialNo == 9 || x.SerialNo == 10 || x.SerialNo == 11 || x.SerialNo == 12 || x.SerialNo == 13).ToArray();
////更新到数据库
//var updateResult = sonTaskDb.Update(subTasks, x => new { x.StartBarcode, x.EndBarcode, x.SubTaskState }, null, x => x.SubTaskNo);
//if (!updateResult.Success)
//{
// foreach (var subTaskItem in subTasks)
// {
// subTaskItem.SubTaskState = EnumMsg.SubTaskState.等待分配终点;
// }
// var msg = $"更新子任务到数据库失败，原因：{updateResult.Msg}";
// agv.StopReason = ComnMethod.GetStopReason(agv.StopReason, msg);
// App.ExFile.MessageError("AssignePath", msg);
// continue;
//}
#endregion
}
#endregion
#region 倒车行走的临时路径处理旧方法，先注释
//if (subTask.ActionType == EnumMsg.ActionType.倒车行走)
//{
////如果是去站台附属点、充电桩，就把路径方向设置为ture，并且清空方向锁，等生成完路径，再还原。
//if (subTask.EndPoint.PointType == EnumMsg.PointType.站台附属点 || subTask.EndPoint.PointType == EnumMsg.PointType.充电点)
//{
// if (agvPoint.IntX > subTask.EndPoint.IntX && agvPoint.XNegPoint != null)
// {
// var direction = EnumMsg.Direction.无方向;
// var tuple = new Tuple<Base_Point, List<TmpDirection>>(agvPoint.XNegPoint, agvPoint.XNegPoint.TmpDirectionList);
// agvPoint.XNegPoint.TmpDirectionList = new List<TmpDirection>();
// if (agvPoint.IsXNeg == false)
// {
// agvPoint.IsXNeg = true;
// direction = EnumMsg.Direction.X负方向;
// }
// pointDirectionDic.Add(direction, tuple);
// }
// if (agvPoint.IntX < subTask.EndPoint.IntX && agvPoint.XPosPoint != null)
// {
// var direction = EnumMsg.Direction.无方向;
// var tuple = new Tuple<Base_Point, List<TmpDirection>>(agvPoint.XPosPoint, agvPoint.XPosPoint.TmpDirectionList);
// agvPoint.XPosPoint.TmpDirectionList = new List<TmpDirection>();
// if (agvPoint.IsXPos == false)
// {
// agvPoint.IsXPos = true;
// direction = EnumMsg.Direction.X正方向;
// }
// pointDirectionDic.Add(direction, tuple);
// }
// if (agvPoint.IntY > subTask.EndPoint.IntY && agvPoint.YNegPoint != null)
// {
// var direction = EnumMsg.Direction.无方向;
// var tuple = new Tuple<Base_Point, List<TmpDirection>>(agvPoint.YNegPoint, agvPoint.YNegPoint.TmpDirectionList);
// agvPoint.YNegPoint.TmpDirectionList = new List<TmpDirection>();
// if (agvPoint.IsYNeg == false)
// {
// agvPoint.IsYNeg = true;
// direction = EnumMsg.Direction.Y负方向;
// }
// pointDirectionDic.Add(direction, tuple);
// }
// if (agvPoint.IntY < subTask.EndPoint.IntY && agvPoint.YPosPoint != null)
// {
// var direction = EnumMsg.Direction.无方向;
// var tuple = new Tuple<Base_Point, List<TmpDirection>>(agvPoint.YPosPoint, agvPoint.YPosPoint.TmpDirectionList);
// agvPoint.YPosPoint.TmpDirectionList = new List<TmpDirection>();
// if (agvPoint.IsYPos == false)
// {
// agvPoint.IsYPos = true;
// direction = EnumMsg.Direction.Y正方向;
// }
// pointDirectionDic.Add(direction, tuple);
// }
//}
//}
#endregion
List<Base_PathPoint> pathPointList = GetPathList(agv);
if (pathPointList == null || pathPointList.Count == 0)
{
if (subTask.SubTaskNo != App.AvoidSubTaskNo && subTask.SubTaskState != EnumMsg.SubTaskState.建立路径失败)
{
var updateResult = sonTaskDb.Update(it => it.SubTaskNo == subTask.SubTaskNo, it => new Base_SubTask()
{
SubTaskState = EnumMsg.SubTaskState.建立路径失败
});
if (!updateResult.IsSuccess)
{
continue;
}
}
subTask.SubTaskState = EnumMsg.SubTaskState.建立路径失败;
continue;
}
//叉车类型禁止往站台中发普通行走任务，会撞到货架
if (agv.AgvTask.SubTaskList?.Get(0) is Base_SubTask sontask
&& sontask.ActionType == EnumMsg.ActionType.普通行走
&& agv.AgvType == EnumMsg.AgvType.叉车
&& (App.StationList.Any(x => x.Barcode == pathPointList.Last().Barcode)
|| App.StationList.Any(x => x.PreBarcode == pathPointList.Last().Barcode && x.Name.StartsWith("A"))))
{
task.TaskErrMsg = "叉车禁止往站台中发普通行走任务，会撞到货架，请检查任务路径是否合理";
continue;
}
//增加走弧点的判断，写入到路径点属性中
var insertPath = new List<Base_PathPoint>();
for (int i = 0; i < pathPointList.Count; i++)
{
var pathPoint = pathPointList[i];
pathPoint.SerialNo = i + 1;
pathPoint.SubTaskNo = subTask.SubTaskNo;
pathPoint.Barcode = pathPoint.Point.Barcode;
pathPoint.AreaType = pathPoint.Point.AreaType;
pathPoint.PathPointType = pathPoint.Point.PointType;
//不是拐点就不走弧
if (agv.AgvType == AgvType.叉车 && !pathPoint.IsCorner && pathPoint.PathPointType == EnumMsg.PointType.走弧点)
{
pathPoint.PathPointType = EnumMsg.PointType.普通行走点;
}
if (i > 0)
{
var previousPath = pathPointList[i - 1];
if (agv.AgvType == AgvType.叉车 && pathPoint.PathPointType == EnumMsg.PointType.走弧点 && previousPath.PathPointType == EnumMsg.PointType.走弧点)
{
if (previousPath.Point.ArcingLevel > pathPoint.Point.ArcingLevel)
{
pathPoint.PathPointType = EnumMsg.PointType.普通行走点;
}
else if (previousPath.Point.ArcingLevel < pathPoint.Point.ArcingLevel)
{
previousPath.PathPointType = EnumMsg.PointType.普通行走点;
}
else
{
subTask.SubTaskState = EnumMsg.SubTaskState.建立路径失败;
agv.AgvTask.TaskErrMsg = $"路径中出现连续两个走弧点【{previousPath.Barcode}，{pathPoint.Barcode}】并且等级都是【{previousPath.Point.ArcingLevel}】";
continue;
}
}
pathPoint.PreviousPathDirection = previousPath.PathDirection;
}
insertPath.Add(pathPoint);
}
if (subTask.SubTaskNo != App.AvoidSubTaskNo)
{
var pathPointDb = new BaseDal<Base_PathPoint>();
sonTaskDb.BeginTran();
var insertResult = pathPointDb.Insert(insertPath);
var updateResult = sonTaskDb.Update(it => it.SubTaskNo == subTask.SubTaskNo, it => new Base_SubTask()
{
SubTaskState = EnumMsg.SubTaskState.已建立路径
});
if (!updateResult.IsSuccess || !insertResult.IsSuccess)
{
sonTaskDb.RollbackTran();
App.ExFile.MessageBusinessErr("", $"建立路径失败，更新子任务状态【{updateResult.Message}】，插入路径数据【{insertResult.Message}】");
return;
}
else
{
sonTaskDb.CommitTran();
}
}
subTask.PathPointList = pathPointList;
subTask.SubTaskState = EnumMsg.SubTaskState.已建立路径;
agv.PathPointList = subTask.PathPointList.ToList();
task.TaskErrMsg = string.Empty;
}
catch (Exception ex)
{
App.ExFile.MessageError("AssignePath", $"原因：{ex.Message}，\n 堆栈：{ex.StackTrace}");
}
//finally
//{
// foreach (var pointDirection in pointDirectionDic)
// {
// switch (pointDirection.Key)
// {
// case EnumMsg.Direction.X正方向:
// agvPoint.IsXPos = false;
// break;
// case EnumMsg.Direction.X负方向:
// agvPoint.IsXNeg = false;
// break;
// case EnumMsg.Direction.Y正方向:
// agvPoint.IsYPos = false;
// break;
// case EnumMsg.Direction.Y负方向:
// agvPoint.IsYNeg = false;
// break;
// }
// pointDirection.Value.Item1.TmpDirectionList = pointDirection.Value.Item2;
// }
//}
}
}
/// <summary>
/// 生成路径
/// </summary>
/// <returns>路径集合</returns>
public static List<Base_PathPoint> GetPathList(Base_Agv agv, Base_SubTask? defaultSubTask = null)
{
try
{
var listPathPoint = new List<Base_PathPoint>();
if (agv.AgvTask == null || agv.AgvTask.SubTaskList == null || agv.AgvTask.SubTaskList.Count == 0)
{
return listPathPoint;
}
var subTask = defaultSubTask ?? agv.AgvTask.SubTaskList.First();
var startPoint = App.PointList.First(x => x.Barcode == agv.Barcode);
var endPoint = subTask.EndPoint;
//判断地标是否在同一区域，如果不在同一个区域，就获取同一个区域的地标
var listPoints = ComnMethod.GetAreaPoint(startPoint, endPoint);
if (listPoints.Count == 0)
{
return listPathPoint;
}
startPoint = listPoints.First();
endPoint = listPoints.Last();
switch (subTask.ActionType)
{
case EnumMsg.ActionType.普通行走:
case EnumMsg.ActionType.倒车行走:
if (startPoint == endPoint)
{
listPathPoint.Add(ComnMethod.GetPathPoint(endPoint));
return listPathPoint;
}
var pathAStar = new PathAStar();
var listApoint = GetCanUseApoint(agv);
ClearParentPoint();
var parentPoint = pathAStar.GetPathPoint(listApoint, startPoint, endPoint, subTask.ActionType, agv.AgvDirectionByMap, false);
if (parentPoint == null)
{
return new List<Base_PathPoint>();
}
//获取路径点列表
listPathPoint = GetPathPointList(parentPoint, subTask);
var msg = $"AGV【{agv.AgvNo}】执行任务【{subTask.TaskNo}】子任务【{subTask.SubTaskNo}】起点【{subTask.StartBarcode}】终点【{subTask.EndBarcode}】生成路径【{string.Join(", ", listPathPoint.Select(x => x.Point.Barcode))}】";
if (parentPoint != endPoint || parentPoint.ParentPoint == null)
{
App.ExFile.MessageLog($"GetPathList_{agv.AgvNo}", msg + "，路径错误！");
return new List<Base_PathPoint>();
}
App.ExFile.MessageLog($"GetPathList_{agv.AgvNo}", msg);
#region 因为叉车AGV是站台前走弧点再多走一个点，然后再倒退取放货，所以如果下个动作是倒车行走，就根据生成路径的方向来延长一个点，修改子任务的终点。
if (agv.AgvType == AgvType.叉车 && subTask.ActionType == EnumMsg.ActionType.普通行走 && endPoint.PointType == EnumMsg.PointType.走弧点 && agv.AgvTask.SubTaskList.Count > 1)
{
//如果当前子任务的终点是站台附属点、充点电的前置点，且下一个动作是倒车行走，就要对终点延长，并且更新子任务的终点。
var pointType = new EnumMsg.PointType[] { EnumMsg.PointType.站台附属点, EnumMsg.PointType.充电点 };
if (IsPrePoint(endPoint, pointType) || endPoint.IsStop)
{
var nextSonTask = agv.AgvTask.SubTaskList.Skip(1).First();
if (nextSonTask.ActionType == EnumMsg.ActionType.倒车行走)
{
var newEndPoint = GetNewEndPoint(parentPoint);
if (newEndPoint == null)
{
App.ExFile.MessageLog($"GetPathList_{agv.AgvNo}", $"AGV【{agv.AgvNo}】执行任务【{subTask.TaskNo}】子任务【{subTask.SubTaskNo}】起点【{subTask.StartBarcode}】终点【{subTask.EndBarcode}】，获取延伸点失败！");
return new List<Base_PathPoint>();
}
//把新延长的路径，放到路径列表中
listPathPoint.Add(ComnMethod.GetPathPoint(newEndPoint));
App.ExFile.MessageBusinessErr($"GetPathList_{agv.AgvNo}", $"AGV【{agv.AgvNo}】执行任务【{subTask.TaskNo}】子任务【{subTask.SubTaskNo}】起点【{subTask.StartBarcode}】终点【{subTask.EndBarcode}】延伸一个点【{newEndPoint.Barcode}】！");
var oldPoint = subTask.EndPoint;
subTask.EndPoint = newEndPoint;
subTask.EndBarcode = newEndPoint.Barcode;
nextSonTask.StartPoint = newEndPoint;
nextSonTask.StartBarcode = newEndPoint.Barcode;
//更新新获取的子任务终点到数据库
var _sonTask = new BaseDal<Base_SubTask>();
var subTasks = new Base_SubTask[] { subTask, nextSonTask };
//更新到数据库
var updateResult = _sonTask.Update(subTasks, x => new { x.StartBarcode, x.EndBarcode }, null, x => x.SubTaskNo);
if (!updateResult.IsSuccess)
{
subTask.EndPoint = oldPoint;
nextSonTask.StartPoint = oldPoint;
subTask.EndBarcode = oldPoint?.Barcode;
nextSonTask.StartBarcode = oldPoint?.Barcode;
msg = $"更新子任务的点位到数据库失败，原因：{updateResult.Message}";
agv.AgvTask.TaskErrMsg = msg;
App.ExFile.MessageError($"GetPathList_{agv.AgvNo}", msg);
return new List<Base_PathPoint>();
}
}
}
}
#endregion
//如果不是出库区，倒车行走不能超过3个点位
if (agv.AgvType == AgvType.叉车 && subTask.ActionType == EnumMsg.ActionType.倒车行走 && listPathPoint.Count > 5)
{
subTask.SubTaskState = EnumMsg.SubTaskState.建立路径失败;
agv.AgvTask.TaskErrMsg = "倒车行走路径超过3个点，路径有误！";
return new List<Base_PathPoint>();
}
//获取路径方点的方向
GetPathDirection(listPathPoint);
//获取路径点的拐点
var isResult = GetPathCorner(listPathPoint, startPoint);
//锁定临时方向锁
OriLock(listPathPoint, agv);
//车类型不是叉车时，判断agv出站台后下一步动作是否会路线规划成功
if (agv.AgvType != AgvType.叉车
&& subTask.StartPoint.PointType == PointType.站台点
&& App.StationList.Any(x => x.Barcode == subTask.StartBarcode && x.PreBarcode == subTask.EndBarcode))
{
foreach (var _subTask in agv.AgvTask.SubTaskList)
{
if (_subTask == subTask || _subTask.StartBarcode == _subTask.EndBarcode) continue;
var pathList = GetPathList(agv, _subTask);
if (pathList == null || pathList.Count == 0)
{
App.ExFile.MessageBusinessErr("GetPathList", $"agv{agv.AgvNo}退出站台到下一目标点{_subTask.EndBarcode}时，路线规划失败，避免堵路，agv不出站台");
agv.AgvTask.TaskErrMsg = "下一任务路径规划失败";
agv.RCSToAGV = EnumMsg.RCSToAGV.RCS调度避让中;
return pathList ?? [];
}
return listPathPoint;
}
}
return listPathPoint;
case EnumMsg.ActionType.上升到顶:
case EnumMsg.ActionType.下降到底:
case EnumMsg.ActionType.升降到指定高度:
case EnumMsg.ActionType.升降到指定位置:
case EnumMsg.ActionType.充电:
case EnumMsg.ActionType.取消充电:
case EnumMsg.ActionType.等待:
case EnumMsg.ActionType.上层接料:
case EnumMsg.ActionType.上层下料:
case EnumMsg.ActionType.下层接料:
case EnumMsg.ActionType.下层下料:
case EnumMsg.ActionType.上层接料下层接料:
case EnumMsg.ActionType.上层接料下层下料:
case EnumMsg.ActionType.下层接料下层下料:
case EnumMsg.ActionType.下层下料下层下料:
case EnumMsg.ActionType.旋转:
case EnumMsg.ActionType.对中:
case EnumMsg.ActionType.转盘旋转:
listPathPoint.Add(ComnMethod.GetPathPoint(endPoint));
return listPathPoint;
default:
throw new Exception("创建路径时无此任务类型:" + subTask.ActionType.ToString());
}
}
catch (Exception ex)
{
throw new Exception($"GetPathList:{ex.Message}，\n 堆栈：{ex.StackTrace}");
}
}
/// <summary>
/// 是否为指定类型点位的前置点
/// </summary>
/// <param name="point">要检查的点位</param>
/// <param name="pointTypes">点位类型</param>
/// <returns></returns>
private static bool IsPrePoint(Base_Point point, EnumMsg.PointType[] pointTypes)
{
if (point.XPosPoint != null && pointTypes.Contains(point.XPosPoint.PointType))
{
return true;
}
else if (point.XNegPoint != null && pointTypes.Contains(point.XNegPoint.PointType))
{
return true;
}
else if (point.YPosPoint != null && pointTypes.Contains(point.YPosPoint.PointType))
{
return true;
}
else if (point.YNegPoint != null && pointTypes.Contains(point.YNegPoint.PointType))
{
return true;
}
return false;
}
/// <summary>
/// 获取新的终点，因为叉车是倒退出入库，所以要往前延伸一个点
/// </summary>
/// <param name="parentPoint">路径的终点点位</param>
/// <returns></returns>
private static Base_Point? GetNewEndPoint(Base_Point parentPoint)
{
Base_Point? newEndPoint = null;
var pointType = new EnumMsg.PointType[] { EnumMsg.PointType.站台附属点, EnumMsg.PointType.充电点 };
if (parentPoint.ParentPoint == parentPoint.XPosPoint)
{
//如果生成的路径再往前是站台附属点，那么说明路径方向是垂直过来的，那就选择路径终点两旁的点，否则就把终点往前延长，这样才能倒车。
if (parentPoint.IsXNeg && parentPoint.XNegPoint != null && pointType.Contains(parentPoint.XNegPoint.PointType))
{
if (parentPoint.IsYPos && parentPoint.YPosPoint != null && parentPoint.YPosPoint.LockedAgv == null
&& !parentPoint.YPosPoint.TmpDirectionList.Exists(x => x.PathDirection == EnumMsg.Direction.Y负方向))
{
newEndPoint = parentPoint.YPosPoint;
}
else if (parentPoint.IsYNeg && parentPoint.YNegPoint != null && parentPoint.YNegPoint.LockedAgv == null
&& !parentPoint.YNegPoint.TmpDirectionList.Exists(x => x.PathDirection == EnumMsg.Direction.Y正方向))
{
newEndPoint = parentPoint.YNegPoint;
}
}
else if (parentPoint.IsXNeg)
{
newEndPoint = parentPoint.XNegPoint;
}
}
else if (parentPoint.ParentPoint == parentPoint.XNegPoint)
{
//如果生成的路径再往前是站台附属点，那么说明路径方向是垂直过来的，那就选择路径终点两旁的点，否则就把终点往前延长，这样才能倒车。
if (parentPoint.IsXPos && parentPoint.XPosPoint != null && pointType.Contains(parentPoint.XPosPoint.PointType))
{
if (parentPoint.IsYPos && parentPoint.YPosPoint != null && parentPoint.YPosPoint.LockedAgv == null
&& !parentPoint.YPosPoint.TmpDirectionList.Exists(x => x.PathDirection == EnumMsg.Direction.Y负方向))
{
newEndPoint = parentPoint.YPosPoint;
}
else if (parentPoint.IsYNeg && parentPoint.YNegPoint != null && parentPoint.YNegPoint.LockedAgv == null
&& !parentPoint.YNegPoint.TmpDirectionList.Exists(x => x.PathDirection == EnumMsg.Direction.Y正方向))
{
newEndPoint = parentPoint.YNegPoint;
}
}
else if (parentPoint.IsXPos)
{
newEndPoint = parentPoint.XPosPoint;
}
}
else if (parentPoint.ParentPoint == parentPoint.YPosPoint)
{
//如果生成的路径再往前是站台附属点，那么说明路径方向是垂直过来的，那就选择路径终点两旁的点，否则就把终点往前延长。
if (parentPoint.IsYNeg && parentPoint.YNegPoint != null && pointType.Contains(parentPoint.YNegPoint.PointType))
{
if (parentPoint.IsXPos && parentPoint.XPosPoint != null && parentPoint.XPosPoint.LockedAgv == null
&& !parentPoint.XPosPoint.TmpDirectionList.Exists(x => x.PathDirection == EnumMsg.Direction.X负方向))
{
newEndPoint = parentPoint.XPosPoint;
}
else if (parentPoint.IsXNeg && parentPoint.XNegPoint != null && parentPoint.XNegPoint.LockedAgv == null
&& !parentPoint.XNegPoint.TmpDirectionList.Exists(x => x.PathDirection == EnumMsg.Direction.X正方向))
{
newEndPoint = parentPoint.XNegPoint;
}
}
else if (parentPoint.IsYNeg)
{
newEndPoint = parentPoint.YNegPoint;
}
}
else if (parentPoint.ParentPoint == parentPoint.YNegPoint)
{
//如果生成的路径再往前是站台附属点，那么说明路径方向是垂直过来的，那就选择路径终点两旁的点，否则就把终点往前延长。
if (parentPoint.IsYPos && parentPoint.YPosPoint != null && pointType.Contains(parentPoint.YPosPoint.PointType))
{
if (parentPoint.IsXPos && parentPoint.XPosPoint != null && parentPoint.XPosPoint.LockedAgv == null
&& !parentPoint.XPosPoint.TmpDirectionList.Exists(x => x.PathDirection == EnumMsg.Direction.X负方向))
{
newEndPoint = parentPoint.XPosPoint;
}
else if (parentPoint.IsXNeg && parentPoint.XNegPoint != null && parentPoint.XNegPoint.LockedAgv == null
&& !parentPoint.XNegPoint.TmpDirectionList.Exists(x => x.PathDirection == EnumMsg.Direction.X正方向))
{
newEndPoint = parentPoint.XNegPoint;
}
}
else if (parentPoint.IsYPos)
{
newEndPoint = parentPoint.YPosPoint;
}
}
return newEndPoint;
}
/// <summary>
/// 清除点的父类属性
/// </summary>
public static void ClearParentPoint()
{
foreach (var point in App.PointList)
{
if (point.ParentPoint != null)
{
point.ParentPoint = null;
}
}
}
/// <summary>
///获取可用的路径点
/// </summary>
private static List<Base_Point> GetCanUseApoint(Base_Agv agv)
{
var listApoint = new List<Base_Point>();
//if (agv.AgvTask == null)
//{
// return listApoint;
//}
var sonTask = agv.AgvTask?.SubTaskList.First();
var startPoint = sonTask?.StartPoint;
var agvPoint = App.PointList.First(a => a.Barcode == agv.Barcode);
if (sonTask?.SubTaskState == EnumMsg.SubTaskState.重建路径中)
{
startPoint = App.PointList.First(a => a.Barcode == agv.Barcode);
}
var endPoint = sonTask?.EndPoint;
//获取可用点
foreach (var point in App.PointList)
{
if (point != startPoint && point != endPoint)
{//以下点类型不使用
if (point.PointType == EnumMsg.PointType.死点) continue;
//禁用的点位不去
if (!point.IsEnable) continue;
//对站台点进行判断
//if (point.PointType == EnumMsg.PointType.站台点
// && agv.HeightState != EnumMsg.HeightState.低位)
//{
// var endSt = App.StationList.Find(a => a.Barcode == agv.AgvTask.EndBarcode);
// if (endSt == null || point.Barcode != endSt.PreBarcode)
// {
// continue;
// }
//}
//if (point.pointType == EnumMsg.PointType.站台点
// && !App.StationList.Find(a => a.strBarcode == point.strBarcode).strStationNo.Contains("Empty")) continue;
//不包含agv的区域则不选择
if (point.AreaType != agv.CurrMap) continue;
}
listApoint.Add(point);
}
return listApoint;
}
/// <summary>
/// 路径点添加至路径集合
/// </summary>
/// <param name="parent"></param>
/// <param name="type">任务预览/派发路径</param>
/// <returns></returns>
public static List<Base_PathPoint> GetPathPointList(Base_Point? parent, Base_SubTask subTask)
{
var listPathPoint = new List<Base_PathPoint>();
while (parent != null)
{
listPathPoint.Add(ComnMethod.GetPathPoint(parent));
parent = parent.ParentPoint;
}
listPathPoint.Reverse();
//for (int i = 0; i < listPathPoint.Count; i++)
//{
// listPathPoint[i].SerialNo = i + 1;
// listPathPoint[i].AreaType = listPathPoint[i].Point.AreaType;
// listPathPoint[i].Barcode = listPathPoint[i].Point.Barcode;
// if (subTask != null)
// {
// listPathPoint[i].SubTaskNo = subTask.SubTaskNo;
// }
//}
return listPathPoint;
}
public static List<Base_PathPoint> GetPathPointList(Base_Point? parent)
{
List<Base_PathPoint> listPathPoint = new List<Base_PathPoint>();
while (parent != null)
{
listPathPoint.Add(ComnMethod.GetPathPoint(parent));
parent = parent.ParentPoint;
}
listPathPoint.Reverse();
return listPathPoint;
}
/// <summary>
/// 获取路径方向
/// </summary>
/// <param name="pathList">路径集合</param>
public static void GetPathDirection(List<Base_PathPoint> pathList)
{
//终点不比较
for (int i = 0; i < pathList.Count - 1; i++)
{
var point = pathList[i].Point;
var nextPoint = pathList[i + 1].Point;
var pathAStar = new PathAStar();
var pathDirection = pathAStar.GetDircition(nextPoint, point);
pathList[i].PathDirection = pathDirection;
}
}
/// <summary>
/// 获取路径拐点
/// </summary>
/// <param name="pathList">路径点</param>
public static bool GetPathCorner(List<Base_PathPoint> pathList, Base_Point beginPoint)
{
try
{
if (beginPoint == null)
{
throw new Exception("获取路径拐点失败，起点终点不能为空");
}
for (int i = 0; i < pathList.Count - 1; i++)
{
//当前路径点
var currentPoint = pathList[i];
if (currentPoint.Point == beginPoint)
{
//currentPoint.isCorner = true;
continue;
}
//上个路径点
var beforePoint = pathList[i - 1];
if (currentPoint.PathDirection != beforePoint.PathDirection)
{
currentPoint.IsCorner = true;
}
else
{
currentPoint.IsCorner = false;
}
}
return true;
}
catch (Exception ex)
{
throw new Exception($"GetPathCorner:{ex.Message}，\n 堆栈：{ex.StackTrace}");
}
}
/// <summary>
/// 锁定路径方向（最后一个点位，也就是终点不加方向所）
/// </summary>
/// <param name="pathList">从起点到终点的路径集合</param>
public static void OriLock(List<Base_PathPoint> pathList, Base_Agv agv)
{
//终点不比较
// int dir0 = 0;
for (int i = 0; i < pathList.Count - 1; i++)
{
var point = pathList[i].Point;
var nextPoint = pathList[i + 1].Point;
if (point == nextPoint || point.RegionName.Contains("Rotote") || point.RegionName.Contains("Mutex") || point.RegionName.Contains("Area"))
{
continue;
}
//高位发送目标点的防撞属性
//if (agv.HeightState == EnumMsg.HeightState.高位)
//{
// var task = agv.AgvTask;
// int flag = 0;
// //临时根据任务条码判断防撞类型
// if (!string.IsNullOrEmpty(task.TaskPallet) && task.TaskGroup == EnumMsg.TaskGroup.RCS单步任务.ToString() && int.TryParse(task.TaskPallet, out flag))
// {
// pathList[i].PathTim = int.Parse(task.TaskPallet);
// }
// else
// {
// var endStation = App.StationList.Find(a => a.Barcode == agv.AgvTask.EndPoint.Barcode);
// if (endStation != null)
// {
// pathList[i].PathTim = endStation.Tim;
// }
// }
//}
//if (nextPoint.PointType == EnumMsg.PointType.站台点 && point.PointType == EnumMsg.PointType.站台附属点)
//{
// continue;
//}
var pathAStar = new PathAStar();
var pathDirection = pathAStar.GetDircition(nextPoint, point);
pathList[i].PathDirection = pathDirection;
var td = new TmpDirection
{
PathDirection = pathDirection,
AgvNo = agv.AgvNo
};
point.TmpDirectionList.Add(td);
}
}
/// <summary>
/// 清除点的父类属性
/// </summary>
public static void ClearDemoParentPoint()
{
foreach (Base_Point point in App.DemoPointList)
{
if (point.ParentPoint != null)
point.ParentPoint = null;
}
}
/// <summary>
/// 检查 终点 + 车身长度的距离内，是否有其他反向方向锁。如果有反向方向锁，说明有其他车来过来，那么去就会撞，或是卡住。
/// </summary>
/// <param name="lastPoint"></param>
/// <param name="endPoint"></param>
public static bool CheckEndPoint(Base_Point? lastPoint, Base_Point endPoint, Base_Agv agv)
{
if (lastPoint.IntX == endPoint.IntX)
{
//如果最后的行走方向是Y正方向
if (lastPoint.YPosPoint != null && lastPoint.YPosPoint.IntY == endPoint.IntY)
{
//找到最近的有反向方向锁的点位
var directionPoint = App.PointList.Where(x => x.IntY > endPoint.IntY && x.IntX == endPoint.IntX &&
x.TmpDirectionList.Exists(y => y.PathDirection == EnumMsg.Direction.X负方向))
.OrderBy(x => x.IntY)
.FirstOrDefault();
if (directionPoint != null)
{
//如果当前子任务的终点 + 车身长度 > 有反向方向锁的点位（因为终点是没有方向锁的，所以用directionPoint.YNegPoint），就不能生成路径
if (endPoint.XLength + agv.HeadLength > directionPoint.YNegPoint.XLength)
{
agv.StopReason = ComnMethod.GetStopReason(agv.StopReason, "创建行走路径的时候，终点附近有其他路径的方向锁，要等待其他任务完成！");
return false;
}
}
return true;
}
//如果最后的行走方向是Y负方向
if (lastPoint.YNegPoint != null && lastPoint.YNegPoint.IntY == endPoint.IntY)
{
//找到最近的有反向方向锁的点位
var directionPoint = App.PointList.Where(x => x.IntY < endPoint.IntY && x.IntX == endPoint.IntX &&
x.TmpDirectionList.Exists(y => y.PathDirection == EnumMsg.Direction.X正方向))
.OrderByDescending(x => x.IntY)
.FirstOrDefault();
if (directionPoint != null)
{
//如果当前子任务的终点 - 车身长度 < 有反向方向锁的点位（因为终点是没有方向锁的，所以用directionPoint.YNegPoint），就不能生成路径
if (endPoint.XLength - agv.HeadLength < directionPoint.YPosPoint.XLength)
{
agv.StopReason = ComnMethod.GetStopReason(agv.StopReason, "创建行走路径的时候，终点附近有其他路径的方向锁，要等待其他任务完成！");
return false;
}
}
return true;
}
}
if (lastPoint.IntY == endPoint.IntY)
{
//如果最后的行走方向是X正方向
if (lastPoint.XPosPoint != null && lastPoint.XPosPoint.IntX == endPoint.IntX)
{
//找到最近的有反向方向锁的点位
var directionPoint = App.PointList.Where(x => x.IntX > endPoint.IntX && x.IntY == endPoint.IntY &&
x.TmpDirectionList.Exists(y => y.PathDirection == EnumMsg.Direction.X负方向))
.OrderBy(x => x.IntX)
.FirstOrDefault();
if (directionPoint != null)
{
//如果当前子任务的终点 + 车身长度 > 有反向方向锁的点位（因为终点是没有方向锁的，所以用directionPoint.XNegPoint），就不能生成路径
if (endPoint.YLength + agv.HeadLength >= directionPoint.XNegPoint.YLength)
{
agv.StopReason = ComnMethod.GetStopReason(agv.StopReason, "创建行走路径的时候，终点附近有其他路径的方向锁，要等待其他任务完成！");
return false;
}
}
return true;
}
//如果最后的行走方向是X负方向
if (lastPoint.XNegPoint != null && lastPoint.XNegPoint.IntX == endPoint.IntX)
{
//找到最近的有反向方向锁的点位
var directionPoint = App.PointList.Where(x => x.IntX < endPoint.IntX && x.IntY == endPoint.IntY &&
x.TmpDirectionList.Exists(y => y.PathDirection == EnumMsg.Direction.X正方向))
.OrderByDescending(x => x.IntX)
.FirstOrDefault();
if (directionPoint != null)
{
//如果当前子任务的终点 - 车身长度 < 有反向方向锁的点位（因为终点是没有方向锁的，所以用directionPoint.XNegPoint），就不能生成路径
if (endPoint.YLength - agv.HeadLength <= directionPoint.XPosPoint.YLength)
{
agv.StopReason = ComnMethod.GetStopReason(agv.StopReason, "创建行走路径的时候，终点附近有其他路径的方向锁，要等待其他任务完成！");
return false;
}
}
return true;
}
}
return false;
}
/// <summary>
/// 获取两点之前的路径
/// </summary>
/// <param name="agv"></param>
/// <param name="startPoint"></param>
/// <param name="endPoint"></param>
/// <returns></returns>
public static List<Base_PathPoint> GetPathList(Base_Agv agv, Base_Point startPoint, Base_Point endPoint, bool flag = false)
{
List<Base_PathPoint> listPathPoint = new List<Base_PathPoint>();
try
{
//获取能使用的点，并克隆对象，清除父节点
List<Base_Point> listApoint = GetCanUseApoint(agv);
listApoint = CopyPathPointList(listApoint);
startPoint = CopyPathPoint(listApoint, startPoint)!;
endPoint = CopyPathPoint(listApoint, endPoint)!;
if (startPoint.Barcode == endPoint.Barcode)
{
listPathPoint.Add(ComnMethod.GetPathPoint(endPoint));
return listPathPoint;
}
PathAStar pathAStar = new PathAStar();
Base_Point Parent = pathAStar.GetPathPoint(listApoint, startPoint, endPoint, agv, true);
if (Parent == null || (Parent != null && Parent.Barcode != endPoint.Barcode))
{
return listPathPoint;
}
listPathPoint = GetPathPointList(Parent);
}
catch (Exception ex)
{
App.ExFile.MessageError("GetPathList", ex.Message + ex.StackTrace);
}
return listPathPoint;
}
/// <summary>
/// 浅克隆点对象
/// </summary>
/// <param name="oldPoints"></param>
/// <returns></returns>
public static Base_Point? CopyPathPoint(List<Base_Point> newPoints, Base_Point oldPoint)
{
Base_Point? newPoint = null;
try
{
newPoint = oldPoint.Clone() as Base_Point;
newPoint!.ParentPoint = null;
//替换引用对象
newPoint.XPosPoint = newPoints.Find(y => newPoint.XPosPoint != null && y.Barcode == newPoint.XPosPoint.Barcode);
newPoint.YPosPoint = newPoints.Find(y => newPoint.YPosPoint != null && y.Barcode == newPoint.YPosPoint.Barcode);
newPoint.XNegPoint = newPoints.Find(y => newPoint.XNegPoint != null && y.Barcode == newPoint.XNegPoint.Barcode);
newPoint.YNegPoint = newPoints.Find(y => newPoint.YNegPoint != null && y.Barcode == newPoint.YNegPoint.Barcode);
return newPoint;
}
catch (Exception ex)
{
App.ExFile.MessageError("CopyPathPoint", "路径点对象复制失败:" + ex.Message + ex.StackTrace);
}
return newPoint;
}
public static List<Base_Point> CopyPathPointList(List<Base_Point> oldPoints)
{
List<Base_Point> newPoints = new List<Base_Point>();
Base_Point? newPoint;
try
{
foreach (Base_Point oldPoint in oldPoints)
{
newPoint = oldPoint.Clone() as Base_Point;
newPoint!.ParentPoint = null;
newPoints.Add(newPoint);
}
//替换引用对象
newPoints.ForEach(x =>
{
x.XPosPoint = newPoints.Find(y => x.XPosPoint != null && y.Barcode == x.XPosPoint.Barcode);
x.YPosPoint = newPoints.Find(y => x.YPosPoint != null && y.Barcode == x.YPosPoint.Barcode);
x.XNegPoint = newPoints.Find(y => x.XNegPoint != null && y.Barcode == x.XNegPoint.Barcode);
x.YNegPoint = newPoints.Find(y => x.YNegPoint != null && y.Barcode == x.YNegPoint.Barcode);
});
}
catch (Exception ex)
{
App.ExFile.MessageError("CopyPathPointList", "路径点对象复制失败:" + ex.Message + ex.StackTrace);
}
return newPoints;
}
}
}