imu_to_tf.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 '''
00003 Created on 18.10.2012
00004 
00005 @author: Martin Guenther <mguenthe@uos.de>
00006 
00007 This ROS node publishes a TF frame for IMU messages (for visualization in
00008 RViz). Note that ROS Fuerte+ has a RViz plugin that can directly visualize IMU
00009 messages.
00010 '''
00011 
00012 import roslib
00013 roslib.load_manifest('imu_recalibration')
00014 
00015 import rospy
00016 
00017 from sensor_msgs.msg import Imu
00018 from geometry_msgs.msg import PoseStamped, Quaternion
00019 import tf
00020 
00021 broadcaster = None
00022 
00023 def callback(msg_in):
00024     (r, p, y) = tf.transformations.euler_from_quaternion(
00025         [msg_in.orientation.x,
00026          msg_in.orientation.y,
00027          msg_in.orientation.z,
00028          msg_in.orientation.w])
00029 
00030     # mirror orientations
00031     q = tf.transformations.quaternion_from_euler(-r, -p, -y)    
00032     rotation = (q[0], q[1], q[2], q[3])
00033 
00034     broadcaster.sendTransform((0, 0, 0), rotation, msg_in.header.stamp, "/imu_frame", msg_in.header.frame_id)
00035     #broadcaster.sendTransform((0, 0, 0), rotation, rospy.Time.now(), "/imu_frame", msg_in.header.frame_id)
00036 
00037 def main():
00038     global broadcaster
00039     rospy.init_node('imu_to_tf', anonymous=True)
00040     broadcaster = tf.TransformBroadcaster()
00041     rospy.Subscriber('imu', Imu, callback)
00042     rospy.spin()
00043 
00044 if __name__ == '__main__':
00045     try:
00046         main()
00047     except rospy.ROSInterruptException:
00048         pass
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


imu_recalibration
Author(s): Jochen Sprickerhof, Martin Günther
autogenerated on Tue May 28 2013 15:07:56