Merge pull request #183 from Courseplay/79-tools-sometimes-stop-working-after-transition-from-headland-to-center-or-vice-versa

fix: headland/center transition

Author: Tensuko

scripts/ai/strategies/AIDriveStrategyFieldWorkCourse.lua

@@ -28,10 +28,10 @@ AIDriveStrategyFieldWorkCourse.myStates = {
     WAITING_FOR_LOWER_DELAYED = {},
     WAITING_FOR_STOP = {},
     WAITING_FOR_WEATHER = {},
-    TURNING = {showTurnContextDebug = true},
+    TURNING = { showTurnContextDebug = true },
     TEMPORARY = {},
     RETURNING_TO_START = {},
-    DRIVING_TO_WORK_START_WAYPOINT = {showTurnContextDebug = true},
+    DRIVING_TO_WORK_START_WAYPOINT = { showTurnContextDebug = true },
 }
 
 AIDriveStrategyFieldWorkCourse.normalFillLevelFullPercentage = 99.5
@@ -94,7 +94,7 @@ function AIDriveStrategyFieldWorkCourse:start(course, startIx, jobParameters)
     --- Store a reference to the original generated course
     self.originalGeneratedFieldWorkCourse = self.vehicle:getFieldWorkCourse()
 
-    if self.fieldPolygon == nil then 
+    if self.fieldPolygon == nil then
         self:debug("No field polygon received, so regenerate it by the course.")
         self.fieldPolygon = self.fieldWorkCourse:getFieldPolygon()
     end
@@ -114,7 +114,7 @@ end
 function AIDriveStrategyFieldWorkCourse:update(dt)
     AIDriveStrategyCourse.update(self, dt)
     if CpDebug:isChannelActive(CpDebug.DBG_TURN, self.vehicle) then
-        if self.state == self.states.TURNING  then
+        if self.state == self.states.TURNING then
             if self.aiTurn then
                 self.aiTurn:drawDebug()
             end
@@ -188,7 +188,9 @@ function AIDriveStrategyFieldWorkCourse:getDriveData(dt, vX, vY, vZ)
         self:setMaxSpeed(turnMaxSpeed)
         -- if turn tells us which way to go, use that, otherwise just do whatever PPC tells us
         gx, gz = turnGx or gx, turnGz or gz
-        if turnMoveForwards ~= nil then moveForwards = turnMoveForwards end
+        if turnMoveForwards ~= nil then
+            moveForwards = turnMoveForwards
+        end
     elseif self.state == self.states.RETURNING_TO_START then
         local isReadyToDrive, blockingVehicle = self.vehicle:getIsAIReadyToDrive()
         if isReadyToDrive or not self.waitingForPrepare:get() then
@@ -263,7 +265,8 @@ function AIDriveStrategyFieldWorkCourse:initializeImplementControllers(vehicle)
 
     self:addImplementController(vehicle, PickupController, Pickup, defaultDisabledStates)
     self:addImplementController(vehicle, SprayerController, Sprayer, {})
-    self:addImplementController(vehicle, CutterController, Cutter, {}) --- Makes sure the cutter timer gets reset always.
+    self:addImplementController(vehicle, CutterController, Cutter, {})
+    --- Makes sure the cutter timer gets reset always.
     self:addImplementController(vehicle, StonePickerController, StonePicker, defaultDisabledStates)
 
     self:addImplementController(vehicle, FoldableController, Foldable, {})
@@ -312,7 +315,9 @@ function AIDriveStrategyFieldWorkCourse:shouldRaiseThisImplement(object, turnSta
     local aiFrontMarker, _, aiBackMarker = WorkWidthUtil.getAIMarkers(object, true)
     -- if something (like a combine) does not have an AI marker it should not prevent from raising other implements
     -- like the header, which does have markers), therefore, return true here
-    if not aiBackMarker or not aiFrontMarker then return true end
+    if not aiBackMarker or not aiFrontMarker then
+        return true
+    end
     local marker = self:getImplementRaiseLate() and aiBackMarker or aiFrontMarker
     -- turn start node in the back marker node's coordinate system
     local _, _, dz = localToLocal(marker, turnStartNode, 0, 0, 0)
@@ -355,7 +360,9 @@ end
 --- in meters (<0) when driving forward, nil when driving backwards.
 function AIDriveStrategyFieldWorkCourse:shouldLowerThisImplement(object, workStartNode, reversing)
     local aiLeftMarker, aiRightMarker, aiBackMarker = WorkWidthUtil.getAIMarkers(object, true)
-    if not aiLeftMarker then return false, false, nil end
+    if not aiLeftMarker then
+        return false, false, nil
+    end
     local dxLeft, _, dzLeft = localToLocal(aiLeftMarker, workStartNode, 0, 0, 0)
     local dxRight, _, dzRight = localToLocal(aiRightMarker, workStartNode, 0, 0, 0)
     local dxBack, _, dzBack = localToLocal(aiBackMarker, workStartNode, 0, 0, 0)
@@ -408,7 +415,9 @@ end
 
 function AIDriveStrategyFieldWorkCourse:isThisImplementAligned(object, node)
     local aiFrontMarker, _, _ = WorkWidthUtil.getAIMarkers(object, true)
-    if not aiFrontMarker then return true end
+    if not aiFrontMarker then
+        return true
+    end
     return CpMathUtil.isSameDirection(aiFrontMarker, node, 5)
 end
 
@@ -426,7 +435,8 @@ function AIDriveStrategyFieldWorkCourse:onWaypointChange(ix, course)
         end
         self:startTurn(ix)
     elseif self.state == self.states.WORKING then
-        if self.course:isOnConnectingPath(ix + 1) or self.course:shouldUsePathfinderToNextWaypoint(ix) then
+        if (self.course:isOnConnectingPath(ix + 1) and not self.course:isOnConnectingPath(ix)) or
+                self.course:shouldUsePathfinderToNextWaypoint(ix) then
             local fm, bm = self:getFrontAndBackMarkers()
             self.turnContext = RowStartOrFinishContext(self.vehicle, self.course, ix, ix, self.turnNodes, self:getWorkWidth(),
                     fm, bm, 0, 0)
@@ -626,7 +636,6 @@ end
 -----------------------------------------------------------------------------------------------------------------------
 function AIDriveStrategyFieldWorkCourse:startPathfindingToNextWaypoint(ix)
     self:debug('start pathfinding to waypoint %d', ix + 1)
-    self:raiseImplements()
     local fm, bm = self:getFrontAndBackMarkers()
     self.turnContext = RowStartOrFinishContext(self.vehicle, self.fieldWorkCourse, ix + 1, ix + 1,
             self.turnNodes, self:getWorkWidth(), fm, bm, self:getTurnEndSideOffset(), self:getTurnEndForwardOffset())
@@ -643,10 +652,17 @@ function AIDriveStrategyFieldWorkCourse:startPathfindingToNextWaypoint(ix)
     self.pathfinderController:findPathToNode(context, targetNode, 0, zOffset)
 end
 
-function AIDriveStrategyFieldWorkCourse:onPathfindingFailedToNextWaypoint()
-    self:debug('Pathfinding to next waypoint failed, continue directly at waypoint %d', self.waypointToContinueOnFailedPathfinding)
-    self.state = self.states.WORKING
-    self:startCourse(self.fieldWorkCourse, self.waypointToContinueOnFailedPathfinding)
+function AIDriveStrategyFieldWorkCourse:onPathfindingFailedToNextWaypoint(controller, lastContext, wasLastRetry, currentRetryAttempt)
+    if wasLastRetry then
+        self:debug('Pathfinding to next waypoint failed again, continue directly at waypoint %d', self.waypointToContinueOnFailedPathfinding)
+        self:startWaitingForLower()
+        self:lowerImplements()
+        self:startCourse(self.fieldWorkCourse, self.waypointToContinueOnFailedPathfinding)
+    else
+        self:debug('Pathfinding to next waypoint failed once, retry with disabled collisions')
+        lastContext:collisionMask(0)
+        controller:retry(lastContext)
+    end
 end
 
 function AIDriveStrategyFieldWorkCourse:onPathfindingDoneToNextWaypoint(controller, success, course, goalNodeInvalid)
@@ -663,22 +679,24 @@ end
 -----------------------------------------------------------------------------------------------------------------------
 function AIDriveStrategyFieldWorkCourse:startConnectingPath(ix)
     -- ix was the last waypoint to work before the connecting path, ix + 1 is the first on the connecting path
-    self:debug('on a connecting path now at waypoint %d, raising implements.', ix + 1)
-    self:raiseImplements()
+    self:debug('Row finished before starting on a connecting path at waypoint %d.', ix + 1)
     -- gather the connecting path waypoints
     local connectingPath = {}
     local targetWaypointIx
     for i = ix + 1, self.fieldWorkCourse:getNumberOfWaypoints() do
         if self.fieldWorkCourse:isOnConnectingPath(i) then
             local x, _, z = self.fieldWorkCourse:getWaypointPosition(i)
-            table.insert(connectingPath, {x = x, z = z})
+            table.insert(connectingPath, { x = x, z = z })
         else
             targetWaypointIx = i
             break
         end
     end
     if targetWaypointIx == nil then
-        self:onPathfindingFailedToConnectingPathEnd()
+        self:debug('Can\'t find end of connecting path, continuing work')
+        self:startWaitingForLower()
+        self:lowerImplements()
+        self:startCourse(self.fieldWorkCourse, ix + 1)
     else
         -- set up the turn context for the work starter to use when the pathfinding succeeds
         local fm, bm = self:getFrontAndBackMarkers()
@@ -688,28 +706,34 @@ function AIDriveStrategyFieldWorkCourse:startConnectingPath(ix)
         local targetNode, zOffset = self.turnContext:getTurnEndNodeAndOffsets(steeringLength)
         local context = PathfinderContext(self.vehicle):allowReverse(self:getAllowReversePathfinding())
         context:preferredPath(connectingPath):mustBeAccurate(true)
-        self.waypointToContinueOnFailedPathfinding = ix + 1
+        self.rawConnectingPath = Course(self.vehicle, connectingPath, true)
         self.pathfinderController:registerListeners(self, self.onPathfindingDoneToConnectingPathEnd,
                 self.onPathfindingFailedToConnectingPathEnd)
         self:debug('Connecting path has %d waypoints, start pathfinding to target waypoint %d, zOffset %.1f',
                 #connectingPath, targetWaypointIx, zOffset)
         self.state = self.states.WAITING_FOR_PATHFINDER
-        self.pathfinderController:findPathToNode(context, targetNode, 0, zOffset)
+        self.pathfinderController:findPathToNode(context, targetNode, 0, zOffset, 1)
     end
 end
 
-function AIDriveStrategyFieldWorkCourse:onPathfindingFailedToConnectingPathEnd()
-    self:debug('Pathfinding to end of connecting path failed, use the connecting path as is')
-    self.state = self.states.WORKING
-    self:startCourse(self.fieldWorkCourse, self.waypointToContinueOnFailedPathfinding)
+function AIDriveStrategyFieldWorkCourse:onPathfindingFailedToConnectingPathEnd(controller, lastContext, wasLastRetry, currentRetryAttempt)
+    if wasLastRetry then
+        self:debug('Pathfinding to end of connecting path failed again, use the connecting path as is')
+        self:startCourseToWorkStart(self.rawConnectingPath)
+    else
+        self:debug('Pathfinding to end of connecting path failed once, retry with disabled collisions')
+        lastContext:collisionMask(0)
+        controller:retry(lastContext)
+    end
 end
 
 function AIDriveStrategyFieldWorkCourse:onPathfindingDoneToConnectingPathEnd(controller, success, course, goalNodeInvalid)
     if success then
         self:debug('Pathfinding to end of connecting path finished')
         self:startCourseToWorkStart(course)
     else
-        self:onPathfindingFailedToConnectingPathEnd()
+        self:debug('Pathfinding to end of connecting path failed, use the connecting path as is')
+        self:startCourseToWorkStart(self.rawConnectingPath)
     end
 end
 
@@ -732,7 +756,7 @@ function AIDriveStrategyFieldWorkCourse:setAllStaticParameters()
 end
 
 function AIDriveStrategyFieldWorkCourse:setFieldPolygon(polygon)
-    self.fieldPolygon = polygon    
+    self.fieldPolygon = polygon
 end
 
 -----------------------------------------------------------------------------------------------------------------------

scripts/dev/DevHelper.lua

@@ -91,7 +91,7 @@ function DevHelper:update()
     self.data.collidingShapes = {}
     overlapBox(self.data.x, self.data.y + 0.2 + DevHelper.overlapBoxHeight / 2, self.data.z, 0, self.yRot, 0,
             DevHelper.overlapBoxWidth / 2, DevHelper.overlapBoxHeight / 2, DevHelper.overlapBoxLength / 2,
-            "overlapBoxCallback", self, collisionMask, true, true, true)
+            "overlapBoxCallback", self, collisionMask, true, true, true, true)
 
 end
 
@@ -115,7 +115,7 @@ function DevHelper:overlapBoxCallback(transformId, subShapeIndex)
 			if collidingObject:isa(Bale) then
 				text = text .. ' Bale ' .. tostring(collidingObject.id) .. ' ' .. tostring(collidingObject.nodeId)
 			else
-            	text = text .. ' ' .. collidingObject.getName and collidingObject:getName() or 'N/A'
+            	text = text .. ' ' .. (collidingObject.getName and collidingObject:getName() or 'N/A')
 			end
         end
     end

scripts/pathfinder/PathfinderConstraints.lua

@@ -72,6 +72,7 @@ function PathfinderConstraints:init(context)
     self.ignoreTrailerAtStartRange = context._ignoreTrailerAtStartRange or 0
     self.initialMaxFruitPercent = self.maxFruitPercent
     self.initialOffFieldPenalty = self.offFieldPenalty
+    self.collisionMask = context._collisionMask
     self.strictMode = false
     self:resetCounts()
     local areaToAvoidText = self.areaToAvoid and
@@ -204,8 +205,8 @@ function PathfinderConstraints:isValidNode(node, ignoreTrailer, offFieldValid)
             node.x, -node.y, CpMathUtil.angleToGame(node.t), 0.5)
 
     -- for debug purposes only, store validity info on node
-    node.collidingShapes = PathfinderUtil.collisionDetector:findCollidingShapes(
-            PathfinderUtil.helperNode, self.vehicleData, self.vehiclesToIgnore, self.objectsToIgnore, self.ignoreFruitHeaps)
+    node.collidingShapes = PathfinderUtil.collisionDetector:findCollidingShapes(PathfinderUtil.helperNode,
+            self.vehicleData, self.vehiclesToIgnore, self.objectsToIgnore, self.ignoreFruitHeaps, self.collisionMask)
     ignoreTrailer = ignoreTrailer or node.d < self.ignoreTrailerAtStartRange
     if self.vehicleData.trailer and not ignoreTrailer then
         -- now check the trailer or towed implement
@@ -217,7 +218,7 @@ function PathfinderConstraints:isValidNode(node, ignoreTrailer, offFieldValid)
 
         node.collidingShapes = node.collidingShapes + PathfinderUtil.collisionDetector:findCollidingShapes(
                 PathfinderUtil.helperNode, self.vehicleData.trailerRectangle, self.vehiclesToIgnore,
-                self.objectsToIgnore, self.ignoreFruitHeaps)
+                self.objectsToIgnore, self.ignoreFruitHeaps, self.collisionMask)
         if node.collidingShapes > 0 then
             self.trailerCollisionNodeCount = self.trailerCollisionNodeCount + 1
         end

scripts/pathfinder/PathfinderContext.lua

@@ -103,7 +103,9 @@ PathfinderContext.attributesToDefaultValue = {
     ["ignoreTrailerAtStartRange"] = 0,
     -- Maximum number of iterations before the hybrid A* pathfinding gives up. The default works well
     -- for normal sized fields, on bigger maps with huge fields you may want to double or triple this
-    ["maxIterations"] = HybridAStar.defaultMaxIterations
+    ["maxIterations"] = HybridAStar.defaultMaxIterations,
+    -- Collision mask to use, to disable collisions completely, set to 0objectsToIgnore
+    ["collisionMask"] = CpUtil.getDefaultCollisionFlags() + CollisionFlag.TERRAIN_DELTA
 }
 
 function PathfinderContext:init(vehicle)

scripts/pathfinder/PathfinderUtil.lua

@@ -245,9 +245,10 @@ PathfinderUtil.CollisionDetector = CpObject()
 --- Nodes of a trigger for example, that will be ignored as collision.
 PathfinderUtil.CollisionDetector.NODES_TO_IGNORE = {}
 
-function PathfinderUtil.CollisionDetector:init()
+function PathfinderUtil.CollisionDetector:init(collisionMask)
     self.vehiclesToIgnore = {}
     self.collidingShapes = 0
+    self.collisionMask = collisionMask or CpUtil.getDefaultCollisionFlags()
 end
 
 --- Adds a node which collision will be ignored global for every pathfinder.
@@ -318,7 +319,8 @@ function PathfinderUtil.CollisionDetector:overlapBoxCallback(transformId)
     self.collidingShapes = self.collidingShapes + 1
 end
 
-function PathfinderUtil.CollisionDetector:findCollidingShapes(node, vehicleData, vehiclesToIgnore, objectsToIgnore, ignoreFruitHeaps)
+function PathfinderUtil.CollisionDetector:findCollidingShapes(node, vehicleData, vehiclesToIgnore, objectsToIgnore,
+                                                              ignoreFruitHeaps, collisionMask)
     self.vehiclesToIgnore = vehiclesToIgnore or {}
     self.objectsToIgnore = objectsToIgnore or {}
     self.vehicleData = vehicleData
@@ -343,9 +345,8 @@ function PathfinderUtil.CollisionDetector:findCollidingShapes(node, vehicleData,
     self.collidingShapes = 0
     self.collidingShapesText = 'unknown'
 
-    local collisionMask = CpUtil.getDefaultCollisionFlags() + CollisionFlag.TERRAIN_DELTA
-
-    overlapBox(x, y + 0.2, z, xRot, yRot, zRot, width, 1, length, 'overlapBoxCallback', self, collisionMask, true, true, true)
+    overlapBox(x, y + 0.2, z, xRot, yRot, zRot, width, 1, length, 'overlapBoxCallback', self, collisionMask,
+            true, true, true, true)
 
     if true and self.collidingShapes > 0 then
         table.insert(PathfinderUtil.overlapBoxes,