feat(kobling): rummelig koblingsrækkevidde (3 m) og bakning de sidste 3-8 m hen til vognens trækøje ved Connect-markører; lagervisning: bredere mængde-/rate-kolonner så teksten ikke klippes (v1.53.0.0)
This commit is contained in:
@@ -0,0 +1,193 @@
|
||||
-- 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.
|
||||
|
||||
ADHitchCreep = {}
|
||||
ADHitchCreep.LOG_PREFIX = "ADSmartPickup: kobling: "
|
||||
ADHitchCreep.ROOMY_ATTACH_DISTANCE = 3 -- m: herinden for kobler ADHitch direkte (samme som ADHitch.ROOMY_ATTACH_DISTANCE)
|
||||
ADHitchCreep.CREEP_MAX_DISTANCE = 8 -- m: længere væk end det bakkes ikke — markøren ligger forkert
|
||||
ADHitchCreep.CREEP_MAX_LATERAL = 1.5 -- m til siden: mere end det rammer en lige bakning ikke trækøjet
|
||||
ADHitchCreep.CREEP_TIMEOUT_MS = 25000
|
||||
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"
|
||||
function ADHitchCreep.plan(offset)
|
||||
if offset == nil then return "none" end
|
||||
local distance = math.sqrt(offset.x ^ 2 + offset.z ^ 2)
|
||||
if distance <= ADHitchCreep.ROOMY_ATTACH_DISTANCE then return "attach" end
|
||||
if offset.z >= 0 or distance > ADHitchCreep.CREEP_MAX_DISTANCE or math.abs(offset.x) > ADHitchCreep.CREEP_MAX_LATERAL then
|
||||
return "none"
|
||||
end
|
||||
return "creep"
|
||||
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
|
||||
|
||||
-- ---- 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 bagerste krog-noder.
|
||||
local function freeRearJoints(vehicle)
|
||||
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 lz < 0 then table.insert(joints, joint.jointTransform) 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 en ledig krog. -> {wagon, joint, target, offset, plan} | nil
|
||||
function ADHitchCreep.findTarget(vehicle)
|
||||
local joints = freeRearJoints(vehicle)
|
||||
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, bestDistance = nil, math.huge
|
||||
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
|
||||
local offset = offsetOf(vehicle, joint, input.node)
|
||||
local distance = math.sqrt(offset.x ^ 2 + offset.z ^ 2)
|
||||
if distance < bestDistance and distance <= ADHitchCreep.CREEP_MAX_DISTANCE + 2 then
|
||||
bestDistance = distance
|
||||
best = {wagon = object, joint = joint, target = input.node, offset = offset}
|
||||
end
|
||||
end
|
||||
end
|
||||
end
|
||||
end
|
||||
end
|
||||
if best ~= nil then best.plan = ADHitchCreep.plan(best.offset) 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, ADHitchCreep.ROOMY_ATTACH_DISTANCE ^ 2, 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' bakker %.1f m hen til '%s' (%.1f m til siden)", vehicleName(self.vehicle),
|
||||
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, distance = distance, startDistance = self.startDistance,
|
||||
elapsedMs = self.elapsedMs, passed = offset.z > 0.5})
|
||||
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
|
||||
-- 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
|
||||
|
||||
function CreepTask:abort() end
|
||||
function CreepTask:finished() end
|
||||
function CreepTask:getInfoText() return "Bakker hen til vognen" end
|
||||
function CreepTask:getI18nInfo() return "" end
|
||||
function CreepTask:getExcludedVehiclesForCollisionCheck() return {self.target.wagon} 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
|
||||
local target = ADHitchCreep.findTarget(self.vehicle)
|
||||
if target == nil or target.plan ~= "creep" then 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
|
||||
Reference in New Issue
Block a user