def get_projection_of_pt_on_line(point, line_point1, line_point2): """ This func helps to find angle sign.""" projection = Point(-1, -1) projection.x = point.x if (line_point2.x - line_point1.x) != 0: projection.y = (projection.x - line_point1.x) * (line_point2.y - line_point1.y) / \ (line_point2.x - line_point1.x) + line_point1.y else: projection.y = (projection.x - line_point1.x) * (line_point2.y - line_point1.y) / 1 + line_point1.y return projection
def find_4th_point(cnt_pt, side_point, front_back_point, scale_size): x1, y1 = cnt_pt.x, cnt_pt.y x2, y2 = side_point.x, side_point.y x3, y3 = front_back_point.x, front_back_point.y x = x1 + (x2 - x1) * scale_size + (x3 - x1) * scale_size * 2 y = y1 + (y2 - y1) * scale_size + (y3 - y1) * scale_size * 2 return Point(x, y)
def __init__(self, id, corners, x_pos=None, y_pos=None): self.id = id self.corners = list(Point(*corner) for corner in corners) self.center = self.get_center(corners) self.direction = self.get_direction_point() self.x_pos = x_pos self.y_pos = y_pos
def check_out_dst_point(self, point): x, y = point.to_list() min_x = out + robot_size max_x = field_w * 2 - dx - out - robot_size # max_x = 640 - out - robot_size min_y = out + robot_size # max_y = field_h*2 - dy - out - robot_size max_y = field_h - dy - out - robot_size # max_y = 480 - out - robot_size x, y = self.ball_pos.to_list() if x <= min_x: new_x = min_x elif x >= max_x: new_x = max_x else: new_x = x if y <= min_y: new_y = min_y elif y >= max_y: new_y = max_y else: new_y = y return Point(new_x, new_y)
def get_vision_area_crossing_point(self, robot, size, hit_point): cX, cY = robot.position.x, robot.position.y R = size dx, dy = cX - hit_point.x, cY - hit_point.y dist = get_distance_between_points(robot.position, hit_point) eps = R / dist return Point(cX-(dx*eps), cY-(dy*eps))
def get_real_world_position(self): pos1 = self.left_marker.center pos2 = self.right_marker.center x = (pos1.x + pos2.x) / 2 y = (pos1.y + pos2.y) / 2 return Point(x, y)
def get_path_point_for_most_free_robot(self, free_robot_pos, nearest_obstacle_pos): # print(free_robot_pos, nearest_obstacle_pos=NONE) dx = nearest_obstacle_pos.x - free_robot_pos.x dy = nearest_obstacle_pos.y - free_robot_pos.y new_x = free_robot_pos.x - dx new_y = free_robot_pos.y - dy return Point(new_x, new_y)
def get_direction_point(self): x_lst = [self.corners[0].x, self.corners[1].x] y_lst = [self.corners[0].y, self.corners[1].y] x = np.mean(x_lst) y = np.mean(y_lst) dx = (x - self.center.x) * 3 dy = (y - self.center.y) * 3 dir = Point(self.center.x + dx, self.center.y + dy) return dir
def estimate_ball_position(self, ball, dx=0, dy=0): if ball.x_posistion == 'left': x = ball.center.x else: x = ball.center.x + self.field_w - dx if ball.y_position == 'top': y = ball.center.y else: y = ball.center.y + self.field_h - dy return Point(x, y)
def gates_callback(self, gates_msg): for gate in gates_msg.gates: open_side = Point(gate.open_side.x, gate.open_side.y) if self.center is not None: if self.center.x is not None and self.center.y is not None: dx, dy = self.get_dxdy(open_side) hit_point = self.get_hit_point(dx, dy, open_side) hit_point_msg = Point2d() hit_point_msg.x, hit_point_msg.y = hit_point.x, hit_point.y self.hit_point_pub.publish(hit_point_msg)
def get_point_projection_on_line(pt, line_pt1, line_pt2): point = shPoint(pt.x, pt.y) line = LineString([(line_pt1.x, line_pt1.y), (line_pt2.x, line_pt2.y)]) x = np.array(point.coords[0]) u = np.array(line.coords[0]) v = np.array(line.coords[len(line.coords)-1]) n = v - u n /= np.linalg.norm(n, 2) P = u + n*np.dot(x - u, n) P = Point(P[0], P[1]) return P
def ball_callback(self, msg): self.ball.update(Point(msg.x, msg.y), self.robot_size) self.finish_point = self.ball.position for robot in self.robots_obstacles.values(): if robot.id != self.current_id: if robot.contour is not None: if self.check_is_obstacle_in_vision_area(robot.contour): if self.finish_point is not None: dst_pt = self.l_p if get_distance_between_points(self.l_p, robot.position) >= \ get_distance_between_points(self.r_p, robot.position) else \ self.r_p if abs(degrees(get_angle_by_3_points(self.finish_point, robot.position, self.start_point))) <= 90: self.finish_point = Point(dst_pt.x, dst_pt.y) if self.finish_point is not None: pmsg = FinishPoint() pmsg.id = self.current_id pmsg.point = Point2d(self.finish_point.x, self.finish_point.y) self.final_point_pub.publish(pmsg)
def get_ball_position_on_map(self, dx=0, dy=0): x, y = [], [] for ball in self.balls_list: center = self.estimate_ball_position(ball, dx, dy) x.append(center.x) y.append(center.y) if len(x) and len(y): center = Point(np.mean(x), np.mean(y)) return center else: return None
def get_vector_coords(start_pt, end_pt): try: x1, y1 = start_pt.x, start_pt.y except: x1, y1 = start_pt[0], start_pt[1] try: x2, y2 = end_pt.x, end_pt.y except: x2, y2 = end_pt[0], end_pt[1] vec_coords = ((x2 - x1), (y2- y1)) return Point(*vec_coords)
def finish_point_callback(self, msg): if not self.stop: attacker_id = msg.id finish_point = Point(msg.point.x, msg.point.y) for robot in self.robots.values(): if robot.id in attackers and robot.id == attacker_id: f_point = self.check_out_dst_point(finish_point) robot.set_movement_point(f_point) else: for robot in self.robots.values(): self.ball_pos = None robot.stop()
def robot_callback(self, msg): for robot in msg.robots: if robot.id not in self.robots_obstacles.keys(): self.robots_obstacles[robot.id] = RobotObstacle(robot.id, self.robot_size) self.robots_obstacles[robot.id].update(robot.center, robot.corners) if robot.id == self.current_id: self.vision_area = robot.vision_contour self.r_p, self.l_p = self.vision_area[-2], self.vision_area[2] self.vis_radius = get_distance_between_points(robot.center, self.vision_area[3]) for i in range(3): self.vision_area = self.append_contours(self.vision_area) self.start_point = Point(robot.center.x, robot.center.y)
def get_center(self, corners): pt0 = self.corners[0] pt1 = self.corners[1] pt2 = self.corners[2] pt3 = self.corners[3] cntr1 = get_line_cntr(pt0, pt2) cntr2 = get_line_cntr(pt1, pt3) x = np.mean([cntr1.x, cntr2.x]) y = np.mean([cntr1.y, cntr2.y]) return Point(x, y)
def cv_callback(self, event, x, y, flags, param): if event == cv2.EVENT_LBUTTONUP: if self.active_robot_idx is not None: self.X, self.Y = x, y if self.X is not None and self.Y is not None: if len(self.active_path_points) < 3: self.active_path_points.append(Point(self.X, self.Y)) self.X, self.Y = None, None elif len(self.active_path_points) == 3: self.robots[self.active_robot_idx].set_path( self.active_path_points) self.active_robot_idx = None self.active_path_points = [] self.X, self.Y = None, None
def ball_callback(self, msg): if not self.stop: self.ball_pos = Point(msg.x, msg.y) self.is_goal() self.is_out() ball_pos = Point(msg.x, msg.y) self.ball_pos = Point(msg.x, msg.y) for robot in self.robots.values(): if robot.id not in attackers: if robot.id == goalkeeper_1_id: y_pt = self.gates_1.left_point.y + robot_size x_pt = self.ball_pos.x min_x = min([ self.gates_1.left_point.x, self.gates_1.right_point.x ]) max_x = max([ self.gates_1.left_point.x, self.gates_1.right_point.x ]) if x_pt <= min_x: x_pt = min_x elif x_pt >= max_x: x_pt = max_x robot.set_movement_point(Point(x_pt, y_pt)) elif robot.id == goalkeeper_2_id: y_pt = self.gates_2.left_point.y - robot_size x_pt = self.ball_pos.x min_x = min([ self.gates_2.left_point.x, self.gates_2.right_point.x ]) max_x = max([ self.gates_2.left_point.x, self.gates_2.right_point.x ]) if x_pt <= min_x: x_pt = min_x elif x_pt >= max_x: x_pt = max_x robot.set_movement_point(Point(x_pt, y_pt)) else: for robot in self.robots.values(): self.ball_pos = None robot.stop()
def __init__(self, center=Point(None, None), hit_distance=150): self.center = center self.hit_distance = hit_distance rospy.init_node("ball_detector_node") image_sub = rospy.Subscriber("stack_of_cams", stack_of_cams, self.img_callback) self.field_w = field_w self.field_h = field_h gates_sub = rospy.Subscriber("gates_data", AllGates, self.gates_callback) self.hit_point_pub = rospy.Publisher("hit_point", Point2d, queue_size=5) self.ball_publisher = rospy.Publisher("ball_postion", Point2d, queue_size=5) def sh(): pass rospy.on_shutdown(sh) rospy.spin()
def detect_ball(self, msgs_list): new_balls_list = [] for msg in msgs_list: img = msg.img original_h, original_w, _ = img.shape resized = cv2.resize(img, (original_w // self.resize_scale, original_h // self.resize_scale)) hsv = self.img_to_hsv(img) mask = self.get_mask(hsv) contours = self.get_contours(mask) # cnt = list(p[0] for p in contours[0]) center = self.get_ball_center(contours) if center is not None: ball = Ball(Point(*center), msg.x_pos.data, msg.y_pos.data) new_balls_list.append(ball) if len(new_balls_list): self.balls_list = new_balls_list
def get_hit_point(self, dx, dy, open_side_point): top_border_pt = Point(self.center.x, 0) bot_border_pt = Point(self.center.x, self.field_h) left_border_pt = Point(0, self.center.y) right_border_pt = Point(self.field_w, self.center.y) pts = [top_border_pt, bot_border_pt, left_border_pt, right_border_pt] distances = list(get_distance_between_points(self.center, pt) for pt in pts) min_d_idx = int(np.argmin(distances)) if distances[min_d_idx] <= self.hit_distance: if min_d_idx == 0: return Point(self.center.x, self.center.y + self.hit_distance) elif min_d_idx == 1: return Point(self.center.x, self.center.y - self.hit_distance) elif min_d_idx == 2: return Point(self.center.x + self.hit_distance, self.center.y) elif min_d_idx == 3: return Point(self.center.x - self.hit_distance, self.center.y) else: y = self.center.y - dy if open_side_point.y >= self.center.y else self.center.y + dy x = self.center.x - dx if open_side_point.x >= self.center.x else self.center.x + dx return Point(x, y)
def set_hit_point(self, hit_point): self.hit_point = Point(hit_point.x, hit_point.y)
def remap_point(self, new_w, new_h, old_w, old_h, pt): pt_x_prcnt, pt_y_prcnt = float(pt.x) / old_w, float(pt.y) / old_h new_pt = Point(pt_x_prcnt*new_w, pt_y_prcnt*new_h) return new_pt
def hit_point_callback(self, msg): self.finish_point = Point(msg.x, msg.y)
def get_points_of_circle(self, center, n=8): dx = center.x dy = center.y points_as_lst = set((int(math.cos(2*math.pi / n * x) * self.ball_radius + dx), int(math.sin(2*math.pi / n * x) * self.ball_radius + dy) ) for x in range(0, n + 1)) return list( Point(p[0], p[1]) for p in points_as_lst )
def get_mean_data_val(self, data_list): x = [pt.x for pt in data_list] y = [pt.y for pt in data_list] return Point(np.mean(x), np.mean(y))
def get_direction_point(self): x_lst = [self.corners[0].x, self.corners[1].x] y_lst = [self.corners[0].y, self.corners[1].y] return Point(np.mean(x_lst), np.mean(y_lst))
def resize_contour(self, cnt, scale): tmp_cnt = list( Point(p.x - self.position.x, p.y - self.position.y) for p in cnt ) resized = list( Point(p.x*scale + self.position.x, p.y*scale + self.position.y) for p in tmp_cnt ) return resized
class RobotAnalyzer: def __init__(self): self.robots = {} self.gates_1 = Gate(goal1_pt1, goal1_pt2) self.gates_2 = Gate(goal2_pt1, goal2_pt2) rospy.init_node("robots_analizer_node") aruco_sub = rospy.Subscriber("aruco_data", MarkersOnField, self.aruco_callback) # path_sub = rospy.Subscriber("paths_data", PathsList, self.path_callback) ball_sub = rospy.Subscriber("ball_postion", Point2d, self.ball_callback) start_sub = rospy.Subscriber("START", Bool, self.start_callback) self.goals_to_top = 0 self.goals_to_bot = 0 self.ball_pos = None final_point_sub = rospy.Subscriber("final_point", FinishPoint, self.finish_point_callback) self.gates_publisher = rospy.Publisher("gates_data", GatesMsg, queue_size=1) self.robot_publisher = rospy.Publisher("robots_data", RobotsOnField, queue_size=1) self.game_status_pub = rospy.Publisher("game_status", GameStatus, queue_size=1) self.goal = False self.out = False self.stop = True rospy.spin() def stop_all_robots(self): for r in self.robots.values(): r.stop() def start_callback(self, msg): self.stop = not self.stop # print(self.stop) def check_out_dst_point(self, point): x, y = point.to_list() min_x = out + robot_size max_x = field_w * 2 - dx - out - robot_size # max_x = 640 - out - robot_size min_y = out + robot_size # max_y = field_h*2 - dy - out - robot_size max_y = field_h - dy - out - robot_size # max_y = 480 - out - robot_size x, y = self.ball_pos.to_list() if x <= min_x: new_x = min_x elif x >= max_x: new_x = max_x else: new_x = x if y <= min_y: new_y = min_y elif y >= max_y: new_y = max_y else: new_y = y return Point(new_x, new_y) def finish_point_callback(self, msg): if not self.stop: attacker_id = msg.id finish_point = Point(msg.point.x, msg.point.y) for robot in self.robots.values(): if robot.id in attackers and robot.id == attacker_id: f_point = self.check_out_dst_point(finish_point) robot.set_movement_point(f_point) else: for robot in self.robots.values(): self.ball_pos = None robot.stop() def prepare_robots_msg(self): msg = RobotsOnField() robot_msgs_list = [] for r in self.robots.values(): r_msg = r.prepare_msg() robot_msgs_list.append(r_msg) msg.robots = robot_msgs_list return msg def ball_callback(self, msg): if not self.stop: self.ball_pos = Point(msg.x, msg.y) self.is_goal() self.is_out() ball_pos = Point(msg.x, msg.y) self.ball_pos = Point(msg.x, msg.y) for robot in self.robots.values(): if robot.id not in attackers: if robot.id == goalkeeper_1_id: y_pt = self.gates_1.left_point.y + robot_size x_pt = self.ball_pos.x min_x = min([ self.gates_1.left_point.x, self.gates_1.right_point.x ]) max_x = max([ self.gates_1.left_point.x, self.gates_1.right_point.x ]) if x_pt <= min_x: x_pt = min_x elif x_pt >= max_x: x_pt = max_x robot.set_movement_point(Point(x_pt, y_pt)) elif robot.id == goalkeeper_2_id: y_pt = self.gates_2.left_point.y - robot_size x_pt = self.ball_pos.x min_x = min([ self.gates_2.left_point.x, self.gates_2.right_point.x ]) max_x = max([ self.gates_2.left_point.x, self.gates_2.right_point.x ]) if x_pt <= min_x: x_pt = min_x elif x_pt >= max_x: x_pt = max_x robot.set_movement_point(Point(x_pt, y_pt)) else: for robot in self.robots.values(): self.ball_pos = None robot.stop() # self.is_out() def aruco_callback(self, msg_data): print(self.goals_to_bot, self.goals_to_top) for marker in msg_data.markers: if marker.id not in self.robots.keys(): corners = list( (corner.x, corner.y) for corner in marker.corners) if marker.id in attackers: robot = Robot(marker.id, corners, role="attacker") else: robot = Robot(marker.id, corners, role="goalkeeper", path_eps=40, angle_eps=15) robot.set_robot_geometry(marker_size, from_center_to_front_side, from_center_to_back_side, from_center_to_left_side, from_center_to_right_side) self.robots[marker.id] = robot self.robots[marker.id].update(marker.corners) if marker.id == goalkeeper_1_id: self.gates_1.set_goalkeeper(robot) else: self.robots[marker.id].update(marker.corners) self.gates_1.update_open_side_points() gates_msg = GatesMsg() msg = self.gates_1.prepare_msg() gates_msg.gates = [msg] self.gates_publisher.publish(gates_msg) if len(self.robots): msg = self.prepare_robots_msg() self.robot_publisher.publish(msg) game_msg = GameStatus() game_status_msg = Bool() game_status_msg.data = not self.stop game_msg.game_status = game_status_msg game_msg.goals_to_bot = self.goals_to_bot game_msg.goals_to_top = self.goals_to_top self.game_status_pub.publish(game_msg) def is_goal(self): ball_x, ball_y = self.ball_pos.x, self.ball_pos.y min_x = min(goal1_pt1.x, goal1_pt2.x) max_x = max(goal1_pt1.x, goal1_pt2.x) g1_min_y = 0 g1_max_y = max(goal1_pt1.y, goal1_pt2.y) g2_max_y = field_h * 2 - dy g2_min_y = max(goal2_pt1.y, goal2_pt2.y) if ball_x >= min_x and ball_x <= max_x: if ball_y >= g1_min_y and ball_y <= g1_max_y: if not self.stop: self.goals_to_top += 1 self.stop = True return True if ball_x >= min_x and ball_x <= max_x: if ball_y >= g2_min_y and ball_y <= g2_max_y: if not self.stop: self.goals_to_bot += 1 self.stop = True return True def is_out(self): min_x = out # max_x = field_w*2 - dx max_x = 640 - out min_y = out # max_y = field_h*2 - dy max_y = 480 - out x, y = self.ball_pos.to_list() if x <= min_x or x >= max_x or y <= min_y or y >= max_y: self.stop = True