ros|实现IMU航向锁定

 该代码块实现机器人强制面向z轴90度方向的功能，无论怎么改变机器人方向，他都会自我调整方向。

double target_yaw=90;
double diff_angle = target_yaw - yaw;
geometry_msgs::Twist vel_cmd;
vel_cmd.angular.z=diff_angle*0.01;
vel_pub.publish(vel_cmd);