class Visualizer(object):
def __init__(self, field_w, field_h, dx, dy):
self.IMG = self.create_map(field_w, field_h, dx, dy)
self.img_to_show = None
self.ball_position = None
self.markers = None
self.robots = None
self.pathes = None
self.gates = []
self.hit_point = None
self.RUN = True
self.start_game = False
self.game_status = False
self.goals_to_bot = 0
self.goals_to_top = 0
rospy.init_node("visualizer_node")
ball_sub = rospy.Subscriber("ball_postion", Point2d, self.ball_callback)
aruco_sub = rospy.Subscriber("aruco_data", markers_on_field_msg, self.aruco_callback)
robot_sun = rospy.Subscriber("robots_data", RobotsOnField, self.robot_callback)
img_sub = rospy.Subscriber("map", Image, self.img_callback)
gates_sub = rospy.Subscriber("gates_data", AllGates, self.gates_callback)
game_status_sub = rospy.Subscriber("game_status", GameStatus, self.game_status_callback)
# hit_point_sup = rospy.Subscriber("hit_point", Point2d, self.hit_point_callback)
self.start_pub = rospy.Publisher("START", Bool, queue_size=1)
rospy.spin()
def img_callback(self, data):
img = bridge.imgmsg_to_cv2(data)
self.draw_objects(img)
cv2.namedWindow("img", cv2.WINDOW_NORMAL)
cv2.imshow('img', img)
if cv2.waitKey(1) & 0xFF == ord("s"):
self.start_pub.publish(self.RUN)
self.RUN = not self.RUN
def game_status_callback(self, msg):
self.game_status = msg.game_status.data
self.goals_to_bot = msg.goals_to_bot
self.goals_to_top = msg.goals_to_top
def gates_callback(self, msg):
self.gates = msg.gates
def hit_point_callback(self, msg):
self.hit_point = Point(msg.x, msg.y)
def draw_field_borders(self, img):
h, w, _ = img.shape
pts = [Point(out, out), Point(w - out, out), Point(w - out, h - out), Point(out, h - out)]
for i in range(len(pts)):
cv2.line(img, pts[i-1].to_tuple(), pts[i].to_tuple(), (0, 0, 255), 4)
def ball_callback(self, msg_data):
point = (int(msg_data.x), int(msg_data.y))
self.ball_position = point
def aruco_callback(self, msg_data):
self.markers = msg_data.markers
def robot_callback(self, msg_data):
self.robots = msg_data.robots
def draw_objects(self, img):
game_status_color = (50, 255, 50) if self.game_status else (50, 50, 255)
cv2.putText(img, "game: "+str(self.game_status), (10, 10), cv2.FONT_ITALIC, 0.4, game_status_color)
cv2.putText(img, "goals_to_top: "+str(self.goals_to_top), (10, 25), cv2.FONT_ITALIC, 0.4, game_status_color)
cv2.putText(img, "goals_to_bot: "+str(self.goals_to_bot), (10, 45), cv2.FONT_ITALIC, 0.4, game_status_color)
self.draw_field_borders(img)
if self.ball_position is not None:
self.draw_ball(img, self.ball_position)
if self.markers is not None:
for marker in self.markers:
self.draw_fields_marker(img, marker)
if self.robots is not None:
for robot in self.robots:
self.draw_robot(img, robot)
self.draw_gates(img, goal1_pt1, goal1_pt2)
self.draw_gates(img, goal2_pt1, goal2_pt2)
self.draw_gates_open_side(img)
if self.hit_point is not None:
cv2.circle(img, self.hit_point.to_tuple(), 4, (255, 50, 50), 8)
def draw_gates_open_side(self, img):
for gate in self.gates:
pt = gate.open_side
self.draw_crest(img, pt)
def draw_robot(self, img, robot):
front_x, front_y = robot.front_side.x, robot.front_side.y
back_x, back_y = robot.back_side.x, robot.back_side.y
left_x, left_y = robot.left_side.x, robot.left_side.y
right_x, right_y = robot.right_side.x, robot.right_side.y
cv2.circle(img, (front_x, front_y), 5, (255, 0, 0), 3)
cv2.circle(img, (back_x, back_y), 5, (0, 255, 0), 3)
cv2.circle(img, (left_x, left_y), 5, (0, 0, 255), 3)
cv2.circle(img, (right_x, right_y), 5, (255, 0, 255), 3)
if robot.id in attackers:
vis_contour = robot.vision_contour
for i in range(len(vis_contour)):
p1 = robot.vision_contour[i]
p2 = robot.vision_contour[i-1]
pt1 = (int(p1.x), int(p1.y))
pt2 = (int(p2.x), int(p2.y))
cv2.line(img, pt1, pt2, (0, 200, 50), 2)
if robot.movement or robot.rotation:
self.draw_path(img, robot.id, robot.path.points_list)
if robot.actual_point.x and robot.actual_point.y:
p = (robot.actual_point.x, robot.actual_point.y)
c = (robot.center.x, robot.center.y)
d = (robot.direction.x, robot.direction.y)
cv2.circle(img, (p), 5, (255, 0, 40), 10)
cv2.putText(img, str(robot.id), (p[0] + 5, p[1] + 5), cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (255, 0, 0), 2)
cv2.line(img, p, c, (0, 50, 200), 2)
cv2.line(img, d, c, (0, 50, 200), 2)
def draw_path(self, img, id, path):
for point in path:
p = (point.x, point.y)
cv2.circle(img, (p), 3, (0, 255, 0), thickness=4)
cv2.putText(img, str(id), (p[0] + 3, p[1] + 3), cv2.FONT_HERSHEY_COMPLEX_SMALL, 0.7, (0, 255, 0), 2)
def draw_ball(self, img, point):
point = tuple(point)
cv2.circle(img, point, 10, (80, 50, 250), 10)
def draw_fields_marker(self, img, marker):
center = (marker.center.x, marker.center.y)
direction = (marker.direction.x, marker.direction.y)
cv2.circle(img, center, 5, (255, 100, 50), 5)
cv2.circle(img, direction, 4, (0, 200, 50), 2)
for corner in marker.corners:
xy = (corner.x, corner.y)
cv2.circle(img, xy, 2, (255, 100, 100), 1)
def create_map(self, field_w, field_h, dx, dy):
map = np.zeros((field_h*2-dy, field_w*2-dx, 3), dtype=np.uint8)
cv2.line(map, ((field_w*2-dx)//2, 0), ((field_w*2-dx)//2, field_h*2-dy), (50, 50, 50), 2)
cv2.line(map, (0, field_h-dy//2), (field_w*2-dx, field_h-dy//2), (50, 50, 50), 2)
return map
def draw_gates(self, img, pt1, pt2):
cv2.line(img, pt1.to_tuple(), pt2.to_tuple(), (0, 0, 255), 3)
def draw_crest(self, img, pt, color=(100, 255, 0)):
pt1 = (pt.x, pt.y-5)
pt2 = (pt.x, pt.y+5)
pt3 = (pt.x-5, pt.y)
pt4 = (pt.x+5, pt.y)
cv2.line(img, pt1, pt2, color, 3)
cv2.line(img, pt3, pt4, color, 3)
class Visualizer(object):
def __init__(self, field_w, field_h, dx, dy):
self.IMG = self.create_map(field_w, field_h, dx, dy)
self.img_to_show = None
self.ball_position = None
self.markers = None
self.robots = None
self.pathes = None
self.gates = []
self.hit_point = None
self.RUN = True
self.bridge = CvBridge()
rospy.init_node("visualizer_node")
ball_sub = rospy.Subscriber("ball_postion", Point2d, self.ball_callback)
aruco_sub = rospy.Subscriber("aruco_data", markers_on_field_msg, self.aruco_callback)
robot_sun = rospy.Subscriber("robots_data", RobotsOnField, self.robot_callback)
img_sub = rospy.Subscriber("map", Image, self.img_callback)
hit_point_sup = rospy.Subscriber("hit_point", Point2d, self.hit_point_callback)
rospy.spin()
def img_callback(self, data):
img = self.bridge.imgmsg_to_cv2(data)
cv2.namedWindow("img", cv2.WINDOW_NORMAL)
self.draw_objects(img)
cv2.imshow('img', img)
cv2.waitKey(1)
def hit_point_callback(self, msg):
self.hit_point = Point(msg.x, msg.y)
def ball_callback(self, msg_data):
point = (int(msg_data.x), int(msg_data.y))
self.ball_position = point
def aruco_callback(self, msg_data):
self.markers = msg_data.markers
def robot_callback(self, msg_data):
self.robots = msg_data.robots
def draw_objects(self, img):
if self.ball_position is not None:
self.draw_ball(img, self.ball_position)
if self.markers is not None:
for marker in self.markers:
self.draw_fields_marker(img, marker)
if self.robots is not None:
for robot in self.robots:
self.draw_robot(img, robot)
self.draw_gates_open_side(img)
if self.hit_point is not None:
cv2.circle(img, self.hit_point.to_tuple(), 4, (255, 50, 50), 8)
def draw_gates_open_side(self, img):
for gate in self.gates:
pt = gate.open_side
self.draw_crest(img, pt)
def draw_robot(self, img, robot):
front_x, front_y = robot.front_side.x, robot.front_side.y
back_x, back_y = robot.back_side.x, robot.back_side.y
left_x, left_y = robot.left_side.x, robot.left_side.y
right_x, right_y = robot.right_side.x, robot.right_side.y
cv2.circle(img, (front_x, front_y), 5, (255, 0, 0), 3)
cv2.circle(img, (back_x, back_y), 5, (0, 255, 0), 3)
cv2.circle(img, (left_x, left_y), 5, (0, 0, 255), 3)
cv2.circle(img, (right_x, right_y), 5, (255, 0, 255), 3)
self.draw_crest(img, robot.direction, (50, 100, 255), 15)
# cv2.putText(img, str(robot.id), (robot.center.x-20, robot.center.y), cv2.FONT_HERSHEY_COMPLEX, 1.5, (0, 255, 255))
# if robot.movement or robot.rotation:
def draw_path(self, img, id, path):
for point in path:
p = (point.x, point.y)
cv2.circle(img, (p), 3, (0, 255, 0), thickness=4)
cv2.putText(img, str(id), (p[0] + 3, p[1] + 3), cv2.FONT_HERSHEY_COMPLEX_SMALL, 0.7, (0, 255, 0), 2)
def draw_ball(self, img, point):
point = tuple(point)
cv2.circle(img, point, 10, (0, 50, 250), 10)
def draw_fields_marker(self, img, marker):
center = (marker.center.x, marker.center.y)
direction = (marker.direction.x, marker.direction.y)
cv2.circle(img, center, 5, (255, 100, 50), 5)
cv2.circle(img, direction, 4, (0, 200, 50), 2)
for corner in marker.corners:
xy = (corner.x, corner.y)
cv2.circle(img, xy, 2, (255, 100, 100), 1)
def create_map(self, field_w, field_h, dx, dy):
map = np.zeros((field_h*2-dy, field_w*2-dx, 3), dtype=np.uint8)
cv2.line(map, ((field_w*2-dx)//2, 0), ((field_w*2-dx)//2, field_h*2-dy), (50, 50, 50), 2)
cv2.line(map, (0, field_h-dy//2), (field_w*2-dx, field_h-dy//2), (50, 50, 50), 2)
return map
def draw_gates(self, img, pt1, pt2):
cv2.line(img, pt1.to_tuple(), pt2.to_tuple(), (0, 0, 255), 3)
def draw_crest(self, img, pt, color=(100, 255, 0), d=5):
pt1 = (pt.x, pt.y-d)
pt2 = (pt.x, pt.y+d)
pt3 = (pt.x-d, pt.y)
pt4 = (pt.x+d, pt.y)
cv2.line(img, pt1, pt2, color, 3)
cv2.line(img, pt3, pt4, color, 3)