fix: pathfinder return value on failure

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

scripts/ai/PathfinderController.lua

@@ -62,16 +62,31 @@ 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 
 		// Something else ...
 		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!")

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)

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