MOVE_BACKWARD 支持

servo/src/main/java/com/galaxis/rcs/common/enums/PlanTaskType.java

@@ -5,6 +5,7 @@ package com.galaxis.rcs.common.enums;
  */
 public enum PlanTaskType {
     MOVE,        // 移动任务
+    MOVE_BACKWARD,        // 移动任务
     ROTATION,    // 旋转任务
     LOAD, // 取货任务
     UNLOAD, // 装载任务

servo/src/main/java/com/galaxis/rcs/plan/PlanTaskSequence.java

@@ -63,6 +63,13 @@ public class PlanTaskSequence {
         return task;
     }
 
+    public RcsTaskPlan addMoveBackward(String wayPointId) {
+        RcsTaskPlan task = this.createTaskPlanEntity(PlanTaskType.MOVE_BACKWARD.toString());
+        task.setTargetId(wayPointId);
+        this.lastWayPointId = wayPointId;
+        return task;
+    }
+
     // 添加旋转动作
     public RcsTaskPlan addRotationTo(float rotationAngle) {
         RcsTaskPlan task = this.createTaskPlanEntity(PlanTaskType.ROTATION.toString());
@@ -116,6 +123,9 @@ public class PlanTaskSequence {
                 case MOVE:
                     taskStr = "MOVE " + task.getTargetId();
                     break;
+                case MOVE_BACKWARD:
+                    taskStr = "MOVE_BACKWARD " + task.getTargetId();
+                    break;
                 case LOAD:
                     taskStr = "LOAD " + task.getTargetId() + "_" + task.getTargetBay() + "_" + task.getTargetLevel() + "_" + task.getTargetCell();
                     break;

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

@@ -8,7 +8,7 @@ import java.util.*;
 import static com.galaxis.rcs.plan.path2.PathUtils.*;
 
 public class AStarPathPlanner {
-    private static final float ROTATION_COST_PER_DEGREE = 0.1f;
+    private static final float ROTATION_COST_PER_DEGREE = 0.01f;
     private static final float BLOCKED_COST = 10000f; // 阻塞系数成本
     private static final float WEIGHT_FACTOR = 1.5f; // 权重因子
 
@@ -44,8 +44,7 @@ public class AStarPathPlanner {
             State current = open.poll();
 
             // 到达目标节点且方向匹配
-            if (current.node().id().equals(endId) &&
-                current.direction() == endDirection) {
+            if (current.node().id().equals(endId) && current.direction() == endDirection) {
                 return buildPath(current);
             }
 
@@ -54,55 +53,85 @@ public class AStarPathPlanner {
                 // 检查节点是否被阻塞
                 if (isBlocked(neighbor.id())) continue;
 
+                // 计算移动方向
                 LCCDirection moveDirection = calculateMoveDirection(current.node(), neighbor);
 
                 // 前进所需方向
                 LCCDirection forwardHeading = moveDirection;
                 // 后退所需方向
                 LCCDirection backwardHeading = getOppositeDirection(moveDirection);
-
-                // 尝试前进
-                if (current.direction() == forwardHeading) {
-                    float moveCost = calculateMoveCost(current.node(), neighbor);
-                    considerState(current, neighbor, forwardHeading,
+// 尝试前进
+                if (canMoveForward(current.direction(), moveDirection)) {
+                    float moveCost = calculateMoveCost(current.node(), neighbor, false);
+                    considerState(current, neighbor, current.direction(),
                         moveCost, open, visited, end);
-                } else if (current.node().rotatable()) {
-                    float rotationCost = calculateRotationCost(
-                        current.direction(), forwardHeading, ROTATION_COST_PER_DEGREE
-                    );
-                    float moveCost = calculateMoveCost(current.node(), neighbor);
-                    float totalCost = rotationCost + moveCost;
-                    considerState(current, neighbor, forwardHeading,
-                        totalCost, open, visited, end);
                 }
-
                 // 尝试后退
-                if (current.direction() == backwardHeading) {
-                    float moveCost = calculateMoveCost(current.node(), neighbor);
-                    considerState(current, neighbor, backwardHeading,
+                else if (canMoveBackward(current.direction(), moveDirection)) {
+                    float moveCost = calculateMoveCost(current.node(), neighbor, true);
+                    considerState(current, neighbor, current.direction(),
                         moveCost, open, visited, end);
-                } else if (current.node().rotatable()) {
+                }
+                // 需要旋转
+                else if (current.node().rotatable()) {
+                    // 计算需要旋转到的方向
+                    LCCDirection requiredDirection = calculateRequiredDirection(moveDirection);
+
+                    // 考虑旋转后移动
                     float rotationCost = calculateRotationCost(
-                        current.direction(), backwardHeading, ROTATION_COST_PER_DEGREE
+                        current.direction(), requiredDirection, ROTATION_COST_PER_DEGREE
                     );
-                    float moveCost = calculateMoveCost(current.node(), neighbor);
+                    float moveCost = calculateMoveCost(current.node(), neighbor, false);
                     float totalCost = rotationCost + moveCost;
-                    considerState(current, neighbor, backwardHeading,
-                        totalCost, open, visited, end);
+
+                    // 创建旋转状态
+                    State rotatedState = new State(
+                        current.node(), requiredDirection,
+                        current.g() + rotationCost,
+                        heuristic(current.node(), end),
+                        current
+                    );
+
+                    // 考虑旋转后的移动
+                    considerState(rotatedState, neighbor, requiredDirection,
+                        moveCost, open, visited, end);
                 }
             }
 
-            // 处理原地旋转
+            // 处理原地旋转 - 只考虑目标方向
             if (current.node().rotatable()) {
-                for (LCCDirection rotation : LCCDirection.values()) {
-                    if (rotation == current.direction()) continue;
-
+                // 只考虑旋转到目标方向（如果可能）
+                if (current.direction() != endDirection) {
                     float rotationCost = calculateRotationCost(
-                        current.direction(), rotation, ROTATION_COST_PER_DEGREE
+                        current.direction(), endDirection, ROTATION_COST_PER_DEGREE
                     );
-                    considerState(current, current.node(), rotation,
+                    considerState(current, current.node(), endDirection,
                         rotationCost, open, visited, end);
                 }
+
+                // 另外考虑旋转到移动所需的方向
+                for (Node neighbor : graph.getNeighbors(current.node())) {
+                    LCCDirection moveDirection = calculateMoveDirection(current.node(), neighbor);
+                    LCCDirection requiredDirection = calculateRequiredDirection(moveDirection);
+
+                    if (requiredDirection != current.direction()) {
+                        float rotationCost = calculateRotationCost(
+                            current.direction(), requiredDirection, ROTATION_COST_PER_DEGREE
+                        );
+                        considerState(current, current.node(), requiredDirection,
+                            rotationCost, open, visited, end);
+                    }
+                }
+
+                // for (LCCDirection rotation : LCCDirection.values()) {
+                //     if (rotation == current.direction()) continue;
+                //
+                //     float rotationCost = calculateRotationCost(
+                //         current.direction(), rotation, ROTATION_COST_PER_DEGREE
+                //     );
+                //     considerState(current, current.node(), rotation,
+                //         rotationCost, open, visited, end);
+                // }
             }
         }
         return Collections.emptyList();
@@ -166,15 +195,30 @@ public class AStarPathPlanner {
         blockedNodes.put(nodeId, blockedFactor);
     }
 
-    private float calculateMoveCost(Node from, Node to) {
+    private float calculateMoveCost(Node from, Node to, boolean isBackward) {
         float baseCost = graph.distance(from, to);
         float weight = nodeWeights.getOrDefault(to.id(), 1.0f);
         float blockedFactor = blockedNodes.getOrDefault(to.id(), 0f);
 
+        // 后退移动增加额外成本??
+        // return baseCost * weight * (1 + blockedFactor) * (isBackward ? 1.2f : 1.0f);
         return baseCost * weight * (1 + blockedFactor);
     }
 
     private boolean isBlocked(String nodeId) {
         return blockedNodes.containsKey(nodeId) && blockedNodes.get(nodeId) > 0.8f;
     }
+
+    private boolean canMoveForward(LCCDirection currentDir, LCCDirection moveDir) {
+        return currentDir == moveDir;
+    }
+
+    private boolean canMoveBackward(LCCDirection currentDir, LCCDirection moveDir) {
+        return currentDir == getOppositeDirection(moveDir);
+    }
+
+    private LCCDirection calculateRequiredDirection(LCCDirection moveDirection) {
+        // 侧插式AGV只能前进或后退，不能横移
+        return moveDirection;
+    }
 }

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

@@ -10,12 +10,14 @@ public class PathUtils {
      */
     public static LCCDirection calculateMoveDirection(Node from, Node to) {
         float dx = to.x() - from.x();
-        float dz = to.z() - from.z();
+        float dz = to.z() - from.z(); // 注意：Z轴向下增长
 
+        // 考虑左手坐标系：X向右，Z向下
         if (Math.abs(dx) > Math.abs(dz)) {
-            return dx > 1 ? LCCDirection.RIGHT : LCCDirection.LEFT;
+            return dx > 0 ? LCCDirection.RIGHT : LCCDirection.LEFT;
         } else {
-            return dz > 1 ? LCCDirection.DOWN : LCCDirection.UP;
+            // 注意：Z向下增长，所以Z值增加表示向下移动
+            return dz > 0 ? LCCDirection.DOWN : LCCDirection.UP;
         }
     }
 
@@ -55,27 +57,43 @@ public class PathUtils {
     /**
      * 获取最近的旋转点
      */
-    public static Node findNearestRotationNode(NavigationGraph graph, Node from) {
-        // 使用BFS查找最近的旋转点
-        Queue<Node> queue = new LinkedList<>();
-        Set<String> visited = new HashSet<>();
+    public static Node findNearestRotationNode(NavigationGraph graph, Node from, LCCDirection currentDir,
+                                               LCCDirection requiredDir) {
+        // 如果当前方向已经匹配，不需要旋转
+        if (currentDir == requiredDir) return from;
+
+        // 使用Dijkstra算法查找最近的旋转点
+        Map<Node, Float> distances = new HashMap<>();
+        Map<Node, Node> predecessors = new HashMap<>();
+        PriorityQueue<Node> queue = new PriorityQueue<>(Comparator.comparingDouble(distances::get));
+
+        distances.put(from, 0f);
         queue.add(from);
-        visited.add(from.id());
+
+        Node result = null;
 
         while (!queue.isEmpty()) {
             Node current = queue.poll();
+
+            // 找到可旋转点
             if (current.rotatable()) {
-                return current;
+                result = current;
+                break;
             }
 
+            // 探索邻居
             for (Node neighbor : graph.getNeighbors(current)) {
-                if (!visited.contains(neighbor.id())) {
-                    visited.add(neighbor.id());
+                float newDist = distances.get(current) + graph.distance(current, neighbor);
+
+                if (newDist < distances.getOrDefault(neighbor, Float.MAX_VALUE)) {
+                    distances.put(neighbor, newDist);
+                    predecessors.put(neighbor, current);
                     queue.add(neighbor);
                 }
             }
         }
-        return null;
+
+        return result;
     }
 
     /**
@@ -98,7 +116,8 @@ public class PathUtils {
         float angle2 = getRequiredDirection(to);
         float diff = Math.abs(angle1 - angle2);
         diff = Math.min(diff, 360 - diff);
-        return diff * ROTATION_COST_PER_DEGREE;
+        // return diff * ROTATION_COST_PER_DEGREE;
+        return 0.1f;
     }
 
     /**

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

@@ -38,6 +38,32 @@ public class PtrPathPlanner {
         // 规划到取货点路径
         List<State> 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<State> toRotation = astar.findPath(
+                        lastState.node().id(), lastState.direction(),
+                        rotationNode.id(), loadNodeDirection.direction()
+                    );
+                    toLoadPath.addAll(toRotation);
+
+                    // 从旋转点到目标点
+                    List<State> fromRotation = astar.findPath(
+                        rotationNode.id(), loadNodeDirection.direction(),
+                        loadNodeDirection.node().id(), loadNodeDirection.direction()
+                    );
+                    toLoadPath.addAll(fromRotation);
+                }
+            }
+        }
+
         // 规划到放货点路径
         List<State> toUnloadPath = astar.findPath(loadNodeDirection.node().id(), loadNodeDirection.direction(), unloadNodeDirection.node().id(), unloadNodeDirection.direction());
 
@@ -89,12 +115,22 @@ public class PtrPathPlanner {
 
             // 如果是旋转动作
             if (current.node().equals(prevState.node())) {
-                float angle = getRequiredDirection(current.direction());
+                float angle = PathUtils.getRequiredDirection(current.direction());
                 sequence.addRotationTo(angle);
             }
-            // 移动动作
+            // 移动动作 - 检查是否需要后退
             else {
-                sequence.addMoveTo(current.node().id());
+                // 检查移动方向
+                LCCDirection moveDir = PathUtils.calculateMoveDirection(prevState.node(), current.node());
+
+                // 如果移动方向与车头方向相反，需要后退
+                boolean isBackward = (current.direction() == PathUtils.getOppositeDirection(moveDir));
+
+                if (isBackward) {
+                    sequence.addMoveBackward(current.node().id());
+                } else {
+                    sequence.addMoveTo(current.node().id());
+                }
             }
             prevState = current;
         }