def calc_velocity_vector(cache, number_of_members): if len(cache) <= 1: return (int(0), int(0)) else: older_newer_elem_tuple = Locator.simple_get_members( cache, number_of_members) if older_newer_elem_tuple != None: velocity_vector = Math.sub_vectors( older_newer_elem_tuple[1][0], older_newer_elem_tuple[0][0]) the_time = older_newer_elem_tuple[1][ 1] - older_newer_elem_tuple[0][1] try: velocity_vector = tuple([ element * (1.0 / the_time) for element in velocity_vector ]) velocity_vector = tuple([ int(round(element * 60.0)) for element in velocity_vector ]) except: return (int(0), int(0)) return velocity_vector else: return (int(0), int(0))
def get_state(self): bal_X, bal_Y = Math.sub_vectors(self.bal_center_, self.pitch_tl_) blu_X, blu_Y = Math.sub_vectors(self.blu_robot_center_, self.pitch_tl_) ylw_X, ylw_Y = Math.sub_vectors(self.ylw_robot_center_, self.pitch_tl_) blu_dir_X, blu_dir_Y = self.blu_dir_ ylw_dir_X, ylw_dir_Y = self.ylw_dir_ trust_bal = int(round(self.trust_bal_)) trust_blu = int(round(self.trust_blu_)) trust_ylw = int(round(self.trust_ylw_)) bal_velocity_X, bal_velocity_Y = self.bal_vel_ blu_velocity_X, blu_velocity_Y = self.blu_vel_ ylw_velocity_X, ylw_velocity_Y = self.ylw_vel_ # message to be encoded is <msg_code><nr_args><arg1><arg2>... return [ CommProto.STATE, 19, bal_X, bal_Y, blu_X, blu_Y, ylw_X, ylw_Y, blu_dir_X, blu_dir_Y, ylw_dir_X, ylw_dir_Y, trust_bal, trust_blu, trust_ylw, bal_velocity_X, bal_velocity_Y, blu_velocity_X, blu_velocity_Y, ylw_velocity_X, ylw_velocity_Y ]
def get_state(self): bal_X, bal_Y = Math.sub_vectors(self.bal_center_, self.pitch_tl_) blu_X, blu_Y = Math.sub_vectors( self.blu_robot_center_, self.pitch_tl_) ylw_X, ylw_Y = Math.sub_vectors( self.ylw_robot_center_, self.pitch_tl_) blu_dir_X, blu_dir_Y = self.blu_dir_ ylw_dir_X, ylw_dir_Y = self.ylw_dir_ trust_bal = int(round(self.trust_bal_)) trust_blu = int(round(self.trust_blu_)) trust_ylw = int(round(self.trust_ylw_)) bal_velocity_X, bal_velocity_Y = self.bal_vel_ blu_velocity_X, blu_velocity_Y = self.blu_vel_ ylw_velocity_X, ylw_velocity_Y = self.ylw_vel_ # message to be encoded is <msg_code><nr_args><arg1><arg2>... return [CommProto.STATE, 19, bal_X, bal_Y, blu_X, blu_Y, ylw_X, ylw_Y, blu_dir_X, blu_dir_Y, ylw_dir_X, ylw_dir_Y, trust_bal, trust_blu, trust_ylw, bal_velocity_X, bal_velocity_Y, blu_velocity_X, blu_velocity_Y, ylw_velocity_X, ylw_velocity_Y]
def calc_velocity_vector(cache, number_of_members): if len(cache) <= 1: return (int(0),int(0)) else: older_newer_elem_tuple = Locator.simple_get_members(cache, number_of_members) if older_newer_elem_tuple != None: velocity_vector = Math.sub_vectors(older_newer_elem_tuple[1][0], older_newer_elem_tuple[0][0]) the_time = older_newer_elem_tuple[1][1] - older_newer_elem_tuple[0][1] try: velocity_vector = tuple([element*(1.0/the_time) for element in velocity_vector]) velocity_vector = tuple([int(round(element*60.0)) for element in velocity_vector]) except: return (int(0),int(0)) return velocity_vector else: return (int(0),int(0))
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)