From e30eb39187c1cf446b5a38c2c1385328f2dc4ab1 Mon Sep 17 00:00:00 2001 From: Johnathon Slightham Date: Wed, 11 Mar 2026 01:41:30 -0400 Subject: [PATCH] Allow multiple robots on the same network (#5) * Support multiple bots on the same network * Bulk OTa * Bugfixes --- examples/rpc_call/conanfile.txt | 4 +-- examples/rpc_call/main.cpp | 30 ++++++++++++------- examples/rpc_call/rpc/RemoteManagement.cpp | 16 ++++++---- examples/servo/conanfile.txt | 4 +-- examples/servo/main.cpp | 11 ++++--- include/Module.h | 2 +- include/actuators/Actuator.h | 6 ++-- include/actuators/PositionalActuator1D.h | 2 +- .../flatbuffers/RobotConfigurationBuilder.h | 1 + include/lib_c_control.h | 3 +- include/libcontrol.h | 6 ++++ src/lib_c_control.cpp | 22 +++++++++++++- src/libcontrol.cpp | 30 ++++++++++++++++++- 13 files changed, 105 insertions(+), 32 deletions(-) diff --git a/examples/rpc_call/conanfile.txt b/examples/rpc_call/conanfile.txt index a61eff3..f578d99 100644 --- a/examples/rpc_call/conanfile.txt +++ b/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 diff --git a/examples/rpc_call/main.cpp b/examples/rpc_call/main.cpp index 38d9652..3c3ad9e 100644 --- a/examples/rpc_call/main.cpp +++ b/examples/rpc_call/main.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include #include @@ -10,19 +11,27 @@ #include "rpc/RemoteManagement.h" void progress_monitor_task(std::shared_ptr rm, uint8_t module_id) { + std::random_device dev; + std::mt19937 rng(dev()); + std::uniform_int_distribution dist6(10, 100); + + std::cout << dist6(rng) << std::endl; while (true) { - std::this_thread::sleep_for(std::chrono::seconds(5)); - std::cout << "Module " << (int)module_id << ": " << rm->ota_progress() * 100 << "% complete\n"; - if (rm->ota_progress() >= 1.0) return; + std::this_thread::sleep_for(std::chrono::milliseconds(5000 + dist6(rng))); + std::cout << "Module " << (int)module_id << ": " << rm->ota_progress() * 100 << "% complete" + << std::endl; + if (rm->ota_progress() >= 1.0) + return; } } -void task(std::shared_ptr controller, uint8_t module_id, const std::string& filepath) { +void task(std::shared_ptr controller, uint8_t module_id, + const std::string &filepath) { auto rm = std::make_shared(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() { @@ -31,10 +40,12 @@ int main() { std::this_thread::sleep_for(std::chrono::seconds(5)); std::vector to_update{}; - for (const auto& maybe_module : robot_controller->getModules()) { + for (const auto &maybe_module : robot_controller->getModules()) { if (const auto &module = maybe_module.lock()) { - if (module->get_leader() != module->get_device_id()) { continue; } - std::cout << "Updating module " << (int)module->get_device_id(); + if (module->get_leader() != 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 = - "/Users/jslightham/Documents/Classes/capstone/firmware/build/firmware.bin"; + std::string filename = "/home/jslightham/Documents/capstone/firmware/build/firmware.bin"; std::vector threads; for (int i = 0; i < to_update.size(); i++) { threads.emplace_back(task, robot_controller, to_update[i], filename); diff --git a/examples/rpc_call/rpc/RemoteManagement.cpp b/examples/rpc_call/rpc/RemoteManagement.cpp index 9ab0382..e6f573c 100644 --- a/examples/rpc_call/rpc/RemoteManagement.cpp +++ b/examples/rpc_call/rpc/RemoteManagement.cpp @@ -23,7 +23,7 @@ bool RemoteManagement::perform_ota() { m_file.seekg(0, std::ios::end); std::streamsize total_size = m_file.tellg(); m_file.seekg(0, std::ios::beg); - m_total_packets = total_size/OTA_CHUNK_SIZE; + m_total_packets = total_size / OTA_CHUNK_SIZE; // std::cout << "Total number of chunks: " << total_size/OTA_CHUNK_SIZE << std::endl; while (m_file) { @@ -62,11 +62,15 @@ void RemoteManagement::restart() { bool RemoteManagement::start_ota() { // std::cout << "Starting OTA" << std::endl; - 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; + 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; } diff --git a/examples/servo/conanfile.txt b/examples/servo/conanfile.txt index a61eff3..f578d99 100644 --- a/examples/servo/conanfile.txt +++ b/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 diff --git a/examples/servo/main.cpp b/examples/servo/main.cpp index 7b2bdc2..9a28b48 100644 --- a/examples/servo/main.cpp +++ b/examples/servo/main.cpp @@ -24,17 +24,20 @@ 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; diff --git a/include/Module.h b/include/Module.h index 8942e98..49d967e 100644 --- a/include/Module.h +++ b/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 m_leader{0}; std::chrono::time_point m_last_updated; std::vector m_neighbours; std::shared_ptr m_messaging_interface; diff --git a/include/actuators/Actuator.h b/include/actuators/Actuator.h index 90c7ddf..27c9654 100644 --- a/include/actuators/Actuator.h +++ b/include/actuators/Actuator.h @@ -10,13 +10,13 @@ class Actuator : public Module { public: - explicit Actuator(uint8_t device_id) : Module(device_id) {}; + explicit Actuator(uint8_t device_id) : Module(device_id){}; - Actuator(uint8_t device_id, ModuleType module_type) : Module(device_id, module_type) {}; + Actuator(uint8_t device_id, ModuleType module_type) : Module(device_id, module_type){}; Actuator(uint8_t device_id, ModuleType module_type, Messaging::ConnectionType connection_type, uint8_t leader) - : Module(device_id, module_type, connection_type, leader) {}; + : Module(device_id, module_type, connection_type, leader){}; protected: virtual std::vector get_actuation_message() = 0; diff --git a/include/actuators/PositionalActuator1D.h b/include/actuators/PositionalActuator1D.h index 215f8ce..a7644b8 100644 --- a/include/actuators/PositionalActuator1D.h +++ b/include/actuators/PositionalActuator1D.h @@ -16,7 +16,7 @@ class PositionalActuator1D final : public Actuator { public: PositionalActuator1D(uint8_t device_id, ModuleType type) : Actuator(device_id, type), - m_acm_builder(std::make_unique()) {}; + m_acm_builder(std::make_unique()){}; double get_position() override; void actuate(double position) override; diff --git a/include/flatbuffers/RobotConfigurationBuilder.h b/include/flatbuffers/RobotConfigurationBuilder.h index 90a4fb7..f655532 100644 --- a/include/flatbuffers/RobotConfigurationBuilder.h +++ b/include/flatbuffers/RobotConfigurationBuilder.h @@ -15,6 +15,7 @@ struct ModuleInstance { uint8_t id; ModuleType type; int angle; + uint8_t leader; }; struct ModuleConnectionInstance { diff --git a/include/lib_c_control.h b/include/lib_c_control.h index b9f3de0..7f73b40 100644 --- a/include/lib_c_control.h +++ b/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, diff --git a/include/libcontrol.h b/include/libcontrol.h index 61f5ac7..c2b69d8 100644 --- a/include/libcontrol.h +++ b/include/libcontrol.h @@ -93,6 +93,9 @@ class RobotController { */ void resetModules(); + bool select_leader(uint8_t leader_id); + std::vector get_leaders(); + /** * \brief Poll for devices accessible to the PC. * @@ -103,6 +106,8 @@ class RobotController { std::optional>> remote_call(uint8_t function_tag, uint8_t module, const std::vector ¶meters); + uint8_t get_selected_leader(); + private: std::shared_ptr m_logger; std::unordered_map> 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 m_selected_leader{0}; void metadata_loop(); void transmit_loop(); diff --git a/src/lib_c_control.cpp b/src/lib_c_control.cpp index b27036f..34957f7 100644 --- a/src/lib_c_control.cpp +++ b/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 modules_vec{}; std::vector connections_vec{}; + std::unordered_set 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 leaders; + leaders = robot_controller->get_leaders(); + *length = leaders.size(); + spdlog::info("Returning {} leaders", *length); + return leaders.data(); +} diff --git a/src/libcontrol.cpp b/src/libcontrol.cpp index fc6703b..bda7c2e 100644 --- a/src/libcontrol.cpp +++ b/src/libcontrol.cpp @@ -113,7 +113,7 @@ std::vector RobotController::getModuleList() { std::vector 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 ¶meters) { return m_messaging_interface->remote_call(function_tag, module, parameters); } + +std::vector RobotController::get_leaders() { + std::shared_lock lock(m_module_lock); + std::unordered_set 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; +}