Skip to content

Commit

Permalink
Merge pull request #3476 from Courseplay/3469-bug_sp-vehicle-reverses…
Browse files Browse the repository at this point in the history
…-uncontrollably

fix: reverse at turn end
  • Loading branch information
Tensuko authored Sep 28, 2024
2 parents 86c73d8 + c8e1ddd commit 7c96787
Show file tree
Hide file tree
Showing 4 changed files with 42 additions and 7 deletions.
2 changes: 2 additions & 0 deletions config/VehicleConfigurations.xml
Original file line number Diff line number Diff line change
Expand Up @@ -554,6 +554,8 @@ You can define the following custom settings:
turnRadius = "7.5"
implementWheelAlwaysOnGround = "true"
workingWidth = "10.5"
raiseLate = "true"
lowerEarly = "true"
/>
<!--Mod: KOECKERLING Vector 460-->
<Vehicle name="vector460.xml"
Expand Down
6 changes: 4 additions & 2 deletions scripts/Course.lua
Original file line number Diff line number Diff line change
Expand Up @@ -1324,17 +1324,19 @@ function Course:getSectionAsNewCourse(startIx, endIx, reverse, allAttributes)
end

--- @param node number the node around we are looking for waypoints
--- @param startIx number|nil start looking for waypoints at this index
--- @return number, number, number, number the waypoint closest to node, its distance, the waypoint closest to the node
--- pointing approximately (+-45) in the same direction as the node and its distance
function Course:getNearestWaypoints(node)
function Course:getNearestWaypoints(node, startIx)
local nx, _, nz = getWorldTranslation(node)
local lx, _, lz = localDirectionToWorld(node, 0, 0, 1)
local nodeAngle = math.atan2(lx, lz)
local maxDeltaAngle = math.pi / 2
local dClosest, dClosestRightDirection = math.huge, math.huge
local ixClosest, ixClosestRightDirection = 1, 1

for i, p in ipairs(self.waypoints) do
for i = startIx or 1, #self.waypoints do
local p = self.waypoints[i]
local x, _, z = self:getWaypointPosition(i)
local d = MathUtil.getPointPointDistance(x, z, nx, nz)
if d < dClosest then
Expand Down
35 changes: 31 additions & 4 deletions scripts/ai/PurePursuitController.lua
Original file line number Diff line number Diff line change
Expand Up @@ -163,6 +163,28 @@ function PurePursuitController:initialize(ix)
self.case = 0
end

-- Initialize to a waypoint when reversing.
-- TODO: this has to be called explicitly but could be done automatically by the vanilla initialize()
-- make sure the waypoint we initialize to is close to the controlled node, otherwise the PPC will
-- remain in initializing mode if the waypoint is too far back from the controlled node, and just
-- reverse forever
function PurePursuitController:initializeForReversing(ix)
local reverserNode = self:getReverserNode()
if reverserNode then
self:debug('Reverser node found, initializing with it')
-- don't use ix as it is, instead, find the waypoint closest to the reverser node
local dPrev, d = math.huge, self.course:getWaypoint(ix):getDistanceFromNode(reverserNode)
while d < dPrev and self.course:isReverseAt(ix) and ix < self.course:getNumberOfWaypoints() do
dPrev = d
ix = ix + 1
d = self.course:getWaypoint(ix):getDistanceFromNode(reverserNode)
end
else
self:debug('No reverser node found, initializing with default controlled node')
end
self:initialize(ix)
end

-- TODO: make this more generic and allow registering multiple listeners?
-- could also implement listeners for events like notify me when within x meters of a waypoint, etc.
function PurePursuitController:registerListeners(waypointListener, onWaypointPassedFunc, onWaypointChangeFunc)
Expand Down Expand Up @@ -230,16 +252,21 @@ function PurePursuitController:getLastPassedWaypointIx()
return self.lastPassedWaypointIx
end

---@return number, string node that would be used for reversing, debug text explaining what node it is
function PurePursuitController:getReverserNode()
if not self.reversingImplement then
self.reversingImplement = AIUtil.getFirstReversingImplementWithWheels(self.vehicle, true)
end
return AIUtil.getReverserNode(self.vehicle, self.reversingImplement, true)
end

--- When reversing, use the towed implement's node as a reference
function PurePursuitController:switchControlledNode()
local lastControlledNode = self.controlledNode
local debugText = 'AIDirectionNode'
local reverserNode
if self:isReversing() then
if not self.reversingImplement then
self.reversingImplement = AIUtil.getFirstReversingImplementWithWheels(self.vehicle, true)
end
reverserNode, debugText = AIUtil.getReverserNode(self.vehicle, self.reversingImplement, true)
reverserNode, debugText = self:getReverserNode()
if reverserNode then
self:setControlledNode(reverserNode)
else
Expand Down
6 changes: 5 additions & 1 deletion scripts/ai/turns/AITurn.lua
Original file line number Diff line number Diff line change
Expand Up @@ -663,7 +663,11 @@ function CourseTurn:changeDirectionWhenAligned()
local nextDirectionChangeIx = self.turnCourse:getNextDirectionChangeFromIx(self.turnCourse:getCurrentWaypointIx())
if nextDirectionChangeIx then
self:debug('skipping to next direction change at %d', nextDirectionChangeIx + 1)
self.ppc:initialize(nextDirectionChangeIx + 1)
if self.turnCourse:isReverseAt(nextDirectionChangeIx + 1) then
self.ppc:initializeForReversing(nextDirectionChangeIx + 1)
else
self.ppc:initialize(nextDirectionChangeIx + 1)
end
end
end
end
Expand Down

0 comments on commit 7c96787

Please sign in to comment.