feat: Graph pathfinder

.github/workflows/unit-test.yml

@@ -35,7 +35,7 @@ jobs:
           lua LoggerTest.lua
       - name: Run course generator unit tests
         run: |
-          cd scripts/courseGenerator/test
+          pushd scripts/courseGenerator/test
           lua BlockSequencerTest.lua
           lua CacheMapTest.lua
           lua CenterTest.lua
@@ -53,3 +53,6 @@ jobs:
           lua TransformTest.lua
           lua VertexTest.lua
           lua WrapAroundIndexTest.lua
+          popd
+          pushd scripts/pathfinder/test
+          lua GraphPathfinderTest.lua

config/MasterTranslations.xml

@@ -547,6 +547,22 @@
 			<Text language="de"><![CDATA[Fahrzeug bleibt nach der Arbeit am Endpunkt stehen.]]></Text>
 			<Text language="en"><![CDATA[Vehicle stops at the end point after work.]]></Text>
 		</Translation>
+		<Translation name="CP_vehicle_setting_useJps_title">
+			<Text language="de"><![CDATA[Verwende JPS Pathfinder]]></Text>
+			<Text language="en"><![CDATA[Use JPS pathfinder]]></Text>
+		</Translation>
+		<Translation name="CP_vehicle_setting_useJps_tooltip">
+			<Text language="de"><![CDATA[Benutze für lange Strecken den Jump Point Search Pathfinder Algorithmus. Dieser läuft meistens schneller.]]></Text>
+			<Text language="en"><![CDATA[Use jump point search for longer distances, wich results in faster pathfinding.]]></Text>
+		</Translation>
+		<Translation name="CP_vehicle_setting_penaltyFactor_title">
+			<Text language="de"><![CDATA[Straffaktor für Pathfinder]]></Text>
+			<Text language="en"><![CDATA[Pathfinder penalty factor]]></Text>
+		</Translation>
+		<Translation name="CP_vehicle_setting_penaltyFactor_tooltip">
+			<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>
+			<Text language="en"><![CDATA[A higher value will take longer, but the path stays more on the field and outside the fruit.]]></Text>
+		</Translation>
 		<Translation name="CP_vehicle_setting_allowReversePathfinding_title">
 			<Text language="de"><![CDATA[Pathfinder darf rückwärtsfahren]]></Text>
 			<Text language="en"><![CDATA[Allow reverse for pathfinding]]></Text>

config/VehicleSettingsSetup.xml

@@ -33,6 +33,8 @@
 		<Setting classType="AIParameterBooleanSetting" name="turnOnField" defaultBool="true" isVisible="areCourseSettingsVisible"/>
 		<!--Avoid Fruit-->
 		<Setting classType="AIParameterBooleanSetting" name="avoidFruit" defaultBool="true" isExpertModeOnly="true"/>
+		<Setting classType="AIParameterBooleanSetting" name="useJps" defaultBool="true" isExpertModeOnly="true"/>
+		<Setting classType="AIParameterSettingList" name="penaltyFactor" min="1" max="10" incremental="0.1" default="1" />
 		<!--Pathfinder Reverse-->
 		<Setting classType="AIParameterBooleanSetting" name="allowReversePathfinding" defaultBool="true" isExpertModeOnly="true" isVisible="areCourseSettingsVisible"/>
 		<Setting classType="AIParameterBooleanSetting" name="allowPathfinderTurns" defaultBool="false" isExpertModeOnly="true" isVisible="areCourseSettingsVisible"/>

modDesc.xml

@@ -164,6 +164,7 @@ Changelog 8.0.0.0:
 		<sourceFile filename="scripts/pathfinder/HybridAStar.lua"/>
 		<sourceFile filename="scripts/pathfinder/AStar.lua"/>
 		<sourceFile filename="scripts/pathfinder/HybridAStarWithAStarInTheMiddle.lua"/>
+		<sourceFile filename="scripts/pathfinder/JumpPointSearch.lua"/>
 		<sourceFile filename="scripts/pathfinder/PathfinderCollisionDetector.lua"/>
 		<sourceFile filename="scripts/pathfinder/PathfinderConstraints.lua"/>
 		<sourceFile filename="scripts/pathfinder/PathfinderContext.lua"/>

scripts/geometry/Polyline.lua

@@ -46,8 +46,12 @@ function Polyline:prepend(v)
     end
 end
 
+function Polyline:_getNewInstance()
+    return Polyline()
+end
+
 function Polyline:clone()
-    local clone = Polyline({})
+    local clone = self:_getNewInstance()
     for _, v in ipairs(self) do
         clone:append(v:clone())
     end
@@ -460,7 +464,9 @@ end
 --- as a turn waypoint.
 ---@param r number turning radius
 ---@param makeCorners boolean if true, make corners for turn maneuvers instead of rounding them.
-function Polyline:ensureMinimumRadius(r, makeCorners)
+---@param maxCrossTrackError number|nil maximum cross track error allowed before we start adjusting corners,
+---defaults to CourseGenerator.cMaxCrossTrackError
+function Polyline:ensureMinimumRadius(r, makeCorners, maxCrossTrackError)
 
     ---@param entry CourseGenerator.Slider
     ---@param exit CourseGenerator.Slider
@@ -488,8 +494,9 @@ function Polyline:ensureMinimumRadius(r, makeCorners)
         nextIx = currentIx + 1
         local xte = self:at(currentIx):getXte(r)
         local radius = self:at(currentIx):getRadius()
-        if xte > CourseGenerator.cMaxCrossTrackError then
-            self.logger:debug('ensureMinimumRadius (%s): found a corner at %d with r: %.1f, r: %.1f, xte: %.1f', debugId, currentIx, radius, r, xte)
+        if xte > (maxCrossTrackError or CourseGenerator.cMaxCrossTrackError) then
+            self.logger:debug('ensureMinimumRadius (%s): found a corner at %d with r: %.1f, r: %.1f, xte: %.1f',
+                    debugId, currentIx, radius, r, xte)
             -- looks like we can't make this turn without deviating too much from the course,
             local entry = CourseGenerator.Slider(self, currentIx, 0)
             local exit = CourseGenerator.Slider(self, currentIx, 0)
@@ -864,7 +871,7 @@ end
 function Polyline:__tostring()
     local result = ''
     for i, v in ipairs(self) do
-        result = result .. string.format('%d %s\n', i, v)
+        result = result .. string.format('%d %s %s\n', i, v, v.xte)
     end
     return result
 end

scripts/pathfinder/AStar.lua

@@ -6,12 +6,13 @@ AStar = CpObject(HybridAStar)
 
 function AStar:init(vehicle, yieldAfter, maxIterations)
     HybridAStar.init(self, vehicle, yieldAfter, maxIterations)
+    self.name = 'AStar'
     -- this needs to be small enough that no vehicle fit between the grid points (and remain undetected)
     self.deltaPos = 3
     self.deltaPosGoal = self.deltaPos
     self.deltaThetaDeg = 181
     self.deltaThetaGoal = math.rad(self.deltaThetaDeg)
-    self.maxDeltaTheta = math.pi
+    self.maxDeltaTheta = self.deltaThetaGoal
     self.originalDeltaThetaGoal = self.deltaThetaGoal
     self.analyticSolverEnabled = false
     self.ignoreValidityAtStart = false
@@ -31,14 +32,14 @@ function AStar.SimpleMotionPrimitives:init(gridSize, allowReverse)
     self.gridSize = gridSize
     local d = gridSize
     local dSqrt2 = math.sqrt(2) * d
-    table.insert(self.primitives, { dx = d, dy = 0, dt = 0, d = d, gear = Gear.Forward, steer = Steer.Straight, type = HybridAStar.MotionPrimitiveTypes.NA })
-    table.insert(self.primitives, { dx = d, dy = d, dt = 1 * math.pi / 4, d = dSqrt2, gear = Gear.Forward, steer = Steer.Straight, type = HybridAStar.MotionPrimitiveTypes.NA })
-    table.insert(self.primitives, { dx = 0, dy = d, dt = 2 * math.pi / 4, d = d, gear = Gear.Forward, steer = Steer.Straight, type = HybridAStar.MotionPrimitiveTypes.NA })
-    table.insert(self.primitives, { dx = -d, dy = d, dt = 3 * math.pi / 4, d = dSqrt2, gear = Gear.Forward, steer = Steer.Straight, type = HybridAStar.MotionPrimitiveTypes.NA })
-    table.insert(self.primitives, { dx = -d, dy = 0, dt = 4 * math.pi / 4, d = d, gear = Gear.Forward, steer = Steer.Straight, type = HybridAStar.MotionPrimitiveTypes.NA })
-    table.insert(self.primitives, { dx = -d, dy = -d, dt = 5 * math.pi / 4, d = dSqrt2, gear = Gear.Forward, steer = Steer.Straight, type = HybridAStar.MotionPrimitiveTypes.NA })
-    table.insert(self.primitives, { dx = 0, dy = -d, dt = 6 * math.pi / 4, d = d, gear = Gear.Forward, steer = Steer.Straight, type = HybridAStar.MotionPrimitiveTypes.NA })
-    table.insert(self.primitives, { dx = d, dy = -d, dt = 7 * math.pi / 4, d = dSqrt2, gear = Gear.Forward, steer = Steer.Straight, type = HybridAStar.MotionPrimitiveTypes.NA })
+    table.insert(self.primitives, { dx = d, dy = 0, dt = 0, d = d, gear = Gear.Forward, steer = Steer.Straight})
+    table.insert(self.primitives, { dx = d, dy = d, dt = 1 * math.pi / 4, d = dSqrt2, gear = Gear.Forward, steer = Steer.Straight})
+    table.insert(self.primitives, { dx = 0, dy = d, dt = 2 * math.pi / 4, d = d, gear = Gear.Forward, steer = Steer.Straight})
+    table.insert(self.primitives, { dx = -d, dy = d, dt = 3 * math.pi / 4, d = dSqrt2, gear = Gear.Forward, steer = Steer.Straight})
+    table.insert(self.primitives, { dx = -d, dy = 0, dt = 4 * math.pi / 4, d = d, gear = Gear.Forward, steer = Steer.Straight})
+    table.insert(self.primitives, { dx = -d, dy = -d, dt = 5 * math.pi / 4, d = dSqrt2, gear = Gear.Forward, steer = Steer.Straight})
+    table.insert(self.primitives, { dx = 0, dy = -d, dt = 6 * math.pi / 4, d = d, gear = Gear.Forward, steer = Steer.Straight})
+    table.insert(self.primitives, { dx = d, dy = -d, dt = 7 * math.pi / 4, d = dSqrt2, gear = Gear.Forward, steer = Steer.Straight})
 end
 
 --- A* successors are simply the grid neighbors