rectangle_viz.py
Go to the documentation of this file.
1 #! /usr/bin/python
2 
3 #***********************************************************
4 #* Software License Agreement (BSD License)
5 #*
6 #* Copyright (c) 2009, Willow Garage, Inc.
7 #* All rights reserved.
8 #*
9 #* Redistribution and use in source and binary forms, with or without
10 #* modification, are permitted provided that the following conditions
11 #* are met:
12 #*
13 #* * Redistributions of source code must retain the above copyright
14 #* notice, this list of conditions and the following disclaimer.
15 #* * Redistributions in binary form must reproduce the above
16 #* copyright notice, this list of conditions and the following
17 #* disclaimer in the documentation and/or other materials provided
18 #* with the distribution.
19 #* * Neither the name of the Willow Garage nor the names of its
20 #* contributors may be used to endorse or promote products derived
21 #* from this software without specific prior written permission.
22 #*
23 #* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 #* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 #* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 #* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 #* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 #* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 #* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 #* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 #* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 #* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 #* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 #* POSSIBILITY OF SUCH DAMAGE.
35 #***********************************************************
36 
37 # Author: Blaise Gassend
38 
39 # Reads fingertip pressure data from /pressure and publishes it as a
40 # visualization_marker
41 
42 import roslib
43 roslib.load_manifest('fingertip_pressure')
44 import rospy
45 import threading
46 
47 from fingertip_pressure.msg import PressureInfo, PressureInfoElement
48 from pr2_msgs.msg import PressureState
49 from visualization_msgs.msg import Marker
50 from geometry_msgs.msg import Vector3
51 from fingertip_pressure.colormap import color
52 
53 #def norm(v):
54 # return sqrt(v.x * v.x + v.y * v.y + v.z * v.z)
55 
57  def info_callback(self, info):
58  self.lock.acquire()
59  self.frame = []
60  self.center = []
61  self.hside1 = []
62  self.hside2 = []
63  for i in [0, 1]:
64  self.frame.append(info.sensor[i].frame_id)
65  self.center.append(info.sensor[i].center)
66  self.hside1.append(info.sensor[i].halfside1)
67  self.hside2.append(info.sensor[i].halfside2)
68  self.got_info = True
69  self.lock.release()
70  #print "callback"
71 
72  def callback(self, pressurestate):
73  self.lock.acquire()
74  #print "callback"
75  self.l_finger_tip = pressurestate.l_finger_tip
76  self.r_finger_tip = pressurestate.r_finger_tip
77  self.datatimestamp = pressurestate.header.stamp
78  self.dataready = True
79  self.lock.release()
80 
81  def publish(self):
82  if self.dataready and self.got_info:
83  self.lock.acquire()
84  #print 'publish'
85  self.dataready = False
86  self.makeVisualization(self.l_finger_tip, 0)
87  self.makeVisualization(self.r_finger_tip, 1)
88  self.lock.release()
89 
90  def makeVisualization(self, data, tipnum):
91  mk = Marker()
92  mk.header.frame_id = self.frame[tipnum]
93  mk.header.stamp = self.datatimestamp
94  mk.ns = mk.header.frame_id + "/line"
95  mk.type = Marker.LINE_STRIP
96  mk.action = Marker.ADD
97  #mk.lifetime = rospy.Duration(1)
98  mk.points = []
99  for i in range(0,5):
100  mk.points.append(Vector3())
101  #for i in range(0,1):
102  for i in range(0,22):
103  mk.id = i
104  mk.pose.position = self.center[tipnum][i]
105  mk.pose.orientation.w = 1.0
106  mk.scale.x = 0.001
107  h1 = self.hside1[tipnum][i]
108  h2 = self.hside2[tipnum][i]
109  mk.points[0].x = h1.x + h2.x
110  mk.points[1].x = h1.x - h2.x
111  mk.points[2].x = -h1.x - h2.x
112  mk.points[3].x = -h1.x + h2.x
113  mk.points[0].y = h1.y + h2.y
114  mk.points[1].y = h1.y - h2.y
115  mk.points[2].y = -h1.y - h2.y
116  mk.points[3].y = -h1.y + h2.y
117  mk.points[0].z = h1.z + h2.z
118  mk.points[1].z = h1.z - h2.z
119  mk.points[2].z = -h1.z - h2.z
120  mk.points[3].z = -h1.z + h2.z
121  mk.points[4] = mk.points[0]
122  mk.color.a = 1.0
123  (mk.color.r, mk.color.g, mk.color.b) = color(data[i] / 6000.)
124  #print "%f %f %f"%(mk.color.r, mk.color.g, mk.color.b)
125  self.vis_pub.publish(mk)
126 
127  def __init__(self, source):
128  self.got_info = False;
129  self.dataready = False
130  self.lock = threading.Lock()
131 
132  self.vis_pub = rospy.Publisher('visualization_marker', Marker, queue_size=1000)
133  rospy.Subscriber(source, PressureState, self.callback)
134  rospy.Subscriber(source + "_info", PressureInfo, self.info_callback)
135 
136 
137 
138 if __name__ == '__main__':
139  #@todo it would be nice to read an xml configuration file to get these parameters.
140  rospy.init_node('pressure_sphere_viz')
141  rospy.sleep(.2)
142 
143  pv1 = pressureVisualizer('pressure/r_gripper_motor')
144  pv2 = pressureVisualizer('pressure/l_gripper_motor')
145 
146  while not rospy.is_shutdown():
147  rospy.sleep(0.09)
148  pv1.publish()
149  pv2.publish()
def callback(self, pressurestate)
def makeVisualization(self, data, tipnum)


fingertip_pressure
Author(s): Blaise Gassend
autogenerated on Thu Mar 4 2021 03:10:25