MQTTDeviceDriver

ref：https://mosquitto.org/api/files/mosquitto-h.html#mosquitto_new

include/mqtt_device_driver/MQTTDeviceDriver.hpp

#pragma once

#include <mosquitto.h>
#include <string>
#include <vector>
#include <rclcpp/rclcpp.hpp>

class MQTTDeviceDriver {
public:
    MQTTDeviceDriver(const std::string& id,
                     const std::string& host,
                     int port,
                     int keepalive,
                     const std::vector<std::string>& topics,
                     rclcpp::Logger logger);
    ~MQTTDeviceDriver();

    void connect();
    void spin_async(); // non-blocking loop

private:
    static void on_connect(struct mosquitto* mosq, void* obj, int rc);
    static void on_message(struct mosquitto* mosq, void* obj, const struct mosquitto_message* message);

    std::string client_id_;
    std::string host_;
    int port_;
    int keepalive_;
    std::vector<std::string> topics_;
    struct mosquitto* mosq_;
    rclcpp::Logger logger_;
};

src/MQTTDeviceDriver.cpp

#include "mqtt_device_driver/MQTTDeviceDriver.hpp"
#include <iostream>
#include <thread>

MQTTDeviceDriver::MQTTDeviceDriver(const std::string& id,
                                   const std::string& host,
                                   int port,
                                   int keepalive,
                                   const std::vector<std::string>& topics,
                                   rclcpp::Logger logger)
    : client_id_(id), host_(host), port_(port), keepalive_(keepalive),
      topics_(topics), mosq_(nullptr), logger_(logger)
{
    mosquitto_lib_init();
    mosq_ = mosquitto_new(client_id_.c_str(), true, this);
    if (!mosq_) throw std::runtime_error("Failed to create Mosquitto instance");

    mosquitto_connect_callback_set(mosq_, on_connect);
    mosquitto_message_callback_set(mosq_, on_message);
}

MQTTDeviceDriver::~MQTTDeviceDriver()
{
    if (mosq_) {
        mosquitto_disconnect(mosq_);
        mosquitto_destroy(mosq_);
    }
    mosquitto_lib_cleanup();
}

void MQTTDeviceDriver::connect()
{
    int rc = mosquitto_connect(mosq_, host_.c_str(), port_, keepalive_);
    if (rc != MOSQ_ERR_SUCCESS)
        throw std::runtime_error("Mosquitto connect failed: " + std::string(mosquitto_strerror(rc)));
}

void MQTTDeviceDriver::spin_async()
{
    std::thread([this]() {
        mosquitto_loop_forever(mosq_, -1, 1);
    }).detach();
}

void MQTTDeviceDriver::on_connect(struct mosquitto* mosq, void* obj, int rc)
{
    auto* self = static_cast<MQTTDeviceDriver*>(obj);
    if (rc == 0) {
        RCLCPP_INFO(self->logger_, "Connected to MQTT broker.");
        for (const auto& topic : self->topics_) {
            mosquitto_subscribe(mosq, nullptr, topic.c_str(), 0);
            RCLCPP_INFO(self->logger_, "Subscribed to %s", topic.c_str());
        }
    } else {
        RCLCPP_ERROR(self->logger_, "Connect failed: %s", mosquitto_strerror(rc));
    }
}

void MQTTDeviceDriver::on_message(struct mosquitto* mosq, void* obj, const struct mosquitto_message* message)
{
    auto* self = static_cast<MQTTDeviceDriver*>(obj);
    std::string topic(message->topic);
    std::string payload(static_cast<char*>(message->payload), message->payloadlen);

    RCLCPP_INFO(self->logger_, "[Received] %s: %s", topic.c_str(), payload.c_str());

    std::string reply_topic = topic + "/reply";
    std::string response = "Reply: " + payload;

    mosquitto_publish(mosq, nullptr, reply_topic.c_str(), response.size(), response.c_str(), 0, false);
}

src/mqtt_node.cpp

#include <rclcpp/rclcpp.hpp>
#include "mqtt_device_driver/MQTTDeviceDriver.hpp"

class MQTTNode : public rclcpp::Node {
public:
    MQTTNode() : Node("mqtt_device_node")
    {
        declare_parameter<std::string>("mqtt_host", "localhost");
        declare_parameter<int>("mqtt_port", 1883);
        declare_parameter<int>("keepalive", 60);
        declare_parameter<std::vector<std::string>>("topics", {"/topic/A", "/topic/B"});

        auto host = get_parameter("mqtt_host").as_string();
        auto port = get_parameter("mqtt_port").as_int();
        auto keepalive = get_parameter("keepalive").as_int();
        auto topics = get_parameter("topics").as_string_array();

        driver_ = std::make_shared<MQTTDeviceDriver>(
            "ros2-mqtt-node", host, port, keepalive, topics, get_logger());

        try {
            driver_->connect();
            driver_->spin_async();
        } catch (const std::exception& e) {
            RCLCPP_ERROR(this->get_logger(), "Exception during MQTT init: %s", e.what());
        }
    }

private:
    std::shared_ptr<MQTTDeviceDriver> driver_;
};

int main(int argc, char* argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<MQTTNode>());
    rclcpp::shutdown();
    return 0;
}

ros2 run mqtt_device_driver mqtt_node

订阅多个话题

std::unordered_map<std::string, std::function<std::string(const std::string& payload)>> callbacks_;

#include <unordered_map>
#include <functional>

// 新增回调map，回调类型：输入payload，输出响应字符串
using ResponseCallback = std::function<std::string(const std::string& payload)>;

class MQTTDeviceDriver {
public:
    // 省略之前参数，新增回调map参数
    MQTTDeviceDriver(const std::string& id,
                    const std::string& host,
                    int port,
                    int keepalive,
                    const std::unordered_map<std::string, ResponseCallback>& topic_callbacks,
                    rclcpp::Logger logger);

    // ...

private:
    std::unordered_map<std::string, ResponseCallback> callbacks_;

    // ...
};

MQTTDeviceDriver::MQTTDeviceDriver(const std::string& id,
                                   const std::string& host,
                                   int port,
                                   int keepalive,
                                   const std::unordered_map<std::string, ResponseCallback>& topic_callbacks,
                                   rclcpp::Logger logger)
    : client_id_(id), host_(host), port_(port), keepalive_(keepalive),
      callbacks_(topic_callbacks), mosq_(nullptr), logger_(logger)
{
    mosquitto_lib_init();
    mosq_ = mosquitto_new(client_id_.c_str(), true, this);
    if (!mosq_) throw std::runtime_error("Failed to create Mosquitto instance");

    mosquitto_connect_callback_set(mosq_, on_connect);
    mosquitto_message_callback_set(mosq_, on_message);
}

void MQTTDeviceDriver::on_connect(struct mosquitto* mosq, void* obj, int rc)
{
    auto* self = static_cast<MQTTDeviceDriver*>(obj);
    if (rc == 0) {
        RCLCPP_INFO(self->logger_, "Connected to MQTT broker.");
        // 订阅所有回调中声明的topic
        for (const auto& kv : self->callbacks_) {
            const std::string& topic = kv.first;
            mosquitto_subscribe(mosq, nullptr, topic.c_str(), 0);
            RCLCPP_INFO(self->logger_, "Subscribed to %s", topic.c_str());
        }
    } else {
        RCLCPP_ERROR(self->logger_, "Connect failed: %s", mosquitto_strerror(rc));
    }
}

void MQTTDeviceDriver::on_message(struct mosquitto* mosq, void* obj, const struct mosquitto_message* message)
{
    auto* self = static_cast<MQTTDeviceDriver*>(obj);
    std::string topic(message->topic);
    std::string payload(static_cast<char*>(message->payload), message->payloadlen);

    RCLCPP_INFO(self->logger_, "[Received] %s: %s", topic.c_str(), payload.c_str());

    auto it = self->callbacks_.find(topic);
    if (it != self->callbacks_.end()) {
        // 调用对应话题的回调，生成响应字符串
        std::string response = it->second(payload);

        std::string reply_topic = topic + "/reply";
        mosquitto_publish(mosq, nullptr, reply_topic.c_str(), response.size(), response.c_str(), 0, false);
        RCLCPP_INFO(self->logger_, "[Published] %s: %s", reply_topic.c_str(), response.c_str());
    } else {
        RCLCPP_WARN(self->logger_, "No callback registered for topic %s", topic.c_str());
    }
}

std::unordered_map<std::string, MQTTDeviceDriver::ResponseCallback> callbacks{
    {"/topic/A", [](const std::string& payload) {
        return "Response for A: " + payload;
    }},
    {"/topic/B", [](const std::string& payload) {
        return "Response for B with fixed format {\"status\":\"ok\", \"data\":\"" + payload + "\"}";
    }},
};

driver_ = std::make_shared<MQTTDeviceDriver>(
    "ros2-mqtt-node", host, port, keepalive, callbacks, get_logger());