def message(state=0,
cx1=0,
cy1=0,
cx2=0,
cy2=0,
forward=False,
backward=False,
left=False,
right=False,
area=0):
if state > 0:
himg, wimg = image.display.shape[:2]
cx1 = lib.Aconvert(cx1, wimg)
cy1 = -1.0 * lib.Aconvert(cy1, himg)
cx2 = lib.Aconvert(cx2, wimg)
cy2 = -1.0 * lib.Aconvert(cy2, himg)
area = 1.0 * area / (himg * wimg)
msg = vision_drum()
msg.state = state
msg.cx1 = cx1
msg.cy1 = cy1
msg.cx2 = cx2
msg.cy2 = cy2
msg.forward = forward
msg.backward = backward
msg.left = left
msg.right = right
msg.area = area
print msg
return msg
def message(state=0, cx=0.0, cy=0.0, area=0.0):
"""
group value into massage
"""
if (state > 0):
# convert x,y to range -1 - 1
himg, wimg = image.bgr.shape[:2]
cx = lib.Aconvert(cx, wimg)
cy = -1.0 * lib.Aconvert(cy, himg)
area = lib.Aconvert(area, (himg * wimg))
# group value into vision_flare
msg = vision_flare()
msg.state = state
msg.cx = cx
msg.cy = cy
msg.area = area
print msg
return msg
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 message(state=0, pos=0, cx1=0.0, cy1=0.0, cx2=0.0, cy2=0.0, area=0.0):
if (state > 0):
himg, wimg = image.display.shape[:2]
cx1 = lib.Aconvert(cx1, wimg)
cy1 = -1.0 * lib.Aconvert(cy1, himg)
cx2 = lib.Aconvert(cx2, wimg)
cy2 = -1.0 * lib.Aconvert(cy2, himg)
msg = vision_gate()
msg.state = state
msg.pos = pos
msg.cx1 = cx1
msg.cy1 = cy1
msg.cx2 = cx2
msg.cy2 = cy2
msg.area = area
if DEBUG['console'] or DEBUG['detail']:
print msg
rospy.sleep(0.05)
lib.clear_screen()
return msg
def message(state=0, pos=0, x1=0.0, y1=0.0, x2=0.0, y2=0.0, area=0.0):
"""
group value into massage
"""
# convert x,y to range -1 - 1
if(state >= 0):
himg, wimg = image.bgr.shape[:2]
x1 = lib.Aconvert(x1, wimg)
x2 = lib.Aconvert(x2, wimg)
y1 = -1.0*lib.Aconvert(y1, himg)
y2 = -1.0*lib.Aconvert(y2, himg)
# group value into vision_gate
msg = vision_gate()
msg.state = state
msg.pos = pos
msg.cx1 = x1
msg.cy1 = y1
msg.cx2 = x2
msg.cy2 = y2
msg.area = area
if DEBUG['console'] or DEBUG['detail']:
print msg
return msg