00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 import os, rospy, rospkg
00020
00021 from qt_gui.plugin import Plugin
00022 from python_qt_binding import loadUi
00023
00024 from QtCore import QEvent, QObject, Qt, QTimer, Slot, SIGNAL, pyqtSignal
00025 from QtGui import QDockWidget, QShortcut, QMessageBox, QWidget, QFrame, QColor, QLabel, QComboBox, QLineEdit, QPushButton, QHBoxLayout, QVBoxLayout, QGridLayout, QIcon, QFileDialog
00026 from PyQt4.Qt import QTimer, QLayout, QPalette, QDoubleValidator, QIntValidator
00027 from pr2_mechanism_msgs.srv import ListControllers, SwitchController, LoadController
00028
00029
00030 import xml.etree.ElementTree as ET
00031 import time, threading
00032
00033 from sr_hand.shadowhand_ros import ShadowHand_ROS
00034
00035 class Step(QWidget):
00036 """
00037 A step in a sequence of steps which compose a full movement.
00038 Contains add / remove steps buttons, loop control, pause / interpolation time,
00039 grasp type.
00040 """
00041 def __init__(self, parent, step_index, plugin_parent):
00042 QWidget.__init__(self, parent = parent)
00043 self.step_index = step_index
00044 self.parent = plugin_parent
00045 self.grasp = 0
00046 self.pause_time = 0
00047 self.interpolation_time = 1
00048 self.loop_to_step = -1
00049 self.number_of_loops = 0
00050 self.remaining_loops = 0
00051
00052 self.widgets = []
00053
00054 def draw(self):
00055 self.frame = QFrame(self)
00056 self.green = QColor(153, 231, 96)
00057 self.saved_palette = self.palette()
00058 green_palette = self.palette()
00059 green_palette.setBrush(QPalette.Window, self.green)
00060 self.frame.setPalette(green_palette)
00061
00062 self.grasp = self.parent.library.grasp_parser.grasps.values()[0]
00063 label_grasp = QLabel(self.frame)
00064 label_grasp.setText("Grasp: ")
00065 self.widgets.append(label_grasp)
00066
00067 self.list_grasp = QComboBox(self.frame)
00068 self.refresh_list()
00069 self.frame.connect(self.list_grasp, SIGNAL('activated(QString)'), self.grasp_choosed)
00070 self.widgets.append(self.list_grasp)
00071
00072
00073 label_pause = QLabel(self.frame)
00074 label_pause.setText(" Pause Time:")
00075 self.widgets.append(label_pause)
00076
00077 self.pause_input = QLineEdit(self.frame)
00078 self.pause_input.setValidator(QDoubleValidator(self))
00079 self.pause_input.setText("0.0")
00080 self.pause_input.setFixedWidth(35)
00081 self.pause_input.setAlignment(Qt.AlignRight)
00082 self.frame.connect(self.pause_input, SIGNAL('textChanged(QString)'), self.pause_changed)
00083 self.widgets.append(self.pause_input)
00084
00085 label_interp = QLabel(self.frame)
00086 label_interp.setText("s. Interpolation Time:")
00087 self.widgets.append(label_interp)
00088
00089 self.interp_input = QLineEdit(self.frame)
00090 self.interp_input.setValidator(QDoubleValidator(self))
00091 self.interp_input.setText("1.0")
00092 self.interp_input.setAlignment(Qt.AlignRight)
00093 self.interp_input.setFixedWidth(35)
00094 self.frame.connect(self.interp_input, SIGNAL('textChanged(QString)'), self.interp_changed)
00095 self.widgets.append(self.interp_input)
00096
00097 label_looping = QLabel(self.frame)
00098 label_looping.setText("s. Looping from step:")
00099 self.widgets.append(label_looping)
00100
00101 self.loop_input = QComboBox(self.frame)
00102 for i in range(0, self.step_index + 1):
00103 if i == 0:
00104 self.loop_input.addItem("None")
00105 else:
00106 self.loop_input.addItem(str(i))
00107 self.frame.connect(self.loop_input, SIGNAL('activated(QString)'), self.choose_looping)
00108 self.widgets.append(self.loop_input)
00109
00110 self.number_loops = QLineEdit(self.frame)
00111 self.number_loops.setValidator(QIntValidator(self))
00112 self.number_loops.setDisabled(True)
00113 self.number_loops.setText("0")
00114 self.number_loops.setAlignment(Qt.AlignRight)
00115 self.number_loops.setFixedWidth(35)
00116
00117
00118 self.frame.connect(self.number_loops, SIGNAL('textChanged(QString)'), self.number_loops_changed)
00119 self.widgets.append(self.number_loops)
00120
00121 label_times = QLabel(self.frame)
00122 label_times.setText("times. ")
00123 self.widgets.append(label_times)
00124
00125 self.new_step_button = QPushButton(self.frame)
00126 self.new_step_button.setText("+")
00127 self.new_step_button.setFixedWidth(20)
00128 self.frame.connect(self.new_step_button, SIGNAL('clicked()'), self.add_step)
00129 self.widgets.append(self.new_step_button)
00130
00131 self.remove_step_button = QPushButton(self.frame)
00132 self.remove_step_button.setText("-")
00133 self.remove_step_button.setFixedWidth(20)
00134 self.frame.connect(self.remove_step_button, SIGNAL('clicked()'), self.remove_step)
00135 self.widgets.append(self.remove_step_button)
00136
00137 self.layout = QHBoxLayout()
00138 self.layout.setAlignment(Qt.AlignCenter)
00139 self.layout.setSpacing(2)
00140 self.layout.addWidget(label_grasp)
00141 self.layout.addWidget(self.list_grasp)
00142 self.layout.addWidget(label_pause)
00143 self.layout.addWidget(self.pause_input)
00144 self.layout.addWidget(label_interp)
00145 self.layout.addWidget(self.interp_input)
00146 self.layout.addWidget(label_looping)
00147 self.layout.addWidget(self.loop_input)
00148 self.layout.addWidget(self.number_loops)
00149 self.layout.addWidget(label_times)
00150 self.layout.addWidget(self.new_step_button)
00151 self.layout.addWidget(self.remove_step_button)
00152
00153 self.frame.setLayout(self.layout)
00154
00155 layout = QVBoxLayout()
00156 layout.addWidget(self.frame)
00157 self.frame.show()
00158 self.setLayout(layout)
00159
00160 self.show()
00161
00162 def grasp_choosed(self, grasp_name):
00163 self.grasp = self.parent.library.grasp_parser.grasps[str(grasp_name)]
00164
00165 def pause_changed(self, pause_time):
00166 self.pause_time = float(pause_time)
00167
00168 def interp_changed(self, interp_time):
00169 self.interpolation_time = float(interp_time)
00170
00171 def add_step(self):
00172 self.parent.add_step()
00173
00174 def number_loops_changed(self, number_loops):
00175 self.number_of_loops = int(number_loops)
00176
00177 def choose_looping(self, looping):
00178 if looping == "None":
00179 self.number_loops.setDisabled(True)
00180 self.number_loops.setText("0")
00181 self.loop_to_step = -1
00182 else:
00183 self.number_loops.setEnabled(True)
00184 self.number_loops.setText("1")
00185 self.loop_to_step = int(looping) - 1
00186
00187 def remove_step(self, delete_first=False):
00188 """
00189 Make sure we don't delete the first item from the GUI
00190 """
00191 if not delete_first:
00192 if len(self.parent.steps) <= 1:
00193 return
00194 self.close()
00195 self.parent.steps.remove(self)
00196 del self
00197
00198 def set_step_id(self, index):
00199 self.step_index = index
00200
00201 def is_playing(self):
00202 self.frame.setAutoFillBackground(True)
00203 self.frame.repaint()
00204
00205 def stopped_playing(self):
00206 self.frame.setAutoFillBackground(False)
00207 self.frame.repaint()
00208
00209 def save_to_xml(self):
00210 xml_step = ET.Element("step")
00211 grasp = ET.SubElement(xml_step, "grasp")
00212 grasp.set("name", self.grasp.grasp_name)
00213 pause = ET.SubElement(xml_step, "pause_time")
00214 pause.text = str(self.pause_time)
00215 interpolation = ET.SubElement(xml_step, "interpolation_time")
00216 interpolation.text = str(self.interpolation_time)
00217 looping = ET.SubElement(xml_step, "loop_to_step")
00218 looping.text = str(self.loop_to_step)
00219 nb_loops = ET.SubElement(xml_step, "number_loops")
00220 nb_loops.text = str(self.number_of_loops)
00221 return xml_step
00222
00223 def load_from_xml(self, xml_element):
00224 for subelement in xml_element:
00225 if subelement.tag == "grasp":
00226 grasp_name = subelement.attrib.get("name")
00227 self.grasp_choosed(grasp_name)
00228 list_grasps = self.parent.library.grasp_parser.grasps.keys()
00229 list_grasps.sort()
00230 for index, grasp_name_ref in zip(range(0, len(list_grasps)), list_grasps):
00231 if grasp_name == grasp_name_ref:
00232 self.list_grasp.setCurrentIndex(index)
00233 break
00234 if subelement.tag == "pause_time":
00235 self.pause_time = float(subelement.text)
00236 self.pause_input.setText(subelement.text)
00237
00238 if subelement.tag == "interpolation_time":
00239 self.interpolation_time = float(subelement.text)
00240 self.interp_input.setText(subelement.text)
00241
00242 if subelement.tag == "loop_to_step":
00243 self.loop_to_step = int(subelement.text)
00244 self.loop_input.setCurrentIndex(self.loop_to_step + 1)
00245 if self.loop_to_step == -1:
00246 self.number_loops.setDisabled(True)
00247 else:
00248 self.number_loops.setEnabled(True)
00249
00250 if subelement.tag == "number_loops":
00251 self.number_of_loops = int(subelement.text)
00252 self.number_loops.setText(subelement.text)
00253 self.grasp_slider = None
00254
00255 def refresh_list(self, value = 0):
00256 self.list_grasp.clear()
00257 self.parent.library.grasp_parser.refresh()
00258 list_grasps = self.parent.library.grasp_parser.grasps.keys()
00259 list_grasps.sort()
00260 for grasp_name in list_grasps:
00261 self.list_grasp.addItem(grasp_name)
00262
00263 class SignalWidget(QWidget):
00264 """
00265 Qt Signal used to state when a step is playing / stopped.
00266 """
00267 isPlayingSig = pyqtSignal(int)
00268 stoppedPlayingSig = pyqtSignal(int)
00269
00270 def __init__(self, parent=None):
00271 super(SignalWidget, self).__init__(parent)
00272
00273 class SrGuiMovementRecorder(Plugin):
00274 """
00275 A rosgui plugin for recording and replaying movements
00276 """
00277 def __init__(self, context):
00278 super(SrGuiMovementRecorder, self).__init__(context)
00279 self.setObjectName('SrGuiMovementRecorder')
00280
00281 self._publisher = None
00282 self._widget = QWidget()
00283
00284 ui_file = os.path.join(rospkg.RosPack().get_path('sr_gui_movement_recorder'), 'uis', 'SrGuiMovementRecorder.ui')
00285 loadUi(ui_file, self._widget)
00286 self._widget.setObjectName('SrGuiMovementRecorderUi')
00287 context.add_widget(self._widget)
00288
00289 self.library = ShadowHand_ROS()
00290
00291 self.frame = self._widget.frame
00292 self.timer = QTimer(self.frame)
00293
00294 self.layout = QVBoxLayout()
00295 self.layout.setSpacing(2)
00296
00297 self.layout.setAlignment(Qt.AlignCenter)
00298 self.layout.setSizeConstraint(QLayout.SetFixedSize)
00299
00300 self.sublayout = QGridLayout()
00301 self.command_frame = QFrame()
00302
00303 self.play_btn = QPushButton()
00304 self.play_btn.setText("Play")
00305 self.play_btn.setFixedWidth(80)
00306 self.command_frame.connect(self.play_btn, SIGNAL('clicked()'), self.button_play_clicked)
00307 self.sublayout.addWidget(self.play_btn, 0, 0)
00308
00309 self.signal_widget = SignalWidget(self.frame)
00310 self.signal_widget.isPlayingSig['int'].connect(self.started_playing)
00311 self.signal_widget.stoppedPlayingSig['int'].connect(self.stopped_playing)
00312
00313 self.mutex = threading.Lock()
00314 self.stopped = True
00315
00316 self.thread = None
00317
00318 self.stop_btn = QPushButton()
00319 self.stop_btn.setText("Stop")
00320 self.stop_btn.setFixedWidth(80)
00321 self.command_frame.connect(self.stop_btn, SIGNAL('clicked()'), self.stop)
00322 self.sublayout.addWidget(self.stop_btn, 0, 1)
00323
00324 self.sublayout.addWidget(QLabel(''), 0, 2)
00325
00326 self.save_btn = QPushButton()
00327 self.save_btn.setText("Save")
00328 self.save_btn.setFixedWidth(80)
00329 self.command_frame.connect(self.save_btn, SIGNAL('clicked()'), self.save)
00330 self.sublayout.addWidget(self.save_btn, 0, 3)
00331
00332 self.load_btn = QPushButton()
00333 self.load_btn.setText("Load")
00334 self.load_btn.setFixedWidth(80)
00335 self.command_frame.connect(self.load_btn, SIGNAL('clicked()'), self.load)
00336 self.sublayout.addWidget(self.load_btn, 0, 4)
00337
00338 self.command_frame.setLayout(self.sublayout)
00339 self.layout.addWidget(self.command_frame)
00340
00341 self.frame.setLayout(self.layout)
00342
00343 path_to_icons = os.path.join( rospkg.RosPack().get_path('sr_visualization_icons'), 'icons')
00344 self.load_btn.setIcon( QIcon( os.path.join( path_to_icons, 'load.png' )))
00345 self.save_btn.setIcon( QIcon( os.path.join( path_to_icons, 'save.png' )))
00346 self.stop_btn.setIcon( QIcon( os.path.join( path_to_icons, 'stop.png' )))
00347 self.play_btn.setIcon( QIcon( os.path.join( path_to_icons, 'play.png' )))
00348
00349 self.steps = []
00350 self.add_step()
00351
00352 def save(self):
00353 filename = QFileDialog.getSaveFileName(self.frame, 'Save Script', '')
00354 filename = filename[0]
00355
00356 if filename == "":
00357 return
00358
00359 root = ET.Element("movement")
00360 for step in self.steps:
00361 root.append(step.save_to_xml())
00362
00363 self.indent(root)
00364 tree = ET.ElementTree(root)
00365
00366 tree.write(filename)
00367
00368 def indent(self, elem, level=0):
00369 """
00370 print a prettier / indented xml tree
00371 """
00372 i = "\n" + level * " "
00373 if len(elem):
00374 if not elem.text or not elem.text.strip():
00375 elem.text = i + " "
00376 if not elem.tail or not elem.tail.strip():
00377 elem.tail = i
00378 for elem in elem:
00379 self.indent(elem, level + 1)
00380 if not elem.tail or not elem.tail.strip():
00381 elem.tail = i
00382 else:
00383 if level and (not elem.tail or not elem.tail.strip()):
00384 elem.tail = i
00385
00386 def load(self):
00387 """
00388 remove all the present steps
00389 and load new from xml file
00390 """
00391 filename = QFileDialog.getOpenFileName(self.frame, 'Open Script', '')
00392 filename = filename[0]
00393
00394 if filename != "":
00395 self.remove_all_steps()
00396
00397 tree = ET.parse(filename)
00398 root = tree.getroot()
00399 xml_steps = tree.findall("step")
00400
00401 for step in xml_steps:
00402 self.add_step()
00403 self.steps[-1].load_from_xml(step)
00404
00405 def remove_all_steps(self):
00406 while len(self.steps) != 0:
00407 self.steps[0].remove_step(delete_first=True)
00408
00409 def started_playing(self, index):
00410 self.steps[index].is_playing()
00411
00412 def stopped_playing(self, index):
00413 self.steps[index].stopped_playing()
00414
00415 def add_step(self):
00416 step_tmp = Step(self.frame, len(self.steps), self)
00417 self.steps.append(step_tmp)
00418 self.layout.addWidget(step_tmp)
00419 step_tmp.draw()
00420 for step, index in zip(self.steps, range(0, len(self.steps))):
00421 step.set_step_id(index)
00422
00423 def button_play_clicked(self):
00424 if len(self.steps) < 1:
00425 return
00426
00427 self.play_btn.setDisabled(True)
00428 self.load_btn.setDisabled(True)
00429
00430 for step in self.steps:
00431 step.remove_step_button.setDisabled(True)
00432 step.new_step_button.setDisabled(True)
00433 step.remaining_loops = step.number_of_loops
00434
00435 self.thread = threading.Thread(None, self.play)
00436 self.thread.start()
00437
00438 def play(self):
00439 self.stopped = False
00440 first_time = True
00441 index = 0
00442
00443 while index < len(self.steps):
00444 self.mutex.acquire()
00445 if self.stopped:
00446 self.mutex.release()
00447 return
00448 self.mutex.release()
00449
00450 step = self.steps[index]
00451 index = self.play_step(step, first_time, index)
00452 first_time = False
00453
00454 self.stop()
00455
00456 def stop(self):
00457 for step in self.steps:
00458 step.new_step_button.setEnabled(True)
00459 step.remove_step_button.setEnabled(True)
00460 self.mutex.acquire()
00461 self.stopped = True
00462 self.mutex.release()
00463 self.play_btn.setEnabled(True)
00464 self.load_btn.setEnabled(True)
00465
00466 def play_step(self, step, first_time, index):
00467 if first_time:
00468 self.current_step = step
00469 return index + 1
00470 next_step = step
00471
00472 self.signal_widget.isPlayingSig['int'].emit(index)
00473 self.move_step(next_step)
00474 self.signal_widget.stoppedPlayingSig['int'].emit(index)
00475
00476 self.current_step = next_step
00477
00478
00479 if step.loop_to_step != -1:
00480 if step.remaining_loops > 0:
00481 step.remaining_loops = step.remaining_loops - 1
00482 return step.loop_to_step
00483 return index + 1
00484
00485 def move_step(self, next_step):
00486 interpoler = self.library.create_grasp_interpoler(self.current_step.grasp, next_step.grasp)
00487 if self.current_step.interpolation_time == 0.0:
00488 self.library.sendupdate_from_dict(next_step.grasp.joints_and_positions)
00489 else:
00490 for interpolation in range (0, 10 * int(self.current_step.interpolation_time)):
00491 self.mutex.acquire()
00492 if self.stopped:
00493 self.mutex.release()
00494 return
00495 self.mutex.release()
00496
00497 targets_to_send = self.library.grasp_interpoler.interpolate(100.0 * interpolation / (10 * self.current_step.interpolation_time))
00498 self.library.sendupdate_from_dict(targets_to_send)
00499 time.sleep(0.1)
00500
00501 time.sleep(self.current_step.pause_time)
00502
00503 def _unregisterPublisher(self):
00504 if self._publisher is not None:
00505 self._publisher.unregister()
00506 self._publisher = None
00507
00508 def shutdown_plugin(self):
00509 self.remove_all_steps()
00510 self._unregisterPublisher()