def roda_todo_frame(imagem):
global cv_image
global media
global centro
now = rospy.get_rostime()
imgtime = imagem.header.stamp
lag = now - imgtime # calcula o lag
delay = lag.nsecs
if delay > atraso and check_delay == True:
print("Descartando por causa do delay do frame:", delay)
return
try:
antes = time.clock()
cv_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8")
centro, imagem, resultados = visao_module.processa(cv_image)
media, centro, area = cormodule.identifica_cor(cv_image)
for i in resultados:
if i[0] == "car":
global isCar
isCar = True
depois = time.clock()
except CvBridgeError as e:
print('ex', e)
def roda_todo_frame(imagem):
print("frame")
global cv_image
global media
global centro
global area
#global distancia
global now
now = rospy.get_rostime()
imgtime = imagem.header.stamp
lag = now - imgtime
delay = lag.nsecs
if delay > atraso and check_delay == True:
return
try:
antes = time.clock()
cv_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8")
media, centro, area = cormodule.identifica_cor(
cv_image) #distancia removida#
#laser = le_scan.scaneou(dado)
depois = time.clock()
Features(cv_image)
#HoughLinesCode(cv_image)
cv2.imshow("Camera", cv_image)
except CvBridgeError as e:
print('ex', e)
def roda_todo_frame(imagem):
global cv_image
global media
global centro_pista
global resultados
global centro_robo
global media_creep
global maior_area
try:
cv_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8")
img_copy = cv_image.copy()
img_copy_pista = cv_image.copy()
img_copy_aruco = cv_image.copy()
img_copy_cor = cv_image.copy()
centro_pista, antolho, mask = procImg.procesa_imagem_pista(img_copy_pista)
# cv2.imshow("antolho", antolho)
cv2.imshow("mask", mask)
media_creep, maior_area, frame = cormodule.identifica_cor(img_copy_cor, goal1[0])
centro_robo = (img_copy.shape[1]//2, img_copy.shape[0]//2)
if centro_pista is not None:
crosshair(frame, centro_pista, 4, (0,0,255))
# cv2.imshow("cv_image", cv_image)
cv2.imshow("cor", frame)
cv2.waitKey(1)
except CvBridgeError as e:
print('ex', e)
def roda_todo_frame(imagem):
print("frame")
global cv_image
global media
global centro
global contornos
global dx
global stop
global bird
global contador
global centro_bird
global red
now = rospy.get_rostime()
imgtime = imagem.header.stamp
lag = now - imgtime # calcula o lag
delay = lag.nsecs
print("delay ", "{:.3f}".format(delay / 1.0E9))
if delay > atraso and check_delay == True:
print("Descartando por causa do delay do frame: ", delay)
return
try:
antes = time.clock()
cv_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8")
dx = cv_image.shape[1] // 10
depois = time.clock()
media, centro, area = cormodule.identifica_cor(cv_image)
if media[0] + media[1] != 0:
red = True
else:
red = False
centro, imagem, resultados = visao_module.processa(cv_image)
cv2.line(cv_image, (centro[0] + dx, 0),
(centro[0] + dx, cv_image.shape[1]), (0, 0, 255), 2)
cv2.line(cv_image, (centro[0] - dx, 0),
(centro[0] - dx, cv_image.shape[1]), (0, 0, 255), 2)
if resultados[0][0] == "bird":
print("PASSAROOOOOOOOOOOOOOO")
bird = True
x0, y0 = resultados[0][2]
x1, y1 = resultados[0][3]
centro_bird = [x0 + (x1 - x0) // 2, y0 + (y1 - y0) // 2]
cv2.circle(cv_image, (centro_bird[0], centro_bird[1]), 5,
(0, 0, 255), -1)
else:
bird = False
cv2.imshow("Camera", cv_image)
except CvBridgeError as e:
print('ex', e)
def roda_todo_frame(imagem):
global cv_image
global media
global centro_cor
global centro_mobile
global viu_bird
now = rospy.get_rostime()
imgtime = imagem.header.stamp
lag = now - imgtime # calcula o lag
delay = lag.nsecs
if delay > atraso and check_delay == True:
print("Descartando por causa do delay do frame:", delay)
return
try:
antes = time.clock()
cv_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8")
media, centro_cor, area = cormodule.identifica_cor(cv_image)
centro_mobile, imagem, resultados = visao_module.processa(cv_image)
depois = time.clock()
for r in resultados:
if r[0] == "bird":
viu_bird = True
cv2.imshow("Camera", cv_image)
except CvBridgeError as e:
print('ex', e)
def roda_todo_frame(imagem):
# print("frame")
global cv_image
global media
global centro
now = rospy.get_rostime()
imgtime = imagem.header.stamp
lag = now-imgtime # calcula o lag
delay = lag.nsecs
#print("delay ", "{:.3f}".format(delay/1.0E9))
if delay > atraso and check_delay==True:
print("Descartando por causa do delay do frame:", delay)
return
try:
antes = time.clock()
cv_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8")
media, centro, area = cormodule.identifica_cor(cv_image)
depois = time.clock()
cv2.imshow("Camera", cv_image)
except CvBridgeError as e:
print('ex', e)
def roda_todo_frame(imagem):
#print("frame")
global cv_image
global media
global centro
global media1
global maior_area
now = rospy.get_rostime()
imgtime = imagem.header.stamp
lag = now - imgtime # calcula o lag
delay = lag.nsecs
#print("delay ", "{:.3f}".format(delay/1.0E9))
if delay > atraso and check_delay == True:
#print("Descartando por causa do delay do frame:", delay)
return
try:
antes = time.clock()
cv_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8")
cv_image2 = cv_image.copy()
# cv_image = cv2.flip(cv_image, -1) # Descomente se for robo real
media, centro = cormodule.identifica_cor(
cv_image
) #puxa arquivo cormodule.py (centro = centro da image) (media= centro do maior contronro da cor desejada) (maior_area= tamanaho da area d maior contorno)
media1, maior_area = identifica_creeper(cv_image2)
depois = time.clock()
cv2.imshow("Camera", cv_image)
except CvBridgeError as e:
print('ex', e)
def roda_todo_frame(imagem):
print("frame")
global cv_image
global media_cor
global centro
global area
global menorDist
global aceleracao
global media_feature
global centro_feature
now = rospy.get_rostime()
imgtime = imagem.header.stamp
lag = now - imgtime
delay = lag.nsecs
if delay > atraso and check_delay == True:
return
try:
antes = time.clock()
cv_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8")
media_cor, centro, area = cormodule.identifica_cor(cv_image)
media_feature, centro_feature = identifica_feature(cv_image)
#scaneou(cv_image)
depois = time.clock()
cv2.imshow("Camera", cv_image)
except CvBridgeError as e:
print('ex', e)
def identifica_cor(imagem):
global areaCor
global cv_image
global mediaCor
global centroCor
global viu_cor
now = rospy.get_rostime()
imgtime = imagem.header.stamp
lag = now - imgtime
delay = lag.nsecs
if delay > atraso and check_delay == True:
print("Descartando por causa do delay do frame:", delay)
return
try:
antes = time.clock()
cv_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8")
mediaCor, centroCor, areaCor = cormodule.identifica_cor(
cv_image, margin)
print(mediaCor)
viu_cor = True
depois = time.clock()
cv2.imshow("Camera", cv_image)
except CvBridgeError as e:
print('ex', e)
def roda_todo_frame(imagem):
print("frame")
global cv_image
global ccaixa
global cimagem
global classe
now = rospy.get_rostime()
imgtime = imagem.header.stamp
lag = now - imgtime # calcula o lag
delay = lag.nsecs
print("delay ", "{:.3f}".format(delay / 1.0E9))
if delay > atraso and check_delay == True:
print("Descartando por causa do delay do frame:", delay)
return
try:
antes = time.clock()
cv_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8")
cv_image = cv2.flip(cv_image, -1)
centro, imagem, classe = visao_module.processa(cv_image)
documenta_visual(classe)
ccaixa, cimagem, area = cormodule.identifica_cor(cv_image)
depois = time.clock()
cv2.line(cv_image, (cimagem[0] - tolerancia, 0),
(cimagem[0] - tolerancia, cv_image.shape[0] - 1), (255, 0, 0),
5)
cv2.line(cv_image, (cimagem[0] + tolerancia, 0),
(cimagem[0] + tolerancia, cv_image.shape[0] - 1), (255, 0, 0),
5)
cv2.imshow("Camera", cv_image)
except CvBridgeError as e:
print('ex', e)
def roda_todo_frame(imagem):
global objeto, kp1, des1
global bbox, contador, frame
global media_cor, centro, area, fuga
# print("New Frame")
now = rospy.get_rostime()
imgtime = imagem.header.stamp
lag = now - imgtime
delay = lag.nsecs
if delay > atraso and check_delay == True:
return #Ou seja, para a função e descarta o frame
try:
frame = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8")
frame_cor = frame.copy()
media_cor, area = cormodule.identifica_cor(frame_cor)
# print(area)
if area > 14000:
fuga = True
else:
fuga = False
vai(frame, contador)
contador += 1
if contador > 2:
contador = 0
except CvBridgeError as e:
print('ex', e)
def cam_data(imagem):
global centro
global bridge
global cv_image
global dif
global media_blue
global see_bottle
global tracking_bottle
global consecutive_frame
global xy1
global xy0
global fps
global initBB
now = rospy.get_rostime()
imgtime = imagem.header.stamp
lag = now - imgtime # calcula o lag
delay = lag.nsecs
if delay > atraso and check_delay == True:
return
try:
antes = time.clock()
cv_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8")
centro_red, centro, media_blue, area_blue = cormodule.identifica_cor(
cv_image, margem, margem_vertical)
if not tracking_bottle:
centro, cv_image, results = visao_module.processa(cv_image)
for e in results:
if e[0] == "bottle":
xy0 = e[2]
xy1 = e[3]
x0, y0 = xy0
x1, y1 = xy1
centro_bottle = (x1 - x0, y1 - y0)
# dif = centro_bottle[0]-centro[1]
saw_bottle = True
consecutive_frame += 1
if consecutive_frame == 10:
tracking_bottle = True
initBB = (x0, y0, x1, y1)
if tracking_bottle:
tracker.init(cv_image, initBB)
fps = FPS().start()
(success, box) = tracker.update(cv_image)
if success:
(x, y, w, h) = [int(v) for v in box]
cv2.rectangle(cv_image, (x, y), (x + w, y + h), (0, 255, 0), 2)
centro_bottle = (x + w / 2, y + h / 2)
dif = centro_bottle[0] - centro[1]
fps.update()
fps.stop()
cv2.imshow("frame", cv_image)
except CvBridgeError as e:
print('ex', e)
def roda_todo_frame(img):
global cv_image
global media
global centro
try:
cv_image = bridge.compressed_imgmsg_to_cv2(img, "bgr8")
media, centro, maior_area = identifica_cor(cv_image)
except CvBridgeError as e:
print("ex", e)
def acha_cor(imagem):
global cv_image
global media
global centro
cv_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8")
media, centro, area = cormodule.identifica_cor(
cv_image) #media -> centro do contorno #Centro -> centro da tela
global dx
dx = media[0] - centro[1] # centro (y,x) media (x,y)
global dy
dy = media[1] - centro[0]
def Segue_cor(imagem):
# now = rospy.get_rostime()
# imgtime = imagem.header.stamp
# lag = now-imgtime # calcula o lag
# delay = lag.nsecs
# print("delay ", "{:.3f}".format(delay/1.0E9))
# if delay > atraso and check_delay==True:
# print("Descartando por causa do delay do frame:", delay)
# return
antes = time.clock()
# cv_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8")
media_cor, centro, area = cormodule.identifica_cor(imagem)
depois = time.clock()
return media_cor
def roda_todo_frame(imagem):
print("frame")
global temp_image
global media
global centro
global resultados
global distance
global ids
now = rospy.get_rostime()
imgtime = imagem.header.stamp
lag = now - imgtime # calcula o lag
delay = lag.nsecs
# print("delay ", "{:.3f}".format(delay/1.0E9))
if delay > atraso and check_delay == True:
# Esta logica do delay so' precisa ser usada com robo real e rede wifi
# serve para descartar imagens antigas
print("Descartando por causa do delay do frame:", delay)
return
try:
temp_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8")
mask = segmenta_linha_amarela(temp_image)
img = ajuste_linear_grafico_x_fy(mask)
"""Aqui é onde definimos nossas missões, sendo estas a cor e o id"""
#missao = ["orange", 11, "cow"]
missao = ["blue", 12, "dog"]
#missao = ["green", 23, "horse"]
#missao = ["Teste", 0, 0]
acha_creeper(missao, temp_image.copy())
media, centro, maior_area = cormodule.identifica_cor(mask)
cv2.imshow("Camera", img)
cv2.waitKey(1)
centro, saida_net, resultados = visao_module.processa(temp_image)
distance, ids = acha_aruco(temp_image)
ids = ids[0][0]
print("ids:", ids)
cv2.imshow("Aruco", temp_image)
cv2.waitKey(1)
for r in resultados:
# print(r) - print feito para documentar e entender
# o resultado
pass
# Desnecessário - Hough e MobileNet já abrem janelas
#cv_image = saida_net.copy()
#cv2.imshow("cv_image", img)
#cv2.waitKey(1)
except CvBridgeError as e:
print('ex', e)
def roda_todo_frame(imagem):
global cv_image
global media
global centro
global area
global resultados_mnet
global tracking
global contador
global V
now = rospy.get_rostime()
imgtime = imagem.header.stamp
lag = now - imgtime # calcula o lag
delay = lag.nsecs
if delay > atraso and check_delay == True:
print("Descartando por causa do delay do frame:", delay)
return
try:
if mnet.contador_mnet == 0 and contador == 10:
contador = 0
mnet.contador_mnet = 1000
antes = time.clock()
cv_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8")
media, centro, area = cormodule.identifica_cor(cv_image)
if area <= 0:
V = 0
else:
V = 10000 / area
imagem_ = cv_image
if contador < 10 and contador >= 0:
centro_mnet, imagem_, resultados_mnet = visao_module.processa(
cv_image)
tracking = False
if len(resultados_mnet) != 0:
contador += 1
else:
contador = 0
elif contador >= 10:
tracking = True
mnet.track(cv_image)
depois = time.clock()
cv2.imshow("Camera", imagem_)
except CvBridgeError as e:
print('ex', e)
def roda_todo_frame(imagem):
global cv_image
global media
global centro
now = rospy.get_rostime()
imgtime = imagem.header.stamp
lag = now - imgtime # calcula o lag
delay = lag.nsecs
if delay > atraso and check_delay == True:
return
try:
antes = time.clock()
cv_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8")
# cv_image = cv2.flip(cv_image, -1)
media, centro, maior_area = cormodule.identifica_cor(cv_image)
depois = time.clock()
cv2.imshow("Camera", cv_image)
except CvBridgeError as e:
print('ex', e)
def roda_todo_frame(imagem):
#print("frame")
global cv_image
global media
global centro
global maior_area
global colorLowRange
global colorHighRange
global frame_hsv
now = rospy.get_rostime()
imgtime = imagem.header.stamp
lag = now-imgtime # calcula o lag
delay = lag.nsecs
#print("delay ", "{:.3f}".format(delay/1.0E9))
if delay > atraso and check_delay==True:
print("Descartando por causa do delay do frame:", delay)
return
try:
antes = time.clock()
cv_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8")
# cv_image = cv2.flip(cv_image, -1)
frame_hsv = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV)
media, centro, maior_area = cormodule.identifica_cor(cv_image, colorLowRange, colorHighRange)
#verde (36,80, 80), (70,255,255)
depois = time.clock()
# cv2.imshow("Camera", cv_image)
# print("Média: ", media)
# print("Centro: ", centro)
# print("Maior Área: ", maior_area)
except CvBridgeError as e:
#print('ex', e)
print("")
def roda_todo_frame(imagem):
print("frame")
global cv_image
global resultados
global media
global centro
global temp_image
global ptom
global pto
global linha1
global linha2
now = rospy.get_rostime()
imgtime = imagem.header.stamp
lag = now - imgtime # calcula o lag
delay = lag.nsecs
print("delay ", "{:.3f}".format(delay / 1.0E9))
if delay > atraso and check_delay == True:
print("Descartando por causa do delay do frame:", delay)
return
try:
antes = time.clock()
cv_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8")
# media, centro, maior_area = cormodule.identifica_cor(
# cv_image, lista_quero[0])
centro, saida_net, resultados = visao_module.processa(temp_image)
media, centro, maior_contorno_area = cormodule.identifica_cor(
cv_image, lista_quero[0])
ptom = pto_fuga.acha_ponto(cv_image)
for r in resultados:
# print(r)
pass
depois = time.clock()
except CvBridgeError as e:
print('ex', e)
def roda_todo_frame(imagem):
print("frame")
global cv_image
global media
global centro
global area
now = rospy.get_rostime()
imgtime = imagem.header.stamp
lag = now - imgtime
delay = lag.secs
if delay > atraso and check_delay == True:
return
try:
antes = time.clock()
cv_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8")
media, centro, area = cormodule.identifica_cor(cv_image)
print("media {},{}".format(*media))
print("centro {},{}".format(*centro))
depois = time.clock()
cv2.imshow("Camera", cv_image)
except CvBridgeError as e:
print('ex', e)
def roda_todo_frame(imagem):
# print("frame")
global cv_image
global media
global centro
global viu_objeto
global pos_x
global area
now = rospy.get_rostime()
imgtime = imagem.header.stamp
lag = now-imgtime # calcula o lag
delay = lag.nsecs
#print("delay ", "{:.3f}".format(delay/1.0E9))
if delay > atraso and check_delay==True:
print("Descartando por causa do delay do frame:", delay)
return
try:
antes = time.clock()
cv_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8")
media, centro, area = cormodule.identifica_cor(cv_image)
centro1, imagem1, resultados = visao_module.processa(cv_image)
for r in resultados:
# print(r) - print feito para documentar e entender
# o resultado
if r[0] == (objeto):
viu_objeto = True
pos_x = (r[2][0]+r[3][0])/2
depois = time.clock()
depois = time.clock()
cv2.imshow("Camera", cv_image)
except CvBridgeError as e:
print('ex', e)
def roda_todo_frame(imagem):
global cv_image
global media
global centro
global area
global coringao
global caixa_cor
global f
f += 1
now = rospy.get_rostime()
imgtime = imagem.header.stamp
lag = now - imgtime
delay = lag.nsecs
# tratando o delay
if delay > atraso and check_delay == True:
print("delay {:.2f}".format(delay / 1.0E9))
return
try:
antes = time.clock()
cv_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8")
# identifica o símbolo do corinthians
media, centro, area, caixa_cor = cormodule.identifica_cor(cv_image)
# 1 frame a cada 3
if f % 3 == 0:
media_corin, coringao = corinthians.procuracor(cv_image)
media_corin = (0, 0)
if media_corin[0] != 0:
media = media_corin
depois = time.clock()
cv2.imshow("Camera", cv_image)
except CvBridgeError as e:
print('ex', e)
def run_all_frames(imagem):
global cv_image
global media
global centro
global cap_lock
global frame_capturado
now = rospy.get_rostime()
imgtime = imagem.header.stamp
lag = now-imgtime # calcula o lag
delay = lag.nsecs
print("delay ", "{:.3f}".format(delay/1.0E9))
if delay > atraso and check_delay==True:
print("Descartando por causa do delay do frame:", delay)
return
try:
antes = time.clock()
cv_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8")
#cv_image = cv2.flip(cv_image, -1)
media, centro, maior_area = cormodule.identifica_cor(cv_image)
if cap_lock == 0:
frame_capturado = cv_image
else:
cap_lock -= 1
depois = time.clock()
return
except CvBridgeError as e:
print('ex', e)
def CamView(imagem):
global cv_image
global media
global centro
global media_cor
global centro_cor
global area_cor
global viu_obj
global is_centered
global coef
global objeto
global x_obj
global dist_obj_centro_imagem_x
global contador_objeto
global detect_mode
global initBB
global box
# Lag detection
now = rospy.get_rostime()
imgtime = imagem.header.stamp
lag = now - imgtime
delay = lag.nsecs
if print_delay:
print("delay ", "{:.3f}".format(delay / 1.0E9))
if delay > atraso and check_delay:
print("Descartando por causa do delay do frame:", delay)
return None
try:
cv_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8")
media_cor, centro_cor, area_cor = cormodule.identifica_cor(cv_image)
if detect_mode:
centro, imagem, resultados = visao_module.processa(cv_image)
viu_obj = False
resultados = [i for i in resultados if i[0] == objeto]
# Procura pelo objeto
for r in resultados:
# if r[0] == objeto:
viu_obj = True
# Dimensoes do objeto
x0_obj, y0_obj = r[2]
x1_obj, y1_obj = r[3]
posicao_obj = (int(
(x0_obj + x1_obj) / 2.0), int((y0_obj + y1_obj) / 2.0))
# Dimensoes da imagem
x_obj, y_obj = posicao_obj
centro_imagem_x = res_x / 2.0
dist_obj_centro_imagem_x = x_obj - centro_imagem_x
if viu_obj:
contador_objeto += 1
if contador_objeto >= 5:
detect_mode = False
contador_objeto = 0
(x1, y1), (x2, y2) = resultados[0][2:]
initBB = (x1, y1, x2, y2)
tracker.init(cv_image, initBB)
print("\n\nTracking\n\n")
else:
contador_objeto = 0
if not detect_mode:
# TRACKER AQUI
if initBB is not None: # TODO
# grab the new bounding box coordinates of the object
(success, box_) = tracker.update(cv_image)
if success: # check to see if the tracking was a success
viu_obj = True
# print("B", box_)
box = [int(v) for v in box_]
(x, y, w, h) = box
cv2.rectangle(cv_image, (x, y), (x + w, y + h),
(255, 255, 0), 2)
else:
detect_mode = True
else:
detect_mode = True
cv2.imshow("Camera", cv_image)
except CvBridgeError as e:
print('ex', e)
def roda_todo_frame(imagem):
print("frame")
global cv_image
global media
global centro
global viu_objeto
global centro_mnet
global resultados
global count_frame
global initBB
global x1, x2, y1, y2
global tracker
now = rospy.get_rostime()
imgtime = imagem.header.stamp
lag = now-imgtime # calcula o lag
delay = lag.nsecs
#print("delay ", "{:.3f}".format(delay/1.0E9))
if delay > atraso and check_delay==True:
print("Descartando por causa do delay do frame:", delay)
return
try:
antes = time.clock()
cv_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8")
media, centro, area = cormodule.identifica_cor(cv_image)
centro_mnet, imagem, resultados = visao_module.processa(cv_image)
for r in resultados:
if r[0] == objeto:
viu_objeto = True
else:
viu_objeto = False
#######
#Código para funcionamento do tracking
if count_frame < 5:
if len(resultados) != 0:
if viu_objeto:
count_frame += 1
else:
count_frame = 0
else:
if len(resultados) !=0:
x1 = resultados[0][2][0]
y1 = resultados[0][2][1]
x2 = resultados[0][3][0]
y2 = resultados[0][3][1]
difx = x2 - x1
dify = y2 - y1
initBB = (x1, y1, abs(difx), abs(dify))
tracker.init(cv_image, initBB)
fps = FPS().start()
if initBB is not None:
# grab the new bounding box coordinates of the object
(success, box) = tracker.update(cv_image)
# check to see if the tracking was a success
if success:
(x, y, w, h) = [int(v) for v in box]
cv2.rectangle(cv_image, (x, y), (x + w, y + h),
(0, 255, 0), 2)
# update the FPS counter
fps.update()
fps.stop()
#######
depois = time.clock()
cv2.imshow("Camera", cv_image)
except CvBridgeError as e:
print('ex', e)
def processa_frame(self, frame):
self.media, self.centro, self.area = cormodule.identifica_cor(
frame, self.debug)
def roda_todo_frame(imagem): #função usada pelo subscriber recebe_imagem
global cv_image
global centro
global media_vermelho
global area_vermelho
global media_azul
global area_azul
global results
global tracking
global contador
global centro_tracking
global fps
global initBB
global area_tracking
global WIDTH
global HEIGTH
now = rospy.get_rostime()
imgtime = imagem.header.stamp
lag = now - imgtime
delay = lag.secs
if delay > atraso and check_delay == True:
return
try:
antes = time.clock()
cv_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8")
centro, media_vermelho, area_vermelho, media_azul, area_azul = cm.identifica_cor(
cv_image)
WIDTH, HEIGTH, g = cv_image.shape
if tracking == NADA:
results = mb.detect(cv_image, alvo)
else:
tracking_update()
if len(results) > 0:
contador += 1
else:
contador = 0
if contador > 3:
tracking = TRACKING
pi_x = results[0][2][0]
pi_y = results[0][2][1]
pf_x = results[0][3][0]
pf_y = results[0][3][1]
w = pf_x - pi_x
h = pf_y - pi_y
centro_x = pi_x + w / 2
centro_y = pi_y + h / 2
area_tracking = w * h
centro_tracking = (centro_x, centro_y)
initBB = (pi_x, pi_y, w, h)
tracker.init(cv_image, initBB)
fps = FPS().start()
results = []
tracking_update()
depois = time.clock()
cv2.imshow("Camera", cv_image)
except CvBridgeError as e:
print('ex', e)
def roda_todo_frame(imagem):
"""
Funcao com o objetivo de processar as imagens obtidas pela
camera do robo, na seguinte ordem:
1. Processa copia da imagem da camera, usando a funcao procesa_imagem_pista()
do arquivo processaImg.py. Atribui os valores retornados pela funcao
as variaveis global centro_pista, local antolho e local mask. Mostra a mask;
1a. global centro_pista: coordenadas do centro de massa da linha
amarela, que sao as coordenadas do centro da pista.
1b. local antolho: copia da imagem da camera do robo, com
retangulos desenhados por cima, que servem como antolhos,
para o robo. (Usado para fins de teste)
1c. local mask: mascara das linhas amarelas.
2. Processa copia da imagem da camera, usando a funcao identifica_cor()
do arquivo cormodule.py. Atribui os valores retornados pela funcao
as variaveis global media_creep, global maior_area e local frame;
2a. global media_creep: coordenadas do centro de massa do creeper,
que sao as coordenadas do centro do creeper.
2b. global maior_area: area do contorno do creeper mais proximo.
2c. local frame: copia da imagem da camera do robo, com o contorno
do creeper mais proximo desenhado por cima.
3. Processa copia da imagem da camera, usando a funcao aruco_read()
do arquivo processaImg.py(). Atribui os valores retornados pela funcao
as variaveis local aruco_imshow e global ids. Mostra a aruco_imshow;
3a. local aruco_imshow: copia da imagem da camera do robo, com
o contorno dos QRcodes desenhados por cima, e com os IDs dos
QRcodes sobrescritos na imagem.
3b. global ids: lista dos IDs dos QRcodes, que a funcao esta
detectando na imagem.
4. Calcula as coordenadas do centro da imagem obtida pela camera
do robo e as armazena na variavel global centro_robo;
5. Desenha uma crosshair, com as coordenadas do centro da pista,
sobre a imagem local frame. Mostra a imagem frame.
"""
global cv_image
global media
global centro_pista
global resultados
global centro_robo
global media_creep
global maior_area
global ids
try:
cv_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8")
img_copy = cv_image.copy()
img_copy_pista = cv_image.copy()
img_copy_aruco = cv_image.copy()
img_copy_cor = cv_image.copy()
centro_pista, antolho, mask = procImg.procesa_imagem_pista(
img_copy_pista)
# cv2.imshow("antolho", antolho)
cv2.imshow("mask", mask)
media_creep, maior_area, frame = cormodule.identifica_cor(
img_copy_cor, goal1[0])
aruco_imshow, ids = procImg.aruco_read(img_copy_aruco)
cv2.imshow('aruco', aruco_imshow)
centro_robo = (img_copy.shape[1] // 2, img_copy.shape[0] // 2)
if centro_pista is not None:
crosshair(frame, centro_pista, 4, (0, 0, 255))
# cv2.imshow("cv_image", cv_image)
cv2.imshow("cor", frame)
cv2.waitKey(1)
except CvBridgeError as e:
print('ex', e)