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
