def update_internal_state(self, habitat_observation):
super(ORBSLAM2Agent, self).update_internal_state(
habitat_observation
)
self.cur_time += self.timestep
rgb, depth = self.rgb_d_from_observation(habitat_observation)
t = time.time()
try:
self.slam.process_image_rgbd(rgb, depth, self.cur_time)
if self.timing:
print(time.time() - t, "ORB_SLAM2")
self.tracking_is_OK = str(self.slam.get_tracking_state()) == "OK"
except BaseException:
print("Warning!!!! ORBSLAM processing frame error")
self.tracking_is_OK = False
if not self.tracking_is_OK:
self.reset()
t = time.time()
self.set_offset_to_goal(habitat_observation)
if self.tracking_is_OK:
trajectory_history = np.array(self.slam.get_trajectory_points())
self.pose6D = homogenize_p(
torch.from_numpy(trajectory_history[-1])[1:]
.view(3, 4)
.to(self.device)
).view(1, 4, 4)
self.trajectory_history = trajectory_history
if len(self.position_history) > 1:
previous_step = get_distance(
self.pose6D.view(4, 4),
torch.from_numpy(self.position_history[-1])
.view(4, 4)
.to(self.device),
)
if (
SIM_ACTION_TO_NAME[self.action_history[-1]]
== "move_forward"
):
self.unseen_obstacle = (
previous_step.item() <= 0.001
) # hardcoded threshold for not moving
current_obstacles = self.mapper(
torch.from_numpy(depth).to(self.device).squeeze(), self.pose6D
).to(self.device)
self.current_obstacles = current_obstacles
self.map2DObstacles = torch.max(
self.map2DObstacles, current_obstacles.unsqueeze(0).unsqueeze(0)
)
if self.timing:
print(time.time() - t, "Mapping")
return True
def get_waypoint_dist_dir(self):
angle = get_direction(
self.pose6D.squeeze(), self.waypointPose6D.squeeze(), 0, 0
)
dist = get_distance(
self.pose6D.squeeze(), self.waypointPose6D.squeeze()
)
return torch.cat(
[
dist.view(1, 1),
torch.sin(angle).view(1, 1),
torch.cos(angle).view(1, 1),
],
dim=1,
)
def planner_prediction_to_command(self, p_next):
command = "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 = angle_to_pi_2_minus_pi_2(
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 = "move_forward"
else:
if (d_angle > 0) and (d_angle < pi):
command = "look_left"
elif d_angle > pi:
command = "look_right"
elif (d_angle < 0) and (d_angle > -pi):
command = "look_right"
else:
command = "look_left"
return command
def is_waypoint_reached(self, pose6d):
p_init = self.pose6D.squeeze()
dist_diff = get_distance(p_init, pose6d)
reached = dist_diff <= self.pos_th
return reached.item()
def is_waypoint_good(self, pose6d):
p_init = self.pose6D.squeeze()
dist_diff = get_distance(p_init, pose6d)
valid = dist_diff > self.next_wp_th
return valid.item()