Go to the documentation of this file.00001 #include <ros/ros.h>
00002 #include <sensor_msgs/Imu.h>
00003 #include <tf/tf.h>
00004
00005
00006 void imuCallback(const sensor_msgs::Imu::ConstPtr &msg);
00007
00008 int main(int argc, char** argv) {
00009 ros::init(argc,argv, "check_imu_node");
00010 ros::NodeHandle handle;
00011 ros::Subscriber sub = handle.subscribe<sensor_msgs::Imu>("imu_AGQ", 10, imuCallback);
00012 ros::spin();
00013 return 0;
00014 }
00015
00016 void imuCallback(const sensor_msgs::Imu::ConstPtr &msg) {
00017 tf::Quaternion q;
00018 tf::quaternionMsgToTF(msg->orientation,q);
00019 tf::Matrix3x3 m(q);
00020 double roll, pitch, yaw;
00021 roll = pitch = yaw = 0;
00022 m.getRPY(roll, pitch, yaw);
00023 char buff[1024] = {'\0'};
00024 sprintf(buff, "Roll: %.4f Pitch: %.4f Yaw: %.4f", roll * 180 / M_PI ,pitch * 180 / M_PI,yaw * 180 / M_PI);
00025 ROS_INFO("%s",buff);
00026 }