ROS学习记录：用C++实现IMU航向锁定

前言

获取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_pubvoid 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_pubvoid 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、可以看到当拉动蓝色的圈圈后，机器人一边转一边前进。