Files
fs25-adsmartpickup/FS25_ADSmartPickup/adHitchCreep.lua
T

619 lines
34 KiB
Lua

-- AD Hitch Creep
-- Bak de sidste meter hen til vognen: når AD "Kør til" når en Connect-markør, og vognens trækøje står 3-8 m
-- lige bag traktorens krog, lægges en lille bakke-opgave i AD's kø før stop. Traktoren bakker i krybefart og
-- standser, så snart spillet ser vognen inden for koblingsrækkevidde; ADHitch kobler derefter på som før.
-- AD's egen bak-kollisionssensor bruges ikke: den ser vognen selv som forhindring.
-- Frontredskab ("Connect P Front Mower", in-game 8/10): samme, men med traktorens frontkrog, og den kører frem.
ADHitchCreep = {}
ADHitchCreep.LOG_PREFIX = "ADSmartPickup: kobling: "
ADHitchCreep.ROOMY_ATTACH_DISTANCE = 3 -- m: ADHitch's rummelige koblingsforsøg (samme som ADHitch.ROOMY_ATTACH_DISTANCE)
-- Kun så tæt kobles uden at bakke: 2,9 m var for langt for spillet (in-game 6/10: Tippevogn 3 og Læssevogn 5)
ADHitchCreep.DIRECT_ATTACH_DISTANCE = 1.0
-- m: længere væk end det bakkes ikke (in-game 6/10: AD holdt 9-10 m før River 1 / Plov 1; 8/10: såmaskinebåse med Connect
-- 13-14 m fra P gav trækøjet 13,7 m bag krogen, 0,3 m til siden -> "none")
ADHitchCreep.CREEP_MAX_DISTANCE = 18
ADHitchCreep.CREEP_MAX_LATERAL = 1.5 -- m til siden: mere end det rammer en lige bakning ikke trækøjet
-- frem til et frontredskab styres der mod koblingspunktet hele vejen (in-game 8/10: KDF 341 11,1 m foran, 1,8 m til siden)
ADHitchCreep.CREEP_MAX_LATERAL_FRONT = 3
ADHitchCreep.CREEP_TIMEOUT_MS = 35000
ADHitchCreep.CREEP_SPEED = 2 -- km/t
ADHitchCreep.AWAY_TOLERANCE = 1 -- m: længere væk end ved start = den bakker forkert
ADHitchCreep.RANGE_CHECK_MS = 250
-- ---- ren logik ----
-- offset = {x, z}: vognens trækøje i traktorens rum set fra krogen (z < 0 = bagved) -> "attach" | "creep" | "none"
-- front = frontkrog (in-game 8/10: frontslåmaskine): redskabet skal stå foran krogen, traktoren kører frem.
function ADHitchCreep.plan(offset, front)
if offset == nil then return "none" end
local distance = math.sqrt(offset.x ^ 2 + offset.z ^ 2)
if distance <= ADHitchCreep.DIRECT_ATTACH_DISTANCE then return "attach" end
local wrongSide = (front and offset.z <= 0) or (not front and offset.z >= 0)
local maxLateral = front and ADHitchCreep.CREEP_MAX_LATERAL_FRONT or ADHitchCreep.CREEP_MAX_LATERAL
if wrongSide or distance > ADHitchCreep.CREEP_MAX_DISTANCE or math.abs(offset.x) > maxLateral then
return "none"
end
return "creep"
end
-- Er kandidat a bedre end b? {plan, distance}: en der kan bakkes/kobles til slår altid en der ikke kan, ellers
-- nærmeste (in-game 6/10: såmaskinen i nabobåsen 6,2 m til siden var nærmere end K105 lige bagved -> "none")
function ADHitchCreep.isBetter(a, b)
if b == nil then return true end
local usableA, usableB = a.plan ~= "none", b.plan ~= "none"
if usableA ~= usableB then return usableA end
return a.distance < b.distance
end
-- Kørt forbi trækøjet uden kobling? Bag: trækøjet er kommet foran krogen; front: bag krogen.
function ADHitchCreep.passed(offset, front)
if front then return offset.z < -0.5 end
return offset.z > 0.5
end
-- Hente-markøren for et frontredskab ("Connect P Front Mower"): så bruges frontkrogen og der køres frem.
function ADHitchCreep.isFrontMarker(name)
return ADHitch ~= nil and ADHitch.isFrontName(name) or false
end
-- s = {inRange, distance, startDistance, elapsedMs, passed} -> "done" | "drive" | "giveUp"
function ADHitchCreep.step(s)
if s.inRange then return "done" end
if (s.elapsedMs or 0) >= ADHitchCreep.CREEP_TIMEOUT_MS then return "giveUp" end
if s.passed then return "giveUp" end
if s.distance > s.startDistance + ADHitchCreep.AWAY_TOLERANCE then return "giveUp" end
return "drive"
end
-- Bak fri af et frontredskab der lige er sat af (in-game 8/10: T7 stod fast ved "P Front Mower" — AD's sensor ser
-- kun fremad, også når ruten bakker, og så frontslåmaskinen lige foran som forhindring).
ADHitchCreep.BACKOFF_MAX_M = 12 -- m: højst så langt lige bagud; AD kører videre ad båsens egne vejpunkter
ADHitchCreep.BACKOFF_FRONT_M = 6 -- m foran traktorens front, hvor et løst redskab blokerer (AD-sensoren er ~2 m ved krybefart)
ADHitchCreep.BACKOFF_LATERAL = 3 -- m til siden
ADHitchCreep.BACKOFF_TIMEOUT_MS = 20000
-- Løst redskab i traktorens rum (lx, lz fra midten); halfLength = halv traktorlængde -> blokerer det fronten?
function ADHitchCreep.isBlockingFront(lx, lz, halfLength)
return lz > 0 and lz < (halfLength or 0) + ADHitchCreep.BACKOFF_FRONT_M and math.abs(lx) < ADHitchCreep.BACKOFF_LATERAL
end
-- s = {blocking, moved, elapsedMs} -> "done" (redskabet står ikke længere foran) | "drive" | "giveUp"
function ADHitchCreep.backOffStep(s)
if not s.blocking then return "done" end
if (s.moved or 0) >= ADHitchCreep.BACKOFF_MAX_M or (s.elapsedMs or 0) >= ADHitchCreep.BACKOFF_TIMEOUT_MS then return "giveUp" end
return "drive"
end
-- Bak ud af en blindgyde-bås (in-game 8/10: frontbåsen "P Front Mower" kører man forlæns ind i og bakker ud af ad
-- bakkekanter). AD starter aldrig en tur baglæns: findMatchingWayPointForVehicle vil kun have et vejpunkt FORAN
-- traktoren, ellers vender den med stifinderen. Modden følger derfor selv bakkekanterne ud, til vejen fortsætter
-- forlæns, og giver så AD turen.
ADHitchCreep.REVERSE_OUT_RADIUS = 8 -- m: blindgyde-vejpunkt så tæt på traktoren
ADHitchCreep.REVERSE_OUT_MAX_POINTS = 40
ADHitchCreep.REVERSE_OUT_REACHED_M = 1.5
ADHitchCreep.REVERSE_OUT_SPEED = 5 -- km/t
ADHitchCreep.REVERSE_OUT_TIMEOUT_MS = 90000
ADHitchCreep.REVERSE_OUT_STUCK_MS = 10000
ADHitchCreep.STILL_MOVE_M = 0.5 -- m: så langt skal den være kommet, før "står stille"-uret nulstilles
local function contains(list, value)
for _, item in ipairs(list or {}) do if item == value then return true end end
return false
end
-- Bakkekant: AD's konvention — fra står i fra.out, men ikke i til.incoming.
function ADHitchCreep.isReverseEdge(fromId, toWp)
return toWp ~= nil and not contains(toWp.incoming, fromId)
end
-- Blindgyde: vejpunktet har udkørsler, og de er alle bakkekanter.
function ADHitchCreep.isDeadEnd(getWp, wp)
if wp == nil or wp.out == nil or #wp.out == 0 then return false end
for _, id in ipairs(wp.out) do
if not ADHitchCreep.isReverseEdge(wp.id, getWp(id)) then return false end
end
return true
end
-- Bakkevejen ud fra en blindgyde: følg bakkekanterne, til et vejpunkt har en forlæns udkørsel. -> {id, ...} (tom = ingen)
function ADHitchCreep.reverseOutPath(getWp, startId)
local current = getWp(startId)
if not ADHitchCreep.isDeadEnd(getWp, current) then return {} end
local path, seen = {}, {[startId] = true}
while #path < ADHitchCreep.REVERSE_OUT_MAX_POINTS do
local nextWp = nil
for _, id in ipairs(current.out or {}) do
local candidate = getWp(id)
if not seen[id] and ADHitchCreep.isReverseEdge(current.id, candidate) then nextWp = candidate break end
end
if nextWp == nil then break end
seen[nextWp.id] = true
table.insert(path, nextWp.id)
local forward = false
for _, id in ipairs(nextWp.out or {}) do
if not ADHitchCreep.isReverseEdge(nextWp.id, getWp(id)) then forward = true end
end
if forward then break end
current = nextWp
end
return path
end
-- Bakkevejen IND i en bås fra dens Connect: følg bakkekanterne (også forbi punkter med forlæns udkørsel), til der ikke
-- er flere. Til krybning ind til et bagredskab, når traktoren står skævt (in-game 8/10: efter frontbåsens kurve stod
-- T7 3,2 m ved siden af bagklipperens trækøje; bagbåsen har bakkevejen 2104 -> 2105 -> 2106 -> 2107).
function ADHitchCreep.reverseChain(getWp, startId)
local current = getWp(startId)
local path, seen = {}, {[startId] = true}
while current ~= nil and #path < ADHitchCreep.REVERSE_OUT_MAX_POINTS do
local nextWp = nil
for _, id in ipairs(current.out or {}) do
local candidate = getWp(id)
if not seen[id] and ADHitchCreep.isReverseEdge(current.id, candidate) then nextWp = candidate break end
end
if nextWp == nil then break end
seen[nextWp.id] = true
table.insert(path, nextWp.id)
current = nextWp
end
return path
end
-- s = {reached, last, elapsedMs, stillMs} -> "next" | "done" | "drive" | "giveUp"
function ADHitchCreep.reverseOutStep(s)
if s.reached then return s.last and "done" or "next" end
if (s.elapsedMs or 0) >= ADHitchCreep.REVERSE_OUT_TIMEOUT_MS or (s.stillMs or 0) >= ADHitchCreep.REVERSE_OUT_STUCK_MS then return "giveUp" end
return "drive"
end
-- ---- spil ----
local function worldPos(node)
local x, y, z = getWorldTranslation(node)
return {x = x, y = y, z = z}
end
local function vehicleName(vehicle)
return vehicle ~= nil and vehicle.getName ~= nil and vehicle:getName() or "?"
end
-- Traktorens ledige krog-noder på den ene side: bag (lz < 0) eller front (lz > 0, frontslåmaskine 8/10).
local function freeJoints(vehicle, front)
local spec = vehicle.spec_attacherJoints
if spec == nil or spec.attacherJoints == nil or vehicle.rootNode == nil then return {} end
local used = {}
for _, implement in ipairs(spec.attachedImplements or {}) do
if implement.jointDescIndex ~= nil then used[implement.jointDescIndex] = true end
end
local joints = {}
for index, joint in ipairs(spec.attacherJoints) do
if not used[index] and joint.jointTransform ~= nil then
local p = worldPos(joint.jointTransform)
local _, _, lz = worldToLocal(vehicle.rootNode, p.x, p.y, p.z)
if (front and lz > 0) or (not front and lz < 0) then table.insert(joints, {node = joint.jointTransform, jointType = joint.jointType}) end
end
end
return joints
end
-- Vognens trækøje i traktorens rum set fra krogen.
local function offsetOf(vehicle, jointNode, targetNode)
local j, t = worldPos(jointNode), worldPos(targetNode)
local x, _, z = worldDirectionToLocal(vehicle.rootNode, t.x - j.x, 0, t.z - j.z)
return {x = x, z = z}
end
-- Nærmeste frakoblede redskab (samme gård) med et trækøje bag (front: foran) en ledig krog.
-- -> {wagon, joint, target, offset, plan, front} | nil
-- Anden returværdi: tekst om det nærmeste redskab der IKKE kunne bruges (for langt væk, påkoblet, forkert krogtype)
-- — til loggen når intet findes (in-game 9/10: VNX 300 fandt ikke sættevognen ved 'Connect Semi Fladbed Trailer').
function ADHitchCreep.findTarget(vehicle, front)
local joints = freeJoints(vehicle, front)
if #joints == 0 then return nil, "ingen ledig krog " .. (front and "foran" or "bag") .. " på køretøjet" end
if g_currentMission == nil or g_currentMission.vehicleSystem == nil then return nil end
local farmId = vehicle.getOwnerFarmId ~= nil and vehicle:getOwnerFarmId() or nil
local jointTypes = {}
for _, joint in ipairs(joints) do table.insert(jointTypes, tostring(joint.jointType)) end
local best, nearest = nil, nil
local function note(object, input, distance, why)
if nearest == nil or distance < nearest.distance then
nearest = {distance = distance, text = string.format("'%s' (%s) %.0f m væk: %s", vehicleName(object), tostring(input and input.jointType or "?"), distance, why)}
end
end
for _, object in pairs(g_currentMission.vehicleSystem.vehicles or {}) do
local attachable = object.spec_attachable
if object ~= vehicle and attachable ~= nil and object.spec_motorized == nil and attachable.inputAttacherJoints ~= nil
and (farmId == nil or object.getOwnerFarmId == nil or object:getOwnerFarmId() == farmId) then
local attached = object.getAttacherVehicle ~= nil and object:getAttacherVehicle() ~= nil
for _, input in ipairs(attachable.inputAttacherJoints) do
if input.node ~= nil then
local offsetAny = offsetOf(vehicle, joints[1].node, input.node)
local distanceAny = math.sqrt(offsetAny.x ^ 2 + offsetAny.z ^ 2)
if attached then
note(object, input, distanceAny, "er koblet på '" .. vehicleName(object:getAttacherVehicle()) .. "'")
else
local typeOk = false
for _, joint in ipairs(joints) do
if input.jointType == nil or joint.jointType == nil or input.jointType == joint.jointType then
typeOk = true
local offset = offsetOf(vehicle, joint.node, input.node)
local candidate = {wagon = object, joint = joint.node, target = input.node, offset = offset, front = front,
distance = math.sqrt(offset.x ^ 2 + offset.z ^ 2), plan = ADHitchCreep.plan(offset, front)}
if candidate.distance <= ADHitchCreep.CREEP_MAX_DISTANCE + 2 and ADHitchCreep.isBetter(candidate, best) then
best = candidate
elseif candidate.distance > ADHitchCreep.CREEP_MAX_DISTANCE + 2 then
note(object, input, candidate.distance, string.format("for langt (%.0f m %s krogen, %.0f m til siden)",
math.abs(offset.z), offset.z < 0 and "bag" or "foran", offset.x))
end
end
end
if not typeOk then
note(object, input, distanceAny, "krogtype passer ikke (køretøjet har " .. table.concat(jointTypes, "/") .. ")")
end
end
end
end
end
end
return best, nearest ~= nil and nearest.text or "ingen frakoblede redskaber på gården"
end
local function isInRange(vehicle, wagon)
local spec = vehicle.spec_attacherJoints
if spec == nil or AttacherJoints == nil or AttacherJoints.updateVehiclesInAttachRange == nil then return false end
AttacherJoints.updateVehiclesInAttachRange(vehicle, AttacherJoints.MAX_ATTACH_DISTANCE_SQ, AttacherJoints.MAX_ATTACH_ANGLE, true)
local info = spec.attachableInfo
return info ~= nil and info.attachable == wagon
end
-- AD-opgave (samme metoder som AD's AbstractTask).
local CreepTask = {}
CreepTask.__index = CreepTask
function CreepTask.new(adEnv, vehicle, target)
return setmetatable({adEnv = adEnv, vehicle = vehicle, target = target, elapsedMs = 0, checkMs = 0, inRange = false}, CreepTask)
end
function CreepTask:setUp()
local offset = offsetOf(self.vehicle, self.target.joint, self.target.target)
self.startDistance = math.sqrt(offset.x ^ 2 + offset.z ^ 2)
Logging.info(ADHitchCreep.LOG_PREFIX .. "'%s' %s %.1f m hen til '%s' (%.1f m til siden)", vehicleName(self.vehicle),
self.target.front and "kører frem" or "bakker", self.startDistance, vehicleName(self.target.wagon), offset.x)
end
function CreepTask:finish(action, distance)
local sdm = self.vehicle.ad.specialDrivingModule
if sdm ~= nil and sdm.stopVehicle ~= nil then sdm:stopVehicle() end
if action == "done" then
Logging.info(ADHitchCreep.LOG_PREFIX .. "'%s' står ved '%s' — kobler på", vehicleName(self.vehicle), vehicleName(self.target.wagon))
else
Logging.warning(ADHitchCreep.LOG_PREFIX .. "'%s' opgav at bakke hen til '%s' (%.1f m tilbage efter %d s)", vehicleName(self.vehicle),
vehicleName(self.target.wagon), distance, math.floor(self.elapsedMs / 1000))
end
local taskModule = self.adEnv.ADTaskModule
self.vehicle.ad.taskModule:setCurrentTaskFinished(taskModule ~= nil and taskModule.DONT_PROPAGATE or 1)
end
function CreepTask:update(dt)
self.elapsedMs = self.elapsedMs + dt
self.checkMs = self.checkMs + dt
if self.target.wagon.getAttacherVehicle ~= nil and self.target.wagon:getAttacherVehicle() ~= nil then
return self:finish("giveUp", 0) -- en anden har taget vognen
end
local offset = offsetOf(self.vehicle, self.target.joint, self.target.target)
local distance = math.sqrt(offset.x ^ 2 + offset.z ^ 2)
if self.checkMs >= ADHitchCreep.RANGE_CHECK_MS then
self.checkMs = 0
self.inRange = isInRange(self.vehicle, self.target.wagon)
end
local action = ADHitchCreep.step({inRange = self.inRange or distance <= ADHitchCreep.DIRECT_ATTACH_DISTANCE, distance = distance, startDistance = self.startDistance,
elapsedMs = self.elapsedMs, passed = ADHitchCreep.passed(offset, self.target.front)})
if action ~= "drive" then return self:finish(action, distance) end
local autoDrive, vehicle = self.adEnv.AutoDrive, self.vehicle
local p = worldPos(self.target.target)
local lx, lz = autoDrive.getDriveDirection(vehicle, p.x, p.y, p.z)
local sdm = vehicle.ad.specialDrivingModule
if sdm ~= nil and sdm.releaseVehicle ~= nil then sdm:releaseVehicle() end
if vehicle.startMotor ~= nil and not vehicle:getIsMotorStarted() and vehicle:getCanMotorRun() then vehicle:startMotor() end
if self.target.front then
-- frontkrog: kør frem mod frontredskabet (in-game 8/10)
autoDrive.driveInDirection(vehicle, dt, 30, 0.3, 0.2, 20, true, true, lx, lz, ADHitchCreep.CREEP_SPEED, 1)
else
-- som AD's ADSpecialDrivingModule:driveReverse (ikke-guidet): retning spejles ved bakning
autoDrive.driveInDirection(vehicle, dt, 30, 0.3, 0.2, 20, true, false, -lx, -lz, ADHitchCreep.CREEP_SPEED, 1)
end
end
function CreepTask:abort() end
function CreepTask:finished() end
function CreepTask:getInfoText() return self.target.front and "Kører frem til frontredskabet" or "Bakker hen til vognen" end
function CreepTask:getI18nInfo() return "" end
function CreepTask:getExcludedVehiclesForCollisionCheck() return {self.target.wagon} end
local function isBlocking(vehicle, object)
if object.rootNode == nil then return false end
local p = worldPos(object.rootNode)
local lx, _, lz = worldToLocal(vehicle.rootNode, p.x, p.y, p.z)
return ADHitchCreep.isBlockingFront(lx, lz, (vehicle.size ~= nil and vehicle.size.length or 5) / 2)
end
-- Bak lige ud fra et løst frontredskab, før AD's tur starter (AD-opgave, lægges forrest i køen).
local BackOffTask = {}
BackOffTask.__index = BackOffTask
function BackOffTask.new(adEnv, vehicle, implement)
return setmetatable({adEnv = adEnv, vehicle = vehicle, implement = implement, elapsedMs = 0}, BackOffTask)
end
function BackOffTask:setUp()
self.start = worldPos(self.vehicle.rootNode)
Logging.info(ADHitchCreep.LOG_PREFIX .. "'%s' bakker fri af '%s'", vehicleName(self.vehicle), vehicleName(self.implement))
end
function BackOffTask:update(dt)
self.elapsedMs = self.elapsedMs + dt
local vehicle, autoDrive = self.vehicle, self.adEnv.AutoDrive
local p = worldPos(vehicle.rootNode)
local moved = math.sqrt((p.x - self.start.x) ^ 2 + (p.z - self.start.z) ^ 2)
local action = ADHitchCreep.backOffStep({blocking = isBlocking(vehicle, self.implement), moved = moved, elapsedMs = self.elapsedMs})
local sdm = vehicle.ad.specialDrivingModule
if action ~= "drive" then
if sdm ~= nil and sdm.stopVehicle ~= nil then sdm:stopVehicle() end
if action == "done" then
Logging.info(ADHitchCreep.LOG_PREFIX .. "'%s' er fri af '%s' efter %.1f m", vehicleName(vehicle), vehicleName(self.implement), moved)
else
Logging.warning(ADHitchCreep.LOG_PREFIX .. "'%s' kom kun %.1f m fri af '%s' — AD overtager", vehicleName(vehicle), moved,
vehicleName(self.implement))
end
local taskModule = self.adEnv.ADTaskModule
vehicle.ad.taskModule:setCurrentTaskFinished(taskModule ~= nil and taskModule.DONT_PROPAGATE or 1)
return
end
if sdm ~= nil and sdm.releaseVehicle ~= nil then sdm:releaseVehicle() end
if vehicle.startMotor ~= nil and not vehicle:getIsMotorStarted() and vehicle:getCanMotorRun() then vehicle:startMotor() end
local bx, by, bz = localToWorld(vehicle.rootNode, 0, 0, -20)
local lx, lz = autoDrive.getDriveDirection(vehicle, bx, by, bz)
autoDrive.driveInDirection(vehicle, dt, 30, 0.3, 0.2, 20, true, false, -lx, -lz, ADHitchCreep.CREEP_SPEED, 1)
end
function BackOffTask:abort() end
function BackOffTask:finished() end
function BackOffTask:getInfoText() return "Bakker fri af frontredskabet" end
function BackOffTask:getI18nInfo() return "" end
function BackOffTask:getExcludedVehiclesForCollisionCheck() return {self.implement} end
-- Løst redskab (samme gård) lige foran traktoren, ellers nil.
local function blockingFrontImplement(vehicle)
if vehicle.rootNode == nil or g_currentMission == nil or g_currentMission.vehicleSystem == nil then return nil end
local farmId = vehicle.getOwnerFarmId ~= nil and vehicle:getOwnerFarmId() or nil
for _, object in pairs(g_currentMission.vehicleSystem.vehicles or {}) do
if object ~= vehicle and object.spec_attachable ~= nil and object.spec_motorized == nil and object.rootNode ~= nil
and (object.getAttacherVehicle == nil or object:getAttacherVehicle() == nil)
and (farmId == nil or object.getOwnerFarmId == nil or object:getOwnerFarmId() == farmId) then
if isBlocking(vehicle, object) then return object end
end
end
return nil
end
-- Følg bakkevejen (vejpunkt for vejpunkt) baglæns ud af blindgyden.
local ReverseOutTask = {}
ReverseOutTask.__index = ReverseOutTask
-- wagon (valgfri): bakkevej ind til et bagredskab — stopper, så snart spillet ser det inden for koblingsrækkevidde.
function ReverseOutTask.new(adEnv, vehicle, path, wagon)
return setmetatable({adEnv = adEnv, vehicle = vehicle, path = path, wagon = wagon, index = 1, elapsedMs = 0, stillMs = 0,
checkMs = 0}, ReverseOutTask)
end
function ReverseOutTask:setUp()
self.last = worldPos(self.vehicle.rootNode)
Logging.info(ADHitchCreep.LOG_PREFIX .. "'%s' bakker %s ad %d vejpunkter (%d -> %d)", vehicleName(self.vehicle),
self.wagon ~= nil and ("ind til '" .. vehicleName(self.wagon) .. "'") or "ud af båsen", #self.path, self.path[1], self.path[#self.path])
end
function ReverseOutTask:finish(action)
local vehicle = self.vehicle
local sdm = vehicle.ad.specialDrivingModule
if sdm ~= nil and sdm.stopVehicle ~= nil then sdm:stopVehicle() end
if self.wagon ~= nil then
if action == "inRange" or action == "done" then
Logging.info(ADHitchCreep.LOG_PREFIX .. "'%s' står ved '%s' (bakkevej, vejpunkt %d af %d) — kobler på", vehicleName(vehicle),
vehicleName(self.wagon), self.index, #self.path)
else
Logging.warning(ADHitchCreep.LOG_PREFIX .. "'%s' opgav at bakke ind til '%s' ved vejpunkt %d af %d", vehicleName(vehicle),
vehicleName(self.wagon), self.index, #self.path)
end
local taskModule = self.adEnv.ADTaskModule
vehicle.ad.taskModule:setCurrentTaskFinished(taskModule ~= nil and taskModule.DONT_PROPAGATE or 1)
return
end
if action == "done" and self:endsAtDestination() then
-- bakkevejen ender ved turens mål (udturen: Connect P Rear Mower = 2104): fremme — AD må ikke køre en omvej
-- tilbage til et mål, den nu har bag sig. Køreopgaven fjernes, og AD's "fremme" kører (krybning + stop).
Logging.info(ADHitchCreep.LOG_PREFIX .. "'%s' er ude af båsen ved vejpunkt %d = turens mål — fremme", vehicleName(vehicle), self.path[#self.path])
local mode = vehicle.ad.stateModule:getCurrentMode()
local queued = {}
local taskModule = vehicle.ad.taskModule
while taskModule.tasks:Count() > 0 do
local task = taskModule.tasks:Dequeue()
if mode == nil or task ~= mode.driveToDestinationTask then table.insert(queued, task) end
end
for _, task in ipairs(queued) do taskModule.tasks:Enqueue(task) end
taskModule:setCurrentTaskFinished()
return
elseif action == "done" then
Logging.info(ADHitchCreep.LOG_PREFIX .. "'%s' er ude af båsen ved vejpunkt %d — AD overtager", vehicleName(vehicle), self.path[#self.path])
else
Logging.warning(ADHitchCreep.LOG_PREFIX .. "'%s' opgav at bakke ud af båsen ved vejpunkt %d af %d (%d s) — AD overtager",
vehicleName(vehicle), self.index, #self.path, math.floor(self.elapsedMs / 1000))
end
local taskModule = self.adEnv.ADTaskModule
vehicle.ad.taskModule:setCurrentTaskFinished(taskModule ~= nil and taskModule.DONT_PROPAGATE or 1)
end
-- Ender bakkevejen ved "Kør til"-turens mål (vejpunkt-id eller inden for 3 m)?
function ReverseOutTask:endsAtDestination()
local mode = self.vehicle.ad.stateModule ~= nil and self.vehicle.ad.stateModule:getCurrentMode() or nil
local destination = mode ~= nil and mode.destinationID or nil
if destination == nil or mode.driveToDestinationTask == nil then return false end
local lastId = self.path[#self.path]
if destination == lastId then return true end
local graph = self.adEnv.ADGraphManager
local a, b = graph:getWayPointById(destination), graph:getWayPointById(lastId)
return a ~= nil and b ~= nil and (a.x - b.x) ^ 2 + (a.z - b.z) ^ 2 <= 9
end
function ReverseOutTask:update(dt)
self.elapsedMs = self.elapsedMs + dt
local vehicle, autoDrive = self.vehicle, self.adEnv.AutoDrive
local wp = self.adEnv.ADGraphManager:getWayPointById(self.path[self.index])
if wp == nil then return self:finish("giveUp") end
local p = worldPos(vehicle.rootNode)
-- står fast: ikke kommet STILL_MOVE_M fra sidste referencepunkt (pr. billede er krybefart kun ~2 cm — in-game
-- 8/10 v1.62.83 opgav den ved vejpunkt 5 af 9, mens den kørte)
if (p.x - self.last.x) ^ 2 + (p.z - self.last.z) ^ 2 >= ADHitchCreep.STILL_MOVE_M ^ 2 then
self.last, self.stillMs = p, 0
else
self.stillMs = self.stillMs + dt
end
if self.wagon ~= nil then
self.checkMs = self.checkMs + dt
if self.checkMs >= ADHitchCreep.RANGE_CHECK_MS then
self.checkMs = 0
if isInRange(vehicle, self.wagon) then return self:finish("inRange") end
end
end
local _, _, lz = worldToLocal(vehicle.rootNode, wp.x, p.y, wp.z)
local distance = math.sqrt((p.x - wp.x) ^ 2 + (p.z - wp.z) ^ 2)
-- nået: tæt nok på, eller kørt forbi (vejpunktet ligger nu foran traktoren, når den bakker)
-- kørt forbi: vejpunktet ligger foran OG afstanden vokser igen (i en skarp kurve kan et punkt ligge foran/ved
-- siden, før det er nået — kodegennemgang 8/10)
local passed = lz > 0.5 and self.lastDistance ~= nil and distance > self.lastDistance + 0.05
self.lastDistance = distance
local action = ADHitchCreep.reverseOutStep({reached = distance <= ADHitchCreep.REVERSE_OUT_REACHED_M or passed,
last = self.index >= #self.path, elapsedMs = self.elapsedMs, stillMs = self.stillMs})
if action == "next" then
self.index, self.lastDistance = self.index + 1, nil
return
elseif action ~= "drive" then
return self:finish(action)
end
local sdm = vehicle.ad.specialDrivingModule
if sdm ~= nil and sdm.releaseVehicle ~= nil then sdm:releaseVehicle() end
if vehicle.startMotor ~= nil and not vehicle:getIsMotorStarted() and vehicle:getCanMotorRun() then vehicle:startMotor() end
local lx, lzDir = autoDrive.getDriveDirection(vehicle, wp.x, wp.y or p.y, wp.z)
autoDrive.driveInDirection(vehicle, dt, 30, 0.3, 0.2, 20, true, false, -lx, -lzDir, ADHitchCreep.REVERSE_OUT_SPEED, 1)
end
function ReverseOutTask:abort() end
function ReverseOutTask:finished() end
function ReverseOutTask:getInfoText() return "Bakker ud af båsen" end
function ReverseOutTask:getI18nInfo() return "" end
function ReverseOutTask:getExcludedVehiclesForCollisionCheck() return {} end
-- Står traktoren i en blindgyde-bås? -> bakkevej (tom = nej). Nærmeste blindgyde-vejpunkt inden for radius.
local function reverseOutPathFor(adEnv, vehicle)
local graph = adEnv.ADGraphManager
if graph == nil or graph.getWayPointsInRange == nil or vehicle.rootNode == nil then return {} end
local p = worldPos(vehicle.rootNode)
local getWp = function(id) return graph:getWayPointById(id) end
-- kun når det NÆRMESTE vejpunkt er en blindgyde (en traktor i en almindelig bås står ved et punkt med forlæns
-- udkørsel og må aldrig sendes baglæns længere ind ad båsens indkørsel)
local best, bestDistance = nil, nil
for _, id in ipairs(graph:getWayPointsInRange({x = p.x, z = p.z}, 0, ADHitchCreep.REVERSE_OUT_RADIUS) or {}) do
local wp = getWp(id)
if wp ~= nil then
local distance = (wp.x - p.x) ^ 2 + (wp.z - p.z) ^ 2
if bestDistance == nil or distance < bestDistance then best, bestDistance = wp, distance end
end
end
if best == nil or not ADHitchCreep.isDeadEnd(getWp, best) then return {} end
local path = ADHitchCreep.reverseOutPath(getWp, best.id)
-- bakkevejen skal begynde bag traktoren
local first = path[1] ~= nil and getWp(path[1]) or nil
if first == nil then return {} end
local _, _, lz = worldToLocal(vehicle.rootNode, first.x, p.y, first.z)
if lz > 0 then return {} end
return path
end
local function enqueueFirst(taskModule, task)
local queued = {}
while taskModule.tasks:Count() > 0 do table.insert(queued, taskModule.tasks:Dequeue()) end
taskModule.tasks:Enqueue(task)
for _, item in ipairs(queued) do taskModule.tasks:Enqueue(item) end
end
-- Kaldes lige efter AD er startet (før vækningen): blindgyde-bås -> bak ud ad bakkekanterne; ellers løst redskab lige
-- foran -> bak lige fri. Opgaven lægges forrest i AD's kø.
function ADHitchCreep.backOffIfBlocked(adEnv, vehicle)
local taskModule = vehicle ~= nil and vehicle.ad ~= nil and vehicle.ad.taskModule or nil
if taskModule == nil or taskModule.tasks == nil then return false end
-- blindgyde-bås: bak ud ad dens bakkekanter (det klarer også et frontredskab lige foran)
local path = reverseOutPathFor(adEnv, vehicle)
-- på vej for at koble på: redskabet foran er netop målet — ikke bak fra det (kodegennemgang 8/10)
local fetching = ADHitch ~= nil and ADHitch.pendingAttach ~= nil and ADHitch.pendingAttach(vehicle) ~= nil
local implement = #path == 0 and not fetching and blockingFrontImplement(vehicle) or nil
if #path == 0 and implement == nil then return false end
if taskModule:getActiveTask() ~= nil then
Logging.warning(ADHitchCreep.LOG_PREFIX .. "'%s' skal bakke ud først, men AD kører allerede", vehicleName(vehicle))
return false
end
if #path > 0 then
enqueueFirst(taskModule, ReverseOutTask.new(adEnv, vehicle, path))
else
enqueueFirst(taskModule, BackOffTask.new(adEnv, vehicle, implement))
end
return true
end
local isInstalled = false
-- DriveToMode:handleFinishedTask lægger StopAndDisableADTask i køen; bakningen lægges ind lige før.
function ADHitchCreep.install(adEnv)
local mode = adEnv ~= nil and adEnv.DriveToMode or nil
if isInstalled or mode == nil or mode.handleFinishedTask == nil or ADHitch == nil or ADHitch.pendingAttach == nil then return end
isInstalled = true
local original = mode.handleFinishedTask
mode.handleFinishedTask = function(self)
local ok, err = pcall(function()
local job = self.driveToDestinationTask ~= nil and ADHitch.pendingAttach(self.vehicle) or nil
if job == nil or job.markerId ~= self.destinationID then return end
-- "Connect P Front Mower": frontkrogen og frem; ellers bagkrogen og bak (uændret)
local front = ADHitchCreep.isFrontMarker(job.markerName)
local target, why = ADHitchCreep.findTarget(self.vehicle, front)
-- skævt i forhold til båsen: følg båsens bakkevej fra Connect ind til redskabet
if target ~= nil and not front and target.plan == "none" and target.offset.z < 0 then
local graph = adEnv.ADGraphManager
local chain = ADHitchCreep.reverseChain(function(id) return graph:getWayPointById(id) end, self.destinationID)
local last = chain[#chain] ~= nil and graph:getWayPointById(chain[#chain]) or nil
local t = worldPos(target.target)
if last ~= nil and (last.x - t.x) ^ 2 + (last.z - t.z) ^ 2 <= ADHitchCreep.CREEP_MAX_DISTANCE ^ 2 then
job.crept = true
self.vehicle.ad.taskModule:addTask(ReverseOutTask.new(adEnv, self.vehicle, chain, target.wagon))
return
end
end
if target == nil or target.plan ~= "creep" then
-- hvorfor der ikke bakkes (in-game 6/10: Læssevogn 2 'noAttachable' uden bakning)
if target == nil then
Logging.info(ADHitchCreep.LOG_PREFIX .. "'%s' ved '%s': intet frakoblet redskab med trækøje inden for %d m %s krogen — nærmeste: %s",
vehicleName(self.vehicle), tostring(job.markerName), ADHitchCreep.CREEP_MAX_DISTANCE + 2, front and "foran" or "bag", tostring(why))
else
Logging.info(ADHitchCreep.LOG_PREFIX .. "'%s' ved '%s': bakker ikke (%s) — '%s' trækøje %.1f m %s, %.1f m til siden",
vehicleName(self.vehicle), tostring(job.markerName), tostring(target.plan), vehicleName(target.wagon),
math.abs(target.offset.z), target.offset.z < 0 and "bag krogen" or "FORAN krogen", target.offset.x)
end
return
end
job.crept = true
self.vehicle.ad.taskModule:addTask(CreepTask.new(adEnv, self.vehicle, target))
end)
if not ok then Logging.warning(ADHitchCreep.LOG_PREFIX .. "bakning til vognen: %s", tostring(err)) end
return original(self)
end
end