Merge pull request #846 from Courseplay/786-oxbo-pea-harvester-headland-turn

786 oxbo pea harvester headland turn

Author: Tensuko

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
@@ -163,6 +163,13 @@ You can define the following custom settings:
     Forced tip side index for unloading. For now only for an auger wagon.
     As an example the Hawe SUW 5000 has two tipside, but only the one with the pipe is needed. 
 
+- disablePocket: boolean
+    Disables creating a pocket for headland turns. Some harvesters, like mostly potato and other root vegetable
+    harvesters, are not good at making a pocket at the headland corner as they are very long but have a small
+    working width. We automatically try to disable pocket for most of the root vegetable harvesters, but some,
+    like the Oxbo, are not detected correctly. This setting can be used to disable the pocket creation for these
+    harvesters.
+
 -->
 <VehicleConfigurations>
 	<Configurations>
@@ -195,6 +202,7 @@ You can define the following custom settings:
         <Configuration type="BOOL">openPipeEarly</Configuration>
         <Configuration type="BOOL">closePipeAfterUnload</Configuration>
         <Configuration type="INT">tipSideIndex</Configuration>
+        <Configuration type="BOOL">disablePocket</Configuration>
 	</Configurations>
     <!--[GIANTS]-->
 
@@ -229,12 +237,15 @@ You can define the following custom settings:
     <!--\vehicles\oxbo-->
     <Vehicle name="mkb4TR.xml"
              closePipeAfterUnload = "true"
+             disablePocket = "true"
     />
     <Vehicle name="bp2140e.xml"
              closePipeAfterUnload = "true"
+             disablePocket = "true"
     />
     <Vehicle name="epd540E.xml"
              closePipeAfterUnload = "true"
+             disablePocket = "true"
     />
 
     <!--vehicles\gregoire-->

scripts/Course.lua

@@ -648,6 +648,10 @@ function Course:isLastWaypointIx(ix)
     return #self.waypoints == ix
 end
 
+function Course:endsInReverse()
+    return self:isReverseAt(self:getNumberOfWaypoints())
+end
+
 function Course:print()
     for i = 1, #self.waypoints do
         local p = self.waypoints[i]
@@ -915,13 +919,13 @@ function Course:extend(length, dx, dz)
     for i = first, last, step do
         local x = lastWp.x + dx * i
         local z = lastWp.z + dz * i
-        self:appendWaypoint({ x = x, z = z })
+        self:appendWaypoint({ x = x, z = z, rev = lastWp.rev })
     end
     if length % step > 0 then
         -- add the remainder to make sure we extend all the way up to length
         local x = lastWp.x + dx * length
         local z = lastWp.z + dz * length
-        self:appendWaypoint({ x = x, z = z })
+        self:appendWaypoint({ x = x, z = z, rev = lastWp.rev })
     end
     -- enrich the waypoints we added
     self:enrichWaypointData(nWaypoints)

scripts/CpUtil.lua

@@ -466,7 +466,7 @@ function CpUtil.getAllRootVegetables()
         local preparedGrowthState = fruitTypeData.preparedGrowthState
         local name = fruitTypeData.name
 
-        -- check if fruit is needs herb removement to be harvested
+        -- check if fruit is needs herb removal to be harvested
         if minPreparingGrowthState ~= -1 and preparedGrowthState ~= -1 and name ~= "SUGARCANE" then
 			local fruitType = g_fruitTypeManager:getFruitTypeByName(name)
             if fruitType ~= nil then

scripts/ai/strategies/AIDriveStrategyCombineCourse.lua

@@ -1412,17 +1412,12 @@ function AIDriveStrategyCombineCourse:startTurn(ix)
         if self.combineController:isTowed() then
             self:debug('Headland turn but this is a towed harvester using normal turn maneuvers.')
             AIDriveStrategyFieldWorkCourse.startTurn(self, ix)
-            -- The type of fruit being harvested isn't really the indicator if we can make a headland turn
-            -- TODO: either make disabling combine headland turns configurable, or
-            -- TODO: decide automatically based on the vehicle's properties, like turn radius, work width, etc.
-            -- and disable when such a turn does not make sense for the vehicle.
-        elseif self.combineController:isRootVegetableHarvester() then
-            self:debug('Headland turn but this harvester uses normal turn maneuvers.')
-            AIDriveStrategyFieldWorkCourse.startTurn(self, ix)
         elseif self.course:isOnConnectingPath(ix) then
             self:debug('Headland turn but this a connecting track, use normal turn maneuvers.')
             AIDriveStrategyFieldWorkCourse.startTurn(self, ix)
-        elseif self.course:isOnOutermostHeadland(ix) and self:isTurnOnFieldActive() then
+        elseif self.course:isOnOutermostHeadland(ix) and self:isTurnOnFieldActive() and
+                not self.combineController:isRootVegetableHarvester() and
+                not g_vehicleConfigurations:getRecursively(self.vehicle, 'disablePocket') then
             self:debug('Creating a pocket in the corner so the combine stays on the field during the turn')
             self.aiTurn = CombinePocketHeadlandTurn(self.vehicle, self, self.ppc, self.proximityController, self.turnContext,
                     self.course, self:getWorkWidth())

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 = ReedsSheppHeadlandTurnManeuver(self.vehicle, self.turnContext,
+            self.vehicle:getAIDirectionNode(), 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/TurnContext.lua

@@ -367,19 +367,6 @@ function TurnContext:createCorner(vehicle, r)
             vehicle:getCpSettings().toolOffsetX:getValue())
 end
 
---- Course to reverse before starting a turn to make sure the turn is completely on the field
---- @param vehicle table
---- @param reverseDistance number distance to reverse in meters
-function TurnContext:createReverseWaypointsBeforeStartingTurn(vehicle, reverseDistance)
-    local reverserNode = AIUtil.getReverserNode(vehicle)
-    local _, _, dStart = localToLocal(reverserNode or vehicle:getAIDirectionNode(), self.workEndNode, 0, 0, 0)
-    local waypoints = {}
-    for d = dStart, dStart - reverseDistance - 1, -1 do
-        local x, y, z = localToWorld(self.workEndNode, 0, 0, d)
-        table.insert(waypoints, {x = x, y = y, z = z, rev = true})
-    end
-    return waypoints
-end
 
 --- Course to end a pathfinder turn, a straight line from where pathfinder ended, into to next row,
 --- making sure it is long enough so the vehicle reaches the point to lower the implements on this course

scripts/ai/turns/TurnManeuver.lua

@@ -247,14 +247,14 @@ end
 --- Get the distance between the direction node of the vehicle and the reverser node (if there is one). This
 --- is to make sure that when the course changes to reverse and there is a reverse node, the first reverse
 --- waypoint is behind the reverser node. Otherwise we'll just keep backing up until the emergency brake is triggered.
-function TurnManeuver:getReversingOffset()
-    local reverserNode, debugText = AIUtil.getReverserNode(self.vehicle)
+---@return number|nil distance in meters to the reverser node, or nil if there is no reverser node
+function TurnManeuver:getReversingOffset(vehicle, vehicleDirectionNode)
+    local reverserNode, debugText = AIUtil.getReverserNode(vehicle)
     if reverserNode then
-        local _, _, dz = localToLocal(reverserNode, self.vehicleDirectionNode, 0, 0, 0)
+        local _, _, dz = localToLocal(reverserNode, vehicleDirectionNode, 0, 0, 0)
         self:debug('Using reverser node (%s) distance %.1f', debugText, dz)
         return math.abs(dz)
     end
-    return self.steeringLength
 end
 
 --- Set implement lowering control for the end of the turn
@@ -274,7 +274,7 @@ end
 function TurnManeuver:adjustCourseToFitField(course, dBack, ixBeforeEndingTurnSection)
     self:debug('moving course back: d=%.1f', dBack)
     local endingTurnLength
-    local reversingOffset = self:getReversingOffset()
+    local reversingOffset = self:getReversingOffset(self.vehicle, self.vehicleDirectionNode) or self.steeringLength
     -- generate a straight reverse section first (less than 1 m step should make sure we always end up with
     -- at least two waypoints
     local courseWithReversing = Course.createFromNode(self.vehicle, self.vehicle:getAIDirectionNode(),
@@ -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,31 @@ function ReedsSheppTurnManeuver:findAnalyticPath(vehicleDirectionNode, startXOff
     return course
 end
 
+---@class ReedsSheppHeadlandTurnManeuver : TurnManeuver
+ReedsSheppHeadlandTurnManeuver = 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 ReedsSheppHeadlandTurnManeuver:init(vehicle, turnContext, vehicleDirectionNode, turningRadius)
+    self.vehicle = vehicle
+    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:adjustForTowedImplements(2)
+    if self.course:endsInReverse() then
+        -- add a little straight section to the end so we have a little buffer and don't end the turn right at
+        -- the work start
+        local reversingOffset = (self:getReversingOffset(vehicle, vehicleDirectionNode) or 4)
+        self:debug('Extending course by %.1f m', reversingOffset)
+        self.course:extend( reversingOffset + 2, -turnContext.turnEndWp.dx, -turnContext.turnEndWp.dz)
+    end
+    local endingTurnLength = turnContext:appendEndingTurnCourse(self.course, 0)
+    TurnManeuver.setLowerImplements(self.course, endingTurnLength, true)
+end
+
 ---@class TurnEndingManeuver : TurnManeuver
 TurnEndingManeuver = CpObject(TurnManeuver)