diff --git a/IGGLU-VSSS/CMakeLists.txt b/IGGLU-VSSS/CMakeLists.txt index b8d27061..62e699f2 100644 --- a/IGGLU-VSSS/CMakeLists.txt +++ b/IGGLU-VSSS/CMakeLists.txt @@ -12,6 +12,8 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON) find_package(QT NAMES Qt6 Qt5 REQUIRED COMPONENTS Widgets) find_package(Qt${QT_VERSION_MAJOR} REQUIRED COMPONENTS Widgets) find_package(Qt6 REQUIRED COMPONENTS WebSockets) +find_package(msgpack REQUIRED) + set(PROJECT_SOURCES main.cpp @@ -47,6 +49,12 @@ else() endif() endif() +target_link_libraries(IGGLU-VSSS + PRIVATE Qt6::WebSockets + PRIVATE Qt6::Core + PRIVATE msgpackc +) + target_link_libraries(IGGLU-VSSS PRIVATE Qt${QT_VERSION_MAJOR}::Widgets) target_link_libraries(IGGLU-VSSS PUBLIC Qt${QT_VERSION_MAJOR}::WebSockets) diff --git a/IGGLU-VSSS/serverIgglu.cpp b/IGGLU-VSSS/serverIgglu.cpp index 4248ddcb..037dc739 100644 --- a/IGGLU-VSSS/serverIgglu.cpp +++ b/IGGLU-VSSS/serverIgglu.cpp @@ -3,6 +3,12 @@ #include #include #include +#include +#include +#include +#include +#include + using namespace std; QT_USE_NAMESPACE @@ -36,8 +42,7 @@ void ServerIgglu::closeWebSocket() { } void ServerIgglu::onConnected() { - - connect(&m_webSocket, &QWebSocket::textMessageReceived, + connect(&m_webSocket, &QWebSocket::binaryMessageReceived, this, &ServerIgglu::processMessage); } @@ -46,10 +51,73 @@ void ServerIgglu::closed() { qDebug() << "closed"; } -void ServerIgglu::processMessage(const QString &message) +void ServerIgglu::processMessage(const QByteArray &message) { - QTextStream(stdout) << message; + // Recebendo a mensagem binária empacotada do MessagePack + std::string msg_data(message.constData(), message.size()); + + try { + // Desempacotando a mensagem + msgpack::object_handle oh = msgpack::unpack(msg_data.data(), msg_data.size()); + msgpack::object obj = oh.get(); + + // Converta o objeto para um dicionário (std::map em C++) + std::map received_dict; + obj.convert(received_dict); + + // Iterando sobre os elementos do dicionário e imprimindo com qDebug() + for (const auto& pair : received_dict) { + QTextStream(stdout) << "Key: " << QString::fromStdString(pair.first) << "- Value: "; + + // Verifica o tipo do valor + if (pair.second.type == msgpack::type::ARRAY) { + // Se o valor é um array, iteramos sobre os elementos + std::vector array_values; + pair.second.convert(array_values); + for (const auto& element : array_values) { + if (element.type == msgpack::type::FLOAT) { + float float_value; + element.convert(float_value); + QTextStream(stdout) << float_value << ' '; // Imprime valor float + } else { + QTextStream(stdout) << "Unsupported array element type"; // Caso de tipo não suportado no array + } + } + QTextStream(stdout) << "\n"; + } else if (pair.second.type == msgpack::type::MAP) { + // Se o valor é um dicionário, convertemos para um map + std::map inner_dict; + pair.second.convert(inner_dict); + for (const auto& inner_pair : inner_dict) { + QTextStream(stdout) << "Inner Key: " << QString::fromStdString(inner_pair.first) << "- Inner Value: "; + + // Verifica o tipo do valor no dicionário interno + if (inner_pair.second.type == msgpack::type::ARRAY) { + std::vector inner_array_values; + inner_pair.second.convert(inner_array_values); + for (const auto& element : inner_array_values) { + if (element.type == msgpack::type::FLOAT) { + float float_value; + element.convert(float_value); + QTextStream(stdout) << float_value; // Imprime valor float + } else { + QTextStream(stdout) << "Unsupported inner array element type"; // Caso de tipo não suportado + } + } + } else { + QTextStream(stdout) << "Unsupported type in inner dictionary"; // Caso de tipo não suportado no dicionário + } + } + QTextStream(stdout) << "\n"; + } else { + QTextStream(stdout) << "Unsupported type"; // Caso de tipo não suportado + } + } + } catch (const std::exception& e) { + qDebug() << "Error unpacking message:" << e.what(); + } } + void ServerIgglu::sendMessage() { m_webSocket.sendTextMessage("message"); } diff --git a/IGGLU-VSSS/serverIgglu.h b/IGGLU-VSSS/serverIgglu.h index fb267e1b..7169e446 100644 --- a/IGGLU-VSSS/serverIgglu.h +++ b/IGGLU-VSSS/serverIgglu.h @@ -4,6 +4,7 @@ #include #include #include +#include class ServerIgglu : public QObject { @@ -19,7 +20,7 @@ class ServerIgglu : public QObject private slots: void onConnected(); void closed(); - void processMessage(const QString &message); + void processMessage(const QByteArray &message); void onDisconnected(){ qDebug() << "Disconnected"; } diff --git a/src/client/websocket.py b/src/client/websocket.py index 21ecfa3a..52bd40ab 100644 --- a/src/client/websocket.py +++ b/src/client/websocket.py @@ -1,47 +1,90 @@ -import asyncio -from websockets.asyncio.server import serve +import threading +import websockets.sync.server as sync_ws +import time +import websockets +import pickle +import numpy as np +import matplotlib.pyplot as plt +import msgpack -# sleep foi para simular o tempo que o Loop demora para ser executado -async def producer(): - await asyncio.sleep(3) - return "Hello from UnBrain!\n" +def producer(): + while True: + time.sleep(3) + message = "Hello !" -async def consumer(message): - print(f'Message from client: {message}\n') - await queue.put(message) +def consumer(message): + print(f'Hello {message}') class WebSocket: - def __init__(self, port=5001): - global queue - self.host="localhost" + def __init__(self, loop, port=5001): + self.loop = loop + self.host = "localhost" self.port = port - self.message = "" - self.queue = queue - - def run(self): - asyncio.run(main()) + self.server = None -async def consumer_handler(websocket): - async for message in websocket: - await consumer(message) - await websocket.send(f"UnBrain received: {message}\n") + def producer(self): + # Simula uma tarefa demorada de 3 segundos + # time.sleep(3) -async def producer_handler(websocket): - while True: - message = await producer() - await websocket.send(message) + # mensagem com infos dos robos e bola + message_robot = {f"ROBOT {i}": list(self.loop.world.team[i].pos)+list(self.loop.world.team[i].v) for i in self.loop.world.n_robots if i is not None} + message_ball = list(self.loop.world.ball.pos)+list(self.loop.world.ball.v) # lembrar de multiplicar a velocidade por 400 (n sei pq) + + # calculo pra enviar o UVF via mensagem + def message_uvf(robot_i=2, field_dims=(170*4, 130*4), arrow_spaces=16): + x = np.arange(-field_dims[0]/2, field_dims[0]/2, arrow_spaces) + y = np.arange(-field_dims[1]/2, field_dims[1]/2, arrow_spaces) + X, Y = np.meshgrid(x, y) + arrow_positions = np.array([X.flatten(), Y.flatten()]).T + + robot = self.loop.world.raw_team[robot_i] -async def hello(websocket): - print(f"New websocket connected\n") - await asyncio.gather( - consumer_handler(websocket), - producer_handler(websocket), - ) + positions = [] + for i in range(arrow_positions.shape[0]): + positions.append(arrow_positions[i]/400) + + angles = list(map(robot.field.F, positions)) + + return angles if robot.field is not None else 0 + + # self.message = f"{message_robot[0]}{message_robot[1]}{message_robot[2]}{message_ball};\n UVF:{message_uvf()}" # main.loop.world.team + self.message = {"TEAM": message_robot, + "BALL": message_ball} + + if self.loop.draw_uvf: + self.message["UVF"] = message_uvf() + + packed_message = msgpack.packb(self.message) + print(msgpack.unpackb(packed_message)["BALL"]) + + info = packed_message + print("produzindo") + return info + + def run(self): + server_thread = threading.Thread(target=self.start_server) + server_thread.start() -async def main(): - async with serve(hello, "localhost", 5001): - await asyncio.get_running_loop().create_future() + def start_server(self): + if self.server is None: + with sync_ws.serve(self.handle_connection, host=self.host, port=self.port) as server: + print(f"WebSocket server running at ws://{self.host}:{self.port}") + server.serve_forever() + + def handle_connection(self, websocket): + print(f"New websocket connected\n") + while True: + try: + message = websocket.recv() + print(f"Message from client: {message}\n") + websocket.send(f"UnBrain received: {message}\n") + response = self.producer() + websocket.send(response) + except websockets.ConnectionClosed: + print("Client disconnected\n") + break if __name__ == '__main__': - asyncio.run(main()) \ No newline at end of file + ws = WebSocket() + ws.run() diff --git a/src/loop.py b/src/loop.py index 58730163..3b75824e 100644 --- a/src/loop.py +++ b/src/loop.py @@ -5,6 +5,8 @@ from communication.serialWifi import SerialRadio from world import World +import threading + # Importa interface com FiraSim from client import VSS @@ -17,7 +19,7 @@ import signal from vision.receiver import FiraClient from client.client_pickle import ClientPickle -from client.websocket import WebSocket +# from client.websocket import WebSocket from strategy.automaticReplacer import AutomaticReplacer @@ -30,7 +32,7 @@ def __init__(self, loop_freq=90, draw_uvf=False, team_yellow=False, - immediate_start=False, + immediate_start=True, static_entities=False, referee=False, firasim=False, @@ -45,7 +47,9 @@ def __init__(self, ): - + self.loop_thread = None + self.ws_thread = None + self.threadScreen = None # Instancia interface com o simulador self.firasim = VSS(team_yellow=team_yellow) @@ -85,8 +89,10 @@ def __init__(self, # print(f"estado do campo:{self.simulado.get_state()}") # Instancia de sinal caso haja interrupções no processo (ctrl + C) - # signal.signal(signal.SIGINT, self.handle_SIGINT) - + try: + signal.signal(signal.SIGINT, self.handle_SIGINT) + except ValueError: + print("tentou chamar signal fora da thread principal") # Instancia interfaces com o referee self.rc = RefereeCommands() self.rp = RefereePlacement(team_yellow=team_yellow) @@ -112,8 +118,8 @@ def __init__(self, self.draw_uvf = draw_uvf if self.draw_uvf: self.UVF_screen = UVFScreen(self.world, index_uvf_robot=1) - self.UVF_screen.initialiazeScreen() - self.UVF_screen.initialiazeObjects() + # self.UVF_screen.initialiazeScreen() + # self.UVF_screen.initialiazeObjects() # Função do sinal de interrupção (faz com que pare o robô imediatamente, (0,0) ) def handle_SIGINT(self, signum, frame): @@ -184,10 +190,10 @@ def loop(self): if self.world.igglu: for robot in self.world.raw_team: - if robot is not None: robot.turnOff() + if robot is not None: robot.turnOn() # Desenha no ALP-GUI - self.draw() + # self.draw() def busyLoop(self): @@ -247,12 +253,20 @@ def draw(self): clientProvider().drawBall(0, self.world.ball.x, self.world.ball.y) - def run(self): + def websocket_thread(self): + from client.websocket import WebSocket + print("entrou no ws") + webapp = WebSocket(loop=self) + webapp.run() + + def run_loop(self): t0 = 0 + tempo_zero = time.time() logging.info("System is running") while self.running: + # Executa o loop de visão e referee até dar o tempo de executar o resto self.busyLoop() while time.time() - t0 < self.loopTime: @@ -265,7 +279,45 @@ def run(self): # Executa o loop self.loop() - if self.draw_uvf: - self.UVF_screen.updateScreen() + print(f"gfl{time.time()-tempo_zero:.2f}", end="\r", flush=True) + + logging.info("System stopped") - logging.info("System stopped") \ No newline at end of file + def run(self): + if self.ws_thread is None and self.loop_thread is None: + # inicializa threads + self.ws_thread = threading.Thread(target=self.websocket_thread) + self.loop_thread = threading.Thread(target=self.run_loop) + + self.ws_thread.start() + self.loop_thread.start() # inicia thread do loop + + robot_i=0 + field_dims=(170*4, 130*4) + arrow_spaces=16 + + x = np.arange(-field_dims[0]/2, field_dims[0]/2, arrow_spaces) + y = np.arange(-field_dims[1]/2, field_dims[1]/2, arrow_spaces) + X, Y = np.meshgrid(x, y) + arrow_positions = np.array([X.flatten(), Y.flatten()]).T + positions = [] + for i in range(arrow_positions.shape[0]): + positions.append(arrow_positions[i]/400) + + if self.draw_uvf: + plt.ion() + plt.show(block=False) + while self.running: + print(self.world.raw_team[0].field) + robot = self.world.raw_team[robot_i] + + angles = list(map(robot.field.F, positions)) + + plt.quiver(X, Y, np.cos(angles), np.sin(angles)) + plt.draw() + plt.pause(1) + plt.clf() + + # espera threads + self.ws_thread.join() + self.loop_thread.join() diff --git a/src/main.py b/src/main.py index bcc238ae..124c1847 100644 --- a/src/main.py +++ b/src/main.py @@ -66,7 +66,7 @@ # Instancia o programa principal loop = Loop( - draw_uvf=False, + draw_uvf=True, team_yellow=team_yellow, immediate_start=args.immediate_start, static_entities=args.static_entities,