Esempio n. 1
0
 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)
Esempio n. 2
0
    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
Esempio n. 3
0
    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)
Esempio n. 4
0
    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)