fix: harvester headland turn

Author: Peter Vaiko

config/VehicleConfigurations.xml

@@ -80,7 +80,7 @@ You can define the following custom settings:
 - lowerEarly: boolean
     Similar to the above, the default behavior when starting to work is lower the implement when the back of the
     work area reaches to row start/field edge. Setting this to true will make Courseplay lower the implement as
-    soon as the the front reaches the row start/field edge, avoiding unworked patches with irregular implement
+    soon as the front reaches the row start/field edge, avoiding unworked patches with irregular implement
     work areas such as a plow.
 
 - useVehicleSizeForMarkers: boolean

scripts/ai/turns/AITurn.lua

@@ -378,7 +378,6 @@ function KTurn:onWaypointPassed(ix, course)
 end
 
 function KTurn:startTurn()
-    AITurn.startTurn(self)
     self.state = self.states.FORWARD
 end
 
@@ -438,87 +437,6 @@ function KTurn:turn(dt)
     return gx, gz, moveForwards, maxSpeed
 end
 
---[[
-  Headland turn for combines:
-  1. drive forward to the field edge or the headland path edge
-  2. start turning forward
-  3. reverse straight and then align with the direction after the
-     corner while reversing
-  4. forward to the turn start to continue on headland
-]]
----@class CombineHeadlandTurn : AITurn
-CombineHeadlandTurn = CpObject(AITurn)
-
----@param driveStrategy AIDriveStrategyFieldWorkCourse
----@param turnContext TurnContext
-function CombineHeadlandTurn:init(vehicle, driveStrategy, ppc, proximityController, turnContext)
-    AITurn.init(self, vehicle, driveStrategy, ppc, proximityController, turnContext, 'CombineHeadlandTurn')
-    self:addState('FORWARD')
-    self:addState('REVERSE_STRAIGHT')
-    self:addState('REVERSE_ARC')
-    self.turningRadius = AIUtil.getTurningRadius(self.vehicle)
-    self.cornerAngleToTurn = turnContext:getCornerAngleToTurn()
-    -- half the turn angle but not less than 45
-    self.angleToTurnInReverse = math.max(math.pi / 4, math.abs(self.cornerAngleToTurn / 2))
-    self.dxToStartReverseTurn = self.turningRadius - math.abs(self.turningRadius - self.turningRadius * math.cos(self.cornerAngleToTurn))
-end
-
-function CombineHeadlandTurn:startTurn()
-    self.state = self.states.FORWARD
-    self:debug('Starting combine headland turn')
-end
-
-function CombineHeadlandTurn:onWaypointChange(ix, course)
-    -- nothing to do
-end
-
-function CombineHeadlandTurn:onWaypointPassed(ix, course)
-    -- nothing to do, especially because the row finishing course is still active in the PPC and we may
-    -- pass the last waypoint which causes the turn to end and return to field work
-end
-
-function CombineHeadlandTurn:turn(dt)
-    local gx, gz, moveForwards, maxSpeed = AITurn.turn(self)
-    local dx, _, dz = self.turnContext:getLocalPositionFromTurnEnd(self.vehicle:getAIDirectionNode())
-    local angleToTurnEnd = math.abs(self.turnContext:getAngleToTurnEndDirection(self.vehicle:getAIDirectionNode()))
-    if self.state == self.states.FORWARD then
-        maxSpeed = self:getForwardSpeed()
-        moveForwards = true
-        if angleToTurnEnd > self.angleToTurnInReverse then
-            --and not self.turnContext:isLateralDistanceLess(dx, self.dxToStartReverseTurn) then
-            -- full turn towards the turn end direction
-            gx, gz = self:getGoalPointForTurn(moveForwards, self.turnContext:isLeftTurn())
-        else
-            -- reverse until we can make turn to the turn end point
-            self.state = self.states.REVERSE_STRAIGHT
-            self:debug('Combine headland turn start reversing straight')
-        end
-
-    elseif self.state == self.states.REVERSE_STRAIGHT then
-        maxSpeed = self:getReverseSpeed()
-        moveForwards = false
-        gx, gz = self:getGoalPointForTurn(moveForwards, nil)
-        if math.abs(dx) < 0.2 then
-            self.state = self.states.REVERSE_ARC
-            self:debug('Combine headland turn start reversing arc')
-        end
-
-    elseif self.state == self.states.REVERSE_ARC then
-        maxSpeed = self:getReverseSpeed()
-        moveForwards = false
-        gx, gz = self:getGoalPointForTurn(moveForwards, not self.turnContext:isLeftTurn())
-        if angleToTurnEnd < math.rad(20) then
-            self.state = self.states.ENDING_TURN
-            self:debug('Combine headland turn forwarding again')
-            -- lower implements here unconditionally (regardless of the direction, self:endTurn() would wait until we
-            -- are pointing to the turn target direction)
-            self.driveStrategy:lowerImplements()
-            self:resumeFieldworkAfterTurn(self.turnContext.turnEndWpIx)
-        end
-    end
-    return gx, gz, moveForwards, maxSpeed
-end
-
 --[[
 A turn maneuver following a course (waypoints created by turn.lua)
 ]]
@@ -586,7 +504,6 @@ end
 --      = if turn on field setting is off, use pathfinder turns if enabled in settings, calculated turns otherwise
 --
 function CourseTurn:startTurn()
-    AITurn.startTurn(self)
     local canTurnOnField = AITurn.canTurnOnField(self.turnContext, self.vehicle, self.workWidth, self.turningRadius)
     if self.turnContext:isHeadlandCorner() then
         self:debug('Starting a headland corner turn')
@@ -750,7 +667,7 @@ function CourseTurn:generateCalculatedTurn()
             self.enableTightTurnOffset = true
         else
             turnManeuver = HeadlandCornerTurnManeuver(self.vehicle, self.turnContext, self.vehicle:getAIDirectionNode(),
-                self.turningRadius, self.workWidth, self.reversingImplement, self.steeringLength)
+                    self.turningRadius, self.workWidth, self.reversingImplement, self.steeringLength)
             -- adjust turn course for tight turns only for headland corners by default
             self.forceTightTurnOffset = self.steeringLength > 0
         end
@@ -826,6 +743,32 @@ function CourseTurn:drawDebug()
     end
 end
 
+--[[
+  Headland turn for combines:
+  1. drive forward to the field edge or the headland path edge
+  2. start turning forward
+  3. reverse straight and then align with the direction after the
+     corner while reversing
+  4. forward to the turn start to continue on headland
+]]
+---@class CombineHeadlandTurn : CourseTurn
+CombineHeadlandTurn = CpObject(CourseTurn)
+
+function CombineHeadlandTurn:init(vehicle, driveStrategy, ppc, proximityController, turnContext, fieldWorkCourse,
+                                  workWidth, name)
+    CourseTurn.init(self, vehicle, driveStrategy, ppc, proximityController, turnContext, fieldWorkCourse,
+            workWidth, name or 'CombineHeadlandTurn')
+end
+
+function CombineHeadlandTurn:startTurn()
+    self:debug('Starting a combine headland turn')
+    self.turnCourse = ReedsSheppHeadlandTurn(self.vehicle, self.turnContext,
+            self.turnContext.vehicleAtTurnStartNode, self.turningRadius):getCourse()
+    self.state = self.states.TURNING
+    self.ppc:setCourse(self.turnCourse)
+    self.ppc:initialize(1)
+end
+
 --- A turn maneuver to recover when the vehicle is blocked by an object (tree, fence, etc) during the turn
 --- This should only be initiated when the state is TURNING, so after the row is finished and before starting
 --- the new row.

scripts/ai/turns/TurnManeuver.lua

@@ -434,7 +434,7 @@ function AnalyticTurnManeuver:getDistanceToMoveBack(course, workWidth, distanceT
     -- the field, not only the center
     local headlandAngle = self.turnContext:getHeadlandAngle()
     distanceToFieldEdge = distanceToFieldEdge -
-    -- exclude very sharp headland angles to prevent moving back ridiculously far
+            -- exclude very sharp headland angles to prevent moving back ridiculously far
             ((headlandAngle > math.deg(10) and headlandAngle < math.deg(170))
                     and (workWidth / 2 / math.abs(math.tan(headlandAngle))) or 0)
     self:debug('dzMax=%.1f, workWidth=%.1f, spaceNeeded=%.1f, turnEndForwardOffset=%.1f, headlandAngle=%.1f, distanceToFieldEdge=%.1f', dzMax, workWidth,
@@ -465,7 +465,7 @@ end
 ---@class LoopTurnManeuver : TurnManeuver
 LoopTurnManeuver = CpObject(DubinsTurnManeuver)
 function LoopTurnManeuver:init(vehicle, turnContext, vehicleDirectionNode, turningRadius,
-                         workWidth, steeringLength)
+                               workWidth, steeringLength)
     self.debugPrefix = '(LoopTurn): '
     TurnManeuver.init(self, vehicle, turnContext, vehicleDirectionNode, turningRadius,
             workWidth, steeringLength)
@@ -552,6 +552,21 @@ function ReedsSheppTurnManeuver:findAnalyticPath(vehicleDirectionNode, startXOff
     return course
 end
 
+---@class ReedsSheppHeadlandTurn : TurnManeuver
+ReedsSheppHeadlandTurn = CpObject(TurnManeuver)
+
+--- This is a headland turn (~90 degrees) for non-towed harvesters with cutter on the front. Expected to be called
+--- just after the cutter finished the corner, that is, the harvester should drive forward in the original direction
+--- until there is no fruit left. It'll then do a quick 90 degree 3 point turn to align with the new direction.
+function ReedsSheppHeadlandTurn:init(vehicle, turnContext, vehicleDirectionNode, turningRadius)
+    local solver = ReedsSheppSolver()
+    -- use lateWorkStartNode since we covered the corner in the inbound direction already
+    local path = PathfinderUtil.findAnalyticPath(solver, vehicleDirectionNode, 0, 0,
+            turnContext.lateWorkStartNode, 0, -turnContext.backMarkerDistance, turningRadius)
+    self.course = Course.createFromAnalyticPath(vehicle, path, true)
+    self.course:adjustForReversing(2)
+end
+
 ---@class TurnEndingManeuver : TurnManeuver
 TurnEndingManeuver = CpObject(TurnManeuver)