当前位置：首页 > news >正文

2.8 获取IMU数据与航向锁定

news 2025/6/18 15:36:04

1.获取IMU数据

进入src目录

cd  catkin_ws/src/

建立新包imu_pkg，并在对应src目录增加imu_node.cpp

catkin_create_pkg imu_pkg roscpp rospy sensor_msgs

在imu_node.cpp键入代码

#include "ros/ros.h"
#include "sensor_msgs/Imu.h"
#include "tf/tf.h"void IMUCallback(sensor_msgs::Imu msg)
{if(msg.orientation_covariance[0]<0)return;tf::Quaternion quaternion(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.subscribe("/imu/data/",10,IMUCallback);ros::spin();return 0;
}

因为设计旋转，欧拉角存在万向锁问题，所以这里引入4元数来解决问题

#include "tf/tf.h"

启动仿真环境

roslaunch wpr_simulation wpb_simple.launch

2.保持航向锁定

#include "ros/ros.h"
#include "sensor_msgs/Imu.h"
#include "tf/tf.h"
#include "geometry_msgs/Twist.h"ros::Publisher vel_pub;void IMUCallback(sensor_msgs::Imu msg)
{if(msg.orientation_covariance[0]<0)return;tf::Quaternion quaternion(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.publish(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.subscribe("/imu/data/",10,IMUCallback);vel_pub=n.advertise<geometry_msgs::Twist>("/cmd_vel",10);ros::spin();return 0;
}