feat: add get_entity_position method and fix typo in get_blockades method

Author: shima004

adf_core_python/core/agent/info/world_info.py

@@ -3,6 +3,7 @@
 from rcrs_core.entities.area import Area
 from rcrs_core.entities.blockade import Blockade
 from rcrs_core.entities.entity import Entity
+from rcrs_core.entities.human import Human
 from rcrs_core.worldmodel.changeSet import ChangeSet
 from rcrs_core.worldmodel.entityID import EntityID
 from rcrs_core.worldmodel.worldmodel import WorldModel
@@ -145,6 +146,36 @@ def get_distance(self, entity_id1: EntityID, entity_id2: EntityID) -> float:
 
         return distance
 
+    def get_entity_position(self, entity_id: EntityID) -> EntityID:
+        """
+        Get the entity position
+
+        Parameters
+        ----------
+        entity_id : EntityID
+            Entity ID
+
+        Returns
+        -------
+        EntityID
+            Entity position
+
+        Raises
+        ------
+        ValueError
+            If the entity is invalid
+        """
+        entity = self.get_entity(entity_id)
+        if entity is None:
+            raise ValueError(f"Invalid entity: entity_id={entity_id}, entity={entity}")
+        if isinstance(entity, Area):
+            return entity.get_id()
+        if isinstance(entity, Human):
+            return entity.get_position()
+        if isinstance(entity, Blockade):
+            return entity.get_position()
+        raise ValueError(f"Invalid entity type: entity_id={entity_id}, entity={entity}")
+
     def get_change_set(self) -> ChangeSet:
         """
         Get the change set
@@ -156,7 +187,7 @@ def get_change_set(self) -> ChangeSet:
         """
         return self._change_set
 
-    def get_bloackades(self, area: Area) -> set[Blockade]:
+    def get_blockades(self, area: Area) -> set[Blockade]:
         """
         Get the blockades in the area
 
@@ -165,12 +196,12 @@ def get_bloackades(self, area: Area) -> set[Blockade]:
         ChangeSet
             Blockade
         """
-        bloakcades = set()
+        blockades = set()
         for blockade_entity_id in area.get_blockades():
-            bloackde_entity = self.get_entity(blockade_entity_id)
-            if isinstance(bloackde_entity, Blockade):
-                bloakcades.add(cast(Blockade, bloackde_entity))
-        return bloakcades
+            blockades_entity = self.get_entity(blockade_entity_id)
+            if isinstance(blockades_entity, Blockade):
+                blockades.add(cast(Blockade, blockades_entity))
+        return blockades
 
     def add_entity(self, entity: Entity) -> None:
         """

adf_core_python/implement/action/default_extend_action_clear.py

@@ -432,7 +432,7 @@ def _get_intersect_edge_action(
             start_x = int(agent_x + vector[0])
             start_y = int(agent_y + vector[1])
 
-            for blockade in self.world_info.get_bloackades(road):
+            for blockade in self.world_info.get_blockades(road):
                 if self._is_intersecting_blockade(
                     start_x, start_y, bp_x, bp_y, blockade
                 ):
@@ -563,7 +563,7 @@ def _get_area_clear_action(
         if road.get_blockades() == []:
             return None
 
-        blockades = set(self.world_info.get_bloackades(road))
+        blockades = set(self.world_info.get_blockades(road))
         min_distance = sys.float_info.max
         clear_blockade: Optional[Blockade] = None
         for blockade in blockades:
@@ -662,7 +662,7 @@ def _get_neighbour_position_action(
                 start_x = int(agent_x + vector[0])
                 start_y = int(agent_y + vector[1])
 
-                for blockade in self.world_info.get_bloackades(road):
+                for blockade in self.world_info.get_blockades(road):
                     if self._is_intersecting_blockade(
                         start_x, start_y, mid_x, mid_y, blockade
                     ):
@@ -715,7 +715,7 @@ def _get_neighbour_position_action(
             min_point_distance = sys.float_info.max
             clear_x = 0
             clear_y = 0
-            for blockade in self.world_info.get_bloackades(road):
+            for blockade in self.world_info.get_blockades(road):
                 apexes = blockade.get_apexes()
                 for i in range(0, len(apexes) - 2, 2):
                     distance = self._get_distance(