diff --git a/scripts/ai/turns/AITurn.lua b/scripts/ai/turns/AITurn.lua index 6ce73a718..ecf7bf017 100644 --- a/scripts/ai/turns/AITurn.lua +++ b/scripts/ai/turns/AITurn.lua @@ -74,6 +74,7 @@ function AITurn:init(vehicle, driveStrategy, ppc, proximityController, turnConte self.state = self.states.INITIALIZING self.name = name or 'AITurn' self.blocked = false + self.hasChainedAttachments = AIUtil.hasChainedAttachments(self.vehicle) end function AITurn:addState(state) @@ -233,7 +234,7 @@ function AITurn:getDriveData(dt) local maxSpeed = self:getForwardSpeed() local gx, gz, moveForwards if self.state == self.states.INITIALIZING then - local rowFinishingCourse = self.turnContext:createFinishingRowCourse(self.vehicle) + local rowFinishingCourse = self.turnContext:createFinishingRowCourse(self.vehicle, self:getRaiseImplementNode()) self.ppc:setCourse(rowFinishingCourse) self.ppc:initialize(1) self.state = self.states.FINISHING_ROW @@ -276,10 +277,17 @@ function AITurn:setRaiseLowerNodes() -- in headland corners, we want to stay on the field as much as possible to avoid hitting obstacles around the field. local _, backMarkerDistance = self.driveStrategy:getFrontAndBackMarkers() if backMarkerDistance < 0 then - -- implement on the back of the vehicle, so before the corner, we don't work all the way to the field edge, - -- stop a work width before it, then make the turn, back up until the implement reaches the field edge, - -- lower, and continue on the new headland direction - return self.turnContext.workEndNode, self.turnContext.workStartNode + if self.hasChainedAttachments then + -- implements on the back of the vehicle, we'll make a loop turn forward only, so before the corner, + -- we work all the way to the headland pass edge and then make a 270 turn to continue after the corner. + return self.turnContext.workEndNode, self.turnContext.workStartNode + + else + -- implement on the back of the vehicle, so before the corner, we don't work all the way to the field edge, + -- stop a work width before it, then make the turn, back up until the implement reaches the field edge, + -- lower, and continue on the new headland direction + return self.turnContext.workEndNode, self.turnContext.workStartNode + end else -- implement on the front of the vehicle, so we can work all the way to the field edge, then make the turn -- and back up only until the implement reaches the already worked part, work width from the field edge @@ -734,12 +742,18 @@ end function CourseTurn:generateCalculatedTurn() local turnManeuver if self.turnContext:isHeadlandCorner() then - -- TODO_22 self:debug('This is a headland turn') - turnManeuver = HeadlandCornerTurnManeuver(self.vehicle, self.turnContext, self.vehicle:getAIDirectionNode(), + if self.hasChainedAttachments then + -- do a 270° turn forward only + turnManeuver = LoopTurnManeuver(self.vehicle, self.turnContext, self.vehicle:getAIDirectionNode(), + self.turningRadius, self.workWidth, self.steeringLength) + self.enableTightTurnOffset = true + else + turnManeuver = HeadlandCornerTurnManeuver(self.vehicle, self.turnContext, self.vehicle:getAIDirectionNode(), 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 + -- adjust turn course for tight turns only for headland corners by default + self.forceTightTurnOffset = self.steeringLength > 0 + end else local distanceToFieldEdge = self.turnContext:getDistanceToFieldEdge(self.vehicle:getAIDirectionNode()) local turnOnField = self.driveStrategy:isTurnOnFieldActive() @@ -1006,13 +1020,13 @@ function StartRowOnly:init(vehicle, driveStrategy, ppc, turnContext, startRowCou -- implements are now lowering, maneuver ends when they are completely lowered self:addState('IMPLEMENTS_LOWERING') self.vehicle = vehicle - self.settings = vehicle:getCpSettings() self.workStartHandler = WorkStartHandler(vehicle, driveStrategy, turnContext) + self.settings = vehicle:getCpSettings() self.turningRadius = AIUtil.getTurningRadius(self.vehicle) - ---@type AIDriveStrategyFieldWorkCourse - self.driveStrategy = driveStrategy ---@type PurePursuitController self.ppc = ppc + ---@type AIDriveStrategyFieldWorkCourse + self.driveStrategy = driveStrategy ---@type TurnContext self.turnContext = turnContext self.name = 'StartRowOnly' diff --git a/scripts/ai/turns/TurnContext.lua b/scripts/ai/turns/TurnContext.lua index 0411603b9..ffc1628d2 100644 --- a/scripts/ai/turns/TurnContext.lua +++ b/scripts/ai/turns/TurnContext.lua @@ -208,6 +208,11 @@ function TurnContext:getLocalPositionFromTurnEnd(node) return localToLocal(node, self.vehicleAtTurnEndNode, 0, 0, 0) end +---@return number node pointing outwards from the corner (in a headland turn), or into the row in a 180 turn +function TurnContext:getCornerOutboundNode() + return self.turnEndWpNode.node +end + -- node's position in the turn start wp node's coordinate system function TurnContext:getLocalPositionFromTurnStart(node) return localToLocal(node, self.turnStartWpNode.node, 0, 0, 0) @@ -408,7 +413,7 @@ end --- Course to finish a row before the turn, just straight ahead, ignoring the corner ---@return Course -function TurnContext:createFinishingRowCourse(vehicle) +function TurnContext:createFinishingRowCourse(vehicle, workEndNode) local waypoints = {} -- must be at least as long as the back marker distance so we are not reaching the end of the course before -- the implement reaches the field edge (a negative backMarkerDistance means the implement is behind the @@ -419,7 +424,7 @@ function TurnContext:createFinishingRowCourse(vehicle) -- the front marker distance would be here relevant but this is only for creating the course, where the vehicle will -- stop finishing the row and start the turn depends only on the raise implement setting. for d = 0, math.max(self.workWidth * 1.5, -self.backMarkerDistance * 1.5), 1 do - local x, _, z = localToWorld(self.workEndNode, 0, 0, d) + local x, _, z = localToWorld(workEndNode or self.workEndNode, 0, 0, d) table.insert(waypoints, {x = x, z = z}) end return Course(vehicle, waypoints, true) diff --git a/scripts/ai/turns/TurnManeuver.lua b/scripts/ai/turns/TurnManeuver.lua index 190d394c5..c05876cff 100644 --- a/scripts/ai/turns/TurnManeuver.lua +++ b/scripts/ai/turns/TurnManeuver.lua @@ -459,6 +459,32 @@ function DubinsTurnManeuver:findAnalyticPath(startNode, startXOffset, startZOffs return Course.createFromAnalyticPath(self.vehicle, path, true) end +--- Headland turn maneuver to make corners with a 270 turn. This is good for rigs that can't reverse but there is +--- plenty of room on the field to make a 270 loop. Examples are seed drills with a seed cart. The first headland +--- should be round, the second and the rest can have a corner and there, this 270 will be used. +---@class LoopTurnManeuver : TurnManeuver +LoopTurnManeuver = CpObject(DubinsTurnManeuver) +function LoopTurnManeuver:init(vehicle, turnContext, vehicleDirectionNode, turningRadius, + workWidth, steeringLength) + self.debugPrefix = '(LoopTurn): ' + TurnManeuver.init(self, vehicle, turnContext, vehicleDirectionNode, turningRadius, + workWidth, steeringLength) + local turnEndNode, endZOffset = self.turnContext:getTurnEndNodeAndOffsets(steeringLength) + self:debug('r=%.1f, w=%.1f, steeringLength=%.1f, endZOffset=%.1f', turningRadius, workWidth, steeringLength, endZOffset) + -- pull forward a bit to have the implement reach at least the middle of the outgoing edge, so the 270 is + -- easier to turn into the target direction. May need to increase it depending on user feedback. + local pullForward = 0.5 * workWidth + self.course = Course.createFromNode(self.vehicle, vehicleDirectionNode, + 0, 0, pullForward, 1, false) + local path = PathfinderUtil.findAnalyticPath(PathfinderUtil.dubinsSolver, + vehicleDirectionNode, 0, pullForward + 0.5, turnEndNode, 0, -steeringLength, turningRadius) + self.course:append(Course.createFromAnalyticPath(self.vehicle, path, true)) + TurnManeuver.setLowerImplements(self.course, steeringLength, true) + self:applyTightTurnOffsetToAnalyticPath(self.course) + local endingTurnLength = self.turnContext:appendEndingTurnCourse(self.course, steeringLength) + TurnManeuver.setLowerImplements(self.course, endingTurnLength, true) +end + -- This is an experiment to create turns with towed implements that better align with the next row. -- Instead of relying on the dynamic tight turn offset, we offset the turn end already while generating the turn -- to get the implement closer to the next row. diff --git a/scripts/ai/util/AIUtil.lua b/scripts/ai/util/AIUtil.lua index 3a40e94d1..2a34911e5 100644 --- a/scripts/ai/util/AIUtil.lua +++ b/scripts/ai/util/AIUtil.lua @@ -806,3 +806,18 @@ function AIUtil.isOtherVehicleAhead(vehicle, otherVehicle) local _, _, dz = localToLocal(otherVehicle.rootNode, vehicle:getAIDirectionNode(), 0, 0, 0) return dz > (frontMarkerOffset + backMarkerOffset) / 2 end + +---@return boolean if the vehicle has multiple towed attachments connected to each other +function AIUtil.hasChainedAttachments(vehicle) + if vehicle.updateAIAgentAttachments then + local valid = CpUtil.try(vehicle.updateAIAgentAttachments, vehicle) + if valid then + local attachmentChains = vehicle.spec_aiDrivable.attachmentChains + if attachmentChains and #attachmentChains > 0 and #attachmentChains[1] > 1 then + CpUtil.debugVehicle(CpDebug.DBG_IMPLEMENTS, vehicle, 'has %d chained attachments', #attachmentChains[1]) + return true + end + end + end + return false +end \ No newline at end of file