ROS学习记录：获取IMU数据的C++节点

前言

IMU消息包数据格式
继了解了IMU消息包的数据格式后，接下来便是掌握如何通过C++获取IMU的数据

一、IMU设备话题

一般的应用里，通常订阅的是第二个话题

二、实现思路

编写一个节点，订阅IMU数据话题，就能获取到IMU数据了

三、实现步骤

四、开始操作

1、打开一个终端，输入cd catkin_ws1/src/ 进入工作空间

2、输入 catkin_create_pkg imu_pkg roscpp rospy sensor_msgs 创建软件包，包名叫做imu_pkg，依赖项里加入了包含IMU消息格式的sensor_msgs

3、输入code . 打开VScode

4、在刚刚创建的imu_pkg软件包下的src目录下新建一个节点文件

5、名字叫做imu_node.cpp

6、在imu_node.cpp文件中写入如下代码

#include "ros/ros.h" //包含ros头文件
#include "sensor_msgs/Imu.h"  //包含sensor_msgs/Imu消息类型头文件
#include "tf/tf.h"//用于使用TF工具，将四元素转换为欧拉角

void IMUCallback(sensor_msgs::Imu msg)   //IMU消息回调函数
{
    if(msg.orientation_covariance[0]<0)   //检查协方差，确保数据有效性
        return;                            //如果协方差小于0，数据无效，直接返回
    tf::Quaternion quaternion(            //创建四元素
        msg.orientation.x,              //从IMU消息中获取四元素数据
        msg.orientation.y,
        msg.orientation.z,
        msg.orientation.w
    );

    double roll,pitch,yaw;           //定义滚转、俯仰、偏航角

    tf::Matrix3x3(quaternion).getRPY(roll,pitch,yaw);// 利用TF库将四元数转换为欧拉角
    roll = roll*180/M_PI;       // 弧度转换为角度
    pitch = pitch*180/M_PI;     // 弧度转换为角度
    yaw = yaw*180/M_PI;         // 弧度转换为角度

    ROS_INFO("滚转= %.0f  俯仰= %.0f 偏航= %.0f",roll,pitch,yaw);    // 打印欧拉角
}

int main(int argc, char  *argv[])   // 主函数
{
    setlocale(LC_ALL,"");           // 设置本地区域选项
    ros::init(argc,argv,"imu_node");        // 初始化ROS节点

    ros::NodeHandle n;              // 创建节点句柄
    ros::Subscriber imu_sub = n.subscribe("/imu/data",10,IMUCallback); // 创建imu_sub订阅者，订阅IMU数据消息

    ros::spin();// 进入自发循环，阻塞程序直至节点关闭

    return 0;
}

五、给节点设置编译规则

1、打开imu_pkg里的CMakeLists.txt文件

2、CMakeLists.txt文件末尾添加如下代码

add_executable(imu_node src/imu_node.cpp)
add_dependencies(imu_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(imu_node
  ${catkin_LIBRARIES}
)

3、Ctrl+Shift+B进行编译（记得先保存再编译），编译成功

六、运行节点程序

1、再打开一个终端，输入cd ~/catkin_ws1进入工作空间

2、输入source ./devel/setup.bash，设置ROS的环境变量，以便在当前的终端窗口中正确运行ROS软件包。

3、输入roslaunch wpr_simulation wpb_simple.launch启动机器人仿真环境

4、机器人仿真环境启动成功

5、再打开一个终端，进入工作空间

6、输入source ./devel/setup.bash，设置ROS的环境变量，以便在当前的终端窗口中正确运行ROS软件包。

7、输入rosrun imu_pkg imu_node,运行imu_node节点程序

8、可以看到当前三个欧拉角都是0

9、回到Gazebo机器人仿真环境，先点旋转按钮，再点机器人

10、随意拖动旋转机器人，可以看到欧拉角相应的变化,至此，成功的获取了机器人的姿态信息