fix(lifcycle & hw): fixed serial read and better mqtt conf funcs

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<mqtt::async_client>(SERVER_ADDRESS, CLIENT_ID);
@@ -63,9 +85,15 @@ void HardwareInterface::mqtt_listener() {
     if (!mqtt_cb) {
         mqtt_cb = std::make_shared<callback>();
     }
+    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);

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 <random>
 #include <vector>
 #include <chrono>
+#include <thread>
+#include <atomic>
 #include <termios.h>
 #include <fcntl.h>
 #include <unistd.h>
@@ -25,6 +32,7 @@
 #include <mqtt/client.h>
 #include <mqtt/async_client.h>
 
+#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::async_client> mqtt_client;
     std::shared_ptr<mqtt::callback> mqtt_cb;
 
+    std::thread read_thread_;
+    std::atomic_bool reading_{false};
 
 };
 }  // namespace assignments::two::g2_2025_lifecycle_node

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<std_msgs::msg::String>("imu_data", 10);
+    imu_publisher_ = this->create_publisher<sensor_msgs::msg::Imu>("imu_data", 10);
     RCLCPP_INFO(this->get_logger(), "IMU Publisher has been created");
 
     device_path_ = this->declare_parameter<std::string>("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&) {

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<std_msgs::msg::String>::SharedPtr imu_publisher;
-
+    rclcpp::Publisher<sensor_msgs::msg::Imu>::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<HardwareInterface> 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&);