def identifica_imagens(imagem):
print("frame")
global objeto
global cv_image
global controImg
global viu_Obj
global ObjPos
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")
controImg, imagem, resultados = visao_module.processa(cv_image)
for r in resultados:
if r[0] == objeto:
print("Ini", r[2], "Fin", r[3], r[0])
ObjPos = r[2][0] + ((r[3][0] - r[2][0]) / 2)
viu_Obj == 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 resultados
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()
temp_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8")
# Note que os resultados já são guardados automaticamente na variável
# chamada resultados
centro, saida_net, resultados = visao_module.processa(temp_image)
for r in resultados:
# print(r) - print feito para documentar e entender
# o resultado
pass
depois = time.clock()
# Desnecessário - Hough e MobileNet já abrem janelas
cv_image = saida_net.copy()
except CvBridgeError as e:
print('ex', e)
def roda_todo_frame(imagem):
"""
A função a seguir é chamada sempre que chega um novo frame
"""
global cv_image
global cat_seen
global cat_center
global image_center
global atraso
global seen_color
now = rospy.get_rostime()
imgtime = imagem.header.stamp
lag = now - imgtime # calcula o lag
delay = lag.nsecs
#print("-----------DELAY: ", delay)
if delay > atraso and check_delay == True:
print("Descartando por causa do delay do frame:", delay)
return
try:
antes = time.clock()
# Script para o MobileNet
cv_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8")
centro, resultados = visao_module.processa(cv_image)
image_center = centro
for r in resultados:
if r[0] == "bottle":
cat_seen = True
# Pegando o centro da imagem do objeto:
cX = (r[2][0] + r[3][0]) / 2
cY = (r[2][1] + r[3][1]) / 2
cat_center = (cX, cY)
if cat_seen == True:
cat_seen = True
else:
cat_seen = False
# Script para a detecção de cores
color_area = analiza_cor.identifica_cor(cv_image)
if color_area >= max_area_accepted:
seen_color = True
else:
seen_color = False
depois = time.clock()
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:
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 viu_bird
global viu_circulo
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")
centro, imagem, resultados = visao_module.processa(cv_image)
for r in resultados:
# print(r) - print feito para documentar e entender
# o resultado
print(resultados)
depois = time.clock()
# Desnecessário - Hough e MobileNet já abrem janelas
#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 viu_dog
global viu_gato
now = rospy.get_rostime()
imgtime = imagem.header.stamp
lag = now - imgtime
delay = lag.nsecs
if delay > atraso and check_delay == True:
return
antes = time.clock()
cv_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8")
centro, imagem, resultados = visao_module.processa(cv_image)
for r in resultados:
if r[0] == "dog":
viu_dog = True
print("cell phhone\n\n\n\n\n\n")
elif r[0] == "cat":
viu_gato = True
print("gato\n\n\n\n\n\n")
depois = time.clock()
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 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):
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 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(imagem):
global cv_image
global media
global centro
global resultados
global cm_coords
global cm_coords_creeper
global center_image
global cm
global cm_creeper
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()
temp_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8")
# Note que os resultados já são guardados automaticamente na variável
# chamada resultados
centro, saida_net, resultados = visao_module.processa(temp_image)
for r in resultados:
# print(r) - print feito para documentar e entender
# o resultado
pass
depois = time.clock()
# Desnecessário - Hough e MobileNet já abrem janelas
cv_image = saida_net.copy()
center_image = tracker.get_center(cv_image)
mask = cm.filter_color(cv_image)
mask = cv2.blur(mask, (5, 5))
cm_coords, mask_bgr = cm.center_of_mass_region(mask, 0, 160,
cv_image.shape[1],
cv_image.shape[0])
tracker.crosshair(mask_bgr, center_image, 10, (0, 255, 0))
mask_creeper = cm_creeper.filter_color(cv_image)
mask_creeper = cv2.blur(mask_creeper, (5, 5))
cm_coords_creeper, mask_bgr_creeper = cm_creeper.center_of_mass_region(
mask_creeper, 0, 0, cv_image.shape[1], cv_image.shape[0])
tracker.crosshair(mask_bgr_creeper, center_image, 10, (0, 255, 0))
aruco_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8")
aruco_tracker.detect_id(aruco_image, True)
cv2.imshow("cv_image", mask_bgr)
cv2.imshow("cv_image_creeper", mask_bgr_creeper)
cv2.waitKey(1)
except CvBridgeError as e:
print('ex', e)
def roda_todo_frame(imagem):
global cv_image
global media
global centro
global maior_area
global viu_linhas
global viu_linha1
global viu_linha2
global resultados
global achou_obj
global centro_detect
global x_objeto
global area_objeto
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 = visao_module.identifica_cor(cv_image)
# Note que os resultados já são guardados automaticamente na variável
# chamada resultados
centro_detect, imagem, resultados = visao_module.processa(cv_image)
vc_temp = visao_module.find_circles(cv_image)
if vc_temp == 0:
viu_linhas = True
elif vc_temp == 1:
viu_linha1 = True
elif vc_temp == 2:
viu_linha2 = True
for r in resultados:
if r[0] == visao_module.instrucoes["base"]:
achou_obj = True
x_objeto = (r[2][0] + r[3][0]) / 2
area_objeto = (r[3][0] - r[2][0]) * (r[3][1] - r[2][1])
#print(r)
# (CLASSES[idx], confidence*100, (startX, startY),(endX, endY)
pass
depois = time.clock()
except CvBridgeError as e:
print('ex', e)
def roda_todo_frame(imagem):
global cv_image
global media
global centro
global resultados
global cx
global bx
global maior_contorno_area
global cor
global fecha_creeper
global lista_detect
global img_detect
global lista_objects
global achou_objeto
global x_detect
now = rospy.get_rostime()
imgtime = imagem.header.stamp
lag = now - imgtime
delay = lag.nsecs
try:
antes = time.clock()
cv_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8")
temp_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8")
cx = point_fuga.image_callback(temp_image)
bx = point_fuga2.image_callback(temp_image)
centro, img, resultados = visao_module.processa(cv_image)
media, maior_contorno_area = visao_module.identifica_cor(
cv_image, goal[0])
img_detect, lista_detect = mobilenet_simples.detect(cv_image)
for objeto in lista_detect:
if objeto[0] == goal[2]:
x_detect = (objeto[2][0] + objeto[3][0]) / 2
achou_objeto = True
# print(lista_detect)
# fecha_creeper = tutorial.close_gripper()
depois = time.clock()
cv2.imshow("Camera", img_detect)
cv2.waitKey(1)
except CvBridgeError as e:
print('ex', e)
def roda_todo_frame(imagem):
global cv_image
global media
global centro
global maior_area
global resultados
global pink
global red
global green
global bx
global reconheceu
# global 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
try:
antes = time.clock()
cv_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8")
# Note que os resultados já são guardados automaticamente na variável
# chamada resultados
green = "green"
red = "red"
pink = "pink"
bicycle = "bicycle"
dog = "dog"
cat = "cat"
bird = "bird"
centro, imagem, resultados, reconheceu = visao_module.processa(
cv_image, bicycle)
maior_area, bx = visao_module.identifica_cor(cv_image, pink)
#qualquer, centro, maior_area, media = visao_module.identifica_cor(cv_image)
for r in resultados:
# print(r) - print feito para documentar e entender
# o resultado
pass
depois = time.clock()
# Desnecessário - Hough e MobileNet já abrem janelas
# cv2.imshow("Camera", cv_image)
#cv2.waitKey(2)
except CvBridgeError as e:
print('ex', e)
def roda_todo_frame(imagem):
# print("frame")
global cv_image
global media
global centro
global resultados
global circles
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, resultados = visao_module.processa(cv_image)
depois = time.clock()
gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
blur = cv2.GaussianBlur(gray, (5, 5), 0)
bordas = auto_canny(blur)
bordas_color = cv2.cvtColor(bordas, cv2.COLOR_GRAY2BGR)
circles = None
circles = cv2.HoughCircles(bordas,
cv2.HOUGH_GRADIENT,
2,
40,
param1=50,
param2=100,
minRadius=25,
maxRadius=70)
if circles is not None:
circles = np.uint16(np.around(circles))
for i in circles[0, :]:
# draw the outer circle
# cv2.circle(img, center, radius, color[, thickness[, lineType[, shift]]])
cv2.circle(cv_image, (i[0], i[1]), i[2], (0, 255, 0), 2)
# draw the center of the circle
cv2.circle(cv_image, (i[0], i[1]), 2, (0, 0, 255), 3)
cv2.imshow("Camera", cv_image)
except CvBridgeError as e:
print('ex', e)
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):
'''
Funçao é chamada toda vez que chega um novo frame
'''
print("frame")
global cv_image
global media
global centro
global resultados
global cY
global cX
global area
global centro_x_creeper
global centro_y_creeper
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")
# Note que os resultados já são guardados automaticamente na variável
# chamada resultados
centro, saida_net, resultados = visao_module.processa(temp_image)
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()
cX, cY = processa_imagem(temp_image)
centro_x_creeper, centro_y_creeper, area = encontra_creepers(
temp_image)
cv2.imshow("cv_image", cv_image)
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):
print("frame")
global cv_image
global media
global centro
global frame_final
global pf
global resultados
global area
global media2
global centro2
global area2
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")
pf, frame_pf = cormodule_mod.direction(cv_image, lines, pfs, times)
# Note que os resultados já são guardados automaticamente na variável
# chamada resultados
centro, imagem, resultados = visao_module.processa(cv_image)
media, centro, area = cormodule_mod.identifica_cor(cv_image, azul)
media2, centro2, area2 = cormodule_mod.identifica_cor(
cv_image, amarelo)
depois = time.clock()
# for r in resultados:
# print(r)
# pass
depois = time.clock()
frame_final = frame_pf
# Desnecessário - Hough e MobileNet já abrem janelas
#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 resultados
global ang
global dist_aruco
global ids
global maior_contorno_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:
# 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")
img_copy = temp_image.copy()
# Note que os resultados já são guardados automaticamente na variável
# chamada resultados
centro, saida_net, resultados = visao_module.processa(temp_image)
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()
media, _, maior_contorno_area = segmenta_cor.identifica_cor(
cv_image, ciano)
cv_image_copy = cv_image.copy()
img, ang = amarelo.fazTudo(cv_image_copy)
img_aruco, dist_aruco, ids = ler_aruco.roda_aruco(img_copy)
#print(ang)
cv2.imshow("cv_image", img_aruco)
cv2.waitKey(1)
except CvBridgeError as e:
print('ex', e)
def roda_todo_frame(imagem):
global cv_image
global mediaA
global centroA
global areaA
global media
global centro
global viu_dog
global posicao
global Pi
global Pf
global media_dog
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")
mediaA, centroA, areaA = cormodule.identifica_cor_azul(cv_image)
centro, imagem, resultados = visao_module.processa(cv_image)
for r in resultados:
if r[0] == "cat":
viu_dog = True
Pi = r[2]
Pf = r[3]
posicao = (Pi[0], Pi[1], Pf[0], Pf[1])
media_dog = (posicao[0] + ((posicao[2] - posicao[0])) / 2)
depois = time.clock()
except CvBridgeError as e:
print('ex', e)
def roda_todo_frame(imagem):
global cv_image
global cv_image_1
global media
global centro_frame
global centro_bola
global centro_creeper
global resultados
global estado
global GoBox
global found_box
global start_x
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()
temp_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8")
# CHAMADA PARA SEGUIR AS LINHAS AMARELAS
saida_follow, centro_frame, centro_bola = follow.image_callback(
temp_image)
# CHAMADA PARA IDENTIFICACAO DOS CREEPERS
saida_creeper, centro_creeper, estado = creeper.image_callback(
temp_image, COR)
centro, saida_net, resultados, start_x, found_box = visao_module.processa(
temp_image, NOME)
depois = time.clock()
cv_image = saida_follow.copy()
cv_image_1 = saida_net.copy()
except CvBridgeError as e:
print('ex', e)
def roda_todo_frame(imagem):
# print("frame")
global cv_image
global media
global centro
global resultados
try:
antes = time.clock()
temp_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8")
# Note que os resultados já são guardados automaticamente na variável
# chamada resultados
centro, saida_net, resultados = visao_module.processa(temp_image)
for r in resultados:
# print(r) - print feito para documentar e entender
# o resultado
pass
depois = time.clock()
# Desnecessário - Hough e MobileNet já abrem janelas
cv_image = saida_net.copy()
except CvBridgeError as e:
print('ex', e)
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
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, result_frame, result_tuples = visao_module.processa(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 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):
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 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 resultados
global centro
global temp_image
now = rospy.get_rostime()
t = rospy.Time(0)
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:
cv_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8")
temp_image = bridge.compressed_imgmsg_to_cv2(imagem, "bgr8")
centro, saida_net, resultados = visao_module.processa(temp_image)
for resultado in resultados:
x1, y1 = resultado[2]
x2, y2 = resultado[3]
for item in goal:
if resultado[0] == item and resultado[0] in goal:
goal.remove(resultado[0])
ROI = cv_image[y1:y2, x1:x2]
cv2.putText(ROI, 'achou {0}'.format(resultado[0]),
(20, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.5,
(188, 0, 149), 2, cv2.LINE_AA)
print(ROI)
if len(ROI) > 0:
cv2.imshow('ROI', ROI)
cv_image = saida_net.copy()
verde, verm, azul = identidica_esfera.acha_esfera(cv_image)
if cv_image is not None:
#print(cv_image)
identidica_esfera.acha_esfera(cv_image)
if verm == True and "red_sphere" in goal:
print("entrou")
cv2.putText(cv_image, 'achou esfera vermelha', (50, 50),
cv2.FONT_HERSHEY_SIMPLEX, 1, (188, 0, 149), 2,
cv2.LINE_AA)
goal.remove("red_sphere")
if azul == True and "blue_sphere" in goal:
print("entrou")
cv2.putText(cv_image, 'achou esfera azul', (50, 50),
cv2.FONT_HERSHEY_SIMPLEX, 1, (188, 0, 149), 2,
cv2.LINE_AA)
goal.remove("blue_sphere")
if verde == True and "green_sphere" in goal:
cv2.putText(cv_image, 'achou esfera verde', (50, 50),
cv2.FONT_HERSHEY_SIMPLEX, 1, (188, 0, 149), 2,
cv2.LINE_AA)
goal.remove("green_sphere")
checa_lista(velocidade_saida, goal, cv_image)
print(goal)
except CvBridgeError as e:
print('ex', e)
cv2.imshow("Video", cv_image)
cv2.waitKey(5)
def roda_todo_frame(self, imagem):
atraso = 1.5E9
now = rospy.get_rostime()
imgtime = imagem.header.stamp
lag = now - imgtime # calcula o lag
delay = lag.nsecs
if delay > atraso and check_delay == True:
# Esta lógica do delay só precisa ser usada com robo real e rede wifi e 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")
if self.contadorFrame % 3 == 0 or self.PEGACREEPER or self.OBJETIVONATELA or self.CREEPERNAMAO:
self.gray = cv2.cvtColor(temp_image, cv2.COLOR_BGR2GRAY)
self.corners, self.ids, self.rejectedImgPoints = aruco.detectMarkers(
self.gray, aruco_dict, parameters=parameters)
self.centro, self.saida_net, self.resultados = visao_module.processa(
temp_image)
#Exemplo de categoria de resultados (MobileNet)
# [('chair', 86.965459585189819, (90, 141), (177, 265))]
self.bgr = self.saida_net.copy()
self.contadorFrame = 0
else:
self.bgr = temp_image
self.ids = None
hsv = cv2.cvtColor(self.bgr, cv2.COLOR_BGR2HSV)
self.contadorFrame += 1
#Aqui é onde se conferem os estados do robô para aplicar as máscaras na pista que manterão ele seguindo a linha amarela:
mask_yellow = rl.segmenta_linha_amarela_bgr(temp_image)
mask_yellow = rl.morpho_limpa(mask_yellow)
if self.ESTADO == "DIREITA":
mask_yellow = rl.maskYellowBloqueiaEsquerda(mask_yellow)
if self.ESTADO == "ESQUERDA":
mask_yellow = rl.maskYellowBloqueiaDireita(mask_yellow)
if self.ESTADO == "DIREITAMAIOR":
mask_yellow = rl.maskYellowBloqueiaEsquerdaMaior(mask_yellow)
if self.COR == "orange":
maskLaranja, self.media, self.centro, maior_area = cM.identifica_cor_laranja(
temp_image)
if self.COR == "blue":
maskAzul, self.media, self.centro, maior_area = cM.identifica_cor_azul(
temp_image)
if self.COR == "green":
maskVerde, self.media, self.centro, maior_area = cM.identifica_cor_verde(
temp_image)
output, self.coef_angular, self.x_linha = rl.ajuste_linear_grafico_x_fy(
mask_yellow)
if self.ids is not None:
if self.id in self.ids:
index = list(self.ids).index(self.id)
ret = aruco.estimatePoseSingleMarkers(
self.corners[index], self.marker_size,
self.camera_matrix, self.camera_distortion)
else:
ret = aruco.estimatePoseSingleMarkers(
self.corners, self.marker_size, self.camera_matrix,
self.camera_distortion)
#-- ret = [rvec, tvec, ?]
#-- rvec = [[rvec_1], [rvec_2], ...] vetor de rotação
#-- tvec = [[tvec_1], [tvec_2], ...] vetor de translação
self.rvec, self.tvec = ret[0][0, 0, :], ret[1][0, 0, :]
#-- Desenha um retanculo e exibe Id do marker encontrado
aruco.drawDetectedMarkers(temp_image, self.corners, self.ids)
aruco.drawAxis(self.bgr, self.camera_matrix,
self.camera_distortion, self.rvec, self.tvec, 1)
#-- Print tvec vetor de tanslação em x y z
str_position = "Marker x=%4.0f y=%4.0f z=%4.0f" % (
self.tvec[0], self.tvec[1], self.tvec[2])
print(str_position)
cv2.putText(self.bgr, str_position, (0, 100), font, 1,
(0, 255, 0), 1, cv2.LINE_AA)
##############----- Referencia dos Eixos------###########################
# Linha referencia em X
cv2.line(
self.bgr,
(int(self.bgr.shape[1] / 2), int(self.bgr.shape[0] / 2)),
((int(self.bgr.shape[1] / 2) + 50),
(int(self.bgr.shape[0] / 2))), (0, 0, 255), 5)
# Linha referencia em Y
cv2.line(
self.bgr,
(int(self.bgr.shape[1] / 2), int(self.bgr.shape[0] / 2)),
(int(self.bgr.shape[1] / 2),
int((self.bgr.shape[0] / 2 + 50))), (0, 255, 0), 5)
#####################---- Distancia Euclidiana ----#####################
# Calcula a distancia usando apenas a matriz tvec, matriz de tanslação
# Pode usar qualquer uma das duas formas
distance = np.sqrt(self.tvec[0]**2 + self.tvec[1]**2 +
self.tvec[2]**2)
self.distancenp = np.linalg.norm(self.tvec)
#-- Print distance
str_dist = "Dist aruco=%4.0f dis.np=%4.0f" % (distance,
self.distancenp)
print(str_dist)
cv2.putText(self.bgr, str_dist, (0, 15), font, 1, (0, 255, 0),
1, cv2.LINE_AA)
#####################---- Distancia pelo foco ----#####################
#https://www.pyimagesearch.com/2015/01/19/find-distance-camera-objectmarker-using-python-opencv/
# raspicam v2 focal legth
FOCAL_LENGTH = 3.6 #3.04
# pixel por unidade de medida
m = (self.camera_matrix[0][0] / FOCAL_LENGTH +
self.camera_matrix[1][1] / FOCAL_LENGTH) / 2
# corners[0][0][0][0] = [ID][plano?][pos_corner(sentido horario)][0=valor_pos_x, 1=valor_pos_y]
pixel_length1 = math.sqrt(
math.pow(
self.corners[0][0][0][0] -
self.corners[0][0][1][0], 2) + math.pow(
self.corners[0][0][0][1] -
self.corners[0][0][1][1], 2))
pixel_length2 = math.sqrt(
math.pow(
self.corners[0][0][2][0] -
self.corners[0][0][3][0], 2) + math.pow(
self.corners[0][0][2][1] -
self.corners[0][0][3][1], 2))
pixlength = (pixel_length1 + pixel_length2) / 2
dist = self.marker_size * FOCAL_LENGTH / (pixlength / m)
#-- Print distancia focal
str_distfocal = "Dist focal=%4.0f" % (dist)
print(str_distfocal)
cv2.putText(self.bgr, str_distfocal, (0, 30), font, 1,
(0, 255, 0), 1, cv2.LINE_AA)
####################--------- desenha o cubo -----------#########################
# https://github.com/RaviJoshii/3DModeler/blob/eb7ca48fa06ca85fcf5c5ec9dc4b562ce9a22a76/opencv/program/detect.py
m = self.marker_size / 2
pts = np.float32([[-m, m, m], [-m, -m, m], [m, -m,
m], [m, m, m],
[-m, m, 0], [-m, -m, 0], [m, -m, 0],
[m, m, 0]])
imgpts, _ = cv2.projectPoints(pts, self.rvec, self.tvec,
self.camera_matrix,
self.camera_distortion)
imgpts = np.int32(imgpts).reshape(-1, 2)
bgr = cv2.drawContours(self.bgr, [imgpts[:4]], -1, (0, 0, 255),
4)
for i, j in zip(range(4), range(4, 8)):
bgr = cv2.line(bgr, tuple(imgpts[i]), tuple(imgpts[j]),
(0, 0, 255), 4)
bgr = cv2.drawContours(bgr, [imgpts[4:]], -1, (0, 0, 255), 4)
# Desnecessário - Hough e MobileNet já abrem janelas
cv2.imshow("bgr", self.bgr)
cv2.imshow("Output", output)
cv2.waitKey(1)
self.dicCamera = {
"coef_angular": self.coef_angular,
"x_linha": self.x_linha,
"ids": self.ids,
"media": self.media,
"centro": self.centro,
"distancianp": self.distancenp,
"resultados": self.resultados,
"rvec": self.rvec
}
self.jsonDic = json.dumps(self.dicCamera, cls=MyEncoder)
self.pubCamera.publish(self.jsonDic)
except CvBridgeError as e:
print('ex', e)
