ROS学习——IMU惯性测量单元节点的原理与编写(含C++和Python代码)

本文链接：https://blog.csdn.net/2403_88733025/article/details/147726159

ROS 学习

IMU（惯性测量单元）

IMU简介

IMU(Inertial Measurement Unit)即惯性测量单元,是一种安装在机器人内部的一种传感器模块，用于测量机器人的空间姿态

ROS Index中的介绍

在ROS Index中搜索sensor_msgs
找到对应自己ROS版本的软件包
进入Website详情页面
进入右侧Msg/Srv API消息包详情页
进入IMU详情介绍页面，观察官网对IMU的介绍

分析IMU消息类型的格式

在这里插入图片描述

头部Header: 用于记录消息发送的时间戳和消息ID
orientation: 融合消息包其余数据得到的对机器人的空间姿态描述，用于描述机器人相对于空间中XYZ三个轴的偏移量
angular_velocity: 是描述机器人在XYZ三个坐标轴上的旋转速度，即角速度
linear_acceleration: 是描述机器人在在XYZ三个坐标轴上的直行速度的变化速度，即线速度加速度
带有covariance的数据: 协方差矩阵，主要用于后期优化和滤波
学习注释，当要使用消息包中的某个数据时，需要先对协方差矩阵第一个数据的数值进行判断，当数值为-1时，表示数据不存在；当协方差矩阵中所有数据的数值均为0时，表示协方差内容未知

接收IMU消息的话题订阅者节点的编写

IMU消息发布的话题

imu/data_raw(sensor_msgs/Imu): 用于输出矢量加速度和陀螺仪输出的旋转角速度
imu/data(sensor_msgs/Imu): data_raw话题中的数据再加上融合后的四元数姿态描述即orientation中的数据
imu/mag(sensor_msgs/MagneticField): 磁强计输出的磁强数据

代码实现思路

构建一个新的软件包，包名叫做imu_pkg
在软件包中新建一个节点，节点名叫做imu_node。
在节点中，向ROS大管家NodeHandle/rospy申请订阅话题/imu/data,并设置回调函数为IMUCallback()
构建回调函数IMUCallback(), 用来接收和处理IMU数据
使用TF工具将四元数转换成欧拉角
调用ROS_INFO()显示转换后的欧拉角数值

C ++ 节点的实现

imu_node.cpp

#include "ros/ros.h"
#include "tf/tf.h"
#include "sensor_msgs/Imu.h"

inline double toDegrees(double radian) {
    return radian * 180.0 / M_PI; // 弧度转化角度
}

void IMUCallback(const sensor_msgs::Imu& msg)
{
    if (msg.orientation_covariance[0] < 0) {
        ROS_INFO("数据无效");
        return;
    }
    // Imu原始四元数数据 转换为 tf 中的四元数对象
    tf::Quaternion quaternion(
        msg.orientation.x,
        msg.orientation.y,
        msg.orientation.z,
        msg.orientation.w
    );

    double roll, pitch, yaw;
    //通过3x3矩阵的getRPY方法将四元数对象转化成欧拉角，即（滚转角，俯仰角，朝向角）
    tf::Matrix3x3(quaternion).getRPY(roll, pitch, yaw);

    ROS_INFO("\n滚转(Roll): %.2f°\n俯仰(Pitch): %.2f°\n朝向(Yaw): %.2f°",
             toDegrees(roll), toDegrees(pitch), toDegrees(yaw));
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL, ""); // 避免中文输出乱码

    ros::init(argc, argv, "imu_node"); // 初始化节点
    ros::NodeHandle nh;

    ros::Subscriber imu_sub = nh.subscribe("/imu/data", 10, IMUCallback);
    // 订阅消息， 话题， 队伍长度， 回调函数

    ros::spin(); // 定时执行回调函数
    return 0;
}

在CMakeLists.txt文件末尾添加节点声明

add_executable(imu_node src/imu_node.cpp)
target_link_libraries(imu_node
  ${catkin_LIBRARIES}
)

Python 节点的实现

imu_node.py

#!/usr/bin/env python3
#coding = utf-8
import math
import rospy
from tf.transformations import euler_from_quaternion
from sensor_msgs.msg import Imu

# 弧度转化成角度
def toDegree(x):
    return x * 180.0 / math.pi

# 回调函数
def IMUCallback(msg):
    if msg.orientation_covariance[0] < 0:
        rospy.loginfo("数据无效")
        return
    quaternion = [
        msg.orientation.x,
        msg.orientation.y,
        msg.orientation.z,
        msg.orientation.w
    ]
    # 将imu四元数对象转化成欧拉角，即（滚转角，俯仰角，朝向角）
    roll, pitch, yaw = euler_from_quaternion(quaternion)
    rospy.loginfo(f"\n滚转(Roll): {roll:.2f}°\n俯仰(Pitch): {pitch:.2f}°\n朝向(Yaw): {yaw:.2f}°\n")
    
def main():
    rospy.init_node("imu_node")  # 初始化 ROS 节点
    # 话题名称 消息类型 回调函数名 队伍长度
    lidar_sub = rospy.Subscriber("/imu/data", Imu, IMUCallback, queue_size=10)
    rospy.spin()
    
if __name__ == "__main__":
    main()