前言
获取IMU数据的C++节点
在了解了如何获取到IMU的姿态信息（链接在上面）后，接下来尝试实现让一个节点在订阅IMU数据的时候，还能发布运动控制指令，使机器人能对姿态变化做出反应，达到一个航向锁定的效果。
一、实现步骤
二、开始操作
1、打开一个终端，输入cd ~/catkin_ws1/src，进入工作空间
2、输入 code . 打开VScode
3、在VScode中打开上一篇文章编写的imu_node.cpp， 上一篇文章链接已经放在开头
4、在imu_node.cpp中写入如下代码
#include "ros/ros.h" //包含ros头文件
#include "sensor_msgs/Imu.h" //包含sensor_msgs/Imu消息类型头文件
#include "tf/tf.h"//用于使用TF工具，将四元素转换为欧拉角
#include "geometry_msgs/Twist.h"//引入速度消息包的头文件
ros::Publisher vel_pub;//定义一个发布对象vel_pub
void IMUCallback(sensor_msgs::Imu msg) //IMU消息回调函数
{
if(msg.orientation_covariance[0]<0) //检查协方差，确保数据有效性
return; //如果协方差小于0，数据无效，直接返回
tf::Quaternion quaternion( //创建四元素
msg.orientation.x, //从IMU消息中获取四元素数据
msg.orientation.y,
msg.orientation.z,
msg.orientation.w
);
double roll,pitch,yaw; //定义滚转、俯仰、偏航角
tf::Matrix3x3(quaternion).getRPY(roll,pitch,yaw);// 利用TF库将四元数转换为欧拉角
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;// 创建Twist类型消息对象用于发布速度指令
vel_cmd.angular.z = diff_angle*0.01;// 计算角速度
vel_pub.publish(vel_cmd);// 发布速度指令
}
int main(int argc, char *argv[]) // 主函数
{
setlocale(LC_ALL,""); // 设置本地区域选项
ros::init(argc,argv,"imu_node"); // 初始化ROS节点
ros::NodeHandle n; // 创建节点句柄
ros::Subscriber imu_sub = n.subscribe("/imu/data",10,IMUCallback); // 创建imu_sub订阅者，订阅IMU数据消息
vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel",10);//创建速度指令发布器
ros::spin();// 进入自发循环，阻塞程序直至节点关闭
return 0;
}
5、按CTRL+S进行保存，再按CTRL+SHIFT进行编译，编译成功
6、在终端中输入cd ~/catkin_ws1，进入工作空间
7、再输入source ./devel/setup.bash，设置ROS的环境变量，以便在当前的终端窗口中正确运行ROS软件包。
8、再输入roslaunch wpr_simulation wpb_simple.launch，启动机器人仿真环境
9、再打开一个终端，进入工作空间后输入source ./devel/setup.bash，设置环境变量
10、输入rosrun imu_pkg imu_node运行刚刚我们更新的节点
11、可以看到机器人偏航角锁定在了90度
12、这时因为在代码中，我们设定了目标偏航角为90度，如果想要机器人朝向其它角度可以自行修改
13、点这个旋转按钮，对机器人进行旋转，拖动这个蓝色的圈圈，机器人转动后，一松开鼠标，可以发现机器人会自行转回去直至目标角度90度，这便是偏航角锁定。
ROS机器人偏航角锁定演示
14、回到节点代码，在这里加上这一句代码，给机器一个前进的速度，可以预想到，机器人会一边前进一边转弯。
#include "ros/ros.h" //包含ros头文件
#include "sensor_msgs/Imu.h" //包含sensor_msgs/Imu消息类型头文件
#include "tf/tf.h"//用于使用TF工具，将四元素转换为欧拉角
#include "geometry_msgs/Twist.h"//引入速度消息包的头文件
ros::Publisher vel_pub;//定义一个发布对象vel_pub
void IMUCallback(sensor_msgs::Imu msg) //IMU消息回调函数
{
if(msg.orientation_covariance[0]<0) //检查协方差，确保数据有效性
return; //如果协方差小于0，数据无效，直接返回
tf::Quaternion quaternion( //创建四元素
msg.orientation.x, //从IMU消息中获取四元素数据
msg.orientation.y,
msg.orientation.z,
msg.orientation.w
);
double roll,pitch,yaw; //定义滚转、俯仰、偏航角
tf::Matrix3x3(quaternion).getRPY(roll,pitch,yaw);// 利用TF库将四元数转换为欧拉角
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;// 创建Twist类型消息对象用于发布速度指令
vel_cmd.angular.z = diff_angle*0.01;// 计算角速度
vel_cmd.linear.x = 0.1;//给机器人x轴方向一个0.1m/s的线速度
vel_pub.publish(vel_cmd);// 发布速度指令
}
int main(int argc, char *argv[]) // 主函数
{
setlocale(LC_ALL,""); // 设置本地区域选项
ros::init(argc,argv,"imu_node"); // 初始化ROS节点
ros::NodeHandle n; // 创建节点句柄
ros::Subscriber imu_sub = n.subscribe("/imu/data",10,IMUCallback); // 创建imu_sub订阅者，订阅IMU数据消息
vel_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel",10);//创建速度指令发布器
ros::spin();// 进入自发循环，阻塞程序直至节点关闭
return 0;
}
15、CTRL+S保存后，在按CTRL+SHIFT编译，编译成功
16、先按CTRL+Z停止运行刚刚的节点，在重新运行节点
17、可以看到当拉动蓝色的圈圈后，机器人一边转一边前进。
