def detect_robot(self, trsh_im):
# angle might not be accurate, but always determines a line
# that is collinear to the longest part of the T on the robot plate.
t_contour, t_center, ang = self.detect_robot_via_moments(trsh_im)
if not (t_center and t_contour):
return trsh_im, None, None, None
# create dir vec (may be wrong when error_expected() returns True)
# pointing upwards (systems 0 degree) and rotate it by the angle.
t_dir = Math.int_vec(Math.rotate_vec((0, -20), ang))
trust_dir = True # lets assume this is the correct vector.
# if mistake possible(robot orientation in danger zone), perform check
if self.dir_error_expected(ang, 10):
# Try to get the direction of the back using
# get_back_dir(centroid, radius, supposed_direction_of_T,
# line_thikness, thresholded_bin_image)
t_bck_dir = self.get_back_dir(t_contour, t_center, 20, t_dir, 12,
cv.GetSize(trsh_im))
# if the vectors are pointing relatively in one direction
# there is an error, so invert vector
if t_bck_dir and Math.ang_btw_vecs(t_dir, t_bck_dir) < 90:
t_dir = Math.invert_vec(t_dir)
return trsh_im, t_center, t_dir, ang
def detect_robot(self, trsh_im): # angle might not be accurate, but always determines a line # that is collinear to the longest part of the T on the robot plate. t_contour, t_center, ang = self.detect_robot_via_moments(trsh_im) if not (t_center and t_contour): return trsh_im, None, None, None # create dir vec (may be wrong when error_expected() returns True) # pointing upwards (systems 0 degree) and rotate it by the angle. t_dir = Math.int_vec(Math.rotate_vec((0, -20), ang)) trust_dir = True # lets assume this is the correct vector. # if mistake possible(robot orientation in danger zone), perform check if self.dir_error_expected(ang, 10): # Try to get the direction of the back using # get_back_dir(centroid, radius, supposed_direction_of_T, # line_thikness, thresholded_bin_image) t_bck_dir = self.get_back_dir(t_contour, t_center, 20, t_dir, 12, cv.GetSize(trsh_im)) # if the vectors are pointing relatively in one direction # there is an error, so invert vector if t_bck_dir and Math.ang_btw_vecs(t_dir, t_bck_dir) < 90: t_dir = Math.invert_vec(t_dir) return trsh_im, t_center, t_dir, ang
def get_back_dir(self, t_contour, centroid, rad, t_dir, thickness, size):
roi = self.draw_contour(t_contour, size)
#roi = self.get_circular_roi(centroid, rad, bin_im)
# draw a thick line from tip of 'dir_vec' to tip of the inverted
# 'dir_vec'. This is done, to ensure that the 'stand' of the T is
# removed and it's hat is split in two.
cv.Line(roi, Math.add_vectors(t_dir, centroid),
Math.add_vectors(Math.invert_vec(t_dir), centroid),
ColorSpace.RGB_BLACK, thickness)
tmp_im = cv.CreateImage(cv.GetSize(roi), cv.IPL_DEPTH_8U, 1)
cv.Copy(roi, tmp_im) # create image for FindContours
seq = cv.FindContours(tmp_im, cv.CreateMemStorage(0),
cv.CV_RETR_EXTERNAL, cv.CV_CHAIN_APPROX_NONE)
# sort contours from ROI and try to take two with the biggest area
contours = contours_area_sort(seq)[-2:]
if not contours: return None
nr_contours = len(contours)
if nr_contours == 2: # if two available, get vec to midpoint
pt1 = get_contour_center(cv.Moments(contours[0]))
pt2 = get_contour_center(cv.Moments(contours[1]))
mid = Math.add_vectors(pt1, pt2, 1 / 2.0)
elif nr_contours: # if only one, retun it as mid point
mid = get_contour_center(cv.Moments(contours[0]))
# no contours found, check failed, get prev value
else:
return None
mid = Math.int_vec(mid)
dir_vec = Math.sub_vectors(mid, centroid)
# show vector
cv.Line(roi, centroid, Math.add_vectors(centroid, dir_vec),
ColorSpace.RGB_WHITE, 1)
cv.ShowImage('w', roi)
return dir_vec # return the back direction vec
def get_back_dir(self, t_contour, centroid, rad, t_dir, thickness, size):
roi = self.draw_contour(t_contour, size)
#roi = self.get_circular_roi(centroid, rad, bin_im)
# draw a thick line from tip of 'dir_vec' to tip of the inverted
# 'dir_vec'. This is done, to ensure that the 'stand' of the T is
# removed and it's hat is split in two.
cv.Line(roi, Math.add_vectors(t_dir, centroid),
Math.add_vectors(Math.invert_vec(t_dir), centroid),
ColorSpace.RGB_BLACK, thickness)
tmp_im = cv.CreateImage(cv.GetSize(roi), cv.IPL_DEPTH_8U, 1)
cv.Copy(roi, tmp_im) # create image for FindContours
seq = cv.FindContours(tmp_im, cv.CreateMemStorage(0),
cv.CV_RETR_EXTERNAL, cv.CV_CHAIN_APPROX_NONE)
# sort contours from ROI and try to take two with the biggest area
contours = contours_area_sort(seq)[-2:]
if not contours : return None
nr_contours = len(contours)
if nr_contours == 2: # if two available, get vec to midpoint
pt1 = get_contour_center(cv.Moments(contours[0]))
pt2 = get_contour_center(cv.Moments(contours[1]))
mid = Math.add_vectors(pt1, pt2, 1 / 2.0)
elif nr_contours: # if only one, retun it as mid point
mid = get_contour_center(cv.Moments(contours[0]))
# no contours found, check failed, get prev value
else: return None
mid = Math.int_vec(mid)
dir_vec = Math.sub_vectors(mid, centroid)
# show vector
cv.Line(roi, centroid, Math.add_vectors(centroid, dir_vec),
ColorSpace.RGB_WHITE, 1)
cv.ShowImage('w', roi)
return dir_vec # return the back direction vec
class VisionGUI:
WIN_COLOR = 'colored feed'
WIN_RED_BIN = 'red binary image'
WIN_BLU_BIN = 'blu binary image'
WIN_YLW_BIN = 'ylw binary image'
# TODO: Change integer values with these constancs all over the application
COLOR_FEED = 0
RED_BIN_FEED = 1
BLU_BIN_FEED = 2
YLW_BIN_FEED = 3
GUI_SLIDERS = 4
def __init__():
abstract
def create_gui(red_color, blu_color, ylw_color):
if 0 in V_SETT.DISPLAY_FEED:
cv.NamedWindow(VisionGUI.WIN_COLOR, 1)
if 1 in V_SETT.DISPLAY_FEED:
cv.NamedWindow(VisionGUI.WIN_RED_BIN, 1)
if 4 in V_SETT.DISPLAY_FEED:
VisionGUI.atch_trackbars_to_win(VisionGUI.WIN_RED_BIN,
red_color)
else:
VisionGUI.deatch_trackbars(VisionGUI.WIN_RED_BIN)
if 2 in V_SETT.DISPLAY_FEED:
cv.NamedWindow(VisionGUI.WIN_BLU_BIN, 1)
if 4 in V_SETT.DISPLAY_FEED:
VisionGUI.atch_trackbars_to_win(VisionGUI.WIN_BLU_BIN,
blu_color)
else:
VisionGUI.deatch_trackbars(VisionGUI.WIN_BLU_BIN)
if 3 in V_SETT.DISPLAY_FEED:
cv.NamedWindow(VisionGUI.WIN_YLW_BIN, 1)
if 4 in V_SETT.DISPLAY_FEED:
VisionGUI.atch_trackbars_to_win(VisionGUI.WIN_YLW_BIN,
ylw_color)
else:
VisionGUI.deatch_trackbars(VisionGUI.WIN_YLW_BIN)
create_gui = staticmethod(create_gui)
def display_visual_feedback(img, red_bin_img, blu_bin_img, ylw_bin_img,
(bal_center, blu_center, ylw_center, blu_dir,
ylw_dir), (l_goal_t_, l_goal_b_, r_goal_t_,
r_goal_b_), (pitch_tl_, pitch_bl_,
pitch_tr_, pitch_br_),
(trust_bal, trust_blu,
trust_ylw), (bal_vel, blu_vel, ylw_vel)):
if 0 in V_SETT.DISPLAY_FEED:
# display robot and ball centers
# Ball
cv.Circle(img, bal_center, 4, ColorSpace.RGB_BLACK, 2)
# ylw velocity
cv.Line(
img, bal_center,
Math.add_vectors(
bal_center, Math.int_vec(Math.scale_vec(bal_vel,
1 / 60.0))),
ColorSpace.RGB_WHITE, 1, cv.CV_AA)
# Blue
left = Math.rotate_vec(blu_dir, -90)
left_point = Math.add_vectors(left, blu_center)
cv.Line(img, Math.int_vec(Math.add_vectors(left_point, blu_dir)),
Math.int_vec(Math.sub_vectors(left_point, blu_dir)), 1)
right_point = Math.add_vectors(Math.invert_vec(left), blu_center)
cv.Line(img, Math.int_vec(Math.add_vectors(right_point, blu_dir)),
Math.int_vec(Math.sub_vectors(right_point, blu_dir)), 1)
cv.Circle(img, blu_center, 4, ColorSpace.RGB_BLU, -2)
cv.Circle(img, blu_center, 20, ColorSpace.RGB_BLACK, 1)
# blue_dir
cv.Line(img, blu_center, Math.add_vectors(blu_center, blu_dir),
ColorSpace.RGB_BLACK, 1, cv.CV_AA)
# blu velocity
cv.Line(
img, blu_center,
Math.add_vectors(
blu_center, Math.int_vec(Math.scale_vec(blu_vel,
1 / 60.0))),
ColorSpace.RGB_WHITE, 1, cv.CV_AA)
# Yellow
left = Math.rotate_vec(ylw_dir, -90)
left_point = Math.add_vectors(left, ylw_center)
cv.Line(img, Math.int_vec(Math.add_vectors(left_point, ylw_dir)),
Math.int_vec(Math.sub_vectors(left_point, ylw_dir)), 1)
right_point = Math.add_vectors(Math.invert_vec(left), ylw_center)
cv.Line(img, Math.int_vec(Math.add_vectors(right_point, ylw_dir)),
Math.int_vec(Math.sub_vectors(right_point, ylw_dir)), 1)
cv.Circle(img, ylw_center, 4, ColorSpace.RGB_YLW, -2)
cv.Circle(img, ylw_center, 20, ColorSpace.RGB_BLACK, 1)
# ylw_dir
cv.Line(img, ylw_center, Math.add_vectors(ylw_center, ylw_dir),
ColorSpace.RGB_BLACK, 1, cv.CV_AA)
# ylw velocity
cv.Line(
img, ylw_center,
Math.add_vectors(
ylw_center, Math.int_vec(Math.scale_vec(ylw_vel,
1 / 60.0))),
ColorSpace.RGB_WHITE, 1, cv.CV_AA)
# display goal lines
cv.Circle(img, l_goal_t_, 1, ColorSpace.RGB_WHITE, 2)
cv.Circle(img, l_goal_b_, 1, ColorSpace.RGB_WHITE, 2)
cv.Circle(img, r_goal_t_, 1, ColorSpace.RGB_WHITE, 2)
cv.Circle(img, r_goal_b_, 1, ColorSpace.RGB_WHITE, 2)
# display pitch
cv.Circle(img, pitch_tl_, 1, ColorSpace.RGB_WHITE, 2)
cv.Circle(img, pitch_bl_, 1, ColorSpace.RGB_WHITE, 2)
cv.Circle(img, pitch_tr_, 1, ColorSpace.RGB_WHITE, 2)
cv.Circle(img, pitch_br_, 1, ColorSpace.RGB_WHITE, 2)
# show image
cv.ShowImage(VisionGUI.WIN_COLOR, img)
# display binary images if parameters given
if 1 in V_SETT.DISPLAY_FEED:
cv.Circle(red_bin_img, bal_center, 1, ColorSpace.RGB_BLACK, 3)
cv.Circle(red_bin_img, bal_center, 40, ColorSpace.RGB_WHITE, 1)
cv.ShowImage(VisionGUI.WIN_RED_BIN, red_bin_img)
if 2 in V_SETT.DISPLAY_FEED:
cv.Circle(blu_bin_img, blu_center, 1, ColorSpace.RGB_BLACK, 3)
cv.Circle(blu_bin_img, blu_center, 40, ColorSpace.RGB_WHITE, 1)
cv.ShowImage(VisionGUI.WIN_BLU_BIN, blu_bin_img)
if 3 in V_SETT.DISPLAY_FEED:
cv.Circle(ylw_bin_img, ylw_center, 1, ColorSpace.RGB_BLACK, 3)
cv.Circle(ylw_bin_img, ylw_center, 40, ColorSpace.RGB_WHITE, 1)
cv.ShowImage(VisionGUI.WIN_YLW_BIN, ylw_bin_img)
cv.WaitKey(1000 / 35)
