ROS2 QT 多线程功能包设计

一个完整的 ROS2 QT 多线程功能包实现，包含可选 GUI 界面、发布者线程、接收者线程和参数服务器交互功能。

功能包结构

plaintext

ros2_qt_multi_thread/
├── CMakeLists.txt
├── package.xml
├── src/
│   ├── main.cpp
│   ├── main_window.cpp
│   ├── main_window.ui
│   ├── ros_node.cpp
│   ├── publisher_thread.cpp
│   ├── subscriber_thread.cpp
│   └── param_manager.cpp
└── include/
    └── ros2_qt_multi_thread/
        ├── main_window.hpp
        ├── ros_node.hpp
        ├── publisher_thread.hpp
        ├── subscriber_thread.hpp
        └── param_manager.hpp

核心文件实现

1. package.xml

xml

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>ros2_qt_multi_thread</name>
  <version>0.0.0</version>
  <description>ROS2 QT Multi-threaded Package with GUI, Publisher, Subscriber and Param Server</description>
  <maintainer email="your@email.com">Your Name</maintainer>
  <license>Apache-2.0</license>
  
  <buildtool_depend>ament_cmake</buildtool_depend>
  <buildtool_depend>ament_cmake_qt</buildtool_depend>
  <depend>rclcpp</depend>
  <depend>std_msgs</depend>
  <depend>Qt5Widgets</depend>
  <depend>Qt5Core</depend>
  
  <build_depend>ament_cmake</build_depend>
  <exec_depend>rclcpp</exec_depend>
  <exec_depend>std_msgs</exec_depend>
  <exec_depend>Qt5Widgets</exec_depend>
  
  <build_type>ament_cmake</build_type>
  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

2. CMakeLists.txt

cmake

cmake_minimum_required(VERSION 3.5)
project(ros2_qt_multi_thread)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# Qt5 配置
find_package(Qt5 COMPONENTS Widgets Core REQUIRED)
set(CMAKE_AUTOMOC ON)
set(CMAKE_AUTORCC ON)
set(CMAKE_AUTOUIC ON)
set(Qt5_LIBRARIES Qt5::Widgets Qt5::Core)

# ROS2 配置
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED)

# 包含目录
include_directories(
  include
  ${Qt5Widgets_INCLUDE_DIRS}
)

# 源文件
set(SOURCES
  src/main.cpp
  src/main_window.cpp
  src/ros_node.cpp
  src/publisher_thread.cpp
  src/subscriber_thread.cpp
  src/param_manager.cpp
)

# UI文件
set(UIS
  src/main_window.ui
)

# 生成可执行文件
add_executable(${PROJECT_NAME}
  ${SOURCES}
  ${UIS}
)

# 链接库
target_link_libraries(${PROJECT_NAME}
  ${Qt5_LIBRARIES}
  rclcpp::rclcpp
  ${std_msgs_TARGETS}
)

# 安装
install(TARGETS
  ${PROJECT_NAME}
  DESTINATION lib/${PROJECT_NAME}
)

ament_package()

3. 头文件

include/ros2_qt_multi_thread/main_window.hpp

cpp

运行

#ifndef ROS2_QT_MULTI_THREAD_MAIN_WINDOW_HPP_
#define ROS2_QT_MULTI_THREAD_MAIN_WINDOW_HPP_

#include <QMainWindow>
#include <QThread>
#include <QTimer>
#include <memory>
#include "ui_main_window.h"
#include "ros_node.hpp"
#include "param_manager.hpp"

namespace ros2_qt_multi_thread
{

class MainWindow : public QMainWindow
{
  Q_OBJECT

public:
  MainWindow(bool use_gui = true, QWidget *parent = nullptr);
  ~MainWindow();

  void showStatusMessage(const QString &message);
  void updatePublisherRate(int rate);
  void updateSubscriberCount(int count);
  void addSubscriberMessage(const QString &message);

public slots:
  void onStartPublishClicked();
  void onStopPublishClicked();
  void onUpdateParamsClicked();
  void onRosSpinOnce();

private:
  Ui::MainWindow ui_;
  bool use_gui_;
  std::shared_ptr<RosNode> ros_node_;
  std::unique_ptr<ParamManager> param_manager_;
  QTimer *ros_timer_;
};

}  // namespace ros2_qt_multi_thread

#endif  // ROS2_QT_MULTI_THREAD_MAIN_WINDOW_HPP_

include/ros2_qt_multi_thread/ros_node.hpp

cpp

运行

#ifndef ROS2_QT_MULTI_THREAD_ROS_NODE_HPP_
#define ROS2_QT_MULTI_THREAD_ROS_NODE_HPP_

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>
#include <QObject>
#include <memory>
#include "publisher_thread.hpp"
#include "subscriber_thread.hpp"
#include "param_manager.hpp"

namespace ros2_qt_multi_thread
{

class RosNode : public rclcpp::Node
{
  Q_OBJECT

public:
  RosNode(const std::string &node_name = "qt_ros2_node");
  ~RosNode() override;

  void startPublishing(int rate);
  void stopPublishing();
  void startSubscribing();
  void setParameters(const std::map<std::string, std::string> &params);
  std::map<std::string, std::string> getParameters();

  std::shared_ptr<PublisherThread> getPublisherThread() { return publisher_thread_; }
  std::shared_ptr<SubscriberThread> getSubscriberThread() { return subscriber_thread_; }
  std::shared_ptr<ParamManager> getParamManager() { return param_manager_; }

signals:
  void statusUpdated(const QString &status);
  void messageReceived(const QString &message);
  void subscriberCountUpdated(int count);

private:
  std::shared_ptr<PublisherThread> publisher_thread_;
  std::shared_ptr<SubscriberThread> subscriber_thread_;
  std::shared_ptr<ParamManager> param_manager_;
};

}  // namespace ros2_qt_multi_thread

#endif  // ROS2_QT_MULTI_THREAD_ROS_NODE_HPP_

include/ros2_qt_multi_thread/publisher_thread.hpp

cpp

运行

#ifndef ROS2_QT_MULTI_THREAD_PUBLISHER_THREAD_HPP_
#define ROS2_QT_MULTI_THREAD_PUBLISHER_THREAD_HPP_

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>
#include <QThread>
#include <QMutex>
#include <atomic>

namespace ros2_qt_multi_thread
{

class PublisherThread : public QThread
{
  Q_OBJECT

public:
  PublisherThread(rclcpp::Node::SharedPtr node);
  ~PublisherThread() override;

  void setRate(int rate);
  void startPublishing();
  void stopPublishing();
  bool isPublishing() const { return is_publishing_; }

signals:
  void statusUpdated(const QString &status);
  void messagePublished(const QString &message);

protected:
  void run() override;

private:
  rclcpp::Node::SharedPtr node_;
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
  mutable QMutex mutex_;
  std::atomic<bool> is_publishing_;
  int publish_rate_;
  int message_count_;
};

}  // namespace ros2_qt_multi_thread

#endif  // ROS2_QT_MULTI_THREAD_PUBLISHER_THREAD_HPP_

include/ros2_qt_multi_thread/subscriber_thread.hpp

cpp

运行

#ifndef ROS2_QT_MULTI_THREAD_SUBSCRIBER_THREAD_HPP_
#define ROS2_QT_MULTI_THREAD_SUBSCRIBER_THREAD_HPP_

#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>
#include <QThread>
#include <QMutex>
#include <atomic>

namespace ros2_qt_multi_thread
{

class SubscriberThread : public QThread
{
  Q_OBJECT

public:
  SubscriberThread(rclcpp::Node::SharedPtr node);
  ~SubscriberThread() override;

  void startSubscribing();
  void stopSubscribing();
  int getMessageCount() const { return message_count_; }

signals:
  void messageReceived(const QString &message);
  void countUpdated(int count);

private:
  void topicCallback(const std_msgs::msg::String::SharedPtr msg);

  rclcpp::Node::SharedPtr node_;
  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscriber_;
  mutable QMutex mutex_;
  std::atomic<bool> is_subscribing_;
  std::atomic<int> message_count_;
};

}  // namespace ros2_qt_multi_thread

#endif  // ROS2_QT_MULTI_THREAD_SUBSCRIBER_THREAD_HPP_

include/ros2_qt_multi_thread/param_manager.hpp

cpp

运行

#ifndef ROS2_QT_MULTI_THREAD_PARAM_MANAGER_HPP_
#define ROS2_QT_MULTI_THREAD_PARAM_MANAGER_HPP_

#include <rclcpp/rclcpp.hpp>
#include <QObject>
#include <map>
#include <string>

namespace ros2_qt_multi_thread
{

class ParamManager : public QObject
{
  Q_OBJECT

public:
  ParamManager(rclcpp::Node::SharedPtr node);
  ~ParamManager() override = default;

  void setParameters(const std::map<std::string, std::string> &params);
  std::map<std::string, std::string> getParameters() const;
  void declareDefaultParameters();

signals:
  void parametersUpdated(const std::map<std::string, std::string> &params);
  void statusUpdated(const QString &status);

private:
  rclcpp::Node::SharedPtr node_;
  mutable std::recursive_mutex param_mutex_;
  std::map<std::string, std::string> current_params_;
};

}  // namespace ros2_qt_multi_thread

#endif  // ROS2_QT_MULTI_THREAD_PARAM_MANAGER_HPP_

4. 源文件实现

src/main.cpp

cpp

运行

#include <QApplication>
#include <rclcpp/rclcpp.hpp>
#include <iostream>
#include "ros2_qt_multi_thread/main_window.hpp"

int main(int argc, char *argv[])
{
  // 初始化ROS2
  rclcpp::init(argc, argv);
  
  // 检查是否需要启动GUI
  bool use_gui = true;
  for (int i = 0; i < argc; ++i) {
    if (std::string(argv[i]) == "--no-gui") {
      use_gui = false;
      break;
    }
  }

  // 初始化QT应用
  QApplication app(argc, argv);
  
  // 创建主窗口
  ros2_qt_multi_thread::MainWindow main_window(use_gui);
  
  // 如果使用GUI，则显示窗口
  if (use_gui) {
    main_window.show();
  } else {
    main_window.showStatusMessage("Running in headless mode");
  }

  // 运行QT事件循环
  int result = app.exec();
  
  // 关闭ROS2
  rclcpp::shutdown();
  return result;
}

src/main_window.cpp

cpp

运行

#include "ros2_qt_multi_thread/main_window.hpp"
#include <QMessageBox>
#include <QTimer>

namespace ros2_qt_multi_thread
{

MainWindow::MainWindow(bool use_gui, QWidget *parent)
  : QMainWindow(parent), use_gui_(use_gui)
{
  // 设置UI
  if (use_gui_) {
    ui_.setupUi(this);
    setWindowTitle("ROS2 QT Multi-Thread");

    // 连接信号槽
    connect(ui_.start_publish_btn, &QPushButton::clicked, 
            this, &MainWindow::onStartPublishClicked);
    connect(ui_.stop_publish_btn, &QPushButton::clicked, 
            this, &MainWindow::onStopPublishClicked);
    connect(ui_.update_params_btn, &QPushButton::clicked, 
            this, &MainWindow::onUpdateParamsClicked);
  }

  // 创建ROS节点
  ros_node_ = std::make_shared<RosNode>();
  
  // 设置参数管理器
  param_manager_ = std::make_unique<ParamManager>(ros_node_);
  param_manager_->declareDefaultParameters();

  // 启动订阅者
  ros_node_->startSubscribing();

  // 连接ROS信号
  connect(ros_node_->getSubscriberThread().get(), &SubscriberThread::messageReceived,
          this, &MainWindow::addSubscriberMessage);
  connect(ros_node_->getSubscriberThread().get(), &SubscriberThread::countUpdated,
          this, &MainWindow::updateSubscriberCount);
  connect(ros_node_.get(), &RosNode::statusUpdated,
          this, &MainWindow::showStatusMessage);

  // 创建ROS定时器，定期处理事件
  ros_timer_ = new QTimer(this);
  connect(ros_timer_, &QTimer::timeout, this, &MainWindow::onRosSpinOnce);
  ros_timer_->start(10);  // 每10ms处理一次
}

MainWindow::~MainWindow()
{
  ros_node_->stopPublishing();
  delete ros_timer_;
}

void MainWindow::showStatusMessage(const QString &message)
{
  if (use_gui_) {
    ui_.status_bar->showMessage(message, 3000);
    ui_.log_textEdit->append(message);
  } else {
    std::cout << "[STATUS] " << message.toStdString() << std::endl;
  }
}

void MainWindow::updatePublisherRate(int rate)
{
  if (use_gui_) {
    ui_.publish_rate_label->setText(QString("Publish Rate: %1 Hz").arg(rate));
  }
}

void MainWindow::updateSubscriberCount(int count)
{
  if (use_gui_) {
    ui_.sub_count_label->setText(QString("Received Messages: %1").arg(count));
  } else {
    std::cout << "Total received messages: " << count << std::endl;
  }
}

void MainWindow::addSubscriberMessage(const QString &message)
{
  if (use_gui_) {
    ui_.sub_messages_textEdit->append(message);
    // 限制显示行数
    while (ui_.sub_messages_textEdit->document()->blockCount() > 100) {
      QTextCursor cursor(ui_.sub_messages_textEdit->document()->firstBlock());
      cursor.select(QTextCursor::BlockUnderCursor);
      cursor.removeSelectedText();
      cursor.deleteChar();
    }
  } else {
    std::cout << "[RECEIVED] " << message.toStdString() << std::endl;
  }
}

void MainWindow::onStartPublishClicked()
{
  if (use_gui_) {
    int rate = ui_.publish_rate_spinBox->value();
    ros_node_->startPublishing(rate);
    showStatusMessage(QString("Starting publishing at %1 Hz").arg(rate));
  }
}

void MainWindow::onStopPublishClicked()
{
  ros_node_->stopPublishing();
  showStatusMessage("Stopped publishing");
}

void MainWindow::onUpdateParamsClicked()
{
  if (use_gui_) {
    std::map<std::string, std::string> params;
    params["publish_topic"] = ui_.pub_topic_lineEdit->text().toStdString();
    params["subscribe_topic"] = ui_.sub_topic_lineEdit->text().toStdString();
    params["message_prefix"] = ui_.msg_prefix_lineEdit->text().toStdString();
    
    ros_node_->setParameters(params);
    showStatusMessage("Parameters updated");
  }
}

void MainWindow::onRosSpinOnce()
{
  // 处理ROS事件
  rclcpp::spin_some(ros_node_);
}

}  // namespace ros2_qt_multi_thread

src/ros_node.cpp

cpp

运行

#include "ros2_qt_multi_thread/ros_node.hpp"

namespace ros2_qt_multi_thread
{

RosNode::RosNode(const std::string &node_name)
  : Node(node_name)
{
  // 创建发布者线程
  publisher_thread_ = std::make_shared<PublisherThread>(shared_from_this());
  
  // 创建订阅者线程
  subscriber_thread_ = std::make_shared<SubscriberThread>(shared_from_this());
  
  // 创建参数管理器
  param_manager_ = std::make_shared<ParamManager>(shared_from_this());
  
  // 连接信号
  connect(publisher_thread_.get(), &PublisherThread::statusUpdated,
          this, &RosNode::statusUpdated);
  connect(subscriber_thread_.get(), &SubscriberThread::messageReceived,
          this, &RosNode::messageReceived);
  connect(subscriber_thread_.get(), &SubscriberThread::countUpdated,
          this, &RosNode::subscriberCountUpdated);
}

RosNode::~RosNode()
{
  stopPublishing();
}

void RosNode::startPublishing(int rate)
{
  publisher_thread_->setRate(rate);
  publisher_thread_->startPublishing();
}

void RosNode::stopPublishing()
{
  if (publisher_thread_ && publisher_thread_->isPublishing()) {
    publisher_thread_->stopPublishing();
  }
}

void RosNode::startSubscribing()
{
  subscriber_thread_->startSubscribing();
}

void RosNode::setParameters(const std::map<std::string, std::string> &params)
{
  param_manager_->setParameters(params);
}

std::map<std::string, std::string> RosNode::getParameters()
{
  return param_manager_->getParameters();
}

}  // namespace ros2_qt_multi_thread

src/publisher_thread.cpp

cpp

运行

#include "ros2_qt_multi_thread/publisher_thread.hpp"
#include <chrono>
#include <thread>

namespace ros2_qt_multi_thread
{

PublisherThread::PublisherThread(rclcpp::Node::SharedPtr node)
  : node_(node), is_publishing_(false), publish_rate_(10), message_count_(0)
{
  // 创建发布者
  std::string topic_name = "qt_publisher_topic";
  if (node_->has_parameter("publish_topic")) {
    node_->get_parameter("publish_topic", topic_name);
  } else {
    node_->declare_parameter("publish_topic", topic_name);
  }
  
  publisher_ = node_->create_publisher<std_msgs::msg::String>(topic_name, 10);
}

PublisherThread::~PublisherThread()
{
  stopPublishing();
  wait();
}

void PublisherThread::setRate(int rate)
{
  QMutexLocker locker(&mutex_);
  publish_rate_ = rate;
}

void PublisherThread::startPublishing()
{
  QMutexLocker locker(&mutex_);
  if (!is_publishing_) {
    is_publishing_ = true;
    if (!isRunning()) {
      start();
    }
  }
}

void PublisherThread::stopPublishing()
{
  QMutexLocker locker(&mutex_);
  is_publishing_ = false;
}

void PublisherThread::run()
{
  while (is_publishing_) {
    // 获取当前参数
    std::string topic_name = "qt_publisher_topic";
    std::string message_prefix = "Message from QT: ";
    
    node_->get_parameter("publish_topic", topic_name);
    node_->get_parameter("message_prefix", message_prefix);
    
    // 如果话题名称已更改，重新创建发布者
    if (publisher_->get_topic_name() != topic_name) {
      publisher_ = node_->create_publisher<std_msgs::msg::String>(topic_name, 10);
      emit statusUpdated(QString("Publisher topic changed to: %1").arg(QString::fromStdString(topic_name)));
    }
    
    // 创建并发布消息
    auto msg = std_msgs::msg::String();
    msg.data = message_prefix + std::to_string(message_count_++);
    publisher_->publish(msg);
    
    emit messagePublished(QString::fromStdString(msg.data));
    emit statusUpdated(QString("Published: %1").arg(QString::fromStdString(msg.data)));
    
    // 控制发布频率
    int rate;
    {
      QMutexLocker locker(&mutex_);
      rate = publish_rate_;
    }
    
    if (rate > 0) {
      std::this_thread::sleep_for(std::chrono::milliseconds(1000 / rate));
    } else {
      std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }
  }
}

}  // namespace ros2_qt_multi_thread

src/subscriber_thread.cpp

cpp

运行

#include "ros2_qt_multi_thread/subscriber_thread.hpp"

namespace ros2_qt_multi_thread
{

SubscriberThread::SubscriberThread(rclcpp::Node::SharedPtr node)
  : node_(node), is_subscribing_(false), message_count_(0)
{
  // 初始化订阅者
  std::string topic_name = "qt_subscriber_topic";
  if (node_->has_parameter("subscribe_topic")) {
    node_->get_parameter("subscribe_topic", topic_name);
  } else {
    node_->declare_parameter("subscribe_topic", topic_name);
  }
  
  createSubscriber(topic_name);
}

SubscriberThread::~SubscriberThread()
{
  stopSubscribing();
  wait();
}

void SubscriberThread::startSubscribing()
{
  QMutexLocker locker(&mutex_);
  if (!is_subscribing_) {
    is_subscribing_ = true;
    if (!isRunning()) {
      start();
    }
  }
}

void SubscriberThread::stopSubscribing()
{
  QMutexLocker locker(&mutex_);
  is_subscribing_ = false;
}

void SubscriberThread::createSubscriber(const std::string &topic_name)
{
  subscriber_ = node_->create_subscription<std_msgs::msg::String>(
    topic_name,
    10,
    std::bind(&SubscriberThread::topicCallback, this, std::placeholders::_1)
  );
}

void SubscriberThread::topicCallback(const std_msgs::msg::String::SharedPtr msg)
{
  message_count_++;
  emit messageReceived(QString::fromStdString(msg->data));
  emit countUpdated(message_count_);
}

void SubscriberThread::run()
{
  std::string current_topic;
  
  while (is_subscribing_ && rclcpp::ok()) {
    // 检查话题是否更改
    std::string topic_name;
    node_->get_parameter("subscribe_topic", topic_name);
    
    if (topic_name != current_topic) {
      current_topic = topic_name;
      createSubscriber(topic_name);
    }
    
    // 处理回调
    rclcpp::spin_some(node_);
    
    // 短暂休眠，降低CPU占用
    std::this_thread::sleep_for(std::chrono::milliseconds(10));
  }
}

}  // namespace ros2_qt_multi_thread

src/param_manager.cpp

cpp

运行

#include "ros2_qt_multi_thread/param_manager.hpp"

namespace ros2_qt_multi_thread
{

ParamManager::ParamManager(rclcpp::Node::SharedPtr node)
  : node_(node)
{
  declareDefaultParameters();
}

void ParamManager::declareDefaultParameters()
{
  std::lock_guard<std::recursive_mutex> lock(param_mutex_);
  
  // 声明默认参数
  node_->declare_parameter("publish_topic", "qt_publisher_topic");
  node_->declare_parameter("subscribe_topic", "qt_subscriber_topic");
  node_->declare_parameter("message_prefix", "Message from QT: ");
  node_->declare_parameter("log_level", "info");
  
  // 初始化当前参数映射
  current_params_["publish_topic"] = "qt_publisher_topic";
  current_params_["subscribe_topic"] = "qt_subscriber_topic";
  current_params_["message_prefix"] = "Message from QT: ";
  current_params_["log_level"] = "info";
}

void ParamManager::setParameters(const std::map<std::string, std::string> &params)
{
  std::lock_guard<std::recursive_mutex> lock(param_mutex_);
  
  for (const auto &[key, value] : params) {
    // 设置ROS参数
    node_->set_parameter(rclcpp::Parameter(key, value));
    current_params_[key] = value;
    
    emit statusUpdated(QString("Set parameter: %1 = %2").arg(QString::fromStdString(key), QString::fromStdString(value)));
  }
  
  emit parametersUpdated(current_params_);
}

std::map<std::string, std::string> ParamManager::getParameters() const
{
  std::lock_guard<std::recursive_mutex> lock(param_mutex_);
  return current_params_;
}

}  // namespace ros2_qt_multi_thread

使用说明

1. 编译功能包

bash

colcon build --packages-select ros2_qt_multi_thread
source install/setup.bash

2. 运行程序

带 GUI 模式

bash

ros2 run ros2_qt_multi_thread ros2_qt_multi_thread

无头模式（无 GUI）

bash

ros2 run ros2_qt_multi_thread ros2_qt_multi_thread --no-gui

3. 功能说明

• 发布者线程：可配置发布频率和消息前缀，通过 ROS 参数动态更改发布话题
• 接收者线程：订阅指定话题，统计接收消息数量，支持动态更改订阅话题
• 参数服务器：管理所有可配置参数，包括话题名称、消息前缀等
• GUI 界面：提供直观的控制界面，显示发布 / 订阅状态和消息内容

4. 关键特性

• 多线程分离：发布者和订阅者在独立线程中运行，避免相互阻塞
• 参数动态更新：支持运行时更改参数，无需重启程序
• 可选 GUI：根据需求选择是否启动图形界面
• 信号槽机制：QT 信号槽与 ROS 回调无缝集成，确保线程安全

这个功能包可以作为 ROS2 与 QT 集成的基础框架，你可以根据实际需求扩展消息类型和功能。