379 lines
18 KiB
Lua
379 lines
18 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
|
|
for _, p in ipairs(trail or {}) do
|
|
if p.x == nil or p.z == nil then return nil end
|
|
meters = meters + dist(prev, p)
|
|
if meters > ADHeadOn.MAX_REVERSE_M then return nil 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
|
|
end
|
|
|
|
-- Spærrer et andet køretøj (positioner {x, z}) bakkevejen?
|
|
function ADHeadOn.isBlockedBehind(points, others)
|
|
for _, other in ipairs(others or {}) do
|
|
if minDistance(other, points) <= ADHeadOn.BEHIND_BLOCK_M then return true 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})
|
|
end
|
|
end
|
|
return list
|
|
end
|
|
|
|
local function optionFor(adEnv, vehicle, other, others)
|
|
local behind = routeOf(vehicle)
|
|
local _, otherAhead = routeOf(other)
|
|
local escape = ADHeadOn.escapePath(pose(vehicle), behind, otherAhead, rigLength(adEnv, vehicle) + ADHeadOn.REACHED_SLACK_M)
|
|
if escape ~= nil and ADHeadOn.isBlockedBehind(escape.points, others) then escape = nil end
|
|
return {escape = escape, unloading = isUnloading(vehicle)}
|
|
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
|
|
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
|
|
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
|