$search
00001 //================================================================================================= 00002 // Copyright (c) 2012, Stefan Kohlbrecher, TU Darmstadt 00003 // All rights reserved. 00004 00005 // Redistribution and use in source and binary forms, with or without 00006 // modification, are permitted provided that the following conditions are met: 00007 // * Redistributions of source code must retain the above copyright 00008 // notice, this list of conditions and the following disclaimer. 00009 // * Redistributions in binary form must reproduce the above copyright 00010 // notice, this list of conditions and the following disclaimer in the 00011 // documentation and/or other materials provided with the distribution. 00012 // * Neither the name of the Simulation, Systems Optimization and Robotics 00013 // group, TU Darmstadt nor the names of its contributors may be used to 00014 // endorse or promote products derived from this software without 00015 // specific prior written permission. 00016 00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY 00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 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_frame")); 00068 pn.param("base_frame", p_base_frame_, std::string("base_frame")); 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 }