Go to the documentation of this file.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 geometry_msgs.msg import Vector3
00051 from fingertip_pressure.colormap import color
00052
00053
00054
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
00071
00072 def callback(self, pressurestate):
00073 self.lock.acquire()
00074
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
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
00098 mk.points = []
00099 for i in range(0,5):
00100 mk.points.append(Vector3())
00101
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
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
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()