ROS中IMU数据

IMU数据格式：

属于sensor_msgs中的Imu

 其中angular_velocity是三个方向的角速度，数据结构是Vector3

linear_acceleration是加速度，数据结构也是Vector3

orientation描述的是机器人的朝向相对于xyz轴的偏移量，数据结构是Quaternion

 IMU会发布的话题：

订阅IMU数据

#including
#including
#including

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;
}

设置朝向角，根据IMU数据向朝向角转动

#including
#including
#including

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("/cmd_vel",10);
    
    ros::spin();
    return 0;
}