From e67d44a536a990d92c5564a802cf28b7af22c464 Mon Sep 17 00:00:00 2001 From: Johnathon Slightham Date: Tue, 3 Mar 2026 21:11:34 -0500 Subject: [PATCH] Add distance sensor and display --- CMakeLists.txt | 1 + conanfile.py | 4 +- control.def | 2 + examples/display/build.sh | 4 + include/Module.h | 2 + include/flatbuffers/SensorMessageBuilder.h | 10 +- .../SensorMessage_generated.h | 187 +++++++++++++++++- include/lib_c_control.h | 2 + include/sensors/DistanceSensor.h | 23 +++ src/Module.cpp | 5 + src/ModuleFactory.cpp | 7 + src/actuators/BoundedPositionalActuator1D.cpp | 9 +- src/actuators/OledActuator.cpp | 9 +- src/actuators/PositionalActuator1D.cpp | 8 +- src/lib_c_control.cpp | 24 +++ src/sensors/DistanceSensor.cpp | 18 ++ 16 files changed, 288 insertions(+), 27 deletions(-) create mode 100755 examples/display/build.sh create mode 100644 include/sensors/DistanceSensor.h create mode 100644 src/sensors/DistanceSensor.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 95a9210..d4c8d71 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(crashpad REQUIRED) file(GLOB_RECURSE COMMON_SOURCES src/flatbuffers/*.cpp src/actuators/*.cpp + src/sensors/*.cpp src/Module.cpp src/Hub.cpp src/ModuleFactory.cpp diff --git a/conanfile.py b/conanfile.py index 4de6bf2..74633fd 100644 --- a/conanfile.py +++ b/conanfile.py @@ -7,7 +7,7 @@ from conan.tools.files import copy class MyLibraryConan(ConanFile): name = "libcontrol" - version = "1.0.2" + version = "1.0.3" settings = "os", "compiler", "build_type", "arch" options = {"shared": [True, False], "fPIC": [True, False]} @@ -40,7 +40,7 @@ class MyLibraryConan(ConanFile): def requirements(self): self.requires("flatbuffers/24.12.23") - self.requires("librpc/1.1.7") + self.requires("librpc/1.1.8") self.requires("eigen/3.4.1") self.requires("spdlog/1.16.0") self.requires("sentry-native/0.12.2") diff --git a/control.def b/control.def index d819679..f9aebbc 100644 --- a/control.def +++ b/control.def @@ -2,6 +2,8 @@ EXPORTS init cleanup send_angle_control +send_string_control +get_distance_control get_configuration control_sentry_init control_sentry_set_app_info diff --git a/examples/display/build.sh b/examples/display/build.sh new file mode 100755 index 0000000..8dd8dd2 --- /dev/null +++ b/examples/display/build.sh @@ -0,0 +1,4 @@ +conan install . --build=missing --output-folder=build -s build_type=Release +cmake -S . -B "build" -DCMAKE_TOOLCHAIN_FILE="build/conan_toolchain.cmake" -DCMAKE_BUILD_TYPE="Release" +cmake --build "./build" --config "Release" + diff --git a/include/Module.h b/include/Module.h index 82a08b2..8942e98 100644 --- a/include/Module.h +++ b/include/Module.h @@ -45,6 +45,7 @@ class Module { // Not all modules implement all actuation/sensor values, some are no-ops virtual double get_position(); + virtual double get_distance(); virtual std::string get_text(); virtual void actuate(double x); virtual void actuate(const std::string &text); @@ -53,6 +54,7 @@ class Module { void update_module_metadata(const Messaging::TopologyMessage &message); virtual std::vector get_actuation_message() = 0; + virtual void update_sensor_data(const Flatbuffers::sensor_value &value) = 0; private: diff --git a/include/flatbuffers/SensorMessageBuilder.h b/include/flatbuffers/SensorMessageBuilder.h index 475c5f2..5f2735b 100644 --- a/include/flatbuffers/SensorMessageBuilder.h +++ b/include/flatbuffers/SensorMessageBuilder.h @@ -24,7 +24,11 @@ struct current_text { std::string text; }; -typedef std::variant sensor_value; +struct distance { + float distance; +}; + +typedef std::variant sensor_value; class SensorMessageBuilder { public: @@ -51,6 +55,10 @@ class SensorMessageBuilder { static_cast(value); return current_text{current->value()->str()}; } + case Messaging::SensorValue_Distance: { + const Messaging::Distance *current = static_cast(value); + return distance{current->value()}; + } default: return std::nullopt; } diff --git a/include/flatbuffers_generated/SensorMessage_generated.h b/include/flatbuffers_generated/SensorMessage_generated.h index 27705d5..5c5a9e4 100644 --- a/include/flatbuffers_generated/SensorMessage_generated.h +++ b/include/flatbuffers_generated/SensorMessage_generated.h @@ -1,5 +1,6 @@ // automatically generated by the FlatBuffers compiler, do not modify + #ifndef FLATBUFFERS_GENERATED_SENSORMESSAGE_MESSAGING_H_ #define FLATBUFFERS_GENERATED_SENSORMESSAGE_MESSAGING_H_ @@ -8,9 +9,9 @@ // Ensure the included flatbuffers.h is the same version as when this file was // generated, otherwise it may not be compatible. // static_assert(FLATBUFFERS_VERSION_MAJOR == 25 && -// FLATBUFFERS_VERSION_MINOR == 2 && -// FLATBUFFERS_VERSION_REVISION == 10, -// "Non-compatible flatbuffers version included"); +// FLATBUFFERS_VERSION_MINOR == 2 && +// FLATBUFFERS_VERSION_REVISION == 10, +// "Non-compatible flatbuffers version included"); namespace Messaging { @@ -23,6 +24,15 @@ struct CurrentTextBuilder; struct CurrentAngle; struct CurrentAngleBuilder; +struct Distance; +struct DistanceBuilder; + +struct Temperature; +struct TemperatureBuilder; + +struct Position; +struct PositionBuilder; + struct SensorMessage; struct SensorMessageBuilder; @@ -31,24 +41,29 @@ enum SensorValue : uint8_t { SensorValue_TargetAngle = 1, SensorValue_CurrentAngle = 2, SensorValue_CurrentText = 3, + SensorValue_Distance = 4, + SensorValue_Temperature = 5, + SensorValue_Position = 6, SensorValue_MIN = SensorValue_NONE, - SensorValue_MAX = SensorValue_CurrentText + SensorValue_MAX = SensorValue_Position }; -inline const SensorValue (&EnumValuesSensorValue())[4] { - static const SensorValue values[] = {SensorValue_NONE, SensorValue_TargetAngle, - SensorValue_CurrentAngle, SensorValue_CurrentText}; +inline const SensorValue (&EnumValuesSensorValue())[7] { + static const SensorValue values[] = {SensorValue_NONE, SensorValue_TargetAngle, + SensorValue_CurrentAngle, SensorValue_CurrentText, + SensorValue_Distance, SensorValue_Temperature, + SensorValue_Position}; return values; } inline const char *const *EnumNamesSensorValue() { - static const char *const names[5] = {"NONE", "TargetAngle", "CurrentAngle", "CurrentText", - nullptr}; + static const char *const names[8] = {"NONE", "TargetAngle", "CurrentAngle", "CurrentText", + "Distance", "Temperature", "Position", nullptr}; return names; } inline const char *EnumNameSensorValue(SensorValue e) { - if (::flatbuffers::IsOutRange(e, SensorValue_NONE, SensorValue_CurrentText)) + if (::flatbuffers::IsOutRange(e, SensorValue_NONE, SensorValue_Position)) return ""; const size_t index = static_cast(e); return EnumNamesSensorValue()[index]; @@ -70,6 +85,18 @@ template <> struct SensorValueTraits { static const SensorValue enum_value = SensorValue_CurrentText; }; +template <> struct SensorValueTraits { + static const SensorValue enum_value = SensorValue_Distance; +}; + +template <> struct SensorValueTraits { + static const SensorValue enum_value = SensorValue_Temperature; +}; + +template <> struct SensorValueTraits { + static const SensorValue enum_value = SensorValue_Position; +}; + bool VerifySensorValue(::flatbuffers::Verifier &verifier, const void *obj, SensorValue type); bool VerifySensorValueVector(::flatbuffers::Verifier &verifier, const ::flatbuffers::Vector<::flatbuffers::Offset> *values, @@ -190,6 +217,134 @@ CreateCurrentAngle(::flatbuffers::FlatBufferBuilder &_fbb, int16_t value = 0) { return builder_.Finish(); } +struct Distance FLATBUFFERS_FINAL_CLASS : private ::flatbuffers::Table { + typedef DistanceBuilder Builder; + enum FlatBuffersVTableOffset FLATBUFFERS_VTABLE_UNDERLYING_TYPE { VT_VALUE = 4 }; + float value() const { + return GetField(VT_VALUE, 0.0f); + } + bool Verify(::flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && VerifyField(verifier, VT_VALUE, 4) && + verifier.EndTable(); + } +}; + +struct DistanceBuilder { + typedef Distance Table; + ::flatbuffers::FlatBufferBuilder &fbb_; + ::flatbuffers::uoffset_t start_; + void add_value(float value) { + fbb_.AddElement(Distance::VT_VALUE, value, 0.0f); + } + explicit DistanceBuilder(::flatbuffers::FlatBufferBuilder &_fbb) : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + ::flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = ::flatbuffers::Offset(end); + return o; + } +}; + +inline ::flatbuffers::Offset CreateDistance(::flatbuffers::FlatBufferBuilder &_fbb, + float value = 0.0f) { + DistanceBuilder builder_(_fbb); + builder_.add_value(value); + return builder_.Finish(); +} + +struct Temperature FLATBUFFERS_FINAL_CLASS : private ::flatbuffers::Table { + typedef TemperatureBuilder Builder; + enum FlatBuffersVTableOffset FLATBUFFERS_VTABLE_UNDERLYING_TYPE { VT_VALUE = 4 }; + float value() const { + return GetField(VT_VALUE, 0.0f); + } + bool Verify(::flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && VerifyField(verifier, VT_VALUE, 4) && + verifier.EndTable(); + } +}; + +struct TemperatureBuilder { + typedef Temperature Table; + ::flatbuffers::FlatBufferBuilder &fbb_; + ::flatbuffers::uoffset_t start_; + void add_value(float value) { + fbb_.AddElement(Temperature::VT_VALUE, value, 0.0f); + } + explicit TemperatureBuilder(::flatbuffers::FlatBufferBuilder &_fbb) : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + ::flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = ::flatbuffers::Offset(end); + return o; + } +}; + +inline ::flatbuffers::Offset CreateTemperature(::flatbuffers::FlatBufferBuilder &_fbb, + float value = 0.0f) { + TemperatureBuilder builder_(_fbb); + builder_.add_value(value); + return builder_.Finish(); +} + +struct Position FLATBUFFERS_FINAL_CLASS : private ::flatbuffers::Table { + typedef PositionBuilder Builder; + enum FlatBuffersVTableOffset FLATBUFFERS_VTABLE_UNDERLYING_TYPE { + VT_HEADING = 4, + VT_PITCH = 6, + VT_ROLL = 8 + }; + float heading() const { + return GetField(VT_HEADING, 0.0f); + } + float pitch() const { + return GetField(VT_PITCH, 0.0f); + } + float roll() const { + return GetField(VT_ROLL, 0.0f); + } + bool Verify(::flatbuffers::Verifier &verifier) const { + return VerifyTableStart(verifier) && VerifyField(verifier, VT_HEADING, 4) && + VerifyField(verifier, VT_PITCH, 4) && + VerifyField(verifier, VT_ROLL, 4) && verifier.EndTable(); + } +}; + +struct PositionBuilder { + typedef Position Table; + ::flatbuffers::FlatBufferBuilder &fbb_; + ::flatbuffers::uoffset_t start_; + void add_heading(float heading) { + fbb_.AddElement(Position::VT_HEADING, heading, 0.0f); + } + void add_pitch(float pitch) { + fbb_.AddElement(Position::VT_PITCH, pitch, 0.0f); + } + void add_roll(float roll) { + fbb_.AddElement(Position::VT_ROLL, roll, 0.0f); + } + explicit PositionBuilder(::flatbuffers::FlatBufferBuilder &_fbb) : fbb_(_fbb) { + start_ = fbb_.StartTable(); + } + ::flatbuffers::Offset Finish() { + const auto end = fbb_.EndTable(start_); + auto o = ::flatbuffers::Offset(end); + return o; + } +}; + +inline ::flatbuffers::Offset CreatePosition(::flatbuffers::FlatBufferBuilder &_fbb, + float heading = 0.0f, float pitch = 0.0f, + float roll = 0.0f) { + PositionBuilder builder_(_fbb); + builder_.add_roll(roll); + builder_.add_pitch(pitch); + builder_.add_heading(heading); + return builder_.Finish(); +} + struct SensorMessage FLATBUFFERS_FINAL_CLASS : private ::flatbuffers::Table { typedef SensorMessageBuilder Builder; enum FlatBuffersVTableOffset FLATBUFFERS_VTABLE_UNDERLYING_TYPE { @@ -268,6 +423,18 @@ inline bool VerifySensorValue(::flatbuffers::Verifier &verifier, const void *obj auto ptr = reinterpret_cast(obj); return verifier.VerifyTable(ptr); } + case SensorValue_Distance: { + auto ptr = reinterpret_cast(obj); + return verifier.VerifyTable(ptr); + } + case SensorValue_Temperature: { + auto ptr = reinterpret_cast(obj); + return verifier.VerifyTable(ptr); + } + case SensorValue_Position: { + auto ptr = reinterpret_cast(obj); + return verifier.VerifyTable(ptr); + } default: return true; } diff --git a/include/lib_c_control.h b/include/lib_c_control.h index 493a5f4..b9f3de0 100644 --- a/include/lib_c_control.h +++ b/include/lib_c_control.h @@ -17,6 +17,8 @@ extern "C" { LIB_API void init(); 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 bool remote_call_c(uint8_t function_tag, uint8_t module, const uint8_t *params, diff --git a/include/sensors/DistanceSensor.h b/include/sensors/DistanceSensor.h new file mode 100644 index 0000000..6f392d6 --- /dev/null +++ b/include/sensors/DistanceSensor.h @@ -0,0 +1,23 @@ +#ifndef CONTROL_DISTANCESENSOR_H +#define CONTROL_DISTANCESENSOR_H + +#include "Module.h" +#include "flatbuffers/SensorMessageBuilder.h" +#include "util/Variant.h" + +class DistanceSensor : public Module { + + public: + DistanceSensor(uint8_t device_id, ModuleType type) : Module(device_id, type) { + } + + double get_distance() override; + + std::vector get_actuation_message() override; + void update_sensor_data(const Flatbuffers::sensor_value &value) override; + + private: + double m_current_distance = 0.0; +}; + +#endif // CONTROL_DISTANCESENSOR_H diff --git a/src/Module.cpp b/src/Module.cpp index b62db7e..6fbf41d 100644 --- a/src/Module.cpp +++ b/src/Module.cpp @@ -52,6 +52,11 @@ std::string Module::get_text() { return ""; } +double Module::get_distance() { + // no-op + return 0.0; +} + void Module::actuate(double /* position */) { // no-op } diff --git a/src/ModuleFactory.cpp b/src/ModuleFactory.cpp index 8eed9a7..9c418e1 100644 --- a/src/ModuleFactory.cpp +++ b/src/ModuleFactory.cpp @@ -15,6 +15,13 @@ ModuleFactory::createModule(uint8_t device_id, ModuleType type, std::shared_ptr &messaging_interface) { switch (type) { case ModuleType_SPLITTER: + case ModuleType_SPLITTER_2: + case ModuleType_SPLITTER_3: + case ModuleType_SPLITTER_4: + case ModuleType_SPLITTER_5: + case ModuleType_SPLITTER_6: + case ModuleType_SPLITTER_7: + case ModuleType_SPLITTER_8: return std::make_shared(device_id, type); case ModuleType_BATTERY: return std::make_shared(device_id, type); diff --git a/src/actuators/BoundedPositionalActuator1D.cpp b/src/actuators/BoundedPositionalActuator1D.cpp index 94b62aa..0ea75a7 100644 --- a/src/actuators/BoundedPositionalActuator1D.cpp +++ b/src/actuators/BoundedPositionalActuator1D.cpp @@ -32,10 +32,9 @@ std::vector BoundedPositionalActuator1D::get_actuation_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*/) {}, - }, + 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*/) {}, + [this](Flatbuffers::distance /*d*/) {}}, value); } diff --git a/src/actuators/OledActuator.cpp b/src/actuators/OledActuator.cpp index c4109d9..55f7b10 100644 --- a/src/actuators/OledActuator.cpp +++ b/src/actuators/OledActuator.cpp @@ -25,10 +25,9 @@ std::vector OledActuator::get_actuation_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; }, - }, + std::visit(overloaded{[this](Flatbuffers::target_angle /* a */) {}, + [this](Flatbuffers::current_angle /* a */) {}, + [this](Flatbuffers::current_text t) { m_current_text = t.text; }, + [this](Flatbuffers::distance /*d*/) {}}, value); } diff --git a/src/actuators/PositionalActuator1D.cpp b/src/actuators/PositionalActuator1D.cpp index 7de28e3..9e0342d 100644 --- a/src/actuators/PositionalActuator1D.cpp +++ b/src/actuators/PositionalActuator1D.cpp @@ -25,10 +25,10 @@ std::vector PositionalActuator1D::get_actuation_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; }, + 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); + [this](Flatbuffers::distance /*d*/) {}}, + value); } diff --git a/src/lib_c_control.cpp b/src/lib_c_control.cpp index 4604b8c..b27036f 100644 --- a/src/lib_c_control.cpp +++ b/src/lib_c_control.cpp @@ -39,6 +39,30 @@ LIB_API int send_angle_control(int module_id, int angle) { return 0; } +LIB_API int send_string_control(int module_id, const char *string) { + if (const auto maybe_module = robot_controller->getModule(module_id)) { + const auto module = (*maybe_module).lock(); + if (module) { + module->actuate(std::string(string)); + } else { + spdlog::warn("[c_control] send_angle_control: module {} has expired", module_id); + } + } + return 0; +} + +LIB_API double get_distance_control(int module_id) { + if (const auto maybe_module = robot_controller->getModule(module_id)) { + const auto module = (*maybe_module).lock(); + if (module) { + return module->get_distance(); + } else { + spdlog::warn("[c_control] send_angle_control: module {} has expired", module_id); + } + } + return 0.0; +} + LIB_API char *get_configuration(int *size_out) { std::vector modules_vec{}; std::vector connections_vec{}; diff --git a/src/sensors/DistanceSensor.cpp b/src/sensors/DistanceSensor.cpp new file mode 100644 index 0000000..a04f7b9 --- /dev/null +++ b/src/sensors/DistanceSensor.cpp @@ -0,0 +1,18 @@ + +#include "sensors/DistanceSensor.h" + +double DistanceSensor::get_distance() { + return m_current_distance; +} + +std::vector DistanceSensor::get_actuation_message() { + return {}; +} + +void DistanceSensor::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*/) {}, + [this](Flatbuffers::distance d) { m_current_distance = d.distance; }}, + value); +}