#

vincentlu

4 小时以前 590a64af2cdd33427ed8eda2eb983b07dd60ab8b

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

@@ -1,16 +1,14 @@
package com.zy.acs.manager.core.service;

import com.baomidou.mybatisplus.core.conditions.query.LambdaQueryWrapper;
import com.zy.acs.common.constant.RedisConstant;
import com.zy.acs.common.utils.GsonUtils;
import com.zy.acs.common.utils.RedisSupport;
import com.zy.acs.framework.common.Cools;
import com.zy.acs.framework.common.SnowflakeIdWorker;
import com.zy.acs.manager.core.domain.Lane;
import com.zy.acs.manager.core.domain.LaneDto;
import com.zy.acs.manager.core.service.astart.MapDataDispatcher;
import com.zy.acs.manager.manager.entity.Code;
import com.zy.acs.manager.manager.entity.Lane;
import com.zy.acs.manager.manager.enums.StatusType;
import com.zy.acs.manager.manager.service.CodeService;
import com.zy.acs.manager.manager.service.LaneService;
import com.zy.acs.manager.manager.service.RouteService;
import com.zy.acs.manager.system.service.ConfigService;
import lombok.extern.slf4j.Slf4j;
@@ -32,24 +30,25 @@
@Service
public class LaneBuilder {

    private final RedisSupport redis = RedisSupport.defaultRedisSupport;
//    private final RedisSupport redis = RedisSupport.defaultRedisSupport;

    private List<Lane> lanes = new ArrayList<>();
    private final List<LaneDto> laneDtoList = new ArrayList<>();

    private final Map<String, List<String>> adjacencyCodeMap = new HashMap<>();

    private final Map<String, List<int[]>> laneCodeIdxMap = new HashMap<>();

    private final Map<String, Lane> codeLaneMap = new HashMap<>();
    private final Map<String, LaneDto> codeLaneMap = new HashMap<>();

    private boolean initialized = Boolean.FALSE;

    @Autowired
    private LaneService laneService;
    @Autowired
    private CodeService codeService;
    @Autowired
    private RouteService routeService;
    @Autowired
    private SnowflakeIdWorker snowflakeIdWorker;

    @Autowired
    private ConfigService configService;
    @Autowired
@@ -61,7 +60,7 @@
        return this.initialized;
    }

    public Lane search(String codeData) {
    public LaneDto search(String codeData) {
        if (Cools.isEmpty(codeData) || !this.initialized) {
            return null;
        }
@@ -91,25 +90,31 @@
    @EventListener(ApplicationReadyEvent.class)
    public void init() {
        Integer lev = MapDataDispatcher.MAP_DEFAULT_LEV;
        String laneDataStr = redis.getValue(RedisConstant.MAP_LANE_DATA, String.valueOf(lev));
        if (!Cools.isEmpty(laneDataStr)) {
            this.lanes = GsonUtils.fromJsonToList(laneDataStr, Lane.class);
        } else {
//        String laneDataStr = redis.getValue(RedisConstant.MAP_LANE_DATA, String.valueOf(lev));
        List<Lane> lanes = laneService.list(new LambdaQueryWrapper<Lane>()
//                      .eq(Lane::getZoneId, zoneId)
                        .eq(Lane::getStatus, StatusType.ENABLE.val)
                        .orderByAsc(Lane::getId)
        );

        if (!Cools.isEmpty(lanes)) {
            lanes.forEach(lane -> this.laneDtoList.add(LaneDto.parse(lane)));
//            this.laneDtoList = GsonUtils.fromJsonToList(laneDataStr, LaneDto.class);
        } else {
            StopWatch stopWatch = new StopWatch();
            stopWatch.start();

            this.initLaneData();

            stopWatch.stop();
            log.info("the rcs system calculated lane data which has spend {} millisecond......", stopWatch.getTime());

            redis.setValue(RedisConstant.MAP_LANE_DATA, String.valueOf(lev), GsonUtils.toJson(this.lanes));
            if (!Cools.isEmpty(this.laneDtoList)) {
                laneService.batchSaveByLaneDtoList(null, this.laneDtoList);
//                redis.setValue(RedisConstant.MAP_LANE_DATA, String.valueOf(lev), GsonUtils.toJson(this.laneDtoList));
            }
        }

        this.generateLaneCodeIdx(null);
        this.initialized = Boolean.TRUE;
//        System.out.println(GsonUtils.toJson(this.lanes));
    }

    private void initLaneData() {
@@ -125,7 +130,7 @@

        this.deleteInteractionPoint();

        this.filterLanesWithFewPoints();
//        this.filterLanesWithFewPoints();

        this.generateLaneHash();

@@ -149,29 +154,51 @@
                List<String> neighbors = this.adjacencyCodeMap.get(codeData);
                for (String neighbor : neighbors) {
                    if (this.adjacencyCodeMap.get(neighbor).size() == 2 && !visited.contains(neighbor)) {
                        Lane lane = new Lane(String.valueOf(snowflakeIdWorker.nextId()).substring(3));
                        lane.getCodes().add(codeData); // 包含起点
                        this.dfsCalcIncludingEnd(codeData, neighbor, lane, visited);
                        this.lanes.add(lane);
                        LaneDto laneDto = new LaneDto();
                        laneDto.getCodes().add(codeData); // 包含起点
                        this.dfsCalcIncludingEnd(codeData, neighbor, laneDto, visited);
                        this.laneDtoList.add(laneDto);
                    }
                }
            }
        }

        for (String codeData : codeDataList) {
            if (this.adjacencyCodeMap.get(codeData).size() == 2 && !visited.contains(codeData)) {
            if (this.adjacencyCodeMap.get(codeData).size() == 2
                    && !visited.contains(codeData)
                    && !this.isTurnCorner(codeData)) {
                // 检查是否为环路的一部分
                Lane lane = new Lane(String.valueOf(snowflakeIdWorker.nextId()).substring(3));
                this.dfsCalcForLoop(codeData, null, lane, visited);
                this.lanes.add(lane);
                LaneDto laneDto = new LaneDto();
                this.dfsCalcForLoop(codeData, null, laneDto, visited);
                if (!Cools.isEmpty(laneDto.getCodes())) {
                    this.laneDtoList.add(laneDto);
                }
            }
        }


        // === 补：未被任何 DFS 覆盖的端点（degree <= 1），作为单点 lane ===
        for (String codeData : codeDataList) {
            if (visited.contains(codeData)) {
                continue;
            }
            List<String> neighbors = adjacencyCodeMap.get(codeData);
            int degree = neighbors == null ? 0 : neighbors.size();
            if (degree <= 1) {
                LaneDto laneDto = new LaneDto();
                laneDto.getCodes().add(codeData);
                laneDtoList.add(laneDto);
                visited.add(codeData);
            }
        }
    }

    private void dfsCalcIncludingEnd(String start, String current, Lane lane, Set<String> visited) {
        lane.getCodes().add(current);
    private void dfsCalcIncludingEnd(String start, String current, LaneDto laneDto, Set<String> visited) {
        // 如果 current 本身是拐角（degree=2 且不共线），不应该进入 lane
        if (this.isTurnCorner(current)) {
            return;
        }

        laneDto.getCodes().add(current);
        visited.add(current);

        List<String> neighbors = this.adjacencyCodeMap.get(current);
@@ -187,20 +214,29 @@
            if (!visited.contains(neighbor)) {
                int degree = this.adjacencyCodeMap.get(neighbor).size();
                if (degree == 2) {
                    // 下一跳如果是拐角：直接在拐角前终止，且不把拐角算进 lane
                    if (this.isTurnCorner(neighbor)) {
                        continue;
                    }
                    if (this.isSameDirection(current, neighbor, start)) {
                        this.dfsCalcIncludingEnd(current, neighbor, lane, visited);
                        this.dfsCalcIncludingEnd(current, neighbor, laneDto, visited);
                    }
                } else {
                    // 终点或拐弯点，包含并停止
                    lane.getCodes().add(neighbor);
                    laneDto.getCodes().add(neighbor);
                    visited.add(neighbor);
                }
            }
        }
    }

    private void dfsCalcForLoop(String current, String parent, Lane lane, Set<String> visited) {
        lane.getCodes().add(current);
    private void dfsCalcForLoop(String current, String parent, LaneDto laneDto, Set<String> visited) {
        // 环路 lane：拐角点不算入 lane，并且在拐角前停止
        if (this.isTurnCorner(current)) {
            return;
        }

        laneDto.getCodes().add(current);
        visited.add(current);

        List<String> neighbors = this.adjacencyCodeMap.get(current);
@@ -216,11 +252,15 @@
            if (!visited.contains(neighbor)) {
                int degree = this.adjacencyCodeMap.get(neighbor).size();
                if (degree == 2) {
                    // 下一跳如果是拐角：在拐角前终止，不把拐角加入 lane，也不标 visited
                    if (this.isTurnCorner(neighbor)) {
                        continue;
                    }
                    if (this.isSameDirection(current, neighbor, parent)) {
                        this.dfsCalcForLoop(current, neighbor, lane, visited);
                        this.dfsCalcForLoop(neighbor, current, laneDto, visited);
                    }
                } else {
                    lane.getCodes().add(neighbor);
                    laneDto.getCodes().add(neighbor);
                    visited.add(neighbor);
                }
            }
@@ -247,6 +287,49 @@
        return angleDifference < Math.toRadians(3);
    }

    private boolean isTurnCorner(String codeData) {
        if (Cools.isEmpty(codeData)) {
            return false;
        }
        List<String> neighbors = this.adjacencyCodeMap.get(codeData);
        if (neighbors == null || neighbors.size() != 2) {
            return false;
        }

        String n1 = neighbors.get(0);
        String n2 = neighbors.get(1);

        Code c1 = codeService.getCacheByData(n1);
        Code c = codeService.getCacheByData(codeData);
        Code c2 = codeService.getCacheByData(n2);

        // v1 = c - c1, v2 = c2 - c
        double v1x = c.getX() - c1.getX();
        double v1y = c.getY() - c1.getY();
        double v2x = c2.getX() - c.getX();
        double v2y = c2.getY() - c.getY();

        // 两个向量任一为零向量：不按拐角处理
        double len1 = Math.hypot(v1x, v1y);
        double len2 = Math.hypot(v2x, v2y);
        if (len1 == 0 || len2 == 0) {
            return false;
        }

        // 夹角 cos = dot / (|v1||v2|)
        double dot = v1x * v2x + v1y * v2y;
        double cos = dot / (len1 * len2);

        // 共线有两种：
        // 1) 180°：cos ≈ -1（直线走廊）
        // 2) 0°：cos ≈ +1（路线异常回头），一般也视为共线
        // 只要“不接近 ±1”，就说明不在一条线上 -> 拐角
        double eps = Math.cos(Math.toRadians(3)); // 3度容差
        boolean nearOpposite = cos <= -eps; // 接近 -1
        boolean nearSame = cos >= eps;      // 接近 +1
        return !(nearOpposite || nearSame);
    }

    /**
     * 计算两个点之间的方向角
     */
@@ -262,10 +345,10 @@
     * 3.then merge above two lane because they can connect each other
     */
    private void mergeDeadEndLane() {
        Iterator<Lane> iterator = this.lanes.iterator();
        Iterator<LaneDto> iterator = this.laneDtoList.iterator();
        while (iterator.hasNext()) {
            Lane lane = iterator.next();
            String[] endPoints = lane.queryEndPoints();
            LaneDto laneDto = iterator.next();
            String[] endPoints = laneDto.queryEndPoints();
            if (null == endPoints) {
                continue;
            }
@@ -294,15 +377,15 @@
                String anotherPoint = deadEndPoint.equals(startPoint) ? endPoint : startPoint;
                List<String> anotherPointNears = this.adjacencyCodeMap.get(anotherPoint);
                for (String anotherPointNear : anotherPointNears) {
                    if (!lane.getCodes().contains(anotherPointNear) && this.adjacencyCodeMap.get(anotherPointNear).size() == 2) {
                    if (!laneDto.getCodes().contains(anotherPointNear) && this.adjacencyCodeMap.get(anotherPointNear).size() == 2) {

                        for (Lane lane0 : new ArrayList<>(lanes)) {
                            if (lane0.getCodes().contains(anotherPointNear)) {
                        for (LaneDto laneDto0 : new ArrayList<>(laneDtoList)) {
                            if (laneDto0.getCodes().contains(anotherPointNear)) {

                                lane0.getCodes().addAll(lane.getCodes());
                                laneDto0.getCodes().addAll(laneDto.getCodes());
                                iterator.remove();

                                lane0.sortUsingDfs(this.adjacencyCodeMap);
                                laneDto0.sortUsingDfs(this.adjacencyCodeMap);

                            }
                        }
@@ -315,21 +398,21 @@
    }

    private void deleteInteractionPoint() {
        for (Lane lane : this.lanes) {
            lane.removeInteraction(this.adjacencyCodeMap);
        for (LaneDto laneDto : this.laneDtoList) {
            laneDto.removeInteraction(this.adjacencyCodeMap);
        }
    }

    private void filterLanesWithFewPoints() {
        Integer maxAgvCountInLane = configService.getVal("maxAgvCountInLane", Integer.class);
        this.lanes.removeIf(next -> next.getCodes().size() <= Optional.ofNullable(maxAgvCountInLane).orElse(2));
        this.laneDtoList.removeIf(next -> next.getCodes().size() <= Optional.ofNullable(maxAgvCountInLane).orElse(2));
    }


    private void generateLaneHash() {
        for (Lane lane : this.lanes) {
            String hashCode = generateDigest(lane.getCodes());
            lane.setHashCode(hashCode);
        for (LaneDto laneDto : this.laneDtoList) {
            String hashCode = generateDigest(laneDto.getCodes());
            laneDto.setHashCode(hashCode);
        }
    }

@@ -353,17 +436,17 @@

    public void generateLaneCodeIdx(Integer lev) {
        log.info("There is initializing Lane CodeIdxMap......");
        if (Cools.isEmpty(this.lanes)) {
        if (Cools.isEmpty(this.laneDtoList)) {
            return;
        }
        for (Lane lane : this.lanes) {
        for (LaneDto laneDto : this.laneDtoList) {
            List<int[]> codeIdxList = new ArrayList<>();
            for (String code : lane.getCodes()) {
            for (String code : laneDto.getCodes()) {
                int[] codeMatrixIdx = mapDataDispatcher.getCodeMatrixIdx(lev, code);
                codeIdxList.add(codeMatrixIdx);
                this.laneCodeIdxMap.put(code, codeIdxList);
                this.codeLaneMap.put(code, lane);
                this.codeLaneMap.put(code, laneDto);
            }
        }
    }
}
}