Merge pull request #955 from Courseplay/940-harvester-back-measurement

fix: more robust harvester back measurement

Author: Tensuko

scripts/CpUtil.lua

@@ -492,6 +492,10 @@ function CpUtil.getDefaultCollisionFlags()
 		CollisionFlag.VEHICLE + CollisionFlag.BUILDING + CollisionFlag.STATIC_OBJECT
 end
 
+function CpUtil.getVehicleCollisionFlags()
+	return CollisionFlag.VEHICLE
+end
+
 --- Removes all event listeners of a given Spec Class.
 function CpUtil.removeEventListenersBySpecialization(vehicle, specClass)
 	for _, listeners in pairs(vehicle.eventListeners) do

scripts/ai/Markers.lua

@@ -45,7 +45,7 @@ local function setBackMarkerNode(vehicle, measuredBackDistance)
         lastImplement, backMarkerOffset = AIUtil.getLastAttachedImplement(vehicle)
         referenceNode = AIUtil.getDirectionNode(vehicle)
         CpUtil.debugVehicle(CpDebug.DBG_IMPLEMENTS, vehicle, 'Using the last implement\'s rear distance for the back marker node, %d m from root node', backMarkerOffset)
-    elseif measuredBackDistance then
+    elseif measuredBackDistance and measuredBackDistance ~= 0 then
         referenceNode = AIUtil.getDirectionNode(vehicle)
         backMarkerOffset = -measuredBackDistance
         CpUtil.debugVehicle(CpDebug.DBG_IMPLEMENTS, vehicle, 'back marker node on measured back distance %.1f', measuredBackDistance)

scripts/ai/parameters/AIParameterSettingList.lua

@@ -394,7 +394,7 @@ end
 ---@return boolean value is not valid and could not be set.
 function AIParameterSettingList:setFloatValue(value, epsilon, noEventSend)
 	local failed = setValueInternal(self, value, function(a, b)
-		local epsilon = epsilon or self.data.incremental or 0.1
+		epsilon = epsilon or self.data.incremental or 0.1
 		if a == nil or b == nil then return false end
 		return a > b - epsilon / 2 and a <= b + epsilon / 2 end)
 	if not failed and not noEventSend then
@@ -450,7 +450,7 @@ function AIParameterSettingList:setDefault(noEventSend)
 				local value = g_vehicleConfigurations:get(object, configName)
 				if value then 
 					if tonumber(value) then 
-						self:setFloatValue(value, noEventSend)
+						self:setFloatValue(value, nil, noEventSend)
 					else
 						self:setValue(value, noEventSend)
 					end
@@ -462,7 +462,7 @@ function AIParameterSettingList:setDefault(noEventSend)
 	end
 	--- If default values were setup use these.
 	if self.data.default ~=nil then
-		AIParameterSettingList.setFloatValue(self, self.data.default, noEventSend)
+		AIParameterSettingList.setFloatValue(self, self.data.default, nil, noEventSend)
 		self:debug("set to default %s", self.data.default)
 		return
 	end

scripts/ai/strategies/AIDriveStrategyCombineCourse.lua

@@ -234,6 +234,7 @@ end
 
 function AIDriveStrategyCombineCourse:getDriveData(dt, vX, vY, vZ)
     self:handlePipe(dt)
+    self:measureBackDistance()
     Markers.refreshMarkerNodes(self.vehicle, self.measuredBackDistance)
     if self.temporaryHold:get() then
         self:setMaxSpeed(0)
@@ -2058,34 +2059,48 @@ end
 --- Proximity
 ------------------------------------------------------------------------------------------------------------------------
 
-AIDriveStrategyCombineCourse.maxBackDistance = 10
+AIDriveStrategyCombineCourse.maxBackDistance = 20
 
 function AIDriveStrategyCombineCourse:getMeasuredBackDistance()
     return self.measuredBackDistance
 end
 
 --- Determine how far the back of the combine is from the direction node
--- TODO: attached/towed harvesters
 function AIDriveStrategyCombineCourse:measureBackDistance()
-    self.measuredBackDistance = 0
-    -- raycast from a point behind the vehicle forward towards the direction node
-    local nx, ny, nz = localDirectionToWorld(AIUtil.getDirectionNode(self.vehicle), 0, 0, 1)
-    local x, y, z = localToWorld(AIUtil.getDirectionNode(self.vehicle), 0, 1.5, -self.maxBackDistance)
-    raycastAll(x, y, z, nx, ny, nz, self.maxBackDistance, 'raycastBackCallback', self)
+    -- check and adjust every 10 seconds, mainly for the towed harvesters which may be at an angle when
+    -- the driver starts and our raycast may miss it.
+    if not self.lastBackMeasurementDue then
+        self.lastBackMeasurementDue = CpTemporaryObject(true)
+    end
+    if not self.lastBackMeasurementDue:get() then
+        return
+    end
+    self.lastBackMeasurementDue:set(false, 10000)
+    self.measuredBackDistance = self.measuredBackDistance or 0
+    local previousMeasuredBackDistance = self.measuredBackDistance
+    -- raycast from a point behind the vehicle forward towards the direction node, in different heights as harvesters
+    -- can be very tricky, such as the Oxbo, which we'll get no hits at all at exactly 1.5 meters...
+    for height = 1.3, 2.5, 0.2 do
+        local nx, ny, nz = localDirectionToWorld(AIUtil.getDirectionNode(self.vehicle), 0, 0, 1)
+        -- use the tool offset, that should cover offset towed harvesters as well
+        local x, y, z = localToWorld(AIUtil.getDirectionNode(self.vehicle),
+                self.settings.toolOffsetX:getValue(), height, -self.maxBackDistance)
+        raycastAll(x, y, z, nx, ny, nz, self.maxBackDistance, 'raycastBackCallback', self, CpUtil.getVehicleCollisionFlags())
+    end
+    if previousMeasuredBackDistance ~= self.measuredBackDistance then
+        self:debug('Measured back distance changed from %.1f to %.1f m', previousMeasuredBackDistance, self.measuredBackDistance)
+    end
 end
 
 -- I believe this tries to figure out how far the back of a combine is from its direction node.
 function AIDriveStrategyCombineCourse:raycastBackCallback(hitObjectId, x, y, z, distance, nx, ny, nz, subShapeIndex)
     if hitObjectId ~= 0 then
         local object = g_currentMission:getNodeObject(hitObjectId)
-        if object and object == self.vehicle then
+        if object and object.getRootVehicle and object:getRootVehicle() == self.vehicle then
             local d = self.maxBackDistance - distance
             if d > self.measuredBackDistance then
                 self.measuredBackDistance = d
-                self:debug('Measured back distance is %.1f m', self.measuredBackDistance)
             end
-        else
-            return true
         end
     end
 end

scripts/courseGenerator/Headland.lua

@@ -190,7 +190,7 @@ function Headland:connectTo(other, ix, workingWidth, turningRadius, headlandFirs
                         length, transitionEndIx, i)
                 return transitionEndIx
             else
-                self.logger:warning('Generated path to next headland too long (%.1f > %.1f), try %d.',
+                self.logger:debug('Generated path to next headland too long (%.1f > %.1f), try %d.',
                         length, maxPlausiblePathLength, i)
             end
             if 0.8 * radius > turningRadius then