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 roslib; roslib.load_manifest('razor_imu_9dof')
00031 import rospy
00032 
00033 from visual import *
00034 import serial
00035 import string
00036 import math
00037 
00038 from time import time
00039 from sensor_msgs.msg import Imu
00040 from razor_imu_9dof.msg import RazorImu
00041 
00042 grad2rad = 3.141592/180.0
00043 
00044 
00045 # Main scene
00046 scene=display(title="9DOF Razor IMU Main Screen")
00047 scene.range=(1.2,1.2,1.2)
00048 #scene.forward = (0,-1,-0.25)
00049 scene.forward = (1,0,-0.25)
00050 scene.up=(0,0,1)
00051 
00052 # Second scene (Roll, Pitch, Yaw)
00053 scene2 = display(title='Second Screen',x=0, y=0, width=500, height=200,center=(0,0,0), background=(0,0,0))
00054 scene2.range=(1,1,1)
00055 scene.width=500
00056 scene.height=500
00057 scene.y=200
00058 
00059 scene2.select()
00060 #Roll, Pitch, Yaw
00061 cil_roll = cylinder(pos=(-0.4,0,0),axis=(0.2,0,0),radius=0.01,color=color.red)
00062 cil_roll2 = cylinder(pos=(-0.4,0,0),axis=(-0.2,0,0),radius=0.01,color=color.red)
00063 cil_pitch = cylinder(pos=(0.1,0,0),axis=(0.2,0,0),radius=0.01,color=color.green)
00064 cil_pitch2 = cylinder(pos=(0.1,0,0),axis=(-0.2,0,0),radius=0.01,color=color.green)
00065 #cil_course = cylinder(pos=(0.6,0,0),axis=(0.2,0,0),radius=0.01,color=color.blue)
00066 #cil_course2 = cylinder(pos=(0.6,0,0),axis=(-0.2,0,0),radius=0.01,color=color.blue)
00067 arrow_course = arrow(pos=(0.6,0,0),color=color.cyan,axis=(-0.2,0,0), shaftwidth=0.02, fixedwidth=1)
00068 
00069 #Roll,Pitch,Yaw labels
00070 label(pos=(-0.4,0.3,0),text="Roll",box=0,opacity=0)
00071 label(pos=(0.1,0.3,0),text="Pitch",box=0,opacity=0)
00072 label(pos=(0.55,0.3,0),text="Yaw",box=0,opacity=0)
00073 label(pos=(0.6,0.22,0),text="N",box=0,opacity=0,color=color.yellow)
00074 label(pos=(0.6,-0.22,0),text="S",box=0,opacity=0,color=color.yellow)
00075 label(pos=(0.38,0,0),text="W",box=0,opacity=0,color=color.yellow)
00076 label(pos=(0.82,0,0),text="E",box=0,opacity=0,color=color.yellow)
00077 label(pos=(0.75,0.15,0),height=7,text="NE",box=0,color=color.yellow)
00078 label(pos=(0.45,0.15,0),height=7,text="NW",box=0,color=color.yellow)
00079 label(pos=(0.75,-0.15,0),height=7,text="SE",box=0,color=color.yellow)
00080 label(pos=(0.45,-0.15,0),height=7,text="SW",box=0,color=color.yellow)
00081 
00082 L1 = label(pos=(-0.4,0.22,0),text="-",box=0,opacity=0)
00083 L2 = label(pos=(0.1,0.22,0),text="-",box=0,opacity=0)
00084 L3 = label(pos=(0.7,0.3,0),text="-",box=0,opacity=0)
00085 
00086 # Main scene objects
00087 scene.select()
00088 # Reference axis (x,y,z)
00089 arrow(color=color.green,axis=(1,0,0), shaftwidth=0.02, fixedwidth=1)
00090 arrow(color=color.green,axis=(0,-1,0), shaftwidth=0.02 , fixedwidth=1)
00091 arrow(color=color.green,axis=(0,0,-1), shaftwidth=0.02, fixedwidth=1)
00092 # labels
00093 label(pos=(0,0,0.8),text="9DOF Razor IMU test",box=0,opacity=0)
00094 label(pos=(1,0,0),text="X",box=0,opacity=0)
00095 label(pos=(0,-1,0),text="Y",box=0,opacity=0)
00096 label(pos=(0,0,-1),text="Z",box=0,opacity=0)
00097 # IMU object
00098 platform = box(length=1, height=0.05, width=1, color=color.red)
00099 p_line = box(length=1,height=0.08,width=0.1,color=color.yellow)
00100 plat_arrow = arrow(color=color.cyan,axis=(1,0,0), shaftwidth=0.06, fixedwidth=1)
00101 
00102 
00103 rospy.init_node("display_3D_visualization_node")
00104 
00105 def processIMU_message(rawMsg):
00106     roll=0
00107     pitch=0
00108     yaw=0
00109 
00110     roll = rawMsg.roll
00111     pitch = rawMsg.pitch
00112     yaw = rawMsg.yaw
00113     
00114     axis=(cos(pitch)*cos(yaw),-cos(pitch)*sin(yaw),sin(pitch)) 
00115     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))
00116     platform.axis=axis
00117     platform.up=up
00118     platform.length=1.0
00119     platform.width=0.65
00120     plat_arrow.axis=axis
00121     plat_arrow.up=up
00122     plat_arrow.length=-0.8
00123     p_line.axis=axis
00124     p_line.up=up
00125     cil_roll.axis=(0.2*cos(roll),0.2*sin(roll),0)
00126     cil_roll2.axis=(-0.2*cos(roll),-0.2*sin(roll),0)
00127     cil_pitch.axis=(0.2*cos(pitch),0.2*sin(pitch),0)
00128     cil_pitch2.axis=(-0.2*cos(pitch),-0.2*sin(pitch),0)
00129     arrow_course.axis=(0.2*sin(yaw),0.2*cos(yaw),0)
00130     L1.text = str(roll)
00131     L2.text = str(pitch)
00132     L3.text = str(yaw)
00133 
00134 
00135 sub = rospy.Subscriber('imuRaw', RazorImu, processIMU_message)


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