-- 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