botchain/control
1
0
Fork 0
mirror of https://github.com/BotChain-Robots/control.git synced 2026-03-10 00:32:26 +01:00
Code Issues Packages Projects Releases Wiki Activity

Formatting, add remote calling to c library

Browse Source
This commit is contained in:
Johnathon Slightham
2026-02-18 09:18:26 -05:00
parent 0088a9070b
commit 376b0b5285
49 changed files with 1915 additions and 1593 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
src/Hub.cpp
View File

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

49
src/Module.cpp
View File

@@ -4,33 +4,40 @@
#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;
std::vector<neighbour> Module::get_neighbours() {
return m_neighbours;
}
uint8_t Module::get_leader() { return m_leader; }
uint8_t Module::get_device_id() {
return m_device_id;
}
std::chrono::time_point<std::chrono::system_clock>
Module::get_last_updated_time() {
return m_last_updated;
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_module_type = message.module_type();
m_connection_type = message.connection();
m_leader = message.leader();
m_last_updated = std::chrono::system_clock::now();
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)});
}
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)});
}
}

41
src/ModuleFactory.cpp
View File

@@ -10,26 +10,25 @@
#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:
case ModuleType_GRIPPER:
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);
case ModuleType_DISPLAY:
return std::make_shared<OledActuator>(device_id, type);
default:
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:
case ModuleType_GRIPPER:
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);
case ModuleType_DISPLAY:
return std::make_shared<OledActuator>(device_id, type);
default:
return nullptr;
}
return nullptr;
}
return nullptr;
}

55
src/actuators/BoundedPositionalActuator1D.cpp
View File

@@ -7,46 +7,45 @@
#include "util/Variant.h"
double BoundedPositionalActuator1D::get_position() {
return m_current_position;
return m_current_position;
}
std::string BoundedPositionalActuator1D::get_text() { return ""; }
std::string BoundedPositionalActuator1D::get_text() {
return "";
}
void BoundedPositionalActuator1D::actuate(double position) {
if (position < m_min_value || position > m_max_value) {
return;
}
if (position < m_min_value || position > m_max_value) {
return;
}
m_target_position = position;
m_target_position = position;
}
void BoundedPositionalActuator1D::actuate(double x, double y) {}
void BoundedPositionalActuator1D::actuate(double x, double y) {
}
void BoundedPositionalActuator1D::actuate(const std::string &text) {}
void BoundedPositionalActuator1D::actuate(const std::string &text) {
}
std::vector<uint8_t> BoundedPositionalActuator1D::get_actuation_message() {
std::vector<uint8_t> message{};
std::vector<uint8_t> message{};
if (m_target_position == m_current_position) {
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;
}
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;
},
[this](Flatbuffers::current_text /*t*/) {},
},
value);
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; },
[this](Flatbuffers::current_text /*t*/) {},
},
value);
}

48
src/actuators/OledActuator.cpp
View File

@@ -3,36 +3,42 @@
#include "util/Variant.h"
#include <cstring>
double OledActuator::get_position() { return 0.0; }
double OledActuator::get_position() {
return 0.0;
}
std::string OledActuator::get_text() { return m_current_text; }
std::string OledActuator::get_text() {
return m_current_text;
}
void OledActuator::actuate(double /* position */) {}
void OledActuator::actuate(double /* position */) {
}
void OledActuator::actuate(double /* x */, double /* y */) {}
void OledActuator::actuate(double /* x */, double /* y */) {
}
void OledActuator::actuate(const std::string &text) { m_target_text = text; }
void OledActuator::actuate(const std::string &text) {
m_target_text = text;
}
std::vector<uint8_t> OledActuator::get_actuation_message() {
std::vector<uint8_t> message{};
std::vector<uint8_t> message{};
if (m_target_text == m_current_text) {
if (m_target_text == m_current_text) {
return message;
}
auto [data, size] = m_text_message_builder->build_text_control_message(m_target_text);
message.resize(size);
memcpy(message.data(), data, size);
return message;
}
auto [data, size] =
m_text_message_builder->build_text_control_message(m_target_text);
message.resize(size);
memcpy(message.data(), data, size);
return message;
}
void OledActuator::update_sensor_data(const Flatbuffers::sensor_value &value) {
std::visit(
overloaded{
[this](Flatbuffers::target_angle /* a */) {},
[this](Flatbuffers::current_angle /* a */) {},
[this](Flatbuffers::current_text t) { m_current_text = t.text; },
},
value);
std::visit(overloaded{
[this](Flatbuffers::target_angle /* a */) {},
[this](Flatbuffers::current_angle /* a */) {},
[this](Flatbuffers::current_text t) { m_current_text = t.text; },
},
value);
}

52
src/actuators/PositionalActuator1D.cpp
View File

@@ -3,42 +3,42 @@
#include "util/Variant.h"
#include <cstring>
double PositionalActuator1D::get_position() { return m_current_position; }
double PositionalActuator1D::get_position() {
return m_current_position;
}
std::string PositionalActuator1D::get_text() { return ""; }
std::string PositionalActuator1D::get_text() {
return "";
}
void PositionalActuator1D::actuate(double position) {
m_target_position = position;
m_target_position = position;
}
void PositionalActuator1D::actuate(double /* x */, double /* y */) {}
void PositionalActuator1D::actuate(double /* x */, double /* y */) {
}
void PositionalActuator1D::actuate(const std::string &text) {}
void PositionalActuator1D::actuate(const std::string &text) {
}
std::vector<uint8_t> PositionalActuator1D::get_actuation_message() {
std::vector<uint8_t> message{};
std::vector<uint8_t> message{};
if (m_target_position == m_board_target_position) {
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;
}
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;
},
[this](Flatbuffers::current_text /*t*/) {},
},
value);
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; },
[this](Flatbuffers::current_text /*t*/) {},
},
value);
}

13
src/flatbuffers/AngleControlMessageBuilder.cpp
View File

@@ -7,17 +7,16 @@
namespace Flatbuffers {
const Messaging::AngleControlMessage *
AngleControlMessageBuilder::parse_angle_control_message(const uint8_t *buffer) {
return flatbuffers::GetRoot<Messaging::AngleControlMessage>(buffer);
return flatbuffers::GetRoot<Messaging::AngleControlMessage>(buffer);
}
SerializedMessage
AngleControlMessageBuilder::build_angle_control_message(const int16_t angle) {
builder_.Clear();
SerializedMessage AngleControlMessageBuilder::build_angle_control_message(const int16_t angle) {
builder_.Clear();
const auto message = Messaging::CreateAngleControlMessage(builder_, angle);
const auto message = Messaging::CreateAngleControlMessage(builder_, angle);
builder_.Finish(message);
builder_.Finish(message);
return {builder_.GetBufferPointer(), builder_.GetSize()};
return {builder_.GetBufferPointer(), builder_.GetSize()};
}
} // namespace Flatbuffers

49
src/flatbuffers/RobotConfigurationBuilder.cpp
View File

@@ -12,41 +12,38 @@ namespace Flatbuffers {
SerializedMessage RobotConfigurationBuilder::build_robot_configuration(
const std::vector<ModuleInstance> &modules,
const std::vector<ModuleConnectionInstance> &connections) {
builder_.Clear();
builder_.Clear();
std::vector<flatbuffers::Offset<RobotModule>> module_vector;
std::vector<flatbuffers::Offset<Frontend::ModuleConnection>>
connection_vector;
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 &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));
}
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 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);
const auto message =
Frontend::CreateRobotConfiguration(builder_, module_vector_fb, connection_vector_fb);
builder_.Finish(message);
builder_.Finish(message);
return {builder_.GetBufferPointer(), builder_.GetSize()};
return {builder_.GetBufferPointer(), builder_.GetSize()};
}
const Frontend::RobotConfiguration *
RobotConfigurationBuilder::parse_robot_configuration(
const std::uint8_t *buffer) {
return flatbuffers::GetRoot<Frontend::RobotConfiguration>(buffer);
RobotConfigurationBuilder::parse_robot_configuration(const std::uint8_t *buffer) {
return flatbuffers::GetRoot<Frontend::RobotConfiguration>(buffer);
}
} // namespace Flatbuffers

5
src/flatbuffers/SensorMessageBuilder.cpp
View File

@@ -3,9 +3,8 @@
namespace Flatbuffers {
const Messaging::SensorMessage *
SensorMessageBuilder::parse_sensor_message(const uint8_t *buffer) {
return flatbuffers::GetRoot<Messaging::SensorMessage>(buffer);
const Messaging::SensorMessage *SensorMessageBuilder::parse_sensor_message(const uint8_t *buffer) {
return flatbuffers::GetRoot<Messaging::SensorMessage>(buffer);
}
} // namespace Flatbuffers

14
src/flatbuffers/TextControlMessageBuilder.cpp
View File

@@ -6,16 +6,14 @@
namespace Flatbuffers {
SerializedMessage
TextControlMessageBuilder::build_text_control_message(std::string &t) {
builder_.Clear();
SerializedMessage TextControlMessageBuilder::build_text_control_message(std::string &t) {
builder_.Clear();
auto text_offset = builder_.CreateString(t);
const auto message =
Messaging::CreateTextControlMessage(builder_, text_offset);
auto text_offset = builder_.CreateString(t);
const auto message = Messaging::CreateTextControlMessage(builder_, text_offset);
builder_.Finish(message);
builder_.Finish(message);
return {builder_.GetBufferPointer(), builder_.GetSize()};
return {builder_.GetBufferPointer(), builder_.GetSize()};
}
} // namespace Flatbuffers

36
src/flatbuffers/TopologyMessageBuilder.cpp
View File

@@ -7,34 +7,32 @@
#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();
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 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);
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);
builder_.Finish(message);
return {builder_.GetBufferPointer(), builder_.GetSize()};
return {builder_.GetBufferPointer(), builder_.GetSize()};
}
const Messaging::TopologyMessage *
TopologyMessageBuilder::parse_topology_message(const uint8_t *buffer) {
return flatbuffers::GetRoot<Messaging::TopologyMessage>(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);
bool TopologyMessageBuilder::is_valid_topology_message(const uint8_t *buffer, size_t size) {
flatbuffers::Verifier verifier(buffer, size);
return Messaging::VerifyTopologyMessageBuffer(verifier);
}
} // namespace Flatbuffers

176
src/lib_c_control.cpp
View File

@@ -14,93 +14,117 @@
extern "C" {
const auto robot_controller = std::make_unique<RobotController>();
const auto acm_builder =
std::make_unique<Flatbuffers::AngleControlMessageBuilder>();
const auto acm_builder = std::make_unique<Flatbuffers::AngleControlMessageBuilder>();
const auto robot_configuration_builder =
std::make_unique<Flatbuffers::RobotConfigurationBuilder>();
const auto robot_configuration_builder = std::make_unique<Flatbuffers::RobotConfigurationBuilder>();
LIB_API void init() {
spdlog::info("[c_control] Initializing");
robot_controller->fetchDirectlyConnectedModules(false);
spdlog::info("[c_control] Initializing");
robot_controller->fetchDirectlyConnectedModules(false);
}
LIB_API void cleanup() { spdlog::info("[c_control] Cleanup"); }
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;
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{};
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);
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);
}
}
}
}
}
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,
uint16_t *out_bytes_written) {
if (!out_bytes_written)
return false;
*out_bytes_written = 0;
std::vector<uint8_t> vec(params, params + params_len);
auto result = robot_controller->remote_call(function_tag, module, vec);
if (!result.has_value())
return false;
const std::vector<uint8_t> &out = *result.value();
size_t needed = out.size();
if (!out_buffer || out_buffer_capacity < needed)
return false;
memcpy(out_buffer, out.data(), needed);
return true;
}

338
src/libcontrol.cpp
View File

@@ -34,227 +34,223 @@
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();
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<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);
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);
for (auto const &[_, value] : m_connection_map) {
for (const auto conn : value) {
out.push_back(conn);
}
}
}
return out;
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::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;
}
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());
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());
});
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();
}
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;
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;
}
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)});
}
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);
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))});
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;
}
}
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();
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);
}
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);
}
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);
while (!m_stop_thread) {
std::this_thread::sleep_for(MODULE_EXPIRE_TIME);
std::unordered_set<uint8_t> delete_modules{};
std::unordered_set<uint8_t> delete_modules{};
std::unique_lock module_lock(m_module_lock);
std::unique_lock connection_lock(m_connection_lock);
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;
}
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;
// }
// }
// }
}
// 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;
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;
}
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;
}
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;
}
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);
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);
}
}
}
}
}
}
}
std::optional<std::unique_ptr<std::vector<uint8_t>>>
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);
}