From 76680421761b730593900e0ce350a349616618a7 Mon Sep 17 00:00:00 2001 From: schwiti6190 <58079399+schwiti6190@users.noreply.github.com> Date: Tue, 10 Oct 2023 12:01:46 +0200 Subject: [PATCH] WIP Pathfinder controller to unify the pathfinder handling --- scripts/ai/AIDriveStrategyCourse.lua | 6 +- scripts/ai/PathfinderController.lua | 364 +++++++++++++++++++++++++++ 2 files changed, 369 insertions(+), 1 deletion(-) create mode 100644 scripts/ai/PathfinderController.lua diff --git a/scripts/ai/AIDriveStrategyCourse.lua b/scripts/ai/AIDriveStrategyCourse.lua index 6012b541d..3a8c7ede2 100644 --- a/scripts/ai/AIDriveStrategyCourse.lua +++ b/scripts/ai/AIDriveStrategyCourse.lua @@ -111,6 +111,9 @@ function AIDriveStrategyCourse:setAIVehicle(vehicle, jobParameters) self:initializeImplementControllers(vehicle) self.ppc = PurePursuitController(vehicle) self.ppc:registerListeners(self, 'onWaypointPassed', 'onWaypointChange') + + self.pathfinderController = PathfinderController(vehicle) + self.storage = vehicle.spec_cpAIWorker self.settings = vehicle:getCpSettings() @@ -398,8 +401,9 @@ function AIDriveStrategyCourse:getCurrentCourse() return self.ppc:getCourse() or self.course end -function AIDriveStrategyCourse:update() +function AIDriveStrategyCourse:update(dt) self.ppc:update() + self.pathfinderController:update(dt) self:updatePathfinding() self:updateInfoTexts() end diff --git a/scripts/ai/PathfinderController.lua b/scripts/ai/PathfinderController.lua new file mode 100644 index 000000000..a85bed4e5 --- /dev/null +++ b/scripts/ai/PathfinderController.lua @@ -0,0 +1,364 @@ +--[[ +This file is part of Courseplay (https://github.com/Courseplay/Courseplay_FS22) +Copyright (C) 2023 Courseplay Dev Team +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 . +]] + +--[[ +PathfinderController for easy access to the pathfinder. +- Enables retrying with adjustable parameters compared to the last try, like fruit allowed and so.. +- Handles the pathfinder coroutines if needed. +- One callback when the path finding finished. + - Triggered if a valid path was found. + - Gets triggered if the goal node is invalid. + - Also gets triggered if no valid path was found and all retry attempts are used. +- Every time the path finding failed a callback gets triggered, if there are retry attempts left over. + - Enabled the changing of the pathfinder context and restart with the new context. + +PathfinderControllerContext implements all pathfinder parameters and the number of retries allowed. +- Other Context Classes could be derived for commonly used contexts. + + + +Example implementations: + +function Strategy:startPathfindingToGoal() + local numRetries = 2 + local context = PathfinderControllerContext(self.vehicle, numRetries) + context:set( + ... + ) + self.pathfinderController:setCallbacks(self, self.onPathfingFinished, self.onPathfindingFailed) + + self.pathfinderController:findPathToNode(context, ...) + +end + +function Strategy:onPathfingFinished(controller : PathfinderController, success : boolean, + path : table, goalNodeInvalid : boolean|nil) + if success then + // Path finding finished successfully + local course = self.pathfinderController:getTemporaryCourseFromPath(path) + ... + else + if goalNodeInvalid then + // Goal position can't be reached! + else + // Num retries reached without path! + end + end +end + +function Strategy:onPathfindingFailed(controller : PathfinderController, lastContext : PathfinderControllerContext, + wasLastRetry : boolean, currentRetryAttempt : number) + if currentRetryAttempt == 1 then + // Reduced fruit impact: + lastContext:ignoreFruit() + self.pathfinderController:findPathToNode(lastContext, ...) + else + // Something else ... + self.pathfinderController:findPathToNode(lastContext, ...) + end +end + +]] + +---@class PathfinderControllerContext +PathfinderControllerContext = CpObject() +PathfinderControllerContext.defaultNumRetries = 0 +function PathfinderControllerContext:init(vehicle, numRetries) + self.vehicle = vehicle + self.numRetries = numRetries or self.defaultNumRetries + --- Percentage of fruit allowed, math.hug means no fruit avoidance + self.maxFruitPercent = nil + --- Penalty sticking on the field and avoiding outside of the field + self.offFieldPenalty = nil + --- Should none owned field be avoid? + self.useFieldNum = false + --- A given area that has to be avoided. + self.areaToAvoid = nil + --- Is reverse driving allowed? + self.allowReverse = false + --- Vehicle Collisions that are ignored. + self.vehiclesToIgnore = nil + --- Is a accurate pathfinder goal position needed? + self.mustBeAccurate = false + self.areaToIgnoreFruit = nil +end + +--- Sets the Pathfinder context +---@param mustBeAccurate boolean|nil +---@param allowReverse boolean|nil +---@param maxFruitPercent number|nil +---@param offFieldPenalty number|nil +---@param useFieldNum boolean|nil +---@param areaToAvoid table|nil +---@param vehiclesToIgnore table|nil +---@param areaToIgnoreFruit table|nil +function PathfinderControllerContext:set( + mustBeAccurate, allowReverse, + maxFruitPercent, offFieldPenalty, useFieldNum, + areaToAvoid, vehiclesToIgnore, areaToIgnoreFruit) + self.maxFruitPercent = maxFruitPercent + self.offFieldPenalty = offFieldPenalty + self.useFieldNum = useFieldNum + self.areaToAvoid = areaToAvoid + self.allowReverse = allowReverse + self.vehiclesToIgnore = vehiclesToIgnore + self.mustBeAccurate = mustBeAccurate + self.areaToIgnoreFruit = areaToIgnoreFruit +end + +--- Disables the fruit avoidance +function PathfinderControllerContext:ignoreFruit() + self.maxFruitPercent = math.huge +end + +function PathfinderControllerContext:getNumRetriesAllowed() + return self.numRetries +end + +function PathfinderControllerContext:__tostring() + local str = "PathfinderControllerContext(vehicle name=%s, maxFruitPercent=%s, "+ + "offFieldPenalty=%s, useFieldNum=%s, areaToAvoid=%s, allowReverse=%s, "+ + "vehiclesToIgnore=%s, mustBeAccurate=%s, areaToIgnoreFruit=%s)" + return string.format(str, + CpUtil.getName(self.vehicle), + tostring(self.maxFruitPercent), + tostring(self.offFieldPenalty), + tostring(self.useFieldNum), + tostring(self.areaToAvoid), + tostring(self.allowReverse), + tostring(self.vehiclesToIgnore), + tostring(self.mustBeAccurate), + tostring(self.areaToIgnoreFruit)) +end + +---@class DefaultFieldPathfinderControllerContext : PathfinderControllerContext +DefaultFieldPathfinderControllerContext = CpObject(PathfinderControllerContext) + +function DefaultFieldPathfinderControllerContext:init(...) + PathfinderControllerContext.init(self, ...) +end + +---@class PathfinderController +PathfinderController= CpObject() + +PathfinderController.SUCCESS_FOUND_VALID_PATH = 0 +PathfinderController.ERROR_NO_PATH_FOUND = 1 +PathfinderController.ERROR_INVALID_GOAL_NODE = 2 +function PathfinderController:init(vehicle) + self.vehicle = vehicle + ---@type PathfinderInterface + self.pathfinder = nil + ---@type PathfinderControllerContext + self.lastContext = nil + self:reset() +end + +function PathfinderController:__tostring() + return string.format("PathfinderController(failCount=%d, numRetries=%s, active=%s)", + self.failCount, self.numRetries, tostring(self.pathfinder ~= nil)) +end + +function PathfinderController:reset() + self.numRetries = 0 + self.failCount = 0 + self.startedAt = 0 + self.timeTakenMs = 0 + self.callbackClass = nil + self.callbackSuccessFunction = nil + self.callbackRetryFunction = nil + self.lastContext = nil +end + +function PathfinderController:update(dt) + if self:isActive() then + --- Applies coroutine for path finding + local done, path, goalNodeInvalid = self.pathfinder:resume() + if done then + self:finished(path, goalNodeInvalid) + end + end +end + +function PathfinderController:isActive() + return self.pathfinder and self.pathfinder:isActive() +end + +---@return PathfinderControllerContext +function PathfinderController:getLastContext() + return self.lastContext +end + +--- Sets the callbacks which get called on the pathfinder finish. +---@param class table +---@param successFunc function func(PathfinderController, success, path, goalNodeInvalid) +---@param retryFunc function func(PathfinderController, last context, was last retry, retry attempt number) +function PathfinderController:setCallbacks(class, successFunc, retryFunc) + self.callbackClass = class + self.callbackSuccessFunction = successFunc + self.callbackRetryFunction = retryFunc +end + +--- Pathfinder was started +---@param context PathfinderControllerContext +function PathfinderController:started(context) + self:debug("Started pathfinding with context: %s.", tostring(context)) + self.startedAt = g_time + self.lastContext = context + self.numRetries = context:getNumRetriesAllowed() +end + +function PathfinderController:callCallback(callbackFunc, ...) + if self.callbackClass then + callbackFunc(self.callbackClass, self, ...) + else + callbackFunc(self, ...) + end +end + +--- Path finding has finished +---@param path table|nil +---@param goalNodeInvalid boolean|nil +function PathfinderController:finished(path, goalNodeInvalid) + self.pathfinder = nil + self.timeTakenMs = g_time - self.startedAt + local retValue = self:isValidPath(path, goalNodeInvalid) + if retValue == self.ERROR_NO_PATH_FOUND then + if self.callbackRetryFunction then + --- Retry is allowed, so check if any tries are leftover + if self.failCount < self.numRetries then + self:debug("Failed with try %d of %d.", self.failCount, self.numRetries) + --- Retrying the path finding + self.failCount = self.failCount + 1 + self:callCallback(self.callbackRetryFunction, self.lastContext, self.failCount == self.numRetries, self.failCount) + return + end + end + end + self:callCallback(self.callbackSuccessFunction, retValue == self.SUCCESS_FOUND_VALID_PATH, path, goalNodeInvalid) + self:reset() +end + +--- Is the path found and valid? +---@param path table|nil +---@param goalNodeInvalid boolean|nil +---@return integer +function PathfinderController:isValidPath(path, goalNodeInvalid) + if path and #path > 2 then + self:debug('Found a path (%d waypoints, after %d ms)', #path, self.timeTakenMs) + return self.SUCCESS_FOUND_VALID_PATH + end + if goalNodeInvalid then + self:error('No path found, goal node is invalid') + return self.ERROR_INVALID_GOAL_NODE + end + self:error("No path found after %d ms", self.timeTakenMs) + return self.ERROR_NO_PATH_FOUND +end + +--- Finds a path to given goal node +---@param context PathfinderControllerContext +---@param goalNode number +---@param xOffset number +---@param zOffset number +---@return boolean Was path finding started? +function PathfinderController:findPathToNode(context, + goalNode, xOffset, zOffset) + + if not self.callbackSuccessFunction then + self:error("No valid success callback was given!") + return false + end + self:started(context) + local pathfinder, done, path, goalNodeInvalid = PathfinderUtil.startPathfindingFromVehicleToNode( + context.vehicle, + goalNode, + xOffset, + zOffset, + context.allowReverse, + context.useFieldNum and 1, + context.vehiclesToIgnore, + context.maxFruitPercent, + context.offFieldPenalty, + context.areaToAvoid, + context.mustBeAccurate + ) + if done then + self:finished(path, goalNodeInvalid) + else + self:debug("Continuing as coroutine...") + self.pathfinder = pathfinder + end + return true +end + +--- Finds a path to a waypoint of a course. +---@param context PathfinderControllerContext +---@param course Course +---@param waypointIndex number +---@param xOffset number +---@param zOffset number +---@return boolean Was path finding started? +function PathfinderController:findPathToWaypoint(context, + course, waypointIndex, xOffset, zOffset) + + if not self.callbackSuccessFunction then + self:error("No valid success callback was given!") + return false + end + self:started(context) + local pathfinder, done, path, goalNodeInvalid = PathfinderUtil.startPathfindingFromVehicleToWaypoint( + context.vehicle, + course, + waypointIndex, + xOffset, + zOffset, + context.allowReverse, + context.useFieldNum and CpFieldUtil.getFieldNumUnderVehicle(context.vehicle), + context.vehiclesToIgnore, + context.maxFruitPercent, + context.offFieldPenalty, + context.areaToAvoid, + context.areaToIgnoreFruit) + if done then + self:finished(path, goalNodeInvalid) + else + self:debug("Continuing as coroutine...") + self.pathfinder = pathfinder + end +end + +function PathfinderController:getTemporaryCourseFromPath(path) + return Course(self.vehicle, CourseGenerator.pointsToXzInPlace(path), true) +end + +-------------------------------------------- +--- Debug functions +-------------------------------------------- + +function PathfinderController:debugStr(str, ...) + return "Pathfinder controller: " .. str, ... +end + +function PathfinderController:debug(...) + CpUtil.debugVehicle(CpDebug.DBG_PATHFINDER, self.vehicle, self:debugStr(...)) +end + +function PathfinderController:info(...) + CpUtil.infoVehicle(self.vehicle, self:debugStr(...)) +end + +function PathfinderController:error(...) + self:info(...) +end \ No newline at end of file