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
