初识Qos并创建一个订阅Mavros话题的ROS2节点

环境

树莓派4B
ArduCopter 4.0.7
Ubuntu mate 22.04
ROS2 Humble
(本人搭建所使用的软件硬件版本，仅供参考)

上篇文章我们实现了在终端中打印mavros中的imu数据。但是光打印是不够的，本节的任务是创建一个订阅Mavros话题的ROS2节点。

然而事情并不简单。刚开始尝试写了一个基础的订阅者程序imu_node，订阅“/mavros/imu/data”话题并打印加速度数据

结果出现以下警告，没有任何输出。警告提示QoS策略不兼容

[WARN] [1723023136.939607569] [imu_node]: New publisher discovered on topic '/mavros/imu/data', offering incompatible QoS. No messages will be sent to it. Last incompatible policy: RELIABILITY_QOS_POLICY

 运行下面指令查看详细信息

ros2 topic info /mavros/imu/data --verbose

输出结果 

Type: sensor_msgs/msg/Imu

Publisher count: 1

Node name: imu
Node namespace: /mavros
Topic type: sensor_msgs/msg/Imu
Endpoint type: PUBLISHER
GID: 01.0f.80.84.b1.11.a3.7d.00.00.00.00.00.01.b0.03.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: BEST_EFFORT
  History (Depth): UNKNOWN
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

Subscription count: 1

Node name: imu_node
Node namespace: /
Topic type: sensor_msgs/msg/Imu
Endpoint type: SUBSCRIPTION
GID: 01.0f.80.84.5b.13.91.16.00.00.00.00.00.00.12.04.00.00.00.00.00.00.00.00
QoS profile:
  Reliability: RELIABLE
  History (Depth): UNKNOWN
  Durability: VOLATILE
  Lifespan: Infinite
  Deadline: Infinite
  Liveliness: AUTOMATIC
  Liveliness lease duration: Infinite

结果显示发布者发布的话题/mavros/imu/data的QoS策略是Best effort，imu_node（订阅者）的策略是Reliable，根据下表发现二者确实是不兼容的。

一：创建打印加速度的订阅者节点

下面来看下订阅Mavros的正确步骤：

首先进入工作空间下的src文件夹，我的是colcon_ws

cd ~/colcon_ws/src

再创建一个功能包，名为imupy_pkg

ros2 pkg create imupy_pkg --build-type ament_python --dependencies rclpy sensor_msgs --license Apache-2.0

在imupy_pkg中的imupy_pkg文件夹下新建一个imu_node.py

程序参考了这篇文章：

Rviz显示不出数据了！一文搞懂Qos (qq.com)

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Imu  # 导入Imu消息类型
from rclpy.qos import ReliabilityPolicy, QoSProfile

class ImuSubscriber(Node):

    def __init__(self):
        super().__init__('imu_subscriber_node')
        # 创建订阅者对象，并设置为best effort策略
        self.subscriber = self.create_subscription(
            Imu,  # 使用Imu消息类型
            '/mavros/imu/data',
            self.listener_callback,
            QoSProfile(depth=10, reliability=ReliabilityPolicy.BEST_EFFORT)  # 使用best effort策略
        )

    def listener_callback(self, msg):
        # 打印加速度信息
        self.get_logger().info('Accelerometer: x={:.3f}, y={:.3f}, z={:.3f} m/s^2'.format(
            msg.linear_acceleration.x,
            msg.linear_acceleration.y,
            msg.linear_acceleration.z))

def main(args=None):
    rclpy.init(args=args)
    imu_subscriber = ImuSubscriber()
    rclpy.spin(imu_subscriber)
    imu_subscriber.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

在setup.py中的console_scripts的方括号里添加节点

'console_scripts': [
            "imu_node = imupy_pkg.imu_node:main"
        ],

接下来编译： 

cd ~/colcon_ws #或者cd ..
colcon build

 source环境：

source ~/colcon_ws/install/setup.bash
#如果还在colcon_ws文件夹下，执行下面这个也可以
source install/setup.bash

由于我已经将设置工作空间环境参数的source指令添加到终端程序初始化的脚本~/.bashrc文件中，这样每次打开终端就可以自动设置，这里就不需要手动输入上面的指令了。美中不足时需要新开一个终端才能运行新创建的节点。设置方法可以参考：解决ROS工作空间每次使用都要source的问题_ros指令怎么免source-CSDN博客

运行发布者mavros的apm.launch文件

sudo chmod 777 /dev/ttyAMA0
ros2 launch mavros apm.launch

 新打开一个终端运行imu_node订阅者节点

ros2 service call /mavros/set_stream_rate mavros_msgs/StreamRate "{stream_id: 0, message_rate: 10, on_off: true}"
ros2 run imupy_pkg imu_node

二：创建打印姿态角的订阅者节点

在imupy_pkg中的imupy_pkg文件夹下再新建一个rpy_node.py，用来打印三轴姿态角：

为了将sensor_msgs/Imu消息中的四元数转化为欧拉角，需要使用tf2库来处理。这里为了减少添加的依赖，直接使用定义式暴力转化，效果是一样的。

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Imu  # 导入Imu消息类型
from rclpy.qos import ReliabilityPolicy, QoSProfile
import math

class RpyNode(Node):

    def __init__(self):
        super().__init__('rpy_node')
        self.subscriber = self.create_subscription(
            Imu,
            '/mavros/imu/data',
            self.listener_callback,
            QoSProfile(depth=10, reliability=ReliabilityPolicy.BEST_EFFORT)
        )

    def listener_callback(self, msg):
        # 四元数的四个分量
        x, y, z, w = msg.orientation.x, msg.orientation.y, msg.orientation.z, msg.orientation.w
       
        # 使用数学公式从四元数中提取roll, pitch, yaw
        roll = math.atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y))
        pitch = math.asin(2 * (w * y - z * x))
        yaw = math.atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z))

        # 将弧度转换为度
        roll_deg = math.degrees(roll)
        pitch_deg = math.degrees(pitch)
        yaw_deg = math.degrees(yaw)

        # 打印姿态角信息
        self.get_logger().info('Orientation: roll={:.3f}, pitch={:.3f}, yaw={:.3f} degrees'.format(
            roll_deg, pitch_deg, yaw_deg))

def main(args=None):
    rclpy.init(args=args)
    rpy_node = RpyNode()
    rclpy.spin(rpy_node)
    rpy_node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

 在setup.py中的console_scripts的方括号里添加节点

'console_scripts': [
            "imu_node = imupy_pkg.imu_node:main",
            "rpy_node = imupy_pkg.rpy_node:main"
        ],

 注意添加的两个节点之间一定要添加逗号，否则运行会报错。

接下来编译： 

cd ~/colcon_ws
colcon build

 运行发布者mavros的apm.launch文件

#sudo chmod 777 /dev/ttyAMA0
ros2 launch mavros apm.launch

 新打开一个终端运行rpy_node订阅者节点，成功显示三轴姿态角：

#ros2 service call /mavros/set_stream_rate mavros_msgs/StreamRate "{stream_id: 0, message_rate: 10, on_off: true}"
ros2 run imupy_pkg rpy_node