diff --git a/scripts/ai/AIDriveStrategyUnloadCombine.lua b/scripts/ai/AIDriveStrategyUnloadCombine.lua index 02f5c3f3c..afdb93dfd 100644 --- a/scripts/ai/AIDriveStrategyUnloadCombine.lua +++ b/scripts/ai/AIDriveStrategyUnloadCombine.lua @@ -1508,7 +1508,9 @@ function AIDriveStrategyUnloadCombine:onPathfindingFailedToMovingTarget(...) end, ...) end -function AIDriveStrategyUnloadCombine:onPathfindingObstacleAtStart(controller, lastContext, maxDistance, trailerCollisionsOnly) +function AIDriveStrategyUnloadCombine:onPathfindingObstacleAtStart(controller, lastContext, maxDistance, + trailerCollisionsOnly, fruitPenaltyNodePercent, + offFieldPenaltyNodePercent) if trailerCollisionsOnly then self:debug('Pathfinding detected obstacle at start, trailer collisions only, retry with ignoring the trailer') lastContext:ignoreTrailerAtStartRange(1.5 * self.turningRadius) @@ -1520,12 +1522,18 @@ function AIDriveStrategyUnloadCombine:onPathfindingObstacleAtStart(controller, l end function AIDriveStrategyUnloadCombine:onPathfindingFailed(giveUpFunc, controller, lastContext, wasLastRetry, - currentRetryAttempt) + currentRetryAttempt, trailerCollisionsOnly, + fruitPenaltyNodePercent, offFieldPenaltyNodePercent) if wasLastRetry then giveUpFunc() elseif currentRetryAttempt == 1 then - self:debug('First attempt to find path failed, trying with reduced off-field penalty') - lastContext:offFieldPenalty(PathfinderContext.defaultOffFieldPenalty / 2) + if fruitPenaltyNodePercent > offFieldPenaltyNodePercent then + self:debug('First attempt to find path failed, trying with reduced fruit percent') + lastContext:maxFruitPercent(self:getMaxFruitPercent() / 2) + else + self:debug('First attempt to find path failed, trying with reduced off-field penalty') + lastContext:offFieldPenalty(PathfinderContext.defaultOffFieldPenalty / 2) + end controller:retry(lastContext) elseif currentRetryAttempt == 2 then self:debug('Second attempt to find path failed, trying with reduced off-field penalty and fruit percent') @@ -2459,18 +2467,28 @@ function AIDriveStrategyUnloadCombine:startSelfUnload(ignoreFruit) self.pathfinderController:registerListeners(self, self.onPathfindingDoneBeforeSelfUnload, self.onPathfindingFailedBeforeSelfUnload, self.onPathfindingObstacleAtStart) - self.pathfinderController:findPathToNode(context, self.selfUnloadTargetNode, offsetX, -alignLength, 1) + self.pathfinderController:findPathToNode(context, self.selfUnloadTargetNode, offsetX, -alignLength, 2) else self:debug('Pathfinder already active') end return true end -function AIDriveStrategyUnloadCombine:onPathfindingFailedBeforeSelfUnload(controller, lastContext, - wasLastRetry, currentRetryAttempt) +function AIDriveStrategyUnloadCombine:onPathfindingFailedBeforeSelfUnload(controller, lastContext, wasLastRetry, + currentRetryAttempt, trailerCollisionsOnly, + fruitPenaltyNodePercent, offFieldPenaltyNodePercent) if currentRetryAttempt == 1 then - self:debug('Pathfinding to self unload failed once, retry with fruit avoidance disabled') - lastContext:maxFruitPercent(math.huge) + if fruitPenaltyNodePercent > offFieldPenaltyNodePercent then + self:debug('Pathfinding to self unload failed once, retry with fruit avoidance disabled') + lastContext:ignoreFruit() + else + self:debug('Pathfinding to self unload failed once, retry with off field penalty disabled') + lastContext:offFieldPenalty(0) + end + controller:retry(lastContext) + else + self:debug('Pathfinding to self unload failed again, retry with all penalties disabled') + lastContext:offFieldPenalty(0):ignoreFruit() controller:retry(lastContext) end end diff --git a/scripts/ai/PathfinderController.lua b/scripts/ai/PathfinderController.lua index 7f290d753..41755699b 100644 --- a/scripts/ai/PathfinderController.lua +++ b/scripts/ai/PathfinderController.lua @@ -62,9 +62,10 @@ function Strategy:onPathfindingFinished(controller : PathfinderController, succe end function Strategy:onPathfindingFailed(controller : PathfinderController, currentContext : PathfinderContext, - wasLastRetry : boolean, currentRetryAttempt : number) + wasLastRetry : boolean, currentRetryAttempt : number, trailerCollisionsOnly : boolean, + fruitPenaltyNodePercent : number, offFieldPenaltyNodePercent : number) if currentRetryAttempt == 1 then - // Reduced fruit impact: + // try whatever has better chances: currentContext:ignoreFruit() self.pathfinderController:findPathToNode(currentContext, ...) else @@ -72,6 +73,20 @@ function Strategy:onPathfindingFailed(controller : PathfinderController, current self.pathfinderController:findPathToNode(currentContext, ...) end end + + +function onPathfindingObstacleAtStart(controller, lastContext, maxDistance, + trailerCollisionsOnly, fruitPenaltyNodePercent, offFieldPenaltyNodePercent) + if trailerCollisionsOnly then + self:debug('Pathfinding detected obstacle at start, trailer collisions only, retry with ignoring the trailer') + lastContext:ignoreTrailerAtStartRange(1.5 * self.turningRadius) + controller:retry(lastContext) + else + self:debug('Pathfinding detected obstacle at start, back up and retry') + self:startMovingBackBeforePathfinding(controller, lastContext) + end +end + ]] ---@class DefaultFieldPathfinderControllerContext : PathfinderContext @@ -141,12 +156,18 @@ end --- TODO: Decide if multiple registered listeners are needed or not? ---@param object table ---@param successFunc function func(PathfinderController, success, Course, goalNodeInvalid) ----@param failedFunc function func(PathfinderController, last context, was last retry, retry attempt number) ----@param obstacleAtStartFunc function|nil func(PathfinderController, last context, maxDistance, trailerCollisionsOnly), +---@param failedFunc function +---func(PathfinderController, last context, was last retry, retry attempt number, trailerCollisionsOnly, fruitPenaltyNodePercent, offFieldPenaltyNodePercent) +---@param obstacleAtStartFunc function|nil +--- func(PathfinderController, last context, maxDistance, trailerCollisionsOnly, fruitPenaltyNodePercent, offFieldPenaltyNodePercent), --- called when the pathfinding failed within maxDistance (there is an obstacle ahead of the vehicle) so it can't even --- start driving anywhere forward. In this case pathfinding makes no sense. No check if no callback is registered. +--- --- trailerCollisionsOnly will be set to true if there were no other collisions other then between the trailer and --- some other obstacle. +--- +--- fruitPenaltyNodePercent and offFieldPenaltyNodePercent can be used to determine the most likely reason the pathfinding +--- failed, returning the percent of nodes with fruit/off-field penalty (of total nodes searched) function PathfinderController:registerListeners(object, successFunc, failedFunc, obstacleAtStartFunc) self.callbackObject = object self.callbackSuccessFunction = successFunc @@ -186,10 +207,12 @@ function PathfinderController:handleFailedPathfinding(result) --- Retry is allowed, so check if any tries are leftover if self.failCount < self.numRetries then self:debug("Failed with try %d of %d.", self.failCount, self.numRetries) - --- Retrying the path finding + local wasLastRetry = self.failCount == self.numRetries self.failCount = self.failCount + 1 + --- Let the callback decide what next: retry or give up self:callCallback(self.callbackFailedFunction, - self.currentContext, self.failCount == self.numRetries, self.failCount, false) + self.currentContext, wasLastRetry, self.failCount, false, + result.fruitPenaltyNodePercent, result.offFieldPenaltyNodePercent) return elseif self.numRetries > 0 then self:debug("Max number of retries already reached!") diff --git a/scripts/pathfinder/HybridAStar.lua b/scripts/pathfinder/HybridAStar.lua index f92d41438..18cf8da9a 100644 --- a/scripts/pathfinder/HybridAStar.lua +++ b/scripts/pathfinder/HybridAStar.lua @@ -99,18 +99,21 @@ PathfinderResult = CpObject() ---@param goalNodeInvalid boolean if true, the goal node is invalid (for instance a vehicle or obstacle is there) so --- the pathfinding can never succeed. ---@param maxDistance number the furthest distance the pathfinding tried from the start, only when no path found ----@param trailerCollisionsOnly boolean only collisions with the trailer was detected. -function PathfinderResult:init(done, path, goalNodeInvalid, maxDistance, trailerCollisionsOnly) +---@param constraints PathfinderConstraints detailed statistics. +function PathfinderResult:init(done, path, goalNodeInvalid, maxDistance, constraints) self.done = done self.path = path self.goalNodeInvalid = goalNodeInvalid or false self.maxDistance = maxDistance or 0 - self.trailerCollisionsOnly = trailerCollisionsOnly or false + self.trailerCollisionsOnly = constraints and constraints:trailerCollisionsOnly() or false + self.fruitPenaltyNodePercent = constraints and constraints:getFruitPenaltyNodePercent() or 0 + self.offFieldPenaltyNodePercent = constraints and constraints:getOffFieldPenaltyNodePercent() or 0 end function PathfinderResult:__tostring() - return string.format('[done: %s, path waypoints: %d, goalNodeInvalid: %s, maxDistance: %.1f, trailerCollisionOnly: %s]', - self.done, self.path and #self.path or - 1, self.goalNodeInvalid, self.maxDistance, self.trailerCollisionsOnly) + return string.format('[done: %s, path waypoints: %d, goalNodeInvalid: %s, maxDistance: %.1f, trailerCollisionOnly: %s, fruit/off-field penalty %.1f/%.1f]', + self.done, self.path and #self.path or - 1, self.goalNodeInvalid, self.maxDistance, + self.trailerCollisionsOnly, self.fruitPenaltyNodePercent, self.offFieldPenaltyNodePercent) end --- Interface definition for pathfinder constraints (for dependency injection of node penalty/validity checks @@ -779,8 +782,8 @@ function HybridAStarWithAStarInTheMiddle:resume(...) print(done) printCallstack() self.coroutine = nil - return PathfinderResult(true, nil, goalNodeInvalid, - self.currentPathfinder.nodes.highestDistance, self.constraints:trailerCollisionsOnly()) + return PathfinderResult(true, nil, goalNodeInvalid, self.currentPathfinder.nodes.highestDistance, + self.constraints) end if done then self.coroutine = nil @@ -794,13 +797,14 @@ function HybridAStarWithAStarInTheMiddle:resume(...) return PathfinderResult(true, result) else self:debug('all hybrid: no path found') - return PathfinderResult(true, nil, goalNodeInvalid, - self.currentPathfinder.nodes.highestDistance, self.constraints:trailerCollisionsOnly()) + return PathfinderResult(true, nil, goalNodeInvalid, self.currentPathfinder.nodes.highestDistance, + self.constraints) end elseif self.phase == self.MIDDLE then self.constraints:resetStrictMode() if not path then - return PathfinderResult(true, nil, goalNodeInvalid) + return PathfinderResult(true, nil, goalNodeInvalid, self.currentPathfinder.nodes.highestDistance, + self.constraints) end local lMiddlePath = HybridAStar.length(path) self:debug('Direct path is %d m', lMiddlePath) @@ -813,7 +817,8 @@ function HybridAStarWithAStarInTheMiddle:resume(...) HybridAStar.shortenStart(self.middlePath, self.hybridRange) HybridAStar.shortenEnd(self.middlePath, self.hybridRange) if #self.middlePath < 2 then - return PathfinderResult(true, nil) + return PathfinderResult(true, nil, goalNodeInvalid, self.currentPathfinder.nodes.highestDistance, + self.constraints) end State3D.smooth(self.middlePath) State3D.setAllHeadings(self.middlePath) @@ -836,8 +841,8 @@ function HybridAStarWithAStarInTheMiddle:resume(...) return self:findPathFromMiddleToEnd() else self:debug('start to middle: no path found') - return PathfinderResult(true, nil, goalNodeInvalid, - self.currentPathfinder.nodes.highestDistance, self.constraints:trailerCollisionsOnly()) + return PathfinderResult(true, nil, goalNodeInvalid, self.currentPathfinder.nodes.highestDistance, + self.constraints) end elseif self.phase == self.MIDDLE_TO_END then if path then @@ -850,13 +855,13 @@ function HybridAStarWithAStarInTheMiddle:resume(...) table.insert(self.path, path[i]) end State3D.smooth(self.path) + self.constraints:showStatistics() + return PathfinderResult(true, self.path) else self:debug('middle to end: no path found') - return PathfinderResult(true, nil, goalNodeInvalid, - self.currentPathfinder.nodes.highestDistance, self.constraints:trailerCollisionsOnly()) + return PathfinderResult(true, nil, goalNodeInvalid, self.currentPathfinder.nodes.highestDistance, + self.constraints) end - self.constraints:showStatistics() - return PathfinderResult(true, self.path) end end return PathfinderResult(false) diff --git a/scripts/pathfinder/PathfinderConstraints.lua b/scripts/pathfinder/PathfinderConstraints.lua index 669ed12e5..40fa529e6 100644 --- a/scripts/pathfinder/PathfinderConstraints.lua +++ b/scripts/pathfinder/PathfinderConstraints.lua @@ -216,6 +216,14 @@ function PathfinderConstraints:trailerCollisionsOnly() return self.trailerCollisionNodeCount > 0 and self.collisionNodeCount == self.trailerCollisionNodeCount end +function PathfinderConstraints:getFruitPenaltyNodePercent() + return self.totalNodeCount > 0 and (self.fruitPenaltyNodeCount / self.totalNodeCount) or 0 +end + +function PathfinderConstraints:getOffFieldPenaltyNodePercent() + return self.totalNodeCount > 0 and (self.offFieldPenaltyNodeCount / self.totalNodeCount) or 0 +end + function PathfinderConstraints:debug(...) self.vehicleData:debug(...) end