自动驾驶 人工智能 机器学习 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;

}

相关链接

 您阅读本篇文章共花了：