Allow multiple robots on the same network (#5)

* Support multiple bots on the same network

* Bulk OTa

* Bugfixes

examples/rpc_call/conanfile.txt

@@ -1,8 +1,8 @@
 [requires]
-libcontrol/1.0.0
+libcontrol/1.0.3
 flatbuffers/24.12.23
 spdlog/1.16.0
-librpc/1.1.7
+librpc/1.1.8
 
 [generators]
 CMakeDeps

examples/rpc_call/main.cpp

@@ -3,6 +3,7 @@
 #include <cstring>
 #include <format>
 #include <iostream>
+#include <random>
 #include <thread>
 #include <vector>
 
@@ -10,19 +11,27 @@
 #include "rpc/RemoteManagement.h"
 
 void progress_monitor_task(std::shared_ptr<RemoteManagement> rm, uint8_t module_id) {
+    std::random_device dev;
+    std::mt19937 rng(dev());
+    std::uniform_int_distribution<std::mt19937::result_type> dist6(10, 100);
+
+    std::cout << dist6(rng) << std::endl;
     while (true) {
-        std::this_thread::sleep_for(std::chrono::seconds(5));
+        std::this_thread::sleep_for(std::chrono::milliseconds(5000 + dist6(rng)));
-        std::cout << "Module " << (int)module_id << ": " << rm->ota_progress() * 100 << "% complete\n";
+        std::cout << "Module " << (int)module_id << ": " << rm->ota_progress() * 100 << "% complete"
-        if (rm->ota_progress() >= 1.0) return;
+                  << std::endl;
+        if (rm->ota_progress() >= 1.0)
+            return;
     }
 }
 
-void task(std::shared_ptr<RobotController> controller, uint8_t module_id, const std::string& filepath) {
+void task(std::shared_ptr<RobotController> controller, uint8_t module_id,
+          const std::string &filepath) {
     auto rm = std::make_shared<RemoteManagement>(module_id, filepath, controller);
     std::thread t(progress_monitor_task, rm, module_id);
     rm->perform_ota();
     t.join();
-    std::cout << "Done updating " << (int)module_id << "\n";
+    std::cout << "Done updating " << (int)module_id << std::endl;
 }
 
 int main() {
@@ -33,8 +42,10 @@ int main() {
     std::vector<uint8_t> to_update{};
     for (const auto &maybe_module : robot_controller->getModules()) {
         if (const auto &module = maybe_module.lock()) {
-            if (module->get_leader() != module->get_device_id()) { continue; }
+            if (module->get_leader() != module->get_device_id()) {
-            std::cout << "Updating module " << (int)module->get_device_id();
+                continue;
+            }
+            std::cout << "Updating module " << (int)module->get_device_id() << std::endl;
             to_update.emplace_back(module->get_device_id());
         }
     }
@@ -44,8 +55,7 @@ int main() {
         return 0;
     }
 
-    std::string filename =
+    std::string filename = "/home/jslightham/Documents/capstone/firmware/build/firmware.bin";
-            "/Users/jslightham/Documents/Classes/capstone/firmware/build/firmware.bin";
     std::vector<std::thread> threads;
     for (int i = 0; i < to_update.size(); i++) {
         threads.emplace_back(task, robot_controller, to_update[i], filename);

examples/rpc_call/rpc/RemoteManagement.cpp

@@ -62,12 +62,16 @@ void RemoteManagement::restart() {
 
 bool RemoteManagement::start_ota() {
     // std::cout << "Starting OTA" << std::endl;
+    int attempts = 0;
+    while (attempts < MAX_PACKET_TX_ATTEMPTS) {
         const auto maybe = m_robot_controller->remote_call(4, m_module_id, {});
         if (maybe) {
             // std::cout << "Got valid response" << std::endl;
             m_sequence_num = 1;
             return (*maybe)->at(0) > 0;
         }
+        attempts++;
+    }
     return false;
 }
 

examples/servo/conanfile.txt

@@ -1,8 +1,8 @@
 [requires]
-libcontrol/1.0.0
+libcontrol/1.0.3
 flatbuffers/24.12.23
 spdlog/1.16.0
-librpc/1.1.7
+librpc/1.1.8
 
 [generators]
 CMakeDeps

examples/servo/main.cpp

@@ -24,18 +24,21 @@ int main() {
         std::cout << "Found " << controller->getModules().size() << " modules" << std::endl;
         for (const auto &maybe_module : controller->getModules()) {
             if (const auto &module = maybe_module.lock()) {
-                if (module->get_type() == ModuleType_DC_MOTOR) {
+                if (module->get_type() == ModuleType_SERVO_2 ||
+                    module->get_type() == ModuleType_SERVO_1) {
+                    std::cout << "Actuating " << (int)module->get_device_id() << " leader "
+                              << (int)module->get_leader() << std::endl;
                     int randomNumber = dist(gen);
                     if (module->get_position() > 90) {
-                        module->actuate(70 - randomNumber);
+                        module->actuate(50 - randomNumber);
                     } else {
-                        module->actuate(110 + randomNumber);
+                        module->actuate(150 + randomNumber);
+                    }
+                    std::this_thread::sleep_for(std::chrono::milliseconds(250));
                 }
             }
         }
     }
-        std::this_thread::sleep_for(std::chrono::milliseconds(100));
-    }
 
     return 0;
 }

include/Module.h

@@ -61,7 +61,7 @@ class Module {
     uint8_t m_device_id;
     ModuleType m_module_type;
     Messaging::ConnectionType m_connection_type;
-    uint8_t m_leader;
+    std::atomic<uint8_t> m_leader{0};
     std::chrono::time_point<std::chrono::system_clock> m_last_updated;
     std::vector<neighbour> m_neighbours;
     std::shared_ptr<MessagingInterface> m_messaging_interface;

include/flatbuffers/RobotConfigurationBuilder.h

@@ -15,6 +15,7 @@ struct ModuleInstance {
     uint8_t id;
     ModuleType type;
     int angle;
+    uint8_t leader;
 };
 
 struct ModuleConnectionInstance {

include/lib_c_control.h

@@ -19,7 +19,8 @@ LIB_API void cleanup();
 LIB_API int send_angle_control(int module_id, int angle);
 LIB_API int send_string_control(int module_id, const char *string);
 LIB_API double get_distance_control(int module_id);
-LIB_API char *get_configuration(int *size_out);
+LIB_API char *get_configuration(int *size_out, int leader_id);
+LIB_API const uint8_t *get_leaders(int *length);
 
 LIB_API bool remote_call_c(uint8_t function_tag, uint8_t module, const uint8_t *params,
                            uint16_t params_len, uint8_t *out_buffer, uint16_t out_buffer_capacity,

include/libcontrol.h

@@ -93,6 +93,9 @@ class RobotController {
    */
     void resetModules();
 
+    bool select_leader(uint8_t leader_id);
+    std::vector<uint8_t> get_leaders();
+
     /**
    * \brief Poll for devices accessible to the PC.
    *
@@ -103,6 +106,8 @@ class RobotController {
     std::optional<std::unique_ptr<std::vector<uint8_t>>>
     remote_call(uint8_t function_tag, uint8_t module, const std::vector<uint8_t> &parameters);
 
+    uint8_t get_selected_leader();
+
   private:
     std::shared_ptr<spdlog::logger> m_logger;
     std::unordered_map<uint8_t, std::shared_ptr<Module>> m_id_to_module{};
@@ -118,6 +123,7 @@ class RobotController {
     std::thread m_configuration_loop;
     std::thread m_sensor_loop;
     std::thread m_expiry_looop;
+    std::atomic<uint8_t> m_selected_leader{0};
 
     void metadata_loop();
     void transmit_loop();

src/lib_c_control.cpp

@@ -63,15 +63,27 @@ LIB_API double get_distance_control(int module_id) {
     return 0.0;
 }
 
-LIB_API char *get_configuration(int *size_out) {
+LIB_API char *get_configuration(int *size_out, int leader_id) {
     std::vector<Flatbuffers::ModuleInstance> modules_vec{};
     std::vector<Flatbuffers::ModuleConnectionInstance> connections_vec{};
+    std::unordered_set<uint8_t> modules_send{};
+
+    robot_controller->select_leader(leader_id);
 
     for (const auto &module : robot_controller->getModuleList()) {
+        if (robot_controller->get_selected_leader() != module.leader) {
+            continue;
+        }
         modules_vec.emplace_back(module);
+        modules_send.emplace(module.id);
     }
 
     for (const auto &connection : robot_controller->getConnections()) {
+        if (!modules_send.contains(connection.from_module_id) ||
+            !modules_send.contains(connection.to_module_id)) {
+            continue;
+        }
+
         connections_vec.emplace_back(connection);
     }
 
@@ -156,3 +168,11 @@ LIB_API bool remote_call_c(uint8_t function_tag, uint8_t module, const uint8_t *
 
     return true;
 }
+
+LIB_API const uint8_t *get_leaders(int *length) {
+    static std::vector<uint8_t> leaders;
+    leaders = robot_controller->get_leaders();
+    *length = leaders.size();
+    spdlog::info("Returning {} leaders", *length);
+    return leaders.data();
+}

src/libcontrol.cpp

@@ -113,7 +113,7 @@ 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()});
+        out.push_back({key, value->get_type(), 0, value->get_leader()});
     }
     return out;
 }
@@ -201,6 +201,9 @@ void RobotController::transmit_loop() {
         std::this_thread::sleep_for(CONTROL_MESSAGE_FREQUENCY);
         std::shared_lock lock(m_module_lock);
         for (const auto [id, module] : m_id_to_module) {
+            if (m_selected_leader != module->get_leader()) {
+                continue;
+            }
             auto out = module->get_actuation_message();
 
             if (out.size() > 0) {
@@ -297,3 +300,28 @@ RobotController::remote_call(uint8_t function_tag, uint8_t module,
                              const std::vector<uint8_t> &parameters) {
     return m_messaging_interface->remote_call(function_tag, module, parameters);
 }
+
+std::vector<uint8_t> RobotController::get_leaders() {
+    std::shared_lock lock(m_module_lock);
+    std::unordered_set<uint8_t> out{};
+
+    spdlog::info("number of modules {}", m_id_to_module.size());
+    for (const auto m : map_to_values(m_id_to_module)) {
+        out.insert(m->get_leader());
+    }
+
+    return {out.begin(), out.end()};
+}
+
+bool RobotController::select_leader(uint8_t leader_id) {
+    std::shared_lock lock(m_module_lock);
+    if (m_id_to_module.contains(leader_id)) {
+        m_selected_leader = leader_id;
+        return true;
+    }
+    return false;
+}
+
+uint8_t RobotController::get_selected_leader() {
+    return m_selected_leader;
+}