feat: Add support for retrieving paths to multiple destinations in path planning algorithms

Author: shima004

adf_core_python/core/component/module/algorithm/path_planning.py

@@ -36,6 +36,12 @@ def get_path(
     ) -> list[EntityID]:
         pass
 
+    @abstractmethod
+    def get_path_to_multiple_destinations(
+        self, from_entity_id: EntityID, destination_entity_ids: list[EntityID]
+    ) -> list[EntityID]:
+        pass
+
     @abstractmethod
     def get_distance(self, from_entity_id: EntityID, to_entity_id: EntityID) -> float:
         pass

adf_core_python/core/gateway/component/module/algorithm/gateway_path_planning.py

@@ -14,8 +14,8 @@
     from adf_core_python.core.agent.communication.message_manager import MessageManager
     from adf_core_python.core.agent.develop.develop_data import DevelopData
     from adf_core_python.core.agent.info.agent_info import AgentInfo
-    from adf_core_python.core.agent.info.world_info import WorldInfo
     from adf_core_python.core.agent.info.scenario_info import ScenarioInfo
+    from adf_core_python.core.agent.info.world_info import WorldInfo
     from adf_core_python.core.agent.module.module_manager import ModuleManager
     from adf_core_python.core.agent.precompute.precompute_data import PrecomputeData
     from adf_core_python.core.gateway.gateway_module import GatewayModule
@@ -77,6 +77,25 @@ def get_path(
             entity_ids.append(EntityID(entity_id))
         return entity_ids
 
+    def get_path_to_multiple_destinations(
+        self, from_entity_id: EntityID, destination_entity_ids: list[EntityID]
+    ) -> list[EntityID]:
+        arguments: dict[str, str] = {
+            "From": str(from_entity_id.get_value()),
+            "Destinations": json.dumps(
+                [entity_id.get_value() for entity_id in destination_entity_ids]
+            ),
+        }
+        result = self._gateway_module.execute(
+            "getResult(EntityID, List[EntityID])", arguments
+        )
+        json_str = result.get_value_or_default("Result", "[]")
+        raw_entity_ids: list[int] = json.loads(json_str)
+        entity_ids: list[EntityID] = []
+        for entity_id in raw_entity_ids:
+            entity_ids.append(EntityID(entity_id))
+        return entity_ids
+
     def get_distance(self, from_entity_id: EntityID, to_entity_id: EntityID) -> float:
         arguments: dict[str, str] = {
             "From": str(from_entity_id.get_value()),

adf_core_python/implement/module/algorithm/a_star_path_planning.py

@@ -69,6 +69,11 @@ def get_path(
 
         return []
 
+    def get_path_to_multiple_destinations(
+        self, from_entity_id: EntityID, destination_entity_ids: list[EntityID]
+    ) -> list[EntityID]:
+        raise NotImplementedError
+
     def heuristic(self, from_entity_id: EntityID, to_entity_id: EntityID) -> float:
         # Implement a heuristic function, for example, Euclidean distance
         return self._world_info.get_distance(from_entity_id, to_entity_id)

adf_core_python/implement/module/algorithm/dijkstra_path_planning.py

@@ -0,0 +1,135 @@
+from __future__ import annotations
+
+import heapq
+from typing import Dict, List, Tuple
+
+from rcrs_core.entities.area import Area
+from rcrs_core.entities.entity import Entity
+from rcrs_core.worldmodel.entityID import EntityID
+
+from adf_core_python.core.agent.develop.develop_data import DevelopData
+from adf_core_python.core.agent.info.agent_info import AgentInfo
+from adf_core_python.core.agent.info.scenario_info import ScenarioInfo
+from adf_core_python.core.agent.info.world_info import WorldInfo
+from adf_core_python.core.agent.module.module_manager import ModuleManager
+from adf_core_python.core.component.module.algorithm.path_planning import PathPlanning
+
+
+class DijkstraPathPlanning(PathPlanning):
+    def __init__(
+        self,
+        agent_info: AgentInfo,
+        world_info: WorldInfo,
+        scenario_info: ScenarioInfo,
+        module_manager: ModuleManager,
+        develop_data: DevelopData,
+    ) -> None:
+        super().__init__(
+            agent_info, world_info, scenario_info, module_manager, develop_data
+        )
+        self.graph: Dict[EntityID, List[Tuple[EntityID, float]]] = {}
+        # グラフの構築
+        for area in self._world_info.get_entities_of_types([Area]):
+            if not isinstance(area, Area):
+                continue
+            if (neighbors := area.get_neighbours()) is None:
+                continue
+            area_id = area.get_id()
+            self.graph[area_id] = [
+                (
+                    neighbor,
+                    self._world_info.get_distance(area_id, entity_id2=neighbor),
+                )
+                for neighbor in neighbors
+                if neighbor.get_value() != 0
+            ]
+
+    def calculate(self) -> PathPlanning:
+        return self
+
+    def get_path(
+        self, from_entity_id: EntityID, to_entity_id: EntityID
+    ) -> List[EntityID]:
+        # ダイクストラ法で最短経路を計算
+        queue = []
+        heapq.heappush(queue, (0, from_entity_id))
+        distance = {from_entity_id: 0}
+        previous = {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 = []
+        current_node = to_entity_id
+        while current_node is not None:
+            path.append(current_node)
+            current_node = previous[current_node]
+
+        return path[::-1]
+
+    def get_path_to_multiple_destinations(
+        self, from_entity_id: EntityID, destination_entity_ids: List[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 = []
+        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)
+
+        return path
+
+    def is_goal(self, entity_id: EntityID, target_ids: List[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])
+        return distance

java/lib/src/main/java/adf_core_python/gateway/mapper/module/algorithm/PathPlanningMapper.java

@@ -1,18 +1,19 @@
 package adf_core_python.gateway.mapper.module.algorithm;
 
+import java.util.Collection;
+import java.util.List;
+
+import com.fasterxml.jackson.core.JsonProcessingException;
+import com.fasterxml.jackson.core.type.TypeReference;
+import com.fasterxml.jackson.databind.ObjectMapper;
+
 import adf.core.agent.communication.MessageManager;
 import adf_core_python.agent.precompute.PrecomputeData;
 import adf_core_python.component.module.algorithm.PathPlanning;
 import adf_core_python.gateway.mapper.module.AbstractModuleMapper;
-import com.fasterxml.jackson.core.JsonProcessingException;
-import com.fasterxml.jackson.core.type.TypeReference;
-import com.fasterxml.jackson.databind.ObjectMapper;
 import rescuecore2.config.Config;
 import rescuecore2.worldmodel.EntityID;
 
-import java.util.Collection;
-import java.util.List;
-
 public class PathPlanningMapper extends AbstractModuleMapper {
     public PathPlanningMapper(PathPlanning pathPlanning, PrecomputeData precomputeData, MessageManager messageManager) {
         super(pathPlanning, precomputeData, messageManager);
@@ -42,10 +43,24 @@ public Config execMethod(String methodName, Config arguments) {
             result = execGetDistance();
         }
         if (methodName.equals("getDistance(EntityID, EntityID)")) {
-            result = execGetDistance(new EntityID(arguments.getIntValue("From")), new EntityID(arguments.getIntValue("Dest")));
+            result = execGetDistance(new EntityID(arguments.getIntValue("From")),
+                    new EntityID(arguments.getIntValue("Dest")));
         }
         if (methodName.equals("getResult(EntityID, EntityID)")) {
-            result = execGetResult(new EntityID(arguments.getIntValue("From")), new EntityID(arguments.getIntValue("Dest")));
+            result = execGetResult(new EntityID(arguments.getIntValue("From")),
+                    new EntityID(arguments.getIntValue("Dest")));
+        }
+        if (methodName.equals("getResult(EntityID, List[EntityID])")) {
+            ObjectMapper objectMapper = new ObjectMapper();
+            Collection<EntityID> destinations;
+            try {
+                destinations = objectMapper.readValue(arguments.getValue("Destinations"),
+                        new TypeReference<List<EntityID>>() {
+                        });
+            } catch (JsonProcessingException e) {
+                throw new RuntimeException(e);
+            }
+            result = execGetResult(new EntityID(arguments.getIntValue("From")), destinations);
         }
         return result;
     }
@@ -105,4 +120,21 @@ private Config execGetResult(EntityID from, EntityID dest) {
         result.setValue("Result", String.valueOf(jsonStr));
         return result;
     }
+
+    private Config execGetResult(EntityID from, Collection<EntityID> destinations) {
+        PathPlanning pathPlanning = (PathPlanning) abstractModule;
+        pathPlanning.setFrom(from);
+        pathPlanning.setDestination(destinations);
+        List<EntityID> entityIDs = pathPlanning.getResult();
+        Config result = new Config();
+        ObjectMapper objectMapper = new ObjectMapper();
+        String jsonStr;
+        try {
+            jsonStr = objectMapper.writeValueAsString(entityIDs.stream().map(EntityID::getValue).toArray());
+        } catch (JsonProcessingException e) {
+            throw new RuntimeException(e);
+        }
+        result.setValue("Result", String.valueOf(jsonStr));
+        return result;
+    }
 }