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
