zy-acs-manager/src/main/java/com/zy/acs/manager/core/domain/PathDto.java

@@ -1,6 +1,9 @@
package com.zy.acs.manager.core.domain;

import com.zy.acs.framework.common.Cools;
import lombok.Data;

import java.util.List;

/**
 * Created by vincent on 2023/6/19
@@ -8,16 +11,34 @@
@Data
public class PathDto {

    private String codeData;
    private String code;

    private int[] coordinate;
    private Double direction;

    private boolean turn = false;

    public PathDto() {
    }

    public PathDto(String codeData, int[] coordinate) {
        this.codeData = codeData;
        this.coordinate = coordinate;
    public PathDto(String code, Double direction) {
        this.code = code;
        this.direction = direction;
    }

    public PathDto(String code, Double direction, boolean turn) {
        this.code = code;
        this.direction = direction;
        this.turn = turn;
    }

    public static void markTurn(List<PathDto> pathTrace, Double direction) {
        if (Cools.isEmpty(pathTrace) || direction == null) {
            return;
        }
        PathDto current = pathTrace.get(pathTrace.size() - 1);
        current.setDirection(direction);
        current.setTurn(true);
    }


}

 zy-acs-manager/src/main/java/com/zy/acs/manager/core/domain/VehicleRuntime.java

File was deleted

 zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/AvoidWaveCalculator.java

@@ -7,8 +7,8 @@
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.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;
@@ -187,7 +187,8 @@
        // 当前调度车型的最大旋转半径
        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];
@@ -200,11 +201,20 @@
                    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()];
@@ -217,20 +227,12 @@
        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) {
@@ -299,7 +301,7 @@
        if (null == dynamicMatrix) {
            dynamicMatrix = mapDataDispatcher.getDynamicMatrix(null);
        }
        AgvDetail agvDetail = agvDetailService.selectByAgvId(agv.getId());
        AgvDetail agvDetail = agvDetailService.selectMajorByAgvId(agv.getId());
        if (null == agvDetail) {
            return;
        }
@@ -318,7 +320,12 @@
        DynamicNode dynamicNode = dynamicMatrix[codeMatrixIdx[0]][codeMatrixIdx[1]];
        String vehicle = dynamicNode.getVehicle();
        if (vehicle.equals(DynamicNodeType.ACCESS.val)) {
            mapDataDispatcher.modifyDynamicMatrix(null, Utils.singletonList(codeMatrixIdx), agv.getUuid());
            mapDataDispatcher.modifyDynamicMatrix(
                    null,
                    Utils.singletonList(codeMatrixIdx),
                    agv.getUuid(),
                    Utils.singletonList(new PathDto(code.getData(), MapService.mapToNearest(agvDetail.getAgvAngle())))
            );
        }
    }


 zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/MainService.java

@@ -20,10 +20,11 @@
import com.zy.acs.manager.common.exception.BusinessException;
import com.zy.acs.manager.core.domain.AgvBackpackDto;
import com.zy.acs.manager.core.domain.LaneDto;
import com.zy.acs.manager.core.domain.PathDto;
import com.zy.acs.manager.core.domain.TaskPosDto;
import com.zy.acs.manager.core.integrate.conveyor.ConveyorStationService;
import com.zy.acs.manager.core.service.astart.MapDataDispatcher;
import com.zy.acs.manager.core.integrate.dto.OpenBusSubmitParam;
import com.zy.acs.manager.core.service.astart.MapDataDispatcher;
import com.zy.acs.manager.manager.entity.*;
import com.zy.acs.manager.manager.enums.*;
import com.zy.acs.manager.manager.service.*;
@@ -893,9 +894,10 @@
     * 根据分片生成动作 ( 车辆可能已经做过一些任务了,正在等待下一段任务 )
     */
    @Transactional
    public synchronized void generateAction(Long agvId, List<Segment> segmentList, List<String> pathList, Date algoStartTime) {
    public synchronized List<PathDto> generateAction(Long agvId, List<Segment> segmentList, List<String> pathList, Date algoStartTime) {
        List<PathDto> pathTrace = new ArrayList<>();
        try {
            if (Cools.isEmpty(agvId, segmentList)) { return; }
            if (Cools.isEmpty(agvId, segmentList)) { return pathTrace; }
            Date now = new Date();
            long actionPrepareSts = ActionStsType.PREPARE.val();
//            JSONObject storeDirection = configService.getVal("storeDirection", JSONObject.class);
@@ -924,6 +926,7 @@
            if (!lastCode.getData().equals(pathList.get(0))) {
                throw new CoolException("AGV[" + agvNo + "]定位偏移...");
            }
            pathTrace.add(new PathDto(lastCode.getData(), lastDirection, false));

            boolean first = true;
            for (Segment segment : segmentList) {
@@ -989,6 +992,7 @@
                                        now    // 工作时间
                                ));
                                lastDirection = nextLaneDir;
                                PathDto.markTurn(pathTrace, lastDirection);
                            }

                            if (nextDirection.equals(oppNextLaneDir)) {
@@ -1057,6 +1061,7 @@
                                        ));

                                        lastDirection = nextDirection;
                                        PathDto.markTurn(pathTrace, lastDirection);
                                    }
                                }
                            }
@@ -1082,6 +1087,7 @@
                            ));

                            lastDirection = nextDirection;
                            PathDto.markTurn(pathTrace, lastDirection);
                        }

                        // run
@@ -1104,6 +1110,7 @@
                        ));

                        lastCode = nextCode;
                        pathTrace.add(new PathDto(lastCode.getData(), lastDirection, false));

                    }

@@ -1149,6 +1156,7 @@
                        ));

                        lastDirection = firstTurnDir;
                        PathDto.markTurn(pathTrace, lastDirection);

                    }
                    first = false;
@@ -1276,6 +1284,7 @@
                                    now    // 工作时间
                            ));
                            lastDirection = oriStaWorkDirection;
                            PathDto.markTurn(pathTrace, lastDirection);
                        }
                        // 计算货叉工作方向
                        actuatorDirectionType = ActuatorDirectionType.fromVal(oriSta.getActDir());
@@ -1341,6 +1350,7 @@
                                    now    // 工作时间
                            ));
                            lastDirection = destStaWorkDirection;
                            PathDto.markTurn(pathTrace, lastDirection);
                        }
                        // 背篓取货
                        if (backupAction) {
@@ -1401,6 +1411,7 @@
                                    now    // 工作时间
                            ));
                            lastDirection = chargeDirection;
                            PathDto.markTurn(pathTrace, lastDirection);
                        }

                        // charge
@@ -1555,6 +1566,7 @@
            }

            log.info("{}号Agv动作组装完成，指令数量：{}", agvNo, newActionList.size());
            return pathTrace;
        } catch (Exception e) {
            log.error("mainService.generateAction", e);
            TransactionAspectSupport.currentTransactionStatus().setRollbackOnly();
@@ -1610,8 +1622,9 @@
                    task.setUpdateTime(now);
                    if (!taskService.updateById(task)) {
                        throw new BusinessException(task.getSeqNum() + "任务更新失败");
                    }
                }
    }

}
            }

            AgvAction agvAction = new AgvAction(agvNo, actionGroupId);

 zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/MapService.java

@@ -7,6 +7,7 @@
import com.zy.acs.manager.core.constant.MapDataConstant;
import com.zy.acs.manager.core.domain.DirectionDto;
import com.zy.acs.manager.core.domain.LaneDto;
import com.zy.acs.manager.core.domain.PathDto;
import com.zy.acs.manager.core.domain.SortCodeDto;
import com.zy.acs.manager.core.domain.UnlockPathTask;
import com.zy.acs.manager.core.domain.VehicleFootprint;
@@ -205,9 +206,20 @@
        return Math.sqrt(deltaX * deltaX + deltaY * deltaY);
    }

    public void lockPath(Integer lev, List<String> pathList, String agvNo) {
        List<int[]> codeMatrixIdxList = mapDataDispatcher.getCodeMatrixIdxList(lev, pathList);
        mapDataDispatcher.modifyDynamicMatrix(lev, codeMatrixIdxList, agvNo);
    public void lockPath(Integer lev, List<PathDto> pathList, String agvNo) {
        if (Cools.isEmpty(agvNo, pathList)) {
            return;
        }
        List<PathDto> normalized = pathList.stream()
                .filter(Objects::nonNull)
                .filter(dto -> !Cools.isEmpty(dto.getCode()))
                .collect(Collectors.toList());
        if (Cools.isEmpty(normalized)) {
            return;
        }
        List<String> codeList = normalized.stream().map(PathDto::getCode).collect(Collectors.toList());
        List<int[]> codeMatrixIdxList = mapDataDispatcher.getCodeMatrixIdxList(lev, codeList);
        mapDataDispatcher.modifyDynamicMatrix(lev, codeMatrixIdxList, agvNo, normalized);
    }

    public void unlockPath(String agvNo, String codeData) {

 zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/PathQueueConsumer.java

@@ -51,7 +51,7 @@
                            resetCodeIdxList = this.getResetCodeList(lev, tasks.get(0));
                        } else if (tasks.size() > 1) {
//                            log.info("consumer task count:{}", tasks.size());
                            resetCodeIdxList = this.getResetCodeList(lev,tasks);
                            resetCodeIdxList = this.getResetCodeList(lev, tasks);
                        }

                        if (!Cools.isEmpty(resetCodeIdxList)) {

 zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/TrafficService.java

@@ -9,6 +9,7 @@
import com.zy.acs.manager.common.utils.MapDataUtils;
import com.zy.acs.manager.core.constant.MapDataConstant;
import com.zy.acs.manager.core.domain.BlockVehicleDto;
import com.zy.acs.manager.core.domain.PathDto;
import com.zy.acs.manager.core.domain.TaskPosDto;
import com.zy.acs.manager.core.domain.type.BlockSeverityType;
import com.zy.acs.manager.core.service.astart.MapDataDispatcher;
@@ -277,12 +278,12 @@
                }
            }

            mapService.lockPath(null, pathList, agv.getUuid());

//            startTime = System.currentTimeMillis();
            mainService.generateAction(segment.getAgvId(), segmentList, pathList, now);
            List<PathDto> pathDtoList = mainService.generateAction(segment.getAgvId(), segmentList, pathList, now);
//            System.out.println("generateAction: " + (System.currentTimeMillis() - startTime));

            mapService.lockPath(null, pathDtoList, agv.getUuid());

        } catch (Exception e) {
            log.error("TrafficService.trigger", e);
            TransactionAspectSupport.currentTransactionStatus().setRollbackOnly();

 zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/astart/MapDataDispatcher.java

@@ -7,6 +7,7 @@
import com.zy.acs.framework.common.Cools;
import com.zy.acs.framework.exception.CoolException;
import com.zy.acs.manager.common.utils.MapDataUtils;
import com.zy.acs.manager.core.domain.PathDto;
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.Code;
@@ -16,8 +17,6 @@
import com.zy.acs.manager.manager.service.RouteService;
import lombok.extern.slf4j.Slf4j;
import org.springframework.beans.factory.annotation.Autowired;
import org.springframework.context.event.EventListener;
import org.springframework.boot.context.event.ApplicationReadyEvent;
import org.springframework.stereotype.Service;

import javax.annotation.PostConstruct;
@@ -499,11 +498,15 @@
        return codeMatrixIdxList;
    }

    public void modifyDynamicMatrix(Integer lev, List<int[]> codeIdxList, String vehicle) {
        this.modifyDynamicMatrix(lev, codeIdxList, vehicle, false);
    public void modifyDynamicMatrix(Integer lev, List<int[]> codeIdxList, String vehicle, List<PathDto> pathTrace) {
        this.modifyDynamicMatrix(lev, codeIdxList, vehicle, false, pathTrace);
    }

    public synchronized void modifyDynamicMatrix(Integer lev, List<int[]> codeIdxList, String vehicle, boolean reset) {
        this.modifyDynamicMatrix(lev, codeIdxList, vehicle, reset, null);
    }

    private synchronized void modifyDynamicMatrix(Integer lev, List<int[]> codeIdxList, String vehicle, boolean reset, List<PathDto> pathTrace) {
        if (Cools.isEmpty(vehicle)) {
            return;
        }
@@ -511,10 +514,15 @@
        DynamicNode[][] dynamicMatrix = getDynamicMatrix(lev);

        if (!reset) {
//            long time = System.currentTimeMillis() / 1000;
            int serial = 1;
            for (int[] codeMatrixIdx : codeIdxList) {
                dynamicMatrix[codeMatrixIdx[0]][codeMatrixIdx[1]] = new DynamicNode(vehicle, serial);
            for (int i = 0; i < codeIdxList.size(); i++) {
                int[] codeMatrixIdx = codeIdxList.get(i);

                PathDto pathDto = (pathTrace != null && i < pathTrace.size()) ? pathTrace.get(i) : null;
                Double direction = pathDto == null ? null : pathDto.getDirection();
                boolean turn = pathDto != null && pathDto.isTurn();

                dynamicMatrix[codeMatrixIdx[0]][codeMatrixIdx[1]] = new DynamicNode(vehicle, serial, direction, turn);
                serial++;
            }
        } else {
@@ -532,7 +540,7 @@
    }

    public void clearDynamicMatrixByCodeList(Integer lev, List<int[]> codeIdxList) {
        this.modifyDynamicMatrix(lev, codeIdxList, DynamicNodeType.ACCESS.val);
        this.modifyDynamicMatrix(lev, codeIdxList, DynamicNodeType.ACCESS.val, false, null);
    }

    public int[][] filterMapData(int[][] mapMatrix, Integer lev, List<String> lockNodes) {

 zy-acs-manager/src/main/java/com/zy/acs/manager/core/service/astart/domain/DynamicNode.java

@@ -17,6 +17,10 @@

    private int serial = -1;

    private Double direction = null;

    private boolean turn = false;

    public DynamicNode() {}

    public DynamicNode(String vehicle) {
@@ -28,4 +32,11 @@
        this.serial = DynamicNodeType.ACCESS.val.equals(vehicle) ? -1 : serial;
    }

    public DynamicNode(String vehicle, int serial, Double direction, boolean turn) {
        this.vehicle = vehicle;
        this.serial = serial;
        this.direction = direction;
        this.turn = turn;
    }

}