11from typing import Optional , cast
22
33from rcrs_core .connection .URN import Entity as EntityURN
4+ from rcrs_core .entities .civilian import Civilian
45from rcrs_core .entities .entity import Entity
56from rcrs_core .entities .human import Human
67from rcrs_core .worldmodel .entityID import EntityID
1213from adf_core_python .core .agent .module .module_manager import ModuleManager
1314from adf_core_python .core .component .module .algorithm .clustering import Clustering
1415from adf_core_python .core .component .module .complex .human_detector import HumanDetector
16+ from adf_core_python .core .logger .logger import get_agent_logger
1517
1618
1719class DefaultHumanDetector (HumanDetector ):
@@ -36,6 +38,10 @@ def __init__(
3638 self .register_sub_module (self ._clustering )
3739
3840 self ._result : Optional [EntityID ] = None
41+ self ._logger = get_agent_logger (
42+ f"{ self .__class__ .__module__ } .{ self .__class__ .__qualname__ } " ,
43+ self ._agent_info ,
44+ )
3945
4046 def calculate (self ) -> HumanDetector :
4147 transport_human : Optional [Human ] = self ._agent_info .some_one_on_board ()
@@ -44,8 +50,11 @@ def calculate(self) -> HumanDetector:
4450 return self
4551
4652 if self ._result is not None :
47- if self ._is_valid_human (self ._result ):
48- self ._result = self ._select_target ()
53+ if not self ._is_valid_human (self ._result ):
54+ self ._result = None
55+
56+ if self ._result is None :
57+ self ._result = self ._select_target ()
4958
5059 return self
5160
@@ -59,28 +68,54 @@ def _select_target(self) -> Optional[EntityID]:
5968 cluster_entities : list [Entity ] = self ._clustering .get_cluster_entities (
6069 cluster_index
6170 )
71+
72+ self ._logger .debug (
73+ f"cluster_entities: { [str (entity .get_id ()) for entity in cluster_entities ]} "
74+ )
75+
6276 cluster_valid_human_entities : list [Entity ] = [
6377 entity
6478 for entity in cluster_entities
65- if self ._is_valid_human (entity .get_id ())
79+ if self ._is_valid_human (entity .get_id ()) and isinstance ( entity , Civilian )
6680 ]
67- if len (cluster_valid_human_entities ) == 0 :
68- return None
81+ if len (cluster_valid_human_entities ) != 0 :
82+ nearest_human_entity : Optional [Entity ] = cluster_valid_human_entities [0 ]
83+ nearest_distance : float = self ._world_info .get_distance (
84+ self ._agent_info .get_entity_id (),
85+ nearest_human_entity .get_id (),
86+ )
87+ for entity in cluster_valid_human_entities :
88+ distance : float = self ._world_info .get_distance (
89+ self ._agent_info .get_entity_id (),
90+ entity .get_id (),
91+ )
92+ if distance < nearest_distance :
93+ nearest_distance = distance
94+ nearest_human_entity = entity
95+ return nearest_human_entity .get_id ()
6996
70- nearest_human_entity : Optional [Entity ] = None
71- nearest_distance : float = 10 ** 10
72- for entity in cluster_valid_human_entities :
73- distance : float = self ._world_info .get_distance (
97+ world_valid_human_entities : list [Entity ] = [
98+ entity
99+ for entity in self ._world_info .get_entities_of_types ([Civilian ])
100+ if self ._is_valid_human (entity .get_id ())
101+ ]
102+ if len (world_valid_human_entities ) != 0 :
103+ nearest_human_entity : Optional [Entity ] = world_valid_human_entities [0 ]
104+ nearest_distance : float = self ._world_info .get_distance (
74105 self ._agent_info .get_entity_id (),
75- entity .get_id (),
106+ nearest_human_entity .get_id (),
76107 )
77- if distance < nearest_distance :
78- nearest_distance = distance
79- nearest_human_entity = entity
108+ for entity in world_valid_human_entities :
109+ distance : float = self ._world_info .get_distance (
110+ self ._agent_info .get_entity_id (),
111+ entity .get_id (),
112+ )
113+ if distance < nearest_distance :
114+ nearest_distance = distance
115+ nearest_human_entity = entity
116+ return nearest_human_entity .get_id ()
80117
81- return (
82- nearest_human_entity .get_id () if nearest_human_entity is not None else None
83- )
118+ return None
84119
85120 def _is_valid_human (self , target_entity_id : EntityID ) -> bool :
86121 target : Optional [Entity ] = self ._world_info .get_entity (target_entity_id )
@@ -94,6 +129,9 @@ def _is_valid_human(self, target_entity_id: EntityID) -> bool:
94129 buriedness : Optional [int ] = target .get_buriedness ()
95130 if buriedness is None or buriedness > 0 :
96131 return False
132+ damage : Optional [int ] = target .get_damage ()
133+ if damage is None or damage == 0 :
134+ return False
97135 position_entity_id : Optional [EntityID ] = target .get_position ()
98136 if position_entity_id is None :
99137 return False
0 commit comments