21 changed files with 2105 additions and 179 deletions
File diff suppressed because it is too large
@ -1,8 +0,0 @@ |
|||
package com.galaxis.rcs.plan.path2; |
|||
|
|||
import java.util.ArrayList; |
|||
import java.util.HashMap; |
|||
import java.util.List; |
|||
|
|||
public class AGVPathPlanner { |
|||
} |
|||
@ -0,0 +1,220 @@ |
|||
package com.galaxis.rcs.plan.path2; |
|||
|
|||
import com.galaxis.rcs.common.enums.LCCDirection; |
|||
import com.google.common.collect.Maps; |
|||
|
|||
import java.util.*; |
|||
|
|||
import static com.galaxis.rcs.plan.path2.PathUtils.*; |
|||
|
|||
public class AStarPathPlanner { |
|||
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; // 权重因子
|
|||
|
|||
private final NavigationGraph graph; |
|||
private final Map<String, Float> nodeWeights = Maps.newConcurrentMap(); |
|||
private final Map<String, Float> blockedNodes = Maps.newConcurrentMap(); |
|||
|
|||
public AStarPathPlanner(NavigationGraph graph) { |
|||
this.graph = graph; |
|||
} |
|||
|
|||
// 路径规划状态
|
|||
public List<State> findPath(String startId, LCCDirection startDirection, String endId, LCCDirection endDirection) { |
|||
Node start = graph.getNode(startId); |
|||
Node end = graph.getNode(endId); |
|||
if (start == null) { |
|||
throw new RuntimeException("Start node not found: " + startId); |
|||
} |
|||
if (end == null) { |
|||
throw new RuntimeException("End node not found: " + endId); |
|||
} |
|||
|
|||
// 使用复合键避免状态重复
|
|||
Map<String, State> visited = new HashMap<>(); |
|||
PriorityQueue<State> open = new PriorityQueue<>(); |
|||
|
|||
// 初始状态
|
|||
State initialState = new State(start, startDirection, 0, heuristic(start, end), null); |
|||
open.add(initialState); |
|||
visited.put(stateKey(start.id(), startDirection), initialState); |
|||
|
|||
while (!open.isEmpty()) { |
|||
State current = open.poll(); |
|||
|
|||
// 到达目标节点且方向匹配
|
|||
if (current.node().id().equals(endId) && current.direction() == endDirection) { |
|||
return buildPath(current); |
|||
} |
|||
|
|||
// 处理邻边移动
|
|||
for (Node neighbor : graph.getNeighbors(current.node())) { |
|||
// 检查节点是否被阻塞
|
|||
if (isBlocked(neighbor.id())) continue; |
|||
|
|||
// 计算移动方向
|
|||
LCCDirection moveDirection = calculateMoveDirection(current.node(), neighbor); |
|||
|
|||
// 尝试前进
|
|||
if (canMoveForward(current.direction(), moveDirection)) { |
|||
float moveCost = calculateMoveCost(current.node(), neighbor, false); |
|||
considerState(current, neighbor, current.direction(), |
|||
moveCost, open, visited, end); |
|||
} |
|||
// 尝试后退
|
|||
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()) { |
|||
// 计算需要旋转到的方向
|
|||
LCCDirection requiredDirection = calculateRequiredDirection(moveDirection); |
|||
|
|||
// 考虑旋转后移动
|
|||
float rotationCost = calculateRotationCost( |
|||
current.direction(), requiredDirection, ROTATION_COST_PER_DEGREE |
|||
); |
|||
float moveCost = calculateMoveCost(current.node(), neighbor, false); |
|||
float totalCost = rotationCost + moveCost; |
|||
|
|||
// 创建旋转状态
|
|||
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()) { |
|||
// 只考虑旋转到目标方向（如果可能）
|
|||
if (current.direction() != endDirection) { |
|||
float rotationCost = calculateRotationCost( |
|||
current.direction(), endDirection, ROTATION_COST_PER_DEGREE |
|||
); |
|||
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(); |
|||
} |
|||
|
|||
/** |
|||
* 考虑新的状态并更新开放列表和访问记录 |
|||
* |
|||
* @param current 当前状态 |
|||
* @param nextNode 下一个节点 |
|||
* @param nextDirection 下一个方向 |
|||
* @param cost 移动成本 |
|||
* @param open 开放列表 |
|||
* @param visited 访问记录 |
|||
* @param end 目标节点 |
|||
*/ |
|||
private void considerState(State current, Node nextNode, LCCDirection nextDirection, |
|||
float cost, PriorityQueue<State> open, |
|||
Map<String, State> visited, Node end) { |
|||
String key = stateKey(nextNode.id(), nextDirection); |
|||
float newG = current.g() + cost; |
|||
|
|||
if (!visited.containsKey(key) || visited.get(key).g() > newG) { |
|||
float h = heuristic(nextNode, end); |
|||
State newState = new State(nextNode, nextDirection, newG, h, current); |
|||
open.add(newState); |
|||
visited.put(key, newState); |
|||
} |
|||
} |
|||
|
|||
private List<State> buildPath(State state) { |
|||
LinkedList<State> path = new LinkedList<>(); |
|||
while (state != null) { |
|||
path.addFirst(state); |
|||
state = state.parent(); |
|||
} |
|||
return path; |
|||
} |
|||
|
|||
/** |
|||
* 启发式函数，计算两个节点之间的距离 |
|||
*/ |
|||
private float heuristic(Node a, Node b) { |
|||
return graph.distance(a, b); |
|||
// 使用曼哈顿距离？？
|
|||
// return Math.abs(a.x() - b.x()) + Math.abs(a.z() - b.z());
|
|||
} |
|||
|
|||
/** |
|||
* 生成状态的唯一键 |
|||
*/ |
|||
private String stateKey(String nodeId, LCCDirection direction) { |
|||
return nodeId + "|" + direction; |
|||
} |
|||
|
|||
public void setNodeWeight(String nodeId, float weight) { |
|||
nodeWeights.put(nodeId, weight); |
|||
} |
|||
|
|||
public void setBlocked(String nodeId, float blockedFactor) { |
|||
blockedNodes.put(nodeId, blockedFactor); |
|||
} |
|||
|
|||
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; |
|||
} |
|||
} |
|||
@ -0,0 +1,168 @@ |
|||
package com.galaxis.rcs.plan.path2; |
|||
|
|||
import com.galaxis.rcs.common.enums.LCCDirection; |
|||
|
|||
import java.util.*; |
|||
|
|||
public class PathUtils { |
|||
/** |
|||
* 计算移动方向 |
|||
*/ |
|||
public static LCCDirection calculateMoveDirection(Node from, Node to) { |
|||
float dx = to.x() - from.x(); |
|||
float dz = to.z() - from.z(); // 注意：Z轴向下增长
|
|||
|
|||
// 考虑左手坐标系：X向右，Z向下
|
|||
if (Math.abs(dx) > Math.abs(dz)) { |
|||
return dx > 0.5 ? LCCDirection.RIGHT : LCCDirection.LEFT; |
|||
} else { |
|||
// 注意：Z向下增长，所以Z值增加表示向下移动
|
|||
return dz > 0.5 ? LCCDirection.DOWN : LCCDirection.UP; |
|||
} |
|||
} |
|||
|
|||
/** |
|||
* 检查方向是否有效（侧插式专用） |
|||
*/ |
|||
public static boolean isValidForSideLoader(Node node, int bay, LCCDirection direction) { |
|||
Optional<StoreLink> link = node.storeLinks().stream() |
|||
.filter(l -> l.bay() == bay) |
|||
.findFirst(); |
|||
|
|||
if (link.isEmpty()) return false; |
|||
|
|||
LCCDirection requiredDirection = convertForSideLoader(link.get().direction()); |
|||
return direction == requiredDirection; |
|||
} |
|||
|
|||
/** |
|||
* 转换货位方向到AGV所需方向（侧插式专用） |
|||
*/ |
|||
public static LCCDirection convertForSideLoader(LCCDirection storeDirection) { |
|||
/* |
|||
* 侧插式AGV方向规则： |
|||
* - 货位在上方 → 车头向右（货叉朝左） |
|||
* - 货位在下方 → 车头向左（货叉朝左） |
|||
* - 货位在左方 → 车头向上（货叉朝左） |
|||
* - 货位在右方 → 车头向下（货叉朝左） |
|||
*/ |
|||
return switch (storeDirection) { |
|||
case UP -> LCCDirection.RIGHT; |
|||
case DOWN -> LCCDirection.LEFT; |
|||
case LEFT -> LCCDirection.UP; |
|||
case RIGHT -> LCCDirection.DOWN; |
|||
}; |
|||
} |
|||
|
|||
/** |
|||
* 获取最近的旋转点 |
|||
*/ |
|||
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); |
|||
|
|||
Node result = null; |
|||
|
|||
while (!queue.isEmpty()) { |
|||
Node current = queue.poll(); |
|||
|
|||
// 找到可旋转点
|
|||
if (current.rotatable()) { |
|||
result = current; |
|||
break; |
|||
} |
|||
|
|||
// 探索邻居
|
|||
for (Node neighbor : graph.getNeighbors(current)) { |
|||
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 result; |
|||
} |
|||
|
|||
/** |
|||
* 获取相反方向 |
|||
*/ |
|||
public static LCCDirection getOppositeDirection(LCCDirection dir) { |
|||
return switch (dir) { |
|||
case UP -> LCCDirection.DOWN; |
|||
case DOWN -> LCCDirection.UP; |
|||
case LEFT -> LCCDirection.RIGHT; |
|||
case RIGHT -> LCCDirection.LEFT; |
|||
}; |
|||
} |
|||
|
|||
/** |
|||
* 计算旋转代价 |
|||
*/ |
|||
public static float calculateRotationCost(LCCDirection from, LCCDirection to, float ROTATION_COST_PER_DEGREE) { |
|||
float angle1 = getRequiredDirection(from); |
|||
float angle2 = getRequiredDirection(to); |
|||
float diff = Math.abs(angle1 - angle2); |
|||
diff = Math.min(diff, 360 - diff); |
|||
// return diff * ROTATION_COST_PER_DEGREE;
|
|||
return 0.1f; |
|||
} |
|||
|
|||
/** |
|||
* 获取所需方向的角度 |
|||
*/ |
|||
public static float getRequiredDirection(LCCDirection direction) { |
|||
return switch (direction) { |
|||
case UP -> 90f; |
|||
case DOWN -> 270f; |
|||
case LEFT -> 180f; |
|||
case RIGHT -> 0f; |
|||
}; |
|||
} |
|||
|
|||
/** |
|||
* 转换货位方向到AGV所需方向 |
|||
*/ |
|||
public static LCCDirection convertStoreDirection(LCCDirection storeDirection) { |
|||
// 转换规则: 货位在路径点的方位 -> AGV所需方向
|
|||
return switch (storeDirection) { |
|||
case UP -> LCCDirection.RIGHT; // 货位在上方 → 车头向右
|
|||
case DOWN -> LCCDirection.LEFT; // 货位在下方 → 车头向左
|
|||
case LEFT -> LCCDirection.UP; // 货位在左方 → 车头向上
|
|||
case RIGHT -> LCCDirection.DOWN; // 货位在右方 → 车头向下
|
|||
}; |
|||
} |
|||
|
|||
/** |
|||
* 将角度转换为 LCCDirection |
|||
*/ |
|||
public static LCCDirection convertAngleToDirection(float angle) { |
|||
// 标准化角度
|
|||
angle = ((angle % 360f) + 360f) % 360f; |
|||
|
|||
if (angle >= 315f || angle < 45f) return LCCDirection.RIGHT; |
|||
if (angle >= 45f && angle < 135f) return LCCDirection.UP; |
|||
if (angle >= 135f && angle < 225f) return LCCDirection.LEFT; |
|||
return LCCDirection.DOWN; |
|||
} |
|||
|
|||
/** |
|||
* 计算方向角度差 |
|||
*/ |
|||
public static float angleDifference(float angle1, float angle2) { |
|||
float diff = Math.abs(angle1 - angle2) % 360; |
|||
return diff > 180 ? 360 - diff : diff; |
|||
} |
|||
} |
|||
@ -0,0 +1,142 @@ |
|||
package com.galaxis.rcs.plan.path2; |
|||
|
|||
import com.galaxis.rcs.common.enums.LCCDirection; |
|||
import com.galaxis.rcs.plan.PlanTaskSequence; |
|||
import com.galaxis.rcs.plan.task.CarryTask; |
|||
|
|||
import java.util.List; |
|||
|
|||
import static com.galaxis.rcs.plan.path2.PathUtils.convertStoreDirection; |
|||
import static com.galaxis.rcs.plan.path2.PathUtils.getRequiredDirection; |
|||
|
|||
public class PtrPathPlanner { |
|||
private final NavigationGraph graph; |
|||
private final AStarPathPlanner astar; |
|||
|
|||
public PtrPathPlanner(NavigationGraph graph) { |
|||
this.graph = graph; |
|||
this.astar = new AStarPathPlanner(graph); |
|||
} |
|||
|
|||
public void planCarryTask(PlanTaskSequence plan, String startNodeId, LCCDirection startDirection, CarryTask task) { |
|||
// 取货点
|
|||
String loadRackId = task.from().rackId(); |
|||
int pickupBay = task.from().bay(); |
|||
NodeDirection loadNodeDirection = findNodeForStore(loadRackId, pickupBay); |
|||
if (loadNodeDirection == null) { |
|||
throw new RuntimeException("Pickup node not found for rackId=" + loadRackId + ", bay=" + pickupBay); |
|||
} |
|||
|
|||
// 放货点
|
|||
String unloadRackId = task.to().rackId(); |
|||
int unloadBay = task.to().bay(); |
|||
NodeDirection unloadNodeDirection = findNodeForStore(unloadRackId, unloadBay); |
|||
if (unloadNodeDirection == null) { |
|||
throw new RuntimeException("Drop node not found for rackId=" + unloadRackId + ", bay=" + unloadBay); |
|||
} |
|||
|
|||
// 规划到取货点路径
|
|||
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()); |
|||
|
|||
// 生成指令序列
|
|||
generateMoves(plan, toLoadPath); |
|||
plan.addLoad(task.lpn(), loadRackId, pickupBay, task.from().level(), task.from().cell()); |
|||
|
|||
generateMoves(plan, toUnloadPath); |
|||
plan.addUnload(unloadRackId, task.to().level(), task.to().bay(), task.to().cell()); |
|||
|
|||
plan.addFinish(); |
|||
} |
|||
|
|||
private NodeDirection findNodeForStore(String storeId, int bay) { |
|||
List<Node> nodes = this.graph.getNodesForStore(storeId); |
|||
for (Node node : nodes) { |
|||
for (StoreLink link : node.storeLinks()) { |
|||
if (link.storeId().equals(storeId) && link.bay() == bay) { |
|||
LCCDirection agvDirection = convertStoreDirection(link.direction()); |
|||
return new NodeDirection(node, agvDirection); |
|||
} |
|||
} |
|||
} |
|||
return null; |
|||
} |
|||
|
|||
/** |
|||
* 获取指定节点和货架的所需朝向 |
|||
*/ |
|||
private LCCDirection getRequiredHeading(Node node, int bay) { |
|||
return node.storeLinks().stream() |
|||
.filter(link -> link.bay() == bay) |
|||
.findFirst() |
|||
.map(link -> link.direction()) |
|||
.orElseThrow(() -> new RuntimeException("Not found storeLink in id=" + node.id() + ", bay=" + bay)); |
|||
} |
|||
|
|||
/** |
|||
* 根据A*状态，生成移动指令序列 |
|||
*/ |
|||
private void generateMoves(PlanTaskSequence sequence, List<State> path) { |
|||
if (path.isEmpty()) return; |
|||
|
|||
// 第一个状态是起点，跳过
|
|||
State prevState = path.get(0); |
|||
|
|||
for (int i = 1; i < path.size(); i++) { |
|||
State current = path.get(i); |
|||
|
|||
// 如果是旋转动作
|
|||
if (current.node().equals(prevState.node())) { |
|||
float angle = PathUtils.getRequiredDirection(current.direction()); |
|||
sequence.addRotationTo(angle); |
|||
} |
|||
// 移动动作 - 检查是否需要后退
|
|||
else { |
|||
// 检查移动方向
|
|||
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; |
|||
} |
|||
} |
|||
|
|||
// 辅助记录类
|
|||
private record NodeDirection(Node node, LCCDirection direction) { |
|||
} |
|||
} |
|||
@ -0,0 +1,19 @@ |
|||
package com.galaxis.rcs.plan.path2; |
|||
|
|||
import com.galaxis.rcs.common.enums.LCCDirection; |
|||
|
|||
/** |
|||
* A* 路径规划状态 |
|||
*/ |
|||
public record State(Node node, |
|||
LCCDirection direction, |
|||
float g, |
|||
float h, |
|||
State parent) |
|||
implements Comparable<State> { |
|||
|
|||
@Override |
|||
public int compareTo(State other) { |
|||
return Float.compare(g + h, other.g + other.h); |
|||
} |
|||
} |
|||
@ -0,0 +1,34 @@ |
|||
package com.galaxis.rcs.plan.task; |
|||
|
|||
import com.galaxis.rcs.common.entity.StoreLocation; |
|||
|
|||
/** |
|||
* 搬运任务 |
|||
* <pre> |
|||
* { |
|||
* type: 'carry', // 任务类型
|
|||
* agv: 'cl2', // 车号
|
|||
* lpn: 'pallet1124', // 托盘ID,用于校验，规划器不用管
|
|||
* priority: 1, // 优先级,用于排车，规划器不用管
|
|||
* // 搬运源货位
|
|||
* from: { |
|||
* item: 'rack1',// 货架编号
|
|||
* bay: 0, // 货架列
|
|||
* level: 1, // 货架层(用于机械臂，规划器不用管)
|
|||
* cell: 0 // 货架格(用于机械臂，规划器不用管)
|
|||
* }, |
|||
* // 目标货位
|
|||
* to: { |
|||
* item: '54' // 地堆货位号
|
|||
* } |
|||
* } |
|||
* </pre> |
|||
*/ |
|||
public record CarryTask( |
|||
String agv, |
|||
String lpn, |
|||
int priority, |
|||
StoreLocation from, |
|||
StoreLocation to |
|||
) { |
|||
} |
|||
Loading…
Reference in new issue