def planner_prediction_to_command(self, p_next):
command = SimulatorActions.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 = SimulatorActions.MOVE_FORWARD
else:
if (d_angle > 0) and (d_angle < pi):
command = SimulatorActions.TURN_LEFT
elif d_angle > pi:
command = SimulatorActions.TURN_RIGHT
elif (d_angle < 0) and (d_angle > -pi):
command = SimulatorActions.TURN_RIGHT
else:
command = SimulatorActions.TURN_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()
