ros（noetic）——IMU(学习笔记）

28. ROS 中的 IMU 惯性测量单元消息包_哔哩哔哩_bilibili

一、IMU的消息包格式

header消息头，包含了时间戳和坐标系编号等信息

orientation空间姿态信息，采用四元数（Quaternion）方式表示

angular_velocity角速度信息，采用Vector3表示（和速度一样）

linear_acceleration矢量加速度信息，采用Vector3表示，表示在x/y/z轴方向上的加速度

后三个消息后面都跟了一个协方差矩阵

二、IMU设备发布的话题

其中磁强计数据只有九轴IMU包含。一般应用中订阅第二个话题。

使用rostopic echo /imu/data --noarr命令查看第二个话题中消息包格式如下

三、订阅者节点获取IMU数据的python实现

1.进入工作空间catkin_ws/src/，使用catkin_create_pkg命令创建软件包，创建后回退到catkin_ws目录，使用catkin_make命令进行编译，将新创建的软件包加入ros的包列表里。

~/catkin_ws/src$ catkin_create_pkg imu_pkg roscpp rospy sensor_msgs
~/catkin_ws/src$ cd..
~/catkin_ws$ catkin_make

2.在vscode中找到imu_pkg文件夹，新建scripts子目录，并在其中新建imu_node.py文件，在此文件中编写代码，编写完成后，在终端中打开scripts子目录，通过chmod +x imu_node.py命令为文件添加可执行权限。

#!/usr/bin/env python3
# coding=utf-8


import rospy
from sensor_msgs.msg import Imu  # 引入IMU的消息格式
from tf.transformations import euler_from_quaternion  # 引入四元数转欧拉角的函数
import math # 引入数学库,需要用到PI

def imu_callback(msg):
    if msg.orientation_covariance[0] < 0: #如果协方差矩阵的第一个元素小于0，说明IMU中没有数据，直接返回
        return
    quaternion = [
        msg.orientation.x,
        msg.orientation.y,
        msg.orientation.z,
        msg.orientation.w
    ] #将消息包里的四元数重新组合成tf的四元数对象格式
    (roll, pitch, yaw) = euler_from_quaternion(quaternion) #将四元数转化为欧拉角

    roll = roll*180/math.pi #将弧度转化为角度
    pitch = pitch*180/math.pi
    yaw = yaw*180/math.pi

    rospy.loginfo("滚转: %.0f, 俯仰: %.0f, 偏航: %f", roll, pitch, yaw) #打印欧拉角


if __name__ == "__main__":

    rospy.init_node('imu_node') #初始化节点
    imu_sub = rospy.Subscriber("/imu/data", Imu, imu_callback, queue_size=10)
    rospy.spin()

四、IMU航向锁定的python实现

在前面的程序基础上修改，加入运动控制的部分

1.通过rospy发布速度话题/cmd_vel并创建发布者节点vel_pub（要记得import速度话题的消息格式）

from geometry_msgs.msg import Twist  # 引入Twist消息格式

vel_pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10) 

2.设定一个目标朝向角，当机器人目前航向角和目标朝向角不同时，控制机器人转向目标朝向角

（记得在回调函数中把vel_pub设定为全局变量，否则每次调用回调函数都会新建一个未关联/cmd_vel话题的新发布者变量，导致发布失败）

target_yaw = 0 # 目标偏航角
def imu_callback(msg):
    global vel_pub
    {
    #回调函数里关于获取imu数据的内容
    }
    vel_cmd = Twist()
    vel_cmd.angular.z = (target_yaw - yaw) * 0.01 #计算偏航角的差值
    vel_cmd.linear.x = 0.3

    vel_pub.publish(vel_cmd) #发布速度信息