ChargerTask

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.*;
-import com.galaxis.rcs.plan.task.LoadTask;
-import com.galaxis.rcs.plan.task.MoveTask;
-import com.galaxis.rcs.plan.task.UnloadTask;
 
 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<State> toLoadPath = astar.findPath(startNodeId, startDirection, loadNodeDirection.node().id(), loadNodeDirection.direction());
 
         // 检查方向是否匹配，如果不匹配则插入旋转点
-         if (!toLoadPath.isEmpty()) {
+        if (!toLoadPath.isEmpty()) {
-             State lastState = toLoadPath.get(toLoadPath.size() - 1);
+            State lastState = toLoadPath.get(toLoadPath.size() - 1);
-             if (lastState.direction() != loadNodeDirection.direction()) {
+            if (lastState.direction() != loadNodeDirection.direction()) {
-                 Node rotationNode = PathUtils.findNearestRotationNode(
+                Node rotationNode = PathUtils.findNearestRotationNode(
-                     graph, lastState.node(), lastState.direction(), loadNodeDirection.direction()
+                    graph, lastState.node(), lastState.direction(), loadNodeDirection.direction()
-                 );
+                );
-
+
-                 if (rotationNode != null) {
+                if (rotationNode != null) {
-                     // 插入旋转路径
+                    // 插入旋转路径
-                     List<State> toRotation = astar.findPath(
+                    List<State> toRotation = astar.findPath(
-                         lastState.node().id(), lastState.direction(),
+                        lastState.node().id(), lastState.direction(),
-                         rotationNode.id(), loadNodeDirection.direction()
+                        rotationNode.id(), loadNodeDirection.direction()
-                     );
+                    );
-                     toLoadPath.addAll(toRotation);
+                    toLoadPath.addAll(toRotation);
-
+
-                     // 从旋转点到目标点
+                    // 从旋转点到目标点
-                     List<State> fromRotation = astar.findPath(
+                    List<State> fromRotation = astar.findPath(
-                         rotationNode.id(), loadNodeDirection.direction(),
+                        rotationNode.id(), loadNodeDirection.direction(),
-                         loadNodeDirection.node().id(), loadNodeDirection.direction()
+                        loadNodeDirection.node().id(), loadNodeDirection.direction()
-                     );
+                    );
-                     toLoadPath.addAll(fromRotation);
+                    toLoadPath.addAll(fromRotation);
-                 }
+                }
-             }
+            }
-         }
+        }
 
         // 规划到放货点路径
         List<State> toUnloadPath = astar.findPath(loadNodeDirection.node().id(), loadNodeDirection.direction(), unloadNodeDirection.node().id(), unloadNodeDirection.direction());

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) {
+}

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<String, Object> params) {
+        Object ret = getCommonParamAndCreateBizTask(params);
+        if (ret instanceof Model) {
+            // 异常
+            return (Model<Object>) 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<Object> agvMove(@RequestBody Map<String, Object> 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<Boolean> agvToCharger(@RequestBody Map<String, Object> params) {
-        throw new NotImplementedException("agvToCharger not implemented yet");
-    }
 
     public static Object getCommonParamAndCreateBizTask(@RequestBody Map<String, Object> params) {
         String projectUUID = Conv.asString(params.get("projectUUID"));