2023/5/20

%%订阅IMU话题四元数数据

1.创建订阅者  订阅imu话题，消息格式：sensor_msgs/Imu

sensor_msgs/Imu消息格式：

订阅者：

ros::Subscriber imu_sub = n.subscribe("/imu/data",10,imucallback);

2.imucallback回调函数处理四元数解算机器人方向

2.1声明个四元数对象并赋值：

tf::Quaternion quaternion(
        msg.orientation.x,
        msg.orientation.y,
        msg.orientation.z,
        msg.orientation.w
    );

2.2通过四元数求滚转角，俯仰角，航向角：

tf::Matrix3x3(quaternion).getRPY(roll, pitch,yaw);

2.3roll pitch yaw单位为弧度制，转换为角度

roll = roll*180/M_PI;

3.打印消息

ROS_INFO("linear_x = %.0f, linear_y = %.0f, linear_z = %.0f",roll,pitch,yaw);