From 3d7175dde14f77e57cbaf38cceed289e0f7b0e6c Mon Sep 17 00:00:00 2001
From: luoyifan <luoyifan@qq.com>
Date: Thu, 3 Jul 2025 19:44:42 +0800
Subject: [PATCH] PathNotFoundException

---
 .../java/com/galaxis/rcs/plan/path/AStarPathPlanner.java  |  5 ++++-
 .../com/galaxis/rcs/plan/path/PathNotFoundException.java  | 10 ++++++++++
 .../java/com/yvan/logisticsModel/LogisticsRuntime.java    | 15 ++++++++-------
 .../java/com/yvan/workbench/controller/RcsController.java |  7 +++++++
 4 files changed, 29 insertions(+), 8 deletions(-)
 create mode 100644 servo/src/main/java/com/galaxis/rcs/plan/path/PathNotFoundException.java

diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path/AStarPathPlanner.java b/servo/src/main/java/com/galaxis/rcs/plan/path/AStarPathPlanner.java
index 8347234..05a1d25 100644
--- a/servo/src/main/java/com/galaxis/rcs/plan/path/AStarPathPlanner.java
+++ b/servo/src/main/java/com/galaxis/rcs/plan/path/AStarPathPlanner.java
@@ -130,7 +130,10 @@ public class AStarPathPlanner {
                 // }
             }
         }
-        return Collections.emptyList();
+
+        // 如果找不到路径，抛出异常
+        throw new PathNotFoundException(startId, endId, startDirection, endDirection);
+        // return Collections.emptyList();
     }
 
     /**
diff --git a/servo/src/main/java/com/galaxis/rcs/plan/path/PathNotFoundException.java b/servo/src/main/java/com/galaxis/rcs/plan/path/PathNotFoundException.java
new file mode 100644
index 0000000..c10e0e3
--- /dev/null
+++ b/servo/src/main/java/com/galaxis/rcs/plan/path/PathNotFoundException.java
@@ -0,0 +1,10 @@
+package com.galaxis.rcs.plan.path;
+
+import com.galaxis.rcs.common.enums.LCCDirection;
+
+public class PathNotFoundException extends RuntimeException {
+    public PathNotFoundException(String startId, String endId, LCCDirection startDir, LCCDirection endDir) {
+        super(String.format("Path not found from %s(%s) to %s(%s)",
+            startId, startDir, endId, endDir));
+    }
+}
diff --git a/servo/src/main/java/com/yvan/logisticsModel/LogisticsRuntime.java b/servo/src/main/java/com/yvan/logisticsModel/LogisticsRuntime.java
index db6ec2d..820de2d 100644
--- a/servo/src/main/java/com/yvan/logisticsModel/LogisticsRuntime.java
+++ b/servo/src/main/java/com/yvan/logisticsModel/LogisticsRuntime.java
@@ -132,13 +132,14 @@ public class LogisticsRuntime {
      * 获取当前空闲的执行器列表
      */
     public List<ExecutorItem> getFreeExecutorList() {
-        List<ExecutorItem> freeExecutorList = Lists.newArrayList();
-        for (ExecutorItem executorItem : executorItemMap.values()) {
-            if (executorItem.isFree()) {
-                freeExecutorList.add(executorItem);
-            }
-        }
-        return freeExecutorList;
+        return Lists.newArrayList();
+//        List<ExecutorItem> freeExecutorList = Lists.newArrayList();
+//        for (ExecutorItem executorItem : executorItemMap.values()) {
+//            if (executorItem.isFree()) {
+//                freeExecutorList.add(executorItem);
+//            }
+//        }
+//        return freeExecutorList;
     }
 
 
diff --git a/servo/src/main/java/com/yvan/workbench/controller/RcsController.java b/servo/src/main/java/com/yvan/workbench/controller/RcsController.java
index 026d892..1db96a6 100644
--- a/servo/src/main/java/com/yvan/workbench/controller/RcsController.java
+++ b/servo/src/main/java/com/yvan/workbench/controller/RcsController.java
@@ -294,6 +294,9 @@ public class RcsController {
         }
 
         LogisticsRuntime runtime = LogisticsRuntimeService.INSTANCE.getByProjectEnv(projectUUID, envId);
+        if (runtime == null) {
+            return R.fail("Project not running: " + projectUUID + ", envId=" + envId);
+        }
 
         ExecutorItem executorItem = runtime.executorItemMap.get(agvId);
         if (executorItem == null) {
@@ -311,6 +314,10 @@ public class RcsController {
             fromItem = runtime.getStaticItemById(Conv.asString(option.get("forceStartWayPointId")));
         } else {
             fromItem = runtime.getStaticItemByLogicXY(agv.logicX, agv.logicY);
+            if (agv.logicX == 0 && agv.logicY == 0) {
+                // 如果AGV的逻辑坐标为(0, 0)，则尝试从AGV的当前位置获取静态项
+                return R.fail("AGV position init failed");
+            }
         }
         if (option.get("forceStartDirection") != null) {
             fromDirection = LCCDirection.fromString(Conv.asString(option.get("forceStartDirection")));