diff --git a/servo/src/main/java/com/galaxis/rcs/common/enums/PlanTaskType.java b/servo/src/main/java/com/galaxis/rcs/common/enums/PlanTaskType.java index 55c2b29..8af44bb 100644 --- a/servo/src/main/java/com/galaxis/rcs/common/enums/PlanTaskType.java +++ b/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, // 装载任务 diff --git a/servo/src/main/java/com/galaxis/rcs/plan/PlanTaskSequence.java b/servo/src/main/java/com/galaxis/rcs/plan/PlanTaskSequence.java index 7dff083..5b8beb3 100644 --- a/servo/src/main/java/com/galaxis/rcs/plan/PlanTaskSequence.java +++ b/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; diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path2/AStarPathPlanner.java b/servo/src/main/java/com/galaxis/rcs/plan/path2/AStarPathPlanner.java index de1356f..3cc6306 100644 --- a/servo/src/main/java/com/galaxis/rcs/plan/path2/AStarPathPlanner.java +++ b/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; + } } diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path2/PathUtils.java b/servo/src/main/java/com/galaxis/rcs/plan/path2/PathUtils.java index f373245..a5f5050 100644 --- a/servo/src/main/java/com/galaxis/rcs/plan/path2/PathUtils.java +++ b/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 queue = new LinkedList<>(); - Set visited = new HashSet<>(); + public static Node findNearestRotationNode(NavigationGraph graph, Node from, LCCDirection currentDir, + LCCDirection requiredDir) { + // 如果当前方向已经匹配，不需要旋转 + if (currentDir == requiredDir) return from; + + // 使用Dijkstra算法查找最近的旋转点 + Map distances = new HashMap<>(); + Map predecessors = new HashMap<>(); + PriorityQueue 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; } /** diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path2/PtrPathPlanner.java b/servo/src/main/java/com/galaxis/rcs/plan/path2/PtrPathPlanner.java index aa1fddf..e11e2de 100644 --- a/servo/src/main/java/com/galaxis/rcs/plan/path2/PtrPathPlanner.java +++ b/servo/src/main/java/com/galaxis/rcs/plan/path2/PtrPathPlanner.java @@ -38,6 +38,32 @@ 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); + } + } + } + // 规划到放货点路径 List 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; }