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