From 6f2f20ab7840aacbea3d95254ae3d6586047295c Mon Sep 17 00:00:00 2001 From: yvan Date: Mon, 30 Jun 2025 19:03:58 +0800 Subject: [PATCH] ChargerTask --- .../com/galaxis/rcs/plan/path/PtrPathPlanner.java | 56 +++++++++++----------- .../com/galaxis/rcs/plan/task/ChargerTask.java | 9 ++++ .../yvan/workbench/controller/RcsController.java | 42 ++++++++++++++-- 3 files changed, 76 insertions(+), 31 deletions(-) create mode 100644 servo/src/main/java/com/galaxis/rcs/plan/task/ChargerTask.java diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path/PtrPathPlanner.java b/servo/src/main/java/com/galaxis/rcs/plan/path/PtrPathPlanner.java index 665d6f0..0508c57 100644 --- a/servo/src/main/java/com/galaxis/rcs/plan/path/PtrPathPlanner.java +++ b/servo/src/main/java/com/galaxis/rcs/plan/path/PtrPathPlanner.java @@ -2,10 +2,7 @@ package com.galaxis.rcs.plan.path; import com.galaxis.rcs.common.enums.LCCDirection; import com.galaxis.rcs.plan.PlanTaskSequence; -import com.galaxis.rcs.plan.task.CarryTask; -import com.galaxis.rcs.plan.task.LoadTask; -import com.galaxis.rcs.plan.task.MoveTask; -import com.galaxis.rcs.plan.task.UnloadTask; +import com.galaxis.rcs.plan.task.*; import java.util.List; @@ -20,6 +17,9 @@ public class PtrPathPlanner { this.astar = new AStarPathPlanner(graph); } + public void planChargerTask(PlanTaskSequence planSequence, String id, LCCDirection fromDirection, ChargerTask chargerTask) { + + } public void planMoveTask(PlanTaskSequence plan, String startNodeId, LCCDirection startDirection, MoveTask moveTask) { @@ -51,30 +51,30 @@ public class PtrPathPlanner { List toLoadPath = astar.findPath(startNodeId, startDirection, loadNodeDirection.node().id(), loadNodeDirection.direction()); // 检查方向是否匹配，如果不匹配则插入旋转点 - if (!toLoadPath.isEmpty()) { - State lastState = toLoadPath.get(toLoadPath.size() - 1); - if (lastState.direction() != loadNodeDirection.direction()) { - Node rotationNode = PathUtils.findNearestRotationNode( - graph, lastState.node(), lastState.direction(), loadNodeDirection.direction() - ); - - if (rotationNode != null) { - // 插入旋转路径 - List toRotation = astar.findPath( - lastState.node().id(), lastState.direction(), - rotationNode.id(), loadNodeDirection.direction() - ); - toLoadPath.addAll(toRotation); - - // 从旋转点到目标点 - List fromRotation = astar.findPath( - rotationNode.id(), loadNodeDirection.direction(), - loadNodeDirection.node().id(), loadNodeDirection.direction() - ); - toLoadPath.addAll(fromRotation); - } - } - } + if (!toLoadPath.isEmpty()) { + State lastState = toLoadPath.get(toLoadPath.size() - 1); + if (lastState.direction() != loadNodeDirection.direction()) { + Node rotationNode = PathUtils.findNearestRotationNode( + graph, lastState.node(), lastState.direction(), loadNodeDirection.direction() + ); + + if (rotationNode != null) { + // 插入旋转路径 + List toRotation = astar.findPath( + lastState.node().id(), lastState.direction(), + rotationNode.id(), loadNodeDirection.direction() + ); + toLoadPath.addAll(toRotation); + + // 从旋转点到目标点 + List fromRotation = astar.findPath( + rotationNode.id(), loadNodeDirection.direction(), + loadNodeDirection.node().id(), loadNodeDirection.direction() + ); + toLoadPath.addAll(fromRotation); + } + } + } // 规划到放货点路径 List toUnloadPath = astar.findPath(loadNodeDirection.node().id(), loadNodeDirection.direction(), unloadNodeDirection.node().id(), unloadNodeDirection.direction()); diff --git a/servo/src/main/java/com/galaxis/rcs/plan/task/ChargerTask.java b/servo/src/main/java/com/galaxis/rcs/plan/task/ChargerTask.java new file mode 100644 index 0000000..ca5a008 --- /dev/null +++ b/servo/src/main/java/com/galaxis/rcs/plan/task/ChargerTask.java @@ -0,0 +1,9 @@ +package com.galaxis.rcs.plan.task; + +import com.galaxis.rcs.common.enums.LCCDirection; + +public record ChargerTask( + String agv, + String targetWayPointId, + LCCDirection targetDirection) { +} diff --git a/servo/src/main/java/com/yvan/workbench/controller/RcsController.java b/servo/src/main/java/com/yvan/workbench/controller/RcsController.java index a18fce1..9cde244 100644 --- a/servo/src/main/java/com/yvan/workbench/controller/RcsController.java +++ b/servo/src/main/java/com/yvan/workbench/controller/RcsController.java @@ -12,6 +12,7 @@ import com.yvan.workbench.model.entity.Model; import org.apache.commons.lang3.NotImplementedException; import org.clever.core.Conv; import org.clever.core.id.SnowFlake; +import org.clever.core.model.response.R; import org.clever.web.mvc.annotation.RequestBody; import java.util.Map; @@ -19,6 +20,44 @@ import java.util.Map; public class RcsController { static final SnowFlake snowFlake = new SnowFlake(); + public static Model agvToCharger(@RequestBody Map params) { + Object ret = getCommonParamAndCreateBizTask(params); + if (ret instanceof Model) { + // 异常 + return (Model) ret; + } + RcsCommonParam ps = (RcsCommonParam) ret; + + String targetWayPointId = Conv.asString(params.get("targetWayPointId")); + if (Strings.isNullOrEmpty(targetWayPointId)) { + return Model.newFail("targetWayPointId Must not be empty"); + } + String targetDirection = Conv.asString(params.get("targetDirection")); + if (Strings.isNullOrEmpty(targetDirection)) { + targetDirection = ps.fromDirection.toString(); + } + + StaticItem toItem = ps.runtime.getStaticItemById(targetWayPointId); + if (toItem == null) { + return Model.newFail("target wayPoint not found!"); + } + + ps.bizTask.setTaskTo(targetWayPointId); + + ChargerTask chargerTask = new ChargerTask( + ps.agvId, ps.bizTask.getTaskTo(), LCCDirection.fromString(targetDirection, ps.fromDirection) + ); + + ps.runtime.pathPlannerMap.get(ps.agv.getT()) + .planChargerTask(ps.planSequence, ps.fromItem.getId(), ps.fromDirection, chargerTask); + + ps.agv.logicX = ps.fromItem.logicX; + ps.agv.logicY = ps.fromItem.logicY; + ps.agv.dispatchTask(ps.planSequence); + + return Model.newSuccess(ps.planSequence.toPrettyMap()); + } + public static Model agvMove(@RequestBody Map params) { Object ret = getCommonParamAndCreateBizTask(params); if (ret instanceof Model) { @@ -222,9 +261,6 @@ public class RcsController { return Model.newSuccess(ps.planSequence.toPrettyMap()); } - public static Model agvToCharger(@RequestBody Map params) { - throw new NotImplementedException("agvToCharger not implemented yet"); - } public static Object getCommonParamAndCreateBizTask(@RequestBody Map params) { String projectUUID = Conv.asString(params.get("projectUUID"));