Merge pull request #2764 from Courseplay/reverse-with-offset

fix: reversing with offset tools

config/VehicleConfigurations.xml

@@ -185,7 +185,6 @@ You can define the following custom settings:
     <!--\vehicles\ropa-->
     <Vehicle name="keiler2.xml"
              toolOffsetX = "-2.0"
-             noReverse = "true"
     />
 	<!--\vehicles\ropa-->
     <Vehicle name="nawaRoMaus.xml"
@@ -228,12 +227,10 @@ You can define the following custom settings:
     />
     <!--\vehicles\lemken-->
     <Vehicle name="titan11.xml"
-             noReverse = "true"
              turnRadius = "7.5"
              implementWheelAlwaysOnGround = "true"
 	/>
     <Vehicle name="titan18.xml"
-             noReverse="true"
              turnRadius = "7.5"
              implementWheelAlwaysOnGround = "true"
              raiseLate = "true"
@@ -252,7 +249,6 @@ You can define the following custom settings:
              noReverse = "false"
              turnRadius = "9"
     />
-    <!--vehicles\seedHawk-->
     <Vehicle name="980AirCart.xml"
              noReverse = "true"
     />
@@ -406,7 +402,6 @@ You can define the following custom settings:
              turnRadius = "7.5"
              implementWheelAlwaysOnGround = "true"
 			 workingWidth = "10.5"
-             noReverse = "true"
     />
     <!--Mod: KOECKERLING Vector 460-->
 	<Vehicle name="vector460.xml"

scripts/ai/AIDriveStrategyFieldWorkCourse.lua

@@ -339,18 +339,18 @@ function AIDriveStrategyFieldWorkCourse:shouldLowerImplements(turnEndNode, rever
 end
 
 ---@param object table is a vehicle or implement object with AI markers (marking the working area of the implement)
----@param turnEndNode number node at the first waypoint of the row, pointing in the direction of travel. This is where
+---@param workStartNode number node at the first waypoint of the row, pointing in the direction of travel. This is where
 --- the implement should be in the working position after a turn
 ---@param reversing boolean are we reversing? When reversing towards the turn end point, we must lower the implements
 --- when we are _behind_ the turn end node (dz < 0), otherwise once we reach it (dz > 0)
 ---@return boolean, boolean, number the second one is true when the first is valid, and the distance to the work start
 --- in meters (<0) when driving forward, nil when driving backwards.
-function AIDriveStrategyFieldWorkCourse:shouldLowerThisImplement(object, turnEndNode, reversing)
+function AIDriveStrategyFieldWorkCourse:shouldLowerThisImplement(object, workStartNode, reversing)
     local aiLeftMarker, aiRightMarker, aiBackMarker = WorkWidthUtil.getAIMarkers(object, true)
     if not aiLeftMarker then return false, false, nil end
-    local dxLeft, _, dzLeft = localToLocal(aiLeftMarker, turnEndNode, 0, 0, 0)
-    local dxRight, _, dzRight = localToLocal(aiRightMarker, turnEndNode, 0, 0, 0)
-    local dxBack, _, dzBack = localToLocal(aiBackMarker, turnEndNode, 0, 0, 0)
+    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)
     local loweringDistance
     if AIUtil.hasAIImplementWithSpecialization(self.vehicle, SowingMachine) then
         -- sowing machines are stopped while lowering, but leave a little reserve to allow for stopping
@@ -362,10 +362,14 @@ function AIDriveStrategyFieldWorkCourse:shouldLowerThisImplement(object, turnEnd
         loweringDistance = math.min(self.vehicle.lastSpeed, self.settings.turnSpeed:getValue() / 3600) *
                 self.loweringDurationMs + 0.5 -- vehicle.lastSpeed is in meters per millisecond
     end
-    local dzFront = (dzLeft + dzRight) / 2
+    local aligned = CpMathUtil.isSameDirection(object.rootNode, workStartNode, 15)
+    -- some implements, especially plows may have the left and right markers offset longitudinally
+    -- so if the implement is aligned with the row direction already, then just take the front one
+    -- if not aligned, work with an average
+    local dzFront = aligned and math.max(dzLeft, dzRight) or (dzLeft + dzRight) / 2
     local dxFront = (dxLeft + dxRight) / 2
-    self:debug('%s: dzLeft = %.1f, dzRight = %.1f, dzFront = %.1f, dxFront = %.1f, dzBack = %.1f, loweringDistance = %.1f, reversing %s',
-            CpUtil.getName(object), dzLeft, dzRight, dzFront, dxFront, dzBack, loweringDistance, tostring(reversing))
+    self:debug('%s: dzLeft = %.1f, dzRight = %.1f, aligned = %s, dzFront = %.1f, dxFront = %.1f, dzBack = %.1f, loweringDistance = %.1f, reversing %s',
+            CpUtil.getName(object), dzLeft, dzRight, aligned, dzFront, dxFront, dzBack, loweringDistance, tostring(reversing))
     local dz = self:getImplementLowerEarly() and dzFront or dzBack
     if reversing then
         return dz < 0, true, nil
@@ -397,7 +401,7 @@ end
 function AIDriveStrategyFieldWorkCourse:isThisImplementAligned(object, node)
     local aiFrontMarker, _, _ = WorkWidthUtil.getAIMarkers(object, true)
     if not aiFrontMarker then return true end
-    return CpMathUtil.isSameDirection(aiFrontMarker, node, 2)
+    return CpMathUtil.isSameDirection(aiFrontMarker, node, 5)
 end
 
 -----------------------------------------------------------------------------------------------------------------------

scripts/ai/AIDriveStrategyPlowCourse.lua

@@ -40,6 +40,9 @@ function AIDriveStrategyPlowCourse.new(customMt)
     local self = AIDriveStrategyFieldWorkCourse.new(customMt)
     AIDriveStrategyFieldWorkCourse.initStates(self, AIDriveStrategyPlowCourse.myStates)
     self.debugChannel = CpDebug.DBG_FIELDWORK
+    -- the plow offset is automatically calculated on each waypoint and if it wasn't calculated for a while
+    -- or when some event (like a turn) invalidated it
+    self.plowOffsetUnknown = CpTemporaryObject(true)
     return self
 end
 
@@ -86,6 +89,9 @@ function AIDriveStrategyPlowCourse:getDriveData(dt, vX, vY, vZ)
             self:debug('Plow is unfolded and ready to start')
         end
     end
+    if self.plowOffsetUnknown:get() then
+        self:updatePlowOffset()
+    end
     return AIDriveStrategyFieldWorkCourse.getDriveData(self, dt, vX, vY, vZ)
 end
 
@@ -104,15 +110,23 @@ end
 function AIDriveStrategyPlowCourse:updatePlowOffset()
     local xOffset = 0
     for _, controller in pairs(self.controllers) do 
-        if controller.getAutomaticXOffset then 
-            xOffset = xOffset + controller:getAutomaticXOffset()
+        if controller.getAutomaticXOffset then
+            local autoOffset = controller:getAutomaticXOffset()
+            if autoOffset == nil then
+                self:debugSparse('Plow offset can\'t be calculated now, leaving offset at %.2f', self.aiOffsetX)
+                return
+            end
+            xOffset = xOffset + autoOffset
         end
     end
     local oldOffset = self.aiOffsetX
     -- set to the average of old and new to smooth a little bit to avoid oscillations
-    self.aiOffsetX = (0.5 * self.aiOffsetX + 1.5 * xOffset) / 2
-    self:debug("Plow offset calculated was %.2f and it changed from %.2f to %.2f",
-        xOffset, oldOffset, self.aiOffsetX)
+    -- when we have a valid previous value
+    self.aiOffsetX = self.plowOffsetUnknown:get() and xOffset or ((0.5 * self.aiOffsetX + 1.5 * xOffset) / 2)
+    if math.abs(oldOffset - xOffset) > 0.05 then
+        self:debug("Plow offset calculated was %.2f and it changed from %.2f to %.2f", xOffset, oldOffset, self.aiOffsetX)
+    end
+    self.plowOffsetUnknown:set(false, 3000)
 end
 
 --- Is a plow currently rotating?
@@ -173,8 +187,6 @@ end
 --- When we return from a turn, the offset is reverted and should immediately set, not waiting
 --- for the first waypoint to pass as it is on the wrong side right after the turn
 function AIDriveStrategyPlowCourse:resumeFieldworkAfterTurn(ix)
-    -- call twice to trick the smoothing and reach the desired value sooner.
-    self:updatePlowOffset()
-    self:updatePlowOffset()
+    self.plowOffsetUnknown:reset()
     AIDriveStrategyPlowCourse.superClass().resumeFieldworkAfterTurn(self, ix)
 end