IMU数据格式:
属于sensor_msgs中的Imu
其中angular_velocity是三个方向的角速度,数据结构是Vector3
linear_acceleration是加速度,数据结构也是Vector3
orientation描述的是机器人的朝向相对于xyz轴的偏移量,数据结构是Quaternion
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;
}