def find_near_flare():
if image.bgr is None:
lib.img_is_none()
return message(state=-1)
image.renew_display()
pre_process = lib.pre_process(image.bgr, 'flare')
mask = get_mask()
kernel_box = lib.get_kernel(ksize=(7, 7))
kernel_vertical = lib.get_kernel(ksize=(1, 25))
vertical = cv.erode(mask.copy(), kernel_vertical)
vertical = cv.dilate(vertical.copy(), kernel_box)
kernel_erode = lib.get_kernel(ksize=(3, 13))
vertical = cv.erode(vertical.copy(), kernel_erode)
mask = vertical
cx, cy, area, obj, box = get_obj(mask)
mode = area != 0
color = ct.PURPLE
if mode == 0:
lib.print_result("NOT FOUND " + color + "(NEAR)", ct.RED)
lib.publish_result(image.display, 'bgr', PUBLIC_TOPIC + 'near/display')
lib.publish_result(mask, 'gray', PUBLIC_TOPIC + 'near/mask')
return message()
if mode == 1:
cv.circle(image.display, (int(cx), int(cy)), 3, (0, 255, 255), -1)
cv.drawContours(image.display, [box], 0, (0, 255, 0), 2)
lib.print_result("FOUND " + color + "(NEAR)", ct.GREEN)
lib.publish_result(image.display, 'bgr', PUBLIC_TOPIC + 'near/display')
lib.publish_result(mask, 'gray', PUBLIC_TOPIC + 'near/mask')
return message(cx=cx, cy=cy, area=area, state=mode)
return message(state=0)
def find_drum(color, return_option):
global mask_gripper
if image.bgr is None:
lib.img_is_none()
return message(state=-1)
himg, wimg = image.display.shape[:2]
drum_mask = get_mask(color)
obj = get_obj(drum_mask, color)
state = obj != []
if state == 0:
lib.print_result("CANNOT FOUND DRUM", ct.RED)
lib.publish_result(drum_mask, 'gray', public_topic + 'mask/drum')
lib.publish_result(image.display, 'bgr', public_topic + 'image_result')
return message()
lib.print_result("FOUND DRUM", ct.GREEN)
cv.circle(image.display, lib.most_point(obj, 'right'), 5, (255, 255, 0),
-1)
cv.circle(image.display, lib.most_point(obj, 'left'), 5, (0, 255, 255), -1)
cx1, cy1, cx2, cy2, area = get_cx(obj, return_option=return_option)
forward, backward, left, right = get_excess(obj)
lib.publish_result(drum_mask, 'gray', public_topic + 'mask/drum')
lib.publish_result(image.display, 'bgr', public_topic + 'display')
return message(state=state,
cx1=cx1,
cy1=cy1,
cx2=cx2,
cy2=cy2,
forward=forward,
backward=backward,
left=left,
right=right,
area=area)
def find_drum(color, return_option):
if image.bgr is None:
lib.img_is_none()
return message(state=-1)
image.renew_display()
himg, wimg = image.display.shape[:2]
drum_mask = get_mask(color)
obj = get_obj(drum_mask, color)
state = obj != []
if state == 0:
lib.print_result("CANNOT FOUND DRUM", ct.RED)
lib.publish_result(drum_mask, 'gray', public_topic + 'mask')
lib.publish_result(image.display, 'bgr', public_topic + 'display')
return message()
temp_function(obj)
lib.print_result("FOUND DRUM", ct.GREEN)
cv.circle(image.display, lib.most_point(obj, 'right'), 10, (255, 255, 0),
-1)
cv.circle(image.display, lib.most_point(obj, 'left'), 10, (0, 255, 255),
-1)
cx1, cy1, cx2, cy2, area = get_cx(obj, return_option=return_option)
t1 = lib.Aconvert(cx1, wimg)
ty1 = -1.0 * lib.Aconvert(cy1, himg)
t2 = lib.Aconvert(cx2, wimg)
ty2 = -1.0 * lib.Aconvert(cy2, himg)
forward, backward, left, right = get_excess(obj)
cv.putText(image.display, "pt1 = ({:.2f},{:.2f})".format(t1, ty1),
(0, himg - 100), cv.FONT_HERSHEY_SIMPLEX, 2, 255, 5)
cv.putText(image.display, "pt2 = ({:.2f},{:.2f})".format(t2, ty2),
(0, himg - 20), cv.FONT_HERSHEY_SIMPLEX, 2, 255, 5)
lib.publish_result(drum_mask, 'gray', public_topic + 'mask')
lib.publish_result(image.display, 'bgr', public_topic + 'display')
return message(state=state,
cx1=cx1,
cy1=cy1,
cx2=cx2,
cy2=cy2,
forward=forward,
backward=backward,
left=left,
right=right,
area=area)
def find_mat():
if image.bgr is None:
lib.img_is_none()
return message(state=-1)
image.renew_display()
himg, wimg = image.display.shape[:2]
mat_mask = get_mask("green")
contours = cv.findContours(mat_mask, cv.RETR_EXTERNAL,
cv.CHAIN_APPROX_SIMPLE)[1]
processed_contours = []
for cnt in contours:
if cv.contourArea(cnt) < 1000:
continue
processed_contours.append(cnt)
state = len(processed_contours)
lib.publish_result(mat_mask, 'gray', public_topic + 'mask/mat')
lib.publish_result(image.display, 'bgr', public_topic + 'display/mat')
if state == 0:
lib.print_result("CANNOT FOUND MAT", ct.RED)
return message()
lib.print_result("FOUND MAT", ct.GREEN)
return message(state=1)
def find_golf(objective):
if image.bgr is None:
lib.img_is_none()
return message(state=-1)
image.renew_display()
image.get_gray()
himg, wimg = image.bgr.shape[:2]
obj = lib.bg_subtraction(image.gray, mode='pos')
drum_mask = get_mask("blue", shade='golf')
cnt = get_obj(drum_mask, "blue")
temp = np.zeros((himg, wimg), np.uint8)
if len(cnt) > 0:
(x, y), radius = cv.minEnclosingCircle(cnt)
center = (int(x), int(y))
radius = int(radius)
cv.circle(temp, center, radius, (255), -1)
# cv.drawContours(temp, np.array(cnt), 0, (255), -1)
obj = cv.bitwise_and(obj, temp)
contours = cv.findContours(obj.copy(), cv.RETR_EXTERNAL,
cv.CHAIN_APPROX_NONE)[1]
circle = []
for cnt in contours:
area_cnt = cv.contourArea(cnt)
(x, y), radius = cv.minEnclosingCircle(cnt)
center = (int(x), int(y))
area_cir = math.pi * (radius**2)
if area_cir <= 0 or area_cnt / area_cir < 0.8:
continue
cv.circle(image.display, center, int(radius), lib.get_color('yellow'),
3)
circle.append([x, y, radius])
state = len(circle)
if state == 0:
lib.print_result("CANNOT FOUND GOLF", ct.RED)
lib.publish_result(obj, 'gray', public_topic + 'golf/mask')
lib.publish_result(image.display, 'bgr', public_topic + 'golf/display')
return message()
himg, wimg = image.display.shape[:2]
lib.print_result("FOUND GOLF", ct.GREEN)
circle = sorted(circle, key=itemgetter(2), reverse=True)
circle = circle[0]
x, y, radius = circle
cv.circle(image.display, (int(x), int(y)), int(radius),
lib.get_color('green'), 3)
x1, x2 = max(x - radius, 0), min(x + radius, wimg)
y1, y2 = max(y - radius, 0), min(y + radius, himg)
area = math.pi * (radius**2)
forward, backward, left, right = get_excess(obj)
lib.publish_result(obj, 'gray', public_topic + 'golf/mask')
lib.publish_result(image.display, 'bgr', public_topic + 'golf/display')
return message(state=state,
cx1=x1,
cy1=y1,
cx2=x2,
cy2=y2,
forward=forward,
backward=backward,
left=left,
right=right,
area=area)
def find_qualify_pole():
if image.bgr is None:
lib.img_is_none()
return message(state=-1)
image.renew_display()
log.update_time()
image.get_gray()
hsv = cv.cvtColor(image.bgr, cv.COLOR_BGR2HSV)
hue, s, v = cv.split(hsv)
# hue = cv.equalizeHist(hue)
clahe = cv.createCLAHE(clipLimit=2.0, tileGridSize=(8, 8))
cl1 = clahe.apply(hue)
obj = lib.bg_subtraction(cl1, mode='neg', fg_blur_size=3)
kernel_box = lib.get_kernel(ksize=(7, 7))
kernel_vertical = lib.get_kernel(ksize=(1, 25))
kernel_horizontal = lib.get_kernel(ksize=(25, 1))
vertical = cv.erode(obj.copy(), kernel_vertical)
vertical = cv.dilate(vertical.copy(), kernel_box)
hori = cv.erode(obj.copy(), kernel_horizontal)
hori = cv.dilate(hori.copy(), kernel_box)
kernel_erode = lib.get_kernel(ksize=(3, 11))
kernel_erode_hori = lib.get_kernel(ksize=(11, 3))
vertical = cv.erode(vertical.copy(), kernel_erode)
hori = cv.erode(hori.copy(), kernel_erode_hori)
vertical_pipe, no_pipe_v = find_pipe(vertical)
vertical_cx1 = []
vertical_cx2 = []
vertical_cy1 = []
vertical_cy2 = []
for res in vertical_pipe:
x, y, w, h, angle = res
cv.rectangle(image.display, (int(x - w / 2.), int(y - h / 2.)),
(int(x + w / 2.), int(y + h / 2.)), (108, 105, 255), 2)
vertical_cx1.append((x - w / 2.))
vertical_cx2.append((x + w / 2.))
vertical_cy1.append((y - h / 2.))
vertical_cy2.append((y + h / 2.))
himg, wimg = obj.shape[:2]
mode = no_pipe_v
lib.print_result("NOT FOUND", ct.RED)
lib.publish_result(image.display, 'bgr', PUBLIC_TOPIC + 'display')
lib.publish_result(vertical, 'gray', PUBLIC_TOPIC + 'mask/vertical')
lib.publish_result(obj, 'gray', PUBLIC_TOPIC + 'mask')
lib.publish_result(image.gray, 'gray', PUBLIC_TOPIC + 'gray')
lib.publish_result(hue, 'gray', PUBLIC_TOPIC + 'hue')
lib.publish_result(hori, 'gray', PUBLIC_TOPIC + 'hori')
if mode == 0:
return message()
elif mode == 1:
lib.print_result("FOUNG ONE POLE", ct.YELLOW)
elif mode == 2:
lib.print_result("FOUND", ct.GREEN)
cx1 = min(vertical_cx2)
cx2 = max(vertical_cx1)
cy1 = max(vertical_cy1)
cy2 = min(vertical_cy2)
cx1, cx2 = min(cx1, cx2), max(cx1, cx2)
cy1, cy2 = min(cy1, cy2), max(cy1, cy2)
cx1, cx2 = max(cx1, 0), min(cx2, wimg)
cy1, cy2 = max(cy1, 0), min(cy2, himg)
cv.rectangle(image.display, (int(cx1), int(cy1)), (int(cx2), int(cy2)),
(0, 255, 0), 3)
cv.circle(image.display, (int((cx1 + cx2) / 2), int((cy1 + cy2) / 2)), 3,
(0, 255, 255), -1)
area = 1.0 * abs(cx2 - cx1) * abs(cy2 - cy1) / (himg * wimg)
lib.publish_result(image.display, 'bgr', PUBLIC_TOPIC + 'display')
lib.publish_result(vertical, 'gray', PUBLIC_TOPIC + 'mask/vertical')
# lib.publish_result(obj, 'gray', PUBLIC_TOPIC + 'mask')
lib.publish_result(image.gray, 'gray', PUBLIC_TOPIC + 'gray')
lib.publish_result(hue, 'gray', PUBLIC_TOPIC + 'hue')
# lib.publish_result(hori, 'gray', PUBLIC_TOPIC + 'hori')
log.assume_pole(mode=mode, x=cx1, y=cy1)
pos = log.assume_to_pos()
log.save_data(state=mode, cx1=cx1, cx2=cx2, cy1=cy1, cy2=cy2)
return message(state=mode,
cx1=cx1,
cy1=cy1,
cx2=cx2,
cy2=cy2,
pos=pos,
area=area)
def find_gate():
if image.bgr is None:
lib.img_is_none()
return message(state=-1)
pre_process = lib.pre_process(image.bgr,'gate')
gray = cv.cvtColor(pre_process.copy(), cv.COLOR_BGR2GRAY)
hsv = cv.cvtColor(pre_process,cv.COLOR_BGR2HSV)
b,g,r = cv.split(pre_process)
# equ = lib.equalize(gray)
clahe = cv.createCLAHE(clipLimit=2.0, tileGridSize=(8,8))
equ = clahe.apply(b)
lib.publish_result(equ, 'gray', PUBLIC_TOPIC + 'gray')
obj = lib.bg_subtraction(gray,mode='neg')
kernel_box = lib.get_kernel(ksize=(7, 7))
kernel_vertical = lib.get_kernel(ksize=(1, 21))
vertical = cv.erode(obj.copy(), kernel_vertical)
vertical = cv.dilate(vertical.copy(), kernel_box)
kernel_erode = lib.get_kernel(ksize=(3, 11))
vertical = cv.erode(vertical.copy(), kernel_erode)
# remove flare from vertical
vertical = cv.bitwise_and(vertical,cv.bitwise_not(get_flare(pre_process)))
kernel_horizontal = lib.get_kernel(ksize=(21, 1))
horizontal = cv.erode(obj.copy(), kernel_horizontal)
horizontal = cv.dilate(horizontal.copy(), kernel_box)
kernel_erode = lib.get_kernel(ksize=(11, 3))
horizontal = cv.erode(horizontal.copy(), kernel_erode)
horizontal = cv.bitwise_and(horizontal,cv.bitwise_not(get_flare(pre_process)))
vertical_pipe, no_pipe_v = find_pipe(vertical, 'v')
print('v',no_pipe_v)
horizontal_pipe, no_pipe_h = find_pipe(horizontal, 'h')
print('h',no_pipe_h)
# horizontal
if no_pipe_h > 0:
x, y, w, h, angle = horizontal_pipe[0]
# cv.rectangle(display, (int(x - w / 2.), int(y - h / 2.)),
# (int(x + w / 2.), int(y + h / 2.)), (0, 255, 0), 2)
horizontal_x = [(x - w / 2.), (x + w / 2.)]
horizontal_y = [(y - h / 2.), (y + h / 2.)]
vertical_x1 = []
vertical_x2 = []
vertical_y1 = []
vertical_y2 = []
for res in vertical_pipe:
x, y, w, h, angle = res
cv.rectangle(image.display, (int(x - w / 2.), int(y - h / 2.)),
(int(x + w / 2.), int(y + h / 2.)), (108, 105, 255), 2)
vertical_x1.append((x - w / 2.))
vertical_x2.append((x + w / 2.))
vertical_y1.append((y - h / 2.))
vertical_y2.append((y + h / 2.))
himg, wimg = obj.shape[:2]
state = no_pipe_v
if no_pipe_v == 1:
state == 1
elif no_pipe_v == 2:
state == 2
else:
state == 0
if state == 0:
lib.print_result("NOT FOUND", ct.RED)
lib.publish_result(image.display, 'bgr', PUBLIC_TOPIC + 'display')
lib.publish_result(vertical, 'gray', PUBLIC_TOPIC + 'mask/vertical')
lib.publish_result(horizontal, 'gray',
PUBLIC_TOPIC + 'mask/horizontal')
lib.publish_result(obj, 'gray', PUBLIC_TOPIC + 'mask')
return message()
if state == 2:
lib.print_result("FOUND GATE", ct.GREEN)
x1 = max(vertical_x1)
x2 = min(vertical_x2)
y1 = min(vertical_y1)
y2 = max(vertical_y2)
if state == 1:
lib.print_result("FOUND ONE V", ct.YELLOW)
x1 = int(min(vertical_x1))
x2 = int(max(vertical_x2))
y1 = int(min(vertical_y1))
y2 = int(max(vertical_y2))
h = int(abs(x1-x2))
cy = (y1+y2)/2
# print((y1,h,x1,x2))
gray = equ
temp = int(max(y1-1.5*h,0))
left_h = gray[temp:y1+h/2, 0:x1]
right_h = gray[temp:y1+h/2, x2:wimg]
low_left_h = gray[cy:cy+h/2, 0:x1]
low_right_h = gray[cy:cy+h/2, x2:wimg]
lib.publish_result(left_h, 'gray', PUBLIC_TOPIC + 'l')
lib.publish_result(right_h, 'gray', PUBLIC_TOPIC + 'r')
lib.publish_result(low_left_h, 'gray', PUBLIC_TOPIC + 'll')
lib.publish_result(low_right_h, 'gray', PUBLIC_TOPIC + 'lr')
# mode_left_h = get_mode(left_h.ravel())
# mode_right_h = get_mode(right_h.ravel())
# mode_low_left_h = get_mode(low_left_h.ravel())
# mode_low_right_h = get_mode(low_right_h.ravel())
mean_left_h = 1.0*get_mean(left_h.ravel())
mean_right_h = 1.0*get_mean(right_h.ravel())
mean_low_left_h = 1.0*get_mean(low_left_h.ravel())
mean_low_right_h = 1.0*get_mean(low_right_h.ravel())
diff_left = abs(mean_left_h-mean_low_left_h)
diff_right = abs(mean_right_h-mean_low_right_h)
print(mean_left_h,mean_low_left_h,mean_right_h,mean_low_right_h)
# if(mean_left_h < mean_right_h):
# print(abs(diff_left-diff_right))
# if abs(diff_left-diff_right) < 500:
# pass
if (diff_left > diff_right):
x1, x2 = 0, min(x1,x2)
else:
x1, x2 = max(x1,x2), wimg
right_excess = (x2 > 0.95*wimg)
left_excess = (x1 < (0.05*wimg))
if (right_excess and not left_excess):
pos = 1
elif (not right_excess and left_excess):
pos = -1
else:
pos = 0
cv.rectangle(image.display, (int(x1), int(y1)),
(int(x2), int(y2)), (0, 255, 0), 3)
cv.circle(image.display, (int((x1+x2)/2), int((y1+y2)/2)),
3, (0, 255, 255), -1)
area = 1.0*abs(x2-x1)*abs(y1-y2)/(himg*wimg)
lib.publish_result(image.display, 'bgr', PUBLIC_TOPIC + 'display')
lib.publish_result(vertical, 'gray', PUBLIC_TOPIC + 'mask/vertical')
lib.publish_result(horizontal, 'gray', PUBLIC_TOPIC + 'mask/horizontal')
lib.publish_result(obj, 'gray', PUBLIC_TOPIC + 'mask')
return message(state=state, x1=x1, x2=x2, y1=y1, y2=y2, pos=pos, area=area)
def find_pole():
if image.bgr is None:
lib.img_is_none()
return message(state=-1)
image.renew_display()
log.update_time()
image.get_gray()
obj = lib.bg_subtraction(image.gray, mode='neg', fg_blur_size=3)
kernel_box = lib.get_kernel(ksize=(7, 7))
kernel_vertical = lib.get_kernel(ksize=(1, 25))
vertical = cv.erode(obj.copy(), kernel_vertical)
vertical = cv.dilate(vertical.copy(), kernel_box)
kernel_erode = lib.get_kernel(ksize=(3, 11))
vertical = cv.erode(vertical.copy(), kernel_erode)
vertical_pipe, no_pipe_v = find_pipe(vertical)
vertical_cx1 = []
vertical_cx2 = []
vertical_cy1 = []
vertical_cy2 = []
for res in vertical_pipe:
x, y, w, h, angle = res
cv.rectangle(image.display, (int(x - w / 2.), int(y - h / 2.)),
(int(x + w / 2.), int(y + h / 2.)), (108, 105, 255), 2)
vertical_cx1.append((x - w / 2.))
vertical_cx2.append((x + w / 2.))
vertical_cy1.append((y - h / 2.))
vertical_cy2.append((y + h / 2.))
himg, wimg = obj.shape[:2]
mode = no_pipe_v
if mode == 0:
lib.print_result("NOT FOUND", ct.RED)
lib.publish_result(image.display, 'bgr', PUBLIC_TOPIC + 'display')
lib.publish_result(vertical, 'gray', PUBLIC_TOPIC + 'mask/vertical')
lib.publish_result(obj, 'gray', PUBLIC_TOPIC + 'mask')
return message()
elif mode == 1:
lib.print_result("FOUNG ONE POLE", ct.YELLOW)
elif mode == 2:
lib.print_result("FOUND", ct.GREEN)
cx1 = min(vertical_cx2)
cx2 = max(vertical_cx1)
cy1 = max(vertical_cy1)
cy2 = min(vertical_cy2)
cx1, cx2 = min(cx1, cx2), max(cx1, cx2)
cy1, cy2 = min(cy1, cy2), max(cy1, cy2)
cx1, cx2 = max(cx1, 0), min(cx2, wimg)
cy1, cy2 = max(cy1, 0), min(cy2, himg)
cv.rectangle(image.display, (int(cx1), int(cy1)), (int(cx2), int(cy2)),
(0, 255, 0), 3)
cv.circle(image.display, (int((cx1 + cx2) / 2), int((cy1 + cy2) / 2)), 3,
(0, 255, 255), -1)
area = 1.0 * abs(cx2 - cx1) * abs(cy2 - cy1) / (himg * wimg)
lib.publish_result(image.display, 'bgr', PUBLIC_TOPIC + 'display')
lib.publish_result(vertical, 'gray', PUBLIC_TOPIC + 'mask/vertical')
lib.publish_result(obj, 'gray', PUBLIC_TOPIC + 'mask')
log.assume_pole(mode=mode, x=cx1, y=cy1)
pos = log.assume_to_pos()
log.save_data(state=mode, cx1=cx1, cx2=cx2, cy1=cy1, cy2=cy2)
return message(state=mode,
cx1=cx1,
cy1=cy1,
cx2=cx2,
cy2=cy2,
pos=pos,
area=area)
def find_far_flare():
if image.bgr is None:
lib.img_is_none()
return message(state=-1)
image.renew_display()
pre_process = lib.pre_process(image.bgr, 'flare')
gray = cv.cvtColor(pre_process.copy(), cv.COLOR_BGR2GRAY)
hsv = cv.cvtColor(pre_process, cv.COLOR_BGR2HSV)
h, s, v = cv.split(hsv)
obj = lib.bg_subtraction(h, mode='neg')
obj = rm_bg(obj)
lib.publish_result(obj, 'gray', PUBLIC_TOPIC + 'far/temp')
lib.publish_result(h, 'gray', PUBLIC_TOPIC + 'far/h_from_hsv')
lib.publish_result(gray, 'gray', PUBLIC_TOPIC + 'far/gray')
kernel_box = lib.get_kernel(ksize=(7, 7))
kernel_vertical = lib.get_kernel(ksize=(1, 25))
vertical = cv.erode(obj.copy(), kernel_vertical)
vertical = cv.dilate(vertical.copy(), kernel_box)
kernel_erode = lib.get_kernel(ksize=(3, 13))
vertical = cv.erode(vertical.copy(), kernel_erode)
# kernel = lib.get_kernel(ksize=(1,101))
# vertical = cv.dilate(vertical, kernel)
# vertical = cv.erode(vertical, kernel)
vertical_pipe, no_v_pipe = find_pipe(vertical)
mode = no_v_pipe
color = ct.CYAN
if mode == 0:
lib.print_result("NOT FOUND (FAR)", ct.RED)
lib.publish_result(image.display, 'bgr', PUBLIC_TOPIC + 'far/display')
lib.publish_result(vertical, 'gray',
PUBLIC_TOPIC + 'far/mask/vertical')
lib.publish_result(obj, 'gray', PUBLIC_TOPIC + 'far/mask')
return message()
x, y, w, h, angle = vertical_pipe[0]
cv.rectangle(image.display, (int(x - w / 2.), int(y - h / 2.)),
(int(x + w / 2.), int(y + h / 2.)), (0, 255, 0), 2)
vertical_x = [(x - w / 2.), (x + w / 2.)]
vertical_y = [(y - h / 2.), (y + h / 2.)]
himg, wimg = image.bgr.shape[:2]
x1, x2 = max(min(vertical_x), 0), min(max(vertical_x), wimg)
y1, y2 = max(min(vertical_y), 0), min(max(vertical_y), himg)
area = (x2 - x1) * (y2 - y1)
if area > 7000 or area < 700:
lib.print_result("NOT FOUND " + color + "(FAR)", ct.RED)
lib.publish_result(image.display, 'bgr', PUBLIC_TOPIC + 'far/display')
lib.publish_result(vertical, 'gray',
PUBLIC_TOPIC + 'far/mask/vertical')
lib.publish_result(obj, 'gray', PUBLIC_TOPIC + 'far/mask')
return message()
lib.print_result("FOUND " + color + "(FAR)", ct.GREEN)
cv.rectangle(image.display, (int(x1), int(y1)), (int(x2), int(y2)),
(0, 255, 0), 3)
cv.circle(image.display, (int((x1 + x2) / 2), int((y1 + y2) / 2)), 3,
(0, 255, 255), -1)
lib.publish_result(image.display, 'bgr', PUBLIC_TOPIC + 'far/display')
lib.publish_result(vertical, 'gray', PUBLIC_TOPIC + 'far/mask/vertical')
lib.publish_result(obj, 'gray', PUBLIC_TOPIC + 'far/mask')
return message(cx=(x1 + x2) / 2., cy=(y1 + y2) / 2., area=area, state=1)
def find_golf(objective):
global last_time, his, start_shade, shade
if image.bgr is None:
lib.img_is_none()
return message(state=-1)
gray = cv.cvtColor(image.bgr, cv.COLOR_BGR2GRAY)
# r,c = gray.shape
# mask_gripper = cv.resize(mask_gripper, (r,c))
bg = cv.medianBlur(gray, 61)
fg = cv.medianBlur(gray, 5)
sub_sign = np.int16(fg) - np.int16(bg)
sub_pos = np.clip(sub_sign.copy(), 0, sub_sign.copy().max())
sub_neg = np.clip(sub_sign.copy(), sub_sign.copy().min(), 0)
sub_pos = normalize(sub_pos)
sub_neg = normalize(sub_neg)
# cv.imshow('sub_pos',sub_pos)
# cv.imshow('sub_neg',sub_neg)
_, obj = cv.threshold(sub_pos, 0, 255, cv.THRESH_BINARY + cv.THRESH_OTSU)
print("obj", obj.shape)
print("mask", mask_gripper.shape)
obj = cv.bitwise_and(obj, obj, mask=mask_gripper)
_, contours, _ = cv.findContours(obj.copy(), cv.RETR_EXTERNAL,
cv.CHAIN_APPROX_NONE)
display = cv.cvtColor(sub_neg.copy(), cv.COLOR_GRAY2BGR)
r = 0
circle = []
for cnt in contours:
area_cnt = cv.contourArea(cnt)
(x, y), radius = cv.minEnclosingCircle(cnt)
center = (int(x), int(y))
radius = radius
area_cir = math.pi * (radius**2)
if area_cir <= 0 or area_cnt / area_cir < 0.8:
continue
cv.circle(display, center, int(radius), (255, 0, 0), -1)
circle.append([x, y, radius])
row, col = gray.shape
if len(circle) > 0:
circle = sorted(circle, key=itemgetter(2), reverse=True)
circle = circle[0]
x, y, radius = circle
cv.circle(display, (int(x), int(y)), int(radius), (0, 0, 255), 2)
diameter = 2. * radius
pixel_per_cm = diameter / 4.3
print("radius", radius)
print("pixel_per_cm", pixel_per_cm)
print("depth", depth)
if True:
x_distance_pixel = row / 2. - y
y_distance_pixel = col / 2. - x
print("row col", row, col)
print("x y", x, y)
print("x_distance_pixel y_distance_pixel:", x_distance_pixel,
y_distance_pixel)
x_distance_cm = float(x_distance_pixel) / pixel_per_cm
y_distance_cm = float(y_distance_pixel) / pixel_per_cm
print("x_distance_cm:", x_distance_cm)
print("y_distance_cm:", y_distance_cm)
x_distance_meter = x_distance_cm / 100.
y_distance_meter = y_distance_cm / 100.
print("x_meter:", x_distance_meter)
print("y_meter:", y_distance_meter)
lib.publish_result(display, 'gray', public_topic + 'mask/golf')
# lib.publish_result(image.bgr, 'bgr', public_topic+'image_result')
return message(state=1,
cx1=y_distance_meter,
cy1=-x_distance_meter,
cx2=y_distance_meter,
cy2=-x_distance_meter,
forward=True,
backward=True,
left=True,
right=True,
area=0)
lib.publish_result(display, 'gray', public_topic + 'mask/golf')
return message(state=0,
cx1=y_distance_meter,
cy1=-x_distance_meter,
cx2=y_distance_meter,
cy2=-x_distance_meter,
forward=True,
backward=True,
left=True,
right=True,
area=0)