diff --git a/.github/workflows/unit-test.yml b/.github/workflows/unit-test.yml index 012636f3d..767048df2 100644 --- a/.github/workflows/unit-test.yml +++ b/.github/workflows/unit-test.yml @@ -35,7 +35,7 @@ jobs: lua LoggerTest.lua - name: Run course generator unit tests run: | - cd scripts/courseGenerator/test + pushd scripts/courseGenerator/test lua BlockSequencerTest.lua lua CacheMapTest.lua lua CenterTest.lua @@ -53,3 +53,6 @@ jobs: lua TransformTest.lua lua VertexTest.lua lua WrapAroundIndexTest.lua + popd + pushd scripts/pathfinder/test + lua GraphPathfinderTest.lua \ No newline at end of file diff --git a/config/MasterTranslations.xml b/config/MasterTranslations.xml index 0e30805f2..1623c0bea 100644 --- a/config/MasterTranslations.xml +++ b/config/MasterTranslations.xml @@ -547,6 +547,22 @@ + + + + + + + + + + + + + + + + diff --git a/config/VehicleSettingsSetup.xml b/config/VehicleSettingsSetup.xml index 24eb803ca..ddf27c551 100644 --- a/config/VehicleSettingsSetup.xml +++ b/config/VehicleSettingsSetup.xml @@ -33,6 +33,8 @@ + + diff --git a/modDesc.xml b/modDesc.xml index ad0b3bcf3..5e5911af9 100644 --- a/modDesc.xml +++ b/modDesc.xml @@ -164,6 +164,7 @@ Changelog 8.0.0.0: + diff --git a/scripts/geometry/Polyline.lua b/scripts/geometry/Polyline.lua index 0292892d5..e04bf6f10 100644 --- a/scripts/geometry/Polyline.lua +++ b/scripts/geometry/Polyline.lua @@ -46,8 +46,12 @@ function Polyline:prepend(v) end end +function Polyline:_getNewInstance() + return Polyline() +end + function Polyline:clone() - local clone = Polyline({}) + local clone = self:_getNewInstance() for _, v in ipairs(self) do clone:append(v:clone()) end @@ -460,7 +464,9 @@ end --- as a turn waypoint. ---@param r number turning radius ---@param makeCorners boolean if true, make corners for turn maneuvers instead of rounding them. -function Polyline:ensureMinimumRadius(r, makeCorners) +---@param maxCrossTrackError number|nil maximum cross track error allowed before we start adjusting corners, +---defaults to CourseGenerator.cMaxCrossTrackError +function Polyline:ensureMinimumRadius(r, makeCorners, maxCrossTrackError) ---@param entry CourseGenerator.Slider ---@param exit CourseGenerator.Slider @@ -488,8 +494,9 @@ function Polyline:ensureMinimumRadius(r, makeCorners) nextIx = currentIx + 1 local xte = self:at(currentIx):getXte(r) local radius = self:at(currentIx):getRadius() - if xte > CourseGenerator.cMaxCrossTrackError then - self.logger:debug('ensureMinimumRadius (%s): found a corner at %d with r: %.1f, r: %.1f, xte: %.1f', debugId, currentIx, radius, r, xte) + if xte > (maxCrossTrackError or CourseGenerator.cMaxCrossTrackError) then + self.logger:debug('ensureMinimumRadius (%s): found a corner at %d with r: %.1f, r: %.1f, xte: %.1f', + debugId, currentIx, radius, r, xte) -- looks like we can't make this turn without deviating too much from the course, local entry = CourseGenerator.Slider(self, currentIx, 0) local exit = CourseGenerator.Slider(self, currentIx, 0) @@ -864,7 +871,7 @@ end function Polyline:__tostring() local result = '' for i, v in ipairs(self) do - result = result .. string.format('%d %s\n', i, v) + result = result .. string.format('%d %s %s\n', i, v, v.xte) end return result end \ No newline at end of file diff --git a/scripts/pathfinder/AStar.lua b/scripts/pathfinder/AStar.lua index 4a740b39f..db63c7318 100644 --- a/scripts/pathfinder/AStar.lua +++ b/scripts/pathfinder/AStar.lua @@ -6,12 +6,13 @@ AStar = CpObject(HybridAStar) function AStar:init(vehicle, yieldAfter, maxIterations) HybridAStar.init(self, vehicle, yieldAfter, maxIterations) + self.name = 'AStar' -- this needs to be small enough that no vehicle fit between the grid points (and remain undetected) self.deltaPos = 3 self.deltaPosGoal = self.deltaPos self.deltaThetaDeg = 181 self.deltaThetaGoal = math.rad(self.deltaThetaDeg) - self.maxDeltaTheta = math.pi + self.maxDeltaTheta = self.deltaThetaGoal self.originalDeltaThetaGoal = self.deltaThetaGoal self.analyticSolverEnabled = false self.ignoreValidityAtStart = false @@ -31,14 +32,14 @@ function AStar.SimpleMotionPrimitives:init(gridSize, allowReverse) self.gridSize = gridSize local d = gridSize local dSqrt2 = math.sqrt(2) * d - table.insert(self.primitives, { dx = d, dy = 0, dt = 0, d = d, gear = Gear.Forward, steer = Steer.Straight, type = HybridAStar.MotionPrimitiveTypes.NA }) - table.insert(self.primitives, { dx = d, dy = d, dt = 1 * math.pi / 4, d = dSqrt2, gear = Gear.Forward, steer = Steer.Straight, type = HybridAStar.MotionPrimitiveTypes.NA }) - table.insert(self.primitives, { dx = 0, dy = d, dt = 2 * math.pi / 4, d = d, gear = Gear.Forward, steer = Steer.Straight, type = HybridAStar.MotionPrimitiveTypes.NA }) - table.insert(self.primitives, { dx = -d, dy = d, dt = 3 * math.pi / 4, d = dSqrt2, gear = Gear.Forward, steer = Steer.Straight, type = HybridAStar.MotionPrimitiveTypes.NA }) - table.insert(self.primitives, { dx = -d, dy = 0, dt = 4 * math.pi / 4, d = d, gear = Gear.Forward, steer = Steer.Straight, type = HybridAStar.MotionPrimitiveTypes.NA }) - table.insert(self.primitives, { dx = -d, dy = -d, dt = 5 * math.pi / 4, d = dSqrt2, gear = Gear.Forward, steer = Steer.Straight, type = HybridAStar.MotionPrimitiveTypes.NA }) - table.insert(self.primitives, { dx = 0, dy = -d, dt = 6 * math.pi / 4, d = d, gear = Gear.Forward, steer = Steer.Straight, type = HybridAStar.MotionPrimitiveTypes.NA }) - table.insert(self.primitives, { dx = d, dy = -d, dt = 7 * math.pi / 4, d = dSqrt2, gear = Gear.Forward, steer = Steer.Straight, type = HybridAStar.MotionPrimitiveTypes.NA }) + table.insert(self.primitives, { dx = d, dy = 0, dt = 0, d = d, gear = Gear.Forward, steer = Steer.Straight}) + table.insert(self.primitives, { dx = d, dy = d, dt = 1 * math.pi / 4, d = dSqrt2, gear = Gear.Forward, steer = Steer.Straight}) + table.insert(self.primitives, { dx = 0, dy = d, dt = 2 * math.pi / 4, d = d, gear = Gear.Forward, steer = Steer.Straight}) + table.insert(self.primitives, { dx = -d, dy = d, dt = 3 * math.pi / 4, d = dSqrt2, gear = Gear.Forward, steer = Steer.Straight}) + table.insert(self.primitives, { dx = -d, dy = 0, dt = 4 * math.pi / 4, d = d, gear = Gear.Forward, steer = Steer.Straight}) + table.insert(self.primitives, { dx = -d, dy = -d, dt = 5 * math.pi / 4, d = dSqrt2, gear = Gear.Forward, steer = Steer.Straight}) + table.insert(self.primitives, { dx = 0, dy = -d, dt = 6 * math.pi / 4, d = d, gear = Gear.Forward, steer = Steer.Straight}) + table.insert(self.primitives, { dx = d, dy = -d, dt = 7 * math.pi / 4, d = dSqrt2, gear = Gear.Forward, steer = Steer.Straight}) end --- A* successors are simply the grid neighbors diff --git a/scripts/pathfinder/GraphPathfinder.lua b/scripts/pathfinder/GraphPathfinder.lua new file mode 100644 index 000000000..f620153d7 --- /dev/null +++ b/scripts/pathfinder/GraphPathfinder.lua @@ -0,0 +1,398 @@ +--- A pathfinder based on A* to find the shortest path in a directed graph. +--- The graph is defined by its edges only, there are no nodes needed. +--- +--- Edges are represented by polylines, and they can unidirectional or bidirectional. +--- Unidirectional edges can only be entered at one end (at the first vertex of the +--- polyline) and exited at the other (last vertex of the polyline) +--- +--- Bidirectional edges can be entered at either end, and exited at the other. +--- +--- Edges don't have to be connected, as long as the entry of another edge is close +--- enough to the exit of another, the entry is a valid successor node (of the exit, +--- which is the predecessor) + +---@class GraphPathfinder : HybridAStar +GraphPathfinder = CpObject(HybridAStar) + +--- An edge of a directed graph +---@class GraphPathfinder.GraphEdge : Polyline +GraphPathfinder.GraphEdge = CpObject(Polyline) + +GraphPathfinder.GraphEdge.UNIDIRECTIONAL = 1 +GraphPathfinder.GraphEdge.BIDIRECTIONAL = 2 + +---@param direction table GraphEdge.UNIDIRECTIONAL or GraphEdge.BIDIRECTIONAL +---@param vertices table[] array of tables with x, y (Vector, Vertex, State3D or just plain {x, y} +function GraphPathfinder.GraphEdge:init(direction, vertices) + Polyline.init(self, vertices) + self.direction = direction +end + +-- allow to inherit clone +function GraphPathfinder.GraphEdge:_getNewInstance() + return GraphPathfinder.GraphEdge() +end + +function GraphPathfinder.GraphEdge:clone() + local clone = Polyline.clone(self) + clone.direction = self.direction + return clone +end + +function GraphPathfinder.GraphEdge:getDirection() + return self.direction +end + +---@return boolean is this a bidirectional edge? +function GraphPathfinder.GraphEdge:isBidirectional() + return self.direction == GraphPathfinder.GraphEdge.BIDIRECTIONAL +end + +---@return Vertex[] array of vertices that can be used to enter this edge (one for +--- unidirectional, two for bidirectional edges) +function GraphPathfinder.GraphEdge:getEntries() + if self:isBidirectional() then + return { self[1], self[#self] } + else + return { self[1] } + end +end + +---@param entry Vector +---@return Vector the exit when entered through the given entry +function GraphPathfinder.GraphEdge:getExit(entry) + if entry == self[1] then + return self[#self] + else + return self[1] + end +end + +function GraphPathfinder.GraphEdge:rollUpIterator(entry) + local from, to, step + if entry == self[1] then + -- unidirectional, or bidirectional, travelling from the start to end, roll up backwards + from, to, step = #self + 1, 1, -1 + else + from, to, step = 0, #self, 1 + end + local i = from + return function() + i = i + step + if i == to + step then + return nil, nil + else + return i, self[i] + end + end +end + +function GraphPathfinder.GraphEdge.getDirectionFromGameDirection(gameDirection) + -- for forward compatibility until we get GraphSegment merged + if not GraphSegmentDirection then + GraphSegmentDirection = {FORWARD = 1, REVERSE = 2} + end + if gameDirection == GraphSegmentDirection.FORWARD or gameDirection == GraphSegmentDirection.REVERSE then + return GraphPathfinder.GraphEdge.UNIDIRECTIONAL + else + return GraphPathfinder.GraphEdge.BIDIRECTIONAL + end +end + +GraphPathfinder.HelperGraphEdge = CpObject(GraphPathfinder.GraphEdge) +--- Helper edges are to entry/exit the graph from the goal/start. We don't want these to be part of the path, so we +--- the roll up iterator returns nothing, automatically skip the vertices of these edges +function GraphPathfinder.HelperGraphEdge:rollUpIterator(entry) + return function() + end +end + +--- A pathfinder node, specialized for the GraphPathfinder +---@class GraphPathfinder.Node : State3D +GraphPathfinder.Node = CpObject(State3D) + +---@param edge GraphPathfinder.GraphEdge the edge leading to this node: when rolling up the path, we need to add all +--- vertices of the edge +---@param entry Vector the entry point to this edge (when bidirectional, we may be travelling the edge from the end to +--- the start. +function GraphPathfinder.Node:init(x, y, g, pred, d, edge, entry) + State3D.init(self, x, y, 0, g, pred, Gear.Forward, Steer.Straight, 0, d) + self.edge = edge + self.entry = entry +end + +--- Create a graph pathfinder. +--- +--- Use start(start, goal) as described at PathfinderInterface:start() to run the pathfinder. +--- The entry at the graph will be at the vertex closest to the start location, the exit at the vertex closest +--- to the goal location. (The graph's edges are polylines, consisting of vertices). There is no limitation for the +--- distance between the entry/exit vertices and the start/goal locations. +--- +--- The resulting path will only contain vertices of the edges that are traversed from the entry to the exit, it +--- will not contain the start or the goal. The caller is responsible for creating the sections from the start to the +--- entry (first point of the path) and from the exit (last point of the path) to the goal. +--- +---@param yieldAfter number coroutine yield after so many iterations (number of iterations in one update loop) +---@param maxIterations number maximum iterations before failing +---@param range number when an edge's exit is closer than range to another edge's entry, the +--- two edges are considered as connected (and thus can traverse from one to the other) +---@param graph GraphPathfinder.GraphEdge[] Array of edges, the graph as described in the file header +function GraphPathfinder:init(yieldAfter, maxIterations, range, graph) + HybridAStar.init(self, { }, yieldAfter, maxIterations) + self.logger = Logger('GraphPathfinder', Logger.level.debug, CpDebug.DBG_PATHFINDER) + self.range = range + self.transitionRange = range + self.originalGraph = graph + self.deltaPosGoal = self.range + self.deltaThetaDeg = 181 + self.deltaThetaGoal = math.rad(self.deltaThetaDeg) + self.maxDeltaTheta = self.deltaThetaGoal + self.originalDeltaThetaGoal = self.deltaThetaGoal + self.analyticSolverEnabled = false + self.ignoreValidityAtStart = false +end + +--- for backwards compatibility with the old pathfinder +function GraphPathfinder:debug(...) + self.logger:debug(...) +end + +function GraphPathfinder:getMotionPrimitives(turnRadius, allowReverse) + return GraphPathfinder.GraphMotionPrimitives(self.range, self.graph) +end + +--- Override path roll up since here, the path also includes all edges of the graph, not just the pathfinder nodes +---@param lastNode GraphPathfinder.Node +function GraphPathfinder:rollUpPath(lastNode, goal) + local edges = {} + local currentNode = lastNode + self:debug('Goal node at %.2f/%.2f, cost %.1f (%.1f - %.1f)', goal.x, goal.y, lastNode.cost, + self.nodes.lowestCost, self.nodes.highestCost) + while currentNode.pred and currentNode ~= currentNode.pred do + -- add the edge leading to the node + local edge = Polyline() + for _, node in currentNode.edge:rollUpIterator(currentNode.entry) do + if node ~= edge[1] then + -- don't insert the same node twice (we'll have the same node twice when we split edges) + edge:prepend(node) + end + end + if #edge > 0 then + edge:calculateProperties() + table.insert(edges, 1, edge) + end + currentNode = currentNode.pred + end + local path = self:addTransitions(edges) + self:debug('Nodes %d, iterations %d, yields %d, deltaTheta %.1f', #path, self.iterations, self.yields, + math.deg(self.deltaThetaGoal)) + return path +end + +function GraphPathfinder:start(start, goal, turnRadius, ...) + -- at each run, make a copy of the graph as we'll modify it + self.graph = {} + for _, e in ipairs(self.originalGraph) do + if #e > 1 then + table.insert(self.graph, e:clone()) + elseif #e == 1 then + -- edges with one vertex put the pathfinder in an endless loop, so we skip them + self.logger:error('Skipping edge starting at x = %.1f, z = %.1f with a single vertex', e[1].x, -e[1].y) + else + -- this should not happen, an edge without vertices, but just in case, we skip it + self.logger:error('Skipping edge with no vertices.') + end + end + local graphEntry, graphExit = self:createGraphEntryAndExit(start, goal) + local distance = (graphExit - graphEntry):length() + if distance <= self.range then + -- if the distance between the entry and exit is less than the range, we can just return the entry as the exit + self.logger:error('Graph entry and exit are closer than %.1f meters (%.1f), no point in running the pathfinder.', + self.range, distance) + self.goalNodeInvalid = true + return self:finishRun(true, nil) + end + return HybridAStar.start(self, start, goal, turnRadius, ...) +end + +--- Create the entry and exit edges for the graph. +--- +--- The problem we are trying to solve here, is that the start and goal can be in any distance from any edge of the +--- graph, like when the vehicle sits in the middle of a field, surrounded by streets. Any street could be used as +--- an entry, and the closest street may not be the shortest path to the goal. A special case of this is when +--- the street is a two lane, two way street and our goal is to the left, but the closest edge is the right lane, +--- leading away from the goal, forcing us making an unnecessary detour. +--- +--- Therefore, it isn't enough to find the closest vertex of the graph, we need a list of the closest vertices and +--- as long as their distance is not bigger than the distance to the closest one + the range, we can use them as +--- entries/exits. +--- +--- To make sure that the algorithm actually uses these entry/exit points, we add helper edges to the graph, leading +--- from the start to the entries and from the exits to the goal. +--- +--- When the closest vertex, isn't the first or last point of the edge, we simply split that edge at that vertex so +--- the parts can be used as entries/exits. +--- +---@param start State3D the start location for the pathfinder +---@param goal State3D the goal location for the pathfinder +---@return State3D the entry vertex of the graph, closest to start +---@return State3D the exit vertex of the graph, closest to goal +function GraphPathfinder:createGraphEntryAndExit(start, goal) + + local function splitEdgeWhenNeeded(edge, closestVertex) + -- if the vertex is the first or last vertex of the edge, we can use it directly as the entry/exit, + -- otherwise, we split the edge at the vertex so we can use it as an entry/exit point. + if closestVertex.ix ~= 1 and closestVertex.ix ~= #edge then + self.logger:debug('Graph entry/exit found and split at vertex %d, x: %.1f y: %.1f', + closestVertex.ix, closestVertex.x, closestVertex.y) + local newEdge = GraphPathfinder.GraphEdge(edge:getDirection()) + for j = closestVertex.ix, #edge do + newEdge:append(edge[j]) + end + newEdge:calculateProperties() + table.insert(self.graph, newEdge) + edge:cutEndAtIx(closestVertex.ix) + return newEdge + end + end + + -- find the edges closest to node. If the closest vertex isn't the first or last vertex of the edge, we split the + -- edge at that vertex so we can use it as an entry/exit point. + local function findClosestEdges(node, isEntry) + local closestEdges = {} + + local function addToClosestEdge(edge, vertex, d) + -- only add the edge if the vertex can be used as an entry/exit point + if edge:isBidirectional() or + (isEntry and vertex.ix == 1) or + (not isEntry and vertex.ix == #edge) then + table.insert(closestEdges, { d = d, edge = edge, vertex = vertex }) + end + end + + -- we'll be adding items to self.graph from within the loop, but that should be ok, because the # is evaluated + -- before the loop starts + for i = 1, #self.graph do + local edge = self.graph[i] + local vertex, d = edge:findClosestVertexToPoint(node) + local newEdge = splitEdgeWhenNeeded(edge, vertex) + if newEdge then + addToClosestEdge(newEdge, newEdge[1], d) + end + addToClosestEdge(edge, vertex, d) + end + table.sort(closestEdges, function(a, b) + return a.d < b.d + end) + return closestEdges + end + + local entryEdges, exitEdges = findClosestEdges(start, true), findClosestEdges(goal, false) + -- only use the edge if it is close enough to the closest + local maxDistance = entryEdges[1].d + self.range + for i = 1, math.min(#entryEdges, 2) do + if entryEdges[i].d <= maxDistance then + table.insert(self.graph, GraphPathfinder.HelperGraphEdge(GraphPathfinder.UNIDIRECTIONAL, { start, entryEdges[i].vertex })) + end + end + maxDistance = exitEdges[1].d + self.range + for i = 1, math.min(#exitEdges, 2) do + if exitEdges[i].d <= maxDistance then + table.insert(self.graph, GraphPathfinder.HelperGraphEdge(GraphPathfinder.UNIDIRECTIONAL, { exitEdges[i].vertex, goal })) + end + end + return State3D(entryEdges[1].vertex.x, entryEdges[1].vertex.y, 0, 0), + State3D(exitEdges[1].vertex.x, exitEdges[1].vertex.y, 0, 0) +end + +--- Add the transitions between the edges in the graph. These are the road crossings where the path +--- between two edges is missing: they are either too far apart or too close and not connected with a proper +--- arc of the vehicle's turning radius. +---@param edges GraphPathfinder.GraphEdge[] +function GraphPathfinder:addTransitions(edges) + local path = Polyline() + for i = 1, #edges - 1 do + local lastVertex = edges[i][#edges[i]] + local exitEdge = lastVertex:getEntryEdge() + local firstVertex = edges[i + 1][1] + local entryEdge = firstVertex:getExitEdge() + CourseGenerator.LineSegment.connect(exitEdge, entryEdge, 1, true) + local corner = entryEdge:getBase() + if (corner - lastVertex):length() < self.transitionRange and + (corner - firstVertex):length() < self.transitionRange then + edges[i + 1][1] = Vertex.fromVector(entryEdge:getBase()) + table.remove(edges[i]) + end + path:appendMany(edges[i]) + end + path:appendMany(edges[#edges]) -- append the last edge, this will be the exit edge + path:calculateProperties() + path:splitEdges(5) + path:ensureMinimumRadius(self.turnRadius, false, 0.5) + return path +end + +--- Motion primitives to use with the graph pathfinder, providing the entries +--- to the next edges. +---@class GraphPathfinder.GraphMotionPrimitives : HybridAStar.MotionPrimitives +GraphPathfinder.GraphMotionPrimitives = CpObject(HybridAStar.MotionPrimitives) + +---@param range number when an edge's exit is closer than range to another edge's entry, the +--- two edges are considered as connected (and thus can traverse from one to the other) +---@param graph Vector[] the graph as described in the file header +function GraphPathfinder.GraphMotionPrimitives:init(range, graph) + self.logger = Logger('GraphMotionPrimitives', Logger.level.trace, CpDebug.DBG_PATHFINDER) + self.range = range + self.graph = graph +end + +---@return table [{x, y, d}] array of the next possible entries, their coordinates and +--- the distance to the entry + the length of the edge +function GraphPathfinder.GraphMotionPrimitives:getPrimitives(node) + local primitives = {} + self.logger:trace('\tpredecessor: %.1f %.1f (%.1f)', node.x, node.y, node.g) + for _, edge in ipairs(self.graph) do + local entries = edge:getEntries() + for _, entry in ipairs(entries) do + local distanceToEntry = (node - entry):length() + if distanceToEntry <= self.range then + local exit = edge:getExit(entry) + table.insert(primitives, { x = exit.x, y = exit.y, d = edge:getLength() + distanceToEntry, + edge = edge, entry = entry }) + self.logger:trace('\t primitives: %.1f %.1f', exit.x, exit.y) + end + end + end + return primitives +end + +---@return State3D successor for the given primitive +function GraphPathfinder.GraphMotionPrimitives:createSuccessor(node, primitive, hitchLength) + self.logger:trace('\t\tsuccessor: %.1f %.1f (d=%.1f) from node: %.1f %.1f (g=%.1f, d=%.1f)', + primitive.x, primitive.y, primitive.d, node.x, node.y, node.g, node.d) + return GraphPathfinder.Node(primitive.x, primitive.y, node.g, node, node.d + primitive.d, primitive.edge, primitive.entry) +end + +--- Load a graph from Courseplay.xml, for tests only. +function GraphPathfinder.loadGraphEdgesFromXml(fileName) + local graph = {} + local edge + for line in io.lines(fileName) do + local direction = string.match(line, '') then + -- segment ends + edge:calculateProperties() + table.insert(graph, edge) -- add the edge to the graph + Logger():info('Loaded edge %d direction: %s, length: %.1f', #graph, tostring(edge:getDirection()), edge:getLength()) + end + local x, z = string.match(line, ' 0 and self.iterations < self.maxIterations do + -- yield after the configured iterations or after 20 ms + self.iterationsSinceYield = self.iterationsSinceYield + 1 + if (self.iterationsSinceYield % self.yieldAfter == 0 or readIntervalTimerMs(self.timer) > 20) then + self.yields = self.yields + 1 + closeIntervalTimer(self.timer) + -- if we had the coroutine package, we would coursePlayCoroutine.yield(false) here + return PathfinderResult(false) + end -- pop lowest cost node from queue ---@type State3D local pred = State3D.pop(self.openList) - --self:debug('pop %s', tostring(pred)) + self.logger:trace('pop %s', tostring(pred)) + self.logger:trace('dPos %.1f dTheta %.1f', self.deltaPosGoal, math.deg(self.deltaThetaGoal)) if pred:equals(self.goal, self.deltaPosGoal, self.deltaThetaGoal) then -- done! self:debug('Popped the goal (%d).', self.iterations) return self:finishRun(true, self:rollUpPath(pred, self.goal)) end - self.count = self.count + 1 - -- yield after the configured iterations or after 20 ms - if (self.count % self.yieldAfter == 0 or readIntervalTimerMs(self.timer) > 20) then - self.yields = self.yields + 1 - closeIntervalTimer(self.timer) - -- if we had the coroutine package, we would coursePlayCoroutine.yield(false) here - return false - end + if not pred:isClosed() then -- analytical expansion: try a Dubins/Reeds-Shepp path from here randomly, more often as we getting closer to the goal -- also, try it before we start with the pathfinding if pred.h then - if self.analyticSolverEnabled and not self.goalNodeIsInvalid and + if self.analyticSolverEnabled and not self.goalNodeInvalid and math.random() > 2 * pred.h / self.distanceToGoal then self:debug('Check analytic solution at iteration %d, %.1f, %.1f', self.iterations, pred.h, pred.h / self.distanceToGoal) local analyticPath, _, pathType = self:getAnalyticPath(pred, self.goal, self.turnRadius, self.allowReverse, self.hitchLength) @@ -612,18 +659,19 @@ function HybridAStar:run(start, goal, turnRadius, allowReverse, constraints, hit self:debug('Found collision free analytic path (%s) at iteration %d', pathType, self.iterations) -- remove first node of returned analytic path as it is the same as pred table.remove(analyticPath, 1) - -- TODO why are we calling rollUpPath here? + -- roll up the path from the start of the analytic path back to start return self:finishRun(true, self:rollUpPath(pred, self.goal, analyticPath)) end end end -- create the successor nodes - for _, primitive in ipairs(self.hybridMotionPrimitives:getPrimitives(pred)) do + for _, primitive in ipairs(self.motionPrimitives:getPrimitives(pred)) do ---@type State3D - local succ = self.hybridMotionPrimitives:createSuccessor(pred, primitive, self.hitchLength) + local succ = self.motionPrimitives:createSuccessor(pred, primitive, self.hitchLength) if succ:equals(self.goal, self.deltaPosGoal, self.deltaThetaGoal) then succ.pred = succ.pred self:debug('Successor at the goal (%d).', self.iterations) + self:debug('%s', succ) return self:finishRun(true, self:rollUpPath(succ, self.goal)) end @@ -633,23 +681,23 @@ function HybridAStar:run(start, goal, turnRadius, allowReverse, constraints, hit -- we end up being in overlap with another vehicle when we start the pathfinding and all we need is -- an iteration or two to bring us out of that position if (self.ignoreValidityAtStart and self.iterations < 3) or self.constraints:isValidNode(succ) then - succ:updateG(primitive, self.constraints:getNodePenalty(succ)) + succ:updateG(primitive, self.penaltyCache:get(succ, self.constraints)) local analyticSolutionCost = 0 if self.analyticSolverEnabled then local analyticSolution = self.analyticSolver:solve(succ, self.goal, self.turnRadius) analyticSolutionCost = analyticSolution:getLength(self.turnRadius) succ:updateH(self.goal, analyticSolutionCost) else - succ:updateH(self.goal, 0, succ:distance(self.goal) * 1.5) + succ:updateH(self.goal, 0, succ:distance(self.goal) * 1.0) end - --self:debug(' %s', tostring(succ)) + self.logger:trace(' %s', tostring(succ)) if existingSuccNode then - --self:debug(' existing node %s', tostring(existingSuccNode)) + self.logger:trace(' existing node %s', tostring(existingSuccNode)) -- there is already a node at this (discretized) position -- add a small number before comparing to adjust for floating point calculation differences if existingSuccNode:getCost() + 0.001 >= succ:getCost() then - --self:debug('%.6f replacing %s with %s', succ:getCost() - existingSuccNode:getCost(), tostring(existingSuccNode), tostring(succ)) + self.logger:trace('%.6f replacing %s with %s', succ:getCost() - existingSuccNode:getCost(), tostring(existingSuccNode), tostring(succ)) if self.openList:valueByPayload(existingSuccNode) then -- existing node is on open list already, remove it here, will replace with existingSuccNode:remove(self.openList) @@ -659,7 +707,7 @@ function HybridAStar:run(start, goal, turnRadius, allowReverse, constraints, hit -- add to open list succ:insert(self.openList) else - --self:debug('insert existing node back %s (iteration %d), diff %s', tostring(succ), self.iterations, tostring(succ:getCost() - existingSuccNode:getCost())) + self.logger:trace('insert existing node back %s (iteration %d), diff %s', tostring(succ), self.iterations, tostring(succ:getCost() - existingSuccNode:getCost())) end else -- successor cell does not yet exist @@ -668,13 +716,13 @@ function HybridAStar:run(start, goal, turnRadius, allowReverse, constraints, hit succ:insert(self.openList) end else - --self:debug('Invalid node %s (iteration %d)', tostring(succ), self.iterations) + self.logger:trace('Invalid node %s (iteration %d)', tostring(succ), self.iterations) succ:close() end -- valid node end end -- node as been expanded, close it to prevent expansion again - --self:debug(tostring(pred)) + self.logger:trace(tostring(pred)) pred:close() self.expansions = self.expansions + 1 end diff --git a/scripts/pathfinder/HybridAStarWithAStarInTheMiddle.lua b/scripts/pathfinder/HybridAStarWithAStarInTheMiddle.lua index b6064842c..b79205ef5 100644 --- a/scripts/pathfinder/HybridAStarWithAStarInTheMiddle.lua +++ b/scripts/pathfinder/HybridAStarWithAStarInTheMiddle.lua @@ -10,10 +10,11 @@ HybridAStarWithAStarInTheMiddle = CpObject(PathfinderInterface) ---@param mustBeAccurate boolean must be accurately find the goal position/angle (optional) ---@param analyticSolver AnalyticSolver the analytic solver the use (optional) function HybridAStarWithAStarInTheMiddle:init(vehicle, yieldAfter, maxIterations, mustBeAccurate, analyticSolver) - -- path generation phases + self.name = 'HybridAStarWithAStarInTheMiddle' self.vehicle = vehicle + -- path generation phases self.START_TO_MIDDLE = 1 - self.ASTAR = 2 + self.FAST = 2 self.MIDDLE_TO_END = 3 self.ALL_HYBRID = 4 -- start and goal close enough, we only need a single phase with hybrid self.hybridRange = 20 -- default range around start/goal to use hybrid A * @@ -22,12 +23,12 @@ function HybridAStarWithAStarInTheMiddle:init(vehicle, yieldAfter, maxIterations -- the only reason we have a separate instance for start and end is to be able to draw the nodes after -- the pathfinding is done for debug purposes self.startHybridAStarPathfinder = HybridAStar(vehicle, self.yieldAfter, maxIterations, mustBeAccurate) - self.aStarPathfinder = self:getAStar() + self.middlePathfinder = self:getFastPathfinder() self.endHybridAStarPathfinder = HybridAStar(vehicle, self.yieldAfter, maxIterations, mustBeAccurate) self.analyticSolver = analyticSolver end -function HybridAStarWithAStarInTheMiddle:getAStar() +function HybridAStarWithAStarInTheMiddle:getFastPathfinder() return AStar(self.vehicle, self.yieldAfter, self.maxIterations) end @@ -43,12 +44,7 @@ end --- when we search for a valid analytic solution we use this instead of isValidNode() ---@param hitchLength number hitch length of a trailer (length between hitch on the towing vehicle and the --- rear axle of the trailer), can be nil ----@return boolean true if pathfinding is done (success or failure), false means it isn't ready and ---- resume() must be called to continue until this is true ----@return Polyline the path if found ----@return boolean if true, the goal node is invalid (for instance a vehicle or obstacle is there) so ---- the pathfinding can never succeed. ----@return number the furthest distance the pathfinding tried from the start, only when no path found +---@return PathfinderResult function HybridAStarWithAStarInTheMiddle:start(start, goal, turnRadius, allowReverse, constraints, hitchLength) self.startNode, self.goalNode = State3D.copy(start), State3D.copy(goal) self.originalStartNode = State3D.copy(self.startNode) @@ -58,16 +54,17 @@ function HybridAStarWithAStarInTheMiddle:start(start, goal, turnRadius, allowRev self.hybridRange = self.hybridRange and self.hybridRange or turnRadius * 3 -- how far is start/goal apart? self.startNode:updateH(self.goalNode, turnRadius) - self.phase = self.ASTAR - self:debug('Finding fast A* path between start and goal...') - self.coroutine = coursePlayCoroutine.create(self.aStarPathfinder.run) - self.currentPathfinder = self.aStarPathfinder + self.phase = self.FAST + self:debug('Start first pass, fast pathfinding between start and goal...') + self.coroutine = coursePlayCoroutine.create(self.middlePathfinder.run) + self.currentPathfinder = self.middlePathfinder -- strict mode for the middle part, stay close to the field, for future improvements, disabled for now -- self.constraints:setStrictMode() return self:resume(self.startNode, self.goalNode, turnRadius, false, constraints, hitchLength) end -- distance between start and goal is relatively short, one phase hybrid A* all the way +---@return PathfinderResult function HybridAStarWithAStarInTheMiddle:findHybridStartToEnd() self.phase = self.ALL_HYBRID self:debug('Goal is closer than %d, use one phase pathfinding only', self.hybridRange * 3) @@ -77,6 +74,7 @@ function HybridAStarWithAStarInTheMiddle:findHybridStartToEnd() end -- start and goal far away, this is the hybrid A* from start to the middle section +---@return PathfinderResult function HybridAStarWithAStarInTheMiddle:findPathFromStartToMiddle() self:debug('Finding path between start and middle section...') self.phase = self.START_TO_MIDDLE @@ -88,6 +86,7 @@ function HybridAStarWithAStarInTheMiddle:findPathFromStartToMiddle() end -- start and goal far away, this is the hybrid A* from the middle section to the goal +---@return PathfinderResult function HybridAStarWithAStarInTheMiddle:findPathFromMiddleToEnd() -- generate middle to end self.phase = self.MIDDLE_TO_END @@ -98,47 +97,48 @@ function HybridAStarWithAStarInTheMiddle:findPathFromMiddleToEnd() end --- The resume() of this pathfinder is more complicated as it handles essentially three separate pathfinding runs +---@return PathfinderResult function HybridAStarWithAStarInTheMiddle:resume(...) - local ok, done, path, goalNodeInvalid = coursePlayCoroutine.resume(self.coroutine, self.currentPathfinder, ...) + local ok, result = coursePlayCoroutine.resume(self.coroutine, self.currentPathfinder, ...) if not ok then - print(done) + print(result.done) printCallstack() self:debug('Pathfinding failed') self.coroutine = nil - return PathfinderResult(true, nil, goalNodeInvalid, self.currentPathfinder:getHighestDistance(), + return PathfinderResult(true, nil, result.goalNodeInvalid, self.currentPathfinder:getHighestDistance(), self.constraints) end - if done then + if result.done then self.coroutine = nil if self.phase == self.ALL_HYBRID then - if path then + if result.path then -- start and goal near, just one phase, all hybrid, we are done - return PathfinderResult(true, path) + return PathfinderResult(true, result.path) else self:debug('all hybrid: no path found') - return PathfinderResult(true, nil, goalNodeInvalid, self.currentPathfinder:getHighestDistance(), + return PathfinderResult(true, nil, result.goalNodeInvalid, self.currentPathfinder:getHighestDistance(), self.constraints) end - elseif self.phase == self.ASTAR then + elseif self.phase == self.FAST then self.constraints:resetStrictMode() - if not path then - self:debug('fast A*: no path found') - return PathfinderResult(true, nil, goalNodeInvalid, self.currentPathfinder:getHighestDistance(), + if not result.path then + self:debug('First pass: no path found') + return PathfinderResult(true, nil, result.goalNodeInvalid, self.currentPathfinder:getHighestDistance(), self.constraints) end - CourseGenerator.addDebugPolyline(Polyline(path), {1, 0, 0}) - local lMiddlePath = HybridAStar.length(path) + CourseGenerator.addDebugPolyline(Polyline(result.path), {1, 0, 0}) + local lMiddlePath = HybridAStar.length(result.path) self:debug('Direct path is %d m', lMiddlePath) -- do we even need to use the normal A star or the nodes are close enough that the hybrid A star will be fast enough? if lMiddlePath < self.hybridRange * 2 then return self:findHybridStartToEnd() end -- middle part ready, now trim start and end to make room for the hybrid parts - self.middlePath = path + self.middlePath = result.path HybridAStar.shortenStart(self.middlePath, self.hybridRange) HybridAStar.shortenEnd(self.middlePath, self.hybridRange) if #self.middlePath < 2 then - return PathfinderResult(true, nil, goalNodeInvalid, self.currentPathfinder:getHighestDistance(), + return PathfinderResult(true, nil, result.goalNodeInvalid, self.currentPathfinder:getHighestDistance(), self.constraints) end State3D.smooth(self.middlePath) @@ -146,10 +146,10 @@ function HybridAStarWithAStarInTheMiddle:resume(...) State3D.calculateTrailerHeadings(self.middlePath, self.hitchLength, true) return self:findPathFromStartToMiddle() elseif self.phase == self.START_TO_MIDDLE then - if path then - CourseGenerator.addDebugPolyline(Polyline(path), {0, 1, 0}) + if result.path then + CourseGenerator.addDebugPolyline(Polyline(result.path), {0, 1, 0}) -- start and middle sections ready, continue with the piece from the middle to the end - self.path = path + self.path = result.path -- create start point at the last waypoint of middlePath before shortening self.middleToEndStart = State3D.copy(self.middlePath[#self.middlePath]) -- now shorten both ends of middlePath to avoid short fwd/reverse sections due to overlaps (as the @@ -163,26 +163,26 @@ function HybridAStarWithAStarInTheMiddle:resume(...) return self:findPathFromMiddleToEnd() else self:debug('start to middle: no path found') - return PathfinderResult(true, nil, goalNodeInvalid, self.currentPathfinder:getHighestDistance(), + return PathfinderResult(true, nil, result.goalNodeInvalid, self.currentPathfinder:getHighestDistance(), self.constraints) end elseif self.phase == self.MIDDLE_TO_END then - if path then - CourseGenerator.addDebugPolyline(Polyline(path), {0, 0, 1}) + if result.path then + CourseGenerator.addDebugPolyline(Polyline(result.path), {0, 0, 1}) -- last piece is ready, this was generated from the goal point to the end of the middle section so -- first remove the last point of the middle section to make the transition smoother -- and then add the last section in reverse order -- also, for reasons we don't fully understand, this section may have a direction change at the last waypoint, -- so we just ignore the last one - for i = 1, #path do - table.insert(self.path, path[i]) + for i = 1, #result.path do + table.insert(self.path, result.path[i]) end State3D.smooth(self.path) self.constraints:showStatistics() return PathfinderResult(true, self.path) else self:debug('middle to end: no path found') - return PathfinderResult(true, nil, goalNodeInvalid, self.currentPathfinder:getHighestDistance(), + return PathfinderResult(true, nil, result.goalNodeInvalid, self.currentPathfinder:getHighestDistance(), self.constraints) end end @@ -192,7 +192,7 @@ end function HybridAStarWithAStarInTheMiddle:nodeIterator() local startIt = self.startHybridAStarPathfinder:nodeIterator() - local middleIt = self.aStarPathfinder:nodeIterator() + local middleIt = self.middlePathfinder:nodeIterator() local endIt = self.endHybridAStarPathfinder:nodeIterator() return function() local node, lowestCost, highestCost = startIt() @@ -212,7 +212,7 @@ function HybridAStarWithAStarInTheMiddle:nodeIteratorStart() end function HybridAStarWithAStarInTheMiddle:nodeIteratorMiddle() - return self.aStarPathfinder:nodeIterator() + return self.middlePathfinder:nodeIterator() end function HybridAStarWithAStarInTheMiddle:nodeIteratorEnd() @@ -233,7 +233,7 @@ function DummyAStar:init(vehicle, path) end function DummyAStar:run() - return true, self.path + return PathfinderResult(true, self.path) end --- Similar to HybridAStarWithAStarInTheMiddle, but the middle section is not calculated using the A*, instead @@ -251,6 +251,7 @@ function HybridAStarWithPathInTheMiddle:init(vehicle, yieldAfter, path, mustBeAc self.vehicle = vehicle self.path = path HybridAStarWithAStarInTheMiddle.init(self, vehicle, yieldAfter, 10000, mustBeAccurate, analyticSolver) + self.name = 'HybridAStarWithPathInTheMiddle' end function HybridAStarWithPathInTheMiddle:start(...) @@ -258,6 +259,6 @@ function HybridAStarWithPathInTheMiddle:start(...) return HybridAStarWithAStarInTheMiddle.start(self, ...) end -function HybridAStarWithPathInTheMiddle:getAStar() +function HybridAStarWithPathInTheMiddle:getFastPathfinder() return DummyAStar(self.vehicle, self.path) end diff --git a/scripts/pathfinder/JumpPointSearch.lua b/scripts/pathfinder/JumpPointSearch.lua new file mode 100644 index 000000000..57a0a01dc --- /dev/null +++ b/scripts/pathfinder/JumpPointSearch.lua @@ -0,0 +1,241 @@ +--- A Jump Point Search +---@class JumpPointSearch : AStar, HybridAStar.MotionPrimitives +JumpPointSearch = CpObject(AStar) +JumpPointSearch.markers = {} + +function JumpPointSearch:init(vehicle, yieldAfter, maxIterations) + AStar.init(self, vehicle, yieldAfter, maxIterations) + self.name = "JumpPointSearch" +end + +function JumpPointSearch:initRun(start, goal, turnRadius, allowReverse, constraints, hitchLength) + self.logger:setLevel(Logger.level.debug) + return AStar.initRun(self, start, goal, turnRadius, allowReverse, constraints, hitchLength) +end + +--- The motion primitives in JPS use so many attributes of the pathfinder class itself, that we just implement +--- the motion primitive interface in the pathfinder +function JumpPointSearch:getMotionPrimitives(turnRadius, allowReverse) + self:initMotionPrimitives() + return self +end + +function JumpPointSearch:initMotionPrimitives() + -- similar to the A*, the possible motion primitives (the neighbors) are in all 8 directions. + self.primitives = {} + self.gridSize = self.deltaPos + local d = self.gridSize + table.insert(self.primitives, Vector(d, 0)) + table.insert(self.primitives, Vector(d, d)) + table.insert(self.primitives, Vector(0, d)) + table.insert(self.primitives, Vector(-d, d)) + + table.insert(self.primitives, Vector(-d, 0)) + table.insert(self.primitives, Vector(-d, -d)) + table.insert(self.primitives, Vector(0, -d)) + table.insert(self.primitives, Vector(d, -d)) + + -- set up the pruning rules for each primitive, that is, if we used this primitive to move to the + -- next node, which neighbors of that node should be examined further. + -- always unconditionally include natural neighbors (check = nil) + -- create forced neighbors when the grid identified by the check vector is invalid + + -- right + self.primitives[1].nextPrimitives = { + { check = nil, nextPrimitive = self.primitives[1] }, + { check = Vector(0, self.gridSize), nextPrimitive = self.primitives[2] }, + { check = Vector(0, -self.gridSize), nextPrimitive = self.primitives[8] } + } + -- left + self.primitives[5].nextPrimitives = { + { check = nil, nextPrimitive = self.primitives[5] }, + { check = Vector(0, self.gridSize), nextPrimitive = self.primitives[4] }, + { check = Vector(0, -self.gridSize), nextPrimitive = self.primitives[6] } + } + + -- up + self.primitives[3].nextPrimitives = { + { check = nil, nextPrimitive = self.primitives[3] }, + { check = Vector(self.gridSize, 0), nextPrimitive = self.primitives[2] }, + { check = Vector(-self.gridSize, 0), nextPrimitive = self.primitives[4] } + } + + -- down + self.primitives[7].nextPrimitives = { + { check = nil, nextPrimitive = self.primitives[7] }, + { check = Vector(self.gridSize, 0), nextPrimitive = self.primitives[8] }, + { check = Vector(-self.gridSize, 0), nextPrimitive = self.primitives[6] } + } + + -- up right + self.primitives[2].nextPrimitives = { + { check = nil, nextPrimitive = self.primitives[2] }, + { check = nil, nextPrimitive = self.primitives[1] }, + { check = nil, nextPrimitive = self.primitives[3] }, + { check = Vector(-self.gridSize, 0), nextPrimitive = self.primitives[4] }, + { check = Vector(0, -self.gridSize), nextPrimitive = self.primitives[8] }, + } + + -- up left + self.primitives[4].nextPrimitives = { + { check = nil, nextPrimitive = self.primitives[4] }, + { check = nil, nextPrimitive = self.primitives[3] }, + { check = nil, nextPrimitive = self.primitives[5] }, + { check = Vector(self.gridSize, 0), nextPrimitive = self.primitives[2] }, + { check = Vector(0, -self.gridSize), nextPrimitive = self.primitives[6] }, + } + + -- down left + self.primitives[6].nextPrimitives = { + { check = nil, nextPrimitive = self.primitives[6] }, + { check = nil, nextPrimitive = self.primitives[5] }, + { check = nil, nextPrimitive = self.primitives[7] }, + { check = Vector(self.gridSize, 0), nextPrimitive = self.primitives[8] }, + { check = Vector(0, self.gridSize), nextPrimitive = self.primitives[4] }, + } + + -- down right + self.primitives[8].nextPrimitives = { + { check = nil, nextPrimitive = self.primitives[8] }, + { check = nil, nextPrimitive = self.primitives[1] }, + { check = nil, nextPrimitive = self.primitives[7] }, + { check = Vector(-self.gridSize, 0), nextPrimitive = self.primitives[6] }, + { check = Vector(0, self.gridSize), nextPrimitive = self.primitives[2] }, + } + + for i = 1, #self.primitives do + -- if we moved to the current node using primitive[i], then the primitive we would need to go back to + -- the parent is primitive[i].parent + self.primitives[i].parent = self.primitives[i + 4] or self.primitives[i - 4] + -- cache the length + self.primitives[i].d = self.primitives[i]:length() + end +end + +function JumpPointSearch:isValidNode(node) + return self.constraints:isValidNode(node, true, true) +end + +function JumpPointSearch:isPenaltyChanging(node, predecessor) + local predecessorPenalty = self.penaltyCache:get(predecessor, self.constraints) + local nodePenalty = self.penaltyCache:get(node, self.constraints) + local change = (nodePenalty + 0.0001) / (predecessorPenalty + 0.0001) -- avoid problems if either is zero + if change > 3 then + -- penalty increasing, force a neighbor here + return true + elseif change < 0 then + -- penalty decreasing, force a neighbor here + -- this is currently disabled, as it is not clear if this is a good idea. If we enable this, it results + -- in the scan reentering the non-penalty area and scanning it again from a different direction. + return true + end +end + +---@param node State3D +---@param primitive +---@return table array of primitives pointing to natural and forced neighbors +---@return boolean true if there is at least one forced neighbor +function JumpPointSearch:getNextPrimitives(node, primitive) + local nextPrimitives, forced = {}, false + for _, n in ipairs(primitive.nextPrimitives) do + if n.check then + -- check if the neighbor is valid + local neighborToCheck = State3D(node.x + n.check.x, node.y + n.check.y, n.check:heading()) + if not self:isValidNode(neighborToCheck) or self:isPenaltyChanging(neighborToCheck, node) then + -- we have a forced neighbor + table.insert(nextPrimitives, n.nextPrimitive) + forced = true + end + else + table.insert(nextPrimitives, n.nextPrimitive) + end + end + return nextPrimitives, forced +end + + +function JumpPointSearch:jump(node, primitive, recursionCounter) + recursionCounter = recursionCounter and recursionCounter + 1 or 1 + if recursionCounter > 5 then + return node + end + local v = node + primitive + local successor = State3D(v.x, v.y, primitive:heading()) + if not self:isValidNode(successor) then + return nil + end + if self:isPenaltyChanging(successor, node) then + return successor + end + if successor:equals(self.goal, self.deltaPosGoal, self.deltaThetaGoal) then + return successor + end + + local _, forced = self:getNextPrimitives(successor, primitive) + if forced then + return successor + end + + if primitive.x ~= 0 and primitive.y ~= 0 then + -- diagonal move, must check first horizontally and vertically + -- left or right + local jumpPoint = self:jump(successor, primitive.x > 0 and self.primitives[1] or self.primitives[5]) + if jumpPoint then + return successor + else + -- up or down + jumpPoint = self:jump(successor, primitive.y > 0 and self.primitives[3] or self.primitives[7]) + if jumpPoint then + return successor + end + end + end + + return self:jump(successor, primitive, recursionCounter) +end + +-- Get the possible neighbors when coming from the predecessor node. +-- While the other HybridAStar derived algorithms use a real motion primitive, meaning it gives the relative +-- x, y and theta values which need to be added to the predecessor, JPS supplies the actual coordinates of +-- the successors here instead. +-- This is not the most elegant solution and the only reason we do this is to be able to reuse the +-- getPrimitives()/createSuccessor() interface in HybridAStar.lua with JPS. +function JumpPointSearch:getPrimitives(node) + local primitives + if node.primitive then + -- the primitive that was used to get to this node + primitives = self:getNextPrimitives(node, node.primitive) + else + -- first node, no predecessor, no incoming primitive + primitives = self.primitives + end + local jumpNodes = {} + for _, p in ipairs(primitives) do + local jumpNode = self:jump(node, p) + if jumpNode then + -- here we have the node already, so the usual createSuccessor() does not need to calculate it again + -- from the primitive. However, HybridAStar uses primitive.d to calculate the g cost, so we use an + -- ugly hack here, by creating a fake primitive, setting the d value to the proper length, and + -- add the jump node as .node to the primitive. + table.insert(jumpNodes, { + node = State3D(jumpNode.x, jumpNode.y, jumpNode.t, node.g, node, Gear.Forward, Steer.Straight, nil, node.d, p), + d = (jumpNode - node):length() }) + end + end + return jumpNodes +end + +-- Use the hacky combined nodeAndPrimitive to return the successor node +function JumpPointSearch:createSuccessor(node, nodeAndPrimitive, hitchLength) + nodeAndPrimitive.node.tTrailer = node:getNextTrailerHeading(self.gridSize, hitchLength) + nodeAndPrimitive.node.d = node.d + nodeAndPrimitive.d + return nodeAndPrimitive.node +end + +---@class HybridAStarWithJpsInTheMiddle : HybridAStarWithAStarInTheMiddle +HybridAStarWithJpsInTheMiddle = CpObject(HybridAStarWithAStarInTheMiddle) + +function HybridAStarWithJpsInTheMiddle:getFastPathfinder() + return JumpPointSearch(self.vehicle, self.yieldAfter, self.maxIterations) +end + diff --git a/scripts/pathfinder/PathfinderConstraints.lua b/scripts/pathfinder/PathfinderConstraints.lua index 11360477f..c08f136ad 100644 --- a/scripts/pathfinder/PathfinderConstraints.lua +++ b/scripts/pathfinder/PathfinderConstraints.lua @@ -73,12 +73,13 @@ function PathfinderConstraints:init(context) self.initialMaxFruitPercent = self.maxFruitPercent self.initialOffFieldPenalty = self.offFieldPenalty self.strictMode = false + self.penaltyFactor = self.vehicle:getCpSettings().penaltyFactor:getValue() self:resetCounts() local areaToAvoidText = self.areaToAvoid and string.format('are to avoid %.1f x %.1f m', self.areaToAvoid.length, self.areaToAvoid.width) or 'none' - self.logger:debug('off field penalty %.1f, max fruit percent: %.1f, field number %d, %s, ignore fruit %s, ignore off-field penalty %s', + self.logger:debug(self.vehicle, 'off field penalty %.1f, max fruit percent: %.1f, field number %d, %s, ignore fruit %s, ignore off-field penalty %s, penalty factor %.1f', self.offFieldPenalty, self.maxFruitPercent, self.fieldNum, areaToAvoidText, - self.areaToIgnoreFruit or 'none', self.areaToIgnoreOffFieldPenalty or 'none') + self.areaToIgnoreFruit or 'none', self.areaToIgnoreOffFieldPenalty or 'none', self.penaltyFactor) end function PathfinderConstraints:resetCounts() @@ -131,7 +132,7 @@ function PathfinderConstraints:getNodePenalty(node) end penalty = penalty + self:calculatePreferredPathPenalty(node) self.totalNodeCount = self.totalNodeCount + 1 - return penalty + return penalty * self.penaltyFactor end --- Calculate penalty for this node based on the preferred path. The penalty will be negative to @@ -172,7 +173,7 @@ function PathfinderConstraints:isValidAnalyticSolutionNode(node, log) local analyticLimit = self.maxFruitPercent * 2 if hasFruit and fruitValue > analyticLimit then if log then - self.logger:debug('isValidAnalyticSolutionNode: fruitValue %.1f, max %.1f @ %.1f, %.1f', + self.logger:debug(self.vehicle, 'isValidAnalyticSolutionNode: fruitValue %.1f, max %.1f @ %.1f, %.1f', fruitValue, analyticLimit, node.x, -node.y) end return false @@ -239,10 +240,10 @@ function PathfinderConstraints:resetStrictMode() end function PathfinderConstraints:showStatistics() - self.logger:debug('Nodes: %d, Penalties: fruit: %d, off-field: %d, not owned field: %d, collisions: %d, trailer collisions: %d, area to avoid: %d, preferred path: %d', + self.logger:debug(self.vehicle, 'Nodes: %d, Penalties: fruit: %d, off-field: %d, not owned field: %d, collisions: %d, trailer collisions: %d, area to avoid: %d, preferred path: %d', self.totalNodeCount, self.fruitPenaltyNodeCount, self.offFieldPenaltyNodeCount, self.notOwnedFieldPenaltyNodeCount, self.collisionNodeCount, self.trailerCollisionNodeCount, self.areaToAvoidPenaltyCount, self.preferredPathPenaltyCount) - self.logger:debug(' max fruit %.1f %%, off-field penalty: %.1f', + self.logger:debug(self.vehicle, ' max fruit %.1f %%, off-field penalty: %.1f', self.maxFruitPercent, self.offFieldPenalty) end diff --git a/scripts/pathfinder/PathfinderUtil.lua b/scripts/pathfinder/PathfinderUtil.lua index 9f5293ee8..64561ce0a 100644 --- a/scripts/pathfinder/PathfinderUtil.lua +++ b/scripts/pathfinder/PathfinderUtil.lua @@ -428,7 +428,12 @@ end ---@param maxIterations number maximum number of iterations function PathfinderUtil.startPathfinding(vehicle, start, goal, constraints, allowReverse, mustBeAccurate, maxIterations) PathfinderUtil.overlapBoxes = {} - local pathfinder = HybridAStarWithAStarInTheMiddle(vehicle, 100, maxIterations, mustBeAccurate) + local pathfinder + if vehicle:getCpSettings().useJps:getValue() then + pathfinder = HybridAStarWithJpsInTheMiddle(vehicle, 100, maxIterations, mustBeAccurate) + else + pathfinder = HybridAStarWithAStarInTheMiddle(vehicle, 100, maxIterations, mustBeAccurate) + end return pathfinder, pathfinder:start(start, goal, constraints.turnRadius, allowReverse, constraints, constraints.trailerHitchLength) end @@ -477,12 +482,12 @@ function PathfinderUtil.findPathForTurn(vehicle, startOffset, goalReferenceNode, local _, y, _ = getWorldTranslation(vehicle:getAIDirectionNode()) local dx, _, dz = worldToLocal(vehicle:getAIDirectionNode(), headlandPath[1].x, y, -headlandPath[1].y) local dirDeg = math.deg(math.abs(math.atan2(dx, dz))) - PathfinderUtil.logger:debug('First headland waypoint isn\'t in front of us (%.1f), remove first few waypoints to avoid making a circle %.1f %.1f', dirDeg, dx, dz) + PathfinderUtil.logger:debug(vehicle, 'First headland waypoint isn\'t in front of us (%.1f), remove first few waypoints to avoid making a circle %.1f %.1f', dirDeg, dx, dz) pathfinder = HybridAStarWithPathInTheMiddle(vehicle, 200, headlandPath, true, analyticSolver) end end if pathfinder == nil then - PathfinderUtil.logger:debug('No headland, or there is a headland but wasn\'t able to get the shortest path on the headland to the next row, falling back to hybrid A*') + PathfinderUtil.logger:debug(vehicle, 'No headland, or there is a headland but wasn\'t able to get the shortest path on the headland to the next row, falling back to hybrid A*') pathfinder = HybridAStarWithAStarInTheMiddle(vehicle, 200, 10000, true, analyticSolver) end @@ -666,8 +671,8 @@ function PathfinderUtil.showNodes(pathfinder) if PathfinderUtil.visualDebugLevel > 1 and pathfinder.hybridAStarPathfinder and pathfinder.hybridAStarPathfinder.nodes then nodes = pathfinder.hybridAStarPathfinder.nodes - elseif PathfinderUtil.visualDebugLevel > 0 and pathfinder.aStarPathfinder and pathfinder.aStarPathfinder.nodes then - nodes = pathfinder.aStarPathfinder.nodes + elseif PathfinderUtil.visualDebugLevel > 0 and pathfinder.middlePathfinder and pathfinder.middlePathfinder.nodes then + nodes = pathfinder.middlePathfinder.nodes elseif PathfinderUtil.visualDebugLevel > 0 and pathfinder.nodes then nodes = pathfinder.nodes end diff --git a/scripts/pathfinder/State3D.lua b/scripts/pathfinder/State3D.lua index 02664c3f5..d604c9d2a 100644 --- a/scripts/pathfinder/State3D.lua +++ b/scripts/pathfinder/State3D.lua @@ -29,13 +29,15 @@ State3D = CpObject(Vector) ---@param steer Steer forward/backward ---@param tTrailer number heading (theta) of the attached trailer in radians ---@param d number distance so far (g without the penalty) -function State3D:init(x, y, t, g, pred, gear, steer, tTrailer, d) +---@param primitive|nil the primitive that was used to create this node +function State3D:init(x, y, t, g, pred, gear, steer, tTrailer, d, primitive) self.x = x self.y = y self.t = self:normalizeHeadingRad(t) self.pred = pred self.g = g or 0 self.d = d or 0 + self.primitive = primitive self.h = 0 self.cost = 0 self.goal = false @@ -45,8 +47,6 @@ function State3D:init(x, y, t, g, pred, gear, steer, tTrailer, d) self.tTrailer = tTrailer and self:normalizeHeadingRad(tTrailer) or 0 self.gear = gear or Gear.Forward self.steer = steer - -- penalty for using this node, to avoid obstacles, stay in an area, etc. - self.nodePenalty = 0 end function State3D.copy(other) @@ -142,10 +142,6 @@ function State3D:updateG(primitive, userPenalty) self.g = self.g + penalty * primitive.d + (userPenalty or 0) end -function State3D:setNodePenalty(nodePenalty) - self.nodePenalty = nodePenalty -end - function State3D:getTrailerHeading() return self.tTrailer end diff --git a/scripts/pathfinder/test/GraphPathfinderTest.lua b/scripts/pathfinder/test/GraphPathfinderTest.lua new file mode 100644 index 000000000..a37edd915 --- /dev/null +++ b/scripts/pathfinder/test/GraphPathfinderTest.lua @@ -0,0 +1,430 @@ +package.path = package.path .. ";../../test/?.lua;../../geometry/?.lua;../../courseGenerator/geometry/?.lua;../../courseGenerator/?.lua;../?.lua;../../?.lua;../../util/?.lua" +lu = require("luaunit") +require('mock-GiantsEngine') +require('mock-Courseplay') +require('CpObject') +require('CpUtil') +require('Logger') +require('BinaryHeap') +require('CpMathUtil') +require('Dubins') +require('ReedsShepp') +require('ReedsSheppSolver') +require('AnalyticSolution') +require('CourseGenerator') +require('WaypointAttributes') +require('Vector') +require('LineSegment') +require('State3D') +require('Vertex') +require('Polyline') +require('Polygon') +require('Slider') +require('WrapAroundIndex') -- for the test cases +require('AnalyticHelper') -- for the test cases +require('PathfinderUtil') +require('HybridAStar') +require('GraphPathfinder') + +local GraphEdge = GraphPathfinder.GraphEdge +local TestConstraints = CpObject(PathfinderConstraintInterface) +local pathfinder, start, goal, done, path, goalNodeInvalid + +local function printPath() + for i, p in ipairs(path) do + print(i, Vector.__tostring(p)) + end +end + +local function runPathfinder() + local result = pathfinder:start(start, goal, 6, false, TestConstraints(), 0) + while not result.done do + result = pathfinder:resume() + end + return result.done, result.path, result.goalNodeInvalid +end + +TestWithoutTransitions = {} +function TestWithoutTransitions:setUp() + self.splitEdges = Polyline.splitEdges + self.ensureMinimumRadius = Polyline.ensureMinimumRadius + -- these create the curves between the graph edges, resulting in many vertices which we don't want to test, + -- so disable and work with just the edges + Polyline.splitEdges = function() end + Polyline.ensureMinimumRadius = function() end +end + +function TestWithoutTransitions:tearDown() + -- restore the Polyline functions + Polyline.splitEdges = self.splitEdges + Polyline.ensureMinimumRadius = self.ensureMinimumRadius +end + +function TestWithoutTransitions:testDirection() + + local graph = { + GraphEdge(GraphEdge.UNIDIRECTIONAL, + { + Vertex(100, 100), + Vertex(110, 100), + Vertex(120, 100) + }), + GraphEdge( + GraphEdge.UNIDIRECTIONAL, + { + Vertex(120, 105), + Vertex(110, 105), + Vertex(100, 105), + }), + } + pathfinder = GraphPathfinder(math.huge, 500, 10, graph) + start = State3D(90, 105, 0, 0) + goal = State3D(130, 105, 0, 0) + done, path, _ = runPathfinder() + lu.assertIsTrue(done) + lu.assertEquals(#path, 3) + -- path contains all points of the edge it goes through + path[1]:assertAlmostEquals(Vector(100, 100)) + path[2]:assertAlmostEquals(Vector(110, 100)) + path[#path]:assertAlmostEquals(Vector(120, 100)) + start, goal = goal, start + done, path, _ = runPathfinder() + lu.assertIsTrue(done) + lu.assertEquals(#path, 3) + path[1]:assertAlmostEquals(Vector(120, 105)) + path[2]:assertAlmostEquals(Vector(110, 105)) + path[#path]:assertAlmostEquals(Vector(100, 105)) + +end + +function TestWithoutTransitions:testBidirectional() + + local graph = { + GraphEdge(GraphEdge.BIDIRECTIONAL, + { + Vertex(120, 100), + Vertex(110, 100), + Vertex(100, 100), + }), + GraphEdge( + GraphEdge.UNIDIRECTIONAL, + { + Vertex(120, 105), + Vertex(110, 105), + Vertex(100, 105), + }), + } + pathfinder = GraphPathfinder(math.huge, 500, 10, graph) + start = State3D(90, 105, 0, 0) + goal = State3D(130, 105, 0, 0) + done, path, _ = runPathfinder() + lu.assertIsTrue(done) + lu.assertEquals(#path, 3) + -- path contains the start node and all points of the edge it goes through + path[1]:assertAlmostEquals(Vector(100, 100)) + path[2]:assertAlmostEquals(Vector(110, 100)) + path[#path]:assertAlmostEquals(Vector(120, 100)) + start, goal = goal, start + done, path, _ = runPathfinder() + lu.assertIsTrue(done) + lu.assertEquals(#path, 3) + path[1]:assertAlmostEquals(Vector(120, 105)) + path[2]:assertAlmostEquals(Vector(110, 105)) + path[#path]:assertAlmostEquals(Vector(100, 105)) + +end + +function TestWithoutTransitions:testShorterPath() + + local graph = { + GraphEdge(GraphEdge.UNIDIRECTIONAL, + { + Vertex(100, 100), + Vertex(110, 100), + Vertex(120, 100) + }), + GraphEdge( + GraphEdge.UNIDIRECTIONAL, + { + Vertex(100, 105), + Vertex(110, 200), + Vertex(120, 105), + }), + } + pathfinder = GraphPathfinder(math.huge, 500, 10, graph) + start = State3D(90, 105, 0, 0) + goal = State3D(130, 105, 0, 0) + done, path, _ = runPathfinder() + lu.assertIsTrue(done) + lu.assertEquals(#path, 3) + path[1]:assertAlmostEquals(Vector(100, 100)) + path[2]:assertAlmostEquals(Vector(110, 100)) + path[#path]:assertAlmostEquals(Vector(120, 100)) + +end + +function TestWithoutTransitions:testRange() + + local graph = { + GraphEdge(GraphEdge.UNIDIRECTIONAL, + { + Vertex(100, 100), + Vertex(110, 100), + Vertex(120, 100) + }), + GraphEdge( + GraphEdge.UNIDIRECTIONAL, + { + Vertex(120, 105), + Vertex(110, 105), + Vertex(100, 105), + }), + GraphEdge(GraphEdge.UNIDIRECTIONAL, + { + Vertex(130, 100), + Vertex(140, 100), + Vertex(150, 100) + }), + } + pathfinder = GraphPathfinder(math.huge, 500, 10, graph) + start = State3D(90, 105, 0, 0) + goal = State3D(150, 105, 0, 0) + done, path, _ = runPathfinder() + lu.assertIsTrue(done) + lu.assertEquals(#path, 6) + path[1]:assertAlmostEquals(Vector(100, 100)) + path[2]:assertAlmostEquals(Vector(110, 100)) + path[#path]:assertAlmostEquals(Vector(150, 100)) + pathfinder = GraphPathfinder(math.huge, 500, 9, graph) + done, path, _ = runPathfinder() + lu.assertIsNil(path) + +end + +function TestWithoutTransitions:testStartInTheMiddle() + + local graph = { + GraphEdge(GraphEdge.BIDIRECTIONAL, + { + Vertex(200, 100), + Vertex(150, 100), + Vertex(100, 100), + }), + GraphEdge( + GraphEdge.UNIDIRECTIONAL, + { + Vertex(200, 105), + Vertex(150, 105), + Vertex(100, 105), + }), + } + pathfinder = GraphPathfinder(math.huge, 500, 10, graph) + start = State3D(150, 95, 0, 0) + goal = State3D(95, 95, 0, 0) + done, path, _ = runPathfinder() + lu.assertIsTrue(done) + lu.assertEquals(#path, 2) + path[1]:assertAlmostEquals(Vector(150, 100)) + path[2]:assertAlmostEquals(Vector(100, 100)) + graph = { + GraphEdge(GraphEdge.BIDIRECTIONAL, + { + Vertex(200, 100), + Vertex(150, 100), + Vertex(100, 100), + }), + GraphEdge( + GraphEdge.UNIDIRECTIONAL, + { + Vertex(200, 105), + Vertex(150, 105), + Vertex(100, 105), + }), + } + pathfinder = GraphPathfinder(math.huge, 500, 10, graph) + start, goal = goal, start + done, path, _ = runPathfinder() + lu.assertIsTrue(done) + lu.assertEquals(#path, 2) + path[1]:assertAlmostEquals(Vector(100, 100)) + path[2]:assertAlmostEquals(Vector(150, 100)) + +end + +function TestWithoutTransitions:testTwoPointSegments() + + local graph = { + GraphEdge(GraphEdge.UNIDIRECTIONAL, + { + Vertex(100, 100), + Vertex(120, 100) + }), + GraphEdge( + GraphEdge.UNIDIRECTIONAL, + { + Vertex(120, 105), + Vertex(100, 105), + }), + } + pathfinder = GraphPathfinder(math.huge, 500, 10, graph) + start = State3D(90, 105, 0, 0) + goal = State3D(130, 105, 0, 0) + done, path, _ = runPathfinder() + lu.assertIsTrue(done) + lu.assertEquals(#path, 2) + path[1]:assertAlmostEquals(Vector(100, 100)) + path[#path]:assertAlmostEquals(Vector(120, 100)) + start, goal = goal, start + done, path, _ = runPathfinder() + lu.assertIsTrue(done) + lu.assertEquals(#path, 2) + path[1]:assertAlmostEquals(Vector(120, 105)) + path[#path]:assertAlmostEquals(Vector(100, 105)) + +end + +function TestWithoutTransitions:testEntryExit() + + local graph = { + GraphEdge(GraphEdge.UNIDIRECTIONAL, + { + Vertex(100, 100), + Vertex(110, 100), + Vertex(120, 100) + }), + GraphEdge( + GraphEdge.UNIDIRECTIONAL, + { + Vertex(120, 105), + Vertex(110, 105), + Vertex(100, 105), + }), + } + -- range is 5 so goal is never within the range of start, that is in the testGoalWithinRange() function + pathfinder = GraphPathfinder(math.huge, 500, 5, graph) + -- start/goal far away + start = State3D(0, 0, 0, 0) + goal = State3D(130, 0, 0, 0) + done, path, _ = runPathfinder() + lu.assertIsTrue(done) + lu.assertEquals(#path, 3) + -- path contains all points of the edge it goes through + path[1]:assertAlmostEquals(Vector(100, 100)) + path[2]:assertAlmostEquals(Vector(110, 100)) + path[#path]:assertAlmostEquals(Vector(120, 100)) + -- start/goal far away + start = State3D(130, 200, 0, 0) + goal = State3D(0, 200, 0, 0) + done, path, _ = runPathfinder() + lu.assertIsTrue(done) + lu.assertEquals(#path, 3) + -- path contains all points of the edge it goes through + path[1]:assertAlmostEquals(Vector(120, 105)) + path[2]:assertAlmostEquals(Vector(110, 105)) + path[#path]:assertAlmostEquals(Vector(100, 105)) + -- start/goal far away, middle entry + start = State3D(110, 0, 0, 0) + goal = State3D(130, 100, 0, 0) + done, path, _ = runPathfinder() + lu.assertIsTrue(done) + lu.assertEquals(#path, 2) + path[1]:assertAlmostEquals(Vector(110, 100)) + path[#path]:assertAlmostEquals(Vector(120, 100)) + +end + +function TestWithoutTransitions:testGoalWithinRange() + -- goal too close to start (graph entry too close to graph exit) + + local graph = { + GraphEdge(GraphEdge.UNIDIRECTIONAL, + { + Vertex(100, 100), + Vertex(120, 100) + }), + } + pathfinder = GraphPathfinder(math.huge, 500, 21, graph) + -- start/goal far away + start = State3D(100, 100, 0, 0) + goal = State3D(120, 100, 0, 0) + done, path, goalNodeInvalid = runPathfinder() + lu.assertIsTrue(done) + lu.assertIsTrue(goalNodeInvalid) + lu.assertIsNil(path) + +end + +function TestWithoutTransitions:testTwoWayStreet() + + local graph = { + -- lane to the right, closer to the start location + GraphEdge(GraphEdge.UNIDIRECTIONAL, + { + Vertex(-100, 10), + Vertex(0, 10), + Vertex(100, 10) + }), + -- lane to the left, shortest way to the goal + GraphEdge(GraphEdge.UNIDIRECTIONAL, + { + Vertex(100, 15), -- 15 here so we can traverse from the other lane to this at x=100 + Vertex(0, 20), + Vertex(-100, 20) + }), + } + -- Range is 5, so we won't turn right, but take the longer path, to the left, make a U turn and drive back on + -- the lane to the left + pathfinder = GraphPathfinder(math.huge, 500, 5, graph) + start = State3D(0, 0, 0, 0) + -- goal on the left + goal = State3D(-120, 10, 0, 0) + done, path, _ = runPathfinder() + lu.assertIsTrue(done) + lu.assertEquals(#path, 5) + path[1]:assertAlmostEquals(Vector(0, 10)) + path[2]:assertAlmostEquals(Vector(100, 10)) + path[3]:assertAlmostEquals(Vector(100, 15)) + path[4]:assertAlmostEquals(Vector(0, 20)) + path[#path]:assertAlmostEquals(Vector(-100, 20)) + -- with the bigger range, we should turn left, taking the shortest path + pathfinder = GraphPathfinder(math.huge, 500, 10, graph) + done, path, _ = runPathfinder() + lu.assertIsTrue(done) + lu.assertEquals(#path, 2) + path[1]:assertAlmostEquals(Vector(0, 20)) + path[#path]:assertAlmostEquals(Vector(-100, 20)) +end + +TestWithTransitions = {} +function TestWithTransitions:testTransition() + local graph = { + GraphEdge(GraphEdge.UNIDIRECTIONAL, + { + Vertex(0, 0), + Vertex(100, 0), + Vertex(200, 0) + }), + GraphEdge( + GraphEdge.UNIDIRECTIONAL, + { + Vertex(210, 10), + Vertex(210, 100), + Vertex(210, 200), + }), + } + pathfinder = GraphPathfinder(math.huge, 500, 20, graph) + start = State3D(-5, 0, 0, 0) + goal = State3D(210, 205, 0, 0) + done, path, _ = runPathfinder() + printPath() + lu.assertIsTrue(done) + lu.assertEquals(#path, 90) + -- path contains all points of the edge it goes through + path[1]:assertAlmostEquals(Vector(0, 0)) + path[41]:assertAlmostEquals(Vector(200, 0)) + -- here's the arch + path[52]:assertAlmostEquals(Vector(210, 10)) + path[#path]:assertAlmostEquals(Vector(210, 200)) +end + +os.exit(lu.LuaUnit.run()) diff --git a/scripts/test/mock-Courseplay.lua b/scripts/test/mock-Courseplay.lua index bc499742e..36c910d92 100644 --- a/scripts/test/mock-Courseplay.lua +++ b/scripts/test/mock-Courseplay.lua @@ -23,4 +23,23 @@ CpDebug.getText = function () return '' end g_vehicleConfigurations = {} function g_vehicleConfigurations:get() return false -end \ No newline at end of file +end + +g_Courseplay = { + globalSettings = { + getSettings = function() + return { + deltaAngleRelaxFactorDeg = { + getValue = function() + return 10 + end + }, + maxDeltaAngleAtGoalDeg = { + getValue = function() + return 45 + end + }, + } + end + } +} \ No newline at end of file diff --git a/scripts/test/mock-GiantsEngine.lua b/scripts/test/mock-GiantsEngine.lua index f64745188..88f0cc870 100644 --- a/scripts/test/mock-GiantsEngine.lua +++ b/scripts/test/mock-GiantsEngine.lua @@ -110,3 +110,10 @@ end function printCallstack() end + +CollisionFlag = {} +setmetatable(CollisionFlag, {__index = function() return 0 end}) + +function openIntervalTimer() end +function closeIntervalTimer() end +function readIntervalTimerMs() return 0 end \ No newline at end of file diff --git a/translations/translation_br.xml b/translations/translation_br.xml index 5977b81d3..e5334debf 100644 --- a/translations/translation_br.xml +++ b/translations/translation_br.xml @@ -177,6 +177,10 @@ + + + + diff --git a/translations/translation_cs.xml b/translations/translation_cs.xml index 2de301aca..52f8ce61c 100644 --- a/translations/translation_cs.xml +++ b/translations/translation_cs.xml @@ -177,6 +177,10 @@ + + + + diff --git a/translations/translation_ct.xml b/translations/translation_ct.xml index 59e34edce..40aa9a0f8 100644 --- a/translations/translation_ct.xml +++ b/translations/translation_ct.xml @@ -177,6 +177,10 @@ + + + + diff --git a/translations/translation_cz.xml b/translations/translation_cz.xml index 591a5fd75..2f126cf21 100644 --- a/translations/translation_cz.xml +++ b/translations/translation_cz.xml @@ -177,6 +177,10 @@ + + + + diff --git a/translations/translation_da.xml b/translations/translation_da.xml index de5548477..c028ba1d7 100644 --- a/translations/translation_da.xml +++ b/translations/translation_da.xml @@ -177,6 +177,10 @@ + + + + diff --git a/translations/translation_de.xml b/translations/translation_de.xml index bc5921ceb..daad1b44f 100644 --- a/translations/translation_de.xml +++ b/translations/translation_de.xml @@ -177,6 +177,10 @@ + + + + diff --git a/translations/translation_ea.xml b/translations/translation_ea.xml index bd8405912..88becafb0 100644 --- a/translations/translation_ea.xml +++ b/translations/translation_ea.xml @@ -177,6 +177,10 @@ + + + + diff --git a/translations/translation_en.xml b/translations/translation_en.xml index 2c9fa16c1..e8b9dddfa 100644 --- a/translations/translation_en.xml +++ b/translations/translation_en.xml @@ -177,6 +177,10 @@ + + + + diff --git a/translations/translation_es.xml b/translations/translation_es.xml index 26a3e1a50..d711f4940 100644 --- a/translations/translation_es.xml +++ b/translations/translation_es.xml @@ -177,6 +177,10 @@ + + + + diff --git a/translations/translation_fc.xml b/translations/translation_fc.xml index 473990ecf..2d7d9c918 100644 --- a/translations/translation_fc.xml +++ b/translations/translation_fc.xml @@ -177,6 +177,10 @@ + + + + diff --git a/translations/translation_fi.xml b/translations/translation_fi.xml index 89648342b..caf72b507 100644 --- a/translations/translation_fi.xml +++ b/translations/translation_fi.xml @@ -177,6 +177,10 @@ + + + + diff --git a/translations/translation_fr.xml b/translations/translation_fr.xml index 6ffa53ea2..8cfc7c08a 100644 --- a/translations/translation_fr.xml +++ b/translations/translation_fr.xml @@ -177,6 +177,10 @@ + + + + diff --git a/translations/translation_hu.xml b/translations/translation_hu.xml index 8bb0cb24a..95a3bdcb0 100644 --- a/translations/translation_hu.xml +++ b/translations/translation_hu.xml @@ -177,6 +177,10 @@ + + + + diff --git a/translations/translation_id.xml b/translations/translation_id.xml index d4032d413..db3995fc0 100644 --- a/translations/translation_id.xml +++ b/translations/translation_id.xml @@ -177,6 +177,10 @@ + + + + diff --git a/translations/translation_it.xml b/translations/translation_it.xml index d74244bbe..f70bdf9d2 100644 --- a/translations/translation_it.xml +++ b/translations/translation_it.xml @@ -177,6 +177,10 @@ + + + + diff --git a/translations/translation_jp.xml b/translations/translation_jp.xml index ec6a9ff02..d7b2a2cae 100644 --- a/translations/translation_jp.xml +++ b/translations/translation_jp.xml @@ -177,6 +177,10 @@ + + + + diff --git a/translations/translation_kr.xml b/translations/translation_kr.xml index 0a826daa6..6a10b322a 100644 --- a/translations/translation_kr.xml +++ b/translations/translation_kr.xml @@ -177,6 +177,10 @@ + + + + diff --git a/translations/translation_nl.xml b/translations/translation_nl.xml index 519c4d119..0efed83fa 100644 --- a/translations/translation_nl.xml +++ b/translations/translation_nl.xml @@ -177,6 +177,10 @@ + + + + diff --git a/translations/translation_no.xml b/translations/translation_no.xml index e1ec5ad6e..49c062cb3 100644 --- a/translations/translation_no.xml +++ b/translations/translation_no.xml @@ -177,6 +177,10 @@ + + + + diff --git a/translations/translation_pl.xml b/translations/translation_pl.xml index 996c280da..ec24a2478 100644 --- a/translations/translation_pl.xml +++ b/translations/translation_pl.xml @@ -177,6 +177,10 @@ + + + + diff --git a/translations/translation_pt.xml b/translations/translation_pt.xml index 712112a3e..940f8adb9 100644 --- a/translations/translation_pt.xml +++ b/translations/translation_pt.xml @@ -177,6 +177,10 @@ + + + + diff --git a/translations/translation_ro.xml b/translations/translation_ro.xml index ea298a574..844b6d543 100644 --- a/translations/translation_ro.xml +++ b/translations/translation_ro.xml @@ -177,6 +177,10 @@ + + + + diff --git a/translations/translation_ru.xml b/translations/translation_ru.xml index 21b6e59c1..7ab51b871 100644 --- a/translations/translation_ru.xml +++ b/translations/translation_ru.xml @@ -177,6 +177,10 @@ + + + + diff --git a/translations/translation_sv.xml b/translations/translation_sv.xml index b0142a4df..6c39466f9 100644 --- a/translations/translation_sv.xml +++ b/translations/translation_sv.xml @@ -177,6 +177,10 @@ + + + + diff --git a/translations/translation_tr.xml b/translations/translation_tr.xml index 82f4e1a6c..0a41529fd 100644 --- a/translations/translation_tr.xml +++ b/translations/translation_tr.xml @@ -177,6 +177,10 @@ + + + + diff --git a/translations/translation_uk.xml b/translations/translation_uk.xml index b9200a925..80ab73562 100644 --- a/translations/translation_uk.xml +++ b/translations/translation_uk.xml @@ -177,6 +177,10 @@ + + + + diff --git a/translations/translation_vi.xml b/translations/translation_vi.xml index e3b0cacd9..d5a01bf14 100644 --- a/translations/translation_vi.xml +++ b/translations/translation_vi.xml @@ -177,6 +177,10 @@ + + + +