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()