Merge pull request #979 from Courseplay/972-pflug-sollte-nach-innen-pflugen-auf-vorgewende

refactor: plow rotation logic unified

Author: Tensuko

scripts/Course.lua

@@ -635,6 +635,29 @@ function Course:isNextTurnLeft(ix)
     end
 end
 
+--- Should the plow be rotated to the left at the waypoint ix?
+--- On the headland just check which side was worked last, on the center, check the direction of the next turn
+--- as at the first pass both sides are unworked and need some other indication on which side the plow should be
+function Course:shouldPlowBeOnTheLeft(ix)
+    local plowShouldBeOnTheLeft
+    if self:isOnHeadland(ix) then
+        local clockwise = self:isOnClockwiseHeadland(ix)
+        plowShouldBeOnTheLeft = not clockwise
+        CpUtil.debugVehicle(CpDebug.DBG_TURN, self.vehicle, 'On a headland (clockwise %s), plow should be on the left %s', tostring(clockwise), tostring(plowShouldBeOnTheLeft))
+    else
+        local isNextTurnLeft = self:isNextTurnLeft(ix)
+        if isNextTurnLeft == nil then
+            -- don't know if left or right, so just use the last worked side
+            plowShouldBeOnTheLeft = self:isLeftSideWorked(ix)
+            CpUtil.debugVehicle(CpDebug.DBG_TURN, self.vehicle, 'On the center, next turn direction unknown, plow should be on the left %s', tostring(plowShouldBeOnTheLeft))
+        else
+            plowShouldBeOnTheLeft = not isNextTurnLeft
+            CpUtil.debugVehicle(CpDebug.DBG_TURN, self.vehicle, 'On the center, plow should be on the left %s', tostring(plowShouldBeOnTheLeft))
+        end
+    end
+    return plowShouldBeOnTheLeft
+end
+
 function Course:getIxRollover(ix)
     if ix > #self.waypoints then
         return ix - #self.waypoints
@@ -1455,7 +1478,7 @@ end
 local function calculateYRot(waypoints, i)
     local x1, z1 = waypoints[i].x, waypoints[i].z
     local x2, z2 = waypoints[i + 1].x, waypoints[i + 1].z
-    if (x1 - x2) == 0 and (z1 - z2) == 0 then 
+    if (x1 - x2) == 0 and (z1 - z2) == 0 then
         -- Divide by zero fix ..
         return 0
     end
@@ -1810,13 +1833,13 @@ function Course.MultiVehicleData.createFromStream(stream, nVehicles)
 end
 
 function Course.MultiVehicleData.getAllowedPositions(nMultiToolVehicles)
-    if nMultiToolVehicles == 2 then 
+    if nMultiToolVehicles == 2 then
         return {-1,1}
-    elseif nMultiToolVehicles == 3 then 
+    elseif nMultiToolVehicles == 3 then
         return {-1,0,1}
-    elseif nMultiToolVehicles == 4 then 
+    elseif nMultiToolVehicles == 4 then
         return {-2,-1,1,2}
-    elseif nMultiToolVehicles == 5 then 
+    elseif nMultiToolVehicles == 5 then
         return {-2,-1,0,1,2}
     end
     return {0}

scripts/ai/controllers/PlowController.lua

@@ -12,7 +12,7 @@ function PlowController:init(vehicle, implement)
     -- the plow was not rotated into the working position before lowering. If that's the case, onLowering will
     -- rotate it but it needs to know which direction to lower, which, however is not known at that point
     -- anymore
-    self.lastTurnIsLeftTurn = CpTemporaryObject()
+    self.lastPlowSide = CpTemporaryObject()
 end
 
 
@@ -115,38 +115,38 @@ end
 -- TODO: this whole magic hack would not be necessary if we moved the actual lowering into onTurnEndProgress()
 function PlowController:onLowering()
     -- if we just turned (that is, not starting to work)
-    local lastTurnIsLeftTurn = self.lastTurnIsLeftTurn:get()
-    if lastTurnIsLeftTurn ~= nil and
+    local lastPlowSide = self.lastPlowSide:get()
+    if lastPlowSide ~= nil and
             self:isRotatablePlow() and not self:isFullyRotated() and not self:isRotationActive() then
-        self:debug('Lowering, rotating plow to working position (last turn is left %s).', lastTurnIsLeftTurn)
+        self:debug('Lowering, rotating plow to working position (plow in last turn was left %s).', lastPlowSide)
         -- rotation direction depends on the direction of the last turn
-        self.implement:setRotationMax(lastTurnIsLeftTurn)
+        self.implement:setRotationMax(lastPlowSide)
     end
 end
 
 --- This is called in every loop when we approach the start of the row, the location where
---- the plow must be lowered. Currently AIDriveStrategyFieldworkCourse takes care of the lowering,
+--- the plow must be lowered. Currently the WorkStarter takes care of the lowering,
 --- here we only make sure that the plow is rotated to the work position (from the center position)
 --- in time.
 ---@param workStartNode number node where the work starts as calculated by TurnContext
 ---@param reversing boolean driving in reverse now
 ---@param shouldLower boolean the implement should be lowered as we are close to the work start (this
 --- should most likely be calculated here in the controller, but for now, we get it from an argument
----@param isLeftTurn boolean is this a left turn?
-function PlowController:onTurnEndProgress(workStartNode, reversing, shouldLower, isLeftTurn)
-    self.lastTurnIsLeftTurn:set(isLeftTurn or false, 2000)
+---@param shouldBeOnTheLeft boolean should the plow be turned to the left to be in the good position after the turn?
+function PlowController:onTurnEndProgress(workStartNode, reversing, shouldLower, shouldBeOnTheLeft)
+    self.lastPlowSide:set(shouldBeOnTheLeft or false, 2000)
     if self:isRotatablePlow() and not self:isFullyRotated() and not self:isRotationActive() then
         -- more or less aligned with the first waypoint of the row, start rotating to working position
         if CpMathUtil.isSameDirection(self.implement.rootNode, workStartNode, 30) or shouldLower then
             if self.towed then
                 -- let towed plows remain in the center position while reversing to the start of the row
                 if not reversing then
                     self:debug('Rotating towed plow to working position.')
-                    self.implement:setRotationMax(isLeftTurn)
+                    self.implement:setRotationMax(shouldBeOnTheLeft)
                 end
             else
                 self:debug('Rotating hitch-mounted plow to working position.')
-                self.implement:setRotationMax(isLeftTurn)
+                self.implement:setRotationMax(shouldBeOnTheLeft)
             end
         end
     end

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
@@ -150,49 +150,48 @@ end
 --- Initial plow rotation based on the ridge marker side selection by the course generator.
 function AIDriveStrategyPlowCourse:rotatePlows()
     self:debug('Starting work: check if plow needs to be turned.')
-    -- on the headland just check which side was worked last, on the center, check the direction of the next turn
-    -- as at the first pass both sides are unworked and need some other indication on which side the plow should be
     local ix = self.ppc:getCurrentWaypointIx()
-    local plowShouldBeOnTheLeft
-    if self.course:isOnHeadland(ix) then
-        local clockwise = self.course:isOnClockwiseHeadland(ix)
-        plowShouldBeOnTheLeft = not clockwise
-        self:debug('On a headland (clockwise %s), plow should be on the left %s', tostring(clockwise), tostring(plowShouldBeOnTheLeft))
-    else
-        local isNextTurnLeft = self.course:isNextTurnLeft(ix)
-        if isNextTurnLeft == nil then
-            -- don't know if left or right, so just use the last worked side
-            plowShouldBeOnTheLeft = self.course:isLeftSideWorked(ix)
-            self:debug('On the center, next turn direction unknown, plow should be on the left %s', tostring(plowShouldBeOnTheLeft))
-        else
-            plowShouldBeOnTheLeft = not isNextTurnLeft
-            self:debug('On the center, plow should be on the left %s', tostring(plowShouldBeOnTheLeft))
+    local plowShouldBeOnTheLeft = self.course:shouldPlowBeOnTheLeft(ix)
+    for _, controller in pairs(self.controllers) do
+        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.rotate then 
-            controller:rotate(plowShouldBeOnTheLeft)
+        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/AITurn.lua

@@ -1000,8 +1000,6 @@ function StartRowOnly:getDriveData()
                 TurnManeuver.LOWER_IMPLEMENT_AT_TURN_END) then
             self.state = self.states.APPROACHING_ROW
             self:debug('Approaching row')
-            self.driveStrategy:raiseControllerEvent(AIDriveStrategyCourse.onTurnEndProgressEvent,
-                    self:getLowerImplementNode(), self.ppc:isReversing(), true, not self.turnContext:isNextTurnLeft())
         end
     elseif self.state == self.states.APPROACHING_ROW then
         self.workStartHandler:lowerImplementsAsNeeded(self:getLowerImplementNode(), self.ppc:isReversing())

scripts/ai/turns/TurnContext.lua

@@ -89,7 +89,8 @@ 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.nextTurnLeft = course:isNextTurnLeft(turnEndIx)
+    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)
 end
@@ -238,10 +239,6 @@ function TurnContext:getHeadlandAngle()
     return math.abs(CpMathUtil.getDeltaAngle(math.rad(self.turnEndWp.angle), math.rad(self.turnStartWp.angle)))
 end
 
-function TurnContext:isNextTurnLeft()
-    return self.nextTurnLeft
-end
-
 function TurnContext:isHeadlandCorner()
 	-- in headland turns there's no significant direction change at the turn start waypoint, as the turn end waypoint
 	-- marks the actual corner. In a non-headland turn (usually 180) there is about 90 degrees direction change at
@@ -282,6 +279,11 @@ function TurnContext:isLeftTurn()
     end
 end
 
+--- Should the plow be rotated to the left after the turn?
+function TurnContext:shouldPlowBeOnTheLeft()
+    return self.plowShouldBeOnTheLeft
+end
+
 ---@return number offset of the turn end in meters forward (>0) or back (<0)
 function TurnContext:getTurnEndForwardOffset()
     return self.dz
@@ -363,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