inholland/ros2-assignments
3
1
Fork 0
generated from wessel/boilerplate
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: Working Version

Browse Source
This commit is contained in:
Vincent Kompjoeteraar Winter
2025-11-06 18:20:58 +01:00
parent fd016f05b6
commit 3fc98d51ed
2 changed files with 129 additions and 85 deletions
Download Patch File Download Diff File Expand all files Collapse all files

176
src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/HardwareInterface.cpp
View File

@@ -8,12 +8,29 @@ HardwareInterface::HardwareInterface(MQTTParameters mqtt_config)
{
RCLCPP_INFO(this->get_logger(), "created HardwareInterface node");
imu_publisher = this->create_publisher<sensor_msgs::msg::Imu>("imu_data", 10);
imu_publisher_ = this->create_publisher<sensor_msgs::msg::Imu>("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::async_client>(
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<callback>(*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::client>(
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());
}
}

38
src/g2_2025_imu_reader_pkg/src/g2_2025_lifecycle_node/nodes/HardwareInterface.hpp
View File

@@ -15,9 +15,12 @@
*/
#pragma once
#include <memory>
#include <thread>
#include <atomic>
#include <string>
#include <nlohmann/json.hpp>
#include <mqtt/client.h>
#include <mqtt/async_client.h>
#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<sensor_msgs::msg::Imu>::SharedPtr imu_publisher_;
serialib serial_;
MQTTParameters mqtt_config_ {};
std::shared_ptr<mqtt::async_client> mqtt_client;
std::shared_ptr<mqtt::callback> mqtt_cb;
MQTTParameters mqtt_config_;
std::unique_ptr<mqtt::client> mqtt_client_;
std::thread read_thread_;
std::atomic_bool reading_{false};
// Publisher to emit parsed IMU messages (from serial or MQTT)
rclcpp::Publisher<sensor_msgs::msg::Imu>::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