Files
fs25-adsmartpickup/FS25_ADSmartPickup/adHeadOn.lua
T

388 lines
19 KiB
Lua

-- AD Head On
-- Frontal-blokering: to AutoDrive-maskiner står næse mod næse på et enkeltspor og ser hinanden som forhindring — så
-- venter begge for evigt, og alt bag dem står i kø (in-game 18/10 21:30: sprøjte-8280 TTV mod mælkebil ved TMR 1 Load).
-- Efter HEADON_MS bakker den ene ad sin egen kørte vej, til hele sættet er fri af den andens rute, venter til den
-- anden er forbi, og AutoDrive beregner så ruten forfra. Bakningen bruger AD's egen vognstyring (reverseToTargetLocation,
-- med bak-kollisionstjek). Kan ingen af dem komme fri, får spilleren besked.
ADHeadOn = {}
ADHeadOn.LOG_PREFIX = "ADSmartPickup: modkørsel: "
ADHeadOn.CHECK_MS = 2000
ADHeadOn.HEADON_MS = 60000 -- begge har holdt stille med forhindring så længe
ADHeadOn.MAX_GAP_M = 45 -- m mellem de to køretøjer
ADHeadOn.FACING_COS = 0.6 -- den anden ligger inden for ±53° af fronten
ADHeadOn.CLEAR_M = 8 -- m fra den andens rute, før sættet er fri
ADHeadOn.MAX_REVERSE_M = 80
ADHeadOn.OTHER_PATH_POINTS = 80 -- så mange af den andens kommende vejpunkter tæller som "dens rute"
ADHeadOn.BEHIND_BLOCK_M = 6 -- et andet køretøj så tæt på bakkevejen spærrer den
ADHeadOn.REVERSE_SPEED = 6 -- km/t
ADHeadOn.REVERSE_TIMEOUT_MS = 90000
ADHeadOn.REVERSE_STUCK_MS = 15000
ADHeadOn.STILL_MOVE_M = 0.5
ADHeadOn.WAIT_MAX_MS = 180000
ADHeadOn.PASSED_M = 60 -- den anden er forbi: så langt væk, eller bag os …
ADHeadOn.OTHER_MOVED_M = 12 -- … og den har selv kørt mindst så langt (vores bakning alene gør afstanden større)
ADHeadOn.ATTEMPTS_EXPIRE_MS = 1200000 -- forsøgstælleren for et par glemmes efter 20 min
ADHeadOn.COOLDOWN_MS = 300000 -- samme køretøj prøves højst så ofte
-- AD's reverseToTargetLocation regner et punkt nået 3 m før (enkeltpunkt i ADSpecialDrivingModule:checkWayPointReached)
ADHeadOn.REACHED_SLACK_M = 3
ADHeadOn.MAX_ATTEMPTS = 2 -- samme par: så mange bakninger, derefter besked til spilleren
ADHeadOn.ON_ROUTE_M = 6 -- den anden står på ens kommende rute (samme spor, ikke en parallel vej)
-- kun køreopgaver, mens de kører en rute (detectedObstable opdateres kun dér — ved et læssested står flaget gammelt)
ADHeadOn.DRIVE_TASKS = {"DriveToDestinationTask", "LoadAtDestinationTask", "UnloadAtDestinationTask"}
-- ---- ren logik ----
local function dist(a, b) return math.sqrt((a.x - b.x) ^ 2 + (a.z - b.z) ^ 2) end
-- a, b = {x, z, dx, dz} (dx/dz = fronten, normaliseret). Står de næse mod næse?
function ADHeadOn.isFacing(a, b)
local d = dist(a, b)
if d <= 0.01 or d > ADHeadOn.MAX_GAP_M then return false end
local ux, uz = (b.x - a.x) / d, (b.z - a.z) / d
return ux * a.dx + uz * a.dz >= ADHeadOn.FACING_COS and -ux * b.dx - uz * b.dz >= ADHeadOn.FACING_COS
end
function ADHeadOn.minDistance(point, path)
local best = math.huge
for _, p in ipairs(path) do
if p.x ~= nil and p.z ~= nil then
local d = dist(point, p)
if d < best then best = d end
end
end
return best
end
local minDistance = ADHeadOn.minDistance
-- Står de på samme spor: hver af dem ligger på den andens kommende rute (ikke to maskiner på parallelle veje).
-- aPos/bPos = {x, z}; aAhead/bAhead = kommende vejpunkter.
function ADHeadOn.onEachOthersRoute(aPos, aAhead, bPos, bAhead)
return ADHeadOn.minDistance(bPos, aAhead or {}) <= ADHeadOn.ON_ROUTE_M and ADHeadOn.minDistance(aPos, bAhead or {}) <= ADHeadOn.ON_ROUTE_M
end
-- trail = de vejpunkter køretøjet har kørt, nærmeste først ({x, z, id}); otherPath = den andens kommende vejpunkter;
-- length = sættets længde. Bak til det første punkt fri af den andens rute, og derefter length mere (fri hele vejen),
-- så hele sættet er fri. -> {points = {...}, meters} eller nil (ingen fri plads inden MAX_REVERSE_M / sporet slutter)
function ADHeadOn.escapePath(start, trail, otherPath, length)
local points, meters, prev, clearFor = {}, 0, start, nil
if trail == nil or trail[1] == nil then return nil, "intet kørt spor bag den" end
for _, p in ipairs(trail or {}) do
if p.x == nil or p.z == nil then return nil, "ugyldigt vejpunkt" end
meters = meters + dist(prev, p)
if meters > ADHeadOn.MAX_REVERSE_M then return nil, string.format("ikke fri af den andens rute inden %d m", ADHeadOn.MAX_REVERSE_M) end
table.insert(points, p)
prev = p
if minDistance(p, otherPath or {}) >= ADHeadOn.CLEAR_M then
clearFor = clearFor or meters
if meters - clearFor >= (length or 0) then return {points = points, meters = meters} end
else
clearFor = nil
end
end
return nil, string.format("sporet bag den slutter efter %.0f m (før den er fri)", meters)
end
-- Spærrer et andet køretøj (positioner {x, z, name}) bakkevejen? -> true, navn
function ADHeadOn.isBlockedBehind(points, others)
for _, other in ipairs(others or {}) do
if minDistance(other, points) <= ADHeadOn.BEHIND_BLOCK_M then return true, other.name end
end
return false
end
-- a, b = {escape = {meters} | nil, unloading = bool}. -> "a" | "b" | nil (ingen kan)
-- Kun én kan: den. Begge: den der ikke er på vej til aflæsning (læsset), ellers den korteste bakkevej.
function ADHeadOn.chooseBacker(a, b)
if a.escape == nil and b.escape == nil then return nil end
if b.escape == nil then return "a" end
if a.escape == nil then return "b" end
if a.unloading ~= b.unloading then return a.unloading and "b" or "a" end
return a.escape.meters <= b.escape.meters and "a" or "b"
end
-- Er den anden forbi? moved = hvor langt den anden selv har kørt siden start; gap0 = afstanden mellem de to ved start;
-- distance = afstanden nu; ahead = den anden ligger foran os (i den oprindelige retning).
function ADHeadOn.hasPassed(moved, gap0, distance, ahead)
if moved < ADHeadOn.OTHER_MOVED_M then return false end
return moved >= gap0 + 5 or distance >= ADHeadOn.PASSED_M or not ahead
end
-- s = {phase, reached, last, elapsedMs, stillMs, otherPassed, otherGone} -> "next" | "wait" | "drive" | "hold" | "done"
function ADHeadOn.step(s)
if s.phase == "reverse" then
if s.reached then return s.last and "wait" or "next" end
if s.elapsedMs >= ADHeadOn.REVERSE_TIMEOUT_MS or s.stillMs >= ADHeadOn.REVERSE_STUCK_MS then return "wait" end
return "drive"
end
if s.otherPassed or s.otherGone or s.elapsedMs >= ADHeadOn.WAIT_MAX_MS then return "done" end
return "hold"
end
-- ---- spil ----
local attempts = {} -- "idA|idB" -> {count, at}: bakninger for parret (nulstilles ved ægte passage / efter 20 min)
local function name(vehicle)
if ADRuns ~= nil and ADRuns.vehicleLabel ~= nil then return tostring(ADRuns.vehicleLabel(vehicle)) end
return vehicle ~= nil and vehicle.getName ~= nil and vehicle:getName() or "?"
end
local function pose(vehicle)
local x, _, z = getWorldTranslation(vehicle.rootNode)
local dx, _, dz = localDirectionToWorld(vehicle.rootNode, 0, 0, 1)
local l = math.sqrt(dx * dx + dz * dz)
if l < 0.001 then l = 1 end
return {x = x, z = z, dx = dx / l, dz = dz / l}
end
local function rigLength(adEnv, vehicle)
local total = vehicle.size ~= nil and vehicle.size.length or 6
local ok, units = pcall(adEnv.AutoDrive.getAllUnits, vehicle)
for _, unit in ipairs(ok and units or {}) do
if unit ~= vehicle and unit.size ~= nil then total = total + (unit.size.length or 0) end
end
return total
end
-- Vejpunkterne bag og foran køretøjet på AD's aktuelle rute.
local function routeOf(vehicle)
local dpm = vehicle.ad.drivePathModule
if dpm == nil or dpm.getWayPoints == nil then return {}, {} end
local wayPoints, current = dpm:getWayPoints()
if wayPoints == nil or current == nil then return {}, {} end
local behind, ahead = {}, {}
for i = current - 1, 1, -1 do table.insert(behind, wayPoints[i]) end
for i = current, math.min(#wayPoints, current + ADHeadOn.OTHER_PATH_POINTS - 1) do table.insert(ahead, wayPoints[i]) end
return behind, ahead
end
local function isUnloading(vehicle)
local task = vehicle.ad.taskModule ~= nil and vehicle.ad.taskModule:getActiveTask() or nil
return task ~= nil and task.taskType == "UnloadAtDestinationTask"
end
-- Andre køretøjer (positioner) end de to og deres vogne — til bak-spærre-tjekket.
local function otherPositions(adEnv, a, b)
local skip = {}
for _, vehicle in ipairs({a, b}) do
skip[vehicle] = true
local ok, units = pcall(adEnv.AutoDrive.getAllUnits, vehicle)
for _, unit in ipairs(ok and units or {}) do skip[unit] = true end
end
local list = {}
for _, vehicle in pairs(g_currentMission.vehicleSystem.vehicles or {}) do
if not skip[vehicle] and vehicle.rootNode ~= nil then
local x, _, z = getWorldTranslation(vehicle.rootNode)
table.insert(list, {x = x, z = z, name = name(vehicle)})
end
end
return list
end
local function optionFor(adEnv, vehicle, other, others)
local behind = routeOf(vehicle)
local _, otherAhead = routeOf(other)
local escape, why = ADHeadOn.escapePath(pose(vehicle), behind, otherAhead, rigLength(adEnv, vehicle) + ADHeadOn.REACHED_SLACK_M)
if escape ~= nil then
local blocked, by = ADHeadOn.isBlockedBehind(escape.points, others)
if blocked then escape, why = nil, string.format("'%s' holder på bakkevejen", tostring(by)) end
end
return {escape = escape, unloading = isUnloading(vehicle), why = why}
end
-- Bak ad sporet, vent til den anden er forbi; derefter genoptages AD's opgave (setUp beregner ruten forfra).
local HeadOnTask = {}
HeadOnTask.__index = HeadOnTask
function HeadOnTask.new(adEnv, vehicle, other, escape, key)
return setmetatable({adEnv = adEnv, vehicle = vehicle, other = other, points = escape.points, meters = escape.meters, key = key,
index = 1, phase = "reverse", elapsedMs = 0, stillMs = 0}, HeadOnTask)
end
function HeadOnTask:setUp()
self.last = pose(self.vehicle)
self.lane = self.last -- retningen mod den anden ved start ("forbi" måles mod den, ikke efter en bakket kurve)
self.otherStart = pose(self.other)
self.gap0 = dist(self.last, self.otherStart)
Logging.info(ADHeadOn.LOG_PREFIX .. "'%s' bakker %.0f m (%d vejpunkter) for at give plads til '%s'", name(self.vehicle),
self.meters, #self.points, name(self.other))
end
function HeadOnTask:otherState()
local other = self.other
if other == nil or other.rootNode == nil or other.isDeleted or other.ad == nil or not other.ad.stateModule:isActive() then
return false, true
end
local me, them = pose(self.vehicle), pose(other)
local d = dist(me, them)
local ahead = d > 0.01 and ((them.x - me.x) * self.lane.dx + (them.z - me.z) * self.lane.dz) / d >= 0
return ADHeadOn.hasPassed(dist(them, self.otherStart), self.gap0, d, ahead), false
end
function HeadOnTask:finish(reason)
local vehicle = self.vehicle
local sdm = vehicle.ad.specialDrivingModule
if sdm ~= nil and sdm.stopVehicle ~= nil then sdm:stopVehicle() end
Logging.info(ADHeadOn.LOG_PREFIX .. "'%s' kører videre (%s, %d s) — AutoDrive beregner ruten forfra", name(vehicle), reason,
math.floor(self.elapsedMs / 1000))
local taskModule = self.adEnv.ADTaskModule
vehicle.ad.taskModule:setCurrentTaskFinished(taskModule ~= nil and taskModule.DONT_PROPAGATE or 1)
end
function HeadOnTask:update(dt)
self.elapsedMs = self.elapsedMs + dt
local vehicle = self.vehicle
local sdm = vehicle.ad.specialDrivingModule
local p = pose(vehicle)
if dist(p, self.last) >= ADHeadOn.STILL_MOVE_M then self.last, self.stillMs = p, 0 else self.stillMs = self.stillMs + dt end
local reached = false
if self.phase == "reverse" then
local wp = self.points[self.index]
if sdm.releaseVehicle ~= nil then sdm:releaseVehicle() end
if vehicle.startMotor ~= nil and not vehicle:getIsMotorStarted() and vehicle:getCanMotorRun() then vehicle:startMotor() end
reached = sdm:reverseToTargetLocation(dt, {x = wp.x, y = wp.y or 0, z = wp.z}, ADHeadOn.REVERSE_SPEED) == true
end
local passed, gone = false, false
if self.phase == "wait" then passed, gone = self:otherState() end
local action = ADHeadOn.step({phase = self.phase, reached = reached, last = self.index >= #self.points,
elapsedMs = self.elapsedMs, stillMs = self.stillMs, otherPassed = passed, otherGone = gone})
if action == "next" then
self.index = self.index + 1
elseif action == "wait" then
if not reached or self.index < #self.points then
Logging.info(ADHeadOn.LOG_PREFIX .. "'%s' kom ikke hele vejen tilbage (vejpunkt %d af %d) — venter her", name(vehicle),
self.index, #self.points)
end
self.phase, self.elapsedMs = "wait", 0
if sdm.stopVehicle ~= nil then sdm:stopVehicle() end
elseif action == "done" then
if passed and self.key ~= nil then attempts[self.key] = nil end -- kun ved en ægte passage
self:finish(passed and "den anden er forbi" or (gone and "den anden kører ikke længere" or "ventetiden er gået"))
elseif action == "hold" then
if sdm.stopAndHoldVehicle ~= nil then sdm:stopAndHoldVehicle(dt) end
end
end
function HeadOnTask:abort() end
function HeadOnTask:finished() end
function HeadOnTask:getInfoText() return "Bakker for at give plads" end
function HeadOnTask:getI18nInfo() return "" end
function HeadOnTask:getExcludedVehiclesForCollisionCheck() return {} end
-- Lægger bakningen forrest og den kørende opgave lige bag den (AD kalder dens setUp igen = ny rute).
local function interrupt(vehicle, task)
local taskModule = vehicle.ad.taskModule
local active = taskModule:getActiveTask()
local queued = {}
while taskModule.tasks:Count() > 0 do table.insert(queued, taskModule.tasks:Dequeue()) end
taskModule.tasks:Enqueue(task)
if active ~= nil then taskModule.tasks:Enqueue(active) end
for _, item in ipairs(queued) do taskModule.tasks:Enqueue(item) end
taskModule.activeTask = nil
end
local timerMs = 0
local lastTry = setmetatable({}, {__mode = "k"}) -- køretøj -> ADUtil.now ved sidste forsøg
local function isDriveTask(adEnv, task)
local mt = getmetatable(task)
local class = mt ~= nil and (mt.__index or mt) or nil
if class == nil then return false end
for _, className in ipairs(ADHeadOn.DRIVE_TASKS) do
if adEnv[className] ~= nil and adEnv[className] == class then return true end
end
return false
end
local function isCandidate(adEnv, vehicle)
local ad = vehicle.ad
if ad == nil or ad.collisionDetectionModule == nil or ad.collisionDetectionModule.detectedObstable ~= true then return false end
if ad.taskModule == nil or ad.taskModule.tasks == nil then return false end
local active = ad.taskModule:getActiveTask()
if active == nil or not isDriveTask(adEnv, active) then return false end
-- kører ruten lige nu (ikke ruteplanlægning, ikke fremme ved læssested/aflæsning)
if active.state ~= active.STATE_DRIVING then return false end
local dpm = ad.drivePathModule
if dpm == nil or dpm.wayPoints == nil or dpm.isReversing or (dpm.isTargetReached ~= nil and dpm:isTargetReached()) then return false end
-- holdt med vilje af modden (vent ved aflæsning / udkørsel)
if ADUnloadWait ~= nil and ADUnloadWait.getWaitFor ~= nil and ADUnloadWait.getWaitFor(vehicle) ~= nil then return false end
if ADOutbound ~= nil and ADOutbound.getHoldFor ~= nil and ADOutbound.getHoldFor(vehicle) ~= nil then return false end
return true
end
local function pairKey(a, b)
local ia, ib = tostring(a.id or a.rootNode), tostring(b.id or b.rootNode)
if ia > ib then ia, ib = ib, ia end
return ia .. "|" .. ib
end
local function coolingDown(vehicle, now)
return lastTry[vehicle] ~= nil and now - lastTry[vehicle] < ADHeadOn.COOLDOWN_MS
end
local function giveUp(a, b, why)
local text = string.format("'%s' og '%s' står næse mod næse, og %s — hjælp dem forbi", name(a), name(b), why)
Logging.info(ADHeadOn.LOG_PREFIX .. "%s", text)
if ADFieldJobs ~= nil and ADFieldJobs.notify ~= nil then ADFieldJobs.notify(text, true) end
end
local function resolve(adEnv, a, b, now)
lastTry[a], lastTry[b] = now, now
local key = pairKey(a, b)
local entry = attempts[key]
if entry ~= nil and now - entry.at >= ADHeadOn.ATTEMPTS_EXPIRE_MS then entry = nil; attempts[key] = nil end
if entry ~= nil and entry.count >= ADHeadOn.MAX_ATTEMPTS then
if not entry.told then
entry.told = true
giveUp(a, b, string.format("det hjalp ikke at bakke %d gange", ADHeadOn.MAX_ATTEMPTS))
end
return
end
local _, aheadA = routeOf(a)
local _, aheadB = routeOf(b)
if not ADHeadOn.onEachOthersRoute(pose(a), aheadA, pose(b), aheadB) then return end
local others = otherPositions(adEnv, a, b)
local optA, optB = optionFor(adEnv, a, b, others), optionFor(adEnv, b, a, others)
local pick = ADHeadOn.chooseBacker(optA, optB)
if pick == nil then
local pa, pb = pose(a), pose(b)
Logging.info(ADHeadOn.LOG_PREFIX .. "'%s' (%.0f, %.0f): %s; '%s' (%.0f, %.0f): %s; afstand %.0f m", name(a), pa.x, pa.z,
tostring(optA.why), name(b), pb.x, pb.z, tostring(optB.why), dist(pa, pb))
giveUp(a, b, "ingen af dem kan bakke fri")
return
end
attempts[key] = {count = (entry ~= nil and entry.count or 0) + 1, at = now}
local backer, other, option = a, b, optA
if pick == "b" then backer, other, option = b, a, optB end
local pa, pb = pose(backer), pose(other)
Logging.info(ADHeadOn.LOG_PREFIX .. "valgt til at bakke: '%s' (%.0f, %.0f) for '%s' (%.0f, %.0f), afstand %.0f m", name(backer), pa.x, pa.z, name(other), pb.x, pb.z, dist(pa, pb))
interrupt(backer, HeadOnTask.new(adEnv, backer, other, option.escape, key))
end
function ADHeadOn.tick(adEnv, dt)
timerMs = timerMs + (dt or 0)
if timerMs < ADHeadOn.CHECK_MS then return end
timerMs = 0
if adEnv == nil or adEnv.AutoDrive == nil or ADStuckDiag == nil or ADStuckDiag.stillVehicles == nil then return end
local now = ADUtil.now()
local stuck = {}
for _, entry in ipairs(ADStuckDiag.stillVehicles(ADHeadOn.HEADON_MS)) do
local vehicle = entry.vehicle
if vehicle.rootNode ~= nil and not coolingDown(vehicle, now) and isCandidate(adEnv, vehicle) then
table.insert(stuck, {vehicle = vehicle, pose = pose(vehicle)})
end
end
local handled = {}
for i = 1, #stuck do
for j = i + 1, #stuck do
local a, b = stuck[i], stuck[j]
if not handled[a.vehicle] and not handled[b.vehicle] and ADHeadOn.isFacing(a.pose, b.pose) then
handled[a.vehicle], handled[b.vehicle] = true, true
local ok, err = pcall(resolve, adEnv, a.vehicle, b.vehicle, now)
if not ok then Logging.warning(ADHeadOn.LOG_PREFIX .. "fejl: %s", tostring(err)) end
end
end
end
end