in exploring_exploration/models/navigation.py [0:0]
def act(self, occupancy_map, goal_loc, prev_delta, collision_prev_step):
if self.map_size is None:
self.map_size = occupancy_map.shape[0]
# If the target has already been reached, return STOP.
if self.has_reached_target(goal_loc):
return self.action_space["stop"]
self.high_level_target = goal_loc
path_feasible = True
# Is the current path from the intermediate goal to the target feasible?
if self.low_level_target is not None:
# Update the low_level_target using prev_delta
self.update_target(prev_delta)
# Also update the planned path
updated_path = [[], []]
for x, y in zip(*self.planned_path):
tx, ty = self.ego_transform((x, y), prev_delta)
updated_path[0].append(tx)
updated_path[1].append(ty)
self.planned_path = updated_path
intermediate_path = [
self.planned_path[0][self.intermediate_goal_idx :],
self.planned_path[1][self.intermediate_goal_idx :],
]
path_feasible = self.is_feasible_path(occupancy_map, intermediate_path)
# If a low_level_target has not been sampled, sample one.
if self.low_level_target is None or not path_feasible:
reached_target = self.sample_low_level_target(
occupancy_map, self.high_level_target, collision_prev_step
)
if reached_target:
return self.action_space["stop"]
elif self.low_level_target is None: # Failed to sample low_level_target
# print('======> HighLevelPlanner failed to sample any valid targets!')
return self._rng.choice(
[
self.action_space["left"],
self.action_space["right"],
self.action_space["forward"],
]
)
# Sample an action that takes you to the low_level_target
low_level_action = self.low_level_actor.act(
occupancy_map, self.low_level_target, collision_prev_step
)
# If the low_level_actor reached the low_level_target, return a random action and
# set low_level_target to None
if low_level_action == self.action_space["stop"]:
low_level_action = self._rng.choice(
[
self.action_space["left"],
self.action_space["right"],
self.action_space["forward"],
]
)
self.low_level_target = None
self.low_level_actor.reset()
# If the low_level_actor failed in planning, return a random action and
# set low_level_target to None
elif self.low_level_actor.planning_failure_flag:
low_level_action = self._rng.choice(
[
self.action_space["left"],
self.action_space["right"],
self.action_space["forward"],
]
)
self.low_level_target = None
self.low_level_actor.reset()
if self._enable_planning_visualization:
vis_occ_map = np.copy(occupancy_map)
# Draw final goal in red
vis_occ_map = cv2.circle(vis_occ_map, goal_loc, 7, (255, 0, 0), -1)
# Draw planned path to goal in gray
for x, y in zip(*self.planned_path):
vis_occ_map = cv2.circle(vis_occ_map, (x, y), 3, (128, 128, 128), -1)
# Draw intermediate path in black
intermediate_path = [
self.planned_path[0][self.intermediate_goal_idx :],
self.planned_path[1][self.intermediate_goal_idx :],
]
for x, y in zip(*intermediate_path):
vis_occ_map = cv2.circle(vis_occ_map, (x, y), 3, (0, 0, 0), -1)
# Draw intermediate goal in pink
if self.low_level_target is not None:
vis_occ_map = cv2.circle(
vis_occ_map, self.low_level_target, 7, (255, 192, 203), -1
)
self._final_planning_visualization = vis_occ_map
else:
self._final_planning_visualization = None
if self.show_animation:
cv2.imshow("Hierarchical planning status", np.flip(vis_occ_map, axis=2))
cv2.waitKey(5)
self.past_actions.append(low_level_action)
# If caught in rotation, try to break out of it.
# if self.rotation_stuck_status or self._caught_in_rotation():
# self.rotation_stuck_status = True
# self.rotation_stuck_timer += 1
# if self.rotation_stuck_timer > self.rotation_stuck_thresh:
# self.rotation_stuck_status = False
# self.rotation_stuck_timer = 0
# elif self.rotation_stuck_timer % 2 == 0:
# action = self.action_space['forward']
# else:
# action = self.action_space['left']
return low_level_action