diff --git a/config/VehicleConfigurations.xml b/config/VehicleConfigurations.xml index fb9794ab..712a8126 100644 --- a/config/VehicleConfigurations.xml +++ b/config/VehicleConfigurations.xml @@ -246,6 +246,10 @@ You can define the following custom settings: raiseLate = "true" lowerEarly = "true" /> + + + diff --git a/scripts/Course.lua b/scripts/Course.lua index c8fb4c41..e921fc4a 100644 --- a/scripts/Course.lua +++ b/scripts/Course.lua @@ -273,9 +273,12 @@ function Course:enrichWaypointData(startIx) 'Course with %d waypoints created/updated, %.1f meters, %d turns', #self.waypoints, self.length, self.totalTurns) end +function Course:getDeltaAngle(ix) + return CpMathUtil.getDeltaAngle(self.waypoints[ix].yRot, self.waypoints[ix - 1].yRot) +end + function Course:calculateSignedRadius(ix) - local deltaAngle = CpMathUtil.getDeltaAngle(self.waypoints[ix].yRot, self.waypoints[ix - 1].yRot) - return CpMathUtil.divide(self:getDistanceToNextWaypoint(ix), (2 * math.sin(deltaAngle / 2))) + return CpMathUtil.divide(self:getDistanceToNextWaypoint(ix), (2 * math.sin(self:getDeltaAngle(ix) / 2))) end function Course:calculateRadius(ix) @@ -493,8 +496,12 @@ function Course:getTurnControls(ix) return self.waypoints[ix].turnControls end -function Course:useTightTurnOffset(ix) - return self.waypoints[ix].useTightTurnOffset +function Course:setUseTightTurnOffset(ix) + return self.waypoints[ix]:setUseTightTurnOffset() +end + +function Course:getUseTightTurnOffset(ix) + return self.waypoints[ix]:getUseTightTurnOffset() end --- Returns the position of the waypoint at ix with the current offset applied. @@ -559,18 +566,6 @@ function Course:getYRotationCorrectedForDirectionChanges(ix) end end --- This is the radius from the course generator. For now ony island bypass waypoints nodes have a --- radius. -function Course:getRadiusAtIx(ix) - local r = self.waypoints[ix].radius - if r ~= r then - -- radius can be nan - return nil - else - return r - end -end - -- This is the radius calculated when the course is created. function Course:getCalculatedRadiusAtIx(ix) local r = self.waypoints[ix].calculatedRadius @@ -1242,7 +1237,9 @@ function Course:draw() Utils.renderTextAtWorldPosition(x, y + 3.2, z, tostring(i), getCorrectTextSize(0.012), 0, color) if i < self:getNumberOfWaypoints() then local nx, ny, nz = self:getWaypointPosition(i + 1) - DebugUtil.drawDebugLine(x, y + 3, z, nx, ny + 3, nz, 0, 0, 100) + -- cyan, darker blue with tight turn offset + color = self:getUseTightTurnOffset(i) and {0, 0, 0.5} or {0, 0.5, 0.5} + DebugUtil.drawDebugLine(x, y + 3, z, nx, ny + 3, nz, table.unpack(color)) end end end @@ -1287,7 +1284,7 @@ end function Course:setUseTightTurnOffsetForLastWaypoints(d) self:executeFunctionForLastWaypoints(d, function(wp) - wp.useTightTurnOffset = true + wp:setUseTightTurnOffset() end) end diff --git a/scripts/Waypoint.lua b/scripts/Waypoint.lua index 834e976d..e08b386f 100644 --- a/scripts/Waypoint.lua +++ b/scripts/Waypoint.lua @@ -34,7 +34,6 @@ function Waypoint:init(wp) ---@type CourseGenerator.WaypointAttributes self.attributes = wp.attributes and wp.attributes:clone() or CourseGenerator.WaypointAttributes() self.angle = wp.angle or nil - self.radius = wp.radius or nil self.rev = wp.rev or wp.reverse or false self.rev = self.rev or wp.gear and wp.gear == Gear.Backward -- dynamically added/calculated properties @@ -286,6 +285,14 @@ function Waypoint:setOnConnectingPath(onConnectingPath) self.attributes:setOnConnectingPath(onConnectingPath) end +function Waypoint:setUseTightTurnOffset() + self.useTightTurnOffset = true +end + +function Waypoint:getUseTightTurnOffset() + return self.useTightTurnOffset +end + function Waypoint:copyRowData(other) self.attributes.rowNumber = other.attributes.rowNumber self.attributes.leftSideWorked = other.attributes.leftSideWorked diff --git a/scripts/ai/AIDriveStrategyBunkerSilo.lua b/scripts/ai/AIDriveStrategyBunkerSilo.lua index 3c93eb83..ead8452d 100644 --- a/scripts/ai/AIDriveStrategyBunkerSilo.lua +++ b/scripts/ai/AIDriveStrategyBunkerSilo.lua @@ -415,7 +415,7 @@ function AIDriveStrategyBunkerSilo:startTransitionToNextLane() end local path = PathfinderUtil.findAnalyticPath(ReedsSheppSolver(), self.vehicle:getAIDirectionNode(), - 0, self.turnNode, 0, 0, self.turningRadius) + 0, 0, self.turnNode, 0, 0, self.turningRadius) if not path or #path == 0 then self:debug("No valid turn was found!") self.vehicle:stopCurrentAIJob(AIMessageCpErrorNoPathFound.new()) diff --git a/scripts/ai/AIDriveStrategyFieldWorkCourse.lua b/scripts/ai/AIDriveStrategyFieldWorkCourse.lua index c4ec2224..d865284d 100644 --- a/scripts/ai/AIDriveStrategyFieldWorkCourse.lua +++ b/scripts/ai/AIDriveStrategyFieldWorkCourse.lua @@ -775,7 +775,7 @@ function AIDriveStrategyFieldWorkCourse:calculateTightTurnOffset() if self.state == self.states.WORKING or self.state == self.states.DRIVING_TO_WORK_START_WAYPOINT then -- when rounding small islands or to start on a course with curves self.tightTurnOffset = AIUtil.calculateTightTurnOffset(self.vehicle, self.turningRadius, self.course, - self.tightTurnOffset, true) + self.tightTurnOffset) else self.tightTurnOffset = 0 end diff --git a/scripts/ai/AIDriveStrategyUnloadCombine.lua b/scripts/ai/AIDriveStrategyUnloadCombine.lua index 9ef44703..25b58a0e 100644 --- a/scripts/ai/AIDriveStrategyUnloadCombine.lua +++ b/scripts/ai/AIDriveStrategyUnloadCombine.lua @@ -2829,7 +2829,7 @@ function AIDriveStrategyUnloadCombine:onFieldUnloadPositionReached() self:debug("Starting pathfinding to the reverse unload turn end node with align length: %.2f and steering length: %.2f, turn radius: %.2f", alignLength, steeringLength, self.turningRadius) local path = PathfinderUtil.findAnalyticPath(PathfinderUtil.dubinsSolver, self.fieldUnloadTurnStartNode, - 0, self.fieldUnloadTurnEndNode, 0, 3, self.turningRadius) + 0, 0, self.fieldUnloadTurnEndNode, 0, 3, self.turningRadius) if not path or #path == 0 then self:debug("Reverse alignment course creation failed!") else diff --git a/scripts/ai/AIUtil.lua b/scripts/ai/AIUtil.lua index 190336d7..bf0e6dc4 100644 --- a/scripts/ai/AIUtil.lua +++ b/scripts/ai/AIUtil.lua @@ -50,7 +50,7 @@ end --- making sure that the towed implement's trajectory remains closer to the --- course. ---@param course Course -function AIUtil.calculateTightTurnOffset(vehicle, vehicleTurningRadius, course, previousOffset, useCalculatedRadius) +function AIUtil.calculateTightTurnOffset(vehicle, vehicleTurningRadius, course, previousOffset) local tightTurnOffset local function smoothOffset(offset) @@ -58,12 +58,7 @@ function AIUtil.calculateTightTurnOffset(vehicle, vehicleTurningRadius, course, end -- first of all, does the current waypoint have radius data? - local r - if useCalculatedRadius then - r = course:getCalculatedRadiusAtIx(course:getCurrentWaypointIx()) - else - r = course:getRadiusAtIx(course:getCurrentWaypointIx()) - end + local r = course:getCalculatedRadiusAtIx(course:getCurrentWaypointIx()) if not r then return smoothOffset(0) end @@ -104,13 +99,54 @@ function AIUtil.calculateTightTurnOffset(vehicle, vehicleTurningRadius, course, -- smooth the offset a bit to avoid sudden changes tightTurnOffset = smoothOffset(offset) - CpUtil.debugVehicle(CpDebug.DBG_AI_DRIVER, vehicle, + CpUtil.debugVehicle(CpDebug.DBG_TURN, vehicle, 'Tight turn, r = %.1f, tow bar = %.1f m, currentAngle = %.0f, nextAngle = %.0f, offset = %.1f, smoothOffset = %.1f', r, towBarLength, currentAngle, nextAngle, offset, tightTurnOffset ) -- remember the last value for smoothing return tightTurnOffset end +function AIUtil.calculateTightTurnOffsetForTurnManeuver(vehicle, steeringLength, course, ix, previousOffset) + local tightTurnOffset + + local function smoothOffset(offset) + -- smooth more for articulated axis or track vehicle + -- as those usually have a very small turn radius anyway, causing jackknifing + -- TODO: use the vehicle's solo radius instead? + local factor = AIUtil.hasArticulatedAxis(vehicle) and 6 or 4 + return (offset + factor * (previousOffset or 0 )) / (factor + 1) + end + + -- first of all, does the current waypoint have radius data? + local r = course:getCalculatedRadiusAtIx(ix) + if not r then + return smoothOffset(0) + end + + local offset = AIUtil.getTractorRadiusFromImplementRadius(r, steeringLength) - r + if offset ~= offset then + -- check for nan + return smoothOffset(0) + end + -- figure out left or right now? + local nextAngle = course:getWaypointAngleDeg(ix + 1) + local currentAngle = course:getWaypointAngleDeg(ix) + if not nextAngle or not currentAngle then + return smoothOffset(0) + end + + if CpMathUtil.getDeltaAngle(math.rad(nextAngle), math.rad(currentAngle)) > 0 then offset = -offset end + + -- smooth the offset a bit to avoid sudden changes + tightTurnOffset = smoothOffset(offset) + CpUtil.debugVehicle(CpDebug.DBG_TURN, vehicle, + 'Tight turn, r = %.1f, tow bar = %.1f m, currentAngle = %.0f, nextAngle = %.0f, offset = %.1f, smoothOffset = %.1f', + r, steeringLength, currentAngle, nextAngle, offset, tightTurnOffset ) + -- remember the last value for smoothing + return tightTurnOffset +end + + function AIUtil.getTowBarLength(vehicle) -- is there a wheeled implement behind the tractor and is it on a pivot? local implement = AIUtil.getFirstReversingImplementWithWheels(vehicle, true) @@ -141,8 +177,29 @@ function AIUtil.getSteeringParameters(vehicle) end function AIUtil.getOffsetForTowBarLength(r, towBarLength) - local rTractor = math.sqrt( r * r + towBarLength * towBarLength ) -- the radius the tractor should be on - return rTractor - r + return AIUtil.getTractorRadiusFromImplementRadius(r, towBarLength) - r +end + +--- When a tractor is towing an implement in a turn, on what radius will the implement be if +--- the radius the tractor is driving is known? +---@param r number the radius the tractor is on +---@param towBarLength number the length of the tow bar +---@return number the radius the implement will be on. Can be negative, meaning the implement will be +--- moving backwards in the turn +function AIUtil.getImplementRadiusFromTractorRadius(r, towBarLength) + local rSquared = r * r - towBarLength * towBarLength + local rImplement = rSquared > 0 and math.sqrt(rSquared) or -math.sqrt(-rSquared) + return rImplement +end + +--- When a tractor is towing an implement in a turn, on what radius will the tractor be if +--- the radius the implement is known? +---@param r number the radius the implement is following +---@param towBarLength number the length of the tow bar +---@return number the radius the tractor will be on +function AIUtil.getTractorRadiusFromImplementRadius(r, towBarLength) + local rTractor = math.sqrt( r * r + towBarLength * towBarLength ) + return rTractor end function AIUtil.getArticulatedAxisVehicleReverserNode(vehicle) @@ -359,9 +416,9 @@ function AIUtil.getFirstAttachedImplement(vehicle, suppressLog) -- the distance from the vehicle's root node to the front of the implement local _, _, d = localToLocal(implement.object.rootNode, AIUtil.getDirectionNode(vehicle), 0, 0, implement.object.size.length / 2 + implement.object.size.lengthOffset) - if implement.object.spec_leveler then + if implement.object.spec_leveler then local nodeData = ImplementUtil.getLevelerNode(implement.object) - if nodeData then + if nodeData then _, _, d = localToLocal(nodeData.node, AIUtil.getDirectionNode(vehicle), 0, 0, 0) end end @@ -388,9 +445,9 @@ function AIUtil.getLastAttachedImplement(vehicle,suppressLog) -- the distance from the vehicle's root node to the back of the implement local _, _, d = localToLocal(implement.object.rootNode, AIUtil.getDirectionNode(vehicle), 0, 0, - implement.object.size.length / 2 + implement.object.size.lengthOffset) - if implement.object.spec_leveler then + if implement.object.spec_leveler then local nodeData = ImplementUtil.getLevelerNode(implement.object) - if nodeData then + if nodeData then _, _, d = localToLocal(nodeData.node, AIUtil.getDirectionNode(vehicle), 0, 0, 0) end end @@ -452,7 +509,7 @@ function AIUtil.getNumberOfChildVehiclesWithSpecialization(vehicle, specializati return #vehicles end ---- Gets all child vehicles with a given specialization. +--- Gets all child vehicles with a given specialization. --- This can include the rootVehicle and implements --- that are not directly attached to the rootVehicle. ---@param vehicle table @@ -462,7 +519,7 @@ end ---@return boolean at least one vehicle/implement was found function AIUtil.getAllChildVehiclesWithSpecialization(vehicle, specialization, specializationReference) if vehicle == nil then - printCallstack() + printCallstack() CpUtil.info("Vehicle is nil!") return {}, false end @@ -661,7 +718,7 @@ function AIUtil.getLength(vehicle) if vehicle.getAIAgentSize then vehicle:updateAIAgentAttachments() local width, length, lengthOffset, frontOffset, height = vehicle:getAIAgentSize() - for _, attachment in ipairs(vehicle.spec_aiDrivable.attachments) do + for _, attachment in ipairs(vehicle.spec_aiDrivable.attachments) do length = length + attachment.length end return length @@ -697,7 +754,7 @@ function AIUtil.hasCutterOnTrailerAttached(vehicle) end --- Checks if a cutter is attached and it's not registered as a valid combine cutter. ---- A Example is the New Holland Superflex header, when it is attached as transport trailer. +--- A Example is the New Holland Superflex header, when it is attached as transport trailer. function AIUtil.hasCutterAsTrailerAttached(vehicle) local cutters, found = AIUtil.getAllChildVehiclesWithSpecialization(vehicle, Cutter) if not found then @@ -705,12 +762,12 @@ function AIUtil.hasCutterAsTrailerAttached(vehicle) return false end local combines, found = AIUtil.getAllChildVehiclesWithSpecialization(vehicle, Combine) - if not found then + if not found then --- No valid combine object was found. return false end local spec = combines[1].spec_combine - if spec.numAttachedCutters <= 0 then + if spec.numAttachedCutters <= 0 then --- The cutter is not available for threshing in this combination. return true end diff --git a/scripts/ai/turns/AITurn.lua b/scripts/ai/turns/AITurn.lua index f9524b0d..fd50e5d8 100644 --- a/scripts/ai/turns/AITurn.lua +++ b/scripts/ai/turns/AITurn.lua @@ -649,10 +649,10 @@ end function CourseTurn:onWaypointChange(ix) AITurn.onWaypointChange(self, ix) if self.turnCourse then - if self.forceTightTurnOffset or (self.enableTightTurnOffset and self.turnCourse:useTightTurnOffset(ix)) then + if self.forceTightTurnOffset or (self.enableTightTurnOffset and self.turnCourse:getUseTightTurnOffset(ix)) then -- adjust the course a bit to the outside in a curve to keep a towed implement on the course - self.tightTurnOffset = AIUtil.calculateTightTurnOffset(self.vehicle, self.turningRadius, self.turnCourse, - self.tightTurnOffset, true) + self.tightTurnOffset = AIUtil.calculateTightTurnOffsetForTurnManeuver(self.vehicle, self.steeringLength, + self.turnCourse, self.turnCourse:getCurrentWaypointIx(), self.tightTurnOffset) self.turnCourse:setOffset(self.tightTurnOffset, 0) else -- reset offset to 0 if tight turn offset is not on @@ -734,9 +734,8 @@ function CourseTurn:generateCalculatedTurn() turnManeuver = ReedsSheppTurnManeuver(self.vehicle, self.turnContext, self.vehicle:getAIDirectionNode(), self.turningRadius, self.workWidth, self.steeringLength, distanceToFieldEdge) end - -- only use tight turn offset if we are towing something and not an articulated axis or track vehicle - -- as those usually have a very small turn radius anyway, causing jackknifing - if self.steeringLength > 0 and not AIUtil.hasArticulatedAxis(self.vehicle) then + -- only use tight turn offset if we are towing something + if self.steeringLength > 0 then self:debug('Enabling tight turn offset') self.enableTightTurnOffset = true end @@ -769,7 +768,8 @@ function CourseTurn:onPathfindingDone(path) self.turnCourse = Course(self.vehicle, CpMathUtil.pointsToGameInPlace(path), true) -- make sure we use tight turn offset towards the end of the course so a towed implement is aligned with the new row self.turnCourse:setUseTightTurnOffsetForLastWaypoints(15) - local endingTurnLength = self.turnContext:appendEndingTurnCourse(self.turnCourse, nil, true) + local endingTurnLength = self.turnContext:appendEndingTurnCourse(self.turnCourse, nil) + self.turnCourse:setUseTightTurnOffsetForLastWaypoints(endingTurnLength) local x = AIUtil.getDirectionNodeToReverserNodeOffset(self.vehicle) self:debug('Extending course at direction switch for reversing to %.1f m (or at least 1m)', -x) self.turnCourse:adjustForReversing(math.max(1, -x)) @@ -1013,12 +1013,12 @@ function StartRowOnly:init(vehicle, driveStrategy, ppc, turnContext, startRowCou self.forceTightTurnOffset = false local _, steeringLength = AIUtil.getSteeringParameters(self.vehicle) - self.enableTightTurnOffset = steeringLength > 0 and not AIUtil.hasArticulatedAxis(self.vehicle) - + self.enableTightTurnOffset = steeringLength > 0 -- TODO: do we need tight turn offset here? self.turnCourse:setUseTightTurnOffsetForLastWaypoints(15) -- add a turn ending section into the row to make sure the implements are lowered correctly local endingTurnLength = self.turnContext:appendEndingTurnCourse(self.turnCourse, 3, true) + self.turnCourse:setUseTightTurnOffsetForLastWaypoints(endingTurnLength) TurnManeuver.setLowerImplements(self.turnCourse, endingTurnLength, true) self.turnCourse:adjustForReversing(2) self.state = self.states.DRIVING_TO_ROW diff --git a/scripts/ai/turns/TurnContext.lua b/scripts/ai/turns/TurnContext.lua index ae6d3e9a..c00846c0 100644 --- a/scripts/ai/turns/TurnContext.lua +++ b/scripts/ai/turns/TurnContext.lua @@ -230,6 +230,7 @@ end ---@return number angle (radian) between the row and the headland, 90 degrees means the headland is perpendicular to the row function TurnContext:getHeadlandAngle() + print(self.turnEndWp.angle, self.turnStartWp.angle) return math.abs(CpMathUtil.getDeltaAngle(math.rad(self.turnEndWp.angle), math.rad(self.turnStartWp.angle))) end @@ -382,7 +383,7 @@ end ---@param extraLength number add so many meters to the calculated course (for example to allow towed implements to align --- before reversing) ---@return number length added to the course in meters -function TurnContext:appendEndingTurnCourse(course, extraLength, useTightTurnOffset) +function TurnContext:appendEndingTurnCourse(course, extraLength) -- make sure course reaches the front marker node so end it well behind that node local _, _, dzFrontMarker = course:getWaypointLocalPosition(self.vehicleAtTurnEndNode, course:getNumberOfWaypoints()) local _, _, dzWorkStart = course:getWaypointLocalPosition(self.workStartNode, course:getNumberOfWaypoints()) @@ -398,7 +399,7 @@ function TurnContext:appendEndingTurnCourse(course, extraLength, useTightTurnOff dzFrontMarker, dzWorkStart, extraLength) for d = math.min(dzFrontMarker, dzWorkStart) + 1, extraLength, 1 do local x, y, z = localToWorld(startNode, 0, 0, d) - table.insert(waypoints, {x = x, y = y, z = z, useTightTurnOffset = useTightTurnOffset or nil}) + table.insert(waypoints, {x = x, y = y, z = z}) end local oldLength = course:getLength() course:appendWaypoints(waypoints) diff --git a/scripts/ai/turns/TurnManeuver.lua b/scripts/ai/turns/TurnManeuver.lua index 3dbf1d59..d249e66a 100644 --- a/scripts/ai/turns/TurnManeuver.lua +++ b/scripts/ai/turns/TurnManeuver.lua @@ -17,7 +17,7 @@ TurnManeuver.CHANGE_TO_FWD_WHEN_REACHED = 'changeToFwdWhenReached' -- making sure it is lowered when we reach the start of the next row) TurnManeuver.LOWER_IMPLEMENT_AT_TURN_END = 'lowerImplementAtTurnEnd' -- Mark waypoints for dynamic tight turn offset -TurnManeuver.applyTightTurnOffset = true +TurnManeuver.tightTurnOffsetEnabled = true ---@param course Course function TurnManeuver.hasTurnControl(course, ix, control) @@ -44,6 +44,7 @@ function TurnManeuver:init(vehicle, turnContext, vehicleDirectionNode, turningRa -- how far the furthest point of the maneuver is from the vehicle's direction node, used to -- check if we can turn on the field self.dzMax = -math.huge + self.turnEndXOffset = self.turnEndXOffset or 0 end function TurnManeuver:getCourse() @@ -284,8 +285,9 @@ function TurnManeuver:adjustCourseToFitField(course, dBack, ixBeforeEndingTurnSe self:debug('Reverse to work start (implement in back)') -- vehicle in front of the work start node at turn end local forwardAfterTurn = Course.createFromNode(self.vehicle, self.turnContext.vehicleAtTurnEndNode, 0, - dFromTurnEnd + 1, dFromTurnEnd + 1 + self.steeringLength, 0.8, false) + dFromTurnEnd + 1 + self.steeringLength / 2, dFromTurnEnd + 1 + self.steeringLength, 0.8, false) courseWithReversing:append(forwardAfterTurn) + self:applyTightTurnOffset(forwardAfterTurn:getLength()) -- allow early direction change when aligned TurnManeuver.setTurnControlForLastWaypoints(courseWithReversing, forwardAfterTurn:getLength(), TurnManeuver.CHANGE_DIRECTION_WHEN_ALIGNED, true, true) @@ -306,6 +308,7 @@ function TurnManeuver:adjustCourseToFitField(course, dBack, ixBeforeEndingTurnSe local forwardAfterTurn = Course.createFromNode(self.vehicle, self.turnContext.workStartNode, 0, dFromWorkStart, 1, 0.8, false) courseWithReversing:append(forwardAfterTurn) + self:applyTightTurnOffset(forwardAfterTurn:getLength()) TurnManeuver.setTurnControlForLastWaypoints(courseWithReversing, forwardAfterTurn:getLength(), TurnManeuver.CHANGE_DIRECTION_WHEN_ALIGNED, true, true) end @@ -315,45 +318,90 @@ function TurnManeuver:adjustCourseToFitField(course, dBack, ixBeforeEndingTurnSe endingTurnLength = reverseAfterTurn:getLength() else self:debug('Reverse to work start not needed') - endingTurnLength = self.turnContext:appendEndingTurnCourse(courseWithReversing, self.steeringLength, self.applyTightTurnOffset) + endingTurnLength = self.turnContext:appendEndingTurnCourse(courseWithReversing, self.steeringLength) + self:applyTightTurnOffset(endingTurnLength) end return courseWithReversing, endingTurnLength end +function TurnManeuver:applyTightTurnOffset(length) + if self.tightTurnOffsetEnabled then + -- use the default length (a half circle) unless there is a configured value + length = length or self.turningRadius * math.pi + self.course:setUseTightTurnOffsetForLastWaypoints( + g_vehicleConfigurations:getRecursively(self.vehicle, 'tightTurnOffsetDistanceInTurns') or length) + end +end + +-- Apply tight turn offset to an analytically generated 180 turn section. The goal is to align a towed +-- implement properly with the next row +function TurnManeuver:applyTightTurnOffsetToAnalyticPath(course) + if self.tightTurnOffsetEnabled then + local totalDeltaAngle = 0 + local totalDistance = 0 + local previousDeltaAngle = course:getDeltaAngle(course:getNumberOfWaypoints()) + for i = course:getNumberOfWaypoints(), 2, -1 do + course:setUseTightTurnOffset(i) + local deltaAngle = course:getDeltaAngle(i) + totalDeltaAngle = totalDeltaAngle + deltaAngle + -- check for configured distance + totalDistance = totalDistance + course:getDistanceToNextWaypoint(i - 1) + if totalDistance > + (g_vehicleConfigurations:getRecursively(self.vehicle, 'tightTurnOffsetDistanceInTurns') or math.huge) then + self:debug('Total distance %.1f > configured, stop applying tight turn offset', totalDistance) + break + end + -- Check for direction change: this is to have offset only at the foot of an omega turn and not + -- around the body, only when the foot is significantly narrower than the body. + -- This is for the case when the turn diameter is significantly bigger than the working width + if math.abs(totalDeltaAngle) > math.pi / 6 and + math.abs(deltaAngle) > 0.01 and math.sign(deltaAngle) ~= math.sign(previousDeltaAngle) then + self:debug('Curve direction change at %d (total delta angle %.1f, stop applying tight turn offset', + i, math.deg(totalDeltaAngle)) + break + end + -- in all other cases, apply to half circle + if math.abs(totalDeltaAngle) > math.pi / 2 then + self:debug('Total direction change more than 90, stop applying tight turn offset') + break + end + previousDeltaAngle = deltaAngle + end + end +end + ---@class AnalyticTurnManeuver : TurnManeuver AnalyticTurnManeuver = CpObject(TurnManeuver) function AnalyticTurnManeuver:init(vehicle, turnContext, vehicleDirectionNode, turningRadius, workWidth, steeringLength, distanceToFieldEdge) TurnManeuver.init(self, vehicle, turnContext, vehicleDirectionNode, turningRadius, workWidth, steeringLength) self:debug('Start generating') - self:debug('r=%.1f, w=%.1f, steeringLength=%.1f, distanceToFieldEdge=%.1f', - turningRadius, workWidth, steeringLength, distanceToFieldEdge) - local turnEndNode, goalOffset = self.turnContext:getTurnEndNodeAndOffsets(self.steeringLength) - self.course = self:findAnalyticPath(vehicleDirectionNode, 0, turnEndNode, 0, goalOffset, self.turningRadius) + local turnEndNode, endZOffset = self.turnContext:getTurnEndNodeAndOffsets(self.steeringLength) + local _, _, dz = localToLocal(vehicleDirectionNode, turnEndNode, 0, 0, 0) + -- zOffset from the turn end (work start). If there is a negative zOffset in the turn, that is, the turn end is behind the + -- turn start due to an angled headland, we still want to make the complete 180 turn as close to the field edge + -- as we can, so a towed implement, with an offset arc is turned 180 as soon as possible and has time to align. + -- This way, the tight turn offset can make its magic during the 180 turn. Otherwise, the Dubins generated will split + -- the 180 into two turns, one over 120 at the turn start, and one less than 60 at the turn end. This latter one + -- is not enough direction change for the tight turn offset to work. + endZOffset = math.min(dz, endZOffset) + self:debug('r=%.1f, w=%.1f, steeringLength=%.1f, distanceToFieldEdge=%.1f, goalOffset=%.1f, dz=%.1f', + turningRadius, workWidth, steeringLength, distanceToFieldEdge, endZOffset, dz) + self.course = self:findAnalyticPath(vehicleDirectionNode, 0, 0, turnEndNode, self.turnEndXOffset, endZOffset, self.turningRadius) local endingTurnLength local dBack = self:getDistanceToMoveBack(self.course, workWidth, distanceToFieldEdge) local canReverse = AIUtil.canReverse(vehicle) if dBack > 0 and canReverse then dBack = dBack < 2 and 2 or dBack self:debug('Not enough space on field, regenerating course back %.1f meters', dBack) - self.course = self:findAnalyticPath(vehicleDirectionNode, -dBack, turnEndNode, 0, goalOffset + dBack, self.turningRadius) - if self.applyTightTurnOffset then - self.course:setUseTightTurnOffsetForLastWaypoints( - g_vehicleConfigurations:getRecursively(vehicle, 'tightTurnOffsetDistanceInTurns') or 10) - end + self.course = self:findAnalyticPath(vehicleDirectionNode, 0, -dBack, turnEndNode, self.turnEndXOffset, endZOffset + dBack, self.turningRadius) + self:applyTightTurnOffsetToAnalyticPath(self.course) local ixBeforeEndingTurnSection = self.course:getNumberOfWaypoints() self.course, endingTurnLength = self:adjustCourseToFitField(self.course, dBack, ixBeforeEndingTurnSection) else - if self.applyTightTurnOffset then - self.course:setUseTightTurnOffsetForLastWaypoints( - g_vehicleConfigurations:getRecursively(vehicle, 'tightTurnOffsetDistanceInTurns') or 10) - end - endingTurnLength = self.turnContext:appendEndingTurnCourse(self.course, steeringLength, self.applyTightTurnOffset) - end - if self.applyTightTurnOffset then - -- make sure we use tight turn offset towards the end of the course so a towed implement is aligned with the new row - self.course:setUseTightTurnOffsetForLastWaypoints( - g_vehicleConfigurations:getRecursively(vehicle, 'tightTurnOffsetDistanceInTurns') or 10) + self:applyTightTurnOffsetToAnalyticPath(self.course) + endingTurnLength = self.turnContext:appendEndingTurnCourse(self.course, steeringLength) + self:applyTightTurnOffset(endingTurnLength) end TurnManeuver.setLowerImplements(self.course, endingTurnLength, true) end @@ -363,18 +411,19 @@ function AnalyticTurnManeuver:getDistanceToMoveBack(course, workWidth, distanceT local dzMax = self:getDzMax(course) local spaceNeededOnFieldForTurn = dzMax + workWidth / 2 distanceToFieldEdge = distanceToFieldEdge or 500 -- if not given, assume we have a lot of space - if self.turnContext:getTurnEndForwardOffset() < 0 then - -- in an offset turn, where the turn start (and thus, the vehicle) is on the longer leg, - -- so the turn end is behind the turn start, we have in reality less space, as we measured the - -- distance to the field edge from the turn start, but we need to measure it from the turn end, - -- where there's less space - distanceToFieldEdge = distanceToFieldEdge + self.turnContext:getTurnEndForwardOffset() - end + local turnEndForwardOffset = self.turnContext:getTurnEndForwardOffset() + -- in an offset turn, where the turn start (and thus, the vehicle) is on the longer leg, + -- so the turn end is behind the turn start, we have in reality less space, as we measured the + -- distance to the field edge from the turn start, but we need to measure it from the middle of the turn, + -- where there's less space + distanceToFieldEdge = distanceToFieldEdge + turnEndForwardOffset / 2 -- with a headland at angle, we have to move further back, so the left/right edge of the swath also stays on -- the field, not only the center - distanceToFieldEdge = distanceToFieldEdge - (workWidth / 2 / math.abs(math.tan(self.turnContext:getHeadlandAngle()))) - self:debug('dzMax=%.1f, workWidth=%.1f, spaceNeeded=%.1f, distanceToFieldEdge=%.1f', dzMax, workWidth, - spaceNeededOnFieldForTurn, distanceToFieldEdge) + local headlandAngle = self.turnContext:getHeadlandAngle() + distanceToFieldEdge = distanceToFieldEdge - + (headlandAngle > 0.0001 and (workWidth / 2 / math.abs(math.tan(headlandAngle))) or 0) + self:debug('dzMax=%.1f, workWidth=%.1f, spaceNeeded=%.1f, turnEndForwardOffset=%.1f, headlandAngle=%.1f, distanceToFieldEdge=%.1f', dzMax, workWidth, + spaceNeededOnFieldForTurn, turnEndForwardOffset, math.deg(headlandAngle), distanceToFieldEdge) return spaceNeededOnFieldForTurn - distanceToFieldEdge end @@ -383,46 +432,35 @@ DubinsTurnManeuver = CpObject(AnalyticTurnManeuver) function DubinsTurnManeuver:init(vehicle, turnContext, vehicleDirectionNode, turningRadius, workWidth, steeringLength, distanceToFieldEdge) self.debugPrefix = '(DubinsTurn): ' + self.turnEndXOffset = 0 AnalyticTurnManeuver.init(self, vehicle, turnContext, vehicleDirectionNode, turningRadius, workWidth, steeringLength, distanceToFieldEdge) end -function DubinsTurnManeuver:findAnalyticPath(vehicleDirectionNode, startOffset, turnEndNode, - xOffset, goalOffset, turningRadius) +function DubinsTurnManeuver:findAnalyticPath(startNode, startXOffset, startZOffset, endNode, + endXOffset, endZOffset, turningRadius) local path = PathfinderUtil.findAnalyticPath(PathfinderUtil.dubinsSolver, - vehicleDirectionNode, startOffset, turnEndNode, 0, goalOffset, self.turningRadius) + startNode, startXOffset, startZOffset, endNode, endXOffset, endZOffset, self.turningRadius) return Course.createFromAnalyticPath(self.vehicle, path, 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. ---@class TowedDubinsTurnManeuver : DubinsTurnManeuver TowedDubinsTurnManeuver = CpObject(DubinsTurnManeuver) -TowedDubinsTurnManeuver.applyTightTurnOffset = false function TowedDubinsTurnManeuver:init(vehicle, turnContext, vehicleDirectionNode, turningRadius, workWidth, steeringLength, distanceToFieldEdge) self.debugPrefix = '(TowedDubinsTurn): ' - local offset = AIUtil.getOffsetForTowBarLength(turningRadius, steeringLength) - turningRadius = turningRadius - offset - self:debug('Towed implement, adjusting radius to %.1f to accommodate tight turn offset', turningRadius) + self.vehicle = vehicle + local implementRadius = AIUtil.getImplementRadiusFromTractorRadius(turningRadius, steeringLength) + local xOffset = turningRadius - implementRadius + self.turnEndXOffset = turnContext:isLeftTurn() and -xOffset or xOffset + self:debug('Towed implement, offsetting turn end %.1f to accommodate tight turn, implement radius %.1f ', xOffset, implementRadius) AnalyticTurnManeuver.init(self, vehicle, turnContext, vehicleDirectionNode, turningRadius, workWidth, steeringLength, distanceToFieldEdge) end -function TowedDubinsTurnManeuver:calculateTractorCourse(course) - for _, wp in ipairs(course.waypoints) do - local v = PathfinderUtil.getWaypointAsState3D(wp, 0, self.steeringLength) - wp.x, wp.z = v.x, -v.y - end - course:enrichWaypointData() - return course -end - -function TowedDubinsTurnManeuver:findAnalyticPath(vehicleDirectionNode, startOffset, turnEndNode, - xOffset, goalOffset, turningRadius) - local path = PathfinderUtil.findAnalyticPath(PathfinderUtil.dubinsSolver, - vehicleDirectionNode, startOffset, turnEndNode, 0, goalOffset, self.turningRadius) - return self:calculateTractorCourse(Course.createFromAnalyticPath(self.vehicle, path, true)) -end - ---@class LeftTurnReedsSheppSolver : ReedsSheppSolver LeftTurnReedsSheppSolver = CpObject(ReedsSheppSolver) function LeftTurnReedsSheppSolver:solve(start, goal, turnRadius) @@ -451,8 +489,8 @@ function ReedsSheppTurnManeuver:init(vehicle, turnContext, vehicleDirectionNode, workWidth, steeringLength, distanceToFieldEdge) end -function ReedsSheppTurnManeuver:findAnalyticPath(vehicleDirectionNode, startOffset, turnEndNode, - xOffset, goalOffset, turningRadius) +function ReedsSheppTurnManeuver:findAnalyticPath(vehicleDirectionNode, startXOffset, startZOffset, turnEndNode, + endXOffset, endZOffset, turningRadius) local solver if self.turnContext:isLeftTurn() then self:debug('using LeftTurnReedsSheppSolver') @@ -461,12 +499,12 @@ function ReedsSheppTurnManeuver:findAnalyticPath(vehicleDirectionNode, startOffs self:debug('using RightTurnReedsSheppSolver') solver = RightTurnReedsSheppSolver() end - local path = PathfinderUtil.findAnalyticPath(solver, vehicleDirectionNode, startOffset, turnEndNode, - 0, goalOffset, self.turningRadius) + local path = PathfinderUtil.findAnalyticPath(solver, vehicleDirectionNode, startXOffset, startZOffset, turnEndNode, + 0, endZOffset, self.turningRadius) if not path or #path == 0 then self:debug('Could not find ReedsShepp path, retry with Dubins') - path = PathfinderUtil.findAnalyticPath(PathfinderUtil.dubinsSolver, vehicleDirectionNode, startOffset, - turnEndNode, 0, goalOffset, self.turningRadius) + path = PathfinderUtil.findAnalyticPath(PathfinderUtil.dubinsSolver, vehicleDirectionNode, startXOffset, startZOffset, + turnEndNode, 0, endZOffset, self.turningRadius) end local course = Course.createFromAnalyticPath(self.vehicle, path, true) course:adjustForTowedImplements(1.5 * self.steeringLength + 1) @@ -503,10 +541,7 @@ function TurnEndingManeuver:init(vehicle, turnContext, vehicleDirectionNode, tur self:generateStraightSection(endArc, endStraight) myCorner:delete() self.course = Course(vehicle, self.waypoints, true) - if self.applyTightTurnOffset then - self.course:setUseTightTurnOffsetForLastWaypoints( - g_vehicleConfigurations:getRecursively(vehicle, 'tightTurnOffsetDistanceInTurns') or 20) - end + self:applyTightTurnOffset() TurnManeuver.setLowerImplements(self.course, math.max(math.abs(turnContext.frontMarkerDistance), steeringLength)) end @@ -627,7 +662,7 @@ function VineTurnManeuver:init(vehicle, turnContext, vehicleDirectionNode, turni turningRadius, workWidth, dz, startOffset, goalOffset) local path = PathfinderUtil.findAnalyticPath(PathfinderUtil.dubinsSolver, -- always move the goal a bit backwards to let the vehicle align - vehicleDirectionNode, startOffset, turnEndNode, 0, goalOffset - turnContext.frontMarkerDistance, self.turningRadius) + vehicleDirectionNode, startOffset, 0, turnEndNode, 0, goalOffset - turnContext.frontMarkerDistance, self.turningRadius) self.course = Course.createFromAnalyticPath(self.vehicle, path, true) local endingTurnLength = self.turnContext:appendEndingTurnCourse(self.course, 0, false) TurnManeuver.setLowerImplements(self.course, endingTurnLength, true) diff --git a/scripts/pathfinder/PathfinderUtil.lua b/scripts/pathfinder/PathfinderUtil.lua index 164552fb..f9800981 100644 --- a/scripts/pathfinder/PathfinderUtil.lua +++ b/scripts/pathfinder/PathfinderUtil.lua @@ -604,7 +604,9 @@ end ------------------------------------------------------------------------------------------------------------------------ ---@param solver AnalyticSolver for instance PathfinderUtil.dubinsSolver or PathfinderUtil.reedsSheppSolver ---@param vehicleDirectionNode number Giants node ----@param startOffset number offset in meters relative to the vehicle position (forward positive, backward negative) where +---@param startXOffset number offset in meters relative to the vehicle position (left positive, right negative) where +--- we want the turn to start +---@param startZOffset number offset in meters relative to the vehicle position (forward positive, backward negative) where --- we want the turn to start ---@param goalReferenceNode table node used to determine the goal ---@param xOffset number offset in meters relative to the goal node (left positive, right negative) @@ -613,9 +615,9 @@ end ---@param turnRadius number vehicle turning radius ---@return table|nil path ---@return number length -function PathfinderUtil.findAnalyticPath(solver, vehicleDirectionNode, startOffset, goalReferenceNode, +function PathfinderUtil.findAnalyticPath(solver, vehicleDirectionNode, startXOffset, startZOffset, goalReferenceNode, xOffset, zOffset, turnRadius) - local x, z, yRot = PathfinderUtil.getNodePositionAndDirection(vehicleDirectionNode, 0, startOffset or 0) + local x, z, yRot = PathfinderUtil.getNodePositionAndDirection(vehicleDirectionNode, startXOffset, startZOffset or 0) local start = State3D(x, -z, CpMathUtil.angleFromGame(yRot)) x, z, yRot = PathfinderUtil.getNodePositionAndDirection(goalReferenceNode, xOffset or 0, zOffset or 0) local goal = State3D(x, -z, CpMathUtil.angleFromGame(yRot)) diff --git a/scripts/specializations/CpCourseManager.lua b/scripts/specializations/CpCourseManager.lua index f941826f..35ea9220 100644 --- a/scripts/specializations/CpCourseManager.lua +++ b/scripts/specializations/CpCourseManager.lua @@ -217,8 +217,6 @@ end function CpCourseManager:addCourse(course,noEventSend) local spec = self.spec_cpCourseManager - -- reset temporary offset field course, this will be regenerated based on the current settings when the job starts - spec.offsetFieldWorkCourse = nil course:setVehicle(self) table.insert(spec.courses,course) SpecializationUtil.raiseEvent(self,"onCpCourseChange",course,noEventSend) @@ -226,10 +224,11 @@ end function CpCourseManager:resetCourses() local spec = self.spec_cpCourseManager - spec.offsetFieldWorkCourse = nil - spec.courses = {} - spec.assignedCoursesID = nil - SpecializationUtil.raiseEvent(self,"onCpCourseChange") + if spec.courses then + spec.courses = {} + spec.assignedCoursesID = nil + SpecializationUtil.raiseEvent(self,"onCpCourseChange") + end end function CpCourseManager:resetCpCoursesFromGui()