43 changed files with 2147 additions and 2159 deletions
@ -1,29 +0,0 @@ |
|||
package com.galaxis.rcs.plan.path; |
|||
|
|||
/** |
|||
* A*路径节点状态 |
|||
*/ |
|||
public record AStarNodeState( |
|||
String nodeId, // 当前节点ID
|
|||
int direction, // 当前方向 (0,90,180,270)
|
|||
float gCost, // 实际代价
|
|||
float hCost, // 启发式代价
|
|||
AStarNodeState parent // 父节点
|
|||
) implements Comparable<AStarNodeState> { |
|||
|
|||
// 状态唯一标识
|
|||
public String stateKey() { |
|||
return nodeId + ":" + direction; |
|||
} |
|||
|
|||
// 总代价
|
|||
public float fCost() { |
|||
return gCost + hCost; |
|||
} |
|||
|
|||
@Override |
|||
public int compareTo(AStarNodeState other) { |
|||
return Float.compare(this.fCost(), other.fCost()); |
|||
} |
|||
} |
|||
|
|||
@ -1,122 +1,220 @@ |
|||
package com.galaxis.rcs.plan.path; |
|||
|
|||
import com.galaxis.rcs.common.enums.LCCDirection; |
|||
import com.google.common.collect.Maps; |
|||
|
|||
import java.util.*; |
|||
|
|||
/** |
|||
* A*路径规划器 |
|||
*/ |
|||
import static com.galaxis.rcs.plan.path.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; |
|||
} |
|||
|
|||
/** |
|||
* 规划路径 |
|||
* |
|||
* @param startNodeId 起始节点ID |
|||
* @param startDirection 起始方向 |
|||
* @param endNodeId 目标节点ID |
|||
* @param endDirection 目标方向 |
|||
* @return 路径节点序列 (包含方向信息) |
|||
*/ |
|||
public List<AStarNodeState> planPath(String startNodeId, int startDirection, String endNodeId, int endDirection) { |
|||
// 开放集 (优先队列)
|
|||
PriorityQueue<AStarNodeState> openSet = new PriorityQueue<>(); |
|||
|
|||
// 状态管理
|
|||
Map<String, Float> gScoreMap = new HashMap<>(); |
|||
Map<String, AStarNodeState> cameFrom = new HashMap<>(); |
|||
|
|||
// 初始化起点
|
|||
AStarNodeState start = new AStarNodeState( |
|||
startNodeId, |
|||
startDirection, |
|||
0, |
|||
graph.heuristicCost(startNodeId, endNodeId), |
|||
null |
|||
); |
|||
|
|||
openSet.add(start); |
|||
gScoreMap.put(start.stateKey(), 0.0f); |
|||
|
|||
while (!openSet.isEmpty()) { |
|||
AStarNodeState current = openSet.poll(); |
|||
|
|||
// 到达目标状态
|
|||
if (current.nodeId().equals(endNodeId) && |
|||
current.direction() == endDirection) { |
|||
return reconstructPath(cameFrom, current); |
|||
// 路径规划状态
|
|||
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 (NavigationNode neighbor : graph.getAdjacentNodes(current.nodeId())) { |
|||
PathSegment segment = graph.getPathSegment(current.nodeId(), neighbor.id()); |
|||
if (segment == null) continue; |
|||
|
|||
float moveCost = segment.distance(); |
|||
float tentativeGCost = current.gCost() + moveCost; |
|||
String neighborKey = neighbor.id() + ":" + current.direction(); |
|||
|
|||
// 发现更好路径
|
|||
if (tentativeGCost < gScoreMap.getOrDefault(neighborKey, Float.MAX_VALUE)) { |
|||
AStarNodeState neighborState = new AStarNodeState( |
|||
neighbor.id(), |
|||
current.direction(), |
|||
tentativeGCost, |
|||
graph.heuristicCost(neighbor.id(), endNodeId), |
|||
// 处理邻边移动
|
|||
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 |
|||
); |
|||
|
|||
cameFrom.put(neighborKey, current); |
|||
gScoreMap.put(neighborKey, tentativeGCost); |
|||
openSet.add(neighborState); |
|||
// 考虑旋转后的移动
|
|||
considerState(rotatedState, neighbor, requiredDirection, |
|||
moveCost, open, visited, end); |
|||
} |
|||
} |
|||
|
|||
// 处理旋转操作 (当前节点可旋转时)
|
|||
NavigationNode currentNode = graph.nodes.get(current.nodeId()); |
|||
if (currentNode != null && currentNode.rotatable()) { |
|||
for (int rotation : new int[]{90, -90}) { |
|||
int newDirection = (current.direction() + rotation + 360) % 360; |
|||
float rotateCost = 1.0f; // 旋转代价
|
|||
float tentativeGCost = current.gCost() + rotateCost; |
|||
String rotatedKey = current.nodeId() + ":" + newDirection; |
|||
|
|||
// 发现更好路径
|
|||
if (tentativeGCost < gScoreMap.getOrDefault(rotatedKey, Float.MAX_VALUE)) { |
|||
AStarNodeState rotatedState = new AStarNodeState( |
|||
current.nodeId(), |
|||
newDirection, |
|||
tentativeGCost, |
|||
current.hCost(), // 旋转不改变位置，启发值不变
|
|||
current |
|||
); |
|||
// 处理原地旋转 - 只考虑目标方向
|
|||
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); |
|||
|
|||
cameFrom.put(rotatedKey, current); |
|||
gScoreMap.put(rotatedKey, tentativeGCost); |
|||
openSet.add(rotatedState); |
|||
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(); // 未找到路径
|
|||
return Collections.emptyList(); |
|||
} |
|||
|
|||
private List<AStarNodeState> reconstructPath( |
|||
Map<String, AStarNodeState> cameFrom, |
|||
AStarNodeState endState |
|||
) { |
|||
LinkedList<AStarNodeState> path = new LinkedList<>(); |
|||
AStarNodeState current = endState; |
|||
|
|||
while (current != null) { |
|||
path.addFirst(current); |
|||
current = cameFrom.get(current.stateKey()); |
|||
/** |
|||
* 考虑新的状态并更新开放列表和访问记录 |
|||
* |
|||
* @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; |
|||
} |
|||
} |
|||
|
|||
@ -1,95 +0,0 @@ |
|||
package com.galaxis.rcs.plan.path; |
|||
|
|||
import com.galaxis.rcs.common.enums.OperationSide; |
|||
import com.yvan.logisticsModel.LogisticsRuntime; |
|||
import org.clever.core.Conv; |
|||
|
|||
import java.util.List; |
|||
import java.util.Map; |
|||
|
|||
public class GraphInitializer { |
|||
|
|||
// public NavigationGraph initializeGraph(LogisticsRuntime runtime, String agvType, List<Map<String, Object>> jsonData) {
|
|||
// if (runtime.pathPlannerMap.containsKey(agvType)) {
|
|||
// return runtime.pathPlannerMap.get(agvType);
|
|||
// }
|
|||
// NavigationGraph graph = new NavigationGraph();
|
|||
// runtime.pathPlannerMap.put(agvType, graph);
|
|||
//
|
|||
// // 第一步：创建所有节点
|
|||
// for (Map<String, Object> nodeData : jsonData) {
|
|||
// String id = (String) nodeData.get("id");
|
|||
//
|
|||
// // 判断是否是 way 类型才创建 NavigationNode
|
|||
// if (!"way".equals(nodeData.get("t"))) {
|
|||
// continue;
|
|||
// }
|
|||
//
|
|||
// List<List<Float>> tf = (List<List<Float>>) nodeData.get("tf");
|
|||
// float x = tf.get(0).get(0);
|
|||
// float z = tf.get(0).get(2);
|
|||
//
|
|||
// // 检查是否为可旋转点
|
|||
// Map<String, Object> dt = (Map<String, Object>) nodeData.get("dt");
|
|||
// boolean rotatable = false;
|
|||
// if (dt.containsKey("agvRotation")) {
|
|||
// rotatable = true;
|
|||
// }
|
|||
//
|
|||
// // 添加节点
|
|||
// graph.addNode(new NavigationNode(id, x, z, rotatable));
|
|||
// }
|
|||
//
|
|||
// // 第二步：添加路径连接
|
|||
// for (Map<String, Object> nodeData : jsonData) {
|
|||
// if (!"way".equals(nodeData.get("t"))) continue;
|
|||
//
|
|||
// String id = (String) nodeData.get("id");
|
|||
// Map<String, Object> dt = (Map<String, Object>) nodeData.get("dt");
|
|||
//
|
|||
// List<String> outEdges = (List<String>) dt.get("out");
|
|||
// if (outEdges != null) {
|
|||
// for (String neighborId : outEdges) {
|
|||
// if (graph.nodes.containsKey(id) && graph.nodes.containsKey(neighborId)) {
|
|||
// NavigationNode from = graph.nodes.get(id);
|
|||
// NavigationNode to = graph.nodes.get(neighborId);
|
|||
// graph.addBidirectionalPath(from, to);
|
|||
// }
|
|||
// }
|
|||
// }
|
|||
// }
|
|||
//
|
|||
// // 第三步：添加操作点 OperationPoint
|
|||
// for (Map<String, Object> nodeData : jsonData) {
|
|||
// if (!"way".equals(nodeData.get("t"))) continue;
|
|||
//
|
|||
// String nodeId = (String) nodeData.get("id");
|
|||
// Map<String, Object> dt = (Map<String, Object>) nodeData.get("dt");
|
|||
//
|
|||
// if (dt.containsKey("linkStore")) {
|
|||
// List<Map<String, Object>> linkStores = (List<Map<String, Object>>) dt.get("linkStore");
|
|||
// for (Map<String, Object> store : linkStores) {
|
|||
// String targetId = (String) store.get("item");
|
|||
// Integer bay = Conv.asInteger(store.get("bay"));
|
|||
// Integer level = Conv.asInteger(store.get("level"));
|
|||
// Integer cell = Conv.asInteger(store.get("cell"));
|
|||
//
|
|||
// // 根据位置确定方位（这里假设固定为 TOP，可根据 tf 中的方向判断更精确）
|
|||
// OperationSide side = OperationSide.TOP;
|
|||
//
|
|||
// OperationPoint point = new OperationPoint(
|
|||
// graph.nodes.get(nodeId),
|
|||
// targetId,
|
|||
// side,
|
|||
// bay,
|
|||
// level,
|
|||
// cell
|
|||
// );
|
|||
// graph.addOperationPoint(point);
|
|||
// }
|
|||
// }
|
|||
// }
|
|||
//
|
|||
// return graph;
|
|||
// }
|
|||
} |
|||
@ -1,69 +1,147 @@ |
|||
package com.galaxis.rcs.plan.path; |
|||
|
|||
import com.galaxis.rcs.common.enums.LCCDirection; |
|||
import com.google.common.collect.Maps; |
|||
import com.yvan.logisticsModel.LogisticsRuntime; |
|||
import com.yvan.logisticsModel.StaticItem; |
|||
import lombok.extern.slf4j.Slf4j; |
|||
|
|||
import java.util.*; |
|||
import java.util.concurrent.ConcurrentHashMap; |
|||
|
|||
/** |
|||
* 导航图管理器 |
|||
* A* 导航图 |
|||
*/ |
|||
@Slf4j |
|||
public class NavigationGraph { |
|||
final Map<String, NavigationNode> nodes = new ConcurrentHashMap<>(); |
|||
final Map<String, OperationPoint> operationPoints = new ConcurrentHashMap<>(); |
|||
final Map<String, PathSegment> pathSegments = new ConcurrentHashMap<>(); |
|||
final Map<String, List<NavigationNode>> adjacencyList = new ConcurrentHashMap<>(); |
|||
|
|||
// 添加节点
|
|||
public void addNode(NavigationNode node) { |
|||
nodes.put(node.id(), node); |
|||
adjacencyList.put(node.id(), new ArrayList<>()); |
|||
private final LogisticsRuntime runtime; |
|||
|
|||
/** |
|||
* 缓存距离计算结果，避免重复计算 |
|||
*/ |
|||
private final Map<String, Float> distanceCache = Maps.newConcurrentMap(); |
|||
|
|||
/** |
|||
* 缓存邻居节点列表，避免重复查询 |
|||
*/ |
|||
private final Map<String, List<Node>> neighborCache = Maps.newConcurrentMap(); |
|||
|
|||
/** |
|||
* 添加路径缓存 |
|||
*/ |
|||
private final Map<String, List<State>> pathCache = Maps.newConcurrentMap(); |
|||
|
|||
private final Map<String, Node> nodeMap = new HashMap<>(); |
|||
private final Map<String, List<String>> storeToNodes = new HashMap<>(); |
|||
|
|||
public NavigationGraph(LogisticsRuntime runtime) { |
|||
this.runtime = runtime; |
|||
} |
|||
|
|||
// 添加双向路径
|
|||
public void addBidirectionalPath(NavigationNode a, NavigationNode b) { |
|||
float distance = a.distanceTo(b); |
|||
pathSegments.put(a.id() + "->" + b.id(), new PathSegment(a, b, distance)); |
|||
pathSegments.put(b.id() + "->" + a.id(), new PathSegment(b, a, distance)); |
|||
public List<State> getCachedPath(String startId, LCCDirection startDirection, |
|||
String endId, LCCDirection endDirection) { |
|||
String cacheKey = startId + "|" + startDirection + "->" + endId + "|" + endDirection; |
|||
return pathCache.get(cacheKey); |
|||
} |
|||
|
|||
adjacencyList.get(a.id()).add(b); |
|||
adjacencyList.get(b.id()).add(a); |
|||
public void cachePath(String startId, LCCDirection startDirection, |
|||
String endId, LCCDirection endDirection, |
|||
List<State> path) { |
|||
String cacheKey = startId + "|" + startDirection + "->" + endId + "|" + endDirection; |
|||
pathCache.put(cacheKey, path); |
|||
} |
|||
|
|||
// 添加操作点
|
|||
public void addOperationPoint(OperationPoint point) { |
|||
operationPoints.put(point.locationKey(), point); |
|||
public Node getNodeById(String nodeId){ |
|||
return nodeMap.get(nodeId); |
|||
} |
|||
|
|||
// 查找操作点
|
|||
public OperationPoint findOperationPoint(String locationKey) { |
|||
return operationPoints.get(locationKey); |
|||
public List<Node> getNodesForStore(String storeId) { |
|||
List<Node> nodes = new ArrayList<>(); |
|||
List<String> nodeIds = storeToNodes.get(storeId); |
|||
if (nodeIds != null) { |
|||
for (String id : nodeIds) { |
|||
Node node = nodeMap.get(id); |
|||
if (node != null) { |
|||
nodes.add(node); |
|||
} |
|||
} |
|||
} |
|||
return nodes; |
|||
} |
|||
|
|||
// 获取路径段
|
|||
public PathSegment getPathSegment(String startId, String endId) { |
|||
return pathSegments.get(startId + "->" + endId); |
|||
public List<Node> getNeighbors(Node node) { |
|||
return node.neighbors().stream() |
|||
.map(nodeMap::get) |
|||
.filter(Objects::nonNull) |
|||
.toList(); |
|||
} |
|||
|
|||
// 获取相邻节点
|
|||
public List<NavigationNode> getAdjacentNodes(String nodeId) { |
|||
return adjacencyList.getOrDefault(nodeId, Collections.emptyList()); |
|||
public float distance(Node a, Node b) { |
|||
float dx = a.x() - b.x(); |
|||
float dz = a.z() - b.z(); |
|||
return (float) Math.sqrt(dx * dx + dz * dz); |
|||
} |
|||
|
|||
// 获取最近的旋转点
|
|||
public NavigationNode findNearestRotationPoint(String startId) { |
|||
NavigationNode start = nodes.get(startId); |
|||
if (start == null) return null; |
|||
|
|||
return nodes.values().parallelStream() |
|||
.filter(NavigationNode::rotatable) |
|||
.min(Comparator.comparing(start::distanceTo)) |
|||
.orElse(null); |
|||
public void init() { |
|||
var items = runtime.getStaticItems(); |
|||
for (StaticItem item : items) { |
|||
if ("way".equals(item.getT())) { |
|||
float[][] tf = item.getTf(); |
|||
String id = item.getId(); |
|||
Map<String, Object> dt = item.getDt(); |
|||
|
|||
// 提取坐标
|
|||
float x = tf[0][0]; |
|||
float z = tf[0][2]; // Z向下增长
|
|||
|
|||
// 检查可旋转性
|
|||
boolean rotatable = dt.containsKey("agvRotation"); |
|||
if (rotatable) { |
|||
log.info("Node {} is rotatable", id); |
|||
} |
|||
|
|||
// 提取邻居节点
|
|||
List<String> in = (List<String>) dt.get("in"); |
|||
List<String> out = (List<String>) dt.get("out"); |
|||
Set<String> neighbors = new HashSet<>(); |
|||
if (in != null) neighbors.addAll(in); |
|||
if (out != null) neighbors.addAll(out); |
|||
|
|||
// 提取货位链接
|
|||
List<StoreLink> storeLinks = new ArrayList<>(); |
|||
List<Map<String, Object>> links = (List<Map<String, Object>>) dt.get("linkStore"); |
|||
if (links != null) { |
|||
for (Map<String, Object> link : links) { |
|||
String storeId = (String) link.get("item"); |
|||
int bay = (Integer) link.get("bay"); |
|||
int level = (Integer) link.get("level"); |
|||
int cell = (Integer) link.get("cell"); |
|||
String direction = (String) link.get("direction"); |
|||
storeLinks.add(new StoreLink(storeId, bay, level, cell, LCCDirection.fromString(direction))); |
|||
|
|||
// 建立货位到节点的反向映射
|
|||
storeToNodes.computeIfAbsent(storeId, k -> new ArrayList<>()).add(id); |
|||
} |
|||
} |
|||
|
|||
nodeMap.put(id, new Node(id, x, z, rotatable, |
|||
new ArrayList<>(neighbors), storeLinks)); |
|||
} |
|||
} |
|||
|
|||
// 2. 验证邻居双向连接
|
|||
for (Node node : nodeMap.values()) { |
|||
Iterator<String> it = node.neighbors().iterator(); |
|||
while (it.hasNext()) { |
|||
String neighborId = it.next(); |
|||
if (!nodeMap.containsKey(neighborId)) { |
|||
it.remove(); // 移除无效邻居
|
|||
} |
|||
} |
|||
} |
|||
} |
|||
|
|||
// 计算启发式代价 (使用欧几里得距离)
|
|||
public float heuristicCost(String fromId, String toId) { |
|||
NavigationNode from = nodes.get(fromId); |
|||
NavigationNode to = nodes.get(toId); |
|||
if (from == null || to == null) return Float.MAX_VALUE; |
|||
return from.distanceTo(to); |
|||
public Node getNode(String id) { |
|||
return nodeMap.get(id); |
|||
} |
|||
} |
|||
|
|||
@ -1,25 +0,0 @@ |
|||
package com.galaxis.rcs.plan.path; |
|||
|
|||
import org.clever.core.Conv; |
|||
|
|||
/** |
|||
* 路标节点（地图中的顶点） |
|||
*/ |
|||
public record NavigationNode( |
|||
String id, // 节点ID (如 "20", "charger1")
|
|||
float x, // 向右增长
|
|||
float z, // 向下增长 (y无效)
|
|||
boolean rotatable // 是否为可旋转位
|
|||
) implements Comparable<NavigationNode> { |
|||
// 计算到另一个节点的欧几里得距离
|
|||
public float distanceTo(NavigationNode other) { |
|||
float dx = this.x - other.x; |
|||
float dz = this.z - other.z; |
|||
return Conv.asFloat(Math.sqrt(dx * dx + dz * dz)); |
|||
} |
|||
|
|||
@Override |
|||
public int compareTo(NavigationNode other) { |
|||
return this.id.compareTo(other.id); |
|||
} |
|||
} |
|||
@ -1,4 +1,4 @@ |
|||
package com.galaxis.rcs.plan.path2; |
|||
package com.galaxis.rcs.plan.path; |
|||
|
|||
import java.util.List; |
|||
|
|||
@ -1,33 +0,0 @@ |
|||
package com.galaxis.rcs.plan.path; |
|||
|
|||
import com.galaxis.rcs.common.enums.OperationSide; |
|||
|
|||
/** |
|||
* 操作位置（取货/放货/充电点） |
|||
*/ |
|||
public record OperationPoint( |
|||
NavigationNode node, // 关联的路标节点
|
|||
String targetId, // 货位ID (如 "54") 或货架ID (如 "rack1")
|
|||
OperationSide side, // 方位
|
|||
Integer bay, // 列 (货架时非空)
|
|||
Integer level, // 层 (货架时非空)
|
|||
Integer cell // 格 (货架时非空)
|
|||
) { |
|||
// 计算操作所需方向
|
|||
public int requiredDirection() { |
|||
return switch (side) { |
|||
case LEFT -> 90; |
|||
case RIGHT -> 270; |
|||
case TOP -> 0; |
|||
case BOTTOM -> 180; |
|||
}; |
|||
} |
|||
|
|||
// 位置唯一标识
|
|||
public String locationKey() { |
|||
if (bay == null || level == null || cell == null) { |
|||
return targetId; // 地堆货位
|
|||
} |
|||
return targetId + "-" + bay + "-" + level + "-" + cell; |
|||
} |
|||
} |
|||
@ -1,15 +0,0 @@ |
|||
package com.galaxis.rcs.plan.path; |
|||
|
|||
/** |
|||
* 路径段（节点之间的连接） |
|||
*/ |
|||
public record PathSegment( |
|||
NavigationNode start, // 起点节点
|
|||
NavigationNode end, // 终点节点
|
|||
float distance // 路径距离
|
|||
) { |
|||
// 路径唯一标识
|
|||
public String key() { |
|||
return start.id() + "->" + end.id(); |
|||
} |
|||
} |
|||
@ -1,4 +1,4 @@ |
|||
package com.galaxis.rcs.plan.path2; |
|||
package com.galaxis.rcs.plan.path; |
|||
|
|||
import com.galaxis.rcs.common.enums.LCCDirection; |
|||
|
|||
@ -0,0 +1,236 @@ |
|||
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.LoadTask; |
|||
import com.galaxis.rcs.plan.task.MoveTask; |
|||
import com.galaxis.rcs.plan.task.UnloadTask; |
|||
|
|||
import java.util.List; |
|||
|
|||
import static com.galaxis.rcs.plan.path.PathUtils.convertStoreDirection; |
|||
|
|||
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 planMoveTask(PlanTaskSequence plan, String startNodeId, LCCDirection startDirection, MoveTask moveTask) { |
|||
|
|||
Node node = this.graph.getNodeById(moveTask.targetWayPointId()); |
|||
|
|||
List<State> toPath = astar.findPath(startNodeId, startDirection, node.id(), moveTask.targetDirection()); |
|||
generateMoves(plan, toPath); |
|||
plan.addFinish(); |
|||
} |
|||
|
|||
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(); |
|||
} |
|||
|
|||
|
|||
public void planUnloadTask(PlanTaskSequence plan, String startNodeId, LCCDirection startDirection, UnloadTask task) { |
|||
Node fromNode = this.graph.getNodeById(startNodeId); |
|||
|
|||
// 放货点
|
|||
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> toUnloadPath = astar.findPath(startNodeId, startDirection, unloadNodeDirection.node().id(), unloadNodeDirection.direction()); |
|||
|
|||
// 检查方向是否匹配，如果不匹配则插入旋转点
|
|||
// State lastState = toUnloadPath.get(toUnloadPath.size() - 1);
|
|||
// if (startDirection != lastState.direction()) {
|
|||
// Node rotationNode = PathUtils.findNearestRotationNode(
|
|||
// graph, fromNode, startDirection, lastState.direction()
|
|||
// );
|
|||
//
|
|||
// if (rotationNode != null) {
|
|||
// // 插入旋转路径
|
|||
// List<State> toRotation = astar.findPath(
|
|||
// startNodeId, startDirection,
|
|||
// rotationNode.id(), unloadNodeDirection.direction()
|
|||
// );
|
|||
// toUnloadPath.addAll(toRotation);
|
|||
//
|
|||
// // 从旋转点到目标点
|
|||
// List<State> fromRotation = astar.findPath(
|
|||
// rotationNode.id(), unloadNodeDirection.direction(),
|
|||
// unloadNodeDirection.node().id(), unloadNodeDirection.direction()
|
|||
// );
|
|||
// toUnloadPath.addAll(fromRotation);
|
|||
// }
|
|||
// }
|
|||
|
|||
// 生成指令序列
|
|||
generateMoves(plan, toUnloadPath); |
|||
plan.addUnload(unloadRackId, task.to().level(), task.to().bay(), task.to().cell()); |
|||
|
|||
plan.addFinish(); |
|||
} |
|||
|
|||
|
|||
public void planLoadTask(PlanTaskSequence plan, String startNodeId, LCCDirection startDirection, LoadTask task) { |
|||
Node fromNode = this.graph.getNodeById(startNodeId); |
|||
|
|||
// 放货点
|
|||
String loadRackId = task.to().rackId(); |
|||
int unloadBay = task.to().bay(); |
|||
NodeDirection loadNodeDirection = findNodeForStore(loadRackId, unloadBay); |
|||
if (loadNodeDirection == null) { |
|||
throw new RuntimeException("Drop node not found for rackId=" + loadRackId + ", bay=" + unloadBay); |
|||
} |
|||
|
|||
// 规划到取货点路径
|
|||
List<State> toLoadPath = astar.findPath(startNodeId, startDirection, loadNodeDirection.node().id(), loadNodeDirection.direction()); |
|||
|
|||
// 检查方向是否匹配，如果不匹配则插入旋转点
|
|||
// State lastState = toLoadPath.get(toLoadPath.size() - 1);
|
|||
// if (startDirection != lastState.direction()) {
|
|||
// Node rotationNode = PathUtils.findNearestRotationNode(
|
|||
// graph, fromNode, startDirection, lastState.direction()
|
|||
// );
|
|||
//
|
|||
// if (rotationNode != null) {
|
|||
// // 插入旋转路径
|
|||
// List<State> toRotation = astar.findPath(
|
|||
// startNodeId, startDirection,
|
|||
// rotationNode.id(), loadNodeDirection.direction()
|
|||
// );
|
|||
// toLoadPath.addAll(toRotation);
|
|||
//
|
|||
// // 从旋转点到目标点
|
|||
// List<State> fromRotation = astar.findPath(
|
|||
// rotationNode.id(), loadNodeDirection.direction(),
|
|||
// loadNodeDirection.node().id(), loadNodeDirection.direction()
|
|||
// );
|
|||
// toLoadPath.addAll(fromRotation);
|
|||
// }
|
|||
// }
|
|||
|
|||
// 生成指令序列
|
|||
generateMoves(plan, toLoadPath); |
|||
plan.addLoad("N/A", loadRackId, 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; |
|||
} |
|||
|
|||
/** |
|||
* 根据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) { |
|||
} |
|||
} |
|||
@ -1,4 +1,4 @@ |
|||
package com.galaxis.rcs.plan.path2; |
|||
package com.galaxis.rcs.plan.path; |
|||
|
|||
import com.galaxis.rcs.common.enums.LCCDirection; |
|||
|
|||
@ -1,4 +1,4 @@ |
|||
package com.galaxis.rcs.plan.path2; |
|||
package com.galaxis.rcs.plan.path; |
|||
|
|||
import com.galaxis.rcs.common.enums.LCCDirection; |
|||
|
|||
@ -1,220 +0,0 @@ |
|||
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; |
|||
} |
|||
} |
|||
@ -1,147 +0,0 @@ |
|||
package com.galaxis.rcs.plan.path2; |
|||
|
|||
import com.galaxis.rcs.common.enums.LCCDirection; |
|||
import com.google.common.collect.Maps; |
|||
import com.yvan.logisticsModel.LogisticsRuntime; |
|||
import com.yvan.logisticsModel.StaticItem; |
|||
import lombok.extern.slf4j.Slf4j; |
|||
|
|||
import java.util.*; |
|||
|
|||
/** |
|||
* A* 导航图 |
|||
*/ |
|||
@Slf4j |
|||
public class NavigationGraph { |
|||
private final LogisticsRuntime runtime; |
|||
|
|||
/** |
|||
* 缓存距离计算结果，避免重复计算 |
|||
*/ |
|||
private final Map<String, Float> distanceCache = Maps.newConcurrentMap(); |
|||
|
|||
/** |
|||
* 缓存邻居节点列表，避免重复查询 |
|||
*/ |
|||
private final Map<String, List<Node>> neighborCache = Maps.newConcurrentMap(); |
|||
|
|||
/** |
|||
* 添加路径缓存 |
|||
*/ |
|||
private final Map<String, List<State>> pathCache = Maps.newConcurrentMap(); |
|||
|
|||
private final Map<String, Node> nodeMap = new HashMap<>(); |
|||
private final Map<String, List<String>> storeToNodes = new HashMap<>(); |
|||
|
|||
public NavigationGraph(LogisticsRuntime runtime) { |
|||
this.runtime = runtime; |
|||
} |
|||
|
|||
public List<State> getCachedPath(String startId, LCCDirection startDirection, |
|||
String endId, LCCDirection endDirection) { |
|||
String cacheKey = startId + "|" + startDirection + "->" + endId + "|" + endDirection; |
|||
return pathCache.get(cacheKey); |
|||
} |
|||
|
|||
public void cachePath(String startId, LCCDirection startDirection, |
|||
String endId, LCCDirection endDirection, |
|||
List<State> path) { |
|||
String cacheKey = startId + "|" + startDirection + "->" + endId + "|" + endDirection; |
|||
pathCache.put(cacheKey, path); |
|||
} |
|||
|
|||
public Node getNodeById(String nodeId){ |
|||
return nodeMap.get(nodeId); |
|||
} |
|||
|
|||
public List<Node> getNodesForStore(String storeId) { |
|||
List<Node> nodes = new ArrayList<>(); |
|||
List<String> nodeIds = storeToNodes.get(storeId); |
|||
if (nodeIds != null) { |
|||
for (String id : nodeIds) { |
|||
Node node = nodeMap.get(id); |
|||
if (node != null) { |
|||
nodes.add(node); |
|||
} |
|||
} |
|||
} |
|||
return nodes; |
|||
} |
|||
|
|||
public List<Node> getNeighbors(Node node) { |
|||
return node.neighbors().stream() |
|||
.map(nodeMap::get) |
|||
.filter(Objects::nonNull) |
|||
.toList(); |
|||
} |
|||
|
|||
public float distance(Node a, Node b) { |
|||
float dx = a.x() - b.x(); |
|||
float dz = a.z() - b.z(); |
|||
return (float) Math.sqrt(dx * dx + dz * dz); |
|||
} |
|||
|
|||
|
|||
public void init() { |
|||
var items = runtime.getStaticItems(); |
|||
for (StaticItem item : items) { |
|||
if ("way".equals(item.getT())) { |
|||
float[][] tf = item.getTf(); |
|||
String id = item.getId(); |
|||
Map<String, Object> dt = item.getDt(); |
|||
|
|||
// 提取坐标
|
|||
float x = tf[0][0]; |
|||
float z = tf[0][2]; // Z向下增长
|
|||
|
|||
// 检查可旋转性
|
|||
boolean rotatable = dt.containsKey("agvRotation"); |
|||
if (rotatable) { |
|||
log.info("Node {} is rotatable", id); |
|||
} |
|||
|
|||
// 提取邻居节点
|
|||
List<String> in = (List<String>) dt.get("in"); |
|||
List<String> out = (List<String>) dt.get("out"); |
|||
Set<String> neighbors = new HashSet<>(); |
|||
if (in != null) neighbors.addAll(in); |
|||
if (out != null) neighbors.addAll(out); |
|||
|
|||
// 提取货位链接
|
|||
List<StoreLink> storeLinks = new ArrayList<>(); |
|||
List<Map<String, Object>> links = (List<Map<String, Object>>) dt.get("linkStore"); |
|||
if (links != null) { |
|||
for (Map<String, Object> link : links) { |
|||
String storeId = (String) link.get("item"); |
|||
int bay = (Integer) link.get("bay"); |
|||
int level = (Integer) link.get("level"); |
|||
int cell = (Integer) link.get("cell"); |
|||
String direction = (String) link.get("direction"); |
|||
storeLinks.add(new StoreLink(storeId, bay, level, cell, LCCDirection.fromString(direction))); |
|||
|
|||
// 建立货位到节点的反向映射
|
|||
storeToNodes.computeIfAbsent(storeId, k -> new ArrayList<>()).add(id); |
|||
} |
|||
} |
|||
|
|||
nodeMap.put(id, new Node(id, x, z, rotatable, |
|||
new ArrayList<>(neighbors), storeLinks)); |
|||
} |
|||
} |
|||
|
|||
// 2. 验证邻居双向连接
|
|||
for (Node node : nodeMap.values()) { |
|||
Iterator<String> it = node.neighbors().iterator(); |
|||
while (it.hasNext()) { |
|||
String neighborId = it.next(); |
|||
if (!nodeMap.containsKey(neighborId)) { |
|||
it.remove(); // 移除无效邻居
|
|||
} |
|||
} |
|||
} |
|||
} |
|||
|
|||
public Node getNode(String id) { |
|||
return nodeMap.get(id); |
|||
} |
|||
} |
|||
@ -1,143 +0,0 @@ |
|||
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 com.galaxis.rcs.plan.task.MoveTask; |
|||
|
|||
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 planMoveTask(PlanTaskSequence plan, String startNodeId, LCCDirection startDirection, MoveTask moveTask) { |
|||
|
|||
Node node = this.graph.getNodeById(moveTask.targetWayPointId()); |
|||
|
|||
List<State> toPath = astar.findPath(startNodeId, startDirection, node.id(), startDirection); |
|||
generateMoves(plan, toPath); |
|||
plan.addFinish(); |
|||
} |
|||
|
|||
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; |
|||
} |
|||
|
|||
/** |
|||
* 根据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,15 @@ |
|||
package com.galaxis.rcs.plan.task; |
|||
|
|||
import com.galaxis.rcs.common.entity.StoreLocation; |
|||
|
|||
/** |
|||
* 装入任务 |
|||
*/ |
|||
public record LoadTask( |
|||
String agv, |
|||
int priority, |
|||
StoreLocation to |
|||
) { |
|||
} |
|||
|
|||
|
|||
@ -1,8 +1,11 @@ |
|||
package com.galaxis.rcs.plan.task; |
|||
|
|||
import com.galaxis.rcs.common.enums.LCCDirection; |
|||
|
|||
public record MoveTask( |
|||
String agv, |
|||
String targetWayPointId, |
|||
LCCDirection targetDirection, |
|||
int priority |
|||
) { |
|||
} |
|||
|
|||
@ -0,0 +1,15 @@ |
|||
package com.galaxis.rcs.plan.task; |
|||
|
|||
import com.galaxis.rcs.common.entity.StoreLocation; |
|||
|
|||
/** |
|||
* 卸载任务 |
|||
*/ |
|||
public record UnloadTask( |
|||
String agv, |
|||
int priority, |
|||
StoreLocation to |
|||
) { |
|||
} |
|||
|
|||
|
|||
@ -0,0 +1,5 @@ |
|||
package com.galaxis.rcs.ptr; |
|||
|
|||
public abstract class PtrAgvConnector { |
|||
|
|||
} |
|||
@ -1,7 +0,0 @@ |
|||
package com.yvan.logisticsModel; |
|||
|
|||
import com.galaxis.rcs.common.entity.RcsTaskPlan; |
|||
|
|||
public abstract class PtrAgvConnector { |
|||
|
|||
} |
|||
Loading…
Reference in new issue