in utils/house3d.py [0:0]
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))