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 PKG = 'fingertip_pressure'
00036
00037 import roslib
00038 roslib.load_manifest(PKG)
00039
00040 import sys
00041 import rospy
00042 from pr2_msgs.msg import PressureState
00043 import math
00044
00045 from fingertip_pressure.colormap import color255
00046 from fingertip_pressure.msg import PressureInfo
00047 from geometry_msgs.msg import Vector3
00048
00049 import wxversion
00050 wxversion.ensureMinimal("2.8")
00051
00052 import wx
00053 import threading
00054 from wx import xrc
00055
00056 NUMSENSORS = 22
00057 RATE = 5.
00058
00059 def txtcolor(data):
00060 (r,g,b)=color255(data)
00061 if r + g + b < 3 *128:
00062 (rt,gt,bt) = (255,255,255)
00063 else:
00064 (rt,gt,bt) = (0,0,0)
00065 return ("#%02x%02x%02x"%(r,g,b),"#%02x%02x%02x"%(rt,gt,bt))
00066
00067 class GripperPressurePanel(wx.Panel):
00068 def __init__(self, parent, topic):
00069 wx.Panel.__init__(self, parent, wx.ID_ANY)
00070
00071
00072 sizer = wx.BoxSizer(wx.HORIZONTAL)
00073 self.panel0 = FingertipPressurePanel(self)
00074 self.panel1 = FingertipPressurePanel(self)
00075 sizer.Add(self.panel0.panel, 1, wx.EXPAND)
00076 sizer.AddSpacer(5)
00077 sizer.Add(self.panel1.panel, 1, wx.EXPAND)
00078 self.SetSizer(sizer)
00079
00080
00081 self._mutex = threading.Lock()
00082 self.new_message_ = None
00083 self.new_info_ = None
00084 rospy.Subscriber(topic, PressureState, self.message_callback)
00085 rospy.Subscriber(topic+"_info", PressureInfo, self.info_callback)
00086
00087 self.timer = wx.Timer(self, 1)
00088 self.Bind(wx.EVT_TIMER, self.on_timer, self.timer)
00089
00090 def info_callback(self, message):
00091 self._mutex.acquire()
00092 self.new_info_ = message
00093 self._mutex.release()
00094
00095 def message_callback(self, message):
00096
00097 self._mutex.acquire()
00098
00099 self.new_message_ = message
00100
00101 if not self.timer.IsRunning():
00102 self.timer.Start(1000./RATE, True)
00103 wx.CallAfter(self.display)
00104
00105 self._mutex.release()
00106
00107 def on_timer(self, event):
00108
00109 pass
00110
00111 def display(self):
00112 self._mutex.acquire()
00113
00114 if self.new_message_ != None:
00115
00116
00117 self.panel0.new_message(self.new_message_.l_finger_tip)
00118 self.panel1.new_message(self.new_message_.r_finger_tip)
00119
00120 self.new_message_ = None
00121
00122 self.Refresh()
00123
00124 if self.new_info_ != None:
00125 self.panel0.set_info(self.new_info_.sensor[0])
00126 self.panel1.set_info(self.new_info_.sensor[1])
00127 self.new_info_ = None
00128 self.Refresh()
00129
00130 self._mutex.release()
00131
00132 class FingertipPressurePanel:
00133 Newton = 'N';
00134 kPascal = 'kPa';
00135
00136 def __init__(self, parent):
00137 self._mutex = threading.Lock()
00138 xrc_path = roslib.packages.get_pkg_dir(PKG) + '/ui/fingertip_panel.xrc'
00139 panelxrc = xrc.XmlResource(xrc_path)
00140
00141 self.panel = panelxrc.LoadPanel(parent, 'FingertipPressurePanel')
00142
00143 bag =self.panel.GetSizer()
00144 bag.SetEmptyCellSize(wx.Size(0,0))
00145
00146 self.scalings = None
00147 self.zeros = [0 for i in range(0,22)]
00148 self.recentmean = [0 for i in range(0,22)]
00149
00150 self.pad = []
00151 for i in range(0, NUMSENSORS):
00152 self.pad.append(xrc.XRCCTRL(self.panel, 'pad%i'%i))
00153 self.pad[i].SetEditable(False)
00154 font = self.pad[i].GetFont()
00155 font.SetPointSize(6)
00156 self.pad[i].SetFont(font)
00157 self.pad[i].SetMinSize(wx.Size(40,35))
00158 self.frame_id_box = xrc.XRCCTRL(self.panel, 'frame_id')
00159
00160 self.panel.Bind(wx.EVT_RADIOBUTTON, self.set_Newton, id=xrc.XRCID('button_N'))
00161 self.panel.Bind(wx.EVT_RADIOBUTTON, self.set_kPascal, id=xrc.XRCID('button_kPa'))
00162 self.unit = self.Newton
00163
00164 self.panel.Bind(wx.EVT_CHECKBOX, self.set_Zero, id=xrc.XRCID('button_Zero'))
00165
00166 def set_Newton(self, event):
00167 self._mutex.acquire()
00168 self.unit = self.Newton
00169 self._mutex.release()
00170
00171 def set_kPascal(self, event):
00172 self._mutex.acquire()
00173 self.unit = self.kPascal
00174 self._mutex.release()
00175
00176 def set_Zero(self, event):
00177 self._mutex.acquire()
00178 cb = event.GetEventObject()
00179 if cb.IsChecked():
00180 self.zeros = list(self.recentmean);
00181 else:
00182 self.zeros = [0 for i in range(0,22)]
00183 self._mutex.release()
00184
00185 def calc_area(self,u,v):
00186 w = Vector3()
00187 w.x = u.y * v.z - u.z * v.y
00188 w.y = u.z * v.x - u.x * v.z
00189 w.z = u.x * v.y - u.y * v.x
00190 return math.sqrt(w.x*w.x + w.y*w.y + w.z*w.z)
00191
00192 def set_info(self, info):
00193 self._mutex.acquire()
00194 self.scalings = info.force_per_unit
00195 self.areas = [self.calc_area(info.halfside1[i], info.halfside2[i]) * 4 for i in range(0,22)]
00196 self.frame_id_box.SetValue(info.frame_id)
00197 self._mutex.release()
00198
00199 def new_message(self, data):
00200 self._mutex.acquire()
00201
00202 for i in range(0, NUMSENSORS):
00203
00204
00205 self.recentmean[i] = 0.8 * self.recentmean[i] + 0.2 * data[i]
00206 dat = data[i] - self.zeros[i]
00207 if self.scalings != None:
00208 if self.unit == self.kPascal:
00209 val = dat / self.scalings[i] / self.areas[i] / 1e3
00210 colval = val / 30.
00211 scaled = '%.0f'%val
00212 else:
00213 val = dat / self.scalings[i]
00214 colval = val / 3.
00215 scaled = '%.2f'%val
00216 else:
00217 val = dat
00218 colval = val / 6000.
00219 scaled = '??'
00220 (colb,colf)=txtcolor(colval)
00221 self.pad[i].SetBackgroundColour(colb)
00222 self.pad[i].SetForegroundColour(colf)
00223 self.pad[i].SetValue('#%i\n%i\n%s %s'%(i,dat,scaled,self.unit))
00224 self._mutex.release()
00225
