345 lines
18 KiB
Lua
345 lines
18 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
|
|
ADHitchCreep.CREEP_MAX_DISTANCE = 12 -- m: længere væk end det bakkes ikke (in-game 6/10: AD holdt 9-10 m før River 1 / Plov 1)
|
|
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
|
|
|
|
-- ---- 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
|
|
function ADHitchCreep.findTarget(vehicle, front)
|
|
local joints = freeJoints(vehicle, front)
|
|
if #joints == 0 or g_currentMission == nil or g_currentMission.vehicleSystem == nil then return nil end
|
|
local farmId = vehicle.getOwnerFarmId ~= nil and vehicle:getOwnerFarmId() or nil
|
|
local best = nil
|
|
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 (object.getAttacherVehicle == nil or object:getAttacherVehicle() == nil)
|
|
and (farmId == nil or object.getOwnerFarmId == nil or object:getOwnerFarmId() == farmId) then
|
|
for _, input in ipairs(attachable.inputAttacherJoints) do
|
|
if input.node ~= nil then
|
|
for _, joint in ipairs(joints) do
|
|
if input.jointType == nil or joint.jointType == nil or input.jointType == joint.jointType then
|
|
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
|
|
end
|
|
end
|
|
end
|
|
end
|
|
end
|
|
end
|
|
end
|
|
return best
|
|
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
|
|
|
|
-- Kaldes lige efter AD er startet: står et løst redskab foran, bakkes der fri først (opgaven lægges forrest).
|
|
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
|
|
local implement = blockingFrontImplement(vehicle)
|
|
if implement == nil then return false end
|
|
if taskModule:getActiveTask() ~= nil then
|
|
Logging.warning(ADHitchCreep.LOG_PREFIX .. "'%s' har '%s' lige foran, men AD kører allerede — bakker ikke fri", vehicleName(vehicle),
|
|
vehicleName(implement))
|
|
return false
|
|
end
|
|
local queued = {}
|
|
while taskModule.tasks:Count() > 0 do table.insert(queued, taskModule.tasks:Dequeue()) end
|
|
taskModule.tasks:Enqueue(BackOffTask.new(adEnv, vehicle, implement))
|
|
for _, task in ipairs(queued) do taskModule.tasks:Enqueue(task) 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 = ADHitchCreep.findTarget(self.vehicle, front)
|
|
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",
|
|
vehicleName(self.vehicle), tostring(job.markerName), ADHitchCreep.CREEP_MAX_DISTANCE + 2, front and "foran" or "bag")
|
|
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
|