|
|
|
@ -1,5 +1,6 @@ |
|
|
|
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; |
|
|
|
|
|
|
|
@ -14,55 +15,63 @@ public class PtrPathPlanner { |
|
|
|
this.astar = new AStarPathPlanner(graph); |
|
|
|
} |
|
|
|
|
|
|
|
public void planCarryTask(PlanTaskSequence seq, String startId, float initDirectionAngle, CarryTask task) { |
|
|
|
public void planCarryTask(PlanTaskSequence plan, String startId, LCCDirection initDirectionAngle, CarryTask task) { |
|
|
|
// 取货点
|
|
|
|
String pickupRackId = task.from().rackId(); |
|
|
|
int pickupBay = task.from().bay(); |
|
|
|
Node pickupNode = findStoreNode(pickupRackId, pickupBay); |
|
|
|
float pickupRotationAngle = getRequiredHeading(pickupNode, pickupBay); |
|
|
|
LCCDirection pickupDirectionAngle = getRequiredHeading(pickupNode, pickupBay); |
|
|
|
|
|
|
|
// 放货点
|
|
|
|
String dropRackId = task.to().rackId(); |
|
|
|
int dropBay = task.to().bay(); |
|
|
|
Node dropNode = findStoreNode(dropRackId, dropBay); |
|
|
|
float dropRotationAngle = getRequiredHeading(dropNode, 0); |
|
|
|
LCCDirection dropDirectionAngle = getRequiredHeading(dropNode, 0); |
|
|
|
|
|
|
|
// 规划到取货点路径
|
|
|
|
List<Node> toPickupPath = astar.findPath(startId, initDirectionAngle, pickupNode.id(), pickupRotationAngle); |
|
|
|
List<Node> toPickupPath = astar.findPath(startId, initDirectionAngle, pickupNode.id(), pickupDirectionAngle); |
|
|
|
|
|
|
|
// 规划到放货点路径
|
|
|
|
List<Node> toDeliverPath = astar.findPath(pickupNode.id(), pickupRotationAngle, dropNode.id(), dropRotationAngle); |
|
|
|
List<Node> toDeliverPath = astar.findPath(pickupNode.id(), pickupDirectionAngle, dropNode.id(), dropDirectionAngle); |
|
|
|
|
|
|
|
// 生成指令序列
|
|
|
|
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(); |
|
|
|
generateMoves(plan, toPickupPath); |
|
|
|
plan.addLoad(task.lpn(), pickupRackId, pickupBay, task.from().level(), task.from().cell()); |
|
|
|
generateMoves(plan, toDeliverPath); |
|
|
|
plan.addUnload(dropRackId, task.to().level(), task.to().bay(), task.to().cell()); |
|
|
|
plan.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(); |
|
|
|
List<Node> nodes = this.graph.getNodesForStore(storeId); |
|
|
|
for (Node node : nodes) { |
|
|
|
for (StoreLink link : node.storeLinks()) { |
|
|
|
if (link.storeId().equals(storeId) && link.bay() == bay) { |
|
|
|
return node; |
|
|
|
} |
|
|
|
} |
|
|
|
} |
|
|
|
throw new RuntimeException("Not found WayPoint link Store, rackId=" + storeId + ", bay=" + bay); |
|
|
|
} |
|
|
|
|
|
|
|
private float getRequiredHeading(Node node, int bay) { |
|
|
|
/** |
|
|
|
* 获取指定节点和货架的所需朝向 |
|
|
|
*/ |
|
|
|
private LCCDirection getRequiredHeading(Node node, int bay) { |
|
|
|
return node.storeLinks().stream() |
|
|
|
.filter(link -> link.bay() == bay) |
|
|
|
.findFirst() |
|
|
|
.map(link -> AStarPathPlanner.getRequiredDirection(link.direction())) |
|
|
|
.orElse(0f); |
|
|
|
.map(link -> link.direction()) |
|
|
|
.orElseThrow(() -> new RuntimeException("Not found storeLink in id=" + node.id() + ", bay=" + bay)); |
|
|
|
} |
|
|
|
|
|
|
|
private void generateMoves(PlanTaskSequence seq, List<Node> path) { |
|
|
|
private void generateMoves(PlanTaskSequence plan, List<Node> path) { |
|
|
|
// 简化的指令生成（实际需处理方向变化）
|
|
|
|
for (int i = 1; i < path.size(); i++) { |
|
|
|
Node node = path.get(i); |
|
|
|
seq.addMoveTo(node.id()); |
|
|
|
plan.addMoveTo(node.id()); |
|
|
|
} |
|
|
|
} |
|
|
|
|
|
|
|
|
|
|
|
} |
|
|
|
|
