From f8054cc8f0bb65d998cf6e03eb42b5aa7c38544b Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Fri, 28 Feb 2025 10:04:43 -0500 Subject: [PATCH 01/28] feat: Graph pathfinder --- scripts/pathfinder/GraphPathfinder.lua | 113 +++++++++++++++++++++++++ 1 file changed, 113 insertions(+) create mode 100644 scripts/pathfinder/GraphPathfinder.lua diff --git a/scripts/pathfinder/GraphPathfinder.lua b/scripts/pathfinder/GraphPathfinder.lua new file mode 100644 index 000000000..c0e0938ec --- /dev/null +++ b/scripts/pathfinder/GraphPathfinder.lua @@ -0,0 +1,113 @@ +--- 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 = {} +GraphPathfinder.GraphEdge.BIDIRECTIONAL = {} + +---@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 + +---@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 + +---@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 Vector[] the graph as described in the file header +function GraphPathfinder:init(yieldAfter, maxIterations, range, graph) + HybridAStar.init(self, { }, yieldAfter, maxIterations) + self.range = range + self.graph = graph + self.deltaPosGoal = self.range + self.deltaThetaDeg = 181 + self.deltaThetaGoal = math.rad(self.deltaThetaDeg) + self.maxDeltaTheta = math.pi + self.originalDeltaThetaGoal = self.deltaThetaGoal + self.analyticSolverEnabled = false + self.ignoreValidityAtStart = false +end + +function GraphPathfinder:getMotionPrimitives(turnRadius, allowReverse) + return GraphPathfinder.GraphMotionPrimitives(self.range, self.graph) +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.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 = {} + 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} ) + end + end + end + return primitives +end + +---@return State3D successor for the given primitive +function GraphPathfinder.GraphMotionPrimitives:createSuccessor(node, primitive, hitchLength) + return State3D(primitive.x, primitive.y, 0, node.g, node, node.gear, node.steer, + 0, node.d + primitive.d) +end + From 3df8ef03e6e1b4550cfa235ac025d15c23db923b Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Fri, 28 Mar 2025 08:30:41 -0400 Subject: [PATCH 02/28] fix: goal node heading threshold --- scripts/pathfinder/GraphPathfinder.lua | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/scripts/pathfinder/GraphPathfinder.lua b/scripts/pathfinder/GraphPathfinder.lua index c0e0938ec..7c47b43cf 100644 --- a/scripts/pathfinder/GraphPathfinder.lua +++ b/scripts/pathfinder/GraphPathfinder.lua @@ -57,7 +57,7 @@ end ---@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 Vector[] the graph as described in the file header +---@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.range = range @@ -65,7 +65,7 @@ function GraphPathfinder:init(yieldAfter, maxIterations, range, graph) self.deltaPosGoal = self.range 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 From cfe683c970ff6b41eb1f15f2f606cb1abeb9195b Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Sat, 29 Mar 2025 07:44:26 -0400 Subject: [PATCH 03/28] feat: graph pathfinder Add all points of the edges to the path. Add unit tests. --- scripts/pathfinder/GraphPathfinder.lua | 61 +++++- .../pathfinder/test/GraphPathfinderTest.lua | 181 ++++++++++++++++++ 2 files changed, 239 insertions(+), 3 deletions(-) create mode 100644 scripts/pathfinder/test/GraphPathfinderTest.lua diff --git a/scripts/pathfinder/GraphPathfinder.lua b/scripts/pathfinder/GraphPathfinder.lua index 7c47b43cf..01d3e63d1 100644 --- a/scripts/pathfinder/GraphPathfinder.lua +++ b/scripts/pathfinder/GraphPathfinder.lua @@ -53,6 +53,39 @@ function GraphPathfinder.GraphEdge:getExit(entry) 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 + +--- 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 + ---@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 @@ -75,6 +108,28 @@ 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, path) + path = path or {} + 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 + if currentNode.edge then + -- add the edge leading to the node + for _, node in currentNode.edge:rollUpIterator(currentNode.entry) do + table.insert(path, 1, node) + end + end + currentNode = currentNode.pred + end + table.insert(path, 1, currentNode) + self:debug('Nodes %d, iterations %d, yields %d, deltaTheta %.1f', #path, self.iterations, self.yields, + math.deg(self.deltaThetaGoal)) + return path +end + --- Motion primitives to use with the graph pathfinder, providing the entries --- to the next edges. ---@class GraphPathfinder.GraphMotionPrimitives : HybridAStar.MotionPrimitives @@ -98,7 +153,8 @@ function GraphPathfinder.GraphMotionPrimitives:getPrimitives(node) 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} ) + table.insert(primitives, { x = exit.x, y = exit.y, d = edge:getLength() + distanceToEntry, + edge = edge, entry = entry }) end end end @@ -107,7 +163,6 @@ end ---@return State3D successor for the given primitive function GraphPathfinder.GraphMotionPrimitives:createSuccessor(node, primitive, hitchLength) - return State3D(primitive.x, primitive.y, 0, node.g, node, node.gear, node.steer, - 0, node.d + primitive.d) + return GraphPathfinder.Node(primitive.x, primitive.y, node.g, node, node.d + primitive.d, primitive.edge, primitive.entry) end diff --git a/scripts/pathfinder/test/GraphPathfinderTest.lua b/scripts/pathfinder/test/GraphPathfinderTest.lua new file mode 100644 index 000000000..1b11db763 --- /dev/null +++ b/scripts/pathfinder/test/GraphPathfinderTest.lua @@ -0,0 +1,181 @@ +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('PathfinderUtil') +require('HybridAStar') +require('GraphPathfinder') + +local GraphEdge = GraphPathfinder.GraphEdge +local TestConstraints = CpObject(PathfinderConstraintInterface) +local pathfinder, start, goal, done, path, goalNodeInvalid +local function printPath() + for _, p in ipairs(path) do + print(p) + end +end + +function 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, 20, graph) + start = State3D(90, 105, 0, 0) + goal = State3D(130, 105, 0, 0) + done, path, goalNodeInvalid = pathfinder:run(start, goal, 1, false, TestConstraints(), 0) + lu.assertIsTrue(done) + lu.assertEquals(#path, 4) + -- path contains the start node and all points of the edge it goes through + lu.assertEquals(start, path[1]) + path[2]:assertAlmostEquals(Vector(100, 100)) + path[3]:assertAlmostEquals(Vector(110, 100)) + path[#path]:assertAlmostEquals(Vector(120, 100)) + start, goal = goal, start + done, path, goalNodeInvalid = pathfinder:run(start, goal, 1, false, TestConstraints(), 0) + lu.assertIsTrue(done) + lu.assertEquals(#path, 4) + lu.assertEquals(start, path[1]) + path[2]:assertAlmostEquals(Vector(120, 105)) + path[3]:assertAlmostEquals(Vector(110, 105)) + path[#path]:assertAlmostEquals(Vector(100, 105)) +end + +function 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, 20, graph) + start = State3D(90, 105, 0, 0) + goal = State3D(130, 105, 0, 0) + done, path, goalNodeInvalid = pathfinder:run(start, goal, 1, false, TestConstraints(), 0) + lu.assertIsTrue(done) + lu.assertEquals(#path, 4) + -- path contains the start node and all points of the edge it goes through + lu.assertEquals(start, path[1]) + path[2]:assertAlmostEquals(Vector(100, 100)) + path[3]:assertAlmostEquals(Vector(110, 100)) + path[#path]:assertAlmostEquals(Vector(120, 100)) + start, goal = goal, start + done, path, goalNodeInvalid = pathfinder:run(start, goal, 1, false, TestConstraints(), 0) + lu.assertIsTrue(done) + lu.assertEquals(#path, 4) + lu.assertEquals(start, path[1]) + -- TODO: here, it should have taken the other path, over y = 105, as it is slightly shorter since both start and + -- goal are on y = 105, but since we reach the goal in a single step, + -- it just goes with the first one it finds. This isn't the hill we want to die on, so for now, + -- we will just accept this behavior. + path[2]:assertAlmostEquals(Vector(120, 100)) + path[3]:assertAlmostEquals(Vector(110, 100)) + path[#path]:assertAlmostEquals(Vector(100, 100)) +end + +function 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, 20, graph) + start = State3D(90, 105, 0, 0) + goal = State3D(130, 105, 0, 0) + done, path, goalNodeInvalid = pathfinder:run(start, goal, 1, false, TestConstraints(), 0) + lu.assertIsTrue(done) + lu.assertEquals(#path, 4) + -- path contains the start node and all points of the edge it goes through + lu.assertEquals(start, path[1]) + path[2]:assertAlmostEquals(Vector(100, 100)) + path[3]:assertAlmostEquals(Vector(110, 100)) + path[#path]:assertAlmostEquals(Vector(120, 100)) +end + +function 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, 20, graph) + start = State3D(90, 105, 0, 0) + goal = State3D(150, 105, 0, 0) + done, path, goalNodeInvalid = pathfinder:run(start, goal, 1, false, TestConstraints(), 0) + lu.assertIsTrue(done) + lu.assertEquals(#path, 7) + -- path contains the start node and all points of the edge it goes through + lu.assertEquals(start, path[1]) + path[2]:assertAlmostEquals(Vector(100, 100)) + path[3]:assertAlmostEquals(Vector(110, 100)) + path[#path]:assertAlmostEquals(Vector(150, 100)) + pathfinder = GraphPathfinder(math.huge, 500, 10, graph) + done, path, goalNodeInvalid = pathfinder:run(start, goal, 1, false, TestConstraints(), 0) + lu.assertIsNil(path) +end + + +os.exit(lu.LuaUnit.run()) From 1d1814ee986ab06a32937813c18ddacedbc72a75 Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Sat, 29 Mar 2025 07:46:13 -0400 Subject: [PATCH 04/28] test: pipeline and mocks --- .github/workflows/unit-test.yml | 2 ++ scripts/test/mock-Courseplay.lua | 21 ++++++++++++++++++++- scripts/test/mock-GiantsEngine.lua | 7 +++++++ 3 files changed, 29 insertions(+), 1 deletion(-) diff --git a/.github/workflows/unit-test.yml b/.github/workflows/unit-test.yml index 012636f3d..b02f9bdef 100644 --- a/.github/workflows/unit-test.yml +++ b/.github/workflows/unit-test.yml @@ -53,3 +53,5 @@ jobs: lua TransformTest.lua lua VertexTest.lua lua WrapAroundIndexTest.lua + cd scripts/pathfinder/test + lua GraphPathfinderTest.lua \ No newline at end of file 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 From 723cc8e4b22a5cff70d790d2f1685b5c3f85fc11 Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Sat, 29 Mar 2025 07:52:51 -0400 Subject: [PATCH 05/28] chore: pipeline fix --- .github/workflows/unit-test.yml | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/.github/workflows/unit-test.yml b/.github/workflows/unit-test.yml index b02f9bdef..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,5 +53,6 @@ jobs: lua TransformTest.lua lua VertexTest.lua lua WrapAroundIndexTest.lua - cd scripts/pathfinder/test + popd + pushd scripts/pathfinder/test lua GraphPathfinderTest.lua \ No newline at end of file From ae2adcc19a42779942c2fc078b67b5e89f88b676 Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Sat, 29 Mar 2025 10:29:17 -0400 Subject: [PATCH 06/28] feat: entry/exit graph mid-edge --- scripts/pathfinder/GraphPathfinder.lua | 51 +++++++++++++++++++ scripts/pathfinder/HybridAStar.lua | 1 + .../pathfinder/test/GraphPathfinderTest.lua | 51 +++++++++++++++++++ 3 files changed, 103 insertions(+) diff --git a/scripts/pathfinder/GraphPathfinder.lua b/scripts/pathfinder/GraphPathfinder.lua index 01d3e63d1..638003ec1 100644 --- a/scripts/pathfinder/GraphPathfinder.lua +++ b/scripts/pathfinder/GraphPathfinder.lua @@ -28,6 +28,10 @@ function GraphPathfinder.GraphEdge:init(direction, vertices) self.direction = direction 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 @@ -92,6 +96,7 @@ end --- 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) + self.logger = Logger('GraphPathfinder', Logger.level.debug, CpDebug.DBG_PATHFINDER) HybridAStar.init(self, { }, yieldAfter, maxIterations) self.range = range self.graph = graph @@ -104,6 +109,11 @@ function GraphPathfinder:init(yieldAfter, maxIterations, range, graph) 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 @@ -130,6 +140,43 @@ function GraphPathfinder:rollUpPath(lastNode, goal, path) return path end +function GraphPathfinder:initRun(start, goal, ...) + self:createGraphEntryAndExit(start, goal) + return HybridAStar.initRun(self, start, goal, ...) +end + +--- The start location may not be close to the start or end of an edge. Therefore, +--- we need to look for entries among all the vertices of all edges in the graph. When we find that vertex, and +--- it 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. +--- We do the same for the goal node to be able to exit the graph at the middle of an edge. +function GraphPathfinder:createGraphEntryAndExit(start, goal) + local function splitClosestEdge(node) + local closestEdge, closestVertex + local closestDistance = math.huge + for _, edge in ipairs(self.graph) do + local v, d = edge:findClosestVertexToPoint(node) + if d and d < closestDistance then + closestDistance = d + closestEdge = edge + closestVertex = v + end + end + if closestVertex.ix ~= 1 and closestVertex.ix ~= #closestEdge then + self.logger:trace('Graph entry found and split at vertex %d, %.1f %.1f', closestVertex.ix, closestVertex.x, closestVertex.y) + local newEdge = GraphPathfinder.GraphEdge(closestEdge:getDirection()) + for i = closestVertex.ix, #closestEdge do + newEdge:append(closestEdge[i]) + end + newEdge:calculateProperties() + table.insert(self.graph, newEdge) + closestEdge:cutEndAtIx(closestVertex.ix) + end + end + splitClosestEdge(start) + splitClosestEdge(goal) +end + --- Motion primitives to use with the graph pathfinder, providing the entries --- to the next edges. ---@class GraphPathfinder.GraphMotionPrimitives : HybridAStar.MotionPrimitives @@ -139,6 +186,7 @@ GraphPathfinder.GraphMotionPrimitives = CpObject(HybridAStar.MotionPrimitives) --- 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.debug, CpDebug.DBG_PATHFINDER) self.range = range self.graph = graph end @@ -155,6 +203,7 @@ function GraphPathfinder.GraphMotionPrimitives:getPrimitives(node) 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 @@ -163,6 +212,8 @@ 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 diff --git a/scripts/pathfinder/HybridAStar.lua b/scripts/pathfinder/HybridAStar.lua index cb4651cdb..e1228be6e 100644 --- a/scripts/pathfinder/HybridAStar.lua +++ b/scripts/pathfinder/HybridAStar.lua @@ -624,6 +624,7 @@ function HybridAStar:run(start, goal, turnRadius, allowReverse, constraints, hit 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 diff --git a/scripts/pathfinder/test/GraphPathfinderTest.lua b/scripts/pathfinder/test/GraphPathfinderTest.lua index 1b11db763..114cb1bcd 100644 --- a/scripts/pathfinder/test/GraphPathfinderTest.lua +++ b/scripts/pathfinder/test/GraphPathfinderTest.lua @@ -177,5 +177,56 @@ function testRange() lu.assertIsNil(path) end +function 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, 20, graph) + start = State3D(150, 95, 0, 0) + goal = State3D(95, 95, 0, 0) + done, path, goalNodeInvalid = pathfinder:run(start, goal, 1, false, TestConstraints(), 0) + lu.assertIsTrue(done) + lu.assertEquals(#path, 3) + -- path contains the start node and all points of the edge it goes through + lu.assertEquals(start, path[1]) + path[2]:assertAlmostEquals(Vector(150, 100)) + path[3]: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, goalNodeInvalid = pathfinder:run(start, goal, 1, false, TestConstraints(), 0) + printPath() + lu.assertIsTrue(done) + lu.assertEquals(#path, 3) + lu.assertEquals(start, path[1]) + path[2]:assertAlmostEquals(Vector(100, 100)) + path[3]:assertAlmostEquals(Vector(150, 100)) +end os.exit(lu.LuaUnit.run()) From ee49d3894004a8b92cf3f755d186e8bdfaf1d4b2 Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Sat, 29 Mar 2025 15:50:32 -0400 Subject: [PATCH 07/28] test: edges with two points only --- .../pathfinder/test/GraphPathfinderTest.lua | 34 +++++++++++++++++++ 1 file changed, 34 insertions(+) diff --git a/scripts/pathfinder/test/GraphPathfinderTest.lua b/scripts/pathfinder/test/GraphPathfinderTest.lua index 114cb1bcd..441e5340d 100644 --- a/scripts/pathfinder/test/GraphPathfinderTest.lua +++ b/scripts/pathfinder/test/GraphPathfinderTest.lua @@ -229,4 +229,38 @@ function testStartInTheMiddle() path[3]:assertAlmostEquals(Vector(150, 100)) end +function 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, 20, graph) + start = State3D(90, 105, 0, 0) + goal = State3D(130, 105, 0, 0) + done, path, goalNodeInvalid = pathfinder:run(start, goal, 1, false, TestConstraints(), 0) + lu.assertIsTrue(done) + lu.assertEquals(#path, 3) + -- path contains the start node and all points of the edge it goes through + lu.assertEquals(start, path[1]) + path[2]:assertAlmostEquals(Vector(100, 100)) + path[#path]:assertAlmostEquals(Vector(120, 100)) + start, goal = goal, start + done, path, goalNodeInvalid = pathfinder:run(start, goal, 1, false, TestConstraints(), 0) + lu.assertIsTrue(done) + lu.assertEquals(#path, 3) + lu.assertEquals(start, path[1]) + path[2]:assertAlmostEquals(Vector(120, 105)) + path[#path]:assertAlmostEquals(Vector(100, 105)) +end + + os.exit(lu.LuaUnit.run()) From 5ea29f74fdbb1f8e1b4df3468a7c5f8c4657c1d3 Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Sat, 29 Mar 2025 19:15:07 -0400 Subject: [PATCH 08/28] feat: more debugs --- scripts/pathfinder/GraphPathfinder.lua | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/scripts/pathfinder/GraphPathfinder.lua b/scripts/pathfinder/GraphPathfinder.lua index 638003ec1..9f4d3ba5a 100644 --- a/scripts/pathfinder/GraphPathfinder.lua +++ b/scripts/pathfinder/GraphPathfinder.lua @@ -163,7 +163,8 @@ function GraphPathfinder:createGraphEntryAndExit(start, goal) end end if closestVertex.ix ~= 1 and closestVertex.ix ~= #closestEdge then - self.logger:trace('Graph entry found and split at vertex %d, %.1f %.1f', closestVertex.ix, closestVertex.x, closestVertex.y) + self.logger:debug('Graph entry/exit found and split at vertex %d, d: %.1f, x: %.1f y: %.1f', + closestVertex.ix, closestDistance, closestVertex.x, closestVertex.y) local newEdge = GraphPathfinder.GraphEdge(closestEdge:getDirection()) for i = closestVertex.ix, #closestEdge do newEdge:append(closestEdge[i]) From dc050435ef00c0734718332753a7ecd8d8b768f8 Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Sun, 30 Mar 2025 07:32:50 -0400 Subject: [PATCH 09/28] feat: start/goal can be at any distance Don't limit the distance when finding the graph entry and exit points, start/goal can be arbitrarily far away from the graph. --- scripts/pathfinder/GraphPathfinder.lua | 39 +++-- .../pathfinder/test/GraphPathfinderTest.lua | 144 ++++++++++++------ 2 files changed, 129 insertions(+), 54 deletions(-) diff --git a/scripts/pathfinder/GraphPathfinder.lua b/scripts/pathfinder/GraphPathfinder.lua index 9f4d3ba5a..53d213cad 100644 --- a/scripts/pathfinder/GraphPathfinder.lua +++ b/scripts/pathfinder/GraphPathfinder.lua @@ -90,6 +90,17 @@ function GraphPathfinder.Node:init(x, y, g, pred, d, edge, entry) 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 @@ -134,22 +145,26 @@ function GraphPathfinder:rollUpPath(lastNode, goal, path) end currentNode = currentNode.pred end - table.insert(path, 1, currentNode) self:debug('Nodes %d, iterations %d, yields %d, deltaTheta %.1f', #path, self.iterations, self.yields, math.deg(self.deltaThetaGoal)) return path end function GraphPathfinder:initRun(start, goal, ...) - self:createGraphEntryAndExit(start, goal) - return HybridAStar.initRun(self, start, goal, ...) + local graphEntry, graphExit = self:createGraphEntryAndExit(start, goal) + return HybridAStar.initRun(self, graphEntry, graphExit, ...) end ---- The start location may not be close to the start or end of an edge. Therefore, ---- we need to look for entries among all the vertices of all edges in the graph. When we find that vertex, and +--- Create the entry and exit vertices for the graph. +--- +--- Look for the vertex closest to the start/end location. When we find that vertex, and --- it 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. ---- We do the same for the goal node to be able to exit the graph at the middle of an edge. +--- 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 splitClosestEdge(node) local closestEdge, closestVertex @@ -162,6 +177,8 @@ function GraphPathfinder:createGraphEntryAndExit(start, goal) closestVertex = v end end + -- 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 ~= #closestEdge then self.logger:debug('Graph entry/exit found and split at vertex %d, d: %.1f, x: %.1f y: %.1f', closestVertex.ix, closestDistance, closestVertex.x, closestVertex.y) @@ -173,9 +190,11 @@ function GraphPathfinder:createGraphEntryAndExit(start, goal) table.insert(self.graph, newEdge) closestEdge:cutEndAtIx(closestVertex.ix) end + return State3D(closestVertex.x, closestVertex.y, 0) end - splitClosestEdge(start) - splitClosestEdge(goal) + local entry, exit = splitClosestEdge(start), splitClosestEdge(goal) + self.logger:debug('Graph entry at (%.1f, %.1f), exit at (%.1f, %.1f)', entry.x, entry.y, exit.x, exit.y) + return entry, exit end --- Motion primitives to use with the graph pathfinder, providing the entries @@ -187,7 +206,7 @@ GraphPathfinder.GraphMotionPrimitives = CpObject(HybridAStar.MotionPrimitives) --- 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.debug, CpDebug.DBG_PATHFINDER) + self.logger = Logger('GraphMotionPrimitives', Logger.level.trace, CpDebug.DBG_PATHFINDER) self.range = range self.graph = graph end diff --git a/scripts/pathfinder/test/GraphPathfinderTest.lua b/scripts/pathfinder/test/GraphPathfinderTest.lua index 441e5340d..090d5cd1d 100644 --- a/scripts/pathfinder/test/GraphPathfinderTest.lua +++ b/scripts/pathfinder/test/GraphPathfinderTest.lua @@ -28,7 +28,7 @@ local TestConstraints = CpObject(PathfinderConstraintInterface) local pathfinder, start, goal, done, path, goalNodeInvalid local function printPath() for _, p in ipairs(path) do - print(p) + print(Vector.__tostring(p)) end end @@ -53,19 +53,17 @@ function testDirection() goal = State3D(130, 105, 0, 0) done, path, goalNodeInvalid = pathfinder:run(start, goal, 1, false, TestConstraints(), 0) lu.assertIsTrue(done) - lu.assertEquals(#path, 4) - -- path contains the start node and all points of the edge it goes through - lu.assertEquals(start, path[1]) - path[2]:assertAlmostEquals(Vector(100, 100)) - path[3]:assertAlmostEquals(Vector(110, 100)) + 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, goalNodeInvalid = pathfinder:run(start, goal, 1, false, TestConstraints(), 0) lu.assertIsTrue(done) - lu.assertEquals(#path, 4) - lu.assertEquals(start, path[1]) - path[2]:assertAlmostEquals(Vector(120, 105)) - path[3]:assertAlmostEquals(Vector(110, 105)) + lu.assertEquals(#path, 3) + path[1]:assertAlmostEquals(Vector(120, 105)) + path[2]:assertAlmostEquals(Vector(110, 105)) path[#path]:assertAlmostEquals(Vector(100, 105)) end @@ -90,23 +88,21 @@ function testBidirectional() goal = State3D(130, 105, 0, 0) done, path, goalNodeInvalid = pathfinder:run(start, goal, 1, false, TestConstraints(), 0) lu.assertIsTrue(done) - lu.assertEquals(#path, 4) + lu.assertEquals(#path, 3) -- path contains the start node and all points of the edge it goes through - lu.assertEquals(start, path[1]) - path[2]:assertAlmostEquals(Vector(100, 100)) - path[3]:assertAlmostEquals(Vector(110, 100)) + path[1]:assertAlmostEquals(Vector(100, 100)) + path[2]:assertAlmostEquals(Vector(110, 100)) path[#path]:assertAlmostEquals(Vector(120, 100)) start, goal = goal, start done, path, goalNodeInvalid = pathfinder:run(start, goal, 1, false, TestConstraints(), 0) lu.assertIsTrue(done) - lu.assertEquals(#path, 4) - lu.assertEquals(start, path[1]) + lu.assertEquals(#path, 3) -- TODO: here, it should have taken the other path, over y = 105, as it is slightly shorter since both start and -- goal are on y = 105, but since we reach the goal in a single step, -- it just goes with the first one it finds. This isn't the hill we want to die on, so for now, -- we will just accept this behavior. - path[2]:assertAlmostEquals(Vector(120, 100)) - path[3]:assertAlmostEquals(Vector(110, 100)) + path[1]:assertAlmostEquals(Vector(120, 100)) + path[2]:assertAlmostEquals(Vector(110, 100)) path[#path]:assertAlmostEquals(Vector(100, 100)) end @@ -131,11 +127,9 @@ function testShorterPath() goal = State3D(130, 105, 0, 0) done, path, goalNodeInvalid = pathfinder:run(start, goal, 1, false, TestConstraints(), 0) lu.assertIsTrue(done) - lu.assertEquals(#path, 4) - -- path contains the start node and all points of the edge it goes through - lu.assertEquals(start, path[1]) - path[2]:assertAlmostEquals(Vector(100, 100)) - path[3]:assertAlmostEquals(Vector(110, 100)) + lu.assertEquals(#path, 3) + path[1]:assertAlmostEquals(Vector(100, 100)) + path[2]:assertAlmostEquals(Vector(110, 100)) path[#path]:assertAlmostEquals(Vector(120, 100)) end @@ -166,13 +160,11 @@ function testRange() goal = State3D(150, 105, 0, 0) done, path, goalNodeInvalid = pathfinder:run(start, goal, 1, false, TestConstraints(), 0) lu.assertIsTrue(done) - lu.assertEquals(#path, 7) - -- path contains the start node and all points of the edge it goes through - lu.assertEquals(start, path[1]) - path[2]:assertAlmostEquals(Vector(100, 100)) - path[3]:assertAlmostEquals(Vector(110, 100)) + 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, 10, graph) + pathfinder = GraphPathfinder(math.huge, 500, 9, graph) done, path, goalNodeInvalid = pathfinder:run(start, goal, 1, false, TestConstraints(), 0) lu.assertIsNil(path) end @@ -198,11 +190,9 @@ function testStartInTheMiddle() goal = State3D(95, 95, 0, 0) done, path, goalNodeInvalid = pathfinder:run(start, goal, 1, false, TestConstraints(), 0) lu.assertIsTrue(done) - lu.assertEquals(#path, 3) - -- path contains the start node and all points of the edge it goes through - lu.assertEquals(start, path[1]) - path[2]:assertAlmostEquals(Vector(150, 100)) - path[3]:assertAlmostEquals(Vector(100, 100)) + lu.assertEquals(#path, 2) + path[1]:assertAlmostEquals(Vector(150, 100)) + path[2]:assertAlmostEquals(Vector(100, 100)) graph = { GraphEdge(GraphEdge.BIDIRECTIONAL, { @@ -223,10 +213,9 @@ function testStartInTheMiddle() done, path, goalNodeInvalid = pathfinder:run(start, goal, 1, false, TestConstraints(), 0) printPath() lu.assertIsTrue(done) - lu.assertEquals(#path, 3) - lu.assertEquals(start, path[1]) - path[2]:assertAlmostEquals(Vector(100, 100)) - path[3]:assertAlmostEquals(Vector(150, 100)) + lu.assertEquals(#path, 2) + path[1]:assertAlmostEquals(Vector(100, 100)) + path[2]:assertAlmostEquals(Vector(150, 100)) end function testTwoPointSegments() @@ -248,19 +237,86 @@ function testTwoPointSegments() goal = State3D(130, 105, 0, 0) done, path, goalNodeInvalid = pathfinder:run(start, goal, 1, false, TestConstraints(), 0) lu.assertIsTrue(done) - lu.assertEquals(#path, 3) - -- path contains the start node and all points of the edge it goes through - lu.assertEquals(start, path[1]) - path[2]:assertAlmostEquals(Vector(100, 100)) + lu.assertEquals(#path, 2) + path[1]:assertAlmostEquals(Vector(100, 100)) path[#path]:assertAlmostEquals(Vector(120, 100)) start, goal = goal, start done, path, goalNodeInvalid = pathfinder:run(start, goal, 1, false, TestConstraints(), 0) lu.assertIsTrue(done) + lu.assertEquals(#path, 2) + path[1]:assertAlmostEquals(Vector(120, 105)) + path[#path]:assertAlmostEquals(Vector(100, 105)) +end + +function 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, goalNodeInvalid = pathfinder:run(start, goal, 1, false, TestConstraints(), 0) + 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, goalNodeInvalid = pathfinder:run(start, goal, 1, false, TestConstraints(), 0) + lu.assertIsTrue(done) lu.assertEquals(#path, 3) - lu.assertEquals(start, path[1]) - path[2]:assertAlmostEquals(Vector(120, 105)) + -- 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, 0, 0, 0) + done, path, goalNodeInvalid = pathfinder:run(start, goal, 1, false, TestConstraints(), 0) + lu.assertIsTrue(done) + lu.assertEquals(#path, 2) + path[1]:assertAlmostEquals(Vector(110, 100)) + path[#path]:assertAlmostEquals(Vector(120, 100)) end +function testGoalWithinRange() + + 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) + print('******** single ******') + done, path, goalNodeInvalid = pathfinder:run(start, goal, 1, false, TestConstraints(), 0) + lu.assertIsTrue(done) + lu.assertEquals(#path, 2) + -- 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)) +end os.exit(lu.LuaUnit.run()) From 2f3dd1176ae67cf1d642a6f570550a4bd3b17c4c Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Sun, 30 Mar 2025 08:06:11 -0400 Subject: [PATCH 10/28] refactor: big scary pathfinder refactoring Always return PathfinderResult, also in private functions. --- scripts/pathfinder/HybridAStar.lua | 38 ++++++------- .../HybridAStarWithAStarInTheMiddle.lua | 55 +++++++++---------- .../pathfinder/test/GraphPathfinderTest.lua | 41 ++++++++------ 3 files changed, 71 insertions(+), 63 deletions(-) diff --git a/scripts/pathfinder/HybridAStar.lua b/scripts/pathfinder/HybridAStar.lua index e1228be6e..997901468 100644 --- a/scripts/pathfinder/HybridAStar.lua +++ b/scripts/pathfinder/HybridAStar.lua @@ -63,6 +63,7 @@ end -- -- After start(), call resume() until it returns done == true. ---@see PathfinderInterface#run also on how to use. +---@return PathfinderResult function PathfinderInterface:start(...) if not self.coroutine then self.coroutine = coursePlayCoroutine.create(self.run) @@ -83,19 +84,17 @@ function PathfinderInterface:isActive() end --- Resume the pathfinding ----@return boolean true if the pathfinding is done, false if it isn't ready. In this case you'll have to call resume() again ----@return Polyline path if the path found or nil if none found. --- @return array of the points of the grid used for the pathfinding, for test purposes only +---@return PathfinderResult function PathfinderInterface:resume(...) - local ok, done, path, goalNodeInvalid = coursePlayCoroutine.resume(self.coroutine, self, ...) - if not ok or done then + local ok, result = coursePlayCoroutine.resume(self.coroutine, self, ...) + if not ok or result.done then if not ok then - print(done) + print(result.done) end self.coroutine = nil - return true, path, goalNodeInvalid + return result end - return false + return PathfinderResult(false) end function PathfinderInterface:debug(...) @@ -504,7 +503,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, {}|nil, boolean done, path, goal node invalid +---@return PathfinderResult function HybridAStar:initRun(start, goal, turnRadius, allowReverse, constraints, hitchLength) self:debug('Start pathfinding between %s and %s', tostring(start), tostring(goal)) self:debug(' turnRadius = %.1f, allowReverse: %s', turnRadius, tostring(allowReverse)) @@ -533,12 +532,12 @@ function HybridAStar:initRun(start, goal, turnRadius, allowReverse, constraints, -- ignore trailer for the first check, we don't know its heading anyway if not constraints:isValidNode(goal, true) then self:debug('Goal node is invalid, abort pathfinding.') - return true, nil, true + return PathfinderResult(true, nil, true) end if not constraints:isValidAnalyticSolutionNode(goal, true) then -- goal node is invalid (for example in fruit), does not make sense to try analytic solutions - self.goalNodeIsInvalid = true + self.goalNodeInvalid = true self:debug('Goal node is invalid for analytical path.') end @@ -548,7 +547,7 @@ function HybridAStar:initRun(start, goal, turnRadius, allowReverse, constraints, if self:isPathValid(analyticPath) then self:debug('Found collision free analytic path (%s) from start to goal', pathType) CourseGenerator.addDebugPolyline(Polyline(analyticPath)) - return true, analyticPath + return PathfinderResult(true, analyticPath, self.goalNodeInvalid) end self:debug('Length of analytic solution is %.1f', analyticSolutionLength) end @@ -561,7 +560,7 @@ function HybridAStar:initRun(start, goal, turnRadius, allowReverse, constraints, self.expansions = 0 self.yields = 0 self.initialized = true - return false + return PathfinderResult(false) end --- Wrap up this run, clean up timer, reset initialized flag so next run will start cleanly @@ -569,15 +568,16 @@ function HybridAStar:finishRun(result, path) self.initialized = false self.constraints:showStatistics() closeIntervalTimer(self.timer) - return result, path + return PathfinderResult(result, path, self.goalNodeInvalid) end --- Reentry-safe pathfinder runner +---@return PathfinderResult function HybridAStar:run(start, goal, turnRadius, allowReverse, constraints, hitchLength) if not self.initialized then - local done, path, goalNodeInvalid = self:initRun(start, goal, turnRadius, allowReverse, constraints, hitchLength) - if done then - return done, path, goalNodeInvalid + local result = self:initRun(start, goal, turnRadius, allowReverse, constraints, hitchLength) + if result.done then + return result end end self.timer = openIntervalTimer() @@ -598,13 +598,13 @@ function HybridAStar:run(start, goal, turnRadius, allowReverse, constraints, hit self.yields = self.yields + 1 closeIntervalTimer(self.timer) -- if we had the coroutine package, we would coursePlayCoroutine.yield(false) here - return false + return PathfinderResult(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) diff --git a/scripts/pathfinder/HybridAStarWithAStarInTheMiddle.lua b/scripts/pathfinder/HybridAStarWithAStarInTheMiddle.lua index b6064842c..261c3178e 100644 --- a/scripts/pathfinder/HybridAStarWithAStarInTheMiddle.lua +++ b/scripts/pathfinder/HybridAStarWithAStarInTheMiddle.lua @@ -43,12 +43,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) @@ -68,6 +63,7 @@ function HybridAStarWithAStarInTheMiddle:start(start, goal, turnRadius, allowRev 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 +73,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 +85,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 +96,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 self.constraints:resetStrictMode() - if not path then + if not result.path then self:debug('fast A*: no path found') - return PathfinderResult(true, nil, goalNodeInvalid, self.currentPathfinder:getHighestDistance(), + 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 +145,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 +162,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 diff --git a/scripts/pathfinder/test/GraphPathfinderTest.lua b/scripts/pathfinder/test/GraphPathfinderTest.lua index 090d5cd1d..e7ee504d6 100644 --- a/scripts/pathfinder/test/GraphPathfinderTest.lua +++ b/scripts/pathfinder/test/GraphPathfinderTest.lua @@ -25,13 +25,22 @@ require('GraphPathfinder') local GraphEdge = GraphPathfinder.GraphEdge local TestConstraints = CpObject(PathfinderConstraintInterface) -local pathfinder, start, goal, done, path, goalNodeInvalid +local pathfinder, start, goal, done, path + local function printPath() for _, p in ipairs(path) do print(Vector.__tostring(p)) end end +local function runPathfinder() + local result = pathfinder:start(start, goal, 1, false, TestConstraints(), 0) + while not result.done do + result = pathfinder:resume() + end + return result.done, result.path +end + function testDirection() local graph = { GraphEdge(GraphEdge.UNIDIRECTIONAL, @@ -51,7 +60,7 @@ function testDirection() pathfinder = GraphPathfinder(math.huge, 500, 20, graph) start = State3D(90, 105, 0, 0) goal = State3D(130, 105, 0, 0) - done, path, goalNodeInvalid = pathfinder:run(start, goal, 1, false, TestConstraints(), 0) + done, path, _ = runPathfinder() lu.assertIsTrue(done) lu.assertEquals(#path, 3) -- path contains all points of the edge it goes through @@ -59,7 +68,7 @@ function testDirection() path[2]:assertAlmostEquals(Vector(110, 100)) path[#path]:assertAlmostEquals(Vector(120, 100)) start, goal = goal, start - done, path, goalNodeInvalid = pathfinder:run(start, goal, 1, false, TestConstraints(), 0) + done, path, _ = runPathfinder() lu.assertIsTrue(done) lu.assertEquals(#path, 3) path[1]:assertAlmostEquals(Vector(120, 105)) @@ -86,7 +95,7 @@ function testBidirectional() pathfinder = GraphPathfinder(math.huge, 500, 20, graph) start = State3D(90, 105, 0, 0) goal = State3D(130, 105, 0, 0) - done, path, goalNodeInvalid = pathfinder:run(start, goal, 1, false, TestConstraints(), 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 @@ -94,7 +103,7 @@ function testBidirectional() path[2]:assertAlmostEquals(Vector(110, 100)) path[#path]:assertAlmostEquals(Vector(120, 100)) start, goal = goal, start - done, path, goalNodeInvalid = pathfinder:run(start, goal, 1, false, TestConstraints(), 0) + done, path, _ = runPathfinder() lu.assertIsTrue(done) lu.assertEquals(#path, 3) -- TODO: here, it should have taken the other path, over y = 105, as it is slightly shorter since both start and @@ -125,7 +134,7 @@ function testShorterPath() pathfinder = GraphPathfinder(math.huge, 500, 20, graph) start = State3D(90, 105, 0, 0) goal = State3D(130, 105, 0, 0) - done, path, goalNodeInvalid = pathfinder:run(start, goal, 1, false, TestConstraints(), 0) + done, path, _ = runPathfinder() lu.assertIsTrue(done) lu.assertEquals(#path, 3) path[1]:assertAlmostEquals(Vector(100, 100)) @@ -158,14 +167,14 @@ function testRange() pathfinder = GraphPathfinder(math.huge, 500, 20, graph) start = State3D(90, 105, 0, 0) goal = State3D(150, 105, 0, 0) - done, path, goalNodeInvalid = pathfinder:run(start, goal, 1, false, TestConstraints(), 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, goalNodeInvalid = pathfinder:run(start, goal, 1, false, TestConstraints(), 0) + done, path, _ = runPathfinder() lu.assertIsNil(path) end @@ -188,7 +197,7 @@ function testStartInTheMiddle() pathfinder = GraphPathfinder(math.huge, 500, 20, graph) start = State3D(150, 95, 0, 0) goal = State3D(95, 95, 0, 0) - done, path, goalNodeInvalid = pathfinder:run(start, goal, 1, false, TestConstraints(), 0) + done, path, _ = runPathfinder() lu.assertIsTrue(done) lu.assertEquals(#path, 2) path[1]:assertAlmostEquals(Vector(150, 100)) @@ -210,7 +219,7 @@ function testStartInTheMiddle() } pathfinder = GraphPathfinder(math.huge, 500, 10, graph) start, goal = goal, start - done, path, goalNodeInvalid = pathfinder:run(start, goal, 1, false, TestConstraints(), 0) + done, path, _ = runPathfinder() printPath() lu.assertIsTrue(done) lu.assertEquals(#path, 2) @@ -235,13 +244,13 @@ function testTwoPointSegments() pathfinder = GraphPathfinder(math.huge, 500, 20, graph) start = State3D(90, 105, 0, 0) goal = State3D(130, 105, 0, 0) - done, path, goalNodeInvalid = pathfinder:run(start, goal, 1, false, TestConstraints(), 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, goalNodeInvalid = pathfinder:run(start, goal, 1, false, TestConstraints(), 0) + done, path, _ = runPathfinder() lu.assertIsTrue(done) lu.assertEquals(#path, 2) path[1]:assertAlmostEquals(Vector(120, 105)) @@ -269,7 +278,7 @@ function testEntryExit() -- start/goal far away start = State3D(0, 0, 0, 0) goal = State3D(130, 0, 0, 0) - done, path, goalNodeInvalid = pathfinder:run(start, goal, 1, false, TestConstraints(), 0) + done, path, _ = runPathfinder() lu.assertIsTrue(done) lu.assertEquals(#path, 3) -- path contains all points of the edge it goes through @@ -279,7 +288,7 @@ function testEntryExit() -- start/goal far away start = State3D(130, 200, 0, 0) goal = State3D(0, 200, 0, 0) - done, path, goalNodeInvalid = pathfinder:run(start, goal, 1, false, TestConstraints(), 0) + done, path, _ = runPathfinder() lu.assertIsTrue(done) lu.assertEquals(#path, 3) -- path contains all points of the edge it goes through @@ -289,7 +298,7 @@ function testEntryExit() -- start/goal far away, middle entry start = State3D(110, 0, 0, 0) goal = State3D(130, 0, 0, 0) - done, path, goalNodeInvalid = pathfinder:run(start, goal, 1, false, TestConstraints(), 0) + done, path, _ = runPathfinder() lu.assertIsTrue(done) lu.assertEquals(#path, 2) path[1]:assertAlmostEquals(Vector(110, 100)) @@ -310,7 +319,7 @@ function testGoalWithinRange() start = State3D(100, 100, 0, 0) goal = State3D(120, 100, 0, 0) print('******** single ******') - done, path, goalNodeInvalid = pathfinder:run(start, goal, 1, false, TestConstraints(), 0) + done, path, _ = runPathfinder() lu.assertIsTrue(done) lu.assertEquals(#path, 2) -- path contains all points of the edge it goes through From 753a614b47d06f529010e349fd314436d1e92b6c Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Sun, 30 Mar 2025 12:23:05 -0400 Subject: [PATCH 11/28] feat: return goalNodeInvalid if entry too close to exit If the graph entry (closest vertex to start location) is closer to the graph exit (closest vertex to the goal location) than the range, the GraphPathfinder can't generate a useful path. Return goalNodeInvalid == true in this case and log an error. --- scripts/pathfinder/GraphPathfinder.lua | 7 +++++ .../pathfinder/test/GraphPathfinderTest.lua | 28 ++++++++----------- 2 files changed, 19 insertions(+), 16 deletions(-) diff --git a/scripts/pathfinder/GraphPathfinder.lua b/scripts/pathfinder/GraphPathfinder.lua index 53d213cad..262e76e47 100644 --- a/scripts/pathfinder/GraphPathfinder.lua +++ b/scripts/pathfinder/GraphPathfinder.lua @@ -152,6 +152,13 @@ end function GraphPathfinder:initRun(start, goal, ...) 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) + return PathfinderResult(true, nil, true) + end return HybridAStar.initRun(self, graphEntry, graphExit, ...) end diff --git a/scripts/pathfinder/test/GraphPathfinderTest.lua b/scripts/pathfinder/test/GraphPathfinderTest.lua index e7ee504d6..9065c5b32 100644 --- a/scripts/pathfinder/test/GraphPathfinderTest.lua +++ b/scripts/pathfinder/test/GraphPathfinderTest.lua @@ -25,7 +25,7 @@ require('GraphPathfinder') local GraphEdge = GraphPathfinder.GraphEdge local TestConstraints = CpObject(PathfinderConstraintInterface) -local pathfinder, start, goal, done, path +local pathfinder, start, goal, done, path, goalNodeInvalid local function printPath() for _, p in ipairs(path) do @@ -38,7 +38,7 @@ local function runPathfinder() while not result.done do result = pathfinder:resume() end - return result.done, result.path + return result.done, result.path, result.goalNodeInvalid end function testDirection() @@ -57,7 +57,7 @@ function testDirection() Vertex(100, 105), }), } - pathfinder = GraphPathfinder(math.huge, 500, 20, graph) + pathfinder = GraphPathfinder(math.huge, 500, 10, graph) start = State3D(90, 105, 0, 0) goal = State3D(130, 105, 0, 0) done, path, _ = runPathfinder() @@ -92,7 +92,7 @@ function testBidirectional() Vertex(100, 105), }), } - pathfinder = GraphPathfinder(math.huge, 500, 20, graph) + pathfinder = GraphPathfinder(math.huge, 500, 10, graph) start = State3D(90, 105, 0, 0) goal = State3D(130, 105, 0, 0) done, path, _ = runPathfinder() @@ -131,7 +131,7 @@ function testShorterPath() Vertex(120, 105), }), } - pathfinder = GraphPathfinder(math.huge, 500, 20, graph) + pathfinder = GraphPathfinder(math.huge, 500, 10, graph) start = State3D(90, 105, 0, 0) goal = State3D(130, 105, 0, 0) done, path, _ = runPathfinder() @@ -164,7 +164,7 @@ function testRange() Vertex(150, 100) }), } - pathfinder = GraphPathfinder(math.huge, 500, 20, graph) + pathfinder = GraphPathfinder(math.huge, 500, 10, graph) start = State3D(90, 105, 0, 0) goal = State3D(150, 105, 0, 0) done, path, _ = runPathfinder() @@ -194,7 +194,7 @@ function testStartInTheMiddle() Vertex(100, 105), }), } - pathfinder = GraphPathfinder(math.huge, 500, 20, graph) + pathfinder = GraphPathfinder(math.huge, 500, 10, graph) start = State3D(150, 95, 0, 0) goal = State3D(95, 95, 0, 0) done, path, _ = runPathfinder() @@ -241,7 +241,7 @@ function testTwoPointSegments() Vertex(100, 105), }), } - pathfinder = GraphPathfinder(math.huge, 500, 20, graph) + pathfinder = GraphPathfinder(math.huge, 500, 10, graph) start = State3D(90, 105, 0, 0) goal = State3D(130, 105, 0, 0) done, path, _ = runPathfinder() @@ -306,7 +306,7 @@ function testEntryExit() end function testGoalWithinRange() - + -- goal too close to start (graph entry too close to graph exit) local graph = { GraphEdge(GraphEdge.UNIDIRECTIONAL, { @@ -318,14 +318,10 @@ function testGoalWithinRange() -- start/goal far away start = State3D(100, 100, 0, 0) goal = State3D(120, 100, 0, 0) - print('******** single ******') - done, path, _ = runPathfinder() + done, path, goalNodeInvalid = runPathfinder() lu.assertIsTrue(done) - lu.assertEquals(#path, 2) - -- 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)) + lu.assertIsTrue(goalNodeInvalid) + lu.assertIsNil(path) end os.exit(lu.LuaUnit.run()) From 798e92a3aea4255629b06f04ed46d2e9fc7b8eda Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Wed, 2 Apr 2025 10:02:38 -0400 Subject: [PATCH 12/28] feat: can make left turns to two-lane roads --- scripts/geometry/Polyline.lua | 6 +- scripts/pathfinder/GraphPathfinder.lua | 137 ++++++++++++++---- scripts/pathfinder/HybridAStar.lua | 17 ++- .../pathfinder/test/GraphPathfinderTest.lua | 51 ++++++- 4 files changed, 163 insertions(+), 48 deletions(-) diff --git a/scripts/geometry/Polyline.lua b/scripts/geometry/Polyline.lua index 0292892d5..9ab07227a 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 diff --git a/scripts/pathfinder/GraphPathfinder.lua b/scripts/pathfinder/GraphPathfinder.lua index 262e76e47..29a360bd8 100644 --- a/scripts/pathfinder/GraphPathfinder.lua +++ b/scripts/pathfinder/GraphPathfinder.lua @@ -28,6 +28,17 @@ function GraphPathfinder.GraphEdge:init(direction, 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 @@ -76,6 +87,14 @@ function GraphPathfinder.GraphEdge:rollUpIterator(entry) 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) @@ -107,10 +126,14 @@ end --- 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) - self.logger = Logger('GraphPathfinder', Logger.level.debug, CpDebug.DBG_PATHFINDER) HybridAStar.init(self, { }, yieldAfter, maxIterations) + self.logger = Logger('GraphPathfinder', Logger.level.trace, CpDebug.DBG_PATHFINDER) self.range = range - self.graph = graph + -- make a copy of the graph as we'll modify it + self.graph = {} + for _, e in ipairs(graph) do + table.insert(self.graph, e:clone()) + end self.deltaPosGoal = self.range self.deltaThetaDeg = 181 self.deltaThetaGoal = math.rad(self.deltaThetaDeg) @@ -137,9 +160,10 @@ function GraphPathfinder:rollUpPath(lastNode, goal, path) 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 - if currentNode.edge then - -- add the edge leading to the node - for _, node in currentNode.edge:rollUpIterator(currentNode.entry) do + -- add the edge leading to the node + for _, node in currentNode.edge:rollUpIterator(currentNode.entry) do + if node ~= path[1] then + -- don't insert the same node twice (we'll have the same node twice when we split edges) table.insert(path, 1, node) end end @@ -159,49 +183,97 @@ function GraphPathfinder:initRun(start, goal, ...) self.range, distance) return PathfinderResult(true, nil, true) end - return HybridAStar.initRun(self, graphEntry, graphExit, ...) + return HybridAStar.initRun(self, start, goal, ...) end ---- Create the entry and exit vertices for the graph. +--- 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. --- ---- Look for the vertex closest to the start/end location. When we find that vertex, and ---- it 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. +--- 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 splitClosestEdge(node) - local closestEdge, closestVertex - local closestDistance = math.huge - for _, edge in ipairs(self.graph) do - local v, d = edge:findClosestVertexToPoint(node) - if d and d < closestDistance then - closestDistance = d - closestEdge = edge - closestVertex = v - end - end + + 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 ~= #closestEdge then - self.logger:debug('Graph entry/exit found and split at vertex %d, d: %.1f, x: %.1f y: %.1f', - closestVertex.ix, closestDistance, closestVertex.x, closestVertex.y) - local newEdge = GraphPathfinder.GraphEdge(closestEdge:getDirection()) - for i = closestVertex.ix, #closestEdge do - newEdge:append(closestEdge[i]) + 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) - closestEdge:cutEndAtIx(closestVertex.ix) + 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 - return State3D(closestVertex.x, closestVertex.y, 0) end - local entry, exit = splitClosestEdge(start), splitClosestEdge(goal) - self.logger:debug('Graph entry at (%.1f, %.1f), exit at (%.1f, %.1f)', entry.x, entry.y, exit.x, exit.y) - return entry, exit + return State3D(entryEdges[1].vertex.x, entryEdges[1].vertex.y, 0, 0), + State3D(exitEdges[1].vertex.x, exitEdges[1].vertex.y, 0, 0) end --- Motion primitives to use with the graph pathfinder, providing the entries @@ -222,6 +294,7 @@ end --- 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 diff --git a/scripts/pathfinder/HybridAStar.lua b/scripts/pathfinder/HybridAStar.lua index 997901468..87df50139 100644 --- a/scripts/pathfinder/HybridAStar.lua +++ b/scripts/pathfinder/HybridAStar.lua @@ -455,6 +455,7 @@ HybridAStar.defaultMaxIterations = 40000 ---@param maxIterations number ---@param mustBeAccurate boolean|nil function HybridAStar:init(vehicle, yieldAfter, maxIterations, mustBeAccurate) + self.logger = Logger('HybridAStar', Logger.level.error, CpDebug.DBG_PATHFINDER) self.vehicle = vehicle self.count = 0 self.yields = 0 @@ -585,7 +586,7 @@ function HybridAStar:run(start, goal, turnRadius, allowReverse, constraints, hit -- 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)) if pred:equals(self.goal, self.deltaPosGoal, self.deltaThetaGoal) then -- done! @@ -641,16 +642,16 @@ function HybridAStar:run(start, goal, turnRadius, allowReverse, constraints, hit 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) @@ -660,7 +661,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 @@ -669,13 +670,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/test/GraphPathfinderTest.lua b/scripts/pathfinder/test/GraphPathfinderTest.lua index 9065c5b32..8ea6aa8d2 100644 --- a/scripts/pathfinder/test/GraphPathfinderTest.lua +++ b/scripts/pathfinder/test/GraphPathfinderTest.lua @@ -106,13 +106,9 @@ function testBidirectional() done, path, _ = runPathfinder() lu.assertIsTrue(done) lu.assertEquals(#path, 3) - -- TODO: here, it should have taken the other path, over y = 105, as it is slightly shorter since both start and - -- goal are on y = 105, but since we reach the goal in a single step, - -- it just goes with the first one it finds. This isn't the hill we want to die on, so for now, - -- we will just accept this behavior. - path[1]:assertAlmostEquals(Vector(120, 100)) - path[2]:assertAlmostEquals(Vector(110, 100)) - path[#path]:assertAlmostEquals(Vector(100, 100)) + path[1]:assertAlmostEquals(Vector(120, 105)) + path[2]:assertAlmostEquals(Vector(110, 105)) + path[#path]:assertAlmostEquals(Vector(100, 105)) end function testShorterPath() @@ -168,6 +164,7 @@ function testRange() start = State3D(90, 105, 0, 0) goal = State3D(150, 105, 0, 0) done, path, _ = runPathfinder() + printPath() lu.assertIsTrue(done) lu.assertEquals(#path, 6) path[1]:assertAlmostEquals(Vector(100, 100)) @@ -324,4 +321,44 @@ function testGoalWithinRange() lu.assertIsNil(path) end +function 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 + os.exit(lu.LuaUnit.run()) From accd2a17de09a515d987c84e6a225f0f5e4fccab Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Fri, 4 Apr 2025 21:02:59 -0400 Subject: [PATCH 13/28] feat: transition between edges --- scripts/geometry/Polyline.lua | 11 ++- scripts/pathfinder/GraphPathfinder.lua | 59 +++++++++++---- scripts/pathfinder/HybridAStar.lua | 26 ++++--- scripts/pathfinder/State3D.lua | 6 -- .../pathfinder/test/GraphPathfinderTest.lua | 75 +++++++++++++++++-- 5 files changed, 137 insertions(+), 40 deletions(-) diff --git a/scripts/geometry/Polyline.lua b/scripts/geometry/Polyline.lua index 9ab07227a..e04bf6f10 100644 --- a/scripts/geometry/Polyline.lua +++ b/scripts/geometry/Polyline.lua @@ -464,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 @@ -492,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) @@ -868,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/GraphPathfinder.lua b/scripts/pathfinder/GraphPathfinder.lua index 29a360bd8..e0f8d94f7 100644 --- a/scripts/pathfinder/GraphPathfinder.lua +++ b/scripts/pathfinder/GraphPathfinder.lua @@ -127,13 +127,10 @@ end ---@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.trace, CpDebug.DBG_PATHFINDER) + self.logger = Logger('GraphPathfinder', Logger.level.debug, CpDebug.DBG_PATHFINDER) self.range = range - -- make a copy of the graph as we'll modify it - self.graph = {} - for _, e in ipairs(graph) do - table.insert(self.graph, e:clone()) - end + self.transitionRange = range + self.originalGraph = graph self.deltaPosGoal = self.range self.deltaThetaDeg = 181 self.deltaThetaGoal = math.rad(self.deltaThetaDeg) @@ -154,36 +151,48 @@ 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, path) - path = path or {} +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 ~= path[1] then + if node ~= edge[1] then -- don't insert the same node twice (we'll have the same node twice when we split edges) - table.insert(path, 1, node) + 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:initRun(start, goal, ...) +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 + table.insert(self.graph, e:clone()) + 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) - return PathfinderResult(true, nil, true) + self.goalNodeInvalid = true + return self:finishRun(true, nil) end - return HybridAStar.initRun(self, start, goal, ...) + return HybridAStar.start(self, start, goal, turnRadius, ...) end --- Create the entry and exit edges for the graph. @@ -276,6 +285,30 @@ function GraphPathfinder:createGraphEntryAndExit(start, goal) State3D(exitEdges[1].vertex.x, exitEdges[1].vertex.y, 0, 0) end +---@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 diff --git a/scripts/pathfinder/HybridAStar.lua b/scripts/pathfinder/HybridAStar.lua index 87df50139..b11f01e17 100644 --- a/scripts/pathfinder/HybridAStar.lua +++ b/scripts/pathfinder/HybridAStar.lua @@ -457,7 +457,7 @@ HybridAStar.defaultMaxIterations = 40000 function HybridAStar:init(vehicle, yieldAfter, maxIterations, mustBeAccurate) self.logger = Logger('HybridAStar', Logger.level.error, CpDebug.DBG_PATHFINDER) self.vehicle = vehicle - self.count = 0 + self.iterationsSinceYield = 0 self.yields = 0 self.yieldAfter = yieldAfter or 200 self.maxIterations = maxIterations or HybridAStar.defaultMaxIterations @@ -567,7 +567,9 @@ end --- Wrap up this run, clean up timer, reset initialized flag so next run will start cleanly function HybridAStar:finishRun(result, path) self.initialized = false - self.constraints:showStatistics() + if self.constraints then + self.constraints:showStatistics() + end closeIntervalTimer(self.timer) return PathfinderResult(result, path, self.goalNodeInvalid) end @@ -583,7 +585,14 @@ function HybridAStar:run(start, goal, turnRadius, allowReverse, constraints, hit end self.timer = openIntervalTimer() while self.openList:size() > 0 and self.iterations < self.maxIterations do - -- pop lowest cost node from queue + -- 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.logger:trace('pop %s', tostring(pred)) @@ -593,14 +602,7 @@ function HybridAStar:run(start, goal, turnRadius, allowReverse, constraints, hit 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 PathfinderResult(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 @@ -613,7 +615,7 @@ 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 diff --git a/scripts/pathfinder/State3D.lua b/scripts/pathfinder/State3D.lua index 02664c3f5..d9b4c3216 100644 --- a/scripts/pathfinder/State3D.lua +++ b/scripts/pathfinder/State3D.lua @@ -45,8 +45,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 +140,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 index 8ea6aa8d2..0dc2514ad 100644 --- a/scripts/pathfinder/test/GraphPathfinderTest.lua +++ b/scripts/pathfinder/test/GraphPathfinderTest.lua @@ -19,6 +19,9 @@ 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') @@ -27,9 +30,24 @@ local GraphEdge = GraphPathfinder.GraphEdge local TestConstraints = CpObject(PathfinderConstraintInterface) local pathfinder, start, goal, done, path, goalNodeInvalid +local splitEdges, ensureMinimumRadius = Polyline.splitEdges, Polyline.ensureMinimumRadius + +local function setup() + -- 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 + +local function teardown() + -- restore the Polyline functions + Polyline.splitEdges = splitEdges + Polyline.ensureMinimumRadius = ensureMinimumRadius +end + local function printPath() - for _, p in ipairs(path) do - print(Vector.__tostring(p)) + for i, p in ipairs(path) do + print(i, Vector.__tostring(p)) end end @@ -42,6 +60,7 @@ local function runPathfinder() end function testDirection() + setup() local graph = { GraphEdge(GraphEdge.UNIDIRECTIONAL, { @@ -74,9 +93,11 @@ function testDirection() path[1]:assertAlmostEquals(Vector(120, 105)) path[2]:assertAlmostEquals(Vector(110, 105)) path[#path]:assertAlmostEquals(Vector(100, 105)) + teardown() end function testBidirectional() + setup() local graph = { GraphEdge(GraphEdge.BIDIRECTIONAL, { @@ -109,9 +130,11 @@ function testBidirectional() path[1]:assertAlmostEquals(Vector(120, 105)) path[2]:assertAlmostEquals(Vector(110, 105)) path[#path]:assertAlmostEquals(Vector(100, 105)) + teardown() end function testShorterPath() + setup() local graph = { GraphEdge(GraphEdge.UNIDIRECTIONAL, { @@ -136,9 +159,11 @@ function testShorterPath() path[1]:assertAlmostEquals(Vector(100, 100)) path[2]:assertAlmostEquals(Vector(110, 100)) path[#path]:assertAlmostEquals(Vector(120, 100)) + teardown() end function testRange() + setup() local graph = { GraphEdge(GraphEdge.UNIDIRECTIONAL, { @@ -164,7 +189,6 @@ function testRange() start = State3D(90, 105, 0, 0) goal = State3D(150, 105, 0, 0) done, path, _ = runPathfinder() - printPath() lu.assertIsTrue(done) lu.assertEquals(#path, 6) path[1]:assertAlmostEquals(Vector(100, 100)) @@ -173,9 +197,11 @@ function testRange() pathfinder = GraphPathfinder(math.huge, 500, 9, graph) done, path, _ = runPathfinder() lu.assertIsNil(path) + teardown() end function testStartInTheMiddle() + setup() local graph = { GraphEdge(GraphEdge.BIDIRECTIONAL, { @@ -217,14 +243,15 @@ function testStartInTheMiddle() pathfinder = GraphPathfinder(math.huge, 500, 10, graph) start, goal = goal, start done, path, _ = runPathfinder() - printPath() lu.assertIsTrue(done) lu.assertEquals(#path, 2) path[1]:assertAlmostEquals(Vector(100, 100)) path[2]:assertAlmostEquals(Vector(150, 100)) + teardown() end function testTwoPointSegments() + setup() local graph = { GraphEdge(GraphEdge.UNIDIRECTIONAL, { @@ -252,9 +279,11 @@ function testTwoPointSegments() lu.assertEquals(#path, 2) path[1]:assertAlmostEquals(Vector(120, 105)) path[#path]:assertAlmostEquals(Vector(100, 105)) + teardown() end function testEntryExit() + setup() local graph = { GraphEdge(GraphEdge.UNIDIRECTIONAL, { @@ -294,16 +323,18 @@ function testEntryExit() path[#path]:assertAlmostEquals(Vector(100, 105)) -- start/goal far away, middle entry start = State3D(110, 0, 0, 0) - goal = State3D(130, 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)) + teardown() end function testGoalWithinRange() -- goal too close to start (graph entry too close to graph exit) + setup() local graph = { GraphEdge(GraphEdge.UNIDIRECTIONAL, { @@ -319,9 +350,11 @@ function testGoalWithinRange() lu.assertIsTrue(done) lu.assertIsTrue(goalNodeInvalid) lu.assertIsNil(path) + teardown() end function testTwoWayStreet() + setup() local graph = { -- lane to the right, closer to the start location GraphEdge(GraphEdge.UNIDIRECTIONAL, @@ -359,6 +392,38 @@ function testTwoWayStreet() lu.assertEquals(#path, 2) path[1]:assertAlmostEquals(Vector(0, 20)) path[#path]:assertAlmostEquals(Vector(-100, 20)) + teardown() end +function 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() + 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()) From 18e7e63529eacf98fdecfabcfce8bbd80d6e2185 Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Sat, 5 Apr 2025 05:57:22 -0400 Subject: [PATCH 14/28] doc: GrapPathfinder comments --- scripts/pathfinder/GraphPathfinder.lua | 3 +++ 1 file changed, 3 insertions(+) diff --git a/scripts/pathfinder/GraphPathfinder.lua b/scripts/pathfinder/GraphPathfinder.lua index e0f8d94f7..e0b25d69d 100644 --- a/scripts/pathfinder/GraphPathfinder.lua +++ b/scripts/pathfinder/GraphPathfinder.lua @@ -285,6 +285,9 @@ function GraphPathfinder:createGraphEntryAndExit(start, goal) 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() From c18ab6ce9bba5eb6e79a8882872c7b2b03c3b233 Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Sat, 5 Apr 2025 08:14:51 -0400 Subject: [PATCH 15/28] fix: skip edges with less than 2 vertices An edge with a single vertex results in an infinite loop. --- scripts/pathfinder/GraphPathfinder.lua | 45 ++++++++++++++++++++++++-- 1 file changed, 42 insertions(+), 3 deletions(-) diff --git a/scripts/pathfinder/GraphPathfinder.lua b/scripts/pathfinder/GraphPathfinder.lua index e0b25d69d..c79870c51 100644 --- a/scripts/pathfinder/GraphPathfinder.lua +++ b/scripts/pathfinder/GraphPathfinder.lua @@ -18,8 +18,8 @@ GraphPathfinder = CpObject(HybridAStar) ---@class GraphPathfinder.GraphEdge : Polyline GraphPathfinder.GraphEdge = CpObject(Polyline) -GraphPathfinder.GraphEdge.UNIDIRECTIONAL = {} -GraphPathfinder.GraphEdge.BIDIRECTIONAL = {} +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} @@ -87,6 +87,14 @@ function GraphPathfinder.GraphEdge:rollUpIterator(entry) end end +function GraphPathfinder.GraphEdge.getDirectionFromGameDirection(gameDirection) + 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 @@ -181,7 +189,15 @@ 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 - table.insert(self.graph, e:clone()) + 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() @@ -353,3 +369,26 @@ function GraphPathfinder.GraphMotionPrimitives:createSuccessor(node, primitive, 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, ' Date: Sat, 5 Apr 2025 08:27:20 -0400 Subject: [PATCH 16/28] test: include path --- scripts/pathfinder/test/GraphPathfinderTest.lua | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/scripts/pathfinder/test/GraphPathfinderTest.lua b/scripts/pathfinder/test/GraphPathfinderTest.lua index 0dc2514ad..77dcd9d20 100644 --- a/scripts/pathfinder/test/GraphPathfinderTest.lua +++ b/scripts/pathfinder/test/GraphPathfinderTest.lua @@ -20,7 +20,7 @@ require('Vertex') require('Polyline') require('Polygon') require('Slider') -require('WraparoundIndex') -- for the test cases +require('WrapAroundIndex') -- for the test cases require('AnalyticHelper') -- for the test cases require('PathfinderUtil') require('HybridAStar') From 9a4e5c39a00499f8bc21064859207e14641704d1 Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Sat, 5 Apr 2025 08:49:50 -0400 Subject: [PATCH 17/28] test: proper setup/teardown --- .../pathfinder/test/GraphPathfinderTest.lua | 91 ++++++++++--------- 1 file changed, 46 insertions(+), 45 deletions(-) diff --git a/scripts/pathfinder/test/GraphPathfinderTest.lua b/scripts/pathfinder/test/GraphPathfinderTest.lua index 77dcd9d20..a37edd915 100644 --- a/scripts/pathfinder/test/GraphPathfinderTest.lua +++ b/scripts/pathfinder/test/GraphPathfinderTest.lua @@ -30,21 +30,6 @@ local GraphEdge = GraphPathfinder.GraphEdge local TestConstraints = CpObject(PathfinderConstraintInterface) local pathfinder, start, goal, done, path, goalNodeInvalid -local splitEdges, ensureMinimumRadius = Polyline.splitEdges, Polyline.ensureMinimumRadius - -local function setup() - -- 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 - -local function teardown() - -- restore the Polyline functions - Polyline.splitEdges = splitEdges - Polyline.ensureMinimumRadius = ensureMinimumRadius -end - local function printPath() for i, p in ipairs(path) do print(i, Vector.__tostring(p)) @@ -52,15 +37,31 @@ local function printPath() end local function runPathfinder() - local result = pathfinder:start(start, goal, 1, false, TestConstraints(), 0) + 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 -function testDirection() - setup() +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, { @@ -93,11 +94,11 @@ function testDirection() path[1]:assertAlmostEquals(Vector(120, 105)) path[2]:assertAlmostEquals(Vector(110, 105)) path[#path]:assertAlmostEquals(Vector(100, 105)) - teardown() + end -function testBidirectional() - setup() +function TestWithoutTransitions:testBidirectional() + local graph = { GraphEdge(GraphEdge.BIDIRECTIONAL, { @@ -130,11 +131,11 @@ function testBidirectional() path[1]:assertAlmostEquals(Vector(120, 105)) path[2]:assertAlmostEquals(Vector(110, 105)) path[#path]:assertAlmostEquals(Vector(100, 105)) - teardown() + end -function testShorterPath() - setup() +function TestWithoutTransitions:testShorterPath() + local graph = { GraphEdge(GraphEdge.UNIDIRECTIONAL, { @@ -159,11 +160,11 @@ function testShorterPath() path[1]:assertAlmostEquals(Vector(100, 100)) path[2]:assertAlmostEquals(Vector(110, 100)) path[#path]:assertAlmostEquals(Vector(120, 100)) - teardown() + end -function testRange() - setup() +function TestWithoutTransitions:testRange() + local graph = { GraphEdge(GraphEdge.UNIDIRECTIONAL, { @@ -197,11 +198,11 @@ function testRange() pathfinder = GraphPathfinder(math.huge, 500, 9, graph) done, path, _ = runPathfinder() lu.assertIsNil(path) - teardown() + end -function testStartInTheMiddle() - setup() +function TestWithoutTransitions:testStartInTheMiddle() + local graph = { GraphEdge(GraphEdge.BIDIRECTIONAL, { @@ -247,11 +248,11 @@ function testStartInTheMiddle() lu.assertEquals(#path, 2) path[1]:assertAlmostEquals(Vector(100, 100)) path[2]:assertAlmostEquals(Vector(150, 100)) - teardown() + end -function testTwoPointSegments() - setup() +function TestWithoutTransitions:testTwoPointSegments() + local graph = { GraphEdge(GraphEdge.UNIDIRECTIONAL, { @@ -279,11 +280,11 @@ function testTwoPointSegments() lu.assertEquals(#path, 2) path[1]:assertAlmostEquals(Vector(120, 105)) path[#path]:assertAlmostEquals(Vector(100, 105)) - teardown() + end -function testEntryExit() - setup() +function TestWithoutTransitions:testEntryExit() + local graph = { GraphEdge(GraphEdge.UNIDIRECTIONAL, { @@ -329,12 +330,12 @@ function testEntryExit() lu.assertEquals(#path, 2) path[1]:assertAlmostEquals(Vector(110, 100)) path[#path]:assertAlmostEquals(Vector(120, 100)) - teardown() + end -function testGoalWithinRange() +function TestWithoutTransitions:testGoalWithinRange() -- goal too close to start (graph entry too close to graph exit) - setup() + local graph = { GraphEdge(GraphEdge.UNIDIRECTIONAL, { @@ -350,11 +351,11 @@ function testGoalWithinRange() lu.assertIsTrue(done) lu.assertIsTrue(goalNodeInvalid) lu.assertIsNil(path) - teardown() + end -function testTwoWayStreet() - setup() +function TestWithoutTransitions:testTwoWayStreet() + local graph = { -- lane to the right, closer to the start location GraphEdge(GraphEdge.UNIDIRECTIONAL, @@ -392,10 +393,10 @@ function testTwoWayStreet() lu.assertEquals(#path, 2) path[1]:assertAlmostEquals(Vector(0, 20)) path[#path]:assertAlmostEquals(Vector(-100, 20)) - teardown() end -function testTransition() +TestWithTransitions = {} +function TestWithTransitions:testTransition() local graph = { GraphEdge(GraphEdge.UNIDIRECTIONAL, { @@ -415,6 +416,7 @@ function testTransition() 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 @@ -425,5 +427,4 @@ function testTransition() path[#path]:assertAlmostEquals(Vector(210, 200)) end - os.exit(lu.LuaUnit.run()) From e1d5a1664c568b013e4418ed7f66ef8387967044 Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Sat, 12 Apr 2025 07:15:56 -0400 Subject: [PATCH 18/28] chore: forward compatibility --- scripts/pathfinder/GraphPathfinder.lua | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/scripts/pathfinder/GraphPathfinder.lua b/scripts/pathfinder/GraphPathfinder.lua index c79870c51..f620153d7 100644 --- a/scripts/pathfinder/GraphPathfinder.lua +++ b/scripts/pathfinder/GraphPathfinder.lua @@ -88,6 +88,10 @@ function GraphPathfinder.GraphEdge:rollUpIterator(entry) 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 From 8cad8edf322c11544bbc64e8073923c47169abea Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Wed, 16 Apr 2025 19:36:23 -0400 Subject: [PATCH 19/28] feat: pathinder caches penalties --- scripts/pathfinder/HybridAStar.lua | 51 +++++++++++++++++++++++++++++- 1 file changed, 50 insertions(+), 1 deletion(-) diff --git a/scripts/pathfinder/HybridAStar.lua b/scripts/pathfinder/HybridAStar.lua index b11f01e17..ad784b64c 100644 --- a/scripts/pathfinder/HybridAStar.lua +++ b/scripts/pathfinder/HybridAStar.lua @@ -448,6 +448,48 @@ function HybridAStar.NodeList:iterator() end end +--- Cache penalties for nodes to avoid calling getNodePenalty() multiple times for the same node +--- This is a 2D grid discretized to the gridSize, anything falling in the same grid cell will have the same +--- penalty. This also assumes the penalty is the same for all theta values in the cell, that is, getNodePenalty() +--- is not theta dependent. +---@class HybridAStar.PenaltyCache +HybridAStar.PenaltyCache = CpObject() + +---@param gridSize number size of the cell in the x/y dimensions +function HybridAStar.PenaltyCache:init(gridSize) + self.nodes = {} + self.gridSize = gridSize +end + +---@param node State3D +function HybridAStar.PenaltyCache:getNodeIndexes(node) + local x = math.floor(node.x / self.gridSize) + local y = math.floor(node.y / self.gridSize) + return x, y +end + +function HybridAStar.PenaltyCache:inSameCell(n1, n2) + local x1, y1 = self:getNodeIndexes(n1) + local x2, y2 = self:getNodeIndexes(n2) + return x1 == x2 and y1 == y2 +end + +---@param node State3D +function HybridAStar.PenaltyCache:get(node) + local x, y, t = self:getNodeIndexes(node) + return self.nodes[x] and self.nodes[x][y] +end + +---@param node State3D +function HybridAStar.PenaltyCache:add(node, penalty) + local x, y, t = self:getNodeIndexes(node) + if not self.nodes[x] then + self.nodes[x] = {} + end + self.nodes[x][y] = penalty +end + + --- A reasonable default maximum iterations that works for the majority of our use cases HybridAStar.defaultMaxIterations = 40000 @@ -524,6 +566,8 @@ function HybridAStar:initRun(start, goal, turnRadius, allowReverse, constraints, -- create the configuration space ---@type HybridAStar.NodeList closedList self.nodes = HybridAStar.NodeList(self.deltaPos, self.deltaThetaDeg) + ---@type HybridAStar.NodeList penaltyCache is not direction dependent + self.penaltyCache = HybridAStar.PenaltyCache(self.deltaPos) if allowReverse then self.analyticSolver = self.analyticSolver or ReedsSheppSolver() else @@ -637,7 +681,12 @@ 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)) + local penalty = self.penaltyCache:get(succ) + if penalty == nil then + penalty = self.constraints:getNodePenalty(succ) + self.penaltyCache:add(succ, penalty) + end + succ:updateG(primitive, penalty) local analyticSolutionCost = 0 if self.analyticSolverEnabled then local analyticSolution = self.analyticSolver:solve(succ, self.goal, self.turnRadius) From 2e47154d8cbc3313c4b4ec861a6f8f716302b272 Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Thu, 17 Apr 2025 07:08:51 -0400 Subject: [PATCH 20/28] perf: pathfinder penalty cache Once calculated for a grid cell, cache the penalty in the pathfinder. The penalty depends currently only on the position and not the heading, so this requires a limited amount of memory. This should reduce the calls for getNodePenalty() by an order of magnitude, but needs to be tested on a bigger field with difficult situations. --- scripts/pathfinder/HybridAStarWithAStarInTheMiddle.lua | 2 +- scripts/pathfinder/PathfinderConstraints.lua | 8 ++++---- scripts/pathfinder/PathfinderUtil.lua | 4 ++-- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/scripts/pathfinder/HybridAStarWithAStarInTheMiddle.lua b/scripts/pathfinder/HybridAStarWithAStarInTheMiddle.lua index 261c3178e..e8e5a7ea9 100644 --- a/scripts/pathfinder/HybridAStarWithAStarInTheMiddle.lua +++ b/scripts/pathfinder/HybridAStarWithAStarInTheMiddle.lua @@ -232,7 +232,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 diff --git a/scripts/pathfinder/PathfinderConstraints.lua b/scripts/pathfinder/PathfinderConstraints.lua index 11360477f..ef8bd674f 100644 --- a/scripts/pathfinder/PathfinderConstraints.lua +++ b/scripts/pathfinder/PathfinderConstraints.lua @@ -76,7 +76,7 @@ function PathfinderConstraints:init(context) 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', self.offFieldPenalty, self.maxFruitPercent, self.fieldNum, areaToAvoidText, self.areaToIgnoreFruit or 'none', self.areaToIgnoreOffFieldPenalty or 'none') end @@ -172,7 +172,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 +239,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..31ee40e28 100644 --- a/scripts/pathfinder/PathfinderUtil.lua +++ b/scripts/pathfinder/PathfinderUtil.lua @@ -477,12 +477,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 From 1c6e16da21dc758c8c9b6fcd7182462fd1531a23 Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Sun, 20 Apr 2025 06:36:47 -0400 Subject: [PATCH 21/28] wip --- scripts/pathfinder/AStar.lua | 16 +- scripts/pathfinder/HybridAStar.lua | 20 +- scripts/pathfinder/JumpPointSearch.lua | 256 +++++++++++++++++++++++++ 3 files changed, 270 insertions(+), 22 deletions(-) create mode 100644 scripts/pathfinder/JumpPointSearch.lua diff --git a/scripts/pathfinder/AStar.lua b/scripts/pathfinder/AStar.lua index 4a740b39f..cf73dcc41 100644 --- a/scripts/pathfinder/AStar.lua +++ b/scripts/pathfinder/AStar.lua @@ -31,14 +31,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/HybridAStar.lua b/scripts/pathfinder/HybridAStar.lua index ad784b64c..c4f636103 100644 --- a/scripts/pathfinder/HybridAStar.lua +++ b/scripts/pathfinder/HybridAStar.lua @@ -249,8 +249,6 @@ end --- only dependent on the turn radius, and then use the precalculated values during the search. ---@class HybridAstar.MotionPrimitives HybridAStar.MotionPrimitives = CpObject() --- forward straight/right/left -HybridAStar.MotionPrimitiveTypes = { FS = 'FS', FR = 'FR', FL = 'FL', RS = 'RS', RR = 'RR', RL = 'RL', LL = 'LL', RR = 'RR', NA = 'NA' } ---@param r number turning radius ---@param expansionDegree number degrees of arc in one expansion step @@ -267,34 +265,28 @@ function HybridAStar.MotionPrimitives:init(r, expansionDegree, allowReverse) -- forward straight table.insert(self.primitives, { dx = d, dy = 0, dt = 0, d = d, gear = Gear.Forward, - steer = Steer.Straight, - type = HybridAStar.MotionPrimitiveTypes.FS }) + steer = Steer.Straight }) -- forward right table.insert(self.primitives, { dx = dx, dy = -dy, dt = dt, d = d, gear = Gear.Forward, - steer = Steer.Right, - type = HybridAStar.MotionPrimitiveTypes.FR }) + steer = Steer.Right }) -- forward left table.insert(self.primitives, { dx = dx, dy = dy, dt = -dt, d = d, gear = Gear.Forward, - steer = Steer.Left, - type = HybridAStar.MotionPrimitiveTypes.FL }) + steer = Steer.Left }) if allowReverse then -- reverse straight table.insert(self.primitives, { dx = -d, dy = 0, dt = 0, d = d, gear = Gear.Backward, - steer = Steer.Straight, - type = HybridAStar.MotionPrimitiveTypes.RS }) + steer = Steer.Straight }) -- reverse right table.insert(self.primitives, { dx = -dx, dy = -dy, dt = dt, d = d, gear = Gear.Backward, - steer = Steer.Right, - type = HybridAStar.MotionPrimitiveTypes.RR }) + steer = Steer.Right }) -- reverse left table.insert(self.primitives, { dx = -dx, dy = dy, dt = -dt, d = d, gear = Gear.Backward, - steer = Steer.Left, - type = HybridAStar.MotionPrimitiveTypes.RL }) + steer = Steer.Left }) end end diff --git a/scripts/pathfinder/JumpPointSearch.lua b/scripts/pathfinder/JumpPointSearch.lua new file mode 100644 index 000000000..d0fb5ea4d --- /dev/null +++ b/scripts/pathfinder/JumpPointSearch.lua @@ -0,0 +1,256 @@ +---@class HybridAStar.JpsMotionPrimitives : HybridAStar.SimpleMotionPrimitives +HybridAStar.JpsMotionPrimitives = CpObject(HybridAStar.SimpleMotionPrimitives) +function HybridAStar.JpsMotionPrimitives:init(gridSize, deltaPosGoal, deltaThetaGoal, constraints, goal) + -- similar to the A*, the possible motion primitives (the neighbors) are in all 8 directions. + AStar.SimpleMotionPrimitives.init(self, gridSize) + 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] + if self.primitives[i].dy == 0 then + -- horizontal move + self.primitives[i].neighbors = { + -- unconditionally include natural neighbors. + -- always need this neighbor to the left or right of the current node + { check = nil, neighbor = { dx = self.primitives[i].dx, dy = 0 } }, + -- if there is an obstacle at 'check' if we got to the node through this primitive, then + -- create a forced neighbors at 'forcedNeighbor' + { check = { dx = 0, dy = self.gridSize }, neighbor = { dx = self.primitives[i].dx, dy = self.gridSize } }, + { check = { dx = 0, dy = -self.gridSize }, neighbor = { dx = self.primitives[i].dx, dy = -self.gridSize } } + } + elseif self.primitives[i].dx == 0 then + -- vertical move + self.primitives[i].neighbors = { + -- unconditionally include natural neighbors. + -- always need this neighbor to the left or right of the current node + { check = nil, neighbor = { dx = 0, dy = self.primitives[i].dy } }, + -- forced neighbors are included only if the checked neighbors are invalid + { check = { dx = self.gridSize, dy = 0 }, neighbor = { dx = self.gridSize, dy = self.primitives[i].dy } }, + { check = { dx = -self.gridSize, dy = 0 }, neighbor = { dx = -self.gridSize, dy = self.primitives[i].dy } } + } + else + -- diagonal move + self.primitives[i].neighbors = { + -- unconditionally include natural neighbors. + -- we always need this neighbor to the left or right of the current node, plus the one in the same direction + { check = nil, neighbor = { dx = self.primitives[i].dx, dy = 0 } }, + { check = nil, neighbor = { dx = 0, dy = self.primitives[i].dy } }, + { check = nil, neighbor = { dx = self.primitives[i].dx, dy = self.primitives[i].dy } }, + -- forced neighbors are included only if the checked neighbors are invalid + { check = { dx = -self.primitives[i].dx, dy = 0 }, + neighbor = { dx = -self.primitives[i].dx, dy = self.primitives[i].dy } }, + { check = { dx = 0, dy = -self.primitives[i].dy }, + neighbor = { dx = self.primitives[i].dx, dy = -self.primitives[i].dy } }, + } + + end + end + self.deltaPosGoal = deltaPosGoal + self.deltaThetaGoal = deltaThetaGoal + self.constraints = constraints + self.goal = goal +end + +function HybridAStar.JpsMotionPrimitives:getGridSize() + return self.gridSize +end + +function HybridAStar.JpsMotionPrimitives:isValidNode(x, y, t, constraints) + local node = { x = x, y = y, t = t } + return constraints:isValidNode(node, true, true) +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 whole +-- framework in HybridAStar.lua with JPS. +function HybridAStar.JpsMotionPrimitives:getPrimitives(node) + local primitives = {} + if node.pred then + local x, y, t = node.x, node.y, node.t + -- Node has a parent, we will prune some neighbours + -- Gets the direction of move + local dx = self.gridSize * (x - node.pred.x) / math.max(1, math.abs(x - node.pred.x)) + local dy = self.gridSize * (y - node.pred.y) / math.max(1, math.abs(y - node.pred.y)) + local dDiag = math.sqrt(dx * dx + dy * dy) + local xOk, yOk = false, false + if math.abs(dx) > 0.1 and math.abs(dy) > 0.1 then + -- diagonal move + if self:isValidNode(x, y + dy, t, self.constraints) then + table.insert(primitives, { x = x, y = y + dy, t = math.atan2(dy, 0), d = math.abs(dy) }) + yOk = true + end + if self:isValidNode(x + dx, y, t, self.constraints) then + table.insert(primitives, { x = x + dx, y = y, t = math.atan2(0, dx), d = math.abs(dx) }) + xOk = true + end + if xOk or yOk then + table.insert(primitives, { x = x + dx, y = y + dy, t = math.atan2(dy, dx), d = dDiag }) + end + -- Forced neighbors + if not self:isValidNode(x - dx, y, t, self.constraints) and yOk then + table.insert(primitives, { x = x - dx, y = y + dy, t = math.atan2(dy, -dx), d = dDiag }) + table.insert(JumpPointSearch.markers, { label = 'forced x - y +', x = x - dx, y = y + dy }) + node.forced = true + end + if not self:isValidNode(x, y - dy, t, self.constraints) and xOk then + table.insert(primitives, { x = x + dx, y = y - dy, t = math.atan2(-dy, dx), d = dDiag }) + table.insert(JumpPointSearch.markers, { label = 'forced x + y -', x = x + dx, y = y - dy }) + node.forced = true + end + else + if math.abs(dx) < 0.1 then + -- move along the y axis + if self:isValidNode(x, y + dy, t, self.constraints) then + table.insert(primitives, { x = x, y = y + dy, t = math.atan2(dy, 0), d = math.abs(dy) }) + end + -- Forced neighbors + dDiag = math.sqrt(dy * dy + self.gridSize * self.gridSize) + if not self:isValidNode(x + self.gridSize, y, t, self.constraints) then + table.insert(primitives, { x = x + self.gridSize, y = y + dy, + t = math.atan2(dy, self.gridSize), d = dDiag }) + table.insert(JumpPointSearch.markers, { label = 'forced x +', x = x + self.gridSize, y = y + dy }) + node.forced = true + end + if not self:isValidNode(x - self.gridSize, y, t, self.constraints) then + table.insert(primitives, { x = x - self.gridSize, y = y + dy, + t = math.atan2(dy, -self.gridSize), d = dDiag }) + table.insert(JumpPointSearch.markers, { label = 'forced x -', x = x - self.gridSize, y = y + dy }) + node.forced = true + end + else + -- move along the x axis + if self:isValidNode(x + dx, y, t, self.constraints) then + table.insert(primitives, { x = x + dx, y = y, t = math.atan2(0, dx), d = math.abs(dx) }) + end + -- Forced neighbors + dDiag = math.sqrt(dx * dx + self.gridSize * self.gridSize) + if not self:isValidNode(x, y + self.gridSize, t, self.constraints) then + table.insert(primitives, { x = x + dx, y = y + self.gridSize, + t = math.atan2(self.gridSize, dx), d = dDiag }) + table.insert(JumpPointSearch.markers, { label = 'forced y +', x = x + dx, y = y + self.gridSize }) + node.forced = true + end + if not self:isValidNode(x, y - self.gridSize, t, self.constraints) then + table.insert(primitives, { x = x + dx, y = y - self.gridSize, + t = math.atan2(-self.gridSize, dx), d = dDiag }) + table.insert(JumpPointSearch.markers, { label = 'forced y -', x = x + dx, y = y - self.gridSize }) + node.forced = true + end + end + end + else + -- no parent, this is the start node + for _, p in pairs(self.primitives) do + -- JPS does not really use motion primitives, what we call primitives are actually the + -- successors, with their real coordinates, not just a delta. + table.insert(primitives, { x = node.x + p.dx, y = node.y + p.dy, t = p.dt, d = p.d }) + end + end + return primitives +end + +function HybridAStar.JpsMotionPrimitives:jump(node, pred, recursionCounter) + if recursionCounter and recursionCounter > 2 then + return node, recursionCounter + end + recursionCounter = recursionCounter and recursionCounter + 1 or 1 + local x, y, t = node.x, node.y, node.t + if not self:isValidNode(x, y, t, self.constraints) then + return nil + end + if node:equals(self.goal, self.deltaPosGoal, self.deltaThetaGoal) then + return node + end + local dx = x - pred.x + local dy = y - pred.y + if math.abs(dx) > 0.1 and math.abs(dy) > 0.1 then + -- diagonal move + if (self:isValidNode(x - dx, y + dy, t, self.constraints) and not self:isValidNode(x - dx, y, t, self.constraints)) or + (self:isValidNode(x + dx, y - dy, t, self.constraints) and not self:isValidNode(x, y - dy, t, self.constraints)) then + -- Current node is a jump point if one of its left or right neighbors ahead is forced + return node + end + else + if math.abs(dx) > 0.1 then + -- move along the x axis + if (self:isValidNode(x + dx, y + self.gridSize, t, self.constraints) and not self:isValidNode(x, y + self.gridSize, t, self.constraints)) or + (self:isValidNode(x + dx, y - self.gridSize, t, self.constraints) and not self:isValidNode(x, y - self.gridSize, t, self.constraints)) then + -- Current node is a jump point if one of its left or right neighbors ahead is forced + return node + end + else + -- move along the y axis + if (self:isValidNode(x + self.gridSize, y + dy, t, self.constraints) and not self:isValidNode(x + self.gridSize, y, t, self.constraints)) or + (self:isValidNode(x - self.gridSize, y + dy, t, self.constraints) and not self:isValidNode(x - self.gridSize, y, t, self.constraints)) then + -- Current node is a jump point if one of its left or right neighbors ahead is forced + return node + end + end + end + -- Recursive horizontal/vertical search + if math.abs(dx) > 0.1 and math.abs(dy) > 0.1 then + local nextNode = State3D.copy(node) + nextNode.x = nextNode.x + dx + nextNode.g = nextNode.g + dx + if self:jump(nextNode, node, recursionCounter) then + return node + end + nextNode = State3D.copy(node) + nextNode.y = nextNode.y + dy + nextNode.g = nextNode.g + dy + if self:jump(nextNode, node, recursionCounter) then + return node + end + end + -- Recursive diagonal search + if self:isValidNode(x + dx, y, t, self.constraints) or self:isValidNode(x, y + dy, t, self.constraints) then + local nextNode = State3D.copy(node) + nextNode.x = nextNode.x + dx + nextNode.y = nextNode.y + dy + + nextNode.g = nextNode.g + dy + return self:jump(nextNode, node, recursionCounter) + end +end + +function HybridAStar.JpsMotionPrimitives:createSuccessor(node, primitive, hitchLength) + local neighbor = State3D(primitive.x, primitive.y, primitive.t) + local jumpNode, jumps = self:jump(neighbor, node) + primitive.d = jumps and jumps * primitive.d or primitive.d + if jumpNode then + return State3D(jumpNode.x, jumpNode.y, jumpNode.t, node.g, node, Gear.Forward, Steer.Straight, + node:getNextTrailerHeading(self.gridSize, hitchLength)) + end +end +--- A Jump Point Search +---@class JumpPointSearch : AStar +JumpPointSearch = CpObject(AStar) +JumpPointSearch.markers = {} + +function JumpPointSearch:init(vehicle, yieldAfter, maxIterations) + AStar.init(self, vehicle, yieldAfter, maxIterations) + JumpPointSearch.markers = {} +end + +function JumpPointSearch:initRun(start, goal, turnRadius, allowReverse, constraints, hitchLength) + self.motionPrimitives = HybridAStar.JpsMotionPrimitives(self.deltaPos, self.deltaPosGoal, self.deltaThetaGoal, constraints, goal) + return AStar.initRun(self, start, goal, turnRadius, allowReverse, constraints, hitchLength) +end + +function JumpPointSearch:getMotionPrimitives(turnRadius, allowReverse) + return self.motionPrimitives +end + +---@class HybridAStarWithJpsInTheMiddle : HybridAStarWithAStarInTheMiddle +HybridAStarWithJpsInTheMiddle = CpObject(HybridAStarWithAStarInTheMiddle) + +function HybridAStarWithJpsInTheMiddle:init(hybridRange, yieldAfter, maxIterations, mustBeAccurate) + HybridAStarWithAStarInTheMiddle.init(self, hybridRange, yieldAfter, maxIterations, mustBeAccurate) +end + +function HybridAStarWithJpsInTheMiddle:getAStar() + return JumpPointSearch(self.yieldAfter) +end From 071d53650c238fd7c6353c7c81f70e7807bebb5f Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Sun, 20 Apr 2025 17:04:16 -0400 Subject: [PATCH 22/28] wip, JPS working now --- scripts/pathfinder/AStar.lua | 2 +- scripts/pathfinder/HybridAStar.lua | 10 +- scripts/pathfinder/JumpPointSearch.lua | 361 ++++++++++++------------- scripts/pathfinder/State3D.lua | 4 +- 4 files changed, 189 insertions(+), 188 deletions(-) diff --git a/scripts/pathfinder/AStar.lua b/scripts/pathfinder/AStar.lua index cf73dcc41..b22b7a574 100644 --- a/scripts/pathfinder/AStar.lua +++ b/scripts/pathfinder/AStar.lua @@ -11,7 +11,7 @@ function AStar:init(vehicle, yieldAfter, maxIterations) 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 diff --git a/scripts/pathfinder/HybridAStar.lua b/scripts/pathfinder/HybridAStar.lua index c4f636103..c6492011c 100644 --- a/scripts/pathfinder/HybridAStar.lua +++ b/scripts/pathfinder/HybridAStar.lua @@ -548,7 +548,7 @@ function HybridAStar:initRun(start, goal, turnRadius, allowReverse, constraints, self.hitchLength = hitchLength self.constraints = constraints -- a motion primitive is straight or a few degree turn to the right or left - self.hybridMotionPrimitives = self:getMotionPrimitives(turnRadius, allowReverse) + self.motionPrimitives = self:getMotionPrimitives(turnRadius, allowReverse) -- create the open list for the nodes as a binary heap where -- the node with the lowest total cost is at the top self.openList = BinaryHeap.minUnique(function(a, b) @@ -628,10 +628,12 @@ function HybridAStar:run(start, goal, turnRadius, allowReverse, constraints, hit 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 + end + -- pop lowest cost node from queue ---@type State3D local pred = State3D.pop(self.openList) 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! @@ -657,9 +659,9 @@ function HybridAStar:run(start, goal, turnRadius, allowReverse, constraints, hit 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) diff --git a/scripts/pathfinder/JumpPointSearch.lua b/scripts/pathfinder/JumpPointSearch.lua index d0fb5ea4d..e8cd519c8 100644 --- a/scripts/pathfinder/JumpPointSearch.lua +++ b/scripts/pathfinder/JumpPointSearch.lua @@ -2,48 +2,117 @@ HybridAStar.JpsMotionPrimitives = CpObject(HybridAStar.SimpleMotionPrimitives) function HybridAStar.JpsMotionPrimitives:init(gridSize, deltaPosGoal, deltaThetaGoal, constraints, goal) -- similar to the A*, the possible motion primitives (the neighbors) are in all 8 directions. - AStar.SimpleMotionPrimitives.init(self, gridSize) + self.primitives = {} + + self.gridSize = gridSize + local d = 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. + -- right + self.primitives[1].nextPrimitives = { + -- unconditionally include natural neighbors. + -- always need this neighbor to the left or right of the current node + { check = nil, nextPrimitive = self.primitives[1] }, + -- if there is an obstacle at 'check' if we got to the node through this primitive, then + -- create a forced neighbors at 'forcedNeighbor' + { check = Vector(0, self.gridSize), nextPrimitive = self.primitives[2] }, + { check = Vector(0, -self.gridSize), nextPrimitive = self.primitives[8] } + } + -- left + self.primitives[5].nextPrimitives = { + -- unconditionally include natural neighbors. + -- always need this neighbor to the left or right of the current node + { check = nil, nextPrimitive = self.primitives[5] }, + -- if there is an obstacle at 'check' if we got to the node through this primitive, then + -- create a forced neighbors at 'forcedNeighbor' + { check = Vector(0, self.gridSize), nextPrimitive = self.primitives[4] }, + { check = Vector(0, -self.gridSize), nextPrimitive = self.primitives[6] } + } + + -- up + self.primitives[3].nextPrimitives = { + -- unconditionally include natural neighbors. + -- always need this neighbor to the left or right of the current node + { check = nil, nextPrimitive = self.primitives[3] }, + -- forced neighbors are included only if the checked neighbors are invalid + { check = Vector(self.gridSize, 0), nextPrimitive = self.primitives[2] }, + { check = Vector(-self.gridSize, 0), nextPrimitive = self.primitives[4] } + } + + -- down + self.primitives[7].nextPrimitives = { + -- unconditionally include natural neighbors. + -- always need this neighbor to the left or right of the current node + { check = nil, nextPrimitive = self.primitives[7] }, + -- forced neighbors are included only if the checked neighbors are invalid + { check = Vector(self.gridSize, 0), nextPrimitive = self.primitives[8] }, + { check = Vector(-self.gridSize, 0), nextPrimitive = self.primitives[6] } + } + + -- up right + self.primitives[2].nextPrimitives = { + -- unconditionally include natural neighbors. + -- we always need this neighbor to the left or right of the current node, plus the one in the same direction + { check = nil, nextPrimitive = self.primitives[2] }, + { check = nil, nextPrimitive = self.primitives[1] }, + { check = nil, nextPrimitive = self.primitives[3] }, + -- forced neighbors are included only if the checked neighbors are invalid + { check = Vector(-self.gridSize, 0), nextPrimitive = self.primitives[4] }, + { check = Vector(0, -self.gridSize), nextPrimitive = self.primitives[8] }, + } + + -- up left + self.primitives[4].nextPrimitives = { + -- unconditionally include natural neighbors. + -- we always need this neighbor to the left or right of the current node, plus the one in the same direction + { check = nil, nextPrimitive = self.primitives[4] }, + { check = nil, nextPrimitive = self.primitives[3] }, + { check = nil, nextPrimitive = self.primitives[5] }, + -- forced neighbors are included only if the checked neighbors are invalid + { check = Vector(self.gridSize, 0), nextPrimitive = self.primitives[2] }, + { check = Vector(0, -self.gridSize), nextPrimitive = self.primitives[6] }, + } + + -- down left + self.primitives[6].nextPrimitives = { + -- unconditionally include natural neighbors. + -- we always need this neighbor to the left or right of the current node, plus the one in the same direction + { check = nil, nextPrimitive = self.primitives[6] }, + { check = nil, nextPrimitive = self.primitives[5] }, + { check = nil, nextPrimitive = self.primitives[7] }, + -- forced neighbors are included only if the checked neighbors are invalid + { check = Vector(self.gridSize, 0), nextPrimitive = self.primitives[8] }, + { check = Vector(0, self.gridSize), nextPrimitive = self.primitives[4] }, + } + + -- down right + self.primitives[8].nextPrimitives = { + -- unconditionally include natural neighbors. + -- we always need this neighbor to the left or right of the current node, plus the one in the same direction + { check = nil, nextPrimitive = self.primitives[8] }, + { check = nil, nextPrimitive = self.primitives[1] }, + { check = nil, nextPrimitive = self.primitives[7] }, + -- forced neighbors are included only if the checked neighbors are invalid + { 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] - if self.primitives[i].dy == 0 then - -- horizontal move - self.primitives[i].neighbors = { - -- unconditionally include natural neighbors. - -- always need this neighbor to the left or right of the current node - { check = nil, neighbor = { dx = self.primitives[i].dx, dy = 0 } }, - -- if there is an obstacle at 'check' if we got to the node through this primitive, then - -- create a forced neighbors at 'forcedNeighbor' - { check = { dx = 0, dy = self.gridSize }, neighbor = { dx = self.primitives[i].dx, dy = self.gridSize } }, - { check = { dx = 0, dy = -self.gridSize }, neighbor = { dx = self.primitives[i].dx, dy = -self.gridSize } } - } - elseif self.primitives[i].dx == 0 then - -- vertical move - self.primitives[i].neighbors = { - -- unconditionally include natural neighbors. - -- always need this neighbor to the left or right of the current node - { check = nil, neighbor = { dx = 0, dy = self.primitives[i].dy } }, - -- forced neighbors are included only if the checked neighbors are invalid - { check = { dx = self.gridSize, dy = 0 }, neighbor = { dx = self.gridSize, dy = self.primitives[i].dy } }, - { check = { dx = -self.gridSize, dy = 0 }, neighbor = { dx = -self.gridSize, dy = self.primitives[i].dy } } - } - else - -- diagonal move - self.primitives[i].neighbors = { - -- unconditionally include natural neighbors. - -- we always need this neighbor to the left or right of the current node, plus the one in the same direction - { check = nil, neighbor = { dx = self.primitives[i].dx, dy = 0 } }, - { check = nil, neighbor = { dx = 0, dy = self.primitives[i].dy } }, - { check = nil, neighbor = { dx = self.primitives[i].dx, dy = self.primitives[i].dy } }, - -- forced neighbors are included only if the checked neighbors are invalid - { check = { dx = -self.primitives[i].dx, dy = 0 }, - neighbor = { dx = -self.primitives[i].dx, dy = self.primitives[i].dy } }, - { check = { dx = 0, dy = -self.primitives[i].dy }, - neighbor = { dx = self.primitives[i].dx, dy = -self.primitives[i].dy } }, - } - - end + -- cache the length + self.primitives[i].d = self.primitives[i]:length() end self.deltaPosGoal = deltaPosGoal self.deltaThetaGoal = deltaThetaGoal @@ -60,6 +129,26 @@ function HybridAStar.JpsMotionPrimitives:isValidNode(x, y, t, constraints) return constraints:isValidNode(node, true, true) end +---@param node State3D +---@param primitive +function HybridAStar.JpsMotionPrimitives: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, node.y, n.check:heading()) + n.check + if not self.constraints:isValidNode(neighborToCheck, true, true) 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 + -- 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 @@ -67,163 +156,67 @@ end -- This is not the most elegant solution and the only reason we do this is to be able to reuse the whole -- framework in HybridAStar.lua with JPS. function HybridAStar.JpsMotionPrimitives:getPrimitives(node) - local primitives = {} - if node.pred then - local x, y, t = node.x, node.y, node.t - -- Node has a parent, we will prune some neighbours - -- Gets the direction of move - local dx = self.gridSize * (x - node.pred.x) / math.max(1, math.abs(x - node.pred.x)) - local dy = self.gridSize * (y - node.pred.y) / math.max(1, math.abs(y - node.pred.y)) - local dDiag = math.sqrt(dx * dx + dy * dy) - local xOk, yOk = false, false - if math.abs(dx) > 0.1 and math.abs(dy) > 0.1 then - -- diagonal move - if self:isValidNode(x, y + dy, t, self.constraints) then - table.insert(primitives, { x = x, y = y + dy, t = math.atan2(dy, 0), d = math.abs(dy) }) - yOk = true - end - if self:isValidNode(x + dx, y, t, self.constraints) then - table.insert(primitives, { x = x + dx, y = y, t = math.atan2(0, dx), d = math.abs(dx) }) - xOk = true - end - if xOk or yOk then - table.insert(primitives, { x = x + dx, y = y + dy, t = math.atan2(dy, dx), d = dDiag }) - end - -- Forced neighbors - if not self:isValidNode(x - dx, y, t, self.constraints) and yOk then - table.insert(primitives, { x = x - dx, y = y + dy, t = math.atan2(dy, -dx), d = dDiag }) - table.insert(JumpPointSearch.markers, { label = 'forced x - y +', x = x - dx, y = y + dy }) - node.forced = true - end - if not self:isValidNode(x, y - dy, t, self.constraints) and xOk then - table.insert(primitives, { x = x + dx, y = y - dy, t = math.atan2(-dy, dx), d = dDiag }) - table.insert(JumpPointSearch.markers, { label = 'forced x + y -', x = x + dx, y = y - dy }) - node.forced = true - end - else - if math.abs(dx) < 0.1 then - -- move along the y axis - if self:isValidNode(x, y + dy, t, self.constraints) then - table.insert(primitives, { x = x, y = y + dy, t = math.atan2(dy, 0), d = math.abs(dy) }) - end - -- Forced neighbors - dDiag = math.sqrt(dy * dy + self.gridSize * self.gridSize) - if not self:isValidNode(x + self.gridSize, y, t, self.constraints) then - table.insert(primitives, { x = x + self.gridSize, y = y + dy, - t = math.atan2(dy, self.gridSize), d = dDiag }) - table.insert(JumpPointSearch.markers, { label = 'forced x +', x = x + self.gridSize, y = y + dy }) - node.forced = true - end - if not self:isValidNode(x - self.gridSize, y, t, self.constraints) then - table.insert(primitives, { x = x - self.gridSize, y = y + dy, - t = math.atan2(dy, -self.gridSize), d = dDiag }) - table.insert(JumpPointSearch.markers, { label = 'forced x -', x = x - self.gridSize, y = y + dy }) - node.forced = true - end - else - -- move along the x axis - if self:isValidNode(x + dx, y, t, self.constraints) then - table.insert(primitives, { x = x + dx, y = y, t = math.atan2(0, dx), d = math.abs(dx) }) - end - -- Forced neighbors - dDiag = math.sqrt(dx * dx + self.gridSize * self.gridSize) - if not self:isValidNode(x, y + self.gridSize, t, self.constraints) then - table.insert(primitives, { x = x + dx, y = y + self.gridSize, - t = math.atan2(self.gridSize, dx), d = dDiag }) - table.insert(JumpPointSearch.markers, { label = 'forced y +', x = x + dx, y = y + self.gridSize }) - node.forced = true - end - if not self:isValidNode(x, y - self.gridSize, t, self.constraints) then - table.insert(primitives, { x = x + dx, y = y - self.gridSize, - t = math.atan2(-self.gridSize, dx), d = dDiag }) - table.insert(JumpPointSearch.markers, { label = 'forced y -', x = x + dx, y = y - self.gridSize }) - node.forced = true - end - end - end + local primitives + if node.primitive then + -- the primitive that was used to get to this node + primitives = self:getNextPrimitives(node, node.primitive) else - -- no parent, this is the start node - for _, p in pairs(self.primitives) do - -- JPS does not really use motion primitives, what we call primitives are actually the - -- successors, with their real coordinates, not just a delta. - table.insert(primitives, { x = node.x + p.dx, y = node.y + p.dy, t = p.dt, d = p.d }) + -- 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 + -- an ugly hack here, HybridAStar uses primitive.d to calculate the g cost but we pass the + -- actual successor node, not the primitive, so + table.insert(jumpNodes, State3D(jumpNode.x, jumpNode.y, jumpNode.t, node.g, node, Gear.Forward, Steer.Straight, + nil, (jumpNode - node):length(), p)) end end - return primitives + return jumpNodes end -function HybridAStar.JpsMotionPrimitives:jump(node, pred, recursionCounter) - if recursionCounter and recursionCounter > 2 then - return node, recursionCounter - end +function HybridAStar.JpsMotionPrimitives:jump(node, primitive, recursionCounter) recursionCounter = recursionCounter and recursionCounter + 1 or 1 - local x, y, t = node.x, node.y, node.t - if not self:isValidNode(x, y, t, self.constraints) then + if recursionCounter > 5 then + return node + end + local v = node + primitive + local successor = State3D(v.x, v.y, primitive:heading()) + if not self.constraints:isValidNode(successor) then return nil end - if node:equals(self.goal, self.deltaPosGoal, self.deltaThetaGoal) then - return node + if successor:equals(self.goal, self.deltaPosGoal, self.deltaThetaGoal) then + return successor end - local dx = x - pred.x - local dy = y - pred.y - if math.abs(dx) > 0.1 and math.abs(dy) > 0.1 then - -- diagonal move - if (self:isValidNode(x - dx, y + dy, t, self.constraints) and not self:isValidNode(x - dx, y, t, self.constraints)) or - (self:isValidNode(x + dx, y - dy, t, self.constraints) and not self:isValidNode(x, y - dy, t, self.constraints)) then - -- Current node is a jump point if one of its left or right neighbors ahead is forced - return node - end - else - if math.abs(dx) > 0.1 then - -- move along the x axis - if (self:isValidNode(x + dx, y + self.gridSize, t, self.constraints) and not self:isValidNode(x, y + self.gridSize, t, self.constraints)) or - (self:isValidNode(x + dx, y - self.gridSize, t, self.constraints) and not self:isValidNode(x, y - self.gridSize, t, self.constraints)) then - -- Current node is a jump point if one of its left or right neighbors ahead is forced - return node - 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 - -- move along the y axis - if (self:isValidNode(x + self.gridSize, y + dy, t, self.constraints) and not self:isValidNode(x + self.gridSize, y, t, self.constraints)) or - (self:isValidNode(x - self.gridSize, y + dy, t, self.constraints) and not self:isValidNode(x - self.gridSize, y, t, self.constraints)) then - -- Current node is a jump point if one of its left or right neighbors ahead is forced - return node + -- 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 - -- Recursive horizontal/vertical search - if math.abs(dx) > 0.1 and math.abs(dy) > 0.1 then - local nextNode = State3D.copy(node) - nextNode.x = nextNode.x + dx - nextNode.g = nextNode.g + dx - if self:jump(nextNode, node, recursionCounter) then - return node - end - nextNode = State3D.copy(node) - nextNode.y = nextNode.y + dy - nextNode.g = nextNode.g + dy - if self:jump(nextNode, node, recursionCounter) then - return node - end - end - -- Recursive diagonal search - if self:isValidNode(x + dx, y, t, self.constraints) or self:isValidNode(x, y + dy, t, self.constraints) then - local nextNode = State3D.copy(node) - nextNode.x = nextNode.x + dx - nextNode.y = nextNode.y + dy - nextNode.g = nextNode.g + dy - return self:jump(nextNode, node, recursionCounter) - end + return self:jump(successor, primitive, recursionCounter) end -function HybridAStar.JpsMotionPrimitives:createSuccessor(node, primitive, hitchLength) - local neighbor = State3D(primitive.x, primitive.y, primitive.t) - local jumpNode, jumps = self:jump(neighbor, node) - primitive.d = jumps and jumps * primitive.d or primitive.d - if jumpNode then - return State3D(jumpNode.x, jumpNode.y, jumpNode.t, node.g, node, Gear.Forward, Steer.Straight, - node:getNextTrailerHeading(self.gridSize, hitchLength)) - end +function HybridAStar.JpsMotionPrimitives:createSuccessor(node, jumpNode, hitchLength) + jumpNode.tTrailer = node:getNextTrailerHeading(self.gridSize, hitchLength) + return jumpNode end --- A Jump Point Search ---@class JumpPointSearch : AStar @@ -232,18 +225,21 @@ JumpPointSearch.markers = {} function JumpPointSearch:init(vehicle, yieldAfter, maxIterations) AStar.init(self, vehicle, yieldAfter, maxIterations) - JumpPointSearch.markers = {} end function JumpPointSearch:initRun(start, goal, turnRadius, allowReverse, constraints, hitchLength) - self.motionPrimitives = HybridAStar.JpsMotionPrimitives(self.deltaPos, self.deltaPosGoal, self.deltaThetaGoal, constraints, goal) + self.logger:setLevel(Logger.level.trace) return AStar.initRun(self, start, goal, turnRadius, allowReverse, constraints, hitchLength) end function JumpPointSearch:getMotionPrimitives(turnRadius, allowReverse) - return self.motionPrimitives + return HybridAStar.JpsMotionPrimitives(self.deltaPos, self.deltaPosGoal, self.deltaThetaGoal, self.constraints, self.goal) end +--[[function JumpPointSearch:rollUpPath(node, goal, path) + return HybridAStar.rollUpPath(self, node, goal, path) +end]] + ---@class HybridAStarWithJpsInTheMiddle : HybridAStarWithAStarInTheMiddle HybridAStarWithJpsInTheMiddle = CpObject(HybridAStarWithAStarInTheMiddle) @@ -254,3 +250,4 @@ end function HybridAStarWithJpsInTheMiddle:getAStar() return JumpPointSearch(self.yieldAfter) end + diff --git a/scripts/pathfinder/State3D.lua b/scripts/pathfinder/State3D.lua index d9b4c3216..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 From ebd419045d9e0dbf5473089b21891621536fdd75 Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Tue, 22 Apr 2025 08:13:08 -0400 Subject: [PATCH 23/28] feat: hybrid pathfinder with JPS --- scripts/pathfinder/HybridAStar.lua | 16 +- .../HybridAStarWithAStarInTheMiddle.lua | 24 +-- scripts/pathfinder/JumpPointSearch.lua | 183 ++++++++---------- scripts/pathfinder/PathfinderUtil.lua | 4 +- 4 files changed, 105 insertions(+), 122 deletions(-) diff --git a/scripts/pathfinder/HybridAStar.lua b/scripts/pathfinder/HybridAStar.lua index c6492011c..ba533bf83 100644 --- a/scripts/pathfinder/HybridAStar.lua +++ b/scripts/pathfinder/HybridAStar.lua @@ -467,9 +467,14 @@ function HybridAStar.PenaltyCache:inSameCell(n1, n2) end ---@param node State3D -function HybridAStar.PenaltyCache:get(node) +function HybridAStar.PenaltyCache:get(node, constraints) local x, y, t = self:getNodeIndexes(node) - return self.nodes[x] and self.nodes[x][y] + local cachedPenalty = self.nodes[x] and self.nodes[x][y] + if cachedPenalty == nil then + cachedPenalty = constraints:getNodePenalty(node) + self:add(node, cachedPenalty) + end + return cachedPenalty end ---@param node State3D @@ -675,12 +680,7 @@ 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 - local penalty = self.penaltyCache:get(succ) - if penalty == nil then - penalty = self.constraints:getNodePenalty(succ) - self.penaltyCache:add(succ, penalty) - end - succ:updateG(primitive, penalty) + 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) diff --git a/scripts/pathfinder/HybridAStarWithAStarInTheMiddle.lua b/scripts/pathfinder/HybridAStarWithAStarInTheMiddle.lua index e8e5a7ea9..1c4730756 100644 --- a/scripts/pathfinder/HybridAStarWithAStarInTheMiddle.lua +++ b/scripts/pathfinder/HybridAStarWithAStarInTheMiddle.lua @@ -13,7 +13,7 @@ function HybridAStarWithAStarInTheMiddle:init(vehicle, yieldAfter, maxIterations -- path generation phases self.vehicle = vehicle 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 +22,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 @@ -53,10 +53,10 @@ 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) @@ -118,10 +118,10 @@ function HybridAStarWithAStarInTheMiddle:resume(...) 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 result.path then - self:debug('fast A*: no path found') + self:debug('First pass: no path found') return PathfinderResult(true, nil, result.goalNodeInvalid, self.currentPathfinder:getHighestDistance(), self.constraints) end @@ -191,7 +191,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() @@ -211,7 +211,7 @@ function HybridAStarWithAStarInTheMiddle:nodeIteratorStart() end function HybridAStarWithAStarInTheMiddle:nodeIteratorMiddle() - return self.aStarPathfinder:nodeIterator() + return self.middlePathfinder:nodeIterator() end function HybridAStarWithAStarInTheMiddle:nodeIteratorEnd() @@ -257,6 +257,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 index e8cd519c8..dd05aac8a 100644 --- a/scripts/pathfinder/JumpPointSearch.lua +++ b/scripts/pathfinder/JumpPointSearch.lua @@ -1,11 +1,25 @@ ----@class HybridAStar.JpsMotionPrimitives : HybridAStar.SimpleMotionPrimitives -HybridAStar.JpsMotionPrimitives = CpObject(HybridAStar.SimpleMotionPrimitives) -function HybridAStar.JpsMotionPrimitives:init(gridSize, deltaPosGoal, deltaThetaGoal, constraints, goal) +--- A Jump Point Search +---@class JumpPointSearch : AStar, HybridAStar.MotionPrimitives +JumpPointSearch = CpObject(AStar) +JumpPointSearch.markers = {} + +function JumpPointSearch:initRun(start, goal, turnRadius, allowReverse, constraints, hitchLength) + self.logger:setLevel(Logger.level.trace) + 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 = gridSize - local d = gridSize + 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)) @@ -18,91 +32,68 @@ function HybridAStar.JpsMotionPrimitives:init(gridSize, deltaPosGoal, deltaTheta -- 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 = { - -- unconditionally include natural neighbors. - -- always need this neighbor to the left or right of the current node { check = nil, nextPrimitive = self.primitives[1] }, - -- if there is an obstacle at 'check' if we got to the node through this primitive, then - -- create a forced neighbors at 'forcedNeighbor' { check = Vector(0, self.gridSize), nextPrimitive = self.primitives[2] }, { check = Vector(0, -self.gridSize), nextPrimitive = self.primitives[8] } } -- left self.primitives[5].nextPrimitives = { - -- unconditionally include natural neighbors. - -- always need this neighbor to the left or right of the current node { check = nil, nextPrimitive = self.primitives[5] }, - -- if there is an obstacle at 'check' if we got to the node through this primitive, then - -- create a forced neighbors at 'forcedNeighbor' { check = Vector(0, self.gridSize), nextPrimitive = self.primitives[4] }, { check = Vector(0, -self.gridSize), nextPrimitive = self.primitives[6] } } -- up self.primitives[3].nextPrimitives = { - -- unconditionally include natural neighbors. - -- always need this neighbor to the left or right of the current node { check = nil, nextPrimitive = self.primitives[3] }, - -- forced neighbors are included only if the checked neighbors are invalid { check = Vector(self.gridSize, 0), nextPrimitive = self.primitives[2] }, { check = Vector(-self.gridSize, 0), nextPrimitive = self.primitives[4] } } -- down self.primitives[7].nextPrimitives = { - -- unconditionally include natural neighbors. - -- always need this neighbor to the left or right of the current node { check = nil, nextPrimitive = self.primitives[7] }, - -- forced neighbors are included only if the checked neighbors are invalid { check = Vector(self.gridSize, 0), nextPrimitive = self.primitives[8] }, { check = Vector(-self.gridSize, 0), nextPrimitive = self.primitives[6] } } -- up right self.primitives[2].nextPrimitives = { - -- unconditionally include natural neighbors. - -- we always need this neighbor to the left or right of the current node, plus the one in the same direction { check = nil, nextPrimitive = self.primitives[2] }, { check = nil, nextPrimitive = self.primitives[1] }, { check = nil, nextPrimitive = self.primitives[3] }, - -- forced neighbors are included only if the checked neighbors are invalid { check = Vector(-self.gridSize, 0), nextPrimitive = self.primitives[4] }, { check = Vector(0, -self.gridSize), nextPrimitive = self.primitives[8] }, } -- up left self.primitives[4].nextPrimitives = { - -- unconditionally include natural neighbors. - -- we always need this neighbor to the left or right of the current node, plus the one in the same direction { check = nil, nextPrimitive = self.primitives[4] }, { check = nil, nextPrimitive = self.primitives[3] }, { check = nil, nextPrimitive = self.primitives[5] }, - -- forced neighbors are included only if the checked neighbors are invalid { check = Vector(self.gridSize, 0), nextPrimitive = self.primitives[2] }, { check = Vector(0, -self.gridSize), nextPrimitive = self.primitives[6] }, } -- down left self.primitives[6].nextPrimitives = { - -- unconditionally include natural neighbors. - -- we always need this neighbor to the left or right of the current node, plus the one in the same direction { check = nil, nextPrimitive = self.primitives[6] }, { check = nil, nextPrimitive = self.primitives[5] }, { check = nil, nextPrimitive = self.primitives[7] }, - -- forced neighbors are included only if the checked neighbors are invalid { check = Vector(self.gridSize, 0), nextPrimitive = self.primitives[8] }, { check = Vector(0, self.gridSize), nextPrimitive = self.primitives[4] }, } -- down right self.primitives[8].nextPrimitives = { - -- unconditionally include natural neighbors. - -- we always need this neighbor to the left or right of the current node, plus the one in the same direction { check = nil, nextPrimitive = self.primitives[8] }, { check = nil, nextPrimitive = self.primitives[1] }, { check = nil, nextPrimitive = self.primitives[7] }, - -- forced neighbors are included only if the checked neighbors are invalid { check = Vector(-self.gridSize, 0), nextPrimitive = self.primitives[6] }, { check = Vector(0, self.gridSize), nextPrimitive = self.primitives[2] }, } @@ -114,30 +105,38 @@ function HybridAStar.JpsMotionPrimitives:init(gridSize, deltaPosGoal, deltaTheta -- cache the length self.primitives[i].d = self.primitives[i]:length() end - self.deltaPosGoal = deltaPosGoal - self.deltaThetaGoal = deltaThetaGoal - self.constraints = constraints - self.goal = goal end -function HybridAStar.JpsMotionPrimitives:getGridSize() - return self.gridSize +function JumpPointSearch:isValidNode(node) + return self.constraints:isValidNode(node, true, true) end -function HybridAStar.JpsMotionPrimitives:isValidNode(x, y, t, constraints) - local node = { x = x, y = y, t = t } - return constraints:isValidNode(node, true, true) +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 -function HybridAStar.JpsMotionPrimitives:getNextPrimitives(node, 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, node.y, n.check:heading()) + n.check - if not self.constraints:isValidNode(neighborToCheck, true, true) then + if not self:isValidNode(neighborToCheck) or self:isPenaltyChanging(neighborToCheck, node) then -- we have a forced neighbor table.insert(nextPrimitives, n.nextPrimitive) forced = true @@ -149,44 +148,20 @@ function HybridAStar.JpsMotionPrimitives:getNextPrimitives(node, primitive) return nextPrimitives, forced 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 whole --- framework in HybridAStar.lua with JPS. -function HybridAStar.JpsMotionPrimitives: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 - -- an ugly hack here, HybridAStar uses primitive.d to calculate the g cost but we pass the - -- actual successor node, not the primitive, so - table.insert(jumpNodes, State3D(jumpNode.x, jumpNode.y, jumpNode.t, node.g, node, Gear.Forward, Steer.Straight, - nil, (jumpNode - node):length(), p)) - end - end - return jumpNodes -end -function HybridAStar.JpsMotionPrimitives:jump(node, primitive, recursionCounter) +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.constraints:isValidNode(successor) then + 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 @@ -214,40 +189,48 @@ function HybridAStar.JpsMotionPrimitives:jump(node, primitive, recursionCounter) return self:jump(successor, primitive, recursionCounter) end -function HybridAStar.JpsMotionPrimitives:createSuccessor(node, jumpNode, hitchLength) - jumpNode.tTrailer = node:getNextTrailerHeading(self.gridSize, hitchLength) - return jumpNode -end ---- A Jump Point Search ----@class JumpPointSearch : AStar -JumpPointSearch = CpObject(AStar) -JumpPointSearch.markers = {} - -function JumpPointSearch:init(vehicle, yieldAfter, maxIterations) - AStar.init(self, vehicle, yieldAfter, maxIterations) -end - -function JumpPointSearch:initRun(start, goal, turnRadius, allowReverse, constraints, hitchLength) - self.logger:setLevel(Logger.level.trace) - return AStar.initRun(self, start, goal, turnRadius, allowReverse, constraints, hitchLength) +-- 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 -function JumpPointSearch:getMotionPrimitives(turnRadius, allowReverse) - return HybridAStar.JpsMotionPrimitives(self.deltaPos, self.deltaPosGoal, self.deltaThetaGoal, self.constraints, self.goal) +-- 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 ---[[function JumpPointSearch:rollUpPath(node, goal, path) - return HybridAStar.rollUpPath(self, node, goal, path) -end]] - ---@class HybridAStarWithJpsInTheMiddle : HybridAStarWithAStarInTheMiddle HybridAStarWithJpsInTheMiddle = CpObject(HybridAStarWithAStarInTheMiddle) -function HybridAStarWithJpsInTheMiddle:init(hybridRange, yieldAfter, maxIterations, mustBeAccurate) - HybridAStarWithAStarInTheMiddle.init(self, hybridRange, yieldAfter, maxIterations, mustBeAccurate) -end - -function HybridAStarWithJpsInTheMiddle:getAStar() - return JumpPointSearch(self.yieldAfter) +function HybridAStarWithJpsInTheMiddle:getFastPathfinder() + return JumpPointSearch(self.vehicle, self.yieldAfter, self.maxIterations) end diff --git a/scripts/pathfinder/PathfinderUtil.lua b/scripts/pathfinder/PathfinderUtil.lua index 31ee40e28..ed4c1ebc8 100644 --- a/scripts/pathfinder/PathfinderUtil.lua +++ b/scripts/pathfinder/PathfinderUtil.lua @@ -666,8 +666,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 From 8e4259d0ddfebbe4375988bd8b92e2ff88d9f79e Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Wed, 23 Apr 2025 07:43:28 -0400 Subject: [PATCH 24/28] feat: added penalty factor setting for JPS --- config/VehicleSettingsSetup.xml | 1 + modDesc.xml | 1 + scripts/pathfinder/JumpPointSearch.lua | 2 +- scripts/pathfinder/PathfinderConstraints.lua | 7 ++++--- scripts/pathfinder/PathfinderUtil.lua | 2 +- 5 files changed, 8 insertions(+), 5 deletions(-) diff --git a/config/VehicleSettingsSetup.xml b/config/VehicleSettingsSetup.xml index 24eb803ca..5c5250747 100644 --- a/config/VehicleSettingsSetup.xml +++ b/config/VehicleSettingsSetup.xml @@ -33,6 +33,7 @@ + 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/pathfinder/JumpPointSearch.lua b/scripts/pathfinder/JumpPointSearch.lua index dd05aac8a..cb0783425 100644 --- a/scripts/pathfinder/JumpPointSearch.lua +++ b/scripts/pathfinder/JumpPointSearch.lua @@ -135,7 +135,7 @@ function JumpPointSearch:getNextPrimitives(node, primitive) for _, n in ipairs(primitive.nextPrimitives) do if n.check then -- check if the neighbor is valid - local neighborToCheck = State3D(node.x, node.y, n.check:heading()) + n.check + 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) diff --git a/scripts/pathfinder/PathfinderConstraints.lua b/scripts/pathfinder/PathfinderConstraints.lua index ef8bd674f..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(self.vehicle, '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 diff --git a/scripts/pathfinder/PathfinderUtil.lua b/scripts/pathfinder/PathfinderUtil.lua index ed4c1ebc8..c3ff6ffe8 100644 --- a/scripts/pathfinder/PathfinderUtil.lua +++ b/scripts/pathfinder/PathfinderUtil.lua @@ -428,7 +428,7 @@ 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 = HybridAStarWithJpsInTheMiddle(vehicle, 100, maxIterations, mustBeAccurate) return pathfinder, pathfinder:start(start, goal, constraints.turnRadius, allowReverse, constraints, constraints.trailerHitchLength) end From abf53089d7c78016a6321be578796323953d60c3 Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Wed, 23 Apr 2025 08:01:34 -0400 Subject: [PATCH 25/28] test: debug level reduced --- scripts/pathfinder/JumpPointSearch.lua | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/scripts/pathfinder/JumpPointSearch.lua b/scripts/pathfinder/JumpPointSearch.lua index cb0783425..138713209 100644 --- a/scripts/pathfinder/JumpPointSearch.lua +++ b/scripts/pathfinder/JumpPointSearch.lua @@ -4,7 +4,7 @@ JumpPointSearch = CpObject(AStar) JumpPointSearch.markers = {} function JumpPointSearch:initRun(start, goal, turnRadius, allowReverse, constraints, hitchLength) - self.logger:setLevel(Logger.level.trace) + self.logger:setLevel(Logger.level.debug) return AStar.initRun(self, start, goal, turnRadius, allowReverse, constraints, hitchLength) end From c9d52c259c004fe920d7e21a375271dc1116c26e Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Wed, 23 Apr 2025 13:44:38 -0400 Subject: [PATCH 26/28] feat: enable/disable JPS Also log should now indicate which pathfinder is being used. --- config/VehicleSettingsSetup.xml | 1 + scripts/pathfinder/AStar.lua | 1 + scripts/pathfinder/HybridAStar.lua | 5 +++-- scripts/pathfinder/HybridAStarWithAStarInTheMiddle.lua | 4 +++- scripts/pathfinder/JumpPointSearch.lua | 5 +++++ scripts/pathfinder/PathfinderUtil.lua | 7 ++++++- 6 files changed, 19 insertions(+), 4 deletions(-) diff --git a/config/VehicleSettingsSetup.xml b/config/VehicleSettingsSetup.xml index 5c5250747..ddf27c551 100644 --- a/config/VehicleSettingsSetup.xml +++ b/config/VehicleSettingsSetup.xml @@ -33,6 +33,7 @@ + diff --git a/scripts/pathfinder/AStar.lua b/scripts/pathfinder/AStar.lua index b22b7a574..db63c7318 100644 --- a/scripts/pathfinder/AStar.lua +++ b/scripts/pathfinder/AStar.lua @@ -6,6 +6,7 @@ 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 diff --git a/scripts/pathfinder/HybridAStar.lua b/scripts/pathfinder/HybridAStar.lua index ba533bf83..83d713912 100644 --- a/scripts/pathfinder/HybridAStar.lua +++ b/scripts/pathfinder/HybridAStar.lua @@ -494,7 +494,8 @@ HybridAStar.defaultMaxIterations = 40000 ---@param maxIterations number ---@param mustBeAccurate boolean|nil function HybridAStar:init(vehicle, yieldAfter, maxIterations, mustBeAccurate) - self.logger = Logger('HybridAStar', Logger.level.error, CpDebug.DBG_PATHFINDER) + self.name = 'HybridAStar' + self.logger = Logger(self.name, Logger.level.error, CpDebug.DBG_PATHFINDER) self.vehicle = vehicle self.iterationsSinceYield = 0 self.yields = 0 @@ -545,7 +546,7 @@ end --- rear axle of the trailer), can be nil ---@return PathfinderResult function HybridAStar:initRun(start, goal, turnRadius, allowReverse, constraints, hitchLength) - self:debug('Start pathfinding between %s and %s', tostring(start), tostring(goal)) + self:debug('Start %s pathfinding between %s and %s', self.name, tostring(start), tostring(goal)) self:debug(' turnRadius = %.1f, allowReverse: %s', turnRadius, tostring(allowReverse)) self.goal = goal self.turnRadius = turnRadius diff --git a/scripts/pathfinder/HybridAStarWithAStarInTheMiddle.lua b/scripts/pathfinder/HybridAStarWithAStarInTheMiddle.lua index 1c4730756..b79205ef5 100644 --- a/scripts/pathfinder/HybridAStarWithAStarInTheMiddle.lua +++ b/scripts/pathfinder/HybridAStarWithAStarInTheMiddle.lua @@ -10,8 +10,9 @@ 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.FAST = 2 self.MIDDLE_TO_END = 3 @@ -250,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(...) diff --git a/scripts/pathfinder/JumpPointSearch.lua b/scripts/pathfinder/JumpPointSearch.lua index 138713209..57a0a01dc 100644 --- a/scripts/pathfinder/JumpPointSearch.lua +++ b/scripts/pathfinder/JumpPointSearch.lua @@ -3,6 +3,11 @@ 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) diff --git a/scripts/pathfinder/PathfinderUtil.lua b/scripts/pathfinder/PathfinderUtil.lua index c3ff6ffe8..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 = HybridAStarWithJpsInTheMiddle(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 From 639d02d2437530c6793a95a22ab027df52e2977c Mon Sep 17 00:00:00 2001 From: Tensuko Date: Thu, 24 Apr 2025 20:24:14 +0200 Subject: [PATCH 27/28] Translations --- config/MasterTranslations.xml | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) 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 @@ + + + + + + + + + + + + + + + + From 6b91266b4f26d7821efa45934092397c8d29f634 Mon Sep 17 00:00:00 2001 From: Tensuko Date: Thu, 24 Apr 2025 18:24:30 +0000 Subject: [PATCH 28/28] Updated translations --- translations/translation_br.xml | 4 ++++ translations/translation_cs.xml | 4 ++++ translations/translation_ct.xml | 4 ++++ translations/translation_cz.xml | 4 ++++ translations/translation_da.xml | 4 ++++ translations/translation_de.xml | 4 ++++ translations/translation_ea.xml | 4 ++++ translations/translation_en.xml | 4 ++++ translations/translation_es.xml | 4 ++++ translations/translation_fc.xml | 4 ++++ translations/translation_fi.xml | 4 ++++ translations/translation_fr.xml | 4 ++++ translations/translation_hu.xml | 4 ++++ translations/translation_id.xml | 4 ++++ translations/translation_it.xml | 4 ++++ translations/translation_jp.xml | 4 ++++ translations/translation_kr.xml | 4 ++++ translations/translation_nl.xml | 4 ++++ translations/translation_no.xml | 4 ++++ translations/translation_pl.xml | 4 ++++ translations/translation_pt.xml | 4 ++++ translations/translation_ro.xml | 4 ++++ translations/translation_ru.xml | 4 ++++ translations/translation_sv.xml | 4 ++++ translations/translation_tr.xml | 4 ++++ translations/translation_uk.xml | 4 ++++ translations/translation_vi.xml | 4 ++++ 27 files changed, 108 insertions(+) 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 @@ + + + +