From d5937c540d7a0a204a767bcd60923006f0d50113 Mon Sep 17 00:00:00 2001 From: Wessel Tip Date: Wed, 5 Nov 2025 07:47:49 +0100 Subject: [PATCH 01/17] chore: rebase --- src/g2_2025_imu_reader_pkg/CMakeLists.txt | 52 +++++++++++++++++++ .../src/g2_2025_lifecycle_node/main.cpp | 23 ++++++++ .../nodes/hardware_interface.cpp | 14 +++++ .../nodes/hardware_interface.hpp | 31 +++++++++++ .../nodes/lifecycle_manager.cpp | 28 ++++++++++ .../nodes/lifecycle_manager.hpp | 33 ++++++++++++ 6 files changed, 181 insertions(+) create mode 100644 src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/main.cpp create mode 100644 src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.cpp create mode 100644 src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.hpp create mode 100644 src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.cpp create mode 100644 src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.hpp diff --git a/src/g2_2025_imu_reader_pkg/CMakeLists.txt b/src/g2_2025_imu_reader_pkg/CMakeLists.txt index 3a9f332..ee90b12 100644 --- a/src/g2_2025_imu_reader_pkg/CMakeLists.txt +++ b/src/g2_2025_imu_reader_pkg/CMakeLists.txt @@ -22,6 +22,8 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(std_msgs REQUIRED) find_package(sensor_msgs REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(nlohmann_json REQUIRED) add_executable(g2_2025_imu_database_writer_node src/g2_2025_imu_database_writer_node/Main.cpp @@ -29,19 +31,45 @@ add_executable(g2_2025_imu_database_writer_node src/config/ConfigManager.cpp src/g2_2025_imu_database_writer_node/nodes/IMUDatabaseWriter.cpp ) + target_include_directories(g2_2025_imu_database_writer_node PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/src/g2_2025_imu_database_writer_node ) ament_target_dependencies(g2_2025_imu_database_writer_node rclcpp sensor_msgs) target_link_libraries(g2_2025_imu_database_writer_node pqxx pq tomlplusplus::tomlplusplus) +ament_target_dependencies(g2_2025_imu_database_writer_node rclcpp sensor_msgs) +target_link_libraries(g2_2025_imu_database_writer_node pqxx pq tomlplusplus::tomlplusplus) + + +add_executable(g2_2025_lifecycle_node + src/g2_2025_lifecycle_node/main.cpp + src/g2_2025_lifecycle_node/nodes/HardwareInterface.cpp + src/g2_2025_lifecycle_node/nodes/LifecycleManager.cpp + src/config/serialib.cpp +) + +target_include_directories(g2_2025_lifecycle_node PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/src + ${CMAKE_CURRENT_SOURCE_DIR}/src/g2_2025_lifecycle_node +) +ament_target_dependencies(g2_2025_lifecycle_node rclcpp rclcpp_lifecycle std_msgs sensor_msgs) + +target_link_libraries(g2_2025_lifecycle_node + paho-mqttpp3 + paho-mqtt3a + nlohmann_json::nlohmann_json +) install( TARGETS g2_2025_imu_database_writer_node + g2_2025_lifecycle_node DESTINATION lib/${PROJECT_NAME} ) +set_target_properties(g2_2025_lifecycle_node PROPERTIES INSTALL_RPATH "/usr/local/lib") + if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) @@ -95,6 +123,30 @@ if(BUILD_TESTING) pqxx pq tomlplusplus::tomlplusplus ) + ament_add_gtest(${PROJECT_NAME}_test_lifecycle_manager + test/LifecycleManager.test.cpp + src/g2_2025_lifecycle_node/nodes/lifecycle_manager.cpp + src/g2_2025_lifecycle_node/nodes/hardware_interface.cpp + src/config/serialib.cpp + ) + target_include_directories(${PROJECT_NAME}_test_lifecycle_manager PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/src + ${CMAKE_CURRENT_SOURCE_DIR}/src/g2_2025_lifecycle_node + ) + ament_target_dependencies(${PROJECT_NAME}_test_lifecycle_manager + rclcpp + rclcpp_lifecycle + std_msgs + sensor_msgs + ) + target_link_libraries(${PROJECT_NAME}_test_lifecycle_manager + paho-mqttpp3 + paho-mqtt3a + nlohmann_json::nlohmann_json + ) + set_target_properties(${PROJECT_NAME}_test_lifecycle_manager PROPERTIES INSTALL_RPATH "/usr/local/lib") + + # Add Python integration tests # find_package(ament_cmake_pytest REQUIRED) # ament_add_pytest_test(${PROJECT_NAME}_integration_test test/test_integration_system.py diff --git a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/main.cpp b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/main.cpp new file mode 100644 index 0000000..145d62b --- /dev/null +++ b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/main.cpp @@ -0,0 +1,23 @@ +/* main.cpp + * Entry point for lifecycle node + * + * Reviewed by: + * Changelog: + * [23-09-2025] Wessel T: Simplified main.cpp to entry point only + */ + +#include "rclcpp/rclcpp.hpp" +#include "nodes/hardware_interface.hpp" +#include "nodes/lifecycle_manager.hpp" + + +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + + auto node = std::make_shared(); + + rclcpp::spin(node->get_node_base_interface()); + rclcpp::shutdown(); + + return 0; +} diff --git a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.cpp b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.cpp new file mode 100644 index 0000000..cdc06ce --- /dev/null +++ b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.cpp @@ -0,0 +1,14 @@ +#include "hardware_interface.hpp" + +namespace assignments::two::g2_2025_lifecycle_node { + +HardwareInterface::HardwareInterface() : Node("hardware_interface") { + RCLCPP_INFO(this->get_logger(), "HardwareInterface node has been created"); + +} + +void HardwareInterface::some_hardware_method() { + RCLCPP_INFO(this->get_logger(), "Interacting with hardware..."); +} + +} // namespace assignments::two::g2_2025_lifecycle_node diff --git a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.hpp b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.hpp new file mode 100644 index 0000000..c347245 --- /dev/null +++ b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.hpp @@ -0,0 +1,31 @@ +/* nodes/hardware_interface.hpp + * Hardware interface implementation for the IMU reader system. + * + * Manages the serial communication with the IMU hardware, including initialization, + * data acquisition, and shutdown procedures. + * + * Changelog: + * [28-10-2025] M.khalaf: Implemented template. + */ +#pragma once + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" + +namespace assignments::two::g2_2025_lifecycle_node { + +class HardwareInterface : public rclcpp::Node { + +public: + HardwareInterface(); +private: + void some_hardware_method(); + +}; +} // namespace assignments::two::g2_2025_lifecycle_node diff --git a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.cpp b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.cpp new file mode 100644 index 0000000..c155955 --- /dev/null +++ b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.cpp @@ -0,0 +1,28 @@ +#include "lifecycle_manager.hpp" + + +namespace assignments::two::g2_2025_lifecycle_node { + +LifecycleManager::LifecycleManager() : rclcpp_lifecycle::LifecycleNode("lifecycle_manager") { + + + RCLCPP_INFO(this->get_logger(), "LifecycleManager node is ready"); + imu_publisher = this->create_publisher("imu_data", 10); + RCLCPP_INFO(this->get_logger(), "IMU Publisher has been created"); + + +} + +void LifecycleManager::publish_imu_data(const std::string & data) { + auto message = std_msgs::msg::String(); + message.data = data; + imu_publisher->publish(message); + RCLCPP_INFO(this->get_logger(), "Published IMU data: %s", data.c_str()); +} + +void LifecycleManager::Unconfigured() { + RCLCPP_INFO(this->get_logger(), "Managing lifecycle..."); + +} + +} // namespace assignments::two::g2_2025_lifecycle_node diff --git a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.hpp b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.hpp new file mode 100644 index 0000000..6a814cc --- /dev/null +++ b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.hpp @@ -0,0 +1,33 @@ +/* nodes/lifecycle_manager.hpp + * Lifecycle node implementation for managing the lifecycle of the IMU reader system. + * + * Manages the different states of the lifecycle node, including configuration, + * and hardware interface management. + * + * Changelog: + * [28-10-2025] M.khalaf: Implemented template. + */ +#pragma once + +#include +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "std_msgs/msg/string.hpp" + + +namespace assignments::two::g2_2025_lifecycle_node { +class LifecycleManager : public rclcpp_lifecycle::LifecycleNode { +public: + LifecycleManager(); +private: + rclcpp::Publisher::SharedPtr imu_publisher; + void publish_imu_data(const std::string & data); + void Unconfigured(); + +}; +} // namespace assignments::two::g2_2025_lifecycle_node From 8696eee1978c1a9a4866facbe45e9a8f8c41fefd Mon Sep 17 00:00:00 2001 From: Mohammad Date: Tue, 28 Oct 2025 19:18:55 +0100 Subject: [PATCH 02/17] feat(lifecycle): added core functions with beginner test to lifecycle_manager --- src/g2_2025_imu_reader_pkg/CMakeLists.txt | 5 +-- .../nodes/lifecycle_manager.cpp | 33 +++++++++++++++++-- .../nodes/lifecycle_manager.hpp | 5 ++- 3 files changed, 37 insertions(+), 6 deletions(-) diff --git a/src/g2_2025_imu_reader_pkg/CMakeLists.txt b/src/g2_2025_imu_reader_pkg/CMakeLists.txt index ee90b12..d2fe2b3 100644 --- a/src/g2_2025_imu_reader_pkg/CMakeLists.txt +++ b/src/g2_2025_imu_reader_pkg/CMakeLists.txt @@ -24,6 +24,7 @@ find_package(std_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(nlohmann_json REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) add_executable(g2_2025_imu_database_writer_node src/g2_2025_imu_database_writer_node/Main.cpp @@ -44,8 +45,8 @@ target_link_libraries(g2_2025_imu_database_writer_node pqxx pq tomlplusplus::tom add_executable(g2_2025_lifecycle_node src/g2_2025_lifecycle_node/main.cpp - src/g2_2025_lifecycle_node/nodes/HardwareInterface.cpp - src/g2_2025_lifecycle_node/nodes/LifecycleManager.cpp + src/g2_2025_lifecycle_node/nodes/hardware_interface.cpp + src/g2_2025_lifecycle_node/nodes/lifecycle_manager.cpp src/config/serialib.cpp ) diff --git a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.cpp b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.cpp index c155955..c80846e 100644 --- a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.cpp +++ b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.cpp @@ -13,16 +13,43 @@ LifecycleManager::LifecycleManager() : rclcpp_lifecycle::LifecycleNode("lifecycl } -void LifecycleManager::publish_imu_data(const std::string & data) { +void LifecycleManager::publish_imu_data(const std::string & data) { // placeholderrrrr auto message = std_msgs::msg::String(); message.data = data; imu_publisher->publish(message); RCLCPP_INFO(this->get_logger(), "Published IMU data: %s", data.c_str()); } +/* -void LifecycleManager::Unconfigured() { - RCLCPP_INFO(this->get_logger(), "Managing lifecycle..."); +*in terminal commands to manage lifecycle: +*ros2 lifecycle list /lifecycle_manager ## for status checking +*ros2 lifecycle set /lifecycle_manager configure +*ros2 lifecycle set /lifecycle_manager activate +*ros2 lifecycle set /lifecycle_manager deactivate +*ros2 lifecycle set /lifecycle_manager shutdown +*/ +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +LifecycleManager::on_configure(const rclcpp_lifecycle::State&) { + RCLCPP_INFO(this->get_logger(), "configuring lifecycle..."); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +LifecycleManager::on_activate(const rclcpp_lifecycle::State&) { + RCLCPP_INFO(this->get_logger(), "activating lifecycle..."); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +LifecycleManager::on_deactivate(const rclcpp_lifecycle::State&) { + RCLCPP_INFO(this->get_logger(), "deactivating lifecycle..."); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +LifecycleManager::on_shutdown(const rclcpp_lifecycle::State&) { + RCLCPP_INFO(this->get_logger(), "shutting down lifecycle..."); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } } // namespace assignments::two::g2_2025_lifecycle_node diff --git a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.hpp b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.hpp index 6a814cc..49f3f95 100644 --- a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.hpp +++ b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.hpp @@ -27,7 +27,10 @@ public: private: rclcpp::Publisher::SharedPtr imu_publisher; void publish_imu_data(const std::string & data); - void Unconfigured(); + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(const rclcpp_lifecycle::State&); + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate(const rclcpp_lifecycle::State&); + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State&); + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown(const rclcpp_lifecycle::State&); }; } // namespace assignments::two::g2_2025_lifecycle_node From bd8400027fae86fe4ccfba6e22b61ec0de8cd04f Mon Sep 17 00:00:00 2001 From: Mohammad Date: Wed, 29 Oct 2025 18:29:10 +0100 Subject: [PATCH 03/17] feat(lifecycle & hw): added serial lib and serial comunication functions with params. --- doc/installation/installation.md | 55 + src/g2_2025_imu_reader_pkg/CMakeLists.txt | 7 + .../src/config/serialib.cpp | 1137 +++++++++++++++++ .../src/config/serialib.h | 270 ++++ .../nodes/hardware_interface.cpp | 95 +- .../nodes/hardware_interface.hpp | 27 +- .../nodes/lifecycle_manager.cpp | 68 +- .../nodes/lifecycle_manager.hpp | 13 +- 8 files changed, 1667 insertions(+), 5 deletions(-) create mode 100644 src/g2_2025_imu_reader_pkg/src/config/serialib.cpp create mode 100644 src/g2_2025_imu_reader_pkg/src/config/serialib.h diff --git a/doc/installation/installation.md b/doc/installation/installation.md index 635d66d..a6ad224 100644 --- a/doc/installation/installation.md +++ b/doc/installation/installation.md @@ -40,4 +40,59 @@ You can configure specific database settings in the `docker-compose.yaml` in the ```bash ros2 launch g2_2025_imu_reader_pkg imu_reader.launch.xml ``` +<<<<<<< HEAD To change parameters when using the launch file it will need to be edited in the `src/g2_2025_imu_reader_pkg/launch` folder. All parameters are already added to this document and thus only the values will need to be changed +======= +To change parameters when using the launch file it will need to be edited in the `src/g2_2025_grade_calculator_pkg/launch` folder. All parameters are already added to this document and thus only the values will need to be changed + + + +### installation and setup for mqtt + +```bash +sudo apt install mosquitto + +sudo apt-get install libpaho-mqtt-dev + + +git clone https://github.com/eclipse/paho.mqtt.cpp +cd paho.mqtt.cpp +git co v1.5.4 +git submodule init +git submodule update + +cmake -Bbuild -H. -DPAHO_WITH_MQTT_C=ON -DPAHO_BUILD_EXAMPLES=ON +sudo cmake --build build/ --target install +``` + +## for launching lifecycle mqtt node + +first: +```bash +ros2 run g2_2025_imu_reader_pkg g2_2025_lifecycle_node --ros-args -p comm_t:='mqtt' +``` + +in other terminal: + +```bash +mosquitto -p 1884 +``` + +and in other terminal to inialize the subsecriber: +```bash + +ros2 lifecycle set /lifecycle_manager configure +ros2 lifecycle set /lifecycle_manager activate + +``` + +an finally publish a mesg to the sub in other terminal: + +```bash +mosquitto_pub -h localhost -p 1884 -t "esp32/imu" -m "nirvana" +``` +close conn via: +```bash +ros2 lifecycle set /lifecycle_manager deactivate +``` +>>>>>>> 8b04168 (feat(lifecycle & hw): added mqtt connection and installation setup) diff --git a/src/g2_2025_imu_reader_pkg/CMakeLists.txt b/src/g2_2025_imu_reader_pkg/CMakeLists.txt index d2fe2b3..45961a4 100644 --- a/src/g2_2025_imu_reader_pkg/CMakeLists.txt +++ b/src/g2_2025_imu_reader_pkg/CMakeLists.txt @@ -59,9 +59,16 @@ ament_target_dependencies(g2_2025_lifecycle_node rclcpp rclcpp_lifecycle std_msg target_link_libraries(g2_2025_lifecycle_node paho-mqttpp3 paho-mqtt3a +<<<<<<< HEAD nlohmann_json::nlohmann_json ) +======= +) + + + +>>>>>>> 8b04168 (feat(lifecycle & hw): added mqtt connection and installation setup) install( TARGETS g2_2025_imu_database_writer_node diff --git a/src/g2_2025_imu_reader_pkg/src/config/serialib.cpp b/src/g2_2025_imu_reader_pkg/src/config/serialib.cpp new file mode 100644 index 0000000..a01c7aa --- /dev/null +++ b/src/g2_2025_imu_reader_pkg/src/config/serialib.cpp @@ -0,0 +1,1137 @@ +/*! + \file serialib.cpp + \brief Source file of the class serialib. This class is used for communication over a serial device. + \author Philippe Lucidarme (University of Angers) + \version 2.0 + \date december the 27th of 2019 + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, +INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR +PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE X CONSORTIUM BE LIABLE FOR ANY CLAIM, +DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + + +This is a licence-free software, it can be used by anyone who try to build a better world. + */ + +#include "serialib.h" + + + +//_____________________________________ +// ::: Constructors and destructors ::: + + +/*! + \brief Constructor of the class serialib. +*/ +serialib::serialib() +{ +#if defined (_WIN32) || defined( _WIN64) + // Set default value for RTS and DTR (Windows only) + currentStateRTS=true; + currentStateDTR=true; + hSerial = INVALID_HANDLE_VALUE; +#endif +#if defined (__linux__) || defined(__APPLE__) + fd = -1; +#endif +} + + +/*! + \brief Destructor of the class serialib. It close the connection +*/ +// Class desctructor +serialib::~serialib() +{ + closeDevice(); +} + + + +//_________________________________________ +// ::: Configuration and initialization ::: + + + +/*! + \brief Open the serial port + \param Device : Port name (COM1, COM2, ... for Windows ) or (/dev/ttyS0, /dev/ttyACM0, /dev/ttyUSB0 ... for linux) + \param Bauds : Baud rate of the serial port. + + \n Supported baud rate for Windows : + - 110 + - 300 + - 600 + - 1200 + - 2400 + - 4800 + - 9600 + - 14400 + - 19200 + - 38400 + - 56000 + - 57600 + - 115200 + - 128000 + - 256000 + + \n Supported baud rate for Linux :\n + - 110 + - 300 + - 600 + - 1200 + - 2400 + - 4800 + - 9600 + - 19200 + - 38400 + - 57600 + - 115200 + + \n Optionally supported baud rates, depending on Linux kernel:\n + - 230400 + - 460800 + - 500000 + - 576000 + - 921600 + - 1000000 + - 1152000 + - 1500000 + - 2000000 + - 2500000 + - 3000000 + - 3500000 + - 4000000 + + \param Databits : Number of data bits in one UART transmission. + + \n Supported values: \n + - SERIAL_DATABITS_5 (5) + - SERIAL_DATABITS_6 (6) + - SERIAL_DATABITS_7 (7) + - SERIAL_DATABITS_8 (8) + - SERIAL_DATABITS_16 (16) (not supported on Unix) + + \param Parity: Parity type + + \n Supported values: \n + - SERIAL_PARITY_NONE (N) + - SERIAL_PARITY_EVEN (E) + - SERIAL_PARITY_ODD (O) + - SERIAL_PARITY_MARK (MARK) (not supported on Unix) + - SERIAL_PARITY_SPACE (SPACE) (not supported on Unix) + \param Stopbit: Number of stop bits + + \n Supported values: + - SERIAL_STOPBITS_1 (1) + - SERIAL_STOPBITS_1_5 (1.5) (not supported on Unix) + - SERIAL_STOPBITS_2 (2) + + \return 1 success + \return -1 device not found + \return -2 error while opening the device + \return -3 error while getting port parameters + \return -4 Speed (Bauds) not recognized + \return -5 error while writing port parameters + \return -6 error while writing timeout parameters + \return -7 Databits not recognized + \return -8 Stopbits not recognized + \return -9 Parity not recognized + */ +char serialib::openDevice(const char *Device, const unsigned int Bauds, + SerialDataBits Databits, + SerialParity Parity, + SerialStopBits Stopbits) { +#if defined (_WIN32) || defined( _WIN64) + // Open serial port + hSerial = CreateFileA(Device,GENERIC_READ | GENERIC_WRITE,0,0,OPEN_EXISTING,/*FILE_ATTRIBUTE_NORMAL*/0,0); + if(hSerial==INVALID_HANDLE_VALUE) { + if(GetLastError()==ERROR_FILE_NOT_FOUND) + return -1; // Device not found + + // Error while opening the device + return -2; + } + + // Set parameters + + // Structure for the port parameters + DCB dcbSerialParams; + dcbSerialParams.DCBlength=sizeof(dcbSerialParams); + + // Get the port parameters + if (!GetCommState(hSerial, &dcbSerialParams)) return -3; + + // Set the speed (Bauds) + switch (Bauds) + { + case 110 : dcbSerialParams.BaudRate=CBR_110; break; + case 300 : dcbSerialParams.BaudRate=CBR_300; break; + case 600 : dcbSerialParams.BaudRate=CBR_600; break; + case 1200 : dcbSerialParams.BaudRate=CBR_1200; break; + case 2400 : dcbSerialParams.BaudRate=CBR_2400; break; + case 4800 : dcbSerialParams.BaudRate=CBR_4800; break; + case 9600 : dcbSerialParams.BaudRate=CBR_9600; break; + case 14400 : dcbSerialParams.BaudRate=CBR_14400; break; + case 19200 : dcbSerialParams.BaudRate=CBR_19200; break; + case 38400 : dcbSerialParams.BaudRate=CBR_38400; break; + case 56000 : dcbSerialParams.BaudRate=CBR_56000; break; + case 57600 : dcbSerialParams.BaudRate=CBR_57600; break; + case 115200 : dcbSerialParams.BaudRate=CBR_115200; break; + case 128000 : dcbSerialParams.BaudRate=CBR_128000; break; + case 256000 : dcbSerialParams.BaudRate=CBR_256000; break; + default : return -4; + } + //select data size + BYTE bytesize = 0; + switch(Databits) { + case SERIAL_DATABITS_5: bytesize = 5; break; + case SERIAL_DATABITS_6: bytesize = 6; break; + case SERIAL_DATABITS_7: bytesize = 7; break; + case SERIAL_DATABITS_8: bytesize = 8; break; + case SERIAL_DATABITS_16: bytesize = 16; break; + default: return -7; + } + BYTE stopBits = 0; + switch(Stopbits) { + case SERIAL_STOPBITS_1: stopBits = ONESTOPBIT; break; + case SERIAL_STOPBITS_1_5: stopBits = ONE5STOPBITS; break; + case SERIAL_STOPBITS_2: stopBits = TWOSTOPBITS; break; + default: return -8; + } + BYTE parity = 0; + switch(Parity) { + case SERIAL_PARITY_NONE: parity = NOPARITY; break; + case SERIAL_PARITY_EVEN: parity = EVENPARITY; break; + case SERIAL_PARITY_ODD: parity = ODDPARITY; break; + case SERIAL_PARITY_MARK: parity = MARKPARITY; break; + case SERIAL_PARITY_SPACE: parity = SPACEPARITY; break; + default: return -9; + } + // configure byte size + dcbSerialParams.ByteSize = bytesize; + // configure stop bits + dcbSerialParams.StopBits = stopBits; + // configure parity + dcbSerialParams.Parity = parity; + + // Write the parameters + if(!SetCommState(hSerial, &dcbSerialParams)) return -5; + + // Set TimeOut + + // Set the Timeout parameters + timeouts.ReadIntervalTimeout=0; + // No TimeOut + timeouts.ReadTotalTimeoutConstant=MAXDWORD; + timeouts.ReadTotalTimeoutMultiplier=0; + timeouts.WriteTotalTimeoutConstant=MAXDWORD; + timeouts.WriteTotalTimeoutMultiplier=0; + + // Write the parameters + if(!SetCommTimeouts(hSerial, &timeouts)) return -6; + + // Opening successfull + return 1; +#endif +#if defined (__linux__) || defined(__APPLE__) + // Structure with the device's options + struct termios options; + + + // Open device + fd = open(Device, O_RDWR | O_NOCTTY | O_NDELAY); + // If the device is not open, return -2 + if (fd == -1) return -2; + // Open the device in nonblocking mode + fcntl(fd, F_SETFL, FNDELAY); + + + // Get the current options of the port + tcgetattr(fd, &options); + // Clear all the options + bzero(&options, sizeof(options)); + + // Prepare speed (Bauds) + speed_t Speed; + switch (Bauds) + { + case 110 : Speed=B110; break; + case 300 : Speed=B300; break; + case 600 : Speed=B600; break; + case 1200 : Speed=B1200; break; + case 2400 : Speed=B2400; break; + case 4800 : Speed=B4800; break; + case 9600 : Speed=B9600; break; + case 19200 : Speed=B19200; break; + case 38400 : Speed=B38400; break; + case 57600 : Speed=B57600; break; + case 115200 : Speed=B115200; break; +#if defined (B230400) + case 230400 : Speed=B230400; break; +#endif +#if defined (B460800) + case 460800 : Speed=B460800; break; +#endif +#if defined (B500000) + case 500000 : Speed=B500000; break; +#endif +#if defined (B576000) + case 576000 : Speed=B576000; break; +#endif +#if defined (B921600) + case 921600 : Speed=B921600; break; +#endif +#if defined (B1000000) + case 1000000 : Speed=B1000000; break; +#endif +#if defined (B1152000) + case 1152000 : Speed=B1152000; break; +#endif +#if defined (B1500000) + case 1500000 : Speed=B1500000; break; +#endif +#if defined (B2000000) + case 2000000 : Speed=B2000000; break; +#endif +#if defined (B2500000) + case 2500000 : Speed=B2500000; break; +#endif +#if defined (B3000000) + case 3000000 : Speed=B3000000; break; +#endif +#if defined (B3500000) + case 3500000 : Speed=B3500000; break; +#endif +#if defined (B4000000) + case 4000000 : Speed=B4000000; break; +#endif + default : return -4; + } + int databits_flag = 0; + switch(Databits) { + case SERIAL_DATABITS_5: databits_flag = CS5; break; + case SERIAL_DATABITS_6: databits_flag = CS6; break; + case SERIAL_DATABITS_7: databits_flag = CS7; break; + case SERIAL_DATABITS_8: databits_flag = CS8; break; + //16 bits and everything else not supported + default: return -7; + } + int stopbits_flag = 0; + switch(Stopbits) { + case SERIAL_STOPBITS_1: stopbits_flag = 0; break; + case SERIAL_STOPBITS_2: stopbits_flag = CSTOPB; break; + //1.5 stopbits and everything else not supported + default: return -8; + } + int parity_flag = 0; + switch(Parity) { + case SERIAL_PARITY_NONE: parity_flag = 0; break; + case SERIAL_PARITY_EVEN: parity_flag = PARENB; break; + case SERIAL_PARITY_ODD: parity_flag = (PARENB | PARODD); break; + //mark and space parity not supported + default: return -9; + } + + // Set the baud rate + cfsetispeed(&options, Speed); + cfsetospeed(&options, Speed); + // Configure the device : data bits, stop bits, parity, no control flow + // Ignore modem control lines (CLOCAL) and Enable receiver (CREAD) + options.c_cflag |= ( CLOCAL | CREAD | databits_flag | parity_flag | stopbits_flag); + options.c_iflag |= ( IGNPAR | IGNBRK ); + // Timer unused + options.c_cc[VTIME]=0; + // At least on character before satisfy reading + options.c_cc[VMIN]=0; + // Activate the settings + tcsetattr(fd, TCSANOW, &options); + // Success + return (1); +#endif + +} + +bool serialib::isDeviceOpen() +{ +#if defined (_WIN32) || defined( _WIN64) + return hSerial != INVALID_HANDLE_VALUE; +#endif +#if defined (__linux__) || defined(__APPLE__) + return fd >= 0; +#endif +} + +/*! + \brief Close the connection with the current device +*/ +void serialib::closeDevice() +{ +#if defined (_WIN32) || defined( _WIN64) + CloseHandle(hSerial); + hSerial = INVALID_HANDLE_VALUE; +#endif +#if defined (__linux__) || defined(__APPLE__) + close (fd); + fd = -1; +#endif +} + + + + +//___________________________________________ +// ::: Read/Write operation on characters ::: + + + +/*! + \brief Write a char on the current serial port + \param Byte : char to send on the port (must be terminated by '\0') + \return 1 success + \return -1 error while writting data + */ +int serialib::writeChar(const char Byte) +{ +#if defined (_WIN32) || defined( _WIN64) + // Number of bytes written + DWORD dwBytesWritten; + // Write the char to the serial device + // Return -1 if an error occured + if(!WriteFile(hSerial,&Byte,1,&dwBytesWritten,NULL)) return -1; + // Write operation successfull + return 1; +#endif +#if defined (__linux__) || defined(__APPLE__) + // Write the char + if (write(fd,&Byte,1)!=1) return -1; + + // Write operation successfull + return 1; +#endif +} + + + +//________________________________________ +// ::: Read/Write operation on strings ::: + + +/*! + \brief Write a string on the current serial port + \param receivedString : string to send on the port (must be terminated by '\0') + \return 1 success + \return -1 error while writting data + */ +int serialib::writeString(const char *receivedString) +{ +#if defined (_WIN32) || defined( _WIN64) + // Number of bytes written + DWORD dwBytesWritten; + // Write the string + if(!WriteFile(hSerial,receivedString,strlen(receivedString),&dwBytesWritten,NULL)) + // Error while writing, return -1 + return -1; + // Write operation successfull + return 1; +#endif +#if defined (__linux__) || defined(__APPLE__) + // Lenght of the string + int Lenght=strlen(receivedString); + // Write the string + if (write(fd,receivedString,Lenght)!=Lenght) return -1; + // Write operation successfull + return 1; +#endif +} + +// _____________________________________ +// ::: Read/Write operation on bytes ::: + + + +/*! + \brief Write an array of data on the current serial port + \param Buffer : array of bytes to send on the port + \param NbBytes : number of byte to send + \return 1 success + \return -1 error while writting data + */ +int serialib::writeBytes(const void *Buffer, const unsigned int NbBytes, unsigned int *NbBytesWritten) +{ +#if defined (_WIN32) || defined( _WIN64) + // Write data: + if(!WriteFile(hSerial, Buffer, NbBytes, (LPDWORD)NbBytesWritten, NULL)) + // Error while writing, return -1 + return -1; + // Write operation successfull + return 1; +#endif +#if defined (__linux__) || defined(__APPLE__) + // Write data + *NbBytesWritten = write (fd,Buffer,NbBytes); + if (*NbBytesWritten !=(ssize_t)NbBytes) return -1; + // Write operation successfull + return 1; +#endif +} + +int serialib::writeBytes(const void *Buffer, const unsigned int NbBytes) +{ + unsigned int NbBytesWritten; + return writeBytes(Buffer, NbBytes, &NbBytesWritten); +} + +/*! + \brief Wait for a byte from the serial device and return the data read + \param pByte : data read on the serial device + \param timeOut_ms : delay of timeout before giving up the reading + If set to zero, timeout is disable (Optional) + \return 1 success + \return 0 Timeout reached + \return -1 error while setting the Timeout + \return -2 error while reading the byte + */ +int serialib::readChar(char *pByte,unsigned int timeOut_ms) +{ +#if defined (_WIN32) || defined(_WIN64) + // Number of bytes read + DWORD dwBytesRead = 0; + + // Set the TimeOut + timeouts.ReadTotalTimeoutConstant=timeOut_ms; + + // Write the parameters, return -1 if an error occured + if(!SetCommTimeouts(hSerial, &timeouts)) return -1; + + // Read the byte, return -2 if an error occured + if(!ReadFile(hSerial,pByte, 1, &dwBytesRead, NULL)) return -2; + + // Return 0 if the timeout is reached + if (dwBytesRead==0) return 0; + + // The byte is read + return 1; +#endif +#if defined (__linux__) || defined(__APPLE__) + // Timer used for timeout + timeOut timer; + // Initialise the timer + timer.initTimer(); + // While Timeout is not reached + while (timer.elapsedTime_ms()0 success, return the number of bytes read + \return -1 error while setting the Timeout + \return -2 error while reading the byte + \return -3 MaxNbBytes is reached + */ +int serialib::readStringNoTimeOut(char *receivedString,char finalChar,unsigned int maxNbBytes) +{ + // Number of characters read + unsigned int NbBytes=0; + // Returned value from Read + char charRead; + + // While the buffer is not full + while (NbBytes0 success, return the number of bytes read (including the null character) + \return 0 timeout is reached + \return -1 error while setting the Timeout + \return -2 error while reading the character + \return -3 MaxNbBytes is reached + */ +int serialib::readString(char *receivedString,char finalChar,unsigned int maxNbBytes,unsigned int timeOut_ms) +{ + // Check if timeout is requested + if (timeOut_ms==0) return readStringNoTimeOut(receivedString,finalChar,maxNbBytes); + + // Number of bytes read + unsigned int nbBytes=0; + // Character read on serial device + char charRead; + // Timer used for timeout + timeOut timer; + long int timeOutParam; + + // Initialize the timer (for timeout) + timer.initTimer(); + + // While the buffer is not full + while (nbBytes0) + { + // Wait for a byte on the serial link with the remaining time as timeout + charRead=readChar(&receivedString[nbBytes],timeOutParam); + + // If a byte has been received + if (charRead==1) + { + // Check if the character received is the final one + if (receivedString[nbBytes]==finalChar) + { + // Final character: add the end character 0 + receivedString [++nbBytes]=0; + // Return the number of bytes read + return nbBytes; + } + // This is not the final character, just increase the number of bytes read + nbBytes++; + } + // Check if an error occured during reading char + // If an error occurend, return the error number + if (charRead<0) return charRead; + } + // Check if timeout is reached + if (timer.elapsedTime_ms()>timeOut_ms) + { + // Add the end caracter + receivedString[nbBytes]=0; + // Return 0 (timeout reached) + return 0; + } + } + + // Buffer is full : return -3 + return -3; +} + + +/*! + \brief Read an array of bytes from the serial device (with timeout) + \param buffer : array of bytes read from the serial device + \param maxNbBytes : maximum allowed number of bytes read + \param timeOut_ms : delay of timeout before giving up the reading + \param sleepDuration_us : delay of CPU relaxing in microseconds (Linux only) + In the reading loop, a sleep can be performed after each reading + This allows CPU to perform other tasks + \return >=0 return the number of bytes read before timeout or + requested data is completed + \return -1 error while setting the Timeout + \return -2 error while reading the byte + */ +int serialib::readBytes (void *buffer,unsigned int maxNbBytes,unsigned int timeOut_ms, unsigned int sleepDuration_us) +{ +#if defined (_WIN32) || defined(_WIN64) + // Avoid warning while compiling + UNUSED(sleepDuration_us); + + // Number of bytes read + DWORD dwBytesRead = 0; + + // Set the TimeOut + timeouts.ReadTotalTimeoutConstant=(DWORD)timeOut_ms; + + // Write the parameters and return -1 if an error occrured + if(!SetCommTimeouts(hSerial, &timeouts)) return -1; + + + // Read the bytes from the serial device, return -2 if an error occured + if(!ReadFile(hSerial,buffer,(DWORD)maxNbBytes,&dwBytesRead, NULL)) return -2; + + // Return the byte read + return dwBytesRead; +#endif +#if defined (__linux__) || defined(__APPLE__) + // Timer used for timeout + timeOut timer; + // Initialise the timer + timer.initTimer(); + unsigned int NbByteRead=0; + // While Timeout is not reached + while (timer.elapsedTime_ms()0) + { + // Increase the number of read bytes + NbByteRead+=Ret; + // Success : bytes has been read + if (NbByteRead>=maxNbBytes) + return NbByteRead; + } + // Suspend the loop to avoid charging the CPU + usleep (sleepDuration_us); + } + // Timeout reached, return the number of bytes read + return NbByteRead; +#endif +} + + + + +// _________________________ +// ::: Special operation ::: + + + +/*! + \brief Empty receiver buffer + Note that when using serial over USB on Unix systems, a delay of 20ms may be necessary before calling the flushReceiver function + \return If the function succeeds, the return value is nonzero. + If the function fails, the return value is zero. +*/ +char serialib::flushReceiver() +{ +#if defined (_WIN32) || defined(_WIN64) + // Purge receiver + return PurgeComm (hSerial, PURGE_RXCLEAR); +#endif +#if defined (__linux__) || defined(__APPLE__) + // Purge receiver + tcflush(fd,TCIFLUSH); + return true; +#endif +} + + + +/*! + \brief Return the number of bytes in the received buffer (UNIX only) + \return The number of bytes received by the serial provider but not yet read. +*/ +int serialib::available() +{ +#if defined (_WIN32) || defined(_WIN64) + // Device errors + DWORD commErrors; + // Device status + COMSTAT commStatus; + // Read status + ClearCommError(hSerial, &commErrors, &commStatus); + // Return the number of pending bytes + return commStatus.cbInQue; +#endif +#if defined (__linux__) || defined(__APPLE__) + int nBytes=0; + // Return number of pending bytes in the receiver + ioctl(fd, FIONREAD, &nBytes); + return nBytes; +#endif + +} + + + +// __________________ +// ::: I/O Access ::: + +/*! + \brief Set or unset the bit DTR (pin 4) + DTR stands for Data Terminal Ready + Convenience method :This method calls setDTR and clearDTR + \param status = true set DTR + status = false unset DTR + \return If the function fails, the return value is false + If the function succeeds, the return value is true. +*/ +bool serialib::DTR(bool status) +{ + if (status) + // Set DTR + return this->setDTR(); + else + // Unset DTR + return this->clearDTR(); +} + + +/*! + \brief Set the bit DTR (pin 4) + DTR stands for Data Terminal Ready + \return If the function fails, the return value is false + If the function succeeds, the return value is true. +*/ +bool serialib::setDTR() +{ +#if defined (_WIN32) || defined(_WIN64) + // Set DTR + currentStateDTR=true; + return EscapeCommFunction(hSerial,SETDTR); +#endif +#if defined (__linux__) || defined(__APPLE__) + // Set DTR + int status_DTR=0; + ioctl(fd, TIOCMGET, &status_DTR); + status_DTR |= TIOCM_DTR; + ioctl(fd, TIOCMSET, &status_DTR); + return true; +#endif +} + +/*! + \brief Clear the bit DTR (pin 4) + DTR stands for Data Terminal Ready + \return If the function fails, the return value is false + If the function succeeds, the return value is true. +*/ +bool serialib::clearDTR() +{ +#if defined (_WIN32) || defined(_WIN64) + // Clear DTR + currentStateDTR=false; + return EscapeCommFunction(hSerial,CLRDTR); +#endif +#if defined (__linux__) || defined(__APPLE__) + // Clear DTR + int status_DTR=0; + ioctl(fd, TIOCMGET, &status_DTR); + status_DTR &= ~TIOCM_DTR; + ioctl(fd, TIOCMSET, &status_DTR); + return true; +#endif +} + + + +/*! + \brief Set or unset the bit RTS (pin 7) + RTS stands for Data Termina Ready + Convenience method :This method calls setDTR and clearDTR + \param status = true set DTR + status = false unset DTR + \return false if the function fails + \return true if the function succeeds +*/ +bool serialib::RTS(bool status) +{ + if (status) + // Set RTS + return this->setRTS(); + else + // Unset RTS + return this->clearRTS(); +} + + +/*! + \brief Set the bit RTS (pin 7) + RTS stands for Data Terminal Ready + \return If the function fails, the return value is false + If the function succeeds, the return value is true. +*/ +bool serialib::setRTS() +{ +#if defined (_WIN32) || defined(_WIN64) + // Set RTS + currentStateRTS=true; + return EscapeCommFunction(hSerial,SETRTS); +#endif +#if defined (__linux__) || defined(__APPLE__) + // Set RTS + int status_RTS=0; + ioctl(fd, TIOCMGET, &status_RTS); + status_RTS |= TIOCM_RTS; + ioctl(fd, TIOCMSET, &status_RTS); + return true; +#endif +} + + + +/*! + \brief Clear the bit RTS (pin 7) + RTS stands for Data Terminal Ready + \return If the function fails, the return value is false + If the function succeeds, the return value is true. +*/ +bool serialib::clearRTS() +{ +#if defined (_WIN32) || defined(_WIN64) + // Clear RTS + currentStateRTS=false; + return EscapeCommFunction(hSerial,CLRRTS); +#endif +#if defined (__linux__) || defined(__APPLE__) + // Clear RTS + int status_RTS=0; + ioctl(fd, TIOCMGET, &status_RTS); + status_RTS &= ~TIOCM_RTS; + ioctl(fd, TIOCMSET, &status_RTS); + return true; +#endif +} + + + + +/*! + \brief Get the CTS's status (pin 8) + CTS stands for Clear To Send + \return Return true if CTS is set otherwise false + */ +bool serialib::isCTS() +{ +#if defined (_WIN32) || defined(_WIN64) + DWORD modemStat; + GetCommModemStatus(hSerial, &modemStat); + return modemStat & MS_CTS_ON; +#endif +#if defined (__linux__) || defined(__APPLE__) + int status=0; + //Get the current status of the CTS bit + ioctl(fd, TIOCMGET, &status); + return status & TIOCM_CTS; +#endif +} + + + +/*! + \brief Get the DSR's status (pin 6) + DSR stands for Data Set Ready + \return Return true if DTR is set otherwise false + */ +bool serialib::isDSR() +{ +#if defined (_WIN32) || defined(_WIN64) + DWORD modemStat; + GetCommModemStatus(hSerial, &modemStat); + return modemStat & MS_DSR_ON; +#endif +#if defined (__linux__) || defined(__APPLE__) + int status=0; + //Get the current status of the DSR bit + ioctl(fd, TIOCMGET, &status); + return status & TIOCM_DSR; +#endif +} + + + + + + +/*! + \brief Get the DCD's status (pin 1) + CDC stands for Data Carrier Detect + \return true if DCD is set + \return false otherwise + */ +bool serialib::isDCD() +{ +#if defined (_WIN32) || defined(_WIN64) + DWORD modemStat; + GetCommModemStatus(hSerial, &modemStat); + return modemStat & MS_RLSD_ON; +#endif +#if defined (__linux__) || defined(__APPLE__) + int status=0; + //Get the current status of the DCD bit + ioctl(fd, TIOCMGET, &status); + return status & TIOCM_CAR; +#endif +} + + +/*! + \brief Get the RING's status (pin 9) + Ring Indicator + \return Return true if RING is set otherwise false + */ +bool serialib::isRI() +{ +#if defined (_WIN32) || defined(_WIN64) + DWORD modemStat; + GetCommModemStatus(hSerial, &modemStat); + return modemStat & MS_RING_ON; +#endif +#if defined (__linux__) || defined(__APPLE__) + int status=0; + //Get the current status of the RING bit + ioctl(fd, TIOCMGET, &status); + return status & TIOCM_RNG; +#endif +} + + +/*! + \brief Get the DTR's status (pin 4) + DTR stands for Data Terminal Ready + May behave abnormally on Windows + \return Return true if CTS is set otherwise false + */ +bool serialib::isDTR() +{ +#if defined (_WIN32) || defined( _WIN64) + return currentStateDTR; +#endif +#if defined (__linux__) || defined(__APPLE__) + int status=0; + //Get the current status of the DTR bit + ioctl(fd, TIOCMGET, &status); + return status & TIOCM_DTR ; +#endif +} + + + +/*! + \brief Get the RTS's status (pin 7) + RTS stands for Request To Send + May behave abnormally on Windows + \return Return true if RTS is set otherwise false + */ +bool serialib::isRTS() +{ +#if defined (_WIN32) || defined(_WIN64) + return currentStateRTS; +#endif +#if defined (__linux__) || defined(__APPLE__) + int status=0; + //Get the current status of the CTS bit + ioctl(fd, TIOCMGET, &status); + return status & TIOCM_RTS; +#endif +} + + + + + + +// ****************************************** +// Class timeOut +// ****************************************** + + +/*! + \brief Constructor of the class timeOut. +*/ +// Constructor +timeOut::timeOut() +{} + + +/*! + \brief Initialise the timer. It writes the current time of the day in the structure PreviousTime. +*/ +//Initialize the timer +void timeOut::initTimer() +{ +#if defined (NO_POSIX_TIME) + LARGE_INTEGER tmp; + QueryPerformanceFrequency(&tmp); + counterFrequency = tmp.QuadPart; + // Used to store the previous time (for computing timeout) + QueryPerformanceCounter(&tmp); + previousTime = tmp.QuadPart; +#else + gettimeofday(&previousTime, NULL); +#endif +} + +/*! + \brief Returns the time elapsed since initialization. It write the current time of the day in the structure CurrentTime. + Then it returns the difference between CurrentTime and PreviousTime. + \return The number of microseconds elapsed since the functions InitTimer was called. + */ +//Return the elapsed time since initialization +unsigned long int timeOut::elapsedTime_ms() +{ +#if defined (NO_POSIX_TIME) + // Current time + LARGE_INTEGER CurrentTime; + // Number of ticks since last call + int sec; + + // Get current time + QueryPerformanceCounter(&CurrentTime); + + // Compute the number of ticks elapsed since last call + sec=CurrentTime.QuadPart-previousTime; + + // Return the elapsed time in milliseconds + return sec/(counterFrequency/1000); +#else + // Current time + struct timeval CurrentTime; + // Number of seconds and microseconds since last call + int sec,usec; + + // Get current time + gettimeofday(&CurrentTime, NULL); + + // Compute the number of seconds and microseconds elapsed since last call + sec=CurrentTime.tv_sec-previousTime.tv_sec; + usec=CurrentTime.tv_usec-previousTime.tv_usec; + + // If the previous usec is higher than the current one + if (usec<0) + { + // Recompute the microseonds and substract one second + usec=1000000-previousTime.tv_usec+CurrentTime.tv_usec; + sec--; + } + + // Return the elapsed time in milliseconds + return sec*1000+usec/1000; +#endif +} diff --git a/src/g2_2025_imu_reader_pkg/src/config/serialib.h b/src/g2_2025_imu_reader_pkg/src/config/serialib.h new file mode 100644 index 0000000..5d2d4eb --- /dev/null +++ b/src/g2_2025_imu_reader_pkg/src/config/serialib.h @@ -0,0 +1,270 @@ +/*! +\file serialib.h +\brief Header file of the class serialib. This class is used for communication over a serial device. +\author Philippe Lucidarme (University of Angers) +\version 2.0 +\date december the 27th of 2019 +This Serial library is used to communicate through serial port. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, +INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR +PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE X CONSORTIUM BE LIABLE FOR ANY CLAIM, +DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + +This is a licence-free software, it can be used by anyone who try to build a better world. +*/ + + +#ifndef SERIALIB_H +#define SERIALIB_H + +#if defined(__CYGWIN__) + // This is Cygwin special case + #include +#endif + +// Include for windows +#if defined (_WIN32) || defined (_WIN64) +#if defined(__GNUC__) + // This is MinGW special case + #include +#else + // sys/time.h does not exist on "actual" Windows + #define NO_POSIX_TIME +#endif + // Accessing to the serial port under Windows + #include +#endif + +// Include for Linux +#if defined (__linux__) || defined(__APPLE__) + #include + #include + #include + #include + #include + #include + #include + // File control definitions + #include + #include + #include +#endif + +/*! To avoid unused parameters */ +#define UNUSED(x) (void)(x) + +/** + * number of serial data bits + */ +enum SerialDataBits { + SERIAL_DATABITS_5, /**< 5 databits */ + SERIAL_DATABITS_6, /**< 6 databits */ + SERIAL_DATABITS_7, /**< 7 databits */ + SERIAL_DATABITS_8, /**< 8 databits */ + SERIAL_DATABITS_16, /**< 16 databits */ +}; + +/** + * number of serial stop bits + */ +enum SerialStopBits { + SERIAL_STOPBITS_1, /**< 1 stop bit */ + SERIAL_STOPBITS_1_5, /**< 1.5 stop bits */ + SERIAL_STOPBITS_2, /**< 2 stop bits */ +}; + +/** + * type of serial parity bits + */ +enum SerialParity { + SERIAL_PARITY_NONE, /**< no parity bit */ + SERIAL_PARITY_EVEN, /**< even parity bit */ + SERIAL_PARITY_ODD, /**< odd parity bit */ + SERIAL_PARITY_MARK, /**< mark parity */ + SERIAL_PARITY_SPACE /**< space bit */ +}; + +/*! \class serialib + \brief This class is used for communication over a serial device. +*/ +class serialib +{ +public: + + //_____________________________________ + // ::: Constructors and destructors ::: + + + + // Constructor of the class + serialib (); + + // Destructor + ~serialib (); + + + + //_________________________________________ + // ::: Configuration and initialization ::: + + + // Open a device + char openDevice(const char *Device, const unsigned int Bauds, + SerialDataBits Databits = SERIAL_DATABITS_8, + SerialParity Parity = SERIAL_PARITY_NONE, + SerialStopBits Stopbits = SERIAL_STOPBITS_1); + + // Check device opening state + bool isDeviceOpen(); + + // Close the current device + void closeDevice(); + + + + + //___________________________________________ + // ::: Read/Write operation on characters ::: + + + // Write a char + int writeChar (char); + + // Read a char (with timeout) + int readChar (char *pByte,const unsigned int timeOut_ms=0); + + + + + //________________________________________ + // ::: Read/Write operation on strings ::: + + + // Write a string + int writeString (const char *String); + + // Read a string (with timeout) + int readString ( char *receivedString, + char finalChar, + unsigned int maxNbBytes, + const unsigned int timeOut_ms=0); + + + + // _____________________________________ + // ::: Read/Write operation on bytes ::: + + + // Write an array of bytes + int writeBytes(const void *Buffer, const unsigned int NbBytes, unsigned int *NbBytesWritten); + int writeBytes (const void *Buffer, const unsigned int NbBytes); + + // Read an array of byte (with timeout) + int readBytes (void *buffer,unsigned int maxNbBytes,const unsigned int timeOut_ms=0, unsigned int sleepDuration_us=100); + + + + + // _________________________ + // ::: Special operation ::: + + + // Empty the received buffer + char flushReceiver(); + + // Return the number of bytes in the received buffer + int available(); + + + + + // _________________________ + // ::: Access to IO bits ::: + + + // Set CTR status (Data Terminal Ready, pin 4) + bool DTR(bool status); + bool setDTR(); + bool clearDTR(); + + // Set RTS status (Request To Send, pin 7) + bool RTS(bool status); + bool setRTS(); + bool clearRTS(); + + // Get RI status (Ring Indicator, pin 9) + bool isRI(); + + // Get DCD status (Data Carrier Detect, pin 1) + bool isDCD(); + + // Get CTS status (Clear To Send, pin 8) + bool isCTS(); + + // Get DSR status (Data Set Ready, pin 9) + bool isDSR(); + + // Get RTS status (Request To Send, pin 7) + bool isRTS(); + + // Get CTR status (Data Terminal Ready, pin 4) + bool isDTR(); + + +private: + // Read a string (no timeout) + int readStringNoTimeOut (char *String,char FinalChar,unsigned int MaxNbBytes); + + // Current DTR and RTS state (can't be read on WIndows) + bool currentStateRTS; + bool currentStateDTR; + + + + + +#if defined (_WIN32) || defined( _WIN64) + // Handle on serial device + HANDLE hSerial; + // For setting serial port timeouts + COMMTIMEOUTS timeouts; +#endif +#if defined (__linux__) || defined(__APPLE__) + int fd; +#endif + +}; + + + +/*! \class timeOut + \brief This class can manage a timer which is used as a timeout. + */ +// Class timeOut +class timeOut +{ +public: + + // Constructor + timeOut(); + + // Init the timer + void initTimer(); + + // Return the elapsed time since initialization + unsigned long int elapsedTime_ms(); + +private: +#if defined (NO_POSIX_TIME) + // Used to store the previous time (for computing timeout) + LONGLONG counterFrequency; + LONGLONG previousTime; +#else + // Used to store the previous time (for computing timeout) + struct timeval previousTime; +#endif +}; + +#endif // serialib_H diff --git a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.cpp b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.cpp index cdc06ce..8929a79 100644 --- a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.cpp +++ b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.cpp @@ -7,8 +7,99 @@ HardwareInterface::HardwareInterface() : Node("hardware_interface") { } -void HardwareInterface::some_hardware_method() { +void HardwareInterface::read() { + + char buffer[100]; RCLCPP_INFO(this->get_logger(), "Interacting with hardware..."); + serial.readString(buffer, '\n', 14, 2000); // or readBytes depending on the data format ;) + RCLCPP_INFO(this->get_logger(), "Data read from hardware: %s", buffer); + } -} // namespace assignments::two::g2_2025_lifecycle_node +void HardwareInterface::write(const std::string& data) { + RCLCPP_INFO(this->get_logger(), "Writing to hardware..."); + serial.writeString(data.c_str()); +} + +bool HardwareInterface::open_device(const std::string& device_path, int baud_rate) { + + char errorOpening = serial.openDevice(device_path.c_str(), baud_rate); + + if (errorOpening != 1) { + RCLCPP_ERROR(this->get_logger(), "Error opening serial port: %d", errorOpening); + return false; + } + + RCLCPP_INFO(this->get_logger(), "Serial port opened successfully."); + return true; +} + +bool HardwareInterface::is_device_open() { + return serial.isDeviceOpen(); +} + + +void HardwareInterface::close_device() { + + serial.closeDevice(); + +} + +class callback : public virtual mqtt::callback { +public: + void message_arrived(mqtt::const_message_ptr msg) override { + // Use a named logger since this class isn't a rclcpp::Node + RCLCPP_INFO(rclcpp::get_logger("hardware_interface"), + "Message received: %s", msg->get_payload_str().c_str()); + } +}; + + +void HardwareInterface::mqtt_listener() { + // Create persistent client and callback so they outlive this function. + if (!mqtt_client) { + mqtt_client = std::make_shared(SERVER_ADDRESS, CLIENT_ID); + } + if (!mqtt_cb) { + mqtt_cb = std::make_shared(); + } + + // attach callback (expects a reference) + mqtt_client->set_callback(*mqtt_cb); + + mqtt::connect_options connOpts; + connOpts.set_keep_alive_interval(20); + connOpts.set_clean_session(true); + + try { + mqtt_client->connect(connOpts)->wait(); + RCLCPP_INFO(this->get_logger(), "Connected to broker"); + + mqtt_client->subscribe(TOPIC, 1)->wait(); + RCLCPP_INFO(this->get_logger(), "Subscribed to topic: %s", TOPIC.c_str()); + // Note: do not disconnect here; keep client alive until close_mqtt_conn() + } catch (const mqtt::exception& exc) { + RCLCPP_ERROR(this->get_logger(), "Error: %s", exc.what()); + } +} + +void HardwareInterface::close_mqtt_conn() { + try { + if (mqtt_client) { + if (mqtt_client->is_connected()) { + mqtt_client->disconnect()->wait(); + RCLCPP_INFO(this->get_logger(), "Disconnected MQTT client"); + } + // reset to allow destruction + mqtt_client.reset(); + } + if (mqtt_cb) { + mqtt_cb.reset(); + } + } catch (const mqtt::exception& exc) { + RCLCPP_ERROR(this->get_logger(), "Error while disconnecting MQTT: %s", exc.what()); + } +} + + +} // namespace assignments::two::g2_2025_lifecycle_node diff --git a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.hpp b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.hpp index c347245..7db910f 100644 --- a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.hpp +++ b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.hpp @@ -14,6 +14,16 @@ #include #include #include +#include +#include +#include +#include +#include "config/serialib.h" +#include + + +#include +#include #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" @@ -24,8 +34,23 @@ class HardwareInterface : public rclcpp::Node { public: HardwareInterface(); + void read(); + void write(const std::string& data); + bool open_device(const std::string& device_path, int baud_rate); + bool is_device_open(); + void close_device(); + void mqtt_listener(); + void close_mqtt_conn(); private: - void some_hardware_method(); + serialib serial; + // MQTT defaults. Use static inline so they can be initialized in-class. + static inline const std::string SERVER_ADDRESS = "tcp://localhost:1884"; + static inline const std::string CLIENT_ID = "cpp_mqtt_client"; + static inline const std::string TOPIC = "esp32/imu"; + + std::shared_ptr mqtt_client; + std::shared_ptr mqtt_cb; + }; } // namespace assignments::two::g2_2025_lifecycle_node diff --git a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.cpp b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.cpp index c80846e..8e556be 100644 --- a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.cpp +++ b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.cpp @@ -1,4 +1,5 @@ #include "lifecycle_manager.hpp" +#include "hardware_interface.hpp" namespace assignments::two::g2_2025_lifecycle_node { @@ -10,10 +11,16 @@ LifecycleManager::LifecycleManager() : rclcpp_lifecycle::LifecycleNode("lifecycl imu_publisher = this->create_publisher("imu_data", 10); RCLCPP_INFO(this->get_logger(), "IMU Publisher has been created"); + device_path_ = this->declare_parameter("device_path", "/dev/ttyUSB0"); + baudrate_ = this->declare_parameter("baudrate", 115200); + communication_type_ = this->declare_parameter("comm_t", "serial"); // placeholder default serial or param mqtt + + // hardware interface instance + hw_interface = std::make_shared(); } -void LifecycleManager::publish_imu_data(const std::string & data) { // placeholderrrrr +void LifecycleManager::publish_imu_data(const std::string& data) { // placeholderrrrr auto message = std_msgs::msg::String(); message.data = data; imu_publisher->publish(message); @@ -31,25 +38,84 @@ void LifecycleManager::publish_imu_data(const std::string & data) { // placehold rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleManager::on_configure(const rclcpp_lifecycle::State&) { + + RCLCPP_INFO(this->get_logger(), "configuring lifecycle..."); + + if (communication_type_ == "mqtt") { + // RCLCPP_INFO(this->get_logger(), "Setting up MQTT communication..."); + // hw_interface->mqtt_listener(); + + } else { + RCLCPP_INFO(this->get_logger(), "Using serial communication."); + + if (!hw_interface->open_device(device_path_, baudrate_)) { + RCLCPP_ERROR(this->get_logger(), "Failed to open hardware device during configuration."); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; + } + } + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleManager::on_activate(const rclcpp_lifecycle::State&) { + RCLCPP_INFO(this->get_logger(), "activating lifecycle..."); + + if (communication_type_ == "mqtt") { + RCLCPP_INFO(this->get_logger(), "Setting up MQTT communication..."); + hw_interface->mqtt_listener(); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + + } else { + RCLCPP_INFO(this->get_logger(), "Using serial communication."); + if (!hw_interface->is_device_open()) { + RCLCPP_ERROR(this->get_logger(), "Hardware device is not open during activation."); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; + } + if (!hw_interface->open_device(device_path_, baudrate_)) { + + RCLCPP_ERROR(this->get_logger(), "Failed to open hardware device during configuration."); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; + } + } + + RCLCPP_INFO(this->get_logger(), "Hardware device is open, starting data read..."); + + hw_interface->read(); + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleManager::on_deactivate(const rclcpp_lifecycle::State&) { + RCLCPP_INFO(this->get_logger(), "deactivating lifecycle..."); + + if (communication_type_ == "mqtt") + { + hw_interface->close_mqtt_conn(); + } else { + hw_interface->close_device(); + } + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleManager::on_shutdown(const rclcpp_lifecycle::State&) { + RCLCPP_INFO(this->get_logger(), "shutting down lifecycle..."); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +LifecycleManager::on_cleanup(const rclcpp_lifecycle::State&) { + + RCLCPP_INFO(this->get_logger(), "cleaning up lifecycle..."); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + } // namespace assignments::two::g2_2025_lifecycle_node diff --git a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.hpp b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.hpp index 49f3f95..2549f2a 100644 --- a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.hpp +++ b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.hpp @@ -18,6 +18,7 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "std_msgs/msg/string.hpp" +#include "hardware_interface.hpp" namespace assignments::two::g2_2025_lifecycle_node { @@ -26,11 +27,21 @@ public: LifecycleManager(); private: rclcpp::Publisher::SharedPtr imu_publisher; - void publish_imu_data(const std::string & data); + + std::string device_path_; + int baudrate_; + std::string communication_type_; + + // Hardware interface to interact with the IMU device. + std::shared_ptr hw_interface; + + void publish_imu_data(const std::string & data); rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(const rclcpp_lifecycle::State&); rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate(const rclcpp_lifecycle::State&); rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State&); rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown(const rclcpp_lifecycle::State&); + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State&); + }; } // namespace assignments::two::g2_2025_lifecycle_node From 7fbc44cd930c77a09d0f0c1a18ffc7add8372a83 Mon Sep 17 00:00:00 2001 From: Vincent Winter Date: Fri, 31 Oct 2025 17:48:13 +0100 Subject: [PATCH 04/17] fix(hw_interface): Correct the buffersize for incomming string --- .../src/g2_2025_lifecycle_node/nodes/hardware_interface.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.cpp b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.cpp index 8929a79..d6b9c8d 100644 --- a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.cpp +++ b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.cpp @@ -9,9 +9,9 @@ HardwareInterface::HardwareInterface() : Node("hardware_interface") { void HardwareInterface::read() { - char buffer[100]; + char buffer[114]; RCLCPP_INFO(this->get_logger(), "Interacting with hardware..."); - serial.readString(buffer, '\n', 14, 2000); // or readBytes depending on the data format ;) + serial.readString(buffer, '\n', 114, 2000); // or readBytes depending on the data format ;) RCLCPP_INFO(this->get_logger(), "Data read from hardware: %s", buffer); } From aff882ebdd794726a646501d08275effaa948ebb Mon Sep 17 00:00:00 2001 From: Mohammad Date: Fri, 31 Oct 2025 19:36:49 +0100 Subject: [PATCH 05/17] fix(lifcycle & hw): fixed serial read and better mqtt conf funcs --- .../nodes/hardware_interface.cpp | 42 ++++++++++++--- .../nodes/hardware_interface.hpp | 19 +++++-- .../nodes/lifecycle_manager.cpp | 54 +++++++++---------- .../nodes/lifecycle_manager.hpp | 5 +- 4 files changed, 77 insertions(+), 43 deletions(-) diff --git a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.cpp b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.cpp index d6b9c8d..a9eb9b9 100644 --- a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.cpp +++ b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.cpp @@ -7,13 +7,35 @@ HardwareInterface::HardwareInterface() : Node("hardware_interface") { } -void HardwareInterface::read() { +void HardwareInterface::start_read() { + if (reading_.load()) { + return; + } - char buffer[114]; - RCLCPP_INFO(this->get_logger(), "Interacting with hardware..."); - serial.readString(buffer, '\n', 114, 2000); // or readBytes depending on the data format ;) - RCLCPP_INFO(this->get_logger(), "Data read from hardware: %s", buffer); + reading_.store(true); + + read_thread_ = std::thread([this]() { + char buffer[114]; + RCLCPP_INFO(this->get_logger(), "reader thread started"); + while (reading_.load()) { + int ret = serial.readString(buffer, '\n', 114, 1000); + if (ret > 0) { + RCLCPP_INFO(this->get_logger(), "Data read from hardware: %s", buffer); + } else { + // timeout of geen data. + } + } + RCLCPP_INFO(this->get_logger(), "reader thread exiting"); + }); +} + +void HardwareInterface::stop_read() { + if (!reading_.load()) return; + reading_.store(false); + if (read_thread_.joinable()) { + read_thread_.join(); + } } void HardwareInterface::write(const std::string& data) { @@ -55,7 +77,7 @@ public: }; -void HardwareInterface::mqtt_listener() { +void HardwareInterface::mqtt_configure() { // Create persistent client and callback so they outlive this function. if (!mqtt_client) { mqtt_client = std::make_shared(SERVER_ADDRESS, CLIENT_ID); @@ -63,9 +85,15 @@ void HardwareInterface::mqtt_listener() { if (!mqtt_cb) { mqtt_cb = std::make_shared(); } + mqtt_connect(); +} - // attach callback (expects a reference) +void HardwareInterface::mqtt_reader() { + RCLCPP_INFO(this->get_logger(), "MQTT listener started"); mqtt_client->set_callback(*mqtt_cb); +} + +void HardwareInterface::mqtt_connect() { mqtt::connect_options connOpts; connOpts.set_keep_alive_interval(20); diff --git a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.hpp b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.hpp index 7db910f..26419ca 100644 --- a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.hpp +++ b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.hpp @@ -6,6 +6,11 @@ * * Changelog: * [28-10-2025] M.khalaf: Implemented template. + * [28-10-2025] M.khalaf: Added serial communication support. + * [30-10-2025] M.khalaf: Added MQTT support. + * [20-10-2025] M.khalaf: Refactored code for readability. + * [30-10-2025] M.khalaf: Improved error handling. + * [31-10-2025] M.khalaf: Fixed serial read and mqtt configurations. */ #pragma once @@ -14,6 +19,8 @@ #include #include #include +#include +#include #include #include #include @@ -25,6 +32,7 @@ #include #include +#include "sensor_msgs/msg/imu.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" @@ -34,23 +42,28 @@ class HardwareInterface : public rclcpp::Node { public: HardwareInterface(); - void read(); + void start_read(); + void stop_read(); void write(const std::string& data); bool open_device(const std::string& device_path, int baud_rate); bool is_device_open(); void close_device(); - void mqtt_listener(); + void mqtt_connect(); void close_mqtt_conn(); + void mqtt_configure(); + void mqtt_reader(); private: serialib serial; // MQTT defaults. Use static inline so they can be initialized in-class. - static inline const std::string SERVER_ADDRESS = "tcp://localhost:1884"; + static inline const std::string SERVER_ADDRESS = "tcp://localhost:1883"; static inline const std::string CLIENT_ID = "cpp_mqtt_client"; static inline const std::string TOPIC = "esp32/imu"; std::shared_ptr mqtt_client; std::shared_ptr mqtt_cb; + std::thread read_thread_; + std::atomic_bool reading_{false}; }; } // namespace assignments::two::g2_2025_lifecycle_node diff --git a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.cpp b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.cpp index 8e556be..893ab3f 100644 --- a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.cpp +++ b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.cpp @@ -8,7 +8,7 @@ LifecycleManager::LifecycleManager() : rclcpp_lifecycle::LifecycleNode("lifecycl RCLCPP_INFO(this->get_logger(), "LifecycleManager node is ready"); - imu_publisher = this->create_publisher("imu_data", 10); + imu_publisher_ = this->create_publisher("imu_data", 10); RCLCPP_INFO(this->get_logger(), "IMU Publisher has been created"); device_path_ = this->declare_parameter("device_path", "/dev/ttyUSB0"); @@ -20,11 +20,9 @@ LifecycleManager::LifecycleManager() : rclcpp_lifecycle::LifecycleNode("lifecycl } -void LifecycleManager::publish_imu_data(const std::string& data) { // placeholderrrrr - auto message = std_msgs::msg::String(); - message.data = data; - imu_publisher->publish(message); - RCLCPP_INFO(this->get_logger(), "Published IMU data: %s", data.c_str()); +void LifecycleManager::publish_imu_data(const sensor_msgs::msg::Imu::SharedPtr msg) { + imu_publisher_->publish(*msg); + RCLCPP_INFO(this->get_logger(), "Published IMU data"); } /* @@ -43,9 +41,8 @@ LifecycleManager::on_configure(const rclcpp_lifecycle::State&) { RCLCPP_INFO(this->get_logger(), "configuring lifecycle..."); if (communication_type_ == "mqtt") { - // RCLCPP_INFO(this->get_logger(), "Setting up MQTT communication..."); - // hw_interface->mqtt_listener(); - + RCLCPP_INFO(this->get_logger(), "Setting up MQTT communication..."); + hw_interface->mqtt_configure(); } else { RCLCPP_INFO(this->get_logger(), "Using serial communication."); @@ -54,7 +51,7 @@ LifecycleManager::on_configure(const rclcpp_lifecycle::State&) { return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; } } - + RCLCPP_INFO(this->get_logger(), "Lifecycle configured successfully."); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } @@ -64,28 +61,15 @@ LifecycleManager::on_activate(const rclcpp_lifecycle::State&) { RCLCPP_INFO(this->get_logger(), "activating lifecycle..."); if (communication_type_ == "mqtt") { - RCLCPP_INFO(this->get_logger(), "Setting up MQTT communication..."); - hw_interface->mqtt_listener(); + RCLCPP_INFO(this->get_logger(), "Reading on MQTT..."); + hw_interface->mqtt_reader(); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } else { - RCLCPP_INFO(this->get_logger(), "Using serial communication."); - if (!hw_interface->is_device_open()) { - RCLCPP_ERROR(this->get_logger(), "Hardware device is not open during activation."); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; - } - if (!hw_interface->open_device(device_path_, baudrate_)) { - - RCLCPP_ERROR(this->get_logger(), "Failed to open hardware device during configuration."); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; - } + RCLCPP_INFO(this->get_logger(), "Reading on Serial..."); + hw_interface->start_read(); } - RCLCPP_INFO(this->get_logger(), "Hardware device is open, starting data read..."); - - hw_interface->read(); - - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } @@ -97,12 +81,22 @@ LifecycleManager::on_deactivate(const rclcpp_lifecycle::State&) { if (communication_type_ == "mqtt") { hw_interface->close_mqtt_conn(); - } else { + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + + } else + { + if (!hw_interface->is_device_open()) { + RCLCPP_ERROR(this->get_logger(), "Hardware device is not open during activation."); + hw_interface->close_device(); + } + RCLCPP_INFO(this->get_logger(), "Hardware device is open,closing device..."); + hw_interface->stop_read(); hw_interface->close_device(); + RCLCPP_INFO(this->get_logger(), "Lifecycle deactivated successfully."); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } - - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleManager::on_shutdown(const rclcpp_lifecycle::State&) { diff --git a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.hpp b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.hpp index 2549f2a..ac33900 100644 --- a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.hpp +++ b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.hpp @@ -26,8 +26,7 @@ class LifecycleManager : public rclcpp_lifecycle::LifecycleNode { public: LifecycleManager(); private: - rclcpp::Publisher::SharedPtr imu_publisher; - + rclcpp::Publisher::SharedPtr imu_publisher_; std::string device_path_; int baudrate_; @@ -36,7 +35,7 @@ private: // Hardware interface to interact with the IMU device. std::shared_ptr hw_interface; - void publish_imu_data(const std::string & data); + void publish_imu_data(const sensor_msgs::msg::Imu::SharedPtr msg); rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(const rclcpp_lifecycle::State&); rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate(const rclcpp_lifecycle::State&); rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State&); From 03cf747ad6c5f6bf6911df827fd41ded1375a86d Mon Sep 17 00:00:00 2001 From: Vincent Winter Date: Fri, 31 Oct 2025 20:10:36 +0100 Subject: [PATCH 06/17] fix(hw_interface): Increase buffersize by 2 --- doc/architecture/nodes/HardwareInterface.md | 452 ++++++++++++++++++ doc/architecture/nodes/LifecycleManager.md | 223 +++++++++ doc/installation/installation.md | 13 +- doc/tests/LifecycleManager.md | 229 +++++++++ src/g2_2025_imu_reader_pkg/CMakeLists.txt | 32 ++ .../nodes/hardware_interface.cpp | 59 ++- .../nodes/hardware_interface.hpp | 4 + .../nodes/lifecycle_manager.cpp | 19 +- .../nodes/lifecycle_manager.hpp | 9 +- .../test/LifecycleManager.test.cpp | 234 +++++++++ 10 files changed, 1248 insertions(+), 26 deletions(-) create mode 100644 doc/architecture/nodes/HardwareInterface.md create mode 100644 doc/architecture/nodes/LifecycleManager.md create mode 100644 doc/tests/LifecycleManager.md create mode 100644 src/g2_2025_imu_reader_pkg/test/LifecycleManager.test.cpp diff --git a/doc/architecture/nodes/HardwareInterface.md b/doc/architecture/nodes/HardwareInterface.md new file mode 100644 index 0000000..cbdf127 --- /dev/null +++ b/doc/architecture/nodes/HardwareInterface.md @@ -0,0 +1,452 @@ +# HardwareInterface (`assignments::two::g2_2025_lifecycle_node`) + +## Overview +The `HardwareInterface` is a ROS2 node responsible for managing low-level hardware communication with IMU sensors. It abstracts the complexity of dual communication backends (serial and MQTT), provides JSON parsing of sensor data, and publishes standardized `sensor_msgs::msg::Imu` messages to the ROS2 ecosystem. The node handles continuous data acquisition, buffering of fragmented reads, and robust error recovery. + +#### Implementation Details + +**Public Methods** + +- **`start_read()`**: Spawns a background thread that continuously reads JSON payloads from the serial device +- **`stop_read()`**: Signals the reader thread to exit and joins it for clean shutdown +- **`write(const std::string& data)`**: Writes data to the serial device +- **`open_device(const std::string& device_path, int baud_rate)`**: Opens and configures a serial port +- **`is_device_open()`**: Checks if the serial device is currently open +- **`close_device()`**: Closes the serial port and releases resources +- **`mqtt_configure()`**: Initializes MQTT client and broker connection setup +- **`mqtt_reader()`**: Attaches MQTT callbacks to begin receiving messages +- **`mqtt_connect()`**: Establishes connection to the MQTT broker and subscribes to topics +- **`close_mqtt_conn()`**: Disconnects from broker and cleans up MQTT resources +- **`parse_data(const std::string& data)`**: Parses JSON payload into `sensor_msgs::msg::Imu` and publishes +- **`publish_imu_data(const sensor_msgs::msg::Imu::SharedPtr msg)`**: Publishes an IMU message to the ROS topic + +**Constructor** + +```cpp +HardwareInterface() +``` + +- Initializes ROS2 node with name `hardware_interface` +- Creates a ROS2 publisher for `sensor_msgs::msg::Imu` on topic `imu_data` with queue size 10 +- Logs initialization status + +**Member Variables** + +- **`serialib serial`**: Encapsulates serial port communication +- **`std::shared_ptr mqtt_client`**: Persistent MQTT async client for broker communication +- **`std::shared_ptr mqtt_cb`**: MQTT callback handler for message arrival events +- **`std::thread read_thread_`**: Background reader thread for continuous serial data acquisition +- **`std::atomic_bool reading_`**: Thread-safe flag to signal the reader thread to stop +- **`std::string partial_buffer_`**: Accumulates fragmented serial reads until complete messages are available +- **`rclcpp::Publisher::SharedPtr imu_publisher`**: ROS2 publisher for IMU data + +**MQTT Constants** + +- **`SERVER_ADDRESS`**: `"tcp://localhost:1883"` — Default MQTT broker address +- **`CLIENT_ID`**: `"cpp_mqtt_client"` — MQTT client identifier +- **`TOPIC`**: `"esp32/imu"` — Default subscription topic for IMU data + +--- + +## Core Functions + +### `void start_read()` + +Initiates continuous serial data acquisition in a background thread. + +**Behavior:** +- Checks if a reader thread is already running; returns if so +- Sets the `reading_` atomic flag to true +- Spawns a thread that: + 1. Allocates a 116-byte buffer + 2. Enters a loop that runs while `reading_` is true + 3. Calls `serial.readString()` with a 1-second timeout + 4. Accumulates received bytes into `partial_buffer_` + 5. Splits on newline (`\n`) to extract complete lines + 6. Trims whitespace and strips leading garbage up to the first `{` + 7. Validates that a closing `}` exists; if not, waits for more data + 8. Calls `parse_data()` on each complete JSON line +- Returns immediately while the thread continues running + +**Error Handling:** +- Invalid JSON lines are logged as errors in `parse_data()` but do not crash the thread + +--- + +### `void stop_read()` + +Cleanly terminates the background reader thread. + +**Behavior:** +- Returns immediately if not currently reading +- Sets `reading_` atomic flag to false to signal the thread +- Joins the thread to wait for its completion +- Ensures all resources are released before returning + +**Thread Safety:** +- Uses atomic flag for lock-free signaling +- Blocks until thread joins, guaranteeing clean shutdown + +--- + +### `void parse_data(const std::string& data)` + +Deserializes a JSON string into a ROS2 IMU message and publishes it. + +**Behavior:** +- Attempts to parse the input string as JSON using `nlohmann::json` +- Creates a new `sensor_msgs::msg::Imu` message and populates: + - **Header**: Sets `stamp` to current time via `this->now()` and `frame_id` to `"imu_link"` + - **Linear Acceleration**: Extracts from JSON `"accel"` object fields `"x"`, `"y"`, `"z"` (defaults to 0.0 if missing) + - **Angular Velocity**: Extracts from JSON `"gyro"` object fields `"x"`, `"y"`, `"z"` (defaults to 0.0 if missing) +- Logs the parsed IMU values at INFO level +- Calls `publish_imu_data()` to send the message to the ROS topic + +**Expected JSON Format:** +```json +{ + "accel": {"x": 0.037, "y": -1.164, "z": 9.775}, + "gyro": {"x": -0.024, "y": -0.014, "z": -0.001}, + "Temp": 41.01 +} +``` + +**Error Handling:** +- Catches `nlohmann::json::exception` and logs parsing errors without crashing +- Handles missing fields gracefully using `.value()` with default 0.0 + +--- + +### `void publish_imu_data(const sensor_msgs::msg::Imu::SharedPtr msg)` + +Publishes an IMU message to the ROS2 topic. + +**Behavior:** +- Dereferences the shared pointer and publishes to `imu_publisher` +- Operation is thread-safe (rclcpp publishers support multi-threaded access) + +--- + +### `void mqtt_configure()` + +Sets up the MQTT infrastructure for broker communication. + +**Behavior:** +- Creates a persistent `mqtt::async_client` pointing to `SERVER_ADDRESS` if not already created +- Creates a persistent MQTT callback handler if not already created +- Calls `mqtt_connect()` to establish the connection + +**Rationale for Persistence:** +- Client and callback objects must outlive this function to maintain the connection +- Using `shared_ptr` ensures proper lifetime management + +--- + +### `void mqtt_reader()` + +Attaches callbacks to the MQTT client to begin receiving messages. + +**Behavior:** +- Sets the callback handler on the async client via `mqtt_client->set_callback(*mqtt_cb)` +- Logs that the listener has started +- Returns; the async client handles message reception in background threads + +--- + +### `void mqtt_connect()` + +Establishes connection to the MQTT broker and subscribes to the sensor topic. + +**Behavior:** +- Creates `mqtt::connect_options` with: + - Keep-alive interval: 20 seconds + - Clean session: true (no prior session state restored) +- Calls `mqtt_client->connect()` and waits for completion +- Subscribes to `TOPIC` (default: `"esp32/imu"`) with QoS level 1 +- Logs successful connection and subscription + +**Error Handling:** +- Catches `mqtt::exception` and logs errors; does not throw or crash + +--- + +### `void close_mqtt_conn()` + +Cleanly disconnects from the MQTT broker and cleans up resources. + +**Behavior:** +- Checks if the client is connected before attempting disconnect +- Calls `mqtt_client->disconnect()` and waits for completion +- Resets `mqtt_client` and `mqtt_cb` shared pointers to allow object destruction +- Logs disconnection and cleanup status + +**Error Handling:** +- Catches `mqtt::exception` and logs errors +- Continues cleanup even if errors occur + +--- + +### `bool open_device(const std::string& device_path, int baud_rate)` + +Opens and configures a serial port device. + +**Parameters:** +- `device_path`: Path to the serial device (e.g., `"/dev/ttyUSB0"`) +- `baud_rate`: Communication speed in bits per second (e.g., `115200`) + +**Returns:** +- `true` if device opened successfully +- `false` if an error occurs + +**Behavior:** +- Calls `serial.openDevice()` with the provided path and baud rate +- Checks if the returned value is 1 (success) +- Logs success or error status + +--- + +### `bool is_device_open()` + +Queries the current state of the serial device. + +**Returns:** +- `true` if the device is open +- `false` otherwise + +--- + +### `void close_device()` + +Closes the serial port and releases resources. + +**Behavior:** +- Calls `serial.closeDevice()` +- Ensures the device is no longer accessible for reads/writes + +--- + +### `void write(const std::string& data)` + +Writes data to the serial device. + +**Behavior:** +- Logs the write operation +- Calls `serial.writeString()` with the data + +--- + +## MQTT Callback Handler + +### `class callback : public virtual mqtt::callback` + +A nested class that implements the Paho MQTT callback interface. + +**Method: `message_arrived(mqtt::const_message_ptr msg)`** + +- Invoked when a message arrives on a subscribed topic +- Extracts the payload string via `msg->get_payload_str()` +- Calls `parse_data()` to deserialize and publish the IMU message +--- + +## Data Flow Architecture + +### Serial Data Path + +``` +Physical IMU Device + ↓ +Serial Port (e.g., /dev/ttyUSB0 @ 115200 baud) + ↓ +start_read() Background Thread + ↓ +serial.readString(buffer, 1000ms timeout) + ↓ +Accumulate into partial_buffer_ + ↓ +Split on '\n' and Extract Complete Lines + ↓ +Sanitize (trim, strip garbage before '{') + ↓ +Validate JSON Structure (must have '{' and '}') + ↓ +parse_data(json_line) + ↓ +JSON Parse → sensor_msgs::msg::Imu + ↓ +publish_imu_data() → ROS Topic `imu_data` +``` + +### MQTT Data Path + +``` +MQTT Broker (tcp://localhost:1883) + ↓ +MQTT Async Client (mqtt_client) + ↓ +Topic Subscription (esp32/imu) + ↓ +MQTT Callback (message_arrived) + ↓ +parse_data(payload_string) + ↓ +JSON Parse → sensor_msgs::msg::Imu + ↓ +publish_imu_data() → ROS Topic `imu_data` +``` + +--- + +## Buffer Management & Message Reconstruction + +The `partial_buffer_` member implements a robust strategy for handling fragmented serial reads: + +1. **Accumulation**: Each serial read chunk is appended to `partial_buffer_` +2. **Line Splitting**: Buffer is searched for newline delimiters +3. **Validation**: Each line is checked for JSON structure (presence of `{` and `}`) +4. **Sanitization**: Leading garbage (characters before `{`) is stripped +5. **Incomplete Message Handling**: If a line lacks a closing brace, it's pushed back to the buffer and the loop waits for more data +6. **Parse & Publish**: Complete JSON lines are parsed and published + +**Why This Matters:** +- Serial reads may return fragments of a JSON message (e.g., `",\"gyro\":{...}"`) +- Multiple messages can arrive in a single read +- Buffering ensures robust handling of all edge cases + +--- + +## Error Handling & Recovery + +| Scenario | Behavior | Recovery | +|----------|----------|----------| +| Serial read timeout | Loop continues, checks `reading_` flag | Automatic retry on next iteration | +| Incomplete JSON in buffer | Fragment is retained; waits for next read | No action needed; accumulation handles it | +| JSON parse error | Error logged; thread continues listening | Move to next message | +| Serial device disconnect | readString returns 0; loop continues | Application can reconnect via `open_device()` | +| MQTT broker unreachable | Exception caught and logged | Retry via `mqtt_connect()` | +| MQTT message error | Exception caught and logged | Connection remains for next message | + +--- + +## Thread Safety + +- **Atomic Flag**: `reading_` uses `std::atomic_bool` for lock-free thread signaling +- **Publisher Thread-Safety**: rclcpp publishers are thread-safe; `parse_data()` can safely publish from reader thread +- **Resource Cleanup**: `stop_read()` joins the thread before returning, ensuring clean shutdown +- **No Shared Mutable State**: Aside from `reading_` and the publisher, thread does not access other class members during execution + +--- + +## Integration with LifecycleManager + +The `LifecycleManager` orchestrates `HardwareInterface` lifecycle: + +| Lifecycle Phase | LifecycleManager Call | HardwareInterface Action | +|---|---|---| +| **Configure** | `hw_interface->open_device()` or `mqtt_configure()` | Open serial port or set up MQTT client | +| **Activate** | `hw_interface->start_read()` or `mqtt_reader()` | Spawn reader thread or attach MQTT callbacks | +| **Deactivate** | `hw_interface->stop_read()` or `close_mqtt_conn()` | Stop reader thread and join; disconnect MQTT | +| **Cleanup** | `hw_interface->close_device()` | Release serial port | + +--- + +## Usage Example + +### Direct Instantiation (Advanced) + +```cpp +// Create an instance (normally managed by LifecycleManager) +auto hw = std::make_shared(); + +// Serial workflow +hw->open_device("/dev/ttyUSB0", 115200); +hw->start_read(); +// ... node spins and publishes IMU data ... +hw->stop_read(); +hw->close_device(); + +// MQTT workflow +hw->mqtt_configure(); +hw->mqtt_reader(); +// ... node spins and publishes IMU data ... +hw->close_mqtt_conn(); +``` + +### Via LifecycleManager (Recommended) + +```bash +# Launch and manage via lifecycle +ros2 run g2_2025_imu_reader_pkg g2_2025_lifecycle_node \ + --ros-args -p device_path:=/dev/ttyUSB0 -p baudrate:=115200 -p comm_t:=serial + +# Configure and activate +ros2 lifecycle set /lifecycle_manager configure +ros2 lifecycle set /lifecycle_manager activate + +# Subscribe to IMU data +ros2 topic echo /imu_data + +# Deactivate and cleanup +ros2 lifecycle set /lifecycle_manager deactivate +ros2 lifecycle set /lifecycle_manager shutdown +``` + +--- + +## Design Patterns + +1. **Abstraction Pattern**: Encapsulates serial and MQTT complexity behind a unified interface +2. **Thread Management**: Background reader thread with atomic signaling for clean shutdown +3. **Buffer Accumulation**: Handles fragmented reads and multi-message batches robustly +4. **Dual Backend Strategy**: Runtime selection of communication mode (serial or MQTT) +5. **JSON Deserialization**: Uses industry-standard `nlohmann::json` for robust parsing + +--- + + +## Dependencies + +- **rclcpp**: ROS2 C++ client library +- **sensor_msgs**: ROS2 standard sensor message definitions +- **paho-mqtt**: Paho C/C++ MQTT client library +- **nlohmann/json**: Header-only JSON parsing library +- **serialib**: Custom serial communication wrapper + +--- + +## Class Diagram + +``` +┌─────────────────────────────────────┐ +│ HardwareInterface │ +│ (rclcpp::Node) │ +├─────────────────────────────────────┤ +│ Private Members: │ +│ - serialib serial │ +│ - async_client mqtt_client │ +│ - callback mqtt_cb │ +│ - thread read_thread_ │ +│ - atomic_bool reading_ │ +│ - string partial_buffer_ │ +│ - Publisher imu_publisher │ +├─────────────────────────────────────┤ +│ Public Methods: │ +│ + start_read() │ +│ + stop_read() │ +│ + open_device() │ +│ + close_device() │ +│ + is_device_open() │ +│ + write() │ +│ + mqtt_configure() │ +│ + mqtt_reader() │ +│ + mqtt_connect() │ +│ + close_mqtt_conn() │ +│ + parse_data() │ +│ + publish_imu_data() │ +└─────────────────────────────────────┘ + ↑ + │ orchestrated by + │ + ┌──────────────────┐ + │ LifecycleManager │ + └──────────────────┘ +``` diff --git a/doc/architecture/nodes/LifecycleManager.md b/doc/architecture/nodes/LifecycleManager.md new file mode 100644 index 0000000..82f9f6a --- /dev/null +++ b/doc/architecture/nodes/LifecycleManager.md @@ -0,0 +1,223 @@ +# LifecycleManager (`assignments::two::g2_2025_lifecycle_node`) + +## Overview +The `LifecycleManager` is the core lifecycle-aware node responsible for managing the IMU reader system's operational states and hardware communication. It orchestrates transitions between configuration, activation, and deactivation phases, abstracting the complexity of dual communication backends (serial and MQTT) into a unified interface. + +#### Implementation Details + +**Parameters** + +- **`device_path`** (string, default: "/dev/ttyUSB0"): Serial device path for hardware connection (e.g., USB serial adapter). +- **`baudrate`** (int, default: 115200): Serial communication baud rate in bits per second. +- **`comm_t`** (string, default: "serial"): Communication type selector—either "serial" or "mqtt" to determine which backend to use. + +**Constructor** +```cpp +LifecycleManager() +``` +- Initializes ROS2 lifecycle node with name `lifecycle_manager` +- Declares and reads configuration parameters: `device_path`, `baudrate`, and `comm_t` +- Creates a shared instance of `HardwareInterface` for managing all hardware operations +- Logs initialization status and readiness + +**Core Functions** + +**`CallbackReturn on_configure(const State&)`** +- Enters the *Unconfigured* → *Inactive* transition +- Checks the `comm_t` parameter to route initialization: + - **MQTT mode**: Calls `hw_interface->mqtt_configure()` to set up the MQTT client and broker connection + - **Serial mode**: Calls `hw_interface->open_device(device_path_, baudrate_)` to open and configure the serial port +- Returns `SUCCESS` if device initialization succeeds, `FAILURE` if serial/MQTT setup fails +- Logs configuration status and any errors +- Example test code (currently commented) demonstrates direct JSON parsing for validation + +**`CallbackReturn on_activate(const State&)`** +- Enters the *Inactive* → *Active* transition +- Checks the `comm_t` parameter to start the appropriate reader: + - **MQTT mode**: Calls `hw_interface->mqtt_reader()` to attach MQTT callbacks and begin receiving messages + - **Serial mode**: Calls `hw_interface->start_read()` to spawn a background thread that continuously reads from the serial device +- Returns `SUCCESS` after reader startup +- Logs activation status and selected communication type + +**`CallbackReturn on_deactivate(const State&)`** +- Enters the *Active* → *Inactive* transition +- Checks the `comm_t` parameter to cleanly stop operations: + - **MQTT mode**: Calls `hw_interface->close_mqtt_conn()` to disconnect from the broker and clean up resources + - **Serial mode**: + - Verifies device state with `hw_interface->is_device_open()` + - Calls `hw_interface->stop_read()` to signal the reader thread to exit and joins it + - Calls `hw_interface->close_device()` to release the serial port +- Returns `SUCCESS` after cleanup completes +- Logs deactivation and resource release + +**`CallbackReturn on_shutdown(const State&)`** +- Enters the *Inactive* → *Finalized* transition +- Performs final shutdown logging +- Returns `SUCCESS` + +**`CallbackReturn on_cleanup(const State&)`** +- Called during error recovery or explicit cleanup commands +- Performs resource cleanup and state logging +- Returns `SUCCESS` + + +## Communication Architecture + +### Dual Backend Support + +The `LifecycleManager` provides a flexible, pluggable communication architecture via the `comm_t` parameter: + +#### Serial Communication Path +1. **Configuration Phase** (`on_configure`): + - Opens the serial device at the path specified by `device_path` and baudrate + - Validates device readiness + +2. **Activation Phase** (`on_activate`): + - Spawns a background reader thread via `hw_interface->start_read()` + - Thread continuously polls the serial device with a timeout + - Reads are accumulated in a partial buffer, split on newline, and parsed as JSON + - Each valid JSON IMU payload is parsed into a `sensor_msgs::msg::Imu` and published to the ROS topic `imu/data` + +3. **Deactivation Phase** (`on_deactivate`): + - Signals the reader thread to stop via atomic flag + - Joins the thread to ensure clean termination + - Closes the serial device + +#### MQTT Communication Path +1. **Configuration Phase** (`on_configure`): + - Creates a persistent MQTT async client pointing to the broker at `SERVER_ADDRESS` (default: `tcp://localhost:1883`) + - Initializes MQTT callback infrastructure + +2. **Activation Phase** (`on_activate`): + - Attaches MQTT callbacks to the client + - Subscribes to the topic specified by `TOPIC` (default: `esp32/imu`) + - The async client runs background threads to receive messages + +3. **Deactivation Phase** (`on_deactivate`): + - Disconnects from the broker + - Cleans up MQTT client and callback resources + +--- + +## Lifecycle Commands + +To interact with the `LifecycleManager` from the command line, use the following ROS2 lifecycle service calls: + +```bash +# List current lifecycle state +ros2 lifecycle list /lifecycle_manager + +# Transition: UNCONFIGURED -> INACTIVE +ros2 lifecycle set /lifecycle_manager configure + +# Transition: INACTIVE -> ACTIVE +ros2 lifecycle set /lifecycle_manager activate + +# Transition: ACTIVE -> INACTIVE +ros2 lifecycle set /lifecycle_manager deactivate + +# Transition: INACTIVE -> FINALIZED +ros2 lifecycle set /lifecycle_manager shutdown +``` + +--- + +## Data Flow + +### Serial Data Flow +``` +Hardware Device + ↓ +Serial Port (/dev/ttyUSB0) + ↓ +Background Reader Thread (start_read) + ↓ +Partial Buffer Accumulation + ↓ +JSON Line Extraction & Sanitization + ↓ +parse_data() ← Deserializes JSON to sensor_msgs::msg::Imu + ↓ +imu_publisher → ROS2 Topic (`imu_data`) +``` + +### MQTT Data Flow +``` +MQTT Broker (localhost:1883) + ↓ +MQTT Async Client (Background Thread) + ↓ +Subscription to Topic (esp32/imu) + ↓ +MQTT Callback Handler + ↓ +parse_data() ← Deserializes JSON to sensor_msgs::msg::Imu + ↓ +imu_publisher → ROS2 Topic (`imu_data`) +``` + +--- + +## Error Handling + +- **Serial Device Failures**: If `open_device()` fails during configuration, `on_configure()` returns `FAILURE` and the system remains in the `UNCONFIGURED` state +- **Communication Errors**: JSON parse errors from invalid payloads are caught and logged without crashing the node; the reader continues listening for the next message +- **Thread Safety**: The reader thread uses an atomic flag (`reading_`) for clean stop signaling and ensures all resources are properly joined before returning from `on_deactivate()` + +--- + +## Design Patterns + +1. **Strategy Pattern**: The `comm_t` parameter enables runtime selection of communication backend without changing node code +2. **Lifecycle Pattern**: Follows ROS2 managed node pattern for predictable initialization, startup, and shutdown sequences +3. **Thread Safety**: Atomic flags and resource cleanup ensure the reader thread can be safely started and stopped +4. **Buffer Accumulation**: Partial message buffering handles fragmented serial reads and ensures complete JSON objects are parsed + +--- + +## Integration with HardwareInterface + +The `LifecycleManager` delegates all hardware operations to the `HardwareInterface` class: + +| Operation | Method | Lifecycle Phase | +|-----------|--------|-----------------| +| Open serial device | `open_device(path, baud)` | on_configure | +| Start reading | `start_read()` | on_activate | +| Stop reading | `stop_read()` | on_deactivate | +| Close serial device | `close_device()` | on_deactivate | +| Configure MQTT | `mqtt_configure()` | on_configure | +| Start MQTT reading | `mqtt_reader()` | on_activate | +| Close MQTT connection | `close_mqtt_conn()` | on_deactivate | +| Parse JSON payload | `parse_data(json_string)` | on_activate (continuous) | +| Publish IMU message | `publish_imu_data(imu_msg)` | on_activate (continuous) | + +--- + +## Usage Example + +```bash +# Launch the node with serial communication at /dev/ttyUSB0, 115200 baud +ros2 run g2_2025_imu_reader_pkg g2_2025_lifecycle_node \ + --ros-args \ + -p device_path:=/dev/ttyUSB0 \ + -p baudrate:=115200 \ + -p comm_t:=serial + +# Alternatively, launch with MQTT communication +ros2 run g2_2025_imu_reader_pkg g2_2025_lifecycle_node \ + --ros-args \ + -p comm_t:=mqtt + +# In another terminal, configure and activate the lifecycle +ros2 lifecycle set /lifecycle_manager configure +ros2 lifecycle set /lifecycle_manager activate + +# Subscribe to published IMU data +ros2 topic echo /imu_data + +# Deactivate and shutdown +ros2 lifecycle set /lifecycle_manager deactivate +ros2 lifecycle set /lifecycle_manager shutdown +``` + +--- diff --git a/doc/installation/installation.md b/doc/installation/installation.md index a6ad224..71c1864 100644 --- a/doc/installation/installation.md +++ b/doc/installation/installation.md @@ -41,10 +41,14 @@ You can configure specific database settings in the `docker-compose.yaml` in the ros2 launch g2_2025_imu_reader_pkg imu_reader.launch.xml ``` <<<<<<< HEAD +<<<<<<< HEAD To change parameters when using the launch file it will need to be edited in the `src/g2_2025_imu_reader_pkg/launch` folder. All parameters are already added to this document and thus only the values will need to be changed ======= To change parameters when using the launch file it will need to be edited in the `src/g2_2025_grade_calculator_pkg/launch` folder. All parameters are already added to this document and thus only the values will need to be changed +======= +To change parameters when using the launch file it will need to be edited in the `src/g2_2025_imu_reader_pkg/launch` folder. All parameters are already added to this document and thus only the values will need to be changed +>>>>>>> 484209c (feat(lifecycle & interface): added docs for arch and testing.) ### installation and setup for mqtt @@ -75,21 +79,26 @@ ros2 run g2_2025_imu_reader_pkg g2_2025_lifecycle_node --ros-args -p comm_t:='mq in other terminal: ```bash -mosquitto -p 1884 +mosquitto ``` and in other terminal to inialize the subsecriber: ```bash + +ros2 lifecycle list /lifecycle_manager ## for status checking ros2 lifecycle set /lifecycle_manager configure ros2 lifecycle set /lifecycle_manager activate +ros2 lifecycle set /lifecycle_manager deactivate +ros2 lifecycle set /lifecycle_manager shutdown + ``` an finally publish a mesg to the sub in other terminal: ```bash -mosquitto_pub -h localhost -p 1884 -t "esp32/imu" -m "nirvana" +mosquitto_pub -h localhost -p 1883 -t "esp32/imu" -m "test" ``` close conn via: ```bash diff --git a/doc/tests/LifecycleManager.md b/doc/tests/LifecycleManager.md new file mode 100644 index 0000000..02b7920 --- /dev/null +++ b/doc/tests/LifecycleManager.md @@ -0,0 +1,229 @@ +# LifecycleManager Unit Tests + +Unit tests for `LifecycleManager` are implemented in `src/g2_2025_imu_reader_pkg/test/LifecycleManager.test.cpp` using Google Test and ROS2 lifecycle test utilities. The tests are designed to validate all lifecycle state transitions, parameter handling, and hardware integration without requiring actual hardware or MQTT broker connectivity. + +## Test Cases + +### 1. ConstructorTest + +**Description:** Verifies that LifecycleManager can be instantiated without crashing and parameters are correctly declared. + +- **Test Action:** Create LifecycleManager instance with default parameters +- **Expected Result:** Instance created successfully; parameters `device_path`, `baudrate`, and `comm_t` are registered with their defaults + +--- + +### 2. ParameterDeclarationTest + +**Description:** Tests that all required parameters are declared during construction with correct default values. + +- **Test Action:** + - Create LifecycleManager + - Query parameters: `device_path`, `baudrate`, `comm_t` +- **Expected Result:** + - `device_path` defaults to `"/dev/ttyUSB0"` + - `baudrate` defaults to `115200` + - `comm_t` defaults to `"serial"` + +--- + +### 3. ParameterRuntimeReadTest + +**Description:** Verifies parameters can be read at runtime and affect behavior. + +- **Test Action:** + - Set parameters via ROS2 parameter API: `device_path="/dev/ttyACM0"`, `comm_t="mqtt"` + - Verify LifecycleManager reads the updated values +- **Expected Result:** Parameters are correctly read and stored in member variables + +--- + +### 4. InitialStateTest + +**Description:** Tests that the node starts in the `UNCONFIGURED` state. + +- **Test Action:** Create LifecycleManager and check lifecycle state +- **Expected Result:** Node is in `UNCONFIGURED` state + +--- + +### 5. ConfigureSerialModeTest + +**Description:** Tests the `on_configure` callback in serial communication mode. + +- **Test Action:** + - Set `comm_t` parameter to `"serial"` + - Call `on_configure()` with valid device path (e.g., `/dev/null` for testing) +- **Expected Result:** + - Transition succeeds + - Node moves to `INACTIVE` state + - `HardwareInterface::open_device()` is called with correct parameters + +--- + +### 6. ConfigureMQTTModeTest + +**Description:** Tests the `on_configure` callback in MQTT communication mode. + +- **Test Action:** + - Set `comm_t` parameter to `"mqtt"` + - Call `on_configure()` +- **Expected Result:** + - Transition succeeds + - Node moves to `INACTIVE` state + - `HardwareInterface::mqtt_configure()` is called + +--- + +### 7. ConfigureSerialFailureTest + +**Description:** Tests graceful failure when serial device cannot be opened. + +- **Test Action:** + - Set `device_path` to non-existent path (e.g., `/dev/invalid_device`) + - Set `comm_t` to `"serial"` + - Call `on_configure()` +- **Expected Result:** + - Transition handled gracefully (node doesn't crash) + - Error logging occurs + +--- + +### 8. DeactivateSerialModeTest + +**Description:** Tests the `on_deactivate` callback in serial communication mode. + +- **Test Action:** + - Configure to `INACTIVE` state in serial mode + - Call `on_deactivate()` +- **Expected Result:** + - Transition handled gracefully + - Resources are properly released + +--- + +### 9. ResourceCleanupTest + +**Description:** Tests that all resources are properly cleaned up on deactivation and shutdown. + +- **Test Action:** + - Configure node with serial settings + - Let node go out of scope + - Verify no crashes or resource leaks +- **Expected Result:** + - No memory leaks + - File descriptors are closed + - No segmentation faults on cleanup + +--- + +### 10. ErrorLoggingTest + +**Description:** Tests that error handling works during configuration attempts. + +- **Test Action:** + - Set invalid device path and attempt configuration + - Verify error is handled gracefully +- **Expected Result:** + - Node doesn't crash on error + - Error logging occurs + +--- + +### 11. HardwareInterfaceIntegrationTest + +**Description:** Tests the complete integration between LifecycleManager and HardwareInterface. + +- **Test Action:** + - Create LifecycleManager instance + - Verify HardwareInterface instance is created and accessible + - Verify we can call HardwareInterface methods +- **Expected Result:** + - HardwareInterface is properly initialized + - No segmentation faults when accessing interface methods + +--- + +### 12. DevicePathParameterTest + +**Description:** Tests that device_path parameter correctly controls serial device selection. + +- **Test Action:** + - Set `device_path` to `/dev/null` + - Call `on_configure()` in serial mode + - Verify correct device path is used +- **Expected Result:** + - Configuration succeeds with correct device path + +--- + +### 13. BaudRateParameterTest + +**Description:** Tests that baudrate parameter is correctly configured. + +- **Test Action:** + - Set `baudrate` to valid value (e.g., 115200) + - Call `on_configure()` in serial mode +- **Expected Result:** + - Correct baud rate is passed to the hardware interface + +--- + +### 14. ParameterUpdateBehaviorTest + +**Description:** Tests that parameter changes are respected across state transitions. + +- **Test Action:** + - Set `comm_t` to `"serial"`, configure + - Deactivate and transition back to UNCONFIGURED + - Change `comm_t` to `"mqtt"` + - Re-configure with new communication mode +- **Expected Result:** + - Communication mode switches work correctly + - Parameter changes are respected + +--- + +## Test Organization + +Tests are organized into logical groups: + +1. **Construction & Initialization** (Tests 1-3): Basic object creation and parameter setup +2. **State Transitions & Configuration** (Tests 4-7): Lifecycle callbacks and state validation +3. **Parameter Validation** (Tests 12-13): Parameter binding and influence on behavior +4. **Complete Sequences** (Test 14): Parameter switching across state transitions +5. **Resource & Thread Safety** (Tests 8-9): Resource cleanup and safe deactivation +6. **Error Handling & Integration** (Tests 10-11): Error resilience and component integration + +--- + +## Test Execution + +### Run All Tests + +```bash +# From workspace root +colcon test --packages-select g2_2025_imu_reader_pkg +``` + +### Run Specific Test Suite + +```bash +# Run only LifecycleManager tests +colcon test --packages-select g2_2025_imu_reader_pkg --ctest-args -R "LifecycleManager" +``` + +### Run with Verbose Output + +```bash +colcon test --packages-select g2_2025_imu_reader_pkg --ctest-args --verbose +``` + +### Run Tests Directly + +```bash +# From workspace root +./build/g2_2025_imu_reader_pkg/g2_2025_imu_reader_pkg_test_lifecycle_manager +``` + +--- diff --git a/src/g2_2025_imu_reader_pkg/CMakeLists.txt b/src/g2_2025_imu_reader_pkg/CMakeLists.txt index 45961a4..9ed23b7 100644 --- a/src/g2_2025_imu_reader_pkg/CMakeLists.txt +++ b/src/g2_2025_imu_reader_pkg/CMakeLists.txt @@ -24,7 +24,10 @@ find_package(std_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(nlohmann_json REQUIRED) +<<<<<<< HEAD find_package(rclcpp_lifecycle REQUIRED) +======= +>>>>>>> 98322ec (feat(lifecycle & hw): data parsing and publishing on both serial and mqtt.) add_executable(g2_2025_imu_database_writer_node src/g2_2025_imu_database_writer_node/Main.cpp @@ -59,11 +62,15 @@ ament_target_dependencies(g2_2025_lifecycle_node rclcpp rclcpp_lifecycle std_msg target_link_libraries(g2_2025_lifecycle_node paho-mqttpp3 paho-mqtt3a +<<<<<<< HEAD <<<<<<< HEAD nlohmann_json::nlohmann_json ) ======= +======= + nlohmann_json::nlohmann_json +>>>>>>> 98322ec (feat(lifecycle & hw): data parsing and publishing on both serial and mqtt.) ) @@ -96,6 +103,7 @@ if(BUILD_TESTING) tomlplusplus::tomlplusplus ) +<<<<<<< HEAD # Add gtest for IMUDatabaseWriter node ament_add_gtest(${PROJECT_NAME}_test_imu_database_writer test/IMUDatabaseWriter.test.cpp @@ -114,6 +122,30 @@ if(BUILD_TESTING) target_link_libraries(${PROJECT_NAME}_test_imu_database_writer pqxx pq tomlplusplus::tomlplusplus ) +======= + ament_add_gtest(${PROJECT_NAME}_test_lifecycle_manager + test/LifecycleManager.test.cpp + src/g2_2025_lifecycle_node/nodes/lifecycle_manager.cpp + src/g2_2025_lifecycle_node/nodes/hardware_interface.cpp + src/config/serialib.cpp + ) + target_include_directories(${PROJECT_NAME}_test_lifecycle_manager PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/src + ${CMAKE_CURRENT_SOURCE_DIR}/src/g2_2025_lifecycle_node + ) + ament_target_dependencies(${PROJECT_NAME}_test_lifecycle_manager + rclcpp + rclcpp_lifecycle + std_msgs + sensor_msgs + ) + target_link_libraries(${PROJECT_NAME}_test_lifecycle_manager + paho-mqttpp3 + paho-mqtt3a + nlohmann_json::nlohmann_json + ) + set_target_properties(${PROJECT_NAME}_test_lifecycle_manager PROPERTIES INSTALL_RPATH "/usr/local/lib") +>>>>>>> 484209c (feat(lifecycle & interface): added docs for arch and testing.) # Add gtest for DatabaseManager ament_add_gtest(${PROJECT_NAME}_test_database_manager diff --git a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.cpp b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.cpp index a9eb9b9..fbc0fe8 100644 --- a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.cpp +++ b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.cpp @@ -1,12 +1,56 @@ #include "hardware_interface.hpp" +#include namespace assignments::two::g2_2025_lifecycle_node { HardwareInterface::HardwareInterface() : Node("hardware_interface") { RCLCPP_INFO(this->get_logger(), "HardwareInterface node has been created"); - + imu_publisher = this->create_publisher("imu_data", 10); + RCLCPP_INFO(this->get_logger(), "IMU Publisher has been created"); } + +void HardwareInterface::publish_imu_data(const sensor_msgs::msg::Imu::SharedPtr msg) { + imu_publisher->publish(*msg); +} + +void HardwareInterface::parse_data(const std::string& data) { + // RCLCPP_INFO(this->get_logger(), "Received data: %s", data.c_str()); + + try { + auto json_data = nlohmann::json::parse(data); + + auto imu_msg = std::make_shared(); + + imu_msg->header.stamp = this->now(); + imu_msg->header.frame_id = "imu_link"; + + // angular velocity + if (json_data.contains("accel") && json_data["accel"].is_object()) { + imu_msg->linear_acceleration.x = json_data["accel"].value("x", 0.0); + imu_msg->linear_acceleration.y = json_data["accel"].value("y", 0.0); + imu_msg->linear_acceleration.z = json_data["accel"].value("z", 0.0); + } + + // linear acceleration + if (json_data.contains("gyro") && json_data["gyro"].is_object()) { + imu_msg->angular_velocity.x = json_data["gyro"].value("x", 0.0); + imu_msg->angular_velocity.y = json_data["gyro"].value("y", 0.0); + imu_msg->angular_velocity.z = json_data["gyro"].value("z", 0.0); + } + //print imu data + RCLCPP_INFO(this->get_logger(), "Publishing IMU Data - Accel: [%.3f, %.3f, %.3f], Gyro: [%.3f, %.3f, %.3f]", + imu_msg->linear_acceleration.x, imu_msg->linear_acceleration.y, imu_msg->linear_acceleration.z, + imu_msg->angular_velocity.x, imu_msg->angular_velocity.y, imu_msg->angular_velocity.z); + publish_imu_data(imu_msg); + + + } catch (const nlohmann::json::exception& e) { + RCLCPP_ERROR(this->get_logger(), "JSON parsing error: %s", e.what()); + } +} + + void HardwareInterface::start_read() { if (reading_.load()) { return; @@ -16,12 +60,13 @@ void HardwareInterface::start_read() { read_thread_ = std::thread([this]() { - char buffer[114]; + char buffer[116]; RCLCPP_INFO(this->get_logger(), "reader thread started"); while (reading_.load()) { - int ret = serial.readString(buffer, '\n', 114, 1000); + int ret = serial.readString(buffer, '\n', sizeof(buffer), 1000); if (ret > 0) { - RCLCPP_INFO(this->get_logger(), "Data read from hardware: %s", buffer); + RCLCPP_INFO(this->get_logger(), "Received buffer: %s", buffer); + parse_data(buffer); } else { // timeout of geen data. } @@ -70,15 +115,17 @@ void HardwareInterface::close_device() { class callback : public virtual mqtt::callback { public: void message_arrived(mqtt::const_message_ptr msg) override { - // Use a named logger since this class isn't a rclcpp::Node RCLCPP_INFO(rclcpp::get_logger("hardware_interface"), "Message received: %s", msg->get_payload_str().c_str()); + parent_.parse_data(msg->get_payload_str()); + } +private: +HardwareInterface parent_; }; void HardwareInterface::mqtt_configure() { - // Create persistent client and callback so they outlive this function. if (!mqtt_client) { mqtt_client = std::make_shared(SERVER_ADDRESS, CLIENT_ID); } diff --git a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.hpp b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.hpp index 26419ca..7ff8a39 100644 --- a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.hpp +++ b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.hpp @@ -52,6 +52,8 @@ public: void close_mqtt_conn(); void mqtt_configure(); void mqtt_reader(); + void parse_data(const std::string& data); + void publish_imu_data(const sensor_msgs::msg::Imu::SharedPtr msg); private: serialib serial; // MQTT defaults. Use static inline so they can be initialized in-class. @@ -64,6 +66,8 @@ private: std::thread read_thread_; std::atomic_bool reading_{false}; + // Publisher to emit parsed IMU messages (from serial or MQTT) + rclcpp::Publisher::SharedPtr imu_publisher; }; } // namespace assignments::two::g2_2025_lifecycle_node diff --git a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.cpp b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.cpp index 893ab3f..98703c2 100644 --- a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.cpp +++ b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.cpp @@ -8,9 +8,6 @@ LifecycleManager::LifecycleManager() : rclcpp_lifecycle::LifecycleNode("lifecycl RCLCPP_INFO(this->get_logger(), "LifecycleManager node is ready"); - imu_publisher_ = this->create_publisher("imu_data", 10); - RCLCPP_INFO(this->get_logger(), "IMU Publisher has been created"); - device_path_ = this->declare_parameter("device_path", "/dev/ttyUSB0"); baudrate_ = this->declare_parameter("baudrate", 115200); communication_type_ = this->declare_parameter("comm_t", "serial"); // placeholder default serial or param mqtt @@ -20,10 +17,6 @@ LifecycleManager::LifecycleManager() : rclcpp_lifecycle::LifecycleNode("lifecycl } -void LifecycleManager::publish_imu_data(const sensor_msgs::msg::Imu::SharedPtr msg) { - imu_publisher_->publish(*msg); - RCLCPP_INFO(this->get_logger(), "Published IMU data"); -} /* *in terminal commands to manage lifecycle: @@ -51,6 +44,11 @@ LifecycleManager::on_configure(const rclcpp_lifecycle::State&) { return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; } } + + //for testing parsing function. + // std::shared_ptr s; + // s = std::make_shared("{\"accel\":{\"x\": 0.037,\"y\": -1.164,\"z\": 9.775},\"gyro\":{\"x\": -0.024,\"y\": -0.014,\"z\": -0.001},\"Temp\": 41.01}"); + // hw_interface->parse_data(*s); RCLCPP_INFO(this->get_logger(), "Lifecycle configured successfully."); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } @@ -105,11 +103,4 @@ LifecycleManager::on_shutdown(const rclcpp_lifecycle::State&) { return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -LifecycleManager::on_cleanup(const rclcpp_lifecycle::State&) { - - RCLCPP_INFO(this->get_logger(), "cleaning up lifecycle..."); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; -} - } // namespace assignments::two::g2_2025_lifecycle_node diff --git a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.hpp b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.hpp index ac33900..72de89f 100644 --- a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.hpp +++ b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.hpp @@ -25,6 +25,11 @@ namespace assignments::two::g2_2025_lifecycle_node { class LifecycleManager : public rclcpp_lifecycle::LifecycleNode { public: LifecycleManager(); + + // Hardware interface to interact with the IMU device. + // Made public for testing purposes + std::shared_ptr hw_interface; + private: rclcpp::Publisher::SharedPtr imu_publisher_; @@ -32,15 +37,11 @@ private: int baudrate_; std::string communication_type_; - // Hardware interface to interact with the IMU device. - std::shared_ptr hw_interface; - void publish_imu_data(const sensor_msgs::msg::Imu::SharedPtr msg); rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(const rclcpp_lifecycle::State&); rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate(const rclcpp_lifecycle::State&); rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State&); rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown(const rclcpp_lifecycle::State&); - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State&); }; } // namespace assignments::two::g2_2025_lifecycle_node diff --git a/src/g2_2025_imu_reader_pkg/test/LifecycleManager.test.cpp b/src/g2_2025_imu_reader_pkg/test/LifecycleManager.test.cpp new file mode 100644 index 0000000..0867e07 --- /dev/null +++ b/src/g2_2025_imu_reader_pkg/test/LifecycleManager.test.cpp @@ -0,0 +1,234 @@ +#include +#include +#include +#include +#include "g2_2025_lifecycle_node/nodes/lifecycle_manager.hpp" + +using namespace assignments::two::g2_2025_lifecycle_node; + +class LifecycleManagerTest : public ::testing::Test { +protected: + void SetUp() override { + // Initialize ROS2 + rclcpp::init(0, nullptr); + } + + void TearDown() override { + // Shutdown ROS2 + rclcpp::shutdown(); + } +}; + +TEST_F(LifecycleManagerTest, ConstructorTest) { + // Test 1: Verifies that LifecycleManager can be instantiated without crashing + ASSERT_NO_THROW({ + auto node = std::make_shared(); + EXPECT_NE(node, nullptr); + }); +} + +TEST_F(LifecycleManagerTest, ParameterDeclarationTest) { + // Test 2: Verify all required parameters are declared with correct defaults + auto node = std::make_shared(); + + // Check that parameters exist + auto device_path = node->get_parameter("device_path").as_string(); + auto baudrate = node->get_parameter("baudrate").as_int(); + auto comm_t = node->get_parameter("comm_t").as_string(); + + EXPECT_EQ(device_path, "/dev/ttyUSB0"); + EXPECT_EQ(baudrate, 115200); + EXPECT_EQ(comm_t, "serial"); +} + +TEST_F(LifecycleManagerTest, ParameterRuntimeReadTest) { + // Test 3: Verify parameters can be read at runtime + auto node = std::make_shared(); + + // Set new parameter values + node->set_parameter(rclcpp::Parameter("device_path", "/dev/ttyACM0")); + node->set_parameter(rclcpp::Parameter("comm_t", "mqtt")); + + // Verify new values are read + auto device_path = node->get_parameter("device_path").as_string(); + auto comm_t = node->get_parameter("comm_t").as_string(); + + EXPECT_EQ(device_path, "/dev/ttyACM0"); + EXPECT_EQ(comm_t, "mqtt"); +} + + +TEST_F(LifecycleManagerTest, InitialStateTest) { + // Test 4: Verify node starts in UNCONFIGURED state + auto node = std::make_shared(); + + EXPECT_EQ(node->get_current_state().id(), lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); +} + +TEST_F(LifecycleManagerTest, ConfigureSerialModeTest) { + // Test 5: Test on_configure in serial communication mode + auto node = std::make_shared(); + + // Use /dev/null as a safe test device + node->set_parameter(rclcpp::Parameter("device_path", "/dev/null")); + node->set_parameter(rclcpp::Parameter("comm_t", "serial")); + + // Attempt to configure + node->configure(); + + // Check state after configure + auto state_id = node->get_current_state().id(); + // Should be INACTIVE (2) if successful or UNCONFIGURED (0) if failed + EXPECT_TRUE(state_id == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + state_id == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); +} + +TEST_F(LifecycleManagerTest, ConfigureMQTTModeTest) { + // Test 6: Test on_configure in MQTT communication mode + auto node = std::make_shared(); + + node->set_parameter(rclcpp::Parameter("comm_t", "mqtt")); + + // Attempt to configure (MQTT setup doesn't require actual broker) + node->configure(); + + //should be INACTIVE or UNCONFIGURED + auto state_id = node->get_current_state().id(); + EXPECT_TRUE(state_id == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + state_id == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); +} + +TEST_F(LifecycleManagerTest, ConfigureSerialFailureTest) { + auto node = std::make_shared(); + + // Use /dev/null which should open successfully for this test + node->set_parameter(rclcpp::Parameter("device_path", "/dev/null")); + node->set_parameter(rclcpp::Parameter("comm_t", "serial")); + + // Attempt to configure + node->configure(); + + SUCCEED(); +} + +TEST_F(LifecycleManagerTest, DeactivateSerialModeTest) { + // Test 8: Test on_deactivate in serial communication mode + auto node = std::make_shared(); + + node->set_parameter(rclcpp::Parameter("device_path", "/dev/null")); + node->set_parameter(rclcpp::Parameter("comm_t", "serial")); + + // Configure to INACTIVE state + node->configure(); + auto config_state = node->get_current_state().id(); + + if (config_state == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { + // Deactivate from INACTIVE + node->deactivate(); + auto final_state = node->get_current_state().id(); + // Should still be in INACTIVE or return to UNCONFIGURED + EXPECT_TRUE(final_state == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE || + final_state == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED); + } +} + + +TEST_F(LifecycleManagerTest, DevicePathParameterTest) { + // Test 24: Verify device_path parameter is correctly used + auto node = std::make_shared(); + + // Test multiple device paths + std::vector device_paths = {"/dev/ttyUSB0", "/dev/ttyACM0", "/dev/null"}; + + for (const auto& path : device_paths) { + node->set_parameter(rclcpp::Parameter("device_path", path)); + auto retrieved_path = node->get_parameter("device_path").as_string(); + EXPECT_EQ(retrieved_path, path); + } +} + +TEST_F(LifecycleManagerTest, BaudRateParameterTest) { + // Test 25: Verify baudrate parameter is correctly used + auto node = std::make_shared(); + + // Test multiple baud rates + std::vector baud_rates = {9600, 115200, 230400}; + + for (int baud : baud_rates) { + node->set_parameter(rclcpp::Parameter("baudrate", baud)); + auto retrieved_baud = node->get_parameter("baudrate").as_int(); + EXPECT_EQ(retrieved_baud, baud); + } +} + +TEST_F(LifecycleManagerTest, ParameterUpdateBehaviorTest) { + // Test 19: Test that parameter changes are respected across transitions + auto node = std::make_shared(); + + // Start with serial + node->set_parameter(rclcpp::Parameter("comm_t", "serial")); + node->set_parameter(rclcpp::Parameter("device_path", "/dev/null")); + + auto comm_type = node->get_parameter("comm_t").as_string(); + EXPECT_EQ(comm_type, "serial"); + + // Switch to MQTT + node->set_parameter(rclcpp::Parameter("comm_t", "mqtt")); + comm_type = node->get_parameter("comm_t").as_string(); + EXPECT_EQ(comm_type, "mqtt"); + + // Switch back to serial + node->set_parameter(rclcpp::Parameter("comm_t", "serial")); + comm_type = node->get_parameter("comm_t").as_string(); + EXPECT_EQ(comm_type, "serial"); +} + +TEST_F(LifecycleManagerTest, ResourceCleanupTest) { + // Test 9: Verify resources are properly cleaned up + { + auto node = std::make_shared(); + + node->set_parameter(rclcpp::Parameter("device_path", "/dev/null")); + node->set_parameter(rclcpp::Parameter("comm_t", "serial")); + + // Just configure, don't activate (avoid thread spawning) + node->configure(); + + // Node goes out of scope; verify no crashes + } + + // If we reach here without crashes, cleanup was successful + SUCCEED(); +} + +TEST_F(LifecycleManagerTest, ErrorLoggingTest) { + // Test 10: Verify error messages are logged during failures + auto node = std::make_shared(); + + // Set invalid device path to trigger error + node->set_parameter(rclcpp::Parameter("device_path", "/dev/nonexistent_device_xyz")); + node->set_parameter(rclcpp::Parameter("comm_t", "serial")); + + // This should attempt to configure + node->configure(); + + // The node may still transition state, but we verify it handles errors gracefully + // by not crashing + SUCCEED(); +} + +TEST_F(LifecycleManagerTest, HardwareInterfaceIntegrationTest) { + // Test 11: Verify complete integration with HardwareInterface + auto node = std::make_shared(); + + // Verify HardwareInterface instance is created + EXPECT_NE(node->hw_interface, nullptr); + + // Verify we can make method calls on the hardware interface + EXPECT_FALSE(node->hw_interface->is_device_open()); +} + +int main(int argc, char** argv) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From fc7b1c923dfbc22fe6043686b2b99642a7e539b6 Mon Sep 17 00:00:00 2001 From: Wessel Tip Date: Wed, 5 Nov 2025 07:55:12 +0100 Subject: [PATCH 07/17] rebase: Further merge conflicts --- doc/installation/installation.md | 20 +++--------- src/g2_2025_imu_reader_pkg/CMakeLists.txt | 39 ----------------------- 2 files changed, 4 insertions(+), 55 deletions(-) diff --git a/doc/installation/installation.md b/doc/installation/installation.md index 71c1864..9c498b9 100644 --- a/doc/installation/installation.md +++ b/doc/installation/installation.md @@ -40,16 +40,7 @@ You can configure specific database settings in the `docker-compose.yaml` in the ```bash ros2 launch g2_2025_imu_reader_pkg imu_reader.launch.xml ``` -<<<<<<< HEAD -<<<<<<< HEAD To change parameters when using the launch file it will need to be edited in the `src/g2_2025_imu_reader_pkg/launch` folder. All parameters are already added to this document and thus only the values will need to be changed -======= -To change parameters when using the launch file it will need to be edited in the `src/g2_2025_grade_calculator_pkg/launch` folder. All parameters are already added to this document and thus only the values will need to be changed - -======= -To change parameters when using the launch file it will need to be edited in the `src/g2_2025_imu_reader_pkg/launch` folder. All parameters are already added to this document and thus only the values will need to be changed ->>>>>>> 484209c (feat(lifecycle & interface): added docs for arch and testing.) - ### installation and setup for mqtt @@ -82,11 +73,8 @@ in other terminal: mosquitto ``` -and in other terminal to inialize the subsecriber: +and in other terminal to inialize the subscriber: ```bash - - -ros2 lifecycle list /lifecycle_manager ## for status checking ros2 lifecycle set /lifecycle_manager configure ros2 lifecycle set /lifecycle_manager activate ros2 lifecycle set /lifecycle_manager deactivate @@ -95,13 +83,13 @@ ros2 lifecycle set /lifecycle_manager shutdown ``` -an finally publish a mesg to the sub in other terminal: +And finally publish a message to the sub in other terminal: ```bash mosquitto_pub -h localhost -p 1883 -t "esp32/imu" -m "test" ``` -close conn via: + +close connection via: ```bash ros2 lifecycle set /lifecycle_manager deactivate ``` ->>>>>>> 8b04168 (feat(lifecycle & hw): added mqtt connection and installation setup) diff --git a/src/g2_2025_imu_reader_pkg/CMakeLists.txt b/src/g2_2025_imu_reader_pkg/CMakeLists.txt index 9ed23b7..c87f446 100644 --- a/src/g2_2025_imu_reader_pkg/CMakeLists.txt +++ b/src/g2_2025_imu_reader_pkg/CMakeLists.txt @@ -24,10 +24,6 @@ find_package(std_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(nlohmann_json REQUIRED) -<<<<<<< HEAD -find_package(rclcpp_lifecycle REQUIRED) -======= ->>>>>>> 98322ec (feat(lifecycle & hw): data parsing and publishing on both serial and mqtt.) add_executable(g2_2025_imu_database_writer_node src/g2_2025_imu_database_writer_node/Main.cpp @@ -62,20 +58,11 @@ ament_target_dependencies(g2_2025_lifecycle_node rclcpp rclcpp_lifecycle std_msg target_link_libraries(g2_2025_lifecycle_node paho-mqttpp3 paho-mqtt3a -<<<<<<< HEAD -<<<<<<< HEAD nlohmann_json::nlohmann_json ) -======= -======= - nlohmann_json::nlohmann_json ->>>>>>> 98322ec (feat(lifecycle & hw): data parsing and publishing on both serial and mqtt.) -) - ->>>>>>> 8b04168 (feat(lifecycle & hw): added mqtt connection and installation setup) install( TARGETS g2_2025_imu_database_writer_node @@ -103,7 +90,6 @@ if(BUILD_TESTING) tomlplusplus::tomlplusplus ) -<<<<<<< HEAD # Add gtest for IMUDatabaseWriter node ament_add_gtest(${PROJECT_NAME}_test_imu_database_writer test/IMUDatabaseWriter.test.cpp @@ -122,30 +108,6 @@ if(BUILD_TESTING) target_link_libraries(${PROJECT_NAME}_test_imu_database_writer pqxx pq tomlplusplus::tomlplusplus ) -======= - ament_add_gtest(${PROJECT_NAME}_test_lifecycle_manager - test/LifecycleManager.test.cpp - src/g2_2025_lifecycle_node/nodes/lifecycle_manager.cpp - src/g2_2025_lifecycle_node/nodes/hardware_interface.cpp - src/config/serialib.cpp - ) - target_include_directories(${PROJECT_NAME}_test_lifecycle_manager PRIVATE - ${CMAKE_CURRENT_SOURCE_DIR}/src - ${CMAKE_CURRENT_SOURCE_DIR}/src/g2_2025_lifecycle_node - ) - ament_target_dependencies(${PROJECT_NAME}_test_lifecycle_manager - rclcpp - rclcpp_lifecycle - std_msgs - sensor_msgs - ) - target_link_libraries(${PROJECT_NAME}_test_lifecycle_manager - paho-mqttpp3 - paho-mqtt3a - nlohmann_json::nlohmann_json - ) - set_target_properties(${PROJECT_NAME}_test_lifecycle_manager PROPERTIES INSTALL_RPATH "/usr/local/lib") ->>>>>>> 484209c (feat(lifecycle & interface): added docs for arch and testing.) # Add gtest for DatabaseManager ament_add_gtest(${PROJECT_NAME}_test_database_manager @@ -186,7 +148,6 @@ if(BUILD_TESTING) ) set_target_properties(${PROJECT_NAME}_test_lifecycle_manager PROPERTIES INSTALL_RPATH "/usr/local/lib") - # Add Python integration tests # find_package(ament_cmake_pytest REQUIRED) # ament_add_pytest_test(${PROJECT_NAME}_integration_test test/test_integration_system.py From 0fd0eb693f0227684490c0e04219d5826c30bc10 Mon Sep 17 00:00:00 2001 From: Mohammad Date: Wed, 5 Nov 2025 12:50:45 +0100 Subject: [PATCH 08/17] docs(architectrue): added lifecycle and hardware-interface arch. --- doc/architecture/architecture.md | 21 +++++++++++++++------ 1 file changed, 15 insertions(+), 6 deletions(-) diff --git a/doc/architecture/architecture.md b/doc/architecture/architecture.md index 94116e9..4c3432c 100644 --- a/doc/architecture/architecture.md +++ b/doc/architecture/architecture.md @@ -33,14 +33,23 @@ The system consists of multiple ROS2 nodes that communicate through standardized *For detailed documentation, see: [IMUDatabaseWriter.md](nodes/IMUDatabaseWriter.md)* -#### 2. LifeCycle Node - NEEDS TO BE EDITED STILL -**Namespace**: `assignments::one::grade_calculator` +#### 2. LifeCycle Node +**Namespace**: `assignments::two::lifecycle_manager` -**Brief Description**: Provides grade calculation service with business logic including bonus points and grade validation. +**Brief Description**: Configures low level communication and manages the hardware interface to read on either serial or mqtt comunications. -**Key Features**: Average calculation, special student rules, grade bounds validation (10-100) +**Key Features**: Configuration of communications, device read activation and deactivation. -*For detailed documentation, see: [GradeCalculator.md](nodes/GradeCalculator.md)* +*For detailed documentation, see: [LifecycleManager.md](nodes/LifecycleManager.md)* + +#### 3. Hardware Interface +**Namespace**: `assignments::two::hardware_interface` + +**Brief Description**: Manages low-level communication protocols (Serial/MQTT) for ESP32 data acquisition and forwards raw sensor data to processing nodes. + +**Key Features**: Multi-protocol communication support, connection management, raw data parsing and streaming. + +*For detailed documentation, see: [HardwareInterface.md](nodes/HardwareInterface.md)* ### Data Management @@ -65,7 +74,7 @@ The system consists of multiple ROS2 nodes that communicate through standardized ## System Workflow -### 1. Exam Result Processing +### 1. Imu data Processing 1. **Input**: IMU data is sent from the ESP32 to the lifecycle node 2. **Collection**: The lifcycle node recieves the data via a serial or MQTT connection From 998d471edad456c4622c9db22d1263652683ffb3 Mon Sep 17 00:00:00 2001 From: Vincent W Date: Thu, 6 Nov 2025 10:56:41 +0100 Subject: [PATCH 09/17] fix(cmake): Compile libpaho with the project --- doc/installation/installation.md | 41 ++--------------------- src/g2_2025_imu_reader_pkg/CMakeLists.txt | 38 ++++++++++++++------- 2 files changed, 28 insertions(+), 51 deletions(-) diff --git a/doc/installation/installation.md b/doc/installation/installation.md index 9c498b9..27bd663 100644 --- a/doc/installation/installation.md +++ b/doc/installation/installation.md @@ -42,54 +42,17 @@ ros2 launch g2_2025_imu_reader_pkg imu_reader.launch.xml ``` To change parameters when using the launch file it will need to be edited in the `src/g2_2025_imu_reader_pkg/launch` folder. All parameters are already added to this document and thus only the values will need to be changed -### installation and setup for mqtt +## For launching lifecycle mqtt node -```bash -sudo apt install mosquitto - -sudo apt-get install libpaho-mqtt-dev - - -git clone https://github.com/eclipse/paho.mqtt.cpp -cd paho.mqtt.cpp -git co v1.5.4 -git submodule init -git submodule update - -cmake -Bbuild -H. -DPAHO_WITH_MQTT_C=ON -DPAHO_BUILD_EXAMPLES=ON -sudo cmake --build build/ --target install -``` - -## for launching lifecycle mqtt node - -first: +First: ```bash ros2 run g2_2025_imu_reader_pkg g2_2025_lifecycle_node --ros-args -p comm_t:='mqtt' ``` -in other terminal: - -```bash -mosquitto -``` - and in other terminal to inialize the subscriber: ```bash ros2 lifecycle set /lifecycle_manager configure ros2 lifecycle set /lifecycle_manager activate ros2 lifecycle set /lifecycle_manager deactivate ros2 lifecycle set /lifecycle_manager shutdown - - -``` - -And finally publish a message to the sub in other terminal: - -```bash -mosquitto_pub -h localhost -p 1883 -t "esp32/imu" -m "test" -``` - -close connection via: -```bash -ros2 lifecycle set /lifecycle_manager deactivate ``` diff --git a/src/g2_2025_imu_reader_pkg/CMakeLists.txt b/src/g2_2025_imu_reader_pkg/CMakeLists.txt index c87f446..b5d9be7 100644 --- a/src/g2_2025_imu_reader_pkg/CMakeLists.txt +++ b/src/g2_2025_imu_reader_pkg/CMakeLists.txt @@ -8,13 +8,35 @@ endif() # external packages include(FetchContent) -fetchcontent_declare( +FetchContent_Declare( tomlplusplus GIT_REPOSITORY https://github.com/marzer/tomlplusplus.git GIT_TAG v3.4.0 ) -fetchcontent_makeavailable(tomlplusplus) +FetchContent_MakeAvailable(tomlplusplus) + +FetchContent_Declare( + paho_mqtt_cpp + GIT_REPOSITORY https://github.com/eclipse/paho.mqtt.cpp.git + GIT_TAG v1.5.2 +) + +set(PAHO_WITH_MQTT_C ON CACHE BOOL "Build with Paho MQTT C library" FORCE) +set(PAHO_MQTT_CPP_BUILD_STATIC ON CACHE BOOL "Build static library" FORCE) +set(PAHO_MQTT_CPP_BUILD_SHARED OFF CACHE BOOL "Build shared library" FORCE) +set(PAHO_WITH_SSL OFF CACHE BOOL "Build with SSL support" FORCE) +set(PAHO_BUILD_EXAMPLES OFF CACHE BOOL "Build example programs" FORCE) + +FetchContent_MakeAvailable(paho_mqtt_cpp) + +FetchContent_Declare( + nlohmann_json + GIT_REPOSITORY https://github.com/nlohmann/json.git + GIT_TAG v3.12.0 +) + +FetchContent_MakeAvailable(nlohmann_json) # find dependencies find_package(ament_cmake REQUIRED) @@ -23,7 +45,6 @@ find_package(rclcpp_action REQUIRED) find_package(std_msgs REQUIRED) find_package(sensor_msgs REQUIRED) find_package(rclcpp_lifecycle REQUIRED) -find_package(nlohmann_json REQUIRED) add_executable(g2_2025_imu_database_writer_node src/g2_2025_imu_database_writer_node/Main.cpp @@ -38,9 +59,6 @@ target_include_directories(g2_2025_imu_database_writer_node PRIVATE ) ament_target_dependencies(g2_2025_imu_database_writer_node rclcpp sensor_msgs) target_link_libraries(g2_2025_imu_database_writer_node pqxx pq tomlplusplus::tomlplusplus) -ament_target_dependencies(g2_2025_imu_database_writer_node rclcpp sensor_msgs) -target_link_libraries(g2_2025_imu_database_writer_node pqxx pq tomlplusplus::tomlplusplus) - add_executable(g2_2025_lifecycle_node src/g2_2025_lifecycle_node/main.cpp @@ -56,13 +74,10 @@ target_include_directories(g2_2025_lifecycle_node PRIVATE ament_target_dependencies(g2_2025_lifecycle_node rclcpp rclcpp_lifecycle std_msgs sensor_msgs) target_link_libraries(g2_2025_lifecycle_node - paho-mqttpp3 - paho-mqtt3a + paho-mqttpp3-static nlohmann_json::nlohmann_json ) - - install( TARGETS g2_2025_imu_database_writer_node @@ -142,8 +157,7 @@ if(BUILD_TESTING) sensor_msgs ) target_link_libraries(${PROJECT_NAME}_test_lifecycle_manager - paho-mqttpp3 - paho-mqtt3a + paho-mqttpp3-static nlohmann_json::nlohmann_json ) set_target_properties(${PROJECT_NAME}_test_lifecycle_manager PROPERTIES INSTALL_RPATH "/usr/local/lib") From f33108e61f0afe7406489a5b0fcf5ae8b40b685d Mon Sep 17 00:00:00 2001 From: Vincent Winter Date: Thu, 6 Nov 2025 13:06:46 +0100 Subject: [PATCH 10/17] fix(layout): Move IMU code to src dir --- {IMU => src/IMU}/.clangd | 0 {IMU => src/IMU}/.devcontainer/Dockerfile | 0 {IMU => src/IMU}/.devcontainer/devcontainer.json | 0 {IMU => src/IMU}/CMakeLists.txt | 0 src/IMU/COLCON_IGNORE | 0 {IMU => src/IMU}/img/wiringdiagram.png | Bin {IMU => src/IMU}/main/CMakeLists.txt | 0 {IMU => src/IMU}/main/Kconfig.projbuild | 0 {IMU => src/IMU}/main/mpu6886.c | 0 {IMU => src/IMU}/main/mpu6886.h | 0 {IMU => src/IMU}/sdkconfig | 0 {IMU => src/IMU}/sdkconfig.old | 0 12 files changed, 0 insertions(+), 0 deletions(-) rename {IMU => src/IMU}/.clangd (100%) rename {IMU => src/IMU}/.devcontainer/Dockerfile (100%) rename {IMU => src/IMU}/.devcontainer/devcontainer.json (100%) rename {IMU => src/IMU}/CMakeLists.txt (100%) create mode 100644 src/IMU/COLCON_IGNORE rename {IMU => src/IMU}/img/wiringdiagram.png (100%) rename {IMU => src/IMU}/main/CMakeLists.txt (100%) rename {IMU => src/IMU}/main/Kconfig.projbuild (100%) rename {IMU => src/IMU}/main/mpu6886.c (100%) rename {IMU => src/IMU}/main/mpu6886.h (100%) rename {IMU => src/IMU}/sdkconfig (100%) rename {IMU => src/IMU}/sdkconfig.old (100%) diff --git a/IMU/.clangd b/src/IMU/.clangd similarity index 100% rename from IMU/.clangd rename to src/IMU/.clangd diff --git a/IMU/.devcontainer/Dockerfile b/src/IMU/.devcontainer/Dockerfile similarity index 100% rename from IMU/.devcontainer/Dockerfile rename to src/IMU/.devcontainer/Dockerfile diff --git a/IMU/.devcontainer/devcontainer.json b/src/IMU/.devcontainer/devcontainer.json similarity index 100% rename from IMU/.devcontainer/devcontainer.json rename to src/IMU/.devcontainer/devcontainer.json diff --git a/IMU/CMakeLists.txt b/src/IMU/CMakeLists.txt similarity index 100% rename from IMU/CMakeLists.txt rename to src/IMU/CMakeLists.txt diff --git a/src/IMU/COLCON_IGNORE b/src/IMU/COLCON_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/IMU/img/wiringdiagram.png b/src/IMU/img/wiringdiagram.png similarity index 100% rename from IMU/img/wiringdiagram.png rename to src/IMU/img/wiringdiagram.png diff --git a/IMU/main/CMakeLists.txt b/src/IMU/main/CMakeLists.txt similarity index 100% rename from IMU/main/CMakeLists.txt rename to src/IMU/main/CMakeLists.txt diff --git a/IMU/main/Kconfig.projbuild b/src/IMU/main/Kconfig.projbuild similarity index 100% rename from IMU/main/Kconfig.projbuild rename to src/IMU/main/Kconfig.projbuild diff --git a/IMU/main/mpu6886.c b/src/IMU/main/mpu6886.c similarity index 100% rename from IMU/main/mpu6886.c rename to src/IMU/main/mpu6886.c diff --git a/IMU/main/mpu6886.h b/src/IMU/main/mpu6886.h similarity index 100% rename from IMU/main/mpu6886.h rename to src/IMU/main/mpu6886.h diff --git a/IMU/sdkconfig b/src/IMU/sdkconfig similarity index 100% rename from IMU/sdkconfig rename to src/IMU/sdkconfig diff --git a/IMU/sdkconfig.old b/src/IMU/sdkconfig.old similarity index 100% rename from IMU/sdkconfig.old rename to src/IMU/sdkconfig.old From a1e480c8b61b007c10d97bd8d6e854bda0005b4c Mon Sep 17 00:00:00 2001 From: Vincent Winter Date: Thu, 6 Nov 2025 13:07:27 +0100 Subject: [PATCH 11/17] fix(layout): Move seriallib to lib dir --- src/g2_2025_imu_reader_pkg/{src/config => lib}/serialib.cpp | 0 src/g2_2025_imu_reader_pkg/{src/config => lib}/serialib.h | 0 2 files changed, 0 insertions(+), 0 deletions(-) rename src/g2_2025_imu_reader_pkg/{src/config => lib}/serialib.cpp (100%) rename src/g2_2025_imu_reader_pkg/{src/config => lib}/serialib.h (100%) diff --git a/src/g2_2025_imu_reader_pkg/src/config/serialib.cpp b/src/g2_2025_imu_reader_pkg/lib/serialib.cpp similarity index 100% rename from src/g2_2025_imu_reader_pkg/src/config/serialib.cpp rename to src/g2_2025_imu_reader_pkg/lib/serialib.cpp diff --git a/src/g2_2025_imu_reader_pkg/src/config/serialib.h b/src/g2_2025_imu_reader_pkg/lib/serialib.h similarity index 100% rename from src/g2_2025_imu_reader_pkg/src/config/serialib.h rename to src/g2_2025_imu_reader_pkg/lib/serialib.h From 808880738c99df851543b516368ec0fd3cef18e1 Mon Sep 17 00:00:00 2001 From: Vincent Winter Date: Thu, 6 Nov 2025 14:35:16 +0100 Subject: [PATCH 12/17] docs: Update lifecycle node documentation --- .gitignore | 1 + doc/architecture/nodes/HardwareInterface.md | 80 +-------------------- doc/architecture/nodes/LifecycleManager.md | 27 ++----- doc/installation/installation.md | 15 ++-- 4 files changed, 15 insertions(+), 108 deletions(-) diff --git a/.gitignore b/.gitignore index 216cdd7..83d12e5 100644 --- a/.gitignore +++ b/.gitignore @@ -31,6 +31,7 @@ qtcreator-* # Colcon custom files COLCON_IGNORE +!src/*/COLCON_IGNORE AMENT_IGNORE .vscode diff --git a/doc/architecture/nodes/HardwareInterface.md b/doc/architecture/nodes/HardwareInterface.md index cbdf127..db4ffe7 100644 --- a/doc/architecture/nodes/HardwareInterface.md +++ b/doc/architecture/nodes/HardwareInterface.md @@ -1,7 +1,7 @@ # HardwareInterface (`assignments::two::g2_2025_lifecycle_node`) ## Overview -The `HardwareInterface` is a ROS2 node responsible for managing low-level hardware communication with IMU sensors. It abstracts the complexity of dual communication backends (serial and MQTT), provides JSON parsing of sensor data, and publishes standardized `sensor_msgs::msg::Imu` messages to the ROS2 ecosystem. The node handles continuous data acquisition, buffering of fragmented reads, and robust error recovery. +The `HardwareInterface` is a c++ class responsible for managing low-level hardware communication with ESP32-IMU combination via serial/MQTT and provides JSON parsing of sensor data, and publishes standardized `sensor_msgs::msg::Imu` messages to the database writer. #### Implementation Details @@ -46,8 +46,6 @@ HardwareInterface() - **`CLIENT_ID`**: `"cpp_mqtt_client"` — MQTT client identifier - **`TOPIC`**: `"esp32/imu"` — Default subscription topic for IMU data ---- - ## Core Functions ### `void start_read()` @@ -71,8 +69,6 @@ Initiates continuous serial data acquisition in a background thread. **Error Handling:** - Invalid JSON lines are logged as errors in `parse_data()` but do not crash the thread ---- - ### `void stop_read()` Cleanly terminates the background reader thread. @@ -87,8 +83,6 @@ Cleanly terminates the background reader thread. - Uses atomic flag for lock-free signaling - Blocks until thread joins, guaranteeing clean shutdown ---- - ### `void parse_data(const std::string& data)` Deserializes a JSON string into a ROS2 IMU message and publishes it. @@ -115,8 +109,6 @@ Deserializes a JSON string into a ROS2 IMU message and publishes it. - Catches `nlohmann::json::exception` and logs parsing errors without crashing - Handles missing fields gracefully using `.value()` with default 0.0 ---- - ### `void publish_imu_data(const sensor_msgs::msg::Imu::SharedPtr msg)` Publishes an IMU message to the ROS2 topic. @@ -125,8 +117,6 @@ Publishes an IMU message to the ROS2 topic. - Dereferences the shared pointer and publishes to `imu_publisher` - Operation is thread-safe (rclcpp publishers support multi-threaded access) ---- - ### `void mqtt_configure()` Sets up the MQTT infrastructure for broker communication. @@ -140,8 +130,6 @@ Sets up the MQTT infrastructure for broker communication. - Client and callback objects must outlive this function to maintain the connection - Using `shared_ptr` ensures proper lifetime management ---- - ### `void mqtt_reader()` Attaches callbacks to the MQTT client to begin receiving messages. @@ -151,8 +139,6 @@ Attaches callbacks to the MQTT client to begin receiving messages. - Logs that the listener has started - Returns; the async client handles message reception in background threads ---- - ### `void mqtt_connect()` Establishes connection to the MQTT broker and subscribes to the sensor topic. @@ -168,8 +154,6 @@ Establishes connection to the MQTT broker and subscribes to the sensor topic. **Error Handling:** - Catches `mqtt::exception` and logs errors; does not throw or crash ---- - ### `void close_mqtt_conn()` Cleanly disconnects from the MQTT broker and cleans up resources. @@ -184,8 +168,6 @@ Cleanly disconnects from the MQTT broker and cleans up resources. - Catches `mqtt::exception` and logs errors - Continues cleanup even if errors occur ---- - ### `bool open_device(const std::string& device_path, int baud_rate)` Opens and configures a serial port device. @@ -203,8 +185,6 @@ Opens and configures a serial port device. - Checks if the returned value is 1 (success) - Logs success or error status ---- - ### `bool is_device_open()` Queries the current state of the serial device. @@ -213,8 +193,6 @@ Queries the current state of the serial device. - `true` if the device is open - `false` otherwise ---- - ### `void close_device()` Closes the serial port and releases resources. @@ -223,8 +201,6 @@ Closes the serial port and releases resources. - Calls `serial.closeDevice()` - Ensures the device is no longer accessible for reads/writes ---- - ### `void write(const std::string& data)` Writes data to the serial device. @@ -233,8 +209,6 @@ Writes data to the serial device. - Logs the write operation - Calls `serial.writeString()` with the data ---- - ## MQTT Callback Handler ### `class callback : public virtual mqtt::callback` @@ -246,7 +220,6 @@ A nested class that implements the Paho MQTT callback interface. - Invoked when a message arrives on a subscribed topic - Extracts the payload string via `msg->get_payload_str()` - Calls `parse_data()` to deserialize and publish the IMU message ---- ## Data Flow Architecture @@ -294,8 +267,6 @@ JSON Parse → sensor_msgs::msg::Imu publish_imu_data() → ROS Topic `imu_data` ``` ---- - ## Buffer Management & Message Reconstruction The `partial_buffer_` member implements a robust strategy for handling fragmented serial reads: @@ -312,8 +283,6 @@ The `partial_buffer_` member implements a robust strategy for handling fragmente - Multiple messages can arrive in a single read - Buffering ensures robust handling of all edge cases ---- - ## Error Handling & Recovery | Scenario | Behavior | Recovery | @@ -325,8 +294,6 @@ The `partial_buffer_` member implements a robust strategy for handling fragmente | MQTT broker unreachable | Exception caught and logged | Retry via `mqtt_connect()` | | MQTT message error | Exception caught and logged | Connection remains for next message | ---- - ## Thread Safety - **Atomic Flag**: `reading_` uses `std::atomic_bool` for lock-free thread signaling @@ -390,8 +357,6 @@ ros2 lifecycle set /lifecycle_manager deactivate ros2 lifecycle set /lifecycle_manager shutdown ``` ---- - ## Design Patterns 1. **Abstraction Pattern**: Encapsulates serial and MQTT complexity behind a unified interface @@ -400,9 +365,6 @@ ros2 lifecycle set /lifecycle_manager shutdown 4. **Dual Backend Strategy**: Runtime selection of communication mode (serial or MQTT) 5. **JSON Deserialization**: Uses industry-standard `nlohmann::json` for robust parsing ---- - - ## Dependencies - **rclcpp**: ROS2 C++ client library @@ -410,43 +372,3 @@ ros2 lifecycle set /lifecycle_manager shutdown - **paho-mqtt**: Paho C/C++ MQTT client library - **nlohmann/json**: Header-only JSON parsing library - **serialib**: Custom serial communication wrapper - ---- - -## Class Diagram - -``` -┌─────────────────────────────────────┐ -│ HardwareInterface │ -│ (rclcpp::Node) │ -├─────────────────────────────────────┤ -│ Private Members: │ -│ - serialib serial │ -│ - async_client mqtt_client │ -│ - callback mqtt_cb │ -│ - thread read_thread_ │ -│ - atomic_bool reading_ │ -│ - string partial_buffer_ │ -│ - Publisher imu_publisher │ -├─────────────────────────────────────┤ -│ Public Methods: │ -│ + start_read() │ -│ + stop_read() │ -│ + open_device() │ -│ + close_device() │ -│ + is_device_open() │ -│ + write() │ -│ + mqtt_configure() │ -│ + mqtt_reader() │ -│ + mqtt_connect() │ -│ + close_mqtt_conn() │ -│ + parse_data() │ -│ + publish_imu_data() │ -└─────────────────────────────────────┘ - ↑ - │ orchestrated by - │ - ┌──────────────────┐ - │ LifecycleManager │ - └──────────────────┘ -``` diff --git a/doc/architecture/nodes/LifecycleManager.md b/doc/architecture/nodes/LifecycleManager.md index 82f9f6a..475dd17 100644 --- a/doc/architecture/nodes/LifecycleManager.md +++ b/doc/architecture/nodes/LifecycleManager.md @@ -1,7 +1,7 @@ # LifecycleManager (`assignments::two::g2_2025_lifecycle_node`) ## Overview -The `LifecycleManager` is the core lifecycle-aware node responsible for managing the IMU reader system's operational states and hardware communication. It orchestrates transitions between configuration, activation, and deactivation phases, abstracting the complexity of dual communication backends (serial and MQTT) into a unified interface. +The `LifecycleManager` is the lifecycle node responsible for receiving the data of the ESP32-IMU combination via serial/MQTT and publishing that data to the database management node. For the functionality outside of the lifecycle see [HardwareInterface.md](HardwareInterface.md]) #### Implementation Details @@ -9,7 +9,7 @@ The `LifecycleManager` is the core lifecycle-aware node responsible for managing - **`device_path`** (string, default: "/dev/ttyUSB0"): Serial device path for hardware connection (e.g., USB serial adapter). - **`baudrate`** (int, default: 115200): Serial communication baud rate in bits per second. -- **`comm_t`** (string, default: "serial"): Communication type selector—either "serial" or "mqtt" to determine which backend to use. +- **`comm_t`** (string, default: "serial"): Communication type selector—either "serial" or "mqtt". **Constructor** ```cpp @@ -63,18 +63,15 @@ LifecycleManager() ## Communication Architecture -### Dual Backend Support +The `LifecycleManager` switches communication type depending on the `comm_t` parameter: -The `LifecycleManager` provides a flexible, pluggable communication architecture via the `comm_t` parameter: - -#### Serial Communication Path +#### Serial Communication 1. **Configuration Phase** (`on_configure`): - Opens the serial device at the path specified by `device_path` and baudrate - - Validates device readiness 2. **Activation Phase** (`on_activate`): - Spawns a background reader thread via `hw_interface->start_read()` - - Thread continuously polls the serial device with a timeout + - Continuously polls the serial device with a timeout - Reads are accumulated in a partial buffer, split on newline, and parsed as JSON - Each valid JSON IMU payload is parsed into a `sensor_msgs::msg::Imu` and published to the ROS topic `imu/data` @@ -83,7 +80,7 @@ The `LifecycleManager` provides a flexible, pluggable communication architecture - Joins the thread to ensure clean termination - Closes the serial device -#### MQTT Communication Path +#### MQTT Communication 1. **Configuration Phase** (`on_configure`): - Creates a persistent MQTT async client pointing to the broker at `SERVER_ADDRESS` (default: `tcp://localhost:1883`) - Initializes MQTT callback infrastructure @@ -97,8 +94,6 @@ The `LifecycleManager` provides a flexible, pluggable communication architecture - Disconnects from the broker - Cleans up MQTT client and callback resources ---- - ## Lifecycle Commands To interact with the `LifecycleManager` from the command line, use the following ROS2 lifecycle service calls: @@ -120,8 +115,6 @@ ros2 lifecycle set /lifecycle_manager deactivate ros2 lifecycle set /lifecycle_manager shutdown ``` ---- - ## Data Flow ### Serial Data Flow @@ -156,16 +149,12 @@ parse_data() ← Deserializes JSON to sensor_msgs::msg::Imu imu_publisher → ROS2 Topic (`imu_data`) ``` ---- - ## Error Handling - **Serial Device Failures**: If `open_device()` fails during configuration, `on_configure()` returns `FAILURE` and the system remains in the `UNCONFIGURED` state - **Communication Errors**: JSON parse errors from invalid payloads are caught and logged without crashing the node; the reader continues listening for the next message - **Thread Safety**: The reader thread uses an atomic flag (`reading_`) for clean stop signaling and ensures all resources are properly joined before returning from `on_deactivate()` ---- - ## Design Patterns 1. **Strategy Pattern**: The `comm_t` parameter enables runtime selection of communication backend without changing node code @@ -173,8 +162,6 @@ imu_publisher → ROS2 Topic (`imu_data`) 3. **Thread Safety**: Atomic flags and resource cleanup ensure the reader thread can be safely started and stopped 4. **Buffer Accumulation**: Partial message buffering handles fragmented serial reads and ensures complete JSON objects are parsed ---- - ## Integration with HardwareInterface The `LifecycleManager` delegates all hardware operations to the `HardwareInterface` class: @@ -191,8 +178,6 @@ The `LifecycleManager` delegates all hardware operations to the `HardwareInterfa | Parse JSON payload | `parse_data(json_string)` | on_activate (continuous) | | Publish IMU message | `publish_imu_data(imu_msg)` | on_activate (continuous) | ---- - ## Usage Example ```bash diff --git a/doc/installation/installation.md b/doc/installation/installation.md index 27bd663..bb4212e 100644 --- a/doc/installation/installation.md +++ b/doc/installation/installation.md @@ -38,18 +38,17 @@ You can configure specific database settings in the `docker-compose.yaml` in the ### Start the IMU Reader program ```bash -ros2 launch g2_2025_imu_reader_pkg imu_reader.launch.xml +For Serial: +ros2 launch g2_2025_imu_reader_pkg serial.launch.xml + +For MQTT: +ros2 launch g2_2025_imu_reader_pkg mqtt.launch.xml ``` To change parameters when using the launch file it will need to be edited in the `src/g2_2025_imu_reader_pkg/launch` folder. All parameters are already added to this document and thus only the values will need to be changed -## For launching lifecycle mqtt node +### Use the Lifecycle Node -First: -```bash -ros2 run g2_2025_imu_reader_pkg g2_2025_lifecycle_node --ros-args -p comm_t:='mqtt' -``` - -and in other terminal to inialize the subscriber: +To setup the lifecycle node the following commands can be used. They must be used in this order. ```bash ros2 lifecycle set /lifecycle_manager configure ros2 lifecycle set /lifecycle_manager activate From 7d135c642510ef7736b8aaa68f74845984f9c56d Mon Sep 17 00:00:00 2001 From: Vincent Winter Date: Thu, 6 Nov 2025 14:35:27 +0100 Subject: [PATCH 13/17] feat: Add launch files --- src/g2_2025_imu_reader_pkg/launch/mqtt.launch.xml | 6 ++++++ src/g2_2025_imu_reader_pkg/launch/serial.launch.xml | 8 ++++++++ 2 files changed, 14 insertions(+) create mode 100644 src/g2_2025_imu_reader_pkg/launch/mqtt.launch.xml create mode 100644 src/g2_2025_imu_reader_pkg/launch/serial.launch.xml diff --git a/src/g2_2025_imu_reader_pkg/launch/mqtt.launch.xml b/src/g2_2025_imu_reader_pkg/launch/mqtt.launch.xml new file mode 100644 index 0000000..9ad1ff9 --- /dev/null +++ b/src/g2_2025_imu_reader_pkg/launch/mqtt.launch.xml @@ -0,0 +1,6 @@ + + + + + + diff --git a/src/g2_2025_imu_reader_pkg/launch/serial.launch.xml b/src/g2_2025_imu_reader_pkg/launch/serial.launch.xml new file mode 100644 index 0000000..4bb5248 --- /dev/null +++ b/src/g2_2025_imu_reader_pkg/launch/serial.launch.xml @@ -0,0 +1,8 @@ + + + + + + + + From fd016f05b6ab91e46aa3a1aaecfab86addad816f Mon Sep 17 00:00:00 2001 From: Vincent Winter Date: Thu, 6 Nov 2025 14:36:19 +0100 Subject: [PATCH 14/17] fix: Rewrite Lifecycle node --- src/g2_2025_imu_reader_pkg/CMakeLists.txt | 36 +++-- .../{main.cpp => Main.cpp} | 4 +- ...re_interface.cpp => HardwareInterface.cpp} | 137 +++++++++--------- ...re_interface.hpp => HardwareInterface.hpp} | 79 +++++----- ...cycle_manager.cpp => LifecycleManager.cpp} | 54 +++---- ...cycle_manager.hpp => LifecycleManager.hpp} | 30 ++-- .../nodes/MQTTParameters.hpp | 13 ++ .../test/LifecycleManager.test.cpp | 5 +- 8 files changed, 194 insertions(+), 164 deletions(-) rename src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/{main.cpp => Main.cpp} (84%) rename src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/{hardware_interface.cpp => HardwareInterface.cpp} (50%) rename src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/{hardware_interface.hpp => HardwareInterface.hpp} (51%) rename src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/{lifecycle_manager.cpp => LifecycleManager.cpp} (71%) rename src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/{lifecycle_manager.hpp => LifecycleManager.hpp} (70%) create mode 100644 src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/MQTTParameters.hpp diff --git a/src/g2_2025_imu_reader_pkg/CMakeLists.txt b/src/g2_2025_imu_reader_pkg/CMakeLists.txt index b5d9be7..4c2f0f9 100644 --- a/src/g2_2025_imu_reader_pkg/CMakeLists.txt +++ b/src/g2_2025_imu_reader_pkg/CMakeLists.txt @@ -28,8 +28,18 @@ set(PAHO_MQTT_CPP_BUILD_SHARED OFF CACHE BOOL "Build shared library" FORCE) set(PAHO_WITH_SSL OFF CACHE BOOL "Build with SSL support" FORCE) set(PAHO_BUILD_EXAMPLES OFF CACHE BOOL "Build example programs" FORCE) +# Suppress warnings from FetchContent dependencies +set(CMAKE_CXX_FLAGS_BACKUP "${CMAKE_CXX_FLAGS}") +set(CMAKE_C_FLAGS_BACKUP "${CMAKE_C_FLAGS}") +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w") +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -w") + FetchContent_MakeAvailable(paho_mqtt_cpp) +# Restore original flags +set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS_BACKUP}") +set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS_BACKUP}") + FetchContent_Declare( nlohmann_json GIT_REPOSITORY https://github.com/nlohmann/json.git @@ -61,20 +71,22 @@ ament_target_dependencies(g2_2025_imu_database_writer_node rclcpp sensor_msgs) target_link_libraries(g2_2025_imu_database_writer_node pqxx pq tomlplusplus::tomlplusplus) add_executable(g2_2025_lifecycle_node - src/g2_2025_lifecycle_node/main.cpp - src/g2_2025_lifecycle_node/nodes/hardware_interface.cpp - src/g2_2025_lifecycle_node/nodes/lifecycle_manager.cpp - src/config/serialib.cpp + src/g2_2025_lifecycle_node/Main.cpp + src/g2_2025_lifecycle_node/nodes/HardwareInterface.cpp + src/g2_2025_lifecycle_node/nodes/LifecycleManager.cpp + lib/serialib.cpp ) target_include_directories(g2_2025_lifecycle_node PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/src + ${CMAKE_CURRENT_SOURCE_DIR}/lib ${CMAKE_CURRENT_SOURCE_DIR}/src/g2_2025_lifecycle_node ) ament_target_dependencies(g2_2025_lifecycle_node rclcpp rclcpp_lifecycle std_msgs sensor_msgs) target_link_libraries(g2_2025_lifecycle_node - paho-mqttpp3-static + paho-mqttpp3 + paho-mqtt3as nlohmann_json::nlohmann_json ) @@ -85,6 +97,10 @@ install( DESTINATION lib/${PROJECT_NAME} ) +install( + DIRECTORY launch + DESTINATION share/${PROJECT_NAME}/ +) set_target_properties(g2_2025_lifecycle_node PROPERTIES INSTALL_RPATH "/usr/local/lib") if(BUILD_TESTING) @@ -142,13 +158,14 @@ if(BUILD_TESTING) ament_add_gtest(${PROJECT_NAME}_test_lifecycle_manager test/LifecycleManager.test.cpp - src/g2_2025_lifecycle_node/nodes/lifecycle_manager.cpp - src/g2_2025_lifecycle_node/nodes/hardware_interface.cpp - src/config/serialib.cpp + src/g2_2025_lifecycle_node/nodes/LifecycleManager.cpp + src/g2_2025_lifecycle_node/nodes/HardwareInterface.cpp + lib/serialib.cpp ) target_include_directories(${PROJECT_NAME}_test_lifecycle_manager PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/src ${CMAKE_CURRENT_SOURCE_DIR}/src/g2_2025_lifecycle_node + ${CMAKE_CURRENT_SOURCE_DIR}/lib ) ament_target_dependencies(${PROJECT_NAME}_test_lifecycle_manager rclcpp @@ -157,7 +174,8 @@ if(BUILD_TESTING) sensor_msgs ) target_link_libraries(${PROJECT_NAME}_test_lifecycle_manager - paho-mqttpp3-static + paho-mqttpp3 + paho-mqtt3as nlohmann_json::nlohmann_json ) set_target_properties(${PROJECT_NAME}_test_lifecycle_manager PROPERTIES INSTALL_RPATH "/usr/local/lib") diff --git a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/main.cpp b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/Main.cpp similarity index 84% rename from src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/main.cpp rename to src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/Main.cpp index 145d62b..89180f6 100644 --- a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/main.cpp +++ b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/Main.cpp @@ -7,9 +7,9 @@ */ #include "rclcpp/rclcpp.hpp" -#include "nodes/hardware_interface.hpp" -#include "nodes/lifecycle_manager.hpp" +#include "nodes/HardwareInterface.hpp" +#include "nodes/LifecycleManager.hpp" int main(int argc, char *argv[]) { rclcpp::init(argc, argv); diff --git a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.cpp b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/HardwareInterface.cpp similarity index 50% rename from src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.cpp rename to src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/HardwareInterface.cpp index fbc0fe8..61739ec 100644 --- a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.cpp +++ b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/HardwareInterface.cpp @@ -1,25 +1,24 @@ -#include "hardware_interface.hpp" -#include +#include "HardwareInterface.hpp" namespace assignments::two::g2_2025_lifecycle_node { -HardwareInterface::HardwareInterface() : Node("hardware_interface") { - RCLCPP_INFO(this->get_logger(), "HardwareInterface node has been created"); - imu_publisher = this->create_publisher("imu_data", 10); - RCLCPP_INFO(this->get_logger(), "IMU Publisher has been created"); -} +HardwareInterface::HardwareInterface(MQTTParameters mqtt_config) +: Node("HardwareInterface") +, mqtt_config_(mqtt_config) +{ + RCLCPP_INFO(this->get_logger(), "created HardwareInterface node"); + imu_publisher = this->create_publisher("imu_data", 10); + RCLCPP_INFO(this->get_logger(), "created IMU Publisher on topic 'imu_data'"); +} void HardwareInterface::publish_imu_data(const sensor_msgs::msg::Imu::SharedPtr msg) { imu_publisher->publish(*msg); } -void HardwareInterface::parse_data(const std::string& data) { - // RCLCPP_INFO(this->get_logger(), "Received data: %s", data.c_str()); - +void HardwareInterface::parse_and_publish_imu_data(const std::string& data) { try { auto json_data = nlohmann::json::parse(data); - auto imu_msg = std::make_shared(); imu_msg->header.stamp = this->now(); @@ -39,43 +38,39 @@ void HardwareInterface::parse_data(const std::string& data) { imu_msg->angular_velocity.z = json_data["gyro"].value("z", 0.0); } //print imu data - RCLCPP_INFO(this->get_logger(), "Publishing IMU Data - Accel: [%.3f, %.3f, %.3f], Gyro: [%.3f, %.3f, %.3f]", + RCLCPP_INFO(this->get_logger(), "Publishing IMU Data - Accel: [%.3f, %.3f, %.3f], Gyro: [%.3f, %.3f, %.3f]", imu_msg->linear_acceleration.x, imu_msg->linear_acceleration.y, imu_msg->linear_acceleration.z, imu_msg->angular_velocity.x, imu_msg->angular_velocity.y, imu_msg->angular_velocity.z); - publish_imu_data(imu_msg); + publish_imu_data(imu_msg); } catch (const nlohmann::json::exception& e) { RCLCPP_ERROR(this->get_logger(), "JSON parsing error: %s", e.what()); } } - -void HardwareInterface::start_read() { +void HardwareInterface::uart_start_read() { if (reading_.load()) { return; } reading_.store(true); - read_thread_ = std::thread([this]() { char buffer[116]; RCLCPP_INFO(this->get_logger(), "reader thread started"); while (reading_.load()) { - int ret = serial.readString(buffer, '\n', sizeof(buffer), 1000); + int ret = serial_.readString(buffer, '\n', sizeof(buffer), 1000); if (ret > 0) { RCLCPP_INFO(this->get_logger(), "Received buffer: %s", buffer); - parse_data(buffer); - } else { - // timeout of geen data. + parse_and_publish_imu_data(buffer); } } RCLCPP_INFO(this->get_logger(), "reader thread exiting"); }); } -void HardwareInterface::stop_read() { +void HardwareInterface::uart_stop_read() { if (!reading_.load()) return; reading_.store(false); if (read_thread_.joinable()) { @@ -83,89 +78,98 @@ void HardwareInterface::stop_read() { } } -void HardwareInterface::write(const std::string& data) { - RCLCPP_INFO(this->get_logger(), "Writing to hardware..."); - serial.writeString(data.c_str()); -} - -bool HardwareInterface::open_device(const std::string& device_path, int baud_rate) { - - char errorOpening = serial.openDevice(device_path.c_str(), baud_rate); +bool HardwareInterface::uart_open_device(const std::string& device_path, int baud_rate) { + char errorOpening = serial_.openDevice(device_path.c_str(), baud_rate); if (errorOpening != 1) { - RCLCPP_ERROR(this->get_logger(), "Error opening serial port: %d", errorOpening); + RCLCPP_ERROR(this->get_logger(), "Error opening serial_ port: %d", errorOpening); return false; } - RCLCPP_INFO(this->get_logger(), "Serial port opened successfully."); + RCLCPP_INFO(this->get_logger(), "serial_ port opened successfully."); return true; } -bool HardwareInterface::is_device_open() { - return serial.isDeviceOpen(); +bool HardwareInterface::uart_device_is_open() { + return serial_.isDeviceOpen(); } - -void HardwareInterface::close_device() { - - serial.closeDevice(); - +void HardwareInterface::uart_close_device() { + serial_.closeDevice(); } -class callback : public virtual mqtt::callback { -public: - void message_arrived(mqtt::const_message_ptr msg) override { - RCLCPP_INFO(rclcpp::get_logger("hardware_interface"), - "Message received: %s", msg->get_payload_str().c_str()); - parent_.parse_data(msg->get_payload_str()); - - } -private: -HardwareInterface parent_; -}; - - void HardwareInterface::mqtt_configure() { - if (!mqtt_client) { - mqtt_client = std::make_shared(SERVER_ADDRESS, CLIENT_ID); + try { + if (!mqtt_client) { + RCLCPP_INFO(this->get_logger(), "Creating MQTT client with server: %s, client_id: %s", + mqtt_config_.server_address.c_str(), mqtt_config_.client_id.c_str()); + + // Validate configuration + if (mqtt_config_.server_address.empty()) { + RCLCPP_ERROR(this->get_logger(), "MQTT server address is empty!"); + throw std::runtime_error("MQTT server address is empty"); + } + if (mqtt_config_.client_id.empty()) { + RCLCPP_ERROR(this->get_logger(), "MQTT client_id is empty!"); + throw std::runtime_error("MQTT client_id is empty"); + } + + mqtt_client = std::make_shared( + mqtt_config_.server_address, + mqtt_config_.client_id + ); + RCLCPP_INFO(this->get_logger(), "MQTT client created successfully"); + } + + if (!mqtt_cb) { + RCLCPP_INFO(this->get_logger(), "Creating MQTT callback"); + mqtt_cb = std::make_shared(*this); + RCLCPP_INFO(this->get_logger(), "MQTT callback created successfully"); + } + + // Set callback BEFORE connecting + RCLCPP_INFO(this->get_logger(), "Setting MQTT callback on client"); + mqtt_client->set_callback(*mqtt_cb); + RCLCPP_INFO(this->get_logger(), "MQTT callback set"); + + mqtt_connect(); + } catch (const std::exception& e) { + RCLCPP_ERROR(this->get_logger(), "Exception in mqtt_configure: %s", e.what()); + throw; } - if (!mqtt_cb) { - mqtt_cb = std::make_shared(); - } - mqtt_connect(); } -void HardwareInterface::mqtt_reader() { +void HardwareInterface::mqtt_start_listen() { RCLCPP_INFO(this->get_logger(), "MQTT listener started"); - mqtt_client->set_callback(*mqtt_cb); + // Callback already set in mqtt_configure() } void HardwareInterface::mqtt_connect() { - mqtt::connect_options connOpts; connOpts.set_keep_alive_interval(20); connOpts.set_clean_session(true); try { + RCLCPP_INFO(this->get_logger(), "Connecting to MQTT broker..."); mqtt_client->connect(connOpts)->wait(); RCLCPP_INFO(this->get_logger(), "Connected to broker"); - mqtt_client->subscribe(TOPIC, 1)->wait(); - RCLCPP_INFO(this->get_logger(), "Subscribed to topic: %s", TOPIC.c_str()); - // Note: do not disconnect here; keep client alive until close_mqtt_conn() + RCLCPP_INFO(this->get_logger(), "Subscribing to topic: %s", mqtt_config_.topic.c_str()); + mqtt_client->subscribe(mqtt_config_.topic, 1)->wait(); + RCLCPP_INFO(this->get_logger(), "Subscribed to topic: %s", mqtt_config_.topic.c_str()); } catch (const mqtt::exception& exc) { - RCLCPP_ERROR(this->get_logger(), "Error: %s", exc.what()); + RCLCPP_ERROR(this->get_logger(), "MQTT Error: %s", exc.what()); + throw; } } -void HardwareInterface::close_mqtt_conn() { +void HardwareInterface::mqtt_close_connection() { try { if (mqtt_client) { if (mqtt_client->is_connected()) { mqtt_client->disconnect()->wait(); RCLCPP_INFO(this->get_logger(), "Disconnected MQTT client"); } - // reset to allow destruction mqtt_client.reset(); } if (mqtt_cb) { @@ -176,5 +180,4 @@ void HardwareInterface::close_mqtt_conn() { } } - } // namespace assignments::two::g2_2025_lifecycle_node diff --git a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.hpp b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/HardwareInterface.hpp similarity index 51% rename from src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.hpp rename to src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/HardwareInterface.hpp index 7ff8a39..5969e1b 100644 --- a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/hardware_interface.hpp +++ b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/HardwareInterface.hpp @@ -1,4 +1,4 @@ -/* nodes/hardware_interface.hpp +/* nodes/HardwareInterface.hpp * Hardware interface implementation for the IMU reader system. * * Manages the serial communication with the IMU hardware, including initialization, @@ -11,55 +11,47 @@ * [20-10-2025] M.khalaf: Refactored code for readability. * [30-10-2025] M.khalaf: Improved error handling. * [31-10-2025] M.khalaf: Fixed serial read and mqtt configurations. + * [06-11-2025] Wessel T, Vincent W: Clean up code, remove uncessary imports */ #pragma once -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "config/serialib.h" -#include - - +#include #include #include -#include "sensor_msgs/msg/imu.hpp" +#include "serialib.h" + #include "rclcpp/rclcpp.hpp" -#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "sensor_msgs/msg/imu.hpp" + +#include "MQTTParameters.hpp" namespace assignments::two::g2_2025_lifecycle_node { class HardwareInterface : public rclcpp::Node { public: - HardwareInterface(); - void start_read(); - void stop_read(); - void write(const std::string& data); - bool open_device(const std::string& device_path, int baud_rate); - bool is_device_open(); - void close_device(); - void mqtt_connect(); - void close_mqtt_conn(); + HardwareInterface(MQTTParameters mqtt_config); + + bool uart_open_device(const std::string& device_path, int baud_rate); + bool uart_device_is_open(); + void uart_close_device(); + + void uart_start_read(); + void uart_stop_read(); + void mqtt_configure(); - void mqtt_reader(); - void parse_data(const std::string& data); + void mqtt_connect(); + + void mqtt_start_listen(); + void mqtt_close_connection(); + void publish_imu_data(const sensor_msgs::msg::Imu::SharedPtr msg); + void parse_and_publish_imu_data(const std::string& data); + private: - serialib serial; - // MQTT defaults. Use static inline so they can be initialized in-class. - static inline const std::string SERVER_ADDRESS = "tcp://localhost:1883"; - static inline const std::string CLIENT_ID = "cpp_mqtt_client"; - static inline const std::string TOPIC = "esp32/imu"; + serialib serial_; + MQTTParameters mqtt_config_ {}; std::shared_ptr mqtt_client; std::shared_ptr mqtt_cb; @@ -68,6 +60,23 @@ private: std::atomic_bool reading_{false}; // Publisher to emit parsed IMU messages (from serial or MQTT) rclcpp::Publisher::SharedPtr imu_publisher; - }; + +class callback : public virtual mqtt::callback { +public: + explicit callback(HardwareInterface& parent) : parent_(parent) {} + + void message_arrived(mqtt::const_message_ptr msg) override { + try { + RCLCPP_INFO(parent_.get_logger(), + "Message received: %s", msg->get_payload_str().c_str()); + parent_.parse_and_publish_imu_data(msg->get_payload_str()); + } catch (const std::exception& e) { + RCLCPP_ERROR(parent_.get_logger(), "Error in message_arrived: %s", e.what()); + } + } +private: + HardwareInterface& parent_; +}; + } // namespace assignments::two::g2_2025_lifecycle_node diff --git a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.cpp b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/LifecycleManager.cpp similarity index 71% rename from src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.cpp rename to src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/LifecycleManager.cpp index 98703c2..df90871 100644 --- a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.cpp +++ b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/LifecycleManager.cpp @@ -1,36 +1,28 @@ -#include "lifecycle_manager.hpp" -#include "hardware_interface.hpp" - +#include "LifecycleManager.hpp" +#include "HardwareInterface.hpp" namespace assignments::two::g2_2025_lifecycle_node { -LifecycleManager::LifecycleManager() : rclcpp_lifecycle::LifecycleNode("lifecycle_manager") { +LifecycleManager::LifecycleManager() : rclcpp_lifecycle::LifecycleNode("LifecycleManager") { + mqtt_config_.server_address = + this->declare_parameter("mqtt_server_address", "tcp://localhost:1883"); + mqtt_config_.client_id = + this->declare_parameter("mqtt_client_id", "cpp_mqtt-client"); + + mqtt_config_.topic = + this->declare_parameter("mqtt_subscribe_topic", "esp32/imu"); RCLCPP_INFO(this->get_logger(), "LifecycleManager node is ready"); device_path_ = this->declare_parameter("device_path", "/dev/ttyUSB0"); baudrate_ = this->declare_parameter("baudrate", 115200); - communication_type_ = this->declare_parameter("comm_t", "serial"); // placeholder default serial or param mqtt - - // hardware interface instance - hw_interface = std::make_shared(); + communication_type_ = this->declare_parameter("comm_t", "serial"); + hw_interface = std::make_shared(mqtt_config_); } -/* - -*in terminal commands to manage lifecycle: -*ros2 lifecycle list /lifecycle_manager ## for status checking -*ros2 lifecycle set /lifecycle_manager configure -*ros2 lifecycle set /lifecycle_manager activate -*ros2 lifecycle set /lifecycle_manager deactivate -*ros2 lifecycle set /lifecycle_manager shutdown -*/ - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleManager::on_configure(const rclcpp_lifecycle::State&) { - - RCLCPP_INFO(this->get_logger(), "configuring lifecycle..."); if (communication_type_ == "mqtt") { @@ -39,33 +31,28 @@ LifecycleManager::on_configure(const rclcpp_lifecycle::State&) { } else { RCLCPP_INFO(this->get_logger(), "Using serial communication."); - if (!hw_interface->open_device(device_path_, baudrate_)) { + if (!hw_interface->uart_open_device(device_path_, baudrate_)) { RCLCPP_ERROR(this->get_logger(), "Failed to open hardware device during configuration."); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; } } - //for testing parsing function. - // std::shared_ptr s; - // s = std::make_shared("{\"accel\":{\"x\": 0.037,\"y\": -1.164,\"z\": 9.775},\"gyro\":{\"x\": -0.024,\"y\": -0.014,\"z\": -0.001},\"Temp\": 41.01}"); - // hw_interface->parse_data(*s); RCLCPP_INFO(this->get_logger(), "Lifecycle configured successfully."); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleManager::on_activate(const rclcpp_lifecycle::State&) { - RCLCPP_INFO(this->get_logger(), "activating lifecycle..."); if (communication_type_ == "mqtt") { RCLCPP_INFO(this->get_logger(), "Reading on MQTT..."); - hw_interface->mqtt_reader(); + hw_interface->mqtt_start_listen(); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } else { RCLCPP_INFO(this->get_logger(), "Reading on Serial..."); - hw_interface->start_read(); + hw_interface->uart_start_read(); } return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; @@ -73,23 +60,22 @@ LifecycleManager::on_activate(const rclcpp_lifecycle::State&) { rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleManager::on_deactivate(const rclcpp_lifecycle::State&) { - RCLCPP_INFO(this->get_logger(), "deactivating lifecycle..."); if (communication_type_ == "mqtt") { - hw_interface->close_mqtt_conn(); + hw_interface->mqtt_close_connection(); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } else { - if (!hw_interface->is_device_open()) { + if (!hw_interface->uart_device_is_open()) { RCLCPP_ERROR(this->get_logger(), "Hardware device is not open during activation."); - hw_interface->close_device(); + hw_interface->uart_close_device(); } RCLCPP_INFO(this->get_logger(), "Hardware device is open,closing device..."); - hw_interface->stop_read(); - hw_interface->close_device(); + hw_interface->uart_stop_read(); + hw_interface->uart_close_device(); RCLCPP_INFO(this->get_logger(), "Lifecycle deactivated successfully."); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; diff --git a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.hpp b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/LifecycleManager.hpp similarity index 70% rename from src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.hpp rename to src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/LifecycleManager.hpp index 72de89f..1fa665a 100644 --- a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/lifecycle_manager.hpp +++ b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/LifecycleManager.hpp @@ -9,39 +9,39 @@ */ #pragma once -#include -#include -#include -#include -#include - #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" #include "std_msgs/msg/string.hpp" -#include "hardware_interface.hpp" +#include "HardwareInterface.hpp" +#include "MQTTParameters.hpp" namespace assignments::two::g2_2025_lifecycle_node { + class LifecycleManager : public rclcpp_lifecycle::LifecycleNode { public: LifecycleManager(); - // Hardware interface to interact with the IMU device. - // Made public for testing purposes + // Hardware interface to interact with the IMU device. Made public for testing purposes std::shared_ptr hw_interface; private: - rclcpp::Publisher::SharedPtr imu_publisher_; - std::string device_path_; int baudrate_; std::string communication_type_; + MQTTParameters mqtt_config_ {}; - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(const rclcpp_lifecycle::State&); - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate(const rclcpp_lifecycle::State&); - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State&); - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown(const rclcpp_lifecycle::State&); + rclcpp::Publisher::SharedPtr imu_publisher_; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_configure(const rclcpp_lifecycle::State&); + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_activate(const rclcpp_lifecycle::State&); + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_deactivate(const rclcpp_lifecycle::State&); + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_shutdown(const rclcpp_lifecycle::State&); }; + } // namespace assignments::two::g2_2025_lifecycle_node diff --git a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/MQTTParameters.hpp b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/MQTTParameters.hpp new file mode 100644 index 0000000..62f7402 --- /dev/null +++ b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/MQTTParameters.hpp @@ -0,0 +1,13 @@ +#pragma once + +#include + +namespace assignments::two { + +struct MQTTParameters { + std::string server_address; + std::string client_id; + std::string topic; +}; + +} // namespace assignments::two diff --git a/src/g2_2025_imu_reader_pkg/test/LifecycleManager.test.cpp b/src/g2_2025_imu_reader_pkg/test/LifecycleManager.test.cpp index 0867e07..cb1cf84 100644 --- a/src/g2_2025_imu_reader_pkg/test/LifecycleManager.test.cpp +++ b/src/g2_2025_imu_reader_pkg/test/LifecycleManager.test.cpp @@ -2,7 +2,8 @@ #include #include #include -#include "g2_2025_lifecycle_node/nodes/lifecycle_manager.hpp" + +#include "g2_2025_lifecycle_node/nodes/LifecycleManager.hpp" using namespace assignments::two::g2_2025_lifecycle_node; @@ -225,7 +226,7 @@ TEST_F(LifecycleManagerTest, HardwareInterfaceIntegrationTest) { EXPECT_NE(node->hw_interface, nullptr); // Verify we can make method calls on the hardware interface - EXPECT_FALSE(node->hw_interface->is_device_open()); + EXPECT_FALSE(node->hw_interface->uart_device_is_open()); } int main(int argc, char** argv) { From 3fc98d51ed66853a2a24062bd4f6f3321ea2cb90 Mon Sep 17 00:00:00 2001 From: Vincent Winter Date: Thu, 6 Nov 2025 18:20:58 +0100 Subject: [PATCH 15/17] feat: Working Version --- .../nodes/HardwareInterface.cpp | 176 ++++++++++++------ .../nodes/HardwareInterface.hpp | 38 ++-- 2 files changed, 129 insertions(+), 85 deletions(-) diff --git a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/HardwareInterface.cpp b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/HardwareInterface.cpp index 61739ec..f02d508 100644 --- a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/HardwareInterface.cpp +++ b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/HardwareInterface.cpp @@ -8,12 +8,29 @@ HardwareInterface::HardwareInterface(MQTTParameters mqtt_config) { RCLCPP_INFO(this->get_logger(), "created HardwareInterface node"); - imu_publisher = this->create_publisher("imu_data", 10); + imu_publisher_ = this->create_publisher("imu_data", 10); RCLCPP_INFO(this->get_logger(), "created IMU Publisher on topic 'imu_data'"); } -void HardwareInterface::publish_imu_data(const sensor_msgs::msg::Imu::SharedPtr msg) { - imu_publisher->publish(*msg); +HardwareInterface::~HardwareInterface() { + try { + // Close MQTT first (stops thread and disconnects) + mqtt_close_connection(); + + // Then stop UART reading + uart_stop_read(); + + // Finally close the device + if (uart_device_is_open()) { + uart_close_device(); + } + } catch (const std::exception& e) { + // Log but don't throw from destructor + RCLCPP_ERROR(this->get_logger(), "Error in destructor: %s", e.what()); + } catch (...) { + // Catch all to prevent throwing from destructor + RCLCPP_ERROR(this->get_logger(), "Unknown error in destructor"); + } } void HardwareInterface::parse_and_publish_imu_data(const std::string& data) { @@ -37,44 +54,48 @@ void HardwareInterface::parse_and_publish_imu_data(const std::string& data) { imu_msg->angular_velocity.y = json_data["gyro"].value("y", 0.0); imu_msg->angular_velocity.z = json_data["gyro"].value("z", 0.0); } - //print imu data - RCLCPP_INFO(this->get_logger(), "Publishing IMU Data - Accel: [%.3f, %.3f, %.3f], Gyro: [%.3f, %.3f, %.3f]", + + RCLCPP_INFO(this->get_logger(), + "Publishing IMU Data - Accel: [%.3f, %.3f, %.3f], Gyro: [%.3f, %.3f, %.3f]", imu_msg->linear_acceleration.x, imu_msg->linear_acceleration.y, imu_msg->linear_acceleration.z, - imu_msg->angular_velocity.x, imu_msg->angular_velocity.y, imu_msg->angular_velocity.z); - - publish_imu_data(imu_msg); + imu_msg->angular_velocity.x, imu_msg->angular_velocity.y, imu_msg->angular_velocity.z + ); + imu_publisher_->publish(*imu_msg); } catch (const nlohmann::json::exception& e) { RCLCPP_ERROR(this->get_logger(), "JSON parsing error: %s", e.what()); } } void HardwareInterface::uart_start_read() { - if (reading_.load()) { + if (uart_reading_.load()) { return; } - reading_.store(true); + uart_reading_.store(true); - read_thread_ = std::thread([this]() { + uart_read_thread_ = std::thread([this]() { char buffer[116]; RCLCPP_INFO(this->get_logger(), "reader thread started"); - while (reading_.load()) { + + while (uart_reading_.load()) { int ret = serial_.readString(buffer, '\n', sizeof(buffer), 1000); if (ret > 0) { RCLCPP_INFO(this->get_logger(), "Received buffer: %s", buffer); parse_and_publish_imu_data(buffer); } } + RCLCPP_INFO(this->get_logger(), "reader thread exiting"); }); } void HardwareInterface::uart_stop_read() { - if (!reading_.load()) return; - reading_.store(false); - if (read_thread_.joinable()) { - read_thread_.join(); + if (!uart_reading_.load()) return; + uart_reading_.store(false); + + if (uart_read_thread_.joinable()) { + uart_read_thread_.join(); } } @@ -98,39 +119,33 @@ void HardwareInterface::uart_close_device() { serial_.closeDevice(); } +void HardwareInterface::mqtt_message_callback( + const std::string& topic, + const std::string& payload, + HardwareInterface* self +) { + if (self) { + RCLCPP_INFO(self->get_logger(), "Message received on topic %s: %s", topic.c_str(), payload.c_str()); + + self->parse_and_publish_imu_data(payload); + } +} + void HardwareInterface::mqtt_configure() { try { - if (!mqtt_client) { - RCLCPP_INFO(this->get_logger(), "Creating MQTT client with server: %s, client_id: %s", - mqtt_config_.server_address.c_str(), mqtt_config_.client_id.c_str()); - - // Validate configuration - if (mqtt_config_.server_address.empty()) { - RCLCPP_ERROR(this->get_logger(), "MQTT server address is empty!"); - throw std::runtime_error("MQTT server address is empty"); - } - if (mqtt_config_.client_id.empty()) { - RCLCPP_ERROR(this->get_logger(), "MQTT client_id is empty!"); - throw std::runtime_error("MQTT client_id is empty"); - } - - mqtt_client = std::make_shared( - mqtt_config_.server_address, - mqtt_config_.client_id - ); - RCLCPP_INFO(this->get_logger(), "MQTT client created successfully"); - } + RCLCPP_INFO(this->get_logger(), "Creating MQTT client with server: %s, client_id: %s", + mqtt_config_.server_address.c_str(), mqtt_config_.client_id.c_str() + ); - if (!mqtt_cb) { - RCLCPP_INFO(this->get_logger(), "Creating MQTT callback"); - mqtt_cb = std::make_shared(*this); - RCLCPP_INFO(this->get_logger(), "MQTT callback created successfully"); - } + // Create client with explicit parameters to avoid memory issues + // Use empty string for persistence (in-memory only) + mqtt_client_ = std::make_unique( + mqtt_config_.server_address, + mqtt_config_.client_id, + mqtt::create_options(MQTTVERSION_5) // Use MQTT v5 with create options + ); - // Set callback BEFORE connecting - RCLCPP_INFO(this->get_logger(), "Setting MQTT callback on client"); - mqtt_client->set_callback(*mqtt_cb); - RCLCPP_INFO(this->get_logger(), "MQTT callback set"); + RCLCPP_INFO(this->get_logger(), "MQTT client created successfully"); mqtt_connect(); } catch (const std::exception& e) { @@ -140,22 +155,47 @@ void HardwareInterface::mqtt_configure() { } void HardwareInterface::mqtt_start_listen() { - RCLCPP_INFO(this->get_logger(), "MQTT listener started"); - // Callback already set in mqtt_configure() + RCLCPP_INFO(this->get_logger(), "MQTT listener started - consuming messages synchronously"); + + // Start a background thread to consume messages + if (!mqtt_reading_.load()) { + mqtt_reading_.store(true); + mqtt_read_thread_ = std::thread([this]() { + RCLCPP_INFO(this->get_logger(), "MQTT consumer thread started"); + + try { + while (mqtt_reading_.load() && mqtt_client_ && mqtt_client_->is_connected()) { + mqtt::const_message_ptr msg; + if (mqtt_client_->try_consume_message(&msg)) { + mqtt_message_callback(msg->get_topic(), msg->get_payload_str(), this); + } else { + // No message available, sleep briefly to avoid busy-waiting + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + } + } catch (const mqtt::exception& exc) { + RCLCPP_ERROR(this->get_logger(), "MQTT consumer error: %s", exc.what()); + } + + RCLCPP_INFO(this->get_logger(), "MQTT consumer thread exiting"); + }); + } } void HardwareInterface::mqtt_connect() { - mqtt::connect_options connOpts; - connOpts.set_keep_alive_interval(20); - connOpts.set_clean_session(true); + if (!mqtt_client_) { + RCLCPP_ERROR(this->get_logger(), "MQTT client is not initialized"); + return; + } try { RCLCPP_INFO(this->get_logger(), "Connecting to MQTT broker..."); - mqtt_client->connect(connOpts)->wait(); + + mqtt_client_->connect(); RCLCPP_INFO(this->get_logger(), "Connected to broker"); RCLCPP_INFO(this->get_logger(), "Subscribing to topic: %s", mqtt_config_.topic.c_str()); - mqtt_client->subscribe(mqtt_config_.topic, 1)->wait(); + mqtt_client_->subscribe(mqtt_config_.topic, 1); RCLCPP_INFO(this->get_logger(), "Subscribed to topic: %s", mqtt_config_.topic.c_str()); } catch (const mqtt::exception& exc) { RCLCPP_ERROR(this->get_logger(), "MQTT Error: %s", exc.what()); @@ -165,18 +205,32 @@ void HardwareInterface::mqtt_connect() { void HardwareInterface::mqtt_close_connection() { try { - if (mqtt_client) { - if (mqtt_client->is_connected()) { - mqtt_client->disconnect()->wait(); - RCLCPP_INFO(this->get_logger(), "Disconnected MQTT client"); + // Stop the reading thread first + if (mqtt_reading_.load()) { + mqtt_reading_.store(false); + if (mqtt_read_thread_.joinable()) { + mqtt_read_thread_.join(); } - mqtt_client.reset(); } - if (mqtt_cb) { - mqtt_cb.reset(); + + // Now safely disconnect and cleanup MQTT client + if (mqtt_client_) { + try { + if (mqtt_client_->is_connected()) { + RCLCPP_INFO(this->get_logger(), "Disconnecting MQTT client..."); + mqtt_client_->unsubscribe(mqtt_config_.topic); + mqtt_client_->disconnect(); + RCLCPP_INFO(this->get_logger(), "Disconnected MQTT client"); + } + } catch (const mqtt::exception& exc) { + RCLCPP_WARN(this->get_logger(), "Error during MQTT disconnect: %s", exc.what()); + } + + // Reset the unique_ptr to destroy the client + mqtt_client_.reset(); } - } catch (const mqtt::exception& exc) { - RCLCPP_ERROR(this->get_logger(), "Error while disconnecting MQTT: %s", exc.what()); + } catch (const std::exception& e) { + RCLCPP_ERROR(this->get_logger(), "Error while closing MQTT connection: %s", e.what()); } } diff --git a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/HardwareInterface.hpp b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/HardwareInterface.hpp index 5969e1b..f5d986f 100644 --- a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/HardwareInterface.hpp +++ b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/HardwareInterface.hpp @@ -15,9 +15,12 @@ */ #pragma once +#include +#include +#include +#include #include #include -#include #include "serialib.h" @@ -32,6 +35,7 @@ class HardwareInterface : public rclcpp::Node { public: HardwareInterface(MQTTParameters mqtt_config); + ~HardwareInterface(); bool uart_open_device(const std::string& device_path, int baud_rate); bool uart_device_is_open(); @@ -46,37 +50,23 @@ public: void mqtt_start_listen(); void mqtt_close_connection(); - void publish_imu_data(const sensor_msgs::msg::Imu::SharedPtr msg); void parse_and_publish_imu_data(const std::string& data); private: + rclcpp::Publisher::SharedPtr imu_publisher_; + serialib serial_; - MQTTParameters mqtt_config_ {}; - std::shared_ptr mqtt_client; - std::shared_ptr mqtt_cb; + MQTTParameters mqtt_config_; + std::unique_ptr mqtt_client_; - std::thread read_thread_; - std::atomic_bool reading_{false}; - // Publisher to emit parsed IMU messages (from serial or MQTT) - rclcpp::Publisher::SharedPtr imu_publisher; -}; + std::thread uart_read_thread_; + std::atomic_bool uart_reading_ {false}; -class callback : public virtual mqtt::callback { -public: - explicit callback(HardwareInterface& parent) : parent_(parent) {} + std::thread mqtt_read_thread_; + std::atomic_bool mqtt_reading_ {false}; - void message_arrived(mqtt::const_message_ptr msg) override { - try { - RCLCPP_INFO(parent_.get_logger(), - "Message received: %s", msg->get_payload_str().c_str()); - parent_.parse_and_publish_imu_data(msg->get_payload_str()); - } catch (const std::exception& e) { - RCLCPP_ERROR(parent_.get_logger(), "Error in message_arrived: %s", e.what()); - } - } -private: - HardwareInterface& parent_; + static void mqtt_message_callback(const std::string& topic, const std::string& payload, HardwareInterface* self); }; } // namespace assignments::two::g2_2025_lifecycle_node From aba170f937fbcbe88e407f729690d8f4156ffab7 Mon Sep 17 00:00:00 2001 From: Vincent Winter Date: Thu, 6 Nov 2025 19:48:57 +0100 Subject: [PATCH 16/17] feat: Working v2 --- doc/architecture/nodes/HardwareInterface.md | 19 ------ doc/architecture/nodes/LifecycleManager.md | 37 ++++++----- doc/installation/installation.md | 30 +++++++-- src/g2_2025_imu_reader_pkg/CMakeLists.txt | 28 +-------- .../nodes/HardwareInterface.cpp | 63 +++++++------------ .../nodes/HardwareInterface.hpp | 1 + .../nodes/LifecycleManager.cpp | 37 +++++++++-- .../nodes/LifecycleManager.hpp | 6 +- 8 files changed, 109 insertions(+), 112 deletions(-) diff --git a/doc/architecture/nodes/HardwareInterface.md b/doc/architecture/nodes/HardwareInterface.md index db4ffe7..2c15569 100644 --- a/doc/architecture/nodes/HardwareInterface.md +++ b/doc/architecture/nodes/HardwareInterface.md @@ -338,25 +338,6 @@ hw->mqtt_reader(); hw->close_mqtt_conn(); ``` -### Via LifecycleManager (Recommended) - -```bash -# Launch and manage via lifecycle -ros2 run g2_2025_imu_reader_pkg g2_2025_lifecycle_node \ - --ros-args -p device_path:=/dev/ttyUSB0 -p baudrate:=115200 -p comm_t:=serial - -# Configure and activate -ros2 lifecycle set /lifecycle_manager configure -ros2 lifecycle set /lifecycle_manager activate - -# Subscribe to IMU data -ros2 topic echo /imu_data - -# Deactivate and cleanup -ros2 lifecycle set /lifecycle_manager deactivate -ros2 lifecycle set /lifecycle_manager shutdown -``` - ## Design Patterns 1. **Abstraction Pattern**: Encapsulates serial and MQTT complexity behind a unified interface diff --git a/doc/architecture/nodes/LifecycleManager.md b/doc/architecture/nodes/LifecycleManager.md index 475dd17..7f1ced8 100644 --- a/doc/architecture/nodes/LifecycleManager.md +++ b/doc/architecture/nodes/LifecycleManager.md @@ -1,7 +1,7 @@ # LifecycleManager (`assignments::two::g2_2025_lifecycle_node`) ## Overview -The `LifecycleManager` is the lifecycle node responsible for receiving the data of the ESP32-IMU combination via serial/MQTT and publishing that data to the database management node. For the functionality outside of the lifecycle see [HardwareInterface.md](HardwareInterface.md]) +The `LifecycleManager` is the core lifecycle-aware node responsible for managing the IMU reader system's operational states and hardware communication. It orchestrates transitions between configuration, activation, and deactivation phases, abstracting the complexity of dual communication backends (serial and MQTT) into a unified interface. #### Implementation Details @@ -9,7 +9,7 @@ The `LifecycleManager` is the lifecycle node responsible for receiving the data - **`device_path`** (string, default: "/dev/ttyUSB0"): Serial device path for hardware connection (e.g., USB serial adapter). - **`baudrate`** (int, default: 115200): Serial communication baud rate in bits per second. -- **`comm_t`** (string, default: "serial"): Communication type selector—either "serial" or "mqtt". +- **`comm_t`** (string, default: "serial"): Communication type selector—either "serial" or "mqtt" to determine which backend to use. **Constructor** ```cpp @@ -63,15 +63,18 @@ LifecycleManager() ## Communication Architecture -The `LifecycleManager` switches communication type depending on the `comm_t` parameter: +### Dual Backend Support -#### Serial Communication +The `LifecycleManager` provides a flexible, pluggable communication architecture via the `comm_t` parameter: + +#### Serial Communication Path 1. **Configuration Phase** (`on_configure`): - Opens the serial device at the path specified by `device_path` and baudrate + - Validates device readiness 2. **Activation Phase** (`on_activate`): - Spawns a background reader thread via `hw_interface->start_read()` - - Continuously polls the serial device with a timeout + - Thread continuously polls the serial device with a timeout - Reads are accumulated in a partial buffer, split on newline, and parsed as JSON - Each valid JSON IMU payload is parsed into a `sensor_msgs::msg::Imu` and published to the ROS topic `imu/data` @@ -80,7 +83,7 @@ The `LifecycleManager` switches communication type depending on the `comm_t` par - Joins the thread to ensure clean termination - Closes the serial device -#### MQTT Communication +#### MQTT Communication Path 1. **Configuration Phase** (`on_configure`): - Creates a persistent MQTT async client pointing to the broker at `SERVER_ADDRESS` (default: `tcp://localhost:1883`) - Initializes MQTT callback infrastructure @@ -100,20 +103,24 @@ To interact with the `LifecycleManager` from the command line, use the following ```bash # List current lifecycle state -ros2 lifecycle list /lifecycle_manager +ros2 lifecycle list /LifecycleManager # Transition: UNCONFIGURED -> INACTIVE -ros2 lifecycle set /lifecycle_manager configure +ros2 lifecycle set /LifecycleManager configure + +# Transition: INACTIVE -> UNCONFIGURED +ros2 lifecycle set /LifecycleManager cleanup # Transition: INACTIVE -> ACTIVE -ros2 lifecycle set /lifecycle_manager activate +ros2 lifecycle set /LifecycleManager activate # Transition: ACTIVE -> INACTIVE -ros2 lifecycle set /lifecycle_manager deactivate +ros2 lifecycle set /LifecycleManager deactivate # Transition: INACTIVE -> FINALIZED -ros2 lifecycle set /lifecycle_manager shutdown +ros2 lifecycle set /LifecycleManager shutdown ``` +![img](https://design.ros2.org/img/node_lifecycle/life_cycle_sm.png) ## Data Flow @@ -194,15 +201,15 @@ ros2 run g2_2025_imu_reader_pkg g2_2025_lifecycle_node \ -p comm_t:=mqtt # In another terminal, configure and activate the lifecycle -ros2 lifecycle set /lifecycle_manager configure -ros2 lifecycle set /lifecycle_manager activate +ros2 lifecycle set /LifecycleManager configure +ros2 lifecycle set /LifecycleManager activate # Subscribe to published IMU data ros2 topic echo /imu_data # Deactivate and shutdown -ros2 lifecycle set /lifecycle_manager deactivate -ros2 lifecycle set /lifecycle_manager shutdown +ros2 lifecycle set /LifecycleManager deactivate +ros2 lifecycle set /LifecycleManager shutdown ``` --- diff --git a/doc/installation/installation.md b/doc/installation/installation.md index bb4212e..6469afb 100644 --- a/doc/installation/installation.md +++ b/doc/installation/installation.md @@ -10,6 +10,22 @@ - Colcon build tool - Docker compose +### Paho MQTT library + +For this project the Paho MQTT library is needed, which can be built with the following commands: + +```bash +git clone https://github.com/eclipse/paho.mqtt.cpp +cd paho.mqtt.cpp +git co v1.5.4 + +git submodule init +git submodule update + +cmake -Bbuild -H. -DPAHO_WITH_MQTT_C=ON -DPAHO_BUILD_EXAMPLES=ON +sudo cmake --build build/ --target install +``` + ### Clone the Repository ```bash @@ -38,10 +54,10 @@ You can configure specific database settings in the `docker-compose.yaml` in the ### Start the IMU Reader program ```bash -For Serial: +# For Serial: ros2 launch g2_2025_imu_reader_pkg serial.launch.xml -For MQTT: +# For MQTT: ros2 launch g2_2025_imu_reader_pkg mqtt.launch.xml ``` To change parameters when using the launch file it will need to be edited in the `src/g2_2025_imu_reader_pkg/launch` folder. All parameters are already added to this document and thus only the values will need to be changed @@ -50,8 +66,10 @@ To change parameters when using the launch file it will need to be edited in the To setup the lifecycle node the following commands can be used. They must be used in this order. ```bash -ros2 lifecycle set /lifecycle_manager configure -ros2 lifecycle set /lifecycle_manager activate -ros2 lifecycle set /lifecycle_manager deactivate -ros2 lifecycle set /lifecycle_manager shutdown +ros2 lifecycle set /LifecycleManager configure +ros2 lifecycle set /LifecycleManager cleanup +ros2 lifecycle set /LifecycleManager activate +ros2 lifecycle set /LifecycleManager deactivate +ros2 lifecycle set /LifecycleManager shutdown ``` +![img](https://design.ros2.org/img/node_lifecycle/life_cycle_sm.png) diff --git a/src/g2_2025_imu_reader_pkg/CMakeLists.txt b/src/g2_2025_imu_reader_pkg/CMakeLists.txt index 4c2f0f9..f12ca95 100644 --- a/src/g2_2025_imu_reader_pkg/CMakeLists.txt +++ b/src/g2_2025_imu_reader_pkg/CMakeLists.txt @@ -16,30 +16,6 @@ FetchContent_Declare( FetchContent_MakeAvailable(tomlplusplus) -FetchContent_Declare( - paho_mqtt_cpp - GIT_REPOSITORY https://github.com/eclipse/paho.mqtt.cpp.git - GIT_TAG v1.5.2 -) - -set(PAHO_WITH_MQTT_C ON CACHE BOOL "Build with Paho MQTT C library" FORCE) -set(PAHO_MQTT_CPP_BUILD_STATIC ON CACHE BOOL "Build static library" FORCE) -set(PAHO_MQTT_CPP_BUILD_SHARED OFF CACHE BOOL "Build shared library" FORCE) -set(PAHO_WITH_SSL OFF CACHE BOOL "Build with SSL support" FORCE) -set(PAHO_BUILD_EXAMPLES OFF CACHE BOOL "Build example programs" FORCE) - -# Suppress warnings from FetchContent dependencies -set(CMAKE_CXX_FLAGS_BACKUP "${CMAKE_CXX_FLAGS}") -set(CMAKE_C_FLAGS_BACKUP "${CMAKE_C_FLAGS}") -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w") -set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -w") - -FetchContent_MakeAvailable(paho_mqtt_cpp) - -# Restore original flags -set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS_BACKUP}") -set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS_BACKUP}") - FetchContent_Declare( nlohmann_json GIT_REPOSITORY https://github.com/nlohmann/json.git @@ -86,7 +62,7 @@ ament_target_dependencies(g2_2025_lifecycle_node rclcpp rclcpp_lifecycle std_msg target_link_libraries(g2_2025_lifecycle_node paho-mqttpp3 - paho-mqtt3as + paho-mqtt3c nlohmann_json::nlohmann_json ) @@ -175,7 +151,7 @@ if(BUILD_TESTING) ) target_link_libraries(${PROJECT_NAME}_test_lifecycle_manager paho-mqttpp3 - paho-mqtt3as + paho-mqtt3c nlohmann_json::nlohmann_json ) set_target_properties(${PROJECT_NAME}_test_lifecycle_manager PROPERTIES INSTALL_RPATH "/usr/local/lib") diff --git a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/HardwareInterface.cpp b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/HardwareInterface.cpp index f02d508..f7a3140 100644 --- a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/HardwareInterface.cpp +++ b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/HardwareInterface.cpp @@ -13,24 +13,13 @@ HardwareInterface::HardwareInterface(MQTTParameters mqtt_config) } HardwareInterface::~HardwareInterface() { - try { - // Close MQTT first (stops thread and disconnects) - mqtt_close_connection(); - - // Then stop UART reading - uart_stop_read(); + uart_stop_read(); - // Finally close the device - if (uart_device_is_open()) { - uart_close_device(); - } - } catch (const std::exception& e) { - // Log but don't throw from destructor - RCLCPP_ERROR(this->get_logger(), "Error in destructor: %s", e.what()); - } catch (...) { - // Catch all to prevent throwing from destructor - RCLCPP_ERROR(this->get_logger(), "Unknown error in destructor"); + if (uart_device_is_open()) { + uart_close_device(); } + + mqtt_close_connection(); } void HardwareInterface::parse_and_publish_imu_data(const std::string& data) { @@ -137,12 +126,10 @@ void HardwareInterface::mqtt_configure() { mqtt_config_.server_address.c_str(), mqtt_config_.client_id.c_str() ); - // Create client with explicit parameters to avoid memory issues - // Use empty string for persistence (in-memory only) + // Create client with basic constructor (no persistence, no create options) mqtt_client_ = std::make_unique( mqtt_config_.server_address, - mqtt_config_.client_id, - mqtt::create_options(MQTTVERSION_5) // Use MQTT v5 with create options + mqtt_config_.client_id ); RCLCPP_INFO(this->get_logger(), "MQTT client created successfully"); @@ -203,34 +190,28 @@ void HardwareInterface::mqtt_connect() { } } +void HardwareInterface::mqtt_stop_listen() { + if (mqtt_reading_.load()) { + mqtt_reading_.store(false); + if (mqtt_read_thread_.joinable()) { + mqtt_read_thread_.join(); + } + } +} + void HardwareInterface::mqtt_close_connection() { try { - // Stop the reading thread first - if (mqtt_reading_.load()) { - mqtt_reading_.store(false); - if (mqtt_read_thread_.joinable()) { - mqtt_read_thread_.join(); - } - } + mqtt_stop_listen(); - // Now safely disconnect and cleanup MQTT client if (mqtt_client_) { - try { - if (mqtt_client_->is_connected()) { - RCLCPP_INFO(this->get_logger(), "Disconnecting MQTT client..."); - mqtt_client_->unsubscribe(mqtt_config_.topic); - mqtt_client_->disconnect(); - RCLCPP_INFO(this->get_logger(), "Disconnected MQTT client"); - } - } catch (const mqtt::exception& exc) { - RCLCPP_WARN(this->get_logger(), "Error during MQTT disconnect: %s", exc.what()); + if (mqtt_client_->is_connected()) { + mqtt_client_->disconnect(); + RCLCPP_INFO(this->get_logger(), "Disconnected MQTT client"); } - - // Reset the unique_ptr to destroy the client mqtt_client_.reset(); } - } catch (const std::exception& e) { - RCLCPP_ERROR(this->get_logger(), "Error while closing MQTT connection: %s", e.what()); + } catch (const mqtt::exception& exc) { + RCLCPP_ERROR(this->get_logger(), "Error while disconnecting MQTT: %s", exc.what()); } } diff --git a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/HardwareInterface.hpp b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/HardwareInterface.hpp index f5d986f..5875aec 100644 --- a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/HardwareInterface.hpp +++ b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/HardwareInterface.hpp @@ -48,6 +48,7 @@ public: void mqtt_connect(); void mqtt_start_listen(); + void mqtt_stop_listen(); void mqtt_close_connection(); void parse_and_publish_imu_data(const std::string& data); diff --git a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/LifecycleManager.cpp b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/LifecycleManager.cpp index df90871..6244ad5 100644 --- a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/LifecycleManager.cpp +++ b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/LifecycleManager.cpp @@ -21,6 +21,12 @@ LifecycleManager::LifecycleManager() : rclcpp_lifecycle::LifecycleNode("Lifecycl hw_interface = std::make_shared(mqtt_config_); } +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +LifecycleManager::on_error(const rclcpp_lifecycle::State&) { + RCLCPP_ERROR(this->get_logger(), "Error occurred in lifecycle..."); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; +} + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleManager::on_configure(const rclcpp_lifecycle::State&) { RCLCPP_INFO(this->get_logger(), "configuring lifecycle..."); @@ -41,6 +47,21 @@ LifecycleManager::on_configure(const rclcpp_lifecycle::State&) { return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +LifecycleManager::on_cleanup(const rclcpp_lifecycle::State&) { + RCLCPP_INFO(this->get_logger(), "cleaning up lifecycle..."); + if (communication_type_ == "mqtt") { + hw_interface->mqtt_close_connection(); + } else { + if (hw_interface->uart_device_is_open()) { + hw_interface->uart_stop_read(); + hw_interface->uart_close_device(); + } + } + + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleManager::on_activate(const rclcpp_lifecycle::State&) { RCLCPP_INFO(this->get_logger(), "activating lifecycle..."); @@ -64,7 +85,7 @@ LifecycleManager::on_deactivate(const rclcpp_lifecycle::State&) { if (communication_type_ == "mqtt") { - hw_interface->mqtt_close_connection(); + hw_interface->mqtt_stop_listen(); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } else @@ -75,15 +96,23 @@ LifecycleManager::on_deactivate(const rclcpp_lifecycle::State&) { } RCLCPP_INFO(this->get_logger(), "Hardware device is open,closing device..."); hw_interface->uart_stop_read(); - hw_interface->uart_close_device(); + RCLCPP_INFO(this->get_logger(), "Lifecycle deactivated successfully."); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; - } - } + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleManager::on_shutdown(const rclcpp_lifecycle::State&) { + if (communication_type_ == "mqtt") { + hw_interface->mqtt_close_connection(); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } else { + if (hw_interface->uart_device_is_open()) { + hw_interface->uart_stop_read(); + hw_interface->uart_close_device(); + } + } RCLCPP_INFO(this->get_logger(), "shutting down lifecycle..."); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; diff --git a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/LifecycleManager.hpp b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/LifecycleManager.hpp index 1fa665a..7d0428a 100644 --- a/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/LifecycleManager.hpp +++ b/src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/LifecycleManager.hpp @@ -1,4 +1,4 @@ -/* nodes/lifecycle_manager.hpp +/* nodes/LifecycleManager.hpp * Lifecycle node implementation for managing the lifecycle of the IMU reader system. * * Manages the different states of the lifecycle node, including configuration, @@ -34,6 +34,10 @@ private: rclcpp::Publisher::SharedPtr imu_publisher_; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_error(const rclcpp_lifecycle::State&); + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_cleanup(const rclcpp_lifecycle::State&); rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(const rclcpp_lifecycle::State&); rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn From 81911f4499efefbebb9acb9d8025911161c10192 Mon Sep 17 00:00:00 2001 From: Vincent Winter Date: Fri, 7 Nov 2025 10:53:13 +0100 Subject: [PATCH 17/17] fix: Add mode indication light Light on : MQTT Light off : Serial --- src/IMU/main/mpu6886.c | 5 +++++ src/IMU/main/mpu6886.h | 3 ++- src/IMU/sdkconfig | 2 -- 3 files changed, 7 insertions(+), 3 deletions(-) diff --git a/src/IMU/main/mpu6886.c b/src/IMU/main/mpu6886.c index 8372af2..e840f1a 100644 --- a/src/IMU/main/mpu6886.c +++ b/src/IMU/main/mpu6886.c @@ -291,6 +291,9 @@ void app_main(void) } init_button(); + gpio_reset_pin(LED_GPIO); + gpio_set_direction(LED_GPIO, GPIO_MODE_OUTPUT); + mpu6886_t mpu; mpu.i2c_port = I2C_NUM_0; mpu.address = MPU6886_ADDR; @@ -311,11 +314,13 @@ void app_main(void) wifi_init(); mqtt_app_start(); ESP_LOGI("BOOT", "Boot button pressed: starting MQTT mode"); + gpio_set_level(LED_GPIO, 1); toggle_completed = true; } else if (!mqtt_toggle && !toggle_completed && wifi_initialized) { mqtt_app_stop(); wifi_deinit(); ESP_LOGI("BOOT", "Boot button not pressed: starting serial-only mode"); + gpio_set_level(LED_GPIO, 0); toggle_completed = true; } mpu6886_read_accel(&mpu, &accel); diff --git a/src/IMU/main/mpu6886.h b/src/IMU/main/mpu6886.h index 0e9ed71..d9f1676 100644 --- a/src/IMU/main/mpu6886.h +++ b/src/IMU/main/mpu6886.h @@ -26,6 +26,7 @@ #define I2C_MASTER_TIMEOUT_MS 1000 #define BOOT_BUTTON_GPIO 0 // GPIO number for boot mode selection button +#define LED_GPIO 33 // GPIO number for onboard LED #define MPU6886_ADDR 0x68 @@ -71,7 +72,7 @@ typedef struct { vec3_t accel_offset; } mpu6886_t; -bool mqtt_toggle = false; +bool mqtt_toggle = true; bool toggle_completed = false; bool wifi_initialized = false; diff --git a/src/IMU/sdkconfig b/src/IMU/sdkconfig index 129d42a..42d24f9 100644 --- a/src/IMU/sdkconfig +++ b/src/IMU/sdkconfig @@ -448,8 +448,6 @@ CONFIG_I2C_MASTER_SDA=21 CONFIG_I2C_MASTER_FREQUENCY=100000 # end of I2C Master Configuration -CONFIG_ENV_MQTT_ENABLED=y - # # MQTT Configuration #