cd catkin_ws/src/

catkin_create_pkg imu_pkg roscpp rospy sensor_msgs

在src目录下创建，imu_node.cpp

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

void IMUCallback(sensor_msgs::Imu msg){
    if(msg.orientation_covariance[0]<0)
        return;
    tf::Quaternion quaternion(
        msg.orientation.x,
        msg.orientation.y,
        msg.orientation.z,
        msg.orientation.w
    );

    double roll, pitch, yaw;

    tf::Matrix3x3(quaternion).getRPY(roll,pitch,yaw);
    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::NodeHandle  n;
    ros::Subscriber imu_sub = n.subscribe("/imu/data", 10, IMUCallback);
    ros::spin();
    return 0;
}

如果显示红色波浪线 ，也没事。

接着编写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}
)

打开仿真环境

roslaunch wpr_simulation wpb_simple.launch

使用terminator终端工具，ctrl shift+O分屏，输入

rosrun imu_pkg imu_node

使用这个，对机器人进行旋转，可以发现，得到的数据角度也变了。