2026.2.9 小车避让

ruoyi-admin/src/main/java/com/ruoyi/schedule/plcRead/srgv/roadUtil/PathFinder.java

@@ -99,17 +99,10 @@ public class PathFinder {
         Collections.reverse(path);
 
 
-
-
         return path;
     }
 
 
-
-
-
-
-
     /**
      * 使用 A* 算法查找从起点到终点的最短路径。
      *
@@ -203,7 +196,9 @@ public class PathFinder {
 
         // 如果终点不可达
         if (gCost[endX][endY] == Integer.MAX_VALUE) {
-            return null;
+            List<int[]> path = new ArrayList<>();
+            path.add(new int[]{-1, -1});
+            return path;
         }
 
         // 回溯路径
@@ -272,14 +267,6 @@ public class PathFinder {
     }
 
 
-
-
-
-
-
-
-
-
     /**
      * 提取路径中的拐点（1→2 或 2→1 的边界点）
      *
@@ -315,15 +302,12 @@ public class PathFinder {
         List<int[]> collect = inflectionPoints.stream().distinct().collect(Collectors.toList());
 
 
-
         return collect;
 
 
-
     }
 
 
-
     /**
      * 使用 BFS 查找从起点开始，路径上所有点的值为 2，直到终点（值为 1 或 2）的最短路径。
      *
@@ -398,9 +382,6 @@ public class PathFinder {
     }
 
 
-
-
-
     /**
      * 提取路径中的拐点：
      * - 区域类型转换点（1 ↔ 2）
@@ -483,12 +464,12 @@ public class PathFinder {
 
 
     /*
-    * 此方法使用情景
+     * 此方法使用情景
-    * 一个原点（目标点）、多个目标点（原点），多个目标点（原点）互相影响，
+     * 一个原点（目标点）、多个目标点（原点），多个目标点（原点）互相影响，
-    * 按算法查找多个目标点（原点）的最优路径
+     * 按算法查找多个目标点（原点）的最优路径
-    *
+     *
-    * */
+     * */
-    public static Site findOptimalCarWithObstacles(int[][] grid, Site srcSite, List<Site> tarSites){
+    public static Site findOptimalCarWithObstacles(int[][] grid, Site srcSite, List<Site> tarSites) {
         //所有车先都设置为障碍
         tarSites.forEach(site -> {
             int[] pos = site.getStartPoint();
@@ -507,7 +488,6 @@ public class PathFinder {
                             }
 
 
-
                             // 计算路径
                             List<int[]> path = findShortestPath(grid, startPoint[0], startPoint[1], srcSite.getStartPoint()[0], srcSite.getStartPoint()[1]);
                             if (path == null) {
@@ -515,7 +495,6 @@ public class PathFinder {
                             }
 
 
-
                             // 检查路径冲突
                             boolean blocked = tarSites.stream()
                                     .filter(other -> other != car)
@@ -552,7 +531,7 @@ public class PathFinder {
      * 载货查找最佳，使用情景
      *
      * */
-    public static Site findOptimalWithCargo(int[][] grid, Site srcSite, List<Site> tarSites){
+    public static Site findOptimalWithCargo(int[][] grid, Site srcSite, List<Site> tarSites) {
 
         // 并行计算所有车辆的路径
         Map<Site, PathInfo> carPathInfo = tarSites.parallelStream()
@@ -572,7 +551,6 @@ public class PathFinder {
                             }
 
 
-
                             // 检查路径冲突
                             boolean blocked = tarSites.stream()
                                     .filter(other -> other != car)
@@ -605,13 +583,12 @@ public class PathFinder {
     }
 
 
-
     /*
      * 空载查找最佳，使用情景
      * 小车不载货，一个原点（目标点），多个目标点（原点）
      *
      * */
-    public static Site findOptimalWithNoCargo(int[][] grid, Site srcSite, List<Site> tarSites){
+    public static Site findOptimalWithNoCargo(int[][] grid, Site srcSite, List<Site> tarSites) {
 
 
         // 并行计算所有车辆的路径
@@ -625,7 +602,6 @@ public class PathFinder {
                             }
 
 
-
                             // 计算路径
                             List<int[]> path = findShortestPath(grid, startPoint[0], startPoint[1], srcSite.getStartPoint()[0], srcSite.getStartPoint()[1]);
                             if (path == null) {
@@ -634,7 +610,6 @@ public class PathFinder {
                             }
 
 
-
                             // 检查路径冲突
                             boolean blocked = tarSites.stream()
                                     .filter(other -> other != car)
@@ -669,11 +644,10 @@ public class PathFinder {
     }
 
 
-
     /*
-    *
+     *
-    * 同层多车选择最优
+     * 同层多车选择最优
-    * */
+     * */
     /*
      *
      * 同层多车选择最优
@@ -743,9 +717,6 @@ public class PathFinder {
     }
 
 
-
-
-
     public static Site findOptimalCarForDestinationByFull(
             int[][] grid, int endX, int endY, List<Site> cars) {
         // 创建网格副本
@@ -878,7 +849,4 @@ public class PathFinder {
     }
 
 
-
-
-
 }

ruoyi-admin/src/main/java/com/ruoyi/schedule/scheduleTask/srgv/BaseSRgvThread.java

@@ -570,67 +570,7 @@ public class BaseSRgvThread implements Runnable, SRgvThreadInterface {
 //            endSite="07050"+deviceSrgv.getCurrFloor();
 //        }
         //
-        String endBlockAgv = null;
-        DeviceSrgv endBlockDevice = null;
-        // TODO 通过arrByRoadSite 中的站点 查询是否存在其他车的站点，如果存在则生成其他车到避让点的队列
-        if (arrByRoadSite != null && !arrByRoadSite.isEmpty()) {
-            // 将arrByRoadSite转为String类型的Set（统一类型，方便匹配）
-            Set<String> roadSiteSet = arrByRoadSite.keySet().stream()
-                    .filter(Objects::nonNull) // 过滤集合中的null值
-                    .map(Object::toString)    // 转为字符串，适配站点类型
-                    .collect(Collectors.toSet());
 
-            // 2. Stream流式处理deviceSrgvMap
-            Optional<Map.Entry<String, DeviceSrgv>> targetEntry = PlcSrgvRead.deviceSrgvMap.entrySet().stream()
-                    .filter(entry -> entry.getValue() != null) // 过滤other为null的条目
-                    // 条件1：other的当前站点存在于roadSiteSet中
-                    .filter(entry -> roadSiteSet.contains(entry.getValue().getCurrSite()))
-                    // 条件2：设备ID不等于当前entry的key
-                    .filter(entry -> !Objects.equals(Convert.toStr(deviceSrgv.getDeviceId()), entry.getKey()))
-                    .findFirst(); // 找到第一个符合条件的条目（等价于原代码的break）
-
-            // 3. 处理结果（若找到则赋值）
-            if (targetEntry.isPresent()) {
-                endBlockAgv = targetEntry.get().getKey();
-                endBlockDevice = targetEntry.get().getValue();
-            }
-        }
-
-        WcsSrgvQueue wcsSrgvQueueBR = new WcsSrgvQueue();
-        wcsSrgvQueueBR.setAgvId(endBlockAgv);
-        List<WcsSrgvQueue> wcsSrgvQueueBRS = wcsSagvQueueMapper.selectWcsSrgvQueueList(wcsSrgvQueueBR);
-        if (endBlockDevice != null && wcsSrgvQueueBRS.isEmpty()) {
-            // 选择当前层最近且可达的缓存点（避让点）
-            WcsSrgvMap cacheParam = new WcsSrgvMap();
-            cacheParam.setRemark("cache");
-            cacheParam.setTheLayer(floor);
-            List<WcsSrgvMap> cacheMaps = wcsSrgvMapService.selectWcsSrgvMapList(cacheParam);
-            // 生成阻挡车的避让任务（点到点，高优先级，未下发）
-            WcsSrgvQueue taskB = new WcsSrgvQueue();
-            taskB.setGuid(UUID.randomUUID().toString());
-            // 生成纯数字任务号（PLC要求），9 + HHmmss
-            taskB.setTaskNo(String.valueOf(taskCache));
-            taskB.setAgvId(endBlockAgv);
-            taskB.setTaskType("1");      // 点到点移动
-            taskB.setDefault02("4");     // 业务类型标识为点到点
-            taskB.setPriority(9L);       // 高优先级，尽快避让
-            taskB.setIssued("0");        // 未下发
-            taskB.setExecutionStatus("1");// 初始状态
-            taskB.setStartSite(endBlockDevice.getCurrSite());
-            taskB.setCargoPickSite(endBlockDevice.getCurrSite());
-            taskB.setCargoPlaceSite("05020"+endBlockDevice.getCurrFloor());
-            taskB.setEndSite("05020"+endBlockDevice.getCurrFloor());
-            taskB.setTheFloor(floor.toString());
-            taskB.setDefault03(endBlockDevice.getCurrSite());
-            taskB.setDefault07(floor.toString());
-            taskB.setDefault08("1");//避让任务，直接完成
-            taskB.setCreateDate(DateUtil.now());
-            wcsSagvQueueMapper.insertWcsSrgvQueue(taskB);
-            taskCache++;
-            log.info("目标点 {} 被小车 {} 占用，已调度其前往避让点 {}", endSite, endBlockAgv, "02050"+endBlockDevice.getCurrFloor());
-            // 当前车等待下一轮调度（待阻挡车移开后自然可达）
-            return false;
-        }
 
         //处理地图
         int[][] emptyMap = roadCacheService.handleMap(arrByRoadSite, deviceSrgv);
@@ -638,6 +578,72 @@ public class BaseSRgvThread implements Runnable, SRgvThreadInterface {
         //计算最短路径
         List<int[]> shortestPath = PathFinder.findShortestPath(emptyMap, arrByRoadSite.get(startSite).getX(), arrByRoadSite.get(startSite).getY(), arrByRoadSite.get(endSite).getX(), arrByRoadSite.get(endSite).getY());
 
+        if (shortestPath.size() == 1 && shortestPath.get(0)[0] == -1 && shortestPath.get(0)[1] == -1) {
+            shortestPath = null;
+            String endBlockAgv = null;
+            DeviceSrgv endBlockDevice = null;
+            // TODO 通过arrByRoadSite 中的站点 查询是否存在其他车的站点，如果存在则生成其他车到避让点的队列
+            if (arrByRoadSite != null && !arrByRoadSite.isEmpty()) {
+                // 将arrByRoadSite转为String类型的Set（统一类型，方便匹配）
+                Set<String> roadSiteSet = arrByRoadSite.keySet().stream()
+                        .filter(Objects::nonNull) // 过滤集合中的null值
+                        .map(Object::toString)    // 转为字符串，适配站点类型
+                        .collect(Collectors.toSet());
+
+                // 2. Stream流式处理deviceSrgvMap
+                Optional<Map.Entry<String, DeviceSrgv>> targetEntry = PlcSrgvRead.deviceSrgvMap.entrySet().stream()
+                        .filter(entry -> entry.getValue() != null) // 过滤other为null的条目
+                        // 条件1：other的当前站点存在于roadSiteSet中
+                        .filter(entry -> roadSiteSet.contains(entry.getValue().getCurrSite()))
+                        // 条件2：设备ID不等于当前entry的key
+                        .filter(entry -> !Objects.equals(Convert.toStr(deviceSrgv.getDeviceId()), entry.getKey()))
+                        .findFirst(); // 找到第一个符合条件的条目（等价于原代码的break）
+
+                // 3. 处理结果（若找到则赋值）
+                if (targetEntry.isPresent()) {
+                    endBlockAgv = targetEntry.get().getKey();
+                    endBlockDevice = targetEntry.get().getValue();
+                }
+            }
+
+            WcsSrgvQueue wcsSrgvQueueBR = new WcsSrgvQueue();
+            wcsSrgvQueueBR.setAgvId(endBlockAgv);
+            List<WcsSrgvQueue> wcsSrgvQueueBRS = wcsSagvQueueMapper.selectWcsSrgvQueueList(wcsSrgvQueueBR);
+            if (endBlockDevice != null && wcsSrgvQueueBRS.isEmpty()) {
+                // 选择当前层最近且可达的缓存点（避让点）
+                WcsSrgvMap cacheParam = new WcsSrgvMap();
+                cacheParam.setRemark("cache");
+                cacheParam.setTheLayer(floor);
+                List<WcsSrgvMap> cacheMaps = wcsSrgvMapService.selectWcsSrgvMapList(cacheParam);
+                // 生成阻挡车的避让任务（点到点，高优先级，未下发）
+                WcsSrgvQueue taskB = new WcsSrgvQueue();
+                taskB.setGuid(UUID.randomUUID().toString());
+                // 生成纯数字任务号（PLC要求），9 + HHmmss
+                taskB.setTaskNo(String.valueOf(taskCache));
+                taskB.setAgvId(endBlockAgv);
+                taskB.setTaskType("1");      // 点到点移动
+                taskB.setDefault02("4");     // 业务类型标识为点到点
+                taskB.setPriority(9L);       // 高优先级，尽快避让
+                taskB.setIssued("0");        // 未下发
+                taskB.setExecutionStatus("1");// 初始状态
+                taskB.setStartSite(endBlockDevice.getCurrSite());
+                taskB.setCargoPickSite(endBlockDevice.getCurrSite());
+                taskB.setCargoPlaceSite("05020" + endBlockDevice.getCurrFloor());
+                taskB.setEndSite("05020" + endBlockDevice.getCurrFloor());
+                taskB.setTheFloor(floor.toString());
+                taskB.setDefault03(endBlockDevice.getCurrSite());
+                taskB.setDefault07(floor.toString());
+                taskB.setDefault08("1");//避让任务，直接完成
+                taskB.setCreateDate(DateUtil.now());
+                wcsSagvQueueMapper.insertWcsSrgvQueue(taskB);
+                taskCache++;
+                log.info("目标点 {" + deviceSrgv.getCurrSite() + "} 被小车 {" + deviceSrgv.getDeviceId() + "} 占用，已调度其前往避让点 {}", endSite, endBlockAgv, "02050" + endBlockDevice.getCurrFloor());
+                // 当前车等待下一轮调度（待阻挡车移开后自然可达）
+                return false;
+            }
+        }
+
+
         //判断是否能规划出最短路径
         if (shortestPath == null || shortestPath.isEmpty()) {
             //判断是否是1层 空车取货 或者 满车放货
@@ -697,9 +703,11 @@ public class BaseSRgvThread implements Runnable, SRgvThreadInterface {
             return false;
         }
 
+
         //获取拐点路径
         List<int[]> trun = PathFinder.extractInflectionPoints2(shortestPath);
 
+
         // 路径点拼接
         StringBuilder pathBuilder = new StringBuilder();
         if (!shortestPath.isEmpty()) {