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)