From 482f944f2512ca3c33f338145a2ca79d8f5e046a Mon Sep 17 00:00:00 2001 From: Peter Vaiko Date: Wed, 13 Jan 2021 10:51:18 -0600 Subject: [PATCH] 6.03.00024 Bridge and proximity sensor fixes - Goal node elevation fixed (#6665) - Proximity sensor directly on the back of the combine/chopper allowing for the unloader to get closer. When driving over non-level bridges, the controlled node isn't vertical and the goal node was positioned on the ground level which is way below the vehicle. Due to this, the goal's position in the controlled node's reference wasn't accurate in this situation, seemed to be a lot closer when driving uphill, lot further when driving downhill. Uphill goal being too close caused oscillations in PPC. Moved the goal point on the level of the vehicle (controlled node), so the error caused by the non-vertical controlled node is negligible. Few proximity sensor and vehicle size fixes - Proximity sensor sits on the back of the combine now - fixed pathfinder vehicle data rectangles, clarified the misconception that the direction node is always on the root node, it is not, the direction node is the turning node, usually sits between the non-steered wheels. --- AIDriver.lua | 7 +++++-- CombineAIDriver.lua | 9 ++++++--- DevHelper.lua | 23 +++++++++++++++-------- FieldworkAIDriver.lua | 5 ++++- ProximitySensor.lua | 8 +++++--- PurePursuitController.lua | 26 ++++++++++++++++++++------ course-generator/PathfinderUtil.lua | 28 ++++++++++++++++++---------- modDesc.xml | 2 +- 8 files changed, 74 insertions(+), 34 deletions(-) diff --git a/AIDriver.lua b/AIDriver.lua index cedd1aed8..27fc270c3 100644 --- a/AIDriver.lua +++ b/AIDriver.lua @@ -1826,7 +1826,10 @@ function AIDriver:setBackMarkerNode(vehicle) local lastImplement lastImplement, backMarkerOffset = AIDriverUtil.getLastAttachedImplement(vehicle) referenceNode = vehicle.rootNode - self:debug('Using the last implement\'s rear distance for the rear proximity sensor, %d m from root node', backMarkerOffset) + self:debug('Using the last implement\'s rear distance for the rear proximity sensor, %d m from root node', backMarkerOffset) elseif self.measuredBackDistance then + referenceNode = vehicle.rootNode + backMarkerOffset = -self.measuredBackDistance + self:debug('back marker node on measured back distance %.1f', self.measuredBackDistance) elseif reverserNode then -- if there is a reverser node, use that, mainly because that most likely will turn with an implement -- or with the back component of an articulated vehicle. Just need to find out the distance correctly @@ -1838,7 +1841,7 @@ function AIDriver:setBackMarkerNode(vehicle) debugText, backMarkerOffset, dBetweenRootAndReverserNode) else referenceNode = vehicle.rootNode - backMarkerOffset = - vehicle.sizeLength / 2 - vehicle.lengthOffset + backMarkerOffset = - vehicle.sizeLength / 2 + vehicle.lengthOffset self:debug('Using the vehicle\'s root node for the rear proximity sensor, %d m from root node', backMarkerOffset) end if not vehicle.cp.driver.aiDriverData.backMarkerNode then diff --git a/CombineAIDriver.lua b/CombineAIDriver.lua index e98ce9b25..b509abf70 100644 --- a/CombineAIDriver.lua +++ b/CombineAIDriver.lua @@ -1640,8 +1640,8 @@ end function CombineAIDriver:measureBackDistance() self.measuredBackDistance = 0 -- raycast from a point behind the vehicle forward towards the direction node - local nx, ny, nz = localDirectionToWorld(self:getDirectionNode(), 0, 0, 1) - local x, y, z = localToWorld(self:getDirectionNode(), 0, 1.5, - self.maxBackDistance) + local nx, ny, nz = localDirectionToWorld(self.vehicle.rootNode, 0, 0, 1) + local x, y, z = localToWorld(self.vehicle.rootNode, 0, 1.5, - self.maxBackDistance) raycastAll(x, y, z, nx, ny, nz, 'raycastBackCallback', self.maxBackDistance, self) end @@ -1661,7 +1661,6 @@ function CombineAIDriver:raycastBackCallback(hitObjectId, x, y, z, distance, nx, end end - function CombineAIDriver:setStrawSwath(enable) local strawSwathCanBeEnabled = false local fruitType = g_fruitTypeManager:getFruitTypeIndexByFillTypeIndex(self.vehicle:getFillUnitFillType(self.combine.fillUnitIndex)) @@ -1683,6 +1682,10 @@ function CombineAIDriver:onDraw() DebugUtil.drawDebugNode(dischargeNode.node, 'discharge') end + if self.aiDriverData.backMarkerNode then + DebugUtil.drawDebugNode(self.aiDriverData.backMarkerNode, 'back marker') + end + UnloadableFieldworkAIDriver.onDraw(self) end diff --git a/DevHelper.lua b/DevHelper.lua index 19e0f1432..bf7b2b535 100644 --- a/DevHelper.lua +++ b/DevHelper.lua @@ -48,12 +48,18 @@ function DevHelper:update() end self.vehicle = g_currentMission.controlledVehicle - self.node = AIDriverUtil.getDirectionNode(g_currentMission.controlledVehicle) + self.node = g_currentMission.controlledVehicle.rootNode lx, _, lz = localDirectionToWorld(self.node, 0, 0, 1) else -- camera node looks backwards so need to flip everything by 180 degrees self.node = g_currentMission.player.cameraNode lx, _, lz = localDirectionToWorld(self.node, 0, 0, -1) + if not self.proxySensor then + self.proxySensor = ProximitySensor(self.node, 180, 10, 1, 0) + else + self.proxySensor:update() + self.proxySensor:showDebugInfo() + end end if self.vehicleData then @@ -243,7 +249,7 @@ end function DevHelper:showVehicleSize() local vehicle = g_currentMission.controlledVehicle if not vehicle then return end - local x, z, yRot = PathfinderUtil.getNodePositionAndDirection(AIDriverUtil.getDirectionNode(vehicle)) + local x, z, yRot = PathfinderUtil.getNodePositionAndDirection(vehicle.rootNode) local node = State3D(x, -z, courseGenerator.fromCpAngle(yRot)) if not g_devHelper.helperNode then g_devHelper.helperNode = courseplay.createNode('pathfinderHelper', node.x, -node.y, 0) @@ -251,7 +257,7 @@ function DevHelper:showVehicleSize() local y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, node.x, 0, -node.y); setTranslation(g_devHelper.helperNode, node.x, y, -node.y) setRotation(g_devHelper.helperNode, 0, courseGenerator.toCpAngle(node.t), 0) - + if self.vehicleData then for _, rectangle in ipairs(self.vehicleData.rectangles) do local x1,y1,z1 = localToWorld(g_devHelper.helperNode, rectangle.dRight, 2, rectangle.dFront); @@ -259,10 +265,10 @@ function DevHelper:showVehicleSize() local x3,y3,z3 = localToWorld(g_devHelper.helperNode, rectangle.dRight, 2, rectangle.dRear); local x4,y4,z4 = localToWorld(g_devHelper.helperNode, rectangle.dLeft, 2, rectangle.dRear); - drawDebugLine(x1,y1,z1,0,0,1,x2,y2,z2,0,0,1); - drawDebugLine(x1,y1,z1,0,0,1,x3,y3,z3,0,0,1); - drawDebugLine(x2,y2,z2,0,0,1,x4,y4,z4,0,0,1); - drawDebugLine(x3,y3,z3,0,0,1,x4,y4,z4,0,0,1); + drawDebugLine(x1,y1,z1,0.2, 0.2 ,1,x2,y2,z2,0.2, 0.2,1); + drawDebugLine(x1,y1,z1,0.2, 0.2 ,1,x3,y3,z3,0.2, 0.2,1); + drawDebugLine(x2,y2,z2,0.2, 0.2 ,1,x4,y4,z4,0.2, 0.2,1); + drawDebugLine(x3,y3,z3,0.2, 0.2 ,1,x4,y4,z4,0.2, 0.2,1); end if self.vehicleData.trailerRectangle then local x, y, z = localToWorld(g_devHelper.helperNode, 0, 0, self.vehicleData.trailerHitchOffset) @@ -283,9 +289,10 @@ function DevHelper:showVehicleSize() for i = 1, 4 do local cp = self.collisionData.corners[i] local pp = self.collisionData.corners[i > 1 and i - 1 or 4] - cpDebug:drawLine(cp.x, cp.y + 0.4, cp.z, 1, 1, 0, pp.x, pp.y + 0.4, pp.z) + cpDebug:drawLine(cp.x, cp.y + 0.4, cp.z, 1, 0, 0, pp.x, pp.y + 0.4, pp.z) end end + DebugUtil.drawDebugNode(g_devHelper.helperNode, 'devhelper') end function DevHelper.saveVehiclePosition(vehicle, vehiclePositionData) diff --git a/FieldworkAIDriver.lua b/FieldworkAIDriver.lua index 3d5ea839c..645eac632 100644 --- a/FieldworkAIDriver.lua +++ b/FieldworkAIDriver.lua @@ -1340,7 +1340,10 @@ function FieldworkAIDriver:onDraw() DebugUtil.drawDebugNode(aiSizeBackMarker, object:getName() .. ' AI Size Back') end end - DebugUtil.drawDebugNode(object.cp.directionNode or object.rootNode, object:getName() .. ' root') + DebugUtil.drawDebugNode(object.rootNode, object:getName() .. ' root') + if object.cp.directionNode then + DebugUtil.drawDebugNode(object.cp.directionNode, object:getName() .. ' dir') + end end showAIMarkersOfObject(self.vehicle) diff --git a/ProximitySensor.lua b/ProximitySensor.lua index bbf613ef8..20c4364c0 100644 --- a/ProximitySensor.lua +++ b/ProximitySensor.lua @@ -63,8 +63,10 @@ function ProximitySensor:update() self.distanceOfClosestObject = math.huge self.objectId = nil if self.enabled then - raycastClosest(x, y + self.height, z, nx, ny, nz, 'raycastCallback', self.range, self, bitOR(AIVehicleUtil.COLLISION_MASK, 2)) --- cpDebug:drawLine(x, y + self.height, z, 0, 1, 0, tx, y2 + self.height, tz) + raycastClosest(x, y + self.height, z, nx, ny, nz, 'raycastCallback', self.range, self, + 0x1000 + 0x100 + 0x80 + 0x40 + 0x20) + --bitOR(AIVehicleUtil.COLLISION_MASK, 0x1f0)) + --cpDebug:drawLine(x, y + self.height, z, 0, 1, 0, x + 5 * nx, y + self.height + 5 * ny, z + 5 * nz) end if courseplay.debugChannels[12] and self.distanceOfClosestObject <= self.range then local green = self.distanceOfClosestObject / self.range @@ -104,7 +106,7 @@ function ProximitySensor:showDebugInfo() local object = g_currentMission:getNodeObject(self.objectId) if object then if object.getRootVehicle then - text = text .. 'vehicle' .. object:getName() + text = text .. 'vehicle ' .. object:getName() else text = text .. object:getName() end diff --git a/PurePursuitController.lua b/PurePursuitController.lua index 2d971ab9e..147291e53 100644 --- a/PurePursuitController.lua +++ b/PurePursuitController.lua @@ -413,6 +413,7 @@ function PurePursuitController:findGoalPoint() -- If we weren't on track yet (after initialization, on our way to the first/initialized waypoint) -- set the goal to the relevant WP self.goalWpNode:setToWaypoint(self.course, self.relevantWpNode.ix) + self:setGoalTranslation() self:showGoalpointDiag(1, 'initializing, ix=%d, q1=%.1f, q2=%.1f, la=%.1f', ix, q1, q2, self.lookAheadDistance) -- and also the current waypoint is now at the relevant WP self:setCurrentWaypoint(self.relevantWpNode.ix) @@ -428,8 +429,8 @@ function PurePursuitController:findGoalPoint() end local cosGamma = ( q2 * q2 - q1 * q1 - l * l ) / (-2 * l * q1) local p = q1 * cosGamma + math.sqrt(q1 * q1 * (cosGamma * cosGamma - 1) + self.lookAheadDistance * self.lookAheadDistance) - local gx, gy, gz = localToWorld(node1.node, 0, 0, p) - setTranslation(self.goalWpNode.node, gx, gy + 1, gz) + local gx, _, gz = localToWorld(node1.node, 0, 0, p) + self:setGoalTranslation(gx, gz) self.wpBeforeGoalPointIx = ix self:showGoalpointDiag(2, 'common case, ix=%d, q1=%.1f, q2=%.1f la=%.1f', ix, q1, q2, self.lookAheadDistance) -- current waypoint is the waypoint at the end of the path segment @@ -445,8 +446,8 @@ function PurePursuitController:findGoalPoint() if math.abs(self.crossTrackError) <= self.lookAheadDistance then -- case iii (two intersection points) local p = math.sqrt(self.lookAheadDistance * self.lookAheadDistance - self.crossTrackError * self.crossTrackError) - local gx, gy, gz = localToWorld(self.projectedPosNode, 0, 0, p) - setTranslation(self.goalWpNode.node, gx, gy + 1, gz) + local gx, _, gz = localToWorld(self.projectedPosNode, 0, 0, p) + self:setGoalTranslation(gx, gz) self.wpBeforeGoalPointIx = ix self:showGoalpointDiag(3, 'two intersection points, ix=%d, q1=%.1f, q2=%.1f, la=%.1f, cte=%.1f', ix, q1, q2, self.lookAheadDistance, self.crossTrackError) @@ -456,8 +457,8 @@ function PurePursuitController:findGoalPoint() -- case iv (no intersection points) -- case v ( goal point dead zone) -- set the goal to the projected position - local gx, gy, gz = localToWorld(self.projectedPosNode, 0, 0, 0) - setTranslation(self.goalWpNode.node, gx, gy + 1, gz) + local gx, _, gz = localToWorld(self.projectedPosNode, 0, 0, 0) + self:setGoalTranslation(gx, gz) self.wpBeforeGoalPointIx = ix self:showGoalpointDiag(4, 'no intersection points, ix=%d, q1=%.1f, q2=%.1f, la=%.1f, cte=%.1f', ix, q1, q2, self.lookAheadDistance, self.crossTrackError) @@ -490,6 +491,18 @@ function PurePursuitController:findGoalPoint() end end +-- set the goal WP node's position. This will make sure the goal node is on the same height +-- as the controlled node, avoiding issues when driving on non-level bridges where the controlled node +-- is not vertical and since the goal is very far below it, it will be much closer/further in the controlled +-- node's reference frame. +-- If everyone is on the same height, this error is negligible +function PurePursuitController:setGoalTranslation(x, z) + local gx, _, gz = getWorldTranslation(self.goalWpNode.node) + local _, cy, _ = getWorldTranslation(self.controlledNode) + -- if there's an x, z passed in, use that, otherwise only adjust y to be the same as the controlled node + setTranslation(self.goalWpNode.node, x or gx, cy + 1, z or gz) +end + -- set the current waypoint for the rest of Courseplay and to notify listeners function PurePursuitController:setCurrentWaypoint(ix) -- this is the current waypoint for the rest of Courseplay code, the waypoint we are driving to @@ -514,6 +527,7 @@ function PurePursuitController:showGoalpointDiag(case, ...) local diagText = string.format(...) if courseplay.debugChannels[12] then DebugUtil.drawDebugNode(self.goalWpNode.node, diagText) + DebugUtil.drawDebugNode(self.controlledNode, 'controlled') end if case ~= self.case then self:debug(...) diff --git a/course-generator/PathfinderUtil.lua b/course-generator/PathfinderUtil.lua index 0b23a2813..352b0eb28 100644 --- a/course-generator/PathfinderUtil.lua +++ b/course-generator/PathfinderUtil.lua @@ -38,8 +38,8 @@ function PathfinderUtil.VehicleData:init(vehicle, withImplements, buffer) self.vehicle = vehicle self.rootVehicle = vehicle:getRootVehicle() self.name = vehicle.getName and vehicle:getName() or 'N/A' - -- distance of the sides of a rectangle from the direction node of the vehicle - -- in other words, the X and Z offsets of the corners from the direction node + -- distance of the sides of a rectangle from the root node of the vehicle + -- in other words, the X and Z offsets of the corners from the root node -- negative is to the rear and to the right -- this is the bounding box of the entire vehicle with all attached implements, -- except a towed trailer as that is calculated independently @@ -62,6 +62,7 @@ function PathfinderUtil.VehicleData:init(vehicle, withImplements, buffer) dLeft = self.trailer.sizeWidth / 2, dRight = -self.trailer.sizeWidth / 2 } + -- TODO: this should be the attacher joint's offset, not the back of the vehicle. self.trailerHitchOffset = self.dRear self.trailerHitchLength = AIDriverUtil.getTowBarLength(vehicle) courseplay.debugVehicle(7, vehicle, 'trailer for the pathfinding is %s, hitch offset is %.1f', @@ -74,22 +75,29 @@ end --- Calculate the relative coordinates of a rectangle's corners around a reference node, representing the implement function PathfinderUtil.VehicleData:getRectangleForImplement(implement, referenceNode, buffer) - local rootToReferenceNodeDistance = 0 + local rootToReferenceNodeOffset = 0 local attacherJoint = implement.object.getActiveInputAttacherJoint and implement.object:getActiveInputAttacherJoint() if attacherJoint and attacherJoint.node then -- the implement may not be aligned with the vehicle so we need to calculate this distance in two - -- steps, first the distance between the vehicle's direction node and the attacher joint and then + -- steps, first the distance between the vehicle's root node and the attacher joint and then -- from the attacher joint to the implement's root node + -- < 0 when the attacher joint is behind the reference node local _, _, referenceToAttacherJoint = localToLocal(attacherJoint.node, referenceNode, 0, 0, 0) + -- > 0 when the attacher node is in front of the implement's root node (we don't use the attacher joint node + -- as a reference as it may point to any direction, we know the implement's root node points forward local _, _, attacherJointToImplementRoot = localToLocal(attacherJoint.node, implement.object.rootNode, 0, 0, 0) - rootToReferenceNodeDistance = attacherJointToImplementRoot - referenceToAttacherJoint + -- we call this offset, and is negative when behind the reference node, positive when in front of it + -- (need to reverse attacherJointToImplementRoot) + rootToReferenceNodeOffset = - attacherJointToImplementRoot + referenceToAttacherJoint + courseplay.debugFormat(7, '%s: ref to attacher joint %.1f, att to implement root %.1f, impl root to ref %.1f', + nameNum(implement.object), referenceToAttacherJoint, attacherJointToImplementRoot, rootToReferenceNodeOffset) else - _, _, rootToReferenceNodeDistance = localToLocal(implement.object.rootNode, referenceNode, 0, 0, 0) + _, _, rootToReferenceNodeOffset = localToLocal(implement.object.rootNode, referenceNode, 0, 0, 0) end -- default size, used by Giants to determine the drop area when buying something local rectangle = { - dFront = rootToReferenceNodeDistance + implement.object.sizeLength / 2 + implement.object.lengthOffset + (buffer or 0), - dRear = rootToReferenceNodeDistance - implement.object.sizeLength / 2 - implement.object.lengthOffset - (buffer or 0), + dFront = rootToReferenceNodeOffset + implement.object.sizeLength / 2 + implement.object.lengthOffset + (buffer or 0), + dRear = rootToReferenceNodeOffset - implement.object.sizeLength / 2 + implement.object.lengthOffset - (buffer or 0), dLeft = implement.object.sizeWidth / 2, dRight = -implement.object.sizeWidth / 2 } @@ -121,7 +129,7 @@ end function PathfinderUtil.VehicleData:calculateSizeOfObjectList(vehicle, implements, buffer, rectangles) for _, implement in ipairs(implements) do --print(implement.object:getName()) - local referenceNode = AIDriverUtil.getDirectionNode(vehicle) + local referenceNode = vehicle.rootNode if implement.object ~= self.trailer then -- everything else is attached to the root vehicle and calculated as it was moving with it (having -- the same heading) @@ -306,7 +314,7 @@ end function PathfinderUtil.CollisionDetector:findCollidingShapes(node, vehicleData, vehiclesToIgnore, log) self.vehiclesToIgnore = vehiclesToIgnore or {} self.vehicleData = vehicleData - -- the box for overlapBox() is symmetric, so if our direction node is not in the middle of the vehicle rectangle, + -- the box for overlapBox() is symmetric, so if our root node is not in the middle of the vehicle rectangle, -- we have to translate it into the middle -- right/rear is negative local xOffset = vehicleData.dRight + vehicleData.dLeft diff --git a/modDesc.xml b/modDesc.xml index c8e3848c7..ced309ef5 100644 --- a/modDesc.xml +++ b/modDesc.xml @@ -1,6 +1,6 @@ - 6.03.00023 + 6.03.00024 <!-- en=English de=German fr=French es=Spanish ru=Russian pl=Polish it=Italian br=Brazilian-Portuguese cs=Chinese(Simplified) ct=Chinese(Traditional) cz=Czech nl=Netherlands hu=Hungary jp=Japanese kr=Korean pt=Portuguese ro=Romanian tr=Turkish --> <en>CoursePlay SIX</en>