agrobot_base/Python/street-follower/line-angle2.py

import cv2
import numpy as np
import math
import json
import time
import socket
import threading
import sys
from flask import Flask, Response, request

# Variável para mostrar as linhas (True = mostrar, False = não mostrar)
max_readings = int(sys.argv[1])
porta = sys.argv[2]
url = sys.argv[3]
arquivoSaida = sys.argv[4]
mostrar_linhas = sys.argv[5] == "1"
socket_porta = int(sys.argv[6])

app = Flask(__name__)

# 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 = arquivoSaida
json_data = None

# Função para salvar os dados em formato JSON
def salvar_json(angulo, distancia_esquerda, distancia_direita, timestamp, json_path):
    global json_data  # Indica que queremos usar a variável global json_data
    
    # 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)

def detect_lines(camera_index):
    # Inicializando a captura de vídeo
    cap = cv2.VideoCapture(camera_index)
    if not cap.isOpened():
        print("Erro: A câmera não pôde ser aberta.")
        return
    
    altura, largura = None, None

    try:
        while True:
            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())
                salvar_json(angulo, distancia_esquerda, distancia_direita, timestamp, json_path)

            # Codificar o frame processado para JPEG
            ret, buffer = cv2.imencode('.jpg', frame_processado)
            if not ret:
                continue

            frame_bytes = buffer.tobytes()
            yield (b'--frame\r\n'
                   b'Content-Type: image/jpeg\r\n\r\n' + frame_bytes + b'\r\n')

    finally:
        # Quando tudo estiver feito, liberar a captura
        cap.release()

# 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=socket_porta):
    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()


# Esta função é para iniciar o servidor de socket em uma thread separada
def run_socket_server():
    start_server()

# Iniciar o servidor Flask em uma thread separada
def run_flask_server():
    app.run(host='0.0.0.0', port=porta, threaded=True, debug=False)

@app.route('/' + url, methods=['GET'])
def video_feed():
    camera_index = int(request.args.get('camera_index'))
    return Response(detect_lines(camera_index), mimetype='multipart/x-mixed-replace; boundary=frame')


# Iniciar o servidor
if __name__ == '__main__':
    # Inicia o servidor de socket em uma thread separada
    socket_server_thread = threading.Thread(target=run_socket_server)
    socket_server_thread.start()

    # Inicia o servidor Flask na thread principal
    run_flask_server()