6.03.00038 Bale unload fixes #5837 (#6798)

Check bale height when unloading bales to find the spot to
unload.

AIDriver.lua

@@ -2000,7 +2000,7 @@ function AIDriver:checkProximitySensor(maxSpeed, allowedToDrive, moveForwards)
 	local d, vehicle, range, deg, dAvg = math.huge, nil, 10, 0
 	local pack = moveForwards and self.forwardLookingProximitySensorPack or self.backwardLookingProximitySensorPack
 	if pack and self:isProximitySpeedControlEnabled() then
-		d, vehicle, deg, dAvg = pack:getClosestObjectDistanceAndRootVehicle()
+		d, vehicle, _, deg, dAvg = pack:getClosestObjectDistanceAndRootVehicle()
 		range = pack:getRange()
 	end
 

BaleLoaderAIDriver.lua

@@ -69,6 +69,12 @@ function BaleLoaderAIDriver:init(vehicle)
 
 	self:initStates(BaleLoaderAIDriver.myStates)
 	self.manualUnloadNode = WaypointNode(self.vehicle:getName() .. 'unloadNode')
+	if self.baleLoader.cp.realUnloadOrFillNode then
+		-- use that realUnloadOrFillNode for now as it includes the balerUnloadDistance config value
+		-- TODO: can we just use the back marker node here as well?
+		self.baleFinderProximitySensorPack = BackwardLookingProximitySensorPack(
+			self.vehicle, self.ppc, self.baleLoader.cp.realUnloadOrFillNode, 5, 1)
+	end
 	self:debug('Initialized, bale loader: %s', self.baleLoader:getName())
 end
 
@@ -95,16 +101,16 @@ function BaleLoaderAIDriver:driveUnloadOrRefill(dt)
 	local nearUnloadPoint, unloadPointIx = self.course:hasUnloadPointWithinDistance(self.ppc:getRelevantWaypointIx(), 50)
 	if self:hasTipTrigger() or nearUnloadPoint then
 		self:setSpeed(self.vehicle.cp.speeds.approach)
-		self:raycast()
+
 		if self:haveBales() and self.unloadRefillState == nil then
 			self.unloadRefillState = self.states.APPROACHING_UNLOAD_POINT
-			self.tooCloseToOtherBales = false
 			self:debug('Approaching unload point.')
+
 		elseif self:haveBales() and self.unloadRefillState == self.states.APPROACHING_UNLOAD_POINT then
 			local unloadNode = self:getUnloadNode(nearUnloadPoint, unloadPointIx)
 			local _, _, dz = localToLocal(unloadNode, self.baleLoader.cp.realUnloadOrFillNode, 0, 0, 0)
 			self:debugSparse('distance to unload point: %.1f', dz)
-			if math.abs(dz) < 1 or self.tooCloseToOtherBales then
+			if math.abs(dz) < 1 or self:tooCloseToOtherBales() then
 				self:debug('Unload point reached.')
 				self.unloadRefillState = self.states.UNLOADING
 			end
@@ -198,24 +204,22 @@ function BaleLoaderAIDriver:getUnloadNode(isUnloadpoint, unloadPointIx)
 	end
 end
 
---- Raycast back to stop when there's already a stack of bales at the unload point.
-function BaleLoaderAIDriver:raycast()
-	if not self.baleLoader.cp.realUnloadOrFillNode then return end
-	local nx, ny, nz = localDirectionToWorld(self.baleLoader.cp.realUnloadOrFillNode, 0, 0, -1)
-	local x, y, z = localToWorld(self.baleLoader.cp.realUnloadOrFillNode, 0, 0, 0)
-	raycastClosest(x, y, z, nx, ny, nz, 'raycastCallback', 5, self)
-end
-
-function BaleLoaderAIDriver:raycastCallback(hitObjectId, x, y, z, distance, nx, ny, nz, subShapeIndex)
-	if hitObjectId ~= 0 then
-		local object = g_currentMission:getNodeObject(hitObjectId)
+--- When backing up to unload, see if we have to stop because there are bales already there
+function BaleLoaderAIDriver:tooCloseToOtherBales()
+	if self.baleFinderProximitySensorPack then
+		local distance, _, object = self.baleFinderProximitySensorPack:getClosestObjectDistanceAndRootVehicle()
 		if object and object:isa(Bale) then
-			if distance < 2 then
-				self.tooCloseToOtherBales = true
-				self:debugSparse('Bale found at d=%.1f', distance)
+			-- round bales have diameter, others height, thanks Giants!
+			local baleHeight = object.baleDiameter and object.baleDiameter or
+				(object.baleHeight and object.baleHeight or 0.8)
+			self:debugSparse('Bale found at d=%.1f, bale height %.1f', distance, baleHeight)
+			if distance < (baleHeight + 1) then
+				return true
 			else
-				self.tooCloseToOtherBales = false
+				return false
 			end
 		end
+	else
+		return false
 	end
-end
+end

DevHelper.lua

@@ -126,11 +126,11 @@ end
 function DevHelper:updateProximitySensors(vehicle)
     if vehicle and vehicle.cp.driver then
         if vehicle.cp.driver.forwardLookingProximitySensorPack then
-            local d, otherVehicle, deg, dAvg =
+            local d, otherVehicle, object, deg, dAvg =
                 vehicle.cp.driver.forwardLookingProximitySensorPack:getClosestObjectDistanceAndRootVehicle()
         end
         if vehicle.cp.driver.backwardLookingProximitySensorPack then
-            local d, otherVehicle, deg, dAvg =
+            local d, otherVehicle, object, deg, dAvg =
                 vehicle.cp.driver.backwardLookingProximitySensorPack:getClosestObjectDistanceAndRootVehicle()
         end
     end

ProximitySensor.lua

@@ -91,6 +91,10 @@ function ProximitySensor:getClosestObjectDistance()
     return self.distanceOfClosestObject
 end
 
+function ProximitySensor:getClosestObject()
+	return g_currentMission:getNodeObject(self.objectId)
+end
+
 function ProximitySensor:getClosestRootVehicle()
     if self.objectId then
         local object = g_currentMission:getNodeObject(self.objectId)
@@ -226,16 +230,16 @@ function ProximitySensorPack:setIgnoredVehicle(vehicle, ttlMs)
     self:callForAllSensors(ProximitySensor.setIgnoredVehicle, vehicle, ttlMs)
 end
 
---- @return number, table, number distance of closest object in meters, root vehicle of the closest object, average direction
---- of the obstacle in degrees, > 0 right, < 0 left
+--- @return number, table, number distance of closest object in meters, root vehicle of the closest object,
+--- the closest object, average direction of the obstacle in degrees, > 0 right, < 0 left
 function ProximitySensorPack:getClosestObjectDistanceAndRootVehicle(deg)
     -- make sure we have the latest info, the sensors will make sure they only raycast once per loop
     self:update()
     if deg and self.sensors[deg] then
         return self.sensors[deg]:getClosestObjectDistance(), self.sensors[deg]:getClosestRootVehicle(), deg
     else
         local closestDistance = math.huge
-        local closestRootVehicle
+        local closestRootVehicle, closestObject
         -- weighted average over the different direction, weight depends on how close the closest object is
         local totalWeight, totalDegs, totalDistance = 0, 0, 0
         for _, deg in ipairs(self.directionsDeg) do
@@ -251,12 +255,13 @@ function ProximitySensorPack:getClosestObjectDistanceAndRootVehicle(deg)
             if d < closestDistance then
                 closestDistance = d
                 closestRootVehicle = self.sensors[deg]:getClosestRootVehicle()
+                closestObject = self.sensors[deg]:getClosestObject()
             end
         end
         if closestRootVehicle == self.vehicle then
             self:adjustForwardPosition()
         end
-        return closestDistance, closestRootVehicle, totalDegs / totalWeight, totalDistance / totalWeight
+        return closestDistance, closestRootVehicle, closestObject, totalDegs / totalWeight, totalDistance / totalWeight
     end
     return math.huge, nil, deg
 end

modDesc.xml

@@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="utf-8" standalone="no" ?>
 <modDesc descVersion="47">
-	<version>6.03.00037</version>
+	<version>6.03.00038</version>
 	<author><![CDATA[Courseplay.devTeam]]></author>
 	<title><!-- en=English de=German fr=French es=Spanish ru=Russian pl=Polish it=Italian br=Brazilian-Portuguese cs=Chinese(Simplified) ct=Chinese(Traditional) cz=Czech nl=Netherlands hu=Hungary jp=Japanese kr=Korean pt=Portuguese ro=Romanian tr=Turkish -->
 		<en>CoursePlay SIX</en>