算法集成

zhang

2 天以前 f6df128b575bdb9d0f2512b5fa5a126edfce2799

 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;
        }
    }

}