From d7060f2fe31caab3ca7d542314ee2b670ea0b2cb Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Sat, 12 Apr 2025 18:42:58 -0400 Subject: [PATCH 1/3] wip --- scripts/ai/turns/AITurn.lua | 32 +++++++++++++++++++++---------- scripts/ai/turns/TurnContext.lua | 9 +++++++-- scripts/ai/turns/TurnManeuver.lua | 18 +++++++++++++++++ scripts/ai/util/AIUtil.lua | 15 +++++++++++++++ 4 files changed, 62 insertions(+), 12 deletions(-) diff --git a/scripts/ai/turns/AITurn.lua b/scripts/ai/turns/AITurn.lua index 6ce73a718..272236d92 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,10 +742,14 @@ 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 + turnManeuver = LoopTurnManeuver(self.vehicle, self.turnContext, self.vehicle:getAIDirectionNode(), + self.turningRadius, self.workWidth, self.steeringLength) + else + turnManeuver = HeadlandCornerTurnManeuver(self.vehicle, self.turnContext, self.vehicle:getAIDirectionNode(), self.turningRadius, self.workWidth, self.reversingImplement, self.steeringLength) + end -- adjust turn course for tight turns only for headland corners by default self.forceTightTurnOffset = self.steeringLength > 0 else @@ -1006,13 +1018,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..3fe6ecd4d 100644 --- a/scripts/ai/turns/TurnManeuver.lua +++ b/scripts/ai/turns/TurnManeuver.lua @@ -459,6 +459,24 @@ function DubinsTurnManeuver:findAnalyticPath(startNode, startXOffset, startZOffs return Course.createFromAnalyticPath(self.vehicle, path, true) end +---@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) + self:debug('r=%.1f, w=%.1f, steeringLength=%.1f', turningRadius, workWidth, steeringLength) + local turnEndNode, endZOffset = self.turnContext:getTurnEndNodeAndOffsets(0) + local path = PathfinderUtil.findAnalyticPath(PathfinderUtil.dubinsSolver, + vehicleDirectionNode, 0, 0, turnEndNode, 0, endZOffset, 5) + self.course = Course.createFromNode(self.vehicle, self.vehicle:getAIDirectionNode(), + 0, 0, workWidth / 2, 1, false) + self.course:append(Course.createFromAnalyticPath(self.vehicle, path, true)) + local endingTurnLength = self.turnContext:appendEndingTurnCourse(self.course, steeringLength) + self:applyTightTurnOffset(endingTurnLength) +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 From 12ff044ade7c29c1b2b3d6597d3d860da50b74df Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Sat, 12 Apr 2025 19:39:50 -0400 Subject: [PATCH 2/3] wip --- scripts/ai/turns/TurnManeuver.lua | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/scripts/ai/turns/TurnManeuver.lua b/scripts/ai/turns/TurnManeuver.lua index 3fe6ecd4d..2a56059d6 100644 --- a/scripts/ai/turns/TurnManeuver.lua +++ b/scripts/ai/turns/TurnManeuver.lua @@ -466,12 +466,13 @@ function LoopTurnManeuver:init(vehicle, turnContext, vehicleDirectionNode, turni self.debugPrefix = '(LoopTurn): ' TurnManeuver.init(self, vehicle, turnContext, vehicleDirectionNode, turningRadius, workWidth, steeringLength) - self:debug('r=%.1f, w=%.1f, steeringLength=%.1f', turningRadius, workWidth, steeringLength) - local turnEndNode, endZOffset = self.turnContext:getTurnEndNodeAndOffsets(0) + local turnEndNode, endZOffset = self.turnContext:getTurnEndNodeAndOffsets(steeringLength) + self:debug('r=%.1f, w=%.1f, steeringLength=%.1f, endZOffset=%.1f', turningRadius, workWidth, steeringLength, endZOffset) + 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, 0, turnEndNode, 0, endZOffset, 5) - self.course = Course.createFromNode(self.vehicle, self.vehicle:getAIDirectionNode(), - 0, 0, workWidth / 2, 1, false) + vehicleDirectionNode, 0, pullForward + 0.5, turnEndNode, 0, -steeringLength, turningRadius) self.course:append(Course.createFromAnalyticPath(self.vehicle, path, true)) local endingTurnLength = self.turnContext:appendEndingTurnCourse(self.course, steeringLength) self:applyTightTurnOffset(endingTurnLength) From 902a5776e95e960ec5701ccdc879722db157c979 Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Sun, 13 Apr 2025 07:27:49 -0400 Subject: [PATCH 3/3] feat: loop turn Add 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. Such rigs are currently auto-detected but we can also make it an option. #737 --- scripts/ai/turns/AITurn.lua | 6 ++++-- scripts/ai/turns/TurnManeuver.lua | 9 ++++++++- 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/scripts/ai/turns/AITurn.lua b/scripts/ai/turns/AITurn.lua index 272236d92..ecf7bf017 100644 --- a/scripts/ai/turns/AITurn.lua +++ b/scripts/ai/turns/AITurn.lua @@ -744,14 +744,16 @@ function CourseTurn:generateCalculatedTurn() if self.turnContext:isHeadlandCorner() then self:debug('This is a headland turn') 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 end - -- adjust turn course for tight turns only for headland corners by default - self.forceTightTurnOffset = self.steeringLength > 0 else local distanceToFieldEdge = self.turnContext:getDistanceToFieldEdge(self.vehicle:getAIDirectionNode()) local turnOnField = self.driveStrategy:isTurnOnFieldActive() diff --git a/scripts/ai/turns/TurnManeuver.lua b/scripts/ai/turns/TurnManeuver.lua index 2a56059d6..c05876cff 100644 --- a/scripts/ai/turns/TurnManeuver.lua +++ b/scripts/ai/turns/TurnManeuver.lua @@ -459,6 +459,9 @@ 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, @@ -468,14 +471,18 @@ function LoopTurnManeuver:init(vehicle, turnContext, vehicleDirectionNode, turni 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) - self:applyTightTurnOffset(endingTurnLength) + TurnManeuver.setLowerImplements(self.course, endingTurnLength, true) end -- This is an experiment to create turns with towed implements that better align with the next row.