def generate_tpg(self):
# Creating type-1 edges
for agent, plan in self.schedule.items():
vertex = Vertex(agent, Location(plan[0]['x'], plan[0]['y']), plan[0]['t'])
self.vertices.append(vertex)
i_prev = 0
for i in range(len(plan)-1):
location_a = Location(plan[i_prev]['x'], plan[i_prev]['y'])
location_b = Location(plan[i+1]['x'], plan[i+1]['y'])
if not location_a == location_b:
vertex_a = Vertex(agent, location_a, plan[i_prev]['t'])
vertex_b = Vertex(agent, location_b, plan[i+1]['t'])
self.vertices.append(vertex_b)
edge_ab = Edge(vertex_a, vertex_b)
self.edges_type_1.append(edge_ab)
i_prev = i+1
# Creating type-2 edges
for agent_j, plan_j in self.schedule.items():
for t_j in range(len(plan_j)):
s_tj = Location(plan_j[t_j]['x'], plan_j[t_j]['y'])
v_tj = Vertex(agent_j, s_tj, plan_j[t_j]['t'])
if v_tj in self.vertices:
for agent_k, plan_k in self.schedule.items():
if agent_j is not agent_k:
for t_k in range(t_j, len(plan_k)):
s_tk = Location(plan_k[t_k]['x'], plan_k[t_k]['y'])
v_tk = Vertex(agent_k, s_tk, plan_k[t_k]['t'])
if v_tk in self.vertices and s_tk==s_tj:
edge = Edge(v_tj, v_tk)
self.edges_type_2.append(edge)
def return_safety_vertex(self, vertex, side=-1):
"""
returns a safety vertex in a side (-1 or +1)
"""
for edge in self.edges_type_1:
if side == -1:
if vertex == edge.vertex_b:
dir = [edge.vertex_b.location.x - edge.vertex_a.location.x, \
edge.vertex_b.location.y - edge.vertex_a.location.y]
mag = (dir[0]**2 + dir[1] ** 2) **0.5
dir = [dir[0]/mag, dir[1]/mag]
new_loc_x = vertex.location.x - self.delta * dir[0]
new_loc_y = vertex.location.y - self.delta * dir[1]
new_loc = Location(new_loc_x, new_loc_y)
new_vertex = Vertex(vertex.agent, new_loc, vertex.time-0.1)
return new_vertex
if side == 1:
if vertex == edge.vertex_a:
dir = [edge.vertex_b.location.x - edge.vertex_a.location.x, \
edge.vertex_b.location.y - edge.vertex_a.location.y]
mag = (dir[0]**2 + dir[1] ** 2) **0.5
dir = [dir[0]/mag, dir[1]/mag]
new_loc_x = vertex.location.x + self.delta * dir[0]
new_loc_y = vertex.location.y + self.delta * dir[1]
new_loc = Location(new_loc_x, new_loc_y)
new_vertex = Vertex(vertex.agent, new_loc, vertex.time+0.1)
return new_vertex
return False
def generate_initial_final_states(self):
for agent, plan in self.schedule.items():
init_state = Vertex(agent, Location(plan[0]['x'], plan[0]['y']), plan[0]['t'])
final_state = Vertex(agent, Location(plan[-1]['x'], plan[-1]['y']), plan[-1]['t'])
self.initial_states.append(init_state)
self.final_states.append(final_state)
def generate_end_edges(self):
start = Vertex('start', Location(-1, -1), -1)
end = Vertex('end', Location(-2, -2), -2)
self.vertices += [start, end]
bound_start = [0., 0.]
bound_end = [0., float('inf')]
for state in self.tpg.initial_states:
edge = Edge(start, state, bound_start)
self.edges.append(edge)
for state in self.tpg.final_states:
edge = Edge(state, end, bound_end)
self.edges.append(edge)