♻️ Handle incomming messages

2026-01-03 12:49:48 +01:00
parent c0c13754f4
commit fa332995f9
11 changed files with 603 additions and 632 deletions
@@ -1,82 +1,42 @@
 #include <communication/websocket_adapter.h>
-#include <string>

-static const char *TAG = "Websocket";
+static const char* TAG = "Websocket";

-Websocket::Websocket(PsychicHttpServer &server, const char *route) : _server(server), _route(route) {
-    _socket.onOpen((std::bind(&Websocket::onWSOpen, this, std::placeholders::_1)));
-    _socket.onClose(std::bind(&Websocket::onWSClose, this, std::placeholders::_1));
-    _socket.onFrame(std::bind(&Websocket::onFrame, this, std::placeholders::_1, std::placeholders::_2));
+Websocket::Websocket(PsychicHttpServer& server, const char* route) : server_(server), route_(route) {
+    socket_.onOpen(std::bind(&Websocket::onWSOpen, this, std::placeholders::_1));
+    socket_.onClose(std::bind(&Websocket::onWSClose, this, std::placeholders::_1));
+    socket_.onFrame(std::bind(&Websocket::onFrame, this, std::placeholders::_1, std::placeholders::_2));
 }

-void Websocket::begin() { _server.on(_route, &_socket); }
+void Websocket::begin() { server_.on(route_, &socket_); }

-void Websocket::onEvent(std::string event, EventCallback callback) {
-    //CommAdapterBase::onEvent(std::move(event), std::move(callback));
+void Websocket::onWSOpen(PsychicWebSocketClient* client) {
+    ESP_LOGI(TAG, "Client connected: %s [%u]", client->remoteIP().toString().c_str(), client->socket());
+    sendPong(client->socket());
 }

-void Websocket::emit(const char *event, JsonVariant &payload, const char *originId, bool onlyToSameOrigin) {
-    //CommAdapterBase::emit(event, payload, originId, onlyToSameOrigin);
+void Websocket::onWSClose(PsychicWebSocketClient* client) {
+    ESP_LOGI(TAG, "Client disconnected: %s [%u]", client->remoteIP().toString().c_str(), client->socket());
+    removeClient(client->socket());
 }

-
-void Websocket::onWSOpen(PsychicWebSocketClient *client) {
-    ESP_LOGI("EventSocket", "ws[%s][%u] connect", client->remoteIP().toString().c_str(), client->socket());
-    ping(client->socket());
-}
-
-void Websocket::onWSClose(PsychicWebSocketClient *client) {
-    xSemaphoreTake(mutex_, portMAX_DELAY);
-    for (auto &event_subscriptions : client_subscriptions) {
-        event_subscriptions.second.remove(client->socket());
+esp_err_t Websocket::onFrame(PsychicWebSocketRequest* request, httpd_ws_frame* frame) {
+    if (frame->type != HTTPD_WS_TYPE_BINARY) {
+        ESP_LOGW(TAG, "Expected binary frame, got type %d", frame->type);
+        return ESP_OK;
    }
-    xSemaphoreGive(mutex_);
-    ESP_LOGI("EventSocket", "ws[%s][%u] disconnect", client->remoteIP().toString().c_str(), client->socket());
-}
-
-esp_err_t Websocket::onFrame(PsychicWebSocketRequest *request, httpd_ws_frame *frame) {
-//     ESP_LOGV(TAG, "ws[%s][%u] opcode[%d]", request->client()->remoteIP().toString().c_str(),
-//              request->client()->socket(), frame->type);
-
-//     if (frame->type != HTTPD_WS_TYPE_TEXT && frame->type != HTTPD_WS_TYPE_BINARY) {
-//         ESP_LOGE(TAG, "Unsupported frame type: %d", frame->type);
-//         return ESP_OK;
-//     }
-
-// #if USE_MSGPACK
-//     if (frame->type == HTTPD_WS_TYPE_BINARY) {
-//         handleIncoming(frame->payload, frame->len, request->client()->socket());
-//     } else {
-//         ESP_LOGE(TAG, "Expected binary, got text");
-//     }
-// #else
-//     if (frame->type == HTTPD_WS_TYPE_TEXT) {
-//         handleIncoming(frame->payload, frame->len, request->client()->socket());
-//     } else {
-//         ESP_LOGE(TAG, "Expected text, got binary");
-//     }
-// #endif

+    handleIncoming(frame->payload, frame->len, request->client()->socket());
    return ESP_OK;
 }

-void Websocket::send(const uint8_t *data, size_t len, int cid) {
-    if (cid != -1) {
-        auto *client = _socket.getClient(cid);
+void Websocket::send(const uint8_t* data, size_t len, int cid) {
+    if (cid >= 0) {
+        auto* client = socket_.getClient(cid);
        if (client) {
-            ESP_LOGV(TAG, "Sending to client %s: %s", client->remoteIP().toString().c_str(), data);
-#if USE_MSGPACK
            client->sendMessage(HTTPD_WS_TYPE_BINARY, data, len);
-#else
-            client->sendMessage(HTTPD_WS_TYPE_TEXT, data, len);
-#endif
        }
    } else {
-        ESP_LOGV(TAG, "Sending to all clients: %s", data);
-#if USE_MSGPACK
-        _socket.sendAll(HTTPD_WS_TYPE_BINARY, data, len);
-#else
-        _socket.sendAll(HTTPD_WS_TYPE_TEXT, data, len);
-#endif
+        socket_.sendAll(HTTPD_WS_TYPE_BINARY, data, len);
    }
 }
@@ -136,52 +136,41 @@ void setupServer() {
    DefaultHeaders::Instance().addHeader("Access-Control-Max-Age", "86400");
 }

-#define ANGLES_EVENT "angles"
-#define INPUT_EVENT "input"
-#define MODE_EVENT "mode"
-#define WALK_GAIT_EVENT "walk_gait"
-#define EVENT_I2C_SCAN "i2cScan"
-#define EVENT_SERVO_CONFIGURATION_SETTINGS "servoPWM"
-#define EVENT_SERVO_STATE "servoState"
-#define EVENT_IMU_CALIBRATE "imuCalibrate"
-
 void setupEventSocket() {
-    // Motion events
-    // socket.onEvent(INPUT_EVENT, [&](JsonVariant &root, int originId) { motionService.handleInput(root, originId); });
+    socket.on<socket_message_HumanInputData>(
+        [&](const socket_message_HumanInputData &data, int clientId) { motionService.handleInput(data); });

-    // socket.onEvent(MODE_EVENT, [&](JsonVariant &root, int originId) {
-    //     servoController.setMode(SERVO_CONTROL_STATE::ANGLE);
-    //     motionService.handleMode(root, originId);
-    //     motionService.isActive() ? servoController.activate() : servoController.deactivate();
-    // });
+    socket.on<socket_message_ModeData>([&](const socket_message_ModeData &data, int clientId) {
+        servoController.setMode(SERVO_CONTROL_STATE::ANGLE);
+        motionService.handleMode(data);
+        motionService.isActive() ? servoController.activate() : servoController.deactivate();
+    });

-    // socket.onEvent(WALK_GAIT_EVENT,
-    //                [&](JsonVariant &root, int originId) { motionService.handleWalkGait(root, originId); });
+    socket.on<socket_message_WalkGaitData>(
+        [&](const socket_message_WalkGaitData &data, int clientId) { motionService.handleWalkGait(data); });

-    // socket.onEvent(ANGLES_EVENT, [&](JsonVariant &root, int originId) { motionService.anglesEvent(root, originId);
-    // });
+    socket.on<socket_message_AnglesData>(
+        [&](const socket_message_AnglesData &data, int clientId) { motionService.handleAngles(data); });

-    // // Peripherals events
-    // socket.onEvent(EVENT_I2C_SCAN, [&](JsonVariant &root, int originId) {
-    //     peripherals.scanI2C();
-    //     JsonDocument doc;
-    //     JsonVariant results = doc.to<JsonVariant>();
-    //     peripherals.getI2CResult(results);
-    //     socket.emit(EVENT_I2C_SCAN, results);
-    // });
+    socket.on<socket_message_I2CScanDataRequest>([&](const socket_message_I2CScanDataRequest &data, int clientId) {
+        peripherals.scanI2C();
+        socket_message_I2CScanData result = socket_message_I2CScanData_init_zero;
+        peripherals.getI2CResultProto(result);
+        socket.emit(result, clientId);
+    });

-    // socket.onEvent(EVENT_IMU_CALIBRATE, [&](JsonVariant &root, int originId) {
-    //     JsonDocument doc;
-    //     JsonVariant results = doc.to<JsonVariant>();
-    //     results["success"] = peripherals.calibrateIMU();
-    //     socket.emit(EVENT_IMU_CALIBRATE, results);
-    // });
+    socket.on<socket_message_IMUCalibrateExecute>([&](const socket_message_IMUCalibrateExecute &data, int clientId) {
+        socket_message_IMUCalibrateData result = {.success = peripherals.calibrateIMU()};
+        socket.emit(result, clientId);
+    });

-    // // Servo controller events
-    // socket.onEvent(EVENT_SERVO_CONFIGURATION_SETTINGS,
-    //                [&](JsonVariant &root, int originId) { servoController.servoEvent(root, originId); });
-    // socket.onEvent(EVENT_SERVO_STATE,
-    //                [&](JsonVariant &root, int originId) { servoController.stateUpdate(root, originId); });
+    socket.on<socket_message_ServoPWMData>([&](const socket_message_ServoPWMData &data, int clientId) {
+        servoController.setServoPWM(data.servo_id, data.servo_pwm);
+    });
+
+    socket.on<socket_message_ServoStateData>([&](const socket_message_ServoStateData &data, int clientId) {
+        data.active ? servoController.activate() : servoController.deactivate();
+    });
 }

 void IRAM_ATTR SpotControlLoopEntry(void *) {
@@ -2,14 +2,13 @@

 void MotionService::begin() { body_state.updateFeet(KinConfig::default_feet_positions); }

-void MotionService::anglesEvent(JsonVariant &root, int originId) {
-    JsonArray array = root.as<JsonArray>();
-    for (int i = 0; i < 12; i++) {
-        angles[i] = array[i];
+void MotionService::handleAngles(const socket_message_AnglesData& data) {
+    for (int i = 0; i < 12 && i < data.angles_count; i++) {
+        angles[i] = data.angles[i];
    }
 }

-void MotionService::setState(MotionState *newState) {
+void MotionService::setState(MotionState* newState) {
    if (state) {
        state->end();
    }
@@ -19,23 +18,21 @@ void MotionService::setState(MotionState *newState) {
    }
 }

-void MotionService::handleInput(JsonVariant &root, int originId) {
-    command.fromJson(root);
+void MotionService::handleInput(const socket_message_HumanInputData& data) {
+    command.fromProto(data);
    if (state) state->handleCommand(command);
 }

-void MotionService::handleWalkGait(JsonVariant &root, int originId) {
-    ESP_LOGI("MotionService", "Walk Gait %d", root.as<int>());
-
-    WALK_GAIT walkGait = static_cast<WALK_GAIT>(root.as<int>());
-    if (walkGait == WALK_GAIT::TROT)
+void MotionService::handleWalkGait(const socket_message_WalkGaitData& data) {
+    ESP_LOGI("MotionService", "Walk Gait %d", static_cast<int>(data.gait));
+    if (data.gait == socket_message_WalkGaits_TROT)
        walkState.set_mode_trot();
    else
        walkState.set_mode_crawl();
 }

-void MotionService::handleMode(JsonVariant &root, int originId) {
-    MOTION_STATE mode = static_cast<MOTION_STATE>(root.as<int>());
+void MotionService::handleMode(const socket_message_ModeData& data) {
+    MOTION_STATE mode = static_cast<MOTION_STATE>(data.mode);
    ESP_LOGV("MotionService", "Mode %d", static_cast<int>(mode));
    switch (mode) {
        case MOTION_STATE::REST: setState(&restState); break;
@@ -60,7 +57,7 @@ void MotionService::handleGestures(const gesture_t ges) {
    }
 }

-bool MotionService::update(Peripherals *peripherals) {
+bool MotionService::update(Peripherals* peripherals) {
    handleGestures(peripherals->takeGesture());
    if (!state) return false;
    int64_t now = esp_timer_get_time();