Merge pull request #1 from hytech-racing/feature/param_integration

Feature/param integration

.gitignore

@@ -1,4 +1,6 @@
 .vscode/
 result
 result/
-build/
+build/
+result-dev
+result-dev/

CMakeLists.txt

@@ -28,7 +28,7 @@ include(create_package)
 #####################
 
 # drivebrain core library
-add_library(drivebrain_core SHARED drivebrain_core_base/src/JsonFileHandler.cpp)
+add_library(drivebrain_core SHARED drivebrain_core_base/src/JsonFileHandler.cpp drivebrain_core_base/src/Configurable.cpp)
 
 target_include_directories(drivebrain_core PUBLIC 
     $<INSTALL_INTERFACE:drivebrain_core_base/include/public>
@@ -59,6 +59,7 @@ target_link_libraries(foxglove_param_server PUBLIC
     foxglove-schemas_proto_cpp::foxglove-schemas_proto_cpp 
     foxglove_websocket::foxglove_websocket    
 )
+target_compile_features(foxglove_param_server PUBLIC cxx_std_17) 
 make_cmake_package(foxglove_param_server drivebrain)
 
 # CAN driver for parsing and encoding CAN packets and interacting with a socketCAN interface
@@ -123,10 +124,10 @@ target_link_libraries(test_param_server PUBLIC
 add_executable(alpha_build drivebrain_app/main.cpp)
 
 target_link_libraries(alpha_build PUBLIC
-    Boost::boost
     drivebrain_core
     drivebrain_control
     CAN_driver
+    foxglove_param_server
 )
 
 include(GNUInstallDirs)

README.md

@@ -29,22 +29,27 @@ idea: if we have the ability to go both ways for having the ability to both:
 and generated code for the data passing into and out of proto messages, then we can book-end the controllers and estimators. 
 
 ### release planning
+
+#### release notes:
+- verified that we can have both drivebrain and the data_acq bound to the same CAN device and receiving / talking over it
+    - due to this, we dont need to have the live telem in the alpha since we will only be using CAN still for this release for all comms
+
 alpha feature set (~2 weeks, 16 days):
-- [ ] basic controller library (9 days) (first pass I want to try out different types of regen handling)
-    - [ ] generic controller (2 days)
-    - [ ] live parameter controller interface (2 days, 50%)
+- [ ] basic controller library (7 days) (first pass I want to try out different types of regen handling)
+    - [x] live parameter controller interface (2 days, 50%)
+    - [x] simple controller business logic (1 day)
     - [ ] controller manager structure (2 days)
     - [ ] controller manager runtime (2 days)
-    - [ ] controller business logic (1 day)
 - [ ] CAN MCU driver library (4 days)
     - [x] DBC based parsing 
-    - [ ] async receiving and transmitting with Boost.Asio (2 days)
-    - [ ] protobuf message packing (2 days, 50% done, 1 day left)
-    - [ ] simple internal communication with basic controller (1 day)
-- [ ] application runtime (1 day)
-- [ ] live telem integration with foxglove websocket (1 day)
-- [ ] foxglove live parameter server and websocket integration (1 day)
-
+    - [x] async receiving and transmitting with Boost.Asio (2 days)
+    - [x] protobuf message packing (2 days)
+    - [x] simple internal communication with basic controller (1 day)
+    - [ ] make the DBC parser also be able to handle enums 
+- [x] application runtime (1 day)
+- [x] foxglove live parameter server and websocket integration (1 day)
+- [ ] improve protobuf generation from DBC by supporting enums properly (1 day)
+    - tied to making the DBC parser also able to handle enums
 beta feature set (1 week):
 - [ ] vectornav UART driver integration
 - [ ] CASE integrated into controller manager with existing integration methods (1 day)

config/test_config/can_driver.json

@@ -1,11 +1,13 @@
+
 {
     "CANDriver": {
         "canbus_device": "vcan0",
         "path_to_dbc": "config/test_config/hytech.dbc"
     },
     "SimpleController": {
         "max_torque": 22.4,
-        "max_regen_torque": 10.0
-
+        "max_regen_torque": 10.0,
+        "rear_torque_scale": 1.0, 
+        "regen_torque_scale": 0.6
     }
 }

drivebrain_app/main.cpp

@@ -3,58 +3,60 @@
 #include <SimpleController.hpp>
 
 #include <DrivebrainBase.hpp>
+#include <param_server.hpp>
 
 #include <thread> // std::this_thread::sleep_for
 #include <chrono> // std::chrono::seconds
 #include <condition_variable>
 
 #include <cassert>
 
-#define assertm(exp, msg) assert(((void)msg, exp))
-
 #include <boost/asio.hpp>
 #include <memory>
 // TODO first application will have
 
-// - [ ] driver bus message queue https://chatgpt.com/share/03297278-0346-40ba-8ce7-7a75e919ee5c
-// - [ ] CAN driver
+// - [x] message queue that can send messages between the CAN driver and the controller
+// - [x] CAN driver that can receive the pedals messages
+// - [ ] fix the CAN messages that cant currently be encoded into the protobuf messages
 // - [x] simple controller
 
-// - [ ] message queue manager for ensuring that everything is getting the data it needs
-// - [ ] foxglove live telem and parameter server
-
 int main()
 {
+
     boost::asio::io_context io_context;
     core::common::ThreadSafeDeque<std::shared_ptr<google::protobuf::Message>> rx_queue;
     core::common::ThreadSafeDeque<std::shared_ptr<google::protobuf::Message>> tx_queue;
+    std::vector<core::common::Configurable *> configurable_components;
+
     core::JsonFileHandler config("config/test_config/can_driver.json");
+
     comms::CANDriver driver(config, tx_queue, rx_queue, io_context);
+    std::cout << "driver init " << driver.init() << std::endl;
+    configurable_components.push_back(&driver);
 
-    // assertm(driver.init(), "ERROR: driver did not initialize");
+    control::SimpleController controller(config);
+    configurable_components.push_back(&controller);
 
-    std::cout <<"driver init "<< driver.init() << std::endl;
+    auto param_server = core::FoxgloveParameterServer(configurable_components);
 
-    control::SimpleController controller(config);
     auto _ = controller.init();
-    // assertm(controller.init(), "ERROR: controler did not initialize");
+
     // what we will do here is have a temporary super-loop.
     // in this thread we will block on having anything in the rx queue, everything by default goes into the foxglove server (TODO)
     // if we receive the pedals message, we step the controller and get its output to put intot he tx queue
     std::thread io_context_thread([&io_context]()
-                                  { 
-                                    std::cout <<"started io context thread" <<std::endl;
-                                    io_context.run(); });
-    std::thread receive_thread([&rx_queue, &tx_queue, &controller]()
-                               {
+    {
+        std::cout <<"started io context thread" <<std::endl;
+        io_context.run();
+    });
 
-
+    std::thread receive_thread([&rx_queue, &tx_queue, &controller]()
+    {
+        auto to_send = std::make_shared<drivetrain_command>();        
         while(true)
         {
-            // std::cout <<"started recv thread" <<std::endl;
             std::shared_ptr<google::protobuf::Message> input_msg;
             {
-                // std::cout <<"waiting on rx"<<std::endl;
                 std::unique_lock lk(rx_queue.mtx);
                 rx_queue.cv.wait(lk, [&rx_queue]()
                                         { return !rx_queue.deque.empty(); });
@@ -66,9 +68,7 @@ int main()
             if(input_msg->GetTypeName() == "mcu_pedal_readings")
             {
                 auto in_msg = std::static_pointer_cast<mcu_pedal_readings>(input_msg);
-                auto to_send = std::make_shared<drivetrain_command>();
                 to_send->CopyFrom(controller.step_controller(*in_msg));
-
                 {
                     std::unique_lock lk(tx_queue.mtx);
                     tx_queue.deque.push_back(to_send);
@@ -78,13 +78,11 @@ int main()
             } else {
                 std::cout << input_msg->GetTypeName() <<std::endl;
             }
-            
-        } });
+        }
+    });
 
-    // io_context.run();
     while (true)
     {
-        // std::cout <<"started main thread" <<std::endl;
         std::this_thread::sleep_for(std::chrono::seconds(1));
     }
 }

drivebrain_core_base/include/public/Configurable.hpp

@@ -1,15 +1,18 @@
 #pragma once
 
-// TODO make this private 
+// TODO make this private
 #include <JsonFileHandler.hpp>
 
+#include <boost/signals2.hpp>
+
 #include <string>
 #include <iostream>
 #include <functional>
 #include <mutex>
 #include <unordered_map>
 #include <optional>
 #include <variant>
+#include <mutex>
 
 // STORY:
 
@@ -22,11 +25,11 @@
 
 // REQUIREMENTS:
 
-// - [ ] configuration should be nested in the following way:
+// - [x] configuration should be nested in the following way:
 // 1. each component has its own single layer scope
 // 2. each scope doesnt have any further scope (one layer deep only)
-// - [ ] all parameter value getting has to return
-// - [ ] there will only be ONE config file being edited by ONE thing at a time
+// - [x] all parameter value getting has to return
+// - [x] there will only be ONE config file being edited by ONE thing at a time
 // this is pertinent to the parameter server for saving the parameters, for accessing at init time we will initialize before kicking off threads
 
 // this is also pertinent to construction of the core of drivebrain since we want the components to be able to update the in-memory version of the json, but only one thing should be able to write it out to the json file once updated
@@ -35,29 +38,87 @@
 // -> this would be hard to police and ensure that the runtime calling only gets called during runtime
 
 // what if we made all the parameter handling be within a specific function for each component
-    // then the initial init can call the same set param function that gets called at init time 
+// then the initial init can call the same set param function that gets called at init time
 
+// live parameters:
+// - [ ] add live parameter handling through use of boost signals
+
+// the live parameter settings will be handled by having a true or false flag within the get_parameter.
+// if this flag is set, the is gotten from the config file and then the map of live parameters gets it's
 namespace core
 {
     namespace common
     {
-        /// @brief this is the class that configurable components inherit from to get parameter access to the top-level ptree
+        /// @brief this is the (partially virtual) class that configurable components inherit from to get parameter access to the top-level ptree
         class Configurable
         {
+
         public:
+            using ParamTypes = std::variant<bool, int, float, double, std::string, std::monostate>;
+
+            /// @brief constructor for base class
+            /// @param json_file_handler the referrence to the json file loaded in main
+            /// @param component_name name of the component (required to be unique)
             Configurable(core::JsonFileHandler &json_file_handler, const std::string &component_name)
                 : _json_file_handler(json_file_handler), _component_name(component_name) {}
 
+            /// @brief getter for name
+            /// @return name of the component
+            std::string get_name();
+
+            /// @brief gets the names of the parameters for this component
+            /// @return vector of names
+            std::vector<std::string> get_param_names();
+
+            /// @brief gets the map of param names with all param values
+            /// @return unordered map of names and vals (variant)
+            std::unordered_map<std::string, ParamTypes> get_params_map();
+
+            /// @brief external function signature for use by the parameter server for handling the parameter updates. calls the user-implemented boost signal
+            /// @param key the id of the parameter that should be contained within the param map
+            /// @param param_val the parameter value to change to 
+            void handle_live_param_update(const std::string &key, ParamTypes param_val);
+
+            // TODO renamd id to key to stay consistent with naming, also switch to const ref
+
+            /// @brief getter for param value at specified id
+            /// @param id map key for parameter within map
+            /// @return param value
+            Configurable::ParamTypes get_cached_param(std::string id);
+
         protected:
+            /// @brief boost signal that the user is expected to connect their parameter update handler function for changing their internal parameter values
+            boost::signals2::signal<void(const std::unordered_map<std::string, ParamTypes> &)> param_update_handler_sig;
+
+            /// @brief virtual init function that has to be implemented. it is expected that the use puts their getters for live / "static" parameters within this function
+            /// @return false if not all params found that were expected, true if all params were good. other initialization code can be included not pertaining to configuration base class as well
             virtual bool init() = 0;
-            using ParamTypes = std::variant<bool, int, float, std::string>;
+
+
+            // TODO look into making this private
+
+            /// @brief internal type checker to ensure that param types are what they are only what we support
+            /// @tparam ParamType the desired parameter type
+            template <typename ParamType>
+            void _handle_assert()
+            {
+                static_assert(
+                    std::is_same_v<ParamType, bool> ||
+                        std::is_same_v<ParamType, int> ||
+                        std::is_same_v<ParamType, double> ||
+                        std::is_same_v<ParamType, float> ||
+                        std::is_same_v<ParamType, std::string>,
+                    "ParamType must be bool, int, double, float, or std::string");
+            }
             /// @brief Gets a parameter value within the component's scope, ensuring it exists with a default value and if it doesnt it will created it
             /// @tparam ParamType parameter type
             /// @param key the id of the parameter being requested
-            /// @return the optional config value
+            /// @return the optional config value, std::nullopt if not found
             template <typename ParamType>
             std::optional<ParamType> get_parameter_value(const std::string &key)
             {
+                _handle_assert<ParamType>();
+
                 // TODO assert that the template type is only of the specific types supported by nlohmann's json lib
                 auto &config = _json_file_handler.get_config();
 
@@ -71,37 +132,46 @@ namespace core
                 // Access the specific key within the component's section
                 if (!config[_component_name].contains(key))
                 {
-                    std::cout << "WARNING: config file does not contain config: "<< key << " for component: " << _component_name << std::endl;
+                    std::cout << "WARNING: config file does not contain config: " << key << " for component: " << _component_name << std::endl;
                     return std::nullopt;
                 }
                 return config[_component_name][key].get<ParamType>();
             }
 
-            /// @brief TODO: component-scoped parameter setting call. this will be getting called by the parameter server within drivebrain
-            /// @param key 
-            /// @param parameter_val 
-            /// @return true or false depending on if the key exists or not
-            bool handle_update_parameter(const std::string &key, std::variant<bool, int, float, std::string> parameter_val)
+
+            /// @brief same as the @ref get_parameter_value function, however it also registers the parameter to the internal live parameter map
+            /// @tparam ParamType the parameter type
+            /// @param key  the id of the parameter being requested
+            /// @return the optional config value, std::nullopt if not found
+            template <typename ParamType>
+            std::optional<ParamType> get_live_parameter(const std::string &key)
             {
-                auto &config = _json_file_handler.get_config();
-                if (config[_component_name].contains(key))
+                _handle_assert<ParamType>();
+                auto res = get_parameter_value<ParamType>(key);
+
+                if (!res)
                 {
-                    // config[_component_name][key] = parameter_val;
-                    // set_parameter(key, parameter_value);
-                    return true;
+                    return std::nullopt;
                 }
-                return false;
+                else
+                {
+                    {
+                        std::unique_lock lk(_live_params.mtx);
+                        _live_params.param_vals[key] = *res;
+                    }
+                }
+                return res;
             }
 
-            // /// @brief TODO: this is the handler that each component must implement that can take in the parameter ID
-            // /// @tparam ParamType 
-            // /// @param key 
-            // /// @param param_value 
-            // virtual void set_parameter(const std::string &key, std::variant<bool, int, float, std::string> param_value) = 0;
-
         private:
             std::string _component_name;
             core::JsonFileHandler &_json_file_handler;
+
+            struct
+            {
+                std::unordered_map<std::string, ParamTypes> param_vals;
+                std::mutex mtx;
+            } _live_params;
         };
 
     }