| | |
| | | import com.zy.acs.manager.common.utils.MapDataUtils; |
| | | import com.zy.acs.manager.core.constant.MapDataConstant; |
| | | import com.zy.acs.manager.core.domain.VehicleFootprint; |
| | | import com.zy.acs.manager.core.domain.VehicleRuntime; |
| | | import com.zy.acs.manager.core.service.astart.CodeNodeType; |
| | | import com.zy.acs.manager.core.service.astart.DynamicNodeType; |
| | | import com.zy.acs.manager.core.service.astart.MapDataDispatcher; |
| | |
| | | // 当前调度车型的最大旋转半径 |
| | | double bufferRadius = buildFootprint(agvModel).maxExtent(); |
| | | |
| | | Map<String, VehicleRuntime> runtimeCache = new HashMap<>(); |
| | | Map<String, VehicleFootprint> footprintCache = new HashMap<>(); |
| | | |
| | | for (int i = 0; i < dynamicMatrix.length; i++) { |
| | | for (int j = 0; j < dynamicMatrix[i].length; j++) { |
| | | DynamicNode dynamicNode = dynamicMatrix[i][j]; |
| | |
| | | continue; |
| | | } |
| | | |
| | | // cache |
| | | VehicleRuntime runtime = runtimeCache.computeIfAbsent(vehicle, this::loadRuntime); |
| | | assert null != runtime; assert null != runtime.getFootprint(); |
| | | VehicleFootprint footprint = footprintCache.computeIfAbsent(vehicle, this::loadFootprint); |
| | | |
| | | List<NavigateNode> includeList = mapService.getWaveScopeByCode(lev, codeData, runtime.getFootprint(), runtime.getHeadingRad(), bufferRadius); |
| | | 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()]; |
| | |
| | | return true; |
| | | } |
| | | |
| | | private VehicleRuntime loadRuntime(String agvNo) { |
| | | private VehicleFootprint loadFootprint(String agvNo) { |
| | | AgvModel model = agvModelService.getByAgvNo(agvNo); |
| | | if (model == null) { |
| | | return null; |
| | | throw new CoolException(agvNo + " does not have an model."); |
| | | } |
| | | VehicleFootprint footprint = this.buildFootprint(model); |
| | | |
| | | AgvDetail detail = agvDetailService.selectByAgvNo(agvNo); |
| | | if (detail == null || null == detail.getAgvAngle()) { |
| | | throw new CoolException(agvNo + " does not have an agv angle"); |
| | | } |
| | | double headingDeg = detail.getAgvAngle(); |
| | | |
| | | return new VehicleRuntime(footprint, Math.toRadians(90 - headingDeg)); |
| | | return this.buildFootprint(model); |
| | | } |
| | | |
| | | private double toMapLength(Integer mm) { |