diff --git a/BaleCollectorAIDriver.lua b/BaleCollectorAIDriver.lua index e384ee479..abe69ec57 100644 --- a/BaleCollectorAIDriver.lua +++ b/BaleCollectorAIDriver.lua @@ -149,19 +149,32 @@ end ---@return BaleToCollect, number closest bale and its distance function BaleCollectorAIDriver:findClosestBale(bales) local closestBale, minDistance, ix = nil, math.huge + local invalidBales = 0 for i, bale in ipairs(bales) do - local _, _, _, d = bale:getPositionInfoFromNode(AIDriverUtil.getDirectionNode(self.vehicle)) - self:debug('%d. bale (%d) in %.1f m', i, bale:getId(), d) - if d < self.vehicle.cp.turnDiameter * 2 then - -- if it is really close, check the length of the Dubins path - -- as we may need to drive a loop first to get to it - d = self:getDubinsPathLengthToBale(bale) - self:debug(' Dubins length is %.1f m', d) - end - if d < minDistance then - closestBale = bale - minDistance = d - ix = i + if bale:isStillValid() then + local _, _, _, d = bale:getPositionInfoFromNode(AIDriverUtil.getDirectionNode(self.vehicle)) + self:debug('%d. bale (%d, %s) in %.1f m', i, bale:getId(), bale:getBaleObject(), d) + if d < self.vehicle.cp.turnDiameter * 2 then + -- if it is really close, check the length of the Dubins path + -- as we may need to drive a loop first to get to it + d = self:getDubinsPathLengthToBale(bale) + self:debug(' Dubins length is %.1f m', d) + end + if d < minDistance then + closestBale = bale + minDistance = d + ix = i + end + else + --- When a bale gets wrapped it changes its identity and the node becomes invalid. This can happen + --- when we pick up (and wrap) a bale other than the target bale, for example because there's another bale + --- in the grabber's way. That is now wrapped but our bale list does not know about it so let's rescan the field + self:debug('%d. bale (%d, %s) INVALID', i, bale:getId(), bale:getBaleObject()) + invalidBales = invalidBales + 1 + self:debug('Found an invalid bales, rescanning field', invalidBales) + self.bales = self:findBales(self.vehicle.cp.settings.baleCollectionField:get()) + -- return empty, next time this is called everything should be ok + return end end return closestBale, minDistance, ix @@ -209,11 +222,14 @@ function BaleCollectorAIDriver:startPathfindingToBale(bale) self:debug('Start pathfinding to next bale (%d), safe distance from bale %.1f, half vehicle width %.1f', bale:getId(), safeDistanceFromBale, halfVehicleWidth) local goal = self:getBaleTarget(bale) - local offset = Vector(0, safeDistanceFromBale + halfVehicleWidth + 0.2) + local configuredOffset = self:getConfiguredOffset() + local offset = Vector(0, safeDistanceFromBale + + (configuredOffset and configuredOffset or (halfVehicleWidth + 0.2))) goal:add(offset:rotate(goal.t)) local done, path, goalNodeInvalid self.pathfinder, done, path, goalNodeInvalid = - PathfinderUtil.startPathfindingFromVehicleToGoal(self.vehicle, goal, false, self.fieldId, {}) + PathfinderUtil.startPathfindingFromVehicleToGoal(self.vehicle, goal, false, self.fieldId, + {}, self.lastBale and {self.lastBale} or {}) if done then return self:onPathfindingDoneToNextBale(path, goalNodeInvalid) else @@ -272,8 +288,11 @@ function BaleCollectorAIDriver:isObstacleAhead() return true end end - -- then a more thorough check - local leftOk, rightOk, straightOk = PathfinderUtil.checkForObstaclesAhead(self.vehicle, self.turnRadius) + -- then a more thorough check, we want to ignore the last bale we worked on as that may lay around too close + -- to the baler. This happens for example to the Andersen bale wrapper. + self:debug('Check obstacles ahead, ignoring bale object %s', self.lastBale and self.lastBale or 'nil') + local leftOk, rightOk, straightOk = + PathfinderUtil.checkForObstaclesAhead(self.vehicle, self.turnRadius, self.lastBale and{self.lastBale}) -- if at least one is ok, we are good to go. return not (leftOk or rightOk or straightOk) end @@ -297,7 +316,7 @@ function BaleCollectorAIDriver:onLastWaypoint() self:debug('last waypoint on bale pickup reached, start collecting bales again') self:collectNextBale() elseif self.baleCollectingState == self.states.APPROACHING_BALE then - self:debug('looks like somehow missed a bale, rescanning field') + self:debug('looks like somehow we missed a bale, rescanning field') self.bales = self:findBales(self.vehicle.cp.settings.baleCollectionField:get()) self:collectNextBale() elseif self.baleCollectingState == self.states.REVERSING_AFTER_PATHFINDER_FAILURE then @@ -376,7 +395,8 @@ function BaleCollectorAIDriver:workOnBale() if self.baleWrapper then BaleWrapperAIDriver.handleBaleWrapper(self) if self.baleWrapper.spec_baleWrapper.baleWrapperState == BaleWrapper.STATE_NONE then - self:debug('Bale wrapped, moving on to the next') + self.lastBale = self.baleWrapper.spec_baleWrapper.lastDroppedBale + self:debug('Bale wrapped, moving on to the next, last dropped bale %s', self.lastBale) self:collectNextBale() end end @@ -386,6 +406,14 @@ function BaleCollectorAIDriver:calculateTightTurnOffset() self.tightTurnOffset = 0 end +function BaleCollectorAIDriver:getConfiguredOffset() + if self.baleLoader then + return g_vehicleConfigurations:get(self.baleLoader, 'baleCollectorOffset') + elseif self.baleWrapper then + return g_vehicleConfigurations:get(self.baleWrapper, 'baleCollectorOffset') + end +end + function BaleCollectorAIDriver:getFillLevel() local fillLevelInfo = {} self:getAllFillLevels(self.vehicle, fillLevelInfo) diff --git a/BaleLoaderAIDriver.lua b/BaleLoaderAIDriver.lua index f313930ac..d8fbffe9b 100644 --- a/BaleLoaderAIDriver.lua +++ b/BaleLoaderAIDriver.lua @@ -115,10 +115,9 @@ function BaleLoaderAIDriver:driveUnloadOrRefill(dt) self:debug('Approaching unload point.') elseif self:haveBales() and self.unloadRefillState == self.states.APPROACHING_UNLOAD_POINT then - local unloadNode = self:getUnloadNode(nearUnloadPoint, unloadPointIx) - dz = calcDistanceFrom(unloadNode, self.baleLoader.cp.realUnloadOrFillNode) - self:debugSparse('distance to unload point: %.1f', dz) - if math.abs(dz) < 1 or self:tooCloseToOtherBales() then + local d = self:getDistanceFromUnloadNode(nearUnloadPoint, unloadPointIx) + self:debugSparse('distance to unload point: %.1f', d) + if math.abs(d) < 1 or self:tooCloseToOtherBales() then self:debug('Unload point reached.') self.unloadRefillState = self.states.UNLOADING end @@ -203,13 +202,18 @@ function BaleLoaderAIDriver:getFillType() end --- Unload node is either an unload waypoint or an unload trigger -function BaleLoaderAIDriver:getUnloadNode(isUnloadpoint, unloadPointIx) +function BaleLoaderAIDriver:getDistanceFromUnloadNode(isUnloadpoint, unloadPointIx) if isUnloadpoint then self:debugSparse('manual unload point at ix = %d', unloadPointIx) self.manualUnloadNode:setToWaypoint(self.course, unloadPointIx) - return self.manualUnloadNode.node + -- don't use dz here as the course to the manual unload point as it is often on a reverse + -- section and other parts of the course may be very close to the unload point, triggering + -- this way too early + return calcDistanceFrom(self.manualUnloadNode.node, self.baleLoader.cp.realUnloadOrFillNode) else - return self.vehicle.cp.currentTipTrigger.triggerId + local _, _, d = localToLocal(self.vehicle.cp.currentTipTrigger.triggerId, + self.baleLoader.cp.realUnloadOrFillNode, 0, 0, 0) + return d end end diff --git a/BaleToCollect.lua b/BaleToCollect.lua index 61d9606bf..abcb92673 100644 --- a/BaleToCollect.lua +++ b/BaleToCollect.lua @@ -56,6 +56,10 @@ function BaleToCollect.isValidBale(object, baleWrapper) end end +function BaleToCollect:isStillValid() + return BaleToCollect.isValidBale(self.bale) +end + function BaleToCollect:isLoaded() return self.bale.mountObject end @@ -68,6 +72,14 @@ function BaleToCollect:getId() return self.bale.id end +function BaleToCollect:getBaleObjectId() + return NetworkUtil.getObjectId(self.bale) +end + +function BaleToCollect:getBaleObject() + return self.bale +end + function BaleToCollect:getPosition() return getWorldTranslation(self.bale.nodeId) end diff --git a/DevHelper.lua b/DevHelper.lua index 1f13f5992..835eef695 100644 --- a/DevHelper.lua +++ b/DevHelper.lua @@ -108,7 +108,7 @@ function DevHelper:overlapBoxCallback(transformId) text = 'vehicle' .. collidingObject:getName() else if collidingObject:isa(Bale) then - text = 'Bale' + text = 'Bale ' .. tostring(collidingObject) .. ' ' .. tostring(NetworkUtil.getObjectId(collidingObject)) else text = collidingObject.getName and collidingObject:getName() or 'N/A' end @@ -202,7 +202,7 @@ function DevHelper:startPathfinding() local start = State3D:copy(self.start) self.pathfinder, done, path = PathfinderUtil.startPathfindingFromVehicleToGoal(self.vehicle, self.goal, - false, self.fieldNumForPathfinding or 0, {}, 10) + false, self.fieldNumForPathfinding or 0, {}, {}, 10) end diff --git a/VehicleConfigurations.lua b/VehicleConfigurations.lua index 5380c2d3d..737ebc57e 100644 --- a/VehicleConfigurations.lua +++ b/VehicleConfigurations.lua @@ -34,9 +34,10 @@ VehicleConfigurations.attributes = { {name = 'turnRadius', getXmlFunction = getXMLFloat}, {name = 'workingWidth', getXmlFunction = getXMLFloat}, {name = 'balerUnloadDistance', getXmlFunction = getXMLFloat}, - {name = 'directionNodeToOffsetZ', getXmlFunction = getXMLFloat}, + {name = 'directionNodeOffsetZ', getXmlFunction = getXMLFloat}, {name = 'implementWheelAlwaysOnGround', getXmlFunction = getXMLBool}, - {name = 'ignoreCollisionBoxesWhenFolded', getXmlFunction = getXMLBool} + {name = 'ignoreCollisionBoxesWhenFolded', getXmlFunction = getXMLBool}, + {name = 'baleCollectorOffset', getXmlFunction = getXMLFloat}, } function VehicleConfigurations:init() diff --git a/config/VehicleConfigurations.xml b/config/VehicleConfigurations.xml index 5d3661b08..44e0f0269 100644 --- a/config/VehicleConfigurations.xml +++ b/config/VehicleConfigurations.xml @@ -61,6 +61,11 @@ You can define the following custom settings: For this scenario the collision box is useless when folded, so when ignoreCollisionBoxesOnStreet is true, Courseplay will not detect collisions for this vehicle when it is folded. +- baleCollectorOffset: number + Offset in meters to use in bale collector mode (Mode 7). This is the distance between the tractor's centerline + and the edge of the bale when the bale grabber is right where it should be to pick up the bale. + Courseplay will adjust this offset according to the bale's dimensions but you may want to add a little buffer. + --> @@ -170,6 +175,10 @@ You can define the following custom settings: + + + diff --git a/course-generator/PathfinderUtil.lua b/course-generator/PathfinderUtil.lua index 7adb0f27d..0c838d9e1 100644 --- a/course-generator/PathfinderUtil.lua +++ b/course-generator/PathfinderUtil.lua @@ -203,11 +203,12 @@ end --- Pathfinder context ---@class PathfinderUtil.Context PathfinderUtil.Context = CpObject() -function PathfinderUtil.Context:init(vehicle, vehiclesToIgnore) +function PathfinderUtil.Context:init(vehicle, vehiclesToIgnore, objectsToIgnore) self.vehicleData = PathfinderUtil.VehicleData(vehicle, true, 0.5) self.trailerHitchLength = AIDriverUtil.getTowBarLength(vehicle) self.turnRadius = vehicle.cp and vehicle.cp.driver and AIDriverUtil.getTurningRadius(vehicle) or 10 - self.vehiclesToIgnore = vehiclesToIgnore + self.vehiclesToIgnore = vehiclesToIgnore or {} + self.objectsToIgnore = objectsToIgnore or {} end --- Calculate the four corners of a rectangle around a node (for example the area covered by a vehicle) @@ -276,6 +277,10 @@ end function PathfinderUtil.CollisionDetector:overlapBoxCallback(transformId) local collidingObject = g_currentMission.nodeToObject[transformId] + if collidingObject and PathfinderUtil.elementOf(self.objectsToIgnore, collidingObject) then + -- an object we want to ignore + return + end if collidingObject and collidingObject.getRootVehicle then local rootVehicle = collidingObject:getRootVehicle() if rootVehicle == self.vehicleData.rootVehicle or @@ -292,13 +297,14 @@ function PathfinderUtil.CollisionDetector:overlapBoxCallback(transformId) text = text .. ' ' .. key end end - self.collidingShapesText = text + self.collidingShapesText = text self.collidingShapes = self.collidingShapes + 1 end end -function PathfinderUtil.CollisionDetector:findCollidingShapes(node, vehicleData, vehiclesToIgnore, log) +function PathfinderUtil.CollisionDetector:findCollidingShapes(node, vehicleData, vehiclesToIgnore, objectsToIgnore, log) self.vehiclesToIgnore = vehiclesToIgnore or {} + self.objectsToIgnore = objectsToIgnore or {} self.vehicleData = vehicleData -- the box for overlapBox() is symmetric, so if our root node is not in the middle of the vehicle rectangle, -- we have to translate it into the middle @@ -311,8 +317,8 @@ function PathfinderUtil.CollisionDetector:findCollidingShapes(node, vehicleData, local xRot, yRot, zRot = getWorldRotation(node) local x, y, z = localToWorld(node, xOffset, 1, zOffset) - self.collidingShapes = 0 - self.collidingShapesText = 'unknown' + self.collidingShapes = 0 + self.collidingShapesText = 'unknown' overlapBox(x, y + 0.2, z, xRot, yRot, zRot, width, 1, length, 'overlapBoxCallback', self, bitOR(AIVehicleUtil.COLLISION_MASK, 2), true, true, true) if log and self.collidingShapes > 0 then @@ -488,17 +494,18 @@ function PathfinderConstraints:isValidNode(node, log, ignoreTrailer) local myCollisionData = PathfinderUtil.getBoundingBoxInWorldCoordinates(PathfinderUtil.helperNode, self.context.vehicleData, 'me') -- for debug purposes only, store validity info on node node.collidingShapes = PathfinderUtil.collisionDetector:findCollidingShapes( - PathfinderUtil.helperNode, self.context.vehicleData, self.context.vehiclesToIgnore, log) + PathfinderUtil.helperNode, self.context.vehicleData, self.context.vehiclesToIgnore, self.context.objectsToIgnore, log) if self.context.vehicleData.trailer and not ignoreTrailer then -- now check the trailer or towed implement -- move the node to the rear of the vehicle (where approximately the trailer is attached) - local x, y, z = localToWorld(PathfinderUtil.helperNode, 0, 0, self.context.vehicleData.trailerHitchOffset) + local x, y, z = localToWorld(PathfinderUtil.helperNode, 0, 0, self.context.vehicleData.trailerHitchOffset) - PathfinderUtil.setWorldPositionAndRotationOnTerrain(PathfinderUtil.helperNode, x, z, - courseGenerator.toCpAngle(node.tTrailer), 0.5) + PathfinderUtil.setWorldPositionAndRotationOnTerrain(PathfinderUtil.helperNode, x, z, + courseGenerator.toCpAngle(node.tTrailer), 0.5) node.collidingShapes = node.collidingShapes + PathfinderUtil.collisionDetector:findCollidingShapes( - PathfinderUtil.helperNode, self.context.vehicleData.trailerRectangle, self.context.vehiclesToIgnore, log) + PathfinderUtil.helperNode, self.context.vehicleData.trailerRectangle, self.context.vehiclesToIgnore, + self.context.objectsToIgnore, log) end local isValid = node.collidingShapes == 0 if not isValid then @@ -542,17 +549,18 @@ end ---@param goal State3D function PathfinderUtil.startPathfindingFromVehicleToGoal(vehicle, goal, allowReverse, fieldNum, - vehiclesToIgnore, maxFruitPercent, offFieldPenalty, mustBeAccurate) + vehiclesToIgnore, objectsToIgnore, + maxFruitPercent, offFieldPenalty, mustBeAccurate) - local start = PathfinderUtil.getVehiclePositionAsState3D(vehicle) + local start = PathfinderUtil.getVehiclePositionAsState3D(vehicle) - local vehicleData = PathfinderUtil.VehicleData(vehicle, true, 0.5) + local vehicleData = PathfinderUtil.VehicleData(vehicle, true, 0.5) - PathfinderUtil.initializeTrailerHeading(start, vehicleData) + PathfinderUtil.initializeTrailerHeading(start, vehicleData) - local context = PathfinderUtil.Context(vehicle, vehiclesToIgnore) + local context = PathfinderUtil.Context(vehicle, vehiclesToIgnore, objectsToIgnore) - local constraints = PathfinderConstraints(context, + local constraints = PathfinderConstraints(context, maxFruitPercent or (vehicle.cp.settings.useRealisticDriving:is(true) and 50 or math.huge), offFieldPenalty or PathfinderUtil.defaultOffFieldPenalty, fieldNum) @@ -709,7 +717,7 @@ function PathfinderUtil.startPathfindingFromVehicleToWaypoint(vehicle, goalWaypo local offset = Vector(zOffset, -xOffset) goal:add(offset:rotate(goal.t)) return PathfinderUtil.startPathfindingFromVehicleToGoal( - vehicle, goal, allowReverse, fieldNum, vehiclesToIgnore, maxFruitPercent, offFieldPenalty) + vehicle, goal, allowReverse, fieldNum, vehiclesToIgnore, {}, maxFruitPercent, offFieldPenalty) end ------------------------------------------------------------------------------------------------------------------------ --- Interface function to start the pathfinder in the game. The goal is a point at sideOffset meters from the goal node @@ -733,7 +741,7 @@ function PathfinderUtil.startPathfindingFromVehicleToNode(vehicle, goalNode, local goal = State3D(x, -z, courseGenerator.fromCpAngle(yRot)) return PathfinderUtil.startPathfindingFromVehicleToGoal( vehicle, goal, allowReverse, fieldNum, - vehiclesToIgnore, maxFruitPercent, offFieldPenalty, mustBeAccurate) + vehiclesToIgnore, {}, maxFruitPercent, offFieldPenalty, mustBeAccurate) end ------------------------------------------------------------------------------------------------------------------------ @@ -778,7 +786,7 @@ end -- Then check all three for collisions with obstacles. ---@return boolean, boolean, boolean true if no obstacles left, right, straight ahead ------------------------------------------------------------------------------------------------------------------------ -function PathfinderUtil.checkForObstaclesAhead(vehicle, turnRadius) +function PathfinderUtil.checkForObstaclesAhead(vehicle, turnRadius, objectsToIgnore) local function isValidPath(constraints, path) for i, node in ipairs(path) do @@ -806,7 +814,7 @@ function PathfinderUtil.checkForObstaclesAhead(vehicle, turnRadius) local start = PathfinderUtil.getVehiclePositionAsState3D(vehicle) local vehicleData = PathfinderUtil.VehicleData(vehicle, true, 0.5) PathfinderUtil.initializeTrailerHeading(start, vehicleData) - local context = PathfinderUtil.Context(vehicle, {}) + local context = PathfinderUtil.Context(vehicle, {}, objectsToIgnore) local constraints = PathfinderConstraints(context, math.huge, 0, 0) ensureHelperNode() diff --git a/modDesc.xml b/modDesc.xml index c67eebd1d..3b5e6ea37 100644 --- a/modDesc.xml +++ b/modDesc.xml @@ -1,6 +1,6 @@ - 6.03.00055 + 6.03.00056 <!-- 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>