sphere_viz.py
Go to the documentation of this file.
00001 #! /usr/bin/python
00002 
00003 #***********************************************************
00004 #* Software License Agreement (BSD License)
00005 #*
00006 #*  Copyright (c) 2009, Willow Garage, Inc.
00007 #*  All rights reserved.
00008 #*
00009 #*  Redistribution and use in source and binary forms, with or without
00010 #*  modification, are permitted provided that the following conditions
00011 #*  are met:
00012 #*
00013 #*   * Redistributions of source code must retain the above copyright
00014 #*     notice, this list of conditions and the following disclaimer.
00015 #*   * Redistributions in binary form must reproduce the above
00016 #*     copyright notice, this list of conditions and the following
00017 #*     disclaimer in the documentation and/or other materials provided
00018 #*     with the distribution.
00019 #*   * Neither the name of the Willow Garage nor the names of its
00020 #*     contributors may be used to endorse or promote products derived
00021 #*     from this software without specific prior written permission.
00022 #*
00023 #*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024 #*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025 #*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026 #*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027 #*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028 #*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029 #*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030 #*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031 #*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032 #*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033 #*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034 #*  POSSIBILITY OF SUCH DAMAGE.
00035 #***********************************************************
00036 
00037 # Author: Blaise Gassend
00038 
00039 # Reads fingertip pressure data from /pressure and publishes it as a
00040 # visualization_marker
00041 
00042 import roslib
00043 roslib.load_manifest('fingertip_pressure')
00044 import rospy
00045 import threading
00046 
00047 from fingertip_pressure.msg import PressureInfo, PressureInfoElement
00048 from pr2_msgs.msg import PressureState
00049 from visualization_msgs.msg import Marker
00050 from fingertip_pressure.colormap import color
00051 from geometry_msgs.msg import Vector3
00052 
00053 positions = [ # x, y, z, xscale, yscale, zscale 
00054         ( 0.026, 0.007, 0.000, 0.010, 0.010, 0.015),
00055         ( 0.010, 0.010,-0.008, 0.030, 0.010, 0.010), 
00056         ( 0.025, 0.010,-0.008, 0.010, 0.010, 0.010), 
00057         ( 0.030, 0.010,-0.004, 0.010, 0.010, 0.010), 
00058         ( 0.030, 0.010, 0.004, 0.010, 0.010, 0.010), 
00059         ( 0.025, 0.010, 0.008, 0.010, 0.010, 0.010), 
00060         ( 0.010, 0.010, 0.008, 0.030, 0.010, 0.010),
00061         ( 0.025, 0.012,-0.006, 0.010, 0.010, 0.010),
00062         ( 0.025, 0.012, 0.000, 0.010, 0.010, 0.010),
00063         ( 0.025, 0.012, 0.006, 0.010, 0.010, 0.010),
00064         ( 0.019, 0.012,-0.006, 0.010, 0.010, 0.010),
00065         ( 0.019, 0.012, 0.000, 0.010, 0.010, 0.010),
00066         ( 0.019, 0.012, 0.006, 0.010, 0.010, 0.010),
00067         ( 0.013, 0.012,-0.006, 0.010, 0.010, 0.010),
00068         ( 0.013, 0.012, 0.000, 0.010, 0.010, 0.010),
00069         ( 0.013, 0.012, 0.006, 0.010, 0.010, 0.010),
00070         ( 0.007, 0.012,-0.006, 0.010, 0.010, 0.010),
00071         ( 0.007, 0.012, 0.000, 0.010, 0.010, 0.010),
00072         ( 0.007, 0.012, 0.006, 0.010, 0.010, 0.010),
00073         ( 0.001, 0.012,-0.006, 0.010, 0.010, 0.010),
00074         ( 0.001, 0.012, 0.000, 0.010, 0.010, 0.010),
00075         ( 0.001, 0.012, 0.006, 0.010, 0.010, 0.010),
00076         ]
00077 numsensors = len(positions);
00078 
00079 class pressureVisualizer:
00080     def info_callback(self, info):
00081         self.lock.acquire()
00082         self.frame = []
00083         self.center = []
00084         self.hside1 = []
00085         self.hside2 = []
00086         for i in [0, 1]:
00087             self.frame.append(info.sensor[i].frame_id)
00088             self.center.append(info.sensor[i].center)
00089             self.hside1.append(info.sensor[i].halfside1)
00090             self.hside2.append(info.sensor[i].halfside2)
00091         self.got_info = True
00092         self.lock.release()
00093         #print "callback"
00094 
00095     def callback(self, pressurestate):
00096         self.lock.acquire()
00097         #print "callback"
00098         self.l_finger_tip = pressurestate.l_finger_tip
00099         self.r_finger_tip = pressurestate.r_finger_tip
00100         self.datatimestamp = pressurestate.header.stamp
00101         self.dataready = True
00102         self.lock.release()
00103 
00104     def publish(self):
00105         if self.dataready and self.got_info:
00106             self.lock.acquire()
00107             #print 'publish'
00108             self.dataready = False
00109             self.makeVisualization(self.l_finger_tip, 0, -1)
00110             self.makeVisualization(self.r_finger_tip, 1, 1)
00111             self.lock.release()
00112 
00113     def makeVisualization(self, data, tipnum, ydir):
00114         mk = Marker()
00115         mk.header.frame_id = self.frame[tipnum]
00116         mk.header.stamp = self.datatimestamp
00117         mk.ns = mk.header.frame_id + "/sphere"
00118         mk.type = Marker.SPHERE
00119         mk.action = Marker.ADD
00120         #mk.lifetime = rospy.Duration(1)
00121         mk.points = []
00122         #for i in range(0,1):
00123         for i in range(0,numsensors):
00124             mk.id = i
00125             (mk.pose.position.x, mk.pose.position.y, mk.pose.position.z, mk.scale.x, mk.scale.y, mk.scale.z) = positions[i]
00126             mk.pose.position.y = mk.pose.position.y * ydir
00127             mk.pose.position.z = mk.pose.position.z * ydir
00128             #mk.pose.position = Vector3()
00129             #mk.pose.position.x = self.center[tipnum][i].x
00130             #mk.pose.position.y = self.center[tipnum][i].y
00131             #mk.pose.position.z = self.center[tipnum][i].z
00132             mk.pose.orientation.w = 1.0
00133             mk.color.a = 1.0
00134             (mk.color.r, mk.color.g, mk.color.b) = color(data[i] / 6000.)
00135             #print "%f %f %f"%(mk.color.r, mk.color.g, mk.color.b)
00136             self.vis_pub.publish(mk)
00137 
00138     def __init__(self, source):
00139         self.got_info = False;
00140         self.dataready = False
00141         self.lock = threading.Lock()
00142 
00143         self.vis_pub = rospy.Publisher('visualization_marker', Marker)
00144         rospy.Subscriber(source, PressureState, self.callback)
00145         rospy.Subscriber(source + "_info", PressureInfo, self.info_callback)
00146         
00147 
00148 
00149 if __name__ == '__main__':
00150     #@todo it would be nice to read an xml configuration file to get these parameters.
00151     rospy.init_node('pressure_rectangle_viz')
00152     rospy.sleep(.2)
00153         
00154     pv1 = pressureVisualizer('pressure/r_gripper_motor')
00155     pv2 = pressureVisualizer('pressure/l_gripper_motor')
00156     
00157     while not rospy.is_shutdown():
00158         rospy.sleep(0.09)
00159         pv1.publish()
00160         pv2.publish()


fingertip_pressure
Author(s): Blaise Gassend/blaise@willowgarage.com
autogenerated on Thu Apr 24 2014 15:43:41