From 833f099db1f0a64f1d1db10f23f8b5b2dc5a3826 Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Sat, 28 Sep 2024 08:57:17 -0400 Subject: [PATCH 1/2] fix: reverse at turn end When a turn needs to be moved back to fit on the field, it ends with a straight reversing section to back the implement up to the work start point. The actual start of this section depends on when the implement was aligned with the row. The generated start point of this reversing section however, at this point, may lay well behind the implement's reversing node, and this will result the PPC stuck in initializing mode and the vehicle reversing forever, since it is moving further and further away from the start point. So, when changing to reverse, do not initialize to the first reverse waypoint, instead, find the one closest to the implement's reverser node. --- scripts/Course.lua | 6 +++-- scripts/ai/PurePursuitController.lua | 35 ++++++++++++++++++++++++---- scripts/ai/turns/AITurn.lua | 6 ++++- 3 files changed, 40 insertions(+), 7 deletions(-) diff --git a/scripts/Course.lua b/scripts/Course.lua index d05336a77..59806f20c 100644 --- a/scripts/Course.lua +++ b/scripts/Course.lua @@ -1324,9 +1324,10 @@ 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) @@ -1334,7 +1335,8 @@ function Course:getNearestWaypoints(node) 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 diff --git a/scripts/ai/PurePursuitController.lua b/scripts/ai/PurePursuitController.lua index 7339ddfdc..e20ac0d31 100644 --- a/scripts/ai/PurePursuitController.lua +++ b/scripts/ai/PurePursuitController.lua @@ -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) @@ -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 diff --git a/scripts/ai/turns/AITurn.lua b/scripts/ai/turns/AITurn.lua index 852d5265b..716d0ccd8 100644 --- a/scripts/ai/turns/AITurn.lua +++ b/scripts/ai/turns/AITurn.lua @@ -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 From c8e1ddd5dff97922154e9ed52c075c343f414cb1 Mon Sep 17 00:00:00 2001 From: Tensuko Date: Sat, 28 Sep 2024 16:19:37 +0200 Subject: [PATCH 2/2] Update VehicleConfigurations.xml --- config/VehicleConfigurations.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/config/VehicleConfigurations.xml b/config/VehicleConfigurations.xml index d4d8d7e3c..dded4f33b 100644 --- a/config/VehicleConfigurations.xml +++ b/config/VehicleConfigurations.xml @@ -554,6 +554,8 @@ You can define the following custom settings: turnRadius = "7.5" implementWheelAlwaysOnGround = "true" workingWidth = "10.5" + raiseLate = "true" + lowerEarly = "true" />