Skip to content

fix: pathfinder turn on headland #303

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
Tensuko merged 1 commit into from
Jan 6, 2025
Merged

fix: pathfinder turn on headland #303

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
17 changes: 11 additions & 6 deletions scripts/pathfinder/HybridAStar.lua
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -691,6 +691,11 @@ function HybridAStar:rollUpPath(node, goal, path)
return path
end

---@return number|nil the furthest the pathfinder got from the start
function HybridAStar:getHighestDistance()
return self.nodes and self.nodes.highestDistance
end

function HybridAStar:printOpenList(openList)
print('--- Open list ----')
for i, node in ipairs(openList.values) do
Expand Down Expand Up @@ -828,7 +833,7 @@ function HybridAStarWithAStarInTheMiddle:resume(...)
printCallstack()
self:debug('Pathfinding failed')
self.coroutine = nil
return PathfinderResult(true, nil, goalNodeInvalid, self.currentPathfinder.nodes.highestDistance,
return PathfinderResult(true, nil, goalNodeInvalid, self.currentPathfinder:getHighestDistance(),
self.constraints)
end
if done then
Expand All @@ -839,14 +844,14 @@ function HybridAStarWithAStarInTheMiddle:resume(...)
return PathfinderResult(true, path)
else
self:debug('all hybrid: no path found')
return PathfinderResult(true, nil, goalNodeInvalid, self.currentPathfinder.nodes.highestDistance,
return PathfinderResult(true, nil, goalNodeInvalid, self.currentPathfinder:getHighestDistance(),
self.constraints)
end
elseif self.phase == self.ASTAR then
self.constraints:resetStrictMode()
if not path then
self:debug('fast A*: no path found')
return PathfinderResult(true, nil, goalNodeInvalid, self.currentPathfinder.nodes.highestDistance,
return PathfinderResult(true, nil, goalNodeInvalid, self.currentPathfinder:getHighestDistance(),
self.constraints)
end
CourseGenerator.addDebugPolyline(Polyline(path), {1, 0, 0})
Expand All @@ -861,7 +866,7 @@ function HybridAStarWithAStarInTheMiddle:resume(...)
HybridAStar.shortenStart(self.middlePath, self.hybridRange)
HybridAStar.shortenEnd(self.middlePath, self.hybridRange)
if #self.middlePath < 2 then
return PathfinderResult(true, nil, goalNodeInvalid, self.currentPathfinder.nodes.highestDistance,
return PathfinderResult(true, nil, goalNodeInvalid, self.currentPathfinder:getHighestDistance(),
self.constraints)
end
State3D.smooth(self.middlePath)
Expand All @@ -886,7 +891,7 @@ function HybridAStarWithAStarInTheMiddle:resume(...)
return self:findPathFromMiddleToEnd()
else
self:debug('start to middle: no path found')
return PathfinderResult(true, nil, goalNodeInvalid, self.currentPathfinder.nodes.highestDistance,
return PathfinderResult(true, nil, goalNodeInvalid, self.currentPathfinder:getHighestDistance(),
self.constraints)
end
elseif self.phase == self.MIDDLE_TO_END then
Expand All @@ -905,7 +910,7 @@ function HybridAStarWithAStarInTheMiddle:resume(...)
return PathfinderResult(true, self.path)
else
self:debug('middle to end: no path found')
return PathfinderResult(true, nil, goalNodeInvalid, self.currentPathfinder.nodes.highestDistance,
return PathfinderResult(true, nil, goalNodeInvalid, self.currentPathfinder:getHighestDistance(),
self.constraints)
end
end
Expand Down
2 changes: 1 addition & 1 deletion scripts/pathfinder/PathfinderUtil.lua
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -586,7 +586,7 @@ function PathfinderUtil.findPathForTurn(vehicle, startOffset, goalReferenceNode,
if dirDeg > 45 or true then
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)
end
pathfinder = HybridAStarWithPathInTheMiddle(vehicle, turnRadius * 3, 200, headlandPath, true, analyticSolver)
pathfinder = HybridAStarWithPathInTheMiddle(vehicle, turnRadius * 6, 200, headlandPath, true, analyticSolver)
end
end
if pathfinder == nil then
Expand Down