wip

Author: Peter Vaiko

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'

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)

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.

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