def _get_target_point(self, world: World, entity: int) -> Optional[Tuple[int, int]]:
if entity is self.__target:
return None
# Prepare data
try:
self_position = world.component_for_entity(entity, Position)
target_position = world.component_for_entity(self.__target, Position)
target_velocity = world.component_for_entity(self.__target, Velocity)
except KeyError:
return None
v_self_position = np.array([self_position.x, self_position.y])
v_target_position = np.array([target_position.x, target_position.y])
v_target_velocity = np.array([target_velocity.x, target_velocity.y])
# Calculate navigation target
v_navigation_target = v_target_position
if self.__distance > 0:
# Adjust for follow distance and go there
target_velocity_size = np.linalg.norm(v_target_velocity)
if target_velocity_size > 0:
v_follow_velocity = v_target_velocity / target_velocity_size
else:
v_follow_velocity = v_target_position - v_self_position
v_follow_velocity = v_follow_velocity / np.linalg.norm(v_follow_velocity)
v_follow_velocity *= -1 * self.__distance
v_navigation_target = v_target_position + v_follow_velocity
else:
# Navigate right to where the target will be next tick
v_navigation_target = v_target_position + v_target_velocity
return (v_navigation_target[0], v_navigation_target[1])
def path_from_mxCell(cell: Element, draw2entity, world: esper.World):
"""Extracts the points of a path from XML."""
logger = logging.getLogger(__name__)
points = []
lastPoint = None
geometry = cell[0]
# Check if there's a source object - 1st point
if 'source' in cell.attrib:
source_ent = draw2entity.get(cell.attrib['source'], None)
if source_ent is None:
raise DependencyNotFound("Path source not found")
source_pos = world.component_for_entity(source_ent[0], Position)
points.append(source_pos.center)
for el in geometry:
if el.tag == 'mxPoint':
if el.attrib['as'] == 'targetPoint':
lastPoint = parse_mxPoint(el)
else:
points.append(parse_mxPoint(el))
elif el.tag == 'Array' and el.attrib['as'] == 'points':
for p in el:
points.append(parse_mxPoint(p))
else:
logger.error(f'Path object has unknown element {el} in cell geometry')
raise Exception('Failed to create Path')
if lastPoint:
points.append(lastPoint)
# Check if there's a target object - Last point
if 'target' in cell.attrib:
target_ent = draw2entity.get(cell.attrib['target'], None)
if target_ent is None:
raise DependencyNotFound("Path target not found")
target_pos = world.component_for_entity(target_ent[0], Position)
points.append(target_pos.center)
return points
def build_object(object, world: esper.World, window_options, draw2entity):
logger = logging.getLogger(__name__)
mxCell = object[0]
mxGeometry = mxCell[0]
# Get X, Y coordinates
x = float(mxGeometry.attrib.get('x', 0))
y = float(mxGeometry.attrib.get('y', 0))
width = float(mxGeometry.attrib.get('width', 0))
height = float(mxGeometry.attrib.get('height', 0))
pos = Position(x, y, 0, width, height)
x += (width // 2)
y += (height // 2)
# Get the Map for simulation or create one
# Entity 1 is the simulation entity
if world.has_component(1, Map):
simulation_map = world.component_for_entity(1, Map)
else:
simulation_map = Map()
world.add_component(1, simulation_map)
# Get POI tag
if 'tag' in object.attrib:
tag = object.attrib['tag']
else:
tag = 'POI_' + str(len(simulation_map.pois))
logger.warning(f'POI ({x}, {y}) with no TAG. Using {tag}')
simulation_map.pois[tag] = (x, y)
# Alternatively display the POI
if object.attrib.get('display', False):
skeleton = Skeleton(object.attrib['id'], mxCell.attrib['style'], tag)
world.create_entity(pos, skeleton)
return {}, [], {}
def build_object(cell, world: World, *args) -> Tuple[dict, list, dict]:
area_name = cell.attrib['label']
max_resources = cell.attrib.get('max_resources', 0)
geometry = cell[0][0]
x = geometry.attrib['x']
y = geometry.attrib['y']
kitchen_area = KitchenArea(area_name, int(max_resources), (float(x), float(y)))
kitchen_layout = world.component_for_entity(1, KitchenLayout)
kitchen_layout.areas[area_name] = kitchen_area
return {}, [], {}
def execute(self, world: World, entity: int):
# Prepare data
try:
self_position = world.component_for_entity(entity, Position)
self_velocity = world.component_for_entity(entity, Velocity)
self_acceleration = world.component_for_entity(entity, Acceleration)
self.__valid = True
except KeyError:
self.__valid = False
return
target_position = self._get_target_point(world, entity)
if target_position is None:
self.__valid = False
return
v_self_position = np.array([self_position.x, self_position.y])
v_self_velocity = np.array([self_velocity.x, self_velocity.y])
v_navigation_target = np.array(target_position)
# Calculate stopping distance
self_speed = np.linalg.norm(v_self_velocity)
stopping_distance = (self_speed**2) / (2 * self_acceleration.max_acceleration)
# What has to actually happen to get to the navigation target?
v_course_correction = v_navigation_target - (v_self_position + v_self_velocity)
#Enfore maximum acceleration
v_course_correction = v_course_correction / np.linalg.norm(v_course_correction)
v_course_correction *= self_acceleration.max_acceleration
if stopping_distance >= np.linalg.norm(v_navigation_target - v_self_position):
v_course_correction = v_course_correction * -1
#Accelerate to correct course
self_acceleration.x = v_course_correction[0]
self_acceleration.y = v_course_correction[1]
def build_object(cell, world: esper.World, window_options, draw2entity):
logger = logging.getLogger(__name__)
mxCell = cell[0]
points = path_from_mxCell(mxCell, draw2entity, world)
if len(points) <= 1:
raise Exception(f'Map path has {len(points)} points. Minimum is 2.')
# Entity 1 is the simulation entity
if world.has_component(1, Map):
simulation_map = world.component_for_entity(1, Map)
else:
simulation_map = Map()
world.add_component(1, simulation_map)
add_nodes_from_points(simulation_map, points)
return {}, [], {}
def _get_target_point(self, world: World, entity: int) -> Optional[Tuple[int, int]]:
self_position = world.component_for_entity(entity, Position)
patrol_target = self.__get_patrol_target()
v_self_position = np.array([self_position.x, self_position.y])
v_patrol_target = np.array([patrol_target[0], patrol_target[1]])
distance = np.linalg.norm(v_patrol_target - v_self_position)
if (distance <= self.distance):
self.current_target_b = not self.current_target_b
patrol_target = self.__get_patrol_target()
v_patrol_target = np.array([patrol_target[0], patrol_target[1]])
return (v_patrol_target[0], v_patrol_target[1])
def change_hover_state(world: World, ent: int, new_state: HoverState):
hover = world.component_for_entity(ent, Hover)
skeleton = world.component_for_entity(ent, Skeleton)
hover.status = new_state
skeleton.style = re.sub(r'fillColor=#[\d\w]{6}', f'fillColor={new_state.value[0]}', skeleton.style)
skeleton.changed = True
def build_simulation_objects(content_root, batch: pyglet.graphics.Batch,
world: esper.World, window_options):
# Create Walls
draw2entity = {}
interactive = {}
objects = []
windowSize = window_options[0]
for cell in content_root:
if cell.tag == 'mxCell' and 'style' in cell.attrib:
(components,
style) = mxCellDecoder.parse_mxCell(cell, batch, window_options)
ent = world.create_entity()
for c in components:
world.add_component(ent, c)
draw2entity[style['id']] = [ent, style]
if cell.tag == 'object':
if cell.attrib['type'] == 'robot':
(components,
style) = mxCellDecoder.parse_object(cell, batch,
window_options)
ent = world.create_entity()
# Custom components
for key, val in cell.attrib.items():
if key.startswith('component_'):
component_name = key[
10:] # removes "component_" from the name
init_values = json.loads(val)
component = dynamic_importer.init_component(
component_name, init_values)
components.append(component)
for c in components:
world.add_component(ent, c)
draw2entity[style['id']] = [ent, style]
objects.append((ent, style['id']))
elif cell.attrib['type'] == 'pickable':
skeleton = copy.copy(cell)
(components,
style) = mxCellDecoder.parse_object(cell, batch,
window_options)
pick = Pickable(float(cell.attrib['weight']),
cell.attrib['name'], skeleton)
components.append(pick)
ent = world.create_entity()
for c in components:
world.add_component(ent, c)
interactive[style['name']] = ent
elif cell.attrib['type'] == 'path':
mxCell = cell[0]
points = Path.from_mxCell(mxCell, windowSize[1])
obj = mxCell.attrib.get('source', None)
(ent, _) = draw2entity.get(obj, (None, None))
if ent is None:
print(f"Path origin ({obj}) not found. Trying target.")
else:
print(f"Adding path to entity {ent}")
world.add_component(ent, Path(points))
elif cell.attrib['type'] == 'map-path':
mxCell = cell[0]
points = Path.from_mxCell(mxCell, windowSize[1])
objId = cell.attrib.get('origin', '')
key = cell.attrib.get('key', '')
if key == '':
print(f"Map entry without key. Using default value")
key = 'Default'
(ent, _) = draw2entity.get(objId, (None, None))
if ent is None:
print(f"Path origin ({obj}) not found. Trying target.")
else:
if world.has_component(ent, Map):
map = world.component_for_entity(ent, Map)
if key == 'Default':
key += str(len(map))
map.paths[key] = points
else:
if key == 'Default':
key += '0'
newMap = Map({key: points})
world.add_component(ent, newMap)
else:
print("Unrecognized object", cell)
return draw2entity, objects, interactive