sphere_viz.py
Go to the documentation of this file.
1 #! /usr/bin/python3
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 fingertip_pressure.colormap import color
51 from geometry_msgs.msg import Vector3
52 
53 positions = [ # x, y, z, xscale, yscale, zscale
54  ( 0.026, 0.007, 0.000, 0.010, 0.010, 0.015),
55  ( 0.010, 0.010,-0.008, 0.030, 0.010, 0.010),
56  ( 0.025, 0.010,-0.008, 0.010, 0.010, 0.010),
57  ( 0.030, 0.010,-0.004, 0.010, 0.010, 0.010),
58  ( 0.030, 0.010, 0.004, 0.010, 0.010, 0.010),
59  ( 0.025, 0.010, 0.008, 0.010, 0.010, 0.010),
60  ( 0.010, 0.010, 0.008, 0.030, 0.010, 0.010),
61  ( 0.025, 0.012,-0.006, 0.010, 0.010, 0.010),
62  ( 0.025, 0.012, 0.000, 0.010, 0.010, 0.010),
63  ( 0.025, 0.012, 0.006, 0.010, 0.010, 0.010),
64  ( 0.019, 0.012,-0.006, 0.010, 0.010, 0.010),
65  ( 0.019, 0.012, 0.000, 0.010, 0.010, 0.010),
66  ( 0.019, 0.012, 0.006, 0.010, 0.010, 0.010),
67  ( 0.013, 0.012,-0.006, 0.010, 0.010, 0.010),
68  ( 0.013, 0.012, 0.000, 0.010, 0.010, 0.010),
69  ( 0.013, 0.012, 0.006, 0.010, 0.010, 0.010),
70  ( 0.007, 0.012,-0.006, 0.010, 0.010, 0.010),
71  ( 0.007, 0.012, 0.000, 0.010, 0.010, 0.010),
72  ( 0.007, 0.012, 0.006, 0.010, 0.010, 0.010),
73  ( 0.001, 0.012,-0.006, 0.010, 0.010, 0.010),
74  ( 0.001, 0.012, 0.000, 0.010, 0.010, 0.010),
75  ( 0.001, 0.012, 0.006, 0.010, 0.010, 0.010),
76  ]
77 numsensors = len(positions);
78 
80  def info_callback(self, info):
81  self.lock.acquire()
82  self.frame = []
83  self.center = []
84  self.hside1 = []
85  self.hside2 = []
86  for i in [0, 1]:
87  self.frame.append(info.sensor[i].frame_id)
88  self.center.append(info.sensor[i].center)
89  self.hside1.append(info.sensor[i].halfside1)
90  self.hside2.append(info.sensor[i].halfside2)
91  self.got_info = True
92  self.lock.release()
93  #print "callback"
94 
95  def callback(self, pressurestate):
96  self.lock.acquire()
97  #print "callback"
98  self.l_finger_tip = pressurestate.l_finger_tip
99  self.r_finger_tip = pressurestate.r_finger_tip
100  self.datatimestamp = pressurestate.header.stamp
101  self.dataready = True
102  self.lock.release()
103 
104  def publish(self):
105  if self.dataready and self.got_info:
106  self.lock.acquire()
107  #print 'publish'
108  self.dataready = False
109  self.makeVisualization(self.l_finger_tip, 0, -1)
110  self.makeVisualization(self.r_finger_tip, 1, 1)
111  self.lock.release()
112 
113  def makeVisualization(self, data, tipnum, ydir):
114  mk = Marker()
115  mk.header.frame_id = self.frame[tipnum]
116  mk.header.stamp = self.datatimestamp
117  mk.ns = mk.header.frame_id + "/sphere"
118  mk.type = Marker.SPHERE
119  mk.action = Marker.ADD
120  #mk.lifetime = rospy.Duration(1)
121  mk.points = []
122  #for i in range(0,1):
123  for i in range(0,numsensors):
124  mk.id = i
125  (mk.pose.position.x, mk.pose.position.y, mk.pose.position.z, mk.scale.x, mk.scale.y, mk.scale.z) = positions[i]
126  mk.pose.position.y = mk.pose.position.y * ydir
127  mk.pose.position.z = mk.pose.position.z * ydir
128  #mk.pose.position = Vector3()
129  #mk.pose.position.x = self.center[tipnum][i].x
130  #mk.pose.position.y = self.center[tipnum][i].y
131  #mk.pose.position.z = self.center[tipnum][i].z
132  mk.pose.orientation.w = 1.0
133  mk.color.a = 1.0
134  (mk.color.r, mk.color.g, mk.color.b) = color(data[i] / 6000.)
135  #print "%f %f %f"%(mk.color.r, mk.color.g, mk.color.b)
136  self.vis_pub.publish(mk)
137 
138  def __init__(self, source):
139  self.got_info = False;
140  self.dataready = False
141  self.lock = threading.Lock()
142 
143  self.vis_pub = rospy.Publisher('visualization_marker', Marker, queue_size=1000)
144  rospy.Subscriber(source, PressureState, self.callback)
145  rospy.Subscriber(source + "_info", PressureInfo, self.info_callback)
146 
147 
148 
149 if __name__ == '__main__':
150  #@todo it would be nice to read an xml configuration file to get these parameters.
151  rospy.init_node('pressure_rectangle_viz')
152  rospy.sleep(.2)
153 
154  pv1 = pressureVisualizer('pressure/r_gripper_motor')
155  pv2 = pressureVisualizer('pressure/l_gripper_motor')
156 
157  while not rospy.is_shutdown():
158  rospy.sleep(0.09)
159  pv1.publish()
160  pv2.publish()
def makeVisualization(self, data, tipnum, ydir)
Definition: sphere_viz.py:113
def __init__(self, source)
Definition: sphere_viz.py:138
def info_callback(self, info)
Definition: sphere_viz.py:80
def callback(self, pressurestate)
Definition: sphere_viz.py:95


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