00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
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 = [
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
00094
00095 def callback(self, pressurestate):
00096 self.lock.acquire()
00097
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
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
00121 mk.points = []
00122
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
00129
00130
00131
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
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
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()