def main(): #uvlist=[123.0,112.0]316,251 uvlist = [] uvcentrallist = [329, 255] uv0 = UVpub("uv_design_pub", "/camera_uv/uvlist", uvcentrallist, 150) ur_pub = uv0.Init_node() ratet = 2.5 rate = rospy.Rate(ratet) t = 0 a = uv() for i in range(uv0.cont): uvlist.append(uv0.get_draw_circle_uv(i)) print uvlist while not rospy.is_shutdown(): # ur_pub.publish(a) # print uvlist[0] try: a.uvinfo = uvlist[t % uv0.cont] ur_pub.publish(a) print "---------------\n", t % uv0.cont, uvlist[t % uv0.cont] t += 1 except KeyboardInterrupt: sys.exit() rate.sleep()
def main(): #uvlist=[123.0,112.0]316,251 uvlist=[] uvcentrallist=[336,259]#[327, 257] radius = 150 ratet = 9 # 1 cont = 400# 500 perfect for no IMU uv0=UVpub("uv_design_pub","/camera_uv/uvlist",uvcentrallist,radius,cont) ur_pub=uv0.Init_node() rate = rospy.Rate(ratet) t=0 a=uv() for i in range(uv0.cont): uvlist.append(uv0.get_draw_circle_uv(i)) # uvlist.append(uv0.get_draw_line_uv(i % 200)) print uvlist while not rospy.is_shutdown(): # ur_pub.publish(a) # print uvlist[0] try: a.uvinfo = uvlist[t%uv0.cont] ur_pub.publish(a) print "---------------\n",t%uv0.cont,uvlist[t%uv0.cont] t += 1 except KeyboardInterrupt: sys.exit() rate.sleep()
def pub_empty_uv_info(self, tile_id, obj_desire): uvuv = uv() tile_uv = tileuv() tile_uv.tile_id = tile_id tile_uv.obj_desire = obj_desire tile_uv.cen_uv.uvinfo = [0, 0] tile_uv.f1th_uv.uvinfo = [0, 0] tile_uv.s2th_uv.uvinfo = [0, 0] tile_uv.t3th_uv.uvinfo = [0, 0] tile_uv.f4th_uv.uvinfo = [0, 0] self.tile_pub.publish(tile_uv)
def main(): #uvlist=[123.0,112.0] uvlist = [] uvcentrallist = [313, 250] uv0 = UVpub("uv_design_pub", "/xp_uv/xp", uvcentrallist, 100, 5, 0.7) #x=[350,279] #x=[313,250] xo = [515, 89] xd = [60, 365] #xp=uv0.get_fov_desire_uv(x,xo,xd) ur_pub, ar_sub, ar_reader = uv0.Init_node() ratet = 1 rate = rospy.Rate(ratet) t = 0 #a=uv() b = uv() #for i in range(100): # uvlist.append(uv0.get_draw_circle_uv(i)) #print uvlist while not rospy.is_shutdown(): # ur_pub.publish(a) # print uvlist[0] try: # a.uvinfo = uvlist[t%100] pos_dict = ar_reader.ave_pos_dict if len(pos_dict) != 0: x = uv0.get_uv_from_ar(pos_dict[0][:3])[:2] print "now uv -------\n", x xp = uv0.get_fov_desire_uv(x, xo, xd) b.uvinfo = xp # ur_pub.publish(a) ur_pub.publish(b) #print "---------------\n",t%100,uvlist[t%100] #t += 1 print "publish xp uv ------\n", xp else: print "please wait the ar marker ok --------\n" except KeyboardInterrupt: sys.exit() rate.sleep()
def main(): #uvlist=[123.0,112.0] uvlist = [] uvcentral = [331, 229] uv0 = UVpub("uv_design_pub", "/xp_uv/xp", uvcentral, 100, 5, 0.7) #x=[350,279] #x=[313,250] # xo=[515,89] # xd=[60,365] #xp=uv0.get_fov_desire_uv(x,xo,xd) xp_pub, led13 = uv0.Init_node() ratet = 1 rate = rospy.Rate(ratet) #a=uv() b = uv() temp = [] xpp = [] flagg = 0 while not rospy.is_shutdown(): if len(led13.ledstate_buf): uv0.ledstate = led13.ledstate_buf[-1] try: if flagg == 1: if len(uv0.tile_0_buf) == 0 and len(uv0.tile_1_buf) == 0: print "please wait the new tile uv subscribe ok --------\n" time.sleep(3) continue if len(uv0.tile_0_buf) != 0 and len( uv0.tile_1_buf) != 0 and uv0.ledstate == 1: print "second ---- all tiles in fov ----\n" x = uvcentral #right corner xo = list(uv0.tile_0_buf[-1][1]) xd = list(uv0.tile_1_buf[-1][1]) xp = xd b.uvinfo = xp xp_pub.publish(b) print "second xp,xd xd ----------", xp else: try: print "second just one tile in fov ----\n" print " uv0.tile_0_buf=[], uv0.tile_1_buf=[]", uv0.tile_0_buf, "\n", uv0.tile_1_buf x = uvcentral xo = list(uv0.tile_0_buf[-1][1]) xd = list(uv0.tile_0_buf[-1][1]) print "x,xo,xd", x, xo, xd xp = uv0.get_fov_desire_uv(x, xo, xd) b.uvinfo = xo xp_pub.publish(b) print "publish xp uv ------\n", xp except: pass # print "tile_reader",uv0.changeuv else: if len(uv0.tile_0_buf) == 0 and len(uv0.tile_1_buf) == 0: print "please wait the tile uv subscribe ok --------\n" pass elif len(uv0.tile_0_buf) != 0 and len( uv0.tile_1_buf) != 0 and uv0.ledstate == 0: print "first ---- all tiles in fov ----\n" x = uvcentral #left corner xo = list(uv0.tile_0_buf[-1][1]) xd = list(uv0.tile_1_buf[-1][1]) print "x,xo,xd", x, xo, xd if len(temp) == 10: temp = temp[:(len(temp) - 1)] temp.append(xd) else: temp.append(xd) if len(temp) == 1: xpp = temp[0] xp = uv0.get_fov_desire_uv(x, xo, xd) b.uvinfo = xp xp_pub.publish(b) print "temp------\n", temp print "xpp------\n", xpp print "publish xp uv ------\n", xp elif len(uv0.tile_0_buf) == 10 and len( uv0.tile_1_buf) == 0 and uv0.ledstate == 1: # time.sleep(3) print "pick and manipulator-------" try: x = [300, 249] xo = list(uv0.tile_0_buf[-1][1]) xd = xpp #print "x,xo,xd", x, xo, xd xp = numpy.array(xo) - (numpy.array(x) - numpy.array(xd)) b.uvinfo = xp.tolist() xp_pub.publish(b) print "publish xp uv ------", xp u_error = abs(331 - xp.tolist()[0]) v_error = abs(219 - xp.tolist()[1]) if (u_error <= 6 and u_error >= 0) and (v_error <= 33 and v_error >= 0): flagg = 1 uv0.tile_0_buf = [] uv0.tile_1_buf = [] print "flagg-------", flagg print "u_error,v_error", u_error, v_error print " uv0.tile_0_buf=[]", uv0.tile_0_buf print ",uv0.tile_1_buf=[]", uv0.tile_1_buf except: pass else: print "just one tile in fov ----\n" x = uvcentral xo = list(uv0.tile_0_buf[-1][1]) xd = list(uv0.tile_0_buf[-1][1]) print "x,xo,xd", x, xo, xd xp = uv0.get_fov_desire_uv(x, xo, xd) b.uvinfo = xp xp_pub.publish(b) print "publish xp uv ------\n", xp # print "tile_reader",uv0.changeuv except KeyboardInterrupt: sys.exit() rate.sleep()
def main(): #uvlist=[123.0,112.0] uvlist=[] uvcentral=[331,229] uv0=UVpub("uv_design_pub","/xp_uv/xp",uvcentral,100,5,0.7) #x=[350,279] #x=[313,250] # xo=[515,89] # xd=[60,365] #xp=uv0.get_fov_desire_uv(x,xo,xd) xp_pub, tile_reader=uv0.Init_node() ratet=1 rate = rospy.Rate(ratet) t=0 #a=uv() b=uv() temp = [] flagg=0 while not rospy.is_shutdown(): try: if len(tile_reader.tile_0_buf)!=0 and len(tile_reader.tile_1_buf)!=0 and flagg !=1: print "all tiles in fov ----\n" x=uvcentral xo=list(tile_reader.tile_1_buf[-1][1]) xd=list(tile_reader.tile_0_buf[-1][1]) print "x,xo,xd", x, xo, xd temp=xd flag,xp=uv0.get_fov_desire_uv(x,xo,xd) flagg=flag b.uvinfo = xp xp_pub.publish(b) print "publish xp uv ------\n",xp elif len(tile_reader.tile_0_buf)==0 and len(tile_reader.tile_1_buf)==0: print "please wait the tile uv subscribe ok --------\n" pass else: print "just one tile in fov ----\n" x = uvcentral xo=list(tile_reader.tile_0_buf[-1][1]) xd=list(tile_reader.tile_0_buf[-1][1]) print "x,xo,xd", x, xo, xd flag,xp = uv0.get_fov_desire_uv(x, xo, xd) if flag==1: if len(tile_reader.tile_1_buf)!=0: xd=temp print "temp-----",temp print "------------------start publish manipulator------",xd xp = xo flagg=flag time.sleep(4) print "flagg----", flagg else: xd=temp print "temp-----",temp print "------------------start publish manipulator------",xd flag, xp = uv0.get_fov_desire_uv(x, xo, xd) flagg=flag print "flagg----", flagg b.uvinfo = xp xp_pub.publish(b) print "publish xp uv ------\n", xp except KeyboardInterrupt: sys.exit() rate.sleep()
def process_rgb_image(self,image): #print "rgb_image\n",rgb color1 = [0, 0, 255] color2 = [255, 0, 0] color3 = [0, 255, 0] thickness = 5 try: # print "image from process rbg image",image hsv = self.convert_hsv(image) sehsv = self.select_rgb_white_yellow(hsv) select = self.select_white_yellow(image) gray = self.convert_gray_scale(select) smooth = self.apply_smoothing(gray) edges = self.detect_edges(smooth) region = self.select_region(edges) lines = self.hough_lines(region) left_line,right_line=self.lane_lines(image,lines) # print "left_line,right_line", left_line, right_line struct_light=structlight() uvuv=uv() cv2.line(image, (273, 259), (438, 168), color3, thickness) cv2.line(image, (455, 170), (638, 216), color3, thickness) if left_line!=None: cv2.line(image, left_line[0], left_line[1], color1, thickness) # cv2.line(image, right_line[0], right_line[1], color2, thickness) # # cv2.line(image, right_line[0], right_line[1], [255,255,0], thickness) # print "left_line,right_line",left_line,right_line struct_light.left_uv0.uvinfo = [left_line[0][0], left_line[0][1]] struct_light.left_uv1.uvinfo= [left_line[1][0], left_line[1][1]] # struct_light.right_uv0.uvinfo = [right_line[0][0], right_line[0][1]] # struct_light.right_uv1.uvinfo = [right_line[1][0], right_line[1][1]] self.light_pub.publish(struct_light) else: # print "no left_line or no right_line,----->left_line\n",left_line pass # print "no left_line or no right_line----->right_line\n", right_line if right_line!=None: # cv2.line(image, left_line[0], left_line[1], color1, thickness) cv2.line(image, right_line[0], right_line[1], color2, thickness) # # cv2.line(image, right_line[0], right_line[1], [255,255,0], thickness) # print "left_line,right_line",left_line,right_line # struct_light.left_uv0.uvinfo = [left_line[0][0], left_line[0][1]] # struct_light.left_uv1.uvinfo= [left_line[1][0], left_line[1][1]] struct_light.right_uv0.uvinfo = [right_line[0][0], right_line[0][1]] struct_light.right_uv1.uvinfo = [right_line[1][0], right_line[1][1]] self.light_pub.publish(struct_light) else: pass # print "no left_line or no right_line,----->left_line\n",left_line #print "no left_line or no right_line----->right_line\n", right_line print "left_line,right_line",left_line,right_line cv2.namedWindow('img4', cv2.WINDOW_NORMAL) cv2.imshow('img4', image) cv2.namedWindow('select', cv2.WINDOW_NORMAL) cv2.imshow('select', select) cv2.namedWindow('edges', cv2.WINDOW_NORMAL) cv2.imshow('edges', edges) cv2.namedWindow('hsv', cv2.WINDOW_NORMAL) cv2.imshow('hsv',hsv) cv2.namedWindow('hsl', cv2.WINDOW_NORMAL) cv2.imshow('hsl', self.convert_hls(image)) cv2.namedWindow('region', cv2.WINDOW_NORMAL) cv2.imshow('region', region) # return left_line, right_line except: print "draw line error--------------------" cv2.waitKey(8) # # 再将opencv格式额数据转换成ros image格式的数据发布 try: self.image_pub.publish(self.bridge.cv2_to_imgmsg(image, "bgr8")) except CvBridgeError as e: print e
def process_rgb_image(self,rgb_image): central_list=[] sumuv=[] strresuv={} uvuv=uv() strpoint_uv=structure_point() rgb=rgb_image #print "rgb_image\n",rgb t=0 if rgb_image is not None: # ap = argparse.ArgumentParser() # ap.add_argument("-i", "--image", required=True, # help="path to the image file") # args = vars(ap.parse_args()) # load the image, convert it to grayscale, and blur it # image = cv2.imread(args["image"]) # image = cv2.imread("1.jpg") image = rgb_image gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) blurred = cv2.GaussianBlur(gray, (11, 11), 0) # threshold the image to reveal light regions in the # blurred image thresh = cv2.threshold(blurred, 205, 255, cv2.THRESH_BINARY)[1] # cv2.imshow("Image_Gaussian", thresh) # perform a series of erosions and dilations to remove # any small blobs of noise from the thresholded image thresh = cv2.erode(thresh, None, iterations=2) thresh = cv2.dilate(thresh, None, iterations=4) # perform a connected component analysis on the thresholded # image, then initialize a mask to store only the "large" # components labels = measure.label(thresh, neighbors=8, background=0) mask = np.zeros(thresh.shape, dtype="uint8") # print("labels",labels) # loop over the unique components for label in np.unique(labels): # if this is the background label, ignore it if label == 0: continue # otherwise, construct the label mask and count the # number of pixels labelMask = np.zeros(thresh.shape, dtype="uint8") labelMask[labels == label] = 255 numPixels = cv2.countNonZero(labelMask) print("numPixels", numPixels) # if the number of pixels in the component is sufficiently # large, then add it to our mask of "large blobs" if numPixels > 10: mask = cv2.add(mask, labelMask) # find the contours in the mask, then sort them from left to # right cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) cnts = imutils.grab_contours(cnts) cnts = contours.sort_contours(cnts)[0] print("cnts", cnts) # loop over the contours for (i, c) in enumerate(cnts): # draw the bright spot on the image (x, y, w, h) = cv2.boundingRect(c) ((cX, cY), radius) = cv2.minEnclosingCircle(c) # seraching the minimum square circle print("The minimum circle center", (cX, cY)) cv2.circle(image, (int(cX), int(cY)), int(radius), (0, 0, 255), 3) # print("") cv2.putText(image, "#{}".format(i + 1), (x, y - 15), cv2.FONT_HERSHEY_SIMPLEX, 0.45, (0, 0, 255), 2) strresuv.update({i+1:[int(cX), int(cY)]}) # strresuv.append({i+1:[cX,cY]}) print "srereuv",strresuv if len(strresuv)==4: strpoint_uv.tile_id = 0 strpoint_uv.f1th_uv.uvinfo = strresuv[1] strpoint_uv.s2th_uv.uvinfo = strresuv[2] strpoint_uv.t3th_uv.uvinfo = strresuv[3] strpoint_uv.f4th_uv.uvinfo = strresuv[4] self.strpoint_pub.publish(strpoint_uv) else: print "please wait ----------" # strresuv.append([i+1,(cX,cY)]) # if len(strresuv) != 0: # self.strpoint_pub.publish([self.resuv[-1][0], self.resuv[-1][1]]) # else: # print "wait detecting point-------" cv2.namedWindow( 'Structure_point_detecting_window_edges', cv2.WINDOW_NORMAL ) cv2.imshow( 'Structure_point_detecting_window_edges', thresh ) cv2.namedWindow( 'Structure_point_window', cv2.WINDOW_NORMAL ) cv2.imshow( 'Structure_point_window', image ) cv2.waitKey(8) # # 再将opencv格式额数据转换成ros image格式的数据发布 try: self.image_pub.publish(self.bridge.cv2_to_imgmsg(rgb_image, "bgr8")) except CvBridgeError as e: print e return central_list
def Draw_triangle(self, contours, rgb, tile_id, obj_desire): ################## DELAY = 0.02 USE_CAM = 1 IS_FOUND = 0 count = 0 #count feature tile numbers cnt = 0 central_list = [] uvuv = uv() tile_uv = tileuv() ################## _width = 480.0 _height = 640.0 _margin = 0.0 corners = np.array([ [[_margin, _margin]], [[_margin, _height + _margin]], [[_width + _margin, _height + _margin]], [[_width + _margin, _margin]], ]) pts_dst = np.array(corners, np.float32) for cont in contours: resultuv = [] """ #1,num,2,centeral point 3,for angular point uv ,4,clockwise direction #caculating Area for tile selected just one tile """ if cv2.contourArea(cont) > 5000 and cv2.contourArea(cont) < 60000: # print "cont----------", cont # 获取轮廓长度 arc_len = cv2.arcLength(cont, True) # 多边形拟合 approx = cv2.approxPolyDP(cont, 0.1 * arc_len, True) if (len(approx) == 4): IS_FOUND = 1 M = cv2.moments(cont) # 获取图像质心坐标 cX = int(M["m10"] / M["m00"]) cY = int(M["m01"] / M["m00"]) cv2.putText(rgb, obj_desire, (cX, cY), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 255), 3) print "CX,CY", [cX, cY] central_list.append([cX, cY]) pts_src = np.array(approx, np.float32) # print "pts_src", pts_src cv2.circle(rgb, (cX, cY), 5, (0, 0, 0), -1) print approx.tolist() angular_point = [] for i in range(len(approx.tolist())): if i == 0: cv2.circle(rgb, (approx.tolist()[i][0][0], approx.tolist()[i][0][1]), 5, (20, 60, 220), -1) print "first point x,y,others use clockwise---", approx.tolist()[i][0][0], \ approx.tolist()[i][0][1] angular_point.append([ approx.tolist()[i][0][0], approx.tolist()[i][0][1] ]) cv2.putText(rgb, str(i), (approx.tolist()[i][0][0], approx.tolist()[i][0][1]), cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (0, 0, 0), 2) else: cv2.circle(rgb, (approx.tolist()[i][0][0], approx.tolist()[i][0][1]), 5, (0, 255, 0), -1) print "x,y", approx.tolist( )[i][0][0], approx.tolist()[i][0][1] angular_point.append([ approx.tolist()[i][0][0], approx.tolist()[i][0][1] ]) print "chr(i)", str(i) cv2.putText(rgb, str(i), (approx.tolist()[i][0][0], approx.tolist()[i][0][1]), cv2.FONT_HERSHEY_COMPLEX_SMALL, 1, (0, 0, 0), 2) resultuv.append([[tile_id], [cX, cY], angular_point]) # draw trangle in image h, status = cv2.findHomography(pts_src, pts_dst) out = cv2.warpPerspective(rgb, h, (int(_width + _margin * 2), int(_height + _margin * 2))) cv2.drawContours(rgb, [approx], -1, (0, 255, 255), 3) print "all info for tile------", resultuv print "Now tile id", tile_id tile_uv.tile_id = tile_id tile_uv.obj_desire = obj_desire tile_uv.cen_uv.uvinfo = [cX, cY] tile_uv.f1th_uv.uvinfo = angular_point[0] tile_uv.s2th_uv.uvinfo = angular_point[1] tile_uv.t3th_uv.uvinfo = angular_point[2] tile_uv.f4th_uv.uvinfo = angular_point[3] self.tile_pub.publish(tile_uv) else: pass # count += 1 # cnt += 11 return rgb.copy()
def process_rgb_image(self, rgb_image): central_list = [] ################## DELAY = 0.02 USE_CAM = 1 IS_FOUND = 0 count = 0 #count feature tile numbers cnt = 0 MORPH = 7 CANNY = 250 ################## _width = 480.0 _height = 640.0 _margin = 0.0 uvuv = uv() tile_uv = tileuv() ################## corners = np.array([ [[_margin, _margin]], [[_margin, _height + _margin]], [[_width + _margin, _height + _margin]], [[_width + _margin, _margin]], ]) pts_dst = np.array(corners, np.float32) rgb = rgb_image #print "rgb_image\n",rgb if rgb_image is not None: #转化为灰度图 # time.sleep(5) gray = cv2.cvtColor(rgb, cv2.COLOR_BGR2GRAY) #双边滤波 gray = cv2.bilateralFilter(gray, 1, 10, 120) #获取边缘信息 edges = cv2.Canny(gray, 10, CANNY) kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (MORPH, MORPH)) closed = cv2.morphologyEx(edges.copy(), cv2.MORPH_CLOSE, kernel) _, contours, h = cv2.findContours(closed.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) #print "contours",contours print "len(contours)", len(contours) print "h", h for cont in contours: resultuv = [ ] #1,num,2,centeral point 3,for angular point uv ,4,clockwise direction if cv2.contourArea(cont) > 5000: #print "cont", cont #获取轮廓长度 arc_len = cv2.arcLength(cont, True) #多边形拟合 approx = cv2.approxPolyDP(cont, 0.1 * arc_len, True) if (len(approx) == 4): IS_FOUND = 1 M = cv2.moments(cont) #获取图像质心坐标 cX = int(M["m10"] / M["m00"]) cY = int(M["m01"] / M["m00"]) cv2.putText(rgb, chr(ord('o') - cnt), (cX, cY), cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 0, 255), 3) print "CX,CY", [cX, cY] central_list.append([cX, cY]) pts_src = np.array(approx, np.float32) print "pts_src", pts_src cv2.circle(rgb, (cX, cY), 5, (0, 0, 0), -1) print approx.tolist() angular_point = [] for i in range(len(approx.tolist())): if i == 0: cv2.circle(rgb, (approx.tolist()[i][0][0], approx.tolist()[i][0][1]), 5, (20, 60, 220), -1) print "first point x,y,others use clockwise---", approx.tolist( )[i][0][0], approx.tolist()[i][0][1] angular_point.append([ approx.tolist()[i][0][0], approx.tolist()[i][0][1] ]) else: cv2.circle(rgb, (approx.tolist()[i][0][0], approx.tolist()[i][0][1]), 5, (0, 255, 0), -1) print "x,y", approx.tolist( )[i][0][0], approx.tolist()[i][0][1] angular_point.append([ approx.tolist()[i][0][0], approx.tolist()[i][0][1] ]) resultuv.append([[count], [cX, cY], angular_point]) #draw trangle in image h, status = cv2.findHomography(pts_src, pts_dst) out = cv2.warpPerspective(rgb, h, (int(_width + _margin * 2), int(_height + _margin * 2))) cv2.drawContours(rgb, [approx], -1, (0, 255, 255), 1) print "all info for tile------", resultuv tile_uv.tile_id = count tile_uv.obj_desire = chr(ord('o') - cnt) tile_uv.cen_uv.uvinfo = [cX, cY] tile_uv.f1th_uv.uvinfo = angular_point[0] tile_uv.s2th_uv.uvinfo = angular_point[1] tile_uv.t3th_uv.uvinfo = angular_point[2] tile_uv.f4th_uv.uvinfo = angular_point[3] self.tile_pub.publish(tile_uv) else: pass count += 1 cnt += 11 #cv2.imshow( 'closed', closed ) #cv2.imshow( 'gray', gray ) cv2.namedWindow('tile_edges', cv2.WINDOW_NORMAL) cv2.imshow('tile_edges', edges) cv2.namedWindow('tile_pixel_frame', cv2.WINDOW_NORMAL) cv2.imshow('tile_pixel_frame', rgb) cv2.waitKey(8) # # 再将opencv格式额数据转换成ros image格式的数据发布 try: self.image_pub.publish( self.bridge.cv2_to_imgmsg(rgb_image, "bgr8")) except CvBridgeError as e: print e return central_list
def process_rgb_image(self, rgb_image): central_list = [] sumuv = [] minuv = [] minuvuv = [] ################## DELAY = 0.02 USE_CAM = 1 IS_FOUND = 0 count = 0 #count feature tile numbers cnt = 0 MORPH = 7 CANNY = 250 ################## _width = 480.0 _height = 640.0 _margin = 0.0 error = 331 + 229 cornerssuv = [] uvuv = uv() tile_uv = tileuv() ################## corners = np.array([ [[_margin, _margin]], [[_margin, _height + _margin]], [[_width + _margin, _height + _margin]], [[_width + _margin, _margin]], ]) pts_dst = np.array(corners, np.float32) rgb = rgb_image #print "rgb_image\n",rgb t = 0 if rgb_image is not None: #转化为灰度图 # time.sleep(5) gray = cv2.cvtColor(rgb, cv2.COLOR_BGR2GRAY) #双边滤波 gray = cv2.bilateralFilter(gray, 1, 75, 75) #获取边缘信息 # edges = cv2.Canny( gray, 10, CANNY ) edges = cv2.Canny(gray, 50, 150, apertureSize=3) # corners = cv2.goodFeaturesToTrack(edges, 25, 0.01, 10) # # 返回的结果是 [[ 311., 250.]] 两层括号的数组。 # corners = np.int0(corners) # for i in corners: # x, y = i.ravel() # cv2.circle(rgb_image, (x, y), 3, 255, -1) minLineLength = 30 maxLineGap = 10 lines = cv2.HoughLinesP(edges, 1, np.pi / 180, 80, minLineLength, maxLineGap) print lines[0] for x1, y1, x2, y2 in lines[0]: cv2.circle(rgb, (x1, y1), 7, (0, 255, 255), -1) cv2.circle(rgb, (x2, y2), 7, (0, 255, 255), -1) print "(x1,y1)", x1, y1 print "(x2,y2)", x2, y2 # cv2.line(rgb_image, (x1, y1), (x2, y2), (0, 255, 0), 2) cv2.namedWindow('crossline_edges', cv2.WINDOW_NORMAL) cv2.imshow('crossline_edges', edges) cv2.namedWindow('central_frame', cv2.WINDOW_NORMAL) cv2.imshow('central_frame', rgb) cv2.waitKey(8) # # 再将opencv格式额数据转换成ros image格式的数据发布 try: self.image_pub.publish( self.bridge.cv2_to_imgmsg(rgb_image, "bgr8")) except CvBridgeError as e: print e return central_list
def main(): #uvlist=[123.0,112.0] uvlist = [] uvcentral = [331, 229] uv0 = UVpub("uv_design_pub", "/xp_uv/xp", uvcentral, 100, 5, 0.7) #x=[350,279] #x=[313,250] # xo=[515,89] # xd=[60,365] #xp=uv0.get_fov_desire_uv(x,xo,xd) xp_pub, led13, ur_pub = uv0.Init_node() ratet = 1 rate = rospy.Rate(ratet) #a=uv() b = uv() temp = [] xpp = [] flagg = 0 while not rospy.is_shutdown(): if len(led13.ledstate_buf): uv0.ledstate = led13.ledstate_buf[-1] try: if len(uv0.tile_0_buf) == 0: print "please wait the tile uv subscribe ok --------\n" pass elif len(uv0.tile_0_buf) != 0 and uv0.ledstate == 0: print "first ---- all tiles in fov ----\n" xo = list(uv0.tile_0_buf[-1][1]) b.uvinfo = xo xp_pub.publish(b) print "temp------\n", temp print "publish xp uv ------\n", xo elif len(uv0.tile_0_buf) == 10 and uv0.ledstate == 1: try: x = uvcentral xo = list(uv0.tile_0_buf[-1][1]) xd = xpp print "x,xo,xd", x, xo, xd xp = numpy.array(xo) - (numpy.array(x) - numpy.array(xd)) b.uvinfo = xp.tolist() xp_pub.publish(b) print "publish xp uv ------\n", xp except: pass else: print "just one tile in fov ----\n" x = uvcentral xo = list(uv0.tile_0_buf[-1][1]) xd = list(uv0.tile_0_buf[-1][1]) print "x,xo,xd", x, xo, xd xp = uv0.get_fov_desire_uv(x, xo, xd) b.uvinfo = xp xp_pub.publish(b) print "publish xp uv ------\n", xp # print "tile_reader",uv0.changeuv except KeyboardInterrupt: sys.exit() rate.sleep()
def process_rgb_image(self, rgb_image): central_list = [] sumuv = [] minuv = [] minuvuv = [] ################## DELAY = 0.02 USE_CAM = 1 IS_FOUND = 0 count = 0 #count feature tile numbers cnt = 0 MORPH = 7 CANNY = 250 ################## _width = 480.0 _height = 640.0 _margin = 0.0 error = 331 + 229 cornerssuv = [] uvuv = uv() tile_uv = tileuv() ################## corners = np.array([ [[_margin, _margin]], [[_margin, _height + _margin]], [[_width + _margin, _height + _margin]], [[_width + _margin, _margin]], ]) pts_dst = np.array(corners, np.float32) rgb = rgb_image #print "rgb_image\n",rgb t = 0 if rgb_image is not None: #转化为灰度图 # time.sleep(5) gray = cv2.cvtColor(rgb, cv2.COLOR_BGR2GRAY) #双边滤波 gray = cv2.bilateralFilter(gray, 1, 5, 200) #获取边缘信息 edges = cv2.Canny(gray, 10, CANNY) corners = cv2.goodFeaturesToTrack(gray, 3, 0.01, 1) # corners = np.int0(corners) for i in corners: #print "i-------",i x, y = i.ravel() cornerssuv.append([x, y]) #cv2.circle(rgb, (x, y), 7, 255, -1) sumuv.append([t, x + y]) #print "corners",(x,y) t += 1 for i in range(len(sumuv)): minuv.append([i, abs(sumuv[i][1] - error)]) print "minuv----", minuv, minuvuv for i in range(len(sumuv)): uv_u = [minuv[0][1], minuv[1][1], minuv[2][1]] ii = uv_u.index(min(uv_u)) if len(self.resuv) == 100: self.resuv = self.resuv[1:] self.resuv.append(cornerssuv[ii]) else: self.resuv.append(cornerssuv[ii]) if len(self.resuv) != 0: cv2.circle(rgb, (self.resuv[-1][0], self.resuv[-1][1]), 7, (0, 255, 255), -1) self.cross_pub.publish([self.resuv[-1][0], self.resuv[-1][1]]) else: print "wait point ok----" print "resuv----", self.resuv print "sumuv", sumuv print "len(resuv)", len(self.resuv) print "cornerssuv", cornerssuv cv2.namedWindow('crossline_edges', cv2.WINDOW_NORMAL) cv2.imshow('crossline_edges', edges) cv2.namedWindow('central_frame', cv2.WINDOW_NORMAL) cv2.imshow('central_frame', rgb) cv2.waitKey(8) # # 再将opencv格式额数据转换成ros image格式的数据发布 try: self.image_pub.publish( self.bridge.cv2_to_imgmsg(rgb_image, "bgr8")) except CvBridgeError as e: print e return central_list