diff --git a/config/VehicleConfigurations.xml b/config/VehicleConfigurations.xml index dded4f33b..3e9fe4c45 100644 --- a/config/VehicleConfigurations.xml +++ b/config/VehicleConfigurations.xml @@ -551,7 +551,7 @@ You can define the following custom settings: 0.01 then + straight = false + end + i = i + 1 + end + -- i points to the row end, add it to the row + local rowEndIx = i + table.insert(row, waypoints[i]) + if straight and compact then + -- only save the row start and end + waypoints[rowStartIx]:setXmlValue(xmlFile, key, xmlIx) + waypoints[rowEndIx]:setXmlValue(xmlFile, key, xmlIx + 1) + return rowEndIx + 1, xmlIx + 2 + else + i = xmlIx + -- save all waypoints + for _, wp in ipairs(row) do + wp:setXmlValue(xmlFile, key, i) + i = i + 1 + end + return rowEndIx + 1, i + end +end + ---@param compact boolean skip waypoints between row start and end (as for straight rows, these can be regenerated --- easily after the course is loaded) local function saveWaypointsToXml(waypoints, xmlFile, key, compact) - local i = 1 - for _, wp in ipairs(waypoints) do - if not compact or (not wp:getRowNumber() or wp:isRowStart() or wp:isRowEnd()) then - wp:setXmlValue(xmlFile, key, i) - i = i + 1 + local wpIx, xmlIx = 1, 1 + while wpIx <= #waypoints do + local wp = waypoints[wpIx] + if wp:isRowStart() then + wpIx, xmlIx = saveRowToXml(waypoints, xmlFile, key, compact, wpIx, xmlIx) + else + wp:setXmlValue(xmlFile, key, xmlIx) + wpIx = wpIx + 1 + xmlIx = xmlIx + 1 end end end @@ -1492,7 +1520,7 @@ end -- points can be generated by this function local function addIntermediateWaypoints(d, waypoints, rowStart, rowEnd) local dx, dz = (rowEnd.x - rowStart.x) / d, (rowEnd.z - rowStart.z) / d - for n = 1, (d / CourseGenerator.cRowWaypointDistance) - 1 do + for n = 1, math.floor((d -1) / CourseGenerator.cRowWaypointDistance) do local newWp = Waypoint({}) newWp.x = rowStart.x + n * CourseGenerator.cRowWaypointDistance * dx newWp.z = rowStart.z + n * CourseGenerator.cRowWaypointDistance * dz @@ -1719,7 +1747,7 @@ end function Course.MultiVehicleData:copy() local copy = Course.MultiVehicleData(self.position) for position, waypoints in pairs(self.waypoints) do - copy.waypoints[position] = {} + copy.waypoints[position] = Course.initWaypoints() for i, wp in ipairs(waypoints) do table.insert(copy.waypoints[position], Waypoint(wp)) end diff --git a/scripts/Waypoint.lua b/scripts/Waypoint.lua index 9bf7a7363..bea476c72 100644 --- a/scripts/Waypoint.lua +++ b/scripts/Waypoint.lua @@ -203,8 +203,8 @@ function Waypoint:setPosition(x, z, y) end end -function Waypoint:setReverseOffset() - self.reverseOffset = true +function Waypoint:setReverseOffset(reverseOffset) + self.reverseOffset = reverseOffset end function Waypoint:translate(dx, dz) diff --git a/scripts/ai/turns/AITurn.lua b/scripts/ai/turns/AITurn.lua index 716d0ccd8..db34c5b2c 100644 --- a/scripts/ai/turns/AITurn.lua +++ b/scripts/ai/turns/AITurn.lua @@ -275,6 +275,11 @@ end --- Give back control the the drive strategy function AITurn:resumeFieldworkAfterTurn(ix) + -- just in case, raise this event so plows are rotated to the working position. Should really never end up + -- here though, as the course should be long enough for the normal turn end processing to be triggered. + self.driveStrategy:raiseControllerEvent(AIDriveStrategyCourse.onTurnEndProgressEvent, + self.turnContext.workStartNode, self.ppc:isReversing(), true, self.turnContext:isLeftTurn()) + if self.proximityController then self.proximityController:unregisterBlockingObjectListener() end @@ -638,10 +643,13 @@ function CourseTurn:onWaypointChange(ix) if self.turnCourse then if self.forceTightTurnOffset or (self.enableTightTurnOffset and self.turnCourse:useTightTurnOffset(ix)) then -- adjust the course a bit to the outside in a curve to keep a towed implement on the course - -- TODO_22 self.tightTurnOffset = AIUtil.calculateTightTurnOffset(self.vehicle, self.turningRadius, self.turnCourse, self.tightTurnOffset, true) self.turnCourse:setOffset(self.tightTurnOffset, 0) + else + -- reset offset to 0 if tight turn offset is not on + self.tightTurnOffset = 0 + self.turnCourse:setOffset(0, 0) end end end diff --git a/scripts/ai/turns/TurnManeuver.lua b/scripts/ai/turns/TurnManeuver.lua index f889a299b..3dbf1d598 100644 --- a/scripts/ai/turns/TurnManeuver.lua +++ b/scripts/ai/turns/TurnManeuver.lua @@ -21,8 +21,8 @@ TurnManeuver.applyTightTurnOffset = true ---@param course Course function TurnManeuver.hasTurnControl(course, ix, control) - local controls = course:getTurnControls(ix) - return controls and controls[control] + local controls = course:getTurnControls(ix) + return controls and controls[control] end ---@param vehicle table only used for debug, to get the name of the vehicle @@ -33,73 +33,75 @@ end ---@param steeringLength number distance between the tractor's rear axle and the towed implement/trailer's rear axle, --- roughly tells how far we need to pull ahead (or back) relative to our target until the entire rig reaches that target. function TurnManeuver:init(vehicle, turnContext, vehicleDirectionNode, turningRadius, workWidth, steeringLength) - self.vehicleDirectionNode = vehicleDirectionNode - self.turnContext = turnContext - self.vehicle = vehicle - self.waypoints = {} - self.turningRadius = turningRadius - self.workWidth = workWidth - self.steeringLength = steeringLength - self.direction = turnContext:isLeftTurn() and -1 or 1 - -- 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.vehicleDirectionNode = vehicleDirectionNode + self.turnContext = turnContext + self.vehicle = vehicle + self.waypoints = {} + self.turningRadius = turningRadius + self.workWidth = workWidth + self.steeringLength = steeringLength + self.direction = turnContext:isLeftTurn() and -1 or 1 + -- 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 end function TurnManeuver:getCourse() - return self.course + return self.course end function TurnManeuver:debug(...) - CpUtil.debugVehicle(CpDebug.DBG_TURN, self.vehicle, self.debugPrefix .. string.format(...)) + CpUtil.debugVehicle(CpDebug.DBG_TURN, self.vehicle, self.debugPrefix .. string.format(...)) end ---@param course Course function TurnManeuver:getDzMax(course) - local dzMax = -math.huge - for ix = 1, course:getNumberOfWaypoints() do - local _, _, dz = course:getWaypointLocalPosition(self.vehicleDirectionNode, ix) - dzMax = dz > dzMax and dz or dzMax - end - return dzMax + local dzMax = -math.huge + for ix = 1, course:getNumberOfWaypoints() do + local _, _, dz = course:getWaypointLocalPosition(self.vehicleDirectionNode, ix) + dzMax = dz > dzMax and dz or dzMax + end + return dzMax end function TurnManeuver:generateStraightSection(fromPoint, toPoint, reverse, turnEnd, - secondaryReverseDistance, doNotAddLastPoint) - local dist = MathUtil.getPointPointDistance(fromPoint.x, fromPoint.z, toPoint.x, toPoint.z) - local numPointsNeeded = math.ceil(dist / TurnManeuver.wpDistance) - local dx, dz = (toPoint.x - fromPoint.x) / dist, (toPoint.z - fromPoint.z) / dist - - -- add first point - self:addWaypoint(fromPoint.x, fromPoint.z, turnEnd, reverse, nil) - local fromIx = #self.waypoints - - -- add points between the first and last - local x, z - if numPointsNeeded > 1 then - local wpDistance = dist / numPointsNeeded - for i = 1, numPointsNeeded - 1 do - x = fromPoint.x + (i * wpDistance * dx) - z = fromPoint.z + (i * wpDistance * dz) - - self:addWaypoint(x, z, turnEnd, reverse, nil) - end - end - - if doNotAddLastPoint then return fromIx, #self.waypoints end - - -- add last point - local revx, revz - if reverse and secondaryReverseDistance then - revx = toPoint.x + (secondaryReverseDistance * dx) - revz = toPoint.z + (secondaryReverseDistance * dz) - end - - x = toPoint.x - z = toPoint.z - - self:addWaypoint(x, z, turnEnd, reverse, revx, revz, nil) - return fromIx, #self.waypoints + secondaryReverseDistance, doNotAddLastPoint) + local dist = MathUtil.getPointPointDistance(fromPoint.x, fromPoint.z, toPoint.x, toPoint.z) + local numPointsNeeded = math.ceil(dist / TurnManeuver.wpDistance) + local dx, dz = (toPoint.x - fromPoint.x) / dist, (toPoint.z - fromPoint.z) / dist + + -- add first point + self:addWaypoint(fromPoint.x, fromPoint.z, turnEnd, reverse, nil) + local fromIx = #self.waypoints + + -- add points between the first and last + local x, z + if numPointsNeeded > 1 then + local wpDistance = dist / numPointsNeeded + for i = 1, numPointsNeeded - 1 do + x = fromPoint.x + (i * wpDistance * dx) + z = fromPoint.z + (i * wpDistance * dz) + + self:addWaypoint(x, z, turnEnd, reverse, nil) + end + end + + if doNotAddLastPoint then + return fromIx, #self.waypoints + end + + -- add last point + local revx, revz + if reverse and secondaryReverseDistance then + revx = toPoint.x + (secondaryReverseDistance * dx) + revz = toPoint.z + (secondaryReverseDistance * dz) + end + + x = toPoint.x + z = toPoint.z + + self:addWaypoint(x, z, turnEnd, reverse, revx, revz, nil) + return fromIx, #self.waypoints end -- startDir and stopDir are points (x,z). The arc starts where the line from the center of the circle @@ -107,150 +109,154 @@ end -- intersects the circle. -- function TurnManeuver:generateTurnCircle(center, startDir, stopDir, radius, clockwise, addEndPoint, reverse) - -- Convert clockwise to the right format - if clockwise == nil then clockwise = 1 end - if clockwise == false or clockwise < 0 then - clockwise = -1 - else - clockwise = 1 - end - - -- Define some basic values to use - local numWP = 1 - local degreeToTurn = 0 - local wpDistance = 1 - local degreeStep = 360 / (2 * radius * math.pi) * wpDistance - local startRot = 0 - local endRot = 0 - - -- Get the start and end rotation - local dx, dz = CpMathUtil.getPointDirection(center, startDir, false) - startRot = math.deg(MathUtil.getYRotationFromDirection(dx, dz)) - dx, dz = CpMathUtil.getPointDirection(center, stopDir, false) - endRot = math.deg(MathUtil.getYRotationFromDirection(dx, dz)) - - -- Create new transformGroupe to use for placing waypoints - local point = createTransformGroup("cpTempGenerateTurnCircle") - link(g_currentMission.terrainRootNode, point) - - -- Move the point to the center - local cY = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, center.x, 300, center.z) - setTranslation(point, center.x, cY, center.z) - - -- Rotate it to the start direction - setRotation(point, 0, math.rad(startRot), 0) - - -- Fix the rotation values in some special cases - if clockwise == 1 then - --(Turn:generateTurnCircle) startRot=90, endRot=-29, degreeStep=20, degreeToTurn=240, clockwise=1 - if startRot > endRot then - degreeToTurn = endRot + 360 - startRot - else - degreeToTurn = endRot - startRot - end - else - --(Turn:generateTurnCircle) startRot=150, endRot=90, degreeStep=-20, degreeToTurn=60, clockwise=-1 - if startRot < endRot then - degreeToTurn = startRot + 360 - endRot - else - degreeToTurn = startRot - endRot - end - end - self:debug("generateTurnCircle: startRot=%d, endRot=%d, degreeStep=%d, degreeToTurn=%d, clockwise=%d", - startRot, endRot, (degreeStep * clockwise), degreeToTurn, clockwise) - - -- Get the number of waypoints - numWP = math.ceil(degreeToTurn / degreeStep) - -- Recalculate degreeStep - degreeStep = (degreeToTurn / numWP) * clockwise - -- Add extra waypoint if addEndPoint is true - if addEndPoint then numWP = numWP + 1 end - - self:debug("generateTurnCircle: numberOfWaypoints=%d, newDegreeStep=%d", numWP, degreeStep) - - -- Generate the waypoints - for i = 1, numWP, 1 do - if i ~= 1 then - local _,currentRot,_ = getRotation(point) - local newRot = math.deg(currentRot) + degreeStep - - setRotation(point, 0, math.rad(newRot), 0) - end - - local x,_,z = localToWorld(point, 0, 0, radius) - self:addWaypoint(x, z, nil, reverse, nil, nil, true) - - local _,rot,_ = getRotation(point) - self:debug("generateTurnCircle: waypoint %d currentRotation=%d", i, math.deg(rot)) - end - - -- Clean up the created node. - unlink(point) - delete(point) + -- Convert clockwise to the right format + if clockwise == nil then + clockwise = 1 + end + if clockwise == false or clockwise < 0 then + clockwise = -1 + else + clockwise = 1 + end + + -- Define some basic values to use + local numWP = 1 + local degreeToTurn = 0 + local wpDistance = 1 + local degreeStep = 360 / (2 * radius * math.pi) * wpDistance + local startRot = 0 + local endRot = 0 + + -- Get the start and end rotation + local dx, dz = CpMathUtil.getPointDirection(center, startDir, false) + startRot = math.deg(MathUtil.getYRotationFromDirection(dx, dz)) + dx, dz = CpMathUtil.getPointDirection(center, stopDir, false) + endRot = math.deg(MathUtil.getYRotationFromDirection(dx, dz)) + + -- Create new transformGroupe to use for placing waypoints + local point = createTransformGroup("cpTempGenerateTurnCircle") + link(g_currentMission.terrainRootNode, point) + + -- Move the point to the center + local cY = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, center.x, 300, center.z) + setTranslation(point, center.x, cY, center.z) + + -- Rotate it to the start direction + setRotation(point, 0, math.rad(startRot), 0) + + -- Fix the rotation values in some special cases + if clockwise == 1 then + --(Turn:generateTurnCircle) startRot=90, endRot=-29, degreeStep=20, degreeToTurn=240, clockwise=1 + if startRot > endRot then + degreeToTurn = endRot + 360 - startRot + else + degreeToTurn = endRot - startRot + end + else + --(Turn:generateTurnCircle) startRot=150, endRot=90, degreeStep=-20, degreeToTurn=60, clockwise=-1 + if startRot < endRot then + degreeToTurn = startRot + 360 - endRot + else + degreeToTurn = startRot - endRot + end + end + self:debug("generateTurnCircle: startRot=%d, endRot=%d, degreeStep=%d, degreeToTurn=%d, clockwise=%d", + startRot, endRot, (degreeStep * clockwise), degreeToTurn, clockwise) + + -- Get the number of waypoints + numWP = math.ceil(degreeToTurn / degreeStep) + -- Recalculate degreeStep + degreeStep = (degreeToTurn / numWP) * clockwise + -- Add extra waypoint if addEndPoint is true + if addEndPoint then + numWP = numWP + 1 + end + + self:debug("generateTurnCircle: numberOfWaypoints=%d, newDegreeStep=%d", numWP, degreeStep) + + -- Generate the waypoints + for i = 1, numWP, 1 do + if i ~= 1 then + local _, currentRot, _ = getRotation(point) + local newRot = math.deg(currentRot) + degreeStep + + setRotation(point, 0, math.rad(newRot), 0) + end + + local x, _, z = localToWorld(point, 0, 0, radius) + self:addWaypoint(x, z, nil, reverse, nil, nil, true) + + local _, rot, _ = getRotation(point) + self:debug("generateTurnCircle: waypoint %d currentRotation=%d", i, math.deg(rot)) + end + + -- Clean up the created node. + unlink(point) + delete(point) end function TurnManeuver:addWaypoint(x, z, turnEnd, reverse, dontPrint) - local wp = {} - wp.x = x - wp.z = z - if turnEnd then - TurnManeuver.addTurnControlToWaypoint(wp, TurnManeuver.LOWER_IMPLEMENT_AT_TURN_END, true) - end - wp.reverse = reverse - table.insert(self.waypoints, wp) - local dz = worldToLocal(self.vehicleDirectionNode, wp.x, 0, wp.z) - self.dzMax = dz > self.dzMax and dz or self.dzMax - if not dontPrint then - self:debug("addWaypoint %d: x=%.2f, z=%.2f, dz=%.1f, turnEnd=%s, reverse=%s", - #self.waypoints, x, z, dz, - tostring(turnEnd and true or false), tostring(reverse and true or false)) - end + local wp = {} + wp.x = x + wp.z = z + if turnEnd then + TurnManeuver.addTurnControlToWaypoint(wp, TurnManeuver.LOWER_IMPLEMENT_AT_TURN_END, true) + end + wp.reverse = reverse + table.insert(self.waypoints, wp) + local dz = worldToLocal(self.vehicleDirectionNode, wp.x, 0, wp.z) + self.dzMax = dz > self.dzMax and dz or self.dzMax + if not dontPrint then + self:debug("addWaypoint %d: x=%.2f, z=%.2f, dz=%.1f, turnEnd=%s, reverse=%s", + #self.waypoints, x, z, dz, + tostring(turnEnd and true or false), tostring(reverse and true or false)) + end end function TurnManeuver.addTurnControlToWaypoint(wp, control, value) - if not wp.turnControls then - wp.turnControls = {} - end - wp.turnControls[control] = value + if not wp.turnControls then + wp.turnControls = {} + end + wp.turnControls[control] = value end function TurnManeuver.addTurnControl(waypoints, fromIx, toIx, control, value) - for i = fromIx, toIx do - TurnManeuver.addTurnControlToWaypoint(waypoints[i], control, value) - end + for i = fromIx, toIx do + TurnManeuver.addTurnControlToWaypoint(waypoints[i], control, value) + end end --- Set the given control to value for all waypoints of course within d meters of the course end ---@param stopAtDirectionChange boolean if we reach a direction change, stop there, the last waypoint the function --- is called for is the one before the direction change function TurnManeuver.setTurnControlForLastWaypoints(course, d, control, value, stopAtDirectionChange) - course:executeFunctionForLastWaypoints( - d, - function(wp) - TurnManeuver.addTurnControlToWaypoint(wp, control, value) - end, - stopAtDirectionChange) + course:executeFunctionForLastWaypoints( + d, + function(wp) + TurnManeuver.addTurnControlToWaypoint(wp, control, value) + end, + stopAtDirectionChange) end --- Get the distance between the direction node of the vehicle and the reverser node (if there is one). This --- is to make sure that when the course changes to reverse and there is a reverse node, the first reverse --- waypoint is behind the reverser node. Otherwise we'll just keep backing up until the emergency brake is triggered. function TurnManeuver:getReversingOffset() - local reverserNode, debugText = AIUtil.getReverserNode(self.vehicle) - if reverserNode then - local _, _, dz = localToLocal(reverserNode, self.vehicleDirectionNode, 0, 0, 0) - self:debug('Using reverser node (%s) distance %.1f', debugText, dz) - return math.abs(dz) - end - return self.steeringLength + local reverserNode, debugText = AIUtil.getReverserNode(self.vehicle) + if reverserNode then + local _, _, dz = localToLocal(reverserNode, self.vehicleDirectionNode, 0, 0, 0) + self:debug('Using reverser node (%s) distance %.1f', debugText, dz) + return math.abs(dz) + end + return self.steeringLength end --- Set implement lowering control for the end of the turn ---@param stopAtDirectionChange boolean if we reach a direction change, stop there, the last waypoint the function --- is called for is the one before the direction change function TurnManeuver.setLowerImplements(course, distance, stopAtDirectionChange) - TurnManeuver.setTurnControlForLastWaypoints(course, math.max(distance, 3) + 2, - TurnManeuver.LOWER_IMPLEMENT_AT_TURN_END, true, stopAtDirectionChange) + TurnManeuver.setTurnControlForLastWaypoints(course, math.max(distance, 3) + 2, + TurnManeuver.LOWER_IMPLEMENT_AT_TURN_END, true, stopAtDirectionChange) end -- Add reversing sections at the beginning and end of the turn, so the vehicle can make the turn without @@ -259,199 +265,212 @@ end ---@param dBack number distance in meters to move the course back (positive moves it backwards!) ---@param ixBeforeEndingTurnSection number index of the last waypoint of the actual turn, if we can finish the turn --- before we reach the vehicle position at turn end, there's no reversing needed at the turn end. ----@param endingTurnLength number length of the straight ending turn section into the next row -function TurnManeuver:adjustCourseToFitField(course, dBack, ixBeforeEndingTurnSection, endingTurnLength) - self:debug('moving course back: d=%.1f', dBack) - local reversingOffset = self:getReversingOffset() - -- generate a straight reverse section first (less than 1 m step should make sure we always end up with - -- at least two waypoints - local courseWithReversing = Course.createFromNode(self.vehicle, self.vehicle:getAIDirectionNode(), - 0, -reversingOffset, -reversingOffset - dBack, -0.9, true) - -- now add the actual turn, which has already been shifted back before this function was called - courseWithReversing:append(course) - -- the last waypoint of the course after it was translated - local _, _, dFromTurnEnd = course:getWaypointLocalPosition(self.turnContext.vehicleAtTurnEndNode, ixBeforeEndingTurnSection) - local _, _, dFromWorkStart = course:getWaypointLocalPosition(self.turnContext.workStartNode, ixBeforeEndingTurnSection) - self:debug('Work start from curve end %.1f, vehicle at %.1f, %.1f between vehicle and work start)', - dFromWorkStart, dFromTurnEnd, self.turnContext.turnEndForwardOffset) - if self.turnContext.turnEndForwardOffset > 0 and math.max(dFromTurnEnd, dFromWorkStart) > -self.steeringLength then - self:debug('Reverse to work start (implement in back)') - -- vehicle in front of the work start node at turn end - -- allow early direction change when aligned - TurnManeuver.setTurnControlForLastWaypoints(courseWithReversing, endingTurnLength, - TurnManeuver.CHANGE_DIRECTION_WHEN_ALIGNED, true, true) - -- go all the way to the back marker distance so there's plenty of room for lower early too, also, the - -- reversingOffset may be even behind the back marker, especially for vehicles which have a AIToolReverserDirectionNode - -- which is then used as the PPC controlled node, and thus it must be far enough that we reach the lowering - -- point before the controlled node reaches the end of the course - local reverseAfterTurn = Course.createFromNode(self.vehicle, self.turnContext.vehicleAtTurnEndNode, - 0, dFromTurnEnd + self.steeringLength, - math.min(dFromTurnEnd, self.turnContext.backMarkerDistance, -reversingOffset), -0.8, true) - courseWithReversing:append(reverseAfterTurn) - elseif self.turnContext.turnEndForwardOffset <= 0 and dFromTurnEnd >= 0 then - self:debug('Reverse to work start (implement in front)') - -- the work start is in front of the vehicle at the turn end - TurnManeuver.setTurnControlForLastWaypoints(courseWithReversing, endingTurnLength, - TurnManeuver.CHANGE_DIRECTION_WHEN_ALIGNED, true, true) - local reverseAfterTurn = Course.createFromNode(self.vehicle, self.turnContext.workStartNode, - 0, -reversingOffset, -reversingOffset + self.turnContext.turnEndForwardOffset, -1, true) - courseWithReversing:append(reverseAfterTurn) - end - return courseWithReversing +function TurnManeuver:adjustCourseToFitField(course, dBack, ixBeforeEndingTurnSection) + self:debug('moving course back: d=%.1f', dBack) + local endingTurnLength + local reversingOffset = self:getReversingOffset() + -- generate a straight reverse section first (less than 1 m step should make sure we always end up with + -- at least two waypoints + local courseWithReversing = Course.createFromNode(self.vehicle, self.vehicle:getAIDirectionNode(), + 0, -reversingOffset, -reversingOffset - dBack, -0.9, true) + -- now add the actual turn, which has already been shifted back before this function was called + courseWithReversing:append(course) + -- the last waypoint of the course after it was translated + local _, _, dFromTurnEnd = course:getWaypointLocalPosition(self.turnContext.vehicleAtTurnEndNode, ixBeforeEndingTurnSection) + local _, _, dFromWorkStart = course:getWaypointLocalPosition(self.turnContext.workStartNode, ixBeforeEndingTurnSection) + self:debug('Curve end from work start %.1f, from vehicle at turn end %.1f, %.1f between vehicle and work start)', + dFromWorkStart, dFromTurnEnd, self.turnContext.turnEndForwardOffset) + if self.turnContext.turnEndForwardOffset > 0 and math.max(dFromTurnEnd, dFromWorkStart) > -self.steeringLength then + 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) + courseWithReversing:append(forwardAfterTurn) + -- allow early direction change when aligned + TurnManeuver.setTurnControlForLastWaypoints(courseWithReversing, forwardAfterTurn:getLength(), + TurnManeuver.CHANGE_DIRECTION_WHEN_ALIGNED, true, true) + -- go all the way to the back marker distance so there's plenty of room for lower early too, also, the + -- reversingOffset may be even behind the back marker, especially for vehicles which have a AIToolReverserDirectionNode + -- which is then used as the PPC controlled node, and thus it must be far enough that we reach the lowering + -- point before the controlled node reaches the end of the course + local reverseAfterTurn = Course.createFromNode(self.vehicle, self.turnContext.vehicleAtTurnEndNode, + 0, dFromTurnEnd + self.steeringLength - 1, + math.min(dFromTurnEnd, self.turnContext.backMarkerDistance, -reversingOffset), -0.8, true) + courseWithReversing:append(reverseAfterTurn) + endingTurnLength = reverseAfterTurn:getLength() + elseif self.turnContext.turnEndForwardOffset <= 0 and dFromTurnEnd >= 0 then + self:debug('Reverse to work start (implement in front)') + -- the work start is in front of the vehicle at the turn end + if dFromWorkStart < 0 then + -- need a little piece forward to get to the reverse course's start + local forwardAfterTurn = Course.createFromNode(self.vehicle, self.turnContext.workStartNode, 0, + dFromWorkStart, 1, 0.8, false) + courseWithReversing:append(forwardAfterTurn) + TurnManeuver.setTurnControlForLastWaypoints(courseWithReversing, forwardAfterTurn:getLength(), + TurnManeuver.CHANGE_DIRECTION_WHEN_ALIGNED, true, true) + end + local reverseAfterTurn = Course.createFromNode(self.vehicle, self.turnContext.workStartNode, + 0, -reversingOffset, -reversingOffset + self.turnContext.turnEndForwardOffset, -1, true) + courseWithReversing:append(reverseAfterTurn) + endingTurnLength = reverseAfterTurn:getLength() + else + self:debug('Reverse to work start not needed') + endingTurnLength = self.turnContext:appendEndingTurnCourse(courseWithReversing, self.steeringLength, self.applyTightTurnOffset) + end + return courseWithReversing, endingTurnLength 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 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 - local ixBeforeEndingTurnSection = self.course:getNumberOfWaypoints() - endingTurnLength = self.turnContext:appendEndingTurnCourse(self.course, steeringLength, self.applyTightTurnOffset) - self.course = self:adjustCourseToFitField(self.course, dBack, ixBeforeEndingTurnSection, endingTurnLength) - 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) - end - TurnManeuver.setLowerImplements(self.course, endingTurnLength, true) + 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 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 + 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) + end + TurnManeuver.setLowerImplements(self.course, endingTurnLength, true) end ---@return number How far back this course needs to be moved back to stay on the field function AnalyticTurnManeuver:getDistanceToMoveBack(course, workWidth, distanceToFieldEdge) - 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 - -- 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) - return spaceNeededOnFieldForTurn - distanceToFieldEdge + 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 + -- 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) + return spaceNeededOnFieldForTurn - distanceToFieldEdge end ---@class DubinsTurnManeuver : AnalyticTurnManeuver DubinsTurnManeuver = CpObject(AnalyticTurnManeuver) function DubinsTurnManeuver:init(vehicle, turnContext, vehicleDirectionNode, turningRadius, - workWidth, steeringLength, distanceToFieldEdge) - self.debugPrefix = '(DubinsTurn): ' - AnalyticTurnManeuver.init(self, vehicle, turnContext, vehicleDirectionNode, turningRadius, - workWidth, steeringLength, distanceToFieldEdge) + workWidth, steeringLength, distanceToFieldEdge) + self.debugPrefix = '(DubinsTurn): ' + AnalyticTurnManeuver.init(self, vehicle, turnContext, vehicleDirectionNode, turningRadius, + workWidth, steeringLength, distanceToFieldEdge) end function DubinsTurnManeuver:findAnalyticPath(vehicleDirectionNode, startOffset, turnEndNode, - xOffset, goalOffset, turningRadius) - local path = PathfinderUtil.findAnalyticPath(PathfinderUtil.dubinsSolver, - vehicleDirectionNode, startOffset, turnEndNode, 0, goalOffset, self.turningRadius) - return Course.createFromAnalyticPath(self.vehicle, path, true) + xOffset, goalOffset, turningRadius) + local path = PathfinderUtil.findAnalyticPath(PathfinderUtil.dubinsSolver, + vehicleDirectionNode, startOffset, turnEndNode, 0, goalOffset, self.turningRadius) + return Course.createFromAnalyticPath(self.vehicle, path, true) end ---@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) - AnalyticTurnManeuver.init(self, vehicle, turnContext, vehicleDirectionNode, turningRadius, - workWidth, steeringLength, distanceToFieldEdge) + 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) + 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 + 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)) + 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) - return ReedsSheppSolver.solve(self, start, goal, turnRadius, {ReedsShepp.PathWords.LfRbLf}) + return ReedsSheppSolver.solve(self, start, goal, turnRadius, { ReedsShepp.PathWords.LfRbLf }) end ---@class LeftTurnReverseReedsSheppSolver : ReedsSheppSolver LeftTurnReverseReedsSheppSolver = CpObject(ReedsSheppSolver) function LeftTurnReverseReedsSheppSolver:solve(start, goal, turnRadius) - return ReedsSheppSolver.solve(self, start, goal, turnRadius, {ReedsShepp.PathWords.LbSbLb}) + return ReedsSheppSolver.solve(self, start, goal, turnRadius, { ReedsShepp.PathWords.LbSbLb }) end ---@class RightTurnReedsSheppSolver : ReedsSheppSolver RightTurnReedsSheppSolver = CpObject(ReedsSheppSolver) function RightTurnReedsSheppSolver:solve(start, goal, turnRadius) - return ReedsSheppSolver.solve(self, start, goal, turnRadius, {ReedsShepp.PathWords.RfLbRf}) + return ReedsSheppSolver.solve(self, start, goal, turnRadius, { ReedsShepp.PathWords.RfLbRf }) end ---@class ReedsSheppTurnManeuver : AnalyticTurnManeuver ReedsSheppTurnManeuver = CpObject(AnalyticTurnManeuver) function ReedsSheppTurnManeuver:init(vehicle, turnContext, vehicleDirectionNode, turningRadius, - workWidth, steeringLength, distanceToFieldEdge) - self.debugPrefix = '(ReedsSheppTurn): ' - AnalyticTurnManeuver.init(self, vehicle, turnContext, vehicleDirectionNode, turningRadius, - workWidth, steeringLength, distanceToFieldEdge) + workWidth, steeringLength, distanceToFieldEdge) + self.debugPrefix = '(ReedsSheppTurn): ' + AnalyticTurnManeuver.init(self, vehicle, turnContext, vehicleDirectionNode, turningRadius, + workWidth, steeringLength, distanceToFieldEdge) end function ReedsSheppTurnManeuver:findAnalyticPath(vehicleDirectionNode, startOffset, turnEndNode, - xOffset, goalOffset, turningRadius) - local solver - if self.turnContext:isLeftTurn() then - self:debug('using LeftTurnReedsSheppSolver') - solver = LeftTurnReedsSheppSolver() - else - self:debug('using RightTurnReedsSheppSolver') - solver = RightTurnReedsSheppSolver() - end - local path = PathfinderUtil.findAnalyticPath(solver, vehicleDirectionNode, startOffset, turnEndNode, - 0, goalOffset, 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) - end - local course = Course.createFromAnalyticPath(self.vehicle, path, true) - course:adjustForTowedImplements(1.5 * self.steeringLength + 1) - return course + xOffset, goalOffset, turningRadius) + local solver + if self.turnContext:isLeftTurn() then + self:debug('using LeftTurnReedsSheppSolver') + solver = LeftTurnReedsSheppSolver() + else + self:debug('using RightTurnReedsSheppSolver') + solver = RightTurnReedsSheppSolver() + end + local path = PathfinderUtil.findAnalyticPath(solver, vehicleDirectionNode, startOffset, turnEndNode, + 0, goalOffset, 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) + end + local course = Course.createFromAnalyticPath(self.vehicle, path, true) + course:adjustForTowedImplements(1.5 * self.steeringLength + 1) + return course end ---@class TurnEndingManeuver : TurnManeuver @@ -462,33 +481,33 @@ TurnEndingManeuver = CpObject(TurnManeuver) --- Could be using Dubins but that may end up generating a full circle if there's not enough room, even if we --- miss it by just a few centimeters function TurnEndingManeuver:init(vehicle, turnContext, vehicleDirectionNode, turningRadius, workWidth, steeringLength) - self.debugPrefix = '(TurnEnding): ' - TurnManeuver.init(self, vehicle, turnContext, vehicleDirectionNode, turningRadius, workWidth, steeringLength) - self:debug('Start generating') - self:debug('r=%.1f, w=%.1f', turningRadius, workWidth) - - local startAngle = math.deg(CpMathUtil.getNodeDirection(vehicleDirectionNode)) - local r = turningRadius - local startPos, endPos = {}, {} - startPos.x, _, startPos.z = getWorldTranslation(vehicleDirectionNode) - endPos.x, _, endPos.z = getWorldTranslation(turnContext.vehicleAtTurnEndNode) - -- use side offset 0 as all the offsets is already included in the vehicleAtTurnEndNode - local myCorner = Corner(vehicle, startAngle, startPos, self.turnContext.turnEndWp.angle, endPos , r, 0) - local center = myCorner:getArcCenter() - local startArc = myCorner:getArcStart() - local endArc = myCorner:getArcEnd() - self:generateTurnCircle(center, startArc, endArc, r, self.turnContext:isLeftTurn() and 1 or -1, false) - -- make sure course reaches the front marker node so end it well behind that node - local endStraight = {} - endStraight.x, _, endStraight.z = localToWorld(self.turnContext.vehicleAtTurnEndNode, 0, 0, 3) - 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 - TurnManeuver.setLowerImplements(self.course, math.max(math.abs(turnContext.frontMarkerDistance), steeringLength)) + self.debugPrefix = '(TurnEnding): ' + TurnManeuver.init(self, vehicle, turnContext, vehicleDirectionNode, turningRadius, workWidth, steeringLength) + self:debug('Start generating') + self:debug('r=%.1f, w=%.1f', turningRadius, workWidth) + + local startAngle = math.deg(CpMathUtil.getNodeDirection(vehicleDirectionNode)) + local r = turningRadius + local startPos, endPos = {}, {} + startPos.x, _, startPos.z = getWorldTranslation(vehicleDirectionNode) + endPos.x, _, endPos.z = getWorldTranslation(turnContext.vehicleAtTurnEndNode) + -- use side offset 0 as all the offsets is already included in the vehicleAtTurnEndNode + local myCorner = Corner(vehicle, startAngle, startPos, self.turnContext.turnEndWp.angle, endPos, r, 0) + local center = myCorner:getArcCenter() + local startArc = myCorner:getArcStart() + local endArc = myCorner:getArcEnd() + self:generateTurnCircle(center, startArc, endArc, r, self.turnContext:isLeftTurn() and 1 or -1, false) + -- make sure course reaches the front marker node so end it well behind that node + local endStraight = {} + endStraight.x, _, endStraight.z = localToWorld(self.turnContext.vehicleAtTurnEndNode, 0, 0, 3) + 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 + TurnManeuver.setLowerImplements(self.course, math.max(math.abs(turnContext.frontMarkerDistance), steeringLength)) end ---@class HeadlandCornerTurnManeuver : TurnManeuver @@ -501,55 +520,55 @@ HeadlandCornerTurnManeuver = CpObject(TurnManeuver) ------------------------------------------------------------------------ ---@param turnContext TurnContext function HeadlandCornerTurnManeuver:init(vehicle, turnContext, vehicleDirectionNode, turningRadius, workWidth, - reversingWorkTool, steeringLength) - TurnManeuver.init(self, vehicle, turnContext, vehicleDirectionNode, turningRadius, workWidth, steeringLength) - self.debugPrefix = '(HeadlandTurn): ' - self:debug('Start generating') - self:debug('r=%.1f, w=%.1f, steeringLength=%.1f', turningRadius, workWidth, steeringLength) - local fromPoint, toPoint = {}, {} - - local corner = turnContext:createCorner(vehicle, self.turningRadius) - - local centerForward = corner:getArcCenter() - local helperNode = CpUtil.createNode('tmp', 0, 0, 0, self.vehicleDirectionNode) - - -- in reverse our reference point is the implement's turn node so put the first reverse waypoint behind us - fromPoint.x, _, fromPoint.z = localToWorld(self.vehicleDirectionNode, 0, 0, - self.steeringLength ) - - -- now back up so the tractor is at the start of the arc - toPoint = corner:getPointAtDistanceFromArcStart(2 * self.steeringLength + self.reverseBuffer) - -- helper node is where we would be at this point of the turn, so check if next target is behind or in front of us - _, _, dz = worldToLocal( helperNode, toPoint.x, toPoint.y, toPoint.z ) - CpUtil.destroyNode(helperNode) - self:debug("from ( %.2f %.2f ), to ( %.2f %.2f) workWidth: %.1f, dz = %.1f", - fromPoint.x, fromPoint.z, toPoint.x, toPoint.z, self.workWidth, dz ) - local fromIx, toIx = self:generateStraightSection( fromPoint, toPoint, dz < 0) - -- this is where the arc will begin, and once the tractor reaches it, can switch to forward - local changeToFwdIx = #self.waypoints + 1 - -- Generate turn circle (Forward) - local startDir = corner:getArcStart() - local stopDir = corner:getArcEnd() - self:generateTurnCircle(centerForward, startDir, stopDir, self.turningRadius, self.direction * -1, true) - TurnManeuver.addTurnControl(self.waypoints, fromIx, toIx, TurnManeuver.CHANGE_TO_FWD_WHEN_REACHED, changeToFwdIx) - - -- Drive forward until our implement reaches the circle end and a bit more so it is hopefully aligned with the tractor - -- and we can start reversing more or less straight. - fromPoint = corner:getPointAtDistanceFromArcEnd((2 * self.steeringLength + self.forwardBuffer) * 0.2) - toPoint = corner:getPointAtDistanceFromArcEnd(2 * self.steeringLength + self.forwardBuffer) - self:debug("from ( %.2f %.2f ), to ( %.2f %.2f)", fromPoint.x, fromPoint.z, toPoint.x, toPoint.z) - - fromIx, toIx = self:generateStraightSection(fromPoint, toPoint, false, false, 0, true) - TurnManeuver.addTurnControl(self.waypoints, fromIx, toIx, TurnManeuver.CHANGE_DIRECTION_WHEN_ALIGNED, true) - - -- now back up the implement to the edge of the field (or headland) - fromPoint = corner:getArcEnd() - toPoint = corner:getPointAtDistanceFromCornerEnd(-(self.workWidth / 2) - turnContext.frontMarkerDistance - self.reverseBuffer - self.steeringLength) - - self:generateStraightSection(fromPoint, toPoint, true, true, self.reverseBuffer) - - -- lower the implement - self.waypoints[#self.waypoints].lowerImplement = true - self.course = Course(vehicle, self.waypoints, true) + reversingWorkTool, steeringLength) + TurnManeuver.init(self, vehicle, turnContext, vehicleDirectionNode, turningRadius, workWidth, steeringLength) + self.debugPrefix = '(HeadlandTurn): ' + self:debug('Start generating') + self:debug('r=%.1f, w=%.1f, steeringLength=%.1f', turningRadius, workWidth, steeringLength) + local fromPoint, toPoint = {}, {} + + local corner = turnContext:createCorner(vehicle, self.turningRadius) + + local centerForward = corner:getArcCenter() + local helperNode = CpUtil.createNode('tmp', 0, 0, 0, self.vehicleDirectionNode) + + -- in reverse our reference point is the implement's turn node so put the first reverse waypoint behind us + fromPoint.x, _, fromPoint.z = localToWorld(self.vehicleDirectionNode, 0, 0, -self.steeringLength) + + -- now back up so the tractor is at the start of the arc + toPoint = corner:getPointAtDistanceFromArcStart(2 * self.steeringLength + self.reverseBuffer) + -- helper node is where we would be at this point of the turn, so check if next target is behind or in front of us + _, _, dz = worldToLocal(helperNode, toPoint.x, toPoint.y, toPoint.z) + CpUtil.destroyNode(helperNode) + self:debug("from ( %.2f %.2f ), to ( %.2f %.2f) workWidth: %.1f, dz = %.1f", + fromPoint.x, fromPoint.z, toPoint.x, toPoint.z, self.workWidth, dz) + local fromIx, toIx = self:generateStraightSection(fromPoint, toPoint, dz < 0) + -- this is where the arc will begin, and once the tractor reaches it, can switch to forward + local changeToFwdIx = #self.waypoints + 1 + -- Generate turn circle (Forward) + local startDir = corner:getArcStart() + local stopDir = corner:getArcEnd() + self:generateTurnCircle(centerForward, startDir, stopDir, self.turningRadius, self.direction * -1, true) + TurnManeuver.addTurnControl(self.waypoints, fromIx, toIx, TurnManeuver.CHANGE_TO_FWD_WHEN_REACHED, changeToFwdIx) + + -- Drive forward until our implement reaches the circle end and a bit more so it is hopefully aligned with the tractor + -- and we can start reversing more or less straight. + fromPoint = corner:getPointAtDistanceFromArcEnd((2 * self.steeringLength + self.forwardBuffer) * 0.2) + toPoint = corner:getPointAtDistanceFromArcEnd(2 * self.steeringLength + self.forwardBuffer) + self:debug("from ( %.2f %.2f ), to ( %.2f %.2f)", fromPoint.x, fromPoint.z, toPoint.x, toPoint.z) + + fromIx, toIx = self:generateStraightSection(fromPoint, toPoint, false, false, 0, true) + TurnManeuver.addTurnControl(self.waypoints, fromIx, toIx, TurnManeuver.CHANGE_DIRECTION_WHEN_ALIGNED, true) + + -- now back up the implement to the edge of the field (or headland) + fromPoint = corner:getArcEnd() + toPoint = corner:getPointAtDistanceFromCornerEnd(-(self.workWidth / 2) - turnContext.frontMarkerDistance - self.reverseBuffer - self.steeringLength) + + self:generateStraightSection(fromPoint, toPoint, true, true, self.reverseBuffer) + + -- lower the implement + self.waypoints[#self.waypoints].lowerImplement = true + self.course = Course(vehicle, self.waypoints, true) end AlignmentCourse = CpObject(TurnManeuver) @@ -561,55 +580,55 @@ AlignmentCourse = CpObject(TurnManeuver) ---@param ix number end of the alignment course is the ix waypoint of course ---@param zOffset number forward(+)/backward(-) offset for the target, relative to the waypoint function AlignmentCourse:init(vehicle, vehicleDirectionNode, turningRadius, course, ix, zOffset) - self.debugPrefix = '(AlignmentCourse): ' - self.vehicle = vehicle - self:debug('creating alignment course to waypoint %d, zOffset = %.1f', ix, zOffset) - local x, z, yRot = PathfinderUtil.getNodePositionAndDirection(vehicleDirectionNode, 0, 0) - local start = State3D(x, -z, CpMathUtil.angleFromGame(yRot)) - x, _, z = course:getWaypointPosition(ix) - local goal = State3D(x, -z, CpMathUtil.angleFromGame(math.rad(course:getWaypointAngleDeg(ix)))) - - local offset = Vector(zOffset, 0) - goal:add(offset:rotate(goal.t)) - - -- have a little reserve to make sure vehicles can always follow the course - turningRadius = turningRadius * 1.1 - local solution = PathfinderUtil.dubinsSolver:solve(start, goal, turningRadius) - - local alignmentWaypoints = solution:getWaypoints(start, turningRadius) - if not alignmentWaypoints then - self:debug("Can't find an alignment course, may be too close to target wp?" ) - return nil - end - if #alignmentWaypoints < 3 then - self:debug("Alignment course would be only %d waypoints, it isn't needed then.", #alignmentWaypoints ) - return nil - end - self:debug('Alignment course with %d waypoints created.', #alignmentWaypoints) - self.course = Course.createFromAnalyticPath(self.vehicle, alignmentWaypoints, true) + self.debugPrefix = '(AlignmentCourse): ' + self.vehicle = vehicle + self:debug('creating alignment course to waypoint %d, zOffset = %.1f', ix, zOffset) + local x, z, yRot = PathfinderUtil.getNodePositionAndDirection(vehicleDirectionNode, 0, 0) + local start = State3D(x, -z, CpMathUtil.angleFromGame(yRot)) + x, _, z = course:getWaypointPosition(ix) + local goal = State3D(x, -z, CpMathUtil.angleFromGame(math.rad(course:getWaypointAngleDeg(ix)))) + + local offset = Vector(zOffset, 0) + goal:add(offset:rotate(goal.t)) + + -- have a little reserve to make sure vehicles can always follow the course + turningRadius = turningRadius * 1.1 + local solution = PathfinderUtil.dubinsSolver:solve(start, goal, turningRadius) + + local alignmentWaypoints = solution:getWaypoints(start, turningRadius) + if not alignmentWaypoints then + self:debug("Can't find an alignment course, may be too close to target wp?") + return nil + end + if #alignmentWaypoints < 3 then + self:debug("Alignment course would be only %d waypoints, it isn't needed then.", #alignmentWaypoints) + return nil + end + self:debug('Alignment course with %d waypoints created.', #alignmentWaypoints) + self.course = Course.createFromAnalyticPath(self.vehicle, alignmentWaypoints, true) end ---@class VineTurnManeuver : TurnManeuver VineTurnManeuver = CpObject(TurnManeuver) function VineTurnManeuver:init(vehicle, turnContext, vehicleDirectionNode, turningRadius, workWidth) - TurnManeuver.init(self, vehicle, turnContext, vehicleDirectionNode, turningRadius, workWidth, 0) - - self:debug('Start generating') - - local turnEndNode, goalOffset = self.turnContext:getTurnEndNodeAndOffsets(0) - local _, _, dz = turnContext:getLocalPositionOfTurnEnd(vehicle:getAIDirectionNode()) - local startOffset = 0 - if dz > 0 then - startOffset = startOffset + dz - else - goalOffset = goalOffset + dz - end - self:debug('r=%.1f, w=%.1f, dz=%.1f, startOffset=%.1f, goalOffset=%.1f', - 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) - self.course = Course.createFromAnalyticPath(self.vehicle, path, true) - local endingTurnLength = self.turnContext:appendEndingTurnCourse(self.course, 0, false) - TurnManeuver.setLowerImplements(self.course, endingTurnLength, true) + TurnManeuver.init(self, vehicle, turnContext, vehicleDirectionNode, turningRadius, workWidth, 0) + + self:debug('Start generating') + + local turnEndNode, goalOffset = self.turnContext:getTurnEndNodeAndOffsets(0) + local _, _, dz = turnContext:getLocalPositionOfTurnEnd(vehicle:getAIDirectionNode()) + local startOffset = 0 + if dz > 0 then + startOffset = startOffset + dz + else + goalOffset = goalOffset + dz + end + self:debug('r=%.1f, w=%.1f, dz=%.1f, startOffset=%.1f, goalOffset=%.1f', + 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) + self.course = Course.createFromAnalyticPath(self.vehicle, path, true) + local endingTurnLength = self.turnContext:appendEndingTurnCourse(self.course, 0, false) + TurnManeuver.setLowerImplements(self.course, endingTurnLength, true) end \ No newline at end of file diff --git a/scripts/courseGenerator/CourseGenerator.lua b/scripts/courseGenerator/CourseGenerator.lua index e33365464..915793a13 100644 --- a/scripts/courseGenerator/CourseGenerator.lua +++ b/scripts/courseGenerator/CourseGenerator.lua @@ -17,7 +17,7 @@ CourseGenerator.cMinEdgeLength = 0.5 -- edge so the result is an arc. Over this angle, we won't offset, so corners are kept sharp. CourseGenerator.cMaxDeltaAngleForMaxEdgeLength = math.rad(30) -- Approximate distance of waypoints on up/down rows -CourseGenerator.cRowWaypointDistance = 10 +CourseGenerator.cRowWaypointDistance = 7 -- Maximum cross track error we tolerate when a vehicle follows a path. This is used to -- find corners which the vehicle can't make due to its turning radius, without deviating more than -- cMaxCrossTrackError meters from the vertex in the corner.