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