Skip to content

Commit 31e18f0

Browse files
TensukoTensuko
authored
Merge pull request #638 from Courseplay/graph-pathfinder
feat: Graph pathfinder
2 parents c5f1ae3 + 6b91266 commit 31e18f0

43 files changed

Lines changed: 1416 additions & 132 deletions

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

‎.github/workflows/unit-test.yml‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -35,7 +35,7 @@ jobs:
3535
lua LoggerTest.lua
3636
- name: Run course generator unit tests
3737
run: |
38-
cd scripts/courseGenerator/test
38+
pushd scripts/courseGenerator/test
3939
lua BlockSequencerTest.lua
4040
lua CacheMapTest.lua
4141
lua CenterTest.lua
@@ -53,3 +53,6 @@ jobs:
5353
lua TransformTest.lua
5454
lua VertexTest.lua
5555
lua WrapAroundIndexTest.lua
56+
popd
57+
pushd scripts/pathfinder/test
58+
lua GraphPathfinderTest.lua

‎config/MasterTranslations.xml‎

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -547,6 +547,22 @@
547547
<Text language="de"><![CDATA[Fahrzeug bleibt nach der Arbeit am Endpunkt stehen.]]></Text>
548548
<Text language="en"><![CDATA[Vehicle stops at the end point after work.]]></Text>
549549
</Translation>
550+
<Translation name="CP_vehicle_setting_useJps_title">
551+
<Text language="de"><![CDATA[Verwende JPS Pathfinder]]></Text>
552+
<Text language="en"><![CDATA[Use JPS pathfinder]]></Text>
553+
</Translation>
554+
<Translation name="CP_vehicle_setting_useJps_tooltip">
555+
<Text language="de"><![CDATA[Benutze für lange Strecken den Jump Point Search Pathfinder Algorithmus. Dieser läuft meistens schneller.]]></Text>
556+
<Text language="en"><![CDATA[Use jump point search for longer distances, wich results in faster pathfinding.]]></Text>
557+
</Translation>
558+
<Translation name="CP_vehicle_setting_penaltyFactor_title">
559+
<Text language="de"><![CDATA[Straffaktor für Pathfinder]]></Text>
560+
<Text language="en"><![CDATA[Pathfinder penalty factor]]></Text>
561+
</Translation>
562+
<Translation name="CP_vehicle_setting_penaltyFactor_tooltip">
563+
<Text language="de"><![CDATA[Ein höherer Wert dauert länger zu berechnen, bleibt dafür aber auch eher auf dem Feld und außerhalb der Frucht.]]></Text>
564+
<Text language="en"><![CDATA[A higher value will take longer, but the path stays more on the field and outside the fruit.]]></Text>
565+
</Translation>
550566
<Translation name="CP_vehicle_setting_allowReversePathfinding_title">
551567
<Text language="de"><![CDATA[Pathfinder darf rückwärtsfahren]]></Text>
552568
<Text language="en"><![CDATA[Allow reverse for pathfinding]]></Text>

‎config/VehicleSettingsSetup.xml‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -33,6 +33,8 @@
3333
<Setting classType="AIParameterBooleanSetting" name="turnOnField" defaultBool="true" isVisible="areCourseSettingsVisible"/>
3434
<!--Avoid Fruit-->
3535
<Setting classType="AIParameterBooleanSetting" name="avoidFruit" defaultBool="true" isExpertModeOnly="true"/>
36+
<Setting classType="AIParameterBooleanSetting" name="useJps" defaultBool="true" isExpertModeOnly="true"/>
37+
<Setting classType="AIParameterSettingList" name="penaltyFactor" min="1" max="10" incremental="0.1" default="1" />
3638
<!--Pathfinder Reverse-->
3739
<Setting classType="AIParameterBooleanSetting" name="allowReversePathfinding" defaultBool="true" isExpertModeOnly="true" isVisible="areCourseSettingsVisible"/>
3840
<Setting classType="AIParameterBooleanSetting" name="allowPathfinderTurns" defaultBool="false" isExpertModeOnly="true" isVisible="areCourseSettingsVisible"/>

‎modDesc.xml‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -164,6 +164,7 @@ Changelog 8.0.0.0:
164164
<sourceFile filename="scripts/pathfinder/HybridAStar.lua"/>
165165
<sourceFile filename="scripts/pathfinder/AStar.lua"/>
166166
<sourceFile filename="scripts/pathfinder/HybridAStarWithAStarInTheMiddle.lua"/>
167+
<sourceFile filename="scripts/pathfinder/JumpPointSearch.lua"/>
167168
<sourceFile filename="scripts/pathfinder/PathfinderCollisionDetector.lua"/>
168169
<sourceFile filename="scripts/pathfinder/PathfinderConstraints.lua"/>
169170
<sourceFile filename="scripts/pathfinder/PathfinderContext.lua"/>

‎scripts/geometry/Polyline.lua‎

Lines changed: 12 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -46,8 +46,12 @@ function Polyline:prepend(v)
4646
end
4747
end
4848

49+
function Polyline:_getNewInstance()
50+
return Polyline()
51+
end
52+
4953
function Polyline:clone()
50-
local clone = Polyline({})
54+
local clone = self:_getNewInstance()
5155
for _, v in ipairs(self) do
5256
clone:append(v:clone())
5357
end
@@ -460,7 +464,9 @@ end
460464
--- as a turn waypoint.
461465
---@param r number turning radius
462466
---@param makeCorners boolean if true, make corners for turn maneuvers instead of rounding them.
463-
function Polyline:ensureMinimumRadius(r, makeCorners)
467+
---@param maxCrossTrackError number|nil maximum cross track error allowed before we start adjusting corners,
468+
---defaults to CourseGenerator.cMaxCrossTrackError
469+
function Polyline:ensureMinimumRadius(r, makeCorners, maxCrossTrackError)
464470

465471
---@param entry CourseGenerator.Slider
466472
---@param exit CourseGenerator.Slider
@@ -488,8 +494,9 @@ function Polyline:ensureMinimumRadius(r, makeCorners)
488494
nextIx = currentIx + 1
489495
local xte = self:at(currentIx):getXte(r)
490496
local radius = self:at(currentIx):getRadius()
491-
if xte > CourseGenerator.cMaxCrossTrackError then
492-
self.logger:debug('ensureMinimumRadius (%s): found a corner at %d with r: %.1f, r: %.1f, xte: %.1f', debugId, currentIx, radius, r, xte)
497+
if xte > (maxCrossTrackError or CourseGenerator.cMaxCrossTrackError) then
498+
self.logger:debug('ensureMinimumRadius (%s): found a corner at %d with r: %.1f, r: %.1f, xte: %.1f',
499+
debugId, currentIx, radius, r, xte)
493500
-- looks like we can't make this turn without deviating too much from the course,
494501
local entry = CourseGenerator.Slider(self, currentIx, 0)
495502
local exit = CourseGenerator.Slider(self, currentIx, 0)
@@ -864,7 +871,7 @@ end
864871
function Polyline:__tostring()
865872
local result = ''
866873
for i, v in ipairs(self) do
867-
result = result .. string.format('%d %s\n', i, v)
874+
result = result .. string.format('%d %s %s\n', i, v, v.xte)
868875
end
869876
return result
870877
end

‎scripts/pathfinder/AStar.lua‎

Lines changed: 10 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -6,12 +6,13 @@ AStar = CpObject(HybridAStar)
66

77
function AStar:init(vehicle, yieldAfter, maxIterations)
88
HybridAStar.init(self, vehicle, yieldAfter, maxIterations)
9+
self.name = 'AStar'
910
-- this needs to be small enough that no vehicle fit between the grid points (and remain undetected)
1011
self.deltaPos = 3
1112
self.deltaPosGoal = self.deltaPos
1213
self.deltaThetaDeg = 181
1314
self.deltaThetaGoal = math.rad(self.deltaThetaDeg)
14-
self.maxDeltaTheta = math.pi
15+
self.maxDeltaTheta = self.deltaThetaGoal
1516
self.originalDeltaThetaGoal = self.deltaThetaGoal
1617
self.analyticSolverEnabled = false
1718
self.ignoreValidityAtStart = false
@@ -31,14 +32,14 @@ function AStar.SimpleMotionPrimitives:init(gridSize, allowReverse)
3132
self.gridSize = gridSize
3233
local d = gridSize
3334
local dSqrt2 = math.sqrt(2) * d
34-
table.insert(self.primitives, { dx = d, dy = 0, dt = 0, d = d, gear = Gear.Forward, steer = Steer.Straight, type = HybridAStar.MotionPrimitiveTypes.NA })
35-
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 })
36-
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 })
37-
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 })
38-
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 })
39-
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 })
40-
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 })
41-
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 })
35+
table.insert(self.primitives, { dx = d, dy = 0, dt = 0, d = d, gear = Gear.Forward, steer = Steer.Straight})
36+
table.insert(self.primitives, { dx = d, dy = d, dt = 1 * math.pi / 4, d = dSqrt2, gear = Gear.Forward, steer = Steer.Straight})
37+
table.insert(self.primitives, { dx = 0, dy = d, dt = 2 * math.pi / 4, d = d, gear = Gear.Forward, steer = Steer.Straight})
38+
table.insert(self.primitives, { dx = -d, dy = d, dt = 3 * math.pi / 4, d = dSqrt2, gear = Gear.Forward, steer = Steer.Straight})
39+
table.insert(self.primitives, { dx = -d, dy = 0, dt = 4 * math.pi / 4, d = d, gear = Gear.Forward, steer = Steer.Straight})
40+
table.insert(self.primitives, { dx = -d, dy = -d, dt = 5 * math.pi / 4, d = dSqrt2, gear = Gear.Forward, steer = Steer.Straight})
41+
table.insert(self.primitives, { dx = 0, dy = -d, dt = 6 * math.pi / 4, d = d, gear = Gear.Forward, steer = Steer.Straight})
42+
table.insert(self.primitives, { dx = d, dy = -d, dt = 7 * math.pi / 4, d = dSqrt2, gear = Gear.Forward, steer = Steer.Straight})
4243
end
4344

4445
--- A* successors are simply the grid neighbors

0 commit comments

Comments
 (0)