fix: headland corner plow offset

The corner was created with the tool offset, which is calculated
automatically but may be on the wrong side.

Use the current course offset instead.

Author: Peter Vaiko

scripts/ai/strategies/AIDriveStrategyCombineCourse.lua

@@ -1405,7 +1405,7 @@ function AIDriveStrategyCombineCourse:startTurn(ix)
 
     self.turnContext = TurnContext(self.vehicle, self.course, ix, ix + 1, self.turnNodes, self:getWorkWidth(),
             self.frontMarkerDistance, self.backMarkerDistance,
-            self:getTurnEndSideOffset(), self:getTurnEndForwardOffset())
+            self:getTurnEndSideOffset(self.course:isHeadlandTurnAtIx(ix + 1)), self:getTurnEndForwardOffset())
 
     -- Combines drive special headland corner maneuvers, except potato and sugarbeet harvesters
     if self.turnContext:isHeadlandCorner() then

scripts/ai/strategies/AIDriveStrategyFieldWorkCourse.lua

@@ -392,7 +392,7 @@ function AIDriveStrategyFieldWorkCourse:startTurn(ix)
     local fm, bm = self:getFrontAndBackMarkers()
     self.ppc:setShortLookaheadDistance()
     self.turnContext = TurnContext(self.vehicle, self.course, ix, ix + 1, self.turnNodes, self:getWorkWidth(), fm, bm,
-            self:getTurnEndSideOffset(), self:getTurnEndForwardOffset())
+            self:getTurnEndSideOffset(self.course:isHeadlandTurnAtIx(ix + 1)), self:getTurnEndForwardOffset())
     if AITurn.canMakeKTurn(self.vehicle, self.turnContext, self.workWidth, self:isTurnOnFieldActive()) then
         self.aiTurn = KTurn(self.vehicle, self, self.ppc, self.proximityController, self.turnContext, self.workWidth)
     else
@@ -488,7 +488,7 @@ function AIDriveStrategyFieldWorkCourse:startAlignmentTurn(fieldWorkCourse, star
     if alignmentCourse then
         local fm, bm = self:getFrontAndBackMarkers()
         self.turnContext = RowStartOrFinishContext(self.vehicle, fieldWorkCourse, startIx, startIx, self.turnNodes,
-                self:getWorkWidth(), fm, bm, self:getTurnEndSideOffset(), self:getTurnEndForwardOffset())
+                self:getWorkWidth(), fm, bm, self:getTurnEndSideOffset(false), self:getTurnEndForwardOffset())
         self.workStarter = StartRowOnly(self.vehicle, self, self.ppc, self.turnContext, alignmentCourse)
         self.state = self.states.DRIVING_TO_WORK_START_WAYPOINT
         self:startCourse(self.workStarter:getCourse(), 1)
@@ -542,7 +542,7 @@ function AIDriveStrategyFieldWorkCourse:startPathfindingToNextWaypoint(ix)
     self:debug('start pathfinding to waypoint %d', ix + 1)
     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())
+            self.turnNodes, self:getWorkWidth(), fm, bm, self:getTurnEndSideOffset(false), self:getTurnEndForwardOffset())
     local _, steeringLength = AIUtil.getSteeringParameters(self.vehicle)
     local targetNode, zOffset = self.turnContext:getTurnEndNodeAndOffsets(steeringLength)
     local context = PathfinderContext(self.vehicle):allowReverse(self:getAllowReversePathfinding())
@@ -605,7 +605,7 @@ function AIDriveStrategyFieldWorkCourse:startConnectingPath(ix)
         -- set up the turn context for the work starter to use when the pathfinding succeeds
         local fm, bm = self:getFrontAndBackMarkers()
         self.turnContext = RowStartOrFinishContext(self.vehicle, self.fieldWorkCourse, targetWaypointIx, targetWaypointIx,
-                self.turnNodes, self:getWorkWidth(), fm, bm, self:getTurnEndSideOffset(), self:getTurnEndForwardOffset())
+                self.turnNodes, self:getWorkWidth(), fm, bm, self:getTurnEndSideOffset(false), self:getTurnEndForwardOffset())
         local _, steeringLength = AIUtil.getSteeringParameters(self.vehicle)
         local targetNode, zOffset = self.turnContext:getTurnEndNodeAndOffsets(steeringLength)
         local context = PathfinderContext(self.vehicle):allowReverse(self:getAllowReversePathfinding())
@@ -670,7 +670,7 @@ end
 -----------------------------------------------------------------------------------------------------------------------
 --- Dynamic parameters (may change while driving)
 -----------------------------------------------------------------------------------------------------------------------
-function AIDriveStrategyFieldWorkCourse:getTurnEndSideOffset()
+function AIDriveStrategyFieldWorkCourse:getTurnEndSideOffset(isHeadlandTurn)
     return 0
 end
 

scripts/ai/strategies/AIDriveStrategyPlowCourse.lua

@@ -50,15 +50,15 @@ function AIDriveStrategyPlowCourse:getDriveData(dt, vX, vY, vZ)
             --- we still need to check if the rotation 
             --- is in correct direction, as the course generator directed.
             self:rotatePlows()
-			self:debug("Needs to wait until the plow has finished rotating.")
-			self.state = self.states.ROTATING_PLOW
-		else 
+            self:debug("Needs to wait until the plow has finished rotating.")
+            self.state = self.states.ROTATING_PLOW
+        else
             --- The plow can not be rotated, 
             --- so we check if the plow is unfolded
             --- and try again to rotate the plow in the correct direction.
             self:debug("Plows have to be unfolded first!")
-			self.state = self.states.UNFOLDING_PLOW
-		end
+            self.state = self.states.UNFOLDING_PLOW
+        end
     elseif self.state == self.states.ROTATING_PLOW then
         self:setMaxSpeed(0)
         if not self:isPlowRotating() then
@@ -70,13 +70,13 @@ function AIDriveStrategyPlowCourse:getDriveData(dt, vX, vY, vZ)
         end
     elseif self.state == self.states.UNFOLDING_PLOW then
         self:setMaxSpeed(0)
-        if self:isPlowRotationAllowed() then 
+        if self:isPlowRotationAllowed() then
             --- The Unfolding has finished and 
             --- we need to check if the rotation is correct.
             self:rotatePlows()
             self:debug("Plow was unfolded and rotation can begin")
-			self.state = self.states.ROTATING_PLOW
-        elseif self:getCanContinueWork() then 
+            self.state = self.states.ROTATING_PLOW
+        elseif self:getCanContinueWork() then
             --- Unfolding has finished and no extra rotation is needed.
             self:updatePlowOffset()
             self:startWaitingForLower()
@@ -104,7 +104,7 @@ end
 --- Updates the X Offset based on the plows attached.
 function AIDriveStrategyPlowCourse:updatePlowOffset()
     local xOffset = 0
-    for _, controller in pairs(self.controllers) do 
+    for _, controller in pairs(self.controllers) do
         if controller.getAutomaticXOffset then
             local autoOffset = controller:getAutomaticXOffset()
             if autoOffset == nil then
@@ -127,8 +127,8 @@ end
 --- Is a plow currently rotating?
 ---@return boolean
 function AIDriveStrategyPlowCourse:isPlowRotating()
-    for _, controller in pairs(self.controllers) do 
-        if controller.isRotationActive and controller:isRotationActive() then 
+    for _, controller in pairs(self.controllers) do
+        if controller.isRotationActive and controller:isRotationActive() then
             return true
         end
     end
@@ -139,8 +139,8 @@ end
 ---@return boolean
 function AIDriveStrategyPlowCourse:isPlowRotationAllowed()
     local allowed = true
-    for _, controller in pairs(self.controllers) do 
-        if controller.getIsPlowRotationAllowed and not controller:getIsPlowRotationAllowed() then 
+    for _, controller in pairs(self.controllers) do
+        if controller.getIsPlowRotationAllowed and not controller:getIsPlowRotationAllowed() then
             allowed = false
         end
     end
@@ -153,29 +153,45 @@ function AIDriveStrategyPlowCourse:rotatePlows()
     local ix = self.ppc:getCurrentWaypointIx()
     local plowShouldBeOnTheLeft = self.course:shouldPlowBeOnTheLeft(ix)
     for _, controller in pairs(self.controllers) do
-        if controller.rotate then 
+        if controller.rotate then
             controller:rotate(plowShouldBeOnTheLeft)
         end
     end
 end
 
+--- Do we have at least one plow that can be rotated?
+---@return boolean
+function AIDriveStrategyPlowCourse:haveRotatablePlow()
+    for _, controller in pairs(self.controllers) do
+        if controller.isRotatablePlow and controller:isRotatablePlow() then
+            return true
+        end
+    end
+    return false
+end
+
 -----------------------------------------------------------------------------------------------------------------------
 --- Dynamic parameters (may change while driving)
 -----------------------------------------------------------------------------------------------------------------------
-function AIDriveStrategyPlowCourse:getTurnEndSideOffset()
-    if self:isWorking() then
+function AIDriveStrategyPlowCourse:getTurnEndSideOffset(isHeadlandTurn)
+    -- on headland turns we do not rotate the plow, and since the course already has the offset, nothing to do,
+    -- the context will calculate with the offset of the course already, no additional side offset is needed
+    if self:isWorking() and not isHeadlandTurn and self:haveRotatablePlow() then
         self:updatePlowOffset()
-        -- need the double tool offset as the turn end still has the current offset, after the rotation it'll be
-        -- on the other side, (one toolOffsetX would put it to 0 only)
+        -- need the double tool offset as the turn end waypoint still has the current offset, but after rotating,
+        -- the plow will be on the other side, (one toolOffsetX would put it to 0 only)
+        self:debug('Setting turn end side offset to %.2f for the plow', 2 * self.aiOffsetX)
         return 2 * self.aiOffsetX
     else
+        self:debug('No turn end side offset, working: %s, headland turn %s, have rotatable plow %s ',
+                tostring(self:isWorking()), tostring(isHeadlandTurn), tostring(self:haveRotatablePlow()))
         return 0
     end
 end
 
 function AIDriveStrategyPlowCourse:updateFieldworkOffset(course)
-	--- Ignore the tool offset setting.
-	course:setOffset((self.aiOffsetX or 0), (self.aiOffsetZ or 0))
+    --- Ignore the tool offset setting.
+    course:setOffset((self.aiOffsetX or 0), (self.aiOffsetZ or 0))
 end
 
 --- When we return from a turn, the offset is reverted and should immediately set, not waiting

scripts/ai/turns/TurnContext.lua

@@ -89,6 +89,7 @@ function TurnContext:init(vehicle, course, turnStartIx, turnEndIx, turnNodes, wo
 
     self.dx, _, self.dz = localToLocal(self.turnEndWpNode.node, self.workEndNode, 0, 0, 0)
     self.leftTurn = self.dx > 0
+    self.cornerOffsetX, _ = course:getOffset()
     self.plowShouldBeOnTheLeft = course:shouldPlowBeOnTheLeft(self.turnEndWpIx)
     self:debug('start ix = %d, back marker = %.1f, front marker = %.1f',
             turnStartIx, self.backMarkerDistance, self.frontMarkerDistance)
@@ -364,8 +365,8 @@ function TurnContext:createCorner(vehicle, r)
     local endAngleDeg = self:getAverageEndAngleDeg()
     CpUtil.debugVehicle(CpDebug.DBG_TURN, vehicle, 'start angle: %.1f, end angle: %.1f (from %.1f and %.1f)', self.beforeTurnStartWp.angle,
             endAngleDeg, self.turnEndWp.angle, self.afterTurnEndWp.angle)
-    return Corner(vehicle, self.beforeTurnStartWp.angle, self.turnStartWp, endAngleDeg, self.turnEndWp, r,
-            vehicle:getCpSettings().toolOffsetX:getValue())
+    -- use the course X offset as that's the best indication of the current, actual offset X the vehicle is using
+    return Corner(vehicle, self.beforeTurnStartWp.angle, self.turnStartWp, endAngleDeg, self.turnEndWp, r, self.cornerOffsetX)
 end