imu_attitude_to_tf_node.cpp
Go to the documentation of this file.
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 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


hector_imu_attitude_to_tf
Author(s): Stefan Kohlbrecher
autogenerated on Mon Jul 15 2013 16:46:51