def goInstruction(ent: int, args: List[str], script: scriptComponent.Script,
event_store: FilterStore) -> scriptComponent.States:
if len(args) == 1:
payload = GotoPoiPayload(ent, args[0])
new_event = EVENT(GotoPoiEventTag, payload)
elif len(args) == 2:
payload = GotoPosPayload(ent, [float(args[0]), float(args[1])])
new_event = EVENT(GotoPosEventTag, payload)
else:
raise Exception('GO instruction failed. Go <poi> OR Go <x> <y>')
event_store.put(new_event)
# Needs to block the script
script.state = scriptComponent.States.BLOCKED
script.expecting.append(EndOfPathTag)
return script.state
def send_end_of_approximation_event(self, ent, event_store, now):
entity_pos = self.world.component_for_entity(ent, Position).center
history = self.world.component_for_entity(ent, ApproximationHistory)
history.entity_final_approx_pos = (entity_pos[0], entity_pos[1])
history.approximated = True
end_of_approximation_event = EVENT(EndOfApproximationTag,
EndOfApproximationPayload(ent, now))
event_store.put(end_of_approximation_event)
def add_goto_poi_event(self, drawio_id, poi_tag):
"""
Adds an event for the robot to go to a POI passed as a parameter.
- drawio_id: property id from an element of the drawio file.
- poi_tag: the poi_tag property of a POI.
"""
entity_id = self.cast_id(drawio_id)
payload = GotoPoiPayload(entity_id, poi_tag)
new_event = EVENT(GotoPoiEventTag, payload)
event_store = self.simulation.KWARGS['EVENT_STORE']
event_store.put(new_event)
def add_goto_position_event(self, drawio_id, position):
"""
Adds an event for the robot to go to a position passed as a parameter.
- drawio_id: property id from an element of the drawio file.
- position: a tuple (x, y) or array [x, y] with x-axis value and y-axis value.
"""
entity_id = self.cast_id(drawio_id)
x = position[0]
y = position[1]
payload = GotoPosPayload(entity_id, [x, y])
new_event = EVENT(GotoPosEventTag, payload)
event_store = self.simulation.KWARGS['EVENT_STORE']
event_store.put(new_event)
def process(self, kwargs: SystemArgs):
event_store: FilterStore = kwargs.get('EVENT_STORE', None)
env = kwargs.get('ENV', None)
for ent, (pos, path,
vel) in self.world.get_components(Position, Path, Velocity):
# self.logger.debug(f"[{env.now}] Processing {ent}")
point = path.points[path.curr_point]
pos_center = pos.center
if pos_center[0] == point[0] and pos_center[1] == point[1]:
path.curr_point += 1
if path.curr_point == len(path.points):
# end of path
vel.x = 0
vel.y = 0
self.logger.debug(
f"Removed Path component from {ent} (pos={pos.center}). Last point of path is {path.points[-1]}"
)
# Adds an EndOfPath event, in case anyone is listening
end_of_path = EVENT(
EndOfPathTag,
EndOfPathPayload(ent, str(env.now), path=path.points))
# self.logger.debug(f'[{env.now}] PathProcessor adding EndOfPath event {end_of_path}')
event_store.put(end_of_path)
pos.changed = False
self.world.remove_component(ent, Path)
if self.world.has_component(end_of_path.payload.ent,
ApproximationHistory):
history = self.world.component_for_entity(
end_of_path.payload.ent, ApproximationHistory)
if not history.approximated:
self.send_end_of_approximation_event(
end_of_path.payload.ent, event_store,
str(env.now))
return
else:
dx = point[0] - pos_center[0]
if dx > 0:
vel.x = min(path.speed, dx)
else:
vel.x = max(-path.speed, dx)
dy = point[1] - pos_center[1]
if dy > 0:
vel.y = min(path.speed, dy)
else:
vel.y = max(-path.speed, dy)
def move_to_detected_target(entity, target):
target_position = _WORLD.component_for_entity(target, Position)
destiny_position = target_position.center
if _WORLD.has_component(entity, ApproximationHistory):
return
history = ApproximationHistory(target)
history.destiny_position = destiny_position
_WORLD.add_component(entity, history)
payload = GotoPosPayload(entity, destiny_position)
new_event = EVENT(GotoPosEventTag, payload)
_EVENT_STORE.put(new_event)
script = _WORLD.component_for_entity(entity, Script)
script.expecting.append(EndOfApproximationTag)
def process(kwargs: SystemArgs):
logger = logging.getLogger(__name__)
env = kwargs.get('ENV', None)
world = kwargs.get('WORLD', None)
if env is None:
raise Exception("Can't find env")
# Local ref most used variables
get_components = world.get_components
sleep = env.timeout
total = timedelta()
runs = 0
while True:
start = datetime.now()
for ent, (pos, vel, sensor) in get_components(Position, Velocity, sensor_type):
# logger.debug(f'Analysing ent {ent}')
center_x, center_y = pos.center
sensor_range = sensor.range
points = [
(center_x - sensor_range, center_y - sensor_range),
(center_x + sensor_range, center_y - sensor_range),
(center_x + sensor_range, center_y + sensor_range),
(center_x - sensor_range, center_y + sensor_range)
]
col = Collidable([(pos.center, points)])
closeEntities = []
for otherEnt, (otherCol, otherPos) in get_components(Collidable, Position):
if ent == otherEnt:
continue
for s1 in otherCol.shapes:
if collide(col.shapes[0], s1):
closeEntities.append(CloseEntity(otherEnt, otherPos))
break
if closeEntities:
event = EVENT('SensorEvent', SensorPayload(ent, pos, vel, closeEntities))
sensor.reply_channel.put(event)
yield sleep(frequency)
end = datetime.now()
runs += 1
total += end - start
if runs % 50 == 0:
logger.debug(f'runs: {runs}; total: {total}; avg = {total / runs}')
def process(self, kwargs: SystemArgs):
# start = datetime.now()
logger = logging.getLogger(__name__)
eventStore = kwargs.get('EVENT_STORE', None)
all_collidables = self.world.get_components(Collidable, Position)
for ent, (col, pos,
vel) in self.world.get_components(Collidable, Position,
Velocity):
# update the position of the shape
for shape in col.shapes:
shape.pos = Vector(*pos.center)
shape.angle = pos.angle
# self.logger.debug(f'Entity {ent} - Shapes = {col.shapes}')
# check for colision
ents_to_check = filter(
lambda ent_and_components: ent_and_components[1][1].sector in
pos.adjacent_sectors, all_collidables)
for otherEnt, (otherCol, otherPos) in ents_to_check:
if otherEnt == ent:
continue
if otherPos.movable:
for shape in otherCol.shapes:
shape.pos = Vector(*otherPos.center)
shape.angle = otherPos.angle
if self.checkCollide(col.shapes, otherCol.shapes):
# Remove velocity from current entity
vel.x = 0
vel.y = 0
vel.alpha = 0
if eventStore:
# if col.event_tag == 'genericCollision':
# self.logger.debug(choice(COLORS) + f'Collision! {ent} - {otherEnt}')
event = EVENT(col.event_tag,
CollisionPayload(ent, otherEnt))
# self.logger.debug(f'Firing event ent --> otherEnt: {event}')
eventStore.put(event)
def process(kwargs: SystemArgs):
# Init
__event_store = kwargs.get('EVENT_STORE', None)
__world: World = kwargs.get('WORLD', None)
env: Environment = kwargs.get('ENV', None)
if __event_store is None:
raise Exception("Can't find eventStore")
# On the first run, we put all the scripts in the world in the event queue
for ent, Script in __world.get_component(scriptComponent.Script):
payload = ExecutePayload(ent=ent)
new_event = EVENT(ExecuteInstructionTag, payload)
__event_store.put(new_event)
# Now we keep checking for pending events and executing them
while True:
ev = yield __event_store.get(lambda e: e.type in watchlist or type(e) == ERROR)
payload = ev.payload
if type(ev) == ERROR:
# Here we handle errors that occurred in the processing of some entity's script
script = __world.component_for_entity(ev.ent, scriptComponent.Script)
error_handlers = script.error_handlers
handler = error_handlers.get(ev.type, None)
if handler is None:
handler = error_handlers.get(script.default_error_tag, None)
if handler is not None:
script.logs.append(f'[{env.now}] Error Received. {ev}\nHandler for the error above was found: {handler}')
handler(ev.payload, kwargs)
else:
logger.error(f'[{env.now}] No handler for error {ERROR} was found')
script.logs.append(f'Received error {ERROR}, no handler was found.')
else:
try:
script = __world.component_for_entity(payload.ent, scriptComponent.Script)
except KeyError:
logger.warning(f'[ENV TIME {env.now}] Got event {ev.type} for an entity without Script')
continue
if ev.type == ExecuteInstructionTag:
if script.state != scriptComponent.States.READY:
logger.warning(f'Request to execute script not ready')
script.logs.append(
f'[{env.now}] Request to execute. Script not in READY state. (Curr state is {script.state})'
)
i_type, *args = script.instructions[script.curr_instruction].split(' ')
next_state: scriptComponent.States
if i_type in instruction_set:
next_state = instruction_set[i_type](payload.ent, args, script, __event_store)
script.logs.append(f'[{env.now}] Execute instruction {i_type} {args}. Current state {next_state}')
else:
logger.error(f'Unknown instruction {i_type}')
script.logs.append(f'[{env.now}] Executing instruction {i_type} Failed. Unknown instruction.')
next_state = scriptComponent.States.READY
if next_state == scriptComponent.States.READY:
payload = ExecutePayload(payload.ent)
new_event = EVENT(ExecuteInstructionTag, payload)
__event_store.put(new_event)
else:
ent = payload.ent
logger.debug(f'[{env.now}] Got event {ev.type} for ent {ent} - {ev.payload}')
if ev.type not in script.expecting:
logger.warning(f'Was not expecting {ev.type}')
else:
script.expecting.remove(ev.type)
if not script.expecting:
r = unblockEntity(script)
if r == scriptComponent.States.READY:
payload = ExecutePayload(ent=ent)
new_event = EVENT(ExecuteInstructionTag, payload)
__event_store.put(new_event)