From 3fc98d51ed66853a2a24062bd4f6f3321ea2cb90 Mon Sep 17 00:00:00 2001 From: Vincent Winter Date: Thu, 6 Nov 2025 18:20:58 +0100 Subject: [PATCH] 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