13 changed files with 443 additions and 168 deletions
@ -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,115 @@ |
|||||
|
package com.galaxis.rcs.plan.path2; |
||||
|
|
||||
|
import com.galaxis.rcs.common.enums.LCCDirection; |
||||
|
|
||||
|
import java.util.*; |
||||
|
|
||||
|
public class AStarPathPlanner { |
||||
|
private static final float ROTATION_COST_PER_DEGREE = 0.1f; |
||||
|
private final NavigationGraph graph; |
||||
|
|
||||
|
public AStarPathPlanner(NavigationGraph graph) { |
||||
|
this.graph = graph; |
||||
|
} |
||||
|
|
||||
|
// 路径规划状态
|
||||
|
public List<Node> findPath(String startId, float startDirectionAngle, String endId, float endDirectionAngle) { |
||||
|
Node start = graph.getNode(startId); |
||||
|
Node goal = graph.getNode(endId); |
||||
|
if (start == null || goal == null) return Collections.emptyList(); |
||||
|
|
||||
|
// 使用复合键避免状态重复
|
||||
|
Map<String, State> visited = new HashMap<>(); |
||||
|
PriorityQueue<State> open = new PriorityQueue<>(); |
||||
|
|
||||
|
// 初始状态
|
||||
|
State initialState = new State(start, startDirectionAngle, 0, heuristic(start, goal), null); |
||||
|
open.add(initialState); |
||||
|
visited.put(stateKey(start.id(), startDirectionAngle), initialState); |
||||
|
|
||||
|
while (!open.isEmpty()) { |
||||
|
State current = open.poll(); |
||||
|
|
||||
|
// 到达目标节点且方向匹配
|
||||
|
if (current.node().id().equals(endId) && |
||||
|
current.directionAngle() == endDirectionAngle) { |
||||
|
return buildPath(current); |
||||
|
} |
||||
|
|
||||
|
// 处理邻居移动
|
||||
|
for (Node neighbor : graph.getNeighbors(current.node())) { |
||||
|
// 计算可能的两种方向（前进/后退）
|
||||
|
float[] possibleHeadings = { |
||||
|
current.directionAngle(), // 前进方向不变
|
||||
|
(current.directionAngle() + 180) % 360 // 后退方向反转
|
||||
|
}; |
||||
|
|
||||
|
for (float nextHeading : possibleHeadings) { |
||||
|
float moveCost = graph.distance(current.node(), neighbor); |
||||
|
considerState(current, neighbor, nextHeading, moveCost, open, visited, goal); |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
// 处理旋转（仅在可旋转节点）
|
||||
|
if (current.node().rotatable()) { |
||||
|
for (float rotation : new float[]{0, 90, 180, 270}) { |
||||
|
if (rotation == current.directionAngle()) continue; |
||||
|
|
||||
|
float angleDiff = Math.min( |
||||
|
Math.abs(rotation - current.directionAngle()), |
||||
|
360 - Math.abs(rotation - current.directionAngle()) |
||||
|
); |
||||
|
float rotationCost = angleDiff * ROTATION_COST_PER_DEGREE; |
||||
|
|
||||
|
considerState(current, current.node(), rotation, |
||||
|
rotationCost, open, visited, goal); |
||||
|
} |
||||
|
} |
||||
|
} |
||||
|
return Collections.emptyList(); |
||||
|
} |
||||
|
|
||||
|
private void considerState(State current, Node nextNode, float nextHeading, |
||||
|
float cost, PriorityQueue<State> open, |
||||
|
Map<String, State> visited, Node goal) { |
||||
|
String key = stateKey(nextNode.id(), nextHeading); |
||||
|
float newG = current.g() + cost; |
||||
|
|
||||
|
if (!visited.containsKey(key) || visited.get(key).g() > newG) { |
||||
|
float h = heuristic(nextNode, goal); |
||||
|
State newState = new State(nextNode, nextHeading, newG, h, current); |
||||
|
open.add(newState); |
||||
|
visited.put(key, newState); |
||||
|
} |
||||
|
} |
||||
|
|
||||
|
private List<Node> buildPath(State state) { |
||||
|
LinkedList<Node> path = new LinkedList<>(); |
||||
|
while (state != null) { |
||||
|
path.addFirst(state.node()); |
||||
|
state = state.parent(); |
||||
|
} |
||||
|
return path; |
||||
|
} |
||||
|
|
||||
|
private float heuristic(Node a, Node b) { |
||||
|
return graph.distance(a, b); |
||||
|
} |
||||
|
|
||||
|
private String stateKey(String nodeId, float directionAngle) { |
||||
|
return nodeId + "|" + directionAngle; |
||||
|
} |
||||
|
|
||||
|
/** |
||||
|
* 根据方向要求计算目标方向 |
||||
|
*/ |
||||
|
public static float getRequiredDirection(LCCDirection direction) { |
||||
|
return switch (direction) { |
||||
|
case UP -> 90f; // 车头朝上
|
||||
|
case DOWN -> 270f; // 车头朝下
|
||||
|
case LEFT -> 180f; // 车头朝左
|
||||
|
case RIGHT -> 0f; // 车头朝右
|
||||
|
default -> 0; // 默认为右
|
||||
|
}; |
||||
|
} |
||||
|
} |
||||
@ -0,0 +1,68 @@ |
|||||
|
package com.galaxis.rcs.plan.path2; |
||||
|
|
||||
|
import com.galaxis.rcs.plan.PlanTaskSequence; |
||||
|
import com.galaxis.rcs.plan.task.CarryTask; |
||||
|
|
||||
|
import java.util.List; |
||||
|
|
||||
|
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 seq, String startId, float initDirectionAngle, CarryTask task) { |
||||
|
// 取货点
|
||||
|
String pickupRackId = task.from().rackId(); |
||||
|
int pickupBay = task.from().bay(); |
||||
|
Node pickupNode = findStoreNode(pickupRackId, pickupBay); |
||||
|
float pickupRotationAngle = getRequiredHeading(pickupNode, pickupBay); |
||||
|
|
||||
|
// 放货点
|
||||
|
String dropRackId = task.to().rackId(); |
||||
|
int dropBay = task.to().bay(); |
||||
|
Node dropNode = findStoreNode(dropRackId, dropBay); |
||||
|
float dropRotationAngle = getRequiredHeading(dropNode, 0); |
||||
|
|
||||
|
// 规划到取货点路径
|
||||
|
List<Node> toPickupPath = astar.findPath(startId, initDirectionAngle, pickupNode.id(), pickupRotationAngle); |
||||
|
|
||||
|
// 规划到放货点路径
|
||||
|
List<Node> toDeliverPath = astar.findPath(pickupNode.id(), pickupRotationAngle, dropNode.id(), dropRotationAngle); |
||||
|
|
||||
|
// 生成指令序列
|
||||
|
generateMoves(seq, toPickupPath); |
||||
|
seq.addLoad(task.lpn(), pickupRackId, pickupBay, task.from().level(), task.from().cell()); |
||||
|
generateMoves(seq, toDeliverPath); |
||||
|
seq.addUnload(dropRackId, task.to().level(), task.to().bay(), task.to().cell()); |
||||
|
seq.addFinish(); |
||||
|
} |
||||
|
|
||||
|
private Node findStoreNode(String storeId, int bay) { |
||||
|
return graph.getNodesForStore(storeId).stream() |
||||
|
.map(graph::getNode) |
||||
|
.filter(node -> node.storeLinks().stream() |
||||
|
.anyMatch(link -> link.storeId().equals(storeId) && link.bay() == bay)) |
||||
|
.findFirst() |
||||
|
.orElseThrow(); |
||||
|
} |
||||
|
|
||||
|
private float getRequiredHeading(Node node, int bay) { |
||||
|
return node.storeLinks().stream() |
||||
|
.filter(link -> link.bay() == bay) |
||||
|
.findFirst() |
||||
|
.map(link -> AStarPathPlanner.getRequiredDirection(link.direction())) |
||||
|
.orElse(0f); |
||||
|
} |
||||
|
|
||||
|
private void generateMoves(PlanTaskSequence seq, List<Node> path) { |
||||
|
// 简化的指令生成（实际需处理方向变化）
|
||||
|
for (int i = 1; i < path.size(); i++) { |
||||
|
Node node = path.get(i); |
||||
|
seq.addMoveTo(node.id()); |
||||
|
} |
||||
|
} |
||||
|
} |
||||
@ -0,0 +1,13 @@ |
|||||
|
package com.galaxis.rcs.plan.path2; |
||||
|
|
||||
|
public record State(Node node, |
||||
|
float directionAngle, |
||||
|
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