⚡ Moves rest of events to event bus

esp32/include/communication/comm_base.hpp

@@ -6,8 +6,11 @@
 #include <functional>
 #include <list>
 #include <map>
+#include <memory>
+#include <vector>
 #include <type_traits>
 #include <communication/proto_helpers.h>
+#include <event_bus/event_bus.h>
 
 class CommAdapterBase {
   public:
@@ -35,6 +38,26 @@ class CommAdapterBase {
         decoder_.on<T>(handler);
     }
 
+    template <typename T>
+    void onPublish(std::function<void(const T&, int)> handler = nullptr) {
+        decoder_.on<T>([this, handler](const T& data, int clientId) {
+            EventBus::publish(data);
+            if (handler) handler(data, clientId);
+        });
+    }
+
+    template <typename T>
+    void bridgeFromEventBus() {
+        eventBusHandles_.push_back(
+            std::make_unique<EventBusHandleStorage<T>>(EventBus::subscribe<T>([this](const T& data) { emit(data); })));
+    }
+
+    template <typename T>
+    void bridgeFromEventBus(uint32_t intervalMs) {
+        eventBusHandles_.push_back(std::make_unique<EventBusHandleStorage<T>>(
+            EventBus::subscribe<T>(intervalMs, [this](const T& data) { emit(data); })));
+    }
+
     template <typename T>
     void emit(const T& data, int clientId = -1) {
         constexpr pb_size_t tag = MessageTraits<T>::tag;
@@ -47,8 +70,7 @@ class CommAdapterBase {
         size_t out_size;
         pb_get_encoded_size(&out_size, socket_message_Message_fields, &msg_);
         uint8_t* buffer = pb_heap_enc_buf;
-        if (out_size > sizeof(pb_heap_enc_buf)) { // If the encoded size exceeds our buffer size, we needs to malloc a
-                                                  // buffer of a proper size
+        if (out_size > sizeof(pb_heap_enc_buf)) {
             buffer = (uint8_t*)malloc(out_size);
         }
 
@@ -116,6 +138,18 @@ class CommAdapterBase {
     socket_message_Message msg_ = socket_message_Message_init_zero;
     uint8_t pb_heap_enc_buf[PROTO_BUFFER_SIZE];
 
+    struct EventBusHandleBase {
+        virtual ~EventBusHandleBase() = default;
+    };
+
+    template <typename T>
+    struct EventBusHandleStorage : EventBusHandleBase {
+        EventBus::Handle<T> handle;
+        EventBusHandleStorage(EventBus::Handle<T>&& h) : handle(std::move(h)) {}
+    };
+
+    std::vector<std::unique_ptr<EventBusHandleBase>> eventBusHandles_;
+
   private:
     void sendToSubscribers(int32_t tag, const uint8_t* data, size_t len) {
         xSemaphoreTake(mutex_, portMAX_DELAY);

esp32/include/event_bus/event_registry.h

@@ -39,3 +39,7 @@ REGISTER_SETTINGS_TYPE(api_CameraSettings, EventType::CAMERA_SETTINGS)
 REGISTER_EVENT_TYPE(socket_message_IMUData, EventType::IMU_DATA)
 REGISTER_EVENT_TYPE(socket_message_ControllerData, EventType::MOTION_COMMAND)
 REGISTER_EVENT_TYPE(socket_message_ModeData, EventType::MOTION_MODE)
+REGISTER_EVENT_TYPE(socket_message_AnglesData, EventType::MOTION_ANGLES)
+REGISTER_EVENT_TYPE(socket_message_WalkGaitData, EventType::MOTION_WALK_GAIT)
+REGISTER_EVENT_TYPE(socket_message_ServoStateData, EventType::SERVO_STATE)
+REGISTER_EVENT_TYPE(socket_message_ServoPWMData, EventType::SERVO_PWM)

esp32/include/event_bus/event_types.h

@@ -14,7 +14,10 @@ enum class EventType : uint16_t {
     IMU_DATA = 131,
     MOTION_COMMAND = 200,
     MOTION_MODE = 201,
+    MOTION_ANGLES = 202,
+    MOTION_WALK_GAIT = 203,
     SERVO_STATE = 141,
+    SERVO_PWM = 142,
 
     SYSTEM_BOOT = 300,
     STORAGE_HYDRATION_COMPLETE = 301,

esp32/include/motion.h

@@ -14,13 +14,18 @@
 #include <motion_states/stand_state.h>
 #include <motion_states/rest_state.h>
 #include <message_types.h>
+#include <event_bus/event_bus.h>
 
 enum class MOTION_STATE { DEACTIVATED, IDLE, CALIBRATION, REST, STAND, WALK };
 
 class MotionService {
   public:
+    using ModeChangeCallback = std::function<void(bool active)>;
+
     void begin();
 
+    void setModeChangeCallback(ModeChangeCallback callback) { modeChangeCallback_ = callback; }
+
     void handleAngles(const socket_message_AnglesData& data);
 
     void handleInput(const socket_message_ControllerData& data);
@@ -42,6 +47,8 @@ class MotionService {
     inline bool isActive() { return state != nullptr; }
 
   private:
+    void subscribeToEvents();
+
     Kinematics kinematics;
 
     CommandMsg command = {0, 0, 0, 0, 0, 0, 0};
@@ -62,6 +69,13 @@ class MotionService {
     float dir[12] = {1, -1, -1, -1, -1, -1, 1, -1, -1, -1, -1, -1};
 
     int64_t lastUpdate = esp_timer_get_time();
+
+    ModeChangeCallback modeChangeCallback_;
+
+    EventBus::Handle<socket_message_ControllerData> controllerHandle_;
+    EventBus::Handle<socket_message_ModeData> modeHandle_;
+    EventBus::Handle<socket_message_AnglesData> anglesHandle_;
+    EventBus::Handle<socket_message_WalkGaitData> walkGaitHandle_;
 };
 
 #endif

esp32/include/peripherals/servo_controller.h

@@ -5,6 +5,7 @@
 #include <event_bus/event_bus.h>
 #include <utils/math_utils.h>
 #include <platform_shared/api.pb.h>
+#include <platform_shared/message.pb.h>
 #include <esp_http_server.h>
 
 #ifndef FACTORY_SERVO_PWM_FREQUENCY
@@ -41,6 +42,12 @@ class ServoController {
         _settingsHandle = EventBus::subscribe<api_ServoSettings>(
             [this](const api_ServoSettings &settings) { onSettingsChanged(settings); });
 
+        _pwmHandle = EventBus::subscribe<socket_message_ServoPWMData>(
+            [this](const socket_message_ServoPWMData &data) { setServoPWM(data.servo_id, data.servo_pwm); });
+
+        _stateHandle = EventBus::subscribe<socket_message_ServoStateData>(
+            [this](const socket_message_ServoStateData &data) { data.active ? activate() : deactivate(); });
+
         api_ServoSettings initialSettings;
         if (EventBus::peek(initialSettings)) {
             onSettingsChanged(initialSettings);
@@ -125,6 +132,8 @@ class ServoController {
 
     api_ServoSettings _settings = ServoSettings_defaults();
     EventBus::Handle<api_ServoSettings> _settingsHandle;
+    EventBus::Handle<socket_message_ServoPWMData> _pwmHandle;
+    EventBus::Handle<socket_message_ServoStateData> _stateHandle;
 
     PCA9685Driver _pca;