package com.zy.common.utils;
|
|
import com.alibaba.fastjson.JSON;
|
import com.alibaba.fastjson.JSONArray;
|
import com.core.common.SpringUtils;
|
import com.zy.asrs.utils.Utils;
|
import com.zy.common.model.MapNode;
|
import com.zy.common.model.NavigateNode;
|
import com.zy.common.model.enums.NavigationMapType;
|
import com.zy.core.News;
|
import com.zy.core.enums.MapNodeType;
|
import com.zy.core.model.PythonSimilarityResult;
|
import org.springframework.beans.factory.annotation.Value;
|
import org.springframework.stereotype.Component;
|
|
import java.io.BufferedReader;
|
import java.io.InputStreamReader;
|
import java.util.*;
|
|
/**
|
* A*算法使用工具
|
*/
|
@Component
|
public class NavigateUtils {
|
|
@Value("${pythonCalcPath}")
|
private String pythonCalcPath;
|
|
@Value("${pythonCalcSimilarity}")
|
private String pythonCalcSimilarity;
|
|
public List<NavigateNode> calc(String startPoint, String endPoint, Integer mapType, List<int[]> shuttlePoints, List<int[]> whites) {
|
return calcJava(startPoint, endPoint, mapType, shuttlePoints, whites);
|
}
|
|
public List<NavigateNode> calcJava(String startPoint, String endPoint, Integer mapType, List<int[]> shuttlePoints, List<int[]> whites) {
|
//通过开始编号和结束编号获取对应的xy轴坐标
|
int[] startArr = NavigatePositionConvert.positionToXY(startPoint);//开始节点
|
int[] endArr = NavigatePositionConvert.positionToXY(endPoint);//结束节点
|
|
ArrayList<int[]> whiteList = new ArrayList<>();//设置计算节点的白名单
|
whiteList.add(startArr);//将开始节点设置为白名单，以防被过滤
|
if (whites != null && !whites.isEmpty()) {
|
whiteList.addAll(whites);//批量添加白名单节点
|
}
|
|
//获取当前节点计算的层高，并赋值到每一个节点中
|
int lev = Utils.getLev(startPoint);
|
|
//初始化开始节点
|
NavigateNode start = new NavigateNode(startArr[0], startArr[1]);
|
//开始节点无父节点
|
start.setFather(null);
|
|
NavigateNode end = new NavigateNode(endArr[0], endArr[1]);
|
NavigateSolution solution = new NavigateSolution(mapType, lev, whiteList, shuttlePoints);
|
//开始节点，不纳入禁用节点内计算
|
|
NavigateNode res_node = solution.astarSearchJava(start, end);
|
if (res_node == null) {
|
News.error("{} dash {} can't find navigate path!", startPoint, endPoint);
|
return null;
|
}
|
|
ArrayList<NavigateNode> list = new ArrayList<>();
|
//渲染
|
NavigateNode fatherNode = null;//当前循环上一节点，用于拐点计算
|
while (res_node != null) {
|
res_node.setDirection(null);
|
res_node.setIsInflectionPoint(false);
|
res_node.setZ(lev);//设置层高
|
|
//寻找拐点
|
HashMap<String, Object> result = searchInflectionPoint(res_node, fatherNode, res_node.getFather());//分别传入当前节点、父节点、下一节点
|
//判断当前节点是否为拐点
|
if (Boolean.parseBoolean(result.get("result").toString())) {
|
//当前为拐点
|
res_node.setIsInflectionPoint(true);
|
//拐点方向
|
res_node.setDirection(result.get("direction").toString());
|
}
|
list.add(res_node);
|
|
fatherNode = res_node;//把当前节点保存成一个父节点
|
res_node = res_node.getFather();//迭代操作
|
}
|
|
Collections.reverse(list);
|
|
//将每个节点里面的fatherNode至为null(方便后续计算时父节点过多导致显示的节点太多)
|
for (NavigateNode navigateNode : list) {
|
//父节点设置为null，不影响计算结果，不影响后续操作。
|
//此操作仅为后续排查处理提供视觉方便。
|
navigateNode.setFather(null);
|
}
|
|
//起始节点计算方向
|
String direction = calcDirection(list.get(0), list.get(1));
|
NavigateNode startNode = list.get(0);
|
startNode.setDirection(direction);
|
//更新节点列表
|
list.set(0, startNode);
|
return list;
|
}
|
|
public List<NavigateNode> calcPython(String startPoint, String endPoint, Integer mapType, List<int[]> shuttlePoints, List<int[]> whites) {
|
//通过开始编号和结束编号获取对应的xy轴坐标
|
int[] startArr = NavigatePositionConvert.positionToXY(startPoint);//开始节点
|
int[] endArr = NavigatePositionConvert.positionToXY(endPoint);//结束节点
|
|
ArrayList<int[]> whiteList = new ArrayList<>();//设置计算节点的白名单
|
whiteList.add(startArr);//将开始节点设置为白名单，以防被过滤
|
if (whites != null && !whites.isEmpty()) {
|
whiteList.addAll(whites);//批量添加白名单节点
|
}
|
|
//获取当前节点计算的层高，并赋值到每一个节点中
|
int lev = Utils.getLev(startPoint);
|
|
//初始化开始节点
|
NavigateNode start = new NavigateNode(startArr[0], startArr[1]);
|
//开始节点无父节点
|
start.setFather(null);
|
|
NavigateNode end = new NavigateNode(endArr[0], endArr[1]);
|
NavigateSolution solution = new NavigateSolution(mapType, lev, whiteList, shuttlePoints);
|
//开始节点，不纳入禁用节点内计算
|
|
String pathStr = solution.astarSearchPython(start, end, pythonCalcPath);
|
if (pathStr == null) {
|
News.error("{} dash {} can't find navigate path!", startPoint, endPoint);
|
return null;
|
}
|
|
List<NavigateNode> list = new ArrayList<>();
|
|
NavigateNode fatherNode = null;//当前循环上一节点，用于拐点计算
|
JSONArray rows = JSON.parseArray(pathStr);
|
|
for (int i = 0; i < rows.size(); i++) {
|
Object currentObj = rows.get(i);
|
|
NavigateNode nextNode = null;
|
if ((i + 1) < rows.size()) {
|
Object object = rows.get(i + 1);
|
nextNode = pythonObjTransferNode(object, lev);
|
}
|
|
NavigateNode node = pythonObjTransferNode(currentObj, lev);
|
|
//寻找拐点
|
HashMap<String, Object> result = searchInflectionPoint(node, fatherNode, nextNode);//分别传入当前节点、父节点、下一节点
|
//判断当前节点是否为拐点
|
if (Boolean.parseBoolean(result.get("result").toString())) {
|
//当前为拐点
|
node.setIsInflectionPoint(true);
|
//拐点方向
|
node.setDirection(result.get("direction").toString());
|
}
|
|
list.add(node);
|
|
fatherNode = node;//把当前节点保存成一个父节点
|
}
|
|
//将每个节点里面的fatherNode至为null(方便后续计算时父节点过多导致显示的节点太多)
|
for (NavigateNode navigateNode : list) {
|
//父节点设置为null，不影响计算结果，不影响后续操作。
|
//此操作仅为后续排查处理提供视觉方便。
|
navigateNode.setFather(null);
|
}
|
|
//起始节点计算方向
|
String direction = calcDirection(list.get(0), list.get(1));
|
NavigateNode startNode = list.get(0);
|
startNode.setDirection(direction);
|
//更新节点列表
|
list.set(0, startNode);
|
return list;
|
}
|
|
//判断当前节点到下一个节点是否为拐点
|
public HashMap<String,Object> searchInflectionPoint(NavigateNode currentNode, NavigateNode fatherNode, NavigateNode nextNode) {
|
HashMap<String, Object> map = new HashMap<>();
|
map.put("result", false);//是否为拐点，true：拐点，false：直线
|
// 第一个点或直线点
|
if (fatherNode == null || nextNode == null || nextNode.getX() == fatherNode.getX() || nextNode.getY() == fatherNode.getY()) {
|
return map;//不是拐点直接返回
|
}
|
|
//拐点方向
|
String direction = calcDirection(currentNode, fatherNode);
|
|
map.put("result", true);//拐点
|
map.put("direction", direction);//拐点方向（从当前节点视角看的方向）
|
return map;
|
}
|
|
/**
|
* 计算方向
|
*/
|
public String calcDirection(NavigateNode currentNode, NavigateNode fatherNode) {
|
//拐点方向
|
String direction = "";
|
// 普通拐点
|
//计算拐点方向
|
if (fatherNode.getX() != currentNode.getX()) {
|
//x轴数据有差异，判断x轴方向
|
//当前节点X - 父节点X
|
if (currentNode.getX() - fatherNode.getX() > 0) {
|
//大于0，方向top
|
direction = "top";
|
}else {
|
//小于0，方向bottom
|
direction = "bottom";
|
}
|
}
|
|
if (fatherNode.getY() != currentNode.getY()) {
|
//y轴数据有差异，判断y轴方向
|
//当前节点Y - 父节点Y
|
if (currentNode.getY() - fatherNode.getY() > 0) {
|
//大于0，方向left
|
direction = "left";
|
}else {
|
//小于0，方向right
|
direction = "right";
|
}
|
}
|
|
return direction;
|
}
|
|
/**
|
* 加转弯节点
|
* 获取分段路径，每当有一个拐点则进行一次分段，最终返回总分段数据
|
*/
|
public ArrayList<ArrayList<NavigateNode>> getSectionPath(List<NavigateNode> mapList) {
|
ArrayList<ArrayList<NavigateNode>> list = new ArrayList<>();
|
|
ArrayList<NavigateNode> data = new ArrayList<>();
|
String direction = mapList.get(0).getDirection();//行走方向
|
|
for (NavigateNode navigateNode : mapList) {
|
data.add(navigateNode);
|
//拐点
|
if (navigateNode.getIsInflectionPoint()) {
|
//分割数据
|
list.add(data);//添加某一段数据
|
direction = navigateNode.getDirection();//更新行走方向
|
data = new ArrayList<>();
|
data.add(navigateNode);//将拐点的终点，更新成下一段命令的起点坐标
|
} else {
|
//直行线路
|
navigateNode.setDirection(direction);//设置行走方向
|
}
|
Integer distance = getXToNextDistance(navigateNode);//获取当前点到下一点的行走距离
|
navigateNode.setMoveDistance(distance);
|
}
|
|
//将最后一段数据添加进入
|
list.add(data);
|
|
return list;
|
}
|
|
//获取从x点到下一点的行走距离
|
public Integer getXToNextDistance(NavigateNode xNode) {
|
NavigateMapData mapData = SpringUtils.getBean(NavigateMapData.class);
|
List<List<MapNode>> lists = mapData.getJsonData(xNode.getZ(), NavigationMapType.NONE.id, null, null);
|
if (lists != null) {
|
MapNode mapNode = lists.get(xNode.getX()).get(xNode.getY());
|
if (mapNode != null) {
|
switch (xNode.getDirection()) {
|
case "top":
|
return mapNode.getTop();
|
case "bottom":
|
return mapNode.getBottom();
|
case "left":
|
return mapNode.getLeft();
|
case "right":
|
return mapNode.getRight();
|
}
|
}
|
return 0;
|
}
|
return 0;
|
}
|
|
/**
|
* 根据原始节点结果，计算总行走距离
|
*/
|
public Integer getOriginPathAllDistance(List<NavigateNode> path) {
|
ArrayList<ArrayList<NavigateNode>> sectionPath = getSectionPath(path);
|
Integer allDistance = 0;
|
for (ArrayList<NavigateNode> navigateNodes : sectionPath) {
|
Integer distance = getCurrentPathAllDistance(navigateNodes);
|
allDistance += distance;
|
}
|
return allDistance;
|
}
|
|
/**
|
* 获取当前路径总行走距离
|
*/
|
public Integer getCurrentPathAllDistance(List<NavigateNode> path) {
|
if (path.size() == 1) {
|
//路径只有一条数据，则直接返回行走距离
|
return path.get(0).getMoveDistance();
|
}
|
|
//总距离
|
int allDistance = 0;
|
for (int i = 0; i < path.size() - 1; i++) {//路径中最后一个节点不统计到总距离，最后一个节点并不会再行走
|
allDistance += path.get(i).getMoveDistance();
|
}
|
return allDistance;
|
}
|
|
/**
|
* 获取中间点到目标点行走距离
|
*/
|
public Integer getMiddleToDistDistance(List<NavigateNode> path, NavigateNode middlePath) {
|
//最后一条节点不计算进行走距离
|
NavigateNode lastPath = path.get(path.size() - 1);
|
//总距离
|
int allDistance = 0;
|
boolean flag = false;
|
for (NavigateNode navigateNode : path) {
|
if (!flag && navigateNode.equals(middlePath)) {
|
flag = true;
|
}
|
|
if (navigateNode.equals(lastPath)) {
|
continue;//最后一条节点不计算进行走距离
|
}
|
|
if (flag) {
|
allDistance += navigateNode.getMoveDistance();
|
}
|
}
|
return allDistance;
|
}
|
|
/**
|
* 检测路径是否可用(可走)
|
*/
|
public boolean checkPathIsAvailable(List<NavigateNode> path, Integer shuttleNo, Integer lev) {
|
NavigateSolution solution = new NavigateSolution(NavigationMapType.DFX.id, lev, null, Utils.getShuttlePoints(shuttleNo, lev));//获取无白名单地图(该地图包含小车坐标)
|
int[][] map = solution.map;
|
for (NavigateNode node : path) {
|
int value = map[node.getX()][node.getY()];
|
if (value != MapNodeType.NORMAL_PATH.id && value != MapNodeType.MAIN_PATH.id && value != MapNodeType.CHARGE.id && value != MapNodeType.CONVEYOR_CAR_GO.id) {//母轨道3、子轨道0、充电桩5、小车可走输送站
|
return false;
|
}
|
}
|
return true;
|
}
|
|
private NavigateNode pythonObjTransferNode(Object obj, Integer z) {
|
List<Integer> pathData = JSON.parseArray(obj.toString(), Integer.class);
|
Integer x = pathData.get(0);
|
Integer y = pathData.get(1);
|
|
NavigateNode node = new NavigateNode(x, y);
|
node.setDirection(null);
|
node.setIsInflectionPoint(false);
|
node.setZ(z);
|
return node;
|
}
|
|
public Double similarityPath(List<NavigateNode> firstPath, List<NavigateNode> secondPath) {
|
try {
|
List<int[]> first = new ArrayList<>();
|
for (NavigateNode node : firstPath) {
|
first.add(new int[]{node.getX(), node.getY()});
|
}
|
|
List<int[]> second = new ArrayList<>();
|
for (NavigateNode node : secondPath) {
|
second.add(new int[]{node.getX(), node.getY()});
|
}
|
|
ProcessBuilder processBuilder = new ProcessBuilder("python", pythonCalcSimilarity, JSON.toJSONString(first), JSON.toJSONString(second));
|
processBuilder.redirectErrorStream(true);
|
|
Process process = processBuilder.start();
|
|
// 读取Python脚本的输出
|
BufferedReader reader = new BufferedReader(new InputStreamReader(process.getInputStream()));
|
String line;
|
StringBuilder builder = new StringBuilder();
|
while ((line = reader.readLine()) != null) {
|
builder.append(line);
|
}
|
|
// 等待Python脚本执行完成
|
int exitCode = process.waitFor();
|
if (exitCode != 0) {
|
System.out.println("Python script exited with error code: " + exitCode);
|
return null;
|
}
|
reader.close();
|
|
if (builder.length() <= 0) {
|
return null;
|
}
|
|
PythonSimilarityResult result = JSON.parseObject(builder.toString(), PythonSimilarityResult.class);
|
if (result.getCalcResult() != 200) {
|
return 0D;
|
}
|
|
Double similarity = result.getSimilarity();
|
return similarity;
|
} catch (Exception e) {
|
e.printStackTrace();
|
}
|
return 0D;
|
}
|
|
public static void main(String[] args) {
|
// //计算路径
|
// List<NavigateNode> calc = calc("1000901", "1800201", NavigationMapType.NONE.id, null);
|
// System.out.println(calc);
|
// System.out.println("------------------------");
|
//// List<NavigateNode> calc = calc("0501401", "0201801", "out");
|
// //将路径分割成多段
|
// ArrayList<ArrayList<NavigateNode>> data = getSectionPath(calc);
|
// for (ArrayList<NavigateNode> list : data) {
|
// Integer currentPathAllDistance = getCurrentPathAllDistance(list);//计算当前路径总距离
|
// System.out.println(currentPathAllDistance);
|
// System.out.println(list);
|
// }
|
|
System.loadLibrary("example");
|
|
}
|
|
}
|
