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&);