Skip to content

Commit bc5949e

Browse files
jinweihuang20jinweihuang20
committed
修正避車導航流程移動到一半會變成移動至終點的問題/ 修正避讓點搜尋過程中發生錯誤的問題，並考慮避讓點與高優先度車輛同區域且會達車輛數上限的問題
1 parent 311e3aa commit bc5949e

File tree

4 files changed

+59
-13
lines changed

4 files changed

+59
-13
lines changed

‎AGV/TaskDispatch/Tasks/MoveTaskDynamicPathPlanV3.cs‎

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -144,7 +144,7 @@ protected override async Task StartNavigation(MapPoint _finalMapPoint)
144144
bool _isWaitingForNotMovableVehicle = Agv.NavigationState.currentConflicToAGV?.online_state == clsEnums.ONLINE_STATE.OFFLINE ||
145145
Agv.NavigationState.currentConflicToAGV?.main_state == clsEnums.MAIN_STATUS.DOWN;
146146

147-
bool noPathTooLong = !_isWaitingForLoadUnloadingVehicle && noPathGetTimer.Elapsed.TotalSeconds > 60;
147+
bool noPathTooLong = !_isWaitingForLoadUnloadingVehicle && noPathGetTimer.Elapsed.TotalSeconds > 10;
148148

149149
PathPlaner planner = new PathPlaner(new PathPlaner.PlanerOptions
150150
{
@@ -509,7 +509,6 @@ protected override async Task StartNavigation(MapPoint _finalMapPoint)
509509
}
510510
else
511511
{
512-
subStage = Stage;
513512
CurrentNavigationState = NAVIGATION_STATE.DispatchTaskToVehicleProcess;
514513
}
515514
}

‎Dispatch/YieldActions/clsLowPriorityVehicleMove.cs‎

Lines changed: 53 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@
66
using System.Diagnostics;
77
using VMSystem.AGV;
88
using VMSystem.AGV.TaskDispatch.Tasks;
9+
using VMSystem.Dispatch.Regions;
910
using VMSystem.Extensions;
1011
using VMSystem.Services.Navigation;
1112
using VMSystem.TrafficControl;
@@ -76,10 +77,17 @@ internal async Task<MapPoint> DetermineStopMapPoint()
7677
speficMap = StaMap.Map.Clone()
7778
});
7879
int destineOfHighPVehicle = _HightPriorityVehicle.CurrentRunningTask().DestineTag;
79-
(var shorestPathInfoOfHighPVehicle, var secondaryPathInfoOfHighPVehicle) = await planer.GetPathPlanResult(_HightPriorityVehicle, _HightPriorityVehicle.currentMapPoint.TagNumber, destineOfHighPVehicle);
80+
(var shorestPathInfoOfHighPVehicle, var secondaryPathInfoOfHighPVehicle) = await planer.GetPathPlanResult(_HightPriorityVehicle, _HightPriorityVehicle.currentMapPoint.TagNumber
81+
, destineOfHighPVehicle, isGetShortPathWithoutConsiderConstrain: true);
8082

8183

82-
MapCircleArea lastRotationAreaOfHpv = shorestPathInfoOfHighPVehicle.path.Last().GetCircleArea(ref highProrityVehicle);
84+
MapCircleArea lastRotationAreaOfHpv = null;
85+
if (shorestPathInfoOfHighPVehicle != null && shorestPathInfoOfHighPVehicle.path.Last() != null)
86+
lastRotationAreaOfHpv = shorestPathInfoOfHighPVehicle.path.Last().GetCircleArea(ref highProrityVehicle);
87+
else if (highProrityVehicle.main_state != clsEnums.MAIN_STATUS.RUN)
88+
{
89+
lastRotationAreaOfHpv = highProrityVehicle.currentMapPoint.GetCircleArea(ref highProrityVehicle);
90+
}
8391

8492
if (shorestPathInfoOfHighPVehicle != null)
8593
cannotAvoidPartToPoints.AddRange(shorestPathInfoOfHighPVehicle.path);
@@ -99,7 +107,9 @@ internal async Task<MapPoint> DetermineStopMapPoint()
99107

100108
cannotAvoidPartToPoints = cannotAvoidPartToPoints.Union(nearPointsOfHPVCurrentPoint).ToList();
101109
cannotAvoidPartToPoints = cannotAvoidPartToPoints.Union(nearPointsExtend).ToList();
102-
}
110+
}
111+
112+
103113
List<int> tagListOfCannotAvoidPortTo = cannotAvoidPartToPoints.GetTagCollection().ToList();
104114

105115
double[] distanceThres = TrafficControlCenter.TrafficControlParameters.DeadLockUse.searchRadiiForAvoidPoint;
@@ -146,8 +156,8 @@ bool _isStopRotationNOTConflicToHighPAGV(MapPoint pt)
146156
.Select(p => p.path)
147157
.Where(path => path != null);
148158
var _pathesCandidates = wrappersOrdered.Where(path => path != null).ToList()
149-
.Where(path => path.Last().TagNumber != toAvoidVehicle.currentMapPoint.TagNumber && path.Count > 0)
150-
//.Where(path =>predicPathRegionOfHPV.All(reg => !reg.IsIntersectionTo(path.Last().GetCircleArea(ref toAvoidVehicle))))
159+
.Where(path => path.Last().TagNumber != toAvoidVehicle.currentMapPoint.TagNumber && path.Count > 0)
160+
//.Where(path =>predicPathRegionOfHPV.All(reg => !reg.IsIntersectionTo(path.Last().GetCircleArea(ref toAvoidVehicle))))
151161
.ToList();
152162
sw.Stop();
153163
logger?.Info($"計算避讓點花費 Step2:{sw.Elapsed.TotalSeconds}秒");
@@ -166,7 +176,44 @@ bool _isStopRotationNOTConflicToHighPAGV(MapPoint pt)
166176

167177
if (!pathesCandidates.Any())
168178
return null;
169-
return pathesCandidates.First().Last();
179+
180+
var result = pathesCandidates.First().LastOrDefault();
181+
182+
if (result != null)
183+
{
184+
//如果 High Level 車輛的終點位於一個區域且該區域在加上lowlevel 車輛進入後將不可進入，則lowlevel車應該不可以到該區域避車
185+
var regionOfDestineForHPV = destineOfHighPVehicle.GetMapPoint().GetRegion();
186+
var regionOfAvoidPtForLPV = result.GetRegion();
187+
188+
if (regionOfDestineForHPV != null && regionOfAvoidPtForLPV != null && regionOfAvoidPtForLPV.Name == regionOfDestineForHPV.Name && regionOfDestineForHPV.InRegionVehicles.Count() + 1 >= regionOfDestineForHPV.MaxVehicleCapacity)
189+
{
190+
int oriMaxVehicleCapacity = regionOfDestineForHPV.MaxVehicleCapacity;
191+
192+
logger.Info($"因避車點與高優先度車輛的終點為相同區域({regionOfDestineForHPV.Name})，且該區域容量將在低優先度車輛進入後達上限，暫時將該區域可容納車輛數暫時 +1");
193+
194+
regionOfDestineForHPV.MaxVehicleCapacity += 1;
195+
196+
Task.Run(async () =>
197+
{
198+
while (_HightPriorityVehicle.currentMapPoint.GetRegion().Name != regionOfDestineForHPV.Name)
199+
{
200+
bool isAgvDown = _HightPriorityVehicle.main_state == clsEnums.MAIN_STATUS.DOWN;
201+
bool isAgvOffline = _HightPriorityVehicle.online_state == clsEnums.ONLINE_STATE.OFFLINE;
202+
bool isTaskCanceled = _HightPriorityVehicle.taskDispatchModule.OrderExecuteState != clsAGVTaskDisaptchModule.AGV_ORDERABLE_STATUS.EXECUTING;
203+
if (isAgvDown || isAgvOffline || isTaskCanceled)
204+
break;
205+
206+
logger.Info($"等待 {_HightPriorityVehicle.Name} 進入 {regionOfDestineForHPV.Name} 後恢復該區域可容納車輛數量({oriMaxVehicleCapacity})");
207+
await Task.Delay(1000);
208+
}
209+
regionOfDestineForHPV.MaxVehicleCapacity = oriMaxVehicleCapacity;
210+
logger.Info($"{regionOfDestineForHPV.Name} 可容納車輛數已恢復為 {oriMaxVehicleCapacity}");
211+
});
212+
}
213+
}
214+
215+
return result;
216+
170217
}
171218

172219
}

‎Services/Navigation/PathPlaner.cs‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -33,7 +33,7 @@ public PathPlaner(PlanerOptions options = null)
3333
mapUse = this.options.speficMap ?? StaMap.Map;
3434
}
3535

36-
public async Task<(PathPlanResultWrapper? shortestPathWrapper, PathPlanResultWrapper? secondaryPathWrapper)> GetPathPlanResult(IAGV owner, int source, int destine)
36+
public async Task<(PathPlanResultWrapper? shortestPathWrapper, PathPlanResultWrapper? secondaryPathWrapper)> GetPathPlanResult(IAGV owner, int source, int destine, bool isGetShortPathWithoutConsiderConstrain = false)
3737
{
3838
if (destine <= 0)
3939
{
@@ -48,7 +48,7 @@ public PathPlaner(PlanerOptions options = null)
4848
Algorithm = algorithm,
4949
OnlyNormalPoint = true,
5050
Strategy = PathFinder.PathFinderOption.STRATEGY.MINIMAL_ROTATION_ANGLE,
51-
ConstrainTags = _GetConstrainOfShorestPath(owner),
51+
ConstrainTags = isGetShortPathWithoutConsiderConstrain ? new() : _GetConstrainOfShorestPath(owner),
5252
MapUse = mapUse
5353
};
5454

@@ -206,7 +206,7 @@ private PathPlanResultWrapper GetPathResultWrapper(clsPathInfo pathInfo, IAGV ow
206206
}
207207

208208
// 如果衝突的車輛正在等待退出工作站，旋轉若不會與其當前站點的body region干涉，也是做不干涉
209-
if (isConflicPtIsAGVCurrentTag && _agv.main_state != clsEnums.MAIN_STATUS.RUN && !isRotationConclifTo(owner, _agv))
209+
if (isConflicPtIsAGVCurrentTag && _agv.main_state != clsEnums.MAIN_STATUS.RUN && !isRotationConclifTo(owner, _agv))
210210
{
211211
continue;
212212
}

‎VMSystem.csproj‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -9,8 +9,8 @@
99
<ApplicationIcon>favicon2.ico</ApplicationIcon>
1010
<UserSecretsId>5aaaca2e-7b5d-4629-ac34-12d155a519a9</UserSecretsId>
1111
<GenerateDocumentationFile>true</GenerateDocumentationFile>
12-
<AssemblyVersion>2.8.16</AssemblyVersion>
13-
<FileVersion>2.8.16</FileVersion>
12+
<AssemblyVersion>2.8.17</AssemblyVersion>
13+
<FileVersion>2.8.17</FileVersion>
1414
</PropertyGroup>
1515
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|AnyCPU'">
1616
<ShouldCreateLogs>True</ShouldCreateLogs>

0 commit comments

Comments
 (0)