nao_imu_conversion.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # SVN $HeadURL$
00004 # SVN $Id$
00005 
00006 
00007 #
00008 # ROS node to from nao's IMU message to a standard sensor_msgs IMU
00009 #
00010 # Copyright 2013 Armin Hornung, University of Freiburg
00011 # http://www.ros.org/wiki/nao
00012 #
00013 # Redistribution and use in source and binary forms, with or without
00014 # modification, are permitted provided that the following conditions are met:
00015 #
00016 #    # Redistributions of source code must retain the above copyright
00017 #       notice, this list of conditions and the following disclaimer.
00018 #    # Redistributions in binary form must reproduce the above copyright
00019 #       notice, this list of conditions and the following disclaimer in the
00020 #       documentation and/or other materials provided with the distribution.
00021 #    # Neither the name of the University of Freiburg nor the names of its
00022 #       contributors may be used to endorse or promote products derived from
00023 #       this software without specific prior written permission.
00024 #
00025 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00026 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00027 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00028 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00029 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00030 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00031 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00032 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00033 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00034 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00035 # POSSIBILITY OF SUCH DAMAGE.
00036 #
00037 
00038 import roslib
00039 roslib.load_manifest('nao_remote')
00040 import rospy
00041 
00042 from sensor_msgs.msg import Imu
00043 
00044 from nao_msgs.msg import TorsoIMU
00045 from tf import transformations as tf
00046 
00047 pub = None
00048 
00049 def handleIMU(data):
00050   rospy.logdebug("TorsoIMU received for conversion: %f %f", data.angleX, data.angleY)
00051   imu_msg = Imu()
00052   q = tf.quaternion_from_euler(data.angleX, data.angleY, 0.0)
00053   imu_msg.header = data.header
00054   imu_msg.orientation.x = q[0]
00055   imu_msg.orientation.y = q[1]
00056   imu_msg.orientation.z = q[2]
00057   imu_msg.orientation.w = q[3]
00058   
00059   pub.publish(imu_msg)
00060 
00061   
00062 
00063 if __name__ == '__main__':
00064 
00065   rospy.init_node('nao_imu_conversion')
00066   pub = rospy.Publisher('imu', Imu)
00067   rospy.Subscriber("torso_imu", TorsoIMU, handleIMU, queue_size=50)
00068   
00069   
00070   rospy.spin()
00071   exit(0)
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Friends


nao_remote
Author(s): Armin Hornung
autogenerated on Sat Oct 26 2013 11:02:42