def find_empty_right_marker(self, left_marker, pix_dist_between_markers): marker_w_top_dx = left_marker.corners[1].x - left_marker.corners[0].x marker_w_bot_dx = left_marker.corners[2].x - left_marker.corners[3].x w_dx = np.mean([marker_w_bot_dx, marker_w_top_dx]) marker_h_left_dy = left_marker.corners[1].y - left_marker.corners[0].y marker_h_right_dy = left_marker.corners[2].y - left_marker.corners[3].y h_dy = np.mean([marker_h_left_dy, marker_h_right_dy]) w1 = get_distance_between_points(left_marker.corners[0], left_marker.corners[1]) w2 = get_distance_between_points(left_marker.corners[3], left_marker.corners[2]) cos_a1, cos_a2 = float(w_dx) / float(w1), float(w_dx) / float(w2) sin_a1, sin_a2 = float(h_dy) / float(w1), float(h_dy) / float(w2) cos_a = np.mean([cos_a1, cos_a2]) sin_a = np.mean([sin_a1, sin_a2]) dist_dx = pix_dist_between_markers * cos_a dist_dy = pix_dist_between_markers * sin_a corner0 = (left_marker.corners[1].x + dist_dx, left_marker.corners[1].y + dist_dy) corner3 = (left_marker.corners[2].x + dist_dx, left_marker.corners[2].y + dist_dy) corner1 = (left_marker.corners[1].x + dist_dx + w_dx, left_marker.corners[1].y + dist_dy + h_dy) corner2 = (left_marker.corners[2].x + dist_dx + w_dx, left_marker.corners[2].y + dist_dy + h_dy) corners = [corner0, corner1, corner2, corner3] return Marker(left_marker.id, corners)
def get_mean_distance_between_markers_in_pairs(self, pairs): if not len(pairs): return None for pair in pairs: left_marker_top_right_point = pair.left_marker.corners[1] left_marker_bot_right_point = pair.left_marker.corners[2] right_marker_top_left_point = pair.right_marker.corners[0] right_marker_bot_left_point = pair.right_marker.corners[3] top_corners_distance = get_distance_between_points( left_marker_top_right_point, right_marker_top_left_point) bot_corners_distance = get_distance_between_points( left_marker_bot_right_point, right_marker_bot_left_point) return np.mean([top_corners_distance, bot_corners_distance])
def get_path_length(self, path): path.append(self.planner.finish) path_length = 0 for i in range(1, len(path)): pt1 = path[i - 1] pt2 = path[i] path_length += get_distance_between_points(pt1, pt2) return path_length
def get_nearest_line_and_projection_of_robot_on_line(robot, line1, line2): line1_pt1, line1_pt2 = line1 line2_pt1, line2_pt2 = line2 proj_to_line1 = get_point_projection_on_line(robot.center, line1_pt1, line1_pt2) proj_to_line2 = get_point_projection_on_line(robot.center, line2_pt1, line2_pt2) distance_to_line1 = get_distance_between_points( robot.center, proj_to_line1) distance_to_line2 = get_distance_between_points( robot.center, proj_to_line2) if distance_to_line1 <= distance_to_line2: return line1, proj_to_line1 else: return line2, proj_to_line2
def prepare_move_msg(self, dst_point): moving_angle = degrees( get_angle_by_3_points(self.robot.direction, dst_point, self.robot.center)) angle_sign = get_angle_sign_pt_to_line(dst_point, self.robot.center, self.robot.direction) moving_angle *= angle_sign msg = ActionAtTime() msg.angle = self.angle_to_sector(angle_to_180(moving_angle)) msg.movement = True msg.rotation = False if get_distance_between_points(self.robot.center, dst_point) > 25: msg.time = self.get_moving_time( get_distance_between_points(self.robot.center, dst_point)) else: msg.time = 300 return msg
def get_best_free_robot_and_path(self, robots_and_paths, dst_pos): min_distance = 10000 best_robot_and_path = None for r_id, path_point in robots_and_paths: distance = get_distance_between_points(path_point, dst_pos) if distance < min_distance: min_distance = distance best_robot_and_path = (r_id, path_point) return best_robot_and_path
def get_nearest_robot_with_point_in_obs_contour(self, point, robots): min_dist = 10000000 nearest_robot_id = None for robot in robots: dist = get_distance_between_points(point, robot.center) if dist <= min_dist: min_dist = dist nearest_robot_id = robot.id return nearest_robot_id
def get_anomaly_values_idxs(self, data_list): idxs = [] if len(data_list) == self.list_size: mean_distances = [] for i in range(len(data_list)): cp_data = deepcopy(data_list) p = cp_data.pop(i) distances = [get_distance_between_points(p, pt) for pt in data_list] mean_distances.append(np.mean(distances)) for i, distance in enumerate(mean_distances): if distance >= self.distance_eps: idxs.append(i) return idxs
def get_nearest_obs_position(self, free_robot_id, obstacles_for_free_robot): # print("get_nearest", free_robot_id, obstacles_for_free_robot=[]) min_dist = 10000 nearest_obs_pos = None robot_pos = [r for r in self.robots if r.id == free_robot_id][0].center for obs in obstacles_for_free_robot: obs_pos = [r for r in self.robots if r.id == obs][0].center distance = get_distance_between_points(robot_pos, obs_pos) if distance < min_dist: min_dist = distance nearest_obs_pos = obs_pos return robot_pos, nearest_obs_pos
def get_optimal_robot_by_axe(robots, robot_center_axe_point, min_axe_val, max_axe_val, dst_pos): if robot_center_axe_point == "x": robot_with_max_axe_val = [ robot for robot in robots if robot.center.x == max_axe_val ][0] robot_with_min_axe_val = [ robot for robot in robots if robot.center.x == min_axe_val ][0] else: robot_with_max_axe_val = [ robot for robot in robots if robot.center.y == max_axe_val ][0] robot_with_min_axe_val = [ robot for robot in robots if robot.center.y == min_axe_val ][0] max_axe_val_dist = get_distance_between_points( robot_with_max_axe_val.center, dst_pos) min_axe_val_dist = get_distance_between_points( robot_with_min_axe_val.center, dst_pos) optimal_robot = robot_with_max_axe_val if max_axe_val_dist <= min_axe_val_dist else robot_with_min_axe_val return optimal_robot
def get_optimal_robot_if_stacked_all_obstacles(self, robots, dst_pos): x_lst = [robot.center.x for robot in robots] y_lst = [robot.center.y for robot in robots] min_x, max_x = min(x_lst), max(x_lst) min_y, max_y = min(y_lst), max(y_lst) dx, dy = max_x - min_x, max_y - min_y if dx > dy: optimal_robot = self.get_optimal_robot_by_axe( robots, "x", min_x, max_x, dst_pos) elif dx < dy: optimal_robot = self.get_optimal_robot_by_axe( robots, "y", min_y, max_y, dst_pos) else: optimal_x_robot = self.get_optimal_robot_by_axe( robots, "x", min_x, max_x, dst_pos) optimal_y_robot = self.get_optimal_robot_by_axe( robots, "y", min_y, max_y, dst_pos) x_dist = get_distance_between_points(optimal_x_robot.center, dst_pos) y_dist = get_distance_between_points(optimal_y_robot.center, dst_pos) optimal_robot = optimal_x_robot if x_dist <= y_dist else optimal_y_robot return optimal_robot
def prepare_movement_msg(self, dst_point): msg = ActionAtTime() msg.movement = True msg.rotation = False moving_angle = degrees( get_angle_by_3_points(dst_point, self.robot.direction, self.robot.center)) moving_angle_sign = get_angle_sign_pt_to_line(dst_point, self.robot.center, self.robot.direction) msg.angle = angle_to_180( self.angle_to_sector(moving_angle * moving_angle_sign)) # TODO check this msg.time = self.get_moving_time( get_distance_between_points(self.robot.center, dst_point)) return msg
def prepare_msg(self): robot_turning_angle = self.get_turning_angle() robot_turning_angle = angle_to_180(robot_turning_angle) if abs(robot_turning_angle) > self.angle_eps: msg = self.prepare_rotation_msg(robot_turning_angle) else: robot_projection_on_section = self.get_robots_projection_on_section_line( ) dist_to_proj = get_distance_between_points( self.robot.center, robot_projection_on_section) dst_point = self.path_section[ -1] if dist_to_proj <= self.distance_eps else robot_projection_on_section if not self.robot_on_point(dst_point): msg = self.prepare_movement_msg(dst_point) else: msg = self.prepare_stop_msg() return msg
def get_pix_distance_between_markers(self): return get_distance_between_points(self.left_marker.center, self.right_marker.center)
def robot_on_point(self, point): if get_distance_between_points(self.robot.center, point) <= self.distance_eps: return True else: return False