diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 4017782..64ae09c 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -1,23 +1,23 @@ -{ - "name": "Drivebrain dev container", - "dockerComposeFile": [ - "${localWorkspaceFolder}/docker-compose.yml" - ], - "service": "dev", - "workspaceFolder": "/app", - "shutdownAction": "stopCompose", - "customizations": { - "vscode": { - "extensions": [ - "ms-vscode.cpptools", - "ms-vscode.cmake-tools", - "twxs.cmake", - "ms-python.python", - "jeff-hykin.better-cpp-syntax", - "llvm-vs-code-extensions.vscode-clangd" - ] - } - }, - - "remoteUser": "root" -} +{ + "name": "Drivebrain dev container", + "dockerComposeFile": [ + "${localWorkspaceFolder}/docker-compose.yml" + ], + "service": "dev", + "workspaceFolder": "/app", + "shutdownAction": "stopCompose", + "customizations": { + "vscode": { + "extensions": [ + "ms-vscode.cpptools", + "ms-vscode.cmake-tools", + "twxs.cmake", + "ms-python.python", + "jeff-hykin.better-cpp-syntax", + "llvm-vs-code-extensions.vscode-clangd" + ] + } + }, + + "remoteUser": "root" +} diff --git a/.github/workflows/build_and_test.yml b/.github/workflows/build_and_test.yml index 870810d..9c4cc2f 100644 --- a/.github/workflows/build_and_test.yml +++ b/.github/workflows/build_and_test.yml @@ -1,33 +1,33 @@ -name: Build and publish image to Docker Hub -on: - push - -jobs: - publish-image: - runs-on: ubuntu-latest - steps: - - name: Checkout - uses: actions/checkout@v4 - - - name: Build image - run: docker build -t kkittur/drivebrain_cross_compile:latest . - - - name: push image to docker Hub - run: | - docker login -u kkittur -p ${{ secrets.DOCKER_HUB_TOKEN }} - docker push kkittur/drivebrain_cross_compile:latest - - build-executable: - needs: publish-image - runs-on: ubuntu-latest - steps: - - name: Checkout - uses: actions/checkout@v4 - - - name: Pull from container registry - run: | - docker pull kkittur/drivebrain_cross_compile:latest - - - name: Enter container and build - run: | - docker compose run --rm dev ./build_script.sh +name: Build and publish image to Docker Hub +on: + push + +jobs: + publish-image: + runs-on: ubuntu-latest + steps: + - name: Checkout + uses: actions/checkout@v4 + + - name: Build image + run: docker build -t kkittur/drivebrain_cross_compile:latest . + + - name: push image to docker Hub + run: | + docker login -u kkittur -p ${{ secrets.DOCKER_HUB_TOKEN }} + docker push kkittur/drivebrain_cross_compile:latest + + build-executable: + needs: publish-image + runs-on: ubuntu-latest + steps: + - name: Checkout + uses: actions/checkout@v4 + + - name: Pull from container registry + run: | + docker pull kkittur/drivebrain_cross_compile:latest + + - name: Enter container and build + run: | + docker compose run --rm dev ./build_script.sh diff --git a/.gitignore b/.gitignore index e316b4c..b5cccba 100644 --- a/.gitignore +++ b/.gitignore @@ -1,19 +1,19 @@ -.venv -.venv/ - -cmake -cmake/ - -.vscode -.vscode/ - -build -build/ - -build-* - -.vscode -.vscode/ - -.cache -compile_commands.json +.venv +.venv/ + +cmake +cmake/ + +.vscode +.vscode/ + +build +build/ + +build-* + +.vscode +.vscode/ + +.cache +compile_commands.json diff --git a/CMakeLists.txt b/CMakeLists.txt index 5b85247..874668d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,75 +1,74 @@ -# cmake_minimum_required(VERSION 3.10) -set(CMAKE_CXX_STANDARD 17) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -project(drivebrain_software_2026) - -include(FetchContent) -list(APPEND CMAKE_PREFIX_PATH "${CMAKE_SOURCE_DIR}/cmake") - -################################# -# Custom libs -################################# -add_subdirectory(drivebrain_core) -add_subdirectory(drivebrain_comms) - -################################# -# Find packages -################################# -find_package(foxglove-websocket REQUIRED) -find_package(Protobuf REQUIRED CONFIG) -find_package(Boost REQUIRED) -find_package(spdlog REQUIRED) -find_package(mcap REQUIRED) -find_package(GTest REQUIRED) - -################################# -# Upstreams (non-conan) -################################# -FetchContent_Declare( - HT_Proto - GIT_REPOSITORY https://github.com/hytech-racing/HT_proto.git - GIT_TAG 5f61368 -) -FetchContent_MakeAvailable(HT_Proto) - -FetchContent_Declare( - vn_driver_lib - GIT_REPOSITORY https://github.com/hytech-racing/vn_driver_lib.git - GIT_TAG 0d40803 -) -FetchContent_MakeAvailable(vn_driver_lib) - -FetchContent_Declare( - HT_CAN - URL "https://github.com/hytech-racing/HT_CAN/releases/download/231/drivebrain_can_lib.tar.gz" -) -FetchContent_MakeAvailable(HT_CAN) - -################################# -# Linking to main app -################################# -add_executable(drivebrain - drivebrain_app/src/main.cpp -) - -target_include_directories(drivebrain PUBLIC drivebrain_app/include) - -target_link_libraries(drivebrain PUBLIC - foxglove-websocket::foxglove-websocket - mcap::mcap - protobuf::libprotobuf - boost::boost - hytech_msgs_cpp_lib - hytech_can_msgs_cpp_lib - drivebrain_core - drivebrain_comms -) - -################################# -# Unit Testing -################################# - -if(NOT CMAKE_CROSSCOMPILING) - enable_testing() - add_subdirectory(tests) -endif() +cmake_minimum_required(VERSION 3.10) +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +project(drivebrain_software_2026) + +include(FetchContent) +list(APPEND CMAKE_PREFIX_PATH "${CMAKE_SOURCE_DIR}/cmake") + +################################# +# Custom libs +################################# +add_subdirectory(drivebrain_core) +# add_subdirectory(drivebrain_comms) + +################################# +# Find packages +################################# +find_package(foxglove-websocket REQUIRED) +find_package(Protobuf REQUIRED CONFIG) +find_package(Boost REQUIRED) +find_package(spdlog REQUIRED) +find_package(mcap REQUIRED) +find_package(GTest REQUIRED) + +################################# +# Upstreams (non-conan) +################################# +FetchContent_Declare( + HT_Proto + GIT_REPOSITORY https://github.com/hytech-racing/HT_proto.git + GIT_TAG cfbf674c50278cb0bb91f1a51b95e0cabe284cbf +) +FetchContent_MakeAvailable(HT_Proto) + +FetchContent_Declare( + vn_driver_lib + GIT_REPOSITORY https://github.com/hytech-racing/vn_driver_lib.git + GIT_TAG 0d40803 +) +FetchContent_MakeAvailable(vn_driver_lib) + +FetchContent_Declare( + HT_CAN + URL "https://github.com/hytech-racing/HT_CAN/releases/download/231/drivebrain_can_lib.tar.gz" +) +FetchContent_MakeAvailable(HT_CAN) + +################################# +# Linking to main app +################################# +add_executable(drivebrain + drivebrain_app/src/main.cpp +) + +target_include_directories(drivebrain PUBLIC drivebrain_app/include) + +target_link_libraries(drivebrain PUBLIC + foxglove-websocket::foxglove-websocket + mcap::mcap + protobuf::libprotobuf + boost::boost + hytech_msgs_cpp_lib + hytech_can_msgs_cpp_lib + drivebrain_core +) + +################################# +# Unit Testing +################################# + +if(NOT CMAKE_CROSSCOMPILING) + enable_testing() + add_subdirectory(tests) +endif() diff --git a/Dockerfile b/Dockerfile index 303b22b..17c39c4 100644 --- a/Dockerfile +++ b/Dockerfile @@ -1,21 +1,21 @@ -FROM ubuntu:22.04 - -ENV DEBIAN_FRONTEND=noninteractive - -RUN apt-get update && apt-get install -y \ - gcc-aarch64-linux-gnu \ - g++-aarch64-linux-gnu \ - make \ - cmake \ - build-essential \ - git \ - wget \ - vim \ - pkg-config \ - python3 \ - python3-pip \ - python3-setuptools \ - python3-venv \ - && apt-get clean - +FROM ubuntu:22.04 + +ENV DEBIAN_FRONTEND=noninteractive + +RUN apt-get update && apt-get install -y \ + gcc-aarch64-linux-gnu \ + g++-aarch64-linux-gnu \ + make \ + cmake \ + build-essential \ + git \ + wget \ + vim \ + pkg-config \ + python3 \ + python3-pip \ + python3-setuptools \ + python3-venv \ + && apt-get clean + CMD ["/bin/bash"] \ No newline at end of file diff --git a/README.md b/README.md index e029086..23f15cd 100644 --- a/README.md +++ b/README.md @@ -1,61 +1,61 @@ -# DriveBrain Software 2026 - -This guide explains how to build the DriveBrain Software 2026 using the provided Docker cross-compilation environment. - -## Prerequisites - -* Docker installed on your system -* Local clone of the DriveBrain repository - -## Building the Software - -## 0. Build the dockerfile (you only need to do this once) -Run the following command to build the dockerfile: -``` -docker build -t kkittur/drivebrain_cross_compile . -``` -It may take a few minutes. - -### 1. Enter the Docker Container - -Run the following command to start and enter the Docker container with the necessary volumes mounted: - -```bash -docker compose run dev -``` - -Alternatively, if you want to enter a modified container for whatever reason, you can run: -```bash -# running with modified memory and cpu limits -docker run -it \ - --mount type=volume,source=conan_cache,target=/root/.conan2 \ - --mount type=bind,source="$(pwd)",target=/app \ - --memory=16g \ - --cpus=4 \ - -w /app \ - kkittur/drivebrain_cross_compile /bin/bash -``` -and pass in whatever flags you see fit. - -### 2. Run the Build Script - -Inside the container, execute: - -```bash -./build_script.sh -``` - -This script will build the DriveBrain software using the cross-compilation toolchain for the target platform. -The first time you run it, it will take some time because it needs to compile all the dependencies. However, future builds past the first one take <1 minute because dependencies are cached. - -### Unit Tests -Unit tests have to be compiled to your native architecture if you actually want to see them run. To do this, you can pass in the `--test` flag in the build script. -```bash -./build_script.sh --test -``` -and the project along with its unit tests will be compiled and ran. - -### Notes -* Make sure the build script has execute permissions: `chmod +x build_script.sh`. -* The build artifacts will be generated inside an autogenerated folder that's handled by Docker. You can access these artifacts for debugging by using `docker volume inspect`. - +# DriveBrain Software 2026 + +This guide explains how to build the DriveBrain Software 2026 using the provided Docker cross-compilation environment. + +## Prerequisites + +* Docker installed on your system +* Local clone of the DriveBrain repository + +## Building the Software + +## 0. Build the dockerfile (you only need to do this once) +Run the following command to build the dockerfile: +``` +docker build -t kkittur/drivebrain_cross_compile . +``` +It may take a few minutes. + +### 1. Enter the Docker Container + +Run the following command to start and enter the Docker container with the necessary volumes mounted: + +```bash +docker compose run dev +``` + +Alternatively, if you want to enter a modified container for whatever reason, you can run: +```bash +# running with modified memory and cpu limits +docker run -it \ + --mount type=volume,source=conan_cache,target=/root/.conan2 \ + --mount type=bind,source="$(pwd)",target=/app \ + --memory=16g \ + --cpus=4 \ + -w /app \ + kkittur/drivebrain_cross_compile /bin/bash +``` +and pass in whatever flags you see fit. + +### 2. Run the Build Script + +Inside the container, execute: + +```bash +./build_script.sh +``` + +This script will build the DriveBrain software using the cross-compilation toolchain for the target platform. +The first time you run it, it will take some time because it needs to compile all the dependencies. However, future builds past the first one take <1 minute because dependencies are cached. + +### Unit Tests +Unit tests have to be compiled to your native architecture if you actually want to see them run. To do this, you can pass in the `--test` flag in the build script. +```bash +./build_script.sh --test +``` +and the project along with its unit tests will be compiled and ran. + +### Notes +* Make sure the build script has execute permissions: `chmod +x build_script.sh`. +* The build artifacts will be generated inside an autogenerated folder that's handled by Docker. You can access these artifacts for debugging by using `docker volume inspect`. + diff --git a/conanfile.py b/conanfile.py index 49eda6c..a8edd7b 100644 --- a/conanfile.py +++ b/conanfile.py @@ -1,30 +1,30 @@ -from conan import ConanFile -from conan.tools.cmake import CMakeToolchain, CMake, cmake_layout -from os import path - -class DrivebrainSoftware(ConanFile): - name = "_foxglove" - version = "1.0.0-dev" - license = "zlib" - settings = ["os", "compiler", "build_type", "arch"] - exports_sources = "CMakeLists.txt", "LICENSE", "src/*", "include/*" - generators = "CMakeDeps", "CMakeToolchain" - exports = '*' - - - def build(self): - cmake = CMake(self) - cmake.build() - cmake.install() - - def requirements(self): - self.requires("foxglove-websocket/1.4.0", transitive_headers=True) - self.requires("protobuf/5.29.3", transitive_headers=True) - self.requires("boost/1.88.0") - self.requires("spdlog/1.15.3") - self.requires("mcap/2.0.2") - - def build_requirements(self): - if not self.settings_build.get_safe("cross_build"): - self.requires("gtest/1.17.0") - self.tool_requires("protobuf/5.29.3") +from conan import ConanFile +from conan.tools.cmake import CMakeToolchain, CMake, cmake_layout +from os import path + +class DrivebrainSoftware(ConanFile): + name = "_foxglove" + version = "1.0.0-dev" + license = "zlib" + settings = ["os", "compiler", "build_type", "arch"] + exports_sources = "CMakeLists.txt", "LICENSE", "src/*", "include/*" + generators = "CMakeDeps", "CMakeToolchain" + exports = '*' + + + def build(self): + cmake = CMake(self) + cmake.build() + cmake.install() + + def requirements(self): + self.requires("foxglove-websocket/1.4.0", transitive_headers=True) + self.requires("protobuf/5.29.3", transitive_headers=True) + self.requires("boost/1.88.0") + self.requires("spdlog/1.15.3") + self.requires("mcap/2.0.2") + + def build_requirements(self): + if not self.settings_build.get_safe("cross_build"): + self.requires("gtest/1.17.0") + self.tool_requires("protobuf/5.29.3") diff --git a/config/config.json b/config/config.json index f2bc932..82f2efc 100644 --- a/config/config.json +++ b/config/config.json @@ -1,5 +1,5 @@ -{ - "test_one": true, - "test_two": 23094829034.0, - "test_three": 123 +{ + "test_one": true, + "test_two": 23094829034.0, + "test_three": 123 } \ No newline at end of file diff --git a/deploy_script.sh b/deploy_script.sh index 65fb159..e7c5234 100755 --- a/deploy_script.sh +++ b/deploy_script.sh @@ -1,2 +1,2 @@ -scp build-arm/drivebrain hytech@192.168.203.1:/home/hytech/drivebrain/ -scp config/config.json hytech@192.168.203.1:/home/hytech/drivebrain/config +scp build-arm/drivebrain hytech@192.168.203.1:/home/hytech/drivebrain/ +scp config/config.json hytech@192.168.203.1:/home/hytech/drivebrain/config diff --git a/docker-compose.yml b/docker-compose.yml index fdc5a2e..d4a118c 100644 --- a/docker-compose.yml +++ b/docker-compose.yml @@ -1,14 +1,14 @@ -services: - dev: - image: kkittur/drivebrain_cross_compile - volumes: - - conan-cache:/root/.conan2 - - ./:/app - working_dir: /app - tty: true - stdin_open: true - command: ["/bin/bash"] - restart: "no" - -volumes: - conan-cache: +services: + dev: + image: kkittur/drivebrain_cross_compile + volumes: + - conan-cache:/root/.conan2 + - ./:/app + working_dir: /app + tty: true + stdin_open: true + command: ["/bin/bash"] + restart: "no" + +volumes: + conan-cache: diff --git a/drivebrain_app/src/main.cpp b/drivebrain_app/src/main.cpp index 21f758d..4f25189 100644 --- a/drivebrain_app/src/main.cpp +++ b/drivebrain_app/src/main.cpp @@ -1,54 +1,54 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -std::atomic running = true; - -void sig_handler(int signal) { - if(signal == SIGINT) { - std::cout << "halting\n"; - running = false; - } -} - -void get_param_task(int wait_time, core::MsgType msg) { - while(running) { - core::MCAPLogger::instance().log_msg(static_cast(msg)); - std::this_thread::sleep_for((std::chrono::milliseconds(wait_time))); - } -} - -int main(int argc, char* argv[]) { - - - core::FoxgloveServer::create(argv[1]); - core::MCAPLogger::create("recordings/", mcap::McapWriterOptions("")); - core::MCAPLogger::instance().open_new_mcap("test_1.mcap"); - core::MCAPLogger::instance().init_logging(); - - std::signal(SIGINT, sig_handler); - - auto vel_msg = std::make_shared(); - vel_msg->set_velocity_x(1000); - vel_msg->set_velocity_y(10000); - - auto acu_data = std::make_shared(); - acu_data->set_max_cell_temp_id(676767); - - std::thread t1(get_param_task, 20, vel_msg); - std::thread t2(get_param_task, 40, acu_data); - - if(t1.joinable()) t1.join(); - if(t2.joinable()) t2.join(); - core::MCAPLogger::instance().close_active_mcap(); - -} +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +std::atomic running = true; + +void sig_handler(int signal) { + if(signal == SIGINT) { + std::cout << "halting\n"; + running = false; + } +} + +void get_param_task(int wait_time, core::MsgType msg) { + while(running) { + core::MCAPLogger::instance().log_msg(static_cast(msg)); + std::this_thread::sleep_for((std::chrono::milliseconds(wait_time))); + } +} + +int main(int argc, char* argv[]) { + + + core::FoxgloveServer::create(argv[1]); + core::MCAPLogger::create("recordings/", mcap::McapWriterOptions("")); + core::MCAPLogger::instance().open_new_mcap("test_1.mcap"); + core::MCAPLogger::instance().init_logging(); + + std::signal(SIGINT, sig_handler); + + auto vel_msg = std::make_shared(); + vel_msg->set_velocity_x(1000); + vel_msg->set_velocity_y(10000); + + auto acu_data = std::make_shared(); + acu_data->set_max_cell_temp_id(676767); + + std::thread t1(get_param_task, 20, vel_msg); + std::thread t2(get_param_task, 40, acu_data); + + if(t1.joinable()) t1.join(); + if(t2.joinable()) t2.join(); + core::MCAPLogger::instance().close_active_mcap(); + +} diff --git a/drivebrain_comms/CMakeLists.txt b/drivebrain_comms/CMakeLists.txt index a112b11..bf430f0 100644 --- a/drivebrain_comms/CMakeLists.txt +++ b/drivebrain_comms/CMakeLists.txt @@ -1,27 +1,27 @@ -cmake_minimum_required(VERSION 3.10) - -find_package(spdlog REQUIRED) -find_package(Boost REQUIRED) -find_package(nlohmann_json REQUIRED) -find_package(foxglove-websocket REQUIRED) -find_package(Protobuf REQUIRED CONFIG) - -add_library(drivebrain_comms - src/CANComms.cpp - # src/DBServiceImpl.cpp - # src/foxglove_server.cpp - # src/VNComms.cpp -) - -target_include_directories(drivebrain_comms PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/include") - -target_link_libraries(drivebrain_comms PUBLIC - drivebrain_core - spdlog::spdlog - nlohmann_json::nlohmann_json - boost::boost - foxglove-websocket::foxglove-websocket - protobuf::libprotobuf - hytech_msgs_cpp_lib - libvncxx -) +cmake_minimum_required(VERSION 3.10) + +find_package(spdlog REQUIRED) +find_package(Boost REQUIRED) +find_package(nlohmann_json REQUIRED) +find_package(foxglove-websocket REQUIRED) +find_package(Protobuf REQUIRED CONFIG) + +add_library(drivebrain_comms + src/CANComms.cpp + # src/DBServiceImpl.cpp + # src/foxglove_server.cpp + # src/VNComms.cpp +) + +target_include_directories(drivebrain_comms PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/include") + +target_link_libraries(drivebrain_comms PUBLIC + drivebrain_core + spdlog::spdlog + nlohmann_json::nlohmann_json + boost::boost + foxglove-websocket::foxglove-websocket + protobuf::libprotobuf + hytech_msgs_cpp_lib + libvncxx +) diff --git a/drivebrain_comms/include/EthernetComms.hpp b/drivebrain_comms/include/EthernetComms.hpp index 9b4aada..4dc45ea 100644 --- a/drivebrain_comms/include/EthernetComms.hpp +++ b/drivebrain_comms/include/EthernetComms.hpp @@ -1,82 +1,82 @@ -#pragma once - -#include -#include -#include -#include -#include - -#include -#include -#include - -#include "DriverBus.hpp" -#include "hytech_msgs.pb.h" - - -namespace comms -{ - /** - * @brief Ethernet driver class for sending and receiving Protobuf messages - * @tparam MsgType The protobuf message type the initialized Ethernet class will receive. An empty (void) template type indicates send-only mode - */ - template - class ETHDriver { - public: - - using deqtype = core::ThreadSafeDeque>; - - ETHDriver() = delete; - ~ETHDriver(); - - /** - * Constructor for creating a new instance of the Ethernet driver - * - * @param io_context The reference to the initialized io_context - * @param port The port to listen/receive messages from - * @param send_ip The IP address to send messages from (empty by default if in receive-only mode) - * @param bind Whether or not the socket should bind to local endpoint - */ - ETHDriver(boost::asio::io_context &io_context, uint16_t port, std::string send_ip = ""); - - /** - * Method for adding a new message to the send queue - * - * @param send_msg The message to send - */ - void enqueue_msg_send(std::shared_ptr send_msg); - private: - /* Receive helpers */ - - /* Deserializes the received ethernet message and logs to the MessagerLogger and state estimator */ - void _handle_receive(const boost::system::error_code &error, std::size_t size); - - /* Begins message receiving loop */ - void _start_receive(); - - /* Send helpers */ - - /* Logs and sends all messages in the message queue */ - void _handle_send_msg_from_queue(); - - /* Sends one message from the send buffer */ - void _send_message(std::shared_ptr msg_out); - - private: - boost::asio::ip::udp::endpoint _send_endp; - deqtype _send_msg_queue; - std::array _send_buffer; - std::array _recv_buffer; - std::shared_ptr _received_eth_msg; - - // std::shared_ptr _state_estimator = nullptr; //TODO: uncomment when state estimator is added - std::atomic _running = false; - boost::asio::ip::udp::socket _socket; - boost::asio::ip::udp::endpoint _remote_endpoint; - std::thread _output_thread; - - }; - -} - -#include "EthernetComms.tpp" +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "DriverBus.hpp" +#include "hytech_msgs.pb.h" + + +namespace comms +{ + /** + * @brief Ethernet driver class for sending and receiving Protobuf messages + * @tparam MsgType The protobuf message type the initialized Ethernet class will receive. An empty (void) template type indicates send-only mode + */ + template + class ETHDriver { + public: + + using deqtype = core::ThreadSafeDeque>; + + ETHDriver() = delete; + ~ETHDriver(); + + /** + * Constructor for creating a new instance of the Ethernet driver + * + * @param io_context The reference to the initialized io_context + * @param port The port to listen/receive messages from + * @param send_ip The IP address to send messages from (empty by default if in receive-only mode) + * @param bind Whether or not the socket should bind to local endpoint + */ + ETHDriver(boost::asio::io_context &io_context, uint16_t port, std::string send_ip = ""); + + /** + * Method for adding a new message to the send queue + * + * @param send_msg The message to send + */ + void enqueue_msg_send(std::shared_ptr send_msg); + private: + /* Receive helpers */ + + /* Deserializes the received ethernet message and logs to the MessagerLogger and state estimator */ + void _handle_receive(const boost::system::error_code &error, std::size_t size); + + /* Begins message receiving loop */ + void _start_receive(); + + /* Send helpers */ + + /* Logs and sends all messages in the message queue */ + void _handle_send_msg_from_queue(); + + /* Sends one message from the send buffer */ + void _send_message(std::shared_ptr msg_out); + + private: + boost::asio::ip::udp::endpoint _send_endp; + deqtype _send_msg_queue; + std::array _send_buffer; + std::array _recv_buffer; + std::shared_ptr _received_eth_msg; + + // std::shared_ptr _state_estimator = nullptr; //TODO: uncomment when state estimator is added + std::atomic _running = false; + boost::asio::ip::udp::socket _socket; + boost::asio::ip::udp::endpoint _remote_endpoint; + std::thread _output_thread; + + }; + +} + +#include "EthernetComms.tpp" diff --git a/drivebrain_comms/include/EthernetComms.tpp b/drivebrain_comms/include/EthernetComms.tpp index 73a4cbb..9240fd3 100644 --- a/drivebrain_comms/include/EthernetComms.tpp +++ b/drivebrain_comms/include/EthernetComms.tpp @@ -1,118 +1,118 @@ -#include "EthernetComms.hpp" -#include "hytech_msgs.pb.h" -#include -#include -#include -#include -#include - -using boost::asio::ip::udp; -namespace comms -{ - template - ETHDriver::ETHDriver(boost::asio::io_context &io_context, uint16_t port, std::string send_ip) - : _socket(io_context, udp::endpoint(udp::v4(), port)), - _send_endp(send_ip.empty() - ? udp::endpoint(udp::v4(), port) - : udp::endpoint(boost::asio::ip::make_address(send_ip.c_str()), port)) - { - _running = true; - _output_thread = std::thread(&comms::ETHDriver::_handle_send_msg_from_queue, this); - - if constexpr (!std::is_same_v) { - _socket.bind(udp::endpoint(udp::v4(), port)); - _received_eth_msg = std::make_shared(); - _start_receive(); - spdlog::info("ETHDriver full duplex mode initialized for port {}", port); - } else { - spdlog::info("ETHDriver send-only mode initialized for port {}", port); - } - - } - - template - ETHDriver::~ETHDriver() { - _running = false; - _send_msg_queue.cv.notify_all(); - if (_output_thread.joinable()) { - _output_thread.join(); - } - _socket.close(); - spdlog::warn("Ethernet driver destructed"); - } - - /* Receive methods */ - template - void ETHDriver::_handle_receive(const boost::system::error_code &error, std::size_t size) { - if (_running && !error) { - _received_eth_msg ->ParseFromArray(_recv_buffer.data(), size); - auto out_msg = static_cast>(_received_eth_msg); - // log to outputs - // TODO: log to state estimator - _start_receive(); - } - } - - template - void ETHDriver::_start_receive() { - using namespace boost::placeholders; - _socket.async_receive_from( - boost::asio::buffer(_recv_buffer), _remote_endpoint, - boost::bind(ÐDriver::_handle_receive, this, - boost::asio::placeholders::error, - boost::asio::placeholders::bytes_transferred)); - } - - /* Send methods */ - - template - void ETHDriver::enqueue_msg_send(std::shared_ptr send_msg) { - spdlog::debug("enqueing msg"); - std::unique_lock lk(_send_msg_queue.mtx); - _send_msg_queue.deque.push_back(send_msg); - _send_msg_queue.cv.notify_all(); - } - - template - void ETHDriver::_handle_send_msg_from_queue() { - std::deque> temp_msg_queue; - while(_running) { - /* transfer messages to temp queue for duration of lock */ - { - std::unique_lock lk(_send_msg_queue.mtx); - _send_msg_queue.cv.wait(lk, [this]() { return !_send_msg_queue.deque.empty() || !_running; }); - - if(!_running) { - break; - } - - _send_msg_queue.deque.swap(temp_msg_queue); - } - - } - - for (const auto &msg : temp_msg_queue) { - _send_message(msg); - // this->log(msg); - } - - temp_msg_queue.clear(); - } - - template - void ETHDriver::_send_message(std::shared_ptr msg_out) { - spdlog::info("sending ethernet message"); - msg_out->SerializeToArray(_send_buffer.data(), msg_out->ByteSizeLong()); - _socket.async_send_to(boost::asio::buffer(_send_buffer, msg_out->ByteSizeLong()), - _send_endp, - [](const boost::system::error_code &error, std::size_t bytes_transferred) - { - if (error) { - spdlog::error("async send failed: {}", error.message()); - } else { - spdlog::debug("async send completed: {} bytes.", bytes_transferred); - } - }); - } - -} +#include "EthernetComms.hpp" +#include "hytech_msgs.pb.h" +#include +#include +#include +#include +#include + +using boost::asio::ip::udp; +namespace comms +{ + template + ETHDriver::ETHDriver(boost::asio::io_context &io_context, uint16_t port, std::string send_ip) + : _socket(io_context, udp::endpoint(udp::v4(), port)), + _send_endp(send_ip.empty() + ? udp::endpoint(udp::v4(), port) + : udp::endpoint(boost::asio::ip::make_address(send_ip.c_str()), port)) + { + _running = true; + _output_thread = std::thread(&comms::ETHDriver::_handle_send_msg_from_queue, this); + + if constexpr (!std::is_same_v) { + _socket.bind(udp::endpoint(udp::v4(), port)); + _received_eth_msg = std::make_shared(); + _start_receive(); + spdlog::info("ETHDriver full duplex mode initialized for port {}", port); + } else { + spdlog::info("ETHDriver send-only mode initialized for port {}", port); + } + + } + + template + ETHDriver::~ETHDriver() { + _running = false; + _send_msg_queue.cv.notify_all(); + if (_output_thread.joinable()) { + _output_thread.join(); + } + _socket.close(); + spdlog::warn("Ethernet driver destructed"); + } + + /* Receive methods */ + template + void ETHDriver::_handle_receive(const boost::system::error_code &error, std::size_t size) { + if (_running && !error) { + _received_eth_msg ->ParseFromArray(_recv_buffer.data(), size); + auto out_msg = static_cast>(_received_eth_msg); + // log to outputs + // TODO: log to state estimator + _start_receive(); + } + } + + template + void ETHDriver::_start_receive() { + using namespace boost::placeholders; + _socket.async_receive_from( + boost::asio::buffer(_recv_buffer), _remote_endpoint, + boost::bind(ÐDriver::_handle_receive, this, + boost::asio::placeholders::error, + boost::asio::placeholders::bytes_transferred)); + } + + /* Send methods */ + + template + void ETHDriver::enqueue_msg_send(std::shared_ptr send_msg) { + spdlog::debug("enqueing msg"); + std::unique_lock lk(_send_msg_queue.mtx); + _send_msg_queue.deque.push_back(send_msg); + _send_msg_queue.cv.notify_all(); + } + + template + void ETHDriver::_handle_send_msg_from_queue() { + std::deque> temp_msg_queue; + while(_running) { + /* transfer messages to temp queue for duration of lock */ + { + std::unique_lock lk(_send_msg_queue.mtx); + _send_msg_queue.cv.wait(lk, [this]() { return !_send_msg_queue.deque.empty() || !_running; }); + + if(!_running) { + break; + } + + _send_msg_queue.deque.swap(temp_msg_queue); + } + + } + + for (const auto &msg : temp_msg_queue) { + _send_message(msg); + // this->log(msg); + } + + temp_msg_queue.clear(); + } + + template + void ETHDriver::_send_message(std::shared_ptr msg_out) { + spdlog::info("sending ethernet message"); + msg_out->SerializeToArray(_send_buffer.data(), msg_out->ByteSizeLong()); + _socket.async_send_to(boost::asio::buffer(_send_buffer, msg_out->ByteSizeLong()), + _send_endp, + [](const boost::system::error_code &error, std::size_t bytes_transferred) + { + if (error) { + spdlog::error("async send failed: {}", error.message()); + } else { + spdlog::debug("async send completed: {} bytes.", bytes_transferred); + } + }); + } + +} diff --git a/drivebrain_core/CMakeLists.txt b/drivebrain_core/CMakeLists.txt index 355ccc4..37cb0be 100644 --- a/drivebrain_core/CMakeLists.txt +++ b/drivebrain_core/CMakeLists.txt @@ -1,27 +1,28 @@ -cmake_minimum_required(VERSION 3.10) - -find_package(spdlog REQUIRED) -find_package(Boost REQUIRED) -find_package(nlohmann_json REQUIRED) -find_package(foxglove-websocket REQUIRED) -find_package(Protobuf REQUIRED CONFIG) -find_package(mcap REQUIRED) - -add_library(drivebrain_core - src/FoxgloveServer.cpp - src/MCAPLogger.cpp - src/StateTracker.cpp -) - -target_include_directories(drivebrain_core PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/include") - -target_link_libraries(drivebrain_core PUBLIC - spdlog::spdlog - boost::boost - nlohmann_json::nlohmann_json - foxglove-websocket::foxglove-websocket - mcap::mcap - protobuf::libprotobuf - hytech_msgs_cpp_lib - hytech_can_msgs_cpp_lib +cmake_minimum_required(VERSION 3.10) + +find_package(spdlog REQUIRED) +find_package(Boost REQUIRED) +find_package(nlohmann_json REQUIRED) +find_package(foxglove-websocket REQUIRED) +find_package(Protobuf REQUIRED CONFIG) +find_package(mcap REQUIRED) + +add_library(drivebrain_core + src/FoxgloveServer.cpp + src/MCAPLogger.cpp + src/StateTracker.cpp + src/LapTracker.cpp +) + +target_include_directories(drivebrain_core PUBLIC "${CMAKE_CURRENT_SOURCE_DIR}/include") + +target_link_libraries(drivebrain_core PUBLIC + spdlog::spdlog + boost::boost + nlohmann_json::nlohmann_json + foxglove-websocket::foxglove-websocket + mcap::mcap + protobuf::libprotobuf + hytech_msgs_cpp_lib + hytech_can_msgs_cpp_lib ) \ No newline at end of file diff --git a/drivebrain_core/include/Configurable.hpp b/drivebrain_core/include/Configurable.hpp index fabca52..ee21a79 100644 --- a/drivebrain_core/include/Configurable.hpp +++ b/drivebrain_core/include/Configurable.hpp @@ -1,44 +1,44 @@ -#include -#include -#include -#include -#include - -namespace core { - using ParamType = std::variant; - - template - struct Param { - T val; - }; - - template - class Configurable { - public: - std::string name() { return _name; } - bool register_param() {} - std::unordered_map get_params(); - - - private: - std::string _name; - std::unordered_map _params; - - std::mutex mtx; - friend ParamStruct; - }; - - - /* Syntax-sugar macros for cleanly defining and registering params */ - - #define BEGIN_PARAMS(StructName) \ - struct StructName { \ - template \ - void register_all(ConfigType* self) { - - - - #define END_PARAMS() \ - } \ - }; -} +#include +#include +#include +#include +#include + +namespace core { + using ParamType = std::variant; + + template + struct Param { + T val; + }; + + template + class Configurable { + public: + std::string name() { return _name; } + bool register_param() {} + std::unordered_map get_params(); + + + private: + std::string _name; + std::unordered_map _params; + + std::mutex mtx; + friend ParamStruct; + }; + + + /* Syntax-sugar macros for cleanly defining and registering params */ + + #define BEGIN_PARAMS(StructName) \ + struct StructName { \ + template \ + void register_all(ConfigType* self) { + + + + #define END_PARAMS() \ + } \ + }; +} diff --git a/drivebrain_core/include/DriverBus.hpp b/drivebrain_core/include/DriverBus.hpp index b4dcc6b..84c5c23 100644 --- a/drivebrain_core/include/DriverBus.hpp +++ b/drivebrain_core/include/DriverBus.hpp @@ -1,24 +1,24 @@ -#pragma once - -#include -#include -#include - - -// what this should do: -// should contain the instances of the thread-safe deques, contain the thread(s) -// for communication and manage the routing of messages - -// threading paradigm (for now): -// we will still need to have threads within the drivers themselves since otherwise we wont be able to easily wait on - -namespace core -{ - template - struct ThreadSafeDeque - { - std::deque deque; - std::mutex mtx; - std::condition_variable cv; - }; -} +#pragma once + +#include +#include +#include + + +// what this should do: +// should contain the instances of the thread-safe deques, contain the thread(s) +// for communication and manage the routing of messages + +// threading paradigm (for now): +// we will still need to have threads within the drivers themselves since otherwise we wont be able to easily wait on + +namespace core +{ + template + struct ThreadSafeDeque + { + std::deque deque; + std::mutex mtx; + std::condition_variable cv; + }; +} diff --git a/drivebrain_core/include/FoxgloveServer.hpp b/drivebrain_core/include/FoxgloveServer.hpp index b34023e..8428620 100644 --- a/drivebrain_core/include/FoxgloveServer.hpp +++ b/drivebrain_core/include/FoxgloveServer.hpp @@ -1,92 +1,92 @@ -#ifndef ETHERNET_RECEIVE_COMMS_H - -#define ETHERNET_RECEIVE_COMMS_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "hytech_msgs.pb.h" - -namespace core { - class FoxgloveServer { - public: - /** - * Initialzes a new Foxglove server singleton instance - * - * @param parameters_file the json containing parameters for putting on foxglove - */ - static void create(const std::string ¶meters_file); - - /** - * Fetches Foxglove server singleton instance - * - * @retun FoxgloveServer instance - */ - static FoxgloveServer& instance(); - - /** - * Destructs the foxglove server instance by stopping the server. - */ - ~FoxgloveServer() { - _server->stop(); - std::cout << "Destructed and stopped foxglove websocket server" << std::endl; - } - - /** - * Sends a protobuf to be viewed in foxglove. - * broadcastMessage() is thread safe so this method can be called - * by different threads without a mutex. - * - * @param msg the message to be sent - */ - void send_live_telem_msg(std::shared_ptr msg); - - /** - * Thread-safe method to get a foxglove param - * - * @param param_name name of the parameter the user wants to get - * @return the parameter value - */ - // TODO: investigate the type conversion conflicts that arrise from this - template - param_type get_param(std::string param_name) { - std::unique_lock lock(_parameter_mutex); - - // if (_foxglove_params_map.find(param_name) == _foxglove_params_map.end()) { - // return NULL; - // } - - return _foxglove_params_map.at(param_name).getValue(); - } - - private: - FoxgloveServer(std::string parameters_file); - - /* Singleton move semantics */ - FoxgloveServer(const FoxgloveServer&) = delete; - FoxgloveServer& operator=(const FoxgloveServer&) = delete; - - /* Singleton instance */ - inline static std::atomic _s_instance; - - std::unordered_map _foxglove_params_map; - std::unordered_map _name_to_id_map; - - std::unique_ptr> _server; - foxglove::ServerOptions _server_options; - - std::mutex _parameter_mutex; - }; -} - -#endif // ETHERNET_RECEIVE_COMMS_H +#ifndef ETHERNET_RECEIVE_COMMS_H + +#define ETHERNET_RECEIVE_COMMS_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "hytech_msgs.pb.h" + +namespace core { + class FoxgloveServer { + public: + /** + * Initialzes a new Foxglove server singleton instance + * + * @param parameters_file the json containing parameters for putting on foxglove + */ + static void create(const std::string ¶meters_file); + + /** + * Fetches Foxglove server singleton instance + * + * @retun FoxgloveServer instance + */ + static FoxgloveServer& instance(); + + /** + * Destructs the foxglove server instance by stopping the server. + */ + ~FoxgloveServer() { + _server->stop(); + std::cout << "Destructed and stopped foxglove websocket server" << std::endl; + } + + /** + * Sends a protobuf to be viewed in foxglove. + * broadcastMessage() is thread safe so this method can be called + * by different threads without a mutex. + * + * @param msg the message to be sent + */ + void send_live_telem_msg(std::shared_ptr msg); + + /** + * Thread-safe method to get a foxglove param + * + * @param param_name name of the parameter the user wants to get + * @return the parameter value + */ + // TODO: investigate the type conversion conflicts that arrise from this + template + param_type get_param(std::string param_name) { + std::unique_lock lock(_parameter_mutex); + + // if (_foxglove_params_map.find(param_name) == _foxglove_params_map.end()) { + // return NULL; + // } + + return _foxglove_params_map.at(param_name).getValue(); + } + + private: + FoxgloveServer(std::string parameters_file); + + /* Singleton move semantics */ + FoxgloveServer(const FoxgloveServer&) = delete; + FoxgloveServer& operator=(const FoxgloveServer&) = delete; + + /* Singleton instance */ + inline static std::atomic _s_instance; + + std::unordered_map _foxglove_params_map; + std::unordered_map _name_to_id_map; + + std::unique_ptr> _server; + foxglove::ServerOptions _server_options; + + std::mutex _parameter_mutex; + }; +} + +#endif // ETHERNET_RECEIVE_COMMS_H diff --git a/drivebrain_core/include/LapTracker.hpp b/drivebrain_core/include/LapTracker.hpp new file mode 100644 index 0000000..3b50960 --- /dev/null +++ b/drivebrain_core/include/LapTracker.hpp @@ -0,0 +1,74 @@ +#include +#include +#include +#include +// TODO Add any imports you might think are neccesary here + +namespace core { + + struct LineSegment { + Position start; + Position end; + + }; + + struct Point { + double x,y; + }; + + class LapTracker { + + public: + + /* Singleton move semantics */ + LapTracker(const LapTracker&) = delete; + LapTracker& operator=(const LapTracker&) = delete; + + /** + * Steps the lap tracker + * + * @param latest_state the latest state (will be updated in the main loop after get_latest_state_and_validity + * is called by the state tracker) + */ + void step_tracker(core::VehicleState& latest_state); + + /** + * Creates a new instance of the lap tracker + */ + static void create(); + + float magnitude(xyz_vec v); + std::pair offsetFinder(xyz_vec v); + int orientation(Point p, Point q, Point r); + bool onSegment(Point p, Point q, Point r); + bool doIntersect(Point p1, Point q1, Point p2, Point q2); + float meter_to_degree_latitude(float m); + float meter_to_degree_longitude(float m); + /** + * Returns the lap tracker instance + */ + static LapTracker& instance(); + void setStartLine(const Position& start, const Position& end); + + private: + + LapTracker(); + + /** Internal State */ + inline static std::atomic _s_instance; + + // TODO put variables here to keep track of the lap tracker's internal state + bool has_start_line; + int lap_counter; + std::chrono::steady_clock::time_point curr_lap_start_time; + VehicleState prev_state; + LineSegment start_line; + bool has_crossed_start_line; + bool has_prev_state; + float best_lap_time; + float max_lap_speed; + + }; + + +} \ No newline at end of file diff --git a/drivebrain_core/include/MCAPLogger.hpp b/drivebrain_core/include/MCAPLogger.hpp index 48b32ea..abcf2e9 100644 --- a/drivebrain_core/include/MCAPLogger.hpp +++ b/drivebrain_core/include/MCAPLogger.hpp @@ -1,163 +1,163 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace core { - - struct RawMessage_s { - std::string serialized_data; - std::string message_name; - mcap::Timestamp log_time; - }; - - using MsgType = std::shared_ptr; - - class MCAPLogger { - public: - - /** - * Constructor for initializing a new MCAPLogger singleton instance - * - * @param base_dir the directory in which the log file should be created - * @param options options to create the mcap with - */ - static void create(const std::string &base_dir, const mcap::McapWriterOptions &options); - - /** - * Fetches MCAPLogger singleton instance - * - * @retun MCAPLogger instance - */ - static MCAPLogger& instance(); - - /** - * Destructs MCAPLogger instance, frees singleton instance and joins all running log threads - */ - ~MCAPLogger() { - spdlog::info("Destructing message logger"); - _logging = false; - _running = false; - - _s_instance.store(nullptr, std::memory_order_release); - spdlog::info("Msg logger singleton instance released"); - - _param_cv.notify_all(); - if(_param_log_thread.joinable()) _param_log_thread.join(); - if(_msg_log_thread.joinable()) _msg_log_thread.join(); - } - - /** - * Opens a new mcap file by adding options and all protobuf schema - * - * @param name name of the new file to be opened - * @return 0 on success, negative err code on failure - */ - int open_new_mcap(const std::string &name); - - /** - * Closes the current mcap file. Only runs if there is a file open. - * @return 0 on success, negative err code on failure - */ - int close_active_mcap(); - - /** - * Adds params schema to allow for parameter logging - * @return 0 on success, negative err code on failure - */ - int add_params_schema(); - - /** - * Fetches the current logger status - * @return A tuple containing the name of the file being logged to, and whether or not the logging process is active - */ - std::tuple status(); - - /** - * Kicks off the message logging & param logging thread and begins writing to a log file. - */ - void init_logging(); - - /** - * Stops logging to the current file. - * To not be confused with halting, which stops logging and running the MCAPLogger entirely. - */ - void stop_logging(); - - /* - * Stops logging and running the MCAPLogger - */ - void halt(); - - /** - * Logs a protobuf message to the current MCAP file - * - * @param message the message to be logged - * @return 0 on success, negative err code on failure - */ - int log_msg(MsgType message); - - /** - * Logs the json params to the current MCAP file. Doesn't need an input because the schema has already been created. - * - * @return 0 on success, negative err code on failure - */ - int log_params(); - - private: - - /* Private constructor to be called by init method */ - MCAPLogger(const std::string &base_dir, const mcap::McapWriterOptions &options); - - /** - * Spawned by thread, loops until end of program life or error occurs. - * Logs all queued messages to a file. - */ - void _handle_log_to_file(); - - /** - * Spawned by thread, loops until end of program life or error occurs. - * Logs all active params. - */ - void _handle_param_log(); - - /* Singleton move semantics */ - MCAPLogger(const MCAPLogger&) = delete; - MCAPLogger& operator=(const MCAPLogger&) = delete; - - /* Singleton instance */ - inline static std::atomic _s_instance; - - /* Message log management */ - std::deque _input_buffer; - std::mutex _input_buffer_mutex; - std::condition_variable _input_buffer_cv; - std::thread _msg_log_thread; - - /* Param log management */ - std::mutex _param_mutex; - std::condition_variable _param_cv; - std::thread _param_log_thread; - - /* MCAP utilities */ - mcap::McapWriter _writer; - mcap::McapWriterOptions _options; - - /* State */ - std::unordered_map _name_to_id_map; - std::string _log_name = "NONE"; - bool _param_schema_written = false; - bool _logging = false; - bool _running = true; - - }; - -} +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace core { + + struct RawMessage_s { + std::string serialized_data; + std::string message_name; + mcap::Timestamp log_time; + }; + + using MsgType = std::shared_ptr; + + class MCAPLogger { + public: + + /** + * Constructor for initializing a new MCAPLogger singleton instance + * + * @param base_dir the directory in which the log file should be created + * @param options options to create the mcap with + */ + static void create(const std::string &base_dir, const mcap::McapWriterOptions &options); + + /** + * Fetches MCAPLogger singleton instance + * + * @retun MCAPLogger instance + */ + static MCAPLogger& instance(); + + /** + * Destructs MCAPLogger instance, frees singleton instance and joins all running log threads + */ + ~MCAPLogger() { + spdlog::info("Destructing message logger"); + _logging = false; + _running = false; + + _s_instance.store(nullptr, std::memory_order_release); + spdlog::info("Msg logger singleton instance released"); + + _param_cv.notify_all(); + if(_param_log_thread.joinable()) _param_log_thread.join(); + if(_msg_log_thread.joinable()) _msg_log_thread.join(); + } + + /** + * Opens a new mcap file by adding options and all protobuf schema + * + * @param name name of the new file to be opened + * @return 0 on success, negative err code on failure + */ + int open_new_mcap(const std::string &name); + + /** + * Closes the current mcap file. Only runs if there is a file open. + * @return 0 on success, negative err code on failure + */ + int close_active_mcap(); + + /** + * Adds params schema to allow for parameter logging + * @return 0 on success, negative err code on failure + */ + int add_params_schema(); + + /** + * Fetches the current logger status + * @return A tuple containing the name of the file being logged to, and whether or not the logging process is active + */ + std::tuple status(); + + /** + * Kicks off the message logging & param logging thread and begins writing to a log file. + */ + void init_logging(); + + /** + * Stops logging to the current file. + * To not be confused with halting, which stops logging and running the MCAPLogger entirely. + */ + void stop_logging(); + + /* + * Stops logging and running the MCAPLogger + */ + void halt(); + + /** + * Logs a protobuf message to the current MCAP file + * + * @param message the message to be logged + * @return 0 on success, negative err code on failure + */ + int log_msg(MsgType message); + + /** + * Logs the json params to the current MCAP file. Doesn't need an input because the schema has already been created. + * + * @return 0 on success, negative err code on failure + */ + int log_params(); + + private: + + /* Private constructor to be called by init method */ + MCAPLogger(const std::string &base_dir, const mcap::McapWriterOptions &options); + + /** + * Spawned by thread, loops until end of program life or error occurs. + * Logs all queued messages to a file. + */ + void _handle_log_to_file(); + + /** + * Spawned by thread, loops until end of program life or error occurs. + * Logs all active params. + */ + void _handle_param_log(); + + /* Singleton move semantics */ + MCAPLogger(const MCAPLogger&) = delete; + MCAPLogger& operator=(const MCAPLogger&) = delete; + + /* Singleton instance */ + inline static std::atomic _s_instance; + + /* Message log management */ + std::deque _input_buffer; + std::mutex _input_buffer_mutex; + std::condition_variable _input_buffer_cv; + std::thread _msg_log_thread; + + /* Param log management */ + std::mutex _param_mutex; + std::condition_variable _param_cv; + std::thread _param_log_thread; + + /* MCAP utilities */ + mcap::McapWriter _writer; + mcap::McapWriterOptions _options; + + /* State */ + std::unordered_map _name_to_id_map; + std::string _log_name = "NONE"; + bool _param_schema_written = false; + bool _logging = false; + bool _running = true; + + }; + +} diff --git a/drivebrain_core/include/StateTracker.hpp b/drivebrain_core/include/StateTracker.hpp index 6565030..75d31bb 100644 --- a/drivebrain_core/include/StateTracker.hpp +++ b/drivebrain_core/include/StateTracker.hpp @@ -1,302 +1,322 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "hytech_msgs.pb.h" -#include "hytech.pb.h" - -/** - * The state tracker acts - * as a thread-safe translation unit - * between our comms interfaces and - * the rest of the program. It keeps track - * of current internal state. - */ -namespace core { - - /** - * @struct Represents a vehicle vector (4 elements of any type) - */ - template - struct veh_vec { - T FL; - T FR; - T RL; - T RR; - - template - void set_from_index(T val) { - static_assert(ind <= 3, "ERROR: index cannot be greater than 3"); - if constexpr (ind == 0) { - FL = val; - } else if(ind == 1) { - FR = val; - } else if(ind == 2) { - RL = val; - } else { - RR = val; - } - } - - template - T get_from_index() { - static_assert(ind <= 3, "ERROR: index cannot be greater than 3"); - if constexpr (ind == 0) { - return FL; - } else if(ind == 1) { - return FR; - } else if(ind == 2) { - return RL; - } else { - return RR; - } - } - - }; - - /** - * @struct Represents an xyz vector (3 elements of any type) - */ - template - struct xyz_vec { - T x; - T y; - T z; - }; - - /** - * @struct Represents a ypr vector (3 elements of any type that represent angle) - */ - template - struct ypr_vec { - T yaw; - T pitch; - T roll; - }; - - /** - * @struct Represents raw sensor data - */ - struct RawInputData { - veh_vec raw_load_cell_values; - veh_vec raw_shock_pot_values; - float raw_steering_analog; - float raw_steering_digital; - veh_vec raw_inverter_torques; - veh_vec raw_inverter_power; - }; - - /** - * @struct position - */ - struct Position { - double lat; - double lon; - bool valid; - }; - - /** - * @struct Contains requested pedals information - */ - struct DriverInput { - float requested_accel; // float from 0 to 1 representing percent of accel pedal travel - float requested_brake; // float from 0 to 1 representing pedal travel of brake - }; - - /** - * @struct Contains the torque outputs of the controller - */ - struct ControllerTorqueOut { - veh_vec res_torque_lim_nm; - }; - - /** - * @struct Represents the outputs of the tire model - */ - struct TireDynamics { - veh_vec> tire_forces_n; - veh_vec> tire_moments_nm; - veh_vec accel_saturation_nm; - veh_vec brake_saturation_nm; - float v_y_lm; - float psi_dot_lm_rad_s; - TireDynamics() { - tire_forces_n = {}; - tire_moments_nm = {}; - accel_saturation_nm = {}; - brake_saturation_nm = {}; - v_y_lm = 0; - psi_dot_lm_rad_s = 0; - } - }; - - /** - * @struct Represents more specific outputs of the controller - * re. torque vectoring statuses - */ - struct TorqueVectoringStatus { - veh_vec torque_additional_nm; - float additional_mz_moment_nm; - float des_psi_dot; - float psi_dot_err; - float perceived_vx; - float integral_yaw_rate_err; - float perceived_psi_dot; - }; - - /** - * @struct Collection of different outputs - */ - struct MatlabMathResult { - TireDynamics tire_dynamics; - TorqueVectoringStatus torque_vectoring_status; - }; - - /** - * @struct Collection of desired speed and torques - */ - struct SpeedControlOut { - int64_t mcu_recv_millis; - veh_vec desired_rpms; - veh_vec torque_lim_nm; - }; - - /** - * @struct A specific torque control out - */ - struct TorqueControlOut { - veh_vec desired_torques_nm; - }; - - - /** - * @struct A controller output - */ - struct ControllerOutput { - std::variant out; - }; - - /** - * @struct Contains the health of our ins - */ - struct INSStatus { - float vel_uncertainty; - int status_mode; - }; - - /** - * @struct Contains drivetrain information - */ - struct DrivetrainData { - veh_vec inverter_igbt_temps_c; - veh_vec inverter_temps_c; - veh_vec inverter_motor_temps_c; - }; - - /** - * @struct Contains accumulator information - */ - struct AccumulatorData { - float min_cell_voltage; - float pack_voltage; - }; - - /** - * @struct Contains a wealth of data - * representing the entire internal - * state of the vehicle. - */ - struct VehicleState { - bool is_ready_to_drive; - DriverInput input; - xyz_vec current_body_vel_ms; - xyz_vec current_body_accel_mss; - xyz_vec current_angular_rate_rads; - ypr_vec current_ypr_rad; - veh_vec current_rpms; - veh_vec motor_overload_percentages; - bool state_is_valid; - int prev_MCU_recv_millis; - float steering_angle_deg; - ControllerOutput prev_controller_output; - TireDynamics tire_dynamics; - veh_vec driver_torque; - ControllerTorqueOut matlab_math_temp_out; - veh_vec suspension_potentiometers_mm; - Position vehicle_position; - veh_vec loadcells; - veh_vec current_torques_nm; - INSStatus ins_status; - float old_energy_meter_kw; - DrivetrainData dt_data; - AccumulatorData acc_data; - }; - - /** - * Allows different communications interfaces - * to update the internal state of drivebrain in a thread-safe manner - */ - class StateTracker { - - public: - - /** - * Constructs a new state tracker. - */ - StateTracker() = default; - - /** - * Receives a protobuf message and adds any useful information to the internal - * vehicle state. - * - * @param message the message with information to add. - */ - void handle_receive_protobuf_message(std::shared_ptr message); - - /** - * Sets the previous control output of the controller - * - * @param previous_control_output the previous output of the controller - */ - void set_previous_control_output(ControllerOutput &previous_control_output); - - /** - * Returns a pair of the current vehicle state and whether or not it's valid - * - * @return the vehicle state and whether or not its valid. - */ - std::pair get_latest_state_and_validity(); - - - private: - - template - void _handle_set_inverter_dynamics(std::shared_ptr msg); - - template - void _handle_set_inverter_temps(std::shared_ptr msg); - - template - bool _validate_timestamps(const std::array ×tamp_arr); - - void _receive_low_level_state(std::shared_ptr message); - - void _receive_inverter_states(std::shared_ptr message); - - VehicleState _vehicle_state = { }; - RawInputData _raw_input_data = { }; - std::mutex _state_mutex; - std::array _timestamp_array; - - - }; -} +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "hytech_msgs.pb.h" +#include "hytech.pb.h" + +/** + * The state tracker acts + * as a thread-safe translation unit + * between our comms interfaces and + * the rest of the program. It keeps track + * of current internal state. + */ +namespace core { + + /** + * @struct Represents a vehicle vector (4 elements of any type) + */ + template + struct veh_vec { + T FL; + T FR; + T RL; + T RR; + + template + void set_from_index(T val) { + static_assert(ind <= 3, "ERROR: index cannot be greater than 3"); + if constexpr (ind == 0) { + FL = val; + } else if(ind == 1) { + FR = val; + } else if(ind == 2) { + RL = val; + } else { + RR = val; + } + } + + template + T get_from_index() { + static_assert(ind <= 3, "ERROR: index cannot be greater than 3"); + if constexpr (ind == 0) { + return FL; + } else if(ind == 1) { + return FR; + } else if(ind == 2) { + return RL; + } else { + return RR; + } + } + + }; + + /** + * @struct Represents an xyz vector (3 elements of any type) + */ + template + struct xyz_vec { + T x; + T y; + T z; + }; + + /** + * @struct Represents a ypr vector (3 elements of any type that represent angle) + */ + template + struct ypr_vec { + T yaw; + T pitch; + T roll; + }; + + /** + * @struct Represents raw sensor data + */ + struct RawInputData { + veh_vec raw_load_cell_values; + veh_vec raw_shock_pot_values; + float raw_steering_analog; + float raw_steering_digital; + veh_vec raw_inverter_torques; + veh_vec raw_inverter_power; + }; + + /** + * @struct position + */ + struct Position { + double lat; + double lon; + bool valid; + }; + + /** + * @struct Contains requested pedals information + */ + struct DriverInput { + float requested_accel; // float from 0 to 1 representing percent of accel pedal travel + float requested_brake; // float from 0 to 1 representing pedal travel of brake + }; + + /** + * @struct Contains the torque outputs of the controller + */ + struct ControllerTorqueOut { + veh_vec res_torque_lim_nm; + }; + + /** + * @struct Represents the outputs of the tire model + */ + struct TireDynamics { + veh_vec> tire_forces_n; + veh_vec> tire_moments_nm; + veh_vec accel_saturation_nm; + veh_vec brake_saturation_nm; + float v_y_lm; + float psi_dot_lm_rad_s; + TireDynamics() { + tire_forces_n = {}; + tire_moments_nm = {}; + accel_saturation_nm = {}; + brake_saturation_nm = {}; + v_y_lm = 0; + psi_dot_lm_rad_s = 0; + } + }; + + /** + * @struct Represents more specific outputs of the controller + * re. torque vectoring statuses + */ + struct TorqueVectoringStatus { + veh_vec torque_additional_nm; + float additional_mz_moment_nm; + float des_psi_dot; + float psi_dot_err; + float perceived_vx; + float integral_yaw_rate_err; + float perceived_psi_dot; + }; + + /** + * @struct Collection of different outputs + */ + struct MatlabMathResult { + TireDynamics tire_dynamics; + TorqueVectoringStatus torque_vectoring_status; + }; + + /** + * @struct Collection of desired speed and torques + */ + struct SpeedControlOut { + int64_t mcu_recv_millis; + veh_vec desired_rpms; + veh_vec torque_lim_nm; + }; + + /** + * @struct A specific torque control out + */ + struct TorqueControlOut { + veh_vec desired_torques_nm; + }; + + + /** + * @struct A controller output + */ + struct ControllerOutput { + std::variant out; + }; + + /** + * @struct Contains the health of our ins + */ + struct INSStatus { + float vel_uncertainty; + int status_mode; + }; + + /** + * @struct Contains drivetrain information + */ + struct DrivetrainData { + veh_vec inverter_igbt_temps_c; + veh_vec inverter_temps_c; + veh_vec inverter_motor_temps_c; + }; + + /** + * @struct Contains accumulator information + */ + struct AccumulatorData { + float min_cell_voltage; + float pack_voltage; + }; + /** + * @struct Laptime info + */ + struct LaptimeInfo { + float laptime_seconds; + int lapcount; + }; + + /** + * @struct Contains a wealth of data + * representing the entire internal + * state of the vehicle. + */ + struct VehicleState { + bool is_ready_to_drive; + DriverInput input; + xyz_vec current_body_vel_ms; + xyz_vec current_body_accel_mss; + xyz_vec current_angular_rate_rads; + ypr_vec current_ypr_rad; + veh_vec current_rpms; + veh_vec motor_overload_percentages; + bool state_is_valid; + int prev_MCU_recv_millis; + float steering_angle_deg; + ControllerOutput prev_controller_output; + TireDynamics tire_dynamics; + veh_vec driver_torque; + ControllerTorqueOut matlab_math_temp_out; + veh_vec suspension_potentiometers_mm; + Position vehicle_position; + veh_vec loadcells; + veh_vec current_torques_nm; + INSStatus ins_status; + float old_energy_meter_kw; + DrivetrainData dt_data; + AccumulatorData acc_data; + LaptimeInfo laptime_info; + }; + + /** + * Allows different communications interfaces + * to update the internal state of drivebrain in a thread-safe manner + */ + class StateTracker { + + public: + + /* Singleton move semantics */ + StateTracker(const StateTracker&) = delete; + StateTracker& operator=(const StateTracker&) = delete; + + /** + * Receives a protobuf message and adds any useful information to the internal + * vehicle state. + * + * @param message the message with information to add. + */ + void handle_receive_protobuf_message(std::shared_ptr message); + + /** + * Sets the previous control output of the controller + * + * @param previous_control_output the previous output of the controller + */ + void set_previous_control_output(ControllerOutput &previous_control_output); + + /** + * Returns a pair of the current vehicle state and whether or not it's valid + * + * @return the vehicle state and whether or not its valid. + */ + std::pair get_latest_state_and_validity(); + + /** + * Creates a new instance of the state tracker + */ + static void create(); + + /** + * Returns the state tracker instance + */ + static StateTracker& instance(); + + private: + + StateTracker(); + + template + void _handle_set_inverter_dynamics(std::shared_ptr msg); + + template + void _handle_set_inverter_temps(std::shared_ptr msg); + + template + bool _validate_timestamps(const std::array ×tamp_arr); + + void _receive_low_level_state(std::shared_ptr message); + + void _receive_inverter_states(std::shared_ptr message); + + VehicleState _vehicle_state = { }; + RawInputData _raw_input_data = { }; + std::mutex _state_mutex; + std::array _timestamp_array; + + inline static std::atomic _s_instance; + + + }; +} diff --git a/drivebrain_core/src/FoxgloveServer.cpp b/drivebrain_core/src/FoxgloveServer.cpp index d115d35..3b7203c 100644 --- a/drivebrain_core/src/FoxgloveServer.cpp +++ b/drivebrain_core/src/FoxgloveServer.cpp @@ -1,169 +1,169 @@ -#include - -static uint64_t nanosecondsSinceEpoch() { - return uint64_t(std::chrono::duration_cast( - std::chrono::system_clock::now().time_since_epoch()) - .count()); -} - - -static std::vector get_pb_descriptors(const std::vector &filenames) { - std::vector descriptors; - - for (const auto &name : filenames) { - const google::protobuf::FileDescriptor *file_descriptor = google::protobuf::DescriptorPool::generated_pool()->FindFileByName(name); - - if (!file_descriptor) { - std::cout << "File descriptor not found" << std::endl; - } - else { - descriptors.push_back(file_descriptor); - } - } - return descriptors; -} - -static std::string SerializeFdSet(const google::protobuf::Descriptor *toplevelDescriptor) { - google::protobuf::FileDescriptorSet fdSet; - std::queue toAdd; - toAdd.push(toplevelDescriptor->file()); - std::unordered_set seenDependencies; - while (!toAdd.empty()) - { - const google::protobuf::FileDescriptor *next = toAdd.front(); - toAdd.pop(); - next->CopyTo(fdSet.add_file()); - for (int i = 0; i < next->dependency_count(); ++i) - { - const auto &dep = next->dependency(i); - if (seenDependencies.find(dep->name()) == seenDependencies.end()) - { - seenDependencies.insert(dep->name()); - toAdd.push(dep); - } - } - } - return fdSet.SerializeAsString(); -} - -void core::FoxgloveServer::create(const std::string ¶meters_file) { - FoxgloveServer* expected = nullptr; - FoxgloveServer* local = new FoxgloveServer(parameters_file); - if(!_s_instance.compare_exchange_strong(expected, local, std::memory_order_release, std::memory_order_relaxed)) { - // Already initialized, delete local instance - delete local; - } -} - -core::FoxgloveServer& core::FoxgloveServer::instance() { - FoxgloveServer* instance = _s_instance.load(std::memory_order_acquire); - assert(instance != nullptr && "Foxglove server has not been initialized"); - return *instance; -} - -core::FoxgloveServer::FoxgloveServer(std::string file_name) { - - // Read params data - std::fstream params_file(file_name); - nlohmann::json init_param_data = nlohmann::json::parse(params_file); - - for (auto &[key, value]: init_param_data.items()) { - std::string param_name = key; - foxglove::ParameterValue param_value; - if (value.type() == nlohmann::detail::value_t::boolean) { - bool raw_value = value.get(); - param_value = foxglove::ParameterValue(raw_value); - } else if (value.type() == nlohmann::detail::value_t::number_float) { - float raw_value = value.get(); - param_value = foxglove::ParameterValue(raw_value); - } else if (value.type() == nlohmann::detail::value_t::number_integer || value.type() == nlohmann::detail::value_t::number_unsigned) { - int64_t raw_value = value.get(); - param_value = foxglove::ParameterValue(raw_value); - } else { - std::cerr << "Invalid parameter config type: " << value.type_name() << std::endl; - return; - } - - _foxglove_params_map[param_name] = param_value; - std::cout << "Added parameter: " << param_name << std::endl; - } - - // Instantiate handlers and create foxglove server - const auto logHandler = [](foxglove::WebSocketLogLevel, char const *msg) { - std::cout << msg << std::endl; - }; - - _server_options.capabilities.push_back("parameters"); - _server = foxglove::ServerFactory::createServer("Zephyr_Bridge", logHandler, _server_options); - - foxglove::ServerHandlers hdlrs; - - hdlrs.subscribeHandler = [&](foxglove::ChannelId chanId, foxglove::ConnHandle clientHandle) { - const auto clientStr = _server->remoteEndpointString(clientHandle); - std::cout << "Client " << clientStr << " subscribed to " << chanId << std::endl; - }; - - hdlrs.unsubscribeHandler = [&](foxglove::ChannelId chanId, foxglove::ConnHandle clientHandle) { - const auto clientStr = _server->remoteEndpointString(clientHandle); - std::cout << "Client " << clientStr << " unsubscribed from " << chanId << std::endl; - }; - - hdlrs.parameterChangeHandler = [&](const std::vector ¶ms, const std::optional &request_id, foxglove::ConnHandle clientHandle) - { - - std::unordered_map params_map; - - for (const auto ¶m_to_change : params) { - params_map[param_to_change.getName()] = param_to_change.getValue(); - } - - { - std::unique_lock lock(_parameter_mutex); - _foxglove_params_map = std::move(params_map); - } - }; - - hdlrs.parameterRequestHandler = [this](const std::vector ¶m_names, const std::optional &request_id, - foxglove::ConnHandle clientHandle) - { - std::vector foxglove_params; - for (auto &[key, value] : _foxglove_params_map) { - foxglove::Parameter param(key, value); - foxglove_params.push_back(param); - } - _server->publishParameterValues(clientHandle, foxglove_params, request_id); - }; - - auto descriptors = get_pb_descriptors({"hytech_msgs.proto"}); - std::vector channels; - - for (const auto &file_descriptor : descriptors) { - - for (int i = 1; i <= file_descriptor->message_type_count(); ++i) { - const google::protobuf::Descriptor *message_descriptor = file_descriptor->message_type(i); - foxglove::ChannelWithoutId server_channel; - server_channel.topic = message_descriptor->name(); - server_channel.encoding = "protobuf"; - server_channel.schemaName = message_descriptor->full_name(); - server_channel.schema = foxglove::base64Encode(SerializeFdSet(message_descriptor)); - _name_to_id_map[server_channel.topic] = i; - channels.push_back(server_channel); - } - } - - auto res_ids = _server->addChannels(channels); - - _server->setHandlers(std::move(hdlrs)); - _server->start("0.0.0.0", 5555); - - params_file.close(); -} - -void core::FoxgloveServer::send_live_telem_msg(std::shared_ptr msg) { - auto msg_chan_id = _name_to_id_map[msg->GetDescriptor()->name()]; - const auto serialized_msg = msg->SerializeAsString(); - const auto now = nanosecondsSinceEpoch(); - _server->broadcastMessage(msg_chan_id, now, reinterpret_cast(serialized_msg.data()), serialized_msg.size()); -} - - +#include + +static uint64_t nanosecondsSinceEpoch() { + return uint64_t(std::chrono::duration_cast( + std::chrono::system_clock::now().time_since_epoch()) + .count()); +} + + +static std::vector get_pb_descriptors(const std::vector &filenames) { + std::vector descriptors; + + for (const auto &name : filenames) { + const google::protobuf::FileDescriptor *file_descriptor = google::protobuf::DescriptorPool::generated_pool()->FindFileByName(name); + + if (!file_descriptor) { + std::cout << "File descriptor not found" << std::endl; + } + else { + descriptors.push_back(file_descriptor); + } + } + return descriptors; +} + +static std::string SerializeFdSet(const google::protobuf::Descriptor *toplevelDescriptor) { + google::protobuf::FileDescriptorSet fdSet; + std::queue toAdd; + toAdd.push(toplevelDescriptor->file()); + std::unordered_set seenDependencies; + while (!toAdd.empty()) + { + const google::protobuf::FileDescriptor *next = toAdd.front(); + toAdd.pop(); + next->CopyTo(fdSet.add_file()); + for (int i = 0; i < next->dependency_count(); ++i) + { + const auto &dep = next->dependency(i); + if (seenDependencies.find(dep->name()) == seenDependencies.end()) + { + seenDependencies.insert(dep->name()); + toAdd.push(dep); + } + } + } + return fdSet.SerializeAsString(); +} + +void core::FoxgloveServer::create(const std::string ¶meters_file) { + FoxgloveServer* expected = nullptr; + FoxgloveServer* local = new FoxgloveServer(parameters_file); + if(!_s_instance.compare_exchange_strong(expected, local, std::memory_order_release, std::memory_order_relaxed)) { + // Already initialized, delete local instance + delete local; + } +} + +core::FoxgloveServer& core::FoxgloveServer::instance() { + FoxgloveServer* instance = _s_instance.load(std::memory_order_acquire); + assert(instance != nullptr && "Foxglove server has not been initialized"); + return *instance; +} + +core::FoxgloveServer::FoxgloveServer(std::string file_name) { + + // Read params data + std::fstream params_file(file_name); + nlohmann::json init_param_data = nlohmann::json::parse(params_file); + + for (auto &[key, value]: init_param_data.items()) { + std::string param_name = key; + foxglove::ParameterValue param_value; + if (value.type() == nlohmann::detail::value_t::boolean) { + bool raw_value = value.get(); + param_value = foxglove::ParameterValue(raw_value); + } else if (value.type() == nlohmann::detail::value_t::number_float) { + float raw_value = value.get(); + param_value = foxglove::ParameterValue(raw_value); + } else if (value.type() == nlohmann::detail::value_t::number_integer || value.type() == nlohmann::detail::value_t::number_unsigned) { + int64_t raw_value = value.get(); + param_value = foxglove::ParameterValue(raw_value); + } else { + std::cerr << "Invalid parameter config type: " << value.type_name() << std::endl; + return; + } + + _foxglove_params_map[param_name] = param_value; + std::cout << "Added parameter: " << param_name << std::endl; + } + + // Instantiate handlers and create foxglove server + const auto logHandler = [](foxglove::WebSocketLogLevel, char const *msg) { + std::cout << msg << std::endl; + }; + + _server_options.capabilities.push_back("parameters"); + _server = foxglove::ServerFactory::createServer("Zephyr_Bridge", logHandler, _server_options); + + foxglove::ServerHandlers hdlrs; + + hdlrs.subscribeHandler = [&](foxglove::ChannelId chanId, foxglove::ConnHandle clientHandle) { + const auto clientStr = _server->remoteEndpointString(clientHandle); + std::cout << "Client " << clientStr << " subscribed to " << chanId << std::endl; + }; + + hdlrs.unsubscribeHandler = [&](foxglove::ChannelId chanId, foxglove::ConnHandle clientHandle) { + const auto clientStr = _server->remoteEndpointString(clientHandle); + std::cout << "Client " << clientStr << " unsubscribed from " << chanId << std::endl; + }; + + hdlrs.parameterChangeHandler = [&](const std::vector ¶ms, const std::optional &request_id, foxglove::ConnHandle clientHandle) + { + + std::unordered_map params_map; + + for (const auto ¶m_to_change : params) { + params_map[param_to_change.getName()] = param_to_change.getValue(); + } + + { + std::unique_lock lock(_parameter_mutex); + _foxglove_params_map = std::move(params_map); + } + }; + + hdlrs.parameterRequestHandler = [this](const std::vector ¶m_names, const std::optional &request_id, + foxglove::ConnHandle clientHandle) + { + std::vector foxglove_params; + for (auto &[key, value] : _foxglove_params_map) { + foxglove::Parameter param(key, value); + foxglove_params.push_back(param); + } + _server->publishParameterValues(clientHandle, foxglove_params, request_id); + }; + + auto descriptors = get_pb_descriptors({"hytech_msgs.proto"}); + std::vector channels; + + for (const auto &file_descriptor : descriptors) { + + for (int i = 1; i <= file_descriptor->message_type_count(); ++i) { + const google::protobuf::Descriptor *message_descriptor = file_descriptor->message_type(i); + foxglove::ChannelWithoutId server_channel; + server_channel.topic = message_descriptor->name(); + server_channel.encoding = "protobuf"; + server_channel.schemaName = message_descriptor->full_name(); + server_channel.schema = foxglove::base64Encode(SerializeFdSet(message_descriptor)); + _name_to_id_map[server_channel.topic] = i; + channels.push_back(server_channel); + } + } + + auto res_ids = _server->addChannels(channels); + + _server->setHandlers(std::move(hdlrs)); + _server->start("0.0.0.0", 5555); + + params_file.close(); +} + +void core::FoxgloveServer::send_live_telem_msg(std::shared_ptr msg) { + auto msg_chan_id = _name_to_id_map[msg->GetDescriptor()->name()]; + const auto serialized_msg = msg->SerializeAsString(); + const auto now = nanosecondsSinceEpoch(); + _server->broadcastMessage(msg_chan_id, now, reinterpret_cast(serialized_msg.data()), serialized_msg.size()); +} + + diff --git a/drivebrain_core/src/LapTracker.cpp b/drivebrain_core/src/LapTracker.cpp new file mode 100644 index 0000000..dcaf076 --- /dev/null +++ b/drivebrain_core/src/LapTracker.cpp @@ -0,0 +1,192 @@ +#include +#include +#include +#include +#include +core::LapTracker::LapTracker() : has_start_line(false), lap_counter(0), has_crossed_start_line(false), has_prev_state(false), best_lap_time(std::numeric_limits::max()),max_lap_speed(0.0f){ + prev_state = {}; + curr_lap_start_time = std::chrono::steady_clock::now(); +} + +void core::LapTracker::step_tracker(core::VehicleState& latest_state) { + std::shared_ptr laptime_information = std::make_shared(); // TODO you need to fill in the fields of this protobuf message + + /** + * TODO it is your responsibility to fill in this method. Look at the VehicleState + * struct to see all the things it gives you (you should see vn position from there). Using this + * and the private variables you added in the header, complete this method. It should use all of the information + * in the latest state to update it's local variables, create a LapTime protobuf, and invoke handle_receive_protobuf_message + * on the state tracker. Some of this is completed for you. Good luck! + */ + + //Defining starting line + //HARDCODED TRACK WIDTH VALUE (assuming car starts from middle of track) + if(!has_prev_state && latest_state.state_is_valid) { + prev_state = latest_state; + has_prev_state=true; + return; + } + else if (!has_prev_state) { + return; + } + float track_width = 10.0; + if (!has_start_line && latest_state.state_is_valid) { + Position p0 = latest_state.vehicle_position; + xyz_vec vel = latest_state.current_body_vel_ms; + if (magnitude(vel) > 0.1f) { + std::pair offset=offsetFinder(vel); + std::pair newOffset = {offset.first * track_width/2,offset.second*track_width/2}; + + Position startOne = {p0.lat - meter_to_degree_latitude(newOffset.second), p0.lon - meter_to_degree_longitude(newOffset.first), true}; + Position startTwo = {p0.lat + meter_to_degree_latitude(newOffset.second), p0.lon + meter_to_degree_longitude(newOffset.first), true}; + setStartLine(startOne, startTwo); + has_start_line = true; + } + prev_state = latest_state; + return; + } + //Lap detection logic + if(!has_start_line || !latest_state.state_is_valid) { + return; + } + + Point p1 = {start_line.start.lat,start_line.start.lon}; + Point p2 = {start_line.end.lat,start_line.end.lon}; + Point p3 = {prev_state.vehicle_position.lat,prev_state.vehicle_position.lon}; + Point p4 = {latest_state.vehicle_position.lat,latest_state.vehicle_position.lon}; + + + bool crossed = doIntersect(p1,p2,p3,p4); + + if (crossed && !has_crossed_start_line) { + auto now = std::chrono::steady_clock::now(); + if (lap_counter>0) { + auto elapsed = std::chrono::duration(now - curr_lap_start_time); + float laptime_seconds = elapsed.count(); + if (laptime_seconds < best_lap_time) { + best_lap_time = laptime_seconds; + } + laptime_information->set_best_lap_time(best_lap_time); + laptime_information->set_delta_to_best(laptime_seconds - best_lap_time); + laptime_information->set_laptime_seconds(laptime_seconds); + } + lap_counter++; + + curr_lap_start_time=std::chrono::steady_clock::now(); + + + has_crossed_start_line=true; + + + + } + else if (!crossed) { + has_crossed_start_line=false; + } + + //max runtime velo + xyz_vec curr_vel = latest_state.current_body_vel_ms; + float curr_speed = magnitude(curr_vel); + if (curr_speed > max_lap_speed) { + max_lap_speed = curr_speed; + } + laptime_information->set_max_lap_speed(max_lap_speed); + laptime_information->set_lapcount(lap_counter); + + + + + + + + + if(best_lap_time < std::numeric_limits::max()) { + laptime_information->set_best_lap_time(best_lap_time); + } + + prev_state=latest_state; + core::StateTracker::instance().handle_receive_protobuf_message(laptime_information); // What "records" the information + +} + +void core::LapTracker::create() { + LapTracker* expected = nullptr; + LapTracker* local = new LapTracker(); + if(!_s_instance.compare_exchange_strong(expected, local, std::memory_order_release, std::memory_order_relaxed)) { + // Already initialized, delete local instance + delete local; + } + +} + + +int core::LapTracker::orientation(core::Point p, core::Point q, core::Point r) { + double val = (q.y-p.y) * (r.x-q.x) - (q.x-p.x) * (r.y-q.y); + if (std::abs(val) < 0.0001) return 0; + return (val > 0) ? 1:2; +} + +bool core::LapTracker::onSegment(Point p, Point q, Point r) { + return (q.x<=std::max(p.x,r.x) && q.x >= std::min(p.x,r.x) && q.y<=std::max(p.y,r.y) && q.y>=std::min(p.y,r.y)); +} + +bool core::LapTracker::doIntersect(Point p1, Point q1, Point p2, Point q2) { + int o1 = orientation(p1,q1,p2); + int o2 = orientation(p1,q1,q2); + int o3 = orientation(p2,q2,p1); + int o4 = orientation(p2,q2,q1); + + if(o1 != o2 && o3 != o4) return true; + + if (o1 == 0 && onSegment(p1, p2, q1)) return true; + if (o2 == 0 && onSegment(p1, q2, q1)) return true; + if (o3 == 0 && onSegment(p2, p1, q2)) return true; + if (o4 == 0 && onSegment(p2, q1, q2)) return true; + return false; +} + +float core::LapTracker::magnitude(core::xyz_vec v) { + return sqrt(v.x * v.x + v.y * v.y + v.z * v.z); +} + +//using reference latitude: 42.0676000000°N, -84.2424000000°E (michigan racetrack) +float core::LapTracker::meter_to_degree_latitude(float m) { + constexpr float DEG_TO_RAD = static_cast(M_PI)/180.0f; + constexpr float phi = 42.06760000000f * DEG_TO_RAD; + + const float metersPerDegLat = 111132.92f - 559.82f * std::cos(2.0f * phi) + 1.175f * std::cos(4.0f * phi) - 0.0023f * std::cos(6.0f * phi); + return m/metersPerDegLat; +} + +float core::LapTracker::meter_to_degree_longitude(float m) { +constexpr float DEG_TO_RAD = static_cast(M_PI) / 180.0f; + constexpr float phi = 42.0676000000f * DEG_TO_RAD; + + const float metersPerDegLon = + 111412.84f * std::cos(phi) + - 93.5f * std::cos(3.0f * phi) + + 0.118f * std::cos(5.0f * phi); + + return m / metersPerDegLon; +} + + +std::pair core::LapTracker::offsetFinder(core::xyz_vec v) { + float mag = magnitude(v); + return {(-v.y/mag),(v.x/mag)}; +} + + +//setter to set start line +void core::LapTracker::setStartLine(const Position& start, const Position& end) { + start_line.start=start; + start_line.end=end; +} + + +core::LapTracker& core::LapTracker::instance() { + LapTracker* instance = _s_instance.load(std::memory_order_acquire); + assert(instance != nullptr && "LapTracker has not been initialized"); + return *instance; +} + diff --git a/drivebrain_core/src/MCAPLogger.cpp b/drivebrain_core/src/MCAPLogger.cpp index 2aaa112..c0b938f 100644 --- a/drivebrain_core/src/MCAPLogger.cpp +++ b/drivebrain_core/src/MCAPLogger.cpp @@ -1,179 +1,179 @@ -#include -#include -#include -#define MCAP_IMPLEMENTATION - -#include - -/**************************************************************** - * HELPER METHODS - ****************************************************************/ -static std::vector get_pb_descriptors(const std::vector &filenames) { - std::vector descriptors; - - for (const auto &name : filenames) { - const google::protobuf::FileDescriptor *file_descriptor = google::protobuf::DescriptorPool::generated_pool()->FindFileByName(name); - - if (!file_descriptor) { - std::cout << "File descriptor not found" << std::endl; - } - else { - descriptors.push_back(file_descriptor); - } - } - return descriptors; -} - -static std::string serialize_fd_set(const google::protobuf::Descriptor *toplevelDescriptor) { - google::protobuf::FileDescriptorSet fdSet; - std::queue toAdd; - toAdd.push(toplevelDescriptor->file()); - std::unordered_set seenDependencies; - while (!toAdd.empty()) - { - const google::protobuf::FileDescriptor *next = toAdd.front(); - toAdd.pop(); - next->CopyTo(fdSet.add_file()); - for (int i = 0; i < next->dependency_count(); ++i) - { - const auto &dep = next->dependency(i); - if (seenDependencies.find(dep->name()) == seenDependencies.end()) - { - seenDependencies.insert(dep->name()); - toAdd.push(dep); - } - } - } - return fdSet.SerializeAsString(); -} - -static std::string create_log_name() { - -} - -/**************************************************************** - * PUBLIC CLASS METHOD IMPLEMENTATIONS - ****************************************************************/ - -void core::MCAPLogger::create(const std::string &base_dir, const mcap::McapWriterOptions &options) { - MCAPLogger* expected = nullptr; - MCAPLogger* local = new MCAPLogger(base_dir, options); - if(!_s_instance.compare_exchange_strong(expected, local, std::memory_order_release, std::memory_order_relaxed)) { - // Already initialized, delete local instance - delete local; - } -} - -core::MCAPLogger& core::MCAPLogger::instance() { - MCAPLogger* instance = _s_instance.load(std::memory_order_acquire); - assert(instance != nullptr && "MCAPLogger has not been initialized"); - return *instance; -} - -int core::MCAPLogger::open_new_mcap(const std::string &name) { - std::cout << "Attempting to open new MCAP file" << std::endl; - - const auto res = _writer.open(name, _options); - if (!res.ok()) { - std::cout << "Failed to open MCAP :(" << std::endl; - return -1; - } - - std::vector proto_names = {"hytech_msgs.proto", "hytech.proto"}; - auto descriptors = get_pb_descriptors(proto_names); - - for (const auto &file_descriptor : descriptors) { - for (int i = 1; i <= file_descriptor->message_type_count(); i++) { - const google::protobuf::Descriptor *message_descriptor = file_descriptor->message_type(i); - mcap::Schema schema(message_descriptor->full_name(), "protobuf", serialize_fd_set(message_descriptor)); - _writer.addSchema(schema); - mcap::Channel channel(message_descriptor->name(), "protobuf", schema.id); - _writer.addChannel(channel); - _name_to_id_map[message_descriptor->name()] = channel.id; - - } - } - - std::cout << "Successfully opened and added message descriptions to mcap" << std::endl; - return 0; -} - -int core::MCAPLogger::close_active_mcap() { - std::cout << "Closing mcap" << std::endl; - _writer.close(); - - return 0; -} - -void core::MCAPLogger::init_logging() { - _msg_log_thread = std::thread([this]() { _handle_log_to_file(); }); - spdlog::info("Msg log thread spawned"); - _param_log_thread = std::thread([this]() { _handle_param_log(); }); - spdlog::info("Param log thread spawned"); - - _logging = true; -} - -int core::MCAPLogger::log_msg(core::MsgType message) { - mcap::Timestamp log_time = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); - RawMessage_s new_message; - new_message.log_time = log_time; - new_message.serialized_data = message->SerializeAsString(); - new_message.message_name = message->GetDescriptor()->name(); - { - std::unique_lock lock(_input_buffer_mutex); - _input_buffer.push_back(new_message); - _input_buffer_cv.notify_one(); - } - - return 0; -} - -/**************************************************************** - * PRIVATE CLASS METHOD IMPLEMENTATIONS - ****************************************************************/ -core::MCAPLogger::MCAPLogger(const std::string &base_dir, const mcap::McapWriterOptions &options) : _options(options) { - // TODO: spawn logging thread -} - -void core::MCAPLogger::_handle_log_to_file() { - static std::deque write_buffer; // The buffer to be copied over to - - while (true) { - { - std::unique_lock lock(_input_buffer_mutex); - _input_buffer_cv.wait(lock, [this](){ return !_input_buffer.empty() || !_running;}); - if (_running && _input_buffer.empty()) { - break; - } - - write_buffer = std::move(_input_buffer); - } - - for (auto &msg : write_buffer) { - mcap::Message msg_to_log; - msg_to_log.data = reinterpret_cast(msg.serialized_data.data()); - msg_to_log.dataSize = msg.serialized_data.size(); - - msg_to_log.logTime = msg.log_time; - msg_to_log.publishTime = msg.log_time; - - msg_to_log.channelId = _name_to_id_map[msg.message_name]; - auto write_res = _writer.write(msg_to_log); - - } - } -} - -void core::MCAPLogger::_handle_param_log() { - std::chrono::seconds param_log_time(1); - while(true) { - std::unique_lock lk(_param_mutex); - if(_param_cv.wait_for(lk, param_log_time, [this] { return !_logging; }) || !_running) { - spdlog::info("Logging stopped/logger is no longer running. Exiting..."); - return; - } - - } -} - +#include +#include +#include +#define MCAP_IMPLEMENTATION + +#include + +/**************************************************************** + * HELPER METHODS + ****************************************************************/ +static std::vector get_pb_descriptors(const std::vector &filenames) { + std::vector descriptors; + + for (const auto &name : filenames) { + const google::protobuf::FileDescriptor *file_descriptor = google::protobuf::DescriptorPool::generated_pool()->FindFileByName(name); + + if (!file_descriptor) { + std::cout << "File descriptor not found" << std::endl; + } + else { + descriptors.push_back(file_descriptor); + } + } + return descriptors; +} + +static std::string serialize_fd_set(const google::protobuf::Descriptor *toplevelDescriptor) { + google::protobuf::FileDescriptorSet fdSet; + std::queue toAdd; + toAdd.push(toplevelDescriptor->file()); + std::unordered_set seenDependencies; + while (!toAdd.empty()) + { + const google::protobuf::FileDescriptor *next = toAdd.front(); + toAdd.pop(); + next->CopyTo(fdSet.add_file()); + for (int i = 0; i < next->dependency_count(); ++i) + { + const auto &dep = next->dependency(i); + if (seenDependencies.find(dep->name()) == seenDependencies.end()) + { + seenDependencies.insert(dep->name()); + toAdd.push(dep); + } + } + } + return fdSet.SerializeAsString(); +} + +static std::string create_log_name() { + +} + +/**************************************************************** + * PUBLIC CLASS METHOD IMPLEMENTATIONS + ****************************************************************/ + +void core::MCAPLogger::create(const std::string &base_dir, const mcap::McapWriterOptions &options) { + MCAPLogger* expected = nullptr; + MCAPLogger* local = new MCAPLogger(base_dir, options); + if(!_s_instance.compare_exchange_strong(expected, local, std::memory_order_release, std::memory_order_relaxed)) { + // Already initialized, delete local instance + delete local; + } +} + +core::MCAPLogger& core::MCAPLogger::instance() { + MCAPLogger* instance = _s_instance.load(std::memory_order_acquire); + assert(instance != nullptr && "MCAPLogger has not been initialized"); + return *instance; +} + +int core::MCAPLogger::open_new_mcap(const std::string &name) { + std::cout << "Attempting to open new MCAP file" << std::endl; + + const auto res = _writer.open(name, _options); + if (!res.ok()) { + std::cout << "Failed to open MCAP :(" << std::endl; + return -1; + } + + std::vector proto_names = {"hytech_msgs.proto", "hytech.proto"}; + auto descriptors = get_pb_descriptors(proto_names); + + for (const auto &file_descriptor : descriptors) { + for (int i = 1; i <= file_descriptor->message_type_count(); i++) { + const google::protobuf::Descriptor *message_descriptor = file_descriptor->message_type(i); + mcap::Schema schema(message_descriptor->full_name(), "protobuf", serialize_fd_set(message_descriptor)); + _writer.addSchema(schema); + mcap::Channel channel(message_descriptor->name(), "protobuf", schema.id); + _writer.addChannel(channel); + _name_to_id_map[message_descriptor->name()] = channel.id; + + } + } + + std::cout << "Successfully opened and added message descriptions to mcap" << std::endl; + return 0; +} + +int core::MCAPLogger::close_active_mcap() { + std::cout << "Closing mcap" << std::endl; + _writer.close(); + + return 0; +} + +void core::MCAPLogger::init_logging() { + _msg_log_thread = std::thread([this]() { _handle_log_to_file(); }); + spdlog::info("Msg log thread spawned"); + _param_log_thread = std::thread([this]() { _handle_param_log(); }); + spdlog::info("Param log thread spawned"); + + _logging = true; +} + +int core::MCAPLogger::log_msg(core::MsgType message) { + mcap::Timestamp log_time = std::chrono::duration_cast(std::chrono::system_clock::now().time_since_epoch()).count(); + RawMessage_s new_message; + new_message.log_time = log_time; + new_message.serialized_data = message->SerializeAsString(); + new_message.message_name = message->GetDescriptor()->name(); + { + std::unique_lock lock(_input_buffer_mutex); + _input_buffer.push_back(new_message); + _input_buffer_cv.notify_one(); + } + + return 0; +} + +/**************************************************************** + * PRIVATE CLASS METHOD IMPLEMENTATIONS + ****************************************************************/ +core::MCAPLogger::MCAPLogger(const std::string &base_dir, const mcap::McapWriterOptions &options) : _options(options) { + // TODO: spawn logging thread +} + +void core::MCAPLogger::_handle_log_to_file() { + static std::deque write_buffer; // The buffer to be copied over to + + while (true) { + { + std::unique_lock lock(_input_buffer_mutex); + _input_buffer_cv.wait(lock, [this](){ return !_input_buffer.empty() || !_running;}); + if (_running && _input_buffer.empty()) { + break; + } + + write_buffer = std::move(_input_buffer); + } + + for (auto &msg : write_buffer) { + mcap::Message msg_to_log; + msg_to_log.data = reinterpret_cast(msg.serialized_data.data()); + msg_to_log.dataSize = msg.serialized_data.size(); + + msg_to_log.logTime = msg.log_time; + msg_to_log.publishTime = msg.log_time; + + msg_to_log.channelId = _name_to_id_map[msg.message_name]; + auto write_res = _writer.write(msg_to_log); + + } + } +} + +void core::MCAPLogger::_handle_param_log() { + std::chrono::seconds param_log_time(1); + while(true) { + std::unique_lock lk(_param_mutex); + if(_param_cv.wait_for(lk, param_log_time, [this] { return !_logging; }) || !_running) { + spdlog::info("Logging stopped/logger is no longer running. Exiting..."); + return; + } + + } +} + diff --git a/drivebrain_core/src/StateTracker.cpp b/drivebrain_core/src/StateTracker.cpp index c4461c4..56a38a9 100644 --- a/drivebrain_core/src/StateTracker.cpp +++ b/drivebrain_core/src/StateTracker.cpp @@ -1,266 +1,287 @@ -#include - -using namespace core; - -/**************************************************************** - * Public class methods - ****************************************************************/ -void StateTracker::set_previous_control_output(core::ControllerOutput &prev_controller_output) { - std::unique_lock lk(_state_mutex); - _vehicle_state.prev_controller_output = prev_controller_output; -} - -void StateTracker::handle_receive_protobuf_message(std::shared_ptr msg) { - if (msg->GetDescriptor() == hytech_msgs::VNData::descriptor()) { - auto in_msg = std::static_pointer_cast(msg); - xyz_vec body_vel_ms = {(in_msg->vn_vel_m_s().x()), (in_msg->vn_vel_m_s().y()), - (in_msg->vn_vel_m_s().z())}; - - xyz_vec body_accel_mss = {(in_msg->vn_linear_accel_m_ss().x()), - (in_msg->vn_linear_accel_m_ss().y()), - (in_msg->vn_linear_accel_m_ss().z())}; - - xyz_vec angular_rate_rads = {(in_msg->vn_angular_rate_rad_s().x()), - (in_msg->vn_angular_rate_rad_s().y()), - (in_msg->vn_angular_rate_rad_s().z())}; - - ypr_vec ypr_rad = {(in_msg->vn_ypr_rad().yaw()), (in_msg->vn_ypr_rad().pitch()), - (in_msg->vn_ypr_rad().roll())}; - - auto ins_mode_int = in_msg->status().ins_mode_int(); - auto vel_u = in_msg->status().ins_vel_u(); - - { - std::unique_lock lk(_state_mutex); - _vehicle_state.current_body_vel_ms = body_vel_ms; - _vehicle_state.current_body_accel_mss = body_accel_mss; - _vehicle_state.current_angular_rate_rads = angular_rate_rads; - _vehicle_state.current_ypr_rad = ypr_rad; - _vehicle_state.ins_status.status_mode = ins_mode_int; - _vehicle_state.ins_status.vel_uncertainty = vel_u; - } - } else if (msg->GetDescriptor() == hytech_msgs::VCRData_s::descriptor()) { - auto in_msg = std::static_pointer_cast(msg); - bool is_rtd = (in_msg->status().vehicle_state() == hytech_msgs::VehicleState_e::READY_TO_DRIVE); - { - std::unique_lock lk(_state_mutex); - _vehicle_state.is_ready_to_drive = is_rtd; - } - } else if (msg->GetDescriptor() == hytech_msgs::ACUAllData::descriptor()) { - auto in_msg = std::static_pointer_cast(msg); - { - std::unique_lock lk(_state_mutex); - _vehicle_state.acc_data.min_cell_voltage = in_msg->core_data().min_cell_voltage(); - } - } - else { - _receive_low_level_state(msg); - } -} - -std::pair StateTracker::get_latest_state_and_validity() { - auto state_is_valid = _validate_timestamps(_timestamp_array); - - VehicleState current_state = { }; - - { - std::unique_lock lk(_state_mutex); - current_state = _vehicle_state; - } - - return {current_state, state_is_valid}; -} - -/**************************************************************** - * Private class methods - ****************************************************************/ -template -void StateTracker::_handle_set_inverter_dynamics(std::shared_ptr msg) { - auto in_msg = std::static_pointer_cast(msg); - { - std::unique_lock lk(_state_mutex); - _raw_input_data.raw_inverter_torques.set_from_index(in_msg->actual_torque_nm()); - _raw_input_data.raw_inverter_power.set_from_index(in_msg->actual_power_w()); - _vehicle_state.current_rpms.set_from_index(in_msg->actual_speed_rpm()); - _vehicle_state.current_torques_nm = _raw_input_data.raw_inverter_torques; - } -} - -template -void StateTracker::_handle_set_inverter_temps(std::shared_ptr msg) { - - auto in_msg = std::static_pointer_cast(msg); - core::DrivetrainData dt_data = {}; - dt_data.inverter_igbt_temps_c.set_from_index(in_msg->igbt_temp()); - dt_data.inverter_temps_c.set_from_index(in_msg->inverter_temp()); - dt_data.inverter_motor_temps_c.set_from_index(in_msg->motor_temp()); - { - std::unique_lock lk(_state_mutex); - _vehicle_state.dt_data = dt_data; - } -} - -void StateTracker::_receive_low_level_state(std::shared_ptr msg) { - if (msg->GetDescriptor() == hytech::rear_suspension::descriptor()) { - auto in_msg = std::static_pointer_cast(msg); - { - std::unique_lock lk(_state_mutex); - _timestamp_array[0] = std::chrono::duration_cast( - std::chrono::high_resolution_clock::now().time_since_epoch()); - _raw_input_data.raw_load_cell_values.RL = in_msg->rl_load_cell(); - _raw_input_data.raw_load_cell_values.RR = in_msg->rr_load_cell(); - _raw_input_data.raw_shock_pot_values.RL = in_msg->rl_shock_pot(); - _raw_input_data.raw_shock_pot_values.RR = in_msg->rr_shock_pot(); - } - } else if (msg->GetDescriptor() == hytech::front_suspension::descriptor()) { - auto in_msg = std::static_pointer_cast(msg); - { - std::unique_lock lk(_state_mutex); - _timestamp_array[1] = std::chrono::duration_cast( - std::chrono::high_resolution_clock::now().time_since_epoch()); - _raw_input_data.raw_load_cell_values.FL = in_msg->fl_load_cell(); - _raw_input_data.raw_load_cell_values.FR = in_msg->fr_load_cell(); - _raw_input_data.raw_shock_pot_values.FL = in_msg->fl_shock_pot(); - _raw_input_data.raw_shock_pot_values.FR = in_msg->fr_shock_pot(); - } - } else if (msg->GetDescriptor() == hytech::pedals_system_data::descriptor()) { - auto in_msg = std::static_pointer_cast(msg); - core::DriverInput input = {(in_msg->accel_pedal()), (in_msg->brake_pedal())}; - { - std::unique_lock lk(_state_mutex); - _timestamp_array[2] = std::chrono::duration_cast( - std::chrono::high_resolution_clock::now().time_since_epoch()); - _vehicle_state.input = input; - } - } else if (msg->GetDescriptor() == hytech::steering_data::descriptor()) { - auto in_msg = std::static_pointer_cast(msg); - { - std::unique_lock lk(_state_mutex); - _timestamp_array[3] = std::chrono::duration_cast( - std::chrono::high_resolution_clock::now().time_since_epoch()); - _raw_input_data.raw_steering_analog = in_msg->steering_analog_raw(); - _raw_input_data.raw_steering_digital = in_msg->steering_digital_raw(); - } - } else if (msg->GetDescriptor() == hytech::em_measurement::descriptor()) { - auto in_msg = std::static_pointer_cast(msg); - float em_voltage = in_msg->em_voltage(); - float em_current = in_msg->em_current(); - { - std::unique_lock lk(_state_mutex); - _vehicle_state.old_energy_meter_kw = (-1.0f * em_voltage * em_current / 1000.0f); - } - } - else { - _receive_inverter_states(msg); - } -} - -void StateTracker::_receive_inverter_states(std::shared_ptr msg) { - auto descriptor = msg->GetDescriptor(); - if (descriptor == hytech::inv1_dynamics::descriptor()) { - _handle_set_inverter_dynamics<0, hytech::inv1_dynamics>(msg); - } else if (descriptor == hytech::inv2_dynamics::descriptor()) { - _handle_set_inverter_dynamics<1, hytech::inv2_dynamics>(msg); - } else if (descriptor == hytech::inv3_dynamics::descriptor()) { - _handle_set_inverter_dynamics<2, hytech::inv3_dynamics>(msg); - } else if (descriptor == hytech::inv4_dynamics::descriptor()) { - _handle_set_inverter_dynamics<3, hytech::inv4_dynamics>(msg); - } else if (descriptor == hytech::inv1_temps::descriptor()) { - auto in_msg = std::static_pointer_cast(msg); - { - std::unique_lock lk(_state_mutex); - _vehicle_state.dt_data.inverter_igbt_temps_c.FL = in_msg->igbt_temp(); - _vehicle_state.dt_data.inverter_temps_c.FL = in_msg->inverter_temp(); - _vehicle_state.dt_data.inverter_motor_temps_c.FL = in_msg->motor_temp(); - } - } else if (descriptor == hytech::inv2_temps::descriptor()) { - auto in_msg = std::static_pointer_cast(msg); - { - std::unique_lock lk(_state_mutex); - _vehicle_state.dt_data.inverter_igbt_temps_c.FR = in_msg->igbt_temp(); - _vehicle_state.dt_data.inverter_temps_c.FR = in_msg->inverter_temp(); - _vehicle_state.dt_data.inverter_motor_temps_c.FR = in_msg->motor_temp(); - } - } else if (descriptor == hytech::inv3_temps::descriptor()) { - auto in_msg = std::static_pointer_cast(msg); - { - std::unique_lock lk(_state_mutex); - _vehicle_state.dt_data.inverter_igbt_temps_c.RL = in_msg->igbt_temp(); - _vehicle_state.dt_data.inverter_temps_c.RL = in_msg->inverter_temp(); - _vehicle_state.dt_data.inverter_motor_temps_c.RL = in_msg->motor_temp(); - } - } else if (descriptor == hytech::inv4_temps::descriptor()) { - auto in_msg = std::static_pointer_cast(msg); - { - std::unique_lock lk(_state_mutex); - _vehicle_state.dt_data.inverter_igbt_temps_c.RR = in_msg->igbt_temp(); - _vehicle_state.dt_data.inverter_temps_c.RR = in_msg->inverter_temp(); - _vehicle_state.dt_data.inverter_motor_temps_c.RR = in_msg->motor_temp(); - } - } else if (descriptor == hytech::inv1_overload::descriptor()) { - auto in_msg = std::static_pointer_cast(msg); - { - std::unique_lock lk(_state_mutex); - _vehicle_state.motor_overload_percentages.FL = in_msg->motor_overload_percentage(); - } - } else if (descriptor == hytech::inv2_overload::descriptor()) { - auto in_msg = std::static_pointer_cast(msg); - { - std::unique_lock lk(_state_mutex); - _vehicle_state.motor_overload_percentages.FR = in_msg->motor_overload_percentage(); - } - } else if (descriptor == hytech::inv3_overload::descriptor()) { - auto in_msg = std::static_pointer_cast(msg); - { - std::unique_lock lk(_state_mutex); - _vehicle_state.motor_overload_percentages.RL = in_msg->motor_overload_percentage(); - } - } else if (descriptor == hytech::inv4_overload::descriptor()) { - auto in_msg = std::static_pointer_cast(msg); - { - std::unique_lock lk(_state_mutex); - _vehicle_state.motor_overload_percentages.RR = in_msg->motor_overload_percentage(); - } - } else if(descriptor == hytech::inv1_status::descriptor()) { - - auto in_msg = std::static_pointer_cast(msg); - auto voltage = in_msg->dc_bus_voltage(); - { - std::unique_lock lk(_state_mutex); - _vehicle_state.acc_data.pack_voltage = static_cast(voltage); - } - } -} - -template -bool StateTracker::_validate_timestamps(const std::array ×tamp_arr) { - std::array timestamp_array_to_sort; - - { - std::unique_lock lk(_state_mutex); - timestamp_array_to_sort = timestamp_arr; - } - - auto debug_copy = timestamp_array_to_sort; - const std::chrono::microseconds threshold(500000); // 500 milliseconds in microseconds - - // Sort the array - std::sort(timestamp_array_to_sort.begin(), timestamp_array_to_sort.end()); - - // Check if the range between the smallest and largest timestamps is within the threshold - auto min_stamp = timestamp_array_to_sort.front(); - auto max_stamp = timestamp_array_to_sort.back(); - - bool within_threshold = (max_stamp - min_stamp) <= threshold; - - auto curr_time = std::chrono::duration_cast( - std::chrono::high_resolution_clock::now().time_since_epoch()); - - constexpr std::chrono::seconds debug_print_period(1); - - bool all_members_received = min_stamp.count() > 0; // count here is the count in microseconds - bool last_update_recent_enough = - (std::chrono::duration_cast(curr_time - max_stamp)) < threshold; - - return within_threshold && all_members_received && last_update_recent_enough; -} \ No newline at end of file +#include + +using namespace core; + +/**************************************************************** + * Public class methods + ****************************************************************/ +void StateTracker::set_previous_control_output(core::ControllerOutput &prev_controller_output) { + std::unique_lock lk(_state_mutex); + _vehicle_state.prev_controller_output = prev_controller_output; +} + +void StateTracker::handle_receive_protobuf_message(std::shared_ptr msg) { + if (msg->GetDescriptor() == hytech_msgs::VNData::descriptor()) { + auto in_msg = std::static_pointer_cast(msg); + xyz_vec body_vel_ms = {(in_msg->vn_vel_m_s().x()), (in_msg->vn_vel_m_s().y()), + (in_msg->vn_vel_m_s().z())}; + + xyz_vec body_accel_mss = {(in_msg->vn_linear_accel_m_ss().x()), + (in_msg->vn_linear_accel_m_ss().y()), + (in_msg->vn_linear_accel_m_ss().z())}; + + xyz_vec angular_rate_rads = {(in_msg->vn_angular_rate_rad_s().x()), + (in_msg->vn_angular_rate_rad_s().y()), + (in_msg->vn_angular_rate_rad_s().z())}; + + ypr_vec ypr_rad = {(in_msg->vn_ypr_rad().yaw()), (in_msg->vn_ypr_rad().pitch()), + (in_msg->vn_ypr_rad().roll())}; + + auto ins_mode_int = in_msg->status().ins_mode_int(); + auto vel_u = in_msg->status().ins_vel_u(); + + { + std::unique_lock lk(_state_mutex); + _vehicle_state.current_body_vel_ms = body_vel_ms; + _vehicle_state.current_body_accel_mss = body_accel_mss; + _vehicle_state.current_angular_rate_rads = angular_rate_rads; + _vehicle_state.current_ypr_rad = ypr_rad; + _vehicle_state.ins_status.status_mode = ins_mode_int; + _vehicle_state.ins_status.vel_uncertainty = vel_u; + } + } else if (msg->GetDescriptor() == hytech_msgs::VCRData_s::descriptor()) { + auto in_msg = std::static_pointer_cast(msg); + bool is_rtd = (in_msg->status().vehicle_state() == hytech_msgs::VehicleState_e::READY_TO_DRIVE); + { + std::unique_lock lk(_state_mutex); + _vehicle_state.is_ready_to_drive = is_rtd; + } + } else if (msg->GetDescriptor() == hytech_msgs::ACUAllData::descriptor()) { + auto in_msg = std::static_pointer_cast(msg); + { + std::unique_lock lk(_state_mutex); + _vehicle_state.acc_data.min_cell_voltage = in_msg->core_data().min_cell_voltage(); + } + } else if (msg->GetDescriptor() == hytech_msgs::LapTime::descriptor()) { + auto in_msg = std::static_pointer_cast(msg); + { + std::unique_lock lk(_state_mutex); + _vehicle_state.laptime_info.laptime_seconds = in_msg->laptime_seconds(); + _vehicle_state.laptime_info.lapcount = in_msg->lapcount(); + } + } else { + _receive_low_level_state(msg); + } +} + +std::pair StateTracker::get_latest_state_and_validity() { + auto state_is_valid = _validate_timestamps(_timestamp_array); + + VehicleState current_state = { }; + + { + std::unique_lock lk(_state_mutex); + current_state = _vehicle_state; + } + + return {current_state, state_is_valid}; +} + +/**************************************************************** + * Private class methods + ****************************************************************/ +template +void StateTracker::_handle_set_inverter_dynamics(std::shared_ptr msg) { + auto in_msg = std::static_pointer_cast(msg); + { + std::unique_lock lk(_state_mutex); + _raw_input_data.raw_inverter_torques.set_from_index(in_msg->actual_torque_nm()); + _raw_input_data.raw_inverter_power.set_from_index(in_msg->actual_power_w()); + _vehicle_state.current_rpms.set_from_index(in_msg->actual_speed_rpm()); + _vehicle_state.current_torques_nm = _raw_input_data.raw_inverter_torques; + } +} + +template +void StateTracker::_handle_set_inverter_temps(std::shared_ptr msg) { + + auto in_msg = std::static_pointer_cast(msg); + core::DrivetrainData dt_data = {}; + dt_data.inverter_igbt_temps_c.set_from_index(in_msg->igbt_temp()); + dt_data.inverter_temps_c.set_from_index(in_msg->inverter_temp()); + dt_data.inverter_motor_temps_c.set_from_index(in_msg->motor_temp()); + { + std::unique_lock lk(_state_mutex); + _vehicle_state.dt_data = dt_data; + } +} + +void StateTracker::_receive_low_level_state(std::shared_ptr msg) { + if (msg->GetDescriptor() == hytech::rear_suspension::descriptor()) { + auto in_msg = std::static_pointer_cast(msg); + { + std::unique_lock lk(_state_mutex); + _timestamp_array[0] = std::chrono::duration_cast( + std::chrono::high_resolution_clock::now().time_since_epoch()); + _raw_input_data.raw_load_cell_values.RL = in_msg->rl_load_cell(); + _raw_input_data.raw_load_cell_values.RR = in_msg->rr_load_cell(); + _raw_input_data.raw_shock_pot_values.RL = in_msg->rl_shock_pot(); + _raw_input_data.raw_shock_pot_values.RR = in_msg->rr_shock_pot(); + } + } else if (msg->GetDescriptor() == hytech::front_suspension::descriptor()) { + auto in_msg = std::static_pointer_cast(msg); + { + std::unique_lock lk(_state_mutex); + _timestamp_array[1] = std::chrono::duration_cast( + std::chrono::high_resolution_clock::now().time_since_epoch()); + _raw_input_data.raw_load_cell_values.FL = in_msg->fl_load_cell(); + _raw_input_data.raw_load_cell_values.FR = in_msg->fr_load_cell(); + _raw_input_data.raw_shock_pot_values.FL = in_msg->fl_shock_pot(); + _raw_input_data.raw_shock_pot_values.FR = in_msg->fr_shock_pot(); + } + } else if (msg->GetDescriptor() == hytech::pedals_system_data::descriptor()) { + auto in_msg = std::static_pointer_cast(msg); + core::DriverInput input = {(in_msg->accel_pedal()), (in_msg->brake_pedal())}; + { + std::unique_lock lk(_state_mutex); + _timestamp_array[2] = std::chrono::duration_cast( + std::chrono::high_resolution_clock::now().time_since_epoch()); + _vehicle_state.input = input; + } + } else if (msg->GetDescriptor() == hytech::steering_data::descriptor()) { + auto in_msg = std::static_pointer_cast(msg); + { + std::unique_lock lk(_state_mutex); + _timestamp_array[3] = std::chrono::duration_cast( + std::chrono::high_resolution_clock::now().time_since_epoch()); + _raw_input_data.raw_steering_analog = in_msg->steering_analog_raw(); + _raw_input_data.raw_steering_digital = in_msg->steering_digital_raw(); + } + } else if (msg->GetDescriptor() == hytech::em_measurement::descriptor()) { + auto in_msg = std::static_pointer_cast(msg); + float em_voltage = in_msg->em_voltage(); + float em_current = in_msg->em_current(); + { + std::unique_lock lk(_state_mutex); + _vehicle_state.old_energy_meter_kw = (-1.0f * em_voltage * em_current / 1000.0f); + } + } + else { + _receive_inverter_states(msg); + } +} + +void StateTracker::_receive_inverter_states(std::shared_ptr msg) { + auto descriptor = msg->GetDescriptor(); + if (descriptor == hytech::inv1_dynamics::descriptor()) { + _handle_set_inverter_dynamics<0, hytech::inv1_dynamics>(msg); + } else if (descriptor == hytech::inv2_dynamics::descriptor()) { + _handle_set_inverter_dynamics<1, hytech::inv2_dynamics>(msg); + } else if (descriptor == hytech::inv3_dynamics::descriptor()) { + _handle_set_inverter_dynamics<2, hytech::inv3_dynamics>(msg); + } else if (descriptor == hytech::inv4_dynamics::descriptor()) { + _handle_set_inverter_dynamics<3, hytech::inv4_dynamics>(msg); + } else if (descriptor == hytech::inv1_temps::descriptor()) { + auto in_msg = std::static_pointer_cast(msg); + { + std::unique_lock lk(_state_mutex); + _vehicle_state.dt_data.inverter_igbt_temps_c.FL = in_msg->igbt_temp(); + _vehicle_state.dt_data.inverter_temps_c.FL = in_msg->inverter_temp(); + _vehicle_state.dt_data.inverter_motor_temps_c.FL = in_msg->motor_temp(); + } + } else if (descriptor == hytech::inv2_temps::descriptor()) { + auto in_msg = std::static_pointer_cast(msg); + { + std::unique_lock lk(_state_mutex); + _vehicle_state.dt_data.inverter_igbt_temps_c.FR = in_msg->igbt_temp(); + _vehicle_state.dt_data.inverter_temps_c.FR = in_msg->inverter_temp(); + _vehicle_state.dt_data.inverter_motor_temps_c.FR = in_msg->motor_temp(); + } + } else if (descriptor == hytech::inv3_temps::descriptor()) { + auto in_msg = std::static_pointer_cast(msg); + { + std::unique_lock lk(_state_mutex); + _vehicle_state.dt_data.inverter_igbt_temps_c.RL = in_msg->igbt_temp(); + _vehicle_state.dt_data.inverter_temps_c.RL = in_msg->inverter_temp(); + _vehicle_state.dt_data.inverter_motor_temps_c.RL = in_msg->motor_temp(); + } + } else if (descriptor == hytech::inv4_temps::descriptor()) { + auto in_msg = std::static_pointer_cast(msg); + { + std::unique_lock lk(_state_mutex); + _vehicle_state.dt_data.inverter_igbt_temps_c.RR = in_msg->igbt_temp(); + _vehicle_state.dt_data.inverter_temps_c.RR = in_msg->inverter_temp(); + _vehicle_state.dt_data.inverter_motor_temps_c.RR = in_msg->motor_temp(); + } + } else if (descriptor == hytech::inv1_overload::descriptor()) { + auto in_msg = std::static_pointer_cast(msg); + { + std::unique_lock lk(_state_mutex); + _vehicle_state.motor_overload_percentages.FL = in_msg->motor_overload_percentage(); + } + } else if (descriptor == hytech::inv2_overload::descriptor()) { + auto in_msg = std::static_pointer_cast(msg); + { + std::unique_lock lk(_state_mutex); + _vehicle_state.motor_overload_percentages.FR = in_msg->motor_overload_percentage(); + } + } else if (descriptor == hytech::inv3_overload::descriptor()) { + auto in_msg = std::static_pointer_cast(msg); + { + std::unique_lock lk(_state_mutex); + _vehicle_state.motor_overload_percentages.RL = in_msg->motor_overload_percentage(); + } + } else if (descriptor == hytech::inv4_overload::descriptor()) { + auto in_msg = std::static_pointer_cast(msg); + { + std::unique_lock lk(_state_mutex); + _vehicle_state.motor_overload_percentages.RR = in_msg->motor_overload_percentage(); + } + } else if(descriptor == hytech::inv1_status::descriptor()) { + + auto in_msg = std::static_pointer_cast(msg); + auto voltage = in_msg->dc_bus_voltage(); + { + std::unique_lock lk(_state_mutex); + _vehicle_state.acc_data.pack_voltage = static_cast(voltage); + } + } +} + +template +bool StateTracker::_validate_timestamps(const std::array ×tamp_arr) { + std::array timestamp_array_to_sort; + + { + std::unique_lock lk(_state_mutex); + timestamp_array_to_sort = timestamp_arr; + } + + auto debug_copy = timestamp_array_to_sort; + const std::chrono::microseconds threshold(500000); // 500 milliseconds in microseconds + + // Sort the array + std::sort(timestamp_array_to_sort.begin(), timestamp_array_to_sort.end()); + + // Check if the range between the smallest and largest timestamps is within the threshold + auto min_stamp = timestamp_array_to_sort.front(); + auto max_stamp = timestamp_array_to_sort.back(); + + bool within_threshold = (max_stamp - min_stamp) <= threshold; + + auto curr_time = std::chrono::duration_cast( + std::chrono::high_resolution_clock::now().time_since_epoch()); + + constexpr std::chrono::seconds debug_print_period(1); + + bool all_members_received = min_stamp.count() > 0; // count here is the count in microseconds + bool last_update_recent_enough = + (std::chrono::duration_cast(curr_time - max_stamp)) < threshold; + + return within_threshold && all_members_received && last_update_recent_enough; +} + +void core::StateTracker::create() { + StateTracker* expected = nullptr; + StateTracker* local = new StateTracker(); + if(!_s_instance.compare_exchange_strong(expected, local, std::memory_order_release, std::memory_order_relaxed)) { + // Already initialized, delete local instance + delete local; + } +} + +core::StateTracker& core::StateTracker::instance() { + StateTracker* instance = _s_instance.load(std::memory_order_acquire); + assert(instance != nullptr && "StateTracker has not been initialized"); + return *instance; +} diff --git a/drivebrain_simple_hil_test/src/debug_can.cpp b/drivebrain_simple_hil_test/src/debug_can.cpp index 26ed033..6bece07 100644 --- a/drivebrain_simple_hil_test/src/debug_can.cpp +++ b/drivebrain_simple_hil_test/src/debug_can.cpp @@ -1,52 +1,52 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "hytech.pb.h" - -int main() { - - auto period = std::chrono::milliseconds(4); // 250hz - - int s; - struct sockaddr_can addr; - struct ifreq ifr; - - s = socket(PF_CAN, SOCK_RAW, CAN_RAW); - - strcpy(ifr.ifr_name, "can_primary"); - ioctl(s, SIOCGIFINDEX, &ifr); - - addr.can_family = AF_CAN; - addr.can_ifindex = ifr.ifr_ifindex; - - bind(s, (struct sockaddr *)&addr, sizeof(addr)); - - hytech::acu_ok acu_ok; - - can_frame torque_request_msg = { - .can_id = 241, - .can_dlc = 8 - }; - - can_frame speed_request_msg = { - .can_id = 242, - .can_dlc = 8 - }; - - while (true) { - int nbytes_torque = write(s, &torque_request_msg, sizeof(struct can_frame)); - int nbytes_speed = write(s, &speed_request_msg, sizeof(struct can_frame)); - std::this_thread::sleep_for(period); - } - - return 0; +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "hytech.pb.h" + +int main() { + + auto period = std::chrono::milliseconds(4); // 250hz + + int s; + struct sockaddr_can addr; + struct ifreq ifr; + + s = socket(PF_CAN, SOCK_RAW, CAN_RAW); + + strcpy(ifr.ifr_name, "can_primary"); + ioctl(s, SIOCGIFINDEX, &ifr); + + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + + bind(s, (struct sockaddr *)&addr, sizeof(addr)); + + hytech::acu_ok acu_ok; + + can_frame torque_request_msg = { + .can_id = 241, + .can_dlc = 8 + }; + + can_frame speed_request_msg = { + .can_id = 242, + .can_dlc = 8 + }; + + while (true) { + int nbytes_torque = write(s, &torque_request_msg, sizeof(struct can_frame)); + int nbytes_speed = write(s, &speed_request_msg, sizeof(struct can_frame)); + std::this_thread::sleep_for(period); + } + + return 0; } \ No newline at end of file diff --git a/rpi_profile b/rpi_profile index c9928d7..20000c3 100644 --- a/rpi_profile +++ b/rpi_profile @@ -1,13 +1,13 @@ -[settings] -os=Linux -arch=armv8 -compiler=gcc -build_type=Release -compiler.cppstd=17 -compiler.libcxx=libstdc++11 -compiler.version=11 - -[buildenv] -CC=/usr/bin/aarch64-linux-gnu-gcc-11 -CXX=/usr/bin/aarch64-linux-gnu-g++-11 +[settings] +os=Linux +arch=armv8 +compiler=gcc +build_type=Release +compiler.cppstd=17 +compiler.libcxx=libstdc++11 +compiler.version=11 + +[buildenv] +CC=/usr/bin/aarch64-linux-gnu-gcc-11 +CXX=/usr/bin/aarch64-linux-gnu-g++-11 LD=/usr/bin/aarch64-linux-gnu-ld \ No newline at end of file diff --git a/test_1.mcap b/test_1.mcap new file mode 100644 index 0000000..b4ffd54 Binary files /dev/null and b/test_1.mcap differ diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index 019dacd..56359e8 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -1,29 +1,29 @@ -################################# -# Unit Testing -################################# - -add_executable(drivebrain_test - main.cpp - MCAPLoggerTest.cpp - EthernetCommsTest.cpp -) - -target_link_libraries(drivebrain_test PUBLIC - foxglove-websocket::foxglove-websocket - mcap::mcap - drivebrain_core - drivebrain_comms - GTest::gtest -) - -file(COPY - ${CMAKE_SOURCE_DIR}/tests/test_configs - DESTINATION ${CMAKE_BINARY_DIR} -) -target_compile_definitions(drivebrain_test PRIVATE - TEST_CONFIG_DIR="${CMAKE_BINARY_DIR}/test_configs" -) - -include(GoogleTest) -gtest_discover_tests(drivebrain_test) - +################################# +# Unit Testing +################################# + +add_executable(drivebrain_test + main.cpp + MCAPLoggerTest.cpp + EthernetCommsTest.cpp +) + +target_link_libraries(drivebrain_test PUBLIC + foxglove-websocket::foxglove-websocket + mcap::mcap + drivebrain_core + drivebrain_comms + GTest::gtest +) + +file(COPY + ${CMAKE_SOURCE_DIR}/tests/test_configs + DESTINATION ${CMAKE_BINARY_DIR} +) +target_compile_definitions(drivebrain_test PRIVATE + TEST_CONFIG_DIR="${CMAKE_BINARY_DIR}/test_configs" +) + +include(GoogleTest) +gtest_discover_tests(drivebrain_test) + diff --git a/tests/EthernetCommsTest.cpp b/tests/EthernetCommsTest.cpp index 941d378..cee9bfc 100644 --- a/tests/EthernetCommsTest.cpp +++ b/tests/EthernetCommsTest.cpp @@ -1,57 +1,57 @@ -#include "base_msgs.pb.h" -#include "hytech_msgs.pb.h" -#include -#include -#include -#include -#include -#include - -class EthernetSendTest : public testing::Test { - -protected: - boost::asio::io_context io_context; - std::thread io_thread; - comms::ETHDriver<> eth_sender{io_context, 1155, "127.0.0.1"}; - - void SetUp() override { - io_thread = std::thread([this]() { - io_context.run(); - }); - } - - void TearDown() override { - io_context.stop(); - if (io_thread.joinable()) { - io_thread.join(); - } - } - -}; - -class EthernetDuplexTest : public EthernetSendTest { -protected: - comms::ETHDriver eth_transceiver{io_context, 1155}; -}; - -TEST_F(EthernetSendTest, SendSingleMsg) { - auto msg = std::make_shared(); - msg->set_drivebrain_controller_timing_failure(true); - - ASSERT_NO_THROW(eth_sender.enqueue_msg_send(msg)); -} - -TEST_F(EthernetSendTest, SendMultipleMsgs) { - auto msg = std::make_shared(); - for (int i = 0; i < 10; i++) { - msg->set_cell_voltages(0, i); - eth_sender.enqueue_msg_send(msg); - } - - SUCCEED(); -} - -TEST_F(EthernetDuplexTest, SendRecvMsgs) { - -} - +#include "base_msgs.pb.h" +#include "hytech_msgs.pb.h" +#include +#include +#include +#include +#include +#include + +class EthernetSendTest : public testing::Test { + +protected: + boost::asio::io_context io_context; + std::thread io_thread; + comms::ETHDriver<> eth_sender{io_context, 1155, "127.0.0.1"}; + + void SetUp() override { + io_thread = std::thread([this]() { + io_context.run(); + }); + } + + void TearDown() override { + io_context.stop(); + if (io_thread.joinable()) { + io_thread.join(); + } + } + +}; + +class EthernetDuplexTest : public EthernetSendTest { +protected: + comms::ETHDriver eth_transceiver{io_context, 1155}; +}; + +TEST_F(EthernetSendTest, SendSingleMsg) { + auto msg = std::make_shared(); + msg->set_drivebrain_controller_timing_failure(true); + + ASSERT_NO_THROW(eth_sender.enqueue_msg_send(msg)); +} + +TEST_F(EthernetSendTest, SendMultipleMsgs) { + auto msg = std::make_shared(); + for (int i = 0; i < 10; i++) { + msg->set_cell_voltages(0, i); + eth_sender.enqueue_msg_send(msg); + } + + SUCCEED(); +} + +TEST_F(EthernetDuplexTest, SendRecvMsgs) { + +} + diff --git a/tests/MCAPLoggerTest.cpp b/tests/MCAPLoggerTest.cpp index 975a7f6..c7c1119 100644 --- a/tests/MCAPLoggerTest.cpp +++ b/tests/MCAPLoggerTest.cpp @@ -1,19 +1,19 @@ -#include -#include -#include -#include -#include - -class MCAPLoggerTest : public testing::Test { - -protected: - - MCAPLoggerTest() { - core::FoxgloveServer::create(TEST_CONFIG_DIR + std::string("/fake_foxglove_params.json")); - }; -}; - -TEST_F(MCAPLoggerTest, ParamFetch) { - ASSERT_EQ(core::FoxgloveServer::instance().get_param("rpm_limit"), 10000); - ASSERT_EQ(core::FoxgloveServer::instance().get_param("enable_logging"), true); -} +#include +#include +#include +#include +#include + +class MCAPLoggerTest : public testing::Test { + +protected: + + MCAPLoggerTest() { + core::FoxgloveServer::create(TEST_CONFIG_DIR + std::string("/fake_foxglove_params.json")); + }; +}; + +TEST_F(MCAPLoggerTest, ParamFetch) { + ASSERT_EQ(core::FoxgloveServer::instance().get_param("rpm_limit"), 10000); + ASSERT_EQ(core::FoxgloveServer::instance().get_param("enable_logging"), true); +} diff --git a/tests/main.cpp b/tests/main.cpp index 697a9d7..3352fbe 100644 --- a/tests/main.cpp +++ b/tests/main.cpp @@ -1,6 +1,6 @@ -#include - -int main(int argc, char** argv) { - ::testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} +#include + +int main(int argc, char** argv) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/tests/scripts/run_mcap_logger.cpp b/tests/scripts/run_mcap_logger.cpp index 21f758d..4f25189 100644 --- a/tests/scripts/run_mcap_logger.cpp +++ b/tests/scripts/run_mcap_logger.cpp @@ -1,54 +1,54 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -std::atomic running = true; - -void sig_handler(int signal) { - if(signal == SIGINT) { - std::cout << "halting\n"; - running = false; - } -} - -void get_param_task(int wait_time, core::MsgType msg) { - while(running) { - core::MCAPLogger::instance().log_msg(static_cast(msg)); - std::this_thread::sleep_for((std::chrono::milliseconds(wait_time))); - } -} - -int main(int argc, char* argv[]) { - - - core::FoxgloveServer::create(argv[1]); - core::MCAPLogger::create("recordings/", mcap::McapWriterOptions("")); - core::MCAPLogger::instance().open_new_mcap("test_1.mcap"); - core::MCAPLogger::instance().init_logging(); - - std::signal(SIGINT, sig_handler); - - auto vel_msg = std::make_shared(); - vel_msg->set_velocity_x(1000); - vel_msg->set_velocity_y(10000); - - auto acu_data = std::make_shared(); - acu_data->set_max_cell_temp_id(676767); - - std::thread t1(get_param_task, 20, vel_msg); - std::thread t2(get_param_task, 40, acu_data); - - if(t1.joinable()) t1.join(); - if(t2.joinable()) t2.join(); - core::MCAPLogger::instance().close_active_mcap(); - -} +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +std::atomic running = true; + +void sig_handler(int signal) { + if(signal == SIGINT) { + std::cout << "halting\n"; + running = false; + } +} + +void get_param_task(int wait_time, core::MsgType msg) { + while(running) { + core::MCAPLogger::instance().log_msg(static_cast(msg)); + std::this_thread::sleep_for((std::chrono::milliseconds(wait_time))); + } +} + +int main(int argc, char* argv[]) { + + + core::FoxgloveServer::create(argv[1]); + core::MCAPLogger::create("recordings/", mcap::McapWriterOptions("")); + core::MCAPLogger::instance().open_new_mcap("test_1.mcap"); + core::MCAPLogger::instance().init_logging(); + + std::signal(SIGINT, sig_handler); + + auto vel_msg = std::make_shared(); + vel_msg->set_velocity_x(1000); + vel_msg->set_velocity_y(10000); + + auto acu_data = std::make_shared(); + acu_data->set_max_cell_temp_id(676767); + + std::thread t1(get_param_task, 20, vel_msg); + std::thread t2(get_param_task, 40, acu_data); + + if(t1.joinable()) t1.join(); + if(t2.joinable()) t2.join(); + core::MCAPLogger::instance().close_active_mcap(); + +} diff --git a/tests/scripts/test_config.json b/tests/scripts/test_config.json index 7bacab3..ff474ba 100644 --- a/tests/scripts/test_config.json +++ b/tests/scripts/test_config.json @@ -1,5 +1,5 @@ -{ - "enable_logging": true, - "rpm_limit": 10000, - "kP": 15000 -} +{ + "enable_logging": true, + "rpm_limit": 10000, + "kP": 15000 +} diff --git a/tests/test_configs/fake_foxglove_params.json b/tests/test_configs/fake_foxglove_params.json index 7bacab3..ff474ba 100644 --- a/tests/test_configs/fake_foxglove_params.json +++ b/tests/test_configs/fake_foxglove_params.json @@ -1,5 +1,5 @@ -{ - "enable_logging": true, - "rpm_limit": 10000, - "kP": 15000 -} +{ + "enable_logging": true, + "rpm_limit": 10000, + "kP": 15000 +}