Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include "ros/ros.h"
00031 #include "tf/transform_broadcaster.h"
00032 #include "sensor_msgs/Imu.h"
00033
00034 std::string p_base_stabilized_frame_;
00035 std::string p_base_frame_;
00036 tf::TransformBroadcaster* tfB_;
00037 tf::StampedTransform transform_;
00038 tf::Quaternion tmp_;
00039
00040 #ifndef TF_MATRIX3x3_H
00041 typedef btScalar tfScalar;
00042 namespace tf { typedef btMatrix3x3 Matrix3x3; }
00043 #endif
00044
00045 void imuMsgCallback(const sensor_msgs::Imu& imu_msg)
00046 {
00047 tf::quaternionMsgToTF(imu_msg.orientation, tmp_);
00048
00049 tfScalar yaw, pitch, roll;
00050 tf::Matrix3x3(tmp_).getRPY(roll, pitch, yaw);
00051
00052 tmp_.setRPY(roll, pitch, 0.0);
00053
00054 transform_.setRotation(tmp_);
00055
00056 transform_.stamp_ = imu_msg.header.stamp;
00057
00058 tfB_->sendTransform(transform_);
00059 }
00060
00061 int main(int argc, char **argv) {
00062 ros::init(argc, argv, ROS_PACKAGE_NAME);
00063
00064 ros::NodeHandle n;
00065 ros::NodeHandle pn("~");
00066
00067 pn.param("base_stabilized_frame", p_base_stabilized_frame_, std::string("base_stabilized"));
00068 pn.param("base_frame", p_base_frame_, std::string("base_link"));
00069
00070 tfB_ = new tf::TransformBroadcaster();
00071 transform_.getOrigin().setX(0.0);
00072 transform_.getOrigin().setY(0.0);
00073 transform_.getOrigin().setZ(0.0);
00074 transform_.frame_id_ = p_base_stabilized_frame_;
00075 transform_.child_frame_id_ = p_base_frame_;
00076
00077 ros::Subscriber imu_subscriber = n.subscribe("imu_topic", 10, imuMsgCallback);
00078
00079 ros::spin();
00080
00081 delete tfB_;
00082
00083 return 0;
00084 }