Prepare files for public release

src/Hub.cpp

@@ -0,0 +1,24 @@
+#include "Hub.h"
+#include "flatbuffers/SensorMessageBuilder.h"
+
+double Hub::get_position() {
+    // no-op
+    return 0;
+}
+
+void Hub::actuate(double /* position */) {
+    // no-op
+}
+
+void Hub::actuate(double /* x */, double /* y */) {
+    // no-op
+}
+
+std::vector<uint8_t> Hub::get_actuation_message() {
+    // no-op
+    return {};
+}
+
+void Hub::update_sensor_data(const Flatbuffers::sensor_value & /* value */) {
+    // no-op
+}

src/Module.cpp

@@ -0,0 +1,43 @@
+
+#include <chrono>
+#include <ranges>
+
+#include "Module.h"
+
+std::vector<neighbour> Module::get_neighbours() {
+    return m_neighbours;
+}
+
+uint8_t Module::get_device_id() {
+    return m_device_id;
+}
+
+ModuleType Module::get_type() {
+    return m_module_type;
+}
+
+Messaging::ConnectionType Module::get_connection_type() {
+    return m_connection_type;
+}
+
+uint8_t Module::get_leader() {
+    return m_leader;
+}
+
+std::chrono::time_point<std::chrono::system_clock> Module::get_last_updated_time() {
+    return m_last_updated;
+}
+
+void Module::update_module_metadata(const Messaging::TopologyMessage &message) {
+    m_module_type = message.module_type();
+    m_connection_type = message.connection();
+    m_leader = message.leader();
+
+    m_last_updated = std::chrono::system_clock::now();
+
+    m_neighbours.clear();
+    for (auto [id, ori] :
+         std::views::zip(*message.channel_to_module(), *message.channel_to_orientation())) {
+        m_neighbours.emplace_back(neighbour{id, static_cast<Orientation>(ori)});
+    }
+}

src/ModuleFactory.cpp

@@ -0,0 +1,28 @@
+
+#include "ModuleFactory.h"
+#include "Hub.h"
+#include "actuators/BoundedPositionalActuator1D.h"
+#include "actuators/PositionalActuator1D.h"
+#include "flatbuffers_generated/RobotModule_generated.h"
+
+#define SERVO1_MAX_ANGLE 180
+#define SERVO1_MIN_ANGLE 0
+#define SERVO1_DEFAULT_ANGLE 90
+
+std::shared_ptr<Module>
+ModuleFactory::createModule(uint8_t device_id, ModuleType type,
+                            std::shared_ptr<MessagingInterface> &messaging_interface) {
+    switch (type) {
+    case ModuleType_SPLITTER:
+        return std::make_shared<Hub>(device_id, type);
+    case ModuleType_BATTERY:
+        return std::make_shared<Hub>(device_id, type);
+    case ModuleType_SERVO_1:
+    case ModuleType_SERVO_2:
+        return std::make_shared<BoundedPositionalActuator1D>(
+            device_id, type, SERVO1_MAX_ANGLE, SERVO1_MIN_ANGLE, SERVO1_DEFAULT_ANGLE);
+    case ModuleType_DC_MOTOR:
+        return std::make_shared<PositionalActuator1D>(device_id, type);
+    }
+    return nullptr;
+}

src/actuators/BoundedPositionalActuator1D.cpp

@@ -0,0 +1,43 @@
+
+#include <cstring>
+#include <variant>
+
+#include "actuators/BoundedPositionalActuator1D.h"
+#include "flatbuffers/SensorMessageBuilder.h"
+#include "util/Variant.h"
+
+double BoundedPositionalActuator1D::get_position() {
+    return m_current_position;
+}
+
+void BoundedPositionalActuator1D::actuate(double position) {
+    if (position < m_min_value || position > m_max_value) {
+        return;
+    }
+
+    m_target_position = position;
+}
+
+void BoundedPositionalActuator1D::actuate(double x, double y) {
+}
+
+std::vector<uint8_t> BoundedPositionalActuator1D::get_actuation_message() {
+    std::vector<uint8_t> message{};
+
+    if (m_target_position == m_current_position) {
+        return message;
+    }
+
+    auto [data, size] = acm_builder->build_angle_control_message(m_target_position);
+    message.resize(size);
+    memcpy(message.data(), data, size);
+    return message;
+}
+
+void BoundedPositionalActuator1D::update_sensor_data(const Flatbuffers::sensor_value &value) {
+    std::visit(overloaded{
+                   [this](Flatbuffers::target_angle a) { m_current_position = a.angle; },
+                   [this](Flatbuffers::current_angle a) { m_current_position = a.angle; },
+               },
+               value);
+}

src/actuators/PositionalActuator1D.cpp

@@ -0,0 +1,36 @@
+#include "actuators/PositionalActuator1D.h"
+#include "flatbuffers/SensorMessageBuilder.h"
+#include "util/Variant.h"
+#include <cstring>
+
+double PositionalActuator1D::get_position() {
+    return m_current_position;
+}
+
+void PositionalActuator1D::actuate(double position) {
+    m_target_position = position;
+}
+
+void PositionalActuator1D::actuate(double /* x */, double /* y */) {
+}
+
+std::vector<uint8_t> PositionalActuator1D::get_actuation_message() {
+    std::vector<uint8_t> message{};
+
+    if (m_target_position == m_board_target_position) {
+        return message;
+    }
+
+    auto [data, size] = m_acm_builder->build_angle_control_message(m_target_position);
+    message.resize(size);
+    memcpy(message.data(), data, size);
+    return message;
+}
+
+void PositionalActuator1D::update_sensor_data(const Flatbuffers::sensor_value &value) {
+    std::visit(overloaded{
+                   [this](Flatbuffers::target_angle a) { m_board_target_position = a.angle; },
+                   [this](Flatbuffers::current_angle a) { m_current_position = a.angle; },
+               },
+               value);
+}

src/flatbuffers/AngleControlMessageBuilder.cpp

@@ -0,0 +1,22 @@
+//
+// Created by Johnathon Slightham on 2025-06-30.
+//
+
+#include "flatbuffers/AngleControlMessageBuilder.h"
+
+namespace Flatbuffers {
+const Messaging::AngleControlMessage *
+AngleControlMessageBuilder::parse_angle_control_message(const uint8_t *buffer) {
+    return flatbuffers::GetRoot<Messaging::AngleControlMessage>(buffer);
+}
+
+SerializedMessage AngleControlMessageBuilder::build_angle_control_message(const int16_t angle) {
+    builder_.Clear();
+
+    const auto message = Messaging::CreateAngleControlMessage(builder_, angle);
+
+    builder_.Finish(message);
+
+    return {builder_.GetBufferPointer(), builder_.GetSize()};
+}
+} // namespace Flatbuffers

src/flatbuffers/RobotConfigurationBuilder.cpp

@@ -0,0 +1,49 @@
+//
+// Created by Johnathon Slightham on 2025-07-25.
+//
+
+#include "flatbuffers/RobotConfigurationBuilder.h"
+
+#include "flatbuffers/SerializedMessage.h"
+#include "flatbuffers_generated/RobotConfiguration_generated.h"
+
+namespace Flatbuffers {
+
+SerializedMessage RobotConfigurationBuilder::build_robot_configuration(
+    const std::vector<ModuleInstance> &modules,
+    const std::vector<ModuleConnectionInstance> &connections) {
+    builder_.Clear();
+
+    std::vector<flatbuffers::Offset<RobotModule>> module_vector;
+    std::vector<flatbuffers::Offset<Frontend::ModuleConnection>> connection_vector;
+
+    for (const auto &connection : connections) {
+        connection_vector.push_back(Frontend::CreateModuleConnection(
+            builder_, connection.from_module_id, connection.to_module_id, connection.from_socket,
+            connection.to_socket, connection.orientation));
+    }
+
+    for (const auto &module : modules) {
+        // todo: this only works for motors right now
+        auto motor_state = CreateMotorState(builder_, module.angle);
+        const flatbuffers::Offset<void> config_union = motor_state.Union();
+        module_vector.push_back(CreateRobotModule(builder_, module.id, module.type,
+                                                  ModuleState_MotorState, config_union));
+    }
+
+    const auto connection_vector_fb = builder_.CreateVector(connection_vector);
+    const auto module_vector_fb = builder_.CreateVector(module_vector);
+
+    const auto message =
+        Frontend::CreateRobotConfiguration(builder_, module_vector_fb, connection_vector_fb);
+
+    builder_.Finish(message);
+
+    return {builder_.GetBufferPointer(), builder_.GetSize()};
+}
+
+const Frontend::RobotConfiguration *
+RobotConfigurationBuilder::parse_robot_configuration(const std::uint8_t *buffer) {
+    return flatbuffers::GetRoot<Frontend::RobotConfiguration>(buffer);
+}
+} // namespace Flatbuffers

src/flatbuffers/SensorMessageBuilder.cpp

@@ -0,0 +1,10 @@
+#include "flatbuffers/SensorMessageBuilder.h"
+#include "flatbuffers_generated/SensorMessage_generated.h"
+
+namespace Flatbuffers {
+
+const Messaging::SensorMessage *SensorMessageBuilder::parse_sensor_message(const uint8_t *buffer) {
+    return flatbuffers::GetRoot<Messaging::SensorMessage>(buffer);
+}
+
+} // namespace Flatbuffers

src/flatbuffers/TopologyMessageBuilder.cpp

@@ -0,0 +1,38 @@
+//
+// Created by Johnathon Slightham on 2025-06-30.
+//
+
+#include "flatbuffers/TopologyMessageBuilder.h"
+
+#include "flatbuffers/SerializedMessage.h"
+
+namespace Flatbuffers {
+SerializedMessage
+TopologyMessageBuilder::build_topology_message(const uint8_t module_id,
+                                               const ModuleType module_type,
+                                               const std::vector<uint8_t> &channel_to_module,
+                                               const std::vector<int8_t> &orientation_to_module) {
+    builder_.Clear();
+
+    const auto orientation_to_module_vector = builder_.CreateVector(orientation_to_module);
+    const auto channel_to_module_vector = builder_.CreateVector(channel_to_module);
+
+    const auto message =
+        Messaging::CreateTopologyMessage(builder_, module_id, module_type, channel_to_module.size(),
+                                         channel_to_module_vector, orientation_to_module_vector);
+
+    builder_.Finish(message);
+
+    return {builder_.GetBufferPointer(), builder_.GetSize()};
+}
+
+const Messaging::TopologyMessage *
+TopologyMessageBuilder::parse_topology_message(const uint8_t *buffer) {
+    return flatbuffers::GetRoot<Messaging::TopologyMessage>(buffer);
+}
+
+bool TopologyMessageBuilder::is_valid_topology_message(const uint8_t *buffer, size_t size) {
+    flatbuffers::Verifier verifier(buffer, size);
+    return Messaging::VerifyTopologyMessageBuffer(verifier);
+}
+} // namespace Flatbuffers

src/lib_c_control.cpp

@@ -0,0 +1,102 @@
+#include "lib_c_control.h"
+#include "flatbuffers/AngleControlMessageBuilder.h"
+#include "flatbuffers/RobotConfigurationBuilder.h"
+#include "libcontrol.h"
+#include "sentry.h"
+
+#undef min
+#undef max
+
+#define ACTUATOR_CONTROL_TAG 5
+#define TOPOLOGY_CMD_TAG 6
+#define MAX_RMT_CHANNELS 4
+
+extern "C" {
+const auto robot_controller = std::make_unique<RobotController>();
+
+const auto acm_builder = std::make_unique<Flatbuffers::AngleControlMessageBuilder>();
+
+const auto robot_configuration_builder = std::make_unique<Flatbuffers::RobotConfigurationBuilder>();
+
+LIB_API void init() {
+    spdlog::info("[c_control] Initializing");
+    robot_controller->fetchDirectlyConnectedModules(false);
+}
+
+LIB_API void cleanup() {
+    spdlog::info("[c_control] Cleanup");
+}
+
+LIB_API int send_angle_control(int module_id, int angle) {
+    if (const auto maybe_module = robot_controller->getModule(module_id)) {
+        const auto module = (*maybe_module).lock();
+        module->actuate(angle);
+    }
+    return 0;
+}
+
+LIB_API char *get_configuration(int *size_out) {
+    std::vector<Flatbuffers::ModuleInstance> modules_vec{};
+    std::vector<Flatbuffers::ModuleConnectionInstance> connections_vec{};
+
+    for (const auto &module : robot_controller->getModuleList()) {
+        modules_vec.emplace_back(module);
+    }
+
+    for (const auto &connection : robot_controller->getConnections()) {
+        connections_vec.emplace_back(connection);
+    }
+
+    const auto [data, size] =
+        robot_configuration_builder->build_robot_configuration(modules_vec, connections_vec);
+    *size_out = size;
+    return reinterpret_cast<char *>(data);
+}
+
+LIB_API bool control_sentry_init(const char *dsn, const char *environment, const char *release) {
+    sentry_options_t *options = sentry_options_new();
+    sentry_options_set_dsn(options, dsn);
+    sentry_options_set_environment(options, environment);
+    sentry_options_set_release(options, release);
+    sentry_options_set_auto_session_tracking(options, 1);
+
+    sentry_init(options);
+    return true;
+}
+
+LIB_API void control_sentry_shutdown(void) {
+    sentry_close();
+}
+
+LIB_API void control_sentry_set_app_info(const char *app_name, const char *app_version,
+                                         const char *build_number) {
+    sentry_value_t app = sentry_value_new_object();
+
+    if (app_name && *app_name) {
+        sentry_value_set_by_key(app, "name", sentry_value_new_string(app_name));
+    }
+
+    if (app_version && *app_version) {
+        sentry_value_set_by_key(app, "version", sentry_value_new_string(app_version));
+    }
+
+    if (build_number && *build_number) {
+        sentry_value_set_by_key(app, "build", sentry_value_new_string(build_number));
+    }
+
+    sentry_set_context("app", app);
+
+    if (app_version && *app_version) {
+        if (build_number && *build_number) {
+            char release[256];
+            snprintf(release, sizeof(release), "%s@%s+%s", app_name && *app_name ? app_name : "app",
+                     app_version, build_number);
+        } else {
+            // Example: mygame@1.2.3
+            char release[256];
+            snprintf(release, sizeof(release), "%s@%s", app_name && *app_name ? app_name : "app",
+                     app_version);
+        }
+    }
+}
+}

src/libcontrol.cpp

@@ -0,0 +1,249 @@
+//
+// Created by Johnathon Slightham on 2025-11-13.
+//
+
+#include <chrono>
+#include <memory>
+#include <mutex>
+#include <shared_mutex>
+#include <system_error>
+#include <thread>
+
+#include "flatbuffers_generated/SensorMessage_generated.h"
+#include "spdlog/spdlog.h"
+
+#include "ModuleFactory.h"
+#include "flatbuffers/RobotConfigurationBuilder.h"
+#include "flatbuffers/SensorMessageBuilder.h"
+#include "flatbuffers/TopologyMessageBuilder.h"
+#include "flatbuffers_generated/TopologyMessage_generated.h"
+#include "libcontrol.h"
+#include "util/Map.h"
+
+#define SCAN_DURATION_MS 2000
+#define ACTUATOR_CMD_TAG 5
+#define TOPOLOGY_CMD_TAG 6
+#define SENSOR_CMD_TAG 8
+#define NUM_CHANNELS 4
+#define TOPOLOGY_BUFFER_SIZE 1024
+#define SENSOR_BUFFER_SIZE 1024
+#define NETWORK_CONFIG_FETCH_RATE 3s
+#define MODULE_EXPIRE_TIME 3s
+#define CONTROL_MESSAGE_FREQUENCY 33ms
+
+using namespace std::chrono_literals;
+
+RobotController::~RobotController() {
+    m_stop_thread = true;
+    m_metadata_loop.join();
+    m_transmit_loop.join();
+    m_configuration_loop.join();
+    m_sensor_loop.join();
+    m_expiry_looop.join();
+}
+
+std::vector<std::weak_ptr<Module>> RobotController::getModules() {
+    std::vector<std::weak_ptr<Module>> out;
+    std::shared_lock lock(m_module_lock);
+    out.reserve(m_id_to_module.size());
+    for (const auto m : map_to_values(m_id_to_module)) {
+        out.emplace_back(m);
+    }
+    return out;
+}
+
+std::vector<Flatbuffers::ModuleConnectionInstance> RobotController::getConnections() {
+    std::vector<Flatbuffers::ModuleConnectionInstance> out;
+    std::shared_lock lock(m_connection_lock);
+
+    for (auto const &[_, value] : m_connection_map) {
+        for (const auto conn : value) {
+            out.push_back(conn);
+        }
+    }
+    return out;
+}
+
+std::vector<Flatbuffers::ModuleInstance> RobotController::getModuleList() {
+    std::vector<Flatbuffers::ModuleInstance> out;
+    std::shared_lock lock(m_module_lock);
+    for (auto const &[key, value] : m_id_to_module) {
+        out.push_back({key, value->get_type()});
+    }
+    return out;
+}
+
+std::optional<std::weak_ptr<Module>> RobotController::getModule(uint8_t device_id) {
+    std::shared_lock lock(m_module_lock);
+    if (m_id_to_module.contains(device_id)) {
+        return m_id_to_module[device_id];
+    } else {
+        return std::nullopt;
+    }
+}
+
+void RobotController::resetModules() {
+    std::unique_lock module_lock(m_module_lock);
+    std::unique_lock conn_lock(m_connection_lock);
+    m_id_to_module.erase(m_id_to_module.begin(), m_id_to_module.end());
+    m_connection_map.erase(m_connection_map.begin(), m_connection_map.end());
+}
+
+void RobotController::fetchDirectlyConnectedModules(bool block) {
+    spdlog::info("[Control] Fetching modules from network");
+    auto t = std::thread([&] {
+        auto out = m_messaging_interface->find_connected_modules(
+            std::chrono::milliseconds(SCAN_DURATION_MS));
+        spdlog::info("[Control] Found {} modules on the network", out.size());
+    });
+
+    if (block) {
+        t.join();
+    } else {
+        t.detach();
+    }
+}
+
+void RobotController::metadata_loop() {
+    unsigned char buf[TOPOLOGY_BUFFER_SIZE];
+    const auto builder = std::make_unique<Flatbuffers::TopologyMessageBuilder>();
+    while (!m_stop_thread) {
+        if (auto result =
+                m_messaging_interface->recv(buf, TOPOLOGY_BUFFER_SIZE, TOPOLOGY_CMD_TAG)) {
+            const auto &[rx_size, from] = *result;
+
+            flatbuffers::Verifier verifier(buf, rx_size);
+            if (!Messaging::VerifyTopologyMessageBuffer(verifier)) {
+                continue;
+            }
+
+            const auto metadata = builder->parse_topology_message(reinterpret_cast<uint8_t *>(buf));
+            std::unique_lock lock(m_module_lock);
+            if (!m_id_to_module.contains(metadata->module_id())) {
+                spdlog::info("[Control] Creating module entry for {}", metadata->module_id());
+                m_id_to_module.insert(
+                    {metadata->module_id(),
+                     ModuleFactory::createModule(metadata->module_id(), metadata->module_type(),
+                                                 m_messaging_interface)});
+            }
+
+            m_id_to_module[metadata->module_id()]->update_module_metadata(*metadata);
+
+            std::unique_lock conn_lock(m_connection_lock);
+            std::vector<Flatbuffers::ModuleConnectionInstance> conns;
+            for (uint8_t i = 1; i < NUM_CHANNELS; i++) {
+                if (metadata->channel_to_module()->Get(i) > 0) {
+                    conns.push_back(Flatbuffers::ModuleConnectionInstance{
+                        .from_module_id = metadata->module_id(),
+                        .to_module_id = metadata->channel_to_module()->Get(i),
+                        .from_socket = i,
+                        .to_socket = 0,
+                        .orientation =
+                            static_cast<Orientation>(metadata->channel_to_orientation()->Get(i))});
+                }
+            }
+
+            m_connection_map[metadata->module_id()] = conns;
+        }
+    }
+}
+
+void RobotController::transmit_loop() {
+    const auto builder = std::make_unique<Flatbuffers::TopologyMessageBuilder>();
+    while (!m_stop_thread) {
+        std::this_thread::sleep_for(CONTROL_MESSAGE_FREQUENCY);
+        std::shared_lock lock(m_module_lock);
+        for (const auto [id, module] : m_id_to_module) {
+            auto out = module->get_actuation_message();
+
+            if (out.size() > 0) {
+                m_messaging_interface->send(out.data(), out.size(), id, ACTUATOR_CMD_TAG, false);
+            }
+        }
+    }
+}
+
+void RobotController::configuration_loop() {
+    while (!m_stop_thread) {
+        std::this_thread::sleep_for(NETWORK_CONFIG_FETCH_RATE);
+        fetchDirectlyConnectedModules(true);
+    }
+}
+
+void RobotController::expiry_loop() {
+    while (!m_stop_thread) {
+        std::this_thread::sleep_for(MODULE_EXPIRE_TIME);
+
+        std::unordered_set<uint8_t> delete_modules{};
+
+        std::unique_lock module_lock(m_module_lock);
+        std::unique_lock connection_lock(m_connection_lock);
+
+        for (auto it = m_id_to_module.begin(); it != m_id_to_module.end();) {
+            if (it->second->get_last_updated_time() <
+                std::chrono::system_clock::now() - MODULE_EXPIRE_TIME) {
+                delete_modules.emplace(it->first);
+                it = m_id_to_module.erase(it);
+            } else {
+                ++it;
+            }
+        }
+
+        // todo
+        // Remove connections
+        // for (auto it = m_connection_map.begin(); it != m_connection_map.end();) {
+        //     // Remove it->x connections
+        //     if (delete_modules.contains(it->first)) {
+        //         it = m_connection_map.erase(it);
+        //     } else {
+        //         ++it;
+        //     }
+
+        //     // Remove x->it connections
+        //     for (auto it2 = it->second.begin(); it2 != it->second.end();) {
+        //         if (delete_modules.contains(it2->to_module_id)) {
+        //             it2 = it->second.erase(it2);
+        //         } else {
+        //             ++it2;
+        //         }
+        //     }
+        // }
+    }
+}
+
+void RobotController::sensor_loop() {
+    unsigned char buf[SENSOR_BUFFER_SIZE];
+    const auto builder = std::make_unique<Flatbuffers::SensorMessageBuilder>();
+    while (!m_stop_thread) {
+        if (auto result = m_messaging_interface->recv(buf, SENSOR_BUFFER_SIZE, SENSOR_CMD_TAG)) {
+            const auto &[rx_size, from] = *result;
+
+            flatbuffers::Verifier verifier(buf, rx_size);
+            if (!Messaging::VerifySensorMessageBuffer(verifier)) {
+                continue;
+            }
+
+            const auto sensor_message =
+                builder->parse_sensor_message(reinterpret_cast<uint8_t *>(buf));
+            if (sensor_message->values()->size() != sensor_message->values_type()->size()) {
+                spdlog::error(
+                    "[Control] Got a sensor message with different value ({}) and type ({}) sizes",
+                    sensor_message->values()->size(), sensor_message->values_type()->size());
+                continue;
+            }
+
+            std::shared_lock module_lock(m_module_lock);
+            if (!m_id_to_module.contains(from)) {
+                continue;
+            }
+
+            for (int i = 0; i < sensor_message->values()->size(); i++) {
+                if (auto maybe_value = Flatbuffers::SensorMessageBuilder::build_sensor_value(
+                        static_cast<Messaging::SensorValue>(sensor_message->values_type()->Get(i)),
+                        sensor_message->values()->Get(i))) {
+                    m_id_to_module[from]->update_sensor_data(*maybe_value);
+                }
+            }
+        }
+    }
+}