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