node.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Copyright (c) 2012, Tang Tiong Yew
00004 # All rights reserved.
00005 #
00006 # Redistribution and use in source and binary forms, with or without
00007 # modification, are permitted provided that the following conditions are met:
00008 #
00009 #    * Redistributions of source code must retain the above copyright
00010 #      notice, this list of conditions and the following disclaimer.
00011 #    * Redistributions in binary form must reproduce the above copyright
00012 #      notice, this list of conditions and the following disclaimer in the
00013 #      documentation and/or other materials provided with the distribution.
00014 #    * Neither the name of the Willow Garage, Inc. nor the names of its
00015 #      contributors may be used to endorse or promote products derived from
00016 #       this software without specific prior written permission.
00017 #
00018 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00021 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00022 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00023 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00024 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00025 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00026 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00027 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00028 # POSSIBILITY OF SUCH DAMAGE.
00029 
00030 import roslib; roslib.load_manifest('razor_imu_9dof')
00031 import rospy
00032 
00033 import serial
00034 import string
00035 import math
00036 
00037 from time import time
00038 from sensor_msgs.msg import Imu
00039 from razor_imu_9dof.msg import RazorImu
00040 import tf
00041 
00042 grad2rad = 3.141592/180.0
00043 
00044 rospy.init_node("node")
00045 pub = rospy.Publisher('imu', Imu)
00046 pubRaw = rospy.Publisher('imuRaw', RazorImu)
00047 
00048 imuMsg = Imu()
00049 imuRawMsg = RazorImu()
00050 imuMsg.orientation_covariance = [999999 , 0 , 0,
00051 0, 9999999, 0,
00052 0, 0, 999999]
00053 imuMsg.angular_velocity_covariance = [9999, 0 , 0,
00054 0 , 99999, 0,
00055 0 , 0 , 0.02]
00056 imuMsg.linear_acceleration_covariance = [0.2 , 0 , 0,
00057 0 , 0.2, 0,
00058 0 , 0 , 0.2]
00059 
00060 default_port='/dev/ttyUSB1'
00061 port = rospy.get_param('device', default_port)
00062 # Check your COM port and baud rate
00063 ser = serial.Serial(port=port,baudrate=57600, timeout=1)
00064 
00065 #f = open("Serial"+str(time())+".txt", 'w')
00066 
00067 roll=0
00068 pitch=0
00069 yaw=0
00070 rospy.sleep(5) # Sleep for 8 seconds to wait for the board to boot then only write command.
00071 ser.write('#ox' + chr(13)) # To start display angle and sensor reading in text 
00072 while 1:
00073     line = ser.readline()
00074     line = line.replace("#YPRAMG=","")   # Delete "#YPR="
00075     #f.write(line)                     # Write to the output log file
00076     words = string.split(line,",")    # Fields split
00077     if len(words) > 2:
00078         try:
00079             yaw = float(words[0])*grad2rad
00080             pitch = -float(words[1])*grad2rad
00081             roll = -float(words[2])*grad2rad
00082             
00083             # Publish message
00084             imuMsg.linear_acceleration.x = float(words[3])
00085             imuMsg.linear_acceleration.y = float(words[4])
00086             imuMsg.linear_acceleration.z = float(words[5])
00087             
00088             imuMsg.angular_velocity.x = float(words[9])
00089             imuMsg.angular_velocity.y = float(words[10])
00090             imuMsg.angular_velocity.z = float(words[11])
00091         except Exception as e:
00092             print e
00093             
00094         q = tf.transformations.quaternion_from_euler(roll,pitch,yaw)
00095         imuMsg.orientation.x = q[0]
00096         imuMsg.orientation.y = q[1]
00097         imuMsg.orientation.z = q[2]
00098         imuMsg.orientation.w = q[3]
00099         imuMsg.header.stamp= rospy.Time.now()
00100         imuMsg.header.frame_id = 'base_link'
00101         pub.publish(imuMsg)
00102             
00103         # Publish Raw message from Razor board
00104         imuRawMsg.yaw = yaw
00105         imuRawMsg.pitch = pitch
00106         imuRawMsg.roll = roll
00107         pubRaw.publish(imuRawMsg)
00108         
00109 ser.close
00110 #f.close


razor_imu_9dof
Author(s): Tang Tiong Yew
autogenerated on Mon Jan 6 2014 11:37:48