2.8 获取IMU数据与航向锁定

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;
}