A* 算法优化完成

servo/src/main/java/com/galaxis/rcs/RCS.java

@@ -112,6 +112,39 @@ public class RCS {
         return planSequence.toPrettyMap();
     }
 
+    public static Object runPath2() {
+        String executorId = "10"; // 执行器ID
+        String lpn = "pallet1124";
+        long envId = 1;
+
+        LogisticsRuntime logisticsRuntime = LogisticsRuntimeService.INSTANCE.findByEnvCode(envId);
+        RcsTaskBiz bizTask = new RcsTaskBiz();
+        bizTask.setBizTaskId(100L);
+        bizTask.setEnvId(envId);
+        bizTask.setBizType(BizTaskType.CARRY.toString());
+        bizTask.setLpn(lpn);
+        bizTask.setPriority(1);
+        bizTask.setTaskFrom("rack1_0_1_0");
+        bizTask.setTaskTo("54_0_0_0");
+        bizTask.setAllocatedExecutorId(executorId);
+        bizTask.setBizTaskPayload("N/A");
+        bizTask.setBizTaskErrorInfo("N/A");
+        bizTask.setBizTaskDescription("N/A");
+        bizTask.setBizTaskStatus(BizTaskStatus.WAITING_FOR_DISPATCH.toString());
+
+        PlanTaskSequence planSequence = new PlanTaskSequence(executorId, logisticsRuntime, bizTask, "demo");
+
+        CarryTask carryTask = new CarryTask(
+            executorId, lpn, 1,
+            new StoreLocation("rack1", 0, 1, 0),
+            new StoreLocation("54", 0, 0, 0)
+        );
+
+        logisticsRuntime.pathPlannerMap.get("cl2")
+            .planCarryTask(planSequence, "27", LCCDirection.UP, carryTask);
+        return planSequence.toPrettyMap();
+    }
+
     public static void runDemo() {
         String executorId = "10"; // 执行器ID
         String lpn = "pallet1124";

servo/src/main/java/com/galaxis/rcs/plan/path2/AStarPathPlanner.java

@@ -56,11 +56,7 @@ public class AStarPathPlanner {
                 // 计算移动方向
                 LCCDirection moveDirection = calculateMoveDirection(current.node(), neighbor);
 
-                // 前进所需方向
-                LCCDirection forwardHeading = moveDirection;
-                // 后退所需方向
-                LCCDirection backwardHeading = getOppositeDirection(moveDirection);
-// 尝试前进
+                // 尝试前进
                 if (canMoveForward(current.direction(), moveDirection)) {
                     float moveCost = calculateMoveCost(current.node(), neighbor, false);
                     considerState(current, neighbor, current.direction(),

servo/src/main/java/com/galaxis/rcs/plan/path2/PathUtils.java

@@ -14,10 +14,10 @@ public class PathUtils {
 
         // 考虑左手坐标系：X向右，Z向下
         if (Math.abs(dx) > Math.abs(dz)) {
-            return dx > 0 ? LCCDirection.RIGHT : LCCDirection.LEFT;
+            return dx > 0.5 ? LCCDirection.RIGHT : LCCDirection.LEFT;
         } else {
             // 注意：Z向下增长，所以Z值增加表示向下移动
-            return dz > 0 ? LCCDirection.DOWN : LCCDirection.UP;
+            return dz > 0.5 ? LCCDirection.DOWN : LCCDirection.UP;
         }
     }
 

servo/src/main/java/com/yvan/workbench/controller/EnvController.java

@@ -36,6 +36,10 @@ public class EnvController {
         return Model.newSuccess(RCS.runPath());
     }
 
+    public static Model<?> runPath2() {
+        return Model.newSuccess(RCS.runPath2());
+    }
+
     public static Model<?> getDemo() {
         return Model.newSuccess(RCS.getDemo());
     }