ROS2订阅和发布

自定义消息类型

打开终端在ros2_ws/src下输入命令 

1
2
3
4
ros2 pkg create status_interfaces \
--build-type ament_cmake \
--dependencies rosidl_default_generators builtin_interfaces \
--license Apache-2.0

上面的指令用于创建一个名叫 status_interfaces 的功能包，并为其添加 builtin_interfaces 和 rosidl_default_generators 两个依赖。builtin_interfaces 是 ROS2 中已有的一个消息接口功能包，可以使用其时间接口Time，表示记录信息的时间。 rosidl_default_generators 用于将定义的消息文件转换为 C++、Python 源码的模块。

ROS2 中话题消息定义文件需要放置在功能包的 msg 目录下，文件名必须以大写字母开头，且只能是大写小写以及数字组成。接着在功能包下创建 msg 目录,并在该目录下新建文件 SystemStatus.msg 。

1
2
3
4
5
6
7
8
builtin_interfaces/Time stamp   # 记录时间戳
string host_name                # 系统名称
float32 cpu_percent             # cpu使用率
float32 memory_percent          # 内存使用率
float32 memory_total            # 内存总量
float32 memory_available        # 剩余有效内存
float64 net_sent                # 网络发送数据总量
float64 net_recv                # 网络接收数据总量

ros2 消息接口支持的数据类型共有9种：

  • bool
  • byte
  • char
  • string
  • float32，float64
  • int8，unint8
  • int16，unint16
  • int32，unint32
  • int64，unint64

定义好数据接口文件后，需要在 CMakeLists.txt 中对该文件进行注册，声明其为消息接口文件，并为其添加 builtin_interfaces 依赖,

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
cmake_minimum_required(VERSION 3.8)
project(status_interface)

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

# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(builtin_interfaces REQUIRED)

# 注册
rosidl_generate_interfaces(${PROJECT_NAME}
  "msg/SystemStatus.msg"
  DEPENDENCIES builtin_interfaces
)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # comment the line when a copyright and license is added to all source files
  set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # comment the line when this package is in a git repo and when
  # a copyright and license is added to all source files
  set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()

在功能包清单文件 package.xml 中添加申明。在package.xml 中添加 member_of_group 是为了声明该功能包是一个消息接口功能包，方便 ROS 2对其做额外处理。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
<?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>status_interface</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="root@todo.todo">root</maintainer>
  <license>Apache-2.0</license>
    
  <member_of_group>rosidl_interface_packages</member_of_group>
    
  <buildtool_depend>ament_cmake</buildtool_depend>

  <depend>rosidl_default_generators</depend>
  <depend>builtin_interfaces</depend>

  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

接着来构建功能包，构建完成后，可以使用的命令来确定消息接口是否构建成功。

1
2
sourse install/setup.bash
ros2 interface show status_interfaces/msg/SystemStatus

显示：

1
2
3
4
5
6
7
8
9
10
11
(ros2) root@GuFan:~/Desktop/ros2_ws# ros2 interface show status_interfaces/msg/SystemStatus
builtin_interfaces/Time stamp   # 记录时间戳
        int32 sec
        uint32 nanosec
string host_name                # 系统名称
float32 cpu_percent             # cpu使用率
float32 memory_percent          # 内存使用率
float32 memory_total            # 内存总量
float32 memory_available        # 剩余有效内存
float64 net_sent                # 网络发送数据总量
float64 net_recv                # 网络接收数据总量

系统信息的获取与发布

在 src 目录下创建 status_publisher 功能包，并在功能包对应目录下创建 sys_status_pub.py 文件， 并添加消息接 口 status_interfaces 和客户端库 relpy 作为其依赖。 

1
2
3
4
ros2 pkg create status_publisher \
--build-type ament_python \
--dependencies rclpу status_interfaces \
--license Apache-2.0

代码如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
import rclpy
from rclpy.node import Node
from status_interface.msg import SystemStatus
import psutil
import platform

class SysStatusPub(Node):
    def __init__(self, node_name):
       super().__init__(node_name)
       self.status_publisher_ = self.create_publisher(
           SystemStatus, 'sys_status', 10)
       self.timer = self.create_timer(1, self.timer_callback)
       
    def timer_callback(self):
        cpu_percent = psutil.cpu_percent()
        memory_info = psutil.virtual_memory()
        net_io_counters = psutil.net_io_counters()
        
        msg = SystemStatus()
        msg.stamp = self.get_clock().now().to_msg()
        msg.host_name = platform.node()
        msg.cpu_percent = cpu_percent
        msg.memory_percent = memory_info.percent
        msg.memory_total = memory_info.total / 1024 / 1024
        msg.memory_available = memory_info.available /1024 /1024
        msg.net_sent = net_io_counters.bytes_sent / 1024 / 1024
        msg.net_recv = net_io_counters.bytes_recv / 1024 / 1024
        
        self.get_logger().info(f"发布：{str(msg)}")
        self.status_publisher_.publish(msg)
        
def main():
    rclpy.init()
    node = SysStatusPub("sys_status_pub")
    rclpy.spin(node)
    rclpy.shutdown()

在 setup.py 中对 sys_status_pub 节点进行注册。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
from setuptools import find_packages, setup

package_name = 'status_publisher'

setup(
    name=package_name,
    version='0.0.0',
    packages=find_packages(exclude=['test']),
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='root',
    maintainer_email='root@todo.todo',
    description='TODO: Package description',
    license='Apache-2.0',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'sys_status_pub = status_publisher.sys_status_pub:main'
        ],
    },
)

运行

1
ros2 run status_publisher sys_status_pub

打开新的终端，依次输入命令：

1
2
source install/setup.bash
ros2 topic echo /sys_status

到这里，便把系统状态获取以及发布部分完成了。

报错记录

ImportError: /root/miniconda3/envs/ros2/bin/../lib/libstdc++.so.6: version GLIBCXX_3.4.30’ not found`

意思：你的 Conda 环境里的 libstdc++.so.6 太旧，没有 GLIBCXX_3.4.30 这个符号（ROS 2 Jazzy 的 rclpy 模块需要）。

原因：ROS 2 Jazzy 本身是基于 系统的 GCC/libstdc++ 编译的。在 conda 环境里运行时优先加载了conda的libstdc++.so.6。结果就是版本对不上。

解决方法：强制使用系统的 libstdc++

1
export LD_PRELOAD=/usr/lib/x86_64-linux-gnu/libstdc++.so.6

订阅消息并通过QT显示

新建功能包

1
2
3
4
ros2 pkg create status_display \
--build-type ament_cmake \
--dependencies rclcpp status_interfaces \
--license Apache-2.0

在 src/status_display/src 下新建 sys_status_display.cpp ，编写代码。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
#include <QApplication>
#include <QLabel>
#include <QString>

#include "rclcpp/rclcpp.hpp"
#include "status_interfaces/msg/system_status.hpp"

using SystemStatus = status_interfaces::msg::SystemStatus;

class SysStatusDisplay : public rclcpp::Node {

public:
    SysStatusDisplay() : Node("sys_status_display"){
        // 创建一个订阅者，订阅话题 sys_status，队列长度为 10
        // 当收到消息时，执行 回调函数（这里用 C++ lambda），更新 QLabel 的文字
        subscription_ = this->create_subscription<SystemStatus>(
        "sys_status", 10, [&](const SystemStatus::SharedPtr msg) -> void {
          label_->setText(get_qstr_from_msg(msg));
        }); 
        // 创建一个空的 SystemStatus 对象，转化成 QString 进行显示
        label_ = new QLabel(get_qstr_from_msg(std::make_shared<SystemStatus>()));
        label_->show();
    }

    QString get_qstr_from_msg(const SystemStatus::SharedPtr msg) {
    std::stringstream show_str;
    show_str
        << "===========系统状态可视化显示工具============\n"
        << "数 据 时 间:\t" << msg->stamp.sec << "\ts\n"
        << "用  户  名:\t" << msg->host_name << "\t\n"
        << "CPU使用率:\t" << msg->cpu_percent << "\t%\n"
        << "内存使用率:\t" << msg->memory_percent << "\t%\n"
        << "内存总大小:\t" << msg->memory_total << "\tMB\n"
        << "剩余有效内存:\t" << msg->memory_available << "\tMB\n"
        << "网络发送量:\t" << msg->net_sent << "\tMB\n"
        << "网络接收量:\t" << msg->net_recv << "\tMB\n"
        << "==========================================";

    return QString::fromStdString(show_str.str());
  }

private:
    rclcpp::Subscription<SystemStatus>::SharedPtr subscription_;
    QLabel* label_;
};


int main(int argc, char* argv[]) {
  // 初始化 ROS2
  rclcpp::init(argc, argv);
  // 初始化 Qt 应用
  QApplication app(argc, argv);
  // 创建节点对象
  auto node = std::make_shared<SysStatusDisplay>();
  // 启动一个新线程来 spin ROS2
  std::thread spin_thread([&]() -> void { rclcpp::spin(node); });
  spin_thread.detach();
  // 运行 Qt 的事件循环（窗口保持运行）
  app.exec();
  // Qt 退出后关闭 ROS2
  rclcpp::shutdown();
  return 0;
}

ROS2 的 spin 会阻塞，所以单独开一个线程执行 rclcpp::spin(node)。进入 Qt 的事件循环，显示窗口。当窗口关闭时，停止 ROS2。

许可协议

本文采用 署名-非商业性使用-相同方式共享 4.0 国际 许可协议，转载请注明出处。