ROS の IMU データ

IMU データ形式:

sensor_msgs の Imu に属します

 ここで、angular_velocity は 3 方向の角速度であり、データ構造は Vector3 です。

Linear_acceleration は加速度で、データ構造も Vector3 です。

方向は、xyz 軸に対するロボットの方向のオフセットを表し、データ構造はクォータニオンです。

 IMU が公開するトピック:

IMUデータを購読する

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

機首角度を設定し、IMU データに従って機首角度に旋回します。

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