rectangle_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 geometry_msgs.msg import Vector3
00051 from fingertip_pressure.colormap import color
00052 
00053 #def norm(v):
00054 #    return sqrt(v.x * v.x + v.y * v.y + v.z * v.z)
00055 
00056 class pressureVisualizer:
00057     def info_callback(self, info):
00058         self.lock.acquire()
00059         self.frame = []
00060         self.center = []
00061         self.hside1 = []
00062         self.hside2 = []
00063         for i in [0, 1]:
00064             self.frame.append(info.sensor[i].frame_id)
00065             self.center.append(info.sensor[i].center)
00066             self.hside1.append(info.sensor[i].halfside1)
00067             self.hside2.append(info.sensor[i].halfside2)
00068         self.got_info = True
00069         self.lock.release()
00070         #print "callback"
00071 
00072     def callback(self, pressurestate):
00073         self.lock.acquire()
00074         #print "callback"
00075         self.l_finger_tip = pressurestate.l_finger_tip
00076         self.r_finger_tip = pressurestate.r_finger_tip
00077         self.datatimestamp = pressurestate.header.stamp
00078         self.dataready = True
00079         self.lock.release()
00080 
00081     def publish(self):
00082         if self.dataready and self.got_info:
00083             self.lock.acquire()
00084             #print 'publish'
00085             self.dataready = False
00086             self.makeVisualization(self.l_finger_tip, 0)
00087             self.makeVisualization(self.r_finger_tip, 1)
00088             self.lock.release()
00089 
00090     def makeVisualization(self, data, tipnum):
00091         mk = Marker()
00092         mk.header.frame_id = self.frame[tipnum]
00093         mk.header.stamp = self.datatimestamp
00094         mk.ns = mk.header.frame_id + "/line"
00095         mk.type = Marker.LINE_STRIP
00096         mk.action = Marker.ADD
00097         #mk.lifetime = rospy.Duration(1)
00098         mk.points = []
00099         for i in range(0,5):
00100             mk.points.append(Vector3())
00101         #for i in range(0,1):
00102         for i in range(0,22):
00103             mk.id = i
00104             mk.pose.position = self.center[tipnum][i]
00105             mk.pose.orientation.w = 1.0
00106             mk.scale.x = 0.001
00107             h1 = self.hside1[tipnum][i]
00108             h2 = self.hside2[tipnum][i]
00109             mk.points[0].x =  h1.x + h2.x
00110             mk.points[1].x =  h1.x - h2.x  
00111             mk.points[2].x = -h1.x - h2.x
00112             mk.points[3].x = -h1.x + h2.x
00113             mk.points[0].y =  h1.y + h2.y
00114             mk.points[1].y =  h1.y - h2.y  
00115             mk.points[2].y = -h1.y - h2.y
00116             mk.points[3].y = -h1.y + h2.y
00117             mk.points[0].z =  h1.z + h2.z
00118             mk.points[1].z =  h1.z - h2.z  
00119             mk.points[2].z = -h1.z - h2.z
00120             mk.points[3].z = -h1.z + h2.z
00121             mk.points[4] = mk.points[0]
00122             mk.color.a = 1.0
00123             (mk.color.r, mk.color.g, mk.color.b) = color(data[i] / 6000.)
00124             #print "%f %f %f"%(mk.color.r, mk.color.g, mk.color.b)
00125             self.vis_pub.publish(mk)
00126 
00127     def __init__(self, source):
00128         self.got_info = False;
00129         self.dataready = False
00130         self.lock = threading.Lock()
00131 
00132         self.vis_pub = rospy.Publisher('visualization_marker', Marker)
00133         rospy.Subscriber(source, PressureState, self.callback)
00134         rospy.Subscriber(source + "_info", PressureInfo, self.info_callback)
00135         
00136 
00137 
00138 if __name__ == '__main__':
00139     #@todo it would be nice to read an xml configuration file to get these parameters.
00140     rospy.init_node('pressure_sphere_viz')
00141     rospy.sleep(.2)
00142         
00143     pv1 = pressureVisualizer('pressure/r_gripper_motor')
00144     pv2 = pressureVisualizer('pressure/l_gripper_motor')
00145     
00146     while not rospy.is_shutdown():
00147         rospy.sleep(0.09)
00148         pv1.publish()
00149         pv2.publish()


fingertip_pressure
Author(s): Blaise Gassend
autogenerated on Fri Aug 28 2015 12:09:42