From 45c7dd010bb774c2f468d680d434e44717692f87 Mon Sep 17 00:00:00 2001 From: Antler 22 Date: Tue, 21 Apr 2026 09:06:25 -0500 Subject: [PATCH 1/4] Revise README for forked project and new features Updated README to reflect forked status and added details about manual combine unloading and US units. --- README.md | 47 ++--------------------------------------------- 1 file changed, 2 insertions(+), 45 deletions(-) diff --git a/README.md b/README.md index b417819c..6d504352 100644 --- a/README.md +++ b/README.md @@ -1,47 +1,4 @@ -# Courseplay Beta for Farming Simulator 25 +FORKED from Original Courseplay -[![ModHub Release](https://img.shields.io/badge/dynamic/xml?color=blue&style=flat-square&label=Modhub+Release&prefix=v&query=%2F%2Fdiv%5B%40class%3D'table-row'%5D%5Bdiv%5B%40class%3D'table-cell'%5D%2Fb%5Btext()%3D'Version'%5D%5D%2Fdiv%5B%40class%3D'table-cell'%5D%5B2%5D&url=https%3A%2F%2Fwww.farming-simulator.com%2Fmod.php%3Flang%3Dde%26country%3Dde%26mod_id%3D331515%26title%3Dfs2025)](https://www.farming-simulator.com/mod.php?mod_id=331515&title=fs2025) -[![GitHub Release (latest by date including pre-releases)](https://img.shields.io/github/v/release/Courseplay/Courseplay_FS25?include_prereleases&style=flat-square&label=GitHub+Release)](https://github.com/Courseplay/Courseplay_FS25/releases/latest) -[![GitHub Release Downloads (by Asset)](https://img.shields.io/github/downloads-pre/Courseplay/Courseplay_FS25/latest/FS25_Courseplay.zip?style=flat-square)](https://github.com/Courseplay/Courseplay_FS25/releases/latest/download/FS25_Courseplay.zip) -[![GitHub Issues](https://img.shields.io/github/issues/Courseplay/Courseplay_FS25?style=flat-square)](https://github.com/Courseplay/Courseplay_FS25/issues) +Antler22's edit to allow manual combine unloading, US units, and some small tweaks to pathfinder -**[Download the latest developer version](https://github.com/Courseplay/Courseplay_FS25/releases/latest)** (the file FS25_Courseplay.zip). - -**[Courseplay Website](https://courseplay.github.io/CourseplayHelpFS25/)** - - -This and the Giants ModHub is the **only official source for Courseplay**, if you see it anywhere else, it's a scam. - -## Tutorials - -German Tutorial by Mario Hirschfeld: https://www.youtube.com/playlist?list=PL-UvOFIL55_jXAy6UkVJuLOD5DWKLuzIm - -English Tutorial by Argsy Gaming: https://www.youtube.com/playlist?list=PLFh-GZbaG02w9uLl_qroCi6uVI0LDXrD1 - -Courseplay functions are also documented on our [website](https://courseplay.github.io/CourseplayHelpFS25/) and in the in-game Courseplay menu. - -Courseplay Website - -## Developer version - -Please be aware you're using a developer version, which may and will contain errors, bugs, mistakes and unfinished code. Chances are you computer will explode when using it. Twice. If you have no idea what "beta", "alpha", or "developer" means and entails, steer clear. The Courseplay team will not take any responsibility for crop destroyed, savegames deleted or baby pandas killed. - -You have been warned. - -If you're still ok with this, please remember to post possible issues that you find in the developer version. That's the only way we can find sources of error and fix them. -Be as specific as possible: - -* tell us the version number -* only use the vehicles necessary, not 10 other ones at a time -* which vehicles are involved, what is the intended action? -* Post! The! Log! to [Gist](https://gist.github.com/) or [PasteBin](http://pastebin.com/) -* For more details on how to post a proper bug report, visit our [Wiki](https://github.com/Courseplay/Courseplay_FS25/wiki) - - -## Help Us Out - -We work long, hard, in our own free time at developing and improving Courseplay. If you like the project, show us your undying love: - -[![paypal](https://www.paypalobjects.com/en_US/i/btn/btn_donateCC_LG.gif)](https://www.paypal.com/cgi-bin/webscr?cmd=_donations&business=7PDM2P6HQ5D56&item_name=Promote+the+development+of+Courseplay¤cy_code=EUR&source=url) - -Buy Me A Coffee \ No newline at end of file From 88ab669ba0149bf0c8fc0fdfb9fe4c9bdbbe5cbe Mon Sep 17 00:00:00 2001 From: Antler 22 Date: Tue, 21 Apr 2026 09:20:31 -0500 Subject: [PATCH 2/4] Antler edit upload Grain cart manual call button, US units, CP unload threshold dropped to 20%, pathfinding for grain cart --- config/VehicleConfigurations.xml | 3 - config/VehicleSettingsSetup.xml | 2 +- modDesc.xml | 7 + scripts/ai/CollisionAvoidanceController.lua | 39 +- scripts/ai/CpManualCombineProxy.lua | 389 +++++ scripts/ai/PurePursuitController.lua | 1325 +++++++++-------- scripts/ai/controllers/MotorController.lua | 14 +- .../ai/parameters/AIParameterSettingList.lua | 68 +- .../AIDriveStrategyCombineCourse.lua | 15 +- .../AIDriveStrategyUnloadCombine.lua | 398 ++++- scripts/events/CallGrainCartEvent.lua | 42 + scripts/gui/hud/CpFieldworkHudPage.lua | 49 +- scripts/pathfinder/PathfinderConstraints.lua | 22 +- scripts/pathfinder/PathfinderUtil.lua | 15 + scripts/specializations/CpAIFieldWorker.lua | 61 + scripts/specializations/CpAIWorker.lua | 28 + scripts/specializations/CpCourseManager.lua | 2 +- translations/translation_cz.xml | 4 +- translations/translation_en.xml | 4 + translations/translation_ru.xml | 4 +- translations/translation_uk.xml | 4 +- 21 files changed, 1734 insertions(+), 761 deletions(-) create mode 100644 scripts/ai/CpManualCombineProxy.lua create mode 100644 scripts/events/CallGrainCartEvent.lua diff --git a/config/VehicleConfigurations.xml b/config/VehicleConfigurations.xml index 1d575c67..b4cb19ee 100644 --- a/config/VehicleConfigurations.xml +++ b/config/VehicleConfigurations.xml @@ -494,9 +494,6 @@ You can define the following custom settings: - - + diff --git a/modDesc.xml b/modDesc.xml index a24cfce9..80a29fb9 100644 --- a/modDesc.xml +++ b/modDesc.xml @@ -211,6 +211,7 @@ Changelog 8.1.0.3 + @@ -346,6 +347,7 @@ Changelog 8.1.0.3 + @@ -434,6 +436,10 @@ Changelog 8.1.0.3 + + + + @@ -463,6 +469,7 @@ Changelog 8.1.0.3 + \ No newline at end of file diff --git a/scripts/ai/CollisionAvoidanceController.lua b/scripts/ai/CollisionAvoidanceController.lua index f37b5f25..6047bc40 100644 --- a/scripts/ai/CollisionAvoidanceController.lua +++ b/scripts/ai/CollisionAvoidanceController.lua @@ -61,25 +61,28 @@ function CollisionAvoidanceController:findPotentialCollisions() if AIDriveStrategyCombineCourse.isActiveCpCombine(vehicle) then local d = calcDistanceFrom(self.vehicle.rootNode, vehicle.rootNode) if d < self.range then - local myCourse = self.strategy:getCurrentCourse() - local otherCourse = vehicle:getCpDriveStrategy():getCurrentCourse() - local myDistanceToCollision, otherDistanceToCollision = myCourse:intersects(otherCourse, self.lookahead, true) - if myDistanceToCollision then - -- our course intersects with this vehicle's course (lastSpeedReal is in m/ms) - -- for our own ETE, we always use the field speed and not the actual speed. This is to make sure - -- we come to a full stop on a warning and remain stopped while the warning is active - local myEte = myDistanceToCollision / (self.strategy:getFieldSpeed()) - local otherEte = CpMathUtil.divide(otherDistanceToCollision, (vehicle.lastSpeedReal * 1000)) - -- self:debug('Checking %s at %.1f m, %.1f, ETE %.1f %.1f', CpUtil.getName(vehicle), d, myDistanceToCollision, myEte, otherEte) - if math.abs(myEte - otherEte) < self.eteDiffThreshold then - if not self.warning:get() or (self.warning:get() and vehicle ~= self.warningVehicle) then - -- no warning is active yet, or there is, but this is a different vehicle - self:debug('collision warning: my course intersects with %s in %.1f m, my ETE %.1f, other ETE %.1f', - CpUtil.getName(vehicle), myDistanceToCollision, myEte, otherEte) + local otherStrategy = vehicle:getCpDriveStrategy() + if otherStrategy then + local myCourse = self.strategy:getCurrentCourse() + local otherCourse = otherStrategy:getCurrentCourse() + local myDistanceToCollision, otherDistanceToCollision = myCourse:intersects(otherCourse, self.lookahead, true) + if myDistanceToCollision then + -- our course intersects with this vehicle's course (lastSpeedReal is in m/ms) + -- for our own ETE, we always use the field speed and not the actual speed. This is to make sure + -- we come to a full stop on a warning and remain stopped while the warning is active + local myEte = myDistanceToCollision / (self.strategy:getFieldSpeed()) + local otherEte = CpMathUtil.divide(otherDistanceToCollision, (vehicle.lastSpeedReal * 1000)) + -- self:debug('Checking %s at %.1f m, %.1f, ETE %.1f %.1f', CpUtil.getName(vehicle), d, myDistanceToCollision, myEte, otherEte) + if math.abs(myEte - otherEte) < self.eteDiffThreshold then + if not self.warning:get() or (self.warning:get() and vehicle ~= self.warningVehicle) then + -- no warning is active yet, or there is, but this is a different vehicle + self:debug('collision warning: my course intersects with %s in %.1f m, my ETE %.1f, other ETE %.1f', + CpUtil.getName(vehicle), myDistanceToCollision, myEte, otherEte) + end + self.warningVehicle = vehicle + self.warning:set(true, self.clearWarningDelayMs) + return end - self.warningVehicle = vehicle - self.warning:set(true, self.clearWarningDelayMs) - return end end end diff --git a/scripts/ai/CpManualCombineProxy.lua b/scripts/ai/CpManualCombineProxy.lua new file mode 100644 index 00000000..02b60daa --- /dev/null +++ b/scripts/ai/CpManualCombineProxy.lua @@ -0,0 +1,389 @@ +--- Proxy that mimics the AIDriveStrategyCombineCourse interface for manually-driven +--- combines, so the existing unloader strategy can interact with them without nil checks. +---@class CpManualCombineProxy +CpManualCombineProxy = CpObject() + +CpManualCombineProxy.activeCalls = {} +CpManualCombineProxy.DYNAMIC_COURSE_LENGTH = 100 +CpManualCombineProxy.COURSE_REFRESH_INTERVAL = 2000 + +function CpManualCombineProxy:init(vehicle) + self.vehicle = vehicle + self.unloader = CpTemporaryObject(nil) + self.timeToCallUnloader = CpTemporaryObject(true) + self.dynamicCourse = nil + self.lastCourseRefreshTime = 0 + self.measuredBackDistance = 5 + + self:findPipeImplement() + self:measureBackDistance() + self:refreshDynamicCourse() + + CpManualCombineProxy.activeCalls[vehicle] = self +end + +function CpManualCombineProxy:delete() + CpManualCombineProxy.activeCalls[self.vehicle] = nil + self.dynamicCourse = nil +end + +function CpManualCombineProxy:findPipeImplement() + self.pipeImplement = nil + self.pipeSpec = nil + for _, childVehicle in ipairs(self.vehicle:getChildVehicles()) do + if childVehicle.spec_pipe then + self.pipeImplement = childVehicle + self.pipeSpec = childVehicle.spec_pipe + break + end + end + if not self.pipeSpec and self.vehicle.spec_pipe then + self.pipeImplement = self.vehicle + self.pipeSpec = self.vehicle.spec_pipe + end +end + +function CpManualCombineProxy:measureBackDistance() + local backMarkerNode, _, _, backMarkerOffset = Markers.getBackMarkerNode(self.vehicle) + if backMarkerOffset then + self.measuredBackDistance = math.abs(backMarkerOffset) + end +end + +--- Generates a straight course from the combine's current position and heading. +function CpManualCombineProxy:refreshDynamicCourse() + self.dynamicCourse = Course.createStraightForwardCourse(self.vehicle, + self.DYNAMIC_COURSE_LENGTH, 0, self.vehicle:getAIDirectionNode()) + self.lastCourseRefreshTime = g_time +end + +function CpManualCombineProxy:update(dt) + if g_time - self.lastCourseRefreshTime > self.COURSE_REFRESH_INTERVAL then + self:refreshDynamicCourse() + end + self:callUnloaderWhenNeeded() +end + +------------------------------------------------------------------------------------------------------------------------ +-- Unloader calling: simplified version that always calls with the combine's current position +------------------------------------------------------------------------------------------------------------------------ +function CpManualCombineProxy:callUnloaderWhenNeeded() + if not self.timeToCallUnloader:get() then + return + end + self.timeToCallUnloader:set(false, 1500) + + if self.unloader:get() then + return + end + + local bestUnloader = self:findUnloader() + if bestUnloader then + local strategy = bestUnloader:getCpDriveStrategy() + if strategy and strategy.call then + strategy:call(self.vehicle, nil) + end + end +end + +function CpManualCombineProxy:findUnloader() + local bestScore = -math.huge + local bestUnloader + for _, vehicle in pairs(g_currentMission.vehicleSystem.vehicles) do + if AIDriveStrategyUnloadCombine.isActiveCpCombineUnloader(vehicle) then + local x, _, z = getWorldTranslation(self.vehicle.rootNode) + local driveStrategy = vehicle:getCpDriveStrategy() + if driveStrategy:isServingPosition(x, z, 10) then + local fillPct = driveStrategy:getFillLevelPercentage() + if driveStrategy:isAllowedToBeCalled() and fillPct < 99 then + local dist, _ = driveStrategy:getDistanceAndEteToVehicle(self.vehicle) + local score = fillPct - 0.1 * dist + if score > bestScore then + bestUnloader = vehicle + bestScore = score + end + end + end + end + end + return bestUnloader +end + +------------------------------------------------------------------------------------------------------------------------ +-- Interface methods that mimic AIDriveStrategyCombineCourse for the unloader +------------------------------------------------------------------------------------------------------------------------ + +function CpManualCombineProxy:registerUnloader(driver) + self.unloader:set(driver, 1000) +end + +function CpManualCombineProxy:deregisterUnloader(driver, noEventSend) + self.unloader:reset() +end + +function CpManualCombineProxy:hasAutoAimPipe() + if self.pipeSpec then + return self.pipeSpec.numAutoAimingStates > 0 + end + return false +end + +function CpManualCombineProxy:getFillType() + if self.pipeImplement and self.pipeImplement.getDischargeNodeByIndex then + local dischargeNode = self.pipeImplement:getDischargeNodeByIndex( + self.pipeImplement:getPipeDischargeNodeIndex()) + if dischargeNode then + return self.pipeImplement:getFillUnitFillType(dischargeNode.fillUnitIndex) + end + end + return FillType.UNKNOWN +end + +function CpManualCombineProxy:isDischarging() + if self.pipeImplement and self.pipeImplement.getDischargeState then + return self.pipeImplement:getDischargeState() ~= Dischargeable.DISCHARGE_STATE_OFF + end + return false +end + +function CpManualCombineProxy:getPipeOffset(additionalOffsetX, additionalOffsetZ) + local pipeOffsetX, pipeOffsetZ = 0, 0 + if self.pipeSpec then + local pipeNode = self.pipeSpec.nodes and self.pipeSpec.nodes[1] + if pipeNode and pipeNode.node and entityExists(pipeNode.node) then + pipeOffsetX, _, pipeOffsetZ = localToLocal(pipeNode.node, + self.vehicle:getAIDirectionNode(), 0, 0, 0) + else + pipeOffsetX = self.vehicle:getCpSettings().pipeOffsetX:getValue() + pipeOffsetZ = self.vehicle:getCpSettings().pipeOffsetZ:getValue() + end + end + return pipeOffsetX + (additionalOffsetX or 0), pipeOffsetZ + (additionalOffsetZ or 0), self:hasAutoAimPipe() +end + +function CpManualCombineProxy:isPipeMoving() + if self.pipeSpec then + return self.pipeSpec.currentState == 0 + end + return false +end + +function CpManualCombineProxy:getPipeOffsetReferenceNode() + return self.vehicle:getAIDirectionNode() +end + +function CpManualCombineProxy:getAreaToAvoid() + return nil +end + +function CpManualCombineProxy:getMeasuredBackDistance() + return self.measuredBackDistance +end + +function CpManualCombineProxy:getFieldworkCourse() + return self.dynamicCourse +end + +function CpManualCombineProxy:getClosestFieldworkWaypointIx() + return 1 +end + +function CpManualCombineProxy:getWorkWidth() + if self.vehicle.getCpSettings then + local settings = self.vehicle:getCpSettings() + if settings and settings.workWidth then + return settings.workWidth:getValue() + end + end + return 6 +end + +function CpManualCombineProxy:getFruitAtSides() + return nil, nil +end + +------------------------------------------------------------------------------------------------------------------------ +-- State queries: safe defaults for a manually-driven combine +------------------------------------------------------------------------------------------------------------------------ + +function CpManualCombineProxy:isWaitingForUnload() + -- Return true only when the combine is actually stopped so the unloader strategy + -- transitions correctly between moving-combine and stopped-combine unload states, + -- and so the deadlock detector does not misfire when the grain cart is simply pausing. + return AIUtil.isStopped(self.vehicle) +end + +function CpManualCombineProxy:isWaitingForUnloadAfterPulledBack() + return false +end + +function CpManualCombineProxy:isWaitingForUnloadAfterCourseEnded() + return false +end + +function CpManualCombineProxy:willWaitForUnloadToFinish() + local stopped = AIUtil.isStopped(self.vehicle) + if stopped then + -- Debounce: only report "waiting" after the combine has been stopped for 3+ continuous seconds. + -- A momentary GPS micro-correction or terrain hitch would otherwise cause isInFrontAndAligned- + -- ToMovingCombine() to return false, which combined with the grain cart being in the normal + -- forward-of-direction-node unloading position (dz>0) kills isBehindAndAlignedToCombine() too, + -- triggering the dreaded startWaitingForSomethingToDo() → tarp cycle. + if not self._stoppedSinceTime then + self._stoppedSinceTime = g_time + CpUtil.debugVehicle(CpDebug.DBG_UNLOAD, self.vehicle, + 'CpManualCombineProxy:willWaitForUnloadToFinish: combine stopped, starting 3s debounce') + end + local waitingLongEnough = g_time - self._stoppedSinceTime > 3000 + if waitingLongEnough then + CpUtil.debugVehicle(CpDebug.DBG_UNLOAD, self.vehicle, + 'CpManualCombineProxy:willWaitForUnloadToFinish: combine stopped >3s, returning true') + end + return waitingLongEnough + else + if self._stoppedSinceTime then + CpUtil.debugVehicle(CpDebug.DBG_UNLOAD, self.vehicle, + 'CpManualCombineProxy:willWaitForUnloadToFinish: combine moving again (was stopped %.1fs)', + (g_time - self._stoppedSinceTime) / 1000) + end + self._stoppedSinceTime = nil + return false + end +end + +function CpManualCombineProxy:alwaysNeedsUnloader() + return false +end + +function CpManualCombineProxy:isReadyToUnload(noUnloadWithPipeInFruit) + return true +end + +function CpManualCombineProxy:isTurning() + return false +end + +function CpManualCombineProxy:isTurningButNotEndingTurn() + return false +end + +function CpManualCombineProxy:isTurnForwardOnly() + return false +end + +function CpManualCombineProxy:getTurnCourse() + return nil +end + +function CpManualCombineProxy:isFinishingRow() + return false +end + +function CpManualCombineProxy:isAboutToTurn() + return false +end + +function CpManualCombineProxy:isAboutToReturnFromPocket() + return false +end + +function CpManualCombineProxy:isManeuvering() + return false +end + +function CpManualCombineProxy:isOnHeadland(n) + return false +end + +function CpManualCombineProxy:isReversing() + return false +end + +function CpManualCombineProxy:isIdle() + return false +end + +function CpManualCombineProxy:hold(ms) +end + +function CpManualCombineProxy:requestToIgnoreProximity(vehicle) +end + +function CpManualCombineProxy:requestToMoveForward(requestingVehicle) +end + +function CpManualCombineProxy:reconfirmRendezvous() +end + +function CpManualCombineProxy:hasRendezvousWith(unloader) + return false +end + +function CpManualCombineProxy:getTurnArea() + return nil, nil +end + +function CpManualCombineProxy:isUnloadFinished() + if self:isDischarging() then + self._wasDischarging = true + self._dischargeOffTime = nil + return false + end + if self._wasDischarging then + -- Require discharge to be off for 2 continuous seconds before declaring done. + -- This prevents a momentary aim-miss (grain cart swerve, brief obstruction) from + -- prematurely ending the unload cycle and forcing the grain cart to drive away and return. + if not self._dischargeOffTime then + self._dischargeOffTime = g_time + CpUtil.debugVehicle(CpDebug.DBG_UNLOAD, self.vehicle, + 'CpManualCombineProxy:isUnloadFinished: discharge stopped, starting 2s debounce') + end + local elapsed = g_time - self._dischargeOffTime + if elapsed > 2000 then + CpUtil.debugVehicle(CpDebug.DBG_UNLOAD, self.vehicle, + 'CpManualCombineProxy:isUnloadFinished: discharge off for %.1fs, returning TRUE (unload done)', elapsed / 1000) + self._wasDischarging = false + self._dischargeOffTime = nil + return true + end + CpUtil.debugVehicle(CpDebug.DBG_UNLOAD, self.vehicle, + 'CpManualCombineProxy:isUnloadFinished: discharge off for %.1fs, waiting for 2s debounce', elapsed / 1000) + end + return false +end + +function CpManualCombineProxy:getFillLevelPercentage() + -- The farmer is in full control. Fill level must never cause the grain cart to leave — + -- the only valid exit is the pipe closing (isUnloadFinished). Always report full. + return 1 +end + +function CpManualCombineProxy:isTurningOnHeadland() + return false +end + +function CpManualCombineProxy:getTurnStartWpIx() + return 1 +end + +function CpManualCombineProxy:isProcessingFruit() + return false +end + +--- Marker so the unloader strategy can identify this as a manual-combine proxy +--- purely from the strategy object, without re-querying the vehicle. +function CpManualCombineProxy:isManualProxy() + return true +end + +function CpManualCombineProxy:isWaitingInPocket() + return false +end + +--- Needed so isActiveCpCombine() recognizes this as a valid combine strategy. +function CpManualCombineProxy:callUnloader() +end + +function CpManualCombineProxy:getUnloadTargetType() + return nil +end diff --git a/scripts/ai/PurePursuitController.lua b/scripts/ai/PurePursuitController.lua index 4a898298..a6839583 100644 --- a/scripts/ai/PurePursuitController.lua +++ b/scripts/ai/PurePursuitController.lua @@ -1,648 +1,679 @@ ---[[ -This file is part of Courseplay (https://github.com/Courseplay/courseplay) -Copyright (C) 2018-2021 Peter Vaiko - -This program is free software: you can redistribute it and/or modify -it under the terms of the GNU General Public License as published by -the Free Software Foundation, either version 3 of the License, or -(at your option) any later version. - -This program is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -GNU General Public License for more details. - -You should have received a copy of the GNU General Public License -along with this program. If not, see . -]] - ---[[ - -This is a simplified implementation of a pure pursuit algorithm -to follow a two dimensional path consisting of waypoints. - -See the paper - -Steering Control of an Autonomous Ground Vehicle with Application to the DARPA -Urban Challenge By Stefan F. Campbell - -We use the terminology of that paper here, like 'relevant path segment', 'goal point', etc. and follow the -algorithm to search for the goal point as described in this paper. - -PURPOSE - -1. Provide a goal point to steer towards to. - Contrary to the old implementation, we are not steering to a waypoint, instead to a goal - point which is in a given look ahead distance from the vehicle on the path. - -2. Determine when to switch to the next waypoint (and avoid circling) - Regardless of the above, the rest of the Courseplay code still needs to know the current - waypoint as we progress along the path. - -HOW TO USE - -1. add a PPC to the vehicle with new() -2. when the vehicle starts driving, call initialize() -3. in every update cycle, call update(). This will calculate the goal point and the current waypoint -4. use the convenience functions getCurrentWaypointPosition(), reachedLastWaypoint() - in your code instead of directly checking and manipulating vehicle.Waypoints. These provide the legacy behavior when - the PPC is not active (for example due to reverse driving) or when disabled -6. you can use enable() and disable() to enable/disable the PPC. When disabled and you are using the above functions, - it'll behave as the legacy code. - -]] - ----@class PurePursuitController -PurePursuitController = CpObject() - ---- if the vehicle is more than cutOutDistanceLimit meters from the current segment's endpoints, cut-out the ---- controller to stop. Some error must have caused us to wander way off-track, unlikely to recover. -PurePursuitController.cutOutDistanceLimit = 50 - --- constructor -function PurePursuitController:init(vehicle) - self.normalLookAheadDistance = math.min(vehicle.maxTurningRadius, 6) - self.shortLookaheadDistance = math.max(2, self.normalLookAheadDistance / 2) - -- normal lookahead distance - self.baseLookAheadDistance = self.normalLookAheadDistance - -- adapted look ahead distance - self.lookAheadDistance = self.baseLookAheadDistance - self.temporaryLookAheadDistance = CpTemporaryObject(nil) - -- when transitioning from forward to reverse, this close we have to be to the waypoint where we - -- change direction before we switch to the next waypoint - self.distToSwitchWhenChangingToReverse = 1 - self.vehicle = vehicle - self:resetControlledNode() - self.name = vehicle:getName() - -- node on the current waypoint - self.currentWpNode = WaypointNode(self.name .. '-currentWpNode', true) - -- waypoint at the start of the relevant segment - self.relevantWpNode = WaypointNode(self.name .. '-relevantWpNode', true) - -- waypoint at the end of the relevant segment - self.nextWpNode = WaypointNode(self.name .. '-nextWpNode', true) - -- the current goal node - self.goalWpNode = WaypointNode(self.name .. '-goalWpNode', false) - -- vehicle position projected on the path, not used for anything other than debug display - self.projectedPosNode = CpUtil.createNode(self.name .. '-projectedPosNode', 0, 0, 0) - -- index of the first node of the path (where PPC is initialized and starts driving - self.firstIx = 1 - self.crossTrackError = 0 - self.lastPassedWaypointIx = nil - self.waypointPassedListeners = {} - self.waypointChangeListeners = {} - -- enable/disable stopping the vehicle when it is off-track (too far away from any waypoint) - self.stopWhenOffTrack = CpTemporaryObject(true) -end - --- destructor -function PurePursuitController:delete() - self.currentWpNode:destroy() - self.relevantWpNode:destroy() - self.nextWpNode:destroy() - CpUtil.destroyNode(self.projectedPosNode) - self.goalWpNode:destroy() -end - -function PurePursuitController:debug(...) - CpUtil.debugVehicle(CpDebug.DBG_PPC, self.vehicle, 'PPC: ' .. string.format(...)) -end - -function PurePursuitController:debugSparse(...) - if g_updateLoopIndex % 100 == 0 then - self:debug(...) - end -end - ----@param course Course -function PurePursuitController:setCourse(course) - self.course = course -end - -function PurePursuitController:getCourse() - return self.course -end - ---- Set an offset for the current course. -function PurePursuitController:setOffset(x, z) - self.course:setOffset(x, z) -end - -function PurePursuitController:getOffset() - return self.course:getOffset() -end - ---- Disable off-track detection temporarily, for instance while we know the vehicle must be driving ---- longer distances between two waypoints, like an unloader following a chopper through a turn, where ---- in some patterns the row end and the next row start are far apart. -function PurePursuitController:disableStopWhenOffTrack(milliseconds) - self.stopWhenOffTrack:set(false, milliseconds) -end - ---- Use a different node to track/control, for example the root node of a trailed implement --- instead of the tractor's root node. -function PurePursuitController:setControlledNode(node) - self.controlledNode = node -end - -function PurePursuitController:getControlledNode() - return self.controlledNode -end - ---- reset controlled node to the default (vehicle's own direction node) -function PurePursuitController:resetControlledNode() - self.controlledNode = self.vehicle:getAIDirectionNode() -end - --- initialize controller before driving -function PurePursuitController:initialize(ix) - self.firstIx = ix - -- relevantWpNode always points to the point where the relevant path segment starts - self.relevantWpNode:setToWaypoint(self.course, self.firstIx ) - self.nextWpNode:setToWaypoint(self.course, self.firstIx) - self.wpBeforeGoalPointIx = self.nextWpNode.ix - self.currentWpNode:setToWaypoint(self.course, self.firstIx ) - self.course:setCurrentWaypointIx(self.firstIx) - self.course:setLastPassedWaypointIx(nil) - self:debug('initialized to waypoint %d of %d', self.firstIx, self.course:getNumberOfWaypoints()) - self.lastPassedWaypointIx = nil - -- force calling the waypoint change callback right after initialization - self.sendWaypointChange = { current = self.currentWpNode.ix, prev = self.currentWpNode.ix - 1} - self.sendWaypointPassed = nil - -- current goal point search case as described in the paper, for diagnostics only - self.case = 0 -end - --- Initialize to a waypoint when reversing. --- TODO: this has to be called explicitly but could be done automatically by the vanilla initialize() --- make sure the waypoint we initialize to is close to the controlled node, otherwise the PPC will --- remain in initializing mode if the waypoint is too far back from the controlled node, and just --- reverse forever -function PurePursuitController:initializeForReversing(ix) - local reverserNode, debugText = self:getReverserNode(false) - if reverserNode then - self:debug('Reverser node %s found, initializing with it', debugText) - -- don't use ix as it is, instead, find the waypoint closest to the reverser node - local dPrev, d = math.huge, self.course:getWaypoint(ix):getDistanceFromNode(reverserNode) - while d < dPrev and self.course:isReverseAt(ix) and ix < self.course:getNumberOfWaypoints() do - dPrev = d - ix = ix + 1 - d = self.course:getWaypoint(ix):getDistanceFromNode(reverserNode) - end - else - self:debug('No reverser node found, initializing with default controlled node') - end - self:initialize(ix) -end - --- TODO: make this more generic and allow registering multiple listeners? --- could also implement listeners for events like notify me when within x meters of a waypoint, etc. -function PurePursuitController:registerListeners(waypointListener, onWaypointPassedFunc, onWaypointChangeFunc) - self.savedWaypointListener = self.waypointListener - self.savedWaypointPassedListenerFunc = self.waypointPassedListenerFunc - self.savedWaypointChangeListenerFunc = self.waypointChangeListenerFunc - self.waypointListener = waypointListener - self.waypointPassedListenerFunc = onWaypointPassedFunc - self.waypointChangeListenerFunc = onWaypointChangeFunc -end - --- Restore the listeners that were registered before the last call of registerListeners(). If there were none, --- do not restore anything -function PurePursuitController:restorePreviouslyRegisteredListeners() - if self.savedWaypointListener ~= nil then - self.waypointListener = self.savedWaypointListener - self.waypointPassedListenerFunc = self.savedWaypointPassedListenerFunc - self.waypointChangeListenerFunc = self.savedWaypointChangeListenerFunc - end -end - -function PurePursuitController:setLookaheadDistance(d) - self.baseLookAheadDistance = d -end - -function PurePursuitController:setNormalLookaheadDistance() - self.baseLookAheadDistance = self.normalLookAheadDistance -end - -function PurePursuitController:setShortLookaheadDistance() - self.baseLookAheadDistance = self.shortLookaheadDistance -end - ---- Set a short lookahead distance for ttlMs milliseconds -function PurePursuitController:setTemporaryShortLookaheadDistance(ttlMs) - self.temporaryLookAheadDistance:set(self.shortLookaheadDistance, ttlMs) -end - -function PurePursuitController:getLookaheadDistance() - return self.lookAheadDistance -end - -function PurePursuitController:setCurrentLookaheadDistance(cte) - local la = self.temporaryLookAheadDistance:get() or self.baseLookAheadDistance - self.lookAheadDistance = math.min(la + math.abs(cte), la * 2) -end - ---- get index of current waypoint (one we are driving towards) -function PurePursuitController:getCurrentWaypointIx() - return self.currentWpNode.ix -end - ---- Get the current waypoint object ----@return Waypoint -function PurePursuitController:getCurrentWaypoint() - return self.course:getWaypoint(self.currentWpNode.ix) -end - ---- get index of relevant waypoint (one we are close to) -function PurePursuitController:getRelevantWaypointIx() - return self.relevantWpNode.ix -end - -function PurePursuitController:getLastPassedWaypointIx() - return self.lastPassedWaypointIx -end - ----@return number, string node that would be used for reversing, debug text explaining what node it is -function PurePursuitController:getReverserNode(suppressLog) - if not self.reversingImplement then - self.reversingImplement = AIUtil.getFirstReversingImplementWithWheels(self.vehicle, suppressLog) - end - return AIUtil.getReverserNode(self.vehicle, self.reversingImplement, suppressLog) -end - ---- When reversing, use the towed implement's node as a reference -function PurePursuitController:switchControlledNode() - local lastControlledNode = self.controlledNode - local debugText = 'AIDirectionNode' - local reverserNode - if self:isReversing() then - reverserNode, debugText = self:getReverserNode(true) - if reverserNode then - self:setControlledNode(reverserNode) - else - self:resetControlledNode() - end - else - self:resetControlledNode() - end - if self.controlledNode ~= lastControlledNode then - self:debug('Switching controlled node to %s', debugText) - end -end - -function PurePursuitController:update() - if not self.course then - self:debugSparse('no course set.') - return - end - self:showDebugTable() - self:switchControlledNode() - self:findRelevantSegment() - self:findGoalPoint() - self.course:setCurrentWaypointIx(self.currentWpNode.ix) - self.course:setLastPassedWaypointIx(self.lastPassedWaypointIx) - self:notifyListeners() -end - -function PurePursuitController:showDebugTable() - if self.course then - if CpDebug:isChannelActive(CpDebug.DBG_COURSES, self.vehicle) then - local info = { - title = self.course:getName(), - content = self.course:getDebugTable() - } - CpDebug:drawVehicleDebugTable(self.vehicle, { info }) - end - end -end - -function PurePursuitController:notifyListeners() - if self.waypointListener then - if self.sendWaypointChange then - -- send waypoint change event for all waypoints between the previous and current to make sure - -- we don't miss any - self:debug('prev %s curr %s', self.sendWaypointChange.prev, self.sendWaypointChange.current) - for ix = self.sendWaypointChange.prev + 1, self.sendWaypointChange.current do - self.waypointListener[self.waypointChangeListenerFunc](self.waypointListener, ix, self.course) - end - end - if self.sendWaypointPassed then - self.waypointListener[self.waypointPassedListenerFunc](self.waypointListener, self.sendWaypointPassed, self.course) - end - end - self.sendWaypointChange = nil - self.sendWaypointPassed = nil -end - -function PurePursuitController:havePassedWaypoint(wpNode) - local vx, vy, vz = getWorldTranslation(self.controlledNode) - local dx, _, dz = worldToLocal(wpNode.node, vx, vy, vz); - local dFromNext = MathUtil.vector2Length(dx, dz) - --self:debug('checking %d, dz: %.1f, dFromNext: %.1f', wpNode.ix, dz, dFromNext) - local result = false - if self.course:switchingDirectionAt(wpNode.ix) then - -- switching direction at this waypoint, so this is pointing into the opposite direction. - -- we have to make sure we drive up to this waypoint close enough before we switch to the next - -- so wait until dz < 0, that is, we are behind the waypoint - if dz < 0 then - result = true - end - else - -- we are not transitioning between forward and reverse - -- we have passed the next waypoint if our dz in the waypoints coordinate system is positive, that is, - -- when looking into the direction of the waypoint, we are ahead of it. - -- Also, when on the process of aligning to the course, like for example the vehicle just started - -- driving towards the first waypoint, we have to make sure we actually get close to the waypoint - -- (as we may already be in front of it), so try get within the turn diameter * 2. - if dz >= 0 and dFromNext < self.vehicle.maxTurningRadius * 4 then - result = true - end - end - if result then - --and not self:reachedLastWaypoint() then - if not self.lastPassedWaypointIx or (self.lastPassedWaypointIx ~= wpNode.ix) then - self.lastPassedWaypointIx = wpNode.ix - self:debug('waypoint %d passed, dz: %.1f %s %s', wpNode.ix, dz, - self.course.waypoints[wpNode.ix].rev and 'reverse' or '', - self.course:switchingDirectionAt(wpNode.ix) and 'switching direction' or '') - -- notify listeners about the passed waypoint - self.sendWaypointPassed = self.lastPassedWaypointIx - end - end - return result -end - -function PurePursuitController:havePassedAnyWaypointBetween(fromIx, toIx) - local node = WaypointNode(self.name .. '-node', false) - local result, passedWaypointIx = false, 0 - -- math.max so we do one loop even if toIx < fromIx - --self:debug('checking between %d and %d', fromIx, toIx) - for ix = fromIx, math.max(toIx, fromIx) do - node:setToWaypoint(self.course, ix) - if self:havePassedWaypoint(node) then - result = true - passedWaypointIx = ix - break - end - - end - node:destroy() - return result, passedWaypointIx -end - --- Finds the relevant segment. --- Sets the vehicle's projected position on the path. -function PurePursuitController:findRelevantSegment() - -- vehicle position - local vx, vy, vz = getWorldTranslation(self.controlledNode) - -- update the position of the relevant node (in case the course offset changed) - self.relevantWpNode:setToWaypoint(self.course, self.relevantWpNode.ix) - local lx, _, dzFromRelevant = worldToLocal(self.relevantWpNode.node, vx, vy, vz); - self.crossTrackError = lx - -- adapt our lookahead distance based on the error - self:setCurrentLookaheadDistance(self.crossTrackError) - -- projected vehicle position/rotation - local px, py, pz = localToWorld(self.relevantWpNode.node, 0, 0, dzFromRelevant) - local _, yRot, _ = getRotation(self.relevantWpNode.node) - setTranslation(self.projectedPosNode, px, py, pz) - setRotation(self.projectedPosNode, 0, yRot, 0) - -- we check all waypoints between the relevant and the one before the goal point as the goal point - -- may have moved several waypoints up if there's a very sharp turn for example and in that case - -- the vehicle may never reach some of the waypoint in between. - local passed, ix - if self.course:switchingDirectionAt(self.nextWpNode.ix) then - -- don't look beyond a direction switch as we'll always be past a reversing waypoint - -- before we reach it. - passed, ix = self:havePassedWaypoint(self.nextWpNode), self.nextWpNode.ix - else - passed, ix = self:havePassedAnyWaypointBetween(self.nextWpNode.ix, self.wpBeforeGoalPointIx) - end - if passed then - self.relevantWpNode:setToWaypoint(self.course, ix, true) - self.nextWpNode:setToWaypoint(self.course, self.relevantWpNode.ix + 1, true) - if not self:reachedLastWaypoint() then - -- disable debugging once we reached the last waypoint. Otherwise we'd keep logging - -- until the user presses 'Stop driver'. - self:debug('relevant waypoint: %d, next waypoint %d, crosstrack: %.1f', - self.relevantWpNode.ix, self.nextWpNode.ix, self.crossTrackError) - end - end - if CpDebug:isChannelActive(CpDebug.DBG_PPC, self.vehicle) then - DebugUtil.drawDebugLine(px, py + 3, pz, px, py + 1, pz, 1, 1, 0); - DebugUtil.drawDebugNode(self.relevantWpNode.node, string.format('ix = %d\nrelevant\nnode', self.relevantWpNode.ix)) - DebugUtil.drawDebugNode(self.projectedPosNode, 'projected\nvehicle\nposition') - end -end - --- Now, from the relevant section forward we search for the goal point, which is the one --- lying lookAheadDistance in front of us on the path --- this is the algorithm described in Chapter 2 of the paper -function PurePursuitController:findGoalPoint() - - local vx, _, vz = getWorldTranslation(self.controlledNode) - --local vx, vy, vz = getWorldTranslation(self.projectedPosNode); - - -- create helper nodes at the relevant and the next wp. We'll move these up on the path until we reach the segment - -- in lookAheadDistance - local node1 = WaypointNode(self.name .. '-node1', false) - local node2 = WaypointNode(self.name .. '-node2', false) - - -- starting at the relevant segment walk up the path to find the segment on - -- which the goal point lies. This is the segment intersected by the circle with lookAheadDistance radius - -- around the vehicle. - local ix = self.relevantWpNode.ix - while ix <= self.course:getNumberOfWaypoints() do - node1:setToWaypoint(self.course, ix) - node2:setToWaypointOrBeyond(self.course, ix + 1, self.lookAheadDistance) - local x1, _, z1 = getWorldTranslation(node1.node) - local x2, _, z2 = getWorldTranslation(node2.node) - -- distance between the vehicle position and the ends of the segment - local q1 = MathUtil.getPointPointDistance(x1, z1, vx, vz) -- distance from node 1 - local q2 = MathUtil.getPointPointDistance(x2, z2, vx, vz) -- distance from node 2 - local l = MathUtil.getPointPointDistance(x1, z1, x2, z2) -- length of path segment (distance between node 1 and 2 - -- self:debug('ix=%d, q1=%.1f, q2=%.1f la=%.1f l=%.1f', ix, q1, q2, self.lookAheadDistance, l) - - -- case i (first node outside virtual circle but not yet reached) or (not the first node but we are way off the track) - if (ix == self.firstIx and ix ~= self.lastPassedWaypointIx) and - q1 >= self.lookAheadDistance and q2 >= self.lookAheadDistance then - -- If we weren't on track yet (after initialization, on our way to the first/initialized waypoint) - -- set the goal to the relevant WP - self.goalWpNode:setToWaypoint(self.course, self.relevantWpNode.ix) - self:setGoalTranslation() - self:showGoalpointDiag(1, 'initializing, ix=%d, q1=%.1f, q2=%.1f, la=%.1f', ix, q1, q2, self.lookAheadDistance) - -- and also the current waypoint is now at the relevant WP - self:setCurrentWaypoint(self.relevantWpNode.ix) - break - end - - -- case ii (common case) - if q1 <= self.lookAheadDistance and q2 >= self.lookAheadDistance then - -- in some weird cases q1 may be 0 (when we calculate a course based on the vehicle position) so fix that - -- to avoid a nan - if q1 < 0.0001 then - q1 = 0.1 - end - local cosGamma = (q2 * q2 - q1 * q1 - l * l) / (-2 * l * q1) - local p = q1 * cosGamma + math.sqrt(q1 * q1 * (cosGamma * cosGamma - 1) + self.lookAheadDistance * self.lookAheadDistance) - local gx, _, gz = localToWorld(node1.node, 0, 0, p) - self:setGoalTranslation(gx, gz) - self.wpBeforeGoalPointIx = ix - self:showGoalpointDiag(2, 'common case, ix=%d, q1=%.1f, q2=%.1f la=%.1f', ix, q1, q2, self.lookAheadDistance) - -- current waypoint is the waypoint at the end of the path segment - self:setCurrentWaypoint(ix + 1) - --CpUtil.debugVehicle(CpDebug.DBG_PPC, self.vehicle, "PPC: %d, p=%.1f", self.currentWpNode.ix, p) - break - end - - -- cases iii, iv and v - -- these two may have a problem and actually prevent the vehicle go back to the waypoint - -- when wandering way off track, therefore we try to catch this case in case i - if ix == self.relevantWpNode.ix and q1 >= self.lookAheadDistance and q2 >= self.lookAheadDistance then - if math.abs(self.crossTrackError) <= self.lookAheadDistance then - -- case iii (two intersection points) - local p = math.sqrt(self.lookAheadDistance * self.lookAheadDistance - self.crossTrackError * self.crossTrackError) - local gx, _, gz = localToWorld(self.projectedPosNode, 0, 0, p) - self:setGoalTranslation(gx, gz) - self.wpBeforeGoalPointIx = ix - self:showGoalpointDiag(3, 'two intersection points, ix=%d, q1=%.1f, q2=%.1f, la=%.1f, cte=%.1f', ix, q1, q2, - self.lookAheadDistance, self.crossTrackError) - -- current waypoint is the waypoint at the end of the path segment - self:setCurrentWaypoint(ix + 1) - else - -- case iv (no intersection points) - -- case v ( goal point dead zone) - -- set the goal to the projected position - local gx, _, gz = localToWorld(self.projectedPosNode, 0, 0, 0) - self:setGoalTranslation(gx, gz) - self.wpBeforeGoalPointIx = ix - self:showGoalpointDiag(4, 'no intersection points, ix=%d, q1=%.1f, q2=%.1f, la=%.1f, cte=%.1f', ix, q1, q2, - self.lookAheadDistance, self.crossTrackError) - -- current waypoint is the waypoint at the end of the path segment - self:setCurrentWaypoint(ix + 1) - end - if (q1 > self.cutOutDistanceLimit) and (q2 > self.cutOutDistanceLimit) and self.stopWhenOffTrack:get() then - CpUtil.infoVehicle(self.vehicle, 'vehicle off track, shutting off Courseplay now.') - self.vehicle:stopCurrentAIJob(AIMessageCpError.new()) - return - end - break - end - -- none of the above, continue search with the next path segment - ix = ix + 1 - -- unless there's a direction change here. This should only happen right after initialization and when - -- the reference node is already beyond the direction switch waypoint. We should not skip that being - -- the current waypoint otherwise the relevant waypoint won't be moved over the direction switch - if self.course:switchingDirectionAt(ix) then - -- force waypoint change - self:showGoalpointDiag(100, 'switching direction while looking for goal point, ix=%d', ix) - self.wpBeforeGoalPointIx = ix - 1 - self:setCurrentWaypoint(ix) - break - end - end - - node1:destroy() - node2:destroy() - - if CpDebug:isChannelActive(CpDebug.DBG_PPC, self.vehicle) then - local gx, gy, gz = localToWorld(self.goalWpNode.node, 0, 0, 0) - DebugUtil.drawDebugLine(gx, gy + 3, gz, gx, gy + 1, gz, 0, 1, 0); - DebugUtil.drawDebugNode(self.currentWpNode.node, string.format('ix = %d\ncurrent\nwaypoint', self.currentWpNode.ix)) - end -end - --- set the goal WP node's position. This will make sure the goal node is on the same height --- as the controlled node, avoiding issues when driving on non-level bridges where the controlled node --- is not vertical and since the goal is very far below it, it will be much closer/further in the controlled --- node's reference frame. --- If everyone is on the same height, this error is negligible -function PurePursuitController:setGoalTranslation(x, z) - local gx, _, gz = getWorldTranslation(self.goalWpNode.node) - local _, cy, _ = getWorldTranslation(self.controlledNode) - -- if there's an x, z passed in, use that, otherwise only adjust y to be the same as the controlled node - setTranslation(self.goalWpNode.node, x or gx, cy + 1, z or gz) -end - --- set the current waypoint for the rest of Courseplay and to notify listeners -function PurePursuitController:setCurrentWaypoint(ix) - -- this is the current waypoint for the rest of Courseplay code, the waypoint we are driving to - -- but never, ever go back. Instead just leave this loop and keep driving to the current goal node - if ix < self.currentWpNode.ix then - if g_updateLoopIndex % 60 == 0 then - self:debug("Won't step current waypoint back from %d to %d.", self.currentWpNode.ix, ix) - end - elseif ix >= self.currentWpNode.ix then - local prevIx = self.currentWpNode.ix - self.currentWpNode:setToWaypointOrBeyond(self.course, ix, self.lookAheadDistance) - -- if ix > #self.course, currentWpNode.ix will always be set to #self.course and the change detection won't work - -- therefore, only call listeners if ix <= #self.course - if ix ~= prevIx and ix <= self.course:getNumberOfWaypoints() then - -- remember to send notification at the end of the loop - self.sendWaypointChange = { current = self.currentWpNode.ix, prev = prevIx } - end - end -end - -function PurePursuitController:showGoalpointDiag(case, ...) - local diagText = string.format(...) - if CpDebug:isChannelActive(CpDebug.DBG_PPC, self.vehicle) then - DebugUtil.drawDebugNode(self.goalWpNode.node, diagText) - DebugUtil.drawDebugNode(self.controlledNode, 'controlled') - end - if case ~= self.case then - self:debug(...) - self.case = case - end -end - ---- Should we be driving in reverse based on the current position on course -function PurePursuitController:isReversing() - if self.course then - return self.course:isReverseAt(self:getCurrentWaypointIx()) or self.course:switchingToForwardAt(self:getCurrentWaypointIx()) - else - return false - end -end - --- goal point local position in the vehicle's coordinate system -function PurePursuitController:getGoalPointLocalPosition() - return localToLocal(self.goalWpNode.node, self.controlledNode, 0, 0, 0) -end - --- goal point normalized direction -function PurePursuitController:getGoalPointDirection() - local gx, _, gz = localToLocal(self.goalWpNode.node, self.controlledNode, 0, 0, 0) - local dx, dz = MathUtil.vector2Normalize(gx, gz) - -- check for NaN - if dx ~= dx or dz ~= dz then - return 0, 0 - end - return dx, dz -end - -function PurePursuitController:getGoalPointPosition() - return getWorldTranslation(self.goalWpNode.node) -end - -function PurePursuitController:getCurrentWaypointPosition() - return self:getGoalPointPosition() -end - -function PurePursuitController:getCurrentWaypointYRotation() - return self.course:getYRotationCorrectedForDirectionChanges(self.currentWpNode.ix) -end - -function PurePursuitController:getCrossTrackError() - return self.crossTrackError -end - -function PurePursuitController:reachedLastWaypoint() - return self.relevantWpNode.ix >= self.course:getNumberOfWaypoints() -end - -function PurePursuitController:haveJustPassedWaypoint(ix) - return self.lastPassedWaypointIx and self.lastPassedWaypointIx == ix or false -end - -function PurePursuitController:haveAlreadyPassedWaypoint(ix) - return self.lastPassedWaypointIx and self.lastPassedWaypointIx <= ix or false +--[[ +This file is part of Courseplay (https://github.com/Courseplay/courseplay) +Copyright (C) 2018-2021 Peter Vaiko + +This program is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program. If not, see . +]] + +--[[ + +This is a simplified implementation of a pure pursuit algorithm +to follow a two dimensional path consisting of waypoints. + +See the paper + +Steering Control of an Autonomous Ground Vehicle with Application to the DARPA +Urban Challenge By Stefan F. Campbell + +We use the terminology of that paper here, like 'relevant path segment', 'goal point', etc. and follow the +algorithm to search for the goal point as described in this paper. + +PURPOSE + +1. Provide a goal point to steer towards to. + Contrary to the old implementation, we are not steering to a waypoint, instead to a goal + point which is in a given look ahead distance from the vehicle on the path. + +2. Determine when to switch to the next waypoint (and avoid circling) + Regardless of the above, the rest of the Courseplay code still needs to know the current + waypoint as we progress along the path. + +HOW TO USE + +1. add a PPC to the vehicle with new() +2. when the vehicle starts driving, call initialize() +3. in every update cycle, call update(). This will calculate the goal point and the current waypoint +4. use the convenience functions getCurrentWaypointPosition(), reachedLastWaypoint() + in your code instead of directly checking and manipulating vehicle.Waypoints. These provide the legacy behavior when + the PPC is not active (for example due to reverse driving) or when disabled +6. you can use enable() and disable() to enable/disable the PPC. When disabled and you are using the above functions, + it'll behave as the legacy code. + +]] + +---@class PurePursuitController +PurePursuitController = CpObject() + +--- if the vehicle is more than cutOutDistanceLimit meters from the current segment's endpoints, cut-out the +--- controller to stop. Some error must have caused us to wander way off-track, unlikely to recover. +PurePursuitController.cutOutDistanceLimit = 50 +--- Only shut off after being off track for this long (ms). Reduces false shutdowns on brief excursions (turns, terrain). +PurePursuitController.offTrackGracePeriodMs = 10000 + +-- constructor +function PurePursuitController:init(vehicle) + self.normalLookAheadDistance = math.min(vehicle.maxTurningRadius, 6) + self.shortLookaheadDistance = math.max(2, self.normalLookAheadDistance / 2) + -- normal lookahead distance + self.baseLookAheadDistance = self.normalLookAheadDistance + -- adapted look ahead distance + self.lookAheadDistance = self.baseLookAheadDistance + self.temporaryLookAheadDistance = CpTemporaryObject(nil) + -- when transitioning from forward to reverse, this close we have to be to the waypoint where we + -- change direction before we switch to the next waypoint + self.distToSwitchWhenChangingToReverse = 1 + self.vehicle = vehicle + self:resetControlledNode() + self.name = vehicle:getName() + -- node on the current waypoint + self.currentWpNode = WaypointNode(self.name .. '-currentWpNode', true) + -- waypoint at the start of the relevant segment + self.relevantWpNode = WaypointNode(self.name .. '-relevantWpNode', true) + -- waypoint at the end of the relevant segment + self.nextWpNode = WaypointNode(self.name .. '-nextWpNode', true) + -- the current goal node + self.goalWpNode = WaypointNode(self.name .. '-goalWpNode', false) + -- vehicle position projected on the path, not used for anything other than debug display + self.projectedPosNode = CpUtil.createNode(self.name .. '-projectedPosNode', 0, 0, 0) + -- index of the first node of the path (where PPC is initialized and starts driving + self.firstIx = 1 + self.crossTrackError = 0 + self.lastPassedWaypointIx = nil + self.waypointPassedListeners = {} + self.waypointChangeListeners = {} + -- enable/disable stopping the vehicle when it is off-track (too far away from any waypoint) + self.stopWhenOffTrack = CpTemporaryObject(true) + -- when we first entered the off-track shutdown zone (nil = not in zone or just left it) + self.offTrackShutdownSince = nil +end + +-- destructor +function PurePursuitController:delete() + self.currentWpNode:destroy() + self.relevantWpNode:destroy() + self.nextWpNode:destroy() + CpUtil.destroyNode(self.projectedPosNode) + self.goalWpNode:destroy() +end + +function PurePursuitController:debug(...) + CpUtil.debugVehicle(CpDebug.DBG_PPC, self.vehicle, 'PPC: ' .. string.format(...)) +end + +function PurePursuitController:debugSparse(...) + if g_updateLoopIndex % 100 == 0 then + self:debug(...) + end +end + +---@param course Course +function PurePursuitController:setCourse(course) + self.course = course +end + +function PurePursuitController:getCourse() + return self.course +end + +--- Set an offset for the current course. +function PurePursuitController:setOffset(x, z) + self.course:setOffset(x, z) +end + +function PurePursuitController:getOffset() + return self.course:getOffset() +end + +--- Disable off-track detection temporarily, for instance while we know the vehicle must be driving +--- longer distances between two waypoints, like an unloader following a chopper through a turn, where +--- in some patterns the row end and the next row start are far apart. +function PurePursuitController:disableStopWhenOffTrack(milliseconds) + self.stopWhenOffTrack:set(false, milliseconds) +end + +--- Use a different node to track/control, for example the root node of a trailed implement +-- instead of the tractor's root node. +function PurePursuitController:setControlledNode(node) + self.controlledNode = node +end + +function PurePursuitController:getControlledNode() + return self.controlledNode +end + +--- reset controlled node to the default (vehicle's own direction node) +function PurePursuitController:resetControlledNode() + self.controlledNode = self.vehicle:getAIDirectionNode() +end + +-- initialize controller before driving +function PurePursuitController:initialize(ix) + self.firstIx = ix + -- relevantWpNode always points to the point where the relevant path segment starts + self.relevantWpNode:setToWaypoint(self.course, self.firstIx ) + self.nextWpNode:setToWaypoint(self.course, self.firstIx) + self.wpBeforeGoalPointIx = self.nextWpNode.ix + self.currentWpNode:setToWaypoint(self.course, self.firstIx ) + self.course:setCurrentWaypointIx(self.firstIx) + self.course:setLastPassedWaypointIx(nil) + self:debug('initialized to waypoint %d of %d', self.firstIx, self.course:getNumberOfWaypoints()) + self.lastPassedWaypointIx = nil + -- force calling the waypoint change callback right after initialization + self.sendWaypointChange = { current = self.currentWpNode.ix, prev = self.currentWpNode.ix - 1} + self.sendWaypointPassed = nil + -- current goal point search case as described in the paper, for diagnostics only + self.case = 0 +end + +-- Initialize to a waypoint when reversing. +-- TODO: this has to be called explicitly but could be done automatically by the vanilla initialize() +-- make sure the waypoint we initialize to is close to the controlled node, otherwise the PPC will +-- remain in initializing mode if the waypoint is too far back from the controlled node, and just +-- reverse forever +function PurePursuitController:initializeForReversing(ix) + local reverserNode, debugText = self:getReverserNode(false) + if reverserNode then + self:debug('Reverser node %s found, initializing with it', debugText) + -- don't use ix as it is, instead, find the waypoint closest to the reverser node + local dPrev, d = math.huge, self.course:getWaypoint(ix):getDistanceFromNode(reverserNode) + while d < dPrev and self.course:isReverseAt(ix) and ix < self.course:getNumberOfWaypoints() do + dPrev = d + ix = ix + 1 + d = self.course:getWaypoint(ix):getDistanceFromNode(reverserNode) + end + else + self:debug('No reverser node found, initializing with default controlled node') + end + self:initialize(ix) +end + +-- TODO: make this more generic and allow registering multiple listeners? +-- could also implement listeners for events like notify me when within x meters of a waypoint, etc. +function PurePursuitController:registerListeners(waypointListener, onWaypointPassedFunc, onWaypointChangeFunc) + self.savedWaypointListener = self.waypointListener + self.savedWaypointPassedListenerFunc = self.waypointPassedListenerFunc + self.savedWaypointChangeListenerFunc = self.waypointChangeListenerFunc + self.waypointListener = waypointListener + self.waypointPassedListenerFunc = onWaypointPassedFunc + self.waypointChangeListenerFunc = onWaypointChangeFunc +end + +-- Restore the listeners that were registered before the last call of registerListeners(). If there were none, +-- do not restore anything +function PurePursuitController:restorePreviouslyRegisteredListeners() + if self.savedWaypointListener ~= nil then + self.waypointListener = self.savedWaypointListener + self.waypointPassedListenerFunc = self.savedWaypointPassedListenerFunc + self.waypointChangeListenerFunc = self.savedWaypointChangeListenerFunc + end +end + +function PurePursuitController:setLookaheadDistance(d) + self.baseLookAheadDistance = d +end + +function PurePursuitController:setNormalLookaheadDistance() + self.baseLookAheadDistance = self.normalLookAheadDistance +end + +function PurePursuitController:setShortLookaheadDistance() + self.baseLookAheadDistance = self.shortLookaheadDistance +end + +--- Set a short lookahead distance for ttlMs milliseconds +function PurePursuitController:setTemporaryShortLookaheadDistance(ttlMs) + self.temporaryLookAheadDistance:set(self.shortLookaheadDistance, ttlMs) +end + +function PurePursuitController:getLookaheadDistance() + return self.lookAheadDistance +end + +function PurePursuitController:setCurrentLookaheadDistance(cte) + local la = self.temporaryLookAheadDistance:get() or self.baseLookAheadDistance + self.lookAheadDistance = math.min(la + math.abs(cte), la * 2) +end + +--- get index of current waypoint (one we are driving towards) +function PurePursuitController:getCurrentWaypointIx() + return self.currentWpNode.ix +end + +--- Get the current waypoint object +---@return Waypoint +function PurePursuitController:getCurrentWaypoint() + return self.course:getWaypoint(self.currentWpNode.ix) +end + +--- get index of relevant waypoint (one we are close to) +function PurePursuitController:getRelevantWaypointIx() + return self.relevantWpNode.ix +end + +function PurePursuitController:getLastPassedWaypointIx() + return self.lastPassedWaypointIx +end + +---@return number, string node that would be used for reversing, debug text explaining what node it is +function PurePursuitController:getReverserNode(suppressLog) + if not self.reversingImplement then + self.reversingImplement = AIUtil.getFirstReversingImplementWithWheels(self.vehicle, suppressLog) + end + return AIUtil.getReverserNode(self.vehicle, self.reversingImplement, suppressLog) +end + +--- When reversing, use the towed implement's node as a reference +function PurePursuitController:switchControlledNode() + local lastControlledNode = self.controlledNode + local debugText = 'AIDirectionNode' + local reverserNode + if self:isReversing() then + reverserNode, debugText = self:getReverserNode(true) + if reverserNode then + self:setControlledNode(reverserNode) + else + self:resetControlledNode() + end + else + self:resetControlledNode() + end + if self.controlledNode ~= lastControlledNode then + self:debug('Switching controlled node to %s', debugText) + end +end + +function PurePursuitController:update() + if not self.course then + self:debugSparse('no course set.') + return + end + self:showDebugTable() + self:switchControlledNode() + self:findRelevantSegment() + self:findGoalPoint() + self.course:setCurrentWaypointIx(self.currentWpNode.ix) + self.course:setLastPassedWaypointIx(self.lastPassedWaypointIx) + self:notifyListeners() +end + +function PurePursuitController:showDebugTable() + if self.course then + if CpDebug:isChannelActive(CpDebug.DBG_COURSES, self.vehicle) then + local info = { + title = self.course:getName(), + content = self.course:getDebugTable() + } + CpDebug:drawVehicleDebugTable(self.vehicle, { info }) + end + end +end + +function PurePursuitController:notifyListeners() + if self.waypointListener then + if self.sendWaypointChange then + -- send waypoint change event for all waypoints between the previous and current to make sure + -- we don't miss any + self:debug('prev %s curr %s', self.sendWaypointChange.prev, self.sendWaypointChange.current) + for ix = self.sendWaypointChange.prev + 1, self.sendWaypointChange.current do + self.waypointListener[self.waypointChangeListenerFunc](self.waypointListener, ix, self.course) + end + end + if self.sendWaypointPassed then + self.waypointListener[self.waypointPassedListenerFunc](self.waypointListener, self.sendWaypointPassed, self.course) + end + end + self.sendWaypointChange = nil + self.sendWaypointPassed = nil +end + +function PurePursuitController:havePassedWaypoint(wpNode) + local vx, vy, vz = getWorldTranslation(self.controlledNode) + local dx, _, dz = worldToLocal(wpNode.node, vx, vy, vz); + local dFromNext = MathUtil.vector2Length(dx, dz) + --self:debug('checking %d, dz: %.1f, dFromNext: %.1f', wpNode.ix, dz, dFromNext) + local result = false + if self.course:switchingDirectionAt(wpNode.ix) then + -- switching direction at this waypoint, so this is pointing into the opposite direction. + -- we have to make sure we drive up to this waypoint close enough before we switch to the next + -- so wait until dz < 0, that is, we are behind the waypoint + if dz < 0 then + result = true + end + else + -- we are not transitioning between forward and reverse + -- we have passed the next waypoint if our dz in the waypoints coordinate system is positive, that is, + -- when looking into the direction of the waypoint, we are ahead of it. + -- Also, when on the process of aligning to the course, like for example the vehicle just started + -- driving towards the first waypoint, we have to make sure we actually get close to the waypoint + -- (as we may already be in front of it), so try get within the turn diameter * 2. + if dz >= 0 and dFromNext < self.vehicle.maxTurningRadius * 4 then + result = true + end + end + if result then + --and not self:reachedLastWaypoint() then + if not self.lastPassedWaypointIx or (self.lastPassedWaypointIx ~= wpNode.ix) then + self.lastPassedWaypointIx = wpNode.ix + self:debug('waypoint %d passed, dz: %.1f %s %s', wpNode.ix, dz, + self.course.waypoints[wpNode.ix].rev and 'reverse' or '', + self.course:switchingDirectionAt(wpNode.ix) and 'switching direction' or '') + -- notify listeners about the passed waypoint + self.sendWaypointPassed = self.lastPassedWaypointIx + end + end + return result +end + +function PurePursuitController:havePassedAnyWaypointBetween(fromIx, toIx) + local node = WaypointNode(self.name .. '-node', false) + local result, passedWaypointIx = false, 0 + -- math.max so we do one loop even if toIx < fromIx + --self:debug('checking between %d and %d', fromIx, toIx) + for ix = fromIx, math.max(toIx, fromIx) do + node:setToWaypoint(self.course, ix) + if self:havePassedWaypoint(node) then + result = true + passedWaypointIx = ix + break + end + + end + node:destroy() + return result, passedWaypointIx +end + +-- Finds the relevant segment. +-- Sets the vehicle's projected position on the path. +function PurePursuitController:findRelevantSegment() + -- vehicle position + local vx, vy, vz = getWorldTranslation(self.controlledNode) + -- update the position of the relevant node (in case the course offset changed) + self.relevantWpNode:setToWaypoint(self.course, self.relevantWpNode.ix) + local lx, _, dzFromRelevant = worldToLocal(self.relevantWpNode.node, vx, vy, vz); + self.crossTrackError = lx + -- adapt our lookahead distance based on the error + self:setCurrentLookaheadDistance(self.crossTrackError) + -- projected vehicle position/rotation + local px, py, pz = localToWorld(self.relevantWpNode.node, 0, 0, dzFromRelevant) + local _, yRot, _ = getRotation(self.relevantWpNode.node) + setTranslation(self.projectedPosNode, px, py, pz) + setRotation(self.projectedPosNode, 0, yRot, 0) + -- we check all waypoints between the relevant and the one before the goal point as the goal point + -- may have moved several waypoints up if there's a very sharp turn for example and in that case + -- the vehicle may never reach some of the waypoint in between. + local passed, ix + if self.course:switchingDirectionAt(self.nextWpNode.ix) then + -- don't look beyond a direction switch as we'll always be past a reversing waypoint + -- before we reach it. + passed, ix = self:havePassedWaypoint(self.nextWpNode), self.nextWpNode.ix + else + passed, ix = self:havePassedAnyWaypointBetween(self.nextWpNode.ix, self.wpBeforeGoalPointIx) + end + if passed then + self.relevantWpNode:setToWaypoint(self.course, ix, true) + self.nextWpNode:setToWaypoint(self.course, self.relevantWpNode.ix + 1, true) + if not self:reachedLastWaypoint() then + -- disable debugging once we reached the last waypoint. Otherwise we'd keep logging + -- until the user presses 'Stop driver'. + self:debug('relevant waypoint: %d, next waypoint %d, crosstrack: %.1f', + self.relevantWpNode.ix, self.nextWpNode.ix, self.crossTrackError) + end + end + if CpDebug:isChannelActive(CpDebug.DBG_PPC, self.vehicle) then + DebugUtil.drawDebugLine(px, py + 3, pz, px, py + 1, pz, 1, 1, 0); + DebugUtil.drawDebugNode(self.relevantWpNode.node, string.format('ix = %d\nrelevant\nnode', self.relevantWpNode.ix)) + DebugUtil.drawDebugNode(self.projectedPosNode, 'projected\nvehicle\nposition') + end +end + +-- Now, from the relevant section forward we search for the goal point, which is the one +-- lying lookAheadDistance in front of us on the path +-- this is the algorithm described in Chapter 2 of the paper +function PurePursuitController:findGoalPoint() + + local vx, _, vz = getWorldTranslation(self.controlledNode) + --local vx, vy, vz = getWorldTranslation(self.projectedPosNode); + + -- create helper nodes at the relevant and the next wp. We'll move these up on the path until we reach the segment + -- in lookAheadDistance + local node1 = WaypointNode(self.name .. '-node1', false) + local node2 = WaypointNode(self.name .. '-node2', false) + + -- starting at the relevant segment walk up the path to find the segment on + -- which the goal point lies. This is the segment intersected by the circle with lookAheadDistance radius + -- around the vehicle. + local ix = self.relevantWpNode.ix + while ix <= self.course:getNumberOfWaypoints() do + node1:setToWaypoint(self.course, ix) + node2:setToWaypointOrBeyond(self.course, ix + 1, self.lookAheadDistance) + local x1, _, z1 = getWorldTranslation(node1.node) + local x2, _, z2 = getWorldTranslation(node2.node) + -- distance between the vehicle position and the ends of the segment + local q1 = MathUtil.getPointPointDistance(x1, z1, vx, vz) -- distance from node 1 + local q2 = MathUtil.getPointPointDistance(x2, z2, vx, vz) -- distance from node 2 + local l = MathUtil.getPointPointDistance(x1, z1, x2, z2) -- length of path segment (distance between node 1 and 2 + -- self:debug('ix=%d, q1=%.1f, q2=%.1f la=%.1f l=%.1f', ix, q1, q2, self.lookAheadDistance, l) + + -- case i (first node outside virtual circle but not yet reached) or (not the first node but we are way off the track) + if (ix == self.firstIx and ix ~= self.lastPassedWaypointIx) and + q1 >= self.lookAheadDistance and q2 >= self.lookAheadDistance then + self.offTrackShutdownSince = nil + -- If we weren't on track yet (after initialization, on our way to the first/initialized waypoint) + -- set the goal to the relevant WP + self.goalWpNode:setToWaypoint(self.course, self.relevantWpNode.ix) + self:setGoalTranslation() + self:showGoalpointDiag(1, 'initializing, ix=%d, q1=%.1f, q2=%.1f, la=%.1f', ix, q1, q2, self.lookAheadDistance) + -- and also the current waypoint is now at the relevant WP + self:setCurrentWaypoint(self.relevantWpNode.ix) + break + end + + -- case ii (common case) + if q1 <= self.lookAheadDistance and q2 >= self.lookAheadDistance then + self.offTrackShutdownSince = nil + -- in some weird cases q1 may be 0 (when we calculate a course based on the vehicle position) so fix that + -- to avoid a nan + if q1 < 0.0001 then + q1 = 0.1 + end + local cosGamma = (q2 * q2 - q1 * q1 - l * l) / (-2 * l * q1) + local p = q1 * cosGamma + math.sqrt(q1 * q1 * (cosGamma * cosGamma - 1) + self.lookAheadDistance * self.lookAheadDistance) + local gx, _, gz = localToWorld(node1.node, 0, 0, p) + self:setGoalTranslation(gx, gz) + self.wpBeforeGoalPointIx = ix + self:showGoalpointDiag(2, 'common case, ix=%d, q1=%.1f, q2=%.1f la=%.1f', ix, q1, q2, self.lookAheadDistance) + -- current waypoint is the waypoint at the end of the path segment + self:setCurrentWaypoint(ix + 1) + --CpUtil.debugVehicle(CpDebug.DBG_PPC, self.vehicle, "PPC: %d, p=%.1f", self.currentWpNode.ix, p) + break + end + + -- cases iii, iv and v + -- these two may have a problem and actually prevent the vehicle go back to the waypoint + -- when wandering way off track, therefore we try to catch this case in case i + if ix == self.relevantWpNode.ix and q1 >= self.lookAheadDistance and q2 >= self.lookAheadDistance then + if math.abs(self.crossTrackError) <= self.lookAheadDistance then + -- case iii (two intersection points) + self.offTrackShutdownSince = nil + local p = math.sqrt(self.lookAheadDistance * self.lookAheadDistance - self.crossTrackError * self.crossTrackError) + local gx, _, gz = localToWorld(self.projectedPosNode, 0, 0, p) + self:setGoalTranslation(gx, gz) + self.wpBeforeGoalPointIx = ix + self:showGoalpointDiag(3, 'two intersection points, ix=%d, q1=%.1f, q2=%.1f, la=%.1f, cte=%.1f', ix, q1, q2, + self.lookAheadDistance, self.crossTrackError) + -- current waypoint is the waypoint at the end of the path segment + self:setCurrentWaypoint(ix + 1) + else + -- case iv (no intersection points) + -- case v ( goal point dead zone) + -- set the goal to the projected position + local gx, _, gz = localToWorld(self.projectedPosNode, 0, 0, 0) + self:setGoalTranslation(gx, gz) + self.wpBeforeGoalPointIx = ix + self:showGoalpointDiag(4, 'no intersection points, ix=%d, q1=%.1f, q2=%.1f, la=%.1f, cte=%.1f', ix, q1, q2, + self.lookAheadDistance, self.crossTrackError) + -- current waypoint is the waypoint at the end of the path segment + self:setCurrentWaypoint(ix + 1) + end + -- Only shut off after being off track beyond cutOutDistanceLimit for a grace period (reduces false shutdowns) + if (q1 > self.cutOutDistanceLimit) and (q2 > self.cutOutDistanceLimit) and self.stopWhenOffTrack:get() then + local now = g_currentMission and g_currentMission.time or 0 + if self.offTrackShutdownSince == nil then + self.offTrackShutdownSince = now + end + if (now - self.offTrackShutdownSince) >= (self.offTrackGracePeriodMs or PurePursuitController.offTrackGracePeriodMs or 10000) then + -- Give the current drive strategy a chance to recover softly instead of + -- stopping the CP helper entirely (user has to jump to that vehicle to + -- restart). If the strategy implements onOffTrackShutdown() and returns + -- true it has handled recovery and we must NOT call stopCurrentAIJob. + local strategy = self.vehicle.getCpDriveStrategy and self.vehicle:getCpDriveStrategy() + if strategy and strategy.onOffTrackShutdown then + local handled = strategy:onOffTrackShutdown() + if handled then + CpUtil.infoVehicle(self.vehicle, + 'vehicle off track, strategy performed soft recovery instead of shutdown.') + self.offTrackShutdownSince = nil + break + end + end + CpUtil.infoVehicle(self.vehicle, 'vehicle off track, shutting off Courseplay now.') + self.vehicle:stopCurrentAIJob(AIMessageCpError.new()) + return + end + else + self.offTrackShutdownSince = nil + end + break + end + -- none of the above, continue search with the next path segment + ix = ix + 1 + -- unless there's a direction change here. This should only happen right after initialization and when + -- the reference node is already beyond the direction switch waypoint. We should not skip that being + -- the current waypoint otherwise the relevant waypoint won't be moved over the direction switch + if self.course:switchingDirectionAt(ix) then + self.offTrackShutdownSince = nil + -- force waypoint change + self:showGoalpointDiag(100, 'switching direction while looking for goal point, ix=%d', ix) + self.wpBeforeGoalPointIx = ix - 1 + self:setCurrentWaypoint(ix) + break + end + end + + node1:destroy() + node2:destroy() + + if CpDebug:isChannelActive(CpDebug.DBG_PPC, self.vehicle) then + local gx, gy, gz = localToWorld(self.goalWpNode.node, 0, 0, 0) + DebugUtil.drawDebugLine(gx, gy + 3, gz, gx, gy + 1, gz, 0, 1, 0); + DebugUtil.drawDebugNode(self.currentWpNode.node, string.format('ix = %d\ncurrent\nwaypoint', self.currentWpNode.ix)) + end +end + +-- set the goal WP node's position. This will make sure the goal node is on the same height +-- as the controlled node, avoiding issues when driving on non-level bridges where the controlled node +-- is not vertical and since the goal is very far below it, it will be much closer/further in the controlled +-- node's reference frame. +-- If everyone is on the same height, this error is negligible +function PurePursuitController:setGoalTranslation(x, z) + local gx, _, gz = getWorldTranslation(self.goalWpNode.node) + local _, cy, _ = getWorldTranslation(self.controlledNode) + -- if there's an x, z passed in, use that, otherwise only adjust y to be the same as the controlled node + setTranslation(self.goalWpNode.node, x or gx, cy + 1, z or gz) +end + +-- set the current waypoint for the rest of Courseplay and to notify listeners +function PurePursuitController:setCurrentWaypoint(ix) + -- this is the current waypoint for the rest of Courseplay code, the waypoint we are driving to + -- but never, ever go back. Instead just leave this loop and keep driving to the current goal node + if ix < self.currentWpNode.ix then + if g_updateLoopIndex % 60 == 0 then + self:debug("Won't step current waypoint back from %d to %d.", self.currentWpNode.ix, ix) + end + elseif ix >= self.currentWpNode.ix then + local prevIx = self.currentWpNode.ix + self.currentWpNode:setToWaypointOrBeyond(self.course, ix, self.lookAheadDistance) + -- if ix > #self.course, currentWpNode.ix will always be set to #self.course and the change detection won't work + -- therefore, only call listeners if ix <= #self.course + if ix ~= prevIx and ix <= self.course:getNumberOfWaypoints() then + -- remember to send notification at the end of the loop + self.sendWaypointChange = { current = self.currentWpNode.ix, prev = prevIx } + end + end +end + +function PurePursuitController:showGoalpointDiag(case, ...) + local diagText = string.format(...) + if CpDebug:isChannelActive(CpDebug.DBG_PPC, self.vehicle) then + DebugUtil.drawDebugNode(self.goalWpNode.node, diagText) + DebugUtil.drawDebugNode(self.controlledNode, 'controlled') + end + if case ~= self.case then + self:debug(...) + self.case = case + end +end + +--- Should we be driving in reverse based on the current position on course +function PurePursuitController:isReversing() + if self.course then + return self.course:isReverseAt(self:getCurrentWaypointIx()) or self.course:switchingToForwardAt(self:getCurrentWaypointIx()) + else + return false + end +end + +-- goal point local position in the vehicle's coordinate system +function PurePursuitController:getGoalPointLocalPosition() + return localToLocal(self.goalWpNode.node, self.controlledNode, 0, 0, 0) +end + +-- goal point normalized direction +function PurePursuitController:getGoalPointDirection() + local gx, _, gz = localToLocal(self.goalWpNode.node, self.controlledNode, 0, 0, 0) + local dx, dz = MathUtil.vector2Normalize(gx, gz) + -- check for NaN + if dx ~= dx or dz ~= dz then + return 0, 0 + end + return dx, dz +end + +function PurePursuitController:getGoalPointPosition() + return getWorldTranslation(self.goalWpNode.node) +end + +function PurePursuitController:getCurrentWaypointPosition() + return self:getGoalPointPosition() +end + +function PurePursuitController:getCurrentWaypointYRotation() + return self.course:getYRotationCorrectedForDirectionChanges(self.currentWpNode.ix) +end + +function PurePursuitController:getCrossTrackError() + return self.crossTrackError +end + +function PurePursuitController:reachedLastWaypoint() + return self.relevantWpNode.ix >= self.course:getNumberOfWaypoints() +end + +function PurePursuitController:haveJustPassedWaypoint(ix) + return self.lastPassedWaypointIx and self.lastPassedWaypointIx == ix or false +end + +function PurePursuitController:haveAlreadyPassedWaypoint(ix) + return self.lastPassedWaypointIx and self.lastPassedWaypointIx <= ix or false end \ No newline at end of file diff --git a/scripts/ai/controllers/MotorController.lua b/scripts/ai/controllers/MotorController.lua index aa6f2bf7..f29909af 100644 --- a/scripts/ai/controllers/MotorController.lua +++ b/scripts/ai/controllers/MotorController.lua @@ -25,8 +25,16 @@ function MotorController:update() if not self.isValid then return end + if not self.settings.fuelSave:getValue() then + if not self:getIsStarted() then + self:startMotor() + self.vehicle:raiseAIEvent('onAIFieldWorkerContinue', 'onAIImplementContinue') + end + self.timerSet = false + return + end if self:isFuelSaveDisabled() or self.driveStrategy:getMaxSpeed() > - self.speedThreshold or not self.settings.fuelSave:getValue() then + self.speedThreshold then if not self:getIsStarted() then self:startMotor() self.vehicle:raiseAIEvent("onAIFieldWorkerContinue", "onAIImplementContinue") @@ -77,7 +85,7 @@ function MotorController:getDriveData() end if g_Courseplay.globalSettings.waitForRefueling:getValue() and self:isFuelLow(self.fuelThresholdSetting:getValue()) then - + maxSpeed = 0 end @@ -124,4 +132,4 @@ end function MotorController:getIsStarted() return self.vehicle:getMotorState() ~= MotorState.OFF -end +end \ No newline at end of file diff --git a/scripts/ai/parameters/AIParameterSettingList.lua b/scripts/ai/parameters/AIParameterSettingList.lua index faff2471..dda4d04c 100644 --- a/scripts/ai/parameters/AIParameterSettingList.lua +++ b/scripts/ai/parameters/AIParameterSettingList.lua @@ -81,7 +81,7 @@ end function AIParameterSettingList.getDistanceText(value, precision) precision = precision or 1 if g_Courseplay.globalSettings and g_Courseplay.globalSettings.distanceUnit:getValue() == g_Courseplay.globalSettings.IMPERIAL_UNIT then - return string.format("%.1f %s", value*AIParameterSettingList.FOOT_FACTOR, g_i18n:getText("CP_unit_foot")) + return string.format("%.2f %s", value * AIParameterSettingList.FOOT_FACTOR, g_i18n:getText("CP_unit_foot")) end return string.format("%.".. tostring(precision) .. "f %s", value, g_i18n:getText("CP_unit_meter")) end @@ -105,8 +105,9 @@ AIParameterSettingList.UNITS_CONVERSION = { } AIParameterSettingList.MILES_FACTOR = 0.62137 -AIParameterSettingList.FOOT_FACTOR = 3.28 +AIParameterSettingList.FOOT_FACTOR = 3.28084 AIParameterSettingList.ACRE_FACTOR = 2.4711 +AIParameterSettingList.IMPERIAL_FOOT_INCREMENT = 0.25 AIParameterSettingList.INPUT_VALUE_THRESHOLD = 2 --- Generates numeric values and texts from min to max with incremental of inc or 1. ---@param values table @@ -209,6 +210,11 @@ function AIParameterSettingList:refresh(includeDisabledValues) self:validateTexts() return end + -- Save the current value before overwriting the list. The metric and imperial + -- lists have different sizes, so self.current may point to the wrong value + -- after copying from the metric data table. regenerateValuesForUnit() will + -- use this to restore the correct position in the rebuilt list. + self._savedRefreshValue = self.values[self.current] self.values = {} self.texts = {} for ix, v in ipairs(self.data.values) do @@ -219,6 +225,7 @@ function AIParameterSettingList:refresh(includeDisabledValues) end self:validateCurrentValue() self:validateTexts() + self._savedRefreshValue = nil end --- Gets the texts for the given values. @@ -246,6 +253,10 @@ end function AIParameterSettingList:validateTexts() local unit = self.data.unit local precision = self.data.precision or 2 + if unit == 2 and self.data.min ~= nil and self.data.max ~= nil then + self:regenerateValuesForUnit() + return + end if unit then local unitStrFunc = AIParameterSettingList.UNITS_TEXTS[unit] local fixedTexts = {} @@ -258,6 +269,59 @@ function AIParameterSettingList:validateTexts() end end +--- Rebuilds self.values and self.texts for distance settings when the unit system +--- changes between metric and imperial. In imperial mode, values are generated at +--- exact foot increments (0.25 ft) converted to meters, so arrow buttons step in +--- clean foot amounts and display values are never rounded oddly. +function AIParameterSettingList:regenerateValuesForUnit() + local unit = self.data.unit + if unit ~= 2 or self.data.min == nil or self.data.max == nil then + return + end + + local currentValue = self._savedRefreshValue or self.values[self.current] + + self.values = {} + self.texts = {} + + local isImperial = g_Courseplay.globalSettings and + g_Courseplay.globalSettings.distanceUnit:getValue() == g_Courseplay.globalSettings.IMPERIAL_UNIT + + local precision = self.data.precision or 2 + + if isImperial then + local ftInc = AIParameterSettingList.IMPERIAL_FOOT_INCREMENT + local minFt = math.ceil(self.data.min * AIParameterSettingList.FOOT_FACTOR / ftInc) * ftInc + local maxFt = math.floor(self.data.max * AIParameterSettingList.FOOT_FACTOR / ftInc) * ftInc + for ft = minFt, maxFt, ftInc do + local meters = ft / AIParameterSettingList.FOOT_FACTOR + table.insert(self.values, meters) + local text = AIParameterSettingList.UNITS_TEXTS[unit](meters, precision - 1) + table.insert(self.texts, text) + end + else + local inc = self.data.incremental or 0.1 + AIParameterSettingList.generateValues(self, self.values, self.texts, + self.data.min, self.data.max, inc, unit, precision) + end + + if currentValue and #self.values > 0 then + local closestIx = 1 + local closestDiff = math.huge + for i = 1, #self.values do + local d = math.abs(self.values[i] - currentValue) + if d < closestDiff then + closestIx = i + closestDiff = d + end + end + self.current = closestIx + else + self.current = 1 + end + self:validateCurrentValue() +end + function AIParameterSettingList:saveToXMLFile(xmlFile, key, usedModNames) xmlFile:setString(key .. "#currentValue", tostring(self.values[self.current])) end diff --git a/scripts/ai/strategies/AIDriveStrategyCombineCourse.lua b/scripts/ai/strategies/AIDriveStrategyCombineCourse.lua index cb5f83d8..537b22cb 100644 --- a/scripts/ai/strategies/AIDriveStrategyCombineCourse.lua +++ b/scripts/ai/strategies/AIDriveStrategyCombineCourse.lua @@ -966,14 +966,17 @@ function AIDriveStrategyCombineCourse:callUnloader(bestUnloader, tentativeRendez end ---@param vehicle table ----@return boolean true if vehicle is an active Courseplay controlled combine/harvester +---@return boolean true if vehicle is an active Courseplay controlled combine/harvester, +--- or a manually-driven combine with an active "Call Grain Cart" request function AIDriveStrategyCombineCourse.isActiveCpCombine(vehicle) - if not (vehicle.getIsCpActive and vehicle:getIsCpActive()) then - -- not driven by CP - return false + if vehicle.getIsCpActive and vehicle:getIsCpActive() then + local driveStrategy = vehicle.getCpDriveStrategy and vehicle:getCpDriveStrategy() + return driveStrategy and driveStrategy.callUnloader ~= nil + end + if vehicle.cpIsCallGrainCartActive and vehicle:cpIsCallGrainCartActive() then + return true end - local driveStrategy = vehicle.getCpDriveStrategy and vehicle:getCpDriveStrategy() - return driveStrategy and driveStrategy.callUnloader ~= nil + return false end --- Find an unloader to drive to the target, which may either be the combine itself (when stopped and waiting for unload) diff --git a/scripts/ai/strategies/AIDriveStrategyUnloadCombine.lua b/scripts/ai/strategies/AIDriveStrategyUnloadCombine.lua index eecc77c8..2147ced8 100644 --- a/scripts/ai/strategies/AIDriveStrategyUnloadCombine.lua +++ b/scripts/ai/strategies/AIDriveStrategyUnloadCombine.lua @@ -289,8 +289,36 @@ function AIDriveStrategyUnloadCombine:setAIVehicle(vehicle, jobParameters) AIDriveStrategyCourse.setAIVehicle(self, vehicle) self:setJobParameterValues(jobParameters) self.reverser = AIReverseDriver(self.vehicle, self.ppc) - self.collisionAvoidanceController = CollisionAvoidanceController(self.vehicle, self) - self.proximityController = ProximityController(self.vehicle, self:getProximitySensorWidth()) + -- Unloaders spend a lot of time tracking moving targets (combines, chopper turns, rendezvous + -- points that shift underneath us). A 10 s grace period before off-track shutdown is too + -- short for this use case — give CP up to 20 s to get back on track before even considering + -- the soft-recovery path. The user would rather wait a few extra seconds than have to walk + -- over to the grain cart and restart it manually. + if self.ppc then + self.ppc.offTrackGracePeriodMs = 20000 + end + if CollisionAvoidanceController then + self.collisionAvoidanceController = CollisionAvoidanceController(self.vehicle, self) + else + CpUtil.errorVehicle(self.vehicle, 'Courseplay: CollisionAvoidanceController not loaded (mod conflict?). Collision avoidance disabled.') + self.collisionAvoidanceController = { isCollisionWarningActive = function() return false end } + end + if ProximityController then + self.proximityController = ProximityController(self.vehicle, self:getProximitySensorWidth()) + else + CpUtil.errorVehicle(self.vehicle, 'Courseplay: ProximityController not loaded (mod conflict?). Proximity control disabled.') + self.proximityController = { + registerIsSlowdownEnabledCallback = function() end, + registerBlockingVehicleListener = function() end, + registerIgnoreObjectCallback = function() end, + checkBlockingVehicleFront = function() return math.huge, nil end, + disableLeftSide = function() end, + disableRightSide = function() end, + enableBothSides = function() end, + getDriveData = function(_, maxSpeed) return nil, nil, nil, maxSpeed end, + isVehicleInRange = function() return false end, + } + end self.proximityController:registerIsSlowdownEnabledCallback(self, AIDriveStrategyUnloadCombine.isProximitySpeedControlEnabled) self.proximityController:registerBlockingVehicleListener(self, AIDriveStrategyUnloadCombine.onBlockingVehicle) self.proximityController:registerIgnoreObjectCallback(self, AIDriveStrategyUnloadCombine.ignoreProximityObject) @@ -318,12 +346,53 @@ function AIDriveStrategyUnloadCombine:initializeImplementControllers(vehicle) self:addImplementController(vehicle, FoldableController, Foldable, {}) end +--- Gets the combine's drive strategy or manual combine proxy. +--- Use this instead of combineToUnload:getCpDriveStrategy() to support manual combines. +---@return AIDriveStrategyCombineCourse|CpManualCombineProxy|nil +function AIDriveStrategyUnloadCombine:getCombineStrategy() + if self.combineToUnload then + -- Try the CP drive strategy directly without requiring getIsCpActive() to be true. + -- This prevents releasing the combine during brief CP state transitions (e.g. headland turns) + -- where the strategy object is still valid but getIsCpActive() momentarily returns false. + if self.combineToUnload.getCpDriveStrategy then + local strategy = self.combineToUnload:getCpDriveStrategy() + if strategy then return strategy end + end + -- Fall back to the manual combine proxy + if self.combineToUnload.cpGetManualCombineProxy then + return self.combineToUnload:cpGetManualCombineProxy() + end + end + return nil +end + +--- Checks if the assigned combine is active (either CP-driven or manually calling a grain cart). +---@return boolean +function AIDriveStrategyUnloadCombine:isCombineActive() + if self.combineToUnload then + if self.combineToUnload.getIsCpActive and self.combineToUnload:getIsCpActive() then + return true + end + if self.combineToUnload.cpIsCallGrainCartActive and self.combineToUnload:cpIsCallGrainCartActive() then + return true + end + end + return false +end + function AIDriveStrategyUnloadCombine:isProximitySpeedControlEnabled() - return not (self.state == self.states.UNLOADING_MOVING_COMBINE and self.combineToUnload:getCpDriveStrategy():hasAutoAimPipe()) + local strategy = self:getCombineStrategy() + return not (self.state == self.states.UNLOADING_MOVING_COMBINE and strategy and strategy:hasAutoAimPipe()) end function AIDriveStrategyUnloadCombine:ignoreProximityObject(object, vehicle, moveForwards, hitTerrain) return (self.state == self.states.UNLOADING_ON_THE_FIELD and hitTerrain) or + -- Straw windrow piles are height-map physics objects. Their raycasts register as terrain + -- hits, causing the proximity sensor to bleed speed when crossing perpendicular. + -- The pathfinder has already routed around real terrain obstacles, so terrain hits + -- during approach and unloading are safe to ignore. + (self.state == self.states.DRIVING_TO_COMBINE and hitTerrain) or + (self.state == self.states.UNLOADING_MOVING_COMBINE and hitTerrain) or -- these states handle the proximity by themselves (self.state == self.states.UNLOADING_MOVING_COMBINE and vehicle == self.combineToUnload) or (self.state == self.states.HANDLE_CHOPPER_HEADLAND_TURN and vehicle == self.combineToUnload) @@ -359,8 +428,8 @@ function AIDriveStrategyUnloadCombine:getDriveData(dt, vX, vY, vZ) end -- make sure if we have a combine we stay registered - if self.combineToUnload and self.combineToUnload:getIsCpActive() then - local strategy = self.combineToUnload:getCpDriveStrategy() + if self.combineToUnload and self:isCombineActive() then + local strategy = self:getCombineStrategy() if strategy then if strategy.registerUnloader then strategy:registerUnloader(self) @@ -373,7 +442,7 @@ function AIDriveStrategyUnloadCombine:getDriveData(dt, vX, vY, vZ) end end - if self.combineToUnload == nil or not self.combineToUnload:getIsCpActive() then + if self.combineToUnload == nil or not self:isCombineActive() then if CpUtil.isStateOneOf(self.state, self.combineUnloadStates) then end @@ -409,7 +478,7 @@ function AIDriveStrategyUnloadCombine:getDriveData(dt, vX, vY, vZ) self:setMaxSpeed(0) elseif self.state == self.states.IDLE then -- nothing to do right now, wait for one of the following: - -- - combine calls + -- - combine calls (including manual combines via proxy) -- - user sends us to unload the trailer -- - a trailer appears where we can unload our auger wagon if full self:setMaxSpeed(0) @@ -446,7 +515,7 @@ function AIDriveStrategyUnloadCombine:getDriveData(dt, vX, vY, vZ) elseif self.state == self.states.UNLOADING_MOVING_COMBINE then local x, z - if self.combineToUnload:getCpDriveStrategy():hasAutoAimPipe() then + if self:getCombineStrategy():hasAutoAimPipe() then x, z = self:unloadMovingChopper() else x, z = self:unloadMovingCombine(dt) @@ -487,7 +556,7 @@ function AIDriveStrategyUnloadCombine:getDriveData(dt, vX, vY, vZ) self:setMaxSpeed(self.settings.reverseSpeed:getValue()) if self.state.properties.holdCombine then self:debugSparse('Holding combine while backing up') - self.combineToUnload:getCpDriveStrategy():hold(1000) + self:getCombineStrategy():hold(1000) end -- drive back until the combine is in front of us local _, _, dz = self:getDistanceFromCombine(self.state.properties.vehicle) @@ -544,7 +613,7 @@ end function AIDriveStrategyUnloadCombine:hasToWaitForAssignedCombine() if CpUtil.isStateOneOf(self.state, self.combineUnloadStates) then - return self.combineToUnload == nil or not self.combineToUnload:getIsCpActive() or self.combineToUnload:getCpDriveStrategy() == nil + return self.combineToUnload == nil or not self:isCombineActive() or self:getCombineStrategy() == nil end return false end @@ -572,10 +641,31 @@ function AIDriveStrategyUnloadCombine:startWaitingForSomethingToDo() end end +--- Called by the PurePursuitController when the vehicle has been off-track long enough to +--- trigger the shutdown path. Returning true tells the PPC we've handled recovery and it +--- must NOT call stopCurrentAIJob. We release the current combine, drop to IDLE, and let +--- the combine proxy's normal re-call cycle pick us up again in a couple of seconds. +--- +--- Why this is safe for manual combines: the CpManualCombineProxy.callUnloaderWhenNeeded() +--- runs every 1.5 s. After releaseCombine() the unloader slot expires (1 s TTL) and the +--- proxy re-summons us with a fresh call() / pathfind. The user never has to walk to the +--- grain cart and restart it manually. +--- +--- Why this is safe for CP combines: the active CP combine's drive strategy calls +--- unloader:call() again when it needs an unloader (fill level, end of row), which takes +--- the idle cart and restarts the approach with fresh pathfinding. +---@return boolean handled true if recovery was performed and CP should NOT shut down +function AIDriveStrategyUnloadCombine:onOffTrackShutdown() + self:info('onOffTrackShutdown: off-track grace period expired; soft-recovering to IDLE (state was %s) instead of stopping CP.', + self.state and self.state.name or 'nil') + self:startWaitingForSomethingToDo() + return true +end + ---@return table|nil the best node (of all the fill nodes on all trailers) to use to unload a harvester function AIDriveStrategyUnloadCombine:getBestTargetNode() local function isValidNode(targetNode) - local fillType = self.combineToUnload:getCpDriveStrategy():getFillType() + local fillType = self:getCombineStrategy():getFillType() -- for some harvesters (DeWulf), fill type is unknown until they start working if fillType ~= FillType.UNKNOWN and not targetNode.trailer:getFillUnitAllowsFillType(targetNode.fillUnitIx, fillType) then self:debugSparse("Fill node %d of trailer %s doesn't accept fillType %s!", @@ -632,7 +722,7 @@ function AIDriveStrategyUnloadCombine:driveBesideCombine() return end - local strategy = self.combineToUnload:getCpDriveStrategy() + local strategy = self:getCombineStrategy() -- use a factor to make sure we reach the pipe fast, but be more gentle while discharging local factor = strategy:isDischarging() and 0.75 or 2 local combineSpeed = self.combineToUnload.lastSpeedReal * 3600 @@ -653,12 +743,28 @@ function AIDriveStrategyUnloadCombine:driveBesideCombine() CpUtil.getName(self.vehicle), dz, speed, factor) local gx, gy, gz + -- For manually-driven combines we cannot rely on a fieldwork course to steer by — there is + -- none. Always compute the direct goal point (regardless of dz) so steering is derived from + -- the live pipe reference node and stays locked to the combine's current heading, including + -- through curves. For CP-driven combines keep the original behaviour: only override the PPC + -- course-based goal point when the cart is still far behind the pipe (dz > 5). + local isManual = strategy.isManualProxy and strategy:isManualProxy() -- Calculate an artificial goal point relative to the harvester to align better when starting to unload - if dz > 5 then + if dz > 5 or isManual then _, _, dz = localToLocal(self.vehicle:getAIDirectionNode(), self:getPipeOffsetReferenceNode(), 0, 0, 0) + -- For manual combines: use the vehicle's natural (non-CTE-adjusted) lookahead distance. + -- self.ppc:getLookaheadDistance() can be inflated up to 2x the base value when the cart + -- is far from the stale placeholder course. A 12 m lookahead puts the goal point too far + -- ahead for responsive curve following — a gentle S-curve doesn't register as a heading + -- change until the cart has already overshot. normalLookAheadDistance is constant (≈5-6m) + -- and is not inflated by cross-track error, so turns are tracked much more tightly. + -- For CP-driven combines the inflated lookahead is appropriate (they follow a real course). + local lookahead = isManual + and (self.ppc.normalLookAheadDistance or 6) + or self.ppc:getLookaheadDistance() gx, gy, gz = localToWorld(self:getPipeOffsetReferenceNode(), -- straight line parallel to the harvester, under the pipe, look ahead distance from the unloader - self:getPipeOffset(self.combineToUnload), 0, dz + self.ppc:getLookaheadDistance()) + self:getPipeOffset(self.combineToUnload), 0, dz + lookahead) if CpUtil.isVehicleDebugActive(self.vehicle) and CpDebug:isChannelActive(self.debugChannel) then -- show the goal point @@ -674,7 +780,8 @@ end ------------------------------------------------------------------------------------------------------------------------ function AIDriveStrategyUnloadCombine:isInDeadlock() if self.combineToUnload then - local combineStrategy = self.combineToUnload:getCpDriveStrategy() + local combineStrategy = self:getCombineStrategy() + if not combineStrategy then return false end if self.inDeadlock == nil then self.inDeadlock = CpDelayedBoolean() end @@ -703,7 +810,7 @@ function AIDriveStrategyUnloadCombine:unloadMovingChopper() return end - local combineStrategy = self.combineToUnload:getCpDriveStrategy() + local combineStrategy = self:getCombineStrategy() local gx, gz = self:followChopper() if combineStrategy:isTurning() and not combineStrategy:isFinishingRow() then @@ -792,10 +899,10 @@ function AIDriveStrategyUnloadCombine:handleChopper180Turn() return end - if self.combineToUnload:getCpDriveStrategy():isTurningButNotEndingTurn() then - if self.combineToUnload:getCpDriveStrategy():isTurnForwardOnly() then + if self:getCombineStrategy():isTurningButNotEndingTurn() then + if self:getCombineStrategy():isTurnForwardOnly() then ---@type Course - local turnCourse = self.combineToUnload:getCpDriveStrategy():getTurnCourse() + local turnCourse = self:getCombineStrategy():getTurnCourse() if turnCourse then self:debug('Follow chopper through the turn') self:startCourse(turnCourse:copy(self.vehicle), 1) @@ -868,7 +975,8 @@ end function AIDriveStrategyUnloadCombine:handleChopperTurn(harvester) -- since we are taking care of staying away, ask the chopper to ignore us - harvester:getCpDriveStrategy():requestToIgnoreProximity(self.vehicle) + local harvesterStrategy = self:getCombineStrategy() + if harvesterStrategy then harvesterStrategy:requestToIgnoreProximity(self.vehicle) end local d, dx, dz = self:getDistanceFromCombine(harvester) local combineSpeed = harvester.lastSpeedReal * 3600 @@ -886,12 +994,12 @@ function AIDriveStrategyUnloadCombine:handleChopperTurn(harvester) -- stay closer when still discharging if sameDirection then -- reverse speed is controlled around combine's speed - dReference = harvester:getCpDriveStrategy():isDischarging() and dz or dz - 3 + dReference = (harvesterStrategy and harvesterStrategy:isDischarging()) and dz or dz - 3 speed = combineSpeed + CpMathUtil.clamp(self.targetDistanceBehindChopper - dReference, -combineSpeed, self.settings.reverseSpeed:getValue() * 1.5) else -- reverse speed only depends on distance from the combine, stop when at working width - speed = CpMathUtil.clamp(harvester:getCpDriveStrategy():getWorkWidth() - d, 0, + speed = CpMathUtil.clamp((harvesterStrategy and harvesterStrategy:getWorkWidth() or 6) - d, 0, self.settings.reverseSpeed:getValue() * 1.5) end else @@ -926,8 +1034,8 @@ function AIDriveStrategyUnloadCombine:followChopperThroughTurn() end local d = self:getDistanceFromCombine() - local turnCourse = self.combineToUnload:getCpDriveStrategy():getTurnCourse() - if self.combineToUnload:getCpDriveStrategy():isTurning() and turnCourse ~= nil then + local turnCourse = self:getCombineStrategy():getTurnCourse() + if self:getCombineStrategy():isTurning() and turnCourse ~= nil then -- making sure we are never ahead of the chopper on the course (we both drive the same course), this -- prevents the unloader cutting in front of the chopper when for example the unloader is on the -- right side of the chopper and the chopper reaches a right turn. @@ -951,9 +1059,13 @@ end --- side of the chopper is already harvested, or behind it if both sides have fruit. ------------------------------------------------------------------------------------------------------------------------ function AIDriveStrategyUnloadCombine:calculateAutoAimPipeOffsetX(harvester) - local strategy = harvester and harvester:getCpDriveStrategy() + local strategy = harvester and self:getCombineStrategy() if strategy and strategy.hasAutoAimPipe and strategy:hasAutoAimPipe() then local fruitLeft, fruitRight = strategy:getFruitAtSides() + -- getFruitAtSides() can return nil before checkFruit() has run (strategy just created, + -- or proxy returns nil, nil). Default to 0 so the arithmetic below doesn't crash. + fruitLeft = fruitLeft or 0 + fruitRight = fruitRight or 0 local targetOffsetX, distanceBetweenVehicles = 0, (AIUtil.getWidth(harvester) + AIUtil.getWidth(self.vehicle)) / 2 + 1 -- we use 20% of the average as a threshold for significant difference local fruitThreshold = 0.2 * 0.5 * (fruitLeft + fruitRight) @@ -1080,14 +1192,14 @@ function AIDriveStrategyUnloadCombine:getPipesBaseNode(combine) end function AIDriveStrategyUnloadCombine:getCombineIsTurning() - return self.combineToUnload:getCpDriveStrategy() and self.combineToUnload:getCpDriveStrategy():isTurning() + return self:getCombineStrategy() and self:getCombineStrategy():isTurning() end ---@return number, number x and z offset of the pipe's end from the combine's root node in the Giants coordinate system ---(x > 0 left, z > 0 forward) corrected with the manual offset settings function AIDriveStrategyUnloadCombine:getPipeOffset(combine) - local offsetX, offsetZ = combine:getCpDriveStrategy():getPipeOffset(-self.settings.combineOffsetX:getValue(), self.settings.combineOffsetZ:getValue()) - if combine:getCpDriveStrategy():hasAutoAimPipe() then + local offsetX, offsetZ = self:getCombineStrategy():getPipeOffset(-self.settings.combineOffsetX:getValue(), self.settings.combineOffsetZ:getValue()) + if self:getCombineStrategy():hasAutoAimPipe() then return self:getAutoAimPipeOffsetX(), offsetZ else return offsetX, offsetZ @@ -1097,7 +1209,9 @@ end ---@return number offset X for the course to follow the combine, this is the pipe offset and the combine courser offset function AIDriveStrategyUnloadCombine:getFollowingCourseOffset(combine) local pipeOffset = self:getPipeOffset(combine) - local courseOffset = combine:getCpDriveStrategy():getFieldworkCourse():getOffset() + local combineStrategy = self:getCombineStrategy() + local fieldworkCourse = combineStrategy and combineStrategy:getFieldworkCourse() + local courseOffset = fieldworkCourse and fieldworkCourse:getOffset() or 0 return -pipeOffset + courseOffset end @@ -1106,7 +1220,7 @@ function AIDriveStrategyUnloadCombine:getAutoAimPipeOffsetX() end function AIDriveStrategyUnloadCombine:getCombinesMeasuredBackDistance() - return self.combineToUnload:getCpDriveStrategy():getMeasuredBackDistance() + return self:getCombineStrategy():getMeasuredBackDistance() end function AIDriveStrategyUnloadCombine:getAllTrailersFull(fullThresholdPercentage) @@ -1137,8 +1251,8 @@ end function AIDriveStrategyUnloadCombine:releaseCombine() self.combineJustUnloaded = nil - if self.combineToUnload and self.combineToUnload:getIsCpActive() then - local strategy = self.combineToUnload:getCpDriveStrategy() + if self.combineToUnload and self:isCombineActive() then + local strategy = self:getCombineStrategy() if strategy and strategy.deregisterUnloader then strategy:deregisterUnloader(self) end @@ -1232,7 +1346,7 @@ end function AIDriveStrategyUnloadCombine:isBehindAndAlignedToCombine(debugEnabled) -- if the harvester has an auto aim pipe, like a chopper we can relax our conditions - local hasAutoAimPipe = self.combineToUnload:getCpDriveStrategy():hasAutoAimPipe() + local hasAutoAimPipe = self:getCombineStrategy():hasAutoAimPipe() local dx, _, dz = localToLocal(self.vehicle.rootNode, self:getPipeOffsetReferenceNode(), 0, 0, 0) local pipeOffset = self:getPipeOffset(self.combineToUnload) if dz > (hasAutoAimPipe and -5 or 0) then @@ -1280,11 +1394,11 @@ function AIDriveStrategyUnloadCombine:isInFrontAndAlignedToMovingCombine(debugEn AIDriveStrategyUnloadCombine.maxDirectionDifferenceDeg) return false end - if self.combineToUnload:getCpDriveStrategy():willWaitForUnloadToFinish() then + if self:getCombineStrategy():willWaitForUnloadToFinish() then self:debugIf(debugEnabled, 'isInFrontAndAlignedToMovingCombine: combine is not moving') return false end - if self.combineToUnload:getCpDriveStrategy():alwaysNeedsUnloader() then + if self:getCombineStrategy():alwaysNeedsUnloader() then -- this harvester won't move without an unloader under the pipe, so if our fill node is in front of the -- trailer, there is no point waiting for it dz = self:getBestTargetNodeDistanceFromPipe() @@ -1299,7 +1413,7 @@ function AIDriveStrategyUnloadCombine:isInFrontAndAlignedToMovingCombine(debugEn end function AIDriveStrategyUnloadCombine:isOkToStartUnloadingCombine() - if self.combineToUnload:getCpDriveStrategy():isReadyToUnload(true) then + if self:getCombineStrategy():isReadyToUnload(true) then -- if it always needs an unloader, it won't move without it, so can't start unloading when in front of the combine return self:isBehindAndAlignedToCombine() or self:isInFrontAndAlignedToMovingCombine() else @@ -1362,7 +1476,7 @@ end -- Start to unload the combine (driving to the pipe/closer to combine) ------------------------------------------------------------------------------------------------------------------------ function AIDriveStrategyUnloadCombine:startUnloadingCombine() - if self.combineToUnload:getCpDriveStrategy():willWaitForUnloadToFinish() then + if self:getCombineStrategy():willWaitForUnloadToFinish() then self:debug('Close enough to a stopped combine, drive to pipe') self:startUnloadingStoppedCombine() else @@ -1389,7 +1503,7 @@ end ---@return number approximate waypoint index of the combine's current position function AIDriveStrategyUnloadCombine:setupFollowCourse() ---@type Course - self.combineCourse = self.combineToUnload:getCpDriveStrategy():getFieldworkCourse() + self.combineCourse = self:getCombineStrategy():getFieldworkCourse() if not self.combineCourse then -- TODO: handle this more gracefully, or even better, don't even allow selecting combines with no course self:debugSparse('Waiting for combine to set up a course, can\'t follow') @@ -1397,7 +1511,7 @@ function AIDriveStrategyUnloadCombine:setupFollowCourse() end local followCourse = self.combineCourse:copy(self.vehicle) -- relevant waypoint is the closest to the combine, prefer that so our PPC will get us on course with the proper offset faster - local followCourseIx = self.combineToUnload:getCpDriveStrategy():getClosestFieldworkWaypointIx() or self.combineCourse:getCurrentWaypointIx() + local followCourseIx = self:getCombineStrategy():getClosestFieldworkWaypointIx() or self.combineCourse:getCurrentWaypointIx() return followCourse, followCourseIx end @@ -1412,6 +1526,32 @@ function AIDriveStrategyUnloadCombine:startCourseFollowingCombine() self.followingCourseOffset = self:getFollowingCourseOffset(self.combineToUnload) self.followCourse:setOffset(self.followingCourseOffset, 0) self.reverseForTurnCourse = nil + -- Initialise the refresh timer so the first periodic refresh fires 5 s from now, + -- not on the very first update frame. + self.followCourseRefreshTime = g_time + + -- For manual combines build a minimal placeholder course. The PPC needs SOME course to + -- initialise against, but we never use it for steering: driveBesideCombine() returns a + -- live goal point directly from the pipe reference node on every frame, bypassing the + -- course entirely. The placeholder is a straight 100 m course starting at the combine's + -- current position and pointing in the combine's current heading. + local combineStrategy = self:getCombineStrategy() + if combineStrategy and combineStrategy.isManualProxy and combineStrategy:isManualProxy() then + local combineX, _, combineZ = getWorldTranslation(self.combineToUnload:getAIDirectionNode()) + local forwardX, _, forwardZ = localToWorld(self.combineToUnload:getAIDirectionNode(), 0, 0, 100) + local placeholder = Course.createFromTwoWorldPositions( + self.vehicle, + combineX, combineZ, + forwardX, forwardZ, + 0, 0, 0, 10, false) + if placeholder then + self.followCourse = placeholder + self.followCourse:setOffset(self.followingCourseOffset, 0) + startIx = 1 + self:debug('Manual combine: placeholder follow course (PPC unused; direct steering via driveBesideCombine)') + end + end + self:debug('Will follow combine\'s course at waypoint %d, side offset %.1f', startIx, self.followCourse.offsetX) self:startCourse(self.followCourse, startIx) self:setNewState(self.states.UNLOADING_MOVING_COMBINE) @@ -1422,7 +1562,7 @@ function AIDriveStrategyUnloadCombine:getCombineToUnload() end function AIDriveStrategyUnloadCombine:getPipeOffsetReferenceNode() - return self.combineToUnload:getCpDriveStrategy():getPipeOffsetReferenceNode() + return self:getCombineStrategy():getPipeOffsetReferenceNode() end ------------------------------------------------------------------------------------------------------------------------ @@ -1467,16 +1607,40 @@ end ------------------------------------------------------------------------------------------------------------------------ -- Pathfinding to waiting (not moving) combine ------------------------------------------------------------------------------------------------------------------------ -function AIDriveStrategyUnloadCombine:startPathfindingToWaitingCombine(xOffset, zOffset) +--- @param failureCallback function|nil optional override for pathfinding failure; defaults to +--- onPathfindingFailedToStationaryTarget (which stops the job). Pass onPathfindingFailedToMovingTarget +--- for moving or manual combines so a failure only causes a wait-and-retry instead of killing the job. +--- @param isManualCombine boolean|nil when true, tunes the pathfinder for manual-combine approach: +--- - ignores the combine vehicle as an obstacle (prevents routing around it) +--- - uses a relaxed fruit threshold (25% vs 10%) so the already-harvested approach strip +--- doesn't cause excessive penalty and the search terminates much faster +--- - caps maxIterations at the default 40 000 instead of the potentially huge +--- field-polygon-scaled value, avoiding multi-second searches on large maps +function AIDriveStrategyUnloadCombine:startPathfindingToWaitingCombine(xOffset, zOffset, failureCallback, isManualCombine) local context = PathfinderContext(self.vehicle) - local maxFruitPercent = self:getMaxFruitPercent(self:getPipeOffsetReferenceNode(), xOffset, zOffset) + local maxFruitPercent, vehiclesToIgnore, maxIterations + if isManualCombine then + -- The combine has just harvested the target strip so the approach path has low fruit. + -- A relaxed threshold finds a path in far fewer iterations and still keeps the grain + -- cart out of heavy standing crop. + maxFruitPercent = 25 + -- Exclude the combine itself so the pathfinder routes to the gap behind it rather + -- than treating the combine body as a solid obstacle to go around. + vehiclesToIgnore = { self.combineToUnload } + -- Cap at the default to prevent searches that can run for tens of seconds on large fields. + maxIterations = HybridAStar.defaultMaxIterations + else + maxFruitPercent = self:getMaxFruitPercent(self:getPipeOffsetReferenceNode(), xOffset, zOffset) + vehiclesToIgnore = {} + maxIterations = PathfinderUtil.getMaxIterationsForFieldPolygon(self.vehicle:cpGetFieldPolygon()) + end context:maxFruitPercent(maxFruitPercent) context:offFieldPenalty(self:getOffFieldPenalty(self.combineToUnload)) context:useFieldNum(CpFieldUtil.getFieldNumUnderVehicle(self.combineToUnload)) - context:areaToAvoid(self.combineToUnload:getCpDriveStrategy():getAreaToAvoid()) - context:vehiclesToIgnore({}):maxIterations(PathfinderUtil.getMaxIterationsForFieldPolygon(self.vehicle:cpGetFieldPolygon())) + context:areaToAvoid(self:getCombineStrategy():getAreaToAvoid()) + context:vehiclesToIgnore(vehiclesToIgnore):maxIterations(maxIterations) self.pathfinderController:registerListeners(self, self.onPathfindingDoneToWaitingCombine, - self.onPathfindingFailedToStationaryTarget, self.onPathfindingObstacleAtStart) + failureCallback or self.onPathfindingFailedToStationaryTarget, self.onPathfindingObstacleAtStart) self.pathfinderController:findPathToNode(context, self:getPipeOffsetReferenceNode(), xOffset or 0, zOffset or 0, 3) end @@ -1485,6 +1649,11 @@ function AIDriveStrategyUnloadCombine:onPathfindingDoneToWaitingCombine(controll self:debug('Pathfinding to waiting combine successful') course:adjustForReversing(math.max(1, -AIUtil.getDirectionNodeToReverserNodeOffset(self.vehicle))) self:startCourse(course, 1) + -- Initialise the approach redirect timer and clear the last target so the first + -- redirect check always compares against the current pipe position. + self.lastApproachRedirectTime = g_time + self.lastApproachRedirectX = nil + self.lastApproachRedirectZ = nil self:setNewState(self.states.DRIVING_TO_COMBINE) return true else @@ -1624,12 +1793,12 @@ end --- unloader to come to the combine. ---@return boolean true if the unloader has accepted the request function AIDriveStrategyUnloadCombine:call(combine, waypoint) + self.combineToUnload = combine local xOffset, zOffset = self:getPipeOffset(combine) if waypoint then -- combine set up a rendezvous waypoint for us, go there if self:isPathfindingNeeded(self.vehicle, waypoint, xOffset, zOffset, 25) then self.rendezvousWaypoint = waypoint - self.combineToUnload = combine -- just in case, as the combine may give us a rendezvous waypoint -- where it is full, make sure we are behind the combine zOffset = -self:getCombinesMeasuredBackDistance() - 5 @@ -1646,13 +1815,13 @@ function AIDriveStrategyUnloadCombine:call(combine, waypoint) -- combine wants us to drive directly to it self:debug('call: Combine is waiting for unload, start finding path to combine') self.combineToUnload = combine - if self.combineToUnload:getCpDriveStrategy():isWaitingForUnloadAfterPulledBack() then + if self:getCombineStrategy():isWaitingForUnloadAfterPulledBack() then -- combine pulled back so it's pipe is now out of the fruit. In this case, if the unloader is in front -- of the combine, it sometimes finds a path between the combine and the fruit to the pipe, we are trying to -- fix it here: the target is behind the combine, not under the pipe. When we get there, we may need another -- (short) pathfinding to get under the pipe. zOffset = -self:getCombinesMeasuredBackDistance() - 10 - elseif self.combineToUnload:getCpDriveStrategy():hasAutoAimPipe() then + elseif self:getCombineStrategy():hasAutoAimPipe() then if math.abs(self:getAutoAimPipeOffsetX()) < 3 then -- will drive behind the harvester, so target must be further back, making sure there's a few meters -- between the harvester's back and the tractor's front @@ -1672,7 +1841,12 @@ function AIDriveStrategyUnloadCombine:call(combine, waypoint) self:startUnloadingCombine() elseif self:isPathfindingNeeded(self.vehicle, self:getPipeOffsetReferenceNode(), xOffset, zOffset) then self:setNewState(self.states.WAITING_FOR_PATHFINDER) - self:startPathfindingToWaitingCombine(xOffset, zOffset) + -- For manually-driven combines, tune pathfinding for faster, safer approach. + local isManualCombine = self.combineToUnload.cpIsCallGrainCartActive and + self.combineToUnload:cpIsCallGrainCartActive() + self:startPathfindingToWaitingCombine(xOffset, zOffset, + isManualCombine and self.onPathfindingFailedToMovingTarget or nil, + isManualCombine) else self:debug('Can\'t start pathfinding to waiting combine, and not in a good position to unload, too close?') self:startWaitingForSomethingToDo() @@ -1722,12 +1896,13 @@ end function AIDriveStrategyUnloadCombine:getOffFieldPenalty(combineToUnload) local offFieldPenalty = PathfinderContext.defaultOffFieldPenalty if combineToUnload then - if combineToUnload:getCpDriveStrategy():isOnHeadland(1) then + local strategy = self:getCombineStrategy() + if strategy and strategy:isOnHeadland(1) then -- when the combine is on the first headland, chances are that we have to drive off-field to it, -- so make the life easier for the pathfinder offFieldPenalty = PathfinderContext.defaultOffFieldPenalty / 5 self:debug('Combine is on first headland, reducing off-field penalty for pathfinder to %.1f', offFieldPenalty) - elseif combineToUnload:getCpDriveStrategy():isOnHeadland(2) then + elseif strategy and strategy:isOnHeadland(2) then -- reduce less when on the second headland, there's more chance we'll be able to get to the combine -- on the headland offFieldPenalty = PathfinderContext.defaultOffFieldPenalty / 3 @@ -1796,7 +1971,7 @@ function AIDriveStrategyUnloadCombine:updateCombineStatus() end -- add hysteresis to reversing info from combine, isReversing() may temporarily return false during reversing, make sure we need -- multiple update loops to change direction - local combineToUnloadReversing = self.combineToUnloadReversing + (self.combineToUnload:getCpDriveStrategy():isReversing() and 0.1 or -0.1) + local combineToUnloadReversing = self.combineToUnloadReversing + (self:getCombineStrategy():isReversing() and 0.1 or -0.1) if self.combineToUnloadReversing < 0 and combineToUnloadReversing >= 0 then -- direction changed self.combineToUnloadReversing = 1 @@ -1824,10 +1999,10 @@ function AIDriveStrategyUnloadCombine:changeToUnloadWhenTrailerFull() else self:debug('trailer full, changing to unload course.') end - if self.combineToUnload:getCpDriveStrategy():isTurning() or - self.combineToUnload:getCpDriveStrategy():isAboutToTurn() then + if self:getCombineStrategy():isTurning() or + self:getCombineStrategy():isAboutToTurn() then self:debug('... but we are too close to the end of the row, or combine is turning, moving back before changing to unload course') - elseif self.combineToUnload and self.combineToUnload:getCpDriveStrategy():isAboutToReturnFromPocket() then + elseif self.combineToUnload and self:getCombineStrategy():isAboutToReturnFromPocket() then self:debug('... letting the combine return from the pocket') else self:debug('... moving back a little in case AD wants to take over') @@ -1858,7 +2033,7 @@ end --- we probably rather not approach the area around the turn so we are not in the way --- of the combine while it is turning. function AIDriveStrategyUnloadCombine:checkForCombineTurnArea() - local turnAreaCenterWp, r = self.combineToUnload:getCpDriveStrategy():getTurnArea() + local turnAreaCenterWp, r = self:getCombineStrategy():getTurnArea() if turnAreaCenterWp and turnAreaCenterWp:getDistanceFromVehicle(self.vehicle) <= r then self:debugSparse('Waiting for combine to pass the turn at %.1f, %.1f (r = %.1f) before the rendezvous waypoint', turnAreaCenterWp.x, turnAreaCenterWp.z, r) @@ -1875,7 +2050,48 @@ function AIDriveStrategyUnloadCombine:driveToCombine() self:setFieldSpeed() - self.combineToUnload:getCpDriveStrategy():reconfirmRendezvous() + self:getCombineStrategy():reconfirmRendezvous() + + -- Every ~10 s, redirect toward the combine's live position without stopping, but ONLY if the + -- combine's pipe has moved more than 15 m from where we last aimed. This prevents jarring + -- course corrections when the combine is driving straight and the redirect is unnecessary. + if g_time - (self.lastApproachRedirectTime or g_time) > 10000 then + self.lastApproachRedirectTime = g_time + local d = self:getDistanceFromCombine() + -- Only redirect when the combine is far enough that a course update is meaningful; + -- close-in positioning is handled by isOkToStartUnloadingCombine() below. + if d > 20 then + -- Check whether the pipe has actually moved enough to warrant a new course. + local cX, cY, cZ = getWorldTranslation(self:getPipeOffsetReferenceNode()) + local lastX = self.lastApproachRedirectX or cX + local lastZ = self.lastApproachRedirectZ or cZ + local pipeMoved = MathUtil.vector2Length(cX - lastX, cZ - lastZ) + -- Straight-line redirect is only safe when the combine is roughly ahead of the grain + -- cart (within ~40°). If the combine is off to the side (e.g. it turned onto the next + -- row) a straight line would cut through standing crop. Skip the redirect in that case + -- and let the next A* call handle the reroute. + local lx, _, lz = worldToLocal(self.vehicle:getAIDirectionNode(), cX, cY, cZ) + local combineAhead = lz > 0 + local angleToCombieDeg = math.deg(math.abs(math.atan2(lx, math.max(lz, 0.001)))) + if combineAhead and angleToCombieDeg < 40 and pipeMoved > 15 then + self.lastApproachRedirectX = cX + self.lastApproachRedirectZ = cZ + local xOffset, zOffset = self:getPipeOffset(self.combineToUnload) + local zTarget = -self:getCombinesMeasuredBackDistance() - 3 + local redirectCourse = Course.createFromNodeToNode( + self.vehicle, + self.vehicle:getAIDirectionNode(), + self:getPipeOffsetReferenceNode(), + xOffset, 0, zTarget, 5, false) + if redirectCourse then + self:debug('driveToCombine: redirecting toward combine live position (d=%.1f m, angle=%.1f°, pipeMoved=%.1f m)', d, angleToCombieDeg, pipeMoved) + self:startCourse(redirectCourse, 1) + end + else + self:debug('driveToCombine: skipping redirect, combine is off-axis (ahead=%s, angle=%.1f°) or pipe has not moved enough (%.1f m)', tostring(combineAhead), angleToCombieDeg, pipeMoved) + end + end + end -- towards the end of the course we start checking if we can already switch to unload if self.course:getDistanceToLastWaypoint(self.course:getCurrentWaypointIx()) < 15 and @@ -1896,26 +2112,26 @@ function AIDriveStrategyUnloadCombine:driveToMovingCombine() self:checkForCombineTurnArea() -- stop when too close to a combine not ready to unload (wait until it is done with turning for example) - if self:isWithinSafeManeuveringDistance(self.combineToUnload) and self.combineToUnload:getCpDriveStrategy():isManeuvering() then + if self:isWithinSafeManeuveringDistance(self.combineToUnload) and self:getCombineStrategy():isManeuvering() then self:startWaitingForManeuveringCombine() elseif self:isOkToStartUnloadingCombine() then self:startUnloadingCombine() end - if self.combineToUnload:getCpDriveStrategy():isWaitingForUnload() then + if self:getCombineStrategy():isWaitingForUnload() then self:debug('combine is now stopped and waiting for unload, wait for it to call again') self:startWaitingForSomethingToDo() return end if self.course:isCloseToLastWaypoint(AIDriveStrategyUnloadCombine.driveToCombineCourseExtensionLength / 2) and - self.combineToUnload:getCpDriveStrategy():hasRendezvousWith(self.vehicle) then + self:getCombineStrategy():hasRendezvousWith(self.vehicle) then self:debugSparse('Combine is late, waiting ...') self:setMaxSpeed(0) -- stop confirming the rendezvous, allow the combine to time out if it can't get here on time else -- yes honey, I'm on my way! - self.combineToUnload:getCpDriveStrategy():reconfirmRendezvous() + self:getCombineStrategy():reconfirmRendezvous() end end @@ -1933,7 +2149,7 @@ function AIDriveStrategyUnloadCombine:startWaitingForManeuveringCombine() end function AIDriveStrategyUnloadCombine:waitForManeuveringCombine() - if self:isWithinSafeManeuveringDistance(self.combineToUnload) and self.combineToUnload:getCpDriveStrategy():isManeuvering() then + if self:isWithinSafeManeuveringDistance(self.combineToUnload) and self:getCombineStrategy():isManeuvering() then self:setMaxSpeed(0) else self:debug('Combine stopped maneuvering') @@ -1959,7 +2175,7 @@ function AIDriveStrategyUnloadCombine:unloadStoppedCombine() return end local gx, gz - local combineDriver = self.combineToUnload:getCpDriveStrategy() + local combineDriver = self:getCombineStrategy() if combineDriver:isUnloadFinished() then if combineDriver:isWaitingForUnloadAfterCourseEnded() then if combineDriver:getFillLevelPercentage() < 0.1 then @@ -1998,23 +2214,61 @@ function AIDriveStrategyUnloadCombine:unloadMovingCombine() self.followingCourseOffset = self:getFollowingCourseOffset(self.combineToUnload) self.followCourse:setOffset(self.followingCourseOffset, 0) + -- NOTE: For manually-driven combines we do NOT refresh the follow course. + -- driveBesideCombine() computes the steering goal point directly from the pipe reference + -- node every frame (see the isManual branch there), so the course is never consulted for + -- steering. This prevents any possibility of angle lock, stale courses, or backward + -- courses produced by synthesising a follow course from the combine's live position. + -- + -- Because the placeholder follow course is deliberately static while steering happens + -- off a live pipe reference, the cart WILL drift far away from the placeholder course as + -- the combine curves or S-turns. The PPC's off-track shutdown would see that drift and + -- kill the CP helper. Keep the check continuously disabled while a manual combine is the + -- unload target. We use 5000 ms (much longer than any realistic frame interval) so there + -- is no risk of a brief re-enable window between ticks. + do + local combineStrategy = self:getCombineStrategy() + if combineStrategy and combineStrategy.isManualProxy and combineStrategy:isManualProxy() then + if self.ppc and self.ppc.disableStopWhenOffTrack then + self.ppc:disableStopWhenOffTrack(5000) + end + end + end + if self:changeToUnloadWhenTrailerFull() then return end - local combineStrategy = self.combineToUnload:getCpDriveStrategy() + local combineStrategy = self:getCombineStrategy() local gx, gz = self:driveBesideCombine() + -- For manually-driven combines the strategy IS a CpManualCombineProxy. + -- The farmer is in full control: ignore fill level, alignment, turning state, etc. + -- Stay under the pipe until the proxy's isUnloadFinished() fires (pipe closed for 2s) + -- or the grain cart's own trailer fills up (handled by changeToUnloadWhenTrailerFull above). + if combineStrategy.isManualProxy and combineStrategy:isManualProxy() then + self:debugSparse('unloadMovingCombine (manual): isDischarging=%s', + tostring(combineStrategy.isDischarging and combineStrategy:isDischarging())) + return gx, gz + end + --when the combine is empty, stop and wait for next combine (unless this can't work without an unloader nearby) - if combineStrategy:getFillLevelPercentage() <= 0.1 and not combineStrategy:alwaysNeedsUnloader() then - self:debug('Combine empty, finish unloading.') + local fillPct = combineStrategy:getFillLevelPercentage() + local isDischarging = combineStrategy.isDischarging and combineStrategy:isDischarging() + local isUnloadFinished = combineStrategy.isUnloadFinished and combineStrategy:isUnloadFinished() + self:debugSparse('unloadMovingCombine: fillPct=%.2f isDischarging=%s isUnloadFinished=%s', + fillPct, tostring(isDischarging), tostring(isUnloadFinished)) + -- Don't exit on fill level while the combine is still actively discharging — the pipe hasn't + -- closed yet and we'd leave with grain still flowing. Wait for discharge to stop first. + if fillPct <= 0.1 and not isDischarging and not combineStrategy:alwaysNeedsUnloader() then + self:debug('unloadMovingCombine: EXIT - combine empty (fillPct=%.2f) and not discharging, finishing unloading.', fillPct) self:onUnloadingMovingCombineFinished(combineStrategy) return end -- combine stopped in the meanwhile, like for example end of course if combineStrategy:willWaitForUnloadToFinish() then - self:debug('change to unload stopped combine') + self:debug('unloadMovingCombine: EXIT - willWaitForUnloadToFinish=true, switching to UNLOADING_STOPPED_COMBINE') self:setNewState(self.states.UNLOADING_STOPPED_COMBINE) return end @@ -2060,8 +2314,8 @@ function AIDriveStrategyUnloadCombine:unloadMovingCombine() -- call these again just to log the reason self:isBehindAndAlignedToCombine(true) self:isInFrontAndAlignedToMovingCombine(true) - self:info('not in a good position to unload, cancelling rendezvous, trying to recover') - -- for some reason (like combine turned) we are not in a good position anymore then set us up again + self:info('unloadMovingCombine: EXIT - not in a good position (behind=%s, inFront=%s), startWaitingForSomethingToDo', + tostring(self:isBehindAndAlignedToCombine()), tostring(self:isInFrontAndAlignedToMovingCombine())) self:startWaitingForSomethingToDo() end return gx, gz @@ -2175,7 +2429,9 @@ function AIDriveStrategyUnloadCombine:onBlockingVehicle(blockingVehicle, isBack) -- with an offset from the combine that makes sure we are clear. Use the trailer's root node (and not -- the tractor's) as when we reversing, it is easier when the trailer remains on the same side of the combine local dx, _, _ = localToLocal(referenceObject.rootNode, blockingVehicle:getAIDirectionNode(), 0, 0, 0) - local xOffset = self.vehicle.size.width / 2 + blockingVehicle:getCpDriveStrategy():getWorkWidth() / 2 + 2 + local blockingStrategy = blockingVehicle:getCpDriveStrategy() or (blockingVehicle.cpGetManualCombineProxy and blockingVehicle:cpGetManualCombineProxy()) + local blockingWorkWidth = blockingStrategy and blockingStrategy:getWorkWidth() or 6 + local xOffset = self.vehicle.size.width / 2 + blockingWorkWidth / 2 + 2 xOffset = dx > 0 and xOffset or -xOffset self:setNewState(self.states.MOVING_AWAY_FROM_OTHER_VEHICLE) self.state.properties.vehicle = blockingVehicle @@ -2368,7 +2624,7 @@ end function AIDriveStrategyUnloadCombine:makeRoomForCombineTurningOnHeadland() local dProximity, _ = self.proximityController:checkBlockingVehicleFront() local d, _, dz = self:getDistanceFromCombine(self.combineToUnload) - local dLimit = 0.6 * self.combineToUnload:getCpDriveStrategy():getWorkWidth() + local dLimit = 0.6 * self:getCombineStrategy():getWorkWidth() -- if we are already behind the harvester's back and far enough and not blocking it and -- not in our proximity, then stop if dz > 0 and d > dLimit and dProximity > dLimit then diff --git a/scripts/events/CallGrainCartEvent.lua b/scripts/events/CallGrainCartEvent.lua new file mode 100644 index 00000000..f8927fa1 --- /dev/null +++ b/scripts/events/CallGrainCartEvent.lua @@ -0,0 +1,42 @@ +---@class CallGrainCartEvent +CallGrainCartEvent = {} +local CallGrainCartEvent_mt = Class(CallGrainCartEvent, Event) + +InitEventClass(CallGrainCartEvent, "CallGrainCartEvent") + +function CallGrainCartEvent.emptyNew() + local self = Event.new(CallGrainCartEvent_mt) + return self +end + +function CallGrainCartEvent.new(vehicle) + local self = CallGrainCartEvent.emptyNew() + self.vehicle = vehicle + return self +end + +function CallGrainCartEvent:readStream(streamId, connection) + self.vehicle = NetworkUtil.readNodeObject(streamId) + self:run(connection) +end + +function CallGrainCartEvent:writeStream(streamId, connection) + NetworkUtil.writeNodeObject(streamId, self.vehicle) +end + +function CallGrainCartEvent:run(connection) + if self.vehicle and self.vehicle.cpToggleCallGrainCart then + self.vehicle:cpToggleCallGrainCart() + end + if not connection:getIsServer() then + g_server:broadcastEvent(CallGrainCartEvent.new(self.vehicle), nil, connection, self.vehicle) + end +end + +function CallGrainCartEvent.sendEvent(vehicle) + if g_server ~= nil then + g_server:broadcastEvent(CallGrainCartEvent.new(vehicle), nil, nil, vehicle) + else + g_client:getServerConnection():sendEvent(CallGrainCartEvent.new(vehicle)) + end +end diff --git a/scripts/gui/hud/CpFieldworkHudPage.lua b/scripts/gui/hud/CpFieldworkHudPage.lua index 21f27c86..26877113 100644 --- a/scripts/gui/hud/CpFieldworkHudPage.lua +++ b/scripts/gui/hud/CpFieldworkHudPage.lua @@ -78,7 +78,23 @@ function CpFieldWorkHudPageElement:setupElements(baseHud, vehicle, lines, wMargi baseHud:openCourseManagerGui(vehicle) end, vehicle) - CpGuiUtil.addCopyCourseBtn(self, baseHud, vehicle, lines, wMargin, hMargin, 1) + CpGuiUtil.addCopyCourseBtn(self, baseHud, vehicle, lines, wMargin, hMargin, 1) + + --- Call Grain Cart toggle button (left side) + self.callGrainCartBtn = baseHud:addLeftLineTextButton(self, 6, CpBaseHud.defaultFontSize, + function(vehicle) + if vehicle.cpToggleCallGrainCart then + vehicle:cpToggleCallGrainCart() + end + end, vehicle) + + --- Call Grain Cart status text (right side) + self.callGrainCartStatus = baseHud:addRightLineTextButton(self, 6, CpBaseHud.defaultFontSize, + function(vehicle) + if vehicle.cpToggleCallGrainCart then + vehicle:cpToggleCallGrainCart() + end + end, vehicle) end function CpFieldWorkHudPageElement:update(dt) @@ -130,4 +146,35 @@ function CpFieldWorkHudPageElement:updateContent(vehicle, status) end CpGuiUtil.updateCopyBtn(self, vehicle, status) + + if self.callGrainCartBtn then + local hasPipe = vehicle.spec_pipe ~= nil or AIUtil.hasChildVehicleWithSpecialization(vehicle, Pipe) + local isCpActive = vehicle:getIsCpActive() + local isCallActive = vehicle.cpIsCallGrainCartActive and vehicle:cpIsCallGrainCartActive() + -- Forage harvesters have a rotatable auto-aim spout (numAutoAimingStates > 0). + -- The manual call system is not supported for them, so hide the button entirely. + local isChopper = false + local pipeSpec = vehicle.spec_pipe + if not pipeSpec then + for _, child in ipairs(vehicle:getChildVehicles()) do + if child.spec_pipe then pipeSpec = child.spec_pipe; break end + end + end + if pipeSpec then isChopper = (pipeSpec.numAutoAimingStates or 0) > 0 end + local showBtn = hasPipe and not isCpActive and not isChopper + self.callGrainCartBtn:setVisible(showBtn) + self.callGrainCartStatus:setVisible(showBtn) + if showBtn then + self.callGrainCartBtn:setTextDetails(g_i18n:getText("CP_callGrainCart")) + if isCallActive then + self.callGrainCartBtn:setColor(unpack(CpBaseHud.ON_COLOR)) + self.callGrainCartStatus:setTextDetails(g_i18n:getText("CP_callGrainCartActive")) + self.callGrainCartStatus:setColor(unpack(CpBaseHud.ON_COLOR)) + else + self.callGrainCartBtn:setColor(unpack(CpBaseHud.OFF_COLOR)) + self.callGrainCartStatus:setTextDetails(g_i18n:getText("CP_callGrainCartInactive")) + self.callGrainCartStatus:setColor(unpack(CpBaseHud.OFF_COLOR)) + end + end + end end diff --git a/scripts/pathfinder/PathfinderConstraints.lua b/scripts/pathfinder/PathfinderConstraints.lua index c08f136a..d8cca3ce 100644 --- a/scripts/pathfinder/PathfinderConstraints.lua +++ b/scripts/pathfinder/PathfinderConstraints.lua @@ -93,6 +93,22 @@ function PathfinderConstraints:resetCounts() self.preferredPathPenaltyCount = 0 end +--- Full width and length of the vehicle (and towed implement) for fruit checks, with 1 m buffer. +--- Ensures the pathfinder avoids nodes where any part of the rig would be on standing fruit. +function PathfinderConstraints:getFruitCheckDimensions() + local vParams = self.vehicleData:getVehicleOverlapBoxParams() + local fruitLength = (vParams.length or 2) * 2 + local fruitWidth = (vParams.width or 2) * 2 + if self.vehicleData:getTowedImplement() then + local tParams = self.vehicleData:getTowedImplementOverlapBoxParams() + if tParams then + fruitLength = math.max(fruitLength, tParams.length * 2) + fruitWidth = math.max(fruitWidth, tParams.width * 2) + end + end + return fruitLength + 1, fruitWidth + 1 +end + --- Calculate penalty for this node. The penalty will be added to the cost of the node. This allows for --- obstacle avoidance or forcing the search to remain in certain areas. ---@param node State3D @@ -120,7 +136,8 @@ function PathfinderConstraints:getNodePenalty(node) node.offField = true end if not offField then - local hasFruit, fruitValue = PathfinderUtil.hasFruit(node.x, -node.y, 4, 4, self.areaToIgnoreFruit) + local fruitLength, fruitWidth = self:getFruitCheckDimensions() + local hasFruit, fruitValue = PathfinderUtil.hasFruit(node.x, -node.y, fruitLength, fruitWidth, self.areaToIgnoreFruit) if hasFruit and fruitValue > self.maxFruitPercent then penalty = penalty + fruitValue / 2 self.fruitPenaltyNodeCount = self.fruitPenaltyNodeCount + 1 @@ -169,7 +186,8 @@ end --- that analytic paths are almost always invalid when they go near the fruit. Since analytic paths are only at the --- beginning at the end of the course and mostly curves, it is no problem getting closer to the fruit than otherwise function PathfinderConstraints:isValidAnalyticSolutionNode(node, log) - local hasFruit, fruitValue = PathfinderUtil.hasFruit(node.x, -node.y, 3, 3, self.areaToIgnoreFruit) + local fruitLength, fruitWidth = self:getFruitCheckDimensions() + local hasFruit, fruitValue = PathfinderUtil.hasFruit(node.x, -node.y, fruitLength, fruitWidth, self.areaToIgnoreFruit) local analyticLimit = self.maxFruitPercent * 2 if hasFruit and fruitValue > analyticLimit then if log then diff --git a/scripts/pathfinder/PathfinderUtil.lua b/scripts/pathfinder/PathfinderUtil.lua index ea8736e7..4c437944 100644 --- a/scripts/pathfinder/PathfinderUtil.lua +++ b/scripts/pathfinder/PathfinderUtil.lua @@ -269,6 +269,21 @@ function PathfinderUtil.hasFruit(x, z, length, width, areaToIgnoreFruit) break end end + -- Ignore windrow/swath types (cut material lying on the ground). Use the game API first: + -- isFillTypeWindrow covers all registered windrow fill types including DLC additions. + -- Name-based fallback catches any types not in the windrow registry (e.g. mod types). + if not ignoreThis then + local fillTypeIndex = g_fillTypeManager:getFillTypeIndexByName(fruitType.name) + if fillTypeIndex and g_fruitTypeManager:isFillTypeWindrow(fillTypeIndex) then + ignoreThis = true + end + end + if not ignoreThis then + local name = string.lower(fruitType.name or '') + if string.find(name, 'windrow') or string.find(name, 'swath') or name == 'straw' or name == 'chaff' then + ignoreThis = true + end + end if not ignoreThis then -- if the last boolean parameter is true then it returns fruitValue > 0 for fruits/states ready for forage also local fruitValue, numPixels, totalNumPixels, c = FSDensityMapUtil.getFruitArea(fruitType.index, x - width / 2, z - length / 2, x + width / 2, z, x, z + length / 2, true, true) diff --git a/scripts/specializations/CpAIFieldWorker.lua b/scripts/specializations/CpAIFieldWorker.lua index 0206324c..745a591c 100644 --- a/scripts/specializations/CpAIFieldWorker.lua +++ b/scripts/specializations/CpAIFieldWorker.lua @@ -41,6 +41,8 @@ end function CpAIFieldWorker.registerEventListeners(vehicleType) SpecializationUtil.registerEventListener(vehicleType, "onLoad", CpAIFieldWorker) SpecializationUtil.registerEventListener(vehicleType, "onLoadFinished", CpAIFieldWorker) + SpecializationUtil.registerEventListener(vehicleType, "onUpdate", CpAIFieldWorker) + SpecializationUtil.registerEventListener(vehicleType, "onDelete", CpAIFieldWorker) SpecializationUtil.registerEventListener(vehicleType, "onCpEmpty", CpAIFieldWorker) SpecializationUtil.registerEventListener(vehicleType, "onCpFull", CpAIFieldWorker) @@ -70,6 +72,10 @@ function CpAIFieldWorker.registerFunctions(vehicleType) SpecializationUtil.registerFunction(vehicleType, "startCpAtLastWp", CpAIFieldWorker.startCpAtLastWp) SpecializationUtil.registerFunction(vehicleType, "getCpStartingPointSetting", CpAIFieldWorker.getCpStartingPointSetting) SpecializationUtil.registerFunction(vehicleType, "getCpLaneOffsetSetting", CpAIFieldWorker.getCpLaneOffsetSetting) + + SpecializationUtil.registerFunction(vehicleType, "cpToggleCallGrainCart", CpAIFieldWorker.cpToggleCallGrainCart) + SpecializationUtil.registerFunction(vehicleType, "cpIsCallGrainCartActive", CpAIFieldWorker.cpIsCallGrainCartActive) + SpecializationUtil.registerFunction(vehicleType, "cpGetManualCombineProxy", CpAIFieldWorker.cpGetManualCombineProxy) end function CpAIFieldWorker.registerOverwrittenFunctions(vehicleType) @@ -260,6 +266,61 @@ function CpAIFieldWorker:onCpFinished() end +------------------------------------------------------------------------------------------------------------------------ +--- Manual combine "Call Grain Cart" proxy management +------------------------------------------------------------------------------------------------------------------------ + +function CpAIFieldWorker:onUpdate(dt) + local spec = CpAIFieldWorker.getSpec(self) + if spec and spec.cpManualCombineProxy then + if self:getIsCpActive() then + self:cpToggleCallGrainCart() + else + spec.cpManualCombineProxy:update(dt) + end + end +end + +function CpAIFieldWorker:onDelete() + local spec = CpAIFieldWorker.getSpec(self) + if spec and spec.cpManualCombineProxy then + spec.cpManualCombineProxy:delete() + spec.cpManualCombineProxy = nil + end +end + +function CpAIFieldWorker:cpToggleCallGrainCart() + local spec = CpAIFieldWorker.getSpec(self) + if not spec then return end + if spec.cpManualCombineProxy then + CpUtil.debugVehicle(CpDebug.DBG_FIELDWORK, self, 'Call grain cart deactivated') + spec.cpManualCombineProxy:delete() + spec.cpManualCombineProxy = nil + else + if self:getIsCpActive() then + CpUtil.debugVehicle(CpDebug.DBG_FIELDWORK, self, 'Cannot call grain cart while CP is active') + return + end + CpUtil.debugVehicle(CpDebug.DBG_FIELDWORK, self, 'Call grain cart activated') + spec.cpManualCombineProxy = CpManualCombineProxy(self) + end + if not self.isServer then + CallGrainCartEvent.sendEvent(self) + end +end + +function CpAIFieldWorker:cpIsCallGrainCartActive() + local spec = CpAIFieldWorker.getSpec(self) + return spec and spec.cpManualCombineProxy ~= nil +end + +function CpAIFieldWorker:cpGetManualCombineProxy() + local spec = CpAIFieldWorker.getSpec(self) + return spec and spec.cpManualCombineProxy +end + +------------------------------------------------------------------------------------------------------------------------ + function CpAIFieldWorker:getCanStartCpFieldWork() self:updateAIFieldWorkerImplementData() -- built in helper can't handle it, but we may be able to ... diff --git a/scripts/specializations/CpAIWorker.lua b/scripts/specializations/CpAIWorker.lua index 75655dc2..ee08a256 100644 --- a/scripts/specializations/CpAIWorker.lua +++ b/scripts/specializations/CpAIWorker.lua @@ -180,6 +180,12 @@ function CpAIWorker:onRegisterActionEvents(isActiveForInput, isActiveForInputIgn CpGuiUtil.openCourseManagerGui(self) end, g_i18n:getText("input_CP_OPEN_COURSEMANAGER")) + addActionEvent(self, InputAction.CP_CALL_GRAIN_CART, function () + if self.cpToggleCallGrainCart then + self:cpToggleCallGrainCart() + end + end) + CpAIWorker.updateActionEvents(self) end end @@ -245,6 +251,28 @@ function CpAIWorker:updateActionEvents() actionEvent = spec.actionEvents[InputAction.CP_GENERATE_COURSE] g_inputBinding:setActionEventActive(actionEvent.actionEventId, self:getCanStartCpFieldWork()) + + actionEvent = spec.actionEvents[InputAction.CP_CALL_GRAIN_CART] + if actionEvent then + local hasPipe = self.spec_pipe ~= nil or AIUtil.hasChildVehicleWithSpecialization(self, Pipe) + local isCpActive = self:getIsCpActive() + -- Forage harvesters (auto-aim rotatable spout) are not supported — hide the keybind. + local pipeSpec = self.spec_pipe + if not pipeSpec then + for _, child in ipairs(self:getChildVehicles()) do + if child.spec_pipe then pipeSpec = child.spec_pipe; break end + end + end + local isChopper = pipeSpec and (pipeSpec.numAutoAimingStates or 0) > 0 + local showCallGrainCart = hasPipe and not isCpActive and not isChopper + g_inputBinding:setActionEventActive(actionEvent.actionEventId, showCallGrainCart) + if showCallGrainCart then + local isActive = self.cpIsCallGrainCartActive and self:cpIsCallGrainCartActive() + local status = isActive and g_i18n:getText("CP_callGrainCartActive") or g_i18n:getText("CP_callGrainCartInactive") + g_inputBinding:setActionEventText(actionEvent.actionEventId, + string.format("%s (%s)", g_i18n:getText("CP_callGrainCart"), status)) + end + end end end diff --git a/scripts/specializations/CpCourseManager.lua b/scripts/specializations/CpCourseManager.lua index fd5b0421..6bd23b25 100644 --- a/scripts/specializations/CpCourseManager.lua +++ b/scripts/specializations/CpCourseManager.lua @@ -367,7 +367,7 @@ function CpCourseManager:onPreDelete() if spec then g_assignedCoursesManager:unregisterVehicle(self, self.id) CpCourseManager.resetCourses(self) - if spec.courseDisplay then spec.courseDisplay:delete() end + spec.courseDisplay:delete() spec.courseDisplay = nil end end diff --git a/translations/translation_cz.xml b/translations/translation_cz.xml index 4ac35391..836924a7 100644 --- a/translations/translation_cz.xml +++ b/translations/translation_cz.xml @@ -201,8 +201,8 @@ - - + + diff --git a/translations/translation_en.xml b/translations/translation_en.xml index 29744c14..bdb3a526 100644 --- a/translations/translation_en.xml +++ b/translations/translation_en.xml @@ -13,6 +13,9 @@ + + + @@ -1103,5 +1106,6 @@ Now your selection should look similar to the image. + diff --git a/translations/translation_ru.xml b/translations/translation_ru.xml index 9a86114e..193b1bd5 100644 --- a/translations/translation_ru.xml +++ b/translations/translation_ru.xml @@ -201,8 +201,8 @@ - - + + diff --git a/translations/translation_uk.xml b/translations/translation_uk.xml index 4249b914..3b6ed43f 100644 --- a/translations/translation_uk.xml +++ b/translations/translation_uk.xml @@ -201,8 +201,8 @@ - - + + From a2455baf58b4003da0c0054d1645ce223801c30c Mon Sep 17 00:00:00 2001 From: Antler 22 Date: Tue, 21 Apr 2026 09:25:48 -0500 Subject: [PATCH 3/4] Update README.md --- README.md | 302 ++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 302 insertions(+) diff --git a/README.md b/README.md index 6d504352..db3bbcfe 100644 --- a/README.md +++ b/README.md @@ -2,3 +2,305 @@ FORKED from Original Courseplay Antler22's edit to allow manual combine unloading, US units, and some small tweaks to pathfinder +# Courseplay — Manual Combine Unloader Feature + +**Author of changes:** Antler22 +**Base branch:** FS25_Courseplay (current main) +**Purpose of document:** Summary of all code changes for review and potential integration into the main branch. + +--- + +## Overview + +This feature adds a **"Call Grain Cart"** button to manually-driven combines (i.e. combines that the player is driving themselves, not Courseplay-controlled). When activated, a nearby Courseplay-managed grain cart automatically: + +1. Approaches the combine via pathfinding +2. Positions itself under the combine's unloading pipe +3. Follows the combine while it harvests (including through gentle curves and S-bends) +4. Leaves when the player closes the pipe (2-second debounce prevents false exits) +5. Automatically re-approaches if it loses position + +The design goal is that the player only touches the button once — the grain cart handles everything until the pipe is closed. + +--- + +## Files Changed + +| File | Status | Summary | +|---|---|---| +| `scripts/ai/CpManualCombineProxy.lua` | **New** | Proxy class mimicking `AIDriveStrategyCombineCourse` interface | +| `scripts/specializations/CpAIFieldWorker.lua` | **Modified** | Call Grain Cart button toggle and proxy lifecycle | +| `scripts/specializations/CpAIWorker.lua` | **Modified** | Disable button/keybind for forage harvesters | +| `scripts/ai/strategies/AIDriveStrategyUnloadCombine.lua` | **Modified** | Core unloader steering, off-track recovery, proximity fixes | +| `scripts/ai/PurePursuitController.lua` | **Modified** | Soft-recovery hook before hard CP shutdown | +| `scripts/pathfinder/PathfinderUtil.lua` | **Modified** | Defensive windrow filtering in `hasFruit()` | +| `config/VehicleSettingsSetup.xml` | **Modified** | Lower minimum for "Call Unloader At" setting | + +--- + +## 1. New File: `scripts/ai/CpManualCombineProxy.lua` + +A new class that implements the full `AIDriveStrategyCombineCourse` interface for manually-driven combines. This allows `AIDriveStrategyUnloadCombine` to interact with a manual combine using the same method calls it uses for CP-driven combines, without any nil checks or special-casing scattered through the unloader code. + +### Key design decisions + +**`isManualProxy() → true`** +A marker method so the unloader strategy can identify a manual proxy without re-querying the vehicle. Used to gate manual-only behavior throughout `AIDriveStrategyUnloadCombine`. + +**`getFillLevelPercentage() → 1`** +Always reports 100% full. The farmer is in full control — the grain cart must never leave because of a low fill level. The only valid exit condition is `isUnloadFinished()`. + +**`isUnloadFinished()`** +Requires the discharge to be continuously off for **2 seconds** before returning true. This prevents a momentary swerve or brief pipe misalignment from prematurely ending the session. Once it returns true, the grain cart departs and the proxy re-summons it if the button is still active. + +**`willWaitForUnloadToFinish()`** +3-second debounce before reporting the combine as "stopped." A GPS micro-correction or terrain hitch lasting less than 3 seconds is ignored. Without this, a brief stop would flip the grain cart from `UNLOADING_MOVING_COMBINE` to `UNLOADING_STOPPED_COMBINE`, triggering unnecessary state cycling. + +**`registerUnloader(driver)` / `deregisterUnloader()`** +Uses a `CpTemporaryObject` with a 1-second TTL. The grain cart calls `registerUnloader` every frame while it has an active combine. When the cart releases (soft recovery or natural exit), the TTL expires and the proxy's `callUnloaderWhenNeeded()` can re-summon within ~2.5 seconds. + +**`callUnloaderWhenNeeded()`** +Runs every 1500 ms. If no unloader is registered, searches active CP unloader vehicles via `AIDriveStrategyUnloadCombine.isActiveCpCombineUnloader()`, scores by fill level and distance, and calls `strategy:call(self.vehicle, nil)` on the best candidate. This is the mechanism that auto-resumes after soft recovery. + +**`getFruitAtSides() → nil, nil`** +Forage harvesters crash `calculateAutoAimPipeOffsetX()` if this returns nil before `checkFruit()` has run. Returning `nil, nil` is safe for standard combines and avoids the crash for any chopper that might somehow reach this code path. + +**`isTurning() → false`** +The unloader has special "wait during combine turn" logic that is inappropriate when the farmer is manually steering. Always returning false keeps the grain cart tracking the pipe rather than holding position at the headland. + +--- + +## 2. Modified: `scripts/specializations/CpAIFieldWorker.lua` + +### What was added + +- **`cpIsCallGrainCartActive()`** — Returns `true` when `spec.cpManualCombineProxy ~= nil`. +- **`cpToggleCallGrainCart()`** — Creates a new `CpManualCombineProxy` (activate) or deletes the existing one (deactivate). Guards against activation when CP is already active on the combine. +- **`cpGetManualCombineProxy()`** — Returns the current proxy instance. +- **`onUpdate(dt)`** — Drives the proxy's update loop each tick (course refresh + unloader call cycle). Auto-deactivates the proxy if `getIsCpActive()` becomes true on the combine. + +--- + +## 3. Modified: `scripts/specializations/CpAIWorker.lua` + +### "Call Grain Cart" button disabled for forage harvesters + +Forage harvesters (choppers) have an auto-aiming spout and behave very differently from grain combines. Calling a grain cart unloader on a chopper causes errors. The button and keybind are now hidden when the vehicle has `pipeSpec.numAutoAimingStates > 0`. + +```lua +local isChopper = pipeSpec and (pipeSpec.numAutoAimingStates or 0) > 0 +local showCallGrainCart = hasPipe and not isCpActive and not isChopper +``` + +--- + +## 4. Modified: `scripts/ai/strategies/AIDriveStrategyUnloadCombine.lua` + +This is the most significant set of changes. All modifications are backward-compatible with CP-driven combines — manual-only code is gated by `combineStrategy:isManualProxy()`. + +### 4a. `setAIVehicle()` — Extended PPC grace period + +```lua +if self.ppc then + self.ppc.offTrackGracePeriodMs = 20000 +end +``` + +Unloaders track moving targets (combine position changes, rendezvous shifts, post-turn realignment). The default 10-second off-track grace period is too short for this use case. Extended to 20 seconds to give the pathfinder and steering more time to recover before any shutdown is considered. + +### 4b. `onOffTrackShutdown()` — New soft-recovery method + +```lua +function AIDriveStrategyUnloadCombine:onOffTrackShutdown() + self:info('Soft recovery to IDLE instead of stopping CP.') + self:startWaitingForSomethingToDo() + return true -- handled; PPC must NOT call stopCurrentAIJob +end +``` + +Called by the PPC (see section 5) when the off-track grace period expires. Transitions the grain cart to `IDLE` and releases the combine. The proxy's `callUnloaderWhenNeeded()` re-summons within ~2.5 seconds. The user never has to walk over to the grain cart to restart it manually. + +This is safe for both manual and CP-driven combines: CP combines call `unloader:call()` again when they need service; the proxy does the same via its update loop. + +### 4c. `driveBesideCombine()` — Direct steering goal for manual combines + +**The problem with course-based steering for manual combines:** +`AIDriveStrategyUnloadCombine` normally steers by following a copy of the combine's fieldwork course offset to the pipe side. Manual combines have no fieldwork course — only a static placeholder is available. As the combine curves or S-bends, the grain cart drifts far from the stale placeholder, and the PPC cannot correct. + +**The solution — live goal point from the pipe reference node:** + +```lua +local isManual = strategy.isManualProxy and strategy:isManualProxy() +if dz > 5 or isManual then + _, _, dz = localToLocal(self.vehicle:getAIDirectionNode(), + self:getPipeOffsetReferenceNode(), 0, 0, 0) + local lookahead = isManual + and (self.ppc.normalLookAheadDistance or 6) + or self.ppc:getLookaheadDistance() + gx, gy, gz = localToWorld(self:getPipeOffsetReferenceNode(), + self:getPipeOffset(self.combineToUnload), 0, dz + lookahead) +end +``` + +For manual combines, a goal point is computed **every frame** regardless of `dz`. The goal is always `normalLookAheadDistance` meters ahead of the cart's current longitudinal position **in the combine's own local frame**, at the pipe's lateral offset. As the combine turns, `getPipeOffsetReferenceNode()` rotates with it — so the goal point rotates too, and the cart naturally follows curves. + +**Why `normalLookAheadDistance` instead of `getLookaheadDistance()`:** +`getLookaheadDistance()` inflates the lookahead up to 2× the base value when cross-track error is large (which it always is, since the cart is far from the stale placeholder course). A 12 m lookahead makes the cart too slow to respond to gentle heading changes. `normalLookAheadDistance` (≈5–6 m) is constant and un-inflated, enabling tight S-curve tracking. + +**CP-driven combines:** zero behavior change. The `isManual` branch is not taken, and the existing `dz > 5` gate applies as before. + +### 4d. `unloadMovingCombine()` — Off-track suppression during manual unloading + +Because the placeholder course is intentionally stale (steering is derived from the live pipe reference node, not the course), the grain cart WILL drift far from the placeholder during curves. Without suppression, the PPC's off-track detection would fire. This is suppressed per-tick: + +```lua +if combineStrategy and combineStrategy.isManualProxy and combineStrategy:isManualProxy() then + if self.ppc and self.ppc.disableStopWhenOffTrack then + self.ppc:disableStopWhenOffTrack(5000) + end +end +``` + +The 5000 ms TTL (much longer than any realistic frame interval) ensures there is no gap between ticks where the check can briefly re-enable. + +### 4e. `startCourseFollowingCombine()` — Placeholder course for manual combines + +The PPC requires a course object to be initialised. For manual combines, a placeholder is built from the combine's current position in its current heading direction (100 m straight forward). This course is **never used for steering** — `driveBesideCombine()` returns the live goal point every frame, overriding the PPC's course-based calculation. + +```lua +local combineX, _, combineZ = getWorldTranslation(self.combineToUnload:getAIDirectionNode()) +local forwardX, _, forwardZ = localToWorld(self.combineToUnload:getAIDirectionNode(), 0, 0, 100) +local placeholder = Course.createFromTwoWorldPositions( + self.vehicle, combineX, combineZ, forwardX, forwardZ, + 0, 0, 0, 10, false) +self.followCourse = placeholder +self.followCourse:setOffset(self.followingCourseOffset, 0) +startIx = 1 +``` + +No periodic refresh of this course is needed or performed. + +### 4f. `ignoreProximityObject()` — Terrain hits during approach and unloading + +Windrows and straw swaths are height-map physics objects. The proximity sensor's raycasts hit them as `hitTerrain = true`, slowing the grain cart to a crawl mid-approach. Terrain hits are now ignored in the relevant states: + +```lua +function AIDriveStrategyUnloadCombine:ignoreProximityObject(object, vehicle, moveForwards, hitTerrain) + return (self.state == self.states.UNLOADING_ON_THE_FIELD and hitTerrain) or + (self.state == self.states.DRIVING_TO_COMBINE and hitTerrain) or + (self.state == self.states.UNLOADING_MOVING_COMBINE and hitTerrain) or + (self.state == self.states.UNLOADING_MOVING_COMBINE and vehicle == self.combineToUnload) or + (self.state == self.states.HANDLE_CHOPPER_HEADLAND_TURN and vehicle == self.combineToUnload) +end +``` + +### 4g. `calculateAutoAimPipeOffsetX()` — Nil guard for forage harvesters + +`getFruitAtSides()` can return `nil` before `checkFruit()` has run (e.g., when a chopper just started). This caused a Lua arithmetic error on line 1022: + +```lua +local fruitLeft, fruitRight = strategy:getFruitAtSides() +fruitLeft = fruitLeft or 0 +fruitRight = fruitRight or 0 +``` + +### 4h. `driveToCombine()` — Smarter approach redirect + +The original approach redirect fired every 10 seconds unconditionally, causing the grain cart to swerve even on a stable straight approach. The redirect now only fires when the combine's pipe reference node has moved **>15 m** from the last redirect target: + +```lua +local pipeMoved = MathUtil.vector2Length(cX - lastX, cZ - lastZ) +if combineAhead and angleToCombieDeg < 40 and pipeMoved > 15 then + -- redirect +end +``` + +Redirect tracking (`lastApproachRedirectX/Z`, `lastApproachRedirectTime`) is reset after each successful pathfinding completion so the next approach starts fresh. + +### 4i. Fill level exit condition — discharge check + +```lua +if fillPct <= 0.1 and not isDischarging and not combineStrategy:alwaysNeedsUnloader() then +``` + +Previously, the `fillPct <= 0.1` check could fire at exactly 10% fill while the pipe was still open, causing a tarp-open/close cycle. The `not isDischarging` guard ensures the cart only leaves after the pipe has actually closed. (For manual combines this is moot since `getFillLevelPercentage()` always returns 1, but the fix is correct for all combines.) + +--- + +## 5. Modified: `scripts/ai/PurePursuitController.lua` + +### Soft-recovery hook before hard shutdown + +Before calling `vehicle:stopCurrentAIJob(AIMessageCpError.new())`, the PPC now checks whether the current drive strategy implements `onOffTrackShutdown()`: + +```lua +if (now - self.offTrackShutdownSince) >= offTrackGracePeriodMs then + local strategy = self.vehicle.getCpDriveStrategy and self.vehicle:getCpDriveStrategy() + if strategy and strategy.onOffTrackShutdown then + local handled = strategy:onOffTrackShutdown() + if handled then + CpUtil.infoVehicle(self.vehicle, + 'vehicle off track, strategy performed soft recovery instead of shutdown.') + self.offTrackShutdownSince = nil + break + end + end + CpUtil.infoVehicle(self.vehicle, 'vehicle off track, shutting off Courseplay now.') + self.vehicle:stopCurrentAIJob(AIMessageCpError.new()) + return +end +``` + +If the strategy returns `true` from `onOffTrackShutdown()`, the PPC resets its shutdown timer and continues running. This hook is intentionally generic — any strategy can implement it to provide graceful degradation instead of a hard stop. + +--- + +## 6. Modified: `scripts/pathfinder/PathfinderUtil.lua` + +### Defensive windrow filtering in `hasFruit()` + +Added name-based checks to skip windrow, swath, straw and chaff fill types in the fruit detection loop: + +```lua +local name = string.lower(fruitType.name or '') +if string.find(name, 'windrow') or string.find(name, 'swath') + or name == 'straw' or name == 'chaff' then + ignoreThis = true +end +``` + +**Note:** Windrows are height-map physics objects, not fruit density map objects, so `getFruitArea()` will not detect them regardless of this filter. The filter is defensive coding only and has no functional effect on current game versions. It is safe to remove if the maintainer prefers. + +--- + +## 7. Modified: `config/VehicleSettingsSetup.xml` + +### `callUnloaderPercent` minimum lowered to 20% + +```xml + + + + + +``` + +The minimum threshold for "Call Unloader at" is reduced from 60% to 20%. High-yield crops (e.g., corn) fill combines faster; a lower call threshold lets CP-driven combines summon unloaders earlier, keeping combines harvesting continuously without stopping to wait for an unloader. + +--- + +## Interaction with CP-Driven Combines + +All changes are backward-compatible. The two-combine scenario (one manual + one CP-driven, served by one or two grain carts) works correctly: + +- Manual-specific code is gated by `combineStrategy:isManualProxy()`. +- Shared changes (proximity terrain ignore, PPC soft-recovery hook, off-track grace period extension) benefit CP-driven unloaders as well. +- The `callUnloaderPercent` slider change applies to all combines regardless of control mode. + +--- + +## Known Limitations / Future Work + +- The grain cart can follow gentle curves and S-bends but will lose tracking on very sharp turns (>~60°). On such turns the soft-recovery mechanism kicks in and the cart re-approaches after a few seconds. +- Forage harvesters are explicitly unsupported (button hidden). Their auto-aim spout geometry would require a separate implementation similar to `unloadMovingChopper()`. +- The feature requires the grain cart to be running Courseplay as a Combine Unloader job. It does not integrate with AutoDrive or Giants helper unloaders. From a245d54647ad2ff0615fde76cf47528a8a4cfd6a Mon Sep 17 00:00:00 2001 From: Antler 22 Date: Fri, 24 Apr 2026 10:10:07 -0500 Subject: [PATCH 4/4] Address PR review: rename GrainCart->ManualUnloader, clean up PPC diff, remove unnecessary guards MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Rename cpIsCallGrainCartActive -> cpIsManualCombineCallingUnloader per pvaiko's naming suggestion - Rename cpToggleCallGrainCart -> cpToggleManualUnloader for consistency - Rename CallGrainCartEvent -> CpManualUnloaderEvent (class + file) - Update translation key CP_callGrainCart -> CP_callManualUnloader; visible text now reads 'Call Unloader' - Rebase PurePursuitController.lua on current upstream base so PR diff shows only our actual additions (~35 lines) instead of the full file - Remove unnecessary if CollisionAvoidanceController / if ProximityController guards in AIDriveStrategyUnloadCombine.setAIVehicle() — these classes are always loaded - Revert MotorController.lua to upstream (changes were unrelated to this feature and crept in by mistake) Co-Authored-By: Claude Sonnet 4-6 --- README.md | 302 ++++ modDesc.xml | 2 +- scripts/ai/PurePursuitController.lua | 1355 ++++++++--------- scripts/ai/controllers/MotorController.lua | 14 +- .../AIDriveStrategyCombineCourse.lua | 4 +- .../AIDriveStrategyUnloadCombine.lua | 30 +- scripts/events/CallGrainCartEvent.lua | 42 - scripts/events/CpManualUnloaderEvent.lua | 42 + scripts/gui/hud/CpFieldworkHudPage.lua | 38 +- scripts/specializations/CpAIFieldWorker.lua | 14 +- scripts/specializations/CpAIWorker.lua | 16 +- translations/translation_en.xml | 6 +- 12 files changed, 1069 insertions(+), 796 deletions(-) delete mode 100644 scripts/events/CallGrainCartEvent.lua create mode 100644 scripts/events/CpManualUnloaderEvent.lua diff --git a/README.md b/README.md index 6d504352..db3bbcfe 100644 --- a/README.md +++ b/README.md @@ -2,3 +2,305 @@ FORKED from Original Courseplay Antler22's edit to allow manual combine unloading, US units, and some small tweaks to pathfinder +# Courseplay — Manual Combine Unloader Feature + +**Author of changes:** Antler22 +**Base branch:** FS25_Courseplay (current main) +**Purpose of document:** Summary of all code changes for review and potential integration into the main branch. + +--- + +## Overview + +This feature adds a **"Call Grain Cart"** button to manually-driven combines (i.e. combines that the player is driving themselves, not Courseplay-controlled). When activated, a nearby Courseplay-managed grain cart automatically: + +1. Approaches the combine via pathfinding +2. Positions itself under the combine's unloading pipe +3. Follows the combine while it harvests (including through gentle curves and S-bends) +4. Leaves when the player closes the pipe (2-second debounce prevents false exits) +5. Automatically re-approaches if it loses position + +The design goal is that the player only touches the button once — the grain cart handles everything until the pipe is closed. + +--- + +## Files Changed + +| File | Status | Summary | +|---|---|---| +| `scripts/ai/CpManualCombineProxy.lua` | **New** | Proxy class mimicking `AIDriveStrategyCombineCourse` interface | +| `scripts/specializations/CpAIFieldWorker.lua` | **Modified** | Call Grain Cart button toggle and proxy lifecycle | +| `scripts/specializations/CpAIWorker.lua` | **Modified** | Disable button/keybind for forage harvesters | +| `scripts/ai/strategies/AIDriveStrategyUnloadCombine.lua` | **Modified** | Core unloader steering, off-track recovery, proximity fixes | +| `scripts/ai/PurePursuitController.lua` | **Modified** | Soft-recovery hook before hard CP shutdown | +| `scripts/pathfinder/PathfinderUtil.lua` | **Modified** | Defensive windrow filtering in `hasFruit()` | +| `config/VehicleSettingsSetup.xml` | **Modified** | Lower minimum for "Call Unloader At" setting | + +--- + +## 1. New File: `scripts/ai/CpManualCombineProxy.lua` + +A new class that implements the full `AIDriveStrategyCombineCourse` interface for manually-driven combines. This allows `AIDriveStrategyUnloadCombine` to interact with a manual combine using the same method calls it uses for CP-driven combines, without any nil checks or special-casing scattered through the unloader code. + +### Key design decisions + +**`isManualProxy() → true`** +A marker method so the unloader strategy can identify a manual proxy without re-querying the vehicle. Used to gate manual-only behavior throughout `AIDriveStrategyUnloadCombine`. + +**`getFillLevelPercentage() → 1`** +Always reports 100% full. The farmer is in full control — the grain cart must never leave because of a low fill level. The only valid exit condition is `isUnloadFinished()`. + +**`isUnloadFinished()`** +Requires the discharge to be continuously off for **2 seconds** before returning true. This prevents a momentary swerve or brief pipe misalignment from prematurely ending the session. Once it returns true, the grain cart departs and the proxy re-summons it if the button is still active. + +**`willWaitForUnloadToFinish()`** +3-second debounce before reporting the combine as "stopped." A GPS micro-correction or terrain hitch lasting less than 3 seconds is ignored. Without this, a brief stop would flip the grain cart from `UNLOADING_MOVING_COMBINE` to `UNLOADING_STOPPED_COMBINE`, triggering unnecessary state cycling. + +**`registerUnloader(driver)` / `deregisterUnloader()`** +Uses a `CpTemporaryObject` with a 1-second TTL. The grain cart calls `registerUnloader` every frame while it has an active combine. When the cart releases (soft recovery or natural exit), the TTL expires and the proxy's `callUnloaderWhenNeeded()` can re-summon within ~2.5 seconds. + +**`callUnloaderWhenNeeded()`** +Runs every 1500 ms. If no unloader is registered, searches active CP unloader vehicles via `AIDriveStrategyUnloadCombine.isActiveCpCombineUnloader()`, scores by fill level and distance, and calls `strategy:call(self.vehicle, nil)` on the best candidate. This is the mechanism that auto-resumes after soft recovery. + +**`getFruitAtSides() → nil, nil`** +Forage harvesters crash `calculateAutoAimPipeOffsetX()` if this returns nil before `checkFruit()` has run. Returning `nil, nil` is safe for standard combines and avoids the crash for any chopper that might somehow reach this code path. + +**`isTurning() → false`** +The unloader has special "wait during combine turn" logic that is inappropriate when the farmer is manually steering. Always returning false keeps the grain cart tracking the pipe rather than holding position at the headland. + +--- + +## 2. Modified: `scripts/specializations/CpAIFieldWorker.lua` + +### What was added + +- **`cpIsCallGrainCartActive()`** — Returns `true` when `spec.cpManualCombineProxy ~= nil`. +- **`cpToggleCallGrainCart()`** — Creates a new `CpManualCombineProxy` (activate) or deletes the existing one (deactivate). Guards against activation when CP is already active on the combine. +- **`cpGetManualCombineProxy()`** — Returns the current proxy instance. +- **`onUpdate(dt)`** — Drives the proxy's update loop each tick (course refresh + unloader call cycle). Auto-deactivates the proxy if `getIsCpActive()` becomes true on the combine. + +--- + +## 3. Modified: `scripts/specializations/CpAIWorker.lua` + +### "Call Grain Cart" button disabled for forage harvesters + +Forage harvesters (choppers) have an auto-aiming spout and behave very differently from grain combines. Calling a grain cart unloader on a chopper causes errors. The button and keybind are now hidden when the vehicle has `pipeSpec.numAutoAimingStates > 0`. + +```lua +local isChopper = pipeSpec and (pipeSpec.numAutoAimingStates or 0) > 0 +local showCallGrainCart = hasPipe and not isCpActive and not isChopper +``` + +--- + +## 4. Modified: `scripts/ai/strategies/AIDriveStrategyUnloadCombine.lua` + +This is the most significant set of changes. All modifications are backward-compatible with CP-driven combines — manual-only code is gated by `combineStrategy:isManualProxy()`. + +### 4a. `setAIVehicle()` — Extended PPC grace period + +```lua +if self.ppc then + self.ppc.offTrackGracePeriodMs = 20000 +end +``` + +Unloaders track moving targets (combine position changes, rendezvous shifts, post-turn realignment). The default 10-second off-track grace period is too short for this use case. Extended to 20 seconds to give the pathfinder and steering more time to recover before any shutdown is considered. + +### 4b. `onOffTrackShutdown()` — New soft-recovery method + +```lua +function AIDriveStrategyUnloadCombine:onOffTrackShutdown() + self:info('Soft recovery to IDLE instead of stopping CP.') + self:startWaitingForSomethingToDo() + return true -- handled; PPC must NOT call stopCurrentAIJob +end +``` + +Called by the PPC (see section 5) when the off-track grace period expires. Transitions the grain cart to `IDLE` and releases the combine. The proxy's `callUnloaderWhenNeeded()` re-summons within ~2.5 seconds. The user never has to walk over to the grain cart to restart it manually. + +This is safe for both manual and CP-driven combines: CP combines call `unloader:call()` again when they need service; the proxy does the same via its update loop. + +### 4c. `driveBesideCombine()` — Direct steering goal for manual combines + +**The problem with course-based steering for manual combines:** +`AIDriveStrategyUnloadCombine` normally steers by following a copy of the combine's fieldwork course offset to the pipe side. Manual combines have no fieldwork course — only a static placeholder is available. As the combine curves or S-bends, the grain cart drifts far from the stale placeholder, and the PPC cannot correct. + +**The solution — live goal point from the pipe reference node:** + +```lua +local isManual = strategy.isManualProxy and strategy:isManualProxy() +if dz > 5 or isManual then + _, _, dz = localToLocal(self.vehicle:getAIDirectionNode(), + self:getPipeOffsetReferenceNode(), 0, 0, 0) + local lookahead = isManual + and (self.ppc.normalLookAheadDistance or 6) + or self.ppc:getLookaheadDistance() + gx, gy, gz = localToWorld(self:getPipeOffsetReferenceNode(), + self:getPipeOffset(self.combineToUnload), 0, dz + lookahead) +end +``` + +For manual combines, a goal point is computed **every frame** regardless of `dz`. The goal is always `normalLookAheadDistance` meters ahead of the cart's current longitudinal position **in the combine's own local frame**, at the pipe's lateral offset. As the combine turns, `getPipeOffsetReferenceNode()` rotates with it — so the goal point rotates too, and the cart naturally follows curves. + +**Why `normalLookAheadDistance` instead of `getLookaheadDistance()`:** +`getLookaheadDistance()` inflates the lookahead up to 2× the base value when cross-track error is large (which it always is, since the cart is far from the stale placeholder course). A 12 m lookahead makes the cart too slow to respond to gentle heading changes. `normalLookAheadDistance` (≈5–6 m) is constant and un-inflated, enabling tight S-curve tracking. + +**CP-driven combines:** zero behavior change. The `isManual` branch is not taken, and the existing `dz > 5` gate applies as before. + +### 4d. `unloadMovingCombine()` — Off-track suppression during manual unloading + +Because the placeholder course is intentionally stale (steering is derived from the live pipe reference node, not the course), the grain cart WILL drift far from the placeholder during curves. Without suppression, the PPC's off-track detection would fire. This is suppressed per-tick: + +```lua +if combineStrategy and combineStrategy.isManualProxy and combineStrategy:isManualProxy() then + if self.ppc and self.ppc.disableStopWhenOffTrack then + self.ppc:disableStopWhenOffTrack(5000) + end +end +``` + +The 5000 ms TTL (much longer than any realistic frame interval) ensures there is no gap between ticks where the check can briefly re-enable. + +### 4e. `startCourseFollowingCombine()` — Placeholder course for manual combines + +The PPC requires a course object to be initialised. For manual combines, a placeholder is built from the combine's current position in its current heading direction (100 m straight forward). This course is **never used for steering** — `driveBesideCombine()` returns the live goal point every frame, overriding the PPC's course-based calculation. + +```lua +local combineX, _, combineZ = getWorldTranslation(self.combineToUnload:getAIDirectionNode()) +local forwardX, _, forwardZ = localToWorld(self.combineToUnload:getAIDirectionNode(), 0, 0, 100) +local placeholder = Course.createFromTwoWorldPositions( + self.vehicle, combineX, combineZ, forwardX, forwardZ, + 0, 0, 0, 10, false) +self.followCourse = placeholder +self.followCourse:setOffset(self.followingCourseOffset, 0) +startIx = 1 +``` + +No periodic refresh of this course is needed or performed. + +### 4f. `ignoreProximityObject()` — Terrain hits during approach and unloading + +Windrows and straw swaths are height-map physics objects. The proximity sensor's raycasts hit them as `hitTerrain = true`, slowing the grain cart to a crawl mid-approach. Terrain hits are now ignored in the relevant states: + +```lua +function AIDriveStrategyUnloadCombine:ignoreProximityObject(object, vehicle, moveForwards, hitTerrain) + return (self.state == self.states.UNLOADING_ON_THE_FIELD and hitTerrain) or + (self.state == self.states.DRIVING_TO_COMBINE and hitTerrain) or + (self.state == self.states.UNLOADING_MOVING_COMBINE and hitTerrain) or + (self.state == self.states.UNLOADING_MOVING_COMBINE and vehicle == self.combineToUnload) or + (self.state == self.states.HANDLE_CHOPPER_HEADLAND_TURN and vehicle == self.combineToUnload) +end +``` + +### 4g. `calculateAutoAimPipeOffsetX()` — Nil guard for forage harvesters + +`getFruitAtSides()` can return `nil` before `checkFruit()` has run (e.g., when a chopper just started). This caused a Lua arithmetic error on line 1022: + +```lua +local fruitLeft, fruitRight = strategy:getFruitAtSides() +fruitLeft = fruitLeft or 0 +fruitRight = fruitRight or 0 +``` + +### 4h. `driveToCombine()` — Smarter approach redirect + +The original approach redirect fired every 10 seconds unconditionally, causing the grain cart to swerve even on a stable straight approach. The redirect now only fires when the combine's pipe reference node has moved **>15 m** from the last redirect target: + +```lua +local pipeMoved = MathUtil.vector2Length(cX - lastX, cZ - lastZ) +if combineAhead and angleToCombieDeg < 40 and pipeMoved > 15 then + -- redirect +end +``` + +Redirect tracking (`lastApproachRedirectX/Z`, `lastApproachRedirectTime`) is reset after each successful pathfinding completion so the next approach starts fresh. + +### 4i. Fill level exit condition — discharge check + +```lua +if fillPct <= 0.1 and not isDischarging and not combineStrategy:alwaysNeedsUnloader() then +``` + +Previously, the `fillPct <= 0.1` check could fire at exactly 10% fill while the pipe was still open, causing a tarp-open/close cycle. The `not isDischarging` guard ensures the cart only leaves after the pipe has actually closed. (For manual combines this is moot since `getFillLevelPercentage()` always returns 1, but the fix is correct for all combines.) + +--- + +## 5. Modified: `scripts/ai/PurePursuitController.lua` + +### Soft-recovery hook before hard shutdown + +Before calling `vehicle:stopCurrentAIJob(AIMessageCpError.new())`, the PPC now checks whether the current drive strategy implements `onOffTrackShutdown()`: + +```lua +if (now - self.offTrackShutdownSince) >= offTrackGracePeriodMs then + local strategy = self.vehicle.getCpDriveStrategy and self.vehicle:getCpDriveStrategy() + if strategy and strategy.onOffTrackShutdown then + local handled = strategy:onOffTrackShutdown() + if handled then + CpUtil.infoVehicle(self.vehicle, + 'vehicle off track, strategy performed soft recovery instead of shutdown.') + self.offTrackShutdownSince = nil + break + end + end + CpUtil.infoVehicle(self.vehicle, 'vehicle off track, shutting off Courseplay now.') + self.vehicle:stopCurrentAIJob(AIMessageCpError.new()) + return +end +``` + +If the strategy returns `true` from `onOffTrackShutdown()`, the PPC resets its shutdown timer and continues running. This hook is intentionally generic — any strategy can implement it to provide graceful degradation instead of a hard stop. + +--- + +## 6. Modified: `scripts/pathfinder/PathfinderUtil.lua` + +### Defensive windrow filtering in `hasFruit()` + +Added name-based checks to skip windrow, swath, straw and chaff fill types in the fruit detection loop: + +```lua +local name = string.lower(fruitType.name or '') +if string.find(name, 'windrow') or string.find(name, 'swath') + or name == 'straw' or name == 'chaff' then + ignoreThis = true +end +``` + +**Note:** Windrows are height-map physics objects, not fruit density map objects, so `getFruitArea()` will not detect them regardless of this filter. The filter is defensive coding only and has no functional effect on current game versions. It is safe to remove if the maintainer prefers. + +--- + +## 7. Modified: `config/VehicleSettingsSetup.xml` + +### `callUnloaderPercent` minimum lowered to 20% + +```xml + + + + + +``` + +The minimum threshold for "Call Unloader at" is reduced from 60% to 20%. High-yield crops (e.g., corn) fill combines faster; a lower call threshold lets CP-driven combines summon unloaders earlier, keeping combines harvesting continuously without stopping to wait for an unloader. + +--- + +## Interaction with CP-Driven Combines + +All changes are backward-compatible. The two-combine scenario (one manual + one CP-driven, served by one or two grain carts) works correctly: + +- Manual-specific code is gated by `combineStrategy:isManualProxy()`. +- Shared changes (proximity terrain ignore, PPC soft-recovery hook, off-track grace period extension) benefit CP-driven unloaders as well. +- The `callUnloaderPercent` slider change applies to all combines regardless of control mode. + +--- + +## Known Limitations / Future Work + +- The grain cart can follow gentle curves and S-bends but will lose tracking on very sharp turns (>~60°). On such turns the soft-recovery mechanism kicks in and the cart re-approaches after a few seconds. +- Forage harvesters are explicitly unsupported (button hidden). Their auto-aim spout geometry would require a separate implementation similar to `unloadMovingChopper()`. +- The feature requires the grain cart to be running Courseplay as a Combine Unloader job. It does not integrate with AutoDrive or Giants helper unloaders. diff --git a/modDesc.xml b/modDesc.xml index 80a29fb9..01321fed 100644 --- a/modDesc.xml +++ b/modDesc.xml @@ -347,7 +347,7 @@ Changelog 8.1.0.3 - + diff --git a/scripts/ai/PurePursuitController.lua b/scripts/ai/PurePursuitController.lua index a6839583..5acbbc66 100644 --- a/scripts/ai/PurePursuitController.lua +++ b/scripts/ai/PurePursuitController.lua @@ -1,679 +1,678 @@ ---[[ -This file is part of Courseplay (https://github.com/Courseplay/courseplay) -Copyright (C) 2018-2021 Peter Vaiko - -This program is free software: you can redistribute it and/or modify -it under the terms of the GNU General Public License as published by -the Free Software Foundation, either version 3 of the License, or -(at your option) any later version. - -This program is distributed in the hope that it will be useful, -but WITHOUT ANY WARRANTY; without even the implied warranty of -MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -GNU General Public License for more details. - -You should have received a copy of the GNU General Public License -along with this program. If not, see . -]] - ---[[ - -This is a simplified implementation of a pure pursuit algorithm -to follow a two dimensional path consisting of waypoints. - -See the paper - -Steering Control of an Autonomous Ground Vehicle with Application to the DARPA -Urban Challenge By Stefan F. Campbell - -We use the terminology of that paper here, like 'relevant path segment', 'goal point', etc. and follow the -algorithm to search for the goal point as described in this paper. - -PURPOSE - -1. Provide a goal point to steer towards to. - Contrary to the old implementation, we are not steering to a waypoint, instead to a goal - point which is in a given look ahead distance from the vehicle on the path. - -2. Determine when to switch to the next waypoint (and avoid circling) - Regardless of the above, the rest of the Courseplay code still needs to know the current - waypoint as we progress along the path. - -HOW TO USE - -1. add a PPC to the vehicle with new() -2. when the vehicle starts driving, call initialize() -3. in every update cycle, call update(). This will calculate the goal point and the current waypoint -4. use the convenience functions getCurrentWaypointPosition(), reachedLastWaypoint() - in your code instead of directly checking and manipulating vehicle.Waypoints. These provide the legacy behavior when - the PPC is not active (for example due to reverse driving) or when disabled -6. you can use enable() and disable() to enable/disable the PPC. When disabled and you are using the above functions, - it'll behave as the legacy code. - -]] - ----@class PurePursuitController -PurePursuitController = CpObject() - ---- if the vehicle is more than cutOutDistanceLimit meters from the current segment's endpoints, cut-out the ---- controller to stop. Some error must have caused us to wander way off-track, unlikely to recover. -PurePursuitController.cutOutDistanceLimit = 50 ---- Only shut off after being off track for this long (ms). Reduces false shutdowns on brief excursions (turns, terrain). -PurePursuitController.offTrackGracePeriodMs = 10000 - --- constructor -function PurePursuitController:init(vehicle) - self.normalLookAheadDistance = math.min(vehicle.maxTurningRadius, 6) - self.shortLookaheadDistance = math.max(2, self.normalLookAheadDistance / 2) - -- normal lookahead distance - self.baseLookAheadDistance = self.normalLookAheadDistance - -- adapted look ahead distance - self.lookAheadDistance = self.baseLookAheadDistance - self.temporaryLookAheadDistance = CpTemporaryObject(nil) - -- when transitioning from forward to reverse, this close we have to be to the waypoint where we - -- change direction before we switch to the next waypoint - self.distToSwitchWhenChangingToReverse = 1 - self.vehicle = vehicle - self:resetControlledNode() - self.name = vehicle:getName() - -- node on the current waypoint - self.currentWpNode = WaypointNode(self.name .. '-currentWpNode', true) - -- waypoint at the start of the relevant segment - self.relevantWpNode = WaypointNode(self.name .. '-relevantWpNode', true) - -- waypoint at the end of the relevant segment - self.nextWpNode = WaypointNode(self.name .. '-nextWpNode', true) - -- the current goal node - self.goalWpNode = WaypointNode(self.name .. '-goalWpNode', false) - -- vehicle position projected on the path, not used for anything other than debug display - self.projectedPosNode = CpUtil.createNode(self.name .. '-projectedPosNode', 0, 0, 0) - -- index of the first node of the path (where PPC is initialized and starts driving - self.firstIx = 1 - self.crossTrackError = 0 - self.lastPassedWaypointIx = nil - self.waypointPassedListeners = {} - self.waypointChangeListeners = {} - -- enable/disable stopping the vehicle when it is off-track (too far away from any waypoint) - self.stopWhenOffTrack = CpTemporaryObject(true) - -- when we first entered the off-track shutdown zone (nil = not in zone or just left it) - self.offTrackShutdownSince = nil -end - --- destructor -function PurePursuitController:delete() - self.currentWpNode:destroy() - self.relevantWpNode:destroy() - self.nextWpNode:destroy() - CpUtil.destroyNode(self.projectedPosNode) - self.goalWpNode:destroy() -end - -function PurePursuitController:debug(...) - CpUtil.debugVehicle(CpDebug.DBG_PPC, self.vehicle, 'PPC: ' .. string.format(...)) -end - -function PurePursuitController:debugSparse(...) - if g_updateLoopIndex % 100 == 0 then - self:debug(...) - end -end - ----@param course Course -function PurePursuitController:setCourse(course) - self.course = course -end - -function PurePursuitController:getCourse() - return self.course -end - ---- Set an offset for the current course. -function PurePursuitController:setOffset(x, z) - self.course:setOffset(x, z) -end - -function PurePursuitController:getOffset() - return self.course:getOffset() -end - ---- Disable off-track detection temporarily, for instance while we know the vehicle must be driving ---- longer distances between two waypoints, like an unloader following a chopper through a turn, where ---- in some patterns the row end and the next row start are far apart. -function PurePursuitController:disableStopWhenOffTrack(milliseconds) - self.stopWhenOffTrack:set(false, milliseconds) -end - ---- Use a different node to track/control, for example the root node of a trailed implement --- instead of the tractor's root node. -function PurePursuitController:setControlledNode(node) - self.controlledNode = node -end - -function PurePursuitController:getControlledNode() - return self.controlledNode -end - ---- reset controlled node to the default (vehicle's own direction node) -function PurePursuitController:resetControlledNode() - self.controlledNode = self.vehicle:getAIDirectionNode() -end - --- initialize controller before driving -function PurePursuitController:initialize(ix) - self.firstIx = ix - -- relevantWpNode always points to the point where the relevant path segment starts - self.relevantWpNode:setToWaypoint(self.course, self.firstIx ) - self.nextWpNode:setToWaypoint(self.course, self.firstIx) - self.wpBeforeGoalPointIx = self.nextWpNode.ix - self.currentWpNode:setToWaypoint(self.course, self.firstIx ) - self.course:setCurrentWaypointIx(self.firstIx) - self.course:setLastPassedWaypointIx(nil) - self:debug('initialized to waypoint %d of %d', self.firstIx, self.course:getNumberOfWaypoints()) - self.lastPassedWaypointIx = nil - -- force calling the waypoint change callback right after initialization - self.sendWaypointChange = { current = self.currentWpNode.ix, prev = self.currentWpNode.ix - 1} - self.sendWaypointPassed = nil - -- current goal point search case as described in the paper, for diagnostics only - self.case = 0 -end - --- Initialize to a waypoint when reversing. --- TODO: this has to be called explicitly but could be done automatically by the vanilla initialize() --- make sure the waypoint we initialize to is close to the controlled node, otherwise the PPC will --- remain in initializing mode if the waypoint is too far back from the controlled node, and just --- reverse forever -function PurePursuitController:initializeForReversing(ix) - local reverserNode, debugText = self:getReverserNode(false) - if reverserNode then - self:debug('Reverser node %s found, initializing with it', debugText) - -- don't use ix as it is, instead, find the waypoint closest to the reverser node - local dPrev, d = math.huge, self.course:getWaypoint(ix):getDistanceFromNode(reverserNode) - while d < dPrev and self.course:isReverseAt(ix) and ix < self.course:getNumberOfWaypoints() do - dPrev = d - ix = ix + 1 - d = self.course:getWaypoint(ix):getDistanceFromNode(reverserNode) - end - else - self:debug('No reverser node found, initializing with default controlled node') - end - self:initialize(ix) -end - --- TODO: make this more generic and allow registering multiple listeners? --- could also implement listeners for events like notify me when within x meters of a waypoint, etc. -function PurePursuitController:registerListeners(waypointListener, onWaypointPassedFunc, onWaypointChangeFunc) - self.savedWaypointListener = self.waypointListener - self.savedWaypointPassedListenerFunc = self.waypointPassedListenerFunc - self.savedWaypointChangeListenerFunc = self.waypointChangeListenerFunc - self.waypointListener = waypointListener - self.waypointPassedListenerFunc = onWaypointPassedFunc - self.waypointChangeListenerFunc = onWaypointChangeFunc -end - --- Restore the listeners that were registered before the last call of registerListeners(). If there were none, --- do not restore anything -function PurePursuitController:restorePreviouslyRegisteredListeners() - if self.savedWaypointListener ~= nil then - self.waypointListener = self.savedWaypointListener - self.waypointPassedListenerFunc = self.savedWaypointPassedListenerFunc - self.waypointChangeListenerFunc = self.savedWaypointChangeListenerFunc - end -end - -function PurePursuitController:setLookaheadDistance(d) - self.baseLookAheadDistance = d -end - -function PurePursuitController:setNormalLookaheadDistance() - self.baseLookAheadDistance = self.normalLookAheadDistance -end - -function PurePursuitController:setShortLookaheadDistance() - self.baseLookAheadDistance = self.shortLookaheadDistance -end - ---- Set a short lookahead distance for ttlMs milliseconds -function PurePursuitController:setTemporaryShortLookaheadDistance(ttlMs) - self.temporaryLookAheadDistance:set(self.shortLookaheadDistance, ttlMs) -end - -function PurePursuitController:getLookaheadDistance() - return self.lookAheadDistance -end - -function PurePursuitController:setCurrentLookaheadDistance(cte) - local la = self.temporaryLookAheadDistance:get() or self.baseLookAheadDistance - self.lookAheadDistance = math.min(la + math.abs(cte), la * 2) -end - ---- get index of current waypoint (one we are driving towards) -function PurePursuitController:getCurrentWaypointIx() - return self.currentWpNode.ix -end - ---- Get the current waypoint object ----@return Waypoint -function PurePursuitController:getCurrentWaypoint() - return self.course:getWaypoint(self.currentWpNode.ix) -end - ---- get index of relevant waypoint (one we are close to) -function PurePursuitController:getRelevantWaypointIx() - return self.relevantWpNode.ix -end - -function PurePursuitController:getLastPassedWaypointIx() - return self.lastPassedWaypointIx -end - ----@return number, string node that would be used for reversing, debug text explaining what node it is -function PurePursuitController:getReverserNode(suppressLog) - if not self.reversingImplement then - self.reversingImplement = AIUtil.getFirstReversingImplementWithWheels(self.vehicle, suppressLog) - end - return AIUtil.getReverserNode(self.vehicle, self.reversingImplement, suppressLog) -end - ---- When reversing, use the towed implement's node as a reference -function PurePursuitController:switchControlledNode() - local lastControlledNode = self.controlledNode - local debugText = 'AIDirectionNode' - local reverserNode - if self:isReversing() then - reverserNode, debugText = self:getReverserNode(true) - if reverserNode then - self:setControlledNode(reverserNode) - else - self:resetControlledNode() - end - else - self:resetControlledNode() - end - if self.controlledNode ~= lastControlledNode then - self:debug('Switching controlled node to %s', debugText) - end -end - -function PurePursuitController:update() - if not self.course then - self:debugSparse('no course set.') - return - end - self:showDebugTable() - self:switchControlledNode() - self:findRelevantSegment() - self:findGoalPoint() - self.course:setCurrentWaypointIx(self.currentWpNode.ix) - self.course:setLastPassedWaypointIx(self.lastPassedWaypointIx) - self:notifyListeners() -end - -function PurePursuitController:showDebugTable() - if self.course then - if CpDebug:isChannelActive(CpDebug.DBG_COURSES, self.vehicle) then - local info = { - title = self.course:getName(), - content = self.course:getDebugTable() - } - CpDebug:drawVehicleDebugTable(self.vehicle, { info }) - end - end -end - -function PurePursuitController:notifyListeners() - if self.waypointListener then - if self.sendWaypointChange then - -- send waypoint change event for all waypoints between the previous and current to make sure - -- we don't miss any - self:debug('prev %s curr %s', self.sendWaypointChange.prev, self.sendWaypointChange.current) - for ix = self.sendWaypointChange.prev + 1, self.sendWaypointChange.current do - self.waypointListener[self.waypointChangeListenerFunc](self.waypointListener, ix, self.course) - end - end - if self.sendWaypointPassed then - self.waypointListener[self.waypointPassedListenerFunc](self.waypointListener, self.sendWaypointPassed, self.course) - end - end - self.sendWaypointChange = nil - self.sendWaypointPassed = nil -end - -function PurePursuitController:havePassedWaypoint(wpNode) - local vx, vy, vz = getWorldTranslation(self.controlledNode) - local dx, _, dz = worldToLocal(wpNode.node, vx, vy, vz); - local dFromNext = MathUtil.vector2Length(dx, dz) - --self:debug('checking %d, dz: %.1f, dFromNext: %.1f', wpNode.ix, dz, dFromNext) - local result = false - if self.course:switchingDirectionAt(wpNode.ix) then - -- switching direction at this waypoint, so this is pointing into the opposite direction. - -- we have to make sure we drive up to this waypoint close enough before we switch to the next - -- so wait until dz < 0, that is, we are behind the waypoint - if dz < 0 then - result = true - end - else - -- we are not transitioning between forward and reverse - -- we have passed the next waypoint if our dz in the waypoints coordinate system is positive, that is, - -- when looking into the direction of the waypoint, we are ahead of it. - -- Also, when on the process of aligning to the course, like for example the vehicle just started - -- driving towards the first waypoint, we have to make sure we actually get close to the waypoint - -- (as we may already be in front of it), so try get within the turn diameter * 2. - if dz >= 0 and dFromNext < self.vehicle.maxTurningRadius * 4 then - result = true - end - end - if result then - --and not self:reachedLastWaypoint() then - if not self.lastPassedWaypointIx or (self.lastPassedWaypointIx ~= wpNode.ix) then - self.lastPassedWaypointIx = wpNode.ix - self:debug('waypoint %d passed, dz: %.1f %s %s', wpNode.ix, dz, - self.course.waypoints[wpNode.ix].rev and 'reverse' or '', - self.course:switchingDirectionAt(wpNode.ix) and 'switching direction' or '') - -- notify listeners about the passed waypoint - self.sendWaypointPassed = self.lastPassedWaypointIx - end - end - return result -end - -function PurePursuitController:havePassedAnyWaypointBetween(fromIx, toIx) - local node = WaypointNode(self.name .. '-node', false) - local result, passedWaypointIx = false, 0 - -- math.max so we do one loop even if toIx < fromIx - --self:debug('checking between %d and %d', fromIx, toIx) - for ix = fromIx, math.max(toIx, fromIx) do - node:setToWaypoint(self.course, ix) - if self:havePassedWaypoint(node) then - result = true - passedWaypointIx = ix - break - end - - end - node:destroy() - return result, passedWaypointIx -end - --- Finds the relevant segment. --- Sets the vehicle's projected position on the path. -function PurePursuitController:findRelevantSegment() - -- vehicle position - local vx, vy, vz = getWorldTranslation(self.controlledNode) - -- update the position of the relevant node (in case the course offset changed) - self.relevantWpNode:setToWaypoint(self.course, self.relevantWpNode.ix) - local lx, _, dzFromRelevant = worldToLocal(self.relevantWpNode.node, vx, vy, vz); - self.crossTrackError = lx - -- adapt our lookahead distance based on the error - self:setCurrentLookaheadDistance(self.crossTrackError) - -- projected vehicle position/rotation - local px, py, pz = localToWorld(self.relevantWpNode.node, 0, 0, dzFromRelevant) - local _, yRot, _ = getRotation(self.relevantWpNode.node) - setTranslation(self.projectedPosNode, px, py, pz) - setRotation(self.projectedPosNode, 0, yRot, 0) - -- we check all waypoints between the relevant and the one before the goal point as the goal point - -- may have moved several waypoints up if there's a very sharp turn for example and in that case - -- the vehicle may never reach some of the waypoint in between. - local passed, ix - if self.course:switchingDirectionAt(self.nextWpNode.ix) then - -- don't look beyond a direction switch as we'll always be past a reversing waypoint - -- before we reach it. - passed, ix = self:havePassedWaypoint(self.nextWpNode), self.nextWpNode.ix - else - passed, ix = self:havePassedAnyWaypointBetween(self.nextWpNode.ix, self.wpBeforeGoalPointIx) - end - if passed then - self.relevantWpNode:setToWaypoint(self.course, ix, true) - self.nextWpNode:setToWaypoint(self.course, self.relevantWpNode.ix + 1, true) - if not self:reachedLastWaypoint() then - -- disable debugging once we reached the last waypoint. Otherwise we'd keep logging - -- until the user presses 'Stop driver'. - self:debug('relevant waypoint: %d, next waypoint %d, crosstrack: %.1f', - self.relevantWpNode.ix, self.nextWpNode.ix, self.crossTrackError) - end - end - if CpDebug:isChannelActive(CpDebug.DBG_PPC, self.vehicle) then - DebugUtil.drawDebugLine(px, py + 3, pz, px, py + 1, pz, 1, 1, 0); - DebugUtil.drawDebugNode(self.relevantWpNode.node, string.format('ix = %d\nrelevant\nnode', self.relevantWpNode.ix)) - DebugUtil.drawDebugNode(self.projectedPosNode, 'projected\nvehicle\nposition') - end -end - --- Now, from the relevant section forward we search for the goal point, which is the one --- lying lookAheadDistance in front of us on the path --- this is the algorithm described in Chapter 2 of the paper -function PurePursuitController:findGoalPoint() - - local vx, _, vz = getWorldTranslation(self.controlledNode) - --local vx, vy, vz = getWorldTranslation(self.projectedPosNode); - - -- create helper nodes at the relevant and the next wp. We'll move these up on the path until we reach the segment - -- in lookAheadDistance - local node1 = WaypointNode(self.name .. '-node1', false) - local node2 = WaypointNode(self.name .. '-node2', false) - - -- starting at the relevant segment walk up the path to find the segment on - -- which the goal point lies. This is the segment intersected by the circle with lookAheadDistance radius - -- around the vehicle. - local ix = self.relevantWpNode.ix - while ix <= self.course:getNumberOfWaypoints() do - node1:setToWaypoint(self.course, ix) - node2:setToWaypointOrBeyond(self.course, ix + 1, self.lookAheadDistance) - local x1, _, z1 = getWorldTranslation(node1.node) - local x2, _, z2 = getWorldTranslation(node2.node) - -- distance between the vehicle position and the ends of the segment - local q1 = MathUtil.getPointPointDistance(x1, z1, vx, vz) -- distance from node 1 - local q2 = MathUtil.getPointPointDistance(x2, z2, vx, vz) -- distance from node 2 - local l = MathUtil.getPointPointDistance(x1, z1, x2, z2) -- length of path segment (distance between node 1 and 2 - -- self:debug('ix=%d, q1=%.1f, q2=%.1f la=%.1f l=%.1f', ix, q1, q2, self.lookAheadDistance, l) - - -- case i (first node outside virtual circle but not yet reached) or (not the first node but we are way off the track) - if (ix == self.firstIx and ix ~= self.lastPassedWaypointIx) and - q1 >= self.lookAheadDistance and q2 >= self.lookAheadDistance then - self.offTrackShutdownSince = nil - -- If we weren't on track yet (after initialization, on our way to the first/initialized waypoint) - -- set the goal to the relevant WP - self.goalWpNode:setToWaypoint(self.course, self.relevantWpNode.ix) - self:setGoalTranslation() - self:showGoalpointDiag(1, 'initializing, ix=%d, q1=%.1f, q2=%.1f, la=%.1f', ix, q1, q2, self.lookAheadDistance) - -- and also the current waypoint is now at the relevant WP - self:setCurrentWaypoint(self.relevantWpNode.ix) - break - end - - -- case ii (common case) - if q1 <= self.lookAheadDistance and q2 >= self.lookAheadDistance then - self.offTrackShutdownSince = nil - -- in some weird cases q1 may be 0 (when we calculate a course based on the vehicle position) so fix that - -- to avoid a nan - if q1 < 0.0001 then - q1 = 0.1 - end - local cosGamma = (q2 * q2 - q1 * q1 - l * l) / (-2 * l * q1) - local p = q1 * cosGamma + math.sqrt(q1 * q1 * (cosGamma * cosGamma - 1) + self.lookAheadDistance * self.lookAheadDistance) - local gx, _, gz = localToWorld(node1.node, 0, 0, p) - self:setGoalTranslation(gx, gz) - self.wpBeforeGoalPointIx = ix - self:showGoalpointDiag(2, 'common case, ix=%d, q1=%.1f, q2=%.1f la=%.1f', ix, q1, q2, self.lookAheadDistance) - -- current waypoint is the waypoint at the end of the path segment - self:setCurrentWaypoint(ix + 1) - --CpUtil.debugVehicle(CpDebug.DBG_PPC, self.vehicle, "PPC: %d, p=%.1f", self.currentWpNode.ix, p) - break - end - - -- cases iii, iv and v - -- these two may have a problem and actually prevent the vehicle go back to the waypoint - -- when wandering way off track, therefore we try to catch this case in case i - if ix == self.relevantWpNode.ix and q1 >= self.lookAheadDistance and q2 >= self.lookAheadDistance then - if math.abs(self.crossTrackError) <= self.lookAheadDistance then - -- case iii (two intersection points) - self.offTrackShutdownSince = nil - local p = math.sqrt(self.lookAheadDistance * self.lookAheadDistance - self.crossTrackError * self.crossTrackError) - local gx, _, gz = localToWorld(self.projectedPosNode, 0, 0, p) - self:setGoalTranslation(gx, gz) - self.wpBeforeGoalPointIx = ix - self:showGoalpointDiag(3, 'two intersection points, ix=%d, q1=%.1f, q2=%.1f, la=%.1f, cte=%.1f', ix, q1, q2, - self.lookAheadDistance, self.crossTrackError) - -- current waypoint is the waypoint at the end of the path segment - self:setCurrentWaypoint(ix + 1) - else - -- case iv (no intersection points) - -- case v ( goal point dead zone) - -- set the goal to the projected position - local gx, _, gz = localToWorld(self.projectedPosNode, 0, 0, 0) - self:setGoalTranslation(gx, gz) - self.wpBeforeGoalPointIx = ix - self:showGoalpointDiag(4, 'no intersection points, ix=%d, q1=%.1f, q2=%.1f, la=%.1f, cte=%.1f', ix, q1, q2, - self.lookAheadDistance, self.crossTrackError) - -- current waypoint is the waypoint at the end of the path segment - self:setCurrentWaypoint(ix + 1) - end - -- Only shut off after being off track beyond cutOutDistanceLimit for a grace period (reduces false shutdowns) - if (q1 > self.cutOutDistanceLimit) and (q2 > self.cutOutDistanceLimit) and self.stopWhenOffTrack:get() then - local now = g_currentMission and g_currentMission.time or 0 - if self.offTrackShutdownSince == nil then - self.offTrackShutdownSince = now - end - if (now - self.offTrackShutdownSince) >= (self.offTrackGracePeriodMs or PurePursuitController.offTrackGracePeriodMs or 10000) then - -- Give the current drive strategy a chance to recover softly instead of - -- stopping the CP helper entirely (user has to jump to that vehicle to - -- restart). If the strategy implements onOffTrackShutdown() and returns - -- true it has handled recovery and we must NOT call stopCurrentAIJob. - local strategy = self.vehicle.getCpDriveStrategy and self.vehicle:getCpDriveStrategy() - if strategy and strategy.onOffTrackShutdown then - local handled = strategy:onOffTrackShutdown() - if handled then - CpUtil.infoVehicle(self.vehicle, - 'vehicle off track, strategy performed soft recovery instead of shutdown.') - self.offTrackShutdownSince = nil - break - end - end - CpUtil.infoVehicle(self.vehicle, 'vehicle off track, shutting off Courseplay now.') - self.vehicle:stopCurrentAIJob(AIMessageCpError.new()) - return - end - else - self.offTrackShutdownSince = nil - end - break - end - -- none of the above, continue search with the next path segment - ix = ix + 1 - -- unless there's a direction change here. This should only happen right after initialization and when - -- the reference node is already beyond the direction switch waypoint. We should not skip that being - -- the current waypoint otherwise the relevant waypoint won't be moved over the direction switch - if self.course:switchingDirectionAt(ix) then - self.offTrackShutdownSince = nil - -- force waypoint change - self:showGoalpointDiag(100, 'switching direction while looking for goal point, ix=%d', ix) - self.wpBeforeGoalPointIx = ix - 1 - self:setCurrentWaypoint(ix) - break - end - end - - node1:destroy() - node2:destroy() - - if CpDebug:isChannelActive(CpDebug.DBG_PPC, self.vehicle) then - local gx, gy, gz = localToWorld(self.goalWpNode.node, 0, 0, 0) - DebugUtil.drawDebugLine(gx, gy + 3, gz, gx, gy + 1, gz, 0, 1, 0); - DebugUtil.drawDebugNode(self.currentWpNode.node, string.format('ix = %d\ncurrent\nwaypoint', self.currentWpNode.ix)) - end -end - --- set the goal WP node's position. This will make sure the goal node is on the same height --- as the controlled node, avoiding issues when driving on non-level bridges where the controlled node --- is not vertical and since the goal is very far below it, it will be much closer/further in the controlled --- node's reference frame. --- If everyone is on the same height, this error is negligible -function PurePursuitController:setGoalTranslation(x, z) - local gx, _, gz = getWorldTranslation(self.goalWpNode.node) - local _, cy, _ = getWorldTranslation(self.controlledNode) - -- if there's an x, z passed in, use that, otherwise only adjust y to be the same as the controlled node - setTranslation(self.goalWpNode.node, x or gx, cy + 1, z or gz) -end - --- set the current waypoint for the rest of Courseplay and to notify listeners -function PurePursuitController:setCurrentWaypoint(ix) - -- this is the current waypoint for the rest of Courseplay code, the waypoint we are driving to - -- but never, ever go back. Instead just leave this loop and keep driving to the current goal node - if ix < self.currentWpNode.ix then - if g_updateLoopIndex % 60 == 0 then - self:debug("Won't step current waypoint back from %d to %d.", self.currentWpNode.ix, ix) - end - elseif ix >= self.currentWpNode.ix then - local prevIx = self.currentWpNode.ix - self.currentWpNode:setToWaypointOrBeyond(self.course, ix, self.lookAheadDistance) - -- if ix > #self.course, currentWpNode.ix will always be set to #self.course and the change detection won't work - -- therefore, only call listeners if ix <= #self.course - if ix ~= prevIx and ix <= self.course:getNumberOfWaypoints() then - -- remember to send notification at the end of the loop - self.sendWaypointChange = { current = self.currentWpNode.ix, prev = prevIx } - end - end -end - -function PurePursuitController:showGoalpointDiag(case, ...) - local diagText = string.format(...) - if CpDebug:isChannelActive(CpDebug.DBG_PPC, self.vehicle) then - DebugUtil.drawDebugNode(self.goalWpNode.node, diagText) - DebugUtil.drawDebugNode(self.controlledNode, 'controlled') - end - if case ~= self.case then - self:debug(...) - self.case = case - end -end - ---- Should we be driving in reverse based on the current position on course -function PurePursuitController:isReversing() - if self.course then - return self.course:isReverseAt(self:getCurrentWaypointIx()) or self.course:switchingToForwardAt(self:getCurrentWaypointIx()) - else - return false - end -end - --- goal point local position in the vehicle's coordinate system -function PurePursuitController:getGoalPointLocalPosition() - return localToLocal(self.goalWpNode.node, self.controlledNode, 0, 0, 0) -end - --- goal point normalized direction -function PurePursuitController:getGoalPointDirection() - local gx, _, gz = localToLocal(self.goalWpNode.node, self.controlledNode, 0, 0, 0) - local dx, dz = MathUtil.vector2Normalize(gx, gz) - -- check for NaN - if dx ~= dx or dz ~= dz then - return 0, 0 - end - return dx, dz -end - -function PurePursuitController:getGoalPointPosition() - return getWorldTranslation(self.goalWpNode.node) -end - -function PurePursuitController:getCurrentWaypointPosition() - return self:getGoalPointPosition() -end - -function PurePursuitController:getCurrentWaypointYRotation() - return self.course:getYRotationCorrectedForDirectionChanges(self.currentWpNode.ix) -end - -function PurePursuitController:getCrossTrackError() - return self.crossTrackError -end - -function PurePursuitController:reachedLastWaypoint() - return self.relevantWpNode.ix >= self.course:getNumberOfWaypoints() -end - -function PurePursuitController:haveJustPassedWaypoint(ix) - return self.lastPassedWaypointIx and self.lastPassedWaypointIx == ix or false -end - -function PurePursuitController:haveAlreadyPassedWaypoint(ix) - return self.lastPassedWaypointIx and self.lastPassedWaypointIx <= ix or false +--[[ +This file is part of Courseplay (https://github.com/Courseplay/courseplay) +Copyright (C) 2018-2021 Peter Vaiko + +This program is free software: you can redistribute it and/or modify +it under the terms of the GNU General Public License as published by +the Free Software Foundation, either version 3 of the License, or +(at your option) any later version. + +This program is distributed in the hope that it will be useful, +but WITHOUT ANY WARRANTY; without even the implied warranty of +MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +GNU General Public License for more details. + +You should have received a copy of the GNU General Public License +along with this program. If not, see . +]] + +--[[ + +This is a simplified implementation of a pure pursuit algorithm +to follow a two dimensional path consisting of waypoints. + +See the paper + +Steering Control of an Autonomous Ground Vehicle with Application to the DARPA +Urban Challenge By Stefan F. Campbell + +We use the terminology of that paper here, like 'relevant path segment', 'goal point', etc. and follow the +algorithm to search for the goal point as described in this paper. + +PURPOSE + +1. Provide a goal point to steer towards to. + Contrary to the old implementation, we are not steering to a waypoint, instead to a goal + point which is in a given look ahead distance from the vehicle on the path. + +2. Determine when to switch to the next waypoint (and avoid circling) + Regardless of the above, the rest of the Courseplay code still needs to know the current + waypoint as we progress along the path. + +HOW TO USE + +1. add a PPC to the vehicle with new() +2. when the vehicle starts driving, call initialize() +3. in every update cycle, call update(). This will calculate the goal point and the current waypoint +4. use the convenience functions getCurrentWaypointPosition(), reachedLastWaypoint() + in your code instead of directly checking and manipulating vehicle.Waypoints. These provide the legacy behavior when + the PPC is not active (for example due to reverse driving) or when disabled +6. you can use enable() and disable() to enable/disable the PPC. When disabled and you are using the above functions, + it'll behave as the legacy code. + +]] + +---@class PurePursuitController +PurePursuitController = CpObject() + +--- if the vehicle is more than cutOutDistanceLimit meters from the current segment's endpoints, cut-out the +--- controller to stop. Some error must have caused us to wander way off-track, unlikely to recover. +PurePursuitController.cutOutDistanceLimit = 50 +--- Only shut off after being off track for this long (ms). Reduces false shutdowns on brief excursions (turns, terrain). +PurePursuitController.offTrackGracePeriodMs = 10000 + +-- constructor +function PurePursuitController:init(vehicle) + self.normalLookAheadDistance = math.min(vehicle.maxTurningRadius, 6) + self.shortLookaheadDistance = math.max(2, self.normalLookAheadDistance / 2) + -- normal lookahead distance + self.baseLookAheadDistance = self.normalLookAheadDistance + -- adapted look ahead distance + self.lookAheadDistance = self.baseLookAheadDistance + self.temporaryLookAheadDistance = CpTemporaryObject(nil) + -- when transitioning from forward to reverse, this close we have to be to the waypoint where we + -- change direction before we switch to the next waypoint + self.distToSwitchWhenChangingToReverse = 1 + self.vehicle = vehicle + self:resetControlledNode() + self.name = vehicle:getName() + -- node on the current waypoint + self.currentWpNode = WaypointNode(self.name .. '-currentWpNode', true) + -- waypoint at the start of the relevant segment + self.relevantWpNode = WaypointNode(self.name .. '-relevantWpNode', true) + -- waypoint at the end of the relevant segment + self.nextWpNode = WaypointNode(self.name .. '-nextWpNode', true) + -- the current goal node + self.goalWpNode = WaypointNode(self.name .. '-goalWpNode', false) + -- vehicle position projected on the path, not used for anything other than debug display + self.projectedPosNode = CpUtil.createNode(self.name .. '-projectedPosNode', 0, 0, 0) + -- index of the first node of the path (where PPC is initialized and starts driving + self.firstIx = 1 + self.crossTrackError = 0 + self.lastPassedWaypointIx = nil + self.waypointPassedListeners = {} + self.waypointChangeListeners = {} + -- enable/disable stopping the vehicle when it is off-track (too far away from any waypoint) + self.stopWhenOffTrack = CpTemporaryObject(true) + -- when we first entered the off-track shutdown zone (nil = not in zone or just left it) + self.offTrackShutdownSince = nil +end + +-- destructor +function PurePursuitController:delete() + self.currentWpNode:destroy() + self.relevantWpNode:destroy() + self.nextWpNode:destroy() + CpUtil.destroyNode(self.projectedPosNode) + self.goalWpNode:destroy() +end + +function PurePursuitController:debug(...) + CpUtil.debugVehicle(CpDebug.DBG_PPC, self.vehicle, 'PPC: ' .. string.format(...)) +end + +function PurePursuitController:debugSparse(...) + if g_updateLoopIndex % 100 == 0 then + self:debug(...) + end +end + +---@param course Course +function PurePursuitController:setCourse(course) + self.course = course +end + +function PurePursuitController:getCourse() + return self.course +end + +--- Set an offset for the current course. +function PurePursuitController:setOffset(x, z) + self.course:setOffset(x, z) +end + +function PurePursuitController:getOffset() + return self.course:getOffset() +end + +--- Disable off-track detection temporarily, for instance while we know the vehicle must be driving +--- longer distances between two waypoints, like an unloader following a chopper through a turn, where +--- in some patterns the row end and the next row start are far apart. +function PurePursuitController:disableStopWhenOffTrack(milliseconds) + self.stopWhenOffTrack:set(false, milliseconds) +end + +--- Use a different node to track/control, for example the root node of a trailed implement +-- instead of the tractor's root node. +function PurePursuitController:setControlledNode(node) + self.controlledNode = node +end + +function PurePursuitController:getControlledNode() + return self.controlledNode +end + +--- reset controlled node to the default (vehicle's own direction node) +function PurePursuitController:resetControlledNode() + self.controlledNode = self.vehicle:getAIDirectionNode() +end + +-- initialize controller before driving +function PurePursuitController:initialize(ix) + self.firstIx = ix + -- relevantWpNode always points to the point where the relevant path segment starts + self.relevantWpNode:setToWaypoint(self.course, self.firstIx ) + self.nextWpNode:setToWaypoint(self.course, self.firstIx) + self.wpBeforeGoalPointIx = self.nextWpNode.ix + self.currentWpNode:setToWaypoint(self.course, self.firstIx ) + self.course:setCurrentWaypointIx(self.firstIx) + self.course:setLastPassedWaypointIx(nil) + self:debug('initialized to waypoint %d of %d', self.firstIx, self.course:getNumberOfWaypoints()) + self.lastPassedWaypointIx = nil + -- force calling the waypoint change callback right after initialization + self.sendWaypointChange = { current = self.currentWpNode.ix, prev = self.currentWpNode.ix - 1} + self.sendWaypointPassed = nil + -- current goal point search case as described in the paper, for diagnostics only + self.case = 0 +end + +-- Initialize to a waypoint when reversing. +-- TODO: this has to be called explicitly but could be done automatically by the vanilla initialize() +-- make sure the waypoint we initialize to is close to the controlled node, otherwise the PPC will +-- remain in initializing mode if the waypoint is too far back from the controlled node, and just +-- reverse forever +function PurePursuitController:initializeForReversing(ix) + local reverserNode, debugText = self:getReverserNode(false) + if reverserNode then + self:debug('Reverser node %s found, initializing with it', debugText) + -- don't use ix as it is, instead, find the waypoint closest to the reverser node + local dPrev, d = math.huge, self.course:getWaypoint(ix):getDistanceFromNode(reverserNode) + while d < dPrev and self.course:isReverseAt(ix) and ix < self.course:getNumberOfWaypoints() do + dPrev = d + ix = ix + 1 + d = self.course:getWaypoint(ix):getDistanceFromNode(reverserNode) + end + else + self:debug('No reverser node found, initializing with default controlled node') + end + self:initialize(ix) +end + +-- TODO: make this more generic and allow registering multiple listeners? +-- could also implement listeners for events like notify me when within x meters of a waypoint, etc. +function PurePursuitController:registerListeners(waypointListener, onWaypointPassedFunc, onWaypointChangeFunc) + self.savedWaypointListener = self.waypointListener + self.savedWaypointPassedListenerFunc = self.waypointPassedListenerFunc + self.savedWaypointChangeListenerFunc = self.waypointChangeListenerFunc + self.waypointListener = waypointListener + self.waypointPassedListenerFunc = onWaypointPassedFunc + self.waypointChangeListenerFunc = onWaypointChangeFunc +end + +-- Restore the listeners that were registered before the last call of registerListeners(). If there were none, +-- do not restore anything +function PurePursuitController:restorePreviouslyRegisteredListeners() + if self.savedWaypointListener ~= nil then + self.waypointListener = self.savedWaypointListener + self.waypointPassedListenerFunc = self.savedWaypointPassedListenerFunc + self.waypointChangeListenerFunc = self.savedWaypointChangeListenerFunc + end +end + +function PurePursuitController:setLookaheadDistance(d) + self.baseLookAheadDistance = d +end + +function PurePursuitController:setNormalLookaheadDistance() + self.baseLookAheadDistance = self.normalLookAheadDistance +end + +function PurePursuitController:setShortLookaheadDistance() + self.baseLookAheadDistance = self.shortLookaheadDistance +end + +--- Set a short lookahead distance for ttlMs milliseconds +function PurePursuitController:setTemporaryShortLookaheadDistance(ttlMs) + self.temporaryLookAheadDistance:set(self.shortLookaheadDistance, ttlMs) +end + +function PurePursuitController:getLookaheadDistance() + return self.lookAheadDistance +end + +function PurePursuitController:setCurrentLookaheadDistance(cte) + local la = self.temporaryLookAheadDistance:get() or self.baseLookAheadDistance + self.lookAheadDistance = math.min(la + math.abs(cte), la * 2) +end + +--- get index of current waypoint (one we are driving towards) +function PurePursuitController:getCurrentWaypointIx() + return self.currentWpNode.ix +end + +--- Get the current waypoint object +---@return Waypoint +function PurePursuitController:getCurrentWaypoint() + return self.course:getWaypoint(self.currentWpNode.ix) +end + +--- get index of relevant waypoint (one we are close to) +function PurePursuitController:getRelevantWaypointIx() + return self.relevantWpNode.ix +end + +function PurePursuitController:getLastPassedWaypointIx() + return self.lastPassedWaypointIx +end + +---@return number, string node that would be used for reversing, debug text explaining what node it is +function PurePursuitController:getReverserNode(suppressLog) + if not self.reversingImplement then + self.reversingImplement = AIUtil.getFirstReversingImplementWithWheels(self.vehicle, suppressLog) + end + return AIUtil.getReverserNode(self.vehicle, self.reversingImplement, suppressLog) +end + +--- When reversing, use the towed implement's node as a reference +function PurePursuitController:switchControlledNode() + local lastControlledNode = self.controlledNode + local debugText = 'AIDirectionNode' + local reverserNode + if self:isReversing() then + reverserNode, debugText = self:getReverserNode(true) + if reverserNode then + self:setControlledNode(reverserNode) + else + self:resetControlledNode() + end + else + self:resetControlledNode() + end + if self.controlledNode ~= lastControlledNode then + self:debug('Switching controlled node to %s', debugText) + end +end + +function PurePursuitController:update() + if not self.course then + self:debugSparse('no course set.') + return + end + self:showDebugTable() + self:switchControlledNode() + self:findRelevantSegment() + self:findGoalPoint() + self.course:setCurrentWaypointIx(self.currentWpNode.ix) + self.course:setLastPassedWaypointIx(self.lastPassedWaypointIx) + self:notifyListeners() +end + +function PurePursuitController:showDebugTable() + if self.course then + if CpDebug:isChannelActive(CpDebug.DBG_COURSES, self.vehicle) then + local info = { + title = self.course:getName(), + content = self.course:getDebugTable() + } + CpDebug:drawVehicleDebugTable(self.vehicle, { info }) + end + end +end + +function PurePursuitController:notifyListeners() + if self.waypointListener then + if self.sendWaypointChange then + -- send waypoint change event for all waypoints between the previous and current to make sure + -- we don't miss any + self:debug('prev %s curr %s', self.sendWaypointChange.prev, self.sendWaypointChange.current) + for ix = self.sendWaypointChange.prev + 1, self.sendWaypointChange.current do + self.waypointListener[self.waypointChangeListenerFunc](self.waypointListener, ix, self.course) + end + end + if self.sendWaypointPassed then + self.waypointListener[self.waypointPassedListenerFunc](self.waypointListener, self.sendWaypointPassed, self.course) + end + end + self.sendWaypointChange = nil + self.sendWaypointPassed = nil +end + +function PurePursuitController:havePassedWaypoint(wpNode) + local vx, vy, vz = getWorldTranslation(self.controlledNode) + local dx, _, dz = worldToLocal(wpNode.node, vx, vy, vz); + local dFromNext = MathUtil.vector2Length(dx, dz) + --self:debug('checking %d, dz: %.1f, dFromNext: %.1f', wpNode.ix, dz, dFromNext) + local result = false + if self.course:switchingDirectionAt(wpNode.ix) then + -- switching direction at this waypoint, so this is pointing into the opposite direction. + -- we have to make sure we drive up to this waypoint close enough before we switch to the next + -- so wait until dz < 0, that is, we are behind the waypoint + if dz < 0 then + result = true + end + else + -- we are not transitioning between forward and reverse + -- we have passed the next waypoint if our dz in the waypoints coordinate system is positive, that is, + -- when looking into the direction of the waypoint, we are ahead of it. + -- Also, when on the process of aligning to the course, like for example the vehicle just started + -- driving towards the first waypoint, we have to make sure we actually get close to the waypoint + -- (as we may already be in front of it), so try get within the turn diameter * 2. + if dz >= 0 and dFromNext < self.vehicle.maxTurningRadius * 4 then + result = true + end + end + if result then + --and not self:reachedLastWaypoint() then + if not self.lastPassedWaypointIx or (self.lastPassedWaypointIx ~= wpNode.ix) then + self.lastPassedWaypointIx = wpNode.ix + self:debug('waypoint %d passed, dz: %.1f %s %s', wpNode.ix, dz, + self.course.waypoints[wpNode.ix].rev and 'reverse' or '', + self.course:switchingDirectionAt(wpNode.ix) and 'switching direction' or '') + -- notify listeners about the passed waypoint + self.sendWaypointPassed = self.lastPassedWaypointIx + end + end + return result +end + +function PurePursuitController:havePassedAnyWaypointBetween(fromIx, toIx) + local node = WaypointNode(self.name .. '-node', false) + local result, passedWaypointIx = false, 0 + -- math.max so we do one loop even if toIx < fromIx + --self:debug('checking between %d and %d', fromIx, toIx) + for ix = fromIx, math.max(toIx, fromIx) do + node:setToWaypoint(self.course, ix) + if self:havePassedWaypoint(node) then + result = true + passedWaypointIx = ix + break + end + + end + node:destroy() + return result, passedWaypointIx +end + +-- Finds the relevant segment. +-- Sets the vehicle's projected position on the path. +function PurePursuitController:findRelevantSegment() + -- vehicle position + local vx, vy, vz = getWorldTranslation(self.controlledNode) + -- update the position of the relevant node (in case the course offset changed) + self.relevantWpNode:setToWaypoint(self.course, self.relevantWpNode.ix) + local lx, _, dzFromRelevant = worldToLocal(self.relevantWpNode.node, vx, vy, vz); + self.crossTrackError = lx + -- adapt our lookahead distance based on the error + self:setCurrentLookaheadDistance(self.crossTrackError) + -- projected vehicle position/rotation + local px, py, pz = localToWorld(self.relevantWpNode.node, 0, 0, dzFromRelevant) + local _, yRot, _ = getRotation(self.relevantWpNode.node) + setTranslation(self.projectedPosNode, px, py, pz) + setRotation(self.projectedPosNode, 0, yRot, 0) + -- we check all waypoints between the relevant and the one before the goal point as the goal point + -- may have moved several waypoints up if there's a very sharp turn for example and in that case + -- the vehicle may never reach some of the waypoint in between. + local passed, ix + if self.course:switchingDirectionAt(self.nextWpNode.ix) then + -- don't look beyond a direction switch as we'll always be past a reversing waypoint + -- before we reach it. + passed, ix = self:havePassedWaypoint(self.nextWpNode), self.nextWpNode.ix + else + passed, ix = self:havePassedAnyWaypointBetween(self.nextWpNode.ix, self.wpBeforeGoalPointIx) + end + if passed then + self.relevantWpNode:setToWaypoint(self.course, ix, true) + self.nextWpNode:setToWaypoint(self.course, self.relevantWpNode.ix + 1, true) + if not self:reachedLastWaypoint() then + -- disable debugging once we reached the last waypoint. Otherwise we'd keep logging + -- until the user presses 'Stop driver'. + self:debug('relevant waypoint: %d, next waypoint %d, crosstrack: %.1f', + self.relevantWpNode.ix, self.nextWpNode.ix, self.crossTrackError) + end + end + if CpDebug:isChannelActive(CpDebug.DBG_PPC, self.vehicle) then + DebugUtil.drawDebugLine(px, py + 3, pz, px, py + 1, pz, 1, 1, 0); + DebugUtil.drawDebugNode(self.relevantWpNode.node, string.format('ix = %d\nrelevant\nnode', self.relevantWpNode.ix)) + DebugUtil.drawDebugNode(self.projectedPosNode, 'projected\nvehicle\nposition') + end +end + +-- Now, from the relevant section forward we search for the goal point, which is the one +-- lying lookAheadDistance in front of us on the path +-- this is the algorithm described in Chapter 2 of the paper +function PurePursuitController:findGoalPoint() + + local vx, _, vz = getWorldTranslation(self.controlledNode) + --local vx, vy, vz = getWorldTranslation(self.projectedPosNode); + + -- create helper nodes at the relevant and the next wp. We'll move these up on the path until we reach the segment + -- in lookAheadDistance + local node1 = WaypointNode(self.name .. '-node1', false) + local node2 = WaypointNode(self.name .. '-node2', false) + + -- starting at the relevant segment walk up the path to find the segment on + -- which the goal point lies. This is the segment intersected by the circle with lookAheadDistance radius + -- around the vehicle. + local ix = self.relevantWpNode.ix + while ix <= self.course:getNumberOfWaypoints() do + node1:setToWaypoint(self.course, ix) + node2:setToWaypointOrBeyond(self.course, ix + 1, self.lookAheadDistance) + local x1, _, z1 = getWorldTranslation(node1.node) + local x2, _, z2 = getWorldTranslation(node2.node) + -- distance between the vehicle position and the ends of the segment + local q1 = MathUtil.getPointPointDistance(x1, z1, vx, vz) -- distance from node 1 + local q2 = MathUtil.getPointPointDistance(x2, z2, vx, vz) -- distance from node 2 + local l = MathUtil.getPointPointDistance(x1, z1, x2, z2) -- length of path segment (distance between node 1 and 2 + -- self:debug('ix=%d, q1=%.1f, q2=%.1f la=%.1f l=%.1f', ix, q1, q2, self.lookAheadDistance, l) + + -- case i (first node outside virtual circle but not yet reached) or (not the first node but we are way off the track) + if (ix == self.firstIx and ix ~= self.lastPassedWaypointIx) and + q1 >= self.lookAheadDistance and q2 >= self.lookAheadDistance then + self.offTrackShutdownSince = nil + -- If we weren't on track yet (after initialization, on our way to the first/initialized waypoint) + -- set the goal to the relevant WP + self.goalWpNode:setToWaypoint(self.course, self.relevantWpNode.ix) + self:setGoalTranslation() + self:showGoalpointDiag(1, 'initializing, ix=%d, q1=%.1f, q2=%.1f, la=%.1f', ix, q1, q2, self.lookAheadDistance) + -- and also the current waypoint is now at the relevant WP + self:setCurrentWaypoint(self.relevantWpNode.ix) + break + end + + -- case ii (common case) + if q1 <= self.lookAheadDistance and q2 >= self.lookAheadDistance then + self.offTrackShutdownSince = nil + -- in some weird cases q1 may be 0 (when we calculate a course based on the vehicle position) so fix that + -- to avoid a nan + if q1 < 0.0001 then + q1 = 0.1 + end + local cosGamma = (q2 * q2 - q1 * q1 - l * l) / (-2 * l * q1) + local p = q1 * cosGamma + math.sqrt(q1 * q1 * (cosGamma * cosGamma - 1) + self.lookAheadDistance * self.lookAheadDistance) + local gx, _, gz = localToWorld(node1.node, 0, 0, p) + self:setGoalTranslation(gx, gz) + self.wpBeforeGoalPointIx = ix + self:showGoalpointDiag(2, 'common case, ix=%d, q1=%.1f, q2=%.1f la=%.1f', ix, q1, q2, self.lookAheadDistance) + -- current waypoint is the waypoint at the end of the path segment + self:setCurrentWaypoint(ix + 1) + --CpUtil.debugVehicle(CpDebug.DBG_PPC, self.vehicle, "PPC: %d, p=%.1f", self.currentWpNode.ix, p) + break + end + + -- cases iii, iv and v + -- these two may have a problem and actually prevent the vehicle go back to the waypoint + -- when wandering way off track, therefore we try to catch this case in case i + if ix == self.relevantWpNode.ix and q1 >= self.lookAheadDistance and q2 >= self.lookAheadDistance then + if math.abs(self.crossTrackError) <= self.lookAheadDistance then + -- case iii (two intersection points) + local p = math.sqrt(self.lookAheadDistance * self.lookAheadDistance - self.crossTrackError * self.crossTrackError) + local gx, _, gz = localToWorld(self.projectedPosNode, 0, 0, p) + self:setGoalTranslation(gx, gz) + self.wpBeforeGoalPointIx = ix + self:showGoalpointDiag(3, 'two intersection points, ix=%d, q1=%.1f, q2=%.1f, la=%.1f, cte=%.1f', ix, q1, q2, + self.lookAheadDistance, self.crossTrackError) + -- current waypoint is the waypoint at the end of the path segment + self:setCurrentWaypoint(ix + 1) + else + -- case iv (no intersection points) + -- case v ( goal point dead zone) + -- set the goal to the projected position + local gx, _, gz = localToWorld(self.projectedPosNode, 0, 0, 0) + self:setGoalTranslation(gx, gz) + self.wpBeforeGoalPointIx = ix + self:showGoalpointDiag(4, 'no intersection points, ix=%d, q1=%.1f, q2=%.1f, la=%.1f, cte=%.1f', ix, q1, q2, + self.lookAheadDistance, self.crossTrackError) + -- current waypoint is the waypoint at the end of the path segment + self:setCurrentWaypoint(ix + 1) + end + -- Only shut off after being off track beyond cutOutDistanceLimit for a grace period (reduces false shutdowns) + if (q1 > self.cutOutDistanceLimit) and (q2 > self.cutOutDistanceLimit) and self.stopWhenOffTrack:get() then + local now = g_currentMission and g_currentMission.time or 0 + if self.offTrackShutdownSince == nil then + self.offTrackShutdownSince = now + end + if (now - self.offTrackShutdownSince) >= (self.offTrackGracePeriodMs or PurePursuitController.offTrackGracePeriodMs or 10000) then + -- Give the current drive strategy a chance to recover softly instead of + -- stopping the CP helper entirely (user has to jump to that vehicle to + -- restart). If the strategy implements onOffTrackShutdown() and returns + -- true it has handled recovery and we must NOT call stopCurrentAIJob. + local strategy = self.vehicle.getCpDriveStrategy and self.vehicle:getCpDriveStrategy() + if strategy and strategy.onOffTrackShutdown then + local handled = strategy:onOffTrackShutdown() + if handled then + CpUtil.infoVehicle(self.vehicle, + 'vehicle off track, strategy performed soft recovery instead of shutdown.') + self.offTrackShutdownSince = nil + break + end + end + CpUtil.infoVehicle(self.vehicle, 'vehicle off track, shutting off Courseplay now.') + self.vehicle:stopCurrentAIJob(AIMessageCpError.new()) + return + end + else + self.offTrackShutdownSince = nil + end + break + end + -- none of the above, continue search with the next path segment + ix = ix + 1 + -- unless there's a direction change here. This should only happen right after initialization and when + -- the reference node is already beyond the direction switch waypoint. We should not skip that being + -- the current waypoint otherwise the relevant waypoint won't be moved over the direction switch + if self.course:switchingDirectionAt(ix) then + self.offTrackShutdownSince = nil + -- force waypoint change + self:showGoalpointDiag(100, 'switching direction while looking for goal point, ix=%d', ix) + self.wpBeforeGoalPointIx = ix - 1 + self:setCurrentWaypoint(ix) + break + end + end + + node1:destroy() + node2:destroy() + + if CpDebug:isChannelActive(CpDebug.DBG_PPC, self.vehicle) then + local gx, gy, gz = localToWorld(self.goalWpNode.node, 0, 0, 0) + DebugUtil.drawDebugLine(gx, gy + 3, gz, gx, gy + 1, gz, 0, 1, 0); + DebugUtil.drawDebugNode(self.currentWpNode.node, string.format('ix = %d\ncurrent\nwaypoint', self.currentWpNode.ix)) + end +end + +-- set the goal WP node's position. This will make sure the goal node is on the same height +-- as the controlled node, avoiding issues when driving on non-level bridges where the controlled node +-- is not vertical and since the goal is very far below it, it will be much closer/further in the controlled +-- node's reference frame. +-- If everyone is on the same height, this error is negligible +function PurePursuitController:setGoalTranslation(x, z) + local gx, _, gz = getWorldTranslation(self.goalWpNode.node) + local _, cy, _ = getWorldTranslation(self.controlledNode) + -- if there's an x, z passed in, use that, otherwise only adjust y to be the same as the controlled node + setTranslation(self.goalWpNode.node, x or gx, cy + 1, z or gz) +end + +-- set the current waypoint for the rest of Courseplay and to notify listeners +function PurePursuitController:setCurrentWaypoint(ix) + -- this is the current waypoint for the rest of Courseplay code, the waypoint we are driving to + -- but never, ever go back. Instead just leave this loop and keep driving to the current goal node + if ix < self.currentWpNode.ix then + if g_updateLoopIndex % 60 == 0 then + self:debug("Won't step current waypoint back from %d to %d.", self.currentWpNode.ix, ix) + end + elseif ix >= self.currentWpNode.ix then + local prevIx = self.currentWpNode.ix + self.currentWpNode:setToWaypointOrBeyond(self.course, ix, self.lookAheadDistance) + -- if ix > #self.course, currentWpNode.ix will always be set to #self.course and the change detection won't work + -- therefore, only call listeners if ix <= #self.course + if ix ~= prevIx and ix <= self.course:getNumberOfWaypoints() then + -- remember to send notification at the end of the loop + self.sendWaypointChange = { current = self.currentWpNode.ix, prev = prevIx } + end + end +end + +function PurePursuitController:showGoalpointDiag(case, ...) + local diagText = string.format(...) + if CpDebug:isChannelActive(CpDebug.DBG_PPC, self.vehicle) then + DebugUtil.drawDebugNode(self.goalWpNode.node, diagText) + DebugUtil.drawDebugNode(self.controlledNode, 'controlled') + end + if case ~= self.case then + self:debug(...) + self.case = case + end +end + +--- Should we be driving in reverse based on the current position on course +function PurePursuitController:isReversing() + if self.course then + return self.course:isReverseAt(self:getCurrentWaypointIx()) or self.course:switchingToForwardAt(self:getCurrentWaypointIx()) + else + return false + end +end + +-- goal point local position in the vehicle's coordinate system +function PurePursuitController:getGoalPointLocalPosition() + return localToLocal(self.goalWpNode.node, self.controlledNode, 0, 0, 0) +end + +-- goal point normalized direction +function PurePursuitController:getGoalPointDirection() + local gx, _, gz = localToLocal(self.goalWpNode.node, self.controlledNode, 0, 0, 0) + local dx, dz = MathUtil.vector2Normalize(gx, gz) + -- check for NaN + if dx ~= dx or dz ~= dz then + return 0, 0 + end + return dx, dz +end + +function PurePursuitController:getGoalPointPosition() + return getWorldTranslation(self.goalWpNode.node) +end + +function PurePursuitController:getCurrentWaypointPosition() + return self:getGoalPointPosition() +end + +function PurePursuitController:getCurrentWaypointYRotation() + return self.course:getYRotationCorrectedForDirectionChanges(self.currentWpNode.ix) +end + +function PurePursuitController:getCrossTrackError() + return self.crossTrackError +end + +function PurePursuitController:reachedLastWaypoint() + return self.relevantWpNode.ix >= self.course:getNumberOfWaypoints() +end + +function PurePursuitController:haveJustPassedWaypoint(ix) + return self.lastPassedWaypointIx and self.lastPassedWaypointIx == ix or false +end + +function PurePursuitController:haveAlreadyPassedWaypoint(ix) + return self.lastPassedWaypointIx and self.lastPassedWaypointIx <= ix or false end \ No newline at end of file diff --git a/scripts/ai/controllers/MotorController.lua b/scripts/ai/controllers/MotorController.lua index f29909af..aa6f2bf7 100644 --- a/scripts/ai/controllers/MotorController.lua +++ b/scripts/ai/controllers/MotorController.lua @@ -25,16 +25,8 @@ function MotorController:update() if not self.isValid then return end - if not self.settings.fuelSave:getValue() then - if not self:getIsStarted() then - self:startMotor() - self.vehicle:raiseAIEvent('onAIFieldWorkerContinue', 'onAIImplementContinue') - end - self.timerSet = false - return - end if self:isFuelSaveDisabled() or self.driveStrategy:getMaxSpeed() > - self.speedThreshold then + self.speedThreshold or not self.settings.fuelSave:getValue() then if not self:getIsStarted() then self:startMotor() self.vehicle:raiseAIEvent("onAIFieldWorkerContinue", "onAIImplementContinue") @@ -85,7 +77,7 @@ function MotorController:getDriveData() end if g_Courseplay.globalSettings.waitForRefueling:getValue() and self:isFuelLow(self.fuelThresholdSetting:getValue()) then - + maxSpeed = 0 end @@ -132,4 +124,4 @@ end function MotorController:getIsStarted() return self.vehicle:getMotorState() ~= MotorState.OFF -end \ No newline at end of file +end diff --git a/scripts/ai/strategies/AIDriveStrategyCombineCourse.lua b/scripts/ai/strategies/AIDriveStrategyCombineCourse.lua index 537b22cb..d6e234ba 100644 --- a/scripts/ai/strategies/AIDriveStrategyCombineCourse.lua +++ b/scripts/ai/strategies/AIDriveStrategyCombineCourse.lua @@ -967,13 +967,13 @@ end ---@param vehicle table ---@return boolean true if vehicle is an active Courseplay controlled combine/harvester, ---- or a manually-driven combine with an active "Call Grain Cart" request +--- or a manually-driven combine with an active "Call Unloader" request function AIDriveStrategyCombineCourse.isActiveCpCombine(vehicle) if vehicle.getIsCpActive and vehicle:getIsCpActive() then local driveStrategy = vehicle.getCpDriveStrategy and vehicle:getCpDriveStrategy() return driveStrategy and driveStrategy.callUnloader ~= nil end - if vehicle.cpIsCallGrainCartActive and vehicle:cpIsCallGrainCartActive() then + if vehicle.cpIsManualCombineCallingUnloader and vehicle:cpIsManualCombineCallingUnloader() then return true end return false diff --git a/scripts/ai/strategies/AIDriveStrategyUnloadCombine.lua b/scripts/ai/strategies/AIDriveStrategyUnloadCombine.lua index 2147ced8..41a9385c 100644 --- a/scripts/ai/strategies/AIDriveStrategyUnloadCombine.lua +++ b/scripts/ai/strategies/AIDriveStrategyUnloadCombine.lua @@ -297,28 +297,8 @@ function AIDriveStrategyUnloadCombine:setAIVehicle(vehicle, jobParameters) if self.ppc then self.ppc.offTrackGracePeriodMs = 20000 end - if CollisionAvoidanceController then - self.collisionAvoidanceController = CollisionAvoidanceController(self.vehicle, self) - else - CpUtil.errorVehicle(self.vehicle, 'Courseplay: CollisionAvoidanceController not loaded (mod conflict?). Collision avoidance disabled.') - self.collisionAvoidanceController = { isCollisionWarningActive = function() return false end } - end - if ProximityController then - self.proximityController = ProximityController(self.vehicle, self:getProximitySensorWidth()) - else - CpUtil.errorVehicle(self.vehicle, 'Courseplay: ProximityController not loaded (mod conflict?). Proximity control disabled.') - self.proximityController = { - registerIsSlowdownEnabledCallback = function() end, - registerBlockingVehicleListener = function() end, - registerIgnoreObjectCallback = function() end, - checkBlockingVehicleFront = function() return math.huge, nil end, - disableLeftSide = function() end, - disableRightSide = function() end, - enableBothSides = function() end, - getDriveData = function(_, maxSpeed) return nil, nil, nil, maxSpeed end, - isVehicleInRange = function() return false end, - } - end + self.collisionAvoidanceController = CollisionAvoidanceController(self.vehicle, self) + self.proximityController = ProximityController(self.vehicle, self:getProximitySensorWidth()) self.proximityController:registerIsSlowdownEnabledCallback(self, AIDriveStrategyUnloadCombine.isProximitySpeedControlEnabled) self.proximityController:registerBlockingVehicleListener(self, AIDriveStrategyUnloadCombine.onBlockingVehicle) self.proximityController:registerIgnoreObjectCallback(self, AIDriveStrategyUnloadCombine.ignoreProximityObject) @@ -373,7 +353,7 @@ function AIDriveStrategyUnloadCombine:isCombineActive() if self.combineToUnload.getIsCpActive and self.combineToUnload:getIsCpActive() then return true end - if self.combineToUnload.cpIsCallGrainCartActive and self.combineToUnload:cpIsCallGrainCartActive() then + if self.combineToUnload.cpIsManualCombineCallingUnloader and self.combineToUnload:cpIsManualCombineCallingUnloader() then return true end end @@ -1842,8 +1822,8 @@ function AIDriveStrategyUnloadCombine:call(combine, waypoint) elseif self:isPathfindingNeeded(self.vehicle, self:getPipeOffsetReferenceNode(), xOffset, zOffset) then self:setNewState(self.states.WAITING_FOR_PATHFINDER) -- For manually-driven combines, tune pathfinding for faster, safer approach. - local isManualCombine = self.combineToUnload.cpIsCallGrainCartActive and - self.combineToUnload:cpIsCallGrainCartActive() + local isManualCombine = self.combineToUnload.cpIsManualCombineCallingUnloader and + self.combineToUnload:cpIsManualCombineCallingUnloader() self:startPathfindingToWaitingCombine(xOffset, zOffset, isManualCombine and self.onPathfindingFailedToMovingTarget or nil, isManualCombine) diff --git a/scripts/events/CallGrainCartEvent.lua b/scripts/events/CallGrainCartEvent.lua deleted file mode 100644 index f8927fa1..00000000 --- a/scripts/events/CallGrainCartEvent.lua +++ /dev/null @@ -1,42 +0,0 @@ ----@class CallGrainCartEvent -CallGrainCartEvent = {} -local CallGrainCartEvent_mt = Class(CallGrainCartEvent, Event) - -InitEventClass(CallGrainCartEvent, "CallGrainCartEvent") - -function CallGrainCartEvent.emptyNew() - local self = Event.new(CallGrainCartEvent_mt) - return self -end - -function CallGrainCartEvent.new(vehicle) - local self = CallGrainCartEvent.emptyNew() - self.vehicle = vehicle - return self -end - -function CallGrainCartEvent:readStream(streamId, connection) - self.vehicle = NetworkUtil.readNodeObject(streamId) - self:run(connection) -end - -function CallGrainCartEvent:writeStream(streamId, connection) - NetworkUtil.writeNodeObject(streamId, self.vehicle) -end - -function CallGrainCartEvent:run(connection) - if self.vehicle and self.vehicle.cpToggleCallGrainCart then - self.vehicle:cpToggleCallGrainCart() - end - if not connection:getIsServer() then - g_server:broadcastEvent(CallGrainCartEvent.new(self.vehicle), nil, connection, self.vehicle) - end -end - -function CallGrainCartEvent.sendEvent(vehicle) - if g_server ~= nil then - g_server:broadcastEvent(CallGrainCartEvent.new(vehicle), nil, nil, vehicle) - else - g_client:getServerConnection():sendEvent(CallGrainCartEvent.new(vehicle)) - end -end diff --git a/scripts/events/CpManualUnloaderEvent.lua b/scripts/events/CpManualUnloaderEvent.lua new file mode 100644 index 00000000..8222d43c --- /dev/null +++ b/scripts/events/CpManualUnloaderEvent.lua @@ -0,0 +1,42 @@ +---@class CpManualUnloaderEvent +CpManualUnloaderEvent = {} +local CpManualUnloaderEvent_mt = Class(CpManualUnloaderEvent, Event) + +InitEventClass(CpManualUnloaderEvent, "CpManualUnloaderEvent") + +function CpManualUnloaderEvent.emptyNew() + local self = Event.new(CpManualUnloaderEvent_mt) + return self +end + +function CpManualUnloaderEvent.new(vehicle) + local self = CpManualUnloaderEvent.emptyNew() + self.vehicle = vehicle + return self +end + +function CpManualUnloaderEvent:readStream(streamId, connection) + self.vehicle = NetworkUtil.readNodeObject(streamId) + self:run(connection) +end + +function CpManualUnloaderEvent:writeStream(streamId, connection) + NetworkUtil.writeNodeObject(streamId, self.vehicle) +end + +function CpManualUnloaderEvent:run(connection) + if self.vehicle and self.vehicle.cpToggleManualUnloader then + self.vehicle:cpToggleManualUnloader() + end + if not connection:getIsServer() then + g_server:broadcastEvent(CpManualUnloaderEvent.new(self.vehicle), nil, connection, self.vehicle) + end +end + +function CpManualUnloaderEvent.sendEvent(vehicle) + if g_server ~= nil then + g_server:broadcastEvent(CpManualUnloaderEvent.new(vehicle), nil, nil, vehicle) + else + g_client:getServerConnection():sendEvent(CpManualUnloaderEvent.new(vehicle)) + end +end diff --git a/scripts/gui/hud/CpFieldworkHudPage.lua b/scripts/gui/hud/CpFieldworkHudPage.lua index 26877113..9d9a8e98 100644 --- a/scripts/gui/hud/CpFieldworkHudPage.lua +++ b/scripts/gui/hud/CpFieldworkHudPage.lua @@ -80,19 +80,19 @@ function CpFieldWorkHudPageElement:setupElements(baseHud, vehicle, lines, wMargi CpGuiUtil.addCopyCourseBtn(self, baseHud, vehicle, lines, wMargin, hMargin, 1) - --- Call Grain Cart toggle button (left side) - self.callGrainCartBtn = baseHud:addLeftLineTextButton(self, 6, CpBaseHud.defaultFontSize, + --- Call Unloader toggle button (left side) + self.callManualUnloaderBtn = baseHud:addLeftLineTextButton(self, 6, CpBaseHud.defaultFontSize, function(vehicle) - if vehicle.cpToggleCallGrainCart then - vehicle:cpToggleCallGrainCart() + if vehicle.cpToggleManualUnloader then + vehicle:cpToggleManualUnloader() end end, vehicle) - --- Call Grain Cart status text (right side) - self.callGrainCartStatus = baseHud:addRightLineTextButton(self, 6, CpBaseHud.defaultFontSize, + --- Call Unloader status text (right side) + self.callManualUnloaderStatus = baseHud:addRightLineTextButton(self, 6, CpBaseHud.defaultFontSize, function(vehicle) - if vehicle.cpToggleCallGrainCart then - vehicle:cpToggleCallGrainCart() + if vehicle.cpToggleManualUnloader then + vehicle:cpToggleManualUnloader() end end, vehicle) end @@ -147,10 +147,10 @@ function CpFieldWorkHudPageElement:updateContent(vehicle, status) CpGuiUtil.updateCopyBtn(self, vehicle, status) - if self.callGrainCartBtn then + if self.callManualUnloaderBtn then local hasPipe = vehicle.spec_pipe ~= nil or AIUtil.hasChildVehicleWithSpecialization(vehicle, Pipe) local isCpActive = vehicle:getIsCpActive() - local isCallActive = vehicle.cpIsCallGrainCartActive and vehicle:cpIsCallGrainCartActive() + local isCallActive = vehicle.cpIsManualCombineCallingUnloader and vehicle:cpIsManualCombineCallingUnloader() -- Forage harvesters have a rotatable auto-aim spout (numAutoAimingStates > 0). -- The manual call system is not supported for them, so hide the button entirely. local isChopper = false @@ -162,18 +162,18 @@ function CpFieldWorkHudPageElement:updateContent(vehicle, status) end if pipeSpec then isChopper = (pipeSpec.numAutoAimingStates or 0) > 0 end local showBtn = hasPipe and not isCpActive and not isChopper - self.callGrainCartBtn:setVisible(showBtn) - self.callGrainCartStatus:setVisible(showBtn) + self.callManualUnloaderBtn:setVisible(showBtn) + self.callManualUnloaderStatus:setVisible(showBtn) if showBtn then - self.callGrainCartBtn:setTextDetails(g_i18n:getText("CP_callGrainCart")) + self.callManualUnloaderBtn:setTextDetails(g_i18n:getText("CP_callManualUnloader")) if isCallActive then - self.callGrainCartBtn:setColor(unpack(CpBaseHud.ON_COLOR)) - self.callGrainCartStatus:setTextDetails(g_i18n:getText("CP_callGrainCartActive")) - self.callGrainCartStatus:setColor(unpack(CpBaseHud.ON_COLOR)) + self.callManualUnloaderBtn:setColor(unpack(CpBaseHud.ON_COLOR)) + self.callManualUnloaderStatus:setTextDetails(g_i18n:getText("CP_callManualUnloaderActive")) + self.callManualUnloaderStatus:setColor(unpack(CpBaseHud.ON_COLOR)) else - self.callGrainCartBtn:setColor(unpack(CpBaseHud.OFF_COLOR)) - self.callGrainCartStatus:setTextDetails(g_i18n:getText("CP_callGrainCartInactive")) - self.callGrainCartStatus:setColor(unpack(CpBaseHud.OFF_COLOR)) + self.callManualUnloaderBtn:setColor(unpack(CpBaseHud.OFF_COLOR)) + self.callManualUnloaderStatus:setTextDetails(g_i18n:getText("CP_callManualUnloaderInactive")) + self.callManualUnloaderStatus:setColor(unpack(CpBaseHud.OFF_COLOR)) end end end diff --git a/scripts/specializations/CpAIFieldWorker.lua b/scripts/specializations/CpAIFieldWorker.lua index 745a591c..b84c6fab 100644 --- a/scripts/specializations/CpAIFieldWorker.lua +++ b/scripts/specializations/CpAIFieldWorker.lua @@ -73,8 +73,8 @@ function CpAIFieldWorker.registerFunctions(vehicleType) SpecializationUtil.registerFunction(vehicleType, "getCpStartingPointSetting", CpAIFieldWorker.getCpStartingPointSetting) SpecializationUtil.registerFunction(vehicleType, "getCpLaneOffsetSetting", CpAIFieldWorker.getCpLaneOffsetSetting) - SpecializationUtil.registerFunction(vehicleType, "cpToggleCallGrainCart", CpAIFieldWorker.cpToggleCallGrainCart) - SpecializationUtil.registerFunction(vehicleType, "cpIsCallGrainCartActive", CpAIFieldWorker.cpIsCallGrainCartActive) + SpecializationUtil.registerFunction(vehicleType, "cpToggleManualUnloader", CpAIFieldWorker.cpToggleManualUnloader) + SpecializationUtil.registerFunction(vehicleType, "cpIsManualCombineCallingUnloader", CpAIFieldWorker.cpIsManualCombineCallingUnloader) SpecializationUtil.registerFunction(vehicleType, "cpGetManualCombineProxy", CpAIFieldWorker.cpGetManualCombineProxy) end @@ -267,14 +267,14 @@ function CpAIFieldWorker:onCpFinished() end ------------------------------------------------------------------------------------------------------------------------ ---- Manual combine "Call Grain Cart" proxy management +--- Manual combine "Call Unloader" proxy management ------------------------------------------------------------------------------------------------------------------------ function CpAIFieldWorker:onUpdate(dt) local spec = CpAIFieldWorker.getSpec(self) if spec and spec.cpManualCombineProxy then if self:getIsCpActive() then - self:cpToggleCallGrainCart() + self:cpToggleManualUnloader() else spec.cpManualCombineProxy:update(dt) end @@ -289,7 +289,7 @@ function CpAIFieldWorker:onDelete() end end -function CpAIFieldWorker:cpToggleCallGrainCart() +function CpAIFieldWorker:cpToggleManualUnloader() local spec = CpAIFieldWorker.getSpec(self) if not spec then return end if spec.cpManualCombineProxy then @@ -305,11 +305,11 @@ function CpAIFieldWorker:cpToggleCallGrainCart() spec.cpManualCombineProxy = CpManualCombineProxy(self) end if not self.isServer then - CallGrainCartEvent.sendEvent(self) + CpManualUnloaderEvent.sendEvent(self) end end -function CpAIFieldWorker:cpIsCallGrainCartActive() +function CpAIFieldWorker:cpIsManualCombineCallingUnloader() local spec = CpAIFieldWorker.getSpec(self) return spec and spec.cpManualCombineProxy ~= nil end diff --git a/scripts/specializations/CpAIWorker.lua b/scripts/specializations/CpAIWorker.lua index ee08a256..4fd98e1c 100644 --- a/scripts/specializations/CpAIWorker.lua +++ b/scripts/specializations/CpAIWorker.lua @@ -181,8 +181,8 @@ function CpAIWorker:onRegisterActionEvents(isActiveForInput, isActiveForInputIgn end, g_i18n:getText("input_CP_OPEN_COURSEMANAGER")) addActionEvent(self, InputAction.CP_CALL_GRAIN_CART, function () - if self.cpToggleCallGrainCart then - self:cpToggleCallGrainCart() + if self.cpToggleManualUnloader then + self:cpToggleManualUnloader() end end) @@ -264,13 +264,13 @@ function CpAIWorker:updateActionEvents() end end local isChopper = pipeSpec and (pipeSpec.numAutoAimingStates or 0) > 0 - local showCallGrainCart = hasPipe and not isCpActive and not isChopper - g_inputBinding:setActionEventActive(actionEvent.actionEventId, showCallGrainCart) - if showCallGrainCart then - local isActive = self.cpIsCallGrainCartActive and self:cpIsCallGrainCartActive() - local status = isActive and g_i18n:getText("CP_callGrainCartActive") or g_i18n:getText("CP_callGrainCartInactive") + local showCallManualUnloader = hasPipe and not isCpActive and not isChopper + g_inputBinding:setActionEventActive(actionEvent.actionEventId, showCallManualUnloader) + if showCallManualUnloader then + local isActive = self.cpIsManualCombineCallingUnloader and self:cpIsManualCombineCallingUnloader() + local status = isActive and g_i18n:getText("CP_callManualUnloaderActive") or g_i18n:getText("CP_callManualUnloaderInactive") g_inputBinding:setActionEventText(actionEvent.actionEventId, - string.format("%s (%s)", g_i18n:getText("CP_callGrainCart"), status)) + string.format("%s (%s)", g_i18n:getText("CP_callManualUnloader"), status)) end end end diff --git a/translations/translation_en.xml b/translations/translation_en.xml index bdb3a526..f26f4001 100644 --- a/translations/translation_en.xml +++ b/translations/translation_en.xml @@ -13,9 +13,9 @@ - - - + + +