| | |
| | | NavigateNode startNode = new NavigateNode(startMapIdx[0], startMapIdx[1], startCode.getData()); |
| | | NavigateNode endNode = new NavigateNode(endMapIdx[0], endMapIdx[1], endCode.getData()); |
| | | |
| | | long startTime = System.currentTimeMillis(); |
| | | NavigateNode finishNode = aStarNavigateService.execute(agvNo, startNode, endNode, lock, blackList, segment); |
| | | // System.out.println("AStart spend time: " + (System.currentTimeMillis() - startTime)); |
| | | |
| | | if (null == finishNode) { |
| | | return new ArrayList<>(); |
| | | } |
| | | |
| | | ArrayList<NavigateNode> navigateNodes = new ArrayList<>(); |
| | | |
| | | // 渲染 |
| | | NavigateNode parentNode = null; // 当前循环上一节点,用于拐点计算 |
| | | while (finishNode != null) { |
| | | navigateNodes.add(finishNode); |
| | | |
| | | parentNode = finishNode; |
| | | finishNode = finishNode.getParent(); |
| | | } |
| | | |
| | | Collections.reverse(navigateNodes); |
| | | |
| | | // for (NavigateNode navigateNode : navigateNodes) { |
| | | // navigateNode.setParent(null); |
| | | // } |
| | | |
| | | List<String> navigatePath = navigateNodes.stream().map(NavigateNode::getCodeData).collect(Collectors.toList()); |
| | | |