fix: handle unreachable paths in DijkstraPathPlanning

src/adf_core_python/implement/module/algorithm/dijkstra_path_planning.py

@@ -1,7 +1,8 @@
 from __future__ import annotations
 
 import heapq
-from typing import Optional
+from itertools import count
+from typing import Callable
 
 from rcrscore.entities import Area, Building, EntityID, Road
 
@@ -48,84 +49,54 @@ def calculate(self) -> PathPlanning:
   def get_path(
     self, from_entity_id: EntityID, to_entity_id: EntityID
   ) -> list[EntityID]:
-    # ダイクストラ法で最短経路を計算
-    queue: list[tuple[float, EntityID]] = []
-    heapq.heappush(queue, (0, from_entity_id))
-    distance: dict[EntityID, float] = {from_entity_id: 0}
-    previous: dict[EntityID, Optional[EntityID]] = {from_entity_id: None}
-
-    while queue:
-      current_distance, current_node = heapq.heappop(queue)
-      if current_node == to_entity_id:
-        break
-
-      self._logger.info(
-        f"current_node: {current_node}, current_entity: {self._world_info.get_entity(current_node)}"
-      )
-
-      for neighbor, weight in self.graph[current_node]:
-        new_distance = current_distance + weight
-        if neighbor not in distance or new_distance < distance[neighbor]:
-          distance[neighbor] = new_distance
-          heapq.heappush(queue, (new_distance, neighbor))
-          previous[neighbor] = current_node
-
-    path: list[EntityID] = []
-    current_path_node: Optional[EntityID] = to_entity_id
-    while current_path_node is not None:
-      path.append(current_path_node)
-      current_path_node = previous.get(current_path_node)
-
-    return path[::-1]
+    path, _ = self._shortest_path(from_entity_id, lambda node: node == to_entity_id)
+    return path
 
   def get_path_to_multiple_destinations(
     self, from_entity_id: EntityID, destination_entity_ids: set[EntityID]
   ) -> list[EntityID]:
-    open_list = [from_entity_id]
-    ancestors = {from_entity_id: from_entity_id}
-    found = False
-    next_node = None
-
-    while open_list and not found:
-      next_node = open_list.pop(0)
-      if self.is_goal(next_node, destination_entity_ids):
-        found = True
-        break
-
-      neighbors = self.graph.get(next_node, [])
-      if not neighbors:
-        continue
-
-      for neighbor, _ in neighbors:
-        if self.is_goal(neighbor, destination_entity_ids):
-          ancestors[neighbor] = next_node
-          next_node = neighbor
-          found = True
-          break
-        elif neighbor not in ancestors:
-          open_list.append(neighbor)
-          ancestors[neighbor] = next_node
-
-    if not found:
-      return []
-
-    path: list[EntityID] = []
-    current = next_node
-    while current != from_entity_id:
-      if current is None:
-        raise RuntimeError("Found a node with no ancestor! Something is broken.")
-      path.insert(0, current)
-      current = ancestors.get(current)
-    path.insert(0, from_entity_id)
-
+    path, _ = self._shortest_path(
+      from_entity_id, lambda node: node in destination_entity_ids
+    )
     return path
 
   def is_goal(self, entity_id: EntityID, target_ids: set[EntityID]) -> bool:
     return entity_id in target_ids
 
   def get_distance(self, from_entity_id: EntityID, to_entity_id: EntityID) -> float:
-    path = self.get_path(from_entity_id, to_entity_id)
-    distance = 0.0
-    for i in range(len(path) - 1):
-      distance += self._world_info.get_distance(path[i], path[i + 1])
+    _, distance = self._shortest_path(from_entity_id, lambda node: node == to_entity_id)
     return distance
+
+  def _shortest_path(
+    self, source: EntityID, is_target: Callable[[EntityID], bool]
+  ) -> tuple[list[EntityID], float]:
+    if source not in self.graph:
+      return [], float("inf")
+
+    sequence = count()
+    queue: list[tuple[float, int, EntityID]] = [(0.0, next(sequence), source)]
+    distances = {source: 0.0}
+    previous: dict[EntityID, EntityID] = {}
+
+    while queue:
+      current_distance, _, current = heapq.heappop(queue)
+      if current_distance > distances[current]:
+        continue
+      if is_target(current):
+        path = [current]
+        while current in previous:
+          current = previous[current]
+          path.append(current)
+        path.reverse()
+        return path, current_distance
+
+      for neighbor, weight in self.graph.get(current, []):
+        if neighbor not in self.graph:
+          continue
+        new_distance = current_distance + weight
+        if new_distance < distances.get(neighbor, float("inf")):
+          distances[neighbor] = new_distance
+          previous[neighbor] = current
+          heapq.heappush(queue, (new_distance, next(sequence), neighbor))
+
+    return [], float("inf")