From d35c57a1f1773485e4c503efaee61fca55cb26b6 Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Tue, 12 Mar 2024 09:08:42 -0400 Subject: [PATCH 1/8] fix: bale finder retry logic Retry with another bale if the pathfinding fails with invalid goal, meaning there is likely another bale near the target. --- scripts/ai/AIDriveStrategyFindBales.lua | 31 +++++++++++++++++-------- 1 file changed, 21 insertions(+), 10 deletions(-) diff --git a/scripts/ai/AIDriveStrategyFindBales.lua b/scripts/ai/AIDriveStrategyFindBales.lua index 4b4409845..e1b6f0fe5 100644 --- a/scripts/ai/AIDriveStrategyFindBales.lua +++ b/scripts/ai/AIDriveStrategyFindBales.lua @@ -230,17 +230,18 @@ function AIDriveStrategyFindBales:findBales() end ---@param bales table[] +---@param baleToIgnore BaleToCollect|nil exclude this bale from the results ---@return BaleToCollect|nil closest bale ---@return number|nil distance to the closest bale ---@return number|nil index of the bale -function AIDriveStrategyFindBales:findClosestBale(bales) +function AIDriveStrategyFindBales:findClosestBale(bales, baleToIgnore) if not bales then return end local closestBale, minDistance, ix = nil, math.huge, 1 local invalidBales = 0 for i, bale in ipairs(bales) do - if bale:isStillValid() and bale ~= self.lastPathfinderBaleTarget then + if bale:isStillValid() and bale ~= baleToIgnore then local _, _, _, d = bale:getPositionInfoFromNode(self.vehicle:getAIDirectionNode()) self:debug('%d. bale (%d, %s) in %.1f m', i, bale:getId(), bale:getBaleObject(), d) if d < self.turningRadius * 4 then @@ -315,6 +316,11 @@ function AIDriveStrategyFindBales:onPathfindingFinished(controller, if self.state == self.states.DRIVING_TO_NEXT_BALE then if success then self:startCourse(course, 1) + elseif goalNodeInvalid then + -- there may be another bale too close to the previous one + self:debug('Pathfinding failed, goal node invalid.') + self:retryPathfindingWithAnotherBale() + return else self:info('Pathfinding failed, giving up!') self.vehicle:stopCurrentAIJob(AIMessageCpErrorNoPathFound.new()) @@ -322,6 +328,18 @@ function AIDriveStrategyFindBales:onPathfindingFinished(controller, end end +--- After pathfinding failed, retry with another bale +function AIDriveStrategyFindBales:retryPathfindingWithAnotherBale() + self:debug("Retrying with another bale.") + local bale, d, ix = self:findClosestBale(self.bales, self.lastPathfinderBaleTarget) + if bale then + self:startPathfindingToBale(bale) + else + self:debug("No valid bale found on retry!") + self.vehicle:stopCurrentAIJob(AIMessageCpErrorNoPathFound.new()) + end +end + --- Pathfinding failed, but a retry attempt is leftover. ---@param controller PathfinderController ---@param lastContext PathfinderContext @@ -337,14 +355,7 @@ function AIDriveStrategyFindBales:onPathfindingRetry(controller, self:debug("Retrying the same bale again.") self:startPathfindingToBale(self.lastPathfinderBaleTarget) else - self:debug("Retrying with another bale.") - local bale, d, ix = self:findClosestBale(self.bales) - if bale then - self:startPathfindingToBale(bale) - else - self:debug("No valid bale found on retry: %d!", currentRetryAttempt) - self.vehicle:stopCurrentAIJob(AIMessageCpErrorNoPathFound.new()) - end + self:retryPathfindingWithAnotherBale() end else if self:isNearFieldEdge() then From 8d7de9b6e9b77f7ec5246d99bca923d7b9757d89 Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Tue, 12 Mar 2024 16:09:37 -0400 Subject: [PATCH 2/8] fix: bale finder retry logic - did not store the last checked bale properly - obstacle check was not working because missing return #3012 --- scripts/ai/AIDriveStrategyFindBales.lua | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/scripts/ai/AIDriveStrategyFindBales.lua b/scripts/ai/AIDriveStrategyFindBales.lua index e1b6f0fe5..466e07651 100644 --- a/scripts/ai/AIDriveStrategyFindBales.lua +++ b/scripts/ai/AIDriveStrategyFindBales.lua @@ -393,8 +393,8 @@ function AIDriveStrategyFindBales:startPathfindingToBale(bale) self.state = self.states.DRIVING_TO_NEXT_BALE g_baleToCollectManager:lockBale(bale:getBaleObject(), self) local context = PathfinderContext(self.vehicle):objectsToIgnore(self:getBalesToIgnore()):allowReverse(false) - self.pathfinderController:findPathToGoal(context, self:getPathfinderBaleTargetAsGoalNode(bale), 3) self.lastPathfinderBaleTarget = bale + self.pathfinderController:findPathToGoal(context, self:getPathfinderBaleTargetAsGoalNode(bale), 3) end function AIDriveStrategyFindBales:startReversing() @@ -407,7 +407,7 @@ function AIDriveStrategyFindBales:isObstacleAhead() -- then a more thorough check, we want to ignore the last bale we worked on as that may lay around too close -- to the baler. This happens for example to the Andersen bale wrapper. self:debug('Check obstacles ahead, ignoring %d bale object, first is %s', #objectsToIgnore, objectsToIgnore[1] or 'nil') - AIDriveStrategyCourse.isObstacleAhead(self, objectsToIgnore) + return AIDriveStrategyCourse.isObstacleAhead(self, objectsToIgnore) end function AIDriveStrategyFindBales:isNearFieldEdge() From fd841e264cf3cf70af9f11555eb578ebcace2da7 Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Fri, 15 Mar 2024 19:45:01 -0400 Subject: [PATCH 3/8] fix: find closest bale logic --- scripts/ai/AIDriveStrategyFindBales.lua | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/scripts/ai/AIDriveStrategyFindBales.lua b/scripts/ai/AIDriveStrategyFindBales.lua index 466e07651..ec452d34b 100644 --- a/scripts/ai/AIDriveStrategyFindBales.lua +++ b/scripts/ai/AIDriveStrategyFindBales.lua @@ -241,7 +241,7 @@ function AIDriveStrategyFindBales:findClosestBale(bales, baleToIgnore) local closestBale, minDistance, ix = nil, math.huge, 1 local invalidBales = 0 for i, bale in ipairs(bales) do - if bale:isStillValid() and bale ~= baleToIgnore then + if bale:isStillValid() then local _, _, _, d = bale:getPositionInfoFromNode(self.vehicle:getAIDirectionNode()) self:debug('%d. bale (%d, %s) in %.1f m', i, bale:getId(), bale:getBaleObject(), d) if d < self.turningRadius * 4 then @@ -251,9 +251,13 @@ function AIDriveStrategyFindBales:findClosestBale(bales, baleToIgnore) self:debug(' Dubins length is %.1f m', d) end if d < minDistance then - closestBale = bale - minDistance = d - ix = i + if bale ~= baleToIgnore then + closestBale = bale + minDistance = d + ix = i + else + self:debug(' IGNORED') + end end else --- When a bale gets wrapped it changes its identity and the node becomes invalid. This can happen From 13ce3a92b4a93b018a9313fa7a208b451d4cf808 Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Sat, 16 Mar 2024 08:44:34 -0400 Subject: [PATCH 4/8] fix/refactor: bale finder pathfinder failure logic Don't use the pathfinder controller retry logic as we don't want to retry with the same goal. Give up after 3 retries. Try another bale if the goal node is invalid, or failed, but not close to the field edge. If close to the field edge, back up a bit and start again. Keep a list of bales tried to avoid retrying the same two bales and causing a stack overflow. --- scripts/ai/AIDriveStrategyBunkerSilo.lua | 2 +- scripts/ai/AIDriveStrategyCourse.lua | 4 +- scripts/ai/AIDriveStrategyFindBales.lua | 89 +++++++------------ .../ai/AIDriveStrategyShovelSiloLoader.lua | 2 +- scripts/ai/AIDriveStrategySiloLoader.lua | 2 +- scripts/ai/BaleToCollect.lua | 6 +- scripts/ai/PathfinderController.lua | 14 +-- 7 files changed, 51 insertions(+), 68 deletions(-) diff --git a/scripts/ai/AIDriveStrategyBunkerSilo.lua b/scripts/ai/AIDriveStrategyBunkerSilo.lua index da6a4bf28..91bc572e7 100644 --- a/scripts/ai/AIDriveStrategyBunkerSilo.lua +++ b/scripts/ai/AIDriveStrategyBunkerSilo.lua @@ -573,7 +573,7 @@ end ---@param lastContext PathfinderContext ---@param wasLastRetry boolean ---@param currentRetryAttempt number -function AIDriveStrategyBunkerSilo:onPathfindingRetry(controller, +function AIDriveStrategyBunkerSilo:onPathfindingFailed(controller, lastContext, wasLastRetry, currentRetryAttempt) --- TODO: Think of possible points of failures, that could be adjusted here. --- Maybe a small reverse course might help to avoid a deadlock diff --git a/scripts/ai/AIDriveStrategyCourse.lua b/scripts/ai/AIDriveStrategyCourse.lua index efb076615..ee2ff0fbf 100644 --- a/scripts/ai/AIDriveStrategyCourse.lua +++ b/scripts/ai/AIDriveStrategyCourse.lua @@ -124,7 +124,7 @@ function AIDriveStrategyCourse:setAIVehicle(vehicle, jobParameters) self.turningRadius = AIUtil.getTurningRadius(vehicle) self.pathfinderController = PathfinderController(vehicle, self.turningRadius) - self.pathfinderController:registerListeners(self, self.onPathfindingFinished, self.onPathfindingRetry) + self.pathfinderController:registerListeners(self, self.onPathfindingFinished, self.onPathfindingFailed) self:setAllStaticParameters() @@ -611,7 +611,7 @@ end ---@param lastContext PathfinderContext ---@param wasLastRetry boolean ---@param currentRetryAttempt number -function AIDriveStrategyCourse:onPathfindingRetry(controller, lastContext, wasLastRetry, currentRetryAttempt) +function AIDriveStrategyCourse:onPathfindingFailed(controller, lastContext, wasLastRetry, currentRetryAttempt) -- override end diff --git a/scripts/ai/AIDriveStrategyFindBales.lua b/scripts/ai/AIDriveStrategyFindBales.lua index ec452d34b..5a0675d4b 100644 --- a/scripts/ai/AIDriveStrategyFindBales.lua +++ b/scripts/ai/AIDriveStrategyFindBales.lua @@ -176,9 +176,9 @@ function AIDriveStrategyFindBales:setAllStaticParameters() self.turningRadius = AIUtil.getTurningRadius(self.vehicle) -- Set the offset to 0, we'll take care of getting the grabber to the right place self.settings.toolOffsetX:setFloatValue(0) - self.pathfinderFailureCount = 0 self.reverser = AIReverseDriver(self.vehicle, self.ppc) - + -- list of bales we tried (or in the process of trying) to find path for + self.balesTried = {} self.numBalesLeftOver = 0 end @@ -213,7 +213,7 @@ function AIDriveStrategyFindBales:findBales() -- if the bale has a mountObject it is already on the loader so ignore it if not object.mountObject and object:getOwnerFarmId() == self.vehicle:getOwnerFarmId() and self:isBaleOnField(bale) then - -- bales may have multiple nodes, using the object.id deduplicates the list + -- bales may .have multiple nodes, using the object.id deduplicates the list balesFound[object.id] = bale end end @@ -230,11 +230,11 @@ function AIDriveStrategyFindBales:findBales() end ---@param bales table[] ----@param baleToIgnore BaleToCollect|nil exclude this bale from the results +---@param balesToIgnore BaleToCollect[]|nil exclude bales on this list from the results ---@return BaleToCollect|nil closest bale ---@return number|nil distance to the closest bale ---@return number|nil index of the bale -function AIDriveStrategyFindBales:findClosestBale(bales, baleToIgnore) +function AIDriveStrategyFindBales:findClosestBale(bales, balesToIgnore) if not bales then return end @@ -251,7 +251,8 @@ function AIDriveStrategyFindBales:findClosestBale(bales, baleToIgnore) self:debug(' Dubins length is %.1f m', d) end if d < minDistance then - if bale ~= baleToIgnore then + if not table.hasElement(balesToIgnore or {}, bale) then + -- bale is not on the list of bales to ignore closestBale = bale minDistance = d ix = i @@ -265,7 +266,7 @@ function AIDriveStrategyFindBales:findClosestBale(bales, baleToIgnore) --- in the grabber's way. That is now wrapped but our bale list does not know about it so let's rescan the field self:debug('%d. bale (%d, %s) INVALID', i, bale:getId(), bale:getBaleObject()) invalidBales = invalidBales + 1 - self:debug('Found an invalid bales, rescanning field', invalidBales) + self:debug('Found invalid bale(s), rescanning field', invalidBales) self.bales = self:findBales() -- return empty, next time this is called everything should be ok return @@ -317,17 +318,29 @@ end ---@param goalNodeInvalid boolean|nil function AIDriveStrategyFindBales:onPathfindingFinished(controller, success, course, goalNodeInvalid) - if self.state == self.states.DRIVING_TO_NEXT_BALE then - if success then + if self.state == self.states.DRIVING_TO_NEXT_BALE then + if success then + self.balesTried = {} self:startCourse(course, 1) - elseif goalNodeInvalid then - -- there may be another bale too close to the previous one - self:debug('Pathfinding failed, goal node invalid.') - self:retryPathfindingWithAnotherBale() - return else - self:info('Pathfinding failed, giving up!') - self.vehicle:stopCurrentAIJob(AIMessageCpErrorNoPathFound.new()) + g_baleToCollectManager:unlockBalesByDriver(self) + if #self.balesTried < 3 then + if goalNodeInvalid then + -- there may be another bale too close to the previous one + self:debug('Finding path to next bale failed, goal node invalid.') + self:retryPathfindingWithAnotherBale() + elseif self:isNearFieldEdge() then + self.balesTried = {} + self:debug('Finding path to next bale failed, we are close to the field edge, back up a bit and then try again') + self:startReversing() + else + self:debug('Finding path to next bale failed, but we are not too close to the field edge') + self:retryPathfindingWithAnotherBale() + end + else + self:info('Pathfinding failed three times, giving up') + self.vehicle:stopCurrentAIJob(AIMessageCpErrorNoPathFound.new()) + end end end end @@ -335,50 +348,16 @@ end --- After pathfinding failed, retry with another bale function AIDriveStrategyFindBales:retryPathfindingWithAnotherBale() self:debug("Retrying with another bale.") - local bale, d, ix = self:findClosestBale(self.bales, self.lastPathfinderBaleTarget) + local bale, d, ix = self:findClosestBale(self.bales, self.balesTried) if bale then self:startPathfindingToBale(bale) else + self.balesTried = {} self:debug("No valid bale found on retry!") self.vehicle:stopCurrentAIJob(AIMessageCpErrorNoPathFound.new()) end end ---- Pathfinding failed, but a retry attempt is leftover. ----@param controller PathfinderController ----@param lastContext PathfinderContext ----@param wasLastRetry boolean ----@param currentRetryAttempt number -function AIDriveStrategyFindBales:onPathfindingRetry(controller, - lastContext, wasLastRetry, currentRetryAttempt) - if self.state == self.states.DRIVING_TO_NEXT_BALE then - g_baleToCollectManager:unlockBalesByDriver(self) - if currentRetryAttempt == 1 then - if self.lastPathfinderBaleTarget - and g_baleToCollectManager:isValidBale(self.lastPathfinderBaleTarget) then - self:debug("Retrying the same bale again.") - self:startPathfindingToBale(self.lastPathfinderBaleTarget) - else - self:retryPathfindingWithAnotherBale() - end - else - if self:isNearFieldEdge() then - self:debug('Finding path to next bale failed twice, we are close to the field edge, back up a bit and then try again') - self:startReversing() - else - self:debug('Finding path to next bale failed twice, but we are not too close to the field edge, trying another bale') - local bale, d, ix = self:findClosestBale(self.bales) - if bale then - self:startPathfindingToBale(bale) - else - self:debug("No valid bale found on retry: %d!", currentRetryAttempt) - self.vehicle:stopCurrentAIJob(AIMessageCpErrorNoPathFound.new()) - end - end - end - end -end - function AIDriveStrategyFindBales:getPathfinderBaleTargetAsGoalNode(bale) local safeDistanceFromBale = bale:getSafeDistance() local halfVehicleWidth = AIUtil.getWidth(self.vehicle) / 2 @@ -397,8 +376,8 @@ function AIDriveStrategyFindBales:startPathfindingToBale(bale) self.state = self.states.DRIVING_TO_NEXT_BALE g_baleToCollectManager:lockBale(bale:getBaleObject(), self) local context = PathfinderContext(self.vehicle):objectsToIgnore(self:getBalesToIgnore()):allowReverse(false) - self.lastPathfinderBaleTarget = bale - self.pathfinderController:findPathToGoal(context, self:getPathfinderBaleTargetAsGoalNode(bale), 3) + table.insert(self.balesTried, bale) + self.pathfinderController:findPathToGoal(context, self:getPathfinderBaleTargetAsGoalNode(bale)) end function AIDriveStrategyFindBales:startReversing() @@ -468,7 +447,7 @@ function AIDriveStrategyFindBales:getDriveData(dt, vX, vY, vZ) else --- Waiting until the unfolding has finished. if self.bales == nil then - --- Makes sure the hud bale counter already get's updated + --- Makes sure the hud bale counter already gets updated self.bales = self:findBales() end self:setMaxSpeed(0) diff --git a/scripts/ai/AIDriveStrategyShovelSiloLoader.lua b/scripts/ai/AIDriveStrategyShovelSiloLoader.lua index 3bbe30e89..a942f50b4 100644 --- a/scripts/ai/AIDriveStrategyShovelSiloLoader.lua +++ b/scripts/ai/AIDriveStrategyShovelSiloLoader.lua @@ -578,7 +578,7 @@ end ---@param lastContext PathfinderContext ---@param wasLastRetry boolean ---@param currentRetryAttempt number -function AIDriveStrategyShovelSiloLoader:onPathfindingRetry(controller, +function AIDriveStrategyShovelSiloLoader:onPathfindingFailed(controller, lastContext, wasLastRetry, currentRetryAttempt) --- TODO: Think of possible points of failures, that could be adjusted here. --- Maybe a small reverse course might help to avoid a deadlock diff --git a/scripts/ai/AIDriveStrategySiloLoader.lua b/scripts/ai/AIDriveStrategySiloLoader.lua index 6052f1088..2e5cd649f 100644 --- a/scripts/ai/AIDriveStrategySiloLoader.lua +++ b/scripts/ai/AIDriveStrategySiloLoader.lua @@ -282,7 +282,7 @@ end ---@param lastContext PathfinderContext ---@param wasLastRetry boolean ---@param currentRetryAttempt number -function AIDriveStrategySiloLoader:onPathfindingRetry(controller, +function AIDriveStrategySiloLoader:onPathfindingFailed(controller, lastContext, wasLastRetry, currentRetryAttempt) --- TODO: Think of possible points of failures, that could be adjusted here. --- Maybe a small reverse course might help to avoid a deadlock diff --git a/scripts/ai/BaleToCollect.lua b/scripts/ai/BaleToCollect.lua index 55e3110dc..c35f9bba3 100644 --- a/scripts/ai/BaleToCollect.lua +++ b/scripts/ai/BaleToCollect.lua @@ -69,7 +69,7 @@ function BaleToCollect.isValidBale(object, baleWrapper, baleLoader, baleWrapType end function BaleToCollect:isStillValid() - return BaleToCollect.isValidBale(self.bale) + return BaleToCollect.isValidBale(self.bale) and not self:isLocked() end function BaleToCollect:isLoaded() @@ -106,6 +106,10 @@ function BaleToCollect:getBaleObject() return self.bale end +function BaleToCollect:isLocked() + return not g_baleToCollectManager:isValidBale(self.bale) +end + function BaleToCollect:getPosition() return getWorldTranslation(self.bale.nodeId) end diff --git a/scripts/ai/PathfinderController.lua b/scripts/ai/PathfinderController.lua index 39eb82e2b..7a79b6690 100644 --- a/scripts/ai/PathfinderController.lua +++ b/scripts/ai/PathfinderController.lua @@ -39,7 +39,7 @@ function Strategy:startPathfindingToGoal() context:set( ... ) - self.pathfinderController:registerListeners(self, self.onPathfindingFinished, self.onPathfindingRetry, + self.pathfinderController:registerListeners(self, self.onPathfindingFinished, self.onPathfindingFailed, self.onPathfindingObstacleAtStart) local numRetries = 2 @@ -61,7 +61,7 @@ function Strategy:onPathfindingFinished(controller : PathfinderController, succe end end -function Strategy:onPathfindingRetry(controller : PathfinderController, currentContext : PathfinderContext, +function Strategy:onPathfindingFailed(controller : PathfinderController, currentContext : PathfinderContext, wasLastRetry : boolean, currentRetryAttempt : number) if currentRetryAttempt == 1 then // Reduced fruit impact: @@ -143,15 +143,15 @@ end --- TODO: Decide if multiple registered listeners are needed or not? ---@param object table ---@param successFunc function func(PathfinderController, success, Course, goalNodeInvalid) ----@param retryFunc function func(PathfinderController, last context, was last retry, retry attempt number) +---@param failedFunc function func(PathfinderController, last context, was last retry, retry attempt number) ---@param obstacleAtStartFunc function|nil func(PathfinderController, last context, obstacleFront, obstacleBehind), --- called when 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. --- TODO: check aft as well if reverse pathfinding allowed. -function PathfinderController:registerListeners(object, successFunc, retryFunc, obstacleAtStartFunc) +function PathfinderController:registerListeners(object, successFunc, failedFunc, obstacleAtStartFunc) self.callbackObject = object self.callbackSuccessFunction = successFunc - self.callbackRetryFunction = retryFunc + self.callbackFailedFunction = failedFunc self.callbackObstacleAtStartFunction = obstacleAtStartFunc end @@ -195,13 +195,13 @@ function PathfinderController:onFinish(path, goalNodeInvalid) self.timeTakenMs = g_time - self.startedAt local retValue = self:isValidPath(path, goalNodeInvalid) if retValue == self.ERROR_NO_PATH_FOUND then - if self.callbackRetryFunction then + if self.callbackFailedFunction then --- 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 self.failCount = self.failCount + 1 - self:callCallback(self.callbackRetryFunction, + self:callCallback(self.callbackFailedFunction, self.currentContext, self.failCount == self.numRetries, self.failCount, false) return elseif self.numRetries > 0 then From af5f4d8175ba13ae96d62d3ca304378e9b97ea80 Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Sat, 16 Mar 2024 10:03:46 -0400 Subject: [PATCH 5/8] fix/refactor: bale finder pathfinder failure logic when everything fails, reverse and try again --- scripts/ai/AIDriveStrategyFindBales.lua | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/scripts/ai/AIDriveStrategyFindBales.lua b/scripts/ai/AIDriveStrategyFindBales.lua index 5a0675d4b..6fc415bf4 100644 --- a/scripts/ai/AIDriveStrategyFindBales.lua +++ b/scripts/ai/AIDriveStrategyFindBales.lua @@ -179,6 +179,9 @@ function AIDriveStrategyFindBales:setAllStaticParameters() self.reverser = AIReverseDriver(self.vehicle, self.ppc) -- list of bales we tried (or in the process of trying) to find path for self.balesTried = {} + -- when everything fails, reverse and try again. This is reset only when a pathfinding succeeds to avoid + -- backing up forever + self.triedReversingAfterPathfinderFailure = false self.numBalesLeftOver = 0 end @@ -320,6 +323,7 @@ function AIDriveStrategyFindBales:onPathfindingFinished(controller, success, course, goalNodeInvalid) if self.state == self.states.DRIVING_TO_NEXT_BALE then if success then + self.triedReversing = false self.balesTried = {} self:startCourse(course, 1) else @@ -337,6 +341,11 @@ function AIDriveStrategyFindBales:onPathfindingFinished(controller, self:debug('Finding path to next bale failed, but we are not too close to the field edge') self:retryPathfindingWithAnotherBale() end + elseif not self.triedReversing then + self:info('Pathfinding failed three times, back up a bit and try again') + self.triedReversing = true + self.balesTried = {} + self:startReversing() else self:info('Pathfinding failed three times, giving up') self.vehicle:stopCurrentAIJob(AIMessageCpErrorNoPathFound.new()) From 8b33e74e7f02c2b2ef0b2f6df4a42fa2dcda789a Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Sat, 16 Mar 2024 10:11:45 -0400 Subject: [PATCH 6/8] fix/refactor: bale finder pathfinder failure logic Stop retrying also when there are no retryable bales left. --- scripts/ai/AIDriveStrategyFindBales.lua | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/scripts/ai/AIDriveStrategyFindBales.lua b/scripts/ai/AIDriveStrategyFindBales.lua index 6fc415bf4..abbb6d5c1 100644 --- a/scripts/ai/AIDriveStrategyFindBales.lua +++ b/scripts/ai/AIDriveStrategyFindBales.lua @@ -323,12 +323,12 @@ function AIDriveStrategyFindBales:onPathfindingFinished(controller, success, course, goalNodeInvalid) if self.state == self.states.DRIVING_TO_NEXT_BALE then if success then - self.triedReversing = false + self.triedReversingAfterPathfinderFailure = false self.balesTried = {} self:startCourse(course, 1) else g_baleToCollectManager:unlockBalesByDriver(self) - if #self.balesTried < 3 then + if #self.balesTried < 3 and #self.bales > #self.balesTried then if goalNodeInvalid then -- there may be another bale too close to the previous one self:debug('Finding path to next bale failed, goal node invalid.') @@ -341,9 +341,9 @@ function AIDriveStrategyFindBales:onPathfindingFinished(controller, self:debug('Finding path to next bale failed, but we are not too close to the field edge') self:retryPathfindingWithAnotherBale() end - elseif not self.triedReversing then - self:info('Pathfinding failed three times, back up a bit and try again') - self.triedReversing = true + elseif not self.triedReversingAfterPathfinderFailure then + self:info('Pathfinding failed three times or no more bales to try, back up a bit and try again') + self.triedReversingAfterPathfinderFailure = true self.balesTried = {} self:startReversing() else From 27b42ef0f9f150dc9f338ca1fc3087bae0ce51f4 Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Sat, 16 Mar 2024 11:18:22 -0400 Subject: [PATCH 7/8] fix/refactor: bale finder pathfinder failure logic Separate states for reversing due to obstacle or pathfinder failure. --- scripts/ai/AIDriveStrategyFindBales.lua | 20 +++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/scripts/ai/AIDriveStrategyFindBales.lua b/scripts/ai/AIDriveStrategyFindBales.lua index abbb6d5c1..9878f79ec 100644 --- a/scripts/ai/AIDriveStrategyFindBales.lua +++ b/scripts/ai/AIDriveStrategyFindBales.lua @@ -29,7 +29,8 @@ AIDriveStrategyFindBales.myStates = { DRIVING_TO_NEXT_BALE = {}, APPROACHING_BALE = {}, WORKING_ON_BALE = {}, - REVERSING_AFTER_PATHFINDER_FAILURE = {} + REVERSING_AFTER_PATHFINDER_FAILURE = {}, + REVERSING_DUE_TO_OBSTACLE_AHEAD = {} } function AIDriveStrategyFindBales:init(task, job) @@ -297,7 +298,7 @@ function AIDriveStrategyFindBales:findPathToNextBale() table.remove(self.bales, ix) else self:debug('There is an obstacle ahead, backing up a bit and retry') - self:startReversing() + self:startReversing(self.states.REVERSING_DUE_TO_OBSTACLE_AHEAD) end end end @@ -336,7 +337,7 @@ function AIDriveStrategyFindBales:onPathfindingFinished(controller, elseif self:isNearFieldEdge() then self.balesTried = {} self:debug('Finding path to next bale failed, we are close to the field edge, back up a bit and then try again') - self:startReversing() + self:startReversing(self.states.REVERSING_AFTER_PATHFINDER_FAILURE) else self:debug('Finding path to next bale failed, but we are not too close to the field edge') self:retryPathfindingWithAnotherBale() @@ -345,7 +346,7 @@ function AIDriveStrategyFindBales:onPathfindingFinished(controller, self:info('Pathfinding failed three times or no more bales to try, back up a bit and try again') self.triedReversingAfterPathfinderFailure = true self.balesTried = {} - self:startReversing() + self:startReversing(self.states.REVERSING_AFTER_PATHFINDER_FAILURE) else self:info('Pathfinding failed three times, giving up') self.vehicle:stopCurrentAIJob(AIMessageCpErrorNoPathFound.new()) @@ -389,9 +390,9 @@ function AIDriveStrategyFindBales:startPathfindingToBale(bale) self.pathfinderController:findPathToGoal(context, self:getPathfinderBaleTargetAsGoalNode(bale)) end -function AIDriveStrategyFindBales:startReversing() +function AIDriveStrategyFindBales:startReversing(state) self:startCourse(Course.createStraightReverseCourse(self.vehicle, 10), 1) - self.state = self.states.REVERSING_AFTER_PATHFINDER_FAILURE + self.state = state end function AIDriveStrategyFindBales:isObstacleAhead() @@ -430,8 +431,11 @@ function AIDriveStrategyFindBales:onWaypointPassed(ix, course) elseif self.state == self.states.REVERSING_AFTER_PATHFINDER_FAILURE then self:debug('backed up after pathfinder failed, trying again') self.state = self.states.SEARCHING_FOR_NEXT_BALE + elseif self.state == self.states.REVERSING_DUE_TO_OBSTACLE_AHEAD then + self:debug('backed due to obstacle, trying again') + self.state = self.states.SEARCHING_FOR_NEXT_BALE end - elseif self.state == self.states.REVERSING_AFTER_PATHFINDER_FAILURE then + elseif self.state == self.states.REVERSING_DUE_TO_OBSTACLE_AHEAD then if not self:isObstacleAhead() then self:debug('backed up after pathfinder failed, no more obstacle ahead, trying again') self.state = self.states.SEARCHING_FOR_NEXT_BALE @@ -477,6 +481,8 @@ function AIDriveStrategyFindBales:getDriveData(dt, vX, vY, vZ) self:setMaxSpeed(0) elseif self.state == self.states.REVERSING_AFTER_PATHFINDER_FAILURE then self:setMaxSpeed(self.settings.reverseSpeed:getValue()) + elseif self.state == self.states.REVERSING_DUE_TO_OBSTACLE_AHEAD then + self:setMaxSpeed(self.settings.reverseSpeed:getValue()) end local moveForwards = not self.ppc:isReversing() From 60794e395e60e0cff70b9a69641459da4387715d Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Thu, 21 Mar 2024 13:57:27 +0100 Subject: [PATCH 8/8] fix: rescan field if bumped into another bale Rescan field if bumped into another bale while driving to the target, to make sure it isn't on the list anymore --- scripts/ai/AIDriveStrategyFindBales.lua | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/scripts/ai/AIDriveStrategyFindBales.lua b/scripts/ai/AIDriveStrategyFindBales.lua index 9878f79ec..942e479fa 100644 --- a/scripts/ai/AIDriveStrategyFindBales.lua +++ b/scripts/ai/AIDriveStrategyFindBales.lua @@ -118,6 +118,10 @@ function AIDriveStrategyFindBales:isReadyToLoadNextBale() return not isGrabbingBale end +function AIDriveStrategyFindBales:isGrabbingBale() + return not self:isReadyToLoadNextBale() +end + --- Have any bales been loaded? function AIDriveStrategyFindBales:hasBalesLoaded() local hasBales = false @@ -287,6 +291,11 @@ function AIDriveStrategyFindBales:getDubinsPathLengthToBale(bale) end function AIDriveStrategyFindBales:findPathToNextBale() + if self.bumpedIntoAnotherBale then + self.bumpedIntoAnotherBale = false + self:debug("Bumped into a bale other than the target on the way, rescanning.") + self:findBales() + end local bale, d, ix = self:findClosestBale(self.bales) if bale then if bale:isLoaded() then @@ -473,6 +482,10 @@ function AIDriveStrategyFindBales:getDriveData(dt, vX, vY, vZ) self:setMaxSpeed(0) elseif self.state == self.states.DRIVING_TO_NEXT_BALE then self:setMaxSpeed(self.settings.fieldSpeed:getValue()) + if not self.bumpedIntoAnotherBale and self:isGrabbingBale() then + -- we are not at the bale yet but grabbing something, likely bumped into another bale + self.bumpedIntoAnotherBale = true + end elseif self.state == self.states.APPROACHING_BALE then self:setMaxSpeed(self.settings.fieldWorkSpeed:getValue() / 2) self:approachBale()