From a12bc38566e026b047207448f4a855b39b2976f9 Mon Sep 17 00:00:00 2001 From: happyCupcake Date: Wed, 27 Nov 2024 16:03:47 -0500 Subject: [PATCH] creation of drivebrain app wrapper --- CMakeLists.txt | 10 +- drivebrain_app/DriveBrainApp.cpp | 239 +++++++++++++++++++++++++++++++ drivebrain_app/DriveBrainApp.hpp | 92 ++++++++++++ 3 files changed, 340 insertions(+), 1 deletion(-) create mode 100644 drivebrain_app/DriveBrainApp.cpp create mode 100644 drivebrain_app/DriveBrainApp.hpp diff --git a/CMakeLists.txt b/CMakeLists.txt index fd4952a..5142bcb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -227,7 +227,15 @@ target_compile_features(test_vn PUBLIC cxx_std_11) ### system executable ### ### ### -add_executable(alpha_build drivebrain_app/main.cpp) +add_executable(alpha_build + drivebrain_app/main.cpp + drivebrain_app/DriveBrainApp.cpp +) + +# Add include directory for DriveBrainApp header +target_include_directories(alpha_build PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/drivebrain_app +) enable_testing() diff --git a/drivebrain_app/DriveBrainApp.cpp b/drivebrain_app/DriveBrainApp.cpp new file mode 100644 index 0000000..19b1aef --- /dev/null +++ b/drivebrain_app/DriveBrainApp.cpp @@ -0,0 +1,239 @@ +// DriveBrainApp.cpp +#include "DriveBrainApp.hpp" + +std::atomic DriveBrainApp::stop_signal_{false}; + +std::string DriveBrainApp::getParamPathFromArgs(int argc, char* argv[]) { + namespace po = boost::program_options; + po::options_description desc("Allowed options"); + std::string param_path; + + desc.add_options() + ("param-path,p", po::value(¶m_path)->default_value("config/drivebrain_config.json"), + "Path to the parameter JSON file"); + + po::variables_map vm; + po::store(po::command_line_parser(argc, argv).options(desc).allow_unregistered().run(), vm); + po::notify(vm); + + return param_path; +} + +void DriveBrainApp::parseCommandLine(int argc, char* argv[]) { + namespace po = boost::program_options; + po::options_description desc("Allowed options"); + + desc.add_options() + ("help,h", "produce help message") + ("dbc-path,d", po::value(), "Path to the DBC file (optional)"); + + po::variables_map vm; + po::store(po::command_line_parser(argc, argv).options(desc).allow_unregistered().run(), vm); + po::notify(vm); + + if (vm.count("help")) { + std::stringstream ss; + ss << desc; + spdlog::info("{}", ss.str()); + throw std::runtime_error("Help requested"); + } + + if (vm.count("dbc-path")) { + dbc_path_ = vm["dbc-path"].as(); + } +} + +DriveBrainApp::DriveBrainApp(int argc, char* argv[]) + : param_path_(getParamPathFromArgs(argc, argv)) + , logger_(core::LogLevel::INFO) + , config_(param_path_) +{ + parseCommandLine(argc, argv); + setupSignalHandler(); + spdlog::set_level(spdlog::level::warn); +} + +DriveBrainApp::~DriveBrainApp() { + stop(); +} + +void DriveBrainApp::setupSignalHandler() { + std::signal(SIGINT, [](int signal) { + spdlog::info("Interrupt signal ({}) received. Cleaning up...", signal); + DriveBrainApp::stop_signal_.store(true); + }); +} + +bool DriveBrainApp::initialize() { + mcap_logger_ = std::make_unique("temp"); + + controller_ = std::make_unique(logger_, config_); + configurable_components_.push_back(controller_.get()); + + matlab_math_ = std::make_unique( + logger_, config_, construction_failed_); + + if (construction_failed_) { + stop_signal_.store(true); + return false; + } + + configurable_components_.push_back(matlab_math_.get()); + + // live foxglove server server that relies upon having pointers to configurable components for + // getting and handling updates of the registered live parameters + foxglove_server_ = std::make_unique(configurable_components_); + + message_logger_ = std::make_shared>>( + ".mcap", true, + std::bind(&common::MCAPProtobufLogger::log_msg, std::ref(*mcap_logger_), std::placeholders::_1), + std::bind(&common::MCAPProtobufLogger::close_current_mcap, std::ref(*mcap_logger_)), + std::bind(&common::MCAPProtobufLogger::open_new_mcap, std::ref(*mcap_logger_), std::placeholders::_1), + std::bind(&core::FoxgloveWSServer::send_live_telem_msg, std::ref(*foxglove_server_), std::placeholders::_1)); + + state_estimator_ = std::make_unique(logger_, message_logger_, *matlab_math_); + + driver_ = std::make_unique(config_, logger_, message_logger_, tx_queue_, io_context_, dbc_path_, construction_failed_, *state_estimator_); + + if (construction_failed_) { + stop_signal_.store(true); + return false; + } + + configurable_components_.push_back(driver_.get()); + + eth_driver_ = std::make_unique(logger_, eth_tx_queue_, message_logger_, *state_estimator_,io_context_, "192.168.1.30", 2001, 2000); + + vn_driver_ = std::make_unique(config_, logger_, message_logger_, *state_estimator_, io_context_); + + db_service_ = std::make_unique(message_logger_); + + // required init, maybe want to call this in the constructor instead + bool successful_controller_init = controller_->init(); + + return true; +} + +void DriveBrainApp::processLoop() { + auto out_msg = std::make_shared(); + auto loop_time = controller_->get_dt_sec(); + auto loop_time_micros = (int)(loop_time * 1000000.0f); + std::chrono::microseconds loop_chrono_time(loop_time_micros); + + while (!stop_signal_.load()) { + auto start_time = std::chrono::high_resolution_clock::now(); + // samples internal state set by the handle recv functions + + auto state_and_validity = state_estimator_->get_latest_state_and_validity(); + auto out_struct = controller_->step_controller(state_and_validity.first); + auto temp_desired_torques = state_and_validity.first.matlab_math_temp_out; + // feedback + state_estimator_->set_previous_control_output(out_struct); + // output + // if(state_and_validity.second) + // { + out_msg->set_prev_mcu_recv_millis(out_struct.mcu_recv_millis); + + if(temp_desired_torques.res_torque_lim_nm.FL < 0) { + out_msg->mutable_desired_rpms()->set_fl(0); + } else { + out_msg->mutable_desired_rpms()->set_fl(out_struct.desired_rpms.FL); + } + + if(temp_desired_torques.res_torque_lim_nm.FR < 0) { + out_msg->mutable_desired_rpms()->set_fr(0); + } else { + out_msg->mutable_desired_rpms()->set_fr(out_struct.desired_rpms.FR); + } + + if(temp_desired_torques.res_torque_lim_nm.RL < 0) { + out_msg->mutable_desired_rpms()->set_rl(0); + } else { + out_msg->mutable_desired_rpms()->set_rl(out_struct.desired_rpms.RL); + } + + if(temp_desired_torques.res_torque_lim_nm.RR < 0) { + out_msg->mutable_desired_rpms()->set_rr(0); + } else { + out_msg->mutable_desired_rpms()->set_rr(out_struct.desired_rpms.RR); + } + + out_msg->mutable_torque_limit_nm()->set_fl(::abs(temp_desired_torques.res_torque_lim_nm.FL)); + out_msg->mutable_torque_limit_nm()->set_fr(::abs(temp_desired_torques.res_torque_lim_nm.FR)); + out_msg->mutable_torque_limit_nm()->set_rl(::abs(temp_desired_torques.res_torque_lim_nm.RL)); + out_msg->mutable_torque_limit_nm()->set_rr(::abs(temp_desired_torques.res_torque_lim_nm.RR)); + + { + std::unique_lock lk(eth_tx_queue_.mtx); + eth_tx_queue_.deque.push_back(out_msg); + eth_tx_queue_.cv.notify_all(); + } + + auto end_time = std::chrono::high_resolution_clock::now(); + auto elapsed = std::chrono::duration_cast(end_time - start_time); + + if (loop_chrono_time > elapsed) { + std::this_thread::sleep_for(loop_chrono_time - elapsed); + } + } +} + +void DriveBrainApp::startThreads() { + db_service_thread_ = std::make_unique([this]() { + spdlog::info("started db service thread"); + try { + while (!stop_signal_.load()) { + // Run the io_context as long as stop_signal is false + db_service_->run_server(); // Run at least one handler, or return immediately if none + } + } catch (const std::exception& e) { + spdlog::error("Error in drivebrain service thread: {}", e.what()); + } + }); + + // 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 + + io_context_thread_ = std::make_unique([this]() { + spdlog::info("Started io context thread"); + try { + io_context_.run(); + } catch (const std::exception& e) { + spdlog::error("Error in io_context: {}", e.what()); + } + }); + + process_thread_ = std::make_unique([this]() { processLoop(); }); +} + +void DriveBrainApp::run() { + startThreads(); + + while (!stop_signal_.load()) { + std::this_thread::sleep_for(std::chrono::seconds(1)); + } + + stop(); +} + +void DriveBrainApp::stop() { + stop_signal_.store(true); + + if (process_thread_ && process_thread_->joinable()) { + process_thread_->join(); + } + spdlog::info("joined main process"); + io_context_.stop(); + if (io_context_thread_ && io_context_thread_->joinable()) { + io_context_thread_->join(); + } + + if (db_service_) { + db_service_->stop_server(); + } + if (db_service_thread_ && db_service_thread_->joinable()) { + db_service_thread_->join(); + } + spdlog::info("joined io context"); +} \ No newline at end of file diff --git a/drivebrain_app/DriveBrainApp.hpp b/drivebrain_app/DriveBrainApp.hpp new file mode 100644 index 0000000..68843a2 --- /dev/null +++ b/drivebrain_app/DriveBrainApp.hpp @@ -0,0 +1,92 @@ +// DriveBrainApp.hpp +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include // std::this_thread::sleep_for +#include // std::chrono::seconds +#include + +#include + + +#include +#include + + +#include +#include + +#include +#include +#include +#include + +#include +#include + + + +class DriveBrainApp { +public: + DriveBrainApp(int argc, char* argv[]); + ~DriveBrainApp(); + + bool initialize(); + void run(); + void stop(); + +private: + + std::string getParamPathFromArgs(int argc, char* argv[]); + void parseCommandLine(int argc, char* argv[]); + void setupSignalHandler(); + void startThreads(); + void processLoop(); + void ioContextLoop(); + void dbServiceLoop(); + + + const std::string param_path_; + core::Logger logger_; + core::JsonFileHandler config_; + std::optional dbc_path_; + boost::asio::io_context io_context_; + static std::atomic stop_signal_; + bool construction_failed_{false}; + + + core::common::ThreadSafeDeque> rx_queue_; + core::common::ThreadSafeDeque> tx_queue_; + core::common::ThreadSafeDeque> eth_tx_queue_; + core::common::ThreadSafeDeque> live_telem_queue_; + + + std::vector configurable_components_; + std::unique_ptr mcap_logger_; + std::unique_ptr controller_; + std::unique_ptr matlab_math_; + std::unique_ptr foxglove_server_; + std::shared_ptr>> message_logger_; + std::unique_ptr state_estimator_; + std::unique_ptr driver_; + std::unique_ptr eth_driver_; + std::unique_ptr vn_driver_; + std::unique_ptr db_service_; + + + std::unique_ptr process_thread_; + std::unique_ptr io_context_thread_; + std::unique_ptr db_service_thread_; +};