class FrameEditor_StyleWidget(Interface):
def __init__(self, frame_editor):
self.editor = frame_editor
self.editor.observers.append(self)
self.old_frame = None
self.layout = QtWidgets.QGridLayout()
self.widget = QWidget()
self.widget.setLayout(self.layout)
self.mesh_label = QtWidgets.QLineEdit("File:")
self.mesh_label.setSizePolicy(QtWidgets.QSizePolicy.Ignored,
QtWidgets.QSizePolicy.Fixed)
self.mesh_button = QtWidgets.QPushButton("Open")
self.mesh_button.clicked.connect(self.btn_open_mesh_clicked)
self.diameter_label = QtWidgets.QLabel("Diameter:")
self.diameter_spinbox = QtWidgets.QDoubleSpinBox()
self.diameter_spinbox.editingFinished.connect(self.diameter_changed)
self.length_label = QtWidgets.QLabel("Length:")
self.length_spinbox = QtWidgets.QDoubleSpinBox()
self.length_spinbox.editingFinished.connect(self.length_changed)
self.width_label = QtWidgets.QLabel("Width:")
self.width_spinbox = QtWidgets.QDoubleSpinBox()
self.width_spinbox.editingFinished.connect(self.width_changed)
self.height_label = QtWidgets.QLabel("Height:")
self.height_spinbox = QtWidgets.QDoubleSpinBox()
self.height_spinbox.editingFinished.connect(self.height_changed)
self.color_label = QtWidgets.QLabel()
self.color_label.setAutoFillBackground(True)
self.update_color_label(None)
self.color_button = QtWidgets.QPushButton("Set Color")
self.color_button.clicked.connect(self.btn_color_clicked)
self.layout.addWidget(self.mesh_label, 0, 0)
self.layout.addWidget(self.mesh_button, 0, 1)
self.layout.addWidget(self.diameter_label, 1, 0)
self.layout.addWidget(self.diameter_spinbox, 1, 1)
self.layout.addWidget(self.length_label, 2, 0)
self.layout.addWidget(self.length_spinbox, 2, 1)
self.layout.addWidget(self.width_label, 3, 0)
self.layout.addWidget(self.width_spinbox, 3, 1)
self.layout.addWidget(self.height_label, 4, 0)
self.layout.addWidget(self.height_spinbox, 4, 1)
self.layout.addWidget(self.color_label, 5, 0)
self.layout.addWidget(self.color_button, 5, 1)
self.update_widget(None)
def get_widget(self):
return self.widget
def update(self, editor, level, elements):
if level & 2:
## Check for change
if self.editor.active_frame is not self.old_frame:
self.update_widget(self.editor.active_frame)
self.update_values(self.editor.active_frame)
self.update_color_label(self.editor.active_frame)
elif level & 4:
if self.editor.active_frame is not None:
self.update_values(self.editor.active_frame)
self.update_color_label(self.editor.active_frame)
def update_widget(self, frame):
## Clear layout
#while self.layout.count():
# child = self.layout.takeAt(0)
# child.widget().deleteLater()
self.mesh_label.hide()
self.mesh_button.hide()
self.diameter_label.hide()
self.diameter_spinbox.hide()
self.length_label.hide()
self.length_spinbox.hide()
self.width_label.hide()
self.width_spinbox.hide()
self.height_label.hide()
self.height_spinbox.hide()
if frame is None or frame.style == "none":
self.widget.setEnabled(False)
return
if frame.style == "mesh":
self.mesh_label.show()
self.mesh_button.show()
elif frame.style == "sphere":
self.diameter_label.show()
self.diameter_spinbox.show()
else:
self.length_label.show()
self.length_spinbox.show()
self.width_label.show()
self.width_spinbox.show()
if frame.style == "cube":
self.height_label.show()
self.height_spinbox.show()
self.widget.setEnabled(True)
def update_values(self, frame):
if frame is None or frame.style == "none":
return
if frame.style == "mesh":
self.mesh_label.setText(frame.path)
elif frame.style == "sphere":
self.diameter_spinbox.setValue(frame.diameter)
else:
self.length_spinbox.setValue(frame.length)
self.width_spinbox.setValue(frame.width)
if frame.style == "cube":
self.height_spinbox.setValue(frame.height)
def update_color_label(self, frame):
if frame is None:
values = "{}, {}, {}, {}".format(200, 200, 200, 255)
else:
values = "{}, {}, {}, {}".format(frame.color[0] * 255,
frame.color[1] * 255,
frame.color[2] * 255,
frame.color[3] * 255)
self.color_label.setStyleSheet("QLabel { background-color: rgba(" +
values + "); }")
@Slot(float)
def diameter_changed(self):
if self.editor.active_frame.diameter != self.diameter_spinbox.value():
self.editor.command(
Command_SetGeometry(self.editor, self.editor.active_frame,
"diameter", self.diameter_spinbox.value()))
@Slot(float)
def length_changed(self):
if self.editor.active_frame.length != self.length_spinbox.value():
self.editor.command(
Command_SetGeometry(self.editor, self.editor.active_frame,
"length", self.length_spinbox.value()))
@Slot(float)
def width_changed(self):
if self.editor.active_frame.width != self.width_spinbox.value():
self.editor.command(
Command_SetGeometry(self.editor, self.editor.active_frame,
"width", self.width_spinbox.value()))
@Slot(float)
def height_changed(self):
if self.editor.active_frame.height != self.height_spinbox.value():
self.editor.command(
Command_SetGeometry(self.editor, self.editor.active_frame,
"height", self.height_spinbox.value()))
@Slot()
def btn_open_mesh_clicked(self):
path = QtWidgets.QFileDialog.getOpenFileName(
None, 'Open Mesh', '/home', 'Mesh Files (*.stl *.dae)')[0]
self.editor.command(
Command_SetGeometry(self.editor, self.editor.active_frame, "path",
path))
@Slot(bool)
def btn_color_clicked(self, checked):
frame = self.editor.active_frame
color = QtWidgets.QColorDialog.getColor(
QtWidgets.QColor(frame.color[0] * 255, frame.color[1] * 255,
frame.color[2] * 255, frame.color[3] * 255),
None,
"Select Color",
options=QtWidgets.QColorDialog.ShowAlphaChannel)
self.editor.command(
Command_SetStyleColor(self.editor, frame, color.getRgbF()))
class FrameEditor_StyleWidget(Interface):
def __init__(self, frame_editor):
self.editor = frame_editor
self.editor.observers.append(self)
self.old_frame = None
self.layout = QtWidgets.QGridLayout()
self.widget = QWidget()
self.widget.setLayout(self.layout)
self.mesh_label = QtWidgets.QLineEdit("File:")
self.mesh_label.setSizePolicy(QtWidgets.QSizePolicy.Ignored,
QtWidgets.QSizePolicy.Fixed)
self.mesh_button = QtWidgets.QPushButton("Open")
self.mesh_button.clicked.connect(lambda: self.btn_open_mesh_clicked())
self.diameter_label = QtWidgets.QLabel("Diameter:")
self.diameter_spinbox = QtWidgets.QDoubleSpinBox()
self.diameter_spinbox.editingFinished.connect(
lambda: self.diameter_changed())
self.length_label = QtWidgets.QLabel("Length:")
self.length_spinbox = QtWidgets.QDoubleSpinBox()
self.length_spinbox.editingFinished.connect(
lambda: self.length_changed())
self.width_label = QtWidgets.QLabel("Width:")
self.width_spinbox = QtWidgets.QDoubleSpinBox()
self.width_spinbox.editingFinished.connect(
lambda: self.width_changed())
self.height_label = QtWidgets.QLabel("Height:")
self.height_spinbox = QtWidgets.QDoubleSpinBox()
self.height_spinbox.editingFinished.connect(
lambda: self.height_changed())
self.color_label = QtWidgets.QLabel()
self.color_label.setAutoFillBackground(True)
self.update_color_label(None)
self.color_button = QtWidgets.QPushButton("Set Color")
self.color_button.clicked.connect(lambda: self.btn_color_clicked())
self.layout.addWidget(self.mesh_label, 0, 0)
self.layout.addWidget(self.mesh_button, 0, 1)
self.layout.addWidget(self.diameter_label, 1, 0)
self.layout.addWidget(self.diameter_spinbox, 1, 1)
self.layout.addWidget(self.length_label, 2, 0)
self.layout.addWidget(self.length_spinbox, 2, 1)
self.layout.addWidget(self.width_label, 3, 0)
self.layout.addWidget(self.width_spinbox, 3, 1)
self.layout.addWidget(self.height_label, 4, 0)
self.layout.addWidget(self.height_spinbox, 4, 1)
self.layout.addWidget(self.color_label, 5, 0)
self.layout.addWidget(self.color_button, 5, 1)
print("init")
self.update_widget(None)
def get_widget(self):
return self.widget
def update(self, editor, level, elements):
if level & 2:
## Check for change
if self.editor.active_frame is not self.old_frame:
self.update_widget(self.editor.active_frame)
self.update_values(self.editor.active_frame)
self.update_color_label(self.editor.active_frame)
elif level & 4:
if self.editor.active_frame is not None:
self.update_values(self.editor.active_frame)
self.update_color_label(self.editor.active_frame)
def update_widget(self, frame):
## Clear layout
#while self.layout.count():
# child = self.layout.takeAt(0)
# child.widget().deleteLater()
self.mesh_label.hide()
self.mesh_button.hide()
self.diameter_label.hide()
self.diameter_spinbox.hide()
self.length_label.hide()
self.length_spinbox.hide()
self.width_label.hide()
self.width_spinbox.hide()
self.height_label.hide()
self.height_spinbox.hide()
if frame is None or frame.style == "none":
self.widget.setEnabled(False)
return
if frame.style == "mesh":
self.mesh_label.show()
self.mesh_button.show()
elif frame.style == "sphere":
self.diameter_label.show()
self.diameter_spinbox.show()
else:
self.length_label.show()
self.length_spinbox.show()
self.width_label.show()
self.width_spinbox.show()
if frame.style == "cube":
self.height_label.show()
self.height_spinbox.show()
self.widget.setEnabled(True)
def update_values(self, frame):
if frame is None or frame.style == "none":
return
if frame.style == "mesh":
self.mesh_label.setText(frame.path)
elif frame.style == "sphere":
self.diameter_spinbox.setValue(frame.diameter)
else:
self.length_spinbox.setValue(frame.length)
self.width_spinbox.setValue(frame.width)
if frame.style == "cube":
self.height_spinbox.setValue(frame.height)
def update_color_label(self, frame):
if frame is None:
values = "{}, {}, {}, {}".format(200, 200, 200, 255)
else:
values = "{}, {}, {}, {}".format(frame.color[0] * 255,
frame.color[1] * 255,
frame.color[2] * 255,
frame.color[3] * 255)
self.color_label.setStyleSheet("QLabel { background-color: rgba(" +
values + "); }")
@Slot(float)
def diameter_changed(self):
if self.editor.active_frame.diameter != self.diameter_spinbox.value():
self.editor.command(
Command_SetGeometry(self.editor, self.editor.active_frame,
"diameter", self.diameter_spinbox.value()))
@Slot(float)
def length_changed(self):
if self.editor.active_frame.length != self.length_spinbox.value():
self.editor.command(
Command_SetGeometry(self.editor, self.editor.active_frame,
"length", self.length_spinbox.value()))
@Slot(float)
def width_changed(self):
if self.editor.active_frame.width != self.width_spinbox.value():
self.editor.command(
Command_SetGeometry(self.editor, self.editor.active_frame,
"width", self.width_spinbox.value()))
@Slot(float)
def height_changed(self):
if self.editor.active_frame.height != self.height_spinbox.value():
self.editor.command(
Command_SetGeometry(self.editor, self.editor.active_frame,
"height", self.height_spinbox.value()))
@Slot(bool)
def btn_open_mesh_clicked(self):
path = QtWidgets.QFileDialog.getOpenFileName(None, 'Open Mesh',
'/home',
'Mesh Files (*.stl)')[0]
try:
rospackage = rospkg.get_package_name(path)
if rospackage is None:
QtWidgets.QMessageBox.warning(
self.widget, "Saving absolute path to mesh",
"Cannot find rospackage with selected mesh in it!\nSaving absolute path to mesh instead!"
)
#print("WARNING cannot find rospackage with mesh in it, saving absolute path")
self.editor.command(
Command_SetGeometry(self.editor, self.editor.active_frame,
"package", ""))
self.editor.command(
Command_SetGeometry(self.editor, self.editor.active_frame,
"path", path))
else:
rel_path = os.path.relpath(
path,
rospkg.RosPack().get_path(rospackage))
print("Saving: package: {} + relative path: {}".format(
rospackage, rel_path))
self.editor.command(
Command_SetGeometry(self.editor, self.editor.active_frame,
"package", rospackage))
self.editor.command(
Command_SetGeometry(self.editor, self.editor.active_frame,
"path", rel_path))
except:
QtWidgets.QMessageBox.warning(
self.widget, "Saving absolute path to mesh",
"The found rospackage with selected mesh in it is not sourced in your ROS workspace!\n"
+
"Cannot resolve the packagepath\nSaving absolute path to mesh instead!"
)
#print("The package found is not sourced withing the current workspace, saving absolute path instead!")
self.editor.command(
Command_SetGeometry(self.editor, self.editor.active_frame,
"package", ""))
self.editor.command(
Command_SetGeometry(self.editor, self.editor.active_frame,
"path", path))
@Slot(bool)
def btn_color_clicked(self):
frame = self.editor.active_frame
color = QtWidgets.QColorDialog.getColor(
QColor(frame.color[0] * 255, frame.color[1] * 255,
frame.color[2] * 255, frame.color[3] * 255),
None,
"Select Color",
options=QtWidgets.QColorDialog.ShowAlphaChannel)
self.editor.command(
Command_SetStyleColor(self.editor, frame, color.getRgbF()))
class Packml(Plugin):
def __init__(self, context):
super(Packml, self).__init__(context)
self.setObjectName('Packml')
from argparse import ArgumentParser
parser = ArgumentParser()
parser.add_argument("-q", "--quiet", action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
if not args.quiet:
print 'arguments: ', args
print 'unknowns: ', unknowns
# Create QWidget
self._widget = QWidget()
ui_file = os.path.join(rospkg.RosPack().get_path('packml_gui'), 'resource', 'packml.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('Packml')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
context.add_widget(self._widget)
# Custom code begins here
self._widget.reset_button.clicked[bool].connect(self.__handle_reset_clicked)
self._widget.start_button.clicked[bool].connect(self.__handle_start_clicked)
self._widget.stop_button.clicked[bool].connect(self.__handle_stop_clicked)
self._widget.clear_button.clicked[bool].connect(self.__handle_clear_clicked)
self._widget.hold_button.clicked[bool].connect(self.__handle_hold_clicked)
self._widget.unhold_button.clicked[bool].connect(self.__handle_unhold_clicked)
self._widget.suspend_button.clicked[bool].connect(self.__handle_suspend_clicked)
self._widget.unsuspend_button.clicked[bool].connect(self.__handle_unsuspend_clicked)
self._widget.abort_button.clicked[bool].connect(self.__handle_abort_clicked)
self._service_thread = Thread(target=self.wait_for_services, args=())
self._service_thread.start()
self._status_sub = rospy.Subscriber('packml/status', Status, self.status_callback)
self.threadpool = QThreadPool()
def disable_all_buttons(self):
self._widget.clear_button.setEnabled(False)
self._widget.reset_button.setEnabled(False)
self._widget.start_button.setEnabled(False)
self._widget.stop_button.setEnabled(False)
self._widget.hold_button.setEnabled(False)
self._widget.suspend_button.setEnabled(False)
self._widget.unhold_button.setEnabled(False)
self._widget.unsuspend_button.setEnabled(False)
self._widget.abort_button.setEnabled(False)
def set_message_text(self, text):
self._widget.message_box.setText("Message: " + text)
def status_callback(self, msg):
self.update_button_states(msg.state.val)
self.update_status_fields(msg)
def update_button_states(self, state):
self.disable_all_buttons()
if state == State.ABORTED:
self._widget.clear_button.setEnabled(True)
elif state == State.STOPPED:
self._widget.reset_button.setEnabled(True)
elif state == State.IDLE:
self._widget.start_button.setEnabled(True)
elif state == State.EXECUTE:
self._widget.hold_button.setEnabled(True)
self._widget.suspend_button.setEnabled(True)
elif state == State.HELD:
self._widget.unhold_button.setEnabled(True)
elif state == State.SUSPENDED:
self._widget.unsuspend_button.setEnabled(True)
elif state == State.COMPLETE:
self._widget.reset_button.setEnabled(True)
if state != State.STOPPED and \
state != State.STOPPING and \
state != State.ABORTED and \
state != State.ABORTING and \
state != State.CLEARING:
self._widget.stop_button.setEnabled(True)
if state != State.ABORTED and \
state != State.ABORTING:
self._widget.abort_button.setEnabled(True)
def update_status_fields(self, msg):
self.update_state_field(msg.state.val)
self._widget.substate.setText(str(msg.sub_state))
self.update_mode_field(msg.mode.val)
self._widget.error_code.setText(str(msg.error))
self._widget.suberror_code.setText(str(msg.sub_error))
def update_state_field(self, state):
if state == State.UNDEFINED:
self._widget.state_name.setText("UNDEFINED")
elif state == State.OFF:
self._widget.state_name.setText("OFF")
elif state == State.STOPPED:
self._widget.state_name.setText("STOPPED")
elif state == State.STARTING:
self._widget.state_name.setText("STARTING")
elif state == State.IDLE:
self._widget.state_name.setText("IDLE")
elif state == State.SUSPENDED:
self._widget.state_name.setText("SUSPENDED")
elif state == State.EXECUTE:
self._widget.state_name.setText("EXECUTE")
elif state == State.STOPPING:
self._widget.state_name.setText("STOPPING")
elif state == State.ABORTING:
self._widget.state_name.setText("ABORTING")
elif state == State.ABORTED:
self._widget.state_name.setText("ABORTED")
elif state == State.HOLDING:
self._widget.state_name.setText("HOLDING")
elif state == State.HELD:
self._widget.state_name.setText("HELD")
elif state == State.RESETTING:
self._widget.state_name.setText("RESETTING")
elif state == State.SUSPENDING:
self._widget.state_name.setText("SUSPENDING")
elif state == State.UNSUSPENDING:
self._widget.state_name.setText("UNSUSPENDING")
elif state == State.CLEARING:
self._widget.state_name.setText("CLEARING")
elif state == State.UNHOLDING:
self._widget.state_name.setText("UNHOLDING")
elif state == State.COMPLETING:
self._widget.state_name.setText("COMPLETING")
elif state == State.COMPLETE:
self._widget.state_name.setText("COMPLETE")
else:
self._widget.state_name.setTest("UNKNOWN")
def update_mode_field(self, mode):
if mode == Mode.UNDEFINED:
self._widget.mode_name.setText("UNDEFINED")
elif mode == Mode.AUTOMATIC:
self._widget.mode_name.setText("AUTOMATIC")
elif mode == Mode.SEMI_AUTOMATIC:
self._widget.mode_name.setText("SEMI-AUTOMATIC")
elif mode == Mode.MANUAL:
self._widget.mode_name.setText("MANUAL")
elif mode == Mode.IDLE:
self._widget.mode_name.setText("IDLE")
elif mode == Mode.SETUP:
self._widget.mode_name.setText("SETUP")
else:
self._widget.mode_name.setText("UNKNOWN")
def wait_for_services(self):
self._widget.setEnabled(False)
transition_service_name = 'packml/transition'
rospy.wait_for_service(transition_service_name, 30)
self.transition_service = rospy.ServiceProxy(transition_service_name, Transition)
self._widget.setEnabled(True)
def shutdown_plugin(self):
self._status_sub.unregister()
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
def handle_click_thread(self, request):
try:
service_thread = WorkerThread(self.transition_service, request, self.set_message_text)
if self.threadpool.activeThreadCount() >= 1:
return
else:
self.threadpool.start(service_thread)
except rospy.ServiceException as exc:
rospy.logerror("Service did not process request: " + str(exc))
def __handle_start_clicked(self, checked):
rospy.loginfo("Start button press")
self.handle_click_thread(TransitionRequest.START)
def __handle_stop_clicked(self, checked):
rospy.loginfo("Stop button press")
self.handle_click_thread(TransitionRequest.STOP)
def __handle_reset_clicked(self, checked):
rospy.loginfo("Reset button press")
self.handle_click_thread(TransitionRequest.RESET)
def __handle_clear_clicked(self, checked):
rospy.loginfo("Clear button press")
self.handle_click_thread(TransitionRequest.CLEAR)
def __handle_hold_clicked(self, checked):
rospy.loginfo("Hold button press")
self.handle_click_thread(TransitionRequest.HOLD)
def __handle_unhold_clicked(self, checked):
rospy.loginfo("Unhold button press")
self.handle_click_thread(TransitionRequest.UNHOLD)
def __handle_suspend_clicked(self, checked):
rospy.loginfo("Suspend button press")
self.handle_click_thread(TransitionRequest.SUSPEND)
def __handle_unsuspend_clicked(self, checked):
rospy.loginfo("Unsuspend button press")
self.handle_click_thread(TransitionRequest.UNSUSPEND)
def __handle_abort_clicked(self, checked):
rospy.loginfo("Abort button press")
self.handle_click_thread(TransitionRequest.ABORT)
@staticmethod
def add_arguments(parser):
rospy.loginfo("Add arguments callback")
group = parser.add_argument_group('Options for PackML plugin')
group.add_argument('--arg1', action='store_true', help='arg1 help')
