def on_reset_motors(self):
# if any of the breakers is not enabled ask if they'd like to enable them
if (self._dashboard_message is not None
and self._dashboard_message.power_board_state_valid):
all_breakers_enabled = reduce(lambda x, y: x and y, [
state == PowerBoardState.STATE_ON for state in
self._dashboard_message.power_board_state.circuit_state
])
if (not all_breakers_enabled):
if (QMessageBox.question(
self._breakers[0], self.tr('Enable Breakers?'),
self.
tr("Resetting the motors may not work because not all breakers are enabled. Enable all the breakers first?"
), QMessageBox.Yes | QMessageBox.No,
QMessageBox.Yes) == QMessageBox.Yes):
[breaker.set_enable() for breaker in self._breakers]
reset = rospy.ServiceProxy("pr2_etherCAT/reset_motors",
std_srvs.srv.Empty)
try:
reset()
except rospy.ServiceException, e:
QMessageBox.critical(
self._breakers[0], "Error",
"Failed to reset the motors: service call failed with error: %s"
% (e))
def mergeTracks(self, unused=None):
(track1Id, ok) = QInputDialog.getInteger(
self.widget, "Merge two tracks",
"Please enter the ID of the 1st person track you want to merge.")
if not ok:
return
(track2Id, ok) = QInputDialog.getInteger(
self.widget, "Merge two tracks",
"Please enter the ID of the 2nd person track you want to merge.")
if not ok:
return
if track1Id == track2Id:
QMessageBox.critical(self.widget, "Merge two tracks",
"Track IDs cannot be identical!")
return
if self.verifyTrackExists(track1Id) and self.verifyTrackExists(
track2Id):
self.editor.mergeTracks(track1Id, track2Id)
self.updateTrackCount()
QMessageBox.information(
self.widget, "Merge two tracks",
"Person tracks %d and %d have been merged!" %
(track1Id, track2Id))
def verifyTrackExists(self, trackId):
if not trackId in self.database.getTrackIds():
QMessageBox.critical(
self.widget, "Wrong track ID",
"Person track with ID %d does not exist!" % trackId)
return False
return True
def _handle_remove_clicked(self):
name = self.name_edit.text()
if name in self.places_dict.keys():
self.places_dict.pop(name)
for item in self._sceneItems[name].keys():
self._scene.removeItem(self._sceneItems[name][item])
else:
QMessageBox.critical(self, "Error!", "Place does not exist.")
def reset_motorstop_cb(self):
reset_srv = rospy.ServiceProxy("/reset_motorstop", ResetMotorStop)
try:
reset_srv()
except rospy.ServiceException, e:
QMessageBox.critical(
self._drive, "Error",
"Failed to halt the motors: service call failed with error: %s"
% (e))
def enable_motorstop_cb(self):
stop_srv = rospy.ServiceProxy("/emergency_stop", EmergencyStop)
try:
stop_srv()
except rospy.ServiceException, e:
QMessageBox.critical(
self._drive, "Error",
"Failed to emergency stop: service call failed with error: %s"
% (e))
def on_halt_motors(self):
halt = rospy.ServiceProxy(
"{}/halt_motors".format(self._motor_namespace), std_srvs.srv.Empty)
try:
halt()
except rospy.ServiceException as e:
QMessageBox.critical(
self._motors, "Error",
"Failed to halt the motors: service call failed with error: %s"
% (e))
def on_halt_motors(self):
halt = rospy.ServiceProxy("pr2_etherCAT/halt_motors",
std_srvs.srv.Empty)
try:
halt()
except rospy.ServiceException, e:
QMessageBox.critical(
self._motors, "Error",
"Failed to halt the motors: service call failed with error: %s"
% (e))
def freerun_cb(self):
self._freerun_state = not self._freerun_state
freerun_srv = rospy.ServiceProxy("/enable_motors", EnableMotors)
try:
freerun_srv(not self._freerun_state)
except rospy.ServiceException, e:
QMessageBox.critical(
self._drive, "Error",
"Failed to enter freerun: service call failed with error: %s" %
(e))
def _handle_get_clicked(self):
name = self.name_edit.text()
if (name in self.places_dict.keys()):
self.x_spin.setValue(self.places_dict[name][0])
self.y_spin.setValue(self.places_dict[name][1])
theta = euler_from_quaternion(
numpy.asarray(self.places_dict[name][3:7]))[2]
self.theta_spin.setValue(degrees(theta))
else:
QMessageBox.critical(self, "Error!",
"This places has not been added.")
def on_reconnect_clicked(self):
if (self._dashboard_message is not None):
reconnect = rospy.ServiceProxy("/reconnect", std_srvs.srv.Empty)
try:
reconnect()
except rospy.ServiceException, e:
QMessageBox.critical(
self.ethercat, "Error",
"Failed to reconnect the driver: service call failed with error: %s"
% (e))
def on_saveButton_clicked(self):
'''
Saves the current document. This method is called if the C{save button}
was clicked.
'''
saved, errors, msg = self.tabWidget.currentWidget().save(True)
if errors:
QMessageBox.critical(self, "Error", msg)
self.tabWidget.setTabIcon(self.tabWidget.currentIndex(), self._error_icon)
self.tabWidget.setTabToolTip(self.tabWidget.currentIndex(), msg)
elif saved:
self.tabWidget.setTabIcon(self.tabWidget.currentIndex(), self._empty_icon)
self.tabWidget.setTabToolTip(self.tabWidget.currentIndex(), '')
def on_saveButton_clicked(self):
'''
Saves the current document. This method is called if the C{save button}
was clicked.
'''
saved, errors, msg = self.tabWidget.currentWidget().save(True)
if errors:
QMessageBox.critical(self, "Error", msg)
self.tabWidget.setTabIcon(self.tabWidget.currentIndex(), self._error_icon)
self.tabWidget.setTabToolTip(self.tabWidget.currentIndex(), msg)
elif saved:
self.tabWidget.setTabIcon(self.tabWidget.currentIndex(), self._empty_icon)
self.tabWidget.setTabToolTip(self.tabWidget.currentIndex(), '')
def _handle_add_clicked(self):
name = self.name_edit.text()
if len(name) != 0:
# remove and re-draw it
if name in self.places_dict.keys():
self.places_dict.pop(name)
for item in self._sceneItems[name].keys():
self._scene.removeItem(self._sceneItems[name][item])
try:
# storing the values in the dict
x = self.x_spin.value()
y = self.y_spin.value()
theta = self.theta_spin.value()
q = quaternion_from_euler(0.0, 0.0, theta)
self.places_dict[str(name)] = [
x, y, 0.0,
float(q[0]),
float(q[1]),
float(q[2]),
float(q[3])
]
# drawing the items
self._sceneItems[name] = {"text": QGraphicsTextItem()}
self._sceneItems[name]["text"].setDefaultTextColor(
QColor(0, 0, 255))
self._sceneItems[name]["text"].setFont(QFont("Times", 10))
self._sceneItems[name]["text"].setPlainText(name)
scene_pos = self._gridToScene(x, y, q)
x_c = scene_pos[0] - self._sceneItems[name][
"text"].boundingRect().width() / 2.0
self._sceneItems[name]["text"].setPos(x_c, scene_pos[1])
self._scene.addItem(self._sceneItems[name]["text"])
self._sceneItems[name]["rec"] = self._scene.addRect(
QRectF((scene_pos[0] - 2), (scene_pos[1] - 2), 4, 4))
line = QLineF(
scene_pos[0], scene_pos[1],
(scene_pos[0] - self._mag * cos(radians(scene_pos[3]))),
(scene_pos[1] + self._mag * sin(radians(scene_pos[3]))))
self._sceneItems[name]["line"] = self._scene.addLine(
line, pen=QPen(Qt.red, 2))
except ValueError:
QMessageBox.critical(self, "Error!",
"You must insert a valid value.")
else:
QMessageBox.critical(
self, "Error!",
"You have to insert a name and a valid position.")
def focusInEvent(self, event):
# check for file changes
try:
if self.filename and self.file_info:
if self.file_info.lastModified() != QFileInfo(self.filename).lastModified():
self.file_info = QFileInfo(self.filename)
result = QMessageBox.question(self, "File changed", "File was changed, reload?", QMessageBox.Yes | QMessageBox.No)
if result == QMessageBox.Yes:
f = QFile(self.filename)
if f.open(QIODevice.ReadOnly | QIODevice.Text):
self.setText(unicode(f.readAll(), "utf-8"))
self.document().setModified(False)
self.textChanged.emit()
else:
QMessageBox.critical(self, "Error", "Cannot open launch file%s" % self.filename)
except:
pass
QTextEdit.focusInEvent(self, event)
def _handle_load_clicked(self):
filename = QFileDialog.getOpenFileName(
self, self.tr('Load from File'), '.',
self.tr('YAML files {.yaml} (*.yaml)'))
if filename[0] != '':
with open(filename[0], 'r') as infile:
try:
self.places_dict = yaml.load(infile)
for k in self._sceneItems.keys():
for item in self._sceneItems[k].keys():
self._scene.removeItem(self._sceneItems[k][item])
for name in self.places_dict.keys():
#
q = numpy.asarray(self.places_dict[name][3:7])
scene_pos = self._gridToScene(
self.places_dict[name][0],
self.places_dict[name][1], q)
# drawing the items
self._sceneItems[name] = {"text": QGraphicsTextItem()}
self._sceneItems[name]["text"].setDefaultTextColor(
QColor(0, 0, 255))
self._sceneItems[name]["text"].setFont(
QFont("Times", 10))
self._sceneItems[name]["text"].setPlainText(name)
x_c = scene_pos[0] - self._sceneItems[name][
"text"].boundingRect().width() / 2.0
self._sceneItems[name]["text"].setPos(
x_c, scene_pos[1])
self._scene.addItem(self._sceneItems[name]["text"])
self._sceneItems[name]["rec"] = self._scene.addRect(
QRectF((scene_pos[0] - 2), (scene_pos[1] - 2), 4,
4))
line = QLineF(scene_pos[0], scene_pos[1],
(scene_pos[0] -
self._mag * cos(radians(scene_pos[3]))),
(scene_pos[1] +
self._mag * sin(radians(scene_pos[3]))))
self._sceneItems[name]["line"] = self._scene.addLine(
line, pen=QPen(Qt.red, 2))
except yaml.scanner.ScannerError:
QMessageBox.critical(self, "Error!", "Invalid YAML file.")
def msg_type_changed(self, type_name):
if not type_name:
if hasattr(self, 'message_info'):
self._widget.msg_type_combo_box.setEditText(
self.message_info['type_name'])
return # initialization with empty string; just ignore
if self._create_message_instance(type_name) is None:
QMessageBox.critical(self._widget, 'Change Message Type',
'Unrecognized message type', QMessageBox.Ok)
if hasattr(self, 'message_info'):
self._widget.msg_type_combo_box.setEditText(
self.message_info['type_name'])
else:
self._widget.msg_type_combo_box.setEditText('')
return
if hasattr(self, 'message_info'):
if self.message_info['type_name'] == type_name:
return # selected same type as current, just ignore
answer = QMessageBox.question(self._widget, 'Change Message Type',
'Are you sure you want to change current message type?\n'\
'All changes will be discarded!', QMessageBox.Ok, QMessageBox.Cancel)
if answer != QMessageBox.Ok:
self._widget.msg_type_combo_box.setEditText(
self.message_info['type_name'])
return
self.clean_up_message()
self.message_info = {
'type_name': str(type_name),
'instance': self._create_message_instance(str(type_name))
}
# Ask for filling the message with annotation's fields likely to be the same
answer = QMessageBox.question(self._widget, "Fill Message's Fields",
"Do you want to copy matching fields from the annotation?\n" \
"You can change the suggested values later", QMessageBox.Yes, QMessageBox.No)
if answer == QMessageBox.Yes:
self._set_message(self.message_info, self.annotation)
else:
self._set_message(self.message_info)
def msg_type_changed(self, type_name):
if not type_name:
if hasattr(self, 'message_info'):
self._widget.msg_type_combo_box.setEditText(self.message_info['type_name'])
return # initialization with empty string; just ignore
if self._create_message_instance(type_name) is None:
QMessageBox.critical(self._widget, 'Change Message Type',
'Unrecognized message type', QMessageBox.Ok)
if hasattr(self, 'message_info'):
self._widget.msg_type_combo_box.setEditText(self.message_info['type_name'])
else:
self._widget.msg_type_combo_box.setEditText('')
return
if hasattr(self, 'message_info'):
if self.message_info['type_name'] == type_name:
return # selected same type as current, just ignore
answer = QMessageBox.question(self._widget, 'Change Message Type',
'Are you sure you want to change current message type?\n'\
'All changes will be discarded!', QMessageBox.Ok, QMessageBox.Cancel)
if answer != QMessageBox.Ok:
self._widget.msg_type_combo_box.setEditText(self.message_info['type_name'])
return
self.clean_up_message()
self.message_info = {
'type_name': str(type_name),
'instance': self._create_message_instance(str(type_name))
}
# Ask for filling the message with annotation's fields likely to be the same
answer = QMessageBox.question(self._widget, "Fill Message's Fields",
"Do you want to copy matching fields from the annotation?\n" \
"You can change the suggested values later", QMessageBox.Yes, QMessageBox.No)
if answer == QMessageBox.Yes:
self._set_message(self.message_info, self.annotation)
else:
self._set_message(self.message_info)
def focusInEvent(self, event):
# check for file changes
try:
if self.filename and self.file_info:
if self.file_info.lastModified() != QFileInfo(
self.filename).lastModified():
self.file_info = QFileInfo(self.filename)
result = QMessageBox.question(
self, "File changed", "File was changed, reload?",
QMessageBox.Yes | QMessageBox.No)
if result == QMessageBox.Yes:
f = QFile(self.filename)
if f.open(QIODevice.ReadOnly | QIODevice.Text):
self.setText(unicode(f.readAll(), "utf-8"))
self.document().setModified(False)
self.textChanged.emit()
else:
QMessageBox.critical(
self, "Error",
"Cannot open launch file%s" % self.filename)
except:
pass
QTextEdit.focusInEvent(self, event)
def _handle_read_map(self):
try:
self.map = rospy.wait_for_message("/map", OccupancyGrid, timeout=5)
self.add_button.setEnabled(True)
self.load_button.setEnabled(True)
self.save_button.setEnabled(True)
self.name_edit.setEnabled(True)
self.x_spin.setEnabled(True)
self.y_spin.setEnabled(True)
self.theta_spin.setEnabled(True)
self.get_button.setEnabled(True)
self.remove_button.setEnabled(True)
self.graphics_view.setEnabled(True)
self.graphics_view.setTransformationAnchor(
QGraphicsView.AnchorUnderMouse)
self.graphics_view.setDragMode(QGraphicsView.ScrollHandDrag)
self.graphics_view.setInteractive(True)
self.map_read.emit()
except rospy.exceptions.ROSException:
QMessageBox.critical(
self, "Error!",
"Unable to read the map. Make sure to run the map_server node."
)
def control(self, breaker, cmd):
"""
Sends a PowerBoardCommand srv to the pr2
:param breaker: breaker index to send command to
:type breaker: int
:param cmd: command to be sent to the pr2 breaker
:type cmd: str
"""
if (not self._power_board_state):
QMessageBox.critical(
self, "Error",
self.
tr("Cannot control breakers until we have received a power board state message"
))
return False
if (not self._power_board_state.run_stop
or not self._power_board_state.wireless_stop):
if (cmd == "start"):
QMessageBox.critical(
self, "Error",
self.
tr("Breakers will not enable because one of the runstops is pressed"
))
return False
try:
power_cmd = PowerBoardCommandRequest()
power_cmd.breaker_number = breaker
power_cmd.command = cmd
power_cmd.serial_number = self._serial
self._power_control(power_cmd)
return True
except rospy.ServiceException as e:
QMessageBox.critical(self, "Error",
"Service call failed with error: %s" % (e),
"Error")
return False
return False
def on_motors_clicked(self, button_handle, component_name, data_index,
mode_name):
if (self._dashboard_message is not None
and self._dashboard_message.power_board_state_valid):
# if mode == "Off"
ok_state = PowerBoardState.STATE_ENABLED
switch_state = PowerBoardState.STATE_STANDBY
if (mode_name == "On"):
ok_state = PowerBoardState.STATE_STANDBY
switch_state = PowerBoardState.STATE_ENABLED
if (self._dashboard_message.power_board_state.
circuit_state[data_index] == ok_state):
switch_motor_state = rospy.ServiceProxy(
"/" + component_name + "/switch" + mode_name + "Motors",
std_srvs.srv.Empty)
try:
switch_motor_state()
except rospy.ServiceException, e:
QMessageBox.critical(
button_handle, "Error", "Failed to switch " +
mode_name + " " + component_name +
" motors: service call failed with error: %s" % (e))
elif (self._dashboard_message.power_board_state.
circuit_state[data_index] == switch_state):
QMessageBox.critical(
button_handle, "Error", component_name +
" motors are already switched " + mode_name)
elif (self._dashboard_message.power_board_state.
circuit_state[data_index] == PowerBoardState.STATE_DISABLED):
QMessageBox.critical(button_handle, "Error",
component_name + " is not connected")
