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