前言
獲取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、可以看到當拉動藍色的圈圈后，機器人一邊轉一邊前進。
_2)
