def getSortedMapPathData(mapData: dict) -> dict:
'''
获取地图下各节点到目标节点的最短路径数据
Keyword arguments:
mapData -- 地图节点数据
'''
graph = Graph()
sortedPathData = {}
for node in mapData.keys():
for target in mapData[node]:
graph.add_edge(node, target, {'cost': mapData[node][target]})
cost_func = lambda u, v, e, prev_e: e['cost']
for node in mapData.keys():
newData = {node: {}}
for target in mapData.keys():
if target != node:
findPathData = find_path(graph,
node,
target,
cost_func=cost_func)
newData[node].update({
target: {
"Path": findPathData.nodes[1:],
"Time": findPathData.costs
}
})
sortedPathData.update(newData)
return sortedPathData
def find_conversions(source, target, versions_graph):
"""
This finds the minimum number of version upgrades required to reach the target version.
:param source: source version
:param target: target version
:param versions_graph: graph to fetch update path from
:return: list of conversions in order of execution
"""
graph = Graph()
for source_version, target_version in versions_graph.query(
"""SELECT ?source_version ?target_version{
?source_version version:convertsTo ?target_version .
}"""):
graph.add_edge(str(source_version), str(target_version),
{"conversions": 1})
# Find the shortest path
res = find_path(
graph,
str(source),
str(target),
cost_func=lambda u, v, e, prev_e: e["conversions"],
)
# Create and return the conversions
conversions = []
print(" -> ".join(res.nodes))
current = source
for node in res.nodes:
conversions.append((current, node))
current = node
return conversions[1:]
def get_sorted_map_path_data(
map_data: Dict[str, Dict[str, int]]
) -> Dict[str, Dict[str, game_type.TargetPath]]:
"""
获取地图下各节点到目标节点的最短路径数据
Keyword arguments:
map_data -- 地图节点数据 当前节点:可通行节点:所需时间
Return arguments:
Dict[int,Dict[int,game_type.TargetPath]] -- 最短路径数据 当前节点:目标节点:路径对象
"""
graph = Graph()
sorted_path_data = {}
for node in map_data.keys():
for target in map_data[node]:
graph.add_edge(node, target, {"cost": map_data[node][target]})
cost_func = lambda u, v, e, prev_e: e["cost"]
for node in map_data.keys():
new_data = {node: {}}
for target in map_data.keys():
if target != node:
find_path_data = find_path(graph,
node,
target,
cost_func=cost_func)
target_path = game_type.TargetPath()
target_path.path = find_path_data.nodes[1:]
target_path.time = find_path_data.costs
new_data[node][target] = target_path
sorted_path_data.update(new_data)
return sorted_path_data
def findBestPath(self, src, dst):
graph = Graph()
for node1 in costBetweenNodes.keys():
for node2 in costBetweenNodes[node1].keys():
graph.add_edge(node1, node2, {'cost': costBetweenNodes[node1][node2]})
cost_func = lambda u, v, e, prev_e: e['cost']
return find_path(graph, src, dst, cost_func=cost_func).nodes
def dist_mat(train_mat, test_mat, k):
# construct a weighted graph
mat = graph_knn(train_mat, test_mat, k)
n = mat.shape[0]
graph = Graph()
list((graph.add_edge(i, j, {'cost': mat[i, j]})
for i, j in product(range(n), range(n))
if i != j and mat[i, j] != max_dis))
if graph is None:
return
cost_func = lambda u, v, e, prev_e: e['cost']
mat = np.zeros((n, n))
# the shortest path from node i to node j is the distance between i and j
def dis(i):
single_short_path = single_source_shortest_paths(graph,
i,
cost_func=cost_func)
for j in range(n):
if j != i:
mat[i, j] = extract_shortest_path(single_short_path, j)
else:
mat[i, j] = 0
list((dis(i) for i in range(n)))
return mat
def __init__(self, name, program):
self.program = program
self.name = name
self.output = []
self.switch = {
1: self.opt1,
2: self.opt2,
3: self.opt3,
4: self.opt4,
5: self.opt5,
6: self.opt6,
7: self.opt7,
8: self.opt8,
9: self.opt9,
99: self.opt99
}
self.inputp = 0
self.halt = False
self.relative_base = 0
self.memory = {}
self.retpt = 0
self.new_position = (0, 0)
self.position = (0, 0)
self.area = {(0, 0): 'X'}
self.graph = Graph()
self.graph.add_node((0, 0))
self.oxygen = (0, 0)
self.unexplored = set()
self.explored = set([(0, 0)])
self.graph.add_node((0, 0))
self.path = []
def crea_matrice_distanze(dist):
initial_matr = np.array(dist)
print initial_matr
matr = initial_matr.copy()
# dict_matr = {(partenza, arrivi): lunghezza for partenza, arrivi in enumerate(dist) for arrivo, lunghezza in
# enumerate(arrivi)}
graph = Graph()
for partenza, arrivi in enumerate(dist):
for arrivo, lunghezza in enumerate(arrivi):
if lunghezza >= 0:
graph.add_edge(partenza, arrivo, {'cost': lunghezza})
cost_func = lambda u, v, e, prev_e: e['cost']
for partenza, arrivi in enumerate(dist):
for arrivo in range(len(arrivi)):
if partenza == arrivo:
matr[partenza, arrivo] = -1
else:
try:
min_dist = find_path(graph,
partenza,
arrivo,
cost_func=cost_func)[3]
matr[partenza, arrivo] = min_dist
except Exception as e:
matr[partenza, arrivo] = -1
print "matr ditanze\n", matr
return matr
def get_sorted_map_path_data(map_data: dict) -> dict:
"""
获取地图下各节点到目标节点的最短路径数据
Keyword arguments:
map_data -- 地图节点数据
"""
graph = Graph()
sorted_path_data = {}
for node in map_data.keys():
for target in map_data[node]:
graph.add_edge(node, target, {"cost": map_data[node][target]})
cost_func = lambda u, v, e, prev_e: e["cost"]
for node in map_data.keys():
new_data = {node: {}}
for target in map_data.keys():
if target != node:
find_path_data = find_path(graph,
node,
target,
cost_func=cost_func)
new_data[node].update({
target: {
"Path": find_path_data.nodes[1:],
"Time": find_path_data.costs,
}
})
sorted_path_data.update(new_data)
return sorted_path_data
def make_graph(self):
con = pymysql.connect(host='localhost',
user='******',
password='******',
db='drive2',
charset='utf8')
curs = con.cursor()
graph = Graph()
sql = "select nodeid from drive2.c1_node"
sql2 = "select tonode, length, format(speed,0) from drive2.a3_link, jcm_drive.jcm_a3_link where fromnode = "
curs.execute(sql)
rows = curs.fetchall()
for i in rows:
nodeid = list(map(lambda x: x, i))
curs.execute(sql2 + "'" + nodeid[0] + "';")
rows = curs.fetchall()
if rows is None:
continue
else:
for item in list(rows):
cost = item[1] / int(item[2])
graph.add_edge(i[0], item[0], cost)
con.close()
return graph
def dijkstra_planning(self):
graph = Graph()
temp_nodes = self.nodes[:]
temp_nodes.pop(0)
k = 0
for i in range(len(self.nodes)):
k = k + 1
for j in range(len(temp_nodes)):
node_a = self.nodes[i]
node_b = temp_nodes[j]
if np.linalg.norm(node_a - node_b) != 0:
print("add edge:", i, j + k)
graph.add_edge(i, j + k, np.linalg.norm(node_a - node_b))
if temp_nodes:
temp_nodes.pop(0)
print("done")
path = find_path(graph, 1, 3)
print("path:", path)
# if __name__ == "__main__":
# test_nodes = [np.array([21, 34]),
# np.array([45, 28]),
# np.array([76, 14]),
# np.array([12, 56]),
# np.array([48, 32])]
#
# motion_planner = Planner(test_nodes)
# motion_planner.dynamic_programming()
def shortestPathAStar(start_ind, goal_ind):
global R
# R[0] is the node for the start configuration
# R[1] is the node for the goal configuration
# each node has edges in the form: self.edges = [] List of tuples (node_id, dist)
# loop through our graph and convert it to a nice format for dijkstar
graph = Graph()
for node in R:
for edge in node.edges:
graph.add_edge(node.id, edge[0], edge[1])
cfg_array = []
# catch path not found exception
try:
pathinfo = find_path(graph, start_ind, goal_ind)
except:
print('Could NOT find a path from start to goal')
return cfg_array
# get the configurations from each node in this found path
for node_id in pathinfo.nodes:
cfg_array.append(R[node_id].cfg)
return cfg_array
def graph3(self):
graph = Graph({
'a': {
'b': 10,
'c': 100,
'd': 1
},
'b': {
'c': 10
},
'd': {
'b': 1,
'e': 1
},
'e': {
'f': 1
},
})
graph.add_edge('f', 'c', 1)
graph.add_edge('g', 'b', 1)
nodes = list(graph)
nodes.sort()
self.assertEqual(nodes, ['a', 'b', 'd', 'e', 'f', 'g'])
incoming = graph.get_incoming('c')
incoming_nodes = list(incoming.keys())
incoming_nodes.sort()
self.assertEqual(incoming_nodes, ['a', 'b', 'f'])
return graph
def solve_graph():
global maze, maze_cost, pacmanpos, world, goal_state
graph = Graph()
for j in range(0, height):
for i in range(0, width):
if maze[j][i] != 1:
#arriba
if (maze[j-1][i] != 1) and (j-1 > 0):
xi = str((j,i))
xj = str((j-1,i))
c = maze_cost[j-1][i]
graph.add_edge(xi,xj,{'cost':c})
#print(xi+' - '+xj)
#abajo
if (maze[j+1][i] != 1) and (j+1 < height):
xi = str((j,i))
xj = str((j+1,i))
c = maze_cost[j+1][i]
graph.add_edge(xi,xj,{'cost':c})
#print(xi+' - '+xj)
#derecha
if (maze[j][i+1] != 1) and (i+1 < width):
xi = str((j,i))
xj = str((j,i+1))
c = maze_cost[j][i+1]
graph.add_edge(xi,xj,{'cost':c})
#print(xi+' - '+xj)
#izquierda
if (maze[j][i-1] != 1) and (i-1 > 0):
xi = str((j,i))
xj = str((j,i-1))
c = maze_cost[j][i-1]
graph.add_edge(xi,xj,{'cost':c})
#print(xi+' - '+xj)
cost_func = lambda u, v, e, prev_e: e['cost']
pacman = worldToGrid(pacmanpos.pacmanPos.x, pacmanpos.pacmanPos.y)
start_state = (pacman['y'], pacman['x'])
#start_state = (25,14)
# goal_state = closest_bonus()
# print(goal_state)
#goal_state = (25,26)
path = find_path(graph, str(start_state), str(goal_state), cost_func=cost_func)
# print(path.nodes)
if len(path.nodes)>1:
next_state = eval(path.nodes[1])
aux = (start_state[0]-next_state[0], start_state[1]-next_state[1])
else:
print('yuca')
return 4
#print(str(start_state)+' '+str(next_state))
if aux == (1,0):
return 0
if aux == (-1,0):
return 1
if aux == (0,-1):
return 2
if aux == (0,1):
return 3
def _get_graph(nets_ids, links):
graph = Graph()
for link in links:
src_connectable_id = link['src_connectable_id']
dst_connectable_id = link['dst_connectable_id']
if src_connectable_id in nets_ids and dst_connectable_id in nets_ids:
graph.add_edge(src_connectable_id, dst_connectable_id, 100)
return graph
def sol_6_b(data_str):
orbit_tree = read_orbits(data_str)
g = Graph()
for planet in orbit_tree:
g.add_edge(orbit_tree[planet], planet, 1)
g.add_edge(planet, orbit_tree[planet], 1)
path = find_path(g, "YOU", orbit_tree["SAN"])
return path.total_cost - 1
def dijkstra(d):
g = Graph()
for i in range(len(d)):
for j in range(len(d[i])):
dirs = ((i - 1, j), (i + 1, j), (i, j - 1), (i, j + 1))
for x, y in dirs:
if 0 <= x < len(d) and 0 <= y < len(d[i]):
g.add_edge((i, j), (x, y), d[x][y])
return find_path(g, (0, 0), (len(d) - 1, len(d[0]) - 1)).total_cost
def get_routes(name_num, point1, point2):
global_graph = Graph()
loc_name = get_loc_name(name_num)
global_graph = global_graph.load('./djicstra_graph/{}'.format(loc_name))
graph_route = find_path(global_graph, point1, point2).nodes
final_list = list()
for elem in graph_route:
final_list.append((get_position_name(name_num, elem)))
return (final_list)
def setUpGraph(self):
graph = Graph()
for camino in Camino.objects.all():
graph.add_edge(
camino.desde.pk, camino.hasta.pk, {'cost': camino.distancia})
graph.add_edge(
camino.hasta.pk, camino.desde.pk, {'cost': camino.distancia})
return graph
def graph_search(self):
# set up the graph
graph = Graph()
self.nodes = 0
self.total_cost = 0
# add edges
for i in range(len(self.TE)):
# pdb.set_trace()
edge = self.TE[i]
dist = math.sqrt((edge[0][0] - edge[1][0])**2 +
(edge[0][1] - edge[1][1])**2)
# pdb.set_trace()
# find vertex from self.V that matches the edges
vertex1 = self.TV.index(edge[0])
vertex2 = self.TV.index(edge[1])
# pdb.set_trace()
graph.add_edge(vertex1, vertex2, dist)
# pdb.set_trace()
temp = find_path(graph, 0, len(self.TV) - 1)
self.nodes = temp[0]
self.total_cost = temp[3]
# now that we have the final path of the robot, we need to extract the ideal robot positions
# interpolate all of the vertices to make a series of points that the robot can follow
self.robot_positions = []
# iterate over each node in the final path and connect between them
for i in range(len(self.nodes) - 1):
# take the first and second nodes
from_node = self.TV[self.nodes[i]]
to_node = self.TV[self.nodes[i + 1]]
# take each edge and interpolate a distance equivalent 1/10th of the robot speed per second
diff_x = to_node[0] - from_node[0]
diff_y = to_node[1] - from_node[1]
dist_speed = self.speed * 1 / 10 # m/s * s = m
# now find number of points from distance between points and speed
dist_covered = math.sqrt(diff_x**2 + diff_y**2)
num_points = round(dist_covered / dist_speed)
# interpolate using these points
x_int = from_node[0]
y_int = from_node[1]
interpolateds = [x_int, y_int]
for i in range(num_points):
# append the robot positions
self.robot_positions.append(interpolateds)
# add the points in by moving the direction amount calculated in x and y
x_int = interpolateds[0] + diff_x / num_points
y_int = interpolateds[1] + diff_y / num_points
interpolateds = [x_int, y_int]
# append goal
if self.solution_found == 1:
self.robot_positions.append(self.qgoal)
# self.robot_positions.append(np.linspace(edge[0],edge[1], num = self.speed/50,endpoint=True,retstep=True))
return self.nodes, self.total_cost
def __init__(self, addr, heartbeatTime):
"""TODO: add your own class fields and initialization code here"""
Router.__init__(self, addr) # initialize superclass - don't remove
self.heartbeatTime = heartbeatTime
self.last_time = 0
self.G = Graph(undirected=True)
self.SeqNum = {}
self.Neighbours = {
} #Dictionary with key as address and cost and seqnum as values
self.Curr_Seq = 0
def make_graph(self):
graph = Graph()
for x, y in product(range(self.width), range(self.height)):
if isinstance(self.grid[x][y],
Floor) or isinstance(self.grid[x][y], Flag):
for i, j in [(x + i, y + j) for i, j in
((-1, 0), (1, 0), (0, 1), (0, -1))]:
if isinstance(self.grid[i][j],
Floor) or isinstance(self.grid[i][j], Flag):
graph.add_edge((x, y), (i, j), 1)
return graph
def main():
models.Base.metadata.create_all(engine)
logging.info('Created database schema.')
session = Session()
add_random_animals_of_type_to_session(session, create_lion,
config.LION_COUNT)
add_random_animals_of_type_to_session(session, create_hippopotamus,
config.HIPPOPOTAMUS_COUNT)
add_random_animals_of_type_to_session(session, create_antelope,
config.ANTELOPE_COUNT)
add_random_animals_of_type_to_session(session, create_hyena,
config.HYENA_COUNT)
all_animals = session.query(models.Animal).all()
friendship_count = 0
while friendship_count < config.MAX_FRIENDSHIP_COUNT:
random_animal_1 = all_animals[randint(0, len(all_animals) - 1)]
random_animal_2 = all_animals[randint(0, len(all_animals) - 1)]
if can_become_friends(random_animal_1, random_animal_2):
logging.info('Creating friendship between: {}, {}'.format(
random_animal_1, random_animal_2))
random_animal_1.friends.append(random_animal_2)
random_animal_2.friends.append(random_animal_1)
session.add(random_animal_1)
session.add(random_animal_2)
friendship_count += 1
session.commit()
hungriest_lion = get_hungriest_lion(session)
logging.info('Hungriest lion is: {}'.format(hungriest_lion))
slowest_antelope = get_slowest_antelope(session)
logging.info('Slowest antelope is: {}'.format(slowest_antelope))
graph = Graph()
for animal in all_animals:
for friend in animal.friends:
graph.add_edge(animal.id, friend.id)
try:
shortest_path = find_path(graph,
hungriest_lion.id,
slowest_antelope.id,
cost_func=get_cost)
except NoPathError:
logging.warn('The lion stays hungry today')
else:
logging.info(
'Shortest path to slowest antelope is through animal ids: {}'.
format(shortest_path[0]))
def __init__(self, addr, heartbeatTime):
"""TODO: add your own class fields and initialization code here"""
Router.__init__(self, addr) # initialize superclass - don't remove
self.heartbeatTime = heartbeatTime
self.last_time=0
self.addr=addr
self.sequence=0
self.state=[]
self.forward_table={}
self.graph=Graph(undirected=True)
self.add_status=True
self.last_seq={}
def parse_distance_matrix(csv_reader, cost_function):
# Ignore last column for trailhead.
peaks = csv_reader.__next__()[1:-1]
distances = {p: {p: None for p in peaks} for p in peaks}
trailhead_distances = {p: None for p in peaks + [_TRAILHEAD_NAME]}
for row in csv_reader:
if not is_peak(row[0]):
continue
# Note: trailhead distances are read as Peak > Trailhead, but here
# stored as Trailhead > Peak, so they need to be reversed below.
trailhead_distances[row[0]] = _parse_distance_gain_loss_string(row[-1])
i = 0
for d in row[1:-1]:
distances[row[0]][peaks[i]] = _parse_distance_gain_loss_string(d)
i += 1
# Add back distances
for p1 in distances:
for p2 in distances[p1]:
if distances[p1][p2] is not None and distances[p2][p1] is None:
distances[p2][p1] = (
distances[p1][p2][0],
distances[p1][p2][2], # swap climb/desc
distances[p1][p2][1])
# Build graph for shortest path computation
g = Graph()
for p1 in distances:
for p2 in distances[p1]:
g.add_edge(p1, p2, distances[p1][p2])
def internal_cost_function(_0, _1, x, _2):
return cost_function(x)
for p1 in distances:
for p2 in distances[p1]:
distances[p1][p2] = tuple(
sum(x) for x in zip(*find_path(
g, p1, p2, cost_func=internal_cost_function).edges))
# Trailhead has to be added after shortest path computation to make sure
# shortest parths don't go through the artificial trailhead.
distances[_TRAILHEAD_NAME] = {}
for p in trailhead_distances:
d = trailhead_distances[p]
if d is None:
d = (1e9, 1e9, 1e9)
distances[_TRAILHEAD_NAME][p] = (d[0], d[2], d[1])
distances[p].update({_TRAILHEAD_NAME: (d[0], d[1], d[2])})
# Add self distance
for p1 in distances:
distances[p1][p1] = (0, 0, 0)
return distances
def __init__(self, addr, heartbeatTime):
"""TODO: add your own class fields and initialization code here"""
Router.__init__(self, addr) # initialize superclass - don't remove
self.heartbeatTime = heartbeatTime
self.last_time = 0
# Hints: initialize local state
self.link_state = {} #APNA LINK STATE
self.link_state_local = {} #BAKI SUB KA LINK STATE
self.sequence_number = 0 #SEND KARTAY WAKT SEQ NUM
self.check_sequence_number = {} #RECIEVE KARTAY WAKT SEQ NUM
self.network_graph = Graph(undirected=True) #GRAPH
self.forwarding_table = {} #FRWD TABLE
def __init__(self, map, polices, police_vision):
self.graph = Graph()
self.map = map
self.finded_path = None
self.polices = [
self.map.GetNodeByPosition((agent.position.y, agent.position.x))
for agent in polices
]
self.police_vision = police_vision
self.init_graph_terror()
self.cost_function = lambda u, v, e, prev_e: e['cost']
def __init__(self, addr, heartbeatTime):
"""TODO: add your own class fields and initialization code here"""
Router.__init__(self, addr) # initialize superclass - don't remove
self.heartbeatTime = heartbeatTime
self.last_time = 0
self.forwarding_table = {}
self.routing_table = {}
self.graph = Graph()
self.seqno = "1"
self.check = True
# Hints: initialize local state
pass
def load_graph(self, path):
import time
start_time = time.time()
from dijkstar import Graph
self.graph = Graph()
self.graph.load(path)
if self.debug == True:
print("--- %s seconds to load the graph ---" %
(time.time() - start_time))
def find_closest(game_map, width, height, player, players):
# is anyone in range if so don't move
inRange = []
for d in [(0, -1), (-1, 0), (1, 0), (0, 1)]:
key = "{}_{}".format(player.x + d[0], player.y + d[1])
if key in game_map and game_map[key] not in ['.', '#', player.type]:
enemy = list(
filter(
lambda x: x.x == player.x + d[0] and x.y == player.y + d[
1], players))[0]
return [0, enemy.x, enemy.y, []]
# build a map
graph = Graph()
for x in range(0, width):
for y in range(0, height):
for d in [(0, -1), (-1, 0), (1, 0), (0, 1)]:
key = "{}_{}".format(x + d[0], y + d[1])
if key in game_map and game_map[key] == '.':
graph.add_edge("{}_{}".format(x, y), key, 1)
path_data = []
for p in players:
# don't attack your friends (or self)
if p.type == player.type:
continue
# don't attack dead people
if not p.alive:
continue
# look around the target for a free space
for d in [(0, -1), (-1, 0), (1, 0), (0, 1)]:
key = "{}_{}".format(p.x + d[0], p.y + d[1])
# if this block is valid and empty
if key in game_map and game_map[key] == '.':
try:
pdata = find_path(graph,
"{}_{}".format(player.x, player.y),
"{}_{}".format(p.x + d[0], p.y + d[1]))
path_data.append([
pdata.total_cost, p.x + d[0], p.y + d[1], pdata.nodes,
p.reading
])
except:
continue
# sort by the shortest distance, and then the lowest y value, and then the lowest x value
path_data = sorted(path_data, key=lambda x: (x[0], x[4]))
if len(path_data) > 0:
return path_data[0]
else:
return None
def make_graph(grid):
utils.logger.info("Making graph...")
graph = Graph()
for x in range(len(grid)):
for y, cell in enumerate(grid[x]):
neighbors = get_neighbors(grid, x, y)
for neighbor in neighbors:
graph.add_edge(
(x, y),
(neighbor["x"], neighbor["y"]),
)
return graph
def init(self):
""" set up the router using the already loaded network """
self.graph = Graph()
self.edgeMap = {}
for edge in Network.routingEdges:
self.edgeMap[edge.id] = edge
self.graph.add_edge(
edge.fromNodeID, edge.toNodeID, {
'length': edge.length,
'maxSpeed': edge.maxSpeed,
'lanes': len(edge.lanes),
'edgeID': edge.id
})
def load_graph(self, path):
import time
start_time = time.time()
import pickle
g = pickle.load(open(path, 'rb'))
from dijkstar import Graph
self.graph = Graph(g)
#self.graph.load(path) TODO: this doesn't work!!! loads empty graph
if self.debug == True:
print("--- %s seconds to load the graph ---" %
(time.time() - start_time))
def __init__(self, height, width, blocks, spawner_square, exit_square,
lives, souls):
self.height = height
self.width = width
self.blocks = blocks
self.spawner_square = spawner_square
self.exit_square = exit_square
self.lives = lives
self.souls = souls
self.dijk_grid = Graph()
self.route_dict = {}
self.num_loc_list = []
self.tower_list = []
self.square_grid = []
self.forbidden_squares = set()
def __init__(self):
level_str = make_level()
self.width = level_str.index('\n')
self.height = level_str.count('\n')
self.level = []
self.teams = set()
self.players = set()
self.projectiles = set()
self.graph = Graph()
self.starts = set()
for y in range(0, self.height):
row = []
for x in range(0, self.width):
type_id = level_str[y * (self.width + 1) + x]
row.append(Point(
x=x,
y=y,
type_id=type_id,
theme_id=('金' if type_id in 'iwX' else None),
orientation=None
))
self.level.append(row)
for pt in self.all_points:
if pt.type_id == 'X':
self.starts.add(pt)
self.set_node(pt)
def calculate_path(self):
graph = Graph()
for i in range(len(self.links)):
graph.add_edge(self.links[i][0], self.links[i][1], {'cost': self.links[i][2]})
cost_func = lambda u, v, e, prev_e: e['cost']
#print self.links
result = find_path(graph, meta_data.source_id, meta_data.destination_id, cost_func=cost_func)
route = result[0]
# clear link list
del self.crn_manager.role.links[:]
# check and reply
if meta_data.INF in result[2]:
route = []
self.routing_request_log.append([self.crn_manager.get_virtual_time(), 0])
else:
self.routing_request_log.append([self.crn_manager.get_virtual_time(), 1])
#self.crn_manager.route = route
return route
def load_graph(self, path):
import time
start_time = time.time()
from dijkstar import Graph
self.graph = Graph()
self.graph.load(path)
if self.debug == True:
print("--- %s seconds to load the graph ---" %
(time.time() - start_time))
class Game(object):
frame = 0
sync_frame = 0
def __init__(self):
level_str = make_level()
self.width = level_str.index('\n')
self.height = level_str.count('\n')
self.level = []
self.teams = set()
self.players = set()
self.projectiles = set()
self.graph = Graph()
self.starts = set()
for y in range(0, self.height):
row = []
for x in range(0, self.width):
type_id = level_str[y * (self.width + 1) + x]
row.append(Point(
x=x,
y=y,
type_id=type_id,
theme_id=('金' if type_id in 'iwX' else None),
orientation=None
))
self.level.append(row)
for pt in self.all_points:
if pt.type_id == 'X':
self.starts.add(pt)
self.set_node(pt)
def set_node(self, pt):
ptype = ptypes[pt.type_id]
if ptype.layer == 'e':
cost = 50
elif ptype.layer == 'd':
cost = 6
elif ptype.layer == 'c':
cost = 0.99
else:
cost = 1
neighbors = []
if pt.x > 0:
neighbors.append(self.level[pt.y][pt.x - 1])
if pt.x < self.width - 1:
neighbors.append(self.level[pt.y][pt.x + 1])
if pt.y > 0:
neighbors.append(self.level[pt.y - 1][pt.x])
if pt.y < self.height - 1:
neighbors.append(self.level[pt.y + 1][pt.x])
for npt in neighbors:
self.graph.add_edge((npt.x, npt.y), (pt.x, pt.y), cost)
def find_path(self, pt1, pt2):
def manhattan(u, v, edge, prev_edge):
return abs(u[0] - v[0]) + abs(u[1] - v[1])
try:
nodes, edges, costs, final_cost = find_path(
self.graph, pt1, pt2, heuristic_func=manhattan
)
result = []
f = self.frame
for i, (x, y) in enumerate(nodes):
ptype = ptypes[self.level[y][x].type_id]
if ptype.layer in ('d', 'e'):
return result
if f > self.frame and self.check_conflict(f, x, y):
if self.check_conflict((f + 1) % 1000, x, y):
return result
else:
result.append(result[-1])
f += 1
result.append((x, y))
f += 1
return result
except NoPathError:
return []
def find_line(self, x, y, direction):
path = []
layer = None
destroy = False
while not layer or layer not in ('c', 'd', 'e'):
path.append((x, y))
if direction == 'l':
x -= 1
if direction == 'r':
x += 1
if direction == 'u':
y -= 1
if direction == 'd':
y += 1
layer = ptypes[self.level[y][x].type_id].layer
if layer in ('c', 'd'):
path.append((x, y))
destroy = True
return path, destroy
def check_conflict(self, frame, x, y):
for player in self.players:
if not player.plan:
if player.x == x and player.y == y:
return True
else:
continue
if frame in player.plan:
if player.plan[frame] == (x, y):
return True
elif player.target_x == x and player.target_y == y:
return True
def to_str(self):
s = 'GRID '
for row in self.level:
for pt in row:
s += pt.type_id
s += '\n'
return s
@property
def all_points(self):
for row in self.level:
for c in row:
yield c
@asyncio.coroutine
def tick(self):
yield from asyncio.sleep(0.1)
self.frame += 1
self.frame = self.frame % 1000
if self.frame % 100 == 0:
self.sync_frame += 1
if self.sync_frame == 1:
self.sync_frame = 0
yield from self.broadcast("TICK %s" % self.frame)
for player in list(self.players):
yield from player.tick(self.frame)
for proj in list(self.projectiles):
yield from self.check_collide(proj, player)
for proj in list(self.projectiles):
yield from proj.tick(self.frame)
for proj2 in list(self.projectiles):
yield from self.check_collide(proj2, proj)
for player in list(self.players):
yield from self.check_collide(proj, player)
@asyncio.coroutine
def check_collide(self, proj, obj):
if proj.x != obj.x or proj.y != obj.y or proj.id == obj.id:
return
if isinstance(obj, Player) and proj.player.id == obj.id:
return
yield from self.remove_projectile(proj)
if isinstance(obj, Projectile):
yield from self.remove_projectile(obj)
else:
yield from obj.add_hp(-1)
@asyncio.coroutine
def process(self, player, data):
if not data:
return
vals = data.split(" ")
cmd, args = vals[0], vals[1:]
fn = getattr(self, 'process_%s' % cmd.lower(), None)
if fn:
yield from fn(player, *args)
@asyncio.coroutine
def process_point(self, player, x, y, type_id, theme_id=None, orientation=None):
pt = self.level[int(y)][int(x)]
pt.type_id = type_id
pt.theme_id = theme_id
pt.orientation = orientation
self.set_node(pt)
yield from self.broadcast(pt.to_str())
@asyncio.coroutine
def process_go(self, player, x, y):
if player.set_target(self.frame, int(x), int(y)):
yield from self.broadcast(player.path_str(self.frame))
@asyncio.coroutine
def broadcast(self, msg):
print(msg)
for player in self.players:
print("Sending to Player ", player.id)
yield from player.send(msg)
@asyncio.coroutine
def new_player(self, client):
player = Player(client, self)
self.players.add(player)
tstr = "THEMES"
for theme_id, theme in themes.items():
if not theme.elemental:
tstr += " %s" % theme_id
yield from player.send(tstr)
sstr = "SHIPS"
for type_id, ptype in ptypes.items():
if type_id.isdigit():
sstr += " %s" % type_id
yield from player.send(sstr)
for team in self.teams:
yield from player.send(team.to_str())
for pl in self.players:
if not pl.active:
continue
yield from player.send(pl.to_str())
return player
def start_player(self, player):
player.active = True
self.locate_player(player, initial=True)
player.hp = 10
player.lives = 3
yield from self.send_initial(player)
def locate_player(self, player, initial=False):
if not initial:
tx = player.x
ty = player.y
choices = player.team.starts
else:
if len(player.team.starts) == 0:
ty = 0
if player.team.id == 1:
tx = 0
elif player.team.id == 2:
tx = self.width
elif player.team.id == 3:
tx = self.width / 2
else:
tx = random.randint(0, self.width)
else:
tx = 0
ty = 0
for start in player.team.starts:
tx += start.x
ty += start.y
tx /= len(player.team.starts)
ty /= len(player.team.starts)
choices = []
for i in range(0, int(len(self.starts) / 2)):
start = None
while start is None or start.theme_id != '金':
start = random.choice(list(self.starts))
choices.append(start)
choices = sorted(
choices,
key=lambda pt: abs(pt.x - tx) + abs(pt.y - ty)
)
player.x = choices[0].x
player.y = choices[0].y
@asyncio.coroutine
def remove_player(self, player):
if player in self.players:
self.players.remove(player)
@asyncio.coroutine
def process_fire(self, player, direction):
proj = Projectile(player, direction, self)
self.projectiles.add(proj)
yield from self.broadcast(proj.to_str())
yield from self.broadcast(proj.path_str(self.frame))
@asyncio.coroutine
def process_team(self, player, name, theme_id):
for team in self.teams:
if team.theme_id == theme_id:
return
team = Team(self, name, theme_id)
self.teams.add(team)
yield from self.broadcast(team.to_str())
@asyncio.coroutine
def process_join(self, player, team_id):
for team in self.teams:
if str(team.id) == team_id:
team.add_player(player)
player.team = team
if not player.team:
raise Exception("Failed to find team")
@asyncio.coroutine
def process_ship(self, player, ship_id, name):
player.type_id = ship_id
player.name = name
yield from self.start_player(player)
@asyncio.coroutine
def remove_projectile(self, projectile):
if projectile in self.projectiles:
self.projectiles.remove(projectile)
@asyncio.coroutine
def process_edit(self, player):
yield from player.send(self.to_str())
@asyncio.coroutine
def send_initial(self, player):
yield from player.send(self.to_str())
yield from player.send(player.to_str(True))
yield from player.send("TICK %s" % self.frame)
for pt in self.all_points:
if pt.theme_id or pt.orientation:
yield from player.send(pt.to_str())
for pl in self.players:
if not pl.active:
continue
yield from player.send(pl.path_str(self.frame))
yield from self.broadcast(player.to_str())
def directed_and_guided_map_init ():
graph = None
cost_func = None
graph = Graph()
# 4 real beacons
graph.add_edge(1, 2, {'cost': 3})
graph.add_edge(2, 1, {'cost': 3})
graph.add_edge(2, 3, {'cost': 4})
graph.add_edge(3, 2, {'cost': 4})
graph.add_edge(3, 4, {'cost': 2})
graph.add_edge(4, 3, {'cost': 2})
graph.add_edge(1, 7, {'cost': 4})
graph.add_edge(7, 1, {'cost': 4})
# 3 virtual beacons that does not actually exist
graph.add_edge(3, 4, {'cost': 5})
graph.add_edge(4, 3, {'cost': 5})
graph.add_edge(3, 5, {'cost': 5})
graph.add_edge(5, 3, {'cost': 5})
graph.add_edge(2, 5, {'cost': 6})
graph.add_edge(5, 2, {'cost': 6})
graph.add_edge(1, 6, {'cost': 7})
graph.add_edge(6, 1, {'cost': 7})
cost_func = lambda u, v, e, prev_e: e['cost']
return graph, cost_func
def build_graph(self, save_path=None):
import time
start_time = time.time()
collide_res = self.env.house.n_row
from dijkstar import Graph
visit = dict()
self.graph = Graph()
self.mock_obs_map = np.zeros(
(collide_res + 1, collide_res + 1), dtype=np.uint8)
self.mock_obs_map[np.where(self.env.house.connMap == -1)] = 1
for x in range(collide_res + 1):
for y in range(collide_res + 1):
pos = (x, y)
if self.env.house.canMove(x, y) and pos not in visit:
que = [pos]
visit[pos] = True
ptr = 0
while ptr < len(que):
cx, cy = que[ptr]
ptr += 1
# add all angles for (cx, cy) here
# connect first and last
for ang in range(len(self.angles) - 1):
self.graph.add_edge((cx, cy, self.angles[ang]),
(cx, cy, self.angles[ang + 1]),
{
'cost': 1
})
self.graph.add_edge((cx, cy, self.angles[ang + 1]),
(cx, cy, self.angles[ang]), {
'cost': 1
})
self.graph.add_edge((cx, cy, self.angles[-1]),
(cx, cy, self.angles[0]), {
'cost': 1
})
self.graph.add_edge((cx, cy, self.angles[0]),
(cx, cy, self.angles[-1]), {
'cost': 1
})
for deti in range(len(self.dirs)):
det = self.dirs[deti]
tx, ty = cx + det[0], cy + det[1]
if (self.env.house.inside(tx, ty) and
self.mock_obs_map[min(cx, tx):max(cx, tx)+1,
min(cy, ty):max(cy, ty)+1].sum() == 0):
# make changes here to add edges for angle increments as well
#
# cost = 1 from one angle to the next,
# and connect first and last
# this would be for different angles for same tx, ty
#
# then there would be connections for same angle
# and from (cx, cy) to (tx, ty)
self.graph.add_edge(
(cx, cy, self.angle_map[self.dirs[deti]]),
(tx, ty, self.angle_map[self.dirs[deti]]),
{
'cost': 1
})
tp = (tx, ty)
if tp not in visit:
visit[tp] = True
que.append(tp)
if self.debug == True:
print("--- %s seconds to build the graph ---" %
(time.time() - start_time))
if save_path != None:
start_time = time.time()
print("saving graph to %s" % (save_path))
self.graph.dump(save_path)
if self.debug == True:
print("--- %s seconds to save the graph ---" %
(time.time() - start_time))
class House3DUtils():
def __init__(
self,
env,
rotation_sensitivity=9,
move_sensitivity=0.5,
build_graph=False,
graph_dir='/path/to/3d-graphs',
target_obj_conn_map_dir='/path/to/target_obj_connmaps',
debug=True,
load_semantic_classes=True,
collision_reward=0.0,
success_reward=1.0,
dist_reward_scale=0.005,
seeing_rwd=False):
self.env = env
self.debug = debug
self.rotation_sensitivity = rotation_sensitivity
self.move_sensitivity = move_sensitivity
self.angles = [x for x in range(-180, 180, self.rotation_sensitivity)]
self.angle_strings = {1: 'right', -1: 'left'}
self.dirs, self.angle_map = self.calibrate_steps(reset=True)
self.move_multiplier = self.move_sensitivity / np.array([np.abs(x).sum() for x in self.dirs]).mean()
self.graph_dir = graph_dir
self.graph = None
self.target_obj_conn_map_dir = target_obj_conn_map_dir
if build_graph == True:
if os.path.exists(
os.path.join(graph_dir,
self.env.house.house['id'] + '.pkl')):
self.load_graph(
os.path.join(graph_dir,
self.env.house.house['id'] + '.pkl'))
else:
self.build_graph(
save_path=os.path.join(
graph_dir, self.env.house.house['id'] + '.pkl'))
self.rooms, self.objects = self._parse()
self.collision_reward = collision_reward
self.success_reward = success_reward
self.dist_reward_scale = dist_reward_scale
self.seeing_rwd = seeing_rwd
if load_semantic_classes == True:
self._load_semantic_classes()
# Shortest paths are computed in 1000 x 1000 grid coordinates.
# One step in the SUNCG continuous coordinate system however, can be
# multiple grids in the grid coordinate system (since turns aren't 90 deg).
# So even though the grid shortest path is fine-grained,
# an equivalent best-fit path in SUNCG continuous coordinates
# has to be computed by simulating steps. Sucks, but yeah.
#
# For now, we first explicitly calibrate how many steps in the gridworld
# correspond to one step in continuous world, across all directions
def calibrate_steps(self, reset=True):
mults, angle_map = [], {}
cx, cy = self.env.house.to_coor(50, 50)
if reset == True:
self.env.reset(x=cx, y=cy)
for i in range(len(self.angles)):
yaw = self.angles[i]
self.env.cam.yaw = yaw
self.env.cam.updateDirection()
x1, y1 = self.env.house.to_grid(self.env.cam.pos.x,
self.env.cam.pos.z)
pos = self.env.cam.pos
pos = pos + self.env.cam.front * self.move_sensitivity
x2, y2 = self.env.house.to_grid(pos.x, pos.z)
mult = np.array([x2, y2]) - np.array([x1, y1])
mult = (mult[0], mult[1])
angle_map[mult] = yaw
mults.append(mult)
return mults, angle_map
# 0: forward
# 1: left
# 2: right
# 3: stop
#
# returns observation, reward, done, info
def step(self, action, step_reward=False):
if action not in [0, 1, 2, 3]:
raise IndexError
if step_reward == True:
pos = self.env.cam.pos
x1, y1 = self.env.house.to_grid(self.env.cam.pos.x, self.env.cam.pos.z)
init_target_dist = self.env.house.connMap[x1, y1]
reward = 0
done = False
if action == 0:
mv = self.env.move_forward(
dist_fwd=self.move_sensitivity, dist_hor=0)
obs = self.env.render()
if mv == False: # collision
reward -= self.collision_reward
elif mv != False and step_reward == True:
# evaluate connMap dist here
x2, y2 = self.env.house.to_grid(self.env.cam.pos.x,
self.env.cam.pos.z)
final_target_dist = self.env.house.connMap[x2, y2]
reward += self.dist_reward_scale * ((init_target_dist - final_target_dist) / np.abs(
self.dirs[self.angles.index(self.env.cam.yaw % 180)]).sum())
elif action == 1:
self.env.rotate(-self.rotation_sensitivity)
obs = self.env.render()
elif action == 2:
self.env.rotate(self.rotation_sensitivity)
obs = self.env.render()
elif action == 3:
done = True
obs = self.env.render()
return obs, reward, done
# pos: [x, y, z, yaw], or objrender.Vec3
def get_dist_to_target(self, pos):
if isinstance(pos, Vec3) == True:
x, y = self.env.house.to_grid(pos.x, pos.z)
else:
x, y = self.env.house.to_grid(pos[0], pos[2])
dist = self.env.house.connMap[x, y]
return self.move_multiplier * dist
def is_inside_room(self, pos, room):
if isinstance(pos, Vec3) == True:
x = pos.x
y = pos.z
else:
x = pos[0]
y = pos[2]
if x >= room['bbox']['min'][0] and x <= room['bbox']['max'][0] and \
y >= room['bbox']['min'][2] and y <= room['bbox']['max'][2]:
return True
return False
# takes 200-300 seconds(!) when rotation_sensitivity == 9
def build_graph(self, save_path=None):
import time
start_time = time.time()
collide_res = self.env.house.n_row
from dijkstar import Graph
visit = dict()
self.graph = Graph()
self.mock_obs_map = np.zeros(
(collide_res + 1, collide_res + 1), dtype=np.uint8)
self.mock_obs_map[np.where(self.env.house.connMap == -1)] = 1
for x in range(collide_res + 1):
for y in range(collide_res + 1):
pos = (x, y)
if self.env.house.canMove(x, y) and pos not in visit:
que = [pos]
visit[pos] = True
ptr = 0
while ptr < len(que):
cx, cy = que[ptr]
ptr += 1
# add all angles for (cx, cy) here
# connect first and last
for ang in range(len(self.angles) - 1):
self.graph.add_edge((cx, cy, self.angles[ang]),
(cx, cy, self.angles[ang + 1]),
{
'cost': 1
})
self.graph.add_edge((cx, cy, self.angles[ang + 1]),
(cx, cy, self.angles[ang]), {
'cost': 1
})
self.graph.add_edge((cx, cy, self.angles[-1]),
(cx, cy, self.angles[0]), {
'cost': 1
})
self.graph.add_edge((cx, cy, self.angles[0]),
(cx, cy, self.angles[-1]), {
'cost': 1
})
for deti in range(len(self.dirs)):
det = self.dirs[deti]
tx, ty = cx + det[0], cy + det[1]
if (self.env.house.inside(tx, ty) and
self.mock_obs_map[min(cx, tx):max(cx, tx)+1,
min(cy, ty):max(cy, ty)+1].sum() == 0):
# make changes here to add edges for angle increments as well
#
# cost = 1 from one angle to the next,
# and connect first and last
# this would be for different angles for same tx, ty
#
# then there would be connections for same angle
# and from (cx, cy) to (tx, ty)
self.graph.add_edge(
(cx, cy, self.angle_map[self.dirs[deti]]),
(tx, ty, self.angle_map[self.dirs[deti]]),
{
'cost': 1
})
tp = (tx, ty)
if tp not in visit:
visit[tp] = True
que.append(tp)
if self.debug == True:
print("--- %s seconds to build the graph ---" %
(time.time() - start_time))
if save_path != None:
start_time = time.time()
print("saving graph to %s" % (save_path))
self.graph.dump(save_path)
if self.debug == True:
print("--- %s seconds to save the graph ---" %
(time.time() - start_time))
def load_graph(self, path):
import time
start_time = time.time()
from dijkstar import Graph
self.graph = Graph()
self.graph.load(path)
if self.debug == True:
print("--- %s seconds to load the graph ---" %
(time.time() - start_time))
# takes 1-5 seconds when rotation_sensitivity == 9
def compute_shortest_path(self, source, target, graph=None):
from dijkstar import find_path
if graph == None:
if self.graph == None:
if os.path.exists(
os.path.join(self.graph_dir,
self.env.house.house['id'] + '.pkl')):
self.load_graph(
os.path.join(self.graph_dir,
self.env.house.house['id'] + '.pkl'))
else:
self.build_graph(
save_path=os.path.join(
graph_dir, self.env.house.house['id'] + '.pkl'))
graph = self.graph
cost_func = lambda u, v, e, prev_e: e['cost']
shortest_path = find_path(graph, source, target, cost_func=cost_func)
return shortest_path
def fit_grid_path_to_suncg(self, nodes, init_yaw=None, back_skip=2):
# don't mess with the originals
nodes = copy.deepcopy(nodes)
# set initial position
x, y = self.env.house.to_coor(nodes[0][0], nodes[0][1], True)
x, y = x.astype(np.float32).item(), y.astype(np.float32).item()
self.env.cam.pos.x, self.env.cam.pos.y, self.env.cam.pos.z = x, self.env.house.robotHei, y
if init_yaw == None:
self.env.cam.yaw = np.random.choice(self.angles)
else:
self.env.cam.yaw = init_yaw
self.env.cam.updateDirection()
pos_queue, action_queue = [], []
current_pos = self._vec_to_array(self.env.cam.pos, self.env.cam.yaw)
pos_queue = pos_queue + [current_pos]
ptr = 0
while ptr < len(nodes) - 1:
turned = False
# target rotation
target_yaw = self.angle_map[tuple(
np.array(nodes[ptr]) - np.array(nodes[ptr + 1]))]
# turn
if target_yaw != current_pos[3]:
p_q, a_q = self.get_rotate_steps(current_pos, target_yaw)
pos_queue = pos_queue + p_q
action_queue = action_queue + a_q
self.env.cam.yaw = target_yaw
self.env.cam.updateDirection()
turned = True
current_pos = self._vec_to_array(self.env.cam.pos,
self.env.cam.yaw)
# move
cx, cz = self.env.house.to_coor(nodes[ptr + 1][0],
nodes[ptr + 1][1], True)
# if collision, find another sub-path, and delete that edge
if self.env.move(cx, cz) == False:
if nodes[ptr + 1] in self.graph[nodes[ptr]]:
del self.graph[nodes[ptr]][nodes[ptr + 1]]
print('deleted', nodes[ptr], nodes[ptr + 1])
# delete the turns
if turned == True:
pos_queue = pos_queue[:-len(p_q)]
action_queue = action_queue[:-len(a_q)]
if back_skip != 0:
pos_queue = pos_queue[:-back_skip]
action_queue = action_queue[:-back_skip]
dest_ptr = ptr + 1
ptr = ptr - back_skip
sub_shortest_path = self.compute_shortest_path(
nodes[ptr], nodes[dest_ptr])
nodes = nodes[:ptr] + sub_shortest_path.nodes + nodes[dest_ptr
+ 1:]
current_pos = pos_queue[-1]
else:
# this is the new position the agent moved to
current_pos = self._vec_to_array(self.env.cam.pos,
self.env.cam.yaw)
assert current_pos[3] == pos_queue[-1][3] and (
current_pos[0] != pos_queue[-1][0]
or current_pos[2] != pos_queue[-1][2])
pos_queue = pos_queue + [current_pos]
action_queue = action_queue + ['fwd']
ptr = ptr + 1
action_queue.append('stop')
return pos_queue, action_queue
# pos contains [x, y, z, yaw]
# given a position and target yaw, this function
# computes actions needed to turn there
def get_rotate_steps(self, pos, target_yaw):
direction = np.random.choice([1, -1])
cur_yaw = pos[-1]
ptr = self.angles.index(cur_yaw)
pos_queue, action_queue = [], []
while cur_yaw != target_yaw:
if len(pos_queue) == len(self.angles) // 2:
# reset
direction = direction * -1
cur_yaw = pos[-1]
ptr = self.angles.index(cur_yaw)
pos_queue, action_queue = [], []
ptr = (ptr + direction) % len(self.angles)
cur_yaw = self.angles[ptr]
pos_queue.append([pos[0], pos[1], pos[2], self.angles[ptr]])
action_queue.append(self.angle_strings[direction])
return pos_queue, action_queue
def _vec_to_array(self, pos, yaw):
return [pos.x, pos.y, pos.z, yaw]
# render images from camera position queue
def render_images_from_pos_queue(self,
pos_queue=[],
img_dir='tmp/images',
actions=None,
values=None,
rewards=None):
if len(pos_queue) == 0:
return False
action_map = {0: 'FRWD', 1: 'LEFT', 2: 'RGHT', 3: 'STOP'}
import scipy.misc
sgx, sgy = self.env.house.to_grid(pos_queue[0][0], pos_queue[0][2])
tgx, tgy = self.env.house.to_grid(pos_queue[-1][0], pos_queue[-1][2])
for i in range(len(pos_queue)):
# set position
p = pos_queue[i]
self.env.reset(x=p[0], y=p[2], yaw=p[3])
# save image
image = np.array(self.env.render(), copy=False)
# put some text
text = "[%02d]" % (i + 1)
if actions != None and i < len(actions):
text += "[%s]" % action_map[actions[i]]
if values != None and i < len(values):
text += "[V%.03f]" % values[i]
if rewards != None and i > 0 and i <= len(rewards):
text += "[R%.03f]" % rewards[i - 1]
image = cv2.putText(
img=np.copy(image),
text=text,
org=(20, 30),
fontFace=3,
fontScale=0.4,
color=(255, 255, 255),
thickness=1)
scipy.misc.toimage(image).save(
'%s/%s_%04d_%04d_%04d_%04d_%05d_%05d.jpg' %
(img_dir, self.env.house.house['id'], sgx, sgy, tgx, tgy,
i + 1, len(pos_queue)))
return True
# render video from camera position queue
#
# NOTE: call `render_images_from_pos_queue` before calling this
def render_video_from_pos_queue(self,
pos_queue=[],
img_dir='tmp/images',
vid_dir='tmp/videos',
fps=[5],
tag_name='piano'):
if len(pos_queue) == 0:
return False
import subprocess
sgx, sgy = self.env.house.to_grid(pos_queue[0][0], pos_queue[0][2])
tgx, tgy = self.env.house.to_grid(pos_queue[-1][0], pos_queue[-1][2])
for fp in fps:
subprocess.Popen([
'/srv/share/abhshkdz/local/bin/ffmpeg', '-f', 'image2', '-r',
str(fp), '-i',
'%s/%s_%04d_%04d_%04d_%04d' %
(img_dir, self.env.house.house['id'], sgx, sgy, tgx, tgy) +
'_%05d_' + '%05d.jpg' % (len(pos_queue)), '-vcodec', 'libx264',
'-crf', '25', '-y',
'%s/%s_%04d_%04d_%s_%04d_%04d_%d.mp4' %
(vid_dir, self.env.house.house['id'], sgx, sgy, tag_name, tgx,
tgy, fp)
])
if self.debug == True:
print('Rendered video to ' +
'%s/%s_%04d_%04d_%s_%04d_%04d_%d.mp4' %
(vid_dir, self.env.house.house['id'], sgx, sgy, tag_name,
tgx, tgy, fp))
return True
# Go over all nodes of house environment and accumulate objects room-wise.
def _parse(self, levelsToExplore=[0]):
rooms, objects = [], {}
data = self.env.house.house
modelCategoryMapping = {}
import csv
csvFile = csv.reader(open(self.env.house.metaDataFile, 'r'))
headers = next(csvFile)
for row in csvFile:
modelCategoryMapping[row[headers.index('model_id')]] = {
headers[x]: row[x]
for x in range(2, len(headers)) # 0 is index, 1 is model_id
}
for i in levelsToExplore:
for j in range(len(data['levels'][i]['nodes'])):
assert data['levels'][i]['nodes'][j]['type'] != 'Box'
if 'valid' in data['levels'][i]['nodes'][j]:
assert data['levels'][i]['nodes'][j]['valid'] == 1
# Rooms
if data['levels'][i]['nodes'][j]['type'] == 'Room':
if 'roomTypes' not in data['levels'][i]['nodes'][j]:
continue
# Can rooms have more than one type?
# Yes, they can; just found ['Living_Room', 'Dining_Room', 'Kitchen']
# assert len(data['levels'][i]['nodes'][j]['roomTypes']) <= 3
roomType = [
# ' '.join(x.lower().split('_'))
x.lower()
for x in data['levels'][i]['nodes'][j]['roomTypes']
]
nodes = data['levels'][i]['nodes'][j][
'nodeIndices'] if 'nodeIndices' in data['levels'][i][
'nodes'][j] else []
rooms.append({
'type':
roomType,
'bbox':
data['levels'][i]['nodes'][j]['bbox'],
'nodes':
nodes,
'model_id':
data['levels'][i]['nodes'][j]['modelId']
})
# Objects
elif data['levels'][i]['nodes'][j]['type'] == 'Object':
if 'materials' not in data['levels'][i]['nodes'][j]:
material = []
else:
material = data['levels'][i]['nodes'][j]['materials']
objects[data['levels'][i]['nodes'][j]['id']] = {
'id':
data['levels'][i]['nodes'][j]['id'],
'model_id':
data['levels'][i]['nodes'][j]['modelId'],
'fine_class':
modelCategoryMapping[data['levels'][i]['nodes'][j][
'modelId']]['fine_grained_class'],
'coarse_class':
modelCategoryMapping[data['levels'][i]['nodes'][j][
'modelId']]['coarse_grained_class'],
'bbox':
data['levels'][i]['nodes'][j]['bbox'],
'mat':
material
}
return rooms, objects
# Spawn at a randomly selected point in a particular room
def spawn_room(self, room=None):
if room == None:
return False, None
target_room = '_'.join(room.lower().split(' '))
if self.env.house.hasRoomType(target_room) == False:
return False, None
rooms = self.env.house._getRooms(target_room)
room = np.random.choice(rooms)
gx1, gy1, gx2, gy2 = self.env.house._getRoomBounds(room)
available_coords = []
for x in range(gx1, gx2 + 1):
for y in range(gy1, gy2 + 1):
if self.env.house.moveMap[x, y] > 0:
available_coords.append((x, y))
# print(available_coords)
spawn_coord_idx = np.random.choice(len(available_coords))
spawn_coord = available_coords[spawn_coord_idx]
return spawn_coord, room
# Spawn close to an object
# If room given, look for object within room
def spawn_object(self, obj=None, room=None):
if object == None:
return False, None
if isinstance(obj, list) == False:
obj = [obj]
is_door = False
if 'door' in obj:
is_door = True
target_obj = ['_'.join(x.lower().split(' ')) for x in obj]
if room != None:
if 'nodeIndices' in room:
objs = [
self.objects['0_' + str(x)] for x in room['nodeIndices']
if self.objects['0_' + str(x)]['fine_class'] in target_obj
]
else:
objs = [
self.objects['0_' + str(x)] for x in room['nodes']
if self.objects['0_' + str(x)]['fine_class'] in target_obj
]
else:
obj_id_list = list(
itertools.chain.from_iterable(
[x['nodes'] for x in self.rooms if x['type'] != []]))
objs = [
self.objects['0_' + str(x)] for x in obj_id_list
if self.objects['0_' + str(x)]['fine_class'] in target_obj
]
if len(objs) == 0:
return False, None, None
obj_idx = np.random.choice(len(objs))
obj = objs[obj_idx]
self.target_obj_class = obj['fine_class'].lower()
gx1, gy1, gx2, gy2 = self.env.house._getRoomBounds(obj)
if room == None:
obj_node_idx = int(obj['id'][2:])
room = [
x for x in self.env.house.all_rooms
if 'nodeIndices' in x and obj_node_idx in x['nodeIndices']
][0]
self.set_target_object(obj, room)
available_x, available_y = np.where(self.env.house.connMap == 0)
if len(available_x) == 0:
return False, None, None
spawn_coords = []
for i in range(len(available_x)):
spawn_coords.append((available_x[i], available_y[i]))
return spawn_coords, obj, room
# analogous to `setTargetRoom` in the House3D API
def set_target_object(self, obj, room):
object_tp = room['id'] + '_' + obj['id'] + '_' + obj['fine_class'].lower(
)
# Caching
if object_tp in self.env.house.connMapDict:
self.env.house.connMap, self.env.house.connectedCoors, self.env.house.inroomDist, self.env.house.maxConnDist = self.env.house.connMapDict[
object_tp]
return True # object changed!
elif os.path.exists(
os.path.join(
self.target_obj_conn_map_dir,
self.env.house.house['id'] + '_' + object_tp + '.npy')):
self.env.house.connMap = np.load(
os.path.join(
self.target_obj_conn_map_dir,
self.env.house.house['id'] + '_' + object_tp + '.npy'))
if self.env.house.connMap.shape[0] == self.env.house.n_row+1:
self.env.house.connectedCoors, self.env.house.inroomDist, self.env.house.maxConnDist = None, None, None
return True
self.env.house.connMap = connMap = np.ones(
(self.env.house.n_row + 1, self.env.house.n_row + 1),
dtype=np.int32) * -1
self.env.house.inroomDist = inroomDist = np.ones(
(self.env.house.n_row + 1, self.env.house.n_row + 1),
dtype=np.float32) * -1
dirs = [[0, 1], [1, 0], [-1, 0], [0, -1]]
que = []
flag_find_open_components = True
_ox1, _, _oy1 = obj['bbox']['min']
_ox2, _, _oy2 = obj['bbox']['max']
ocx, ocy = (_ox1 + _ox2) / 2, (_oy1 + _oy2) / 2
ox1, oy1, ox2, oy2 = self.env.house.rescale(_ox1, _oy1, _ox2, _oy2)
for _ in range(2):
_x1, _, _y1 = room['bbox']['min']
_x2, _, _y2 = room['bbox']['max']
cx, cy = (_x1 + _x2) / 2, (_y1 + _y2) / 2
x1, y1, x2, y2 = self.env.house.rescale(_x1, _y1, _x2, _y2)
curr_components = self.env.house._find_components(
x1,
y1,
x2,
y2,
dirs=dirs,
return_open=flag_find_open_components
) # find all the open components
if len(curr_components) == 0:
print('No space found! =(')
raise ValueError('no space')
if isinstance(curr_components[0],
list): # join all the coors in the open components
curr_major_coors = list(itertools.chain(*curr_components))
else:
curr_major_coors = curr_components
min_dist_to_center, min_dist_to_edge = 1e50, 1e50
for x, y in curr_major_coors:
###
# Compute minimum dist to edge here
if x in range(ox1, ox2):
dx = 0
elif x < ox1:
dx = ox1 - x
else:
dx = x - ox2
if y in range(oy1, oy2):
dy = 0
elif y < oy1:
dy = oy1 - y
else:
dy = y - oy2
assert dx >= 0 and dy >= 0
if dx != 0 or dy != 0:
dd = np.sqrt(dx**2 + dy**2)
elif dx == 0:
dd = dy
else:
dd = dx
if dd < min_dist_to_edge:
min_dist_to_edge = int(np.ceil(dd))
###
tx, ty = self.env.house.to_coor(x, y)
tdist = np.sqrt((tx - ocx)**2 + (ty - ocy)**2)
if tdist < min_dist_to_center:
min_dist_to_center = tdist
inroomDist[x, y] = tdist
margin = min_dist_to_edge + 1
for x, y in curr_major_coors:
inroomDist[x, y] -= min_dist_to_center
for x, y in curr_major_coors:
if x in range(ox1 - margin, ox2 + margin) and y in range(
oy1 - margin, oy2 + margin):
connMap[x, y] = 0
que.append((x, y))
if len(que) > 0: break
if flag_find_open_components:
flag_find_open_components = False
else:
break
raise ValueError
ptr = 0
self.env.house.maxConnDist = 1
while ptr < len(que):
x, y = que[ptr]
cur_dist = connMap[x, y]
ptr += 1
for dx, dy in dirs:
tx, ty = x + dx, y + dy
if self.env.house.inside(tx, ty) and self.env.house.canMove(
tx, ty) and not self.env.house.isConnect(tx, ty):
que.append((tx, ty))
connMap[tx, ty] = cur_dist + 1
if cur_dist + 1 > self.env.house.maxConnDist:
self.env.house.maxConnDist = cur_dist + 1
self.env.house.connMapDict[object_tp] = (connMap, que, inroomDist,
self.env.house.maxConnDist)
np.save(
os.path.join(
self.target_obj_conn_map_dir,
self.env.house.house['id'] + '_' + object_tp + '.npy'),
connMap)
self.connectedCoors = que
print(' >>>> ConnMap Cached!')
return True # room changed!
def _load_semantic_classes(self, color_file=None):
if color_file == None:
color_file = self.env.config['colorFile']
self.semantic_classes = {}
with open(color_file) as csv_file:
reader = csv.DictReader(csv_file)
for row in reader:
c = np.array((row['r'], row['g'], row['b']), dtype=np.uint8)
fine_cat = row['name'].lower()
self.semantic_classes[fine_cat] = c
return self.semantic_classes
def _get_best_yaw_obj_from_pos(self, obj_id, grid_pos, height=1.0):
obj = self.objects[obj_id]
obj_fine_class = obj['fine_class']
cx, cy = self.env.house.to_coor(grid_pos[0], grid_pos[1])
self.env.cam.pos.x = cx
self.env.cam.pos.y = height
self.env.cam.pos.z = cy
best_yaw, best_coverage = None, 0
for yaw in self.angles:
self.env.cam.yaw = yaw
self.env.cam.updateDirection()
seg = self.env.render(mode='semantic')
c = self.semantic_classes[obj_fine_class.lower()]
mask = np.all(seg == c, axis=2)
coverage = np.sum(mask) / (seg.shape[0] * seg.shape[1])
if best_yaw == None:
best_yaw = yaw
best_coverage = coverage
else:
if coverage > best_coverage:
best_yaw = yaw
best_coverage = coverage
return best_yaw, best_coverage
def _get_best_view_obj(self,
obj,
coverage_thres=0.5,
dist_add=0.5,
robot_height=False):
bbox = obj['bbox']
obj_fine_class = obj['fine_class']
obj_max = np.asarray(bbox['max'])
obj_min = np.asarray(bbox['min'])
obj_center = (obj_min + obj_max) / 2
c_x, c_y, c_z = obj_center
max_radius = np.sqrt(
(obj_max[0] - obj_min[0]) * (obj_max[0] - obj_min[0]) +
(obj_max[2] - obj_min[2]) * (obj_max[2] - obj_min[2])) / 2.0
max_radius += dist_add
best_pos = None
best_coverage = 0
returned_pos_cov = []
for yaw in self.angles:
pos = [
c_x - max_radius * np.cos(yaw * (2 * np.pi) / 360.0), c_y,
c_z - max_radius * np.sin(yaw * (2 * np.pi) / 360.0), yaw
]
if robot_height == True:
pos[1] = min(max(0.75, c_y), 2.00)
self.env.cam.pos.x = pos[0]
self.env.cam.pos.y = pos[1]
self.env.cam.pos.z = pos[2]
self.env.cam.yaw = pos[3]
self.env.cam.updateDirection()
seg = self.env.render(mode='semantic')
c = self.semantic_classes[obj_fine_class.lower()]
mask = np.all(seg == c, axis=2)
coverage = np.sum(mask) / (seg.shape[0] * seg.shape[1])
returned_pos_cov.append([pos, coverage])
if coverage > coverage_thres:
return pos, coverage, returned_pos_cov
elif coverage > best_coverage:
best_coverage = coverage
best_pos = pos
return best_pos, best_coverage, returned_pos_cov
