display_3D_visualization.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 rospy
00031 from visual import *
00032 import math
00033 import wx
00034 
00035 from sensor_msgs.msg import Imu
00036 from tf.transformations import euler_from_quaternion
00037 
00038 rad2degrees = 180.0/math.pi
00039 precision = 2 #round to this number of digits
00040 yaw_offset = 0 #used to align animation upon key press
00041 
00042 
00043 #Create shutdown hook to kill visual displays
00044 def shutdown_hook():
00045     #print "Killing displays"
00046     wx.Exit()
00047 
00048 #register shutdown hook
00049 rospy.on_shutdown(shutdown_hook)
00050 
00051 
00052 # Main scene
00053 scene=display(title="9DOF Razor IMU Main Screen")
00054 scene.range=(1.3,1.3,1.3)
00055 scene.forward = (1,0,-0.25)
00056 # Change reference axis (x,y,z) - z is up
00057 scene.up=(0,0,1)
00058 scene.width=500
00059 scene.height=500
00060 
00061 # Second scene (Roll, Pitch, Yaw)
00062 scene2 = display(title='9DOF Razor IMU Roll, Pitch, Yaw',x=550, y=0, width=500, height=500,center=(0,0,0), background=(0,0,0))
00063 scene2.range=(1,1,1)
00064 scene2.select()
00065 #Roll, Pitch, Yaw
00066 #Default reference, i.e. x runs right, y runs up, z runs backward (out of screen)
00067 cil_roll = cylinder(pos=(-0.5,0.3,0),axis=(0.2,0,0),radius=0.01,color=color.red)
00068 cil_roll2 = cylinder(pos=(-0.5,0.3,0),axis=(-0.2,0,0),radius=0.01,color=color.red)
00069 cil_pitch = arrow(pos=(0.5,0.3,0),axis=(0,0,-0.4),shaftwidth=0.02,color=color.green)
00070 arrow_course = arrow(pos=(0.0,-0.4,0),color=color.cyan,axis=(0,0.2,0), shaftwidth=0.02, fixedwidth=1)
00071 
00072 #Roll,Pitch,Yaw labels
00073 label(pos=(-0.5,0.6,0),text="Roll (degrees / radians)",box=0,opacity=0)
00074 label(pos=(0.5,0.6,0),text="Pitch (degrees / radians)",box=0,opacity=0)
00075 label(pos=(0.0,0.02,0),text="Yaw (degrees / radians)",box=0,opacity=0)
00076 label(pos=(0.0,-0.16,0),text="N",box=0,opacity=0,color=color.yellow)
00077 label(pos=(0.0,-0.64,0),text="S",box=0,opacity=0,color=color.yellow)
00078 label(pos=(-0.24,-0.4,0),text="W",box=0,opacity=0,color=color.yellow)
00079 label(pos=(0.24,-0.4,0),text="E",box=0,opacity=0,color=color.yellow)
00080 label(pos=(0.18,-0.22,0),height=7,text="NE",box=0,color=color.yellow)
00081 label(pos=(-0.18,-0.22,0),height=7,text="NW",box=0,color=color.yellow)
00082 label(pos=(0.18,-0.58,0),height=7,text="SE",box=0,color=color.yellow)
00083 label(pos=(-0.18,-0.58,0),height=7,text="SW",box=0,color=color.yellow)
00084 
00085 rollLabel = label(pos=(-0.5,0.52,0),text="-",box=0,opacity=0,height=12)
00086 pitchLabel = label(pos=(0.5,0.52,0),text="-",box=0,opacity=0,height=12)
00087 yawLabel = label(pos=(0,-0.06,0),text="-",box=0,opacity=0,height=12)
00088 
00089 #acceleration labels
00090 label(pos=(0,0.9,0),text="Linear Acceleration x / y / z (m/s^2)",box=0,opacity=0)
00091 label(pos=(0,-0.8,0),text="Angular Velocity x / y / z (rad/s)",box=0,opacity=0)
00092 linAccLabel = label(pos=(0,0.82,0),text="-",box=0,opacity=0,height=12)
00093 angVelLabel = label(pos=(0,-0.88,0),text="-",box=0,opacity=0,height=12)
00094 
00095 # Main scene objects
00096 scene.select()
00097 # Reference axis (x,y,z) - using ROS conventions (REP 103) - z is up, y left (west, 90 deg), x is forward (north, 0 deg)
00098 # In visual, z runs up, x runs forward, y runs left (see scene.up command earlier) 
00099 # So everything is good
00100 arrow(color=color.green,axis=(1,0,0), shaftwidth=0.04, fixedwidth=1)
00101 arrow(color=color.green,axis=(0,1,0), shaftwidth=0.04 , fixedwidth=1)
00102 arrow(color=color.green,axis=(0,0,1), shaftwidth=0.04, fixedwidth=1)
00103 
00104 # labels
00105 label(pos=(0,0,-1.2),text="Press 'a' to align",box=0,opacity=0)
00106 label(pos=(1,0.1,0),text="X",box=0,opacity=0)
00107 label(pos=(0,1,-0.1),text="Y",box=0,opacity=0)
00108 label(pos=(0,-0.1,1),text="Z",box=0,opacity=0)
00109 # IMU object
00110 platform = box(length=1.0, height=0.05, width=0.65, color=color.red,up=(0,0,1),axis=(-1,0,0))
00111 p_line = box(length=1.1,height=0.08,width=0.1,color=color.yellow,up=(0,0,1),axis=(-1,0,0))
00112 plat_arrow = arrow(length=-0.8,color=color.cyan,up=(0,0,1),axis=(-1,0,0), shaftwidth=0.06, fixedwidth=1)
00113 plat_arrow_up = arrow(length=0.4,color=color.cyan,up=(-1,0,0),axis=(0,0,1), shaftwidth=0.06, fixedwidth=1)
00114 rospy.init_node("display_3D_visualization_node")
00115 
00116 def processIMU_message(imuMsg):
00117     global yaw_offset
00118 
00119     roll=0
00120     pitch=0
00121     yaw=0
00122 
00123     quaternion = (
00124       imuMsg.orientation.x,
00125       imuMsg.orientation.y,
00126       imuMsg.orientation.z,
00127       imuMsg.orientation.w)
00128     (roll,pitch,yaw) = euler_from_quaternion(quaternion)
00129 
00130     #add align offset to yaw
00131     yaw += yaw_offset
00132 
00133     axis=(-cos(pitch)*cos(yaw),-cos(pitch)*sin(yaw),sin(pitch)) 
00134     up=(sin(roll)*sin(yaw)+cos(roll)*sin(pitch)*cos(yaw),-sin(roll)*cos(yaw)+cos(roll)*sin(pitch)*sin(yaw),cos(roll)*cos(pitch))
00135     platform.axis=axis
00136     platform.up=up
00137     platform.length=1.0
00138     plat_arrow_up.axis=up
00139     plat_arrow_up.up=axis
00140     plat_arrow_up.length=0.4
00141     plat_arrow.axis=axis
00142     plat_arrow.up=up
00143     plat_arrow.length=-0.8
00144     p_line.axis=axis
00145     p_line.up=up
00146     p_line.length=1.1
00147     cil_roll.axis=(-0.2*cos(roll),0.2*sin(roll),0)
00148     cil_roll2.axis=(0.2*cos(roll),-0.2*sin(roll),0)
00149     cil_pitch.axis=(0,-0.4*sin(pitch),-0.4*cos(pitch))
00150     #remove yaw_offset from yaw display
00151     arrow_course.axis=(-0.2*sin(yaw-yaw_offset),0.2*cos(yaw-yaw_offset),0)
00152     
00153     #display in degrees / radians
00154     rollLabel.text = str(round(roll*rad2degrees, precision)) + " / " + str(round(roll,precision))
00155     pitchLabel.text = str(round(pitch*rad2degrees, precision)) + " / " + str(round(pitch, precision))
00156     #remove yaw_offset from yaw display
00157     yawLabel.text = str(round((yaw-yaw_offset)*rad2degrees, precision)) + " / " + str(round((yaw-yaw_offset), precision))
00158 
00159     linAccLabel.text = str(round(imuMsg.linear_acceleration.x, precision)) + " / " + str(round(imuMsg.linear_acceleration.y, precision)) + " / " + str(round(imuMsg.linear_acceleration.z, precision))
00160     angVelLabel.text = str(round(imuMsg.angular_velocity.x, precision)) + " / " + str(round(imuMsg.angular_velocity.y, precision)) + " / " + str(round(imuMsg.angular_velocity.z, precision))
00161 
00162     #check for align key press - if pressed, next refresh will be aligned
00163     if scene.kb.keys: # event waiting to be processed?
00164         s = scene.kb.getkey() # get keyboard info
00165         if s == 'a':
00166             #align key pressed - align
00167             yaw_offset += -yaw
00168 
00169 
00170 sub = rospy.Subscriber('imu', Imu, processIMU_message)
00171 rospy.spin()


razor_imu_9dof
Author(s): Tang Tiong Yew, Kristof Robot, Paul Bouchier, Peter Bartz
autogenerated on Sun Jul 3 2016 03:22:09