diff --git a/.gitignore b/.gitignore new file mode 100644 index 00000000..3f2c92b2 --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ + +/.vscode diff --git a/README.md b/README.md index d066aa1c..e2520072 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,7 @@ # FS22_AutoDrive FS22 version of the AutoDrive mod -### Latest Release: 2.0.1.6 +### Latest Release: 2.0.1.8 ![GitHub all releases](https://img.shields.io/github/downloads/Stephan-S/FS22_AutoDrive/total?label=Downloads&style=plastic) Direct Download: https://github.com/Stephan-S/FS22_AutoDrive/releases/latest/download/FS22_AutoDrive.zip diff --git a/gui/userSettingsPage.xml b/gui/userSettingsPage.xml index 746cf513..3063fd4d 100644 --- a/gui/userSettingsPage.xml +++ b/gui/userSettingsPage.xml @@ -114,6 +114,14 @@ + + + + + + + + diff --git a/modDesc.xml b/modDesc.xml index 968ce961..f2f46c69 100644 --- a/modDesc.xml +++ b/modDesc.xml @@ -32,7 +32,7 @@ Différents modes d'utilisation ont été ajoutés depuis les premières version - 2.0.1.6 + 2.0.1.8 icon.dds diff --git a/register.lua b/register.lua index 02e6797d..fdef7964 100644 --- a/register.lua +++ b/register.lua @@ -64,6 +64,7 @@ source(Utils.getFilename("scripts/Utils/Buffer.lua", g_currentModDirectory)) source(Utils.getFilename("scripts/Utils/FlaggedTable.lua", g_currentModDirectory)) source(Utils.getFilename("scripts/Utils/CollisionDetectionUtils.lua", g_currentModDirectory)) source(Utils.getFilename("scripts/Utils/PathFinderUtils.lua", g_currentModDirectory)) +source(Utils.getFilename("scripts/Utils/Dubins.lua", g_currentModDirectory)) source(Utils.getFilename("scripts/Utils/AutoDriveUtilFuncs.lua", g_currentModDirectory)) source(Utils.getFilename("scripts/Utils/SortedQueue.lua", g_currentModDirectory)) source(Utils.getFilename("scripts/Utils/DevFuncs.lua", g_currentModDirectory)) diff --git a/scripts/AutoDrive.lua b/scripts/AutoDrive.lua index 76cfe98d..5b7cf3c3 100644 --- a/scripts/AutoDrive.lua +++ b/scripts/AutoDrive.lua @@ -1,5 +1,5 @@ AutoDrive = {} -AutoDrive.version = "2.0.1.6" +AutoDrive.version = "2.0.1.8" AutoDrive.directory = g_currentModDirectory @@ -17,6 +17,7 @@ AutoDrive.experimentalFeatures.UTurn = true AutoDrive.experimentalFeatures.FoldImplements = true AutoDrive.experimentalFeatures.RefuelOnlyAtValidStations = true AutoDrive.experimentalFeatures.RecordWhileNotInVehicle = false +AutoDrive.experimentalFeatures.NewPathfinder = true AutoDrive.dynamicChaseDistance = true AutoDrive.smootherDriving = true @@ -440,7 +441,7 @@ function AutoDrive.drawNetworkOnMap() if isSubPrio(outNode) then r, g, b, a = unpack(AutoDrive.currentColors.ad_color_subPrioDualConnection) end - elseif ADGraphManager:isReverseRoad(start, target) then + elseif ADGraphManager:isReverseRoad(node, outNode) then r, g, b, a = unpack(AutoDrive.currentColors.ad_color_reverseConnection) end setOverlayColor( AutoDrive.courseOverlayId, r, g, b, a) diff --git a/scripts/ExternalInterface.lua b/scripts/ExternalInterface.lua index 70ac2923..b79de0f9 100644 --- a/scripts/ExternalInterface.lua +++ b/scripts/ExternalInterface.lua @@ -209,8 +209,7 @@ function AutoDrive:notifyDestinationListeners() end function AutoDrive:combineIsCallingDriver(combine) --only for CoursePlay - local openPipe,_ = ADHarvestManager.getOpenPipePercent(combine) - return openPipe or ADHarvestManager.doesHarvesterNeedUnloading(combine, true) + return AutoDrive:getIsCPWaitingForUnload(combine) end function AutoDrive:getCombineOpenPipePercent(combine) --for AIVE @@ -828,9 +827,9 @@ function AutoDrive:getALFillTypes(object) -- used by PullDownList, getSupportedF -- spec_universalAutoload local spec = object.spec_universalAutoload if spec and AutoDrive:hasAL(object) then - AutoDrive.debugPrint(vehicle, AutoDrive.DC_EXTERNALINTERFACEINFO, "AutoDrive:getALCurrentFillType spec_universalAutoload function not supported!") + AutoDrive.debugPrint(object, AutoDrive.DC_EXTERNALINTERFACEINFO, "AutoDrive:getALCurrentFillType spec_universalAutoload function not supported!") end - AutoDrive.debugPrint(vehicle, AutoDrive.DC_EXTERNALINTERFACEINFO, "AutoDrive:getALFillTypes #fillTypes %s", tostring(#fillTypes)) + AutoDrive.debugPrint(object, AutoDrive.DC_EXTERNALINTERFACEINFO, "AutoDrive:getALFillTypes #fillTypes %s", tostring(#fillTypes)) return fillTypes end @@ -847,7 +846,7 @@ function AutoDrive:getALCurrentFillType(object) -- used by onEnterVehicle, onPos -- spec_universalAutoload local spec = object.spec_universalAutoload if spec and AutoDrive:hasAL(object) then - AutoDrive.debugPrint(vehicle, AutoDrive.DC_EXTERNALINTERFACEINFO, "AutoDrive:getALCurrentFillType spec_universalAutoload function not supported!") + AutoDrive.debugPrint(object, AutoDrive.DC_EXTERNALINTERFACEINFO, "AutoDrive:getALCurrentFillType spec_universalAutoload function not supported!") end return nil end diff --git a/scripts/Hud.lua b/scripts/Hud.lua index c1d03d95..25c241c0 100644 --- a/scripts/Hud.lua +++ b/scripts/Hud.lua @@ -429,6 +429,7 @@ function AutoDriveHud:mouseEvent(vehicle, posX, posY, isDown, isUp, button) if mouseActiveForAutoDrive then local mouseEventHandled = false + local silent = false if AutoDrive.splineInterpolation ~= nil then AutoDrive.splineInterpolation.valid = false end diff --git a/scripts/Hud/PullDownList.lua b/scripts/Hud/PullDownList.lua index 7bcbbb7a..aa281c31 100644 --- a/scripts/Hud/PullDownList.lua +++ b/scripts/Hud/PullDownList.lua @@ -497,14 +497,16 @@ function ADPullDownList:createSelection_FillType() if vehicle ~= nil then local trailers, _ = AutoDrive.getAllUnits(vehicle) - supportedFillTypes = {} - for _, trailer in ipairs(trailers) do - if AutoDrive:hasAL(trailer) then - local alFillTypes = AutoDrive:getALFillTypes(trailer) - if alFillTypes ~= nil and #alFillTypes > 0 then - -- self.autoLoadFillTypes is either nil or it contains items. It is never empty. - self.autoLoadFillTypes = alFillTypes - break + if trailers then + supportedFillTypes = {} + for _, trailer in ipairs(trailers) do + if AutoDrive:hasAL(trailer) then + local alFillTypes = AutoDrive:getALFillTypes(trailer) + if alFillTypes ~= nil and #alFillTypes > 0 then + -- self.autoLoadFillTypes is either nil or it contains items. It is never empty. + self.autoLoadFillTypes = alFillTypes + break + end end end end @@ -703,21 +705,29 @@ function ADPullDownList:act(vehicle, posX, posY, isDown, isUp, button) elseif button == 4 and isUp then -- mouse wheel local oldSelected = self.selected - self.selected = math.max(1, self.selected - 1) + local decrement = 1 + if AutoDrive.leftCTRLmodifierKeyPressed then + decrement = 10 + end + self.selected = math.max(1, self.selected - decrement) if oldSelected ~= self.selected then - self.hovered = math.max(1, self.hovered - 1) + self.hovered = math.max(1, self.hovered - decrement) end - if self.hovered > (self.selected + ADPullDownList.MAX_SHOWN - 1) then - self.hovered = self.selected + ADPullDownList.MAX_SHOWN - 1 + if self.hovered > (self.selected + ADPullDownList.MAX_SHOWN - decrement) then + self.hovered = self.selected + ADPullDownList.MAX_SHOWN - decrement end AutoDrive.mouseWheelActive = true return true elseif button == 5 and isUp then -- mouse wheel - if self:getListElementByIndex(vehicle, self.selected + 1 + ADPullDownList.MAX_SHOWN - 3) ~= nil then - self.selected = self.selected + 1 - if self:getListElementByIndex(vehicle, self.hovered + 1 + ADPullDownList.MAX_SHOWN - 3) ~= nil then - self.hovered = self.hovered + 1 + local increment = 1 + if AutoDrive.leftCTRLmodifierKeyPressed then + increment = 10 + end + if self:getListElementByIndex(vehicle, self.selected + increment + ADPullDownList.MAX_SHOWN - 3) ~= nil then + self.selected = self.selected + increment + if self:getListElementByIndex(vehicle, self.hovered + increment + ADPullDownList.MAX_SHOWN - 3) ~= nil then + self.hovered = self.hovered + increment end end AutoDrive.mouseWheelActive = true diff --git a/scripts/Manager/BunkerSiloManager.lua b/scripts/Manager/BunkerSiloManager.lua index 12bfc0d7..9157d57c 100644 --- a/scripts/Manager/BunkerSiloManager.lua +++ b/scripts/Manager/BunkerSiloManager.lua @@ -54,7 +54,11 @@ function ADBunkerSiloManager:update(dt) if triggerX ~= nil then local distance = MathUtil.vector2Length(triggerX - vehicleX, triggerZ - vehicleZ) if distance < bsmRange then - if AutoDrive.isVehicleInBunkerSiloArea(vehicle) or bunkerSilo.adClosestVehicle == vehicle then + local fillLevel, _, _, _ = AutoDrive.getAllFillLevels(AutoDrive.getAllUnits(vehicle)) + if AutoDrive.isVehicleInBunkerSiloArea(vehicle) + or bunkerSilo.adClosestVehicle == vehicle + or fillLevel < 0.1 + then -- IMPORTANT: DO NOT SET setUnPaused to avoid crash with CP silo compacter !!! -- vehicle.ad.drivePathModule:setUnPaused() else diff --git a/scripts/Manager/DrawingManager.lua b/scripts/Manager/DrawingManager.lua index cbfc6b49..99aaf1c3 100644 --- a/scripts/Manager/DrawingManager.lua +++ b/scripts/Manager/DrawingManager.lua @@ -1,4 +1,5 @@ ADDrawingManager = {} +ADDrawingManager.enableDebug = false ADDrawingManager.i3DBaseDir = "drawing/" ADDrawingManager.yOffset = 0 ADDrawingManager.emittivity = 0 @@ -10,6 +11,8 @@ ADDrawingManager.lines.fileName = "line.i3d" ADDrawingManager.lines.buffer = Buffer:new() ADDrawingManager.lines.objects = FlaggedTable:new() ADDrawingManager.lines.tasks = {} +ADDrawingManager.lines.lastTaskCount = 0 +ADDrawingManager.lines.currentTask = 0 ADDrawingManager.lines.itemIDs = {} ADDrawingManager.lines.lastDrawZero = true @@ -18,6 +21,8 @@ ADDrawingManager.arrows.fileName = "arrow.i3d" ADDrawingManager.arrows.buffer = Buffer:new() ADDrawingManager.arrows.objects = FlaggedTable:new() ADDrawingManager.arrows.tasks = {} +ADDrawingManager.arrows.lastTaskCount = 0 +ADDrawingManager.arrows.currentTask = 0 ADDrawingManager.arrows.itemIDs = {} ADDrawingManager.arrows.lastDrawZero = true ADDrawingManager.arrows.position = {} @@ -29,6 +34,8 @@ ADDrawingManager.sSphere.fileName = "sphere_small.i3d" ADDrawingManager.sSphere.buffer = Buffer:new() ADDrawingManager.sSphere.objects = FlaggedTable:new() ADDrawingManager.sSphere.tasks = {} +ADDrawingManager.sSphere.lastTaskCount = 0 +ADDrawingManager.sSphere.currentTask = 0 ADDrawingManager.sSphere.itemIDs = {} ADDrawingManager.sSphere.lastDrawZero = true @@ -37,6 +44,8 @@ ADDrawingManager.sphere.fileName = "sphere.i3d" ADDrawingManager.sphere.buffer = Buffer:new() ADDrawingManager.sphere.objects = FlaggedTable:new() ADDrawingManager.sphere.tasks = {} +ADDrawingManager.sphere.lastTaskCount = 0 +ADDrawingManager.sphere.currentTask = 0 ADDrawingManager.sphere.itemIDs = {} ADDrawingManager.sphere.lastDrawZero = true @@ -48,6 +57,8 @@ ADDrawingManager.markers.fileNames[3] = "marker_3.i3d" ADDrawingManager.markers.buffer = Buffer:new() ADDrawingManager.markers.objects = FlaggedTable:new() ADDrawingManager.markers.tasks = {} +ADDrawingManager.markers.lastTaskCount = 0 +ADDrawingManager.markers.currentTask = 0 ADDrawingManager.markers.itemIDs = {} ADDrawingManager.markers.lastDrawZero = true ADDrawingManager.markers.lastDrawFileUsed = 1 @@ -58,6 +69,8 @@ ADDrawingManager.cross.fileName = "cross.i3d" ADDrawingManager.cross.buffer = Buffer:new() ADDrawingManager.cross.objects = FlaggedTable:new() ADDrawingManager.cross.tasks = {} +ADDrawingManager.cross.lastTaskCount = 0 +ADDrawingManager.cross.currentTask = 0 ADDrawingManager.cross.itemIDs = {} ADDrawingManager.cross.lastDrawZero = true @@ -89,46 +102,199 @@ end function ADDrawingManager:addLineTask(sx, sy, sz, ex, ey, ez, scale, r, g, b) -- storing task - local hash = 0 - -- local hash = string.format("l%.2f%.2f%.2f%.2f%.2f%.2f%.2f%.2f%.2f%.1f", sx, sy, sz, ex, ey, ez, r, g, b, self.yOffset) - table.insert(self.lines.tasks, {sx = sx, sy = sy, sz = sz, ex = ex, ey = ey, ez = ez, scale = scale, r = r, g = g, b = b, hash = hash}) + -- local hash = 0 + -- table.insert(self.lines.tasks, {sx = sx, sy = sy, sz = sz, ex = ex, ey = ey, ez = ez, scale = scale, r = r, g = g, b = b, hash = hash}) + scale = scale or 1 + self.lines.currentTask = self.lines.currentTask + 1 + if self.lines.tasks[self.lines.currentTask] == nil then + -- add new task + table.insert(self.lines.tasks, {sx = sx, sy = sy, sz = sz, ex = ex, ey = ey, ez = ez, scale = scale, r = r, g = g, b = b, taskChanged = true}) + elseif + self.lines.tasks[self.lines.currentTask].sx ~= sx or + self.lines.tasks[self.lines.currentTask].sy ~= sy or + self.lines.tasks[self.lines.currentTask].sz ~= sz or + self.lines.tasks[self.lines.currentTask].ex ~= ex or + self.lines.tasks[self.lines.currentTask].ey ~= ey or + self.lines.tasks[self.lines.currentTask].ez ~= ez or + self.lines.tasks[self.lines.currentTask].scale ~= scale or + self.lines.tasks[self.lines.currentTask].r ~= r or + self.lines.tasks[self.lines.currentTask].g ~= g or + self.lines.tasks[self.lines.currentTask].b ~= b + then + -- task changed + self.lines.tasks[self.lines.currentTask].sx = sx + self.lines.tasks[self.lines.currentTask].sy = sy + self.lines.tasks[self.lines.currentTask].sz = sz + self.lines.tasks[self.lines.currentTask].ex = ex + self.lines.tasks[self.lines.currentTask].ey = ey + self.lines.tasks[self.lines.currentTask].ez = ez + self.lines.tasks[self.lines.currentTask].scale = scale + self.lines.tasks[self.lines.currentTask].r = r + self.lines.tasks[self.lines.currentTask].g = g + self.lines.tasks[self.lines.currentTask].b = b + self.lines.tasks[self.lines.currentTask].taskChanged = true + else + -- task unchanged -> false will be set after update with dFunc + -- self.lines.tasks[self.lines.currentTask].taskChanged = false + end end function ADDrawingManager:addArrowTask(sx, sy, sz, ex, ey, ez, scale, position, r, g, b) -- storing task - local hash = 0 - -- local hash = string.format("a%.2f%.2f%.2f%.2f%.2f%.2f%d%.2f%.2f%.2f%.1f", sx, sy, sz, ex, ey, ez, position, r, g, b, self.yOffset) - table.insert(self.arrows.tasks, {sx = sx, sy = sy, sz = sz, ex = ex, ey = ey, ez = ez, scale = scale, r = r, g = g, b = b, position = position, hash = hash}) + scale = scale or 1 + -- local hash = 0 + -- table.insert(self.arrows.tasks, {sx = sx, sy = sy, sz = sz, ex = ex, ey = ey, ez = ez, scale = scale, r = r, g = g, b = b, position = position, hash = hash}) + self.arrows.currentTask = self.arrows.currentTask + 1 + if self.arrows.tasks[self.arrows.currentTask] == nil then + -- add new task + table.insert(self.arrows.tasks, {sx = sx, sy = sy, sz = sz, ex = ex, ey = ey, ez = ez, scale = scale, position = position, r = r, g = g, b = b, taskChanged = true}) + elseif + self.arrows.tasks[self.arrows.currentTask].sx ~= sx or + self.arrows.tasks[self.arrows.currentTask].sy ~= sy or + self.arrows.tasks[self.arrows.currentTask].sz ~= sz or + self.arrows.tasks[self.arrows.currentTask].ex ~= ex or + self.arrows.tasks[self.arrows.currentTask].ey ~= ey or + self.arrows.tasks[self.arrows.currentTask].ez ~= ez or + self.arrows.tasks[self.arrows.currentTask].scale ~= scale or + self.arrows.tasks[self.arrows.currentTask].position ~= position or + self.arrows.tasks[self.arrows.currentTask].r ~= r or + self.arrows.tasks[self.arrows.currentTask].g ~= g or + self.arrows.tasks[self.arrows.currentTask].b ~= b + then + -- task changed + self.arrows.tasks[self.arrows.currentTask].sx = sx + self.arrows.tasks[self.arrows.currentTask].sy = sy + self.arrows.tasks[self.arrows.currentTask].sz = sz + self.arrows.tasks[self.arrows.currentTask].ex = ex + self.arrows.tasks[self.arrows.currentTask].ey = ey + self.arrows.tasks[self.arrows.currentTask].ez = ez + self.arrows.tasks[self.arrows.currentTask].scale = scale + self.arrows.tasks[self.arrows.currentTask].position = position + self.arrows.tasks[self.arrows.currentTask].r = r + self.arrows.tasks[self.arrows.currentTask].g = g + self.arrows.tasks[self.arrows.currentTask].b = b + self.arrows.tasks[self.arrows.currentTask].taskChanged = true + else + -- task unchanged -> false will be set after update with dFunc + -- self.arrows.tasks[self.arrows.currentTask].taskChanged = false + end end function ADDrawingManager:addSmallSphereTask(x, y, z, r, g, b) -- storing task - local hash = 0 - -- local hash = string.format("ss%.2f%.2f%.2f%.2f%.2f%.2f%.1f", x, y, z, r, g, b, self.yOffset) - table.insert(self.sSphere.tasks, {x = x, y = y, z = z, r = r, g = g, b = b, hash = hash}) + -- local hash = 0 + -- table.insert(self.sSphere.tasks, {x = x, y = y, z = z, r = r, g = g, b = b, hash = hash}) + self.sSphere.currentTask = self.sSphere.currentTask + 1 + if self.sSphere.tasks[self.sSphere.currentTask] == nil then + -- add new task + table.insert(self.sSphere.tasks, {x = x, y = y, z = z, r = r, g = g, b = b, taskChanged = true}) + elseif + self.sSphere.tasks[self.sSphere.currentTask].x ~= x or + self.sSphere.tasks[self.sSphere.currentTask].y ~= y or + self.sSphere.tasks[self.sSphere.currentTask].z ~= z or + self.sSphere.tasks[self.sSphere.currentTask].r ~= r or + self.sSphere.tasks[self.sSphere.currentTask].g ~= g or + self.sSphere.tasks[self.sSphere.currentTask].b ~= b + then + -- task changed + self.sSphere.tasks[self.sSphere.currentTask].x = x + self.sSphere.tasks[self.sSphere.currentTask].y = y + self.sSphere.tasks[self.sSphere.currentTask].z = z + self.sSphere.tasks[self.sSphere.currentTask].r = r + self.sSphere.tasks[self.sSphere.currentTask].g = g + self.sSphere.tasks[self.sSphere.currentTask].b = b + self.sSphere.tasks[self.sSphere.currentTask].taskChanged = true + else + -- task unchanged -> false will be set after update with dFunc + -- self.sSphere.tasks[self.sSphere.currentTask].taskChanged = false + end end function ADDrawingManager:addMarkerTask(x, y, z) -- storing task - local hash = 0 - -- local hash = string.format("m%.2f%.2f%.2f%.1f", x, y, z, self.yOffset) - table.insert(self.markers.tasks, {x = x, y = y, z = z, hash = hash}) + -- local hash = 0 + -- table.insert(self.markers.tasks, {x = x, y = y, z = z, hash = hash}) + self.markers.currentTask = self.markers.currentTask + 1 + if self.markers.tasks[self.markers.currentTask] == nil then + -- add new task + table.insert(self.markers.tasks, {x = x, y = y, z = z, taskChanged = true}) + elseif + self.markers.tasks[self.markers.currentTask].x ~= x or + self.markers.tasks[self.markers.currentTask].y ~= y or + self.markers.tasks[self.markers.currentTask].z ~= z + then + -- task changed + self.markers.tasks[self.markers.currentTask].x = x + self.markers.tasks[self.markers.currentTask].y = y + self.markers.tasks[self.markers.currentTask].z = z + self.markers.tasks[self.markers.currentTask].taskChanged = true + else + -- task unchanged -> false will be set after update with dFunc + -- self.markers.tasks[self.markers.currentTask].taskChanged = false + end end function ADDrawingManager:addCrossTask(x, y, z, scale) -- storing task - local hash = 0 - -- local hash = string.format("c%.2f%.2f%.2f%.1f", x, y, z, self.yOffset) - table.insert(self.cross.tasks, {x = x, y = y, z = z, scale = scale, hash = hash}) + -- local hash = 0 + -- table.insert(self.cross.tasks, {x = x, y = y, z = z, scale = scale, hash = hash}) + scale = scale or 1 + self.cross.currentTask = self.cross.currentTask + 1 + if self.cross.tasks[self.cross.currentTask] == nil then + -- add new task + table.insert(self.cross.tasks, {x = x, y = y, z = z, scale = scale, taskChanged = true}) + elseif + self.cross.tasks[self.cross.currentTask].x ~= x or + self.cross.tasks[self.cross.currentTask].y ~= y or + self.cross.tasks[self.cross.currentTask].z ~= z or + self.cross.tasks[self.cross.currentTask].scale ~= scale + then + -- task changed + self.cross.tasks[self.cross.currentTask].x = x + self.cross.tasks[self.cross.currentTask].y = y + self.cross.tasks[self.cross.currentTask].z = z + self.cross.tasks[self.cross.currentTask].scale = scale + self.cross.tasks[self.cross.currentTask].taskChanged = true + else + -- task unchanged -> false will be set after update with dFunc + -- self.cross.tasks[self.cross.currentTask].taskChanged = false + end end function ADDrawingManager:addSphereTask(x, y, z, scale, r, g, b, a) + -- storing task + -- local hash = 0 + -- table.insert(self.sphere.tasks, {x = x, y = y, z = z, r = r, g = g, b = b, a = a, scale = scale, hash = hash}) scale = scale or 1 a = a or 0 - -- storing task - local hash = 0 - -- local hash = string.format("s%.2f%.2f%.2f%.3f%.2f%.2f%.2f%.2f%.1f", x, y, z, scale, r, g, b, a, self.yOffset) - table.insert(self.sphere.tasks, {x = x, y = y, z = z, r = r, g = g, b = b, a = a, scale = scale, hash = hash}) + self.sphere.currentTask = self.sphere.currentTask + 1 + if self.sphere.tasks[self.sphere.currentTask] == nil then + -- add new task + table.insert(self.sphere.tasks, {x = x, y = y, z = z, r = r, g = g, b = b, a = a, scale = scale, taskChanged = true}) + elseif + self.sphere.tasks[self.sphere.currentTask].x ~= x or + self.sphere.tasks[self.sphere.currentTask].y ~= y or + self.sphere.tasks[self.sphere.currentTask].z ~= z or + self.sphere.tasks[self.sphere.currentTask].r ~= r or + self.sphere.tasks[self.sphere.currentTask].g ~= g or + self.sphere.tasks[self.sphere.currentTask].b ~= b or + self.sphere.tasks[self.sphere.currentTask].a ~= a or + self.sphere.tasks[self.sphere.currentTask].scale ~= scale + then + -- task changed + self.sphere.tasks[self.sphere.currentTask].x = x + self.sphere.tasks[self.sphere.currentTask].y = y + self.sphere.tasks[self.sphere.currentTask].z = z + self.sphere.tasks[self.sphere.currentTask].r = r + self.sphere.tasks[self.sphere.currentTask].g = g + self.sphere.tasks[self.sphere.currentTask].b = b + self.sphere.tasks[self.sphere.currentTask].a = a + self.sphere.tasks[self.sphere.currentTask].scale = scale + self.sphere.tasks[self.sphere.currentTask].taskChanged = true + else + -- task unchanged -> false will be set after update with dFunc + -- self.sphere.tasks[self.sphere.currentTask].taskChanged = false + end end function ADDrawingManager:draw() @@ -181,8 +347,109 @@ function ADDrawingManager:draw() end end +function ADDrawingManager:drawObjects_alternative2(obj, dFunc, iFunc) + -- ADDrawingManager.debugMsg(nil, "ADDrawingManager:drawObjects_alternative2 start...") + local stats = {} + stats["Tasks"] = {Total = obj.currentTask} + stats["itemIDs"] = {Total = #obj.itemIDs} + + if (obj.currentTask == 0 and obj.lastTaskCount > 0) then -- disabled obj -> set all invisible + -- ADDrawingManager.debugMsg(nil, "ADDrawingManager:drawObjects_alternative2 disabled objects %s", tostring(obj.fileName)) + -- set all invisible + for _, itemID in ipairs (obj.itemIDs) do + setVisibility(itemID, false) + end + obj.lastTaskCount = obj.currentTask + return stats + end + + if obj.currentTask == 0 then -- nothing to draw -> exit + -- if (g_updateLoopIndex % 60 == 0) then + -- ADDrawingManager.debugMsg(nil, "ADDrawingManager:drawObjects_alternative2 nothing to draw %s", tostring(obj.fileName)) + -- end + obj.lastTaskCount = obj.currentTask + return stats + end + + local fileToUse + if obj.usesSelection then + if obj.lastDrawFileUsed ~= AutoDrive.getSetting("iconSetToUse") then + -- cleaning up not needed objects + -- ADDrawingManager.debugMsg(nil, "ADDrawingManager:drawObjects_alternative2 iconSetToUse changed %s", tostring(obj.fileName)) + for _, id in pairs(obj.itemIDs) do + -- make invisible unused items + setVisibility(id, false) + end + obj.itemIDs = {} + end + fileToUse = obj.fileNames[AutoDrive.getSetting("iconSetToUse")] + obj.lastDrawFileUsed = AutoDrive.getSetting("iconSetToUse") + else + fileToUse = obj.fileName + end +-- ADDrawingManager.debugMsg(nil, "ADDrawingManager:drawObjects_alternative2 fileToUse %s ", tostring(fileToUse)) + + -- check if enougth objects are available - add missing amount + if obj.currentTask > #obj.itemIDs then + -- ADDrawingManager.debugMsg(nil, "ADDrawingManager:drawObjects_alternative2 obj.currentTask %s #obj.itemIDs %s %s", tostring(obj.currentTask), tostring(#obj.itemIDs), tostring(obj.fileName)) + for i = 1, obj.currentTask - #obj.itemIDs do + -- loading new i3ds + local itemID = g_i3DManager:loadSharedI3DFile(self.i3DBaseDir .. fileToUse, false, false) + table.insert(obj.itemIDs,iFunc(itemID)) + end + end +-- ADDrawingManager.debugMsg(nil, "ADDrawingManager:drawObjects_alternative2 obj.currentTask %s #obj.itemIDs %s ", tostring(obj.currentTask), tostring(#obj.itemIDs)) + + -- handle visibility + if obj.currentTask > 0 then + if obj.currentTask < obj.lastTaskCount then + -- set not needed items invisible + -- ADDrawingManager.debugMsg(nil, "ADDrawingManager:drawObjects_alternative2 obj.currentTask %d < lastTaskCount %d %s", obj.currentTask, obj.lastTaskCount, tostring(obj.fileName)) + for i = obj.currentTask + 1, obj.lastTaskCount do + -- ADDrawingManager.debugMsg(nil, "ADDrawingManager:drawObjects_alternative2 setVisibility itemID %s ", tostring(itemID)) + if obj.itemIDs[i] ~= nil then + setVisibility(obj.itemIDs[i], false) + end + end + elseif obj.currentTask > obj.lastTaskCount then + -- set previous invisible items visible + -- ADDrawingManager.debugMsg(nil, "ADDrawingManager:drawObjects_alternative2 obj.currentTask %d > lastTaskCount %d %s", obj.currentTask, obj.lastTaskCount, tostring(obj.fileName)) + for i = obj.lastTaskCount, obj.currentTask do + -- ADDrawingManager.debugMsg(nil, "ADDrawingManager:drawObjects_alternative2 setVisibility itemID %s ", tostring(itemID)) + if obj.itemIDs[i] ~= nil then + setVisibility(obj.itemIDs[i], true) + end + end + else + -- number of visible items not changed + -- if (g_updateLoopIndex % 60 == 0) then + -- ADDrawingManager.debugMsg(nil, "ADDrawingManager:drawObjects_alternative2 obj.currentTask not changed %s", tostring(obj.fileName)) + -- end + end + end + + +-- ADDrawingManager.debugMsg(nil, "ADDrawingManager:drawObjects_alternative2 drawing... ") + -- drawing tasks + local index = 1 + for _, task in pairs(obj.tasks) do + local itemId = obj.itemIDs[index] +-- ADDrawingManager.debugMsg(nil, "ADDrawingManager:drawObjects_alternative2 index %s itemId %s ", tostring(index), tostring(itemId)) + if task.taskChanged then + -- ADDrawingManager.debugMsg(nil, "ADDrawingManager:drawObjects_alternative2 dFunc %s", tostring(obj.fileName)) + dFunc(self, itemId, task) + task.taskChanged = false + end + index = index + 1 + end +-- ADDrawingManager.debugMsg(nil, "ADDrawingManager:drawObjects_alternative2 end ") + obj.lastTaskCount = obj.currentTask + obj.currentTask = 0 + return stats +end + function ADDrawingManager:drawObjects_alternative(obj, dFunc, iFunc) --- AutoDrive.debugMsg(nil, "ADDrawingManager:drawObjects start...") +-- AutoDrive.debugMsg(nil, "ADDrawingManager:drawObjects_alternative start...") local taskCount = #obj.tasks local stats = {} stats["Tasks"] = {Total = taskCount} @@ -191,13 +458,13 @@ function ADDrawingManager:drawObjects_alternative(obj, dFunc, iFunc) if taskCount > 0 or obj.lastDrawZero == false then -- set all invisible for _, itemID in ipairs (obj.itemIDs) do - -- AutoDrive.debugMsg(nil, "ADDrawingManager:drawObjects setVisibility itemID %s ", tostring(itemID)) + -- AutoDrive.debugMsg(nil, "ADDrawingManager:drawObjects_alternative setVisibility itemID %s ", tostring(itemID)) setVisibility(itemID, false) end end if taskCount == 0 then -- nothing to draw --- AutoDrive.debugMsg(nil, "ADDrawingManager:drawObjects nothing to draw ") +-- AutoDrive.debugMsg(nil, "ADDrawingManager:drawObjects_alternative nothing to draw ") obj.lastDrawZero = true return stats end @@ -217,9 +484,9 @@ function ADDrawingManager:drawObjects_alternative(obj, dFunc, iFunc) else fileToUse = obj.fileName end --- AutoDrive.debugMsg(nil, "ADDrawingManager:drawObjects fileToUse %s ", tostring(fileToUse)) +-- AutoDrive.debugMsg(nil, "ADDrawingManager:drawObjects_alternative fileToUse %s ", tostring(fileToUse)) --- AutoDrive.debugMsg(nil, "ADDrawingManager:drawObjects taskCount %s #obj.itemIDs %s ", tostring(taskCount), tostring(#obj.itemIDs)) +-- AutoDrive.debugMsg(nil, "ADDrawingManager:drawObjects_alternative taskCount %s #obj.itemIDs %s ", tostring(taskCount), tostring(#obj.itemIDs)) -- check if enougth objects are available - add missing amount if taskCount > #obj.itemIDs then for i = 1, taskCount - #obj.itemIDs do @@ -230,19 +497,19 @@ function ADDrawingManager:drawObjects_alternative(obj, dFunc, iFunc) table.insert(obj.itemIDs,iFunc(itemID)) end end --- AutoDrive.debugMsg(nil, "ADDrawingManager:drawObjects taskCount %s #obj.itemIDs %s ", tostring(taskCount), tostring(#obj.itemIDs)) +-- AutoDrive.debugMsg(nil, "ADDrawingManager:drawObjects_alternative taskCount %s #obj.itemIDs %s ", tostring(taskCount), tostring(#obj.itemIDs)) --- AutoDrive.debugMsg(nil, "ADDrawingManager:drawObjects drawing... ") +-- AutoDrive.debugMsg(nil, "ADDrawingManager:drawObjects_alternative drawing... ") -- drawing tasks local index = 1 for _, task in pairs(obj.tasks) do local itemId = obj.itemIDs[index] --- AutoDrive.debugMsg(nil, "ADDrawingManager:drawObjects index %s itemId %s ", tostring(index), tostring(itemId)) +-- AutoDrive.debugMsg(nil, "ADDrawingManager:drawObjects_alternative index %s itemId %s ", tostring(index), tostring(itemId)) dFunc(self, itemId, task) index = index + 1 end obj.tasks = {} --- AutoDrive.debugMsg(nil, "ADDrawingManager:drawObjects end ") +-- AutoDrive.debugMsg(nil, "ADDrawingManager:drawObjects_alternative end ") obj.lastDrawZero = taskCount == 0 return stats end @@ -328,7 +595,8 @@ function ADDrawingManager:drawObjects_original(obj, dFunc, iFunc) end function ADDrawingManager:drawObjects(obj, dFunc, iFunc) - return self:drawObjects_alternative(obj, dFunc, iFunc) + return self:drawObjects_alternative2(obj, dFunc, iFunc) + -- return self:drawObjects_alternative(obj, dFunc, iFunc) -- return self:drawObjects_original(obj, dFunc, iFunc) end @@ -356,9 +624,6 @@ function ADDrawingManager:drawLine(id, task) -- Update line color setShaderParameter(id, "lineColor", task.r, task.g, task.b, self.emittivity, false) - - -- Update line visibility - setVisibility(id, true) end function ADDrawingManager:drawArrow(id, task) @@ -395,32 +660,31 @@ function ADDrawingManager:drawArrow(id, task) -- Update arrow color setShaderParameter(id, "lineColor", task.r, task.g, task.b, self.emittivity, false) - - -- Update arrow visibility - setVisibility(id, true) end function ADDrawingManager:drawSmallSphere(id, task) setTranslation(id, task.x, task.y + self.yOffset, task.z) setShaderParameter(id, "lineColor", task.r, task.g, task.b, self.emittivity, false) - setVisibility(id, true) end function ADDrawingManager:drawMarker(id, task) setTranslation(id, task.x, task.y + self.yOffset, task.z) - setVisibility(id, true) end function ADDrawingManager:drawCross(id, task) setTranslation(id, task.x, task.y + self.yOffset, task.z) local scaleLines = (AutoDrive.getSetting("scaleLines") or 1) * (task.scale or 1) setScale(id, scaleLines, scaleLines, scaleLines) - setVisibility(id, true) end function ADDrawingManager:drawSphere(id, task) setTranslation(id, task.x, task.y + self.yOffset, task.z) setScale(id, task.scale, task.scale, task.scale) setShaderParameter(id, "lineColor", task.r, task.g, task.b, self.emittivity + task.a, false) - setVisibility(id, true) +end + +function ADDrawingManager.debugMsg(vehicle, debugText, ...) + if ADDrawingManager.enableDebug == true then + AutoDrive.debugMsg(vehicle, debugText, ...) + end end diff --git a/scripts/Manager/GraphManager.lua b/scripts/Manager/GraphManager.lua index 7f761fb2..0af1154a 100644 --- a/scripts/Manager/GraphManager.lua +++ b/scripts/Manager/GraphManager.lua @@ -120,10 +120,12 @@ function ADGraphManager:setMapMarkers(mapMarkers) -- create debug markers, debug markers are not saved, so no need to delete them or update map hotspots required -- notifyDestinationListeners is called from caller function -> argument is false self:createDebugMarkers(false) + self:markChanges() end function ADGraphManager:setMapMarker(mapMarker) self.mapMarkers[mapMarker.markerIndex] = mapMarker + self:markChanges() end function ADGraphManager:getPathTo(vehicle, waypointId, startPoint) @@ -485,6 +487,9 @@ function ADGraphManager:removeMapMarker(markerId, sendEvent) AutoDriveDeleteMapMarkerEvent.sendEvent(markerId) else if self.mapMarkers[markerId] ~= nil then + -- remove deleted marker from vehicle destinations + ADGraphManager:checkResetVehicleDestinations(markerId) + table.remove(self.mapMarkers, markerId) --Readjust stored markerIndex values to point to corrected ID for markerID, marker in pairs(self.mapMarkers) do @@ -528,8 +533,6 @@ function ADGraphManager:removeMapMarker(markerId, sendEvent) end end end - -- remove deleted marker from vehicle destinations - ADGraphManager:checkResetVehicleDestinations(markerId) end -- Calling external interop listeners @@ -1137,6 +1140,7 @@ function ADGraphManager:removeDebugMarkers() end end end + self:markChanges() end -- create debug markers for waypoints issues @@ -1230,7 +1234,7 @@ function ADGraphManager:createDebugMarkers(updateMap) -- mark wayPoint without incoming connection if not wp.foundError and wp.out ~= nil then for _, wp_out in pairs(wp.out) do - local missingIncoming = self:checkForMissingIncoming(wp, self:getWayPointById(wp_out)) + local missingIncoming = self:checkForMissingIncoming(wp) if missingIncoming then local debugMapMarkerName = "3_" .. tostring(count3) @@ -1298,6 +1302,7 @@ function ADGraphManager:createDebugMarkers(updateMap) if shouldUpdateMap == true then AutoDrive:notifyDestinationListeners() end + self:markChanges() end function ADGraphManager:checkForWrongReverseStart(wp_ref, wp_current, wp_ahead) diff --git a/scripts/Manager/HarvestManager.lua b/scripts/Manager/HarvestManager.lua index f5249885..a655a3a8 100644 --- a/scripts/Manager/HarvestManager.lua +++ b/scripts/Manager/HarvestManager.lua @@ -1,4 +1,5 @@ ADHarvestManager = {} +ADHarvestManager.debug = false ADHarvestManager.MAX_PREDRIVE_LEVEL = 0.85 ADHarvestManager.MAX_SEARCH_RANGE = 300 @@ -12,9 +13,9 @@ function ADHarvestManager:load() end function ADHarvestManager:registerHarvester(harvester) - AutoDrive.debugPrint(harvester, AutoDrive.DC_COMBINEINFO, "ADHarvestManager:registerHarvester") + ADHarvestManager.debugMsg(harvester, "ADHarvestManager:registerHarvester") if not table.contains(self.idleHarvesters, harvester) and not table.contains(self.harvesters, harvester) then - AutoDrive.debugPrint(harvester, AutoDrive.DC_COMBINEINFO, "ADHarvestManager:registerHarvester - inserted") + ADHarvestManager.debugMsg(harvester, "ADHarvestManager:registerHarvester - inserted") if harvester ~= nil and harvester.ad ~= nil then local rootVehicle = harvester:getRootVehicle() rootVehicle.ad.isRegisterdHarvester = true @@ -26,7 +27,7 @@ function ADHarvestManager:registerHarvester(harvester) end function ADHarvestManager:unregisterHarvester(harvester) - AutoDrive.debugPrint(harvester, AutoDrive.DC_COMBINEINFO, "ADHarvestManager:unregisterHarvester") + ADHarvestManager.debugMsg(harvester, "ADHarvestManager:unregisterHarvester") if harvester ~= nil and harvester.ad ~= nil then local rootVehicle = harvester:getRootVehicle() rootVehicle.ad.isRegisterdHarvester = false @@ -35,22 +36,22 @@ function ADHarvestManager:unregisterHarvester(harvester) if table.contains(self.idleHarvesters, harvester) then local index = table.indexOf(self.idleHarvesters, harvester) local harvester = table.remove(self.idleHarvesters, index) - AutoDrive.debugPrint(harvester, AutoDrive.DC_COMBINEINFO, "ADHarvestManager:unregisterHarvester - removed - idleHarvesters") + ADHarvestManager.debugMsg(harvester, "ADHarvestManager:unregisterHarvester - removed - idleHarvesters") end if table.contains(self.harvesters, harvester) then local index = table.indexOf(self.harvesters, harvester) local harvester = table.remove(self.harvesters, index) - AutoDrive.debugPrint(harvester, AutoDrive.DC_COMBINEINFO, "ADHarvestManager:unregisterHarvester - removed - harvesters") + ADHarvestManager.debugMsg(harvester, "ADHarvestManager:unregisterHarvester - removed - harvesters") end end end function ADHarvestManager:registerAsUnloader(vehicle) - AutoDrive.debugPrint(vehicle, AutoDrive.DC_COMBINEINFO, "ADHarvestManager:registerAsUnloader") + ADHarvestManager.debugMsg(vehicle, "ADHarvestManager:registerAsUnloader") --remove from active and idle list self:unregisterAsUnloader(vehicle) if not table.contains(self.idleUnloaders, vehicle) then - AutoDrive.debugPrint(vehicle, AutoDrive.DC_COMBINEINFO, "ADHarvestManager:registerAsUnloader - inserted") + ADHarvestManager.debugMsg(vehicle, "ADHarvestManager:registerAsUnloader - inserted") table.insert(self.idleUnloaders, vehicle) end end @@ -79,14 +80,20 @@ function ADHarvestManager:unregisterAsUnloader(vehicle) end end -function ADHarvestManager:fireUnloader(unloader) +function ADHarvestManager:fireUnloader(unloader, harvester) if unloader.ad.stateModule:isActive() then local follower = unloader.ad.modes[AutoDrive.MODE_UNLOAD]:getFollowingUnloader() if follower ~= nil then follower.ad.taskModule:abortAllTasks() + if AutoDrive.getSetting("parkInField", follower) then + follower.ad.taskModule:addTask(ClearCropTask:new(follower, harvester)) + end follower.ad.taskModule:addTask(StopAndDisableADTask:new(follower, ADTaskModule.DONT_PROPAGATE, true)) end unloader.ad.taskModule:abortAllTasks() + if AutoDrive.getSetting("parkInField", unloader) then + unloader.ad.taskModule:addTask(ClearCropTask:new(unloader, harvester)) + end unloader.ad.taskModule:addTask(StopAndDisableADTask:new(unloader, ADTaskModule.DONT_PROPAGATE, true)) end self:unregisterAsUnloader(unloader) @@ -115,7 +122,7 @@ function ADHarvestManager:update(dt) local unloader = self:getAssignedUnloader(harvester) if unloader ~= nil then - self:fireUnloader(unloader) + self:fireUnloader(unloader, harvester) end end end @@ -130,7 +137,7 @@ function ADHarvestManager:update(dt) else if AutoDrive.getSetting("callSecondUnloader", harvester) then local unloader = self:getAssignedUnloader(harvester) - if unloader.ad.modes[AutoDrive.MODE_UNLOAD]:getFollowingUnloader() == nil then + if unloader and unloader.ad and unloader.ad.modes and unloader.ad.modes[AutoDrive.MODE_UNLOAD]:getFollowingUnloader() == nil then local trailers, _ = AutoDrive.getAllUnits(unloader) local fillLevel, _, _, fillFreeCapacity = AutoDrive.getAllFillLevels(trailers) @@ -207,7 +214,6 @@ end function ADHarvestManager.doesHarvesterNeedUnloading(harvester, ignorePipe) local ret = false - local _, maxCapacity, _, leftCapacity = AutoDrive.getObjectFillLevels(harvester) local cpIsCalling = AutoDrive:getIsCPWaitingForUnload(harvester) @@ -215,18 +221,19 @@ function ADHarvestManager.doesHarvesterNeedUnloading(harvester, ignorePipe) ret = ( ( ( - (maxCapacity > 0 and leftCapacity / maxCapacity < 0.2) - -- (maxCapacity > 0 and leftCapacity < 1.0) - or cpIsCalling ) - and - -- (pipeOut or ignorePipe) - (pipeOut or (ignorePipe == true)) + or + (pipeOut) ) and harvester.ad.noMovementTimer.elapsedTime > 5000 ) + ADHarvestManager.debugMsg(harvester, "ADHarvestManager.doesHarvesterNeedUnloading cpIsCalling %s pipeOut %s noMovementTimer %s" + , tostring(cpIsCalling) + , tostring(pipeOut) + , tostring(harvester.ad.noMovementTimer.elapsedTime > 5000) + ) return ret end @@ -265,6 +272,7 @@ function ADHarvestManager.isHarvesterActive(harvester) if manuallyControlled then return AutoDrive.isPipeOut(harvester) end + ADHarvestManager.debugMsg(harvester, "ADHarvestManager.isHarvesterActive reachedPreCallLevel %s isAlmostFull %s allowedToChase %s", reachedPreCallLevel, isAlmostFull, allowedToChase) return reachedPreCallLevel and (not isAlmostFull) and allowedToChase end @@ -375,3 +383,11 @@ function ADHarvestManager.getOpenPipePercent(harvester) end return openPipe, pipePercent end + +function ADHarvestManager.debugMsg(vehicle, debugText, ...) + if ADHarvestManager.debug == true then + AutoDrive.debugMsg(vehicle, debugText, ...) + else + AutoDrive.debugPrint(vehicle, AutoDrive.DC_COMBINEINFO, debugText, ...) + end +end diff --git a/scripts/Manager/InputManager.lua b/scripts/Manager/InputManager.lua index ff762d8a..23aa01e6 100644 --- a/scripts/Manager/InputManager.lua +++ b/scripts/Manager/InputManager.lua @@ -262,13 +262,14 @@ function ADInputManager:input_start_stop(vehicle, farmId) end if vehicle.ad.stateModule:isActive() then vehicle.ad.isStoppingWithError = true + vehicle.ad.stateModule:setLoopsDone(0) vehicle:stopAutoDrive() else if farmId and farmId > 0 then -- set the farmId for the vehicle controlled by AD vehicle.ad.stateModule:setActualFarmId(farmId) -- input_start_stop end - + vehicle.ad.stateModule:setLoopsDone(0) vehicle.ad.stateModule:getCurrentMode():start(AutoDrive.USER_PLAYER) if AutoDrive.rightSHIFTmodifierKeyPressed then @@ -278,6 +279,7 @@ function ADInputManager:input_start_stop(vehicle, farmId) if otherVehicle.ad.stateModule.activeBeforeSave then -- g_currentMission:requestToEnterVehicle(otherVehicle) + otherVehicle.ad.stateModule:setLoopsDone(0) otherVehicle.ad.stateModule:getCurrentMode():start(AutoDrive.USER_PLAYER) end if otherVehicle.ad.stateModule.AIVEActiveBeforeSave and otherVehicle.acParameters ~= nil then diff --git a/scripts/Manager/Scheduler.lua b/scripts/Manager/Scheduler.lua index 27d87f2b..f708834b 100644 --- a/scripts/Manager/Scheduler.lua +++ b/scripts/Manager/Scheduler.lua @@ -2,7 +2,7 @@ ADScheduler = {} ADScheduler.UPDATE_TIME = 3000 -- time interval for calculation/check ADScheduler.FRAMES_TO_CHECK_FOR_ACTUAL_FPS = 10 -- number of frames to calculate actual FPS ADScheduler.FRAMES_TO_CHECK_FOR_AVERAGE_FPS = 60 * 60 -- number of frames to calculate average FPS -ADScheduler.MIN_STEPS_PER_FRAME = 8 -- min steps for pathfinder per frame +ADScheduler.MIN_STEPS_PER_FRAME = 2 -- min steps for pathfinder per frame ADScheduler.MAX_STEPS_PER_FRAME = 8 -- max steps for pathfinder per frame ADScheduler.FPS_DIFFERENCE = 0.1 -- 10 % difference to average for calculation/check ADScheduler.MIN_FPS = 20 -- min FPS where stepsPerFrame will always be decreased @@ -108,9 +108,9 @@ function ADScheduler:addPathfinderVehicle(vehicle) table.insert(self.pathFinderVehicles, vehicle) -- set new vehicle a delay if table.count(self.pathFinderVehicles) > 1 then - AutoDrive.debugPrint(vehicle, AutoDrive.DC_PATHINFO, "Scheduler addPathfinderVehicle addDelayTimer 10000") + AutoDrive.debugPrint(vehicle, AutoDrive.DC_PATHINFO, "Scheduler addPathfinderVehicle addDelayTimer 5000") -- if already vehicle in table, pause this new one - vehicle.ad.pathFinderModule:addDelayTimer(10000) + vehicle.ad.pathFinderModule:addDelayTimer(5000) end end end @@ -120,25 +120,37 @@ function ADScheduler:removePathfinderVehicle(vehicle) if table.contains(self.pathFinderVehicles, vehicle) then AutoDrive.debugPrint(vehicle, AutoDrive.DC_PATHINFO, "Scheduler removePathfinderVehicle") table.removeValue(self.pathFinderVehicles, vehicle) + if self.activePathFinderVehicle == vehicle then + self.updateTimer:timer(true, ADScheduler.UPDATE_TIME, ADScheduler.UPDATE_TIME) -- shoutcut timer + end end end function ADScheduler:updateActiveVehicle() - if self.activePathFinderVehicle ~= nil then - -- pause current vehicle - self.activePathFinderVehicle.ad.pathFinderModule:addDelayTimer(10000) + -- pause all vehicles + for _, vehicle in pairs(self.pathFinderVehicles) do + if vehicle and vehicle.ad and vehicle.ad.pathFinderModule then + vehicle.ad.pathFinderModule:addDelayTimer(5000) + end + end + if self.activePathFinderVehicle and table.contains(self.pathFinderVehicles, self.activePathFinderVehicle) then + -- add this vehicle to end of queue + local vehicle = self.activePathFinderVehicle + self.activePathFinderVehicle = nil + self:removePathfinderVehicle(vehicle) + self:addPathfinderVehicle(vehicle) end -- get next vehicle - self.activePathFinderVehicle = self.pathFinderVehicles[1] + local nextVehicle = self.pathFinderVehicles[1] - if self.activePathFinderVehicle ~= nil then - AutoDrive.debugPrint(self.activePathFinderVehicle, AutoDrive.DC_PATHINFO, "Scheduler updateActiveVehicle activePathFinderVehicle") + if nextVehicle ~= nil then + AutoDrive.debugPrint(nextVehicle, AutoDrive.DC_PATHINFO, "Scheduler updateActiveVehicle activate nextVehicle") -- found vehicle - -- add this vehicle to end of queue - self:removePathfinderVehicle(self.activePathFinderVehicle) - self:addPathfinderVehicle(self.activePathFinderVehicle) -- unpause vehicle to continue pathfinder calculation - self.activePathFinderVehicle.ad.pathFinderModule:addDelayTimer(0) + if nextVehicle.ad and nextVehicle.ad.pathFinderModule then + nextVehicle.ad.pathFinderModule:addDelayTimer(0) + end + self.activePathFinderVehicle = nextVehicle end end diff --git a/scripts/Manager/TriggerManager.lua b/scripts/Manager/TriggerManager.lua index fbdb4595..5cdcdd41 100644 --- a/scripts/Manager/TriggerManager.lua +++ b/scripts/Manager/TriggerManager.lua @@ -382,7 +382,7 @@ function ADTriggerManager:getBestPickupLocationFor(vehicle, trailer, fillType) local aifillTypes = loadingStation:getAISupportedFillTypes() if aifillTypes[fillType] and loadingStation:getFillLevel(fillType, farmId) > 0 then if loadingStation.getAITargetPositionAndDirection ~= nil then - x, z, xDir, zDir = loadingStation:getAITargetPositionAndDirection(FillType.UNKNOWN) + local x, z, xDir, zDir = loadingStation:getAITargetPositionAndDirection(FillType.UNKNOWN) table.insert(validLoadingStations, loadingStation) end diff --git a/scripts/Modes/CombineUnloaderMode.lua b/scripts/Modes/CombineUnloaderMode.lua index e90529bb..59f79e3e 100644 --- a/scripts/Modes/CombineUnloaderMode.lua +++ b/scripts/Modes/CombineUnloaderMode.lua @@ -1,4 +1,5 @@ CombineUnloaderMode = ADInheritsFrom(AbstractMode) +CombineUnloaderMode.debug = false CombineUnloaderMode.STATE_INIT = {} CombineUnloaderMode.STATE_WAIT_TO_BE_CALLED = {} @@ -43,7 +44,7 @@ function CombineUnloaderMode:reset() end function CombineUnloaderMode:start(user) - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:start start self.state %s", tostring(self:getStateName())) + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:start start self.state %s", tostring(self:getStateName())) if not self.vehicle.ad.stateModule:isActive() then self.vehicle:startAutoDrive() end @@ -58,13 +59,13 @@ function CombineUnloaderMode:start(user) if self.activeTask ~= nil then self.vehicle.ad.taskModule:addTask(self.activeTask) end - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:start end self.state %s", tostring(self:getStateName())) + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:start end self.state %s", tostring(self:getStateName())) end function CombineUnloaderMode:monitorTasks(dt) if self.combine ~= nil and (self.state == self.STATE_DRIVE_TO_START or self.state == self.STATE_DRIVE_TO_UNLOAD or self.state == self.STATE_EXIT_FIELD) then if AutoDrive.getDistanceBetween(self.vehicle, self.combine) > 25 then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:monitorTasks -> unregisterAsUnloader") + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:monitorTasks -> unregisterAsUnloader") ADHarvestManager:unregisterAsUnloader(self.vehicle) self.followingUnloader = nil self.combine = nil @@ -75,7 +76,7 @@ function CombineUnloaderMode:monitorTasks(dt) end --We are stuck if self.failedPathFinder >= 5 or ((self.vehicle.ad.specialDrivingModule:shouldStopMotor() or self.vehicle.ad.specialDrivingModule.stoppedTimer:done()) and self.vehicle.ad.specialDrivingModule.isBlocked) then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:monitorTasks() - detected stuck vehicle - try reversing out of it now") + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:monitorTasks() - detected stuck vehicle - try reversing out of it now") self.vehicle.ad.specialDrivingModule:releaseVehicle() self.vehicle.ad.taskModule:abortAllTasks() self.activeTask = ReverseFromBadLocationTask:new(self.vehicle) @@ -90,7 +91,7 @@ function CombineUnloaderMode:monitorTasks(dt) end function CombineUnloaderMode:notifyAboutFailedPathfinder() - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:notifyAboutFailedPathfinder self.failedPathFinder %s ADGraphManager:getDistanceFromNetwork(self.vehicle) > 20 %s", tostring(self.failedPathFinder), tostring(ADGraphManager:getDistanceFromNetwork(self.vehicle) > 20)) + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:notifyAboutFailedPathfinder self.failedPathFinder %s ADGraphManager:getDistanceFromNetwork(self.vehicle) > 20 %s", tostring(self.failedPathFinder), tostring(ADGraphManager:getDistanceFromNetwork(self.vehicle) > 20)) --print("CombineUnloaderMode:notifyAboutFailedPathfinder() - blocked: " .. tostring(self.vehicle.ad.pathFinderModule.completelyBlocked) .. " distance: " .. ADGraphManager:getDistanceFromNetwork(self.vehicle)) if self.vehicle.ad.pathFinderModule.completelyBlocked and ADGraphManager:getDistanceFromNetwork(self.vehicle) > 20 then self.failedPathFinder = self.failedPathFinder + 1 @@ -134,14 +135,14 @@ function CombineUnloaderMode:promoteFollowingUnloader(combine) end function CombineUnloaderMode:handleFinishedTask() - -- AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:handleFinishedTask") + -- CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:handleFinishedTask") self.vehicle.ad.trailerModule:reset() self.lastTask = self.activeTask self.activeTask = self:getNextTask() if self.activeTask ~= nil then self.vehicle.ad.taskModule:addTask(self.activeTask) end - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:handleFinishedTask self.state %s", tostring(self:getStateName())) + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:handleFinishedTask self.state %s", tostring(self:getStateName())) end function CombineUnloaderMode:stop() @@ -163,15 +164,15 @@ function CombineUnloaderMode:continue() end if self.state == self.STATE_DRIVE_TO_UNLOAD then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:continue self.STATE_DRIVE_TO_UNLOAD") + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:continue self.STATE_DRIVE_TO_UNLOAD") self.activeTask:continue() else - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:continue self.state" .. tostring(self:getStateName())) + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:continue self.state" .. tostring(self:getStateName())) self.vehicle.ad.taskModule:abortCurrentTask() if AutoDrive.checkIsOnField(x, y, z) and distanceToStart > 30 then -- is activated on a field - use ExitFieldTask to leave field according to setting - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:continue ExitFieldTask...") + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:continue ExitFieldTask...") self.activeTask = ExitFieldTask:new(self.vehicle) self.state = self.STATE_EXIT_FIELD else @@ -181,7 +182,7 @@ function CombineUnloaderMode:continue() self.vehicle.ad.stateModule:setSecondMarker(nextTarget) end end - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:continue UnloadAtDestinationTask...") + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:continue UnloadAtDestinationTask...") self.activeTask = UnloadAtDestinationTask:new(self.vehicle, self.vehicle.ad.stateModule:getSecondMarker().id) self.state = self.STATE_DRIVE_TO_UNLOAD end @@ -194,7 +195,7 @@ function CombineUnloaderMode:continue() end function CombineUnloaderMode:getNextTask() - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:getNextTask start self.state %s", tostring(self:getStateName())) + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:getNextTask start self.state %s", tostring(self:getStateName())) local nextTask local x, y, z = getWorldTranslation(self.vehicle.components[1].node) @@ -202,7 +203,7 @@ function CombineUnloaderMode:getNextTask() local _, _, filledToUnload, _ = AutoDrive.getAllFillLevels(self.trailers) if self.state == self.STATE_INIT then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:getNextTask STATE_INIT filledToUnload %s", tostring(filledToUnload)) + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:getNextTask STATE_INIT filledToUnload %s", tostring(filledToUnload)) if filledToUnload then -- fill level above setting unload level if (AutoDrive.getSetting("rotateTargets", self.vehicle) == AutoDrive.RT_ONLYDELIVER or AutoDrive.getSetting("rotateTargets", self.vehicle) == AutoDrive.RT_PICKUPANDDELIVER) and AutoDrive.getSetting("useFolders") then local nextTarget = ADMultipleTargetsManager:getNextTarget(self.vehicle, false) @@ -224,9 +225,9 @@ function CombineUnloaderMode:getNextTask() self:setToWaitForCall() end end - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:getNextTask - STATE_INIT end self.state %s", tostring(self:getStateName())) + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:getNextTask - STATE_INIT end self.state %s", tostring(self:getStateName())) elseif self.state == self.STATE_DRIVE_TO_COMBINE then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:getNextTask - STATE_DRIVE_TO_COMBINE") + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:getNextTask - STATE_DRIVE_TO_COMBINE") -- we finished the precall to combine route -- check if we should wait / pull up to combines pipe nextTask = FollowCombineTask:new(self.vehicle, self.combine) @@ -234,14 +235,14 @@ function CombineUnloaderMode:getNextTask() self.breadCrumbs = Queue:new() self.lastBreadCrumb = nil elseif self.state == self.STATE_DRIVE_TO_PIPE or self.state == self.STATE_REVERSE_FROM_BAD_LOCATION then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:getNextTask - STATE_DRIVE_TO_PIPE | STATE_REVERSE_FROM_BAD_LOCATION") + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:getNextTask - STATE_DRIVE_TO_PIPE | STATE_REVERSE_FROM_BAD_LOCATION") --Drive to pipe can be finished when combine is emptied or when vehicle has reached 'old' pipe position and should switch to active mode nextTask = self:getTaskAfterUnload(filledToUnload) elseif self.state == self.STATE_LEAVE_CROP then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:getNextTask - STATE_LEAVE_CROP") + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:getNextTask - STATE_LEAVE_CROP") self:setToWaitForCall() elseif self.state == self.STATE_DRIVE_TO_UNLOAD then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:getNextTask - STATE_DRIVE_TO_UNLOAD") + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:getNextTask - STATE_DRIVE_TO_UNLOAD") nextTask = DriveToDestinationTask:new(self.vehicle, self.vehicle.ad.stateModule:getFirstMarker().id) if (AutoDrive.getSetting("rotateTargets", self.vehicle) == AutoDrive.RT_ONLYDELIVER or AutoDrive.getSetting("rotateTargets", self.vehicle) == AutoDrive.RT_PICKUPANDDELIVER) and AutoDrive.getSetting("useFolders") then local nextTarget = ADMultipleTargetsManager:getNextTarget(self.vehicle, true) @@ -251,10 +252,10 @@ function CombineUnloaderMode:getNextTask() end self.state = self.STATE_DRIVE_TO_START elseif self.state == self.STATE_DRIVE_TO_START then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:getNextTask - STATE_DRIVE_TO_START") + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:getNextTask - STATE_DRIVE_TO_START") self:setToWaitForCall() elseif self.state == self.STATE_ACTIVE_UNLOAD_COMBINE then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:getNextTask - STATE_ACTIVE_UNLOAD_COMBINE") + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:getNextTask - STATE_ACTIVE_UNLOAD_COMBINE") if filledToUnload then nextTask = self:getTaskAfterUnload(filledToUnload) else @@ -262,13 +263,13 @@ function CombineUnloaderMode:getNextTask() self.state = self.STATE_DRIVE_TO_COMBINE end elseif self.state == self.STATE_FOLLOW_CURRENT_UNLOADER then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:getNextTask - STATE_FOLLOW_CURRENT_UNLOADER") + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:getNextTask - STATE_FOLLOW_CURRENT_UNLOADER") if self.targetUnloader ~= nil then self.targetUnloader.ad.modes[AutoDrive.MODE_UNLOAD]:unregisterFollowingUnloader() end nextTask = self:getTaskAfterUnload(filledToUnload) elseif self.state == self.STATE_EXIT_FIELD then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:getNextTask - STATE_EXIT_FIELD") + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:getNextTask - STATE_EXIT_FIELD") if (AutoDrive.getSetting("rotateTargets", self.vehicle) == AutoDrive.RT_ONLYDELIVER or AutoDrive.getSetting("rotateTargets", self.vehicle) == AutoDrive.RT_PICKUPANDDELIVER) and AutoDrive.getSetting("useFolders") then local nextTarget = ADMultipleTargetsManager:getNextTarget(self.vehicle, false) if nextTarget ~= nil then @@ -279,19 +280,19 @@ function CombineUnloaderMode:getNextTask() self.state = self.STATE_DRIVE_TO_UNLOAD end - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:getNextTask end self.state %s", tostring(self:getStateName())) + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:getNextTask end self.state %s", tostring(self:getStateName())) return nextTask end function CombineUnloaderMode:setToWaitForCall(keepCombine) - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:setToWaitForCall start self.state %s keepCombine %s", tostring(self.state), tostring(keepCombine)) + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:setToWaitForCall start self.state %s keepCombine %s", tostring(self:getStateName()), tostring(keepCombine)) -- We just have to wait to be wait to be called (again) self.state = self.STATE_WAIT_TO_BE_CALLED self.vehicle.ad.taskModule:addTask(WaitForCallTask:new(self.vehicle)) if self.combine ~= nil and self.combine.ad ~= nil and (keepCombine == nil or keepCombine ~= true) then self.combine = nil end - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:setToWaitForCall end self.state %s self.combine %s", tostring(self:getStateName()), tostring(self.combine)) + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:setToWaitForCall end self.state %s self.combine %s", tostring(self:getStateName()), tostring(self.combine)) end function CombineUnloaderMode:assignToHarvester(harvester) @@ -336,7 +337,7 @@ end function CombineUnloaderMode:getTaskAfterUnload(filledToUnload) local nextTask - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:getTaskAfterUnload start filledToUnload %s", tostring(filledToUnload)) + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:getTaskAfterUnload start filledToUnload %s", tostring(filledToUnload)) local x, y, z = getWorldTranslation(self.vehicle.components[1].node) local point = nil @@ -358,11 +359,11 @@ function CombineUnloaderMode:getTaskAfterUnload(filledToUnload) --self.combine = nil if AutoDrive.checkIsOnField(x, y, z) and distanceToStart > 30 then -- is activated on a field - use ExitFieldTask to leave field according to setting - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:getTaskAfterUnload ExitFieldTask...") + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:getTaskAfterUnload ExitFieldTask...") nextTask = ExitFieldTask:new(self.vehicle) self.state = self.STATE_EXIT_FIELD else - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:getTaskAfterUnload UnloadAtDestinationTask...") + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:getTaskAfterUnload UnloadAtDestinationTask...") nextTask = UnloadAtDestinationTask:new(self.vehicle, self.vehicle.ad.stateModule:getSecondMarker().id) self.state = self.STATE_DRIVE_TO_UNLOAD end @@ -371,11 +372,11 @@ function CombineUnloaderMode:getTaskAfterUnload(filledToUnload) if (self.combine and self.combine.ad.isChopper) or (AutoDrive.getSetting("parkInField", self.vehicle) or (self.lastTask ~= nil and self.lastTask.stayOnField)) then -- If we are in fruit, we should clear it if AutoDrive.isVehicleOrTrailerInCrop(self.vehicle, true) then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:getTaskAfterUnload ClearCropTask...") - nextTask = ClearCropTask:new(self.vehicle) + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:getTaskAfterUnload ClearCropTask...") + nextTask = ClearCropTask:new(self.vehicle, self.combine) self.state = self.STATE_LEAVE_CROP else - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:getTaskAfterUnload setToWaitForCall") + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:getTaskAfterUnload setToWaitForCall") self:setToWaitForCall() end else @@ -384,7 +385,7 @@ function CombineUnloaderMode:getTaskAfterUnload(filledToUnload) self.state = self.STATE_DRIVE_TO_START end end - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:getTaskAfterUnload end") + CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:getTaskAfterUnload end") return nextTask end @@ -543,18 +544,18 @@ function CombineUnloaderMode:getPipeChaseWayPoint(offsetX, offsetZ) local wayPoint = {x = 0, y = 0, z = 0} local trailer = g_currentMission.nodeToObject[self.targetFillNode] if trailer == nil then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "ERROR: CombineUnloaderMode:getPipeChaseWayPoint trailer == nil") + CombineUnloaderMode.debugMsg(self.vehicle, "ERROR: CombineUnloaderMode:getPipeChaseWayPoint trailer == nil") trailer = self.vehicle end local _, trailerDistY, _ = localToLocal(self.targetFillNode, trailer.components[1].node, 0, 0, 0) local dischargeNode = AutoDrive.getDischargeNode(self.combine) if dischargeNode == nil then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "ERROR: CombineUnloaderMode:getPipeChaseWayPoint dischargeNode == nil") + CombineUnloaderMode.debugMsg(self.vehicle, "ERROR: CombineUnloaderMode:getPipeChaseWayPoint dischargeNode == nil") dischargeNode = self.combine.components[1].node end local combinePipeRootNode = AutoDrive.getPipeRoot(self.combine) if combinePipeRootNode == nil then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "ERROR: CombineUnloaderMode:getPipeChaseWayPoint combinePipeRootNode == nil") + CombineUnloaderMode.debugMsg(self.vehicle, "ERROR: CombineUnloaderMode:getPipeChaseWayPoint combinePipeRootNode == nil") combinePipeRootNode = self.combine.components[1].node end local targetX, targetY, targetZ = getWorldTranslation(dischargeNode) @@ -658,11 +659,13 @@ function CombineUnloaderMode:getRearChaseOffsetX(leftBlocked, rightBlocked) end end -function CombineUnloaderMode:getRearChaseOffsetZ() +function CombineUnloaderMode:getRearChaseOffsetZ(planningPhase) local followDistance = AutoDrive.getSetting("followDistance", self.vehicle) local rearChaseOffset = -followDistance - (self.combine.size.length / 2) if self.combine.ad.isAutoAimingChopper and not self.combine.ad.isSugarcaneHarvester then - rearChaseOffset = -followDistance - (self.combine.size.length / 2) + if planningPhase then + rearChaseOffset = - (self.combine.size.length / 2) + end else -- math.sqrt(2) ensures the trailer could straighten if it was turned 90 degrees, and it makes this point further -- back than the pathfinder (straightening) target in PathFinderModule:startPathPlanningToPipe @@ -713,11 +716,11 @@ function CombineUnloaderMode:getPipeChasePosition(planningPhase) local sideChaseTermX = self:getSideChaseOffsetX() local sideChaseTermZ = self:getSideChaseOffsetZ(AutoDrive.dynamicChaseDistance or self.combine.ad.isHarvester) - local rearChaseTermZ = self:getRearChaseOffsetZ() + local rearChaseTermZ = self:getRearChaseOffsetZ(planningPhase) if self.combine.ad.isChopper then -- any chopper - --AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:getPipeChasePosition=IsBufferCombine") + --CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:getPipeChasePosition=IsBufferCombine") local leftChasePos = AutoDrive.createWayPointRelativeToVehicle(self.combine, sideChaseTermX + self:getPipeSlopeCorrection(), sideChaseTermZ - 2) local rightChasePos = AutoDrive.createWayPointRelativeToVehicle(self.combine, -(sideChaseTermX + self:getPipeSlopeCorrection()), sideChaseTermZ - 2) local rearChasePos = AutoDrive.createWayPointRelativeToVehicle(self.combine, 0, rearChaseTermZ) @@ -754,7 +757,7 @@ function CombineUnloaderMode:getPipeChasePosition(planningPhase) end else -- harveser with bunker - --AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CombineUnloaderMode:getPipeChasePosition:IsNormalCombine") + --CombineUnloaderMode.debugMsg(self.vehicle, "CombineUnloaderMode:getPipeChasePosition:IsNormalCombine") local rearChaseTermX = self:getRearChaseOffsetX(leftBlocked, rightBlocked) local sideChasePos = AutoDrive.createWayPointRelativeToVehicle(self.combine, self.pipeSide * (sideChaseTermX + self:getPipeSlopeCorrection()), sideChaseTermZ) @@ -873,5 +876,7 @@ end function CombineUnloaderMode.debugMsg(vehicle, debugText, ...) if CombineUnloaderMode.debug == true then AutoDrive.debugMsg(vehicle, debugText, ...) + else + AutoDrive.debugPrint(vehicle, AutoDrive.DC_COMBINEINFO, debugText, ...) end end diff --git a/scripts/Modes/PickupAndDeliverMode.lua b/scripts/Modes/PickupAndDeliverMode.lua index 89632306..f967fdc1 100644 --- a/scripts/Modes/PickupAndDeliverMode.lua +++ b/scripts/Modes/PickupAndDeliverMode.lua @@ -229,18 +229,21 @@ function PickupAndDeliverMode:getNextTask(forced) elseif self.state == PickupAndDeliverMode.STATE_RETURN_TO_START then -- job done - stop AD and tasks AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_PATHINFO, "PickupAndDeliverMode:getNextTask STATE_RETURN_TO_START StopAndDisableADTask...") + self.vehicle.ad.stateModule:setLoopsDone(0) nextTask = StopAndDisableADTask:new(self.vehicle, ADTaskModule.DONT_PROPAGATE) self.state = PickupAndDeliverMode.STATE_FINISHED AutoDriveMessageEvent.sendMessageOrNotification(self.vehicle, ADMessagesManager.messageTypes.INFO, "$l10n_AD_Driver_of; %s $l10n_AD_has_reached; %s", 5000, self.vehicle.ad.stateModule:getName(), self.vehicle.ad.stateModule:getFirstMarkerName()) elseif self.state == PickupAndDeliverMode.STATE_PARK then -- job done - drive to park position and stop AD and tasks AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_PATHINFO, "PickupAndDeliverMode:getNextTask STATE_RETURN_TO_START StopAndDisableADTask...") + self.vehicle.ad.stateModule:setLoopsDone(0) nextTask = StopAndDisableADTask:new(self.vehicle, ADTaskModule.DONT_PROPAGATE) self.state = PickupAndDeliverMode.STATE_FINISHED -- message for reached park position send by ParkTask as only there the correct destination is known else -- error path - should never appear! AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_PATHINFO, "PickupAndDeliverMode:getNextTask self.state %s NO nextTask assigned !!!", tostring(self.state)) + self.vehicle.ad.stateModule:setLoopsDone(0) nextTask = StopAndDisableADTask:new(self.vehicle, ADTaskModule.DONT_PROPAGATE) self.state = PickupAndDeliverMode.STATE_FINISHED AutoDriveMessageEvent.sendMessageOrNotification(self.vehicle, ADMessagesManager.messageTypes.ERROR, "$l10n_AD_Driver_of; %s $l10n_AD_has_reached; %s", 5000, self.vehicle.ad.stateModule:getName(), "???") diff --git a/scripts/Modules/CollisionDetectionModule.lua b/scripts/Modules/CollisionDetectionModule.lua index 7439dfeb..4d2232a3 100644 --- a/scripts/Modules/CollisionDetectionModule.lua +++ b/scripts/Modules/CollisionDetectionModule.lua @@ -56,6 +56,7 @@ function ADCollisionDetectionModule:detectObstacle() boundingBox[2] = box.topRight boundingBox[3] = box.downRight boundingBox[4] = box.downLeft + boundingBox[1].y = box.y self.detectedCollision = AutoDrive:checkForVehicleCollision(self.vehicle, boundingBox, excludedList) end diff --git a/scripts/Modules/DrivePathModule.lua b/scripts/Modules/DrivePathModule.lua index de5cd0b2..150d175a 100644 --- a/scripts/Modules/DrivePathModule.lua +++ b/scripts/Modules/DrivePathModule.lua @@ -215,7 +215,7 @@ function ADDrivePathModule:isCloseToWaypoint() angle = math.abs(angle) local isReverseStart = wp_ahead.incoming ~= nil and (not table.contains(wp_ahead.incoming, wp_current.id)) - if angle >= 90 and not isReverseStart then + if angle >= 135 and not isReverseStart then return true end end diff --git a/scripts/Modules/PathFinderModule.lua b/scripts/Modules/PathFinderModule.lua index 232fd50e..64e0ad1a 100644 --- a/scripts/Modules/PathFinderModule.lua +++ b/scripts/Modules/PathFinderModule.lua @@ -57,6 +57,7 @@ This is inclusive of the field border cells! ]] PathFinderModule = {} +PathFinderModule.debug = false PathFinderModule.PATHFINDER_MAX_RETRIES = 3 PathFinderModule.MAX_PATHFINDER_STEPS_PER_FRAME = 2 @@ -68,27 +69,7 @@ PathFinderModule.PATHFINDER_TARGET_DISTANCE_PIPE = 16 PathFinderModule.PATHFINDER_TARGET_DISTANCE_PIPE_CLOSE = 6 PathFinderModule.PATHFINDER_START_DISTANCE = 7 PathFinderModule.MAX_FIELDBORDER_CELLS = 5 - -PathFinderModule.PP_UP = 0 -PathFinderModule.PP_UP_RIGHT = 1 -PathFinderModule.PP_RIGHT = 2 -PathFinderModule.PP_DOWN_RIGHT = 3 -PathFinderModule.PP_DOWN = 4 -PathFinderModule.PP_DOWN_LEFT = 5 -PathFinderModule.PP_LEFT = 6 -PathFinderModule.PP_UP_LEFT = 7 - -PathFinderModule.direction_to_text = { -"PathFinderModule.PP_UP", -"PathFinderModule.PP_UP_RIGHT", -"PathFinderModule.PP_RIGHT", -"PathFinderModule.PP_DOWN_RIGHT", -"PathFinderModule.PP_DOWN", -"PathFinderModule.PP_DOWN_LEFT", -"PathFinderModule.PP_LEFT", -"PathFinderModule.PP_UP_LEFT", -"PathFinderModule.unknown" -} +PathFinderModule.PATHFINDER_MIN_DISTANCE_START_TARGET = 50 PathFinderModule.PP_MIN_DISTANCE = 20 PathFinderModule.PP_CELL_X = 9 @@ -101,6 +82,7 @@ PathFinderModule.PP_MAX_EAGER_LOOKAHEAD_STEPS = 1 PathFinderModule.MIN_FRUIT_VALUE = 50 PathFinderModule.SLOPE_DETECTION_THRESHOLD = math.rad(20) +PathFinderModule.NEW_PF_STEP_FACTOR = 4 --[[ from Giants Engine: AITurnStrategy.SLOPE_DETECTION_THRESHOLD = 0.5235987755983 @@ -111,14 +93,20 @@ function PathFinderModule:new(vehicle) setmetatable(o, self) self.__index = self o.vehicle = vehicle + o.dubins = ADDubins:new() PathFinderModule.reset(o) return o end function PathFinderModule:reset() + PathFinderModule.debugMsg(self.vehicle, "PFM:reset start") self.mask = AutoDrive.collisionMaskTerrain self.steps = 0 self.grid = {} + self.wayPoints = {} + self.initNew = false + self.path = {} + self.diffOverallNetTime = 0 self.retryCounter = 0 self.delayTime = 0 self.restrictToField = false @@ -129,6 +117,59 @@ function PathFinderModule:reset() self.fallBackMode3 = false self.isFinished = true self.smoothDone = true + self.fruitAreas = {} + self.goingToNetwork = false + self.goingToPipe = false + self.chasingVehicle = false + self.isSecondChasingVehicle = false + + if AutoDrive.experimentalFeatures.NewPathfinder then + self.PP_UP = 0 + self.PP_UP_LEFT = 1 + self.PP_LEFT = 2 + self.PP_DOWN_LEFT = 3 + self.PP_DOWN = 4 + self.PP_DOWN_RIGHT = 5 + self.PP_RIGHT = 6 + self.PP_UP_RIGHT = 7 + self.direction_to_text = { + "PP_UP", + "PP_UP_LEFT", + "PP_LEFT", + "PP_DOWN_LEFT", + "PP_DOWN", + "PP_DOWN_RIGHT", + "PP_RIGHT", + "PP_UP_RIGHT", + "unknown" + } + self.minTurnRadius = AutoDrive.getDriverRadius(self.vehicle) + self.dubinsDone = false + self.dubinsCount = 0 + self.isNewPF = true + else + self.PP_UP = 0 + self.PP_UP_RIGHT = 1 + self.PP_RIGHT = 2 + self.PP_DOWN_RIGHT = 3 + self.PP_DOWN = 4 + self.PP_DOWN_LEFT = 5 + self.PP_LEFT = 6 + self.PP_UP_LEFT = 7 + self.direction_to_text = { + "PP_UP", + "PP_UP_RIGHT", + "PP_RIGHT", + "PP_DOWN_RIGHT", + "PP_DOWN", + "PP_DOWN_LEFT", + "PP_LEFT", + "PP_UP_LEFT", + "unknown" + } + self.minTurnRadius = AutoDrive.getDriverRadius(self.vehicle) * 2 / 3 + self.isNewPF = false + end end function PathFinderModule:hasFinished() @@ -146,6 +187,9 @@ function PathFinderModule:getPath() end function PathFinderModule:startPathPlanningToNetwork(destinationId) + PathFinderModule.debugMsg(self.vehicle, "PathFinderModule:startPathPlanningToNetwork destinationId %s" + , tostring(destinationId) + ) PathFinderModule.debugVehicleMsg(self.vehicle, string.format("PFM startPathPlanningToNetwork destinationId %s", tostring(destinationId) @@ -157,6 +201,9 @@ function PathFinderModule:startPathPlanningToNetwork(destinationId) end function PathFinderModule:startPathPlanningToWayPoint(wayPointId, destinationId) + PathFinderModule.debugMsg(self.vehicle, "PathFinderModule:startPathPlanningToWayPoint destinationId %s" + , tostring(destinationId) + ) PathFinderModule.debugVehicleMsg(self.vehicle, string.format("PFM startPathPlanningToWayPoint wayPointId %s", tostring(wayPointId) @@ -176,6 +223,9 @@ function PathFinderModule:startPathPlanningToWayPoint(wayPointId, destinationId) end function PathFinderModule:startPathPlanningToPipe(combine, chasing) + PathFinderModule.debugMsg(self.vehicle, "PathFinderModule:startPathPlanningToPipe chasing %s" + , tostring(chasing) + ) AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_PATHINFO, "PathFinderModule:startPathPlanningToPipe") PathFinderModule.debugVehicleMsg(self.vehicle, string.format("PFM startPathPlanningToPipe combine %s", @@ -279,6 +329,9 @@ function PathFinderModule:startPathPlanningToPipe(combine, chasing) end function PathFinderModule:startPathPlanningToVehicle(targetVehicle, targetDistance) + PathFinderModule.debugMsg(self.vehicle, "PathFinderModule:startPathPlanningToVehicle targetDistance %s" + , tostring(targetDistance) + ) PathFinderModule.debugVehicleMsg(self.vehicle, string.format("PFM startPathPlanningToVehicle targetVehicle %s", tostring(targetVehicle:getName()) @@ -300,6 +353,10 @@ function PathFinderModule:startPathPlanningToVehicle(targetVehicle, targetDistan end function PathFinderModule:startPathPlanningTo(targetPoint, targetVector) + PathFinderModule.debugMsg(self.vehicle, "PathFinderModule:startPathPlanningTo self.minTurnRadius %.1f" + , self.minTurnRadius + ) + PathFinderModule.debugVehicleMsg(self.vehicle, string.format("PFM startPathPlanningTo targetPoint x,z %d %d", math.floor(targetPoint.x), @@ -307,6 +364,12 @@ function PathFinderModule:startPathPlanningTo(targetPoint, targetVector) ) ) ADScheduler:addPathfinderVehicle(self.vehicle) + if math.abs(targetVector.x) < 0.001 then + targetVector.x = 0.001 + end + if math.abs(targetVector.z) < 0.001 then + targetVector.z = 0.001 + end self.targetVector = targetVector local vehicleWorldX, vehicleWorldY, vehicleWorldZ = getWorldTranslation(self.vehicle.components[1].node) local vehicleRx, _, vehicleRz = localDirectionToWorld(self.vehicle.components[1].node, 0, 0, 1) @@ -316,7 +379,6 @@ function PathFinderModule:startPathPlanningTo(targetPoint, targetVector) local angleRad = math.atan2(targetVector.z, targetVector.x) angleRad = AutoDrive.normalizeAngle(angleRad) - self.minTurnRadius = AutoDrive.getDriverRadius(self.vehicle) * 2 / 3 self.vectorX = {x = math.cos(angleRad) * self.minTurnRadius, z = math.sin(angleRad) * self.minTurnRadius} self.vectorZ = {x = - math.sin(angleRad) * self.minTurnRadius, z = math.cos(angleRad) * self.minTurnRadius} @@ -336,6 +398,7 @@ function PathFinderModule:startPathPlanningTo(targetPoint, targetVector) self.fruitToCheck = nil + self.start = {x = self.startX, z = self.startZ} self.startCell = {x = 0, z = 0} self.startCell.direction = self:worldDirectionToGridDirection(vehicleVector) self.startCell.visited = false @@ -347,6 +410,12 @@ function PathFinderModule:startPathPlanningTo(targetPoint, targetVector) self.startCell.bordercells = 0 self.currentCell = nil + local vehicleBehindX, _, vehicleBehindZ = localDirectionToWorld(self.vehicle.components[1].node, 0, 0, -self.minTurnRadius) + local vehicleBehindVector = {x = vehicleBehindX, z = vehicleBehindZ} + self.behindStartCell = self:worldLocationToGridLocation(vehicleWorldX + vehicleBehindX, vehicleWorldZ + vehicleBehindZ) + self.behindStartCell.direction = self:worldDirectionToGridDirection(vehicleBehindVector, vehicleVector) + self.behind = {x = vehicleWorldX + vehicleBehindX, z = vehicleWorldZ + vehicleBehindZ} + -- table.insert(self.grid, self.startCell) local gridKey = string.format("%d|%d|%d", self.startCell.x, self.startCell.z, self.startCell.direction) self.grid[gridKey] = self.startCell @@ -357,10 +426,12 @@ function PathFinderModule:startPathPlanningTo(targetPoint, targetVector) local targetCellZ = (((targetX - self.startX) / self.vectorX.x) * self.vectorX.z - targetZ + self.startZ) / (((self.vectorZ.x / self.vectorX.x) * self.vectorX.z) - self.vectorZ.z) local targetCellX = (targetZ - self.startZ - targetCellZ * self.vectorZ.z) / self.vectorX.z - targetCellX = AutoDrive.round(targetCellX) targetCellZ = AutoDrive.round(targetCellZ) self.targetCell = {x = targetCellX, z = targetCellZ, direction = self.PP_UP} + self.targetAhead = {x = targetX + self.vectorX.x, z = targetZ + self.vectorX.z} + self.targetAheadCell = self:worldLocationToGridLocation(self.targetAhead.x, self.targetAhead.z) + self:determineBlockedCells(self.targetCell) --self:checkGridCell(self.targetCell) @@ -371,7 +442,6 @@ function PathFinderModule:startPathPlanningTo(targetPoint, targetVector) self.startIsOnField = AutoDrive.checkIsOnField(vehicleWorldX, vehicleWorldY, vehicleWorldZ) and self.vehicle.ad.sensors.frontSensorField:pollInfo(true) self.endIsOnField = AutoDrive.checkIsOnField(targetX, vehicleWorldY, targetZ) - self.restrictToField = AutoDrive.getSetting("restrictToField", self.vehicle) and self.startIsOnField and self.endIsOnField self.goingToPipe = false self.chasingVehicle = false @@ -392,7 +462,40 @@ function PathFinderModule:startPathPlanningTo(targetPoint, targetVector) -- else -- self.reachedFieldBorder = true -- end + + self.q0 = { + vehicleWorldX + , -vehicleWorldZ + , AutoDrive.normalizeAngle(math.atan2(vehicleRx, vehicleRz) + math.pi + math.pi / 2) + } + + self.q1 = { + targetX + , -targetZ + , AutoDrive.normalizeAngle(math.atan2(targetVector.x, targetVector.z) + math.pi + math.pi / 2) + } + + self:setupNew(self.behindStartCell, self.startCell,self.targetCell) + self.chainStartToTarget = {} + + PathFinderModule.debugMsg(self.vehicle, "PathFinderModule:startPathPlanningTo vehicleWorldX xz %.1f,%.1f" + , vehicleWorldX, vehicleWorldZ + ) + + PathFinderModule.debugMsg(self.vehicle, "PathFinderModule:startPathPlanningTo targetX xz %.1f,%.1f" + , targetX, targetZ + ) + + PathFinderModule.debugMsg(self.vehicle, "PathFinderModule:startPathPlanningTo self.behind xz %.1f,%.1f" + , self.behind.x, self.behind.z + ) + + local location = self:gridLocationToWorldLocation(self.behindStartCell) + PathFinderModule.debugMsg(self.vehicle, "PathFinderModule:startPathPlanningTo behindStartCell location xz %.1f,%.1f" + , location.x, location.z + ) + PathFinderModule.debugVehicleMsg(self.vehicle, string.format("PFM startPathPlanningTo self.minTurnRadius %s vectorX.x,vectorX.z %s %s vectorZ.x,vectorZ.z %s %s", tostring(self.minTurnRadius), @@ -406,14 +509,14 @@ function PathFinderModule:startPathPlanningTo(targetPoint, targetVector) string.format("PFM startPathPlanningTo startCell xz %d %d direction %s", math.floor(self.startCell.x), math.floor(self.startCell.z), - tostring(PathFinderModule.direction_to_text[self.startCell.direction+1]) + tostring(self.direction_to_text[self.startCell.direction+1]) ) ) PathFinderModule.debugVehicleMsg(self.vehicle, string.format("PFM startPathPlanningTo start targetCell xz %d %d direction %s", math.floor(self.targetCell.x), math.floor(self.targetCell.z), - tostring(PathFinderModule.direction_to_text[self.targetCell.direction+1]) + tostring(self.direction_to_text[self.targetCell.direction+1]) ) ) PathFinderModule.debugVehicleMsg(self.vehicle, @@ -454,9 +557,13 @@ function PathFinderModule:restartAtNextWayPoint() end function PathFinderModule:autoRestart() + PathFinderModule.debugMsg(self.vehicle, "PFM:autoRestart start") self.steps = 0 self.grid = {} self.wayPoints = {} + self.initNew = false + self.path = {} + self.diffOverallNetTime = 0 self.startCell.visited = false self.startCell.out = nil self.currentCell = nil @@ -472,6 +579,7 @@ function PathFinderModule:autoRestart() end function PathFinderModule:abort() + PathFinderModule.debugMsg(self.vehicle, "PFM:abort start") self.isFinished = true self.smoothDone = true self.wayPoints = {} @@ -513,7 +621,7 @@ function PathFinderModule:getCurrentState() actualState = actualState + 1 end - return actualState, maxStates + return actualState, maxStates, self.steps, self.max_pathfinder_steps end function PathFinderModule:timedOut() --> not self:isBlocked() -- used in: ExitFieldTask, UnloadAtDestinationTask @@ -525,6 +633,7 @@ function PathFinderModule:addDelayTimer(delayTime) -- used in: ExitFieldTas end function PathFinderModule:update(dt) + --stop if called without prior 'start' method calls if self.startCell == nil then self:abort() @@ -536,19 +645,55 @@ function PathFinderModule:update(dt) end if AutoDrive.isEditorModeEnabled() and AutoDrive.getDebugChannelIsSet(AutoDrive.DC_PATHINFO) then - if self.isFinished and self.smoothDone and self.wayPoints ~= nil and self.chainStartToTarget ~= nil and #self.chainStartToTarget > 0 and self.vehicle.ad.stateModule:getSpeedLimit() > 40 then - self:drawDebugForCreatedRoute() + if self.isNewPF then + self:drawDebugNewPF() + if self.isFinished and self.smoothDone and self.wayPoints ~= nil then + local lastPoint = nil + for index, point in ipairs(self.wayPoints) do + if point.isPathFinderPoint and lastPoint ~= nil then + ADDrawingManager:addLineTask(lastPoint.x, lastPoint.y, lastPoint.z, point.x, point.y, point.z, 1, 1, 0.09, 0.09) + ADDrawingManager:addArrowTask(lastPoint.x, lastPoint.y, lastPoint.z, point.x, point.y, point.z, 1, ADDrawingManager.arrows.position.start, 1, 0.09, 0.09) + + if AutoDrive.getSettingState("lineHeight") == 1 then + local gy = point.y - AutoDrive.drawHeight + 4 + local ty = lastPoint.y - AutoDrive.drawHeight + 4 + ADDrawingManager:addLineTask(point.x, gy, point.z, point.x, point.y, point.z, 1, 1, 0.09, 0.09) + ADDrawingManager:addSphereTask(point.x, gy, point.z, 3, 1, 0.09, 0.09, 0.15) + ADDrawingManager:addLineTask(lastPoint.x, ty, lastPoint.z, point.x, gy, point.z, 1, 1, 0.09, 0.09) + ADDrawingManager:addArrowTask(lastPoint.x, ty, lastPoint.z, point.x, gy, point.z, 1, ADDrawingManager.arrows.position.start, 1, 0.09, 0.09) + else + local gy = point.y - AutoDrive.drawHeight - 4 + local ty = lastPoint.y - AutoDrive.drawHeight - 4 + ADDrawingManager:addLineTask(point.x, gy, point.z, point.x, point.y, point.z, 1, 1, 0.09, 0.09) + ADDrawingManager:addSphereTask(point.x, gy, point.z, 3, 1, 0.09, 0.09, 0.15) + ADDrawingManager:addLineTask(lastPoint.x, ty, lastPoint.z, point.x, gy, point.z, 1, 1, 0.09, 0.09) + ADDrawingManager:addArrowTask(lastPoint.x, ty, lastPoint.z, point.x, gy, point.z, 1, ADDrawingManager.arrows.position.start, 1, 0.09, 0.09) + end + end + lastPoint = point + end + end else - self:drawDebugForPF() + if self.isFinished and self.smoothDone and self.wayPoints ~= nil and self.chainStartToTarget ~= nil and #self.chainStartToTarget > 0 and self.vehicle.ad.stateModule:getSpeedLimit() > 40 then + self:drawDebugForCreatedRoute() + else + self:drawDebugForPF() + end end end - if self.isFinished then if not self.smoothDone then - self:createWayPoints() + if self.isNewPF then + self:createWayPointsNew() + else + self:createWayPoints() + end end if self.smoothDone then ADScheduler:removePathfinderVehicle(self.vehicle) + -- PathFinderModule.debugMsg(self.vehicle, "PFM:update Complete #self.path %s" + -- , tostring(#self.path) + -- ) end return end @@ -586,7 +731,7 @@ function PathFinderModule:update(dt) ) self.fallBackMode1 = true self:autoRestart() - elseif fallBackModeAllowed2 then + elseif fallBackModeAllowed2 and not self.isNewPF then AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_PATHINFO, "PathFinderModule:update - error - retryAllowed: no -> fallBackModeAllowed2: yes -> going fallback now -> disable field borders #self.grid %d", table.count(self.grid)) PathFinderModule.debugVehicleMsg(self.vehicle, string.format("PFM update - error - retryAllowed: no -> fallBackModeAllowed2: yes -> going fallback now -> disable field borders #self.grid %d", @@ -651,63 +796,195 @@ function PathFinderModule:update(dt) return end - --We should see some perfomance increase by localizing the sqrt/pow functions right here - local sqrt = math.sqrt - local distanceFunc = function(a, b) - return sqrt(a * a + b * b) - end - - for i = 1, ADScheduler:getStepsPerFrame(), 1 do - if self.currentCell == nil then - - self.currentCell = self:findClosestCell(self.grid, math.huge) + if self.isNewPF then + if not self.isFinished then + if MathUtil.vector2Length(self.start.x - self.target.x, self.start.z - self.target.z) < PathFinderModule.PATHFINDER_MIN_DISTANCE_START_TARGET then + -- try dubins first before full pathfinder if close to target + if not self.dubinsDone then + self.dubinsCount = self.dubinsCount + 1 + local dubinsPath = self:getDubinsPath() + PathFinderModule.debugMsg(self.vehicle, "PFM:update getDubinsPath dubinsPath %s" + , tostring(dubinsPath) + ) + if dubinsPath then + self.dubinsDone = true + self.wayPoints = dubinsPath + self:appendWayPointsNew() + self.isFinished = true + self.smoothDone = true + return -- found path + else + -- self.completelyBlocked = true + -- no valid path + -- PathFinderModule.debugMsg(self.vehicle, "PFM:update getDubinsPath self.completelyBlocked %s" + -- , tostring(self.completelyBlocked) + -- ) + end + PathFinderModule.debugMsg(self.vehicle, "PFM:update getDubinsPath self.fallBackMode3 %s" + , tostring(self.fallBackMode3) + ) + if self.fallBackMode3 or self.dubinsCount > 4 then + self.dubinsDone = true + -- self.completelyBlocked = false + -- self.fallBackMode1 = false -- disable restrict to field + -- self.fallBackMode2 = false -- disable restrict to field border + -- self.fallBackMode3 = false -- disable avoid fruit + end + -- return + end + end + if not self.initNew then + self:setupNew(self.behindStartCell, self.startCell,self.targetCell) + local dx, dz = self.nodeStart.x - self.nodeGoal.x, self.nodeStart.z - self.nodeGoal.z + local diff = math.sqrt(dx * dx + dz * dz) + local toCloseToTarget = (diff < 3) + dx, dz = self.nodeBehindStart.x - self.nodeGoal.x, self.nodeBehindStart.z - self.nodeGoal.z + diff = math.sqrt(dx * dx + dz * dz) + toCloseToTarget = toCloseToTarget or (diff < 3) + if (self.nodeBehindStart == self.nodeGoal) or toCloseToTarget then + self.completelyBlocked = true + return -- no valid path + end + end + local diffNetTime = netGetTime() + + local current + local add_neighbor_fn = function(neighbor, cost) + if self:isDriveableAstar(neighbor) then + if not self.closedset[neighbor] then + if not cost then cost = self:get_cost(current, neighbor) end + local tentative_g_score = self.g_score[current] + cost + local openset_idx = self.openset[neighbor] + if not openset_idx or tentative_g_score < self.g_score[neighbor] then + self.came_from[neighbor] = current + self.g_score[neighbor] = tentative_g_score + self.h_score[neighbor] = self.h_score[neighbor] or self:estimate_cost(neighbor, self.nodeGoal) + self.f_score[neighbor] = tentative_g_score + self.h_score[neighbor] + self.openset[neighbor] = true + end + end + end + end + local count = 0 + while next(self.openset) do + count = count + 1 + if count > 10000 then + diffNetTime = netGetTime() - diffNetTime + self.diffOverallNetTime = self.diffOverallNetTime + diffNetTime + + AutoDrive.debugMsg(self.vehicle, "PFM:find ERROR exit counter count %d self.diffOverallNetTime %d" + , count + , self.diffOverallNetTime + ) - if self.currentCell ~= nil and distanceFunc(self.targetCell.x - self.currentCell.x, self.targetCell.z - self.currentCell.z) < 1.5 then + self.completelyBlocked = true + return -- no valid path + end + current = self:pop_best_node(self.openset, self.f_score) + if current == self.nodeGoal or self:reachedGoal(current, self.nodeGoal) then + self.came_from[self.nodeGoal] = current + self.path = self:unwind_path({}, self.came_from, self.nodeGoal) + table.insert(self.path, self.nodeGoal) + diffNetTime = netGetTime() - diffNetTime + self.diffOverallNetTime = self.diffOverallNetTime + diffNetTime + if current then + PathFinderModule.debugMsg(self.vehicle, "PFM:update find goal reached self.steps %d diffOverallNetTime %d self.nodeGoal xz %d,%d current xz %d,%d" + , self.steps + , self.diffOverallNetTime + , self.nodeGoal.x, self.nodeGoal.z + , current.x, current.z + ) + end - if self.currentCell.out == nil then - self:determineNextGridCells(self.currentCell) + self.isFinished = true + return -- found path end + if current then self.closedset[current] = true end + local from_node = self.came_from[current] + self:get_neighbors(current, from_node, add_neighbor_fn) + if count > (ADScheduler:getStepsPerFrame() * PathFinderModule.NEW_PF_STEP_FACTOR) then + diffNetTime = netGetTime() - diffNetTime + self.diffOverallNetTime = self.diffOverallNetTime + diffNetTime + + PathFinderModule.debugMsg(self.vehicle, "PFM:find steps in frame count %d diffNetTime %d self.diffOverallNetTime %d" + , count + , diffNetTime + , self.diffOverallNetTime + ) - if self:reachedTargetsNeighbor(self.currentCell.out) then - return + return -- shedule end end + diffNetTime = netGetTime() - diffNetTime + self.diffOverallNetTime = self.diffOverallNetTime + diffNetTime + PathFinderModule.debugMsg(self.vehicle, "PFM:find exit end count %d self.diffOverallNetTime %d" + , count + , self.diffOverallNetTime + ) + + self.completelyBlocked = true + return -- no valid path + end + else + --We should see some perfomance increase by localizing the sqrt/pow functions right here + local sqrt = math.sqrt + local distanceFunc = function(a, b) + return sqrt(a * a + b * b) + end + + for i = 1, ADScheduler:getStepsPerFrame(), 1 do if self.currentCell == nil then - --Mark process stopped if we have no more cells to check - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_PATHINFO, "PathFinderModule:update - Mark process stopped if we have no more cells to check") - PathFinderModule.debugVehicleMsg(self.vehicle, - string.format("PFM update - Mark process stopped if we have no more cells to check #self.grid %d", - table.count(self.grid) + + self.currentCell = self:findClosestCell(self.grid, math.huge) + + if self.currentCell ~= nil and distanceFunc(self.targetCell.x - self.currentCell.x, self.targetCell.z - self.currentCell.z) < 1.5 then + + if self.currentCell.out == nil then + self:determineNextGridCells(self.currentCell) + end + + if self:reachedTargetsNeighbor(self.currentCell.out) then + return + end + end + + if self.currentCell == nil then + --Mark process stopped if we have no more cells to check + AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_PATHINFO, "PathFinderModule:update - Mark process stopped if we have no more cells to check") + PathFinderModule.debugVehicleMsg(self.vehicle, + string.format("PFM update - Mark process stopped if we have no more cells to check #self.grid %d", + table.count(self.grid) + ) ) - ) - self.completelyBlocked = true - break - end - else - if self.currentCell.out == nil then - self:determineNextGridCells(self.currentCell) - end - self:testNextCells(self.currentCell) + self.completelyBlocked = true + break + end + else + if self.currentCell.out == nil then + self:determineNextGridCells(self.currentCell) + end + self:testNextCells(self.currentCell) - --Try shortcutting the process here. We dont have to go through the whole grid if one of the out points is viable and closer than the currenCell which was already closest - local currentDistance = distanceFunc(self.targetCell.x - self.currentCell.x, self.targetCell.z - self.currentCell.z) + --Try shortcutting the process here. We dont have to go through the whole grid if one of the out points is viable and closer than the currenCell which was already closest + local currentDistance = distanceFunc(self.targetCell.x - self.currentCell.x, self.targetCell.z - self.currentCell.z) - local outCells = {} - for _, outCell in pairs(self.currentCell.out) do - local gridKey = string.format("%d|%d|%d", outCell.x, outCell.z, outCell.direction) - if self.grid[gridKey] ~= nil then - table.insert(outCells, self.grid[gridKey]) + local outCells = {} + for _, outCell in pairs(self.currentCell.out) do + local gridKey = string.format("%d|%d|%d", outCell.x, outCell.z, outCell.direction) + if self.grid[gridKey] ~= nil then + table.insert(outCells, self.grid[gridKey]) + end end - end - local nextCell = self:findClosestCell(outCells, currentDistance) + local nextCell = self:findClosestCell(outCells, currentDistance) - -- Lets again check if we have reached our target already - if self:reachedTargetsNeighbor(self.currentCell.out) then - return - end + -- Lets again check if we have reached our target already + if self:reachedTargetsNeighbor(self.currentCell.out) then + return + end - self.currentCell = nextCell + self.currentCell = nextCell + end end end end @@ -764,7 +1041,7 @@ function PathFinderModule:testNextCells(cell) math.floor(cell.z), tostring(cell.isRestricted), tostring(cell.hasFruit), - tostring(PathFinderModule.direction_to_text[cell.direction+1]) + tostring(self.direction_to_text[cell.direction+1]) ) ) end @@ -776,11 +1053,11 @@ function PathFinderModule:testNextCells(cell) string.format("PFM testNextCells location xz %d %d direction %s", math.floor(location.x), math.floor(location.z), - tostring(PathFinderModule.direction_to_text[location.direction+1]) + tostring(self.direction_to_text[location.direction+1]) ) ) end - for i = -1, PathFinderModule.PP_UP_LEFT, 1 do -- important: do not break this loop to check for all directions! + for i = -1, self.PP_UP_LEFT, 1 do -- important: do not break this loop to check for all directions! local gridKey = string.format("%d|%d|%d", location.x, location.z, i) if self.grid[gridKey] ~= nil then -- cell is already in the grid @@ -790,7 +1067,7 @@ function PathFinderModule:testNextCells(cell) tostring(gridKey), tostring(self.grid[gridKey].x), tostring(self.grid[gridKey].z), - tostring(PathFinderModule.direction_to_text[self.grid[gridKey].direction+1]) + tostring(self.direction_to_text[self.grid[gridKey].direction+1]) ) ) end @@ -822,7 +1099,7 @@ function PathFinderModule:testNextCells(cell) string.format("PFM testNextCells location xz %d %d createPoint %s", math.floor(location.x), math.floor(location.z), - tostring(PathFinderModule.direction_to_text[location.direction+1]) + tostring(self.direction_to_text[location.direction+1]) ) ) end @@ -849,7 +1126,7 @@ function PathFinderModule:testNextCells(cell) string.format("PFM testNextCells different direction xz %d %d createPoint %s", math.floor(location.x), math.floor(location.z), - tostring(PathFinderModule.direction_to_text[location.direction+1]) + tostring(self.direction_to_text[location.direction+1]) ) ) end @@ -906,7 +1183,7 @@ function PathFinderModule:checkGridCell(cell) string.format("PFM checkGridCell isRestricted self.restrictToField %s self.fallBackMode1 %s isOnField %s x,z %d %d", tostring(self.restrictToField), tostring(self.fallBackMode1), - tostring(isOnField), + tostring(cell.isOnField), math.floor(worldPos.x), math.floor(worldPos.z) ) @@ -983,8 +1260,9 @@ function PathFinderModule:gridLocationToWorldLocation(cell) return result end -function PathFinderModule:worldDirectionToGridDirection(vector) - local angle = AutoDrive.angleBetween(self.vectorX, vector) +function PathFinderModule:worldDirectionToGridDirection(vector, baseVector) + local baseVector = baseVector or self.vectorX + local angle = AutoDrive.angleBetween(baseVector, vector) local direction = math.floor(angle / 45) local remainder = angle % 45 @@ -1169,7 +1447,7 @@ function PathFinderModule:checkForFruitTypeInArea(cell, fruitTypeIndex, corners) math.floor(cell.x), math.floor(cell.z), tostring(fruitValue), - tostring(PathFinderModule.direction_to_text[cell.direction+1]) + tostring(self.direction_to_text[cell.direction+1]) ) ) end @@ -1196,7 +1474,7 @@ function PathFinderModule:drawDebugForPF() local color_green = 0.1 local color_blue = 0.1 local color_count = 0 - index = 0 + local index = 0 for _, cell in pairs(self.grid) do index = index + 1 @@ -1277,28 +1555,28 @@ function PathFinderModule:drawDebugForPF() if cell.isRestricted == true then -- any restriction if cell.hasFruit == true then - AutoDriveDM:addLineTask(pointA.x, pointA.y, pointA.z, pointB.x, pointB.y, pointB.z, 1, 0, 1, 1) + AutoDriveDM:addLineTask(pointA.x, pointA.y, pointA.z, pointB.x, pointB.y, pointB.z, 1, 0, 1, 1) -- cyan Utils.renderTextAtWorldPosition(pointCenter.x, pointB.y + 0.4, pointCenter.z, tostring(cell.fruitValue), getCorrectTextSize(0.013), 0) else - AutoDriveDM:addLineTask(pointA.x, pointA.y, pointA.z, pointB.x, pointB.y, pointB.z, 1, 1, 0, 0) + AutoDriveDM:addLineTask(pointA.x, pointA.y, pointA.z, pointB.x, pointB.y, pointB.z, 1, 1, 0, 0) -- red end else - AutoDriveDM:addLineTask(pointA.x, pointA.y, pointA.z, pointB.x, pointB.y, pointB.z, 1, 0, 1, 0) + AutoDriveDM:addLineTask(pointA.x, pointA.y, pointA.z, pointB.x, pointB.y, pointB.z, 1, 0, 1, 0) -- green end if cell.hasCollision == true then -- ground collision, slope, water if cell.hasVehicleCollision then -- TODO: hasVehicleCollision ??? - AutoDriveDM:addLineTask(pointC.x, pointC.y, pointC.z, pointD.x, pointD.y, pointD.z, 1, 0, 0, 1) - AutoDriveDM:addLineTask(pointCenter.x, pointCenter.y, pointCenter.z, pointCenterUp.x, pointCenterUp.y, pointCenterUp.z, 1, 0, 0, 1) + AutoDriveDM:addLineTask(pointC.x, pointC.y, pointC.z, pointD.x, pointD.y, pointD.z, 1, 0, 0, 1) -- blue + AutoDriveDM:addLineTask(pointCenter.x, pointCenter.y, pointCenter.z, pointCenterUp.x, pointCenterUp.y, pointCenterUp.z, 1, 0, 0, 1) -- blue else - AutoDriveDM:addLineTask(pointC.x, pointC.y, pointC.z, pointD.x, pointD.y, pointD.z, 1, 1, 1, 0) - AutoDriveDM:addLineTask(pointCenter.x, pointCenter.y, pointCenter.z, pointCenterUp.x, pointCenterUp.y, pointCenterUp.z, 1, 1, 1, 0) + AutoDriveDM:addLineTask(pointC.x, pointC.y, pointC.z, pointD.x, pointD.y, pointD.z, 1, 1, 1, 0) -- yellow + AutoDriveDM:addLineTask(pointCenter.x, pointCenter.y, pointCenter.z, pointCenterUp.x, pointCenterUp.y, pointCenterUp.z, 1, 1, 1, 0) -- yellow end else - AutoDriveDM:addLineTask(pointC.x, pointC.y, pointC.z, pointD.x, pointD.y, pointD.z, 1, 1, 0, 1) - AutoDriveDM:addLineTask(pointCenter.x, pointCenter.y, pointCenter.z, pointCenterUp.x, pointCenterUp.y, pointCenterUp.z, 1, 1, 0, 1) + AutoDriveDM:addLineTask(pointC.x, pointC.y, pointC.z, pointD.x, pointD.y, pointD.z, 1, 1, 0, 1) -- magenta + AutoDriveDM:addLineTask(pointCenter.x, pointCenter.y, pointCenter.z, pointCenterUp.x, pointCenterUp.y, pointCenterUp.z, 1, 1, 0, 1) -- magenta end for i = 0, 10, 1 do @@ -1362,11 +1640,11 @@ function PathFinderModule:drawDebugForPF() pointTarget.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointTarget.x, 1, pointTarget.z) + 6 pointTargetUp.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointTargetUp.x, 1, pointTargetUp.z) + 10 if cell.bordercells == 1 then - AutoDriveDM:addLineTask(pointTarget.x, pointTarget.y, pointTarget.z, pointTargetUp.x, pointTargetUp.y, pointTargetUp.z, 1, 1, 0, 0) + AutoDriveDM:addLineTask(pointTarget.x, pointTarget.y, pointTarget.z, pointTargetUp.x, pointTargetUp.y, pointTargetUp.z, 1, 1, 0, 0) -- red elseif cell.bordercells == 2 then - AutoDriveDM:addLineTask(pointTarget.x, pointTarget.y, pointTarget.z, pointTargetUp.x, pointTargetUp.y, pointTargetUp.z, 1, 0, 1, 0) + AutoDriveDM:addLineTask(pointTarget.x, pointTarget.y, pointTarget.z, pointTargetUp.x, pointTargetUp.y, pointTargetUp.z, 1, 0, 1, 0) -- green elseif cell.bordercells > 2 then - AutoDriveDM:addLineTask(pointTarget.x, pointTarget.y, pointTarget.z, pointTargetUp.x, pointTargetUp.y, pointTargetUp.z, 1, 0, 0, 1) + AutoDriveDM:addLineTask(pointTarget.x, pointTarget.y, pointTarget.z, pointTargetUp.x, pointTargetUp.y, pointTargetUp.z, 1, 0, 0, 1) -- blue end end --[[ @@ -1409,8 +1687,8 @@ function PathFinderModule:drawDebugForPF() pointD.z = pointD.z - self.vectorX.z * size + self.vectorZ.z * size pointD.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointD.x, 1, pointD.z) + 3 - AutoDriveDM:addLineTask(pointA.x, pointA.y, pointA.z, pointB.x, pointB.y, pointB.z, 1, 1, 1, 1) - AutoDriveDM:addLineTask(pointC.x, pointC.y, pointC.z, pointD.x, pointD.y, pointD.z, 1, 1, 1, 1) + AutoDriveDM:addLineTask(pointA.x, pointA.y, pointA.z, pointB.x, pointB.y, pointB.z, 1, 1, 1, 1) -- white + AutoDriveDM:addLineTask(pointC.x, pointC.y, pointC.z, pointD.x, pointD.y, pointD.z, 1, 1, 1, 1) -- white local pointAB = self:gridLocationToWorldLocation(self.targetCell) pointAB.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointAB.x, 1, pointAB.z) + 3 @@ -1419,7 +1697,7 @@ function PathFinderModule:drawDebugForPF() pointTargetVector.x = pointTargetVector.x + self.targetVector.x * 10 pointTargetVector.z = pointTargetVector.z + self.targetVector.z * 10 pointTargetVector.y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, pointTargetVector.x, 1, pointTargetVector.z) + 3 - AutoDriveDM:addLineTask(pointAB.x, pointAB.y, pointAB.z, pointTargetVector.x, pointTargetVector.y, pointTargetVector.z, 1, 1, 1, 1) + AutoDriveDM:addLineTask(pointAB.x, pointAB.y, pointAB.z, pointTargetVector.x, pointTargetVector.y, pointTargetVector.z, 1, 1, 1, 1) -- white end function PathFinderModule:drawDebugForCreatedRoute() @@ -1490,12 +1768,14 @@ function PathFinderModule:drawDebugForCreatedRoute() end end - for i, waypoint in pairs(self.wayPoints) do - Utils.renderTextAtWorldPosition(waypoint.x, waypoint.y + 4, waypoint.z, "Node " .. i, getCorrectTextSize(0.013), 0) - if i > 1 then - local wp = waypoint - local pfWp = self.wayPoints[i - 1] - AutoDriveDM:addLineTask(wp.x, wp.y, wp.z, pfWp.x, pfWp.y, pfWp.z, 1, 0, 1, 1) + if self.wayPoints then + for i, waypoint in pairs(self.wayPoints) do + Utils.renderTextAtWorldPosition(waypoint.x, waypoint.y + 4, waypoint.z, "Node " .. i, getCorrectTextSize(0.013), 0) + if i > 1 then + local wp = waypoint + local pfWp = self.wayPoints[i - 1] + AutoDriveDM:addLineTask(wp.x, wp.y, wp.z, pfWp.x, pfWp.y, pfWp.z, 1, 0, 1, 1) + end end end end @@ -1841,8 +2121,14 @@ function PathFinderModule:smoothResultingPPPath_Refined() local corner4Z = node.z + math.sin(rightAngle) * sideLength if not hasCollision then - local shapes = overlapBox(worldPos.x + vectorX / 2, y + 3, worldPos.z + vectorZ / 2, 0, angleRad, 0, length / 2 + 2.5, 2.65, sideLength + 1.5, "collisionTestCallbackIgnore", nil, self.mask, true, true, true) - hasCollision = hasCollision or (shapes > 0) + if self.isNewPF then + self.collisionhits = 0 + local shapes = overlapBox(worldPos.x + vectorX / 2, y + 3, worldPos.z + vectorZ / 2, 0, angleRad, 0, length / 2 + 2.5, 2.65, sideLength + 1.5, "collisionTestCallback", self, self.mask, true, true, true) + hasCollision = hasCollision or (self.collisionhits > 0) + else + local shapes = overlapBox(worldPos.x + vectorX / 2, y + 3, worldPos.z + vectorZ / 2, 0, angleRad, 0, length / 2 + 2.5, 2.65, sideLength + 1.5, "Ignore", nil, self.mask, true, true, true) + hasCollision = hasCollision or (shapes > 0) + end if hasCollision then if self.vehicle ~= nil and self.vehicle.ad ~= nil and self.vehicle.ad.debug ~= nil and AutoDrive.debugVehicleMsg ~= nil then @@ -2057,6 +2343,15 @@ function PathFinderModule:checkSlopeAngle(x1, z1, x2, z2) ) ) end + PathFinderModule.debugMsg(self.vehicle, "PFM:checkSlopeAngle belowGroundLevel xz %d,%d terrain123 %.1f %.1f %.1f getWaterYAtWorldPosition %s waterY %s " + , math.floor(x1) + , math.floor(z1) + , terrain1 + , terrain2 + , terrain3 + , tostring(g_currentMission.environmentAreaSystem:getWaterYAtWorldPosition(worldPosMiddle.x, terrain3, worldPosMiddle.z)) + , tostring(waterY) + ) end if (angleBetween) > PathFinderModule.SLOPE_DETECTION_THRESHOLD then @@ -2068,6 +2363,14 @@ function PathFinderModule:checkSlopeAngle(x1, z1, x2, z2) ) ) end + PathFinderModule.debugMsg(self.vehicle, "PFM:checkSlopeAngle angleBetween xz %d,%d angleBetween %.1f terrain12 %.1f %.1f length %.1f " + , math.floor(x1) + , math.floor(z1) + , math.deg(angleBetween) + , terrain1 + , terrain2 + , length + ) end if (angleBetweenCenter) > PathFinderModule.SLOPE_DETECTION_THRESHOLD then @@ -2079,6 +2382,36 @@ function PathFinderModule:checkSlopeAngle(x1, z1, x2, z2) ) ) end + PathFinderModule.debugMsg(self.vehicle, "PFM:checkSlopeAngle angleBetweenCenter xz %d,%d angleBetweenCenter %.1f terrain32 %.1f %.1f lengthMiddle %.1f " + , math.floor(x1) + , math.floor(z1) + , math.deg(angleBetweenCenter) + , terrain3 + , terrain2 + , lengthMiddle + ) + end + + if (angleLeft) > PathFinderModule.SLOPE_DETECTION_THRESHOLD then + PathFinderModule.debugMsg(self.vehicle, "PFM:checkSlopeAngle angleLeft xz %d,%d angleLeft %.1f terrainLeft %.1f terrain1 %.1f lengthLeft %.1f " + , math.floor(x1) + , math.floor(z1) + , math.deg(angleLeft) + , terrainLeft + , terrain1 + , lengthLeft + ) + end + + if (angleRight) > PathFinderModule.SLOPE_DETECTION_THRESHOLD then + PathFinderModule.debugMsg(self.vehicle, "PFM:checkSlopeAngle angleRight xz %d,%d angleRight %.1f terrainRight %.1f terrain1 %.1f lengthRight %.1f " + , math.floor(x1) + , math.floor(z1) + , math.deg(angleRight) + , terrainRight + , terrain1 + , lengthRight + ) end if belowGroundLevel or (angleBetween) > PathFinderModule.SLOPE_DETECTION_THRESHOLD or (angleBetweenCenter) > PathFinderModule.SLOPE_DETECTION_THRESHOLD @@ -2089,11 +2422,754 @@ function PathFinderModule:checkSlopeAngle(x1, z1, x2, z2) return false, angleBetween end -function PathFinderModule.debugVehicleMsg(vehicle, msg, ...) +function PathFinderModule.debugVehicleMsg(vehicle, msg) -- collect output for single vehicle - help to examine sequences for a single vehicle if vehicle ~= nil and vehicle.ad ~= nil and vehicle.ad.debug ~= nil then if AutoDrive.debugVehicleMsg ~= nil then - AutoDrive.debugVehicleMsg(vehicle, msg, ...) + AutoDrive.debugVehicleMsg(vehicle, msg) + end + end +end + +function PathFinderModule:drawDebugNewPF() + -- AStar + if self.cachedNodes and #self.cachedNodes > 0 then + for z, row in pairs(self.cachedNodes) do + for x, node in pairs(row) do + -- cell outline + local gridFactor = PathFinderModule.GRID_SIZE_FACTOR + if self.isSecondChasingVehicle then + gridFactor = PathFinderModule.GRID_SIZE_FACTOR_SECOND_UNLOADER + end + local corners = self:getCorners(node, {x = self.vectorX.x * gridFactor, z = self.vectorX.z * gridFactor}, {x = self.vectorZ.x * gridFactor, z = self.vectorZ.z * gridFactor}) + local tempY = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, corners[1].x, 1, corners[1].z) + if node.isOnField then + ADDrawingManager:addLineTask(corners[1].x, tempY, corners[1].z, corners[2].x, tempY, corners[2].z, 1, 0, 1, 0) -- green + ADDrawingManager:addLineTask(corners[2].x, tempY, corners[2].z, corners[4].x, tempY, corners[4].z, 1, 0, 1, 0) + ADDrawingManager:addLineTask(corners[3].x, tempY, corners[3].z, corners[4].x, tempY, corners[4].z, 1, 0, 1, 0) + ADDrawingManager:addLineTask(corners[1].x, tempY, corners[1].z, corners[3].x, tempY, corners[3].z, 1, 0, 1, 0) + else + ADDrawingManager:addLineTask(corners[1].x, tempY, corners[1].z, corners[2].x, tempY, corners[2].z, 1, 1, 0, 0) -- red + ADDrawingManager:addLineTask(corners[2].x, tempY, corners[2].z, corners[4].x, tempY, corners[4].z, 1, 1, 0, 0) + ADDrawingManager:addLineTask(corners[3].x, tempY, corners[3].z, corners[4].x, tempY, corners[4].z, 1, 1, 0, 0) + ADDrawingManager:addLineTask(corners[1].x, tempY, corners[1].z, corners[3].x, tempY, corners[3].z, 1, 1, 0, 0) + end + if node.isRestricted then + if node.hasFruit then + ADDrawingManager:addLineTask(corners[2].x, tempY, corners[2].z, corners[3].x, tempY, corners[3].z, 1, 0, 1, 1) -- cyan + else + ADDrawingManager:addLineTask(corners[2].x, tempY, corners[2].z, corners[3].x, tempY, corners[3].z, 1, 1, 0, 0) -- red + end + else + ADDrawingManager:addLineTask(corners[2].x, tempY, corners[2].z, corners[3].x, tempY, corners[3].z, 1, 0, 1, 0) -- green + end + if node.hasCollision then + if node.hasVehicleCollision then + ADDrawingManager:addLineTask(corners[1].x, tempY, corners[1].z, corners[4].x, tempY, corners[4].z, 1, 1, 0, 1) -- blue + else + ADDrawingManager:addLineTask(corners[1].x, tempY, corners[1].z, corners[4].x, tempY, corners[4].z, 1, 1, 1, 0) -- yellow + end + end + + -- cell text + if node.text == nil then + node.text = string.format("%d,%d", x, z) + end + local point = self:gridLocationToWorldLocation({x = node.x, z = node.z}) + Utils.renderTextAtWorldPosition(point.x, tempY + 3, point.z, node.text, getCorrectTextSize(0.013), 0) + + -- behind point + if node.isBehind then + local text = string.format("B %d,%d", x, z) + Utils.renderTextAtWorldPosition(point.x, tempY + 4, point.z, text, getCorrectTextSize(0.013), 0) + ADDrawingManager:addSphereTask(self.behind.x, tempY + 3, self.behind.z, 6, 0, 0, 1, 0) -- blue + end + + -- start point + if node.isStart then + local text = string.format("S %d,%d", x, z) + Utils.renderTextAtWorldPosition(point.x, tempY + 5, point.z, text, getCorrectTextSize(0.013), 0) + ADDrawingManager:addSphereTask(self.startX, tempY + 3, self.startZ, 6, 0, 1, 0, 0) -- green + end + + -- goal point + if node.isGoal then + local text = string.format("T %d,%d", x, z) + Utils.renderTextAtWorldPosition(point.x, tempY + 6, point.z, text, getCorrectTextSize(0.013), 0) + ADDrawingManager:addSphereTask(self.target.x, tempY + 3, self.target.z, 6, 1, 0, 0, 0) -- red + end + end + end + end + + -- Dubins Path + if self.dubinsPath and #self.dubinsPath > 0 then + + local lastPoint = nil + for index, point in ipairs(self.dubinsPath) do + if lastPoint ~= nil then + ADDrawingManager:addLineTask(lastPoint.x, lastPoint.y, lastPoint.z, point.x, point.y, point.z, 1, 1, 0.09, 0.09) + ADDrawingManager:addArrowTask(lastPoint.x, lastPoint.y, lastPoint.z, point.x, point.y, point.z, 1, ADDrawingManager.arrows.position.start, 1, 0.09, 0.09) + + if AutoDrive.getSettingState("lineHeight") == 1 then + local gy = point.y - AutoDrive.drawHeight + 4 + local ty = lastPoint.y - AutoDrive.drawHeight + 4 + ADDrawingManager:addLineTask(point.x, gy, point.z, point.x, point.y, point.z, 1, 1, 0.09, 0.09) + ADDrawingManager:addSphereTask(point.x, gy, point.z, 3, 1, 0.09, 0.09, 0.15) + ADDrawingManager:addLineTask(lastPoint.x, ty, lastPoint.z, point.x, gy, point.z, 1, 1, 0.09, 0.09) + ADDrawingManager:addArrowTask(lastPoint.x, ty, lastPoint.z, point.x, gy, point.z, 1, ADDrawingManager.arrows.position.start, 1, 0.09, 0.09) + else + local gy = point.y - AutoDrive.drawHeight - 4 + local ty = lastPoint.y - AutoDrive.drawHeight - 4 + ADDrawingManager:addLineTask(point.x, gy, point.z, point.x, point.y, point.z, 1, 1, 0.09, 0.09) + ADDrawingManager:addSphereTask(point.x, gy, point.z, 3, 1, 0.09, 0.09, 0.15) + ADDrawingManager:addLineTask(lastPoint.x, ty, lastPoint.z, point.x, gy, point.z, 1, 1, 0.09, 0.09) + ADDrawingManager:addArrowTask(lastPoint.x, ty, lastPoint.z, point.x, gy, point.z, 1, ADDrawingManager.arrows.position.start, 1, 0.09, 0.09) + end + end + lastPoint = point + end + end + if self.dubinsNodes then + local i = 0 + for z, row in pairs(self.dubinsNodes) do + for x, node in pairs(row) do + local corners = node.corners + i = i + 1 + local text = string.format("%d",i) + -- Utils.renderTextAtWorldPosition(x, node.worldPos.y + 3, z, text, getCorrectTextSize(0.013), 0) + local tempY = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, node.corners[1].x, 1, node.corners[1].z) + if node.isOnField then + ADDrawingManager:addLineTask(corners[1].x, tempY, corners[1].z, corners[2].x, tempY, corners[2].z, 1, 0, 1, 0) -- green + ADDrawingManager:addLineTask(corners[2].x, tempY, corners[2].z, corners[4].x, tempY, corners[4].z, 1, 0, 1, 0) + ADDrawingManager:addLineTask(corners[3].x, tempY, corners[3].z, corners[4].x, tempY, corners[4].z, 1, 0, 1, 0) + ADDrawingManager:addLineTask(corners[1].x, tempY, corners[1].z, corners[3].x, tempY, corners[3].z, 1, 0, 1, 0) + else + ADDrawingManager:addLineTask(corners[1].x, tempY, corners[1].z, corners[2].x, tempY, corners[2].z, 1, 1, 0, 0) -- red + ADDrawingManager:addLineTask(corners[2].x, tempY, corners[2].z, corners[4].x, tempY, corners[4].z, 1, 1, 0, 0) + ADDrawingManager:addLineTask(corners[3].x, tempY, corners[3].z, corners[4].x, tempY, corners[4].z, 1, 1, 0, 0) + ADDrawingManager:addLineTask(corners[1].x, tempY, corners[1].z, corners[3].x, tempY, corners[3].z, 1, 1, 0, 0) + end + if node.isRestricted then + if node.hasFruit then + ADDrawingManager:addLineTask(corners[2].x, tempY, corners[2].z, corners[3].x, tempY, corners[3].z, 1, 0, 1, 1) -- cyan + else + ADDrawingManager:addLineTask(corners[2].x, tempY, corners[2].z, corners[3].x, tempY, corners[3].z, 1, 1, 0, 0) -- red + end + else + ADDrawingManager:addLineTask(corners[2].x, tempY, corners[2].z, corners[3].x, tempY, corners[3].z, 1, 0, 1, 0) -- green + end + if node.hasCollision then + if node.hasVehicleCollision then + ADDrawingManager:addLineTask(corners[1].x, tempY, corners[1].z, corners[4].x, tempY, corners[4].z, 1, 1, 0, 1) -- blue + else + ADDrawingManager:addLineTask(corners[1].x, tempY, corners[1].z, corners[4].x, tempY, corners[4].z, 1, 1, 1, 0) -- yellow + end + end + if node.fruitValue and node.fruitValue > 0 then + local text = string.format("%d",node.fruitValue) + Utils.renderTextAtWorldPosition(x, node.worldPos.y + 3, z, text, getCorrectTextSize(0.013), 0) + end + end + end + end +end + +function PathFinderModule:isDriveableAstar(cell) + cell.isRestricted = false + cell.incoming = cell.from_node + cell.hasCollision = false + + local worldPos = self:gridLocationToWorldLocation(cell) + --Try going through the checks in a way that fast checks happen before slower ones which might then be skipped + + cell.isOnField = AutoDrive.checkIsOnField(worldPos.x, 0, worldPos.z) + + -- check the most probable restrictions on field first to prevent unneccessary checks + if not cell.isRestricted and self.restrictToField and not (self.fallBackMode1 or self.fallBackMode2) then + -- in fallBackMode1 we ignore the field restriction + cell.isRestricted = cell.isRestricted or (not cell.isOnField) + if not cell.isOnField then + PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveableAstar not cell.isOnField xz %d,%d " + , cell.x, cell.z + ) + end + end + + local gridFactor = PathFinderModule.GRID_SIZE_FACTOR + if self.isSecondChasingVehicle then + gridFactor = PathFinderModule.GRID_SIZE_FACTOR_SECOND_UNLOADER + end + local corners = self:getCorners(cell, {x = self.vectorX.x * gridFactor, z = self.vectorX.z * gridFactor}, {x = self.vectorZ.x * gridFactor, z = self.vectorZ.z * gridFactor}) + + if not cell.isRestricted and self.avoidFruitSetting and not self.fallBackMode3 then + -- check for fruit + self:checkForFruitInArea(cell, corners) -- set cell.isRestricted if fruit found + table.insert(self.fruitAreas, corners) + if cell.isRestricted then + PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveableAstar cell.isRestricted xz %d,%d fruit found %s" + , cell.x, cell.z + , self.fruitToCheck + ) + end + end + + if not cell.isRestricted and cell.incoming ~= nil then + -- check for up/down is to big or below water level + local worldPosPrevious = self:gridLocationToWorldLocation(cell.incoming) + local angelToSlope, angle = self:checkSlopeAngle(worldPos.x, worldPos.z, worldPosPrevious.x, worldPosPrevious.z) --> true if up/down or roll is to big or below water level + cell.angle = angle + cell.hasCollision = cell.hasCollision or angelToSlope + cell.isRestricted = cell.isRestricted or cell.hasCollision + if angelToSlope then + PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveableAstar angelToSlope xz %d,%d" + , cell.x, cell.z + ) + end + end + + if not cell.isRestricted then + -- check for obstacles + local shapeDefinition = self:getShapeDefByDirectionType(cell) --> return shape for the cell according to direction, on ground level, 2.65m height + self.collisionhits = 0 + local shapes = overlapBox(shapeDefinition.x, shapeDefinition.y + 3, shapeDefinition.z, 0, shapeDefinition.angleRad, 0, shapeDefinition.widthX, 2.65, shapeDefinition.widthZ, "collisionTestCallback", self, self.mask, true, true, true) + cell.hasCollision = cell.hasCollision or (self.collisionhits > 0) + cell.isRestricted = cell.isRestricted or cell.hasCollision + if cell.hasCollision then + PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveableAstar cell.hasCollision xz %d,%d collision" + , cell.x, cell.z + ) + end + end + + if not cell.isRestricted and cell.incoming ~= nil then + local worldPosPrevious = self:gridLocationToWorldLocation(cell.incoming) + local vectorX = worldPosPrevious.x - worldPos.x + local vectorZ = worldPosPrevious.z - worldPos.z + local dirVec = { x=vectorX, z = vectorZ} + + local cellUsedByVehiclePath = AutoDrive.checkForVehiclePathInBox(corners, self.minTurnRadius, self.vehicle, dirVec) + cell.isRestricted = cell.isRestricted or cellUsedByVehiclePath + self.blockedByOtherVehicle = self.blockedByOtherVehicle or cellUsedByVehiclePath + if cellUsedByVehiclePath then + PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveableAstar cellUsedByVehiclePath xz %d,%d vehicle" + , cell.x, cell.z + ) + end + end + return not(cell.isRestricted) +end + + -- Cost of two adjacent nodes +-- current, neighbor +function PathFinderModule:get_cost(from_node, to_node) + local dx, dz = from_node.x - to_node.x, from_node.z - to_node.z + return math.sqrt(dx * dx + dz * dz) + (from_node.cost + to_node.cost) * 0.5 +end + +-- For heuristic. Estimate cost of current node to goal node +-- neighbor, goal +function PathFinderModule:estimate_cost(node, goal_node) + return self:get_cost(node, goal_node) * 1.5 + (node.cost + goal_node.cost) * 0.5 +end + +-- current = self:pop_best_node(self.openset, self.f_score) +-- return: node / nil +-- self.openset, f_score +function PathFinderModule:pop_best_node(set, score) + local best, node = math.huge, nil + + for k, v in pairs(set) do + local s = score[k] + + if s < best then + best = s or math.huge + node = k + end + end + if not node then return end + set[node] = nil + return node +end + +-- {}, self.came_from, self.nodeGoal +function PathFinderModule:unwind_path(flat_path, came_from, goal) + if came_from[goal] and (came_from[goal] ~= self.nodeGoal) then + table.insert(flat_path, 1, came_from[goal]) + return self:unwind_path(flat_path, came_from, came_from[goal]) + else + return flat_path + end +end + +-- Node must be able to check if they are the same +-- so the example cannot directly return a different table for same coord +function PathFinderModule:get_node(x, z) + local row = self.cachedNodes[z] + if not row then row = {}; self.cachedNodes[z] = row end + local node = row[x] + if not node then node = { x = x, z = z, cost = 0 }; row[x] = node end + return node +end + +function PathFinderModule:getDirections(fromNode, node) + if node == nil then + AutoDrive.debugMsg(self.vehicle, "PFM:getDirections ERROR fromNode %s node %s" + , tostring(fromNode) + , tostring(node) + ) + return + end + local directions = {} + +--[[ if fromNode then + PathFinderModule.debugMsg(self.vehicle, "PFM:getDirections fromNode %d,%d fromNode.direction %s node xz %d,%d node.direction %s" + , fromNode.x, fromNode.z + , tostring(self.direction_to_text[fromNode.direction+1]) + , node.x, node.z + , tostring(self.direction_to_text[node.direction+1]) + ) + else + PathFinderModule.debugMsg(self.vehicle, "PFM:getDirections fromNode %s node xz %d,%d node.direction %s" + , tostring(fromNode) + , node.x, node.z + , tostring(self.direction_to_text[node.direction+1]) + ) + end + ]] + if (fromNode == nil and node.direction == self.PP_RIGHT) or (fromNode and fromNode.x == node.x and fromNode.z < node.z) then + directions[1] = { -1, 1 } + directions[1].direction = self.PP_DOWN_RIGHT + directions[2] = { 0, 1 } + directions[2].direction = self.PP_RIGHT + directions[3] = { 1, 1 } + directions[3].direction = self.PP_UP_RIGHT + elseif (fromNode == nil and node.direction == self.PP_LEFT) or (fromNode and fromNode.x == node.x and fromNode.z > node.z) then + directions[1] = { -1, -1 } + directions[1].direction = self.PP_DOWN_LEFT + directions[2] = { 0, -1 } + directions[2].direction = self.PP_LEFT + directions[3] = { 1, -1 } + directions[3].direction = self.PP_UP_LEFT + elseif (fromNode == nil and node.direction == self.PP_UP) or (fromNode and fromNode.x < node.x and fromNode.z == node.z) then + directions[1] = { 1, -1 } + directions[1].direction = self.PP_UP_LEFT + directions[2] = { 1, 0 } + directions[2].direction = self.PP_UP + directions[3] = { 1, 1 } + directions[3].direction = self.PP_UP_RIGHT + elseif (fromNode == nil and node.direction == self.PP_DOWN) or (fromNode and fromNode.x > node.x and fromNode.z == node.z) then + directions[1] = { -1, -1 } + directions[1].direction = self.PP_DOWN_LEFT + directions[2] = { -1, 0 } + directions[2].direction = self.PP_DOWN + directions[3] = { -1, 1 } + directions[3].direction = self.PP_DOWN_RIGHT + elseif (fromNode == nil and node.direction == self.PP_UP_RIGHT) or (fromNode and fromNode.x < node.x and fromNode.z < node.z) then + directions[1] = { 1, 0 } + directions[1].direction = self.PP_UP + directions[2] = { 1, 1 } + directions[2].direction = self.PP_UP_RIGHT + directions[3] = { 0, 1 } + directions[3].direction = self.PP_RIGHT + elseif (fromNode == nil and node.direction == self.PP_DOWN_LEFT) or (fromNode and fromNode.x > node.x and fromNode.z > node.z) then + directions[1] = { 0, -1 } + directions[1].direction = self.PP_LEFT + directions[2] = { -1, -1 } + directions[2].direction = self.PP_DOWN_LEFT + directions[3] = { -1, 0 } + directions[3].direction = self.PP_DOWN + elseif (fromNode == nil and node.direction == self.PP_UP_LEFT) or (fromNode and fromNode.x < node.x and fromNode.z > node.z) then + directions[1] = { 0, -1 } + directions[1].direction = self.PP_LEFT + directions[2] = { 1, -1 } + directions[2].direction = self.PP_UP_LEFT + directions[3] = { 1, 0 } + directions[3].direction = self.PP_UP + elseif (fromNode == nil and node.direction == self.PP_DOWN_RIGHT) or (fromNode and fromNode.x > node.x and fromNode.z < node.z) then + directions[1] = { -1, 0 } + directions[1].direction = self.PP_DOWN + directions[2] = { -1, 1 } + directions[2].direction = self.PP_DOWN_RIGHT + directions[3] = { 0, 1 } + directions[3].direction = self.PP_RIGHT + else + if fromNode then + AutoDrive.debugMsg(self.vehicle, "PFM:getDirections ERROR fromNode xz %d,%d" + , fromNode.x + , fromNode.z + ) + end + AutoDrive.debugMsg(self.vehicle, "PFM:getDirections ERROR fromNode %s node xz %d,%d node.direction %s" + , tostring(fromNode) + , node.x, node.z + , tostring(node.direction) + ) + end +--[[ + PathFinderModule.debugMsg(self.vehicle, "PFM:getDirections %d,%d direction %s %d,%d direction %s %d,%d direction %s" + , directions[1][1], directions[1][2] + , tostring(self.direction_to_text[directions[1].direction+1]) + , directions[2][1], directions[2][2] + , tostring(self.direction_to_text[directions[2].direction+1]) + , directions[3][1], directions[3][2] + , tostring(self.direction_to_text[directions[3].direction+1]) + ) + ]] + return directions +end + +-- Return all neighbor nodes. Means a target that can be moved from the current node +-- current, from_node, add_neighbor_fn +function PathFinderModule:get_neighbors(node, fromNode, add_neighbor_fn) + local x, z = node.x, node.z + local directions = self:getDirections(fromNode, node) + if directions then + for i, offset in ipairs(directions) do + local tnode = self:get_node(x + offset[1], z + offset[2]) + tnode.direction = offset.direction + tnode.from_node = fromNode + add_neighbor_fn(tnode) + end + end +end + +local all_neighbors_offset = { + { -1, -1 }, { 0, -1 }, { 1, -1 }, + { -1, 0 }, { 1, 0 }, + { -1, 1 }, { 0, 1 }, { 1, 1 } +} + +function PathFinderModule:setBlockedGoal() + local x, z = self.targetAheadCell.x, self.targetAheadCell.z + for i, offset in ipairs(all_neighbors_offset) do + if not (self.targetCell.x == (x + offset[1]) and self.targetCell.z == (z + offset[2])) then + local tnode = self:get_node(x + offset[1], z + offset[2]) + tnode.text = string.format("G %d,%d",x + offset[1], z + offset[2]) + tnode.isBlockedGoal = true + self.closedset[tnode] = true + end + end +end + +function PathFinderModule:reachedGoal(current, goal) + if math.abs(current.x - goal.x) < 2 and math.abs(current.z - goal.z) < 2 then + return true + else + return false + end +end + +function PathFinderModule:setupNew(behindStartCell, startCell, targetCell, userdata) + PathFinderModule.debugMsg(self.vehicle, "PFM:setupNew behindStartCell %s,%s startCell %s,%s targetCell %s,%s" + , tostring(behindStartCell.x) + , tostring(behindStartCell.z) + , tostring(startCell.x) + , tostring(startCell.z) + , tostring(targetCell.x) + , tostring(targetCell.z) + ) + self.cachedNodes = {} + self.openset = {} + self.closedset = {} + self.came_from = {} + self.g_score = {} + self.h_score = {} + self.f_score = {} + + self.nodeBehindStart = self:get_node(behindStartCell.x, behindStartCell.z) + self.nodeBehindStart.isBehind = true + self.nodeBehindStart.direction = behindStartCell.direction + + self.nodeStart = self:get_node(startCell.x, startCell.z) + self.nodeStart.isStart = true + self.nodeStart.direction = startCell.direction + + self.nodeGoal = self:get_node(targetCell.x, targetCell.z) + self.nodeGoal.isGoal = true + self.nodeGoal.direction = targetCell.direction + + self.g_score[self.nodeBehindStart] = math.huge + self.h_score[self.nodeBehindStart] = math.huge + self.f_score[self.nodeBehindStart] = math.huge + + self.g_score[self.nodeStart] = 0 + self.h_score[self.nodeStart] = self:estimate_cost(self.nodeStart, self.nodeGoal) + self.f_score[self.nodeStart] = self.h_score[self.nodeStart] + self.came_from[self.nodeStart] = self.nodeBehindStart + + self.openset[self.nodeStart] = true + self.closedset[self.nodeBehindStart] = true + self:isDriveableAstar(self.nodeStart) + self:setBlockedGoal() + self.initNew = true +end + +function PathFinderModule:createWayPointsNew() + if self.smoothStep == 0 then + self.wayPoints = {} + for index, cell in ipairs(self.path) do + self.wayPoints[index] = self:gridLocationToWorldLocation(cell) + self.wayPoints[index].y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, self.wayPoints[index].x, 1, self.wayPoints[index].z) + self.wayPoints[index].direction = cell.direction + end + -- remove zig zag line + self:smoothResultingPPPath() + end + -- shortcut the path if possible + self:smoothResultingPPPath_Refined() + + if self.smoothStep == 2 then + self:appendWayPointsNew() + end +end + +function PathFinderModule:appendWayPointsNew() + -- When going to network, dont turn actual road network nodes into pathFinderPoints + if self.goingToNetwork then + for i = 1, #self.wayPoints, 1 do + self.wayPoints[i].isPathFinderPoint = true end end + + if self.appendWayPoints ~= nil then + for i = 1, #self.appendWayPoints, 1 do + self.wayPoints[#self.wayPoints + 1] = self.appendWayPoints[i] + end + self.smoothStep = 3 + end + + -- See comment above + if not self.goingToNetwork then + for i = 1, #self.wayPoints, 1 do + self.wayPoints[i].isPathFinderPoint = true + end + end +end + +function PathFinderModule:isDriveableDubins(cell) + cell.isRestricted = false + cell.hasCollision = false + PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveableDubins start xz %d,%d " + , cell.x, cell.z + ) + + --Try going through the checks in a way that fast checks happen before slower ones which might then be skipped + + cell.isOnField = AutoDrive.checkIsOnField(cell.worldPos.x, 0, cell.worldPos.z) + + -- check the most probable restrictions on field first to prevent unneccessary checks + if not cell.isRestricted and self.restrictToField and not (self.fallBackMode1 or self.fallBackMode2) then + cell.isRestricted = cell.isRestricted or (not cell.isOnField) + if not cell.isOnField then + PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveableDubins not cell.isOnField xz %d,%d " + , cell.x, cell.z + ) + end + end + + local angleRad = AutoDrive.normalizeAngle(cell.t) + local sizeMax = self.vehicle.size.width / 2 + local vectorX = {x = math.cos(angleRad) * sizeMax, z = math.sin(angleRad) * sizeMax} + local vectorZ = {x = - math.sin(angleRad) * sizeMax, z = math.cos(angleRad) * sizeMax} + + local corners = {} + local centerLocation = cell.worldPos + corners[1] = {x = centerLocation.x + (-vectorX.x - vectorZ.x), z = centerLocation.z + (-vectorX.z - vectorZ.z)} + corners[2] = {x = centerLocation.x + (vectorX.x - vectorZ.x), z = centerLocation.z + (vectorX.z - vectorZ.z)} + corners[3] = {x = centerLocation.x + (-vectorX.x + vectorZ.x), z = centerLocation.z + (-vectorX.z + vectorZ.z)} + corners[4] = {x = centerLocation.x + (vectorX.x + vectorZ.x), z = centerLocation.z + (vectorX.z + vectorZ.z)} + cell.corners = corners + + if not cell.isRestricted and self.avoidFruitSetting and not self.fallBackMode3 then + -- check for fruit + self:checkForFruitInArea(cell, corners) -- set cell.isRestricted if fruit found + if cell.isRestricted then + PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveableDubins cell.isRestricted xz %d,%d fruit found %s" + , cell.x, cell.z + , self.fruitToCheck + ) + end + end + + if not cell.isRestricted and cell.incoming ~= nil then + -- check for up/down is to big or below water level + local worldPosPrevious = cell.incoming.worldPos + local angelToSlope, angle = self:checkSlopeAngle(cell.worldPos.x, cell.worldPos.z, worldPosPrevious.x, worldPosPrevious.z) --> true if up/down or roll is to big or below water level + cell.angle = angle + cell.hasCollision = cell.hasCollision or angelToSlope + cell.isRestricted = cell.isRestricted or cell.hasCollision + if angelToSlope then + PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveableDubins angelToSlope xz %d,%d" + , cell.x, cell.z + ) + end + end + + if not cell.isRestricted then + -- check for obstacles + self.collisionhits = 0 + local shapes = overlapBox(cell.worldPos.x, cell.worldPos.y + 3, cell.worldPos.z, 0, cell.t, 0, sizeMax, 2.65, sizeMax, "collisionTestCallback", self, self.mask, true, true, true) + cell.hasCollision = cell.hasCollision or (self.collisionhits > 0) + cell.isRestricted = cell.isRestricted or cell.hasCollision + if cell.hasCollision then + PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveableDubins cell.hasCollision xz %d,%d collision" + , cell.x, cell.z + ) + end + end + + if not cell.isRestricted and cell.incoming ~= nil then + local worldPosPrevious = cell.incoming.worldPos + local vectorX = worldPosPrevious.x - cell.worldPos.x + local vectorZ = worldPosPrevious.z - cell.worldPos.z + local dirVec = { x=vectorX, z = vectorZ} + + local cellUsedByVehiclePath = AutoDrive.checkForVehiclePathInBox(corners, self.minTurnRadius, self.vehicle, dirVec) + cell.isRestricted = cell.isRestricted or cellUsedByVehiclePath + self.blockedByOtherVehicle = self.blockedByOtherVehicle or cellUsedByVehiclePath + if cellUsedByVehiclePath then + PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveableDubins cellUsedByVehiclePath xz %d,%d vehicle" + , cell.x, cell.z + ) + end + end + if cell.isRestricted then + PathFinderModule.debugMsg(self.vehicle, "PFM:isDriveableDubins end isRestricted xz %d,%d " + , cell.x, cell.z + ) + end + return not(cell.isRestricted) +end + +function PathFinderModule:getDubinsPath() + PathFinderModule.debugMsg(self.vehicle, "PFM:getDubinsPath start") + self.dubinsPath = nil + local result = ADDubins.EDUBNOPATH + self.dubinsNodes = {} + local diffNetTime = netGetTime() + + local function get_node(x, z) + local row = self.dubinsNodes[z] + if not row then row = {}; self.dubinsNodes[z] = row end + local node = row[x] + if not node then node = { x = x, z = z, cost = 0 }; row[x] = node end + return node + end + + local function checkPath() + local result = self.dubins:dubins_path_sample_many(ADDubins.DubinsPath, 1, self.dubins.createWayPoints) + PathFinderModule.debugMsg(self.vehicle, "PFM:getDubinsPath dubins_path_sample_many result %d" + , result + ) + if result == ADDubins.EDUBOK then + PathFinderModule.debugMsg(self.vehicle, "PFM:getDubinsPath dubins_path_sample_many #self.dubins.outPath %d" + , #self.dubins.outPath + ) + if self.dubins.outPath and #self.dubins.outPath > 0 then + local fromCell = nil + for i, wayPoint in ipairs(self.dubins.outPath) do + local cell = get_node(wayPoint.x, wayPoint.z) + cell.worldPos = {x = wayPoint.x, y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, wayPoint.x, 1, wayPoint.z), z = wayPoint.z} + cell.t = wayPoint.t + cell.incomming = fromCell + fromCell = cell + if not self:isDriveableDubins(cell) then + result = ADDubins.EDUBNOPATH + break + end + end + else + result = ADDubins.EDUBNOPATH + end + end + return result + end + + self.dubins.outPath = {} + result = self.dubins:dubins_shortest_path(ADDubins.DubinsPath, self.q0, self.q1, self.minTurnRadius) + PathFinderModule.debugMsg(self.vehicle, "PFM:getDubinsPath dubins_shortest_path result %d" + , result + ) + if result == ADDubins.EDUBOK then + result = checkPath() + if result == ADDubins.EDUBOK then + PathFinderModule.debugMsg(self.vehicle, "PFM:getDubinsPath found shortest path #self.dubins.outPath %d result %d" + , #self.dubins.outPath + , result + ) + self.dubinsPath = self.dubins.outPath + return self.dubinsPath + end + end + + for i = 1, 6, 1 do + self.dubins.outPath = {} + result = self.dubins:dubins_path(ADDubins.DubinsPath, self.q0, self.q1, self.minTurnRadius, i) + PathFinderModule.debugMsg(self.vehicle, "PFM:getDubinsPath dubins_path i %d result %d" + , i + , result + ) + if result == ADDubins.EDUBOK then + result = checkPath() + end + if result == ADDubins.EDUBOK then + break + end + end + PathFinderModule.debugMsg(self.vehicle, "PFM:getDubinsPath #self.dubins.outPath %d result %d" + , #self.dubins.outPath + , result + ) + if result == ADDubins.EDUBOK then + self.dubinsPath = self.dubins.outPath + end + + diffNetTime = netGetTime() - diffNetTime + PathFinderModule.debugMsg(self.vehicle, "PFM:getDubinsPath end diffNetTime %d" + , diffNetTime + ) + return self.dubinsPath +end + +function PathFinderModule:collisionTestCallback(transformId) + if transformId ~= 0 and transformId ~= g_currentMission.terrainRootNode then + local collisionObject = g_currentMission:getNodeObject(transformId) + if (collisionObject == nil) or (collisionObject ~= nil and not (collisionObject.rootVehicle == self.vehicle)) then + self.collisionhits = self.collisionhits + 1 + if PathFinderModule.debug == true then + local currentCollMask = getCollisionMask(transformId) + if currentCollMask then + local x, _, z = getWorldTranslation(transformId) + x = x + g_currentMission.mapWidth/2 + z = z + g_currentMission.mapHeight/2 + + PathFinderModule.debugMsg(collisionObject, "PathFinderModule:collisionTestCallback transformId ->%s<- collisionObject ->%s<- getRigidBodyType->transformId %s getName->transformId %s getNodePath %s" + , tostring(transformId) + , tostring(collisionObject) + , tostring(getRigidBodyType(transformId)) + , tostring(getName(transformId)) + , tostring(I3DUtil.getNodePath(transformId)) + ) + if collisionObject then + PathFinderModule.debugMsg(collisionObject, "PathFinderModule:collisionTestCallback xmlFilename ->%s<-" + , tostring(collisionObject.xmlFilename) + ) + end + PathFinderModule.debugMsg(collisionObject, "PathFinderModule:collisionTestCallback xz %.0f %.0f currentCollMask %s" + , x, z + , MathUtil.numberToSetBitsStr(currentCollMask) + ) + end + end + end + end +end + +function PathFinderModule.debugMsg(vehicle, debugText, ...) + if PathFinderModule.debug == true then + AutoDrive.debugMsg(vehicle, debugText, ...) + else + AutoDrive.debugPrint(vehicle, AutoDrive.DC_PATHINFO, debugText, ...) + end end diff --git a/scripts/Sensors/VirtualSensors.lua b/scripts/Sensors/VirtualSensors.lua index a689b4c6..ddaeece4 100644 --- a/scripts/Sensors/VirtualSensors.lua +++ b/scripts/Sensors/VirtualSensors.lua @@ -234,9 +234,11 @@ function ADSensor:getLocationByPosition() --local frontToolLength = 0 --AutoDrive.getFrontToolLength(self.vehicle) --lengthOffset = frontToolLength / 2 --end - location = self:getRotatedFront() - if location == nil then + local front = self:getRotatedFront() + if front == nil then location = {x = 0, z = vehicle.size.length / 2} + else + location = front end --location.z = location.z + AutoDrive.getVehicleLeadingEdge(vehicle) --location.z = location.z + 2 @@ -387,7 +389,7 @@ function ADSensor:updateSensor(dt) end end -function ADSensor:onUpdate() +function ADSensor:onUpdate(dt) Logging.warning("[AutoDrive] ADSensor:onUpdate() called - Please override this in instance class") end diff --git a/scripts/Settings.lua b/scripts/Settings.lua index 8321816a..0ddbc8b3 100644 --- a/scripts/Settings.lua +++ b/scripts/Settings.lua @@ -1047,6 +1047,18 @@ AutoDrive.settings.scaleLines = { isUserSpecific = true } +AutoDrive.settings.scaleMarkerText = { + values = {0.5, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10}, + texts = {"50%", "100%", "200%", "300%", "400%", "500%", "600%", "700%", "800%", "900%", "1000%"}, + default = 2, + current = 2, + text = "gui_ad_scaleMarkerText", + tooltip = "gui_ad_scaleMarkerText_tooltip", + translate = false, + isVehicleSpecific = false, + isUserSpecific = true +} + AutoDrive.settings.remainingDriveTimeInterval = { values = {0, 1, 3, 5, 10, 20, 30, 60}, texts = {"gui_ad_off", "1s", "3s", "5s", "10s", "20s", "30s", "60s"}, diff --git a/scripts/Specialization.lua b/scripts/Specialization.lua index 53ce4e91..2796be7b 100644 --- a/scripts/Specialization.lua +++ b/scripts/Specialization.lua @@ -361,17 +361,6 @@ function AutoDrive:onUpdateTick(dt, isActiveForInput, isActiveForInputIgnoreSele -- self:resetClosestWayPoint() -- if we want to update distances every frame, when lines drawing is enabled, we can move this at the end of onDraw function - if AutoDrive.isEditorShowEnabled() then - local x, y, z = getWorldTranslation(self.components[1].node) - local distance = MathUtil.vector2Length(x - self.ad.lastDrawPosition.x, z - self.ad.lastDrawPosition.z) - if distance > AutoDrive.drawDistance / 2 then - self.ad.lastDrawPosition = {x = x, z = z} - self:resetWayPointsDistance() - end - else - self:resetWayPointsDistance() - end - if self.isServer then self.ad.recordingModule:updateTick(dt, isActiveForInput, isActiveForInputIgnoreSelection, isSelected) @@ -761,6 +750,7 @@ function AutoDrive:onEnterVehicle(isControlling) self.ad.stateModule:setActualFarmId(self.ad.stateModule:getPlayerFarmId()) -- onEnterVehicle end end + self:resetWayPointsDistance() end function AutoDrive:onLeaveVehicle(wasEntered) @@ -810,11 +800,26 @@ function AutoDrive:onDrawEditorMode() and not AutoDrive.leftALTmodifierKeyPressed and not AutoDrive.rightSHIFTmodifierKeyPressed + if AutoDrive.isEditorShowEnabled() or AutoDrive.isInExtendedEditorMode() then + local x, y, z = getWorldTranslation(self.components[1].node) + local distance = MathUtil.vector2Length(x - self.ad.lastDrawPosition.x, z - self.ad.lastDrawPosition.z) + if distance > AutoDrive.drawDistance / 2 then + self.ad.lastDrawPosition = {x = x, z = z} + self:resetWayPointsDistance() + end + end + + if AutoDrive:getIsEntered(self) and ADGraphManager:hasChanges() then + self:resetWayPointsDistance() + ADGraphManager:resetChanges() + end + --Draw close destinations for _, marker in pairs(ADGraphManager:getMapMarkers()) do local wp = ADGraphManager:getWayPointById(marker.id) if MathUtil.vector2Length(wp.x - x1, wp.z - z1) < maxDistance then - Utils.renderTextAtWorldPosition(wp.x, wp.y + 4, wp.z, marker.name, getCorrectTextSize(0.013), 0) + local scale = AutoDrive.getSetting("scaleMarkerText") or 1 + Utils.renderTextAtWorldPosition(wp.x, wp.y + 4, wp.z, marker.name, getCorrectTextSize(0.013) * scale, 0) DrawingManager:addMarkerTask(wp.x, wp.y + 0.45, wp.z) end end @@ -1034,7 +1039,6 @@ function AutoDrive:startAutoDrive() if self.isServer then if not self.ad.stateModule:isActive() then self.ad.stateModule:setActive(true) - self.ad.stateModule:setLoopsDone(0) self.ad.isStoppingWithError = false self.ad.onRouteToPark = false @@ -1137,7 +1141,6 @@ function AutoDrive:stopAutoDrive() self.spec_aiVehicle.aiTrafficCollisionTranslation[2] = 0 end - self.ad.stateModule:setLoopsDone(0) self.ad.stateModule:setActive(false) self.ad.taskModule:abortAllTasks() @@ -1455,9 +1458,7 @@ function AutoDrive:getClosestWayPoint(noUpdate) end function AutoDrive:getClosestNotReversedWayPoint() - if self.ad.distances.closestNotReverse.wayPoint == -1 then - self:updateWayPointsDistance() - end + self:updateWayPointsDistance() if self.ad.distances.closestNotReverse.wayPoint ~= nil then return self.ad.distances.closestNotReverse.wayPoint.id, self.ad.distances.closestNotReverse.distance end @@ -1478,9 +1479,7 @@ function AutoDrive:getWayPointsInRange(minDistance, maxDistance) end function AutoDrive:getWayPointIdsInRange(minDistance, maxDistance) - if self.ad.distances.wayPoints == nil then - self:updateWayPointsDistance() - end + self:updateWayPointsDistance() local inRange = {} for _, elem in pairs(self.ad.distances.wayPoints) do if elem.distance >= minDistance and elem.distance <= maxDistance and elem.wayPoint.id > 0 then diff --git a/scripts/Tasks/CatchCombinePipeTask.lua b/scripts/Tasks/CatchCombinePipeTask.lua index e25d9fae..5c5f37eb 100644 --- a/scripts/Tasks/CatchCombinePipeTask.lua +++ b/scripts/Tasks/CatchCombinePipeTask.lua @@ -1,13 +1,14 @@ CatchCombinePipeTask = ADInheritsFrom(AbstractTask) +CatchCombinePipeTask.debug = false CatchCombinePipeTask.TARGET_DISTANCE = 15 -CatchCombinePipeTask.STATE_PATHPLANNING = 1 -CatchCombinePipeTask.STATE_DRIVING = 2 -CatchCombinePipeTask.STATE_REVERSING = 3 -CatchCombinePipeTask.STATE_DELAY_PATHPLANNING = 4 -CatchCombinePipeTask.STATE_WAIT_BEFORE_FINISH = 5 -CatchCombinePipeTask.STATE_FINISHED = 6 +CatchCombinePipeTask.STATE_PATHPLANNING = {} +CatchCombinePipeTask.STATE_DRIVING = {} +CatchCombinePipeTask.STATE_REVERSING = {} +CatchCombinePipeTask.STATE_DELAY_PATHPLANNING = {} +CatchCombinePipeTask.STATE_WAIT_BEFORE_FINISH = {} +CatchCombinePipeTask.STATE_FINISHED = {} CatchCombinePipeTask.MAX_REVERSE_DISTANCE = 18 CatchCombinePipeTask.MIN_COMBINE_DISTANCE = 25 @@ -29,11 +30,12 @@ function CatchCombinePipeTask:new(vehicle, combine) o.taskType = "CatchCombinePipeTask" o.newPathFindingCounter = 0 o.trailers = nil + CatchCombinePipeTask.setStateNames(o) return o end function CatchCombinePipeTask:setUp() - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CatchCombinePipeTask:setUp()") + CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:setUp()") local angleToCombineHeading = self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD]:getAngleToCombineHeading() local angleToCombine = self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD]:getAngleToCombine() @@ -50,82 +52,96 @@ function CatchCombinePipeTask:update(dt) self:finished() return end + if self.lastState ~= self.state then + CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:update %s -> %s", tostring(self:getStateName(self.lastState)), tostring(self:getStateName())) + self.lastState = self.state + self:resetAllTimers() + end + + local checkForStuck = (self.vehicle.lastSpeedReal <= 0.0002) and ( + self.state == CatchCombinePipeTask.STATE_DRIVING + ) + + self.stuckTimer:timer(checkForStuck, self.MAX_STUCK_TIME, dt) + if self.stuckTimer:done() + -- or AutoDrive.getDistanceBetween(self.vehicle, self.combine) < self.MIN_COMBINE_DISTANCE + then + -- got stuck or to close to combine -> reverse + CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:update - STATE_DRIVING stuck") + self.stuckTimer:timer(false) + local x, y, z = getWorldTranslation(self.vehicle.components[1].node) + self.reverseStartLocation = {x = x, y = y, z = z} + self.state = CatchCombinePipeTask.STATE_REVERSING + return + end if self.state == CatchCombinePipeTask.STATE_PATHPLANNING then if self.vehicle.ad.pathFinderModule:hasFinished() then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CatchCombinePipeTask:update - STATE_PATHPLANNING finished") + CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:update - STATE_PATHPLANNING finished") self.newPathFindingCounter = 0 self.wayPoints = self.vehicle.ad.pathFinderModule:getPath() if self.wayPoints == nil or #self.wayPoints < 1 then --restart self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD]:notifyAboutFailedPathfinder() --AutoDriveMessageEvent.sendMessageOrNotification(self.vehicle, ADMessagesManager.messageTypes.WARN, "$l10n_AD_Driver_of; %s $l10n_AD_cannot_find_path; %s", 5000, self.vehicle.ad.stateModule:getName(), self.combine.ad.stateModule:getName()) - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CatchCombinePipeTask:update - STATE_PATHPLANNING restarting path finder - with delay") + CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:update - STATE_PATHPLANNING restarting path finder - with delay") self.state = CatchCombinePipeTask.STATE_DELAY_PATHPLANNING return else self.vehicle.ad.drivePathModule:setWayPoints(self.wayPoints) - self.stuckTimer:timer(false) self.state = CatchCombinePipeTask.STATE_DRIVING + return end else self.vehicle.ad.pathFinderModule:update(dt) self.vehicle.ad.specialDrivingModule:stopVehicle() self.vehicle.ad.specialDrivingModule:update(dt) + return end elseif self.state == CatchCombinePipeTask.STATE_DELAY_PATHPLANNING then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CatchCombinePipeTask:update - STATE_DELAY_PATHPLANNING") + CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:update - STATE_DELAY_PATHPLANNING") if self.waitForCheckTimer:timer(true, 1000, dt) then - if self:startNewPathFinding() then - self.vehicle.ad.pathFinderModule:addDelayTimer(6000) - self.state = CatchCombinePipeTask.STATE_PATHPLANNING - end if self.newPathFindingCounter > self.MAX_COUNT_NEW_PATHFINDING then -- prevent deadlock self.state = CatchCombinePipeTask.STATE_FINISHED return end + if self:startNewPathFinding() then + self.vehicle.ad.pathFinderModule:addDelayTimer(6000) + self.state = CatchCombinePipeTask.STATE_PATHPLANNING + return + end end self.vehicle.ad.specialDrivingModule:stopVehicle() self.vehicle.ad.specialDrivingModule:update(dt) + return elseif self.state == CatchCombinePipeTask.STATE_DRIVING then -- check if this is still a clever path to follow -- do this by distance of the combine to the last location pathfinder started at local x, y, z = getWorldTranslation(self.combine.components[1].node) local combineTravelDistance = MathUtil.vector2Length(x - self.combinesStartLocation.x, z - self.combinesStartLocation.z) - self.stuckTimer:timer(self.vehicle.lastSpeedReal <= 0.0002, self.MAX_STUCK_TIME, dt) - if self.stuckTimer:done() - -- or AutoDrive.getDistanceBetween(self.vehicle, self.combine) < self.MIN_COMBINE_DISTANCE - then - -- got stuck or to close to combine -> reverse - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CatchCombinePipeTask:update - STATE_DRIVING stuck") - self.stuckTimer:timer(false) - local x, y, z = getWorldTranslation(self.vehicle.components[1].node) - self.reverseStartLocation = {x = x, y = y, z = z} - self.state = CatchCombinePipeTask.STATE_REVERSING - return - end - if combineTravelDistance > 85 then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CatchCombinePipeTask:update - combine travelled - recalculate path") + CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:update - combine travelled - recalculate path") self.waitForCheckTimer.elapsedTime = 4000 self.state = CatchCombinePipeTask.STATE_DELAY_PATHPLANNING + return else if self.vehicle.ad.drivePathModule:isTargetReached() then -- check if we have actually reached the target or not -- accept current location if we are in a good position to start chasing: distance and angle are important here - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CatchCombinePipeTask:update - STATE_DRIVING TargetReached") + CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:update - STATE_DRIVING TargetReached") local angleToCombine = self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD]:getAngleToCombineHeading() local isCorrectSide = self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD]:isUnloaderOnCorrectSide() if angleToCombine < 35 and AutoDrive.getDistanceBetween(self.vehicle, self.combine) < 80 and isCorrectSide then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CatchCombinePipeTask:update - STATE_DRIVING -> STATE_FINISHED") + CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:update - STATE_DRIVING -> STATE_FINISHED") self.state = CatchCombinePipeTask.STATE_FINISHED return else - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CatchCombinePipeTask:update - angle or distance to combine too high - recalculate path now") + CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:update - angle or distance to combine too high - recalculate path now") self.state = CatchCombinePipeTask.STATE_DELAY_PATHPLANNING + return end else self.vehicle.ad.drivePathModule:update(dt) @@ -136,24 +152,24 @@ function CatchCombinePipeTask:update(dt) local distanceToReverseStart = MathUtil.vector2Length(x - self.reverseStartLocation.x, z - self.reverseStartLocation.z) self.reverseTimer:timer(true, self.MAX_REVERSE_TIME, dt) if distanceToReverseStart > self.MAX_REVERSE_DISTANCE or self.reverseTimer:done() then - self.reverseTimer:timer(false) self.state = CatchCombinePipeTask.STATE_WAIT_BEFORE_FINISH + return else self.vehicle.ad.specialDrivingModule:driveReverse(dt, 15, 1, self.vehicle.ad.trailerModule:canBeHandledInReverse()) end elseif self.state == CatchCombinePipeTask.STATE_WAIT_BEFORE_FINISH then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CatchCombinePipeTask:update - STATE_WAIT_BEFORE_FINISH") + CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:update - STATE_WAIT_BEFORE_FINISH") self.waitTimer:timer(true, self.MAX_REVERSE_TIME, dt) if self.waitTimer:done() then - self.waitTimer:timer(false) self.state = CatchCombinePipeTask.STATE_FINISHED return else self.vehicle.ad.specialDrivingModule:stopVehicle() self.vehicle.ad.specialDrivingModule:update(dt) + return end elseif self.state == CatchCombinePipeTask.STATE_FINISHED then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CatchCombinePipeTask:update - STATE_FINISHED") + CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:update - STATE_FINISHED") self:finished() return end @@ -163,12 +179,12 @@ function CatchCombinePipeTask:abort() end function CatchCombinePipeTask:finished(propagate) - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CatchCombinePipeTask:finished()") + CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:finished()") self.vehicle.ad.taskModule:setCurrentTaskFinished(propagate) end function CatchCombinePipeTask:startNewPathFinding() - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CatchCombinePipeTask:startNewPathFinding()") + CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:startNewPathFinding()") local pipeChasePos, pipeChaseSide = self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD]:getPipeChasePosition(true) local x, _, z = getWorldTranslation(self.combine.components[1].node) local targetFieldId = g_farmlandManager:getFarmlandIdAtWorldPosition(pipeChasePos.x, pipeChasePos.z) @@ -181,7 +197,7 @@ function CatchCombinePipeTask:startNewPathFinding() local cFillRatio = cfillCapacity > 0 and cfillLevel / cfillCapacity or 0 if cFillRatio > 0.91 or cfillFreeCapacity < 0.1 then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CatchCombinePipeTask:startNewPathFinding() - Combine is almost full - dont chase for active unloading anymore") + CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:startNewPathFinding() - Combine is almost full - dont chase for active unloading anymore") self:finished(ADTaskModule.DONT_PROPAGATE) self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD]:setToWaitForCall() return false @@ -192,14 +208,15 @@ function CatchCombinePipeTask:startNewPathFinding() if self.combine.ad.isChopper or (pipeChaseSide ~= AutoDrive.CHASEPOS_REAR or (targetFieldId == combineFieldId and cFillRatio <= 0.85)) then -- if self.combine:getIsBufferCombine() or (pipeChaseSide ~= AutoDrive.CHASEPOS_REAR and targetFieldId == combineFieldId and cFillRatio <= 0.85) then -- is chopper or chase not rear and harvester on correct field and filled < 85% - i.e. combine pipe not in fruit - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CatchCombinePipeTask:startNewPathFinding() - chase pos looks good - calculate path to it...") + CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:startNewPathFinding() - chase pos looks good - calculate path to it...") + self.vehicle.ad.pathFinderModule:reset() self.vehicle.ad.pathFinderModule:startPathPlanningToPipe(self.combine, false) -- use false to enable pathfinder fallback self.combinesStartLocation = {} self.combinesStartLocation.x, self.combinesStartLocation.y, self.combinesStartLocation.z = getWorldTranslation(self.combine.components[1].node) return true else - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "CatchCombinePipeTask:startNewPathFinding() - chase pos looks bad, is not on the same field or combine's fill level is approaching limit - aborting for now") + CatchCombinePipeTask.debugMsg(self.vehicle, "CatchCombinePipeTask:startNewPathFinding() - chase pos looks bad, is not on the same field or combine's fill level is approaching limit - aborting for now") self.waitForCheckTimer:timer(false) end return false @@ -213,11 +230,40 @@ function CatchCombinePipeTask:getExcludedVehiclesForCollisionCheck() return excludedVehicles end +function CatchCombinePipeTask:setStateNames() + if self.statesToNames == nil then + self.statesToNames = {} + for name, id in pairs(CatchCombinePipeTask) do + if string.sub(name, 1, 6) == "STATE_" then + self.statesToNames[id] = name + end + end + end +end + +function CatchCombinePipeTask:getStateName(state) + local requestedState = state + if requestedState == nil then + requestedState = self.state + end + if requestedState == nil then + Logging.error("[AD] CatchCombinePipeTask: Could not find name for state ->%s<- !", tostring(requestedState)) + end + return self.statesToNames[requestedState] or "" +end + +function CatchCombinePipeTask:resetAllTimers() + -- self.stuckTimer:timer(false) -- stuckTimer reset by speed changes + self.reverseTimer:timer(false) + self.waitTimer:timer(false) + self.waitForCheckTimer:timer(false) +end + function CatchCombinePipeTask:getI18nInfo() local text = "$l10n_AD_task_catch_up_with_combine;" if self.state == CatchCombinePipeTask.STATE_PATHPLANNING then - local actualState, maxStates = self.vehicle.ad.pathFinderModule:getCurrentState() - text = text .. " - " .. "$l10n_AD_task_pathfinding;" .. string.format(" %d / %d ", actualState, maxStates) + local actualState, maxStates, steps, max_pathfinder_steps = self.vehicle.ad.pathFinderModule:getCurrentState() + text = text .. " - " .. "$l10n_AD_task_pathfinding;" .. string.format(" %d / %d - %d / %d", actualState, maxStates, steps, max_pathfinder_steps) elseif self.state == CatchCombinePipeTask.STATE_DELAY_PATHPLANNING then text = text .. " - " .. "$l10n_AD_task_unload_area_in_fruit;" elseif self.state == CatchCombinePipeTask.STATE_REVERSING then @@ -231,5 +277,7 @@ end function CatchCombinePipeTask.debugMsg(vehicle, debugText, ...) if CatchCombinePipeTask.debug == true then AutoDrive.debugMsg(vehicle, debugText, ...) + else + AutoDrive.debugPrint(vehicle, AutoDrive.DC_COMBINEINFO, debugText, ...) end end diff --git a/scripts/Tasks/ClearCropTask.lua b/scripts/Tasks/ClearCropTask.lua index 819b69a4..c59401d3 100644 --- a/scripts/Tasks/ClearCropTask.lua +++ b/scripts/Tasks/ClearCropTask.lua @@ -1,24 +1,36 @@ ClearCropTask = ADInheritsFrom(AbstractTask) +ClearCropTask.debug = false ClearCropTask.TARGET_DISTANCE_SIDE = 10 ClearCropTask.TARGET_DISTANCE_FRONT_STEP = 10 -ClearCropTask.STATE_CLEARING = 1 -ClearCropTask.STATE_REVERSING = 2 +ClearCropTask.MAX_HARVESTER_DISTANCE = 50 +ClearCropTask.WAIT_TIME = 10000 +ClearCropTask.DRIVE_TIME = 30000 +ClearCropTask.STUCK_TIME = 60000 +ClearCropTask.STATE_CLEARING_FIRST = {} +ClearCropTask.STATE_CLEARING_SECOND = {} +ClearCropTask.STATE_REVERSING = {} +ClearCropTask.STATE_WAITING = {} ClearCropTask.LEFT = 1 ClearCropTask.RIGHT = -1 -function ClearCropTask:new(vehicle) +function ClearCropTask:new(vehicle, harvester) local o = ClearCropTask:create() o.vehicle = vehicle + o.harvester = harvester + o.waitTimer = AutoDriveTON:new() + o.driveTimer = AutoDriveTON:new() o.stuckTimer = AutoDriveTON:new() - o.state = ClearCropTask.STATE_CLEARING + o.state = ClearCropTask.STATE_WAITING o.reverseStartLocation = nil + o.vehicleTrainLength = AutoDrive.getTractorTrainLength(vehicle, true, false) + ClearCropTask.setStateNames(o) return o end function ClearCropTask:setUp() - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "Setting up ClearCropTask") + ClearCropTask.debugMsg(self.vehicle, "ClearCropTask:setUp") local leftBlocked = self.vehicle.ad.sensors.leftSensorFruit:pollInfo() or self.vehicle.ad.sensors.leftSensor:pollInfo() local rightBlocked = self.vehicle.ad.sensors.rightSensorFruit:pollInfo() or self.vehicle.ad.sensors.rightSensor:pollInfo() @@ -28,15 +40,6 @@ function ClearCropTask:setUp() leftBlocked = leftBlocked or leftFrontBlocked rightBlocked = rightBlocked or rightFrontBlocked - -- prefer side where front is also free - if (not leftBlocked) and (not rightBlocked) then - if (not leftFrontBlocked) and rightFrontBlocked then - rightBlocked = true - elseif leftFrontBlocked and (not rightFrontBlocked) then - leftBlocked = true - end - end - local cleartowards = ClearCropTask.RIGHT if leftBlocked and rightBlocked then cleartowards = ClearCropTask.RIGHT @@ -47,58 +50,154 @@ function ClearCropTask:setUp() end self.wayPoints = {} - table.insert(self.wayPoints, AutoDrive.createWayPointRelativeToVehicle(self.vehicle, (ClearCropTask.TARGET_DISTANCE_SIDE / 2) * cleartowards, ClearCropTask.TARGET_DISTANCE_FRONT_STEP * 0.5)) - table.insert(self.wayPoints, AutoDrive.createWayPointRelativeToVehicle(self.vehicle, ClearCropTask.TARGET_DISTANCE_SIDE * cleartowards, ClearCropTask.TARGET_DISTANCE_FRONT_STEP * 1)) - table.insert(self.wayPoints, AutoDrive.createWayPointRelativeToVehicle(self.vehicle, ClearCropTask.TARGET_DISTANCE_SIDE * cleartowards, ClearCropTask.TARGET_DISTANCE_FRONT_STEP * 2)) - table.insert(self.wayPoints, AutoDrive.createWayPointRelativeToVehicle(self.vehicle, ClearCropTask.TARGET_DISTANCE_SIDE * cleartowards, ClearCropTask.TARGET_DISTANCE_FRONT_STEP * 3)) - table.insert(self.wayPoints, AutoDrive.createWayPointRelativeToVehicle(self.vehicle, ClearCropTask.TARGET_DISTANCE_SIDE * cleartowards, ClearCropTask.TARGET_DISTANCE_FRONT_STEP * 4)) + if self.harvester then + local distance = AutoDrive.getDistanceBetween(self.vehicle, self.harvester) + ClearCropTask.debugMsg(self.harvester, "ClearCropTask:setUp distance %.0f" + , distance + ) + end + if self.harvester and AutoDrive.getDistanceBetween(self.vehicle, self.harvester) < ClearCropTask.MAX_HARVESTER_DISTANCE then + table.insert(self.wayPoints, AutoDrive.createWayPointRelativeToVehicle(self.harvester, 0, self.vehicleTrainLength * 1)) + table.insert(self.wayPoints, AutoDrive.createWayPointRelativeToVehicle(self.harvester, 0, self.vehicleTrainLength * 2)) + table.insert(self.wayPoints, AutoDrive.createWayPointRelativeToVehicle(self.harvester, 0, self.vehicleTrainLength * 3)) + else + table.insert(self.wayPoints, AutoDrive.createWayPointRelativeToVehicle(self.vehicle, (ClearCropTask.TARGET_DISTANCE_SIDE / 2) * cleartowards, ClearCropTask.TARGET_DISTANCE_FRONT_STEP * 0.5)) + table.insert(self.wayPoints, AutoDrive.createWayPointRelativeToVehicle(self.vehicle, ClearCropTask.TARGET_DISTANCE_SIDE * cleartowards, ClearCropTask.TARGET_DISTANCE_FRONT_STEP * 1)) + table.insert(self.wayPoints, AutoDrive.createWayPointRelativeToVehicle(self.vehicle, ClearCropTask.TARGET_DISTANCE_SIDE * cleartowards, ClearCropTask.TARGET_DISTANCE_FRONT_STEP * 2)) + table.insert(self.wayPoints, AutoDrive.createWayPointRelativeToVehicle(self.vehicle, ClearCropTask.TARGET_DISTANCE_SIDE * cleartowards, ClearCropTask.TARGET_DISTANCE_FRONT_STEP * 3)) + table.insert(self.wayPoints, AutoDrive.createWayPointRelativeToVehicle(self.vehicle, ClearCropTask.TARGET_DISTANCE_SIDE * cleartowards, ClearCropTask.TARGET_DISTANCE_FRONT_STEP * 4)) + end self.vehicle.ad.drivePathModule:setWayPoints(self.wayPoints) end function ClearCropTask:update(dt) - if self.state == ClearCropTask.STATE_CLEARING then - -- Check if the driver and trailers have left the crop yet - if not AutoDrive.isVehicleOrTrailerInCrop(self.vehicle, true) then + if self.lastState ~= self.state then + ClearCropTask.debugMsg(self.vehicle, "ClearCropTask:update %s -> %s", tostring(self:getStateName(self.lastState)), tostring(self:getStateName())) + self.lastState = self.state + end + + -- Check if the driver and trailers have left the crop yet + if not AutoDrive.isVehicleOrTrailerInCrop(self.vehicle, true) then + ClearCropTask.debugMsg(self.vehicle, "ClearCropTask:update not isVehicleOrTrailerInCrop") + self:finished() + return + end + self.stuckTimer:timer(true, ClearCropTask.STUCK_TIME, dt) + if self.stuckTimer:done() then + ClearCropTask.debugMsg(self.vehicle, "ClearCropTask:update stuckTimer:done") + self:finished() + return + end + + if self.state == ClearCropTask.STATE_WAITING then + self.waitTimer:timer(true, ClearCropTask.WAIT_TIME, dt) + if self.waitTimer:done() then + ClearCropTask.debugMsg(self.vehicle, "ClearCropTask:update STATE_WAITING - done waiting - clear now...") + self:resetAllTimers() + self.vehicle.ad.drivePathModule:setWayPoints(self.wayPoints) + self.state = ClearCropTask.STATE_CLEARING_FIRST + return + end + elseif self.state == ClearCropTask.STATE_CLEARING_FIRST then + self.driveTimer:timer(true, ClearCropTask.DRIVE_TIME, dt) + if self.vehicle.ad.drivePathModule:isTargetReached() then + ClearCropTask.debugMsg(self.vehicle, "ClearCropTask:update 1 isTargetReached") self:finished() + return + elseif self.driveTimer:done() then + ClearCropTask.debugMsg(self.vehicle, "ClearCropTask:update 1 driveTimer:done") + self:resetAllTimers() + local x, y, z = getWorldTranslation(self.vehicle.components[1].node) + self.reverseStartLocation = {x = x, y = y, z = z} + self.state = ClearCropTask.STATE_REVERSING + return else - if self.vehicle.ad.drivePathModule:isTargetReached() then - self:finished() - elseif self.stuckTimer:done() then - local x, y, z = getWorldTranslation(self.vehicle.components[1].node) - self.reverseStartLocation = {x = x, y = y, z = z} - self.state = ClearCropTask.STATE_REVERSING - else - self.stuckTimer:timer(true, 30000, dt) - self.vehicle.ad.drivePathModule:update(dt) - end + self.vehicle.ad.drivePathModule:update(dt) end elseif self.state == ClearCropTask.STATE_REVERSING then local x, y, z = getWorldTranslation(self.vehicle.components[1].node) - self.stuckTimer:timer(false) local distanceToReversStart = MathUtil.vector2Length(x - self.reverseStartLocation.x, z - self.reverseStartLocation.z) - if not AutoDrive.isVehicleOrTrailerInCrop(self.vehicle, true) then - self:finished() - elseif distanceToReversStart > 20 then - self.state = ClearCropTask.STATE_CLEARING + if (g_updateLoopIndex % 60 == 0) then + ClearCropTask.debugMsg(self.vehicle, "ClearCropTask:update distanceToReversStart %.0f" + , distanceToReversStart + ) + end + if distanceToReversStart > 20 then + ClearCropTask.debugMsg(self.vehicle, "ClearCropTask:update distanceToReversStart > 20") + self:resetAllTimers() + self.vehicle.ad.drivePathModule:setWayPoints(self.wayPoints) + self.state = ClearCropTask.STATE_CLEARING_SECOND + return else self.vehicle.ad.specialDrivingModule:driveReverse(dt, 15, 1, self.vehicle.ad.trailerModule:canBeHandledInReverse()) end + elseif self.state == ClearCropTask.STATE_CLEARING_SECOND then + if self.vehicle.ad.drivePathModule:isTargetReached() then + ClearCropTask.debugMsg(self.vehicle, "ClearCropTask:update 2 isTargetReached") + self:finished() + return + else + self.vehicle.ad.drivePathModule:update(dt) + end end end function ClearCropTask:abort() + ClearCropTask.debugMsg(self.vehicle, "ClearCropTask:abort") end function ClearCropTask:finished() - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "ClearCropTask:finished()") + ClearCropTask.debugMsg(self.vehicle, "ClearCropTask:finished") self.vehicle.ad.taskModule:setCurrentTaskFinished() end -function ClearCropTask:getInfoText() - return g_i18n:getText("AD_task_clearcrop") +function ClearCropTask:setStateNames() + if self.statesToNames == nil then + self.statesToNames = {} + for name, id in pairs(ClearCropTask) do + if string.sub(name, 1, 6) == "STATE_" then + self.statesToNames[id] = name + end + end + end +end + +function ClearCropTask:getStateName(state) + local requestedState = state + if requestedState == nil then + requestedState = self.state + end + if requestedState == nil then + Logging.error("[AD] ClearCropTask: Could not find name for state ->%s<- !", tostring(requestedState)) + end + return self.statesToNames[requestedState] or "" +end + +function ClearCropTask:resetAllTimers() + -- self.stuckTimer:timer(false) -- stuckTimer reset by speed changes + self.waitTimer:timer(false) + self.driveTimer:timer(false) end function ClearCropTask:getI18nInfo() - return "$l10n_AD_task_clearcrop;" + local text = "$l10n_AD_task_clearcrop;" + if self.state == ClearCropTask.STATE_CLEARING_FIRST then + text = text .. " - 1/2" + elseif self.state == ClearCropTask.STATE_CLEARING_SECOND then + text = text .. " - 2/2" + elseif self.state == ClearCropTask.STATE_REVERSING then + text = text .. " - " .. "$l10n_AD_task_reversing_from_combine;" + elseif self.state == ClearCropTask.STATE_WAITING then + text = text .. " - " .. "$l10n_AD_task_waiting_for_room;" + end + return text +end + +function ClearCropTask.debugMsg(vehicle, debugText, ...) + if ClearCropTask.debug == true then + AutoDrive.debugMsg(vehicle, debugText, ...) + else + AutoDrive.debugPrint(vehicle, AutoDrive.DC_COMBINEINFO, debugText, ...) + end end diff --git a/scripts/Tasks/DriveToDestinationTask.lua b/scripts/Tasks/DriveToDestinationTask.lua index 193b7a26..cff397eb 100644 --- a/scripts/Tasks/DriveToDestinationTask.lua +++ b/scripts/Tasks/DriveToDestinationTask.lua @@ -17,6 +17,7 @@ function DriveToDestinationTask:setUp() self.vehicle.ad.trainModule:setPathTo(self.destinationID) elseif ADGraphManager:getDistanceFromNetwork(self.vehicle) > 30 then self.state = DriveToDestinationTask.STATE_PATHPLANNING + self.vehicle.ad.pathFinderModule:reset() self.vehicle.ad.pathFinderModule:startPathPlanningToNetwork(self.destinationID) else self.state = DriveToDestinationTask.STATE_DRIVING @@ -63,8 +64,8 @@ end function DriveToDestinationTask:getI18nInfo() if self.state == DriveToDestinationTask.STATE_PATHPLANNING then - local actualState, maxStates = self.vehicle.ad.pathFinderModule:getCurrentState() - return "$l10n_AD_task_pathfinding;" .. string.format(" %d / %d ", actualState, maxStates) + local actualState, maxStates, steps, max_pathfinder_steps = self.vehicle.ad.pathFinderModule:getCurrentState() + return "$l10n_AD_task_pathfinding;" .. string.format(" %d / %d - %d / %d", actualState, maxStates, steps, max_pathfinder_steps) else return "$l10n_AD_task_drive_to_destination;" end diff --git a/scripts/Tasks/DriveToVehicleTask.lua b/scripts/Tasks/DriveToVehicleTask.lua index 0f94e6f5..275c3186 100644 --- a/scripts/Tasks/DriveToVehicleTask.lua +++ b/scripts/Tasks/DriveToVehicleTask.lua @@ -17,6 +17,7 @@ function DriveToVehicleTask:new(vehicle, targetVehicle) end function DriveToVehicleTask:setUp() + self.vehicle.ad.pathFinderModule:reset() self.vehicle.ad.pathFinderModule:startPathPlanningToVehicle(self.targetVehicle, DriveToVehicleTask.TARGET_DISTANCE) self.trailers, _ = AutoDrive.getAllUnits(self.vehicle) AutoDrive.setTrailerCoverOpen(self.vehicle, self.trailers, false) @@ -92,8 +93,8 @@ end function DriveToVehicleTask:getI18nInfo() if self.state == DriveToVehicleTask.STATE_PATHPLANNING then - local actualState, maxStates = self.vehicle.ad.pathFinderModule:getCurrentState() - return "$l10n_AD_task_pathfinding;" .. string.format(" %d / %d ", actualState, maxStates) + local actualState, maxStates, steps, max_pathfinder_steps = self.vehicle.ad.pathFinderModule:getCurrentState() + return "$l10n_AD_task_pathfinding;" .. string.format(" %d / %d - %d / %d", actualState, maxStates, steps, max_pathfinder_steps) else return "$l10n_AD_task_drive_to_vehicle;" end diff --git a/scripts/Tasks/EmptyHarvesterTask.lua b/scripts/Tasks/EmptyHarvesterTask.lua index 9dcd7531..8f747744 100644 --- a/scripts/Tasks/EmptyHarvesterTask.lua +++ b/scripts/Tasks/EmptyHarvesterTask.lua @@ -1,13 +1,15 @@ EmptyHarvesterTask = ADInheritsFrom(AbstractTask) +EmptyHarvesterTask.debug = false -EmptyHarvesterTask.STATE_PATHPLANNING = 1 -EmptyHarvesterTask.STATE_DRIVING = 2 -EmptyHarvesterTask.STATE_UNLOADING = 3 -EmptyHarvesterTask.STATE_REVERSING = 4 -EmptyHarvesterTask.STATE_WAITING = 5 -EmptyHarvesterTask.STATE_UNLOADING_FINISHED = 6 +EmptyHarvesterTask.STATE_PATHPLANNING = {} +EmptyHarvesterTask.STATE_DRIVING = {} +EmptyHarvesterTask.STATE_UNLOADING = {} +EmptyHarvesterTask.STATE_REVERSING = {} +EmptyHarvesterTask.STATE_WAITING = {} +EmptyHarvesterTask.STATE_UNLOADING_FINISHED = {} EmptyHarvesterTask.REVERSE_TIME = 30000 +EmptyHarvesterTask.MAX_STUCK_TIME = 60000 EmptyHarvesterTask.WAITING_TIME = 7000 function EmptyHarvesterTask:new(vehicle, combine) @@ -17,17 +19,20 @@ function EmptyHarvesterTask:new(vehicle, combine) o.state = EmptyHarvesterTask.STATE_PATHPLANNING o.wayPoints = nil o.reverseStartLocation = nil + o.stuckTimer = AutoDriveTON:new() o.reverseTimer = AutoDriveTON:new() o.waitTimer = AutoDriveTON:new() o.holdCPCombineTimer = AutoDriveTON:new() o.trailers = nil o.trailerCount = 0 o.tractorTrainLength = 0 + EmptyHarvesterTask.setStateNames(o) return o end function EmptyHarvesterTask:setUp() - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "Setting up EmptyHarvesterTask") + EmptyHarvesterTask.debugMsg(self.vehicle, "EmptyHarvesterTask:setUp") + self.vehicle.ad.pathFinderModule:reset() self.vehicle.ad.pathFinderModule:startPathPlanningToPipe(self.combine, false) self.trailers, self.trailerCount = AutoDrive.getAllUnits(self.vehicle) self.tractorTrainLength = AutoDrive.getTractorTrainLength(self.vehicle, true, false) @@ -39,6 +44,27 @@ function EmptyHarvesterTask:update(dt) self:finished() return end + if self.lastState ~= self.state then + EmptyHarvesterTask.debugMsg(self.vehicle, "EmptyHarvesterTask:update %s -> %s", tostring(self:getStateName(self.lastState)), tostring(self:getStateName())) + self.lastState = self.state + self:resetAllTimers() + end + + local checkForStuck = (self.vehicle.lastSpeedReal <= 0.0002) and ( + self.state == EmptyHarvesterTask.STATE_DRIVING + or self.state == EmptyHarvesterTask.STATE_UNLOADING + ) + + self.stuckTimer:timer(checkForStuck, self.MAX_STUCK_TIME, dt) + if self.stuckTimer:done() + -- or AutoDrive.getDistanceBetween(self.vehicle, self.combine) < self.MIN_COMBINE_DISTANCE + then + -- got stuck or to close to combine -> reverse + EmptyHarvesterTask.debugMsg(self.vehicle, "EmptyHarvesterTask:update - STATE_DRIVING stuck") + local x, y, z = getWorldTranslation(self.vehicle.components[1].node) + self.reverseStartLocation = {x = x, y = y, z = z} + self.state = EmptyHarvesterTask.STATE_REVERSING + end if self.state == EmptyHarvesterTask.STATE_PATHPLANNING then if self.vehicle.ad.pathFinderModule:hasFinished() then @@ -50,25 +76,27 @@ function EmptyHarvesterTask:update(dt) self:finished(ADTaskModule.DONT_PROPAGATE) self.vehicle:stopAutoDrive() AutoDriveMessageEvent.sendMessageOrNotification(self.vehicle, ADMessagesManager.messageTypes.WARN, "$l10n_AD_Driver_of; %s $l10n_AD_cannot_find_path; %s", 5000, self.vehicle.ad.stateModule:getName(), self.combine.ad.stateModule:getName()) + return else self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD]:notifyAboutFailedPathfinder() + self.vehicle.ad.pathFinderModule:reset() self.vehicle.ad.pathFinderModule:startPathPlanningToPipe(self.combine, false) self.vehicle.ad.pathFinderModule:addDelayTimer(10000) end else - --AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "EmptyHarvesterTask:update - next: EmptyHarvesterTask.STATE_DRIVING") self.state = EmptyHarvesterTask.STATE_DRIVING + return end else self.vehicle.ad.pathFinderModule:update(dt) self.vehicle.ad.specialDrivingModule:stopVehicle() self.vehicle.ad.specialDrivingModule:update(dt) + return end elseif self.state == EmptyHarvesterTask.STATE_DRIVING then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "EmptyHarvesterTask:update EmptyHarvesterTask.STATE_DRIVING") if self.vehicle.ad.drivePathModule:isTargetReached() then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "EmptyHarvesterTask:update - next: EmptyHarvesterTask.STATE_UNLOADING") self.state = EmptyHarvesterTask.STATE_UNLOADING + return else self.vehicle.ad.drivePathModule:update(dt) end @@ -78,9 +106,13 @@ function EmptyHarvesterTask:update(dt) AutoDrive:holdCPCombine(self.combine) --Check if the combine is moving / has already moved away and we are supposed to actively unload if self.combine.ad.driveForwardTimer.elapsedTime > 100 then + EmptyHarvesterTask.debugMsg(self.vehicle, "EmptyHarvesterTask:update driveForwardTimer.elapsedTime > 100") if AutoDrive.isVehicleOrTrailerInCrop(self.vehicle, true) then + EmptyHarvesterTask.debugMsg(self.vehicle, "EmptyHarvesterTask:update isVehicleOrTrailerInCrop") self:finished() + return elseif self.combine.ad.driveForwardTimer.elapsedTime > 4000 then + EmptyHarvesterTask.debugMsg(self.vehicle, "EmptyHarvesterTask:update driveForwardTimer.elapsedTime > 4000") self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD].state = CombineUnloaderMode.STATE_ACTIVE_UNLOAD_COMBINE self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD].breadCrumbs = Queue:new() self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD].lastBreadCrumb = nil @@ -100,27 +132,31 @@ function EmptyHarvesterTask:update(dt) local distanceToCombine = AutoDrive.getDistanceBetween(self.vehicle, self.combine) if combineFillLevel <= 0.1 or filledToUnload then - local x, y, z = getWorldTranslation(self.vehicle.components[1].node) - self.reverseStartLocation = {x = x, y = y, z = z} - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "EmptyHarvesterTask:update - next: EmptyHarvesterTask.STATE_UNLOADING_FINISHED") self.state = EmptyHarvesterTask.STATE_UNLOADING_FINISHED + return else -- Drive forward with collision checks active and only for a limited distance if distanceToCombine > 30 then + EmptyHarvesterTask.debugMsg(self.vehicle, "EmptyHarvesterTask:update distanceToCombine > 30") self:finished() + return else self.vehicle.ad.specialDrivingModule:driveForward(dt) end end end elseif self.state == EmptyHarvesterTask.STATE_UNLOADING_FINISHED then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "EmptyHarvesterTask:update - STATE_UNLOADING_FINISHED getIsCPCombineInPocket %s", tostring(AutoDrive:getIsCPCombineInPocket(self.combine))) + EmptyHarvesterTask.debugMsg(self.vehicle, "EmptyHarvesterTask:update - STATE_UNLOADING_FINISHED getIsCPCombineInPocket %s", tostring(AutoDrive:getIsCPCombineInPocket(self.combine))) if AutoDrive:getIsCPCombineInPocket(self.combine) or AutoDrive.combineIsTurning(self.combine) then -- reverse if CP unload in a pocket or pullback position -- reverse if combine is turning + local x, y, z = getWorldTranslation(self.vehicle.components[1].node) + self.reverseStartLocation = {x = x, y = y, z = z} self.state = EmptyHarvesterTask.STATE_REVERSING + return else self.state = EmptyHarvesterTask.STATE_WAITING + return end elseif self.state == EmptyHarvesterTask.STATE_REVERSING then self.vehicle.ad.specialDrivingModule.motorShouldNotBeStopped = false @@ -147,10 +183,8 @@ function EmptyHarvesterTask:update(dt) end self.reverseTimer:timer(true, EmptyHarvesterTask.REVERSE_TIME, dt) if (distanceToReversStart > overallLength) or self.reverseTimer:done() then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "EmptyHarvesterTask:update - next: EmptyHarvesterTask.STATE_WAITING") - self.holdCPCombineTimer:timer(false) - self.reverseTimer:timer(false) self.state = EmptyHarvesterTask.STATE_WAITING + return else self.vehicle.ad.specialDrivingModule:driveReverse(dt, 10, 1, self.vehicle.ad.trailerModule:canBeHandledInReverse()) end @@ -162,11 +196,12 @@ function EmptyHarvesterTask:update(dt) end self.waitTimer:timer(true, waitTime, dt) if self.waitTimer:done() then - self.waitTimer:timer(false) self:finished() + return else self.vehicle.ad.specialDrivingModule:stopVehicle() self.vehicle.ad.specialDrivingModule:update(dt) + return end end end @@ -175,7 +210,7 @@ function EmptyHarvesterTask:abort() end function EmptyHarvesterTask:finished(propagate) - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "EmptyHarvesterTask:finished()") + EmptyHarvesterTask.debugMsg(self.vehicle, "EmptyHarvesterTask:finished()") self.vehicle.ad.specialDrivingModule.motorShouldNotBeStopped = false self.vehicle.ad.taskModule:setCurrentTaskFinished(propagate) end @@ -188,11 +223,40 @@ function EmptyHarvesterTask:getExcludedVehiclesForCollisionCheck() return excludedVehicles end +function EmptyHarvesterTask:setStateNames() + if self.statesToNames == nil then + self.statesToNames = {} + for name, id in pairs(EmptyHarvesterTask) do + if string.sub(name, 1, 6) == "STATE_" then + self.statesToNames[id] = name + end + end + end +end + +function EmptyHarvesterTask:getStateName(state) + local requestedState = state + if requestedState == nil then + requestedState = self.state + end + if requestedState == nil then + Logging.error("[AD] EmptyHarvesterTask: Could not find name for state ->%s<- !", tostring(requestedState)) + end + return self.statesToNames[requestedState] or "" +end + +function EmptyHarvesterTask:resetAllTimers() + -- self.stuckTimer:timer(false) -- stuckTimer reset by speed changes + self.reverseTimer:timer(false) + self.waitTimer:timer(false) + self.holdCPCombineTimer:timer(false) +end + function EmptyHarvesterTask:getI18nInfo() local text = "$l10n_AD_task_unloading_combine;" if self.state == EmptyHarvesterTask.STATE_PATHPLANNING then - local actualState, maxStates = self.vehicle.ad.pathFinderModule:getCurrentState() - text = text .. " - " .. "$l10n_AD_task_pathfinding;" .. string.format(" %d / %d ", actualState, maxStates) + local actualState, maxStates, steps, max_pathfinder_steps = self.vehicle.ad.pathFinderModule:getCurrentState() + text = text .. " - " .. "$l10n_AD_task_pathfinding;" .. string.format(" %d / %d - %d / %d", actualState, maxStates, steps, max_pathfinder_steps) elseif self.state == EmptyHarvesterTask.STATE_DRIVING then text = text .. " - " .. "$l10n_AD_task_drive_to_combine_pipe;" elseif self.state == EmptyHarvesterTask.STATE_UNLOADING then @@ -208,5 +272,7 @@ end function EmptyHarvesterTask.debugMsg(vehicle, debugText, ...) if EmptyHarvesterTask.debug == true then AutoDrive.debugMsg(vehicle, debugText, ...) + else + AutoDrive.debugPrint(vehicle, AutoDrive.DC_COMBINEINFO, debugText, ...) end end diff --git a/scripts/Tasks/ExitFieldTask.lua b/scripts/Tasks/ExitFieldTask.lua index c1598b51..2225b9dc 100644 --- a/scripts/Tasks/ExitFieldTask.lua +++ b/scripts/Tasks/ExitFieldTask.lua @@ -11,6 +11,7 @@ function ExitFieldTask:new(vehicle) local o = ExitFieldTask:create() o.vehicle = vehicle o.trailers = nil + o.failedPathFinder = 0 return o end @@ -27,9 +28,12 @@ function ExitFieldTask:update(dt) if self.vehicle.ad.pathFinderModule:hasFinished() then self.wayPoints = self.vehicle.ad.pathFinderModule:getPath() if self.wayPoints == nil or #self.wayPoints == 0 then - self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD]:notifyAboutFailedPathfinder() - self:selectNextStrategy() - if self.vehicle.ad.pathFinderModule:isTargetBlocked() then + self.failedPathFinder = self.failedPathFinder + 1 + if self.failedPathFinder > 5 then + self.failedPathFinder = 0 + self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD]:notifyAboutFailedPathfinder() + self:selectNextStrategy() + elseif self.vehicle.ad.pathFinderModule:isTargetBlocked() then -- If the selected field exit isn't reachable, try the next strategy and restart without delay self:startPathPlanning() elseif self.vehicle.ad.pathFinderModule:timedOut() or self.vehicle.ad.pathFinderModule:isBlocked() then @@ -74,6 +78,7 @@ function ExitFieldTask:startPathPlanning() if closestDistance > AutoDrive.getDriverRadius(self.vehicle) then -- initiate pathFinder only if distance to closest wayPoint is enought to find a path local vecToNextPoint = {x = wayPoints[2].x - closestNode.x, z = wayPoints[2].z - closestNode.z} + self.vehicle.ad.pathFinderModule:reset() self.vehicle.ad.pathFinderModule:startPathPlanningTo(closestNode, vecToNextPoint) else -- close to network, set task finished @@ -93,6 +98,7 @@ function ExitFieldTask:startPathPlanning() targetNode = wayPoints[5] vecToNextPoint = {x = wayPoints[6].x - targetNode.x, z = wayPoints[6].z - targetNode.z} end + self.vehicle.ad.pathFinderModule:reset() self.vehicle.ad.pathFinderModule:startPathPlanningTo(targetNode, vecToNextPoint) else AutoDriveMessageEvent.sendMessageOrNotification(self.vehicle, ADMessagesManager.messageTypes.WARN, "$l10n_AD_Driver_of; %s $l10n_AD_cannot_find_path;", 5000, self.vehicle.ad.stateModule:getName()) @@ -120,8 +126,8 @@ end function ExitFieldTask:getI18nInfo() if self.state == ExitFieldTask.STATE_PATHPLANNING then - local actualState, maxStates = self.vehicle.ad.pathFinderModule:getCurrentState() - return "$l10n_AD_task_pathfinding;" .. string.format(" %d / %d ", actualState, maxStates) + local actualState, maxStates, steps, max_pathfinder_steps = self.vehicle.ad.pathFinderModule:getCurrentState() + return "$l10n_AD_task_pathfinding;" .. string.format(" %d / %d - %d / %d", actualState, maxStates, steps, max_pathfinder_steps) else return "$l10n_AD_task_exiting_field;" end diff --git a/scripts/Tasks/FollowCombineTask.lua b/scripts/Tasks/FollowCombineTask.lua index f21799a7..69f956a5 100644 --- a/scripts/Tasks/FollowCombineTask.lua +++ b/scripts/Tasks/FollowCombineTask.lua @@ -1,18 +1,16 @@ FollowCombineTask = ADInheritsFrom(AbstractTask) FollowCombineTask.debug = false -FollowCombineTask.STATE_CHASING = 1 -FollowCombineTask.STATE_WAIT_FOR_TURN = 2 -FollowCombineTask.STATE_REVERSING = 3 -FollowCombineTask.STATE_REVERSING_FROM_CHOPPER = 12 -FollowCombineTask.STATE_WAIT_FOR_PASS_BY = 4 -FollowCombineTask.STATE_CIRCLING_PATHPLANNING = 5 -FollowCombineTask.STATE_CIRCLING = 6 -FollowCombineTask.STATE_FINISHED = 7 -FollowCombineTask.STATE_WAIT_BEFORE_FINISH = 8 -FollowCombineTask.STATE_WAIT_FOR_COMBINE_TO_PASS_BY = 9 -FollowCombineTask.STATE_GENERATE_UTURN_PATH = 10 -FollowCombineTask.STATE_DRIVE_UTURN_PATH = 11 +FollowCombineTask.STATE_CHASING = {} +FollowCombineTask.STATE_WAIT_FOR_TURN = {} +FollowCombineTask.STATE_REVERSING = {} +FollowCombineTask.STATE_REVERSING_FROM_CHOPPER = {} +FollowCombineTask.STATE_WAIT_FOR_PASS_BY = {} +FollowCombineTask.STATE_FINISHED = {} +FollowCombineTask.STATE_WAIT_BEFORE_FINISH = {} +FollowCombineTask.STATE_WAIT_FOR_COMBINE_TO_PASS_BY = {} +FollowCombineTask.STATE_GENERATE_UTURN_PATH = {} +FollowCombineTask.STATE_DRIVE_UTURN_PATH = {} FollowCombineTask.MAX_REVERSE_DISTANCE = 20 FollowCombineTask.MIN_COMBINE_DISTANCE = 25 @@ -43,11 +41,12 @@ function FollowCombineTask:new(vehicle, combine) o.angleToCombine = vehicle.ad.modes[AutoDrive.MODE_UNLOAD]:getAngleToCombine() o.trailers = nil o.activeUnloading = AutoDrive.getSetting("activeUnloading", self.combine) + FollowCombineTask.setStateNames(o) return o end function FollowCombineTask:setUp() - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask setUp") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask setUp") self.lastChaseSide = self.chaseSide self.trailers, _ = AutoDrive.getAllUnits(self.vehicle) self.activeUnloading = AutoDrive.getSetting("activeUnloading", self.combine) @@ -62,59 +61,89 @@ function FollowCombineTask:update(dt) self:updateStates(dt) + if self.lastState ~= self.state then + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update %s -> %s", tostring(self:getStateName(self.lastState)), tostring(self:getStateName())) + self.lastState = self.state + self:resetAllTimers() + end + + local checkForStuck = (self.vehicle.lastSpeedReal <= 0.0002) and ( + self.state == FollowCombineTask.STATE_CHASING + or self.state == FollowCombineTask.STATE_WAIT_FOR_TURN + or self.state == FollowCombineTask.STATE_WAIT_FOR_COMBINE_TO_PASS_BY + or self.state == FollowCombineTask.STATE_GENERATE_UTURN_PATH + or self.state == FollowCombineTask.STATE_DRIVE_UTURN_PATH + ) + + self.stuckTimer:timer(checkForStuck, self.MAX_STUCK_TIME, dt) + if self.stuckTimer:done() + -- or AutoDrive.getDistanceBetween(self.vehicle, self.combine) < self.MIN_COMBINE_DISTANCE + then + -- got stuck or to close to combine -> reverse + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update - STATE_DRIVING stuck") + local x, y, z = getWorldTranslation(self.vehicle.components[1].node) + self.reverseStartLocation = {x = x, y = y, z = z} + if self.combine.ad.isChopper then + self.state = FollowCombineTask.STATE_REVERSING_FROM_CHOPPER + else + self.state = FollowCombineTask.STATE_REVERSING -- reverse to get room from harvester + end + end + if self.state == FollowCombineTask.STATE_CHASING then self.chaseTimer:timer(true, 4000, dt) - self.stuckTimer:timer(self.vehicle.lastSpeedReal <= 0.0002, self.MAX_STUCK_TIME, dt) if self.combine.ad.isChopper then if self.filled and self.chaseSide ~= nil and self.chaseSide ~= AutoDrive.CHASEPOS_REAR then --skip reversing - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_CHASING - filled chopper") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_CHASING - filled chopper") self.state = FollowCombineTask.STATE_FINISHED -- finish immediate return elseif self.filledToUnload and self.chaseSide ~= nil and self.chaseSide == AutoDrive.CHASEPOS_REAR then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_CHASING - filledToUnload chopper") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_CHASING - filledToUnload chopper") local x, y, z = getWorldTranslation(self.vehicle.components[1].node) self.reverseStartLocation = {x = x, y = y, z = z} self.state = FollowCombineTask.STATE_REVERSING -- reverse to get room from harvester return end elseif self.filled or (self.combine.ad.isHarvester and self.combineFillPercent <= 0.1 and (not self.activeUnloading)) then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_CHASING - filled harvester") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_CHASING - filled harvester") self.state = FollowCombineTask.STATE_WAIT_BEFORE_FINISH -- unload after some time to let harvester drive away return end - if self.stuckTimer:done() or self.angleWrongTimer.elapsedTime > 15000 then + local wrongChopperHeading = self.combine.ad.isChopper and (self.angleToCombineHeading > 90 and self.distanceToCombine < 30) + + if (self.angleWrongTimer.elapsedTime > 15000) or wrongChopperHeading then -- if stuck with harvester - try reverse if (g_updateLoopIndex % 60 == 0) or self.loop5 == nil then self.loop5 = true FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_CHASING stuckTimer:done -> STATE_REVERSING") end - self.stuckTimer:timer(false) - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_CHASING - stuck -> stuckTimer:%s angleWrongTimer:%s" + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_CHASING - stuck -> stuckTimer:%s angleWrongTimer:%s" , tostring(self.stuckTimer:done()), tostring(self.angleWrongTimer.elapsedTime > 15000)) local x, y, z = getWorldTranslation(self.vehicle.components[1].node) self.reverseStartLocation = {x = x, y = y, z = z} if self.combine.ad.isChopper then self.state = FollowCombineTask.STATE_REVERSING_FROM_CHOPPER + return else self.state = FollowCombineTask.STATE_REVERSING -- reverse to get room from harvester + return end - return end if not self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD]:isUnloaderOnCorrectSide() then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_CHASING - not UnloaderOnCorrectSide -> finished") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_CHASING - not UnloaderOnCorrectSide -> finished") self:finished() return end - if self.combine.ad.isHarvester and self.combineFillPercent > 90 - and AutoDrive.getDistanceBetween(self.vehicle, self.combine) < self.MIN_COMBINE_DISTANCE -- if to close -> reverse + if self.combine.ad.isHarvester and ((self.combineFillPercent > 90 and AutoDrive.getDistanceBetween(self.vehicle, self.combine) < self.MIN_COMBINE_DISTANCE) -- if to close -> reverse + or AutoDrive:getIsCPTurning(self.combine)) then -- Stop chasing and wait for a normal unload call while standing - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_CHASING - to close to harvester -> reverse") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_CHASING - to close to harvester -> reverse") local x, y, z = getWorldTranslation(self.vehicle.components[1].node) self.reverseStartLocation = {x = x, y = y, z = z} self.state = FollowCombineTask.STATE_REVERSING -- reverse to get room from harvester @@ -123,15 +152,16 @@ function FollowCombineTask:update(dt) if (not self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD]:isUnloaderOnCorrectSide(self.chaseSide)) and (not AutoDrive.combineIsTurning(self.combine)) then if self.lastChaseSide ~= CombineUnloaderMode.CHASEPOS_REAR then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_CHASING - switching chase side from side to elsewhere - let's wait for passby next") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_CHASING - switching chase side from side to elsewhere - let's wait for passby next") self.state = FollowCombineTask.STATE_WAIT_FOR_PASS_BY + return end end if AutoDrive.combineIsTurning(self.combine) then -- harvester turns --print("Waiting for turn now - 1- t:" .. tostring(AutoDrive.combineIsTurning(self.combine)) .. " anglewrongtimer: " .. tostring(self.angleWrongTimer.elapsedTime > 10000)) - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_CHASING - combineIsTurning") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_CHASING - combineIsTurning") self.state = FollowCombineTask.STATE_WAIT_FOR_TURN return elseif ((self.combine.lastSpeedReal * self.combine.movingDirection) <= -0.00005) then @@ -140,17 +170,16 @@ function FollowCombineTask:update(dt) self:followChasePoint(dt) end elseif self.state == FollowCombineTask.STATE_WAIT_FOR_TURN then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_FOR_TURN") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_TURN") self.waitForTurnTimer:timer(true, self.MAX_TURN_TIME, dt) if self.waitForTurnTimer:done() then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_FOR_TURN - combine turn took to long - set finished now") - self.waitForTurnTimer:timer(false) + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_TURN - combine turn took to long - set finished now") self.state = FollowCombineTask.STATE_FINISHED return end if AutoDrive.combineIsTurning(self.combine) then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_FOR_TURN - combineIsTurning") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_TURN - combineIsTurning") if self.combine.ad.isHarvester and (self.distanceToCombine < ((self.vehicle.size.length + self.combine.size.length) / 2 + 10)) then -- harvester -- if combine drive reverse to turn -> reverse to keep distance @@ -160,46 +189,43 @@ function FollowCombineTask:update(dt) if self.combine.ad.isAutoAimingChopper then local isdrivingReverse = ((self.combine.lastSpeedReal * self.combine.movingDirection) <= -0.00051) local combineIsDriving = (self.combine.lastSpeedReal > 0.001) - self.stuckTimer:timer(self.vehicle.lastSpeedReal <= 0.0002, 15000, dt) + if isdrivingReverse then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_FOR_TURN -> self:reverse") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_TURN -> self:reverse") self:reverse(dt) - elseif self.stuckTimer:done() or (not combineIsDriving and (self:getAngleToCobine() > 45)) then + elseif (not combineIsDriving and (self:getAngleToCobine() > 45)) then -- if stuck with harvester - try reverse - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_FOR_TURN - stuck / getAngleToCobine() > 45 -> STATE_REVERSING_FROM_CHOPPER combineIsDriving %s", tostring(combineIsDriving)) - self.stuckTimer:timer(false) + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_TURN - stuck / getAngleToCobine() > 45 -> STATE_REVERSING_FROM_CHOPPER combineIsDriving %s", tostring(combineIsDriving)) local x, y, z = getWorldTranslation(self.vehicle.components[1].node) self.reverseStartLocation = {x = x, y = y, z = z} self.state = FollowCombineTask.STATE_REVERSING_FROM_CHOPPER -- reverse to get room from harvester return elseif combineIsDriving then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_FOR_TURN - combineIsDriving -> stopVehicle") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_TURN - combineIsDriving -> stopVehicle") self.vehicle.ad.specialDrivingModule:stopVehicle() self.vehicle.ad.specialDrivingModule:update(dt) else - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_FOR_TURN -> followChasePoint self.chaseSide %s", tostring(self.chaseSide)) + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_TURN -> followChasePoint self.chaseSide %s", tostring(self.chaseSide)) self:followChasePoint(dt) end else -- isFixedPipeChopper - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_FOR_TURN - noMovementTimer %d", self.combine.ad.noMovementTimer.elapsedTime) + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_TURN - noMovementTimer %d", self.combine.ad.noMovementTimer.elapsedTime) local dischargeState = self.combine:getDischargeState() self.fillingTimer:timer(not self.combine.spec_combine.isFilling, 100, dt) if self.fillingTimer:done() and self.combine.ad.noMovementTimer.elapsedTime < 5000 then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_FOR_TURN - fillingTimer:done") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_TURN - fillingTimer:done") -- harvested to end of row AutoDrive:holdCPCombine(self.combine) self.vehicle.ad.specialDrivingModule:stopVehicle() self.vehicle.ad.specialDrivingModule:update(dt) else - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_FOR_TURN -> followChasePoint no AutoAimingChopper") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_TURN -> followChasePoint no AutoAimingChopper") self:followChasePoint(dt) end self.dischargeTimer:timer(dischargeState ~= Dischargeable.DISCHARGE_STATE_OBJECT , 500, dt) if self.dischargeTimer:done() and self.fillingTimer:done() and self.combine.ad.noMovementTimer.elapsedTime < 5000 then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_FOR_TURN - dischargeTimer:done") - self.fillingTimer:timer(false) - self.dischargeTimer:timer(false) + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_TURN - dischargeTimer:done") local x, y, z = getWorldTranslation(self.vehicle.components[1].node) self.reverseStartLocation = {x = x, y = y, z = z} self.state = FollowCombineTask.STATE_REVERSING_FROM_CHOPPER -- reverse to get room from harvester @@ -207,7 +233,7 @@ function FollowCombineTask:update(dt) end end else - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_FOR_TURN -> stopVehicle") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_TURN -> stopVehicle") -- stop while combine is turning self.vehicle.ad.specialDrivingModule:stopVehicle() self.vehicle.ad.specialDrivingModule:update(dt) @@ -227,60 +253,53 @@ function FollowCombineTask:update(dt) or self.waitForTurnTimer.elapsedTime > 15000 -- turn longer than 15 sec ) then if (self.angleToCombineHeading + self.angleToCombine) < 180 and self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD]:isUnloaderOnCorrectSide(self.chaseSide) then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_FOR_TURN - combine turn finished - Heading looks good - start chasing again") - self.waitForTurnTimer:timer(false) - self.chaseTimer:timer(false) + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_TURN - combine turn finished - Heading looks good - start chasing again") self.state = FollowCombineTask.STATE_CHASING return elseif self.angleToCombineHeading > 150 and self.angleToCombineHeading < 210 and self.distanceToCombine < 80 and AutoDrive.experimentalFeatures.UTurn == true and self.combine.ad.isHarvester then -- Instead of directly trying a long way around to get behind the harvester, let's wait for him to pass us by and then U-turn - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_FOR_TURN - combine turn finished - Heading inverted - wait for passby, then U-turn") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_TURN - combine turn finished - Heading inverted - wait for passby, then U-turn") self.state = FollowCombineTask.STATE_WAIT_FOR_COMBINE_TO_PASS_BY - self.waitForTurnTimer:timer(false) - self.chaseTimer:timer(false) - self.waitForPassByTimer:timer(false) + return else - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_FOR_TURN - combine turn finished - Heading looks bad - stop to be able to start pathfinder") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_TURN - combine turn finished - Heading looks bad - stop to be able to start pathfinder") self.stayOnField = true self.state = FollowCombineTask.STATE_FINISHED return end end elseif self.state == FollowCombineTask.STATE_WAIT_FOR_PASS_BY then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_FOR_PASS_BY") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_PASS_BY") self.waitForPassByTimer:timer(true, 2200, dt) self.vehicle.ad.specialDrivingModule:stopVehicle() self.vehicle.ad.specialDrivingModule:update(dt) if self.waitForPassByTimer:done() then - self.waitForPassByTimer:timer(false) - self.chaseTimer:timer(false) if (self.angleToCombineHeading + self.angleToCombine) < 180 and self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD]:isUnloaderOnCorrectSide() then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_FOR_PASS_BY - passby timer elapsed - heading looks good - chasing again") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_PASS_BY - passby timer elapsed - heading looks good - chasing again") self.state = FollowCombineTask.STATE_CHASING return else - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_FOR_PASS_BY - passby timer elapsed - heading looks bad - set finished now") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_PASS_BY - passby timer elapsed - heading looks bad - set finished now") self.stayOnField = true self.state = FollowCombineTask.STATE_WAIT_BEFORE_FINISH return end end elseif self.state == FollowCombineTask.STATE_REVERSING then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_REVERSING") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_REVERSING") local x, y, z = getWorldTranslation(self.vehicle.components[1].node) local distanceToReverseStart = MathUtil.vector2Length(x - self.reverseStartLocation.x, z - self.reverseStartLocation.z) self.reverseTimer:timer(true, self.MAX_REVERSE_TIME, dt) local doneReversing = distanceToReverseStart > self.MAX_REVERSE_DISTANCE or (not self.startedChasing) if doneReversing or self.reverseTimer:done() then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_REVERSING - done reversing - set finished") - self.reverseTimer:timer(false) + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_REVERSING - done reversing - set finished") self.state = FollowCombineTask.STATE_WAIT_BEFORE_FINISH return else self:reverse(dt) end elseif self.state == FollowCombineTask.STATE_REVERSING_FROM_CHOPPER then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_REVERSING_FROM_CHOPPER") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_REVERSING_FROM_CHOPPER") local cx, _, cz = getWorldTranslation(self.combine.components[1].node) local vx, _, vz = getWorldTranslation(self.vehicle.components[1].node) local distanceToReverseStart = MathUtil.vector2Length(vx - self.reverseStartLocation.x, vz - self.reverseStartLocation.z) @@ -288,35 +307,34 @@ function FollowCombineTask:update(dt) self.reverseTimer:timer(true, self.MAX_REVERSE_TIME, dt) local doneReversing = distanceToReverseStart > self.MAX_REVERSE_DISTANCE or distanceToCombine > self.MAX_REVERSE_DISTANCE if doneReversing or self.reverseTimer:done() then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_REVERSING_FROM_CHOPPER - done reversing - set finished") - self.reverseTimer:timer(false) + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_REVERSING_FROM_CHOPPER - done reversing - set finished") if self.combine.ad.isFixedPipeChopper and AutoDrive:getIsCPActive(self.combine) then -- wait for CP to finish a turn maneuver before invoke pathfinder self.state = FollowCombineTask.STATE_WAIT_BEFORE_FINISH + return else self.state = FollowCombineTask.STATE_FINISHED + return end - return else if self.combine.ad.isFixedPipeChopper then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_REVERSING_FROM_CHOPPER - not AutoAimingChopper -> holdCPCombine") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_REVERSING_FROM_CHOPPER - not AutoAimingChopper -> holdCPCombine") AutoDrive:holdCPCombine(self.combine) else if self:getAngleToCobine() > 30 then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_REVERSING_FROM_CHOPPER - AngleToCobine > 30 -> holdCPCombine") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_REVERSING_FROM_CHOPPER - AngleToCobine > 30 -> holdCPCombine") AutoDrive:holdCPCombine(self.combine) end end self:reverse(dt) end elseif self.state == FollowCombineTask.STATE_WAIT_BEFORE_FINISH then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_BEFORE_FINISH") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_BEFORE_FINISH") -- wait for CP to finish a turn maneuver before invoke pathfinder -- TODO: check if following is useful! local combineIsDriving = self.combine.ad.isFixedPipeChopper and AutoDrive:getIsCPActive(self.combine) and (self.combine.lastSpeedReal > 0.001) self.waitTimer:timer(not combineIsDriving, self.WAIT_BEFORE_FINISH_TIME, dt) if self.waitTimer:done() then - self.waitTimer:timer(false) self.state = FollowCombineTask.STATE_FINISHED return else @@ -324,12 +342,12 @@ function FollowCombineTask:update(dt) self.vehicle.ad.specialDrivingModule:update(dt) end elseif self.state == FollowCombineTask.STATE_WAIT_FOR_COMBINE_TO_PASS_BY then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_FOR_COMBINE_TO_PASS_BY") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_COMBINE_TO_PASS_BY") self.waitForPassByTimer:timer(true, 15000, dt) self.vehicle.ad.specialDrivingModule:stopVehicle() self.vehicle.ad.specialDrivingModule:update(dt) if self.waitForPassByTimer:done() then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_FOR_COMBINE_TO_PASS_BY - passby timer elapsed - heading looks bad - set finished now") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_COMBINE_TO_PASS_BY - passby timer elapsed - heading looks bad - set finished now") self.stayOnField = true self.state = FollowCombineTask.STATE_WAIT_BEFORE_FINISH return @@ -337,39 +355,41 @@ function FollowCombineTask:update(dt) local cx, cy, cz = getWorldTranslation(self.combine.components[1].node) local _, _, offsetZ = worldToLocal(self.vehicle.components[1].node, cx, cy, cz) if offsetZ <= -10 then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_WAIT_FOR_COMBINE_TO_PASS_BY - combine passed us. Calculate U-turn now") - self.state = FollowCombineTask.STATE_GENERATE_UTURN_PATH + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_WAIT_FOR_COMBINE_TO_PASS_BY - combine passed us. Calculate U-turn now") local cx, cy, cz = getWorldTranslation(self.combine.components[1].node) local offsetX, _, _ = worldToLocal(self.vehicle.components[1].node, cx, cy, cz) self.vehicle:generateUTurn(offsetX > 0) - self.waitForPassByTimer:timer(false) + self.state = FollowCombineTask.STATE_GENERATE_UTURN_PATH + return end end elseif self.state == FollowCombineTask.STATE_GENERATE_UTURN_PATH then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_GENERATE_UTURN_PATH") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_GENERATE_UTURN_PATH") if self.vehicle.ad.uTurn ~= nil and self.vehicle.ad.uTurn.inProgress then self.vehicle:generateUTurn(true) elseif self.vehicle.ad.uTurn ~= nil and not self.vehicle.ad.uTurn.inProgress then if self.vehicle.ad.uTurn.colliFound or self.vehicle.ad.uTurn.points == nil or #self.vehicle.ad.uTurn.points < 5 then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_GENERATE_UTURN_PATH - U-Turn generation failed due to collision - set finished now") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_GENERATE_UTURN_PATH - U-Turn generation failed due to collision - set finished now") self.stayOnField = true self.state = FollowCombineTask.STATE_WAIT_BEFORE_FINISH + return else - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_GENERATE_UTURN_PATH - U-Turn generation finished - passing points to drivePathModule now") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_GENERATE_UTURN_PATH - U-Turn generation finished - passing points to drivePathModule now") self.vehicle.ad.drivePathModule:setWayPoints(self.vehicle.ad.uTurn.points) self.state = FollowCombineTask.STATE_DRIVE_UTURN_PATH + return end end elseif self.state == FollowCombineTask.STATE_DRIVE_UTURN_PATH then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_DRIVE_UTURN_PATH") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_DRIVE_UTURN_PATH") if self.vehicle.ad.drivePathModule:isTargetReached() then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_DRIVE_UTURN_PATH - U-Turn finished") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_DRIVE_UTURN_PATH - U-Turn finished") if (self.angleToCombineHeading + self.angleToCombine) < 180 and self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD]:isUnloaderOnCorrectSide() then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_DRIVE_UTURN_PATH - passby timer elapsed - heading looks good - chasing again") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_DRIVE_UTURN_PATH - passby timer elapsed - heading looks good - chasing again") self.state = FollowCombineTask.STATE_CHASING return else - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_DRIVE_UTURN_PATH - passby timer elapsed - heading looks bad - set finished now") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_DRIVE_UTURN_PATH - passby timer elapsed - heading looks bad - set finished now") self.stayOnField = true self.state = FollowCombineTask.STATE_WAIT_BEFORE_FINISH return @@ -378,7 +398,7 @@ function FollowCombineTask:update(dt) self.vehicle.ad.drivePathModule:update(dt) end elseif self.state == FollowCombineTask.STATE_FINISHED then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:update STATE_FINISHED") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:update STATE_FINISHED") self:finished() return end @@ -395,6 +415,7 @@ function FollowCombineTask:startPathPlanningForCircling() local targetPos = AutoDrive.createWayPointRelativeToVehicle(self.vehicle, sideOffset, 0) local directionX, directionY, directionZ = localToWorld(self.vehicle.components[1].node, 0, 0, 0) local direction = {x = directionX - targetPos.x, z = directionZ - targetPos.z} + self.vehicle.ad.pathFinderModule:reset() self.vehicle.ad.pathFinderModule:startPathPlanningTo(targetPos, direction) end @@ -434,7 +455,7 @@ end function FollowCombineTask:followChasePoint(dt) if self:shouldWaitForChasePos(dt) then - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:followChasePoint getAngleToChasePos %.0f -> stopVehicle", self:getAngleToChasePos()) + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:followChasePoint getAngleToChasePos %.0f -> stopVehicle", self:getAngleToChasePos()) self.vehicle.ad.specialDrivingModule:stopVehicle() self.vehicle.ad.specialDrivingModule:update(dt) else @@ -452,7 +473,7 @@ function FollowCombineTask:followChasePoint(dt) end function FollowCombineTask:shouldWaitForChasePos(dt) - local angle = self:getAngleToChasePos(dt) + local angle = self:getAngleToChasePos() self.angleWrongTimer:timer(angle > 50, 3000, dt) local _, _, diffZ = worldToLocal(self.vehicle.components[1].node, self.chasePos.x, self.chasePos.y, self.chasePos.z) return self.angleWrongTimer:done() or diffZ <= -1 --or (not self.combine.ad.sensors.frontSensorFruit:pollInfo()) @@ -507,7 +528,7 @@ function FollowCombineTask:abort() end function FollowCombineTask:finished() - AutoDrive.debugPrint(self.vehicle, AutoDrive.DC_COMBINEINFO, "FollowCombineTask:finished()") + FollowCombineTask.debugMsg(self.vehicle, "FollowCombineTask:finished()") self.vehicle.ad.taskModule:setCurrentTaskFinished() end @@ -519,6 +540,41 @@ function FollowCombineTask:getExcludedVehiclesForCollisionCheck() return excludedVehicles end +function FollowCombineTask:setStateNames() + if self.statesToNames == nil then + self.statesToNames = {} + for name, id in pairs(FollowCombineTask) do + if string.sub(name, 1, 6) == "STATE_" then + self.statesToNames[id] = name + end + end + end +end + +function FollowCombineTask:getStateName(state) + local requestedState = state + if requestedState == nil then + requestedState = self.state + end + if requestedState == nil then + Logging.error("[AD] FollowCombineTask: Could not find name for state ->%s<- !", tostring(requestedState)) + end + return self.statesToNames[requestedState] or "" +end + +function FollowCombineTask:resetAllTimers() + -- self.stuckTimer:timer(false) -- stuckTimer reset by speed changes + self.angleWrongTimer:timer(false) + self.waitForTurnTimer:timer(false) + self.dischargeTimer:timer(false) + self.fillingTimer:timer(false) + self.waitForPassByTimer:timer(false) + self.chaseTimer:timer(false) + self.reverseTimer:timer(false) + self.waitTimer:timer(false) +end + + function FollowCombineTask:getI18nInfo() local text = "$l10n_AD_task_chasing_combine;" if self.state == FollowCombineTask.STATE_CHASING then @@ -534,7 +590,7 @@ function FollowCombineTask:getI18nInfo() elseif self.chaseSide == AutoDrive.CHASEPOS_RIGHT then text = text .. " - " .. "$l10n_AD_task_chase_side_right;" end - elseif self.state == FollowCombineTask.STATE_REVERSING_FROM_CHOPPER then + elseif self.state == FollowCombineTask.STATE_REVERSING_FROM_CHOPPER or self.state == FollowCombineTask.STATE_WAIT_FOR_TURN then text = text .. " - " .. "$l10n_AD_task_wait_for_combine_turn;" elseif self.state == FollowCombineTask.STATE_REVERSING then text = text .. " - " .. "$l10n_AD_task_reversing_from_combine;" @@ -549,5 +605,7 @@ end function FollowCombineTask.debugMsg(vehicle, debugText, ...) if FollowCombineTask.debug == true then AutoDrive.debugMsg(vehicle, debugText, ...) + else + AutoDrive.debugPrint(vehicle, AutoDrive.DC_COMBINEINFO, debugText, ...) end end diff --git a/scripts/Tasks/LoadAtDestinationTask.lua b/scripts/Tasks/LoadAtDestinationTask.lua index 6cbb1f27..38056175 100644 --- a/scripts/Tasks/LoadAtDestinationTask.lua +++ b/scripts/Tasks/LoadAtDestinationTask.lua @@ -29,6 +29,7 @@ function LoadAtDestinationTask:setUp() --self.vehicle.ad.pathFinderModule:startPathPlanningToWayPoint(self.vehicle.ad.stateModule:getSecondWayPoint(), self.destinationID) --end --else + self.vehicle.ad.pathFinderModule:reset() self.vehicle.ad.pathFinderModule:startPathPlanningToNetwork(self.destinationID) --end else @@ -173,8 +174,8 @@ end function LoadAtDestinationTask:getI18nInfo() if self.state == LoadAtDestinationTask.STATE_PATHPLANNING then - local actualState, maxStates = self.vehicle.ad.pathFinderModule:getCurrentState() - return "$l10n_AD_task_pathfinding;" .. string.format(" %d / %d ", actualState, maxStates) + local actualState, maxStates, steps, max_pathfinder_steps = self.vehicle.ad.pathFinderModule:getCurrentState() + return "$l10n_AD_task_pathfinding;" .. string.format(" %d / %d - %d / %d", actualState, maxStates, steps, max_pathfinder_steps) else return "$l10n_AD_task_drive_to_load_point;" end diff --git a/scripts/Tasks/ParkTask.lua b/scripts/Tasks/ParkTask.lua index 640d5272..13e3ac21 100644 --- a/scripts/Tasks/ParkTask.lua +++ b/scripts/Tasks/ParkTask.lua @@ -38,6 +38,7 @@ function ParkTask:setUp() self.vehicle.ad.trainModule:setPathTo(targetParkParkDestination) elseif ADGraphManager:getDistanceFromNetwork(self.vehicle) > 30 then self.state = ParkTask.STATE_PATHPLANNING + self.vehicle.ad.pathFinderModule:reset() self.vehicle.ad.pathFinderModule:startPathPlanningToNetwork(targetParkParkDestination) else self.state = ParkTask.STATE_DRIVING @@ -93,8 +94,8 @@ end function ParkTask:getI18nInfo() if self.state == ParkTask.STATE_PATHPLANNING then - local actualState, maxStates = self.vehicle.ad.pathFinderModule:getCurrentState() - return "$l10n_AD_task_pathfinding;" .. string.format(" %d / %d ", actualState, maxStates) + local actualState, maxStates, steps, max_pathfinder_steps = self.vehicle.ad.pathFinderModule:getCurrentState() + return "$l10n_AD_task_pathfinding;" .. string.format(" %d / %d - %d / %d", actualState, maxStates, steps, max_pathfinder_steps) elseif self.vehicle.ad.onRouteToPark == true then return "$l10n_AD_task_drive_to_park;" else diff --git a/scripts/Tasks/RefuelTask.lua b/scripts/Tasks/RefuelTask.lua index 320265c2..fe510f46 100644 --- a/scripts/Tasks/RefuelTask.lua +++ b/scripts/Tasks/RefuelTask.lua @@ -17,6 +17,7 @@ function RefuelTask:setUp() self.matchingFillTypes = {} if ADGraphManager:getDistanceFromNetwork(self.vehicle) > 30 then self.state = RefuelTask.STATE_PATHPLANNING + self.vehicle.ad.pathFinderModule:reset() self.vehicle.ad.pathFinderModule:startPathPlanningToNetwork(self.destinationID) else self.state = RefuelTask.STATE_DRIVING @@ -193,8 +194,8 @@ end function RefuelTask:getI18nInfo() if self.state == RefuelTask.STATE_PATHPLANNING then - local actualState, maxStates = self.vehicle.ad.pathFinderModule:getCurrentState() - return "$l10n_AD_task_pathfinding;" .. string.format(" %d / %d ", actualState, maxStates) + local actualState, maxStates, steps, max_pathfinder_steps = self.vehicle.ad.pathFinderModule:getCurrentState() + return "$l10n_AD_task_pathfinding;" .. string.format(" %d / %d - %d / %d", actualState, maxStates, steps, max_pathfinder_steps) else return "$l10n_AD_task_drive_to_refuel_point;" end diff --git a/scripts/Tasks/RepairTask.lua b/scripts/Tasks/RepairTask.lua index 3133c27e..ca639088 100644 --- a/scripts/Tasks/RepairTask.lua +++ b/scripts/Tasks/RepairTask.lua @@ -17,6 +17,7 @@ function RepairTask:setUp() self.repairTrigger = nil if ADGraphManager:getDistanceFromNetwork(self.vehicle) > 30 then self.state = RepairTask.STATE_PATHPLANNING + self.vehicle.ad.pathFinderModule:reset() self.vehicle.ad.pathFinderModule:startPathPlanningToNetwork(self.destinationID) else self.state = RepairTask.STATE_DRIVING @@ -103,8 +104,8 @@ end function RepairTask:getI18nInfo() if self.state == RepairTask.STATE_PATHPLANNING then - local actualState, maxStates = self.vehicle.ad.pathFinderModule:getCurrentState() - return "$l10n_AD_task_pathfinding;" .. string.format(" %d / %d ", actualState, maxStates) + local actualState, maxStates, steps, max_pathfinder_steps = self.vehicle.ad.pathFinderModule:getCurrentState() + return "$l10n_AD_task_pathfinding;" .. string.format(" %d / %d - %d / %d", actualState, maxStates, steps, max_pathfinder_steps) else return "$l10n_AD_task_drive_to_repair_point;" end diff --git a/scripts/Tasks/UnloadAtDestinationTask.lua b/scripts/Tasks/UnloadAtDestinationTask.lua index 72695a4b..d7da9104 100644 --- a/scripts/Tasks/UnloadAtDestinationTask.lua +++ b/scripts/Tasks/UnloadAtDestinationTask.lua @@ -28,11 +28,14 @@ function UnloadAtDestinationTask:setUp() self.state = UnloadAtDestinationTask.STATE_PATHPLANNING if self.vehicle.ad.restartCP == true then if self.vehicle.ad.stateModule:getMode() == AutoDrive.MODE_PICKUPANDDELIVER then + self.vehicle.ad.pathFinderModule:reset() self.vehicle.ad.pathFinderModule:startPathPlanningToWayPoint(self.vehicle.ad.stateModule:getFirstWayPoint(), self.destinationID) else + self.vehicle.ad.pathFinderModule:reset() self.vehicle.ad.pathFinderModule:startPathPlanningToWayPoint(self.vehicle.ad.stateModule:getSecondWayPoint(), self.destinationID) end else + self.vehicle.ad.pathFinderModule:reset() self.vehicle.ad.pathFinderModule:startPathPlanningToNetwork(self.destinationID) end else @@ -62,14 +65,17 @@ function UnloadAtDestinationTask:update(dt) self.wayPoints = self.vehicle.ad.pathFinderModule:getPath() if self.wayPoints == nil or #self.wayPoints == 0 then if self.vehicle.ad.pathFinderModule:isTargetBlocked() then - -- If the selected field exit isn't reachable, try the closest one + -- If the selected field exit isn't reachable, try the closest one + self.vehicle.ad.pathFinderModule:reset() self.vehicle.ad.pathFinderModule:startPathPlanningToNetwork(self.vehicle.ad.stateModule:getSecondWayPoint()) elseif self.vehicle.ad.pathFinderModule:timedOut() or self.vehicle.ad.pathFinderModule:isBlocked() then -- Add some delay to give the situation some room to clear itself self.vehicle.ad.modes[AutoDrive.MODE_UNLOAD]:notifyAboutFailedPathfinder() + self.vehicle.ad.pathFinderModule:reset() self.vehicle.ad.pathFinderModule:startPathPlanningToNetwork(self.vehicle.ad.stateModule:getSecondWayPoint()) self.vehicle.ad.pathFinderModule:addDelayTimer(10000) else + self.vehicle.ad.pathFinderModule:reset() self.vehicle.ad.pathFinderModule:startPathPlanningToNetwork(self.vehicle.ad.stateModule:getSecondWayPoint()) end @@ -247,8 +253,8 @@ end function UnloadAtDestinationTask:getI18nInfo() if self.state == UnloadAtDestinationTask.STATE_PATHPLANNING then - local actualState, maxStates = self.vehicle.ad.pathFinderModule:getCurrentState() - return "$l10n_AD_task_pathfinding;" .. string.format(" %d / %d ", actualState, maxStates) + local actualState, maxStates, steps, max_pathfinder_steps = self.vehicle.ad.pathFinderModule:getCurrentState() + return "$l10n_AD_task_pathfinding;" .. string.format(" %d / %d - %d / %d", actualState, maxStates, steps, max_pathfinder_steps) else return "$l10n_AD_task_drive_to_unload_point;" end diff --git a/scripts/Utils/AutoDriveVehicleData.lua b/scripts/Utils/AutoDriveVehicleData.lua index f6d847d6..e6ed9002 100644 --- a/scripts/Utils/AutoDriveVehicleData.lua +++ b/scripts/Utils/AutoDriveVehicleData.lua @@ -41,7 +41,7 @@ function AutoDriveVehicleData.registerEventListeners(vehicleType) end end -function AutoDrive.registerFunctions(vehicleType) +function AutoDriveVehicleData.registerFunctions(vehicleType) SpecializationUtil.registerFunction(vehicleType, "getParkDestination", AutoDriveVehicleData.getParkDestination) SpecializationUtil.registerFunction(vehicleType, "setParkDestination", AutoDriveVehicleData.setParkDestination) end @@ -88,7 +88,7 @@ function AutoDriveVehicleData:onEnterVehicle() local actualparkDestination = -1 if g_dedicatedServer == nil then -- only send the client event to server - local selectedWorkTool = AutoDrive.getSelectedWorkTool(self, true) + local selectedWorkTool = AutoDrive.getSelectedWorkTool(self) if selectedWorkTool ~= nil and selectedWorkTool.advd ~= nil then actualparkDestination = selectedWorkTool.advd.parkDestination else diff --git a/scripts/Utils/CollisionDetectionUtils.lua b/scripts/Utils/CollisionDetectionUtils.lua index 6117d34c..37b684e7 100644 --- a/scripts/Utils/CollisionDetectionUtils.lua +++ b/scripts/Utils/CollisionDetectionUtils.lua @@ -22,11 +22,13 @@ function AutoDrive.checkForVehiclesInBox(boundingBox, excludedVehicles) isExcluded = true end if (not isExcluded) and otherVehicle ~= nil and otherVehicle.components ~= nil and otherVehicle.size.width ~= nil and otherVehicle.size.length ~= nil and otherVehicle.rootNode ~= nil then - local x, _, z = getWorldTranslation(otherVehicle.components[1].node) + local x, y, z = getWorldTranslation(otherVehicle.components[1].node) local distance = MathUtil.vector2Length(boundingBox[1].x - x, boundingBox[1].z - z) if distance < 50 then if AutoDrive.boxesIntersect(boundingBox, AutoDrive.getBoundingBoxForVehicle(otherVehicle)) == true then - return true + if math.abs(boundingBox[1].y - y) < 10 then + return true + end end end end @@ -150,6 +152,9 @@ function AutoDrive.getBoundingBoxForVehicle(vehicle, force) end function AutoDrive.getDistanceBetween(vehicleOne, vehicleTwo) + if vehicleOne == nil or vehicleTwo == nil then + printCallstack() + end local x1, _, z1 = getWorldTranslation(vehicleOne.components[1].node) local x2, _, z2 = getWorldTranslation(vehicleTwo.components[1].node) diff --git a/scripts/Utils/CombineUtil.lua b/scripts/Utils/CombineUtil.lua index 30fbe521..2a499fd3 100644 --- a/scripts/Utils/CombineUtil.lua +++ b/scripts/Utils/CombineUtil.lua @@ -164,7 +164,7 @@ function AutoDrive.getPipeLength(combine) if (combine.ad.isFixedPipeChopper or combine.ad.isHarvester) and AutoDrive.isPipeOut(combine) then local combineNode = AutoDrive.getPipeRoot(combine) local dischargeX, dichargeY, dischargeZ = getWorldTranslation(AutoDrive.getDischargeNode(combine)) - diffX, _, _ = worldToLocal(combineNode, dischargeX, dichargeY, dischargeZ) + local diffX, _, _ = worldToLocal(combineNode, dischargeX, dichargeY, dischargeZ) length = math.abs(diffX) -- Store pipe length for 'normal' harvesters diff --git a/scripts/Utils/Dubins.lua b/scripts/Utils/Dubins.lua new file mode 100644 index 00000000..3fbf020d --- /dev/null +++ b/scripts/Utils/Dubins.lua @@ -0,0 +1,428 @@ +--[[ +Dubins curves adapted for use with AutoDrive + +Copyright (c) 2008-2018, Andrew Walker + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. + + +@Misc{DubinsCurves, + author = {Andrew Walker}, + title = {Dubins-Curves: an open implementation of shortest paths for the forward only car}, + year = {2008--}, + url = "https://github.com/AndrewWalker/Dubins-Curves" +} + +]] + +ADDubins = {} +function ADDubins:new() + local o = {} + setmetatable(o, self) + self.__index = self + self.outPath = {} + return o +end + +ADDubins.DubinsPathType = { + LSL = 1, + LSR = 2, + RSL = 3, + RSR = 4, + RLR = 5, + LRL = 6 +} + +ADDubins.DubinsPath = { + -- /* the initial configuration */ + qi = {}, + -- /* the lengths of the three segments */ + param = {}, + -- /* model forward velocity / model angular velocity */ + rho = 0, + -- /* the path type described */ + type = 0 +} + +ADDubins.EDUBOK = 0 +ADDubins.EDUBCOCONFIGS = 1 +ADDubins.EDUBPARAM = 2 +ADDubins.EDUBBADRHO = 3 +ADDubins.EDUBNOPATH = 4 +ADDubins.EPSILON = (10e-10) + +ADDubins.SegmentType = { + L_SEG = 0, + S_SEG = 1, + R_SEG = 2 +} + +ADDubins.DIRDATA = { + { 0, 1, 0 }, + { 0, 1, 2 }, + { 2, 1, 0 }, + { 2, 1, 2 }, + { 2, 0, 2 }, + { 0, 2, 0 } +} + +ADDubins.DubinsIntermediateResults = { + alpha = 0, + beta = 0, + d = 0, + sa = 0, + sb = 0, + ca = 0, + cb = 0, + c_ab = 0, + d_sq = 0 +} + +function ADDubins:fmodr(x, y) + return (x - y * math.floor(x/y)) +end + +function ADDubins:mod2pi( theta ) + return (self:fmodr( theta, 2 * math.pi )) +end + +function ADDubins:dubins_shortest_path(path, q0, q1, rho) + local inTemp = {} + local params = {} + local cost + local best_cost = math.huge + local best_word = -1 + local errcode = self:dubins_intermediate_results(inTemp, q0, q1, rho) + if errcode ~= ADDubins.EDUBOK then + return errcode + end + + path.qi[1] = q0[1] + path.qi[2] = q0[2] + path.qi[3] = q0[3] + path.rho = rho + + for i = 1, 6, 1 do + errcode = ADDubins:dubins_word(inTemp, i, params) + if errcode == ADDubins.EDUBOK then + cost = params[1] + params[2] + params[3] + if(cost < best_cost) then + best_word = i + best_cost = cost + path.param[1] = params[1] + path.param[2] = params[2] + path.param[3] = params[3] + path.type = i + end + end + end + if(best_word == -1) then + return ADDubins.EDUBNOPATH + end + return ADDubins.EDUBOK +end + +function ADDubins:dubins_path(path, q0, q1, rho, pathType) + local inTemp = {} + local errcode = ADDubins:dubins_intermediate_results(inTemp, q0, q1, rho) + if(errcode == ADDubins.EDUBOK) then + local params = {} + errcode = ADDubins:dubins_word(inTemp, pathType, params) + if(errcode == ADDubins.EDUBOK) then + path.param[1] = params[1] + path.param[2] = params[2] + path.param[3] = params[3] + path.qi[1] = q0[1] + path.qi[2] = q0[2] + path.qi[3] = q0[3] + path.rho = rho + path.type = pathType + end + end + return errcode +end + +function ADDubins:dubins_path_length( path ) + local length = 0 + length = length + path.param[1] + length = length + path.param[2] + length = length + path.param[3] + length = length * path.rho; + return length +end + +function ADDubins:dubins_segment_length( path, i ) + if( (i < 1) or (i > 3) ) then + return math.huge + end + return path.param[i] * path.rho; +end + +function ADDubins:dubins_segment_length_normalized( path, i ) + if( (i < 1) or (i > 3) ) then + return math.huge + end + return path.param[i] +end + +function ADDubins:dubins_path_type( path ) + return path.type +end + +function ADDubins:dubins_segment( t, qi, qt, type) + local st = math.sin(qi[3]) + local ct = math.cos(qi[3]) + if( type == ADDubins.SegmentType.L_SEG ) then + qt[1] = math.sin(qi[3]+t) - st + qt[2] = - math.cos(qi[3]+t) + ct + qt[3] = t + elseif( type == ADDubins.SegmentType.R_SEG ) then + qt[1] = - math.sin(qi[3]-t) + st + qt[2] = math.cos(qi[3]-t) - ct + qt[3] = -t + elseif( type == ADDubins.SegmentType.S_SEG ) then + qt[1] = ct * t + qt[2] = st * t + qt[3] = 0 + end + qt[1] = qt[1] + qi[1] + qt[2] = qt[2] + qi[2] + qt[3] = qt[3] + qi[3] +end + +function ADDubins:dubins_path_sample( path, t, q ) + -- /* tprime is the normalised variant of the parameter t */ + local tprime = t / path.rho; + local qi = {} --/* The translated initial configuration */ + local q1 = {} --/* end-of segment 1 */ + local q2 = {} --/* end-of segment 2 */ + local types = ADDubins.DIRDATA[path.type] + local p1, p2 + + if( t < 0 or t > self:dubins_path_length(path) ) then + return ADDubins.EDUBPARAM + end + + -- /* initial configuration */ + qi[1] = 0 + qi[2] = 0 + qi[3] = path.qi[3] + + -- /* generate the target configuration */ + p1 = path.param[1] + p2 = path.param[2] + self:dubins_segment( p1, qi, q1, types[1] ) + self:dubins_segment( p2, q1, q2, types[2] ) + if( tprime < p1 ) then + self:dubins_segment( tprime, qi, q, types[1] ) + elseif( tprime < (p1+p2) ) then + self:dubins_segment( tprime-p1, q1, q, types[2] ) + else + self:dubins_segment( tprime-p1-p2, q2, q, types[3] ) + end + + -- /* scale the target configuration, translate back to the original starting point */ + q[1] = q[1] * path.rho + path.qi[1] + q[2] = q[2] * path.rho + path.qi[2] + q[3] = self:mod2pi(q[3]) + + return ADDubins.EDUBOK +end + +function ADDubins.createWayPoints(q, x, outPath) + local y = getTerrainHeightAtWorldPos(g_currentMission.terrainRootNode, q[1], 1, -q[2]) + -- print(string.format("printConfiguration xyz %.1f %.1f %.1f", q[1], y, q[2])) + table.insert(outPath, {x = q[1], y = y, z = -q[2], t = q[3]}) + return 0 +end + +function ADDubins:dubins_path_sample_many(path, stepSize, cb) + local retcode + local q = {} + local x = 0 + local length = self:dubins_path_length(path) + while( x < length ) do + self:dubins_path_sample( path, x, q ) + retcode = cb(q, x, self.outPath) + if( retcode ~= 0 ) then + return retcode + end + x = x + stepSize + end + return 0 +end + +function ADDubins:dubins_path_endpoint( path, q ) + return self:dubins_path_sample( path, self:dubins_path_length(path) - ADDubins.EPSILON, q ) +end + +function ADDubins:dubins_extract_subpath( path, t, newpath ) + -- /* calculate the true parameter */ + local tprime = t / path.rho + + if ((t < 0) or (t > self:dubins_path_length(path))) then + return ADDubins.EDUBPARAM + end + + -- /* copy most of the data */ + newpath.qi[1] = path.qi[1] + newpath.qi[2] = path.qi[2] + newpath.qi[3] = path.qi[3] + newpath.rho = path.rho + newpath.type = path.type + + -- /* fix the parameters */ + newpath.param[1] = math.min( path.param[1], tprime ) + newpath.param[2] = math.min( path.param[2], tprime - newpath.param[1]) + newpath.param[3] = math.min( path.param[3], tprime - newpath.param[1] - newpath.param[2]) + return 0 +end + +function ADDubins:dubins_intermediate_results(inTemp, q0, q1, rho) + local dx, dy, D, d, theta, alpha, beta + if( rho <= 0 ) then + return ADDubins.EDUBBADRHO + end + + dx = q1[1] - q0[1] + dy = q1[2] - q0[2] + D = math.sqrt( dx * dx + dy * dy ) + d = D / rho + theta = 0 + + -- /* test required to prevent domain errors if dx=0 and dy=0 */ + if(d > 0) then + theta = self:mod2pi(math.atan2( dy, dx )) + end + alpha = self:mod2pi(q0[3] - theta) + beta = self:mod2pi(q1[3] - theta) + + inTemp.alpha = alpha + inTemp.beta = beta + inTemp.d = d + inTemp.sa = math.sin(alpha) + inTemp.sb = math.sin(beta) + inTemp.ca = math.cos(alpha) + inTemp.cb = math.cos(beta) + inTemp.c_ab = math.cos(alpha - beta) + inTemp.d_sq = d * d + + return ADDubins.EDUBOK +end + +function ADDubins:dubins_LSL( inTemp, out) + local tmp0, tmp1, p_sq + tmp0 = inTemp.d + inTemp.sa - inTemp.sb + p_sq = 2 + inTemp.d_sq - (2*inTemp.c_ab) + (2 * inTemp.d * (inTemp.sa - inTemp.sb)) + + if(p_sq >= 0) then + tmp1 = math.atan2( (inTemp.cb - inTemp.ca), tmp0 ) + out[1] = self:mod2pi(tmp1 - inTemp.alpha) + out[2] = math.sqrt(p_sq) + out[3] = self:mod2pi(inTemp.beta - tmp1) + return ADDubins.EDUBOK + end + return ADDubins.EDUBNOPATH +end + +function ADDubins:dubins_RSR(inTemp, out) + local tmp0 = inTemp.d - inTemp.sa + inTemp.sb + local p_sq = 2 + inTemp.d_sq - (2 * inTemp.c_ab) + (2 * inTemp.d * (inTemp.sb - inTemp.sa)) + if( p_sq >= 0 ) then + local tmp1 = math.atan2( (inTemp.ca - inTemp.cb), tmp0 ) + out[1] = self:mod2pi(inTemp.alpha - tmp1) + out[2] = math.sqrt(p_sq) + out[3] = self:mod2pi(tmp1 -inTemp.beta) + return ADDubins.EDUBOK + end + return ADDubins.EDUBNOPATH +end + +function ADDubins:dubins_LSR(inTemp, out) + local p_sq = -2 + (inTemp.d_sq) + (2 * inTemp.c_ab) + (2 * inTemp.d * (inTemp.sa + inTemp.sb)) + if( p_sq >= 0 ) then + local p = math.sqrt(p_sq) + local tmp0 = math.atan2( (-inTemp.ca - inTemp.cb), (inTemp.d + inTemp.sa + inTemp.sb) ) - math.atan2(-2.0, p) + out[1] = self:mod2pi(tmp0 - inTemp.alpha) + out[2] = p + out[3] = self:mod2pi(tmp0 - self:mod2pi(inTemp.beta)) + return ADDubins.EDUBOK + end + return ADDubins.EDUBNOPATH +end + +function ADDubins:dubins_RSL(inTemp, out) + local p_sq = -2 + inTemp.d_sq + (2 * inTemp.c_ab) - (2 * inTemp.d * (inTemp.sa + inTemp.sb)) + if( p_sq >= 0 ) then + local p = math.sqrt(p_sq); + local tmp0 = math.atan2( (inTemp.ca + inTemp.cb), (inTemp.d - inTemp.sa - inTemp.sb) ) - math.atan2(2.0, p) + out[1] = self:mod2pi(inTemp.alpha - tmp0) + out[2] = p + out[3] = self:mod2pi(inTemp.beta - tmp0) + return ADDubins.EDUBOK + end + return ADDubins.EDUBNOPATH +end + +function ADDubins:dubins_RLR(inTemp, out) + local tmp0 = (6. - inTemp.d_sq + 2*inTemp.c_ab + 2*inTemp.d*(inTemp.sa - inTemp.sb)) / 8 + local phi = math.atan2( inTemp.ca - inTemp.cb, inTemp.d - inTemp.sa + inTemp.sb ) + if( math.abs(tmp0) <= 1) then + local p = self:mod2pi((2 * math.pi) - math.acos(tmp0) ) + local t = self:mod2pi(inTemp.alpha - phi + self:mod2pi(p/2)) + out[1] = t + out[2] = p + out[3] = self:mod2pi(inTemp.alpha - inTemp.beta - t + self:mod2pi(p)) + return ADDubins.EDUBOK + end + return ADDubins.EDUBNOPATH +end + +function ADDubins:dubins_LRL(inTemp, out) + local tmp0 = (6. - inTemp.d_sq + 2*inTemp.c_ab + 2*inTemp.d*(inTemp.sb - inTemp.sa)) / 8 + local phi = math.atan2( inTemp.ca - inTemp.cb, inTemp.d + inTemp.sa - inTemp.sb ) + if( math.abs(tmp0) <= 1) then + local p = self:mod2pi( 2 * math.pi - math.acos( tmp0) ) + local t = self:mod2pi(-inTemp.alpha - phi + p/2) + out[1] = t + out[2] = p + out[3] = self:mod2pi(self:mod2pi(inTemp.beta) - inTemp.alpha -t + self:mod2pi(p)) + return ADDubins.EDUBOK + end + return ADDubins.EDUBNOPATH +end + +function ADDubins:dubins_word(inTemp, pathType, out) + local result + if pathType == ADDubins.DubinsPathType.LSL then + result = self:dubins_LSL(inTemp, out) + elseif pathType == ADDubins.DubinsPathType.RSL then + result = self:dubins_RSL(inTemp, out) + elseif pathType == ADDubins.DubinsPathType.LSR then + result = self:dubins_LSR(inTemp, out) + elseif pathType == ADDubins.DubinsPathType.RSR then + result = self:dubins_RSR(inTemp, out) + elseif pathType == ADDubins.DubinsPathType.LRL then + result = self:dubins_LRL(inTemp, out) + elseif pathType == ADDubins.DubinsPathType.RLR then + result = self:dubins_RLR(inTemp, out) + end + + return result +end diff --git a/scripts/Utils/UtilFuncs.lua b/scripts/Utils/UtilFuncs.lua index 2868f412..bc3f445d 100644 --- a/scripts/Utils/UtilFuncs.lua +++ b/scripts/Utils/UtilFuncs.lua @@ -105,7 +105,7 @@ function AutoDrive.streamWriteStringOrEmpty(streamId, string) end function AutoDrive.streamReadUIntNList(streamId, numberOfBits) - list = {} + local list = {} local len = streamReadUIntN(streamId, numberOfBits) for i = 1, len do local v = streamReadUIntN(streamId, numberOfBits) @@ -288,7 +288,7 @@ end function table:concatNil(sep, i, j) local res = table.concat(self, sep, i, j) if res == "" then - res = nil + return nil end return res end @@ -789,11 +789,11 @@ function AutoDrive.debugMsg(vehicle, debugText, ...) local printText = "[AD] " .. tostring(g_updateLoopIndex) .. " " if vehicle ~= nil then if vehicle.ad ~= nil and vehicle.ad.stateModule ~= nil and vehicle == vehicle:getRootVehicle() then - printText = printText .. vehicle.ad.stateModule:getName() .. ": " + printText = printText .. vehicle.ad.stateModule:getName() .. " (" .. tostring(vehicle.id or 0) .. ") : " elseif vehicle.getName ~= nil then - printText = printText .. vehicle:getName() .. ": " + printText = printText .. vehicle:getName() .. " (" .. tostring(vehicle.id or 0) .. ") : " else - printText = printText .. tostring(vehicle) .. ": " + printText = printText .. tostring(vehicle) .. " (" .. tostring(vehicle.id or 0) .. ") : " end end @@ -1186,7 +1186,6 @@ function AutoDrive.checkWaypointsMultipleSameOut(correctit) for j, linkedNodeId_1 in ipairs(network[i].out) do local wp_2 = network[linkedNodeId_1] if wp_2 ~= nil then - found = false for k, linkedNodeId_2 in ipairs(network[i].out) do if k>j then local wp_3 = network[linkedNodeId_2] diff --git a/translations/translation_br.xml b/translations/translation_br.xml index 7f6ce6ed..6d3700fd 100644 --- a/translations/translation_br.xml +++ b/translations/translation_br.xml @@ -222,6 +222,8 @@ + + diff --git a/translations/translation_ct.xml b/translations/translation_ct.xml new file mode 100644 index 00000000..8cbac5d5 --- /dev/null +++ b/translations/translation_ct.xml @@ -0,0 +1,366 @@ + + + 泰迪藍 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/translations/translation_cz.xml b/translations/translation_cz.xml index e1f48dbc..f68307f3 100644 --- a/translations/translation_cz.xml +++ b/translations/translation_cz.xml @@ -224,6 +224,8 @@ + + diff --git a/translations/translation_de.xml b/translations/translation_de.xml index 6909a75b..fbabbbea 100644 --- a/translations/translation_de.xml +++ b/translations/translation_de.xml @@ -228,6 +228,8 @@ + + diff --git a/translations/translation_en.xml b/translations/translation_en.xml index 2f3a8a95..80ca8695 100644 --- a/translations/translation_en.xml +++ b/translations/translation_en.xml @@ -222,6 +222,8 @@ + + diff --git a/translations/translation_es.xml b/translations/translation_es.xml index 43d1c42c..a95a6c52 100644 --- a/translations/translation_es.xml +++ b/translations/translation_es.xml @@ -218,8 +218,10 @@ - - + + + + diff --git a/translations/translation_fr.xml b/translations/translation_fr.xml index 50669231..3b89b7bc 100644 --- a/translations/translation_fr.xml +++ b/translations/translation_fr.xml @@ -224,6 +224,8 @@ + + diff --git a/translations/translation_hu.xml b/translations/translation_hu.xml index 0adef702..729525ce 100644 --- a/translations/translation_hu.xml +++ b/translations/translation_hu.xml @@ -221,6 +221,8 @@ + + diff --git a/translations/translation_it.xml b/translations/translation_it.xml index 6ec061b6..9f1b903d 100644 --- a/translations/translation_it.xml +++ b/translations/translation_it.xml @@ -222,6 +222,8 @@ + + diff --git a/translations/translation_nl.xml b/translations/translation_nl.xml index b50a3eb4..18d2fd5d 100644 --- a/translations/translation_nl.xml +++ b/translations/translation_nl.xml @@ -224,6 +224,8 @@ + + diff --git a/translations/translation_pl.xml b/translations/translation_pl.xml index e71d3ea4..3b41f2bf 100644 --- a/translations/translation_pl.xml +++ b/translations/translation_pl.xml @@ -223,6 +223,8 @@ + + diff --git a/translations/translation_pt.xml b/translations/translation_pt.xml index cde53af4..0e2de0b3 100644 --- a/translations/translation_pt.xml +++ b/translations/translation_pt.xml @@ -222,6 +222,8 @@ + + diff --git a/translations/translation_ru.xml b/translations/translation_ru.xml index 4df34ae5..46b3f517 100644 --- a/translations/translation_ru.xml +++ b/translations/translation_ru.xml @@ -222,6 +222,8 @@ + + diff --git a/translations/translation_tr.xml b/translations/translation_tr.xml index bbb2e135..fd60b65f 100644 --- a/translations/translation_tr.xml +++ b/translations/translation_tr.xml @@ -220,6 +220,8 @@ + +