def decide_what_to_do(self):
distance_to_goal = self.obs["pointgoal"][0]
angle_to_goal = norm_ang(np.array(self.obs["pointgoal"][1]))
command = SimulatorActions.STOP.value
if distance_to_goal <= self.pos_th:
return command
if abs(angle_to_goal) < self.angle_th:
command = SimulatorActions.FORWARD.value
else:
if (angle_to_goal > 0) and (angle_to_goal < pi):
command = SimulatorActions.LEFT.value
elif angle_to_goal > pi:
command = SimulatorActions.RIGHT.value
elif (angle_to_goal < 0) and (angle_to_goal > -pi):
command = SimulatorActions.RIGHT.value
else:
command = SimulatorActions.LEFT.value
return command
def decide_what_to_do(self):
distance_to_goal = self.obs[GOAL_SENSOR_UUID][0]
angle_to_goal = norm_ang(np.array(self.obs[GOAL_SENSOR_UUID][1]))
command = HabitatSimActions.STOP
if distance_to_goal <= self.pos_th:
return command
if abs(angle_to_goal) < self.angle_th:
command = HabitatSimActions.MOVE_FORWARD
else:
if (angle_to_goal > 0) and (angle_to_goal < pi):
command = HabitatSimActions.TURN_LEFT
elif angle_to_goal > pi:
command = HabitatSimActions.TURN_RIGHT
elif (angle_to_goal < 0) and (angle_to_goal > -pi):
command = HabitatSimActions.TURN_RIGHT
else:
command = HabitatSimActions.TURN_LEFT
return command
def planner_prediction_to_command(self, p_next):
command = HabitatSimActions.STOP
p_init = self.pose6D.squeeze()
d_angle_rot_th = self.angle_th
pos_th = self.pos_th
if get_distance(p_init, p_next) <= pos_th:
return command
d_angle = norm_ang(
get_direction(p_init, p_next, ang_th=d_angle_rot_th, pos_th=pos_th)
)
if abs(d_angle) < d_angle_rot_th:
command = HabitatSimActions.MOVE_FORWARD
else:
if (d_angle > 0) and (d_angle < pi):
command = HabitatSimActions.TURN_LEFT
elif d_angle > pi:
command = HabitatSimActions.TURN_RIGHT
elif (d_angle < 0) and (d_angle > -pi):
command = HabitatSimActions.TURN_RIGHT
else:
command = HabitatSimActions.TURN_LEFT
return command
def plan_exploration(self, observation):
"""
If just exploring, shoot rays outwards from current location, and go to the ray
that can go the farthest without hitting and object
"""
num_rays = 72
curr_rays = [2 * np.pi / num_rays * i for i in range(num_rays)]
dist_step = 0.1
max_dist = 10
location_in_obs_map = (observation['gps'] -
self.start_location).astype(int) * 10 + 200
if 'numpy' in str(type(self.map2DObstacles)):
np_obs_map = (self.map2DObstacles > self.obstacle_th).astype(
np.uint8)
else:
np_obs_map = (self.map2DObstacles[0, 0].cpu().numpy() >
self.obstacle_th).astype(np.uint8)
# kernel for image closing
kernel = np.ones((5, 5), np.uint8)
np_obs_map = cv2.morphologyEx(np_obs_map, cv2.MORPH_CLOSE, kernel)
best_ray = None
best_coord = None
best_dist = None
for n_steps in range(int(max_dist / dist_step)):
curr_dist = n_steps * dist_step
next_rays = []
for ray in curr_rays:
y_coord = int(
np.cos(ray) * curr_dist * 10 + location_in_obs_map[0])
x_coord = int(-np.sin(ray) * curr_dist * 10 +
location_in_obs_map[1])
if np_obs_map[y_coord, x_coord] != 1:
next_rays.append(ray)
# color in map to visualize
np_obs_map[y_coord, x_coord] = 2
if curr_dist == best_dist:
curr_ray_angle_delta = norm_ang(
ray - float(observation['compass']))
best_ray_angle_delta = norm_ang(
best_ray - float(observation['compass']))
if abs(curr_ray_angle_delta) > abs(
best_ray_angle_delta):
continue
best_dist = curr_dist
best_coord = [y_coord, x_coord]
best_ray = ray
if len(next_rays) > 0:
curr_rays = next_rays
else:
break
if best_dist is None:
return None
# color in best ray:
for n_steps in range(int(best_dist // dist_step)):
curr_dist = n_steps * dist_step
y_coord = int(
np.cos(best_ray) * curr_dist * 10 + location_in_obs_map[0])
x_coord = int(-np.sin(best_ray) * curr_dist * 10 +
location_in_obs_map[1])
np_obs_map[y_coord, x_coord] = 4
self.obstacle_ray_map = np_obs_map
# translate coord to absolute and relative location
best_gps_coord = np.array([
(y_coord - 200) / 10,
(x_coord - 200) / 10,
]) + self.start_location
best_relative_coord = best_gps_coord - observation['gps']
rotation_from_start = observation['compass'] - self.start_rotation
# assuming compass is counterclockwise from y+
best_relative_polar_coord = np.array([
np.linalg.norm(best_relative_coord),
# substract 90 deg because arctan is from x+ axis
float((np.arctan2(best_relative_coord[0], best_relative_coord[1]) -
np.pi / 2) - observation['compass'])
])
print('start location {}, start rotation {}'.format(
self.start_location, self.start_rotation))
print('''ray {}, dist {}, coord {}, gps coord {}, my gps {},
my compass {}, relative coord {}, rel polar coord {}'''.format(
best_ray,
max_dist,
best_coord,
best_gps_coord,
observation['gps'],
observation['compass'],
best_relative_coord,
best_relative_polar_coord,
))
return best_relative_polar_coord
