2020-07-13

关于turtlebot 的 imu数据的读取，并将四元素转换成了欧拉角。

#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
//#include <std_msgs/Float32.h>
#include <std_msgs/String.h>
#include <Eigen/Core>
#include <Eigen/Geometry>

void acallback(const sensor_msgs::Imu::ConstPtr &msgs){  
    //ROS_INFO("成功输出"); 
    double x = msgs->orientation.x;
    double y = msgs->orientation.y;
    double z = msgs->orientation.z;
    double w = msgs->orientation.w;
    // std::cout<<"  z  "<<z<<"  z "<<std::endl;
    // std::cout<<"  w  "<<w<<"  w "<<std::endl;
    Eigen::Quaterniond quaternion(w,x,y,z);//初始化四元素
    Eigen::AngleAxisd rotation_vector(quaternion);//四元素转旋转向量
    Eigen::Matrix3d rotation_matrix;
    rotation_matrix=quaternion.toRotationMatrix();//四元素转旋转矩阵
    Eigen::Vector3d eulerAngle=quaternion.matrix().eulerAngles(2,1,0);
    double x0 = (eulerAngle(0)/3.1415926)*180;
    // double x1 = (eulerAngle(1)/3.1415926)*180;
    // double x2 = (eulerAngle(2)/3.1415926)*180;

    // int i = 1;
    // if(x0==180&&(i%2==1)){
    //     while (x0<360)
    //     {
    //         x0 = 180 + x0;
    //         i++;
    //     }
    // }
    
    std::cout<<"x0 "<<x0<<std::endl;
    // std::cout<<"x1 "<<x1<<std::endl;
    // std::cout<<"x2 "<<x2<<std::endl;
    std::cout<<"矩阵"<<std::endl;
}

int main(int argc,char **argv){
    ros::init (argc,argv,"listener");
    ros::NodeHandle n;
    ros::Subscriber my_sub = n.subscribe("/mobile_base/sensors/imu_data",2000,acallback);
    ros::Rate rate(20.0);

    while(ros::ok){
        ros::spinOnce();
        rate.sleep();
    }   
    return 0;
}