def __init__(self,
options,
title='Exclusive Options',
selected_index=None):
super(ExclusiveOptionGroup, self).__init__()
self.setTitle(title)
self.setLayout(QVBoxLayout())
self._button_group = QButtonGroup()
self._button_group.setExclusive(True)
self._options = options
button_id = 0
for option in self._options:
button_id += 1
radio_button = QRadioButton(
option.get('title', 'option %d' % button_id))
radio_button.setEnabled(option.get('enabled', True))
radio_button.setChecked(
option.get('selected', False)
or (button_id - 1) == selected_index)
radio_button.setToolTip(option.get('tooltip', ''))
widget = QWidget()
widget.setLayout(QVBoxLayout())
widget.layout().addWidget(radio_button)
if 'description' in option:
widget.layout().addWidget(QLabel(option['description']))
self._button_group.addButton(radio_button, button_id)
self.layout().addWidget(widget)
class PyConsole(Plugin):
"""
Plugin providing an interactive Python console
"""
def __init__(self, context):
super(PyConsole, self).__init__(context)
self.setObjectName('PyConsole')
self._context = context
self._use_spyderlib = _has_spyderlib
self._console_widget = None
self._widget = QWidget()
self._widget.setLayout(QVBoxLayout())
self._widget.layout().setContentsMargins(0, 0, 0, 0)
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + \
(' (%d)' % context.serial_number()))
self._context.add_widget(self._widget)
def _switch_console_widget(self):
self._widget.layout().removeWidget(self._console_widget)
self.shutdown_console_widget()
if _has_spyderlib and self._use_spyderlib:
self._console_widget = SpyderConsoleWidget(self._context)
self._widget.setWindowTitle('SpyderConsole')
else:
self._console_widget = PyConsoleWidget(self._context)
self._widget.setWindowTitle('PyConsole')
if self._context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % self._context.serial_number()))
self._widget.layout().addWidget(self._console_widget)
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('use_spyderlib', self._use_spyderlib)
def restore_settings(self, plugin_settings, instance_settings):
self._use_spyderlib = _has_spyderlib and (instance_settings.value('use_spyderlib', True) in [True, 'true'])
self._switch_console_widget()
def trigger_configuration(self):
options = [
{'title': 'SpyderConsole', 'description': 'Advanced Python console with tab-completion (needs spyderlib to be installed).', 'enabled': _has_spyderlib},
{'title': 'PyConsole', 'description': 'Simple Python console.'},
]
dialog = SimpleSettingsDialog(title='PyConsole Options')
dialog.add_exclusive_option_group(title='Console Type', options=options, selected_index=int(not self._use_spyderlib))
console_type = dialog.get_settings()[0]
new_use_spyderlib = {0: True, 1: False}.get(console_type['selected_index'], self._use_spyderlib)
if self._use_spyderlib != new_use_spyderlib:
self._use_spyderlib = new_use_spyderlib
self._switch_console_widget()
def shutdown_console_widget(self):
if self._console_widget is not None and hasattr(self._console_widget, 'shutdown'):
self._console_widget.shutdown()
def shutdown_plugin(self):
self.shutdown_console_widget()
class PyConsole(Plugin):
"""
Plugin providing an interactive Python console
"""
def __init__(self, context):
super(PyConsole, self).__init__(context)
self.setObjectName('PyConsole')
self._context = context
self._use_spyderlib = _has_spyderlib
self._console_widget = None
self._widget = QWidget()
self._widget.setLayout(QVBoxLayout())
self._widget.layout().setContentsMargins(0, 0, 0, 0)
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
self._context.add_widget(self._widget)
def _switch_console_widget(self):
self._widget.layout().removeWidget(self._console_widget)
self.shutdown_console_widget()
if _has_spyderlib and self._use_spyderlib:
self._console_widget = SpyderConsoleWidget(self._context)
self._widget.setWindowTitle('SpyderConsole')
else:
self._console_widget = PyConsoleWidget(self._context)
self._widget.setWindowTitle('PyConsole')
if self._context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % self._context.serial_number()))
self._widget.layout().addWidget(self._console_widget)
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('use_spyderlib', self._use_spyderlib)
def restore_settings(self, plugin_settings, instance_settings):
self._use_spyderlib = _has_spyderlib and (instance_settings.value('use_spyderlib', True) in [True, 'true'])
self._switch_console_widget()
def trigger_configuration(self):
options = [
{'title': 'SpyderConsole', 'description': 'Advanced Python console with tab-completion (needs spyderlib to be installed).', 'enabled': _has_spyderlib},
{'title': 'PyConsole', 'description': 'Simple Python console.'},
]
dialog = SimpleSettingsDialog(title='PyConsole Options')
dialog.add_exclusive_option_group(title='Console Type', options=options, selected_index=int(not self._use_spyderlib))
console_type = dialog.get_settings()[0]
new_use_spyderlib = {0: True, 1: False}.get(console_type['selected_index'], self._use_spyderlib)
if self._use_spyderlib != new_use_spyderlib:
self._use_spyderlib = new_use_spyderlib
self._switch_console_widget()
def shutdown_console_widget(self):
if self._console_widget is not None and hasattr(self._console_widget, 'shutdown'):
self._console_widget.shutdown()
def shutdown_plugin(self):
self.shutdown_console_widget()
class RqtCalibrationEvaluator(Plugin):
def __init__(self, context):
super(RqtCalibrationEvaluator, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('CalibrationEvaluator')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
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()
self._infoWidget = QWidget()
# Get path to UI file which should be in the "resource" folder of this package
ui_file = os.path.join(rospkg.RosPack().get_path('rqt_easy_handeye'),
'resource', 'rqt_handeye_evaluator.ui')
ui_info_file = os.path.join(
rospkg.RosPack().get_path('rqt_easy_handeye'), 'resource',
'rqt_handeye_info.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
loadUi(ui_info_file, self._infoWidget)
self._widget.layout().insertWidget(0, self._infoWidget)
# Give QObjects reasonable names
self._widget.setObjectName('RqtHandeyeCalibrationUI')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
self._infoWidget.calibNameLineEdit.setText(rospy.get_namespace())
if rospy.get_param('eye_on_hand', False):
self._infoWidget.calibTypeLineEdit.setText("eye on hand")
else:
self._infoWidget.calibTypeLineEdit.setText("eye on base")
self._infoWidget.trackingBaseFrameLineEdit.setText(
rospy.get_param('tracking_base_frame', 'optical_origin'))
self._infoWidget.trackingMarkerFrameLineEdit.setText(
rospy.get_param('tracking_marker_frame', 'optical_target'))
self._infoWidget.robotBaseFrameLineEdit.setText(
rospy.get_param('robot_base_frame', 'base_link'))
self._infoWidget.robotEffectorFrameLineEdit.setText(
rospy.get_param('robot_effector_frame', 'tool0'))
self.output_label = self._widget.label_message
self.output_label.setText('Waiting for samples...')
self._widget.pushButton_reset.clicked.connect(self.reset)
self.is_eye_on_hand = rospy.get_param('~eye_on_hand')
self.robot_base_frame = rospy.get_param('~robot_base_frame')
self.robot_effector_frame = rospy.get_param('~robot_effector_frame')
self.tracking_measurement_frame = rospy.get_param(
'~tracking_marker_frame')
if self.is_eye_on_hand:
self.robot_measurement_frame = self.robot_base_frame
else:
self.robot_measurement_frame = self.robot_effector_frame
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer)
self.update_timer = QTimer(self)
self.update_timer.timeout.connect(self.tick)
self.update_timer.start(500)
self.last_robot_transform = None # used to see if we are in a steady state
self.measurement_transforms = [
] # used to measure the quality of the calibration: should have the same value
self.robot_transforms = [
] # used to determine when to sample: we wait for a steady state that has not been sampled yet
self._widget.show()
def tick(self):
# wait for steady state to avoid problems with lag
# if next to a copy already in the dataset, tell to move
# if could not sample transform, tell how old the last one is
# show average error; show dataset in 3D?
try:
new_robot_transform = self.tf_buffer.lookup_transform(
self.robot_base_frame, self.robot_effector_frame,
rospy.Time.now(), rospy.Duration.from_sec(0.2))
new_measurement_transform = self.tf_buffer.lookup_transform(
self.robot_measurement_frame, self.tracking_measurement_frame,
rospy.Time.now(), rospy.Duration.from_sec(0.2))
if new_robot_transform is None:
rospy.logwarn(
'Could not sample transform between {} and {}'.format(
self.robot_base_frame, self.robot_effector_frame))
if new_measurement_transform is None:
rospy.logwarn(
'Could not sample transform between {} and {}'.format(
self.robot_measurement_frame,
self.tracking_measurement_frame))
if self.last_robot_transform is None:
self.last_robot_transform = new_robot_transform
self.updateUI()
rospy.loginfo('Sampled first transform')
return
if RqtCalibrationEvaluator.transform_too_far(
new_robot_transform,
self.last_robot_transform,
absolute_tolerance=0.001):
self.last_robot_transform = new_robot_transform
msg = 'Waiting for steady state'
rospy.loginfo(msg)
self.output_label.setText(msg)
return
if self.robot_transform_is_too_close_to_previous_sample(
new_robot_transform, absolute_tolerance=0.003):
self.updateUI()
rospy.loginfo(
'Now we have {} samples\ntoo close to an old pose, move around!'
.format(len(self.measurement_transforms)))
self.output_label.setText(
'Too close to an old pose, move around!')
return
self.robot_transforms.append(new_robot_transform)
self.measurement_transforms.append(new_measurement_transform)
rospy.loginfo('Appending transform; we got {} now'.format(
len(self.measurement_transforms)))
self.updateUI()
# TODO: provide feedback if the data is sufficient
except (LookupException, ExtrapolationException,
ConnectivityException) as e:
msg = 'Could not sample pose!'
rospy.loginfo(msg)
rospy.logerr(e)
self.output_label.setText(msg)
def reset(self):
self.robot_transforms = []
self.measurement_transforms = []
self.updateUI()
def updateUI(self):
self._widget.spinBox_samples.setValue(len(self.measurement_transforms))
if len(self.measurement_transforms) > 2:
def translation_from_msg(msg):
t = msg.transform.translation
return t.x, t.y, t.z
translations = [
translation_from_msg(t) for t in self.measurement_transforms
]
translations_np = np.array(translations)
translations_avg = translations_np.mean(axis=0)
translations_from_avg = translations_np - translations_avg
translations_max_divergence = np.max(translations_from_avg)
rospy.loginfo(
"Maximum divergence: {}".format(translations_max_divergence))
self._widget.doubleSpinBox_error.setEnabled(True)
self._widget.doubleSpinBox_error.setValue(
translations_max_divergence.max())
else:
self._widget.doubleSpinBox_error.setValue(0)
self._widget.doubleSpinBox_error.setEnabled(False)
@staticmethod
def transform_to_concatenated_translation_quaternion(transform):
tr = transform.transform.translation
quat = transform.transform.rotation
return np.array([tr.x, tr.y, tr.z, quat.x, quat.y, quat.z, quat.w])
def robot_transform_is_too_close_to_previous_sample(
self, new_robot_transform, absolute_tolerance):
# TODO: use a meaningful metric
posevec = RqtCalibrationEvaluator.transform_to_concatenated_translation_quaternion(
new_robot_transform)
for t in reversed(self.robot_transforms):
old_posevec = RqtCalibrationEvaluator.transform_to_concatenated_translation_quaternion(
t)
if np.allclose(posevec, old_posevec, atol=absolute_tolerance):
return True
return False
@staticmethod
def transform_too_far(t1, t2, absolute_tolerance):
# TODO: use a meaningful metric
return not np.allclose(
RqtCalibrationEvaluator.
transform_to_concatenated_translation_quaternion(t1),
RqtCalibrationEvaluator.
transform_to_concatenated_translation_quaternion(t2),
atol=absolute_tolerance)
def shutdown_plugin(self):
# TODO unregister all publishers here
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
class VelocityControl(Plugin):
signal_topic = Signal(JointState)
def __init__(self, context):
super(VelocityControl, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('VelocityControl')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q", "--quiet", action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
# Create QWidget
self._widget = QWidget()
# Get path to UI file which is a sibling of this file
# in this example the .ui and .py file are in the same folder
#ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'MyPlugin.ui')
# Extend the widget with all attributes and children from UI file
#loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('VelocityControlUi')
vLayout = QVBoxLayout(self._widget)
vLayout.setContentsMargins(0, 0, 0, 0)
vLayout.setSpacing(0)
self._widget.setLayout(vLayout)
self._widget.layout().setSpacing(0)
self._widget.setWindowTitle("VelocityControl");
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
self.context = context
self._topic_list = ''
self._topic_commands = {}
self._jointgroups = {}
self.signal_topic.connect( self.signal_callback_list )
rospy.on_shutdown(self.on_ros_shutdown) #handle the ROS shutdown commands
def on_ros_shutdown(self, *args):
from python_qt_binding.QtGui import QApplication
QApplication.exit(0)
def shutdown_plugin(self):
# TODO unregister all publishers here
self.shutdownRosComm()
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
self._topic_commands.clear()
for caller, joint_frame in self._jointgroups.items():
self._topic_commands[caller] = joint_frame.get_command_topic()
instance_settings.set_value('topic_commands', self._topic_commands)
instance_settings.set_value('topic_list', self._topic_list)
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
self.shutdownRosComm()
self._topic_commands = instance_settings.value('topic_commands', {})
self._topic_list = instance_settings.value('topic_list', 'joint_state')
self.fill_widget()
self.reinitRosComm()
def reinitRosComm(self):
# if self._topic_command:
# self._topic_command = rosgraph.names.script_resolve_name('rostopic', self._topic_command)
# if (self._topic_command):
# self._publisher_command = rospy.Publisher(self._topic_command, PowerSwitch, queue_size=1)
if self._topic_list:
self._topic_list = rosgraph.names.script_resolve_name('rostopic', self._topic_list)
if self._topic_list:
self._subscriber_list = rospy.Subscriber(self._topic_list, JointState, self.callback_jointstate)
def clearstuff(self):
pass
# for name,p in self._devices.items():
# self._widget.layout().removeWidget(p)
# p.clearParent()
# self._devices.clear();
def shutdownRosComm(self):
self.clearstuff()
# if self._topic_command:
# self._publisher_command.unregister()
# self._publisher_command = None
if self._topic_list:
self._subscriber_list.unregister()
self._subscriber_list = None
def fill_widget(self):
# for i in range(len(self._topics)):
self._widget.layout().addItem(QSpacerItem(1,1,QSizePolicy.Expanding, QSizePolicy.Expanding))
def callback_jointstate(self, msg):
self.signal_topic.emit(msg)
def signal_callback_list( self, msg ):
# update
caller = msg._connection_header['callerid']
if caller not in self._jointgroups:
stored_topic = None
if caller in self._topic_commands:
stored_topic = self._topic_commands[caller]
group_frame = JointStateGroup(caller, msg, stored_topic=stored_topic)
self._widget.layout().insertWidget(self._widget.layout().count() - 1, group_frame)
self._jointgroups[caller] = group_frame
else:
# update the values
self._jointgroups[caller].update(msg)
def trigger_configuration(self):
# Comment in to signal that the plugin has a way to configure it
# Usually used to open a configuration dialog
self.dialog_config = QDialog()
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'iop_rqt_velocity_config.ui')
loadUi(ui_file, self.dialog_config)
self.dialog_config.accepted.connect(self.on_dialog_config_accepted)
# fill configuration dialog
ti = TopicInfo()
ti.fill_published_topics(self.dialog_config.comboBox_listTopic, "sensor_msgs/JointState", self._topic_list)
# ti.fill_subscribed_topics(self.dialog_config.comboBox_commandTopic, "std_msgs/Float64MultiArray", self._topic_command)
# stop on cancel pressed
if not self.dialog_config.exec_():
return
def on_dialog_config_accepted(self):
self.shutdownRosComm()
# self._topic_command = self.dialog_config.comboBox_commandTopic.currentText()
self._topic_list = self.dialog_config.comboBox_listTopic.currentText()
self.reinitRosComm()
class RqtHandeyeCalibration(Plugin):
def __init__(self, context):
super(RqtHandeyeCalibration, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('HandeyeCalibration')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
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 QWidgets
self._widget = QWidget()
self._infoWidget = QWidget()
# Get path to UI file which should be in the "resource" folder of this package
ui_file = os.path.join(rospkg.RosPack().get_path('rqt_easy_handeye'),
'resource', 'rqt_handeye.ui')
ui_info_file = os.path.join(
rospkg.RosPack().get_path('rqt_easy_handeye'), 'resource',
'rqt_handeye_info.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
loadUi(ui_info_file, self._infoWidget)
self._widget.horizontalLayout_infoAndActions.insertWidget(
0, self._infoWidget)
self._calibration_algorithm_combobox = QComboBox()
self._calibration_algorithm_layout = QHBoxLayout()
self._calibration_algorithm_layout.insertWidget(
0, QLabel('Calibration algorithm: '))
self._calibration_algorithm_layout.insertWidget(
1, self._calibration_algorithm_combobox)
self._infoWidget.layout().insertLayout(
-1, self._calibration_algorithm_layout)
# Give QObjects reasonable names
self._widget.setObjectName('RqtHandeyeCalibrationUI')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
self.client = HandeyeClient()
resp = self.client.list_algorithms()
for i, a in enumerate(resp.algorithms):
self._calibration_algorithm_combobox.insertItem(i, a)
index_of_curr_alg = resp.algorithms.index(resp.current_algorithm)
self._calibration_algorithm_combobox.setCurrentIndex(index_of_curr_alg)
self._calibration_algorithm_combobox.currentTextChanged.connect(
self.client.set_algorithm)
self._infoWidget.calibNameLineEdit.setText(rospy.get_namespace())
self._infoWidget.trackingBaseFrameLineEdit.setText(
self.client.parameters.tracking_base_frame)
self._infoWidget.trackingMarkerFrameLineEdit.setText(
self.client.parameters.tracking_marker_frame)
self._infoWidget.robotBaseFrameLineEdit.setText(
self.client.parameters.robot_base_frame)
self._infoWidget.robotEffectorFrameLineEdit.setText(
self.client.parameters.robot_effector_frame)
if self.client.parameters.eye_on_hand:
self._infoWidget.calibTypeLineEdit.setText("eye on hand")
else:
self._infoWidget.calibTypeLineEdit.setText("eye on base")
self._widget.takeButton.clicked[bool].connect(self.handle_take_sample)
self._widget.removeButton.clicked[bool].connect(
self.handle_remove_sample)
self._widget.computeButton.clicked[bool].connect(
self.handle_compute_calibration)
self._widget.saveButton.clicked[bool].connect(
self.handle_save_calibration)
self._widget.removeButton.setEnabled(False)
self._widget.computeButton.setEnabled(False)
self._widget.saveButton.setEnabled(False)
def shutdown_plugin(self):
# TODO unregister all publishers here
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 trigger_configuration(self):
# Comment in to signal that the plugin has a way to configure
# This will enable a setting button (gear icon) in each dock widget title bar
# Usually used to open a modal configuration dialog
def _display_sample_list(self, sample_list):
self._widget.sampleListWidget.clear()
for i in range(len(sample_list.hand_world_samples)):
formatted_robot_sample = format_sample(
sample_list.hand_world_samples[i])
formatted_tracking_sample = format_sample(
sample_list.camera_marker_samples[i])
self._widget.sampleListWidget.addItem(
'{}) \n hand->world \n {} \n camera->marker\n {}\n'.format(
i + 1, formatted_robot_sample, formatted_tracking_sample))
self._widget.sampleListWidget.setCurrentRow(
len(sample_list.hand_world_samples) - 1)
self._widget.removeButton.setEnabled(
len(sample_list.hand_world_samples) > 0)
def handle_take_sample(self):
sample_list = self.client.take_sample()
self._display_sample_list(sample_list)
self._widget.computeButton.setEnabled(
len(sample_list.hand_world_samples) > 1)
self._widget.saveButton.setEnabled(False)
def handle_remove_sample(self):
index = self._widget.sampleListWidget.currentRow()
sample_list = self.client.remove_sample(index)
self._display_sample_list(sample_list)
self._widget.computeButton.setEnabled(
len(sample_list.hand_world_samples) > 1)
self._widget.saveButton.setEnabled(False)
def handle_compute_calibration(self):
result = self.client.compute_calibration()
self._widget.computeButton.setEnabled(False)
if result.valid:
self._widget.outputBox.setPlainText(
str(result.calibration.transform.transform))
self._widget.saveButton.setEnabled(True)
else:
self._widget.outputBox.setPlainText(
'The calibration could not be computed')
self._widget.saveButton.setEnabled(False)
def handle_save_calibration(self):
self.client.save()
self._widget.saveButton.setEnabled(False)
class multiQuadGuiPlugin(Plugin):
CMD = []
CMD_LAND = ("LAND", Qt.Key_L)
CMD_QUALYSIS_CONNECT = ("QUALISYS CONNECT", Qt.Key_C)
CMD_STOP = ("STOP", Qt.Key_S)
CMD_GO_UP = ("GO UP", Qt.Key_Q)
CMD_GO_MIDDLE = ("GO MIDDLE", Qt.Key_A)
CMD_GO_DOWN = ("GO DOWN", Qt.Key_Z)
CMD_GOTO = ("GOTO", Qt.Key_G)
CMD_ARM = ("ARM", Qt.Key_T)
CMD_UNARM = ("UNARM", Qt.Key_Y)
CMD_SET_MODE = ("SET_MODE", Qt.Key_U)
CMD_SET_CONTROLLER = ("SET_CONTROLLER", Qt.Key_I)
CMD = [CMD_LAND,CMD_QUALYSIS_CONNECT,CMD_STOP,CMD_GO_UP,CMD_GO_MIDDLE,CMD_GO_DOWN,CMD_GOTO,CMD_ARM,CMD_UNARM,CMD_SET_MODE,CMD_SET_CONTROLLER]
def __init__(self, context,namespace = None):
# it is either "" or the input given at creation of plugin
self.namespace = self._parse_args(context.argv())
super(multiQuadGuiPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('multiQuadGuiPlugin')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
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()
# Get path to UI file which is a sibling of this file
# in this example the .ui and .py file are in the same folder
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'multiQuadGui.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('multiQuadGuiUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
# Add widget to the user interface
self.arena = Arena(self)
self._widget.layout().addWidget(self.arena,0,1)
self._widget.keyPressEvent = self.keyPressEvent
self._widget.keyReleaseEvent = self.keyReleaseEvent
self._widget.table.keyPressEvent = self.keyPressEvent
self._widget.table.keyReleaseEvent = self.keyReleaseEvent
context.add_widget(self._widget)
# # Adding all the tabs
self.quad_ns_list = []
self.quads = {}
self.quad_select = set()
self.command = None
self.update_timer = QTimer(self)
self.update_timer.setInterval(1000)
self.update_timer.timeout.connect(self.update_quad_list)
self.update_timer.start()
self.update_draw = QTimer(self)
self.update_draw.setInterval(1000/30)
self.update_draw.timeout.connect(self.arena.repaint)
self.update_draw.timeout.connect(self.update_table)
self.update_draw.start()
table_sizes = [25,100,150,60,100,120,70,70,100]
for i in range(len(table_sizes)):
self._widget.table.setColumnWidth(i,table_sizes[i])
def _parse_args(self, argv):
parser = argparse.ArgumentParser(prog='saver', add_help=False)
# args = parser.parse_args(argv)
if argv:
namespace = argv[0]
return namespace
else:
# is argv is empty return empty string
return ""
def get_id(self,ns):
i = re.findall('\d+', ns)[0]
return int(i)
#@Slot(bool)
def update_quad_list(self):
self.quad_ns_list = rospy.get_param("/quad_ns_list")
for ns in self.quad_ns_list:
qid = self.get_id(ns)
if qid not in self.quads.keys():
self.add_quad(ns,qid)
def add_quad(self,ns,qid):
print "Add new quad: " + str(ns)
self.quads[qid] = Quad(ns)
idR = self.getRowId(qid)
self._widget.table.insertRow(idR)
def getRowId(self,idq):
ids = sorted(self.quads.keys())
return ids.index(idq)
#@Slot(bool)
def update_table(self):
for (idq,q) in self.quads.iteritems():
t = self.arena.getTable(q)
idT = self.getRowId(idq)
t = [(idq,None)] + t
for (idx,value) in enumerate(t):
item = self._widget.table.item(idT, idx)
if item==None:
item = QTableWidgetItem()
self._widget.table.setItem(idT,idx,item)
if idx==0:
if idq in self.quad_select:
item.setBackground(QColor(25,0,255,100))
else:
item.setBackground(Qt.white)
else:
if value[1] == False:
item.setBackground(QColor(250,0,25,150))
elif value[1] == True:
item.setBackground(QColor(0,250,25,150))
else:
item.setBackground(Qt.white)
item.setText(str(value[0]))
def print_console(self,s):
self._widget.console.append(s)
def call_command(self,cmd,arg=None):
for i in self.quad_select:
func = lambda : call_command(self.quads[i],cmd,arg)
threading.Thread(target=func).start()
self.print_console(' '.join([str(l) for l in self.quad_select]) + " " + cmd[0])
def select(self,i):
if i==0:
for d in self.quads.keys():
self.quad_select.add(d)
if i in self.quads.keys():
self.quad_select.add(i)
self.update_table()
def unselect(self,i):
if i==0:
self.quad_select.clear()
if i in self.quads.keys() and i in self.quad_select:
self.quad_select.remove(i)
self.update_table()
def keyPressEvent(self, event):
for c in multiQuadGuiPlugin.CMD:
if type(event) == QKeyEvent and event.key() == c[1]:
self.call_command(c)
if type(event) == QKeyEvent and (event.key() >= Qt.Key_0 and event.key() <= Qt.Key_9):
self.select(event.key()-Qt.Key_0)
def keyReleaseEvent(self,event):
if type(event) == QKeyEvent and (event.key() >= Qt.Key_0 and event.key() <= Qt.Key_9):
self.unselect(event.key()-Qt.Key_0)
