imu_to_pose.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 pose 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 pub = None
00022 
00023 def callback(msg_in):
00024     msg_out = PoseStamped()
00025 
00026     msg_out.header = msg_in.header
00027     msg_out.pose.position.x = 0
00028     msg_out.pose.position.y = 0
00029     msg_out.pose.position.z = 0
00030     
00031     (r, p, y) = tf.transformations.euler_from_quaternion(
00032         [msg_in.orientation.x,
00033          msg_in.orientation.y,
00034          msg_in.orientation.z,
00035          msg_in.orientation.w])
00036 
00037     # mirror orientations
00038     q = tf.transformations.quaternion_from_euler(-r, -p, -y)    
00039     msg_out.pose.orientation.x = q[0]
00040     msg_out.pose.orientation.y = q[1]
00041     msg_out.pose.orientation.z = q[2]
00042     msg_out.pose.orientation.w = q[3]
00043 
00044     pub.publish(msg_out)
00045 
00046 def main():
00047     global pub
00048     rospy.init_node('imu_to_pose', anonymous=True)
00049     pub = rospy.Publisher('imu_pose', PoseStamped)
00050     rospy.Subscriber('imu', Imu, callback)
00051     rospy.spin()
00052 
00053 if __name__ == '__main__':
00054     try:
00055         main()
00056     except rospy.ROSInterruptException:
00057         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