算法集成

zhang

2 天以前 f6df128b575bdb9d0f2512b5fa5a126edfce2799

算法集成

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

@@ -9,7 +9,6 @@
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;
@@ -27,7 +26,6 @@
import org.springframework.stereotype.Component;
import org.springframework.transaction.annotation.Transactional;
import org.springframework.transaction.interceptor.TransactionAspectSupport;
import com.zy.acs.common.utils.News;

import java.util.ArrayList;
import java.util.Arrays;
@@ -144,14 +142,14 @@
//                            }
//                        }
//                        if (null == endCode) {
//                            News.warn("AGV[{}] failed to search destination，there hadn't any idle funSta，TaskTypeType：{}", segment.getAgvId(), FuncStaType.STANDBY);
//                            log.warn("AGV[{}] failed to search destination，there hadn't any idle funSta，TaskTypeType：{}", segment.getAgvId(), FuncStaType.STANDBY);
//                            return;
//                        }
//
//                        segment.setState(SegmentStateType.INIT.toString());
//                        segment.setUpdateTime(now);
//                        if (!segmentService.updateById(segment)) {
//                            News.error("Segment [{}] failed to update ！！！", segment.getGroupId() + " - " + segment.getSerial());
//                            log.error("Segment [{}] failed to update ！！！", segment.getGroupId() + " - " + segment.getSerial());
//                            return;
//                        }
//
@@ -167,7 +165,7 @@
//                        insertSeg.setState(SegmentStateType.WAITING.toString());
//                        insertSeg.setMemo(MapDataConstant.RESOLVE_DEADLOCK);
//                        if (!segmentService.save(insertSeg)) {
//                            News.error("Segment [{}] failed to save ！！！", segment.getTravelId() + " - " + segment.getSerial());
//                            log.error("Segment [{}] failed to save ！！！", segment.getTravelId() + " - " + segment.getSerial());
//                            return;
//                        }
//                        return;
@@ -182,7 +180,7 @@
            if (!Cools.isEmpty(waitingSegList)) {
                for (Segment waitingSeg : waitingSegList) {
                    if (!waitingSeg.getId().equals(segment.getId())) {
//                        News.error("AGV[{}] 任务异常，服务器错误！！！", agv.getUuid());
//                        log.error("AGV[{}] 任务异常，服务器错误！！！", agv.getUuid());
                        return;
                    }
                }
@@ -203,16 +201,21 @@
            //        ArrayList<List<TaskPosDto>> list = JSON.parseObject(travel.getTaskContent(), new TypeReference<ArrayList<List<TaskPosDto>>>() {});

            // * sync wave scope
            if (!avoidWaveCalculator.calcWaveScope(agvModelService.getByAgvId(agv.getId()))) {
                News.error("failed to calculate avoid wave matrix ...");
                return;
            }
//            if (!avoidWaveCalculator.calcWaveScope(agvModelService.getByAgvId(agv.getId()))) {
//                log.error("failed to calculate avoid wave matrix ...");
//                return;
//            }

            // checkout path
            Code startCode = agvDetailService.getCurrentCode(travel.getAgvId());
            Code endCode = codeService.getCacheById(endNode);
//            long startTime = System.currentTimeMillis();
            List<String> pathList = this.checkoutPath(agv, startCode, endCode, segment);
            List<Jam> unfinishedOriginJamByCurrAgv = jamService.getUnfinishedOriginJamByAvo(agv.getId(), startCode.getId(), segment.getId());
            List<String> blackPath = this.getBlackPathList(unfinishedOriginJamByCurrAgv);
            List<String> pathList = mapService.checkoutPath(agv.getUuid(), startCode, endCode, false, blackPath, segment);


            //List<String> pathList = this.checkoutPath(agv, startCode, endCode, segment);
//            System.out.println("checkoutPath: " + (System.currentTimeMillis() - startTime));
            if (Cools.isEmpty(pathList)) {
                return;
@@ -234,7 +237,7 @@
                segment.setState(SegmentStateType.INIT.toString());
                segment.setUpdateTime(now);
                if (!segmentService.updateById(segment)) {
                    News.error("Segment [{}] failed to update ！！！", segment.getGroupId() + " - " + segment.getSerial());
                    log.error("Segment [{}] failed to update ！！！", segment.getGroupId() + " - " + segment.getSerial());
                }
                segmentList.clear();

@@ -249,7 +252,7 @@
                insertSeg.setPosType(TaskPosDto.PosType.MOVE.toString());
                insertSeg.setState(SegmentStateType.WAITING.toString());
                if (!segmentService.save(insertSeg)) {
                    News.error("Segment [{}] failed to save ！！！", insertSeg.getTravelId() + " - " + insertSeg.getSerial());
                    log.error("Segment [{}] failed to save ！！！", insertSeg.getTravelId() + " - " + insertSeg.getSerial());
                }
                segmentList.add(insertSeg);

@@ -279,14 +282,14 @@
                }
            }

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

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

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

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

            throw new RuntimeException("trigger method caught an exception", e);
@@ -301,7 +304,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(agvNo, unfinishedOriginJamByCurrAgv);
        List<String> blackPath = this.getBlackPathList(unfinishedOriginJamByCurrAgv);

        // 结果集
        List<String> pathList = new ArrayList<>();
@@ -338,7 +341,7 @@

                    boolean hasUnavoidableBlocks = blockVehicleList.stream().anyMatch(blockVehicleDto -> !blockVehicleDto.isAvoidable());
                    if (hasUnavoidableBlocks && pathList.size() <= MapDataConstant.MIN_SLICE_PATH_LENGTH) {
//                        News.info("AGV[{}] waiting in place, because the path list is too short...", agvNo);
                        log.info("AGV[{}] waiting in place, because the path list is too short...", agvNo);
                        pathList.clear();
                    }

@@ -346,7 +349,7 @@
                            blockVehicleDto -> null != jamService.getCycleJam(agv.getId(), segment.getId(), blockVehicleDto.getVehicle())
                    );
                    if (hasCycleJam) {
                        News.info("AGV[{}] waiting in place, because has cycle jam...", agvNo);
                        log.info("AGV[{}] waiting in place, because has cycle jam...", agvNo);
                        pathList.clear();
                    }
                }
@@ -355,7 +358,7 @@
            } else {

                if (Cools.isEmpty(blockVehicleList)) {
                    News.warn("AGV[{}] can't reach to code: {}, because there is too many vehicle in the lane...", agvNo, endCode.getData());
                    log.warn("AGV[{}] can't reach to code: {}, because there is too many vehicle in the lane...", agvNo, endCode.getData());
                } else {

                    assert !Cools.isEmpty(blockVehicleList);
@@ -445,14 +448,14 @@

                                pathList = lockPathList;
                            } else {
                                News.error("{}号车辆检索[{}] ===>> [{}]路径失败，原因：{}"
                                log.error("{}号车辆检索[{}] ===>> [{}]路径失败，原因：{}"
                                        , agvNo, startCode.getData(), endCode.getData(), "路径阻塞超时");
                            }
                        } else {
//                            News.warn("{}号车辆正在等待交通堵塞，阻塞车辆：【{}】"
//                                    , agvNo
//                                    , blockVehicleList.stream().map(BlockVehicleDto::getVehicle).collect(Collectors.toList()).toString()
//                            );
                            log.warn("{}号车辆正在等待交通堵塞，阻塞车辆：【{}】"
                                    , agvNo
                                    , blockVehicleList.stream().map(BlockVehicleDto::getVehicle).collect(Collectors.toList()).toString()
                            );
                        }
                    }

@@ -485,12 +488,12 @@
                            previousJam.setState(JamStateType.DEPRECATED.toString());
                            previousJam.setUpdateTime(now);
                            if (!jamService.updateById(previousJam)) {
                                News.error("Jam[{}] failed to update！！！", previousJam.getUuid());
                                log.error("Jam[{}] failed to update！！！", previousJam.getUuid());
                            }
                        }
                    }
                } else {
                    News.error("Jam[{}] failed to update！！！", jam.getUuid());
                    log.error("Jam[{}] failed to update！！！", jam.getUuid());
                }
            }
            // deal expired jam
@@ -502,7 +505,7 @@
                expiredJam.setUpdateTime(now);
                expiredJam.setState(JamStateType.DEPRECATED.toString());
                if (!jamService.updateById(expiredJam)) {
                    News.error("Jam[{}] failed to update！！！", expiredJam.getUuid());
                    log.error("Jam[{}] failed to update！！！", expiredJam.getUuid());
                }
            }

@@ -553,7 +556,7 @@
    private boolean notifyVehicleAvoid(String agvNo, String agvPosCode, List<String> avoidPathList, String sponsor, Jam jam) {
        Long agvId = agvService.getAgvId(agvNo);
        if (!Cools.isEmpty(segmentService.getByAgvAndState(agvId, SegmentStateType.RUNNING.toString()))) {
            News.warn("{}号车辆避让失败，存在进行中任务！！！", agvNo);
            log.warn("{}号车辆避让失败，存在进行中任务！！！", agvNo);
            return false;
        }

@@ -563,7 +566,7 @@
        assert avoidPathList.size() >= 2;
        RetreatNavigateNode finalNode = retreatNavigateService.execute(agvNo, startNode, avoidPathList, sponsor, jam);
        if (null == finalNode) {
            News.warn("{}号车辆避让失败，检索避让点失败！！！", agvNo);
            log.warn("{}号车辆避让失败，检索避让点失败！！！", agvNo);
            return false;
        }

@@ -574,7 +577,7 @@
        if (!Cools.isEmpty(waitingSegList)) {

            if (waitingSegList.size() > 1) {
                News.error("避让通知失败，{}号车辆存在多个等待中的Segment！！！", agvNo);
                log.error("避让通知失败，{}号车辆存在多个等待中的Segment！！！", agvNo);
                return false;
            }
            // revert
@@ -583,7 +586,7 @@
                seg.setState(SegmentStateType.INIT.toString());
                seg.setUpdateTime(now);
                if (!segmentService.updateById(seg)) {
                    News.error("Segment [{}] 更新失败 ！！！", seg.getTravelId() + " - " + seg.getSerial());
                    log.error("Segment [{}] 更新失败 ！！！", seg.getTravelId() + " - " + seg.getSerial());
                }
            }
            Segment segment = waitingSegList.get(0);
@@ -598,7 +601,7 @@
            insertSeg.setPosType(TaskPosDto.PosType.MOVE.toString());
            insertSeg.setState(SegmentStateType.WAITING.toString());
            if (!segmentService.save(insertSeg)) {
                News.error("Segment [{}] 保存失败 ！！！", segment.getTravelId() + " - " + segment.getSerial());
                log.error("Segment [{}] 保存失败 ！！！", segment.getTravelId() + " - " + segment.getSerial());
                return false;
            } else {
                jam.setAvoSeg(insertSeg.getId());
@@ -624,13 +627,13 @@
            jam.setStartTime(new Date());
            jam.setState(JamStateType.RUNNING.toString());
            if (!jamService.save(jam)) {
                News.error("AGV[{}] failed to save jam", agv.getUuid());
                log.error("AGV[{}] failed to save jam", agv.getUuid());
                throw new CoolException("failed to save jam");
            }
        } else {
            jam.setDuration(System.currentTimeMillis() - jam.getStartTime().getTime());
            if (!jamService.updateById(jam)) {
                News.error("AGV[{}] failed to update jam", agv.getUuid());
                log.error("AGV[{}] failed to update jam", agv.getUuid());
            }
        }
        return jam;
@@ -640,7 +643,7 @@
        originJam.setUpdateTime(new Date());
        originJam.setState(JamStateType.FINISH.toString());
        if (!jamService.updateById(originJam)) {
            News.error("Jam[{}] failed to update", originJam.getUuid());
            log.error("Jam[{}] failed to update", originJam.getUuid());
            return originJam;
        } else {
            return this.createOrUpdateJam(
@@ -653,23 +656,17 @@
        }
    }

    private List<String> getBlackPathList(String agvNo, List<Jam> unfinishedOriginJamByCurrAgv) {
    private List<String> getBlackPathList(List<Jam> unfinishedOriginJamByCurrAgv) {
        List<String> blackPathList = new ArrayList<>();
        Integer lev = MapDataDispatcher.MAP_DEFAULT_LEV;
        if (Cools.isEmpty(unfinishedOriginJamByCurrAgv)) {
            return blackPathList;
        }

        AgvModel agvModel = agvModelService.getByAgvNo(agvNo);
        double bufferRadius = MapDataUtils.buildFootprint(agvModel).maxExtent();

        if (!Cools.isEmpty(unfinishedOriginJamByCurrAgv)) {
        for (Jam jam : unfinishedOriginJamByCurrAgv) {
            if (!Cools.isEmpty(jam.getJamPath())) {

                List<String> list = GsonUtils.fromJsonToList(jam.getJamPath(), String.class);

                String jamAgvNo = agvService.getAgvNo(jam.getJamAgv());
                List<String> jamDynamicNodes = mapService.queryCodeListFromDynamicNode(lev, jamAgvNo);
                    Agv jamAgv = agvService.getById(jam.getJamAgv());
                    List<String> jamDynamicNodes = mapService.queryCodeListFromDynamicNode(lev, jamAgv.getUuid());
                // jamDynamicNodes has sorted
                String firstCodeNode = jamDynamicNodes.stream().findFirst().orElse(null);

@@ -679,8 +676,10 @@
                        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;
                            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);
@@ -693,7 +692,7 @@
                blackPathList.addAll(list);
            }
        }

        }
        return blackPathList.stream().distinct().collect(Collectors.toList());
    }

@@ -744,7 +743,7 @@
            return false;
        }
        if (jamList.size() > 1) {
            News.error("AvoSeg[id = {}] seg data has exception, result in two jams", avoSeg);
            log.error("AvoSeg[id = {}] seg data has exception, result in two jams", avoSeg);
        }
        Jam jam = jamList.get(0);
        if (jam.getState().equals(JamStateType.DEPRECATED.toString())) {

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

@@ -1,26 +1,26 @@
package com.zy.acs.manager.core.service.astart;

import com.fasterxml.jackson.databind.ObjectMapper;
import com.zy.acs.common.utils.RedisSupport;
import com.zy.acs.framework.common.Cools;
import com.zy.acs.manager.common.utils.MapDataUtils;
import com.zy.acs.manager.core.service.AgvAreaDispatcher;
import com.zy.acs.manager.core.service.LaneBuilder;
import com.zy.acs.manager.core.service.astart.domain.AStarNavigateNode;
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.Segment;
import com.zy.acs.manager.manager.service.AgvService;
import com.zy.acs.manager.manager.service.JamService;
import com.zy.acs.manager.system.service.ConfigService;
import lombok.extern.slf4j.Slf4j;
import org.springframework.beans.factory.annotation.Autowired;
import org.springframework.core.io.ClassPathResource;
import org.springframework.stereotype.Service;

import javax.annotation.PostConstruct;
import java.io.InputStream;
import java.util.*;

/**
 * Created by vincent on 6/12/2024
 */

@Slf4j
@Service
public class AStarNavigateService {
@@ -28,10 +28,9 @@
    private final RedisSupport redis = RedisSupport.defaultRedisSupport;

    public static final boolean OPEN_TURN_COST_WEIGHT = Boolean.TRUE;

    public static final int WEIGHT_CALC_FACTOR = 1;

    // right left up down
    // 四方向
    private final static int[][] DIRECTIONS = {{0,1},{0,-1},{-1,0},{1,0}};

    @Autowired
@@ -39,33 +38,154 @@
    @Autowired
    private JamService jamService;
    @Autowired
    private LaneBuilder laneBuilder;
    private LaneBuilder laneService;
    @Autowired
    private ConfigService configService;
    @Autowired
    private AgvAreaDispatcher agvAreaDispatcher;
    @Autowired
    private AgvService agvService;

    public synchronized AStarNavigateNode execute(String agvNo, AStarNavigateNode start, AStarNavigateNode end
            , Boolean lock, List<String> blackList, Segment segment) {
    /**
     * 节点坐标缓存：codeData -> Coordinate
     * 从json加载
     */
    private Map<String, NodeCoordinate> nodeCoordinateMap = new HashMap<>();

    public static class NodeCoordinate {
        private double x;
        private double y;

        public NodeCoordinate() {}

        public NodeCoordinate(double x, double y) {
            this.x = x;
            this.y = y;
        }

        public double getX() { return x; }
        public void setX(double x) { this.x = x; }
        public double getY() { return y; }
        public void setY(double y) { this.y = y; }

        /**
         * 计算与另一个节点的距离（米）
         */
        public double distanceTo(NodeCoordinate other) {
            if (other == null) {
                return 0.0;
            }
            double dx = (this.x - other.x) / 1000.0; // 毫米转米
            double dy = (this.y - other.y) / 1000.0;
            return Math.sqrt(dx * dx + dy * dy);
        }
    }

    private static class MotionParams {
        final double vMax;          // 最大速度 (m/s)
        final double aAccel;        // 加速度 (m/s^2)
        final double aDecel;        // 减速度 (m/s^2)
        final double controlPeriod; // 控制周期 (s)
        final double fallbackGridLength; // 后备网格距离 (m)

        MotionParams(double vMax, double aAccel, double aDecel, double controlPeriod, double fallbackGridLength) {
            this.vMax = vMax;
            this.aAccel = aAccel;
            this.aDecel = aDecel;
            this.controlPeriod = controlPeriod;
            this.fallbackGridLength = fallbackGridLength;
        }
    }

    @PostConstruct
    public void loadNodeCoordinates() {
        try {
            // 文件路径
            String coordinateFile = getStringConfig("nodeCoordinateFile", "man_code.json");

            ClassPathResource resource = new ClassPathResource(coordinateFile);
            InputStream inputStream = resource.getInputStream();

            ObjectMapper objectMapper = new ObjectMapper();
            Map<String, Object> jsonData = objectMapper.readValue(inputStream, Map.class);

            if (jsonData.containsKey("path_id_to_coordinates")) {
                Map<String, List<Map<String, Object>>> pathData =
                        (Map<String, List<Map<String, Object>>>) jsonData.get("path_id_to_coordinates");

                for (Map.Entry<String, List<Map<String, Object>>> entry : pathData.entrySet()) {
                    String nodeCode = entry.getKey();
                    List<Map<String, Object>> coordinates = entry.getValue();

                    if (coordinates != null && !coordinates.isEmpty()) {
                        Map<String, Object> coord = coordinates.get(0);
                        double x = ((Number) coord.get("x")).doubleValue();
                        double y = ((Number) coord.get("y")).doubleValue();
                        nodeCoordinateMap.put(nodeCode, new NodeCoordinate(x, y));
                    }
                }

                log.info("success load node coordinates, total {} nodes", nodeCoordinateMap.size());
            } else {
                log.warn("path_id_to_coordinates not found");
            }

        } catch (Exception e) {
            log.error("load node coordinates failed", e);
        }
    }

    private String getStringConfig(String key, String defaultVal) {
        try {
            String v = configService.getVal(key, String.class);
            if (v != null && !v.isEmpty()) {
                return v;
            }
        } catch (Exception e) {
            log.debug("config {} not available, use default value {}", key, defaultVal);
        }
        return defaultVal;
    }

    public synchronized AStarNavigateNode execute(String agvNo,
                                                  AStarNavigateNode start,
                                                  AStarNavigateNode end,
                                                  Boolean lock,
                                                  List<String> blackList,
                                                  Segment segment) {
        if (start.getX() == end.getX() && start.getY() == end.getY()) {
            return end;
        }

        // scope code area: 4ms
        Long agvId = agvService.getAgvId(agvNo);
        Boolean withinArea = agvAreaDispatcher.isAgvExistsInAnyArea(agvId);
        List<String> scopeCodeList = new ArrayList<>();
        if (withinArea) {
            scopeCodeList = agvAreaDispatcher.getCodesByAgvId(agvId);
            if (!Cools.isEmpty(scopeCodeList) && !scopeCodeList.contains(start.getCodeData())) {
                withinArea = false;
            }
        }

        Integer maxAgvCountInLane = configService.getVal("maxAgvCountInLane", Integer.class);

        // ===== 1. 基于物理参数计算时间步相关的参数 =====
        // 网格中心距（米）- 默认后备值，当无法获取实际坐标时使用
        double gridLength = getDoubleConfig("navigateGridLength", 0.5);
        // 控制周期（秒/时间步）
        double controlPeriod = getDoubleConfig("navigateControlPeriod", 0.5);

        // AGV 物理特性
        double vMax   = getDoubleConfig("agvMaxSpeed", 1.0);      // m/s
        double aAccel = getDoubleConfig("agvAccel", 0.4);         // m/s^2
        double aDecel = getDoubleConfig("agvDecel", 0.4);         // m/s^2
        double tTurn  = getDoubleConfig("agvTurn90TimeSec", 4.0); // s

        // 安全时间间隔（秒）→ 时间步
        double safetyGapTime = getDoubleConfig("navigateSafetyGapTimeSec", 3.0);

        // 转 90° 的时间（秒）直接用配置
        double turnTimeSec = tTurn;
        // 转弯时间步成本
        final int turnStepCost = Math.max(1, (int) Math.ceil(turnTimeSec / controlPeriod));
        final int safetyGapStep = Math.max(1, (int) Math.ceil(safetyGapTime / controlPeriod));

        // 运动学参数打包
        final MotionParams motionParams = new MotionParams(vMax, aAccel, aDecel, controlPeriod, gridLength);

        if (log.isDebugEnabled()) {
            log.debug("A* phys params init: gridLength(fallback)={}, controlPeriod={}, vMax={}, aAccel={}, aDecel={}, " +
                            "tTurn={}, turnStepCost={}, safetyGapStep={}, loaded node coordinates={}",
                    gridLength, controlPeriod, vMax, aAccel, aDecel,
                    tTurn, turnStepCost, safetyGapStep, nodeCoordinateMap.size());
        }

        // ===== 2. A* 初始化 =====
        PriorityQueue<AStarNavigateNode> openQueue = new PriorityQueue<>();
        Set<AStarNavigateNode> existNodes = new HashSet<>();
        Map<String, Integer> bestGMap = new HashMap<>();
@@ -73,6 +193,8 @@
        start.setG(0);
        start.setH(calcNodeCost(start, end));
        start.setF(start.getG() + start.getH());
        // 时间步从 0 开始
        start.setStep(0);
        String startKey = start.getX() + "_" + start.getY();
        bestGMap.put(startKey, start.getG());

@@ -84,24 +206,30 @@
        DynamicNode[][] dynamicMatrix = mapDataDispatcher.getDynamicMatrix(null);
        String[][] waveMatrix = mapDataDispatcher.getWaveMatrix(null);

        // ===== 3. 基于 dynamicMatrix 估算各 AGV 当前进度 =====
        Map<String, Integer> currentSerialMap = buildCurrentSerialMap(dynamicMatrix, agvNo);

        long getNeighborNodesTime = 0;
        int getNeighborNodesCount = 0;

        while (!openQueue.isEmpty()) {
            // 取优先队列顶部元素并且把这个元素从Open表中删除，取F值最小的节点
            // 取 F 值最小的节点
            AStarNavigateNode currentNode = openQueue.poll();

            // 终点
            // 已到终点
            if (currentNode.getX() == end.getX() && currentNode.getY() == end.getY()) {
//                System.out.println("getNeighborNodes spend time: " + getNeighborNodesTime +", count: " + getNeighborNodesCount);
                return currentNode;
            }

            long currentTime = System.currentTimeMillis();
            List<AStarNavigateNode> neighbourNodes = this.getNeighborNodes(currentNode, mapMatrix, existNodes);
            List<AStarNavigateNode> neighbourNodes =
                    this.getNeighborNodes(currentNode, mapMatrix, codeMatrix, existNodes,
                            motionParams, turnStepCost);
            getNeighborNodesTime += System.currentTimeMillis() - currentTime;
            getNeighborNodesCount++;

            for (AStarNavigateNode node : neighbourNodes) {
                node.setCodeData(codeMatrix[node.getX()][node.getY()]);

                if (withinArea) {
                    assert !Cools.isEmpty(scopeCodeList);
                    if (!scopeCodeList.contains(node.getCodeData())) { continue; }
                }

                boolean isEndNode = node.getX() == end.getX() && node.getY() == end.getY();

@@ -110,27 +238,50 @@
                if (!Cools.isEmpty(blackList) && blackList.contains(node.getCodeData())) {
                    continue;
                }
                // 特殊情况，当blackList有且只有一个元素且为startNode时
                // 说明blackList已经知道当前导航起始点和目标点为相邻节点
                // 但是当前blackList的任务是不让系统走相邻的最短路径，所以才会有下面的判断和continue
                // 特殊：blackList 只有 start 节点，避免走直接相邻最短路
                if (blackList.size() == 1 && blackList.get(0).equals(start.getCodeData())) {
                    if (isEndNode && currentNode.getCodeData().equals(start.getCodeData())) {
                        continue;
                    }
                }

                // 节点被占用
                // ===== 3.1 动态避障：基于时间步的时空冲突判断 =====
                DynamicNode dynamicNode = dynamicMatrix[node.getX()][node.getY()];
                String vehicle = dynamicNode.getVehicle();
                assert !vehicle.equals(DynamicNodeType.BLOCK.val);

                if (!vehicle.equals(DynamicNodeType.ACCESS.val)) {
                    if (!vehicle.equals(agvNo)) {
                        if (lock) {
                            continue;
                        boolean conflict = true;

                        Integer otherCurrentSerial = currentSerialMap.get(vehicle);
                        int otherSerial = dynamicNode.getSerial();

                        if (otherCurrentSerial != null) {
                            int remainStep = otherSerial - otherCurrentSerial;
                            if (remainStep < 0) {
                                remainStep = 0;
                            }
                            // 我从起点到该节点需要的时间步
                            int myStep = node.getStep();

                            // 如果我到时对方已经离开，并且预留安全间隔，则允许共享该节点
                            if (myStep > remainStep + safetyGapStep) {
                                conflict = false;
                            }
                        }

                        // 如果存在车辆，则增加权重 2 或者 3，因为拐点会增加权重 1
                        // vehicle已经为当前segment做过了避让，且避让任务已完成，则权重值增加
                        if (conflict) {
                            if (lock) {
                                // 锁定模式下，直接视为不可走
                                continue;
                            } else {
                                // 非锁模式：允许通过，但增加代价
                                weight += (WEIGHT_CALC_FACTOR * 2);
                            }
                        }

                        // 拥堵历史加权（与时间冲突判断正交）
                        if (null != segment) {
                            if (!Cools.isEmpty(jamService.getJamFromSegmentByAvo(segment, vehicle))) {
                                weight += (WEIGHT_CALC_FACTOR * 3);
@@ -141,22 +292,21 @@
                    }
                }

                // 避障波
                // ===== 3.2 避障波 waveMatrix 判断 =====
                String waveNode = waveMatrix[node.getX()][node.getY()];
                assert !waveNode.equals(WaveNodeType.DISABLE.val);
                if (!waveNode.equals(WaveNodeType.ENABLE.val)) {
                    List<String> waveNodeList = MapDataUtils.parseWaveNode(waveNode);
                    List<String> otherWaveList = MapDataUtils.hasOtherWave(waveNodeList, agvNo);
                    if (!Cools.isEmpty(otherWaveList)) {

                        if (lock) {
                            continue;
                        }
                    }
                }

                // 单巷道车辆容载数量
                List<int[]> laneCodeIdxList = laneBuilder.getLaneCodeIdxList(node.getCodeData());
                // ===== 3.3 单巷道容量控制 =====
                List<int[]> laneCodeIdxList = laneService.getLaneCodeIdxList(node.getCodeData());
                if (!Cools.isEmpty(laneCodeIdxList)) {
                    Set<String> lanVehicleSet = new HashSet<>();

@@ -175,17 +325,17 @@
                    }
                }

                // ===== 3.4 转弯额外代价（在步长基础上加权） =====
                if (OPEN_TURN_COST_WEIGHT) {
                    if (this.isTurning(currentNode, node)) {
                        weight += WEIGHT_CALC_FACTOR;
                        node.setTurnCount(currentNode.getTurnCount() + 1);
                    } else {
                        // 方向没变
                        node.setTurnCount(currentNode.getTurnCount());
                    }
                }

                //进行计算对 G, F, H 等值
                // ===== 3.5 计算 G/F/H 等 =====
                node.setWeight(weight);
                node.setLastDistance(calcNodeCost(currentNode, node));
                node.initNode(currentNode, end);
@@ -196,45 +346,52 @@
                Integer recordedG = bestGMap.get(key);
                if (recordedG == null || node.getG() <= recordedG) {
                    bestGMap.put(key, node.getG());

                    openQueue.add(node);
                }

//                openQueue.add(node);
//                existNodes.add(node);
            }
        }
//        System.out.println("getNeighborNodes spend time: " + getNeighborNodesTime +", count: " + getNeighborNodesCount);
        return null;
    }

    // 获取四周节点
    private List<AStarNavigateNode> getNeighborNodes(AStarNavigateNode currentNode, int[][] mapMatrix, Set<AStarNavigateNode> existNodes) {
    /**
     * 获取当前节点的四周邻居节点
     *
     * @param currentNode 当前节点
     * @param mapMatrix 地图矩阵
     * @param codeMatrix 节点编码矩阵
     * @param existNodes 已存在节点集合
     * @param motionParams AGV运动学参数
     * @param turnStepCost 转弯时间步成本
     * @return 邻居节点列表
     */
    private List<AStarNavigateNode> getNeighborNodes(AStarNavigateNode currentNode,
                                                     int[][] mapMatrix,
                                                     String[][] codeMatrix,
                                                     Set<AStarNavigateNode> existNodes,
                                                     MotionParams motionParams,
                                                     int turnStepCost) {
        int x = currentNode.getX();
        int y = currentNode.getY();
        AStarNavigateNode parent = currentNode.getParent();

//        List<AStarNavigateNode> neighbourNodes = new CopyOnWriteArrayList<>();
        List<AStarNavigateNode> neighbourNodes = new ArrayList<>();

        List<AStarNavigateNode> possibleNodes = new ArrayList<>();

        // 遍历四个方向：右、左、上、下
        for (int[] d: DIRECTIONS) {
            int nx = x + d[0];
            int ny = y + d[1];
            // 如果父节点不为空，并且 (nx,ny) 等于父节点坐标，则跳过
            // 不回到父节点
            if (parent != null && nx == parent.getX() && ny == parent.getY()) {
                continue;
            }
            possibleNodes.add(new AStarNavigateNode(nx, ny));
        }

//        possibleNodes.parallelStream()
//                .map(extendNode -> extendNeighborNodes(currentNode, extendNode, mapMatrix, existNodes, null, null))
//                .filter(Objects::nonNull)
//                .forEach(neighbourNodes::add);

        // 扩展每个可能的邻居节点
        for (AStarNavigateNode pn : possibleNodes) {
            AStarNavigateNode next = extendNeighborNodes(currentNode, pn, mapMatrix, existNodes, null, null);
            AStarNavigateNode next = extendNeighborNodes(currentNode, pn, mapMatrix, codeMatrix,
                    existNodes, null, null, motionParams, turnStepCost);
            if (next != null) {
                neighbourNodes.add(next);
            }
@@ -243,10 +400,32 @@
        return neighbourNodes;
    }

    private AStarNavigateNode extendNeighborNodes(AStarNavigateNode currentNode, AStarNavigateNode extendNode, int[][] mapMatrix, Set<AStarNavigateNode> existNodes, Integer dx, Integer dy) {
    /**
     * 扩展邻居节点，计算基于实际距离的时间步成本
     *
     * @param currentNode 当前节点
     * @param extendNode 待扩展节点
     * @param mapMatrix 地图矩阵
     * @param codeMatrix 节点编码矩阵
     * @param existNodes 已存在节点集合
     * @param dx x方向增量（递归调用时使用）
     * @param dy y方向增量（递归调用时使用）
     * @param motionParams AGV运动学参数
     * @param turnStepCost 转弯时间步成本
     * @return 扩展后的节点，若不可达则返回null
     */
    private AStarNavigateNode extendNeighborNodes(AStarNavigateNode currentNode,
                                                  AStarNavigateNode extendNode,
                                                  int[][] mapMatrix,
                                                  String[][] codeMatrix,
                                                  Set<AStarNavigateNode> existNodes,
                                                  Integer dx,
                                                  Integer dy,
                                                  MotionParams motionParams,
                                                  int turnStepCost) {
        AStarNavigateNode nextNode;

        if (null == dx || null == dy) {
        if (dx == null || dy == null) {
            dx = extendNode.getX() - currentNode.getX();
            dy = extendNode.getY() - currentNode.getY();
            nextNode = extendNode;
@@ -256,70 +435,198 @@

        int x = nextNode.getX();
        int y = nextNode.getY();
        // 数组越界
        if (x < 0 || x >= mapMatrix.length
                || y < 0 || y >= mapMatrix[0].length) {

        // 数组越界检查
        if (x < 0 || x >= mapMatrix.length || y < 0 || y >= mapMatrix[0].length) {
            return null;
        }

        // 遇到不可通行节点，继续沿当前方向扩展
        if (mapMatrix[x][y] == MapNodeType.DISABLE.val)  {

            return extendNeighborNodes(currentNode, nextNode, mapMatrix, existNodes, dx, dy);
            return extendNeighborNodes(currentNode, nextNode, mapMatrix, codeMatrix,
                    existNodes, dx, dy, motionParams, turnStepCost);
        }

        assert mapMatrix[x][y] == MapNodeType.ENABLE.val;

//        if (existNodes.contains(nextNode)) {
//            return null;
//        }

        // 判断通过性
        String routeCdaKey = RouteGenerator.generateRouteCdaKey(new int[]{currentNode.getX(), currentNode.getY()}, new int[]{nextNode.getX(), nextNode.getY()});
        // 判断通道通过性
        String routeCdaKey = RouteGenerator.generateRouteCdaKey(
                new int[]{currentNode.getX(), currentNode.getY()},
                new int[]{nextNode.getX(), nextNode.getY()});
        if (!mapDataDispatcher.validRouteCdaKey(routeCdaKey)) {
            return null;
        }

        // ===== 基于实际坐标计算节点间距离和时间步 =====
        boolean turning = isTurning(currentNode, nextNode);
        int deltaStep;

        if (turning) {
            // 转弯：使用固定的转弯时间步
            deltaStep = turnStepCost;
        } else {
            // 直行：根据实际距离计算时间步
            String currentCode = codeMatrix[currentNode.getX()][currentNode.getY()];
            String nextCode = codeMatrix[x][y];

            // 从坐标映射获取实际距离
            double actualDistance = calculateActualDistance(currentCode, nextCode, motionParams.fallbackGridLength);

            // 基于实际距离和运动学模型计算所需时间
            double travelTimeSec = calcStraightTravelTimeSec(actualDistance,
                    motionParams.vMax, motionParams.aAccel, motionParams.aDecel);

            // 转换为时间步（至少为1）
            deltaStep = Math.max(1, (int) Math.ceil(travelTimeSec / motionParams.controlPeriod));

            if (log.isTraceEnabled()) {
                log.trace("节点 {} -> {} 距离={}m, 时间={}s, 时间步={}",
                        currentCode, nextCode, actualDistance, travelTimeSec, deltaStep);
            }
        }

        // 设置累计时间步
        nextNode.setStep(currentNode.getStep() + deltaStep);

        return nextNode;
    }

    /**
     * 计算两个节点间的实际距离
     * 优先使用坐标映射计算欧几里得距离，若无坐标数据则使用后备网格距离
     *
     * @param fromCode 起始节点编码
     * @param toCode 目标节点编码
     * @param fallbackDistance 后备距离（米）
     * @return 实际距离（米）
     */
    private double calculateActualDistance(String fromCode, String toCode, double fallbackDistance) {
        NodeCoordinate fromCoord = nodeCoordinateMap.get(fromCode);
        NodeCoordinate toCoord = nodeCoordinateMap.get(toCode);

        if (fromCoord != null && toCoord != null) {
            return fromCoord.distanceTo(toCoord);
        } else {
            if (log.isTraceEnabled() && (fromCoord == null || toCoord == null)) {
                log.trace("节点 {} 或 {} 缺少坐标数据，使用后备距离 {}m",
                        fromCode, toCode, fallbackDistance);
            }
            return fallbackDistance;
        }
    }

    //------------------A*启发函数------------------//

    //计算通过现在的结点的位置和最终结点的位置计算H值(曼哈顿法：坐标分别取差值相加)
    // 曼哈顿距离
    private int calcNodeCost(AStarNavigateNode node1, AStarNavigateNode node2) {
        return Math.abs(node2.getX() - node1.getX()) + Math.abs(node2.getY() - node1.getY());
    }

    // 转弯判断：只允许“垂直或水平”运动
    /**
     * 根据当前 dynamicMatrix 预估每辆车的“当前时间步”（使用最小 serial 作为近似）。
     */
    private Map<String, Integer> buildCurrentSerialMap(DynamicNode[][] dynamicMatrix, String selfAgvNo) {
        Map<String, Integer> result = new HashMap<>();
        if (dynamicMatrix == null) {
            return result;
        }
        for (int i = 0; i < dynamicMatrix.length; i++) {
            DynamicNode[] row = dynamicMatrix[i];
            if (row == null) {
                continue;
            }
            for (int j = 0; j < row.length; j++) {
                DynamicNode node = row[j];
                if (node == null) {
                    continue;
                }
                String vehicle = node.getVehicle();
                if (vehicle == null
                        || DynamicNodeType.ACCESS.val.equals(vehicle)
                        || DynamicNodeType.BLOCK.val.equals(vehicle)
                        || vehicle.equals(selfAgvNo)) {
                    continue;
                }
                int serial = node.getSerial();
                Integer recorded = result.get(vehicle);
                if (recorded == null || serial < recorded) {
                    result.put(vehicle, serial);
                }
            }
        }
        return result;
    }

    // 转弯判断：是否改变了方向
    private boolean isTurning(AStarNavigateNode currNode, AStarNavigateNode nextNode) {
        // 第一个点
        if (currNode.getParent() == null) {
            return false;
        }
        AStarNavigateNode parent = currNode.getParent();
        // 如果下一点（nextNode）与 parent 在同一行或同一列 => 没有转弯
        // 注意，这实际上等同于“(curr->next) 的方向 == (parent->curr) 的方向”。
        boolean sameRowOrCol =
                (nextNode.getX() == parent.getX())
                        || (nextNode.getY() == parent.getY());
        return !sameRowOrCol;
        // 如果下一点与 parent 在同一行或同一列 => 没有转弯
        if (nextNode.getX() == parent.getX() || nextNode.getY() == parent.getY()) {
            return false;
        }
        return true;
    }

    // 转弯判断：如果这两个向量相同（例如都等于 (0,1)），说明方向相同；否则说明转弯。
//    private boolean isTurning(AStarNavigateNode currNode, AStarNavigateNode nextNode) {
//        // 如果 currNode 没有父节点，说明是起点，不算转弯
//        if (currNode.getParent() == null) {
//            return false;
//        }
//        // 取出坐标
//        AStarNavigateNode parent = currNode.getParent();
//        int px = currNode.getX() - parent.getX();  // parent -> curr 的x偏移
//        int py = currNode.getY() - parent.getY();  // parent -> curr 的y偏移
//
//        int nx = nextNode.getX() - currNode.getX(); // curr -> next 的x偏移
//        int ny = nextNode.getY() - currNode.getY(); // curr -> next 的y偏移
//
//        // 如果 (px, py) 与 (nx, ny) 不一样，就说明转弯
//        return (px != nx) || (py != ny);
//    }
    // --------- 物理参数 → 时间步 的换算辅助方法 --------- //

    /**
     * 从配置中心读取 double，失败则返回默认值
     */
    private double getDoubleConfig(String key, double defaultVal) {
        try {
            Double v = configService.getVal(key, Double.class);
            if (v != null && v > 0) {
                return v;
            }
        } catch (Exception e) {
            log.warn("config {} not available, use default {}", key, defaultVal);
        }
        return defaultVal;
    }

    /**
     * 计算一格直线移动的物理时间（秒）
     * 使用标准的加速-匀速-减速 或 三角速度曲线模型：
     *
     * - 先算达到 vmax 所需的加速距离 s_acc = vmax^2 / (2 a_acc)
     * - 减速距离 s_dec = vmax^2 / (2 a_dec)
     * - 若 dist >= s_acc + s_dec：能跑到 vmax，则 t = t_acc + t_dec + t_cruise
     * - 否则：达不到 vmax，采用三角形速度曲线，求峰值速度 v_peak，再算 t_acc + t_dec
     */
    private double calcStraightTravelTimeSec(double dist,
                                             double vMax,
                                             double aAcc,
                                             double aDec) {
        if (dist <= 0) {
            return 0.0;
        }
        // 避免参数异常
        vMax = Math.max(vMax, 0.01);
        aAcc = Math.max(aAcc, 0.01);
        aDec = Math.max(aDec, 0.01);

        double sAcc = vMax * vMax / (2.0 * aAcc);
        double sDec = vMax * vMax / (2.0 * aDec);

        // 情况1：距离足够长，可以加速到 vmax 再减速
        if (dist >= sAcc + sDec) {
            double tAcc = vMax / aAcc;
            double tDec = vMax / aDec;
            double sCruise = dist - sAcc - sDec;
            double tCruise = sCruise / vMax;
            return tAcc + tCruise + tDec;
        } else {
            // 情况2：距离较短，达不到 vmax，采用三角速度曲线
            // dist = v_peak^2 / (2 aAcc) + v_peak^2 / (2 aDec)
            double denom = (1.0 / aAcc + 1.0 / aDec);
            double vPeak = Math.sqrt(2.0 * dist / denom);
            double tAcc = vPeak / aAcc;
            double tDec = vPeak / aDec;
            return tAcc + tDec;
        }
    }

}

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

@@ -32,6 +32,10 @@
//    private String direction;   //行走方向

    private String codeData;
    private int step;
    public int getStep() { return step; }
    public void setStep(int step) { this.step = step; }


    public AStarNavigateNode(int x, int y) {
        this.x = x;