imu_test

#include <ros/ros.h>

#include <tf/transform_broadcaster.h>

#include <turtlesim/Pose.h>

#include <iostream>

#include <signal.h>

#include "ros/ros.h"

#include "std_msgs/String.h"

#include "sanbot_msgs/ctrlMesg.h"

#include "std_srvs/Empty.h"

#include <actionlib/client/simple_action_client.h>

#include <move_base_msgs/MoveBaseAction.h>

#include "nav_msgs/Path.h"

#include <sys/types.h>

#include <sys/stat.h>

#include <fcntl.h>

#include <iostream>  

#include <fstream>  

#include <string>

#include <stdlib.h>

#include "actionlib_msgs/GoalStatusArray.h"

#include <boost/thread.hpp>

#include <geometry_msgs/PoseStamped.h>

#include <sensor_msgs/Imu.h>

class imuTest{

public:

imuTest()

{

ros::NodeHandle n;

// 订阅move_base状态

status_sub_ = n.subscribe<sensor_msgs::Imu>("imu_data", 1, &imuTest::Q_to_E, this);

// 不断检测topic数据

ros::spin();

}

double angleTurn(double angle)

{

double degrees = angle * 180.0 / M_PI; // conversion to degrees

if( degrees < 0 ) 

degrees += 360.0; // convert negative to positive angles

return degrees;

}

void Q_to_E(sensor_msgs::Imu imu)

{

double yaw,pitch,roll;

tf::Matrix3x3(tf::Quaternion(imu.orientation.x,imu.orientation.y,imu.orientation.z,imu.orientation.w)).getRPY(roll,pitch,yaw);

printf("\nQ_to_E:--- roll:[%f] pitch:[%f] yaw:[%f]\n", angleTurn(roll), angleTurn(pitch), angleTurn(yaw));

}

private:

ros::Subscriber status_sub_;

};

int main(int argc, char *argv[])

{

ros::init(argc, argv, "imu_test_node");

imuTest IT;

return 0;

}