在ROS2中通过Mavros读取、打印并可视化飞控IMU数据

环境

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

1.如果环境没有搭建完成的可以参考这篇文章使用mavros在pixhawk和树莓派4B之间通信_pix mavros-CSDN博客

树莓派上运行的系统版本从下面的网址获得（选择第一个）

Ubuntu MATE Releases - /22.04/arm64/ (ubuntu-mate.org)

2.确保树莓派上的串口有足够的权限：

sudo chmod 777 /dev/ttyAMA0

接下来运行

ros2 launch mavros apm.launch

或者

ros2 run mavros mavros_node fcu_url:=/dev/ttyAMA0:921600

给串口权限这句指令每次运行mavros节点前都要输入，否则会出现：

[mavros_node-1] [WARN] [1723041364.322288939] [mavros.sys]: VER: your FCU don't support AUTOPILOT_VERSION, switched to default capabilities
[mavros_node-1] [INFO] [1723041383.835659031] [mavros.mavros_router]: link[1000] trying to reconnect...
[mavros_node-1] [ERROR] [1723041383.836440051] [mavros.mavros_router]: link[1000] reconnect failed: DeviceError:serial:open: Permission denied

3.打开新的终端输入以下命令，打印imu话题数据

ros2 topic echo /mavros/imu/data

4.如果输入这个命令以后没有输出

那么输入下面这条命令：

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

这个命令等价于ROS1中的

rosservice call /mavros/set_stream_rate 0 10 1

➢第一个参数0表示流的类型为所有传感器数据。

➢第二个参数10是设置的数据流频率为10Hz。

➢第三个参数true/1表示开启数据流。

看到以下结果代表命令生效：

waiting for service to become available...

requester: making request: mavros_msgs.srv.StreamRate_Request(stream_id=0, message_rate=10, on_off=True)

response:

mavros_msgs.srv.StreamRate_Response()

5.再重新打印三轴加速度和角速度：

ros2 topic echo /mavros/imu/data

输出的是包含姿态四元数、角速度、加速度及其协方差矩阵（3*3）的IMU数据

sensor_msgs/Imu消息类型接口定义如下：

std_msgs/Header header

geometry_msgs/Quaternion orientation
float64[9] orientation_covariance # Row major about x, y, z axes

geometry_msgs/Vector3 angular_velocity
float64[9] angular_velocity_covariance # Row major about x, y, z axes

geometry_msgs/Vector3 linear_acceleration
float64[9] linear_acceleration_covariance # Row major x, y z

消息实例：

header:
seq: 111508
stamp:
secs: 2244
nsecs: 228000000
frame_id: “base_link”
orientation:
x: 0.007384428628733207
y: -0.007200897913145637
z: -0.9916137715564253
w: -0.12882411840336733
orientation_covariance: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
angular_velocity:
x: 0.0007936469046398997
y: 0.0006271927268244324
z: -0.0013350683730095625
angular_velocity_covariance: [1.2184696791468346e-07, 0.0, 0.0, 0.0, 1.2184696791468346e-07, 0.0, 0.0, 0.0, 1.2184696791468346e-07]
linear_acceleration:
x: -0.16984868049621582
y: 0.10508567094802976
z: 9.788949966430664
linear_acceleration_covariance: [8.999999999999999e-08, 0.0, 0.0, 0.0, 8.999999999999999e-08, 0.0, 0.0, 0.0, 8.999999999999999e-08]

安装rviz插件

新打开一个终端输入rviz2启动rviz

rviz2

需要提前在rviz2中安装Imu插件

sudo apt install ros-humble-rviz-imu-plugin

点击话题添加按钮后，在弹出窗口的窗口中拉到最下面，选择Imu:

接下来Fixed Frame一栏选择“base_link”

Imu下的Topic一栏选择“/mavros/imu/data”

Topic下的Reliability Policy一栏选择“Best Effort”

这样就可以姿态角实时动态显示了~

后续会在节点中实现mavros的订阅者节点，并实现串口通信，为后续的项目做准备。