IMU data in ROS

IMU data format:

Belongs to Imu in sensor_msgs

 Where angular_velocity is the angular velocity in three directions, and the data structure is Vector3

linear_acceleration is the acceleration, and the data structure is also Vector3

orientation describes the offset of the robot's orientation relative to the xyz axis, and the data structure is Quaternion

 Topics that IMU will publish:

Subscribe to IMU data

#including<ros/ros.h>
#including<sensor_msgs/Imu.h>
#including<tf/tf.h>

void ImuCallback(sensor_msgs::Imu msg)
{
    if(msg.orientation_convariance[0]<0){
        return;
    }
    
    tf::Quaternion quaternion(   //这是利用tf工具将四元数转换
        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.subscriber("/imu/data",10,ImuCallback);
    
    ros::spin();
    return 0;
}

Set the heading angle and turn to the heading angle according to the IMU data

#including<ros/ros.h>
#including<sensor_msgs/Imu.h>
#including<tf/tf.h>

ros::Publisher cmd_pub;

void ImuCallback(sensor_msgs::Imu msg)
{
    if(msg.orientation_convariance[0]<0){
        return;
    }
    
    tf::Quaternion quaternion(   //这是利用tf工具将四元数转换
        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);

    double target_yaw = 90;
    double diff_angle = target_yaw - yaw;
    geometry_msgs::Twist vel_cmd;
    vel_cmd.angular.z = diff_angle * 0.01;
    vel_cmd.linear.x=0.1;
    vel_pub.pub(vel_cmd);
    
}

int main(int argc,char *argv[])
{
    setlocale(LC_ALL,"")
    ros::init(argc,argv,"imu_node")
    
    ros::NodeHandle n;
    ros::Subscriber imu_sub = n.subscriber("/imu/data",10,ImuCallback);
    cmd_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel",10);
    
    ros::spin();
    return 0;
}