zy-acs-manager/src/main/java/com/zy/acs/manager/common/utils/MapDataUtils.java
@@ -2,6 +2,9 @@ import com.alibaba.fastjson.JSON; import com.zy.acs.framework.common.Cools; import com.zy.acs.manager.core.constant.MapDataConstant; import com.zy.acs.manager.core.domain.VehicleFootprint; import com.zy.acs.manager.manager.entity.AgvModel; import java.util.*; @@ -56,12 +59,41 @@ return JSON.toJSONString(set); } public static Double getVehicleWaveSafeDistance(Integer val, Double factor) { if (Cools.isEmpty(val)) { return 0.0D; public static Double getVehicleWaveSafeDistance(Number mm) { return getVehicleWaveSafeDistance(mm, null); } public static Double getVehicleWaveSafeDistance(Number mm, Double factor) { if (mm == null) { throw new IllegalArgumentException("Invalid map length: " + mm); } factor = Optional.ofNullable(factor).orElse(1.0D); double val = mm.doubleValue(); if (val <= 0) { throw new IllegalArgumentException("Invalid map length: " + mm); } factor = Optional.ofNullable(factor).orElse(MapDataConstant.MAX_DISTANCE_BETWEEN_ADJACENT_AGV_FACTOR); return val * factor; } public static VehicleFootprint buildFootprint(AgvModel agvModel) { if (null == agvModel) { throw new IllegalArgumentException("AgvModel is null"); } if (agvModel.getHeadOffset() == null || agvModel.getHeadOffset() <= 0) { throw new IllegalArgumentException("Invalid head offset: " + agvModel.getHeadOffset()); } if (agvModel.getTailOffset() == null || agvModel.getTailOffset() <= 0) { throw new IllegalArgumentException("Invalid tail offset: " + agvModel.getTailOffset()); } if (agvModel.getWidth() == null || agvModel.getWidth() <= 0) { throw new IllegalArgumentException("Invalid width: " + agvModel.getWidth()); } double head = agvModel.getHeadOffset(); double tail = agvModel.getTailOffset(); double halfWidth = (double) agvModel.getWidth() / 2; return new VehicleFootprint(head, tail, halfWidth); } } zy-acs-manager/src/main/java/com/zy/acs/manager/core/domain/VehicleFootprint.java
@@ -1,5 +1,6 @@ package com.zy.acs.manager.core.domain; import com.zy.acs.manager.common.utils.MapDataUtils; import lombok.Data; /** @@ -22,6 +23,8 @@ // double frontDiag = Math.hypot(head, halfWidth); // double rearDiag = Math.hypot(tail, halfWidth); // return Math.max(frontDiag, rearDiag); return Math.hypot(Math.max(head, tail), halfWidth); double hypot = Math.hypot(Math.max(head, tail), halfWidth); return MapDataUtils.getVehicleWaveSafeDistance(hypot); } } zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/AvoidWaveCalculator.java
@@ -2,11 +2,9 @@ import com.baomidou.mybatisplus.core.conditions.query.LambdaQueryWrapper; import com.zy.acs.common.utils.Utils; import com.zy.acs.framework.common.Cools; import com.zy.acs.framework.exception.CoolException; import com.zy.acs.manager.common.config.RedisProperties; import com.zy.acs.manager.common.utils.MapDataUtils; import com.zy.acs.manager.core.constant.MapDataConstant; import com.zy.acs.manager.core.domain.PathDto; import com.zy.acs.manager.core.domain.VehicleFootprint; import com.zy.acs.manager.core.service.astart.CodeNodeType; @@ -31,10 +29,8 @@ import javax.annotation.PostConstruct; import javax.annotation.PreDestroy; import java.io.BufferedReader; import java.io.File; import java.io.InputStream; import java.io.InputStreamReader; import java.nio.file.Files; import java.nio.file.StandardCopyOption; import java.util.HashMap; @@ -120,72 +116,72 @@ } } private boolean calcWaveScopeByPython(Integer lev, AgvModel agvModel) throws Exception { if (null == agvModel.getDiameter() || agvModel.getDiameter() <= 0) { log.warn("There is no diameter or diameter value was wrong..."); } // python Double avoidDistance = MapDataUtils.getVehicleWaveSafeDistance(agvModel.getDiameter(), MapDataConstant.MAX_DISTANCE_BETWEEN_ADJACENT_AGV_FACTOR); if (null == pythonFile) { pythonFile = loadPythonFile(); } ProcessBuilder processBuilder = new ProcessBuilder( "python" // 或者 "python3" 取决于系统配置 , pythonFile.getAbsolutePath() , String.valueOf(avoidDistance) , redisProperties.getHost() , redisProperties.getPassword() , String.valueOf(redisProperties.getPort()) , String.valueOf(redisProperties.getIndex()) ); processBuilder.redirectErrorStream(true); Process process = processBuilder.start(); BufferedReader reader = new BufferedReader(new InputStreamReader(process.getInputStream())); String line; StringBuilder builder = new StringBuilder(); while ((line = reader.readLine()) != null) { builder.append(line); } int exitCode = process.waitFor(); if (exitCode != 0) { log.error("Python script exited with error code: {}", exitCode); log.error("python error:{}", builder.toString()); return false; } reader.close(); if (builder.length() <= 0) { return false; } String result = builder.toString(); if (Cools.isEmpty(result)) { return false; } if (!"1".equals(result)) { log.error("Failed to call python"); return false; } return true; } // private boolean calcWaveScopeByPython(Integer lev, AgvModel agvModel) throws Exception { // if (null == agvModel.getDiameter() || agvModel.getDiameter() <= 0) { // log.warn("There is no diameter or diameter value was wrong..."); // } // // // python // Double avoidDistance = MapDataUtils.getVehicleWaveSafeDistance(agvModel.getDiameter(), MapDataConstant.MAX_DISTANCE_BETWEEN_ADJACENT_AGV_FACTOR); // // if (null == pythonFile) { // pythonFile = loadPythonFile(); // } // // ProcessBuilder processBuilder = new ProcessBuilder( // "python" // 或者 "python3" 取决于系统配置 // , pythonFile.getAbsolutePath() // , String.valueOf(avoidDistance) // , redisProperties.getHost() // , redisProperties.getPassword() // , String.valueOf(redisProperties.getPort()) // , String.valueOf(redisProperties.getIndex()) // ); // // processBuilder.redirectErrorStream(true); // // Process process = processBuilder.start(); // // BufferedReader reader = new BufferedReader(new InputStreamReader(process.getInputStream())); // String line; // StringBuilder builder = new StringBuilder(); // while ((line = reader.readLine()) != null) { // builder.append(line); // } // // int exitCode = process.waitFor(); // if (exitCode != 0) { // log.error("Python script exited with error code: {}", exitCode); // log.error("python error:{}", builder.toString()); // return false; // } // reader.close(); // // if (builder.length() <= 0) { // return false; // } // // String result = builder.toString(); // // if (Cools.isEmpty(result)) { // return false; // } // if (!"1".equals(result)) { // log.error("Failed to call python"); // return false; // } // // return true; // } private boolean calcWaveScopeByJava(Integer lev, AgvModel agvModel) { assert null != agvModel.getDiameter(); // assert null != agvModel.getDiameter(); String[][] codeMatrix = mapDataDispatcher.getCodeMatrix(lev); String[][] waveMatrix = mapDataDispatcher.initWaveMatrix(lev); DynamicNode[][] dynamicMatrix = mapDataDispatcher.getDynamicMatrix(lev); // 当前调度车型的最大旋转半径 double bufferRadius = buildFootprint(agvModel).maxExtent(); double bufferRadius = MapDataUtils.buildFootprint(agvModel).maxExtent(); Map<String, VehicleFootprint> footprintCache = new HashMap<>(); @@ -203,18 +199,21 @@ VehicleFootprint footprint = footprintCache.computeIfAbsent(vehicle, this::loadFootprint); List<NavigateNode> includeList; Double nodeDirection = dynamicNode.getDirection(); if (dynamicNode.isTurn()) { double radius = footprint.maxExtent() + bufferRadius; includeList = mapService.getWaveScopeByCode(lev, codeData, radius); } else if (nodeDirection != null) { double headingRad = Math.toRadians(90 - nodeDirection); includeList = mapService.getWaveScopeByCode(lev, codeData, footprint, headingRad, bufferRadius); } else { double radius = footprint.maxExtent() + bufferRadius; includeList = mapService.getWaveScopeByCode(lev, codeData, radius); } double radius = footprint.maxExtent() + bufferRadius; List<NavigateNode> includeList = mapService.getWaveScopeByCode(lev, codeData, radius); // List<NavigateNode> includeList; // Double nodeDirection = dynamicNode.getDirection(); // if (dynamicNode.isTurn()) { // double radius = footprint.maxExtent() + bufferRadius; // includeList = mapService.getWaveScopeByCode(lev, codeData, radius); // } else if (nodeDirection != null) { // double headingRad = Math.toRadians(90 - nodeDirection); // includeList = mapService.getWaveScopeByCode(lev, codeData, footprint, headingRad, bufferRadius); // } else { // double radius = footprint.maxExtent() + bufferRadius; // includeList = mapService.getWaveScopeByCode(lev, codeData, radius); // } for (NavigateNode navigateNode : includeList) { String waveNode = waveMatrix[navigateNode.getX()][navigateNode.getY()]; @@ -232,42 +231,7 @@ if (model == null) { throw new CoolException(agvNo + " does not have an model."); } return this.buildFootprint(model); } private double toMapLength(Integer mm) { if (mm == null || mm <= 0) { throw new IllegalArgumentException("Invalid map length: " + mm); } return MapDataUtils.getVehicleWaveSafeDistance(mm, MapDataConstant.MAX_DISTANCE_BETWEEN_ADJACENT_AGV_FACTOR); } private double resolveAvoidDistance(AgvModel agvModel) { if (agvModel == null || agvModel.getDiameter() == null || agvModel.getDiameter() <= 0) { return 0D; } return MapDataUtils.getVehicleWaveSafeDistance(agvModel.getDiameter(), MapDataConstant.MAX_DISTANCE_BETWEEN_ADJACENT_AGV_FACTOR); } private VehicleFootprint buildFootprint(AgvModel agvModel) { if (null == agvModel) { throw new IllegalArgumentException("AgvModel is null"); } if (agvModel.getHeadOffset() == null || agvModel.getHeadOffset() <= 0) { throw new IllegalArgumentException("Invalid head offset: " + agvModel.getHeadOffset()); } if (agvModel.getTailOffset() == null || agvModel.getTailOffset() <= 0) { throw new IllegalArgumentException("Invalid tail offset: " + agvModel.getTailOffset()); } if (agvModel.getWidth() == null || agvModel.getWidth() <= 0) { throw new IllegalArgumentException("Invalid width: " + agvModel.getWidth()); } double head = toMapLength(agvModel.getHeadOffset()); double tail = toMapLength(agvModel.getTailOffset()); double halfWidth = toMapLength(agvModel.getWidth()) / 2; return new VehicleFootprint(head, tail, halfWidth); return MapDataUtils.buildFootprint(model); } private File loadPythonFile() { zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/RetreatNavigateService.java
@@ -5,10 +5,10 @@ import com.zy.acs.common.utils.Utils; import com.zy.acs.framework.common.Cools; import com.zy.acs.manager.common.utils.MapDataUtils; import com.zy.acs.manager.core.constant.MapDataConstant; import com.zy.acs.manager.core.service.astart.*; import com.zy.acs.manager.core.service.astart.domain.DynamicNode; import com.zy.acs.manager.core.utils.RouteGenerator; import com.zy.acs.manager.manager.entity.AgvModel; import com.zy.acs.manager.manager.entity.Code; import com.zy.acs.manager.manager.entity.Jam; import com.zy.acs.manager.manager.entity.Segment; @@ -56,6 +56,8 @@ private AgvAreaDispatcher agvAreaDispatcher; /** * agvNo: 阻塞AGV，挡道者 * sponsor： 被阻塞AGV，寻址发起者 * avoidPathList ===>> [ minor vehicle ] [wave] [ curr vehicle ] [ code2 ] [ code3 ] ...... **/ // @SuppressWarnings("all") @@ -80,8 +82,9 @@ String breakPoint = avoidPathList.stream().findFirst().orElse(null); List<String> blackList = Utils.singletonList(sponsor); Double avoidDistance = MapDataUtils.getVehicleWaveSafeDistance(agvModelService.getByAgvNo(sponsor).getDiameter() , MapDataConstant.MAX_DISTANCE_BETWEEN_ADJACENT_AGV_FACTOR); AgvModel agvModel = agvModelService.getByAgvNo(agvNo); AgvModel sponsorModel = agvModelService.getByAgvNo(sponsor); double avoidDistance = MapDataUtils.buildFootprint(agvModel).maxExtent() + MapDataUtils.buildFootprint(sponsorModel).maxExtent(); List<String> avoidPathListWave = mapService.getWaveScopeByCodeList(lev, avoidPathList, avoidDistance); String[][] codeMatrix = mapDataDispatcher.getCodeMatrix(lev); zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/TrafficService.java
@@ -300,7 +300,7 @@ BlockSeverityType blockSeverity = BlockSeverityType.query(null == jam ? null : jam.getDuration()); // judge avoid of jam 如果已经在避让点（因为当前车执行了避让任务），那么则不能再去检索之前的阻塞路径 List<Jam> unfinishedOriginJamByCurrAgv = jamService.getUnfinishedOriginJamByAvo(agv.getId(), startCode.getId(), segment.getId()); List<String> blackPath = this.getBlackPathList(unfinishedOriginJamByCurrAgv); List<String> blackPath = this.getBlackPathList(agvNo, unfinishedOriginJamByCurrAgv); // 结果集 List<String> pathList = new ArrayList<>(); @@ -652,43 +652,47 @@ } } private List<String> getBlackPathList(List<Jam> unfinishedOriginJamByCurrAgv) { private List<String> getBlackPathList(String agvNo, List<Jam> unfinishedOriginJamByCurrAgv) { List<String> blackPathList = new ArrayList<>(); Integer lev = MapDataDispatcher.MAP_DEFAULT_LEV; if (!Cools.isEmpty(unfinishedOriginJamByCurrAgv)) { for (Jam jam : unfinishedOriginJamByCurrAgv) { if (!Cools.isEmpty(jam.getJamPath())) { if (Cools.isEmpty(unfinishedOriginJamByCurrAgv)) { return blackPathList; } List<String> list = GsonUtils.fromJsonToList(jam.getJamPath(), String.class); AgvModel agvModel = agvModelService.getByAgvNo(agvNo); double bufferRadius = MapDataUtils.buildFootprint(agvModel).maxExtent(); Agv jamAgv = agvService.getById(jam.getJamAgv()); List<String> jamDynamicNodes = mapService.queryCodeListFromDynamicNode(lev, jamAgv.getUuid()); // jamDynamicNodes has sorted String firstCodeNode = jamDynamicNodes.stream().findFirst().orElse(null); for (Jam jam : unfinishedOriginJamByCurrAgv) { if (!Cools.isEmpty(jam.getJamPath())) { if (!Cools.isEmpty(firstCodeNode)) { int idx = list.indexOf(firstCodeNode); if (idx != -1) { list = new ArrayList<>(list.subList(idx, list.size())); List<String> list = GsonUtils.fromJsonToList(jam.getJamPath(), String.class); // the wave of first node Double avoidDistance = MapDataUtils.getVehicleWaveSafeDistance( agvModelService.getByAgvId(jamAgv.getId()).getDiameter(), MapDataConstant.MAX_DISTANCE_BETWEEN_ADJACENT_AGV_FACTOR ); List<String> waveCodeList = mapService.getWaveScopeByCode(lev, firstCodeNode, avoidDistance) .stream().map(NavigateNode::getCodeData).distinct().collect(Collectors.toList()); list.addAll(waveCodeList); } else { // 如果被阻塞车辆已经不在原来的阻塞路径中，考虑避让车走行时不需要把之前的阻塞路径加入黑名单 list = new ArrayList<>(); } String jamAgvNo = agvService.getAgvNo(jam.getJamAgv()); List<String> jamDynamicNodes = mapService.queryCodeListFromDynamicNode(lev, jamAgvNo); // jamDynamicNodes has sorted String firstCodeNode = jamDynamicNodes.stream().findFirst().orElse(null); if (!Cools.isEmpty(firstCodeNode)) { int idx = list.indexOf(firstCodeNode); if (idx != -1) { list = new ArrayList<>(list.subList(idx, list.size())); // the wave of first node AgvModel jamAgvModel = agvModelService.getByAgvNo(jamAgvNo); Double avoidDistance = MapDataUtils.buildFootprint(jamAgvModel).maxExtent() + bufferRadius; List<String> waveCodeList = mapService.getWaveScopeByCode(lev, firstCodeNode, avoidDistance) .stream().map(NavigateNode::getCodeData).distinct().collect(Collectors.toList()); list.addAll(waveCodeList); } else { // 如果被阻塞车辆已经不在原来的阻塞路径中，考虑避让车走行时不需要把之前的阻塞路径加入黑名单 list = new ArrayList<>(); } blackPathList.addAll(list); } blackPathList.addAll(list); } } return blackPathList.stream().distinct().collect(Collectors.toList()); }
