feat: Working v2

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

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
 ```
 
 ---

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)

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")

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::client>(
             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());
     }
 }
 

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

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<HardwareInterface>(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;

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<sensor_msgs::msg::Imu>::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