31 changed files with 838 additions and 162 deletions
@ -0,0 +1,46 @@ |
|||
package com.galaxis.rcs.common.enums; |
|||
|
|||
public enum BizTaskStatus { |
|||
/** |
|||
* 待调度 |
|||
*/ |
|||
WAITING_FOR_DISPATCH, |
|||
|
|||
/** |
|||
* 设备执行中 |
|||
*/ |
|||
DEVICE_EXECUTING, |
|||
|
|||
/** |
|||
* 暂停执行 |
|||
*/ |
|||
PAUSED, |
|||
|
|||
/** |
|||
* 调度异常 |
|||
*/ |
|||
DISPATCH_ERROR, |
|||
|
|||
/** |
|||
* 规划异常 |
|||
*/ |
|||
PLANNING_ERROR, |
|||
|
|||
/** |
|||
* 设备执行异常 |
|||
*/ |
|||
DEVICE_ERROR; |
|||
|
|||
public static BizTaskStatus fromString(String value) { |
|||
if (value == null) |
|||
throw new IllegalArgumentException("Value cannot be null"); |
|||
|
|||
for (BizTaskStatus type : BizTaskStatus.values()) { |
|||
if (type.toString().equalsIgnoreCase(value.trim())) { |
|||
return type; |
|||
} |
|||
} |
|||
|
|||
throw new IllegalArgumentException("No constant with name: " + value); |
|||
} |
|||
} |
|||
@ -1,5 +1,18 @@ |
|||
package com.galaxis.rcs.common.enums; |
|||
|
|||
public enum BizTaskType { |
|||
CARRY, // 搬运任务
|
|||
CARRY; // 搬运任务
|
|||
|
|||
public static BizTaskType fromString(String value) { |
|||
if (value == null) |
|||
throw new IllegalArgumentException("Value cannot be null"); |
|||
|
|||
for (BizTaskType type : BizTaskType.values()) { |
|||
if (type.toString().equalsIgnoreCase(value.trim())) { |
|||
return type; |
|||
} |
|||
} |
|||
|
|||
throw new IllegalArgumentException("No constant with name: " + value); |
|||
} |
|||
} |
|||
|
|||
@ -0,0 +1,26 @@ |
|||
package com.galaxis.rcs.common.enums; |
|||
|
|||
/** |
|||
* 表示货位 与 路标点, 在世界坐标中的相对位置关系 |
|||
*/ |
|||
public enum OperationSide { |
|||
/** |
|||
* 货位在路标点的上方 |
|||
*/ |
|||
TOP, |
|||
|
|||
/** |
|||
* 货位在路标点的下方 |
|||
*/ |
|||
BOTTOM, |
|||
|
|||
/** |
|||
* 货位在路标点的左侧 |
|||
*/ |
|||
LEFT, |
|||
|
|||
/** |
|||
* 货位在路标点的右侧 |
|||
*/ |
|||
RIGHT |
|||
} |
|||
@ -0,0 +1,29 @@ |
|||
package com.galaxis.rcs.plan.path; |
|||
|
|||
/** |
|||
* A*路径节点状态 |
|||
*/ |
|||
public record AStarNodeState( |
|||
String nodeId, // 当前节点ID
|
|||
int direction, // 当前方向 (0,90,180,270)
|
|||
double gCost, // 实际代价
|
|||
double hCost, // 启发式代价
|
|||
AStarNodeState parent // 父节点
|
|||
) implements Comparable<AStarNodeState> { |
|||
|
|||
// 状态唯一标识
|
|||
public String stateKey() { |
|||
return nodeId + ":" + direction; |
|||
} |
|||
|
|||
// 总代价
|
|||
public double fCost() { |
|||
return gCost + hCost; |
|||
} |
|||
|
|||
@Override |
|||
public int compareTo(AStarNodeState other) { |
|||
return Double.compare(this.fCost(), other.fCost()); |
|||
} |
|||
} |
|||
|
|||
@ -0,0 +1,122 @@ |
|||
package com.galaxis.rcs.plan.path; |
|||
|
|||
import java.util.*; |
|||
|
|||
/** |
|||
* A*路径规划器 |
|||
*/ |
|||
public class AStarPathPlanner { |
|||
private final NavigationGraph graph; |
|||
|
|||
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, Double> 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.0); |
|||
|
|||
while (!openSet.isEmpty()) { |
|||
AStarNodeState current = openSet.poll(); |
|||
|
|||
// 到达目标状态
|
|||
if (current.nodeId().equals(endNodeId) && |
|||
current.direction() == endDirection) { |
|||
return reconstructPath(cameFrom, current); |
|||
} |
|||
|
|||
// 处理移动操作 (到相邻节点)
|
|||
for (NavigationNode neighbor : graph.getAdjacentNodes(current.nodeId())) { |
|||
PathSegment segment = graph.getPathSegment(current.nodeId(), neighbor.id()); |
|||
if (segment == null) continue; |
|||
|
|||
double moveCost = segment.distance(); |
|||
double tentativeGCost = current.gCost() + moveCost; |
|||
String neighborKey = neighbor.id() + ":" + current.direction(); |
|||
|
|||
// 发现更好路径
|
|||
if (tentativeGCost < gScoreMap.getOrDefault(neighborKey, Double.MAX_VALUE)) { |
|||
AStarNodeState neighborState = new AStarNodeState( |
|||
neighbor.id(), |
|||
current.direction(), |
|||
tentativeGCost, |
|||
graph.heuristicCost(neighbor.id(), endNodeId), |
|||
current |
|||
); |
|||
|
|||
cameFrom.put(neighborKey, current); |
|||
gScoreMap.put(neighborKey, tentativeGCost); |
|||
openSet.add(neighborState); |
|||
} |
|||
} |
|||
|
|||
// 处理旋转操作 (当前节点可旋转时)
|
|||
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; |
|||
double rotateCost = 1.0; // 旋转代价
|
|||
double tentativeGCost = current.gCost() + rotateCost; |
|||
String rotatedKey = current.nodeId() + ":" + newDirection; |
|||
|
|||
// 发现更好路径
|
|||
if (tentativeGCost < gScoreMap.getOrDefault(rotatedKey, Double.MAX_VALUE)) { |
|||
AStarNodeState rotatedState = new AStarNodeState( |
|||
current.nodeId(), |
|||
newDirection, |
|||
tentativeGCost, |
|||
current.hCost(), // 旋转不改变位置，启发值不变
|
|||
current |
|||
); |
|||
|
|||
cameFrom.put(rotatedKey, current); |
|||
gScoreMap.put(rotatedKey, tentativeGCost); |
|||
openSet.add(rotatedState); |
|||
} |
|||
} |
|||
} |
|||
} |
|||
|
|||
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()); |
|||
} |
|||
|
|||
return path; |
|||
} |
|||
} |
|||
@ -1,4 +0,0 @@ |
|||
package com.galaxis.rcs.plan.path; |
|||
|
|||
public class ChargerPoint { |
|||
} |
|||
@ -0,0 +1,95 @@ |
|||
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.executorGraphMap.containsKey(agvType)) { |
|||
return runtime.executorGraphMap.get(agvType); |
|||
} |
|||
NavigationGraph graph = new NavigationGraph(); |
|||
runtime.executorGraphMap.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<Double>> tf = (List<List<Double>>) nodeData.get("tf"); |
|||
double x = tf.get(0).get(0); |
|||
double 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; |
|||
} |
|||
} |
|||
@ -0,0 +1,69 @@ |
|||
package com.galaxis.rcs.plan.path; |
|||
|
|||
import java.util.*; |
|||
import java.util.concurrent.ConcurrentHashMap; |
|||
|
|||
/** |
|||
* 导航图管理器 |
|||
*/ |
|||
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<>()); |
|||
} |
|||
|
|||
// 添加双向路径
|
|||
public void addBidirectionalPath(NavigationNode a, NavigationNode b) { |
|||
double 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)); |
|||
|
|||
adjacencyList.get(a.id()).add(b); |
|||
adjacencyList.get(b.id()).add(a); |
|||
} |
|||
|
|||
// 添加操作点
|
|||
public void addOperationPoint(OperationPoint point) { |
|||
operationPoints.put(point.locationKey(), point); |
|||
} |
|||
|
|||
// 查找操作点
|
|||
public OperationPoint findOperationPoint(String locationKey) { |
|||
return operationPoints.get(locationKey); |
|||
} |
|||
|
|||
// 获取路径段
|
|||
public PathSegment getPathSegment(String startId, String endId) { |
|||
return pathSegments.get(startId + "->" + endId); |
|||
} |
|||
|
|||
// 获取相邻节点
|
|||
public List<NavigationNode> getAdjacentNodes(String nodeId) { |
|||
return adjacencyList.getOrDefault(nodeId, Collections.emptyList()); |
|||
} |
|||
|
|||
// 获取最近的旋转点
|
|||
public NavigationNode findNearestRotationPoint(String startId) { |
|||
NavigationNode start = nodes.get(startId); |
|||
if (start == null) return null; |
|||
|
|||
return nodes.values().parallelStream() |
|||
.filter(NavigationNode::rotatable) |
|||
.min(Comparator.comparingDouble(start::distanceTo)) |
|||
.orElse(null); |
|||
} |
|||
|
|||
// 计算启发式代价 (使用欧几里得距离)
|
|||
public double heuristicCost(String fromId, String toId) { |
|||
NavigationNode from = nodes.get(fromId); |
|||
NavigationNode to = nodes.get(toId); |
|||
if (from == null || to == null) return Double.MAX_VALUE; |
|||
return from.distanceTo(to); |
|||
} |
|||
} |
|||
@ -0,0 +1,23 @@ |
|||
package com.galaxis.rcs.plan.path; |
|||
|
|||
/** |
|||
* 路标节点（地图中的顶点） |
|||
*/ |
|||
public record NavigationNode( |
|||
String id, // 节点ID (如 "20", "charger1")
|
|||
double x, // 向右增长
|
|||
double z, // 向下增长 (y无效)
|
|||
boolean rotatable // 是否为可旋转位
|
|||
) implements Comparable<NavigationNode> { |
|||
// 计算到另一个节点的欧几里得距离
|
|||
public double distanceTo(NavigationNode other) { |
|||
double dx = this.x - other.x; |
|||
double dz = this.z - other.z; |
|||
return Math.sqrt(dx * dx + dz * dz); |
|||
} |
|||
|
|||
@Override |
|||
public int compareTo(NavigationNode other) { |
|||
return this.id.compareTo(other.id); |
|||
} |
|||
} |
|||
@ -0,0 +1,33 @@ |
|||
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,7 +0,0 @@ |
|||
package com.galaxis.rcs.plan.path; |
|||
|
|||
/** |
|||
* 物流任务路径信息管理类 |
|||
*/ |
|||
public class PathInfoManager { |
|||
} |
|||
@ -0,0 +1,15 @@ |
|||
package com.galaxis.rcs.plan.path; |
|||
|
|||
/** |
|||
* 路径段（节点之间的连接） |
|||
*/ |
|||
public record PathSegment( |
|||
NavigationNode start, // 起点节点
|
|||
NavigationNode end, // 终点节点
|
|||
double distance // 路径距离
|
|||
) { |
|||
// 路径唯一标识
|
|||
public String key() { |
|||
return start.id() + "->" + end.id(); |
|||
} |
|||
} |
|||
@ -1,4 +0,0 @@ |
|||
package com.galaxis.rcs.plan.path; |
|||
|
|||
public class Point { |
|||
} |
|||
@ -1,4 +0,0 @@ |
|||
package com.galaxis.rcs.plan.path; |
|||
|
|||
public class StorePoint { |
|||
} |
|||
@ -1,4 +0,0 @@ |
|||
package com.galaxis.rcs.plan.path; |
|||
|
|||
public class WayPoint { |
|||
} |
|||
Loading…
Reference in new issue