agrobot_base/Python/street-follower/angulo-rua6.py

import cv2
import numpy as np
import math
import json
import time
import socket
import threading

# Variável para mostrar as linhas (True = mostrar, False = não mostrar)
mostrar_linhas = True

# Função para calcular o ângulo entre duas linhas
def calcular_angulo(linha1, linha2):
    # Extraia os elementos individuais dos arrays NumPy
    angulo1 = math.atan2(float(linha1[1]) - float(linha1[3]), float(linha1[0]) - float(linha1[2]))
    angulo2 = math.atan2(float(linha2[1]) - float(linha2[3]), float(linha2[0]) - float(linha2[2]))
    angulo = math.degrees(angulo1 - angulo2)
    angulo = (angulo + 360) % 360
    if angulo > 180:
        angulo -= 360
    return angulo

# Função para desenhar as linhas na imagem
def desenhar_linhas(frame, altura, largura, esquerda_x, direita_x, mostrar_linhas):
    if mostrar_linhas:
        meio = largura // 2
        cv2.line(frame, (meio, 0), (meio, altura), (255, 0, 0), 2)
        cv2.line(frame, (esquerda_x, 0), (direita_x, altura), (0, 255, 0), 2)
        cv2.line(frame, (esquerda_x, 0), (esquerda_x, altura), (0, 255, 255), 2)
        cv2.line(frame, (direita_x, 0), (direita_x, altura), (0, 255, 255), 2)

# Função para processar cada frame do vídeo e calcular as distâncias
def processar_frame(frame, largura, altura):
    # Convertendo para escala de cinza
    cinza = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

    # Limiarização para criar uma imagem binária
    _, binarizada = cv2.threshold(cinza, 128, 255, cv2.THRESH_BINARY_INV)

    # Encontrar os contornos
    contornos, _ = cv2.findContours(binarizada, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

    # Se houver contornos, assumir que o maior é a linha preta
    if contornos:
        maior_contorno = max(contornos, key=cv2.contourArea)

        # Ajustar uma linha ao maior contorno (linha preta)
        [vx, vy, x0, y0] = cv2.fitLine(maior_contorno, cv2.DIST_L2, 0, 0.01, 0.01)

        # Verificar se vy é zero para evitar divisão por zero
        if vy == 0:
            esquerda_x = x0
            direita_x = x0
        else:
            # Determinar os pontos extremos da linha preta
            esquerda_x = int(x0 + (-y0 * vx / vy))
            direita_x = int(x0 + ((altura - y0) * vx / vy))

        # Calcular distâncias das bordas
        distancia_esquerda = max(0, esquerda_x)
        distancia_direita = max(0, largura - direita_x)

        # Calcular o ângulo entre a linha central e a linha ajustada
        linha_central = [largura // 2, 0, largura // 2, altura]
        linha_ajustada = [esquerda_x, 0, direita_x, altura]
        angulo = calcular_angulo(linha_central, linha_ajustada)

        # Desenhar linhas, se necessário
        desenhar_linhas(frame, altura, largura, esquerda_x, direita_x, mostrar_linhas)

        return (angulo, distancia_esquerda, distancia_direita, frame)

    return (None, None, None, frame)

# Caminho do arquivo JSON para salvar os dados
json_path = 'leituras.json'
json_data = None

# Função para salvar os dados em formato JSON
def salvar_json(angulo, distancia_esquerda, distancia_direita, timestamp, json_path):
    # Assegure-se que todos os dados são tipos primitivos antes de salvar em JSON
    json_data = {
        'timestamp': timestamp,
        'angulo': angulo,
        'distancia_esquerda': distancia_esquerda,
        'distancia_direita': distancia_direita
    }

    try:
        with open(json_path, 'r') as file:
            leituras = json.load(file)
    except FileNotFoundError:
        leituras = []

    leituras.append(json_data)
    leituras = leituras[-10:]

    with open(json_path, 'w') as file:
        json.dump(leituras, file, indent=4)

# Inicializando a captura de vídeo
cap = cv2.VideoCapture(1)  # Substitua 0 pelo índice correto da sua câmera

altura, largura = None, None

try:
    while True:
        # Capturando frame a frame
        ret, frame = cap.read()
        if not ret:
            print("Falha ao capturar imagem da câmera")
            break

        if altura is None or largura is None:
            altura, largura = frame.shape[:2]

        # Processar o frame atual
        angulo, distancia_esquerda, distancia_direita, frame_processado = processar_frame(frame, largura, altura)

        if angulo is not None and distancia_esquerda is not None and distancia_direita is not None:
            timestamp = int(time.time())
            print(f"Timestamp: {timestamp}, Ângulo: {angulo:.2f}, Distância Esquerda: {distancia_esquerda}, Distância Direita: {distancia_direita}")
            salvar_json(angulo, distancia_esquerda, distancia_direita, timestamp, json_path)

        # Exibir o frame processado
        cv2.imshow('Frame Processado', frame_processado)

        # Pressione 'q' para sair do loop
        if cv2.waitKey(1) & 0xFF == ord('q'):
            break
finally:
    # Quando tudo estiver feito, liberar a captura
    cap.release()
    cv2.destroyAllWindows()


# Função para lidar com a conexão de cada cliente
def handle_client_connection(client_socket):
    try:
        while True:
            # Enviar o dicionário serializado em JSON
            client_socket.send(json.dumps(json_data).encode('utf-8'))

            # Aguardar um pouco antes de enviar os próximos dados
            time.sleep(0.1)
    except socket.error:
        print(f"Cliente desconectado.")
    finally:
        # Fechar a conexão do socket ao sair do loop
        client_socket.close()

# Configuração inicial do servidor de socket
def start_server(address='localhost', port=65432):
    server = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    server.bind((address, port))
    server.listen()
    print(f"Servidor iniciado. Aguardando conexões em {address}:{port}...")
    
    try:
        while True:
            client_sock, address = server.accept()
            print(f"Aceitando conexão de {address[0]}:{address[1]}")
            client_handler = threading.Thread(
                target=handle_client_connection,
                args=(client_sock,)
            )
            client_handler.start()
    finally:
        server.close()

# Iniciar o servidor
if __name__ == '__main__':
    start_server()