def __init__(self):
super(TopicSelection, self).__init__()
master = rosgraph.Master('rqt_bag_recorder')
self.setWindowTitle("Select the topics you want to record")
self.resize(500, 700)
self.topic_list = []
self.selected_topics = []
self.items_list = []
self.area = QScrollArea(self)
self.main_widget = QWidget(self.area)
self.ok_button = QPushButton("Record", self)
self.ok_button.clicked.connect(self.onButtonClicked)
self.ok_button.setEnabled(False)
self.main_vlayout = QVBoxLayout(self)
self.main_vlayout.addWidget(self.area)
self.main_vlayout.addWidget(self.ok_button)
self.setLayout(self.main_vlayout)
self.selection_vlayout = QVBoxLayout(self)
self.item_all = QCheckBox("All", self)
self.item_all.stateChanged.connect(self.updateList)
self.selection_vlayout.addWidget(self.item_all)
topic_data_list = master.getPublishedTopics('')
topic_data_list.sort()
for topic, datatype in topic_data_list:
self.addCheckBox(topic)
self.main_widget.setLayout(self.selection_vlayout)
self.area.setWidget(self.main_widget)
self.show()
def create_label_checkbox_pair(key, value):
label = QLabel(key)
label.setToolTip(key)
label.setSizePolicy(QSizePolicy.Fixed, QSizePolicy.Preferred)
label.setMinimumWidth(400)
label.setMaximumHeight(30)
label.setWordWrap(True)
checkbox = QCheckBox()
checkbox.setChecked(value)
return label, checkbox
def _load_parameter(self,param_index,param_name, param_type,param_value, param_min, param_max, param_desc):
x_check = 160
x ,y ,w ,h = 4,4 + 40 * param_index,150,20
label = QLabel(param_name,self.parameters)
label.setGeometry(x,y,w,h)
label.setToolTip(param_desc)
label.show()
if param_type == 'Boolean':
checkbox = QCheckBox('',self.parameters)
checkbox.setGeometry(x_check,y,w,h)
checkbox.setChecked(param_value == '1')
checkbox.setToolTip(param_desc)
checkbox.stateChanged.connect(partial(self._handle_bool_param_changed,param_name))
checkbox.show()
elif param_type == 'Integer':
if len(param_min) > 0 and len(param_max) > 0 and int(param_max) < 5:
slider = QSlider(Qt.Horizontal,self.parameters)
slider.setMinimum(int(param_min))
slider.setMaximum(int(param_max))
slider.setValue(int(param_value))
slider.setTickPosition(QSlider.TicksBelow)
slider.setGeometry(x_check,y,w,h)
slider.setToolTip(param_desc)
slider.setTickInterval(1)
slider.setSingleStep(1)
slider.setPageStep(1)
slider.valueChanged.connect(partial(self._handle_param_changed,param_name))
slider.show()
else:
spin = QSpinBox(self.parameters)
if len(param_min) > 0:
spin.setMinimum(int(param_min))
if len(param_max) > 0:
spin.setMaximum(int(param_max))
spin.setValue(int(param_value))
spin.setToolTip(param_desc)
spin.setGeometry(x_check,y,w,h)
spin.valueChanged.connect(partial(self._handle_param_changed,param_name))
spin.show()
elif param_type == 'Double':
spin = QDoubleSpinBox(self.parameters)
if len(param_min) > 0:
spin.setMinimum(float(param_min))
if len(param_max) > 0:
spin.setMaximum(float(param_max))
spin.setValue(float(param_value))
spin.valueChanged.connect(partial(self._handle_param_changed,param_name))
spin.setGeometry(x_check,y,w,h)
spin.show()
elif param_type == 'String':
lineEdit = QLineEdit(self.parameters)
lineEdit.setText(param_value)
lineEdit.setToolTip(param_desc)
lineEdit.setGeometry(x_check,y,w,h)
lineEdit.textChanged.connect(partial(self._handle_param_changed,param_name))
lineEdit.show()
self.parameters.update()
def createFieldsFromValues(self, values, exclusive=False):
self.setUpdatesEnabled(False)
try:
if isinstance(values, list):
for v in values:
checkbox = QCheckBox(v)
checkbox.toggled.connect(self._on_checkbox_toggled)
checkbox.setObjectName(v)
checkbox.setAutoExclusive(exclusive)
self.layout().addRow(checkbox)
finally:
self.setUpdatesEnabled(True)
def __init__(self,
options,
title='Checkboxes',
selected_indexes=[],
parent=None):
super(CheckBoxGroup, self).__init__()
self.setTitle(title)
self.setLayout(QVBoxLayout())
self._button_group = QButtonGroup()
self._button_group.setExclusive(False)
self._options = options
if parent == None:
parent = self
for (button_id, option) in enumerate(self._options):
checkbox = QCheckBox(option.get('title', 'option %d' % button_id))
checkbox.setEnabled(option.get('enabled', True))
checkbox.setChecked(button_id in selected_indexes)
checkbox.setToolTip(option.get('tooltip', ''))
self._button_group.addButton(checkbox, button_id)
parent.layout().addWidget(checkbox)
if 'description' in option:
parent.layout().addWidget(QLabel(option['description']))
def _create_found_frame(self):
ff_frame = QFrame(self)
self.found_files_vbox_layout = QVBoxLayout(ff_frame)
self.found_files_vbox_layout.setContentsMargins(0, 0, 0, 0)
self.recursive_search_box = QCheckBox("recursive search")
self.found_files_vbox_layout.addWidget(self.recursive_search_box)
self.found_files_list = QTreeWidget(ff_frame)
self.found_files_list.setColumnCount(1)
self.found_files_list.setFrameStyle(QFrame.StyledPanel)
self.found_files_list.setHeaderHidden(True)
self.found_files_list.itemActivated.connect(self.on_itemActivated)
self.found_files_list.setStyleSheet("QTreeWidget {"
"background-color:transparent;"
"}"
"QTreeWidget::item {"
"background-color:transparent;"
"}"
"QTreeWidget::item:selected {"
"background-color: darkgray;"
"}")
self.found_files_vbox_layout.addWidget(self.found_files_list)
self.recursive_search_box.setChecked(False)
return ff_frame
def createFieldsFromValues(self, values, exclusive=False):
self.setUpdatesEnabled(False)
try:
if isinstance(values, list):
for v in values:
checkbox = QCheckBox(v)
checkbox.toggled.connect(self._on_checkbox_toggled)
checkbox.setObjectName(v)
checkbox.setAutoExclusive(exclusive)
self.layout().addRow(checkbox)
finally:
self.setUpdatesEnabled(True)
def __init__(self, parent=None, subscribe=False):
QWidget.__init__(self, parent)
# start widget
vbox = QVBoxLayout()
vbox.setMargin(0)
vbox.setContentsMargins(0, 0, 0, 0)
# add error status text edit
self.error_status_text_box = QErrorStatusTextBox()
self.error_status_text_box_layout = QHBoxLayout()
self.error_status_text_box_layout.addWidget(self.error_status_text_box)
vbox.addLayout(self.error_status_text_box_layout)
# add panel
hbox = QHBoxLayout()
# clear push button
self.execute_command = QPushButton("Clear")
self.execute_command.clicked.connect(self.error_status_text_box.clear)
hbox.addWidget(self.execute_command)
hbox.addStretch()
# hide window checkbox
hide_window_check_box = QCheckBox("Hide")
hide_window_check_box.stateChanged.connect(self.state_changed)
hbox.addWidget(hide_window_check_box)
# end panel
vbox.addLayout(hbox)
# end widget
self.setLayout(vbox)
#self.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Minimum)
# subscriber
if subscribe:
self.error_status_sub = rospy.Subscriber("error_status", ErrorStatus, self.error_status_callback)
self.subscribed = subscribe
# connect signal slot internally to prevent crash by subscriber
self.error_status_signal.connect(self.append_error_status)
def __init__(self, parent=None):
super(VisualizerWidget, self).__init__(parent)
self.setWindowTitle('Graph Profiler Visualizer')
vbox = QVBoxLayout()
self.setLayout(vbox)
toolbar_layout = QHBoxLayout()
refresh_button = QPushButton()
refresh_button.setIcon(QIcon.fromTheme('view-refresh'))
auto_refresh_checkbox = QCheckBox("Auto Refresh")
hide_disconnected_topics = QCheckBox("Hide Disconnected Topics")
topic_blacklist_button = QPushButton("Topic Blacklist")
node_blacklist_button = QPushButton("Node Blacklist")
refresh_button.clicked.connect(self._refresh)
topic_blacklist_button.clicked.connect(self._edit_topic_blacklist)
node_blacklist_button.clicked.connect(self._edit_node_blacklist)
auto_refresh_checkbox.setCheckState(2)
auto_refresh_checkbox.stateChanged.connect(self._autorefresh_changed)
hide_disconnected_topics.setCheckState(2)
hide_disconnected_topics.stateChanged.connect(self._hidedisconnectedtopics_changed)
toolbar_layout.addWidget(refresh_button)
toolbar_layout.addWidget(auto_refresh_checkbox)
toolbar_layout.addStretch(0)
toolbar_layout.addWidget(hide_disconnected_topics)
toolbar_layout.addWidget(topic_blacklist_button)
toolbar_layout.addWidget(node_blacklist_button)
vbox.addLayout(toolbar_layout)
# Initialize the Visualizer
self._view = qt_view.QtView()
self._adapter = rosprofiler_adapter.ROSProfileAdapter(self._view)
self._adapter.set_topic_quiet_list(TOPIC_BLACKLIST)
self._adapter.set_node_quiet_list(NODE_BLACKLIST)
vbox.addWidget(self._view)
def __init__(self, options, title='Checkboxes', selected_indexes=[], parent=None):
super(CheckBoxGroup, self).__init__()
self.setTitle(title)
self.setLayout(QVBoxLayout())
self._button_group = QButtonGroup()
self._button_group.setExclusive(False)
self._options = options
if parent == None:
parent = self
for (button_id, option) in enumerate(self._options):
checkbox = QCheckBox(option.get('title', 'option %d' % button_id))
checkbox.setEnabled(option.get('enabled', True))
checkbox.setChecked(button_id in selected_indexes)
checkbox.setToolTip(option.get('tooltip', ''))
self._button_group.addButton(checkbox, button_id)
parent.layout().addWidget(checkbox)
if 'description' in option:
parent.layout().addWidget(QLabel(option['description']))
def _create_found_frame(self):
ff_frame = QFrame(self)
self.found_files_vbox_layout = QVBoxLayout(ff_frame)
self.found_files_vbox_layout.setContentsMargins(0, 0, 0, 0)
self.recursive_search_box = QCheckBox("recursive search")
self.found_files_vbox_layout.addWidget(self.recursive_search_box)
self.found_files_list = QTreeWidget(ff_frame)
self.found_files_list.setColumnCount(1)
self.found_files_list.setFrameStyle(QFrame.StyledPanel)
self.found_files_list.setHeaderHidden(True)
self.found_files_list.itemActivated.connect(self.on_itemActivated)
self.found_files_list.setStyleSheet(
"QTreeWidget {"
"background-color:transparent;"
"}"
"QTreeWidget::item {"
"background-color:transparent;"
"}"
"QTreeWidget::item:selected {"
"background-color: darkgray;"
"}")
self.found_files_vbox_layout.addWidget(self.found_files_list)
self.recursive_search_box.setChecked(False)
return ff_frame
def createEditor(self, parent, option, index):
'''
Creates a editor in the TreeView depending on type of the settings data.
'''
item = self._itemFromIndex(index)
if item.edit_type() == SettingsValueItem.EDIT_TYPE_AUTODETECT:
if isinstance(item.value(), bool):
box = QCheckBox(parent)
box.setFocusPolicy(Qt.StrongFocus)
box.setAutoFillBackground(True)
box.stateChanged.connect(self.edit_finished)
return box
elif isinstance(item.value(), int):
box = QSpinBox(parent)
box.setValue(item.value())
if not item.value_min() is None:
box.setMinimum(item.value_min())
if not item.value_max() is None:
box.setMaximum(item.value_max())
return box
elif item.edit_type() == SettingsValueItem.EDIT_TYPE_FOLDER:
editor = PathEditor(item.value(), parent)
editor.editing_finished_signal.connect(self.edit_finished)
return editor
elif item.edit_type() == SettingsValueItem.EDIT_TYPE_LIST:
box = QComboBox(parent)
box.addItems(item.value_list())
index = box.findText(item.value())
if index >= 0:
box.setCurrentIndex(index)
box.setEditable(False)
return box
return QStyledItemDelegate.createEditor(self, parent, option, index)
def __init__(self,
items=list(),
buttons=QDialogButtonBox.Cancel | QDialogButtonBox.Ok,
exclusive=False,
preselect_all=False,
title='',
description='',
icon='',
parent=None,
select_if_single=True,
checkitem1='',
checkitem2='',
closein=0):
'''
Creates an input dialog.
@param items: a list with strings
@type items: C{list()}
'''
QDialog.__init__(self, parent=parent)
self.setObjectName(' - '.join(['SelectDialog', utf8(items)]))
self.verticalLayout = QVBoxLayout(self)
self.verticalLayout.setObjectName("verticalLayout")
self.verticalLayout.setContentsMargins(1, 1, 1, 1)
# add filter row
self.filter_frame = QFrame(self)
filterLayout = QHBoxLayout(self.filter_frame)
filterLayout.setContentsMargins(1, 1, 1, 1)
label = QLabel("Filter:", self.filter_frame)
self.filter_field = EnchancedLineEdit(self.filter_frame)
filterLayout.addWidget(label)
filterLayout.addWidget(self.filter_field)
self.filter_field.textChanged.connect(self._on_filter_changed)
self.verticalLayout.addWidget(self.filter_frame)
if description:
self.description_frame = QFrame(self)
descriptionLayout = QHBoxLayout(self.description_frame)
# descriptionLayout.setContentsMargins(1, 1, 1, 1)
if icon:
self.icon_label = QLabel(self.description_frame)
self.icon_label.setSizePolicy(QSizePolicy.Fixed,
QSizePolicy.Fixed)
self.icon_label.setPixmap(
QPixmap(icon).scaled(30, 30, Qt.KeepAspectRatio))
descriptionLayout.addWidget(self.icon_label)
self.description_label = QLabel(self.description_frame)
self.description_label.setWordWrap(True)
self.description_label.setText(description)
descriptionLayout.addWidget(self.description_label)
self.verticalLayout.addWidget(self.description_frame)
# create area for the parameter
self.content = MainBox(self)
if items:
self.scroll_area = QScrollArea(self)
self.scroll_area.setFocusPolicy(Qt.NoFocus)
self.scroll_area.setObjectName("scroll_area")
self.scroll_area.setWidgetResizable(True)
self.scroll_area.setWidget(self.content)
self.verticalLayout.addWidget(self.scroll_area)
self.checkitem1 = checkitem1
self.checkitem1_result = False
self.checkitem2 = checkitem2
self.checkitem2_result = False
# add select all option
if not exclusive and items:
self._ignore_next_toggle = False
self.select_all_checkbox = QCheckBox('all entries')
self.select_all_checkbox.setTristate(True)
self.select_all_checkbox.stateChanged.connect(
self._on_select_all_checkbox_stateChanged)
self.verticalLayout.addWidget(self.select_all_checkbox)
self.content.toggled.connect(self._on_main_toggle)
if self.checkitem1:
self.checkitem1_checkbox = QCheckBox(self.checkitem1)
self.checkitem1_checkbox.stateChanged.connect(
self._on_select_checkitem1_checkbox_stateChanged)
self.verticalLayout.addWidget(self.checkitem1_checkbox)
if self.checkitem2:
self.checkitem2_checkbox = QCheckBox(self.checkitem2)
self.checkitem2_checkbox.stateChanged.connect(
self._on_select_checkitem2_checkbox_stateChanged)
self.verticalLayout.addWidget(self.checkitem2_checkbox)
if not items:
spacerItem = QSpacerItem(1, 1, QSizePolicy.Expanding,
QSizePolicy.Expanding)
self.verticalLayout.addItem(spacerItem)
self._close_timer = None
self._closein = closein - 1
if closein > 0:
self.closein_label = QLabel("OK in %d sec..." % closein)
self.closein_label.setAlignment(Qt.AlignRight)
self.verticalLayout.addWidget(self.closein_label)
self._close_timer = threading.Timer(1.0, self._on_close_timer)
self._close_timer.start()
# create buttons
self.buttonBox = QDialogButtonBox(self)
self.buttonBox.setObjectName("buttonBox")
self.buttonBox.setOrientation(Qt.Horizontal)
self.buttonBox.setStandardButtons(buttons)
self.buttonBox.accepted.connect(self.accept)
self.buttonBox.rejected.connect(self.reject)
self.verticalLayout.addWidget(self.buttonBox)
# set the input fields
if items:
self.content.createFieldsFromValues(items, exclusive)
if (select_if_single and len(items) == 1) or preselect_all:
self.select_all_checkbox.setCheckState(Qt.Checked)
if not items or len(items) < 7:
self.filter_frame.setVisible(False)
def _create_buttons(self, buttons):
if MessageBox.Ok & buttons:
self._accept_button = MessageBox.Ok
bt = QPushButton(self.tr("&ok"))
bt.clicked.connect(self._on_ok_clicked)
self.buttonBox.addButton(bt, QDialogButtonBox.AcceptRole)
if MessageBox.Open & buttons:
self._accept_button = MessageBox.Open
bt = QPushButton(self.tr("&Open"))
bt.clicked.connect(self._on_open_clicked)
self.buttonBox.addButton(bt, QDialogButtonBox.AcceptRole)
if MessageBox.Save & buttons:
self._accept_button = MessageBox.Save
bt = QPushButton(self.tr("&Save"))
bt.clicked.connect(self._on_save_clicked)
self.buttonBox.addButton(bt, QDialogButtonBox.AcceptRole)
if MessageBox.Cancel & buttons:
self._reject_button = MessageBox.Cancel
bt = QPushButton(self.tr("&Cancel"))
bt.clicked.connect(self._on_cancel_clicked)
self.buttonBox.addButton(bt, QDialogButtonBox.RejectRole)
if MessageBox.Close & buttons:
self._reject_button = MessageBox.Close
bt = QPushButton(self.tr("&Close"))
bt.clicked.connect(self._on_close_clicked)
self.buttonBox.addButton(bt, QDialogButtonBox.RejectRole)
if MessageBox.Discard & buttons:
bt = QPushButton(self.tr("&Discard"))
bt.clicked.connect(self._on_discard_clicked)
self.buttonBox.addButton(bt, QDialogButtonBox.DestructiveRole)
if MessageBox.Apply & buttons:
bt = QPushButton(self.tr("&Apply"))
bt.clicked.connect(self._on_apply_clicked)
self.buttonBox.addButton(bt, QDialogButtonBox.ApplyRole)
if MessageBox.Reset & buttons:
bt = QPushButton(self.tr("&Reset"))
bt.clicked.connect(self._on_reset_clicked)
self.buttonBox.addButton(bt, QDialogButtonBox.ResetRole)
if MessageBox.RestoreDefaults & buttons:
bt = QPushButton(self.tr("&RestoreDefaults"))
bt.clicked.connect(self._on_restore_defaults_clicked)
self.buttonBox.addButton(bt, QDialogButtonBox.ResetRole)
if MessageBox.Help & buttons:
bt = QPushButton(self.tr("&Help"))
bt.clicked.connect(self._on_help_clicked)
self.buttonBox.addButton(bt, QDialogButtonBox.HelpRole)
if MessageBox.SaveAll & buttons:
self._accept_button = MessageBox.SaveAll
bt = QPushButton(self.tr("&SaveAll"))
bt.clicked.connect(self._on_saveall_clicked)
self.buttonBox.addButton(bt, QDialogButtonBox.AcceptRole)
if MessageBox.Yes & buttons:
bt = QPushButton(self.tr("&Yes"))
bt.clicked.connect(self._on_yes_clicked)
self.buttonBox.addButton(bt, QDialogButtonBox.YesRole)
if MessageBox.YesToAll & buttons:
bt = QPushButton(self.tr("&YesToAll"))
bt.clicked.connect(self._on_yestoall_clicked)
self.buttonBox.addButton(bt, QDialogButtonBox.YesRole)
if MessageBox.No & buttons:
bt = QPushButton(self.tr("&No"))
bt.clicked.connect(self._on_no_clicked)
self.buttonBox.addButton(bt, QDialogButtonBox.NoRole)
if MessageBox.NoToAll & buttons:
bt = QPushButton(self.tr("&NoToAll"))
bt.clicked.connect(self._on_notoall_clicked)
self.buttonBox.addButton(bt, QDialogButtonBox.NoRole)
if MessageBox.Abort & buttons:
self._reject_button = MessageBox.Abort
bt = QPushButton(self.tr("&Abort"))
bt.clicked.connect(self._on_abort_clicked)
self.buttonBox.addButton(bt, QDialogButtonBox.RejectRole)
if MessageBox.Retry & buttons:
self._accept_button = MessageBox.Retry
bt = QPushButton(self.tr("&Retry"))
bt.clicked.connect(self._on_retry_clicked)
self.buttonBox.addButton(bt, QDialogButtonBox.AcceptRole)
if MessageBox.Ignore & buttons:
self._accept_button = MessageBox.Ignore
bt = QPushButton(self.tr("&Ignore"))
bt.clicked.connect(self._on_ignore_clicked)
self.buttonBox.addButton(bt, QDialogButtonBox.AcceptRole)
if MessageBox.Avoid & buttons:
if self._use_checkbox:
checkbox = QCheckBox("&Don't show again", self)
checkbox.stateChanged.connect(self._check_ignore)
self.buttonBox.addButton(checkbox, QDialogButtonBox.HelpRole)
else:
bt = QPushButton(self.tr("&Don't show again"))
bt.setMaximumHeight(24)
bt.clicked.connect(self._add_to_ignore)
self.buttonBox.addButton(bt, QDialogButtonBox.HelpRole)
class StepInterfaceWidget(QObject):
command_buttons = []
start_feet = Feet()
def __init__(self, context, add_execute_widget=True):
super(StepInterfaceWidget, self).__init__()
# init signal mapper
self.command_mapper = QSignalMapper(self)
self.command_mapper.mapped.connect(self._publish_step_plan_request)
# start widget
widget = context
error_status_widget = QErrorStatusWidget()
self.logger = Logger(error_status_widget)
vbox = QVBoxLayout()
# start control box
controls_hbox = QHBoxLayout()
# left coloumn
left_controls_vbox = QVBoxLayout()
left_controls_vbox.setMargin(0)
self.add_command_button(left_controls_vbox, "Rotate Left",
PatternParameters.ROTATE_LEFT)
self.add_command_button(left_controls_vbox, "Strafe Left",
PatternParameters.STRAFE_LEFT)
self.add_command_button(left_controls_vbox, "Step Up",
PatternParameters.STEP_UP)
self.add_command_button(left_controls_vbox, "Center on Left",
PatternParameters.FEET_REALIGN_ON_LEFT)
left_controls_vbox.addStretch()
controls_hbox.addLayout(left_controls_vbox, 1)
# center coloumn
center_controls_vbox = QVBoxLayout()
center_controls_vbox.setMargin(0)
self.add_command_button(center_controls_vbox, "Forward",
PatternParameters.FORWARD)
self.add_command_button(center_controls_vbox, "Backward",
PatternParameters.BACKWARD)
self.add_command_button(center_controls_vbox, "Step Over",
PatternParameters.STEP_OVER)
self.add_command_button(center_controls_vbox, "Center Feet",
PatternParameters.FEET_REALIGN_ON_CENTER)
self.add_command_button(center_controls_vbox, "Wide Stance",
PatternParameters.WIDE_STANCE)
center_controls_vbox.addStretch()
controls_hbox.addLayout(center_controls_vbox, 1)
# right coloumn
right_controls_vbox = QVBoxLayout()
right_controls_vbox.setMargin(0)
self.add_command_button(right_controls_vbox, "Rotate Right",
PatternParameters.ROTATE_RIGHT)
self.add_command_button(right_controls_vbox, "Strafe Right",
PatternParameters.STRAFE_RIGHT)
self.add_command_button(right_controls_vbox, "Step Down",
PatternParameters.STEP_DOWN)
self.add_command_button(right_controls_vbox, "Center on Right",
PatternParameters.FEET_REALIGN_ON_RIGHT)
right_controls_vbox.addStretch()
controls_hbox.addLayout(right_controls_vbox, 1)
# end control box
add_layout_with_frame(vbox, controls_hbox, "Commands:")
# start settings
settings_hbox = QHBoxLayout()
settings_hbox.setMargin(0)
# start left column
left_settings_vbox = QVBoxLayout()
left_settings_vbox.setMargin(0)
# frame id
self.frame_id_line_edit = QLineEdit("/world")
add_widget_with_frame(left_settings_vbox, self.frame_id_line_edit,
"Frame ID:")
# do closing step
self.close_step_checkbox = QCheckBox()
self.close_step_checkbox.setText("Do closing step")
self.close_step_checkbox.setChecked(True)
left_settings_vbox.addWidget(self.close_step_checkbox)
# extra seperation
self.extra_seperation_checkbox = QCheckBox()
self.extra_seperation_checkbox.setText("Extra Seperation")
self.extra_seperation_checkbox.setChecked(False)
left_settings_vbox.addWidget(self.extra_seperation_checkbox)
left_settings_vbox.addStretch()
# number of steps
self.step_number = generate_q_double_spin_box(1, 1, 50, 0, 1.0)
add_widget_with_frame(left_settings_vbox, self.step_number,
"Number Steps:")
# start step index
self.start_step_index = generate_q_double_spin_box(0, 0, 1000, 0, 1.0)
add_widget_with_frame(left_settings_vbox, self.start_step_index,
"Start Step Index:")
# end left column
settings_hbox.addLayout(left_settings_vbox, 1)
# start center column
center_settings_vbox = QVBoxLayout()
center_settings_vbox.setMargin(0)
# start foot selection
self.start_foot_selection_combo_box = QComboBox()
self.start_foot_selection_combo_box.addItem("AUTO")
self.start_foot_selection_combo_box.addItem("LEFT")
self.start_foot_selection_combo_box.addItem("RIGHT")
add_widget_with_frame(center_settings_vbox,
self.start_foot_selection_combo_box,
"Start foot selection:")
center_settings_vbox.addStretch()
# step Distance
self.step_distance = generate_q_double_spin_box(0.0, 0.0, 0.5, 2, 0.01)
add_widget_with_frame(center_settings_vbox, self.step_distance,
"Step Distance (m):")
# side step distance
self.side_step = generate_q_double_spin_box(0.0, 0.0, 0.2, 2, 0.01)
add_widget_with_frame(center_settings_vbox, self.side_step,
"Side Step (m):")
# rotation per step
self.step_rotation = generate_q_double_spin_box(
0.0, -30.0, 30.0, 0, 1.0)
add_widget_with_frame(center_settings_vbox, self.step_rotation,
"Step Rotation (deg):")
# end center column
settings_hbox.addLayout(center_settings_vbox, 1)
# start right column
right_settings_vbox = QVBoxLayout()
right_settings_vbox.setMargin(0)
# roll
self.roll = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
add_widget_with_frame(right_settings_vbox, self.roll, "Roll (deg):")
# pitch
self.pitch = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
add_widget_with_frame(right_settings_vbox, self.pitch, "Pitch (deg):")
# use terrain model
self.use_terrain_model_checkbox = QCheckBox()
self.use_terrain_model_checkbox.setText("Use Terrain Model")
self.use_terrain_model_checkbox.setChecked(False)
self.use_terrain_model_checkbox.stateChanged.connect(
self.use_terrain_model_changed)
right_settings_vbox.addWidget(self.use_terrain_model_checkbox)
# override mode
self.override_checkbox = QCheckBox()
self.override_checkbox.setText("Override 3D")
self.override_checkbox.setChecked(False)
right_settings_vbox.addWidget(self.override_checkbox)
right_settings_vbox.addStretch()
# delta z
self.dz = generate_q_double_spin_box(0.0, -0.5, 0.5, 2, 0.01)
add_widget_with_frame(right_settings_vbox, self.dz,
"delta z per step (m):")
# end right column
settings_hbox.addLayout(right_settings_vbox, 1)
# end settings
add_layout_with_frame(vbox, settings_hbox, "Settings:")
# parameter set selection
self.parameter_set_widget = QParameterSetWidget(logger=self.logger)
add_widget_with_frame(vbox, self.parameter_set_widget,
"Parameter Set:")
# execute option
if add_execute_widget:
add_widget_with_frame(
vbox,
QExecuteStepPlanWidget(logger=self.logger,
step_plan_topic="step_plan"),
"Execute:")
# add error status widget
add_widget_with_frame(vbox, error_status_widget, "Status:")
# end widget
widget.setLayout(vbox)
#context.add_widget(widget)
# init widget
self.parameter_set_widget.param_cleared_signal.connect(
self.param_cleared)
self.parameter_set_widget.param_changed_signal.connect(
self.param_selected)
self.commands_set_enabled(False)
# subscriber
self.start_feet_sub = rospy.Subscriber("set_start_feet", Feet,
self.set_start_feet_callback)
# publisher
self.step_plan_pub = rospy.Publisher("step_plan",
StepPlan,
queue_size=1)
# action clients
self.step_plan_request_client = actionlib.SimpleActionClient(
"step_plan_request", StepPlanRequestAction)
def shutdown_plugin(self):
print "Shutting down ..."
print "Done!"
def add_command_button(self, parent, text, command):
button = QPushButton(text)
self.command_mapper.setMapping(button, command)
button.clicked.connect(self.command_mapper.map)
parent.addWidget(button)
self.command_buttons.append(button)
return button
def set_start_feet_callback(self, feet):
self.start_feet = feet
# message publisher
def _publish_step_plan_request(self, walk_command):
params = PatternParameters()
params.steps = self.step_number.value()
params.mode = walk_command
params.close_step = self.close_step_checkbox.isChecked()
params.extra_seperation = self.extra_seperation_checkbox.isChecked()
params.use_terrain_model = self.use_terrain_model_checkbox.isChecked()
params.override = self.override_checkbox.isChecked(
) and not self.use_terrain_model_checkbox.isChecked()
params.roll = math.radians(self.roll.value())
params.pitch = math.radians(self.pitch.value())
params.dz = self.dz.value()
params.step_distance_forward = self.step_distance.value()
params.step_distance_sideward = self.side_step.value()
params.turn_angle = math.radians(self.step_rotation.value())
request = StepPlanRequest()
request.header = std_msgs.msg.Header()
request.header.stamp = rospy.Time.now()
request.header.frame_id = self.frame_id_line_edit.text()
request.start = self.start_feet
request.start_step_index = self.start_step_index.value()
if self.start_foot_selection_combo_box.currentText() == "AUTO":
request.start_foot_selection = StepPlanRequest.AUTO
elif self.start_foot_selection_combo_box.currentText() == "LEFT":
request.start_foot_selection = StepPlanRequest.LEFT
elif self.start_foot_selection_combo_box.currentText() == "RIGHT":
request.start_foot_selection = StepPlanRequest.RIGHT
else:
self.logger.log_error(
"Unknown start foot selection mode ('" +
self.start_foot_selection_combo_box.currentText() + "')!")
return
if walk_command == PatternParameters.FORWARD:
params.mode = PatternParameters.SAMPLING
elif walk_command == PatternParameters.BACKWARD:
params.mode = PatternParameters.SAMPLING
params.step_distance_forward *= -1
params.step_distance_sideward *= -1
params.turn_angle *= -1
request.pattern_parameters = params
request.planning_mode = StepPlanRequest.PLANNING_MODE_PATTERN
request.parameter_set_name.data = self.parameter_set_widget.current_parameter_set_name(
)
print "Send request = ", request
# send request
if self.step_plan_request_client.wait_for_server(rospy.Duration(0.5)):
self.logger.log_info("Sending footstep plan request...")
goal = StepPlanRequestGoal()
goal.plan_request = request
self.step_plan_request_client.send_goal(goal)
if self.step_plan_request_client.wait_for_result(
rospy.Duration(5.0)):
self.logger.log_info("Received footstep plan!")
self.logger.log(
self.step_plan_request_client.get_result().status)
self.step_plan_pub.publish(
self.step_plan_request_client.get_result().step_plan)
else:
self.logger.log_error(
"Didn't received any results. Check communcation!")
else:
self.logger.log_error(
"Can't connect to footstep planner action server!")
def commands_set_enabled(self, enable):
for button in self.command_buttons:
button.setEnabled(enable)
@Slot()
def param_cleared(self):
self.commands_set_enabled(False)
@Slot(str)
def param_selected(self, name):
self.commands_set_enabled(True)
@Slot(int)
def use_terrain_model_changed(self, state):
enable_override = True
if state == Qt.Checked:
enable_override = False
self.roll.setEnabled(enable_override)
self.pitch.setEnabled(enable_override)
self.override_checkbox.setEnabled(enable_override)
self.dz.setEnabled(enable_override)
def __init__(self, context):
super(PatternGeneratorWidget, self).__init__()
# publisher
self.pattern_generator_params_pub = rospy.Publisher('pattern_generator/set_params', PatternGeneratorParameters, queue_size = 1)
# start widget
widget = context
# start upper part
hbox = QHBoxLayout()
# start left column
left_vbox = QVBoxLayout()
# start button
start_command = QPushButton("Start")
start_command.clicked.connect(self.start_command_callback)
left_vbox.addWidget(start_command)
# simulation checkbox
self.simulation_mode_checkbox = QCheckBox()
self.simulation_mode_checkbox.setText("Simulation Mode")
self.simulation_mode_checkbox.setChecked(False)
left_vbox.addWidget(self.simulation_mode_checkbox)
# realtime checkbox
self.realtime_mode_checkbox = QCheckBox()
self.realtime_mode_checkbox.setText("Realtime Mode")
self.realtime_mode_checkbox.setChecked(False)
left_vbox.addWidget(self.realtime_mode_checkbox)
# joystick checkbox
self.joystick_mode_checkbox = QCheckBox()
self.joystick_mode_checkbox.setText("Joystick Mode")
self.joystick_mode_checkbox.setChecked(False)
self.joystick_mode_checkbox.clicked.connect(self.joystick_mode_check_callback)
left_vbox.addWidget(self.joystick_mode_checkbox)
# foot seperation
self.foot_seperation = generate_q_double_spin_box(0.2, 0.15, 0.3, 2, 0.01)
self.foot_seperation.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.foot_seperation, "Foot Seperation (m):")
# delta x
self.delta_x = generate_q_double_spin_box(0.0, -0.4, 0.4, 2, 0.01)
self.delta_x.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.delta_x, "dX (m):")
# delta y
self.delta_y = generate_q_double_spin_box(0.0, -2.2, 2.2, 2, 0.01)
self.delta_y.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.delta_y, "dY (m):")
# delta yaw
self.delta_yaw = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
self.delta_yaw.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.delta_yaw, "dYaw (deg):")
# roll
self.roll = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
self.roll.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.roll, "Roll (deg):")
# pitch
self.pitch = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
self.pitch.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.pitch, "Pitch (deg):")
# end left column
left_vbox.addStretch()
hbox.addLayout(left_vbox, 1)
# start right column
right_vbox = QVBoxLayout()
# stop button
stop_command = QPushButton("Stop")
stop_command.clicked.connect(self.stop_command_callback)
right_vbox.addWidget(stop_command)
# ignore collision
self.collision_checkbox = QCheckBox()
self.collision_checkbox.setText("Ignore Collision")
self.collision_checkbox.setChecked(True)
right_vbox.addWidget(self.collision_checkbox)
# override 3D
self.override_checkbox = QCheckBox()
self.override_checkbox.setText("Override 3D")
self.override_checkbox.setChecked(False)
right_vbox.addWidget(self.override_checkbox)
# end right coloumn
right_vbox.addStretch()
hbox.addLayout(right_vbox, 1)
# add upper part
hbox.setMargin(0)
vbox = QVBoxLayout()
vbox.addLayout(hbox)
# parameter set selection
self.parameter_set_widget = QParameterSetWidget()
add_widget_with_frame(vbox, self.parameter_set_widget, "Parameter Set:")
# end widget
widget.setLayout(vbox)
def __init__(self, context):
super(PatternGeneratorWidget, self).__init__()
# publisher
self.pattern_generator_params_pub = rospy.Publisher(
'pattern_generator/set_params',
PatternGeneratorParameters,
queue_size=1)
# start widget
widget = context
# start upper part
hbox = QHBoxLayout()
# start left column
left_vbox = QVBoxLayout()
# start button
start_command = QPushButton("Start")
left_vbox.addWidget(start_command)
# simulation checkbox
self.simulation_mode_checkbox = QCheckBox()
self.simulation_mode_checkbox.setText("Simulation Mode")
self.simulation_mode_checkbox.setChecked(False)
left_vbox.addWidget(self.simulation_mode_checkbox)
# realtime checkbox
self.realtime_mode_checkbox = QCheckBox()
self.realtime_mode_checkbox.setText("Realtime Mode")
self.realtime_mode_checkbox.setChecked(False)
left_vbox.addWidget(self.realtime_mode_checkbox)
# joystick checkbox
self.joystick_mode_checkbox = QCheckBox()
self.joystick_mode_checkbox.setText("Joystick Mode")
self.joystick_mode_checkbox.setChecked(False)
left_vbox.addWidget(self.joystick_mode_checkbox)
# ignore invalid steps checkbox
self.ignore_invalid_steps_checkbox = QCheckBox()
self.ignore_invalid_steps_checkbox.setText("Ignore Invalid Steps")
self.ignore_invalid_steps_checkbox.setChecked(False)
left_vbox.addWidget(self.ignore_invalid_steps_checkbox)
# foot seperation
self.foot_seperation = generate_q_double_spin_box(
0.2, 0.15, 0.3, 2, 0.01)
self.foot_seperation.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.foot_seperation,
"Foot Seperation (m):")
# delta x
self.delta_x = generate_q_double_spin_box(0.0, -0.4, 0.4, 2, 0.01)
self.delta_x.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.delta_x, "dX (m):")
# delta y
self.delta_y = generate_q_double_spin_box(0.0, -2.2, 2.2, 2, 0.01)
self.delta_y.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.delta_y, "dY (m):")
# delta yaw
self.delta_yaw = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
self.delta_yaw.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.delta_yaw, "dYaw (deg):")
# roll
self.roll = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
self.roll.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.roll, "Roll (deg):")
# pitch
self.pitch = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
self.pitch.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.pitch, "Pitch (deg):")
# end left column
left_vbox.addStretch()
hbox.addLayout(left_vbox, 1)
# start right column
right_vbox = QVBoxLayout()
# stop button
stop_command = QPushButton("Stop")
right_vbox.addWidget(stop_command)
# ignore collision
self.collision_checkbox = QCheckBox()
self.collision_checkbox.setText("Ignore Collision")
self.collision_checkbox.setChecked(True)
right_vbox.addWidget(self.collision_checkbox)
# override 3D
self.override_checkbox = QCheckBox()
self.override_checkbox.setText("Override 3D")
self.override_checkbox.setChecked(False)
right_vbox.addWidget(self.override_checkbox)
# end right coloumn
right_vbox.addStretch()
hbox.addLayout(right_vbox, 1)
# add upper part
hbox.setMargin(0)
vbox = QVBoxLayout()
vbox.addLayout(hbox)
# parameter set selection
self.parameter_set_widget = QParameterSetWidget()
add_widget_with_frame(vbox, self.parameter_set_widget,
"Parameter Set:")
# end widget
widget.setLayout(vbox)
#context.add_widget(widget)
# signal connections
start_command.clicked.connect(self.start_command_callback)
stop_command.clicked.connect(self.stop_command_callback)
self.joystick_mode_checkbox.clicked.connect(
self.joystick_mode_check_callback)
self.ignore_invalid_steps_checkbox.clicked.connect(
self._publish_parameters)
class SelectDialog(QDialog):
'''
This dialog creates an input mask for a string list and return selected entries.
'''
def __init__(self, items=list(), buttons=QDialogButtonBox.Cancel | QDialogButtonBox.Ok, exclusive=False,
preselect_all=False, title='', description='', icon='', parent=None, select_if_single=True,
checkitem1='', checkitem2=''):
'''
Creates an input dialog.
@param items: a list with strings
@type items: C{list()}
'''
QDialog.__init__(self, parent=parent)
self.setObjectName(' - '.join(['SelectDialog', str(items)]))
self.verticalLayout = QVBoxLayout(self)
self.verticalLayout.setObjectName("verticalLayout")
self.verticalLayout.setContentsMargins(1, 1, 1, 1)
# add filter row
self.filter_frame = QFrame(self)
filterLayout = QHBoxLayout(self.filter_frame)
filterLayout.setContentsMargins(1, 1, 1, 1)
label = QLabel("Filter:", self.filter_frame)
self.filter_field = EnchancedLineEdit(self.filter_frame)
filterLayout.addWidget(label)
filterLayout.addWidget(self.filter_field)
self.filter_field.textChanged.connect(self._on_filter_changed)
self.verticalLayout.addWidget(self.filter_frame)
if description:
self.description_frame = QFrame(self)
descriptionLayout = QHBoxLayout(self.description_frame)
# descriptionLayout.setContentsMargins(1, 1, 1, 1)
if icon:
self.icon_label = QLabel(self.description_frame)
self.icon_label.setSizePolicy(QSizePolicy.Fixed, QSizePolicy.Fixed)
self.icon_label.setPixmap(QPixmap(icon).scaled(30, 30, Qt.KeepAspectRatio))
descriptionLayout.addWidget(self.icon_label)
self.description_label = QLabel(self.description_frame)
self.description_label.setWordWrap(True)
self.description_label.setText(description)
descriptionLayout.addWidget(self.description_label)
self.verticalLayout.addWidget(self.description_frame)
# create area for the parameter
self.content = MainBox(self)
if items:
self.scroll_area = QScrollArea(self)
self.scroll_area.setFocusPolicy(Qt.NoFocus)
self.scroll_area.setObjectName("scroll_area")
self.scroll_area.setWidgetResizable(True)
self.scroll_area.setWidget(self.content)
self.verticalLayout.addWidget(self.scroll_area)
self.checkitem1 = checkitem1
self.checkitem1_result = False
self.checkitem2 = checkitem2
self.checkitem2_result = False
# add select all option
if not exclusive and items:
self._ignore_next_toggle = False
self.select_all_checkbox = QCheckBox('all entries')
self.select_all_checkbox.setTristate(True)
self.select_all_checkbox.stateChanged.connect(self._on_select_all_checkbox_stateChanged)
self.verticalLayout.addWidget(self.select_all_checkbox)
self.content.toggled.connect(self._on_main_toggle)
if self.checkitem1:
self.checkitem1_checkbox = QCheckBox(self.checkitem1)
self.checkitem1_checkbox.stateChanged.connect(self._on_select_checkitem1_checkbox_stateChanged)
self.verticalLayout.addWidget(self.checkitem1_checkbox)
if self.checkitem2:
self.checkitem2_checkbox = QCheckBox(self.checkitem2)
self.checkitem2_checkbox.stateChanged.connect(self._on_select_checkitem2_checkbox_stateChanged)
self.verticalLayout.addWidget(self.checkitem2_checkbox)
if not items:
spacerItem = QSpacerItem(1, 1, QSizePolicy.Expanding, QSizePolicy.Expanding)
self.verticalLayout.addItem(spacerItem)
# create buttons
self.buttonBox = QDialogButtonBox(self)
self.buttonBox.setObjectName("buttonBox")
self.buttonBox.setOrientation(Qt.Horizontal)
self.buttonBox.setStandardButtons(buttons)
self.buttonBox.accepted.connect(self.accept)
self.buttonBox.rejected.connect(self.reject)
self.verticalLayout.addWidget(self.buttonBox)
# set the input fields
if items:
self.content.createFieldsFromValues(items, exclusive)
if (select_if_single and len(items) == 1) or preselect_all:
self.select_all_checkbox.setCheckState(Qt.Checked)
if not items or len(items) < 7:
self.filter_frame.setVisible(False)
# print '=============== create', self.objectName()
#
# def __del__(self):
# print "************ destroy", self.objectName()
def _on_main_toggle(self, state):
self._ignore_next_toggle = state != self.select_all_checkbox.checkState()
self.select_all_checkbox.setCheckState(state)
def _on_select_all_checkbox_stateChanged(self, state):
if not self._ignore_next_toggle:
self.content.setState(state)
self._ignore_next_toggle = False
def _on_select_checkitem1_checkbox_stateChanged(self, state):
if state == Qt.Checked:
self.checkitem1_result = True
elif state == Qt.Unchecked:
self.checkitem1_result = False
def _on_select_checkitem2_checkbox_stateChanged(self, state):
if state == Qt.Checked:
self.checkitem2_result = True
elif state == Qt.Unchecked:
self.checkitem2_result = False
def _on_filter_changed(self):
self.content.filter(self.filter_field.text())
def getSelected(self):
return self.content.getSelected()
@staticmethod
def getValue(title, description='', items=list(), exclusive=False, preselect_all=False, icon='', parent=None, select_if_single=True, checkitem1='', checkitem2=''):
selectDia = SelectDialog(items, exclusive=exclusive, preselect_all=preselect_all, description=description, icon=icon, parent=parent, select_if_single=select_if_single, checkitem1=checkitem1, checkitem2=checkitem2)
selectDia.setWindowTitle(title)
selectDia.resize(480, 256)
if selectDia.exec_():
if selectDia.checkitem2:
return selectDia.getSelected(), True, selectDia.checkitem1_result, selectDia.checkitem2_result
if selectDia.checkitem1:
return selectDia.getSelected(), True, selectDia.checkitem1_result
return selectDia.getSelected(), True
if selectDia.checkitem2:
return list(), False, False, False
if selectDia.checkitem1:
return list(), False, False
return list(), False
# %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
# %%%%%%%%%%%%%%%%%% close handling %%%%%%%%%%%%%%%%%%%%%
# %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
def accept(self):
self.setResult(QDialog.Accepted)
self.hide()
def reject(self):
self.setResult(QDialog.Rejected)
self.hide()
def hideEvent(self, event):
self.close()
def closeEvent(self, event):
'''
Test the open files for changes and save this if needed.
'''
self.setAttribute(Qt.WA_DeleteOnClose, True)
QDialog.closeEvent(self, event)
class JointControlWidget(QObject):
updateStateSignal = Signal(object)
updateGhostSignal = Signal(object)
def __init__(self, context):
super(JointControlWidget, self).__init__()
self.updateStateSignal.connect(self.on_update_state)
self.updateGhostSignal.connect(self.on_update_ghost)
self.joint_states = JointState()
self.ghost_joint_states = JointState()
self._widget = context
vbox = QVBoxLayout()
# Define checkboxes
radios = QWidget()
hbox_radio = QHBoxLayout()
self.radioGroup = QButtonGroup()
self.radioGroup.setExclusive(True)
self.radio_ghost_target = QRadioButton()
self.radio_ghost_target.setText("Ghost")
self.radioGroup.addButton(self.radio_ghost_target, 0)
self.radio_ghost_target.setChecked(True)
self.radio_robot_target = QRadioButton()
self.radio_robot_target.setText("Robot")
self.radioGroup.addButton(self.radio_robot_target, 1)
hbox_radio.addStretch()
hbox_radio.addWidget(self.radio_ghost_target)
hbox_radio.addStretch()
hbox_radio.addWidget(self.radio_robot_target)
hbox_radio.addStretch()
radios.setLayout(hbox_radio)
vbox.addWidget(radios)
duration_box = QHBoxLayout()
duration_box.setAlignment(Qt.AlignLeft)
duration_box.addWidget(QLabel("Trajectory duration (s):"))
self.traj_duration_spin = QDoubleSpinBox()
self.traj_duration_spin.setValue(1.0)
self.traj_duration_spin.valueChanged.connect(self.on_traj_duration_changed)
duration_box.addWidget(self.traj_duration_spin)
self.update_controllers_buttonn = QPushButton("Update Controllers")
self.update_controllers_buttonn.pressed.connect(self.on_update_controllers)
duration_box.addWidget(self.update_controllers_buttonn)
vbox.addLayout(duration_box)
widget = QWidget()
hbox = QHBoxLayout()
# Left to right layout
self.joint_control = JointControl(self, hbox)
widget.setLayout(hbox)
vbox.addWidget(widget)
print "Add buttons to apply all ..."
all_widget = QWidget()
all_box = QHBoxLayout()
self.snap_to_ghost_button = QPushButton("SnapAllGhost")
self.snap_to_ghost_button.pressed.connect(self.on_snap_ghost_pressed)
all_box.addWidget(self.snap_to_ghost_button)
self.snap_to_current_button = QPushButton("SnapAllCurrent")
self.snap_to_current_button.pressed.connect(self.on_snap_current_pressed)
all_box.addWidget(self.snap_to_current_button)
self.apply_to_robot_button = QPushButton("ApplyAllRobot")
self.apply_to_robot_button.pressed.connect(self.on_apply_robot_pressed)
all_box.addWidget(self.apply_to_robot_button)
self.apply_to_robot_button = QPushButton("Apply WBC Robot")
self.apply_to_robot_button.pressed.connect(self.on_apply_wbc_robot_pressed)
all_box.addWidget(self.apply_to_robot_button)
all_widget.setLayout(all_box)
vbox.addWidget(all_widget)
override_box = QHBoxLayout()
self.override = QCheckBox()
self.override.setChecked(False)
self.override.stateChanged.connect(self.on_override_changed)
override_box.addWidget(self.override)
override_label = QLabel("SAFETY OVERRIDE")
override_label.setStyleSheet('QLabel { color: red }')
override_box.addWidget(override_label)
override_box.addStretch()
vbox.addLayout(override_box)
vbox.addStretch()
self._widget.setLayout(vbox)
self.first_time = True
self.stateSubscriber = rospy.Subscriber('/joint_states', JointState, self.state_callback_fnc)
self.ghostSubscriber = rospy.Subscriber('/flor/ghost/get_joint_states', JointState, self.ghost_callback_fnc)
self.wbc_robot_pub = rospy.Publisher('/flor/wbc_controller/joint_states', JointState, queue_size=10)
self.time_last_update_state = time.time()
self.time_last_update_ghost = time.time()
def shutdown_plugin(self):
# Just make sure to stop timers and publishers, unsubscribe from Topics etc in the shutdown_plugin method.
print "Shutdown ..."
self.stateSubscriber.unregister()
self.ghostSubscriber.unregister()
self.wbc_robot_pub.unregister()
print "Done!"
def state_callback_fnc(self, atlas_state_msg):
self.updateStateSignal.emit(atlas_state_msg)
def command_callback_fnc(self, atlas_command_msg):
self.updateCommandSignal.emit(atlas_command_msg)
def ghost_callback_fnc(self, ghost_joint_state_msg):
self.updateGhostSignal.emit(ghost_joint_state_msg)
def on_update_controllers(self):
self.joint_control.update_controllers()
def on_override_changed(self):
if self.override.isChecked():
print "WARNING: TURNING OFF SAFETY"
for controller in self.joint_control.controllers:
controller.set_override(True)
else:
print "Enabling safety checks"
for controller in self.joint_control.controllers:
controller.set_override(False)
def on_snap_ghost_pressed(self):
print "Snap all joint values to current ghost joint positions"
for controller in self.joint_control.controllers:
controller.on_snap_ghost_pressed()
def on_snap_current_pressed(self):
print "Snap all joint values to current joint positions"
for controller in self.joint_control.controllers:
controller.on_snap_current_pressed()
def on_apply_robot_pressed(self):
print "Send all latest joint values directly to robot"
for controller in self.joint_control.controllers:
controller.on_apply_robot_pressed()
def on_traj_duration_changed(self, duration):
self.joint_control.update_traj_duration(duration)
def on_apply_wbc_robot_pressed(self):
print "Send all latest joint values directly to robot as a WBC command"
if self.first_time:
print "Uninitialized !"
else:
joint_state = JointState()
joint_state.header.stamp = rospy.Time.now()
joint_state.name = copy.deepcopy(self.joint_states.name)
joint_state.position = list(copy.deepcopy(self.joint_states.position))
joint_state.velocity = list(copy.deepcopy(self.joint_states.velocity))
# print "Positions: ",joint_state.position
for i, name in enumerate(joint_state.name):
# for each joint, retrieve the data from each controller to populate the WBC vectors
for controller in self.joint_control.controllers:
for joint in controller.joints:
if joint.name == self.joint_states.name[i]:
print " fetching value for " + name + " = " + str(joint.position) + " at ndx=" + str(i)
joint_state.position[i] = joint.position
joint_state.velocity[i] = joint.velocity
# joint_state.effort[i] = joint.effort
self.wbc_robot_pub.publish(joint_state)
def on_update_state(self, atlas_state_msg):
if time.time() - self.time_last_update_state >= 0.2:
self.joint_states = atlas_state_msg
self.joint_control.update_joint_positions(self.joint_states)
if self.first_time:
self.joint_control.reset_current_joint_sliders()
self.first_time = False
self.ghost_joint_states = copy.deepcopy(atlas_state_msg)
self.ghost_joint_states.position = list(self.ghost_joint_states.position)
self.time_last_update_state = time.time()
# this runs in the Qt GUI thread and can therefore update the GUI
def on_update_ghost(self, ghost_joint_state_msg):
if time.time() - self.time_last_update_ghost >= 0.2:
for i, new_joint_name in enumerate(list(ghost_joint_state_msg.name)):
name_found = False
for j, stored_joint_name in enumerate(self.ghost_joint_states.name):
if new_joint_name == stored_joint_name:
name_found = True
self.ghost_joint_states.position[j] = ghost_joint_state_msg.position[i]
if not name_found:
# Update the list without regard to order
self.ghost_joint_states.name.append(new_joint_name)
self.ghost_joint_states.position.append(ghost_joint_state_msg.position[i])
self.time_last_update_ghost = time.time()
def addCheckBox(self, node):
item = QCheckBox(node, self)
item.stateChanged.connect(lambda x: self.updateNode(x, node))
self.selection_vlayout.addWidget(item)
def __init__(self, context):
# super(BDIPelvisPoseWidget, self).__init__(context)
# self.setObjectName('BDIPelvisPoseWidget')
super(BDIPelvisPoseWidget, self).__init__()
self.name = "BDIPelvisPoseWidget"
self.updateStateSignal.connect(self.on_updateState)
# self._widget = QWidget()
self._widget = context
vbox = QVBoxLayout()
self.forward_position = 0.0
self.lateral_position = 0.0
self.height_position = 0.91
self.roll_position = 0.0
self.pitch_position = 0.0
self.yaw_position = 0.0
self.currentForward = 0.0
self.currentLateral = 0.0
self.currentHeight = 0.91
self.currentRoll = 0.0
self.currentPitch = 0.0
self.currentYaw = 0.0
# Define checkboxes
vbox = QVBoxLayout()
label = QLabel()
label.setText("BDI Pelvis Height (Manipulate Mode Only)") # todo - connect controller mode
vbox.addWidget(label)
self.enable_checkbox = QCheckBox("Enable")
self.enable_checkbox.stateChanged.connect(self.on_enable_check)
vbox.addWidget(self.enable_checkbox)
self.snap_to_current_button = QPushButton("Snap to Current")
self.snap_to_current_button.pressed.connect(self.on_snapCurrentPressed)
vbox.addWidget(self.snap_to_current_button)
self.roll_slider = QSlider(Qt.Horizontal)
self.roll_label = QLabel()
self.roll_label.setText("Roll")
vbox.addWidget(self.roll_label)
self.roll_slider.setRange(int(-100), int(101))
self.roll_slider.setValue(int(0))
self.roll_slider.setSingleStep((200) / 50)
self.roll_slider.setTickInterval(25)
self.roll_slider.valueChanged.connect(self.on_rollSliderMoved)
vbox.addWidget(self.roll_slider)
self.roll_progress_bar = QProgressBar()
self.roll_progress_bar.setRange(int(-100), int(101))
self.roll_progress_bar.setValue((6.0 / math.pi) * self.currentRoll * 100)
self.roll_progress_bar.setFormat("%.6f" % self.currentRoll)
vbox.addWidget(self.roll_progress_bar)
self.pitch_slider = QSlider(Qt.Horizontal)
self.pitch_label = QLabel()
self.pitch_label.setText("Pitch")
vbox.addWidget(self.pitch_label)
self.pitch_slider.setRange(int(-100), int(101))
self.pitch_slider.setValue(int(0))
self.pitch_slider.setSingleStep((200) / 50)
self.pitch_slider.setTickInterval(25)
self.pitch_slider.valueChanged.connect(self.on_pitchSliderMoved)
vbox.addWidget(self.pitch_slider)
self.pitch_progress_bar = QProgressBar()
self.pitch_progress_bar.setRange(int(-100), int(101))
self.pitch_progress_bar.setValue((6.0 / math.pi) * self.currentPitch * 100)
self.pitch_progress_bar.setFormat("%.6f" % self.currentPitch)
vbox.addWidget(self.pitch_progress_bar)
self.yaw_slider = QSlider(Qt.Horizontal)
self.yaw_label = QLabel()
self.yaw_label.setText("Yaw")
vbox.addWidget(self.yaw_label)
self.yaw_slider.setRange(int(-100), int(101))
self.yaw_slider.setValue(int(0))
self.yaw_slider.setSingleStep((200) / 50)
self.yaw_slider.setTickInterval(25)
self.yaw_slider.valueChanged.connect(self.on_yawSliderMoved)
vbox.addWidget(self.yaw_slider)
self.yaw_progress_bar = QProgressBar()
self.yaw_progress_bar.setRange(int(-100), int(101))
self.yaw_progress_bar.setValue((4.0 / math.pi) * self.currentYaw * 100)
self.yaw_progress_bar.setFormat("%.6f" % self.currentYaw)
vbox.addWidget(self.yaw_progress_bar)
self.forward_label = QLabel()
self.forward_label.setText("Forward")
vbox.addWidget(self.forward_label)
widget = QWidget()
hbox = QHBoxLayout()
hbox.addStretch()
self.forward_slider = QSlider(Qt.Vertical)
# self.forward_slider.setText("Height")
self.forward_slider.setRange(int(-101), int(100))
self.forward_slider.setValue(int(0))
self.forward_slider.setSingleStep(1)
self.forward_slider.setTickInterval(10)
self.forward_slider.valueChanged.connect(self.on_forwardSliderMoved)
hbox.addWidget(self.forward_slider)
self.forward_progress_bar = QProgressBar()
self.forward_progress_bar.setOrientation(Qt.Vertical)
self.forward_progress_bar.setRange(int(-100), int(101))
self.forward_progress_bar.setValue((self.currentForward / 0.075) * 100)
self.forward_progress_bar.setTextVisible(False)
hbox.addWidget(self.forward_progress_bar)
self.forward_progress_bar_label = QLabel()
self.forward_progress_bar_label.setText("%.6f" % self.currentForward)
hbox.addWidget(self.forward_progress_bar_label)
hbox.addStretch()
widget.setLayout(hbox)
vbox.addWidget(widget)
self.lateral_label = QLabel()
self.lateral_label.setText("Lateral")
vbox.addWidget(self.lateral_label)
self.lateral_slider = QSlider(Qt.Horizontal)
# self.lateral_slider.setText("Lateral")
self.lateral_slider.setRange(int(-100), int(101))
self.lateral_slider.setValue(int(0))
self.lateral_slider.setSingleStep((200) / 50)
self.lateral_slider.setTickInterval(25)
self.lateral_slider.valueChanged.connect(self.on_lateralSliderMoved)
vbox.addWidget(self.lateral_slider)
self.lateral_progress_bar = QProgressBar()
self.lateral_progress_bar.setRange(int(-100), int(101))
self.lateral_progress_bar.setValue((self.currentLateral / 0.15) * 100)
self.lateral_progress_bar.setFormat("%.6f" % self.currentLateral)
vbox.addWidget(self.lateral_progress_bar)
self.height_label = QLabel()
self.height_label.setText("Height")
vbox.addWidget(self.height_label)
widget = QWidget()
hbox = QHBoxLayout()
hbox.addStretch()
self.height_slider = QSlider(Qt.Vertical)
# self.height_slider.setText("Height")
self.height_slider.setRange(int(55), int(105))
self.height_slider.setValue(int(91))
self.height_slider.setSingleStep(2)
self.height_slider.setTickInterval(10)
self.height_slider.valueChanged.connect(self.on_heightSliderMoved)
hbox.addWidget(self.height_slider)
self.height_progress_bar = QProgressBar()
self.height_progress_bar.setOrientation(Qt.Vertical)
self.height_progress_bar.setRange(int(55), int(105))
self.height_progress_bar.setValue(self.currentHeight * 100)
self.height_progress_bar.setTextVisible(False)
hbox.addWidget(self.height_progress_bar)
self.height_progress_bar_label = QLabel()
self.height_progress_bar_label.setText("%.6f" % self.currentHeight)
hbox.addWidget(self.height_progress_bar_label)
hbox.addStretch()
widget.setLayout(hbox)
vbox.addWidget(widget)
vbox.addStretch()
self._widget.setLayout(vbox)
# context.add_widget(self._widget)
self.height_position = 0.91
self.lateral_position = 0.0
self.yaw_position = 0.0
self.first_time = True
self.enable_checkbox.setChecked(False)
self.yaw_slider.setEnabled(False)
self.roll_slider.setEnabled(False)
self.pitch_slider.setEnabled(False)
self.forward_slider.setEnabled(False)
self.lateral_slider.setEnabled(False)
self.height_slider.setEnabled(False)
self.pub_robot = rospy.Publisher("/flor/controller/bdi_desired_pelvis_pose", PoseStamped, queue_size=10)
# self.stateSubscriber = rospy.Subscriber('/flor/pelvis_controller/current_states',JointState, self.stateCallbackFnc)
# self.pub_bdi_pelvis = rospy.Publisher('/bdi_manipulate_pelvis_pose_rpy',PoseStamped)
self.pelvis_trajectory_pub = rospy.Publisher(
"/robot_controllers/pelvis_traj_controller/command", JointTrajectory, queue_size=10
)
self.stateSubscriber = rospy.Subscriber(
"/robot_controllers/pelvis_traj_controller/state", JointTrajectoryControllerState, self.stateCallbackFnc
)
def _set_add_rocon_master(self):
if self._connect_dlg_isValid:
console.logdebug("Dialog is live!!")
self._connect_dlg.done(0)
#dialog
self._connect_dlg = QDialog(self._widget_main)
self._connect_dlg.setWindowTitle("Add Ros Master")
self._connect_dlg.setSizePolicy(QSizePolicy.MinimumExpanding,
QSizePolicy.Ignored)
self._connect_dlg.setMinimumSize(350, 0)
# dlg_rect = self._connect_dlg.geometry()
#dialog layout
ver_layout = QVBoxLayout(self._connect_dlg)
ver_layout.setContentsMargins(9, 9, 9, 9)
#param layout
text_grid_sub_widget = QWidget()
text_grid_layout = QGridLayout(text_grid_sub_widget)
text_grid_layout.setColumnStretch(1, 0)
text_grid_layout.setRowStretch(2, 0)
#param 1
title_widget1 = QLabel("MASTER_URI: ")
context_widget1 = QTextEdit()
context_widget1.setSizePolicy(QSizePolicy.MinimumExpanding,
QSizePolicy.Ignored)
context_widget1.setMinimumSize(0, 30)
context_widget1.append(self.master_uri)
#param 2
title_widget2 = QLabel("HOST_NAME: ")
context_widget2 = QTextEdit()
context_widget2.setSizePolicy(QSizePolicy.MinimumExpanding,
QSizePolicy.Ignored)
context_widget2.setMinimumSize(0, 30)
context_widget2.append(self.host_name)
#add param
text_grid_layout.addWidget(title_widget1)
text_grid_layout.addWidget(context_widget1)
text_grid_layout.addWidget(title_widget2)
text_grid_layout.addWidget(context_widget2)
#add param layout
ver_layout.addWidget(text_grid_sub_widget)
#button layout
button_hor_sub_widget = QWidget()
button_hor_layout = QHBoxLayout(button_hor_sub_widget)
uri_text_widget = context_widget1
host_name_text_widget = context_widget2
#check box
use_env_var_check = QCheckBox("Use environment variables")
use_env_var_check.setCheckState(Qt.Unchecked)
def set_use_env_var(data, text_widget1, text_widget2):
if data == Qt.Unchecked:
text_widget1.setText(self.master_uri)
text_widget2.setText(self.host_name)
elif data == Qt.Checked:
self.master_uri = str(text_widget1.toPlainText())
self.host_name = str(text_widget2.toPlainText())
text_widget1.setText(self.env_master_uri)
text_widget2.setText(self.env_host_name)
def check_event(data):
set_use_env_var(data, context_widget1, context_widget2)
use_env_var_check.stateChanged.connect(check_event)
ver_layout.addWidget(use_env_var_check)
#button
btn_call = QPushButton("Add")
btn_cancel = QPushButton("Cancel")
btn_call.clicked.connect(lambda: self._connect_dlg.done(0))
btn_call.clicked.connect(lambda: self._add_rocon_master(
uri_text_widget, host_name_text_widget))
btn_cancel.clicked.connect(lambda: self._connect_dlg.done(0))
#add button
button_hor_layout.addWidget(btn_call)
button_hor_layout.addWidget(btn_cancel)
#add button layout
ver_layout.addWidget(button_hor_sub_widget)
self._connect_dlg.setVisible(True)
self._connect_dlg.finished.connect(self._destroy_connect_dlg)
self._connect_dlg_isValid = True
class TopicSelection(QWidget):
recordSettingsSelected = Signal(bool, list)
def __init__(self):
super(TopicSelection, self).__init__()
master = rosgraph.Master('rqt_bag_recorder')
self.setWindowTitle("Select the topics you want to record")
self.resize(500, 700)
self.topic_list = []
self.selected_topics = []
self.items_list = []
self.area = QScrollArea(self)
self.main_widget = QWidget(self.area)
self.ok_button = QPushButton("Record", self)
self.ok_button.clicked.connect(self.onButtonClicked)
self.ok_button.setEnabled(False)
self.main_vlayout = QVBoxLayout(self)
self.main_vlayout.addWidget(self.area)
self.main_vlayout.addWidget(self.ok_button)
self.setLayout(self.main_vlayout)
self.selection_vlayout = QVBoxLayout(self)
self.item_all = QCheckBox("All", self)
self.item_all.stateChanged.connect(self.updateList)
self.selection_vlayout.addWidget(self.item_all)
topic_data_list = master.getPublishedTopics('')
topic_data_list.sort()
for topic, datatype in topic_data_list:
self.addCheckBox(topic)
self.main_widget.setLayout(self.selection_vlayout)
self.area.setWidget(self.main_widget)
self.show()
def addCheckBox(self, topic):
self.topic_list.append(topic)
item = QCheckBox(topic, self)
item.stateChanged.connect(lambda x: self.updateList(x,topic))
self.items_list.append(item)
self.selection_vlayout.addWidget(item)
def updateList(self, state, topic = None):
if topic is None: # The "All" checkbox was checked / unchecked
if state == Qt.Checked:
self.item_all.setTristate(False)
for item in self.items_list:
if item.checkState() == Qt.Unchecked:
item.setCheckState(Qt.Checked)
elif state == Qt.Unchecked:
self.item_all.setTristate(False)
for item in self.items_list:
if item.checkState() == Qt.Checked:
item.setCheckState(Qt.Unchecked)
else:
if state == Qt.Checked:
self.selected_topics.append(topic)
else:
self.selected_topics.remove(topic)
if self.item_all.checkState()==Qt.Checked:
self.item_all.setCheckState(Qt.PartiallyChecked)
if self.selected_topics == []:
self.ok_button.setEnabled(False)
else:
self.ok_button.setEnabled(True)
def onButtonClicked(self):
self.close()
self.recordSettingsSelected.emit((self.item_all.checkState() == Qt.Checked), self.selected_topics)
def __init__(self, context):
super(SensorParamControlDialog, self).__init__(context)
self.setObjectName('SensorParamControlDialog')
self._widget = QWidget()
vbox = QVBoxLayout()
### Multisense ###
ms_groupbox = QGroupBox( "Multisense" )
ms_vbox = QVBoxLayout()
ms_gain_hbox = QHBoxLayout()
self.ms_gain_label = QLabel("Image Gain [1, 1, 8]")
ms_gain_hbox.addWidget( self.ms_gain_label )
self.ms_gain = QDoubleSpinBox()
self.ms_gain.setSingleStep(.01)
self.ms_gain.setRange(1,8)
ms_gain_hbox.addWidget( self.ms_gain )
ms_vbox.addLayout( ms_gain_hbox )
ms_exp_hbox = QHBoxLayout()
self.ms_exp_auto = QCheckBox("Image Exposure [.03, 0.5]")
ms_exp_hbox.addWidget( self.ms_exp_auto )
self.ms_exp = QDoubleSpinBox()
self.ms_exp.setSingleStep( .001 )
self.ms_exp.setRange( .025,.5 )
ms_exp_hbox.addWidget( self.ms_exp )
ms_vbox.addLayout( ms_exp_hbox )
ms_spindle_hbox = QHBoxLayout()
ms_spindle_label = QLabel("Spindle Speed [0, 5.2]")
ms_spindle_hbox.addWidget( ms_spindle_label )
self.ms_spindle = QDoubleSpinBox()
self.ms_spindle.setSingleStep(.01)
self.ms_spindle.setRange( 0,15.2 )
ms_spindle_hbox.addWidget( self.ms_spindle )
ms_vbox.addLayout( ms_spindle_hbox )
ms_light_hbox = QHBoxLayout()
ms_light_label = QLabel("Light Brightness")
ms_light_hbox.addWidget(ms_light_label)
self.ms_light = QSlider(Qt.Horizontal)
self.ms_light.setRange(0,100)
ms_light_hbox.addWidget( self.ms_light )
ms_vbox.addLayout( ms_light_hbox )
ms_button_hbox = QHBoxLayout()
ms_button_hbox.addStretch(1)
ms_button_get = QPushButton("Get Settings")
ms_button_get.pressed.connect(self.ms_get_callback)
#ms_button_hbox.addWidget( ms_button_get )
ms_button_set = QPushButton("Set Settings")
ms_button_set.pressed.connect(self.ms_set_callback)
ms_button_hbox.addWidget( ms_button_set )
ms_vbox.addLayout( ms_button_hbox )
ms_groupbox.setLayout( ms_vbox )
vbox.addWidget( ms_groupbox )
### Left SA ###
sa_left_groupbox = QGroupBox( "Left SA Camera" )
sa_left_vbox = QVBoxLayout()
sa_left_gain_hbox = QHBoxLayout()
sa_left_gain_label = QLabel("Image Gain [0, 0, 25]")
sa_left_gain_hbox.addWidget( sa_left_gain_label )
self.sa_left_gain = QDoubleSpinBox()
self.sa_left_gain.setSingleStep(.01)
self.sa_left_gain.setRange( 0, 25 )
sa_left_gain_hbox.addWidget( self.sa_left_gain )
sa_left_vbox.addLayout( sa_left_gain_hbox )
sa_left_exp_hbox = QHBoxLayout()
sa_left_exp_label = QLabel("Image Shutter [0, 5.5, 25]")
sa_left_exp_hbox.addWidget( sa_left_exp_label )
self.sa_left_exp = QDoubleSpinBox()
self.sa_left_exp.setSingleStep(.01)
self.sa_left_exp.setRange( 0, 25 )
sa_left_exp_hbox.addWidget( self.sa_left_exp )
sa_left_vbox.addLayout( sa_left_exp_hbox )
sa_left_button_hbox = QHBoxLayout()
sa_left_button_hbox.addStretch(1)
sa_left_button_get = QPushButton("Get Settings")
sa_left_button_get.pressed.connect(self.sa_left_get_callback)
#sa_left_button_hbox.addWidget( sa_left_button_get )
sa_left_button_set = QPushButton("Set Settings")
sa_left_button_set.pressed.connect(self.sa_left_set_callback)
sa_left_button_hbox.addWidget( sa_left_button_set )
sa_left_vbox.addLayout( sa_left_button_hbox )
sa_left_groupbox.setLayout( sa_left_vbox )
vbox.addWidget(sa_left_groupbox)
### Right SA ###
sa_right_groupbox = QGroupBox( "Right SA Camera" )
sa_right_vbox = QVBoxLayout()
sa_right_gain_hbox = QHBoxLayout()
sa_right_gain_label = QLabel("Image Gain [0, 0, 25]")
sa_right_gain_hbox.addWidget( sa_right_gain_label )
self.sa_right_gain = QDoubleSpinBox()
self.sa_right_gain.setSingleStep(.01)
self.sa_right_gain.setRange( 0, 25 )
sa_right_gain_hbox.addWidget( self.sa_right_gain )
sa_right_vbox.addLayout( sa_right_gain_hbox )
sa_right_exp_hbox = QHBoxLayout()
sa_right_exp_label = QLabel("Image Shutter [0, 5.5, 25]")
sa_right_exp_hbox.addWidget( sa_right_exp_label )
self.sa_right_exp = QDoubleSpinBox()
self.sa_right_exp.setSingleStep(.01)
self.sa_right_exp.setRange( 0, 25 )
sa_right_exp_hbox.addWidget( self.sa_right_exp )
sa_right_vbox.addLayout( sa_right_exp_hbox )
sa_right_button_hbox = QHBoxLayout()
sa_right_button_hbox.addStretch(1)
sa_right_button_get = QPushButton("Get Settings")
sa_right_button_get.pressed.connect(self.sa_right_get_callback)
#sa_right_button_hbox.addWidget( sa_right_button_get )
sa_right_button_set = QPushButton("Set Settings")
sa_right_button_set.pressed.connect(self.sa_right_set_callback)
sa_right_button_hbox.addWidget( sa_right_button_set )
sa_right_vbox.addLayout( sa_right_button_hbox )
sa_right_groupbox.setLayout( sa_right_vbox )
vbox.addWidget(sa_right_groupbox)
vbox.addStretch(1)
self._widget.setLayout(vbox)
context.add_widget(self._widget)
# publishers and subscribers
self.ms_cam_pub = rospy.Publisher( '/multisense_sl/set_cam_parameters', Float64MultiArray, queue_size=10)
self.ms_light_pub = rospy.Publisher( '/multisense_sl/set_light_brightness', Float64, queue_size=10)
self.ms_spindle_pub = rospy.Publisher( '/multisense_sl/set_spindle_speed', Float64, queue_size=10)
self.sa_left_cam_pub = rospy.Publisher( '/sa/left/set_cam_parameters', Float64MultiArray, queue_size=10)
self.sa_right_cam_pub = rospy.Publisher( '/sa/right/set_cam_parameters', Float64MultiArray, queue_size=10)
def __init__(self, context):
super(Top, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('Top')
# 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
self._selected_node = ''
self._selected_node_lock = RLock()
# Setup the toolbar
self._toolbar = QToolBar()
self._filter_box = QLineEdit()
self._regex_box = QCheckBox()
self._regex_box.setText('regex')
self._toolbar.addWidget(QLabel('Filter'))
self._toolbar.addWidget(self._filter_box)
self._toolbar.addWidget(self._regex_box)
self._filter_box.returnPressed.connect(self.update_filter)
self._regex_box.stateChanged.connect(self.update_filter)
# Create a container widget and give it a layout
self._container = QWidget()
self._container.setWindowTitle('Process Monitor')
self._layout = QVBoxLayout()
self._container.setLayout(self._layout)
self._layout.addWidget(self._toolbar)
# Create the table widget
self._table_widget = QTreeWidget()
self._table_widget.setObjectName('TopTable')
self._table_widget.setColumnCount(len(self.NODE_LABELS))
self._table_widget.setHeaderLabels(self.NODE_LABELS)
self._table_widget.itemClicked.connect(self._tableItemClicked)
self._table_widget.setSortingEnabled(True)
self._table_widget.setAlternatingRowColors(True)
self._layout.addWidget(self._table_widget)
context.add_widget(self._container)
# Add a button for killing nodes
self._kill_button = QPushButton('Kill Node')
self._layout.addWidget(self._kill_button)
self._kill_button.clicked.connect(self._kill_node)
# Update twice since the first cpu% lookup will always return 0
self.update_table()
self.update_table()
self._table_widget.resizeColumnToContents(0)
# Start a timer to trigger updates
self._update_timer = QTimer()
self._update_timer.setInterval(1000)
self._update_timer.timeout.connect(self.update_table)
self._update_timer.start()
def __init__(self, items=list(), buttons=QDialogButtonBox.Cancel | QDialogButtonBox.Ok, exclusive=False,
preselect_all=False, title='', description='', icon='', parent=None, select_if_single=True,
checkitem1='', checkitem2=''):
'''
Creates an input dialog.
@param items: a list with strings
@type items: C{list()}
'''
QDialog.__init__(self, parent=parent)
self.setObjectName(' - '.join(['SelectDialog', str(items)]))
self.verticalLayout = QVBoxLayout(self)
self.verticalLayout.setObjectName("verticalLayout")
self.verticalLayout.setContentsMargins(1, 1, 1, 1)
# add filter row
self.filter_frame = QFrame(self)
filterLayout = QHBoxLayout(self.filter_frame)
filterLayout.setContentsMargins(1, 1, 1, 1)
label = QLabel("Filter:", self.filter_frame)
self.filter_field = EnchancedLineEdit(self.filter_frame)
filterLayout.addWidget(label)
filterLayout.addWidget(self.filter_field)
self.filter_field.textChanged.connect(self._on_filter_changed)
self.verticalLayout.addWidget(self.filter_frame)
if description:
self.description_frame = QFrame(self)
descriptionLayout = QHBoxLayout(self.description_frame)
# descriptionLayout.setContentsMargins(1, 1, 1, 1)
if icon:
self.icon_label = QLabel(self.description_frame)
self.icon_label.setSizePolicy(QSizePolicy.Fixed, QSizePolicy.Fixed)
self.icon_label.setPixmap(QPixmap(icon).scaled(30, 30, Qt.KeepAspectRatio))
descriptionLayout.addWidget(self.icon_label)
self.description_label = QLabel(self.description_frame)
self.description_label.setWordWrap(True)
self.description_label.setText(description)
descriptionLayout.addWidget(self.description_label)
self.verticalLayout.addWidget(self.description_frame)
# create area for the parameter
self.content = MainBox(self)
if items:
self.scroll_area = QScrollArea(self)
self.scroll_area.setFocusPolicy(Qt.NoFocus)
self.scroll_area.setObjectName("scroll_area")
self.scroll_area.setWidgetResizable(True)
self.scroll_area.setWidget(self.content)
self.verticalLayout.addWidget(self.scroll_area)
self.checkitem1 = checkitem1
self.checkitem1_result = False
self.checkitem2 = checkitem2
self.checkitem2_result = False
# add select all option
if not exclusive and items:
self._ignore_next_toggle = False
self.select_all_checkbox = QCheckBox('all entries')
self.select_all_checkbox.setTristate(True)
self.select_all_checkbox.stateChanged.connect(self._on_select_all_checkbox_stateChanged)
self.verticalLayout.addWidget(self.select_all_checkbox)
self.content.toggled.connect(self._on_main_toggle)
if self.checkitem1:
self.checkitem1_checkbox = QCheckBox(self.checkitem1)
self.checkitem1_checkbox.stateChanged.connect(self._on_select_checkitem1_checkbox_stateChanged)
self.verticalLayout.addWidget(self.checkitem1_checkbox)
if self.checkitem2:
self.checkitem2_checkbox = QCheckBox(self.checkitem2)
self.checkitem2_checkbox.stateChanged.connect(self._on_select_checkitem2_checkbox_stateChanged)
self.verticalLayout.addWidget(self.checkitem2_checkbox)
if not items:
spacerItem = QSpacerItem(1, 1, QSizePolicy.Expanding, QSizePolicy.Expanding)
self.verticalLayout.addItem(spacerItem)
# create buttons
self.buttonBox = QDialogButtonBox(self)
self.buttonBox.setObjectName("buttonBox")
self.buttonBox.setOrientation(Qt.Horizontal)
self.buttonBox.setStandardButtons(buttons)
self.buttonBox.accepted.connect(self.accept)
self.buttonBox.rejected.connect(self.reject)
self.verticalLayout.addWidget(self.buttonBox)
# set the input fields
if items:
self.content.createFieldsFromValues(items, exclusive)
if (select_if_single and len(items) == 1) or preselect_all:
self.select_all_checkbox.setCheckState(Qt.Checked)
if not items or len(items) < 7:
self.filter_frame.setVisible(False)
class SensorParamControlDialog(Plugin):
def __init__(self, context):
super(SensorParamControlDialog, self).__init__(context)
self.setObjectName('SensorParamControlDialog')
self._widget = QWidget()
vbox = QVBoxLayout()
### Multisense ###
ms_groupbox = QGroupBox( "Multisense" )
ms_vbox = QVBoxLayout()
ms_gain_hbox = QHBoxLayout()
self.ms_gain_label = QLabel("Image Gain [1, 1, 8]")
ms_gain_hbox.addWidget( self.ms_gain_label )
self.ms_gain = QDoubleSpinBox()
self.ms_gain.setSingleStep(.01)
self.ms_gain.setRange(1,8)
ms_gain_hbox.addWidget( self.ms_gain )
ms_vbox.addLayout( ms_gain_hbox )
ms_exp_hbox = QHBoxLayout()
self.ms_exp_auto = QCheckBox("Image Exposure [.03, 0.5]")
ms_exp_hbox.addWidget( self.ms_exp_auto )
self.ms_exp = QDoubleSpinBox()
self.ms_exp.setSingleStep( .001 )
self.ms_exp.setRange( .025,.5 )
ms_exp_hbox.addWidget( self.ms_exp )
ms_vbox.addLayout( ms_exp_hbox )
ms_spindle_hbox = QHBoxLayout()
ms_spindle_label = QLabel("Spindle Speed [0, 5.2]")
ms_spindle_hbox.addWidget( ms_spindle_label )
self.ms_spindle = QDoubleSpinBox()
self.ms_spindle.setSingleStep(.01)
self.ms_spindle.setRange( 0,15.2 )
ms_spindle_hbox.addWidget( self.ms_spindle )
ms_vbox.addLayout( ms_spindle_hbox )
ms_light_hbox = QHBoxLayout()
ms_light_label = QLabel("Light Brightness")
ms_light_hbox.addWidget(ms_light_label)
self.ms_light = QSlider(Qt.Horizontal)
self.ms_light.setRange(0,100)
ms_light_hbox.addWidget( self.ms_light )
ms_vbox.addLayout( ms_light_hbox )
ms_button_hbox = QHBoxLayout()
ms_button_hbox.addStretch(1)
ms_button_get = QPushButton("Get Settings")
ms_button_get.pressed.connect(self.ms_get_callback)
#ms_button_hbox.addWidget( ms_button_get )
ms_button_set = QPushButton("Set Settings")
ms_button_set.pressed.connect(self.ms_set_callback)
ms_button_hbox.addWidget( ms_button_set )
ms_vbox.addLayout( ms_button_hbox )
ms_groupbox.setLayout( ms_vbox )
vbox.addWidget( ms_groupbox )
### Left SA ###
sa_left_groupbox = QGroupBox( "Left SA Camera" )
sa_left_vbox = QVBoxLayout()
sa_left_gain_hbox = QHBoxLayout()
sa_left_gain_label = QLabel("Image Gain [0, 0, 25]")
sa_left_gain_hbox.addWidget( sa_left_gain_label )
self.sa_left_gain = QDoubleSpinBox()
self.sa_left_gain.setSingleStep(.01)
self.sa_left_gain.setRange( 0, 25 )
sa_left_gain_hbox.addWidget( self.sa_left_gain )
sa_left_vbox.addLayout( sa_left_gain_hbox )
sa_left_exp_hbox = QHBoxLayout()
sa_left_exp_label = QLabel("Image Shutter [0, 5.5, 25]")
sa_left_exp_hbox.addWidget( sa_left_exp_label )
self.sa_left_exp = QDoubleSpinBox()
self.sa_left_exp.setSingleStep(.01)
self.sa_left_exp.setRange( 0, 25 )
sa_left_exp_hbox.addWidget( self.sa_left_exp )
sa_left_vbox.addLayout( sa_left_exp_hbox )
sa_left_button_hbox = QHBoxLayout()
sa_left_button_hbox.addStretch(1)
sa_left_button_get = QPushButton("Get Settings")
sa_left_button_get.pressed.connect(self.sa_left_get_callback)
#sa_left_button_hbox.addWidget( sa_left_button_get )
sa_left_button_set = QPushButton("Set Settings")
sa_left_button_set.pressed.connect(self.sa_left_set_callback)
sa_left_button_hbox.addWidget( sa_left_button_set )
sa_left_vbox.addLayout( sa_left_button_hbox )
sa_left_groupbox.setLayout( sa_left_vbox )
vbox.addWidget(sa_left_groupbox)
### Right SA ###
sa_right_groupbox = QGroupBox( "Right SA Camera" )
sa_right_vbox = QVBoxLayout()
sa_right_gain_hbox = QHBoxLayout()
sa_right_gain_label = QLabel("Image Gain [0, 0, 25]")
sa_right_gain_hbox.addWidget( sa_right_gain_label )
self.sa_right_gain = QDoubleSpinBox()
self.sa_right_gain.setSingleStep(.01)
self.sa_right_gain.setRange( 0, 25 )
sa_right_gain_hbox.addWidget( self.sa_right_gain )
sa_right_vbox.addLayout( sa_right_gain_hbox )
sa_right_exp_hbox = QHBoxLayout()
sa_right_exp_label = QLabel("Image Shutter [0, 5.5, 25]")
sa_right_exp_hbox.addWidget( sa_right_exp_label )
self.sa_right_exp = QDoubleSpinBox()
self.sa_right_exp.setSingleStep(.01)
self.sa_right_exp.setRange( 0, 25 )
sa_right_exp_hbox.addWidget( self.sa_right_exp )
sa_right_vbox.addLayout( sa_right_exp_hbox )
sa_right_button_hbox = QHBoxLayout()
sa_right_button_hbox.addStretch(1)
sa_right_button_get = QPushButton("Get Settings")
sa_right_button_get.pressed.connect(self.sa_right_get_callback)
#sa_right_button_hbox.addWidget( sa_right_button_get )
sa_right_button_set = QPushButton("Set Settings")
sa_right_button_set.pressed.connect(self.sa_right_set_callback)
sa_right_button_hbox.addWidget( sa_right_button_set )
sa_right_vbox.addLayout( sa_right_button_hbox )
sa_right_groupbox.setLayout( sa_right_vbox )
vbox.addWidget(sa_right_groupbox)
vbox.addStretch(1)
self._widget.setLayout(vbox)
context.add_widget(self._widget)
# publishers and subscribers
self.ms_cam_pub = rospy.Publisher( '/multisense_sl/set_cam_parameters', Float64MultiArray, queue_size=10)
self.ms_light_pub = rospy.Publisher( '/multisense_sl/set_light_brightness', Float64, queue_size=10)
self.ms_spindle_pub = rospy.Publisher( '/multisense_sl/set_spindle_speed', Float64, queue_size=10)
self.sa_left_cam_pub = rospy.Publisher( '/sa/left/set_cam_parameters', Float64MultiArray, queue_size=10)
self.sa_right_cam_pub = rospy.Publisher( '/sa/right/set_cam_parameters', Float64MultiArray, queue_size=10)
# self.param_sub = self.stateSubscriber = rospy.Subscriber('/ros_footstep_planner/params', FootstepPlannerParams, self.getParamCallbackFcn)
def shutdown_plugin(self):
print "Shutdown ..."
# self.param_pub.unregister()
print "Done!"
def ms_get_callback(self):
print "fill values"
def ms_set_callback(self):
ms_cam = Float64MultiArray()
ms_cam_dim_h = MultiArrayDimension()
ms_cam_dim_h.label = "height"
ms_cam_dim_h.size = 1
ms_cam_dim_h.stride = 2
ms_cam.layout.dim.append( ms_cam_dim_h )
ms_cam_dim_w = MultiArrayDimension()
ms_cam_dim_w.label = "width"
ms_cam_dim_w.size = 2
ms_cam_dim_w.stride = 2
ms_cam.layout.dim.append(ms_cam_dim_w)
ms_cam.data.append( self.ms_gain.value() )
if self.ms_exp_auto.isChecked() :
ms_cam.data.append( 1 )
else:
ms_cam.data.append( self.ms_exp.value() )
self.ms_cam_pub.publish( ms_cam )
light = Float64()
light.data = self.ms_light.value()/100.0
self.ms_light_pub.publish( light )
spindle_speed = Float64()
spindle_speed.data = self.ms_spindle.value()
self.ms_spindle_pub.publish( spindle_speed )
def sa_left_get_callback(self):
print "fill values"
def sa_left_set_callback(self):
sa_left_cam = Float64MultiArray()
sa_left_cam_dim_h = MultiArrayDimension()
sa_left_cam_dim_h.label = "height"
sa_left_cam_dim_h.size = 1
sa_left_cam_dim_h.stride = 2
sa_left_cam.layout.dim.append( sa_left_cam_dim_h )
sa_left_cam_dim_w = MultiArrayDimension()
sa_left_cam_dim_w.label = "width"
sa_left_cam_dim_w.size = 2
sa_left_cam_dim_w.stride = 2
sa_left_cam.layout.dim.append(sa_left_cam_dim_w)
sa_left_cam.data.append( self.sa_left_gain.value() )
sa_left_cam.data.append( self.sa_left_exp.value() )
self.sa_left_cam_pub.publish( sa_left_cam )
def sa_right_get_callback(self):
print "fill values"
def sa_right_set_callback(self):
sa_right_cam = Float64MultiArray()
sa_right_cam_dim_h = MultiArrayDimension()
sa_right_cam_dim_h.label = "height"
sa_right_cam_dim_h.size = 1
sa_right_cam_dim_h.stride = 2
sa_right_cam.layout.dim.append( sa_right_cam_dim_h )
sa_right_cam_dim_w = MultiArrayDimension()
sa_right_cam_dim_w.label = "width"
sa_right_cam_dim_w.size = 2
sa_right_cam_dim_w.stride = 2
sa_right_cam.layout.dim.append(sa_right_cam_dim_w)
sa_right_cam.data.append( self.sa_right_gain.value() )
sa_right_cam.data.append( self.sa_right_exp.value() )
self.sa_right_cam_pub.publish( sa_right_cam )
class Top(Plugin):
NODE_FIELDS = [
'pid', 'get_cpu_percent', 'get_memory_percent', 'get_num_threads'
]
OUT_FIELDS = [
'node_name', 'pid', 'cpu_percent', 'memory_percent', 'num_threads'
]
FORMAT_STRS = ['%s', '%s', '%0.2f', '%0.2f', '%s']
NODE_LABELS = ['Node', 'PID', 'CPU %', 'Mem %', 'Num Threads']
SORT_TYPE = [str, str, float, float, float]
TOOLTIPS = {
0: ('cmdline', lambda x: '\n'.join(textwrap.wrap(' '.join(x)))),
3: ('memory_info', lambda x:
('Resident: %0.2f MiB, Virtual: %0.2f MiB' %
(x[0] / 2**20, x[1] / 2**20)))
}
_node_info = NodeInfo()
name_filter = re.compile('')
def __init__(self, context):
super(Top, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('Top')
# 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
self._selected_node = ''
self._selected_node_lock = RLock()
# Setup the toolbar
self._toolbar = QToolBar()
self._filter_box = QLineEdit()
self._regex_box = QCheckBox()
self._regex_box.setText('regex')
self._toolbar.addWidget(QLabel('Filter'))
self._toolbar.addWidget(self._filter_box)
self._toolbar.addWidget(self._regex_box)
self._filter_box.returnPressed.connect(self.update_filter)
self._regex_box.stateChanged.connect(self.update_filter)
# Create a container widget and give it a layout
self._container = QWidget()
self._container.setWindowTitle('Process Monitor')
self._layout = QVBoxLayout()
self._container.setLayout(self._layout)
self._layout.addWidget(self._toolbar)
# Create the table widget
self._table_widget = QTreeWidget()
self._table_widget.setObjectName('TopTable')
self._table_widget.setColumnCount(len(self.NODE_LABELS))
self._table_widget.setHeaderLabels(self.NODE_LABELS)
self._table_widget.itemClicked.connect(self._tableItemClicked)
self._table_widget.setSortingEnabled(True)
self._table_widget.setAlternatingRowColors(True)
self._layout.addWidget(self._table_widget)
context.add_widget(self._container)
# Add a button for killing nodes
self._kill_button = QPushButton('Kill Node')
self._layout.addWidget(self._kill_button)
self._kill_button.clicked.connect(self._kill_node)
# Update twice since the first cpu% lookup will always return 0
self.update_table()
self.update_table()
self._table_widget.resizeColumnToContents(0)
# Start a timer to trigger updates
self._update_timer = QTimer()
self._update_timer.setInterval(1000)
self._update_timer.timeout.connect(self.update_table)
self._update_timer.start()
def _tableItemClicked(self, item, column):
with self._selected_node_lock:
self._selected_node = item.text(0)
def update_filter(self, *args):
if self._regex_box.isChecked():
expr = self._filter_box.text()
else:
expr = re.escape(self._filter_box.text())
self.name_filter = re.compile(expr)
self.update_table()
def _kill_node(self):
self._node_info.kill_node(self._selected_node)
def update_one_item(self, row, info):
twi = TopWidgetItem()
for col, field in enumerate(self.OUT_FIELDS):
val = info[field]
twi.setText(col, self.FORMAT_STRS[col] % val)
self._table_widget.insertTopLevelItem(row, twi)
for col, (key, func) in self.TOOLTIPS.iteritems():
twi.setToolTip(col, func(info[key]))
with self._selected_node_lock:
if twi.text(0) == self._selected_node:
twi.setSelected(True)
self._table_widget.setItemHidden(
twi,
len(self.name_filter.findall(info['node_name'])) == 0)
def update_table(self):
self._table_widget.clear()
infos = self._node_info.get_all_node_fields(self.NODE_FIELDS)
for nx, info in enumerate(infos):
self.update_one_item(nx, info)
def shutdown_plugin(self):
self._update_timer.stop()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('filter_text', self._filter_box.text())
instance_settings.set_value('is_regex',
int(self._regex_box.checkState()))
def restore_settings(self, plugin_settings, instance_settings):
self._filter_box.setText(instance_settings.value('filter_text'))
is_regex_int = instance_settings.value('is_regex')
if is_regex_int:
self._regex_box.setCheckState(Qt.CheckState(is_regex_int))
else:
self._regex_box.setCheckState(Qt.CheckState(0))
self.update_filter()
class PatternGeneratorWidget(QObject):
enable_pattern_generator = False
def __init__(self, context):
super(PatternGeneratorWidget, self).__init__()
# publisher
self.pattern_generator_params_pub = rospy.Publisher('pattern_generator/set_params', PatternGeneratorParameters, queue_size = 1)
# start widget
widget = context
# start upper part
hbox = QHBoxLayout()
# start left column
left_vbox = QVBoxLayout()
# start button
start_command = QPushButton("Start")
start_command.clicked.connect(self.start_command_callback)
left_vbox.addWidget(start_command)
# simulation checkbox
self.simulation_mode_checkbox = QCheckBox()
self.simulation_mode_checkbox.setText("Simulation Mode")
self.simulation_mode_checkbox.setChecked(False)
left_vbox.addWidget(self.simulation_mode_checkbox)
# realtime checkbox
self.realtime_mode_checkbox = QCheckBox()
self.realtime_mode_checkbox.setText("Realtime Mode")
self.realtime_mode_checkbox.setChecked(False)
left_vbox.addWidget(self.realtime_mode_checkbox)
# joystick checkbox
self.joystick_mode_checkbox = QCheckBox()
self.joystick_mode_checkbox.setText("Joystick Mode")
self.joystick_mode_checkbox.setChecked(False)
self.joystick_mode_checkbox.clicked.connect(self.joystick_mode_check_callback)
left_vbox.addWidget(self.joystick_mode_checkbox)
# foot seperation
self.foot_seperation = generate_q_double_spin_box(0.2, 0.15, 0.3, 2, 0.01)
self.foot_seperation.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.foot_seperation, "Foot Seperation (m):")
# delta x
self.delta_x = generate_q_double_spin_box(0.0, -0.4, 0.4, 2, 0.01)
self.delta_x.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.delta_x, "dX (m):")
# delta y
self.delta_y = generate_q_double_spin_box(0.0, -2.2, 2.2, 2, 0.01)
self.delta_y.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.delta_y, "dY (m):")
# delta yaw
self.delta_yaw = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
self.delta_yaw.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.delta_yaw, "dYaw (deg):")
# roll
self.roll = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
self.roll.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.roll, "Roll (deg):")
# pitch
self.pitch = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
self.pitch.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.pitch, "Pitch (deg):")
# end left column
left_vbox.addStretch()
hbox.addLayout(left_vbox, 1)
# start right column
right_vbox = QVBoxLayout()
# stop button
stop_command = QPushButton("Stop")
stop_command.clicked.connect(self.stop_command_callback)
right_vbox.addWidget(stop_command)
# ignore collision
self.collision_checkbox = QCheckBox()
self.collision_checkbox.setText("Ignore Collision")
self.collision_checkbox.setChecked(True)
right_vbox.addWidget(self.collision_checkbox)
# override 3D
self.override_checkbox = QCheckBox()
self.override_checkbox.setText("Override 3D")
self.override_checkbox.setChecked(False)
right_vbox.addWidget(self.override_checkbox)
# end right coloumn
right_vbox.addStretch()
hbox.addLayout(right_vbox, 1)
# add upper part
hbox.setMargin(0)
vbox = QVBoxLayout()
vbox.addLayout(hbox)
# parameter set selection
self.parameter_set_widget = QParameterSetWidget()
add_widget_with_frame(vbox, self.parameter_set_widget, "Parameter Set:")
# end widget
widget.setLayout(vbox)
#context.add_widget(widget)
def shutdown_plugin(self):
print "Shutting down ..."
self.pattern_generator_params_pub.unregister()
print "Done!"
# message publisher
def _publish_parameters(self):
params = PatternGeneratorParameters()
params.enable = self.enable_pattern_generator
params.simulation_mode = self.simulation_mode_checkbox.isChecked()
params.joystick_mode = self.joystick_mode_checkbox.isChecked()
params.d_step.position.x = self.delta_x.value()
params.d_step.position.y = self.delta_y.value()
params.d_step.position.z = 0
q = tf.transformations.quaternion_from_euler(math.radians(self.roll.value()), math.radians(self.pitch.value()), math.radians(self.delta_yaw.value()))
params.d_step.orientation.x = q[0]
params.d_step.orientation.y = q[1]
params.d_step.orientation.z = q[2]
params.d_step.orientation.w = q[3]
params.foot_seperation = self.foot_seperation.value()
params.parameter_set_name.data = self.parameter_set_widget.current_parameter_set_name()
print "Send stepping command = \n",params
self.pattern_generator_params_pub.publish(params)
# Define system command strings
def start_command_callback(self):
self.enable_pattern_generator = True
self._publish_parameters()
def stop_command_callback(self):
self.enable_pattern_generator = False
self._publish_parameters()
def callback_spin_box(self, value_as_int):
if (self.realtime_mode_checkbox.isChecked()):
self._publish_parameters()
def joystick_mode_check_callback(self):
self.enable_pattern_generator = False
self._publish_parameters()
def __init__(self, context):
super(Top, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('Top')
# 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
self._selected_node = ''
self._selected_node_lock = RLock()
# Setup the toolbar
self._toolbar = QToolBar()
self._filter_box = QLineEdit()
self._regex_box = QCheckBox()
self._regex_box.setText('regex')
self._toolbar.addWidget(QLabel('Filter'))
self._toolbar.addWidget(self._filter_box)
self._toolbar.addWidget(self._regex_box)
self._filter_box.returnPressed.connect(self.update_filter)
self._regex_box.stateChanged.connect(self.update_filter)
# Create a container widget and give it a layout
self._container = QWidget()
self._container.setWindowTitle('Process Monitor')
self._layout = QVBoxLayout()
self._container.setLayout(self._layout)
self._layout.addWidget(self._toolbar)
# Create the table widget
self._table_widget = QTreeWidget()
self._table_widget.setObjectName('TopTable')
self._table_widget.setColumnCount(len(self.NODE_LABELS))
self._table_widget.setHeaderLabels(self.NODE_LABELS)
self._table_widget.itemClicked.connect(self._tableItemClicked)
self._table_widget.setSortingEnabled(True)
self._table_widget.setAlternatingRowColors(True)
self._layout.addWidget(self._table_widget)
context.add_widget(self._container)
# Add a button for killing nodes
self._kill_button = QPushButton('Kill Node')
self._layout.addWidget(self._kill_button)
self._kill_button.clicked.connect(self._kill_node)
# Update twice since the first cpu% lookup will always return 0
self.update_table()
self.update_table()
self._table_widget.resizeColumnToContents(0)
# Start a timer to trigger updates
self._update_timer = QTimer()
self._update_timer.setInterval(1000)
self._update_timer.timeout.connect(self.update_table)
self._update_timer.start()
class PatternGeneratorWidget(QObject):
enable_pattern_generator = False
def __init__(self, context):
super(PatternGeneratorWidget, self).__init__()
# publisher
self.pattern_generator_params_pub = rospy.Publisher(
'pattern_generator/set_params',
PatternGeneratorParameters,
queue_size=1)
# start widget
widget = context
# start upper part
hbox = QHBoxLayout()
# start left column
left_vbox = QVBoxLayout()
# start button
start_command = QPushButton("Start")
left_vbox.addWidget(start_command)
# simulation checkbox
self.simulation_mode_checkbox = QCheckBox()
self.simulation_mode_checkbox.setText("Simulation Mode")
self.simulation_mode_checkbox.setChecked(False)
left_vbox.addWidget(self.simulation_mode_checkbox)
# realtime checkbox
self.realtime_mode_checkbox = QCheckBox()
self.realtime_mode_checkbox.setText("Realtime Mode")
self.realtime_mode_checkbox.setChecked(False)
left_vbox.addWidget(self.realtime_mode_checkbox)
# joystick checkbox
self.joystick_mode_checkbox = QCheckBox()
self.joystick_mode_checkbox.setText("Joystick Mode")
self.joystick_mode_checkbox.setChecked(False)
left_vbox.addWidget(self.joystick_mode_checkbox)
# ignore invalid steps checkbox
self.ignore_invalid_steps_checkbox = QCheckBox()
self.ignore_invalid_steps_checkbox.setText("Ignore Invalid Steps")
self.ignore_invalid_steps_checkbox.setChecked(False)
left_vbox.addWidget(self.ignore_invalid_steps_checkbox)
# foot seperation
self.foot_seperation = generate_q_double_spin_box(
0.2, 0.15, 0.3, 2, 0.01)
self.foot_seperation.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.foot_seperation,
"Foot Seperation (m):")
# delta x
self.delta_x = generate_q_double_spin_box(0.0, -0.4, 0.4, 2, 0.01)
self.delta_x.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.delta_x, "dX (m):")
# delta y
self.delta_y = generate_q_double_spin_box(0.0, -2.2, 2.2, 2, 0.01)
self.delta_y.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.delta_y, "dY (m):")
# delta yaw
self.delta_yaw = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
self.delta_yaw.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.delta_yaw, "dYaw (deg):")
# roll
self.roll = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
self.roll.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.roll, "Roll (deg):")
# pitch
self.pitch = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
self.pitch.valueChanged.connect(self.callback_spin_box)
add_widget_with_frame(left_vbox, self.pitch, "Pitch (deg):")
# end left column
left_vbox.addStretch()
hbox.addLayout(left_vbox, 1)
# start right column
right_vbox = QVBoxLayout()
# stop button
stop_command = QPushButton("Stop")
right_vbox.addWidget(stop_command)
# ignore collision
self.collision_checkbox = QCheckBox()
self.collision_checkbox.setText("Ignore Collision")
self.collision_checkbox.setChecked(True)
right_vbox.addWidget(self.collision_checkbox)
# override 3D
self.override_checkbox = QCheckBox()
self.override_checkbox.setText("Override 3D")
self.override_checkbox.setChecked(False)
right_vbox.addWidget(self.override_checkbox)
# end right coloumn
right_vbox.addStretch()
hbox.addLayout(right_vbox, 1)
# add upper part
hbox.setMargin(0)
vbox = QVBoxLayout()
vbox.addLayout(hbox)
# parameter set selection
self.parameter_set_widget = QParameterSetWidget()
add_widget_with_frame(vbox, self.parameter_set_widget,
"Parameter Set:")
# end widget
widget.setLayout(vbox)
#context.add_widget(widget)
# signal connections
start_command.clicked.connect(self.start_command_callback)
stop_command.clicked.connect(self.stop_command_callback)
self.joystick_mode_checkbox.clicked.connect(
self.joystick_mode_check_callback)
self.ignore_invalid_steps_checkbox.clicked.connect(
self._publish_parameters)
def shutdown_plugin(self):
print "Shutting down ..."
self.pattern_generator_params_pub.unregister()
print "Done!"
# message publisher
def _publish_parameters(self):
params = PatternGeneratorParameters()
params.enable = self.enable_pattern_generator
params.simulation_mode = self.simulation_mode_checkbox.isChecked()
params.joystick_mode = self.joystick_mode_checkbox.isChecked()
params.ignore_invalid_steps = self.ignore_invalid_steps_checkbox.isChecked(
)
params.d_step.position.x = self.delta_x.value()
params.d_step.position.y = self.delta_y.value()
params.d_step.position.z = 0
q = tf.transformations.quaternion_from_euler(
math.radians(self.roll.value()), math.radians(self.pitch.value()),
math.radians(self.delta_yaw.value()))
params.d_step.orientation.x = q[0]
params.d_step.orientation.y = q[1]
params.d_step.orientation.z = q[2]
params.d_step.orientation.w = q[3]
params.foot_seperation = self.foot_seperation.value()
params.parameter_set_name.data = self.parameter_set_widget.current_parameter_set_name(
)
print "Send stepping command = \n", params
self.pattern_generator_params_pub.publish(params)
# Define system command strings
def start_command_callback(self):
self.enable_pattern_generator = True
self._publish_parameters()
def stop_command_callback(self):
self.enable_pattern_generator = False
self._publish_parameters()
def callback_spin_box(self, value_as_int):
if self.realtime_mode_checkbox.isChecked():
self._publish_parameters()
def joystick_mode_check_callback(self):
self.enable_pattern_generator = False
self._publish_parameters()
class BDIPelvisPoseWidget(QObject):
updateStateSignal = Signal(object)
def __init__(self, context):
# super(BDIPelvisPoseWidget, self).__init__(context)
# self.setObjectName('BDIPelvisPoseWidget')
super(BDIPelvisPoseWidget, self).__init__()
self.name = "BDIPelvisPoseWidget"
self.updateStateSignal.connect(self.on_updateState)
# self._widget = QWidget()
self._widget = context
vbox = QVBoxLayout()
self.forward_position = 0.0
self.lateral_position = 0.0
self.height_position = 0.91
self.roll_position = 0.0
self.pitch_position = 0.0
self.yaw_position = 0.0
self.currentForward = 0.0
self.currentLateral = 0.0
self.currentHeight = 0.91
self.currentRoll = 0.0
self.currentPitch = 0.0
self.currentYaw = 0.0
# Define checkboxes
vbox = QVBoxLayout()
label = QLabel()
label.setText("BDI Pelvis Height (Manipulate Mode Only)") # todo - connect controller mode
vbox.addWidget(label)
self.enable_checkbox = QCheckBox("Enable")
self.enable_checkbox.stateChanged.connect(self.on_enable_check)
vbox.addWidget(self.enable_checkbox)
self.snap_to_current_button = QPushButton("Snap to Current")
self.snap_to_current_button.pressed.connect(self.on_snapCurrentPressed)
vbox.addWidget(self.snap_to_current_button)
self.roll_slider = QSlider(Qt.Horizontal)
self.roll_label = QLabel()
self.roll_label.setText("Roll")
vbox.addWidget(self.roll_label)
self.roll_slider.setRange(int(-100), int(101))
self.roll_slider.setValue(int(0))
self.roll_slider.setSingleStep((200) / 50)
self.roll_slider.setTickInterval(25)
self.roll_slider.valueChanged.connect(self.on_rollSliderMoved)
vbox.addWidget(self.roll_slider)
self.roll_progress_bar = QProgressBar()
self.roll_progress_bar.setRange(int(-100), int(101))
self.roll_progress_bar.setValue((6.0 / math.pi) * self.currentRoll * 100)
self.roll_progress_bar.setFormat("%.6f" % self.currentRoll)
vbox.addWidget(self.roll_progress_bar)
self.pitch_slider = QSlider(Qt.Horizontal)
self.pitch_label = QLabel()
self.pitch_label.setText("Pitch")
vbox.addWidget(self.pitch_label)
self.pitch_slider.setRange(int(-100), int(101))
self.pitch_slider.setValue(int(0))
self.pitch_slider.setSingleStep((200) / 50)
self.pitch_slider.setTickInterval(25)
self.pitch_slider.valueChanged.connect(self.on_pitchSliderMoved)
vbox.addWidget(self.pitch_slider)
self.pitch_progress_bar = QProgressBar()
self.pitch_progress_bar.setRange(int(-100), int(101))
self.pitch_progress_bar.setValue((6.0 / math.pi) * self.currentPitch * 100)
self.pitch_progress_bar.setFormat("%.6f" % self.currentPitch)
vbox.addWidget(self.pitch_progress_bar)
self.yaw_slider = QSlider(Qt.Horizontal)
self.yaw_label = QLabel()
self.yaw_label.setText("Yaw")
vbox.addWidget(self.yaw_label)
self.yaw_slider.setRange(int(-100), int(101))
self.yaw_slider.setValue(int(0))
self.yaw_slider.setSingleStep((200) / 50)
self.yaw_slider.setTickInterval(25)
self.yaw_slider.valueChanged.connect(self.on_yawSliderMoved)
vbox.addWidget(self.yaw_slider)
self.yaw_progress_bar = QProgressBar()
self.yaw_progress_bar.setRange(int(-100), int(101))
self.yaw_progress_bar.setValue((4.0 / math.pi) * self.currentYaw * 100)
self.yaw_progress_bar.setFormat("%.6f" % self.currentYaw)
vbox.addWidget(self.yaw_progress_bar)
self.forward_label = QLabel()
self.forward_label.setText("Forward")
vbox.addWidget(self.forward_label)
widget = QWidget()
hbox = QHBoxLayout()
hbox.addStretch()
self.forward_slider = QSlider(Qt.Vertical)
# self.forward_slider.setText("Height")
self.forward_slider.setRange(int(-101), int(100))
self.forward_slider.setValue(int(0))
self.forward_slider.setSingleStep(1)
self.forward_slider.setTickInterval(10)
self.forward_slider.valueChanged.connect(self.on_forwardSliderMoved)
hbox.addWidget(self.forward_slider)
self.forward_progress_bar = QProgressBar()
self.forward_progress_bar.setOrientation(Qt.Vertical)
self.forward_progress_bar.setRange(int(-100), int(101))
self.forward_progress_bar.setValue((self.currentForward / 0.075) * 100)
self.forward_progress_bar.setTextVisible(False)
hbox.addWidget(self.forward_progress_bar)
self.forward_progress_bar_label = QLabel()
self.forward_progress_bar_label.setText("%.6f" % self.currentForward)
hbox.addWidget(self.forward_progress_bar_label)
hbox.addStretch()
widget.setLayout(hbox)
vbox.addWidget(widget)
self.lateral_label = QLabel()
self.lateral_label.setText("Lateral")
vbox.addWidget(self.lateral_label)
self.lateral_slider = QSlider(Qt.Horizontal)
# self.lateral_slider.setText("Lateral")
self.lateral_slider.setRange(int(-100), int(101))
self.lateral_slider.setValue(int(0))
self.lateral_slider.setSingleStep((200) / 50)
self.lateral_slider.setTickInterval(25)
self.lateral_slider.valueChanged.connect(self.on_lateralSliderMoved)
vbox.addWidget(self.lateral_slider)
self.lateral_progress_bar = QProgressBar()
self.lateral_progress_bar.setRange(int(-100), int(101))
self.lateral_progress_bar.setValue((self.currentLateral / 0.15) * 100)
self.lateral_progress_bar.setFormat("%.6f" % self.currentLateral)
vbox.addWidget(self.lateral_progress_bar)
self.height_label = QLabel()
self.height_label.setText("Height")
vbox.addWidget(self.height_label)
widget = QWidget()
hbox = QHBoxLayout()
hbox.addStretch()
self.height_slider = QSlider(Qt.Vertical)
# self.height_slider.setText("Height")
self.height_slider.setRange(int(55), int(105))
self.height_slider.setValue(int(91))
self.height_slider.setSingleStep(2)
self.height_slider.setTickInterval(10)
self.height_slider.valueChanged.connect(self.on_heightSliderMoved)
hbox.addWidget(self.height_slider)
self.height_progress_bar = QProgressBar()
self.height_progress_bar.setOrientation(Qt.Vertical)
self.height_progress_bar.setRange(int(55), int(105))
self.height_progress_bar.setValue(self.currentHeight * 100)
self.height_progress_bar.setTextVisible(False)
hbox.addWidget(self.height_progress_bar)
self.height_progress_bar_label = QLabel()
self.height_progress_bar_label.setText("%.6f" % self.currentHeight)
hbox.addWidget(self.height_progress_bar_label)
hbox.addStretch()
widget.setLayout(hbox)
vbox.addWidget(widget)
vbox.addStretch()
self._widget.setLayout(vbox)
# context.add_widget(self._widget)
self.height_position = 0.91
self.lateral_position = 0.0
self.yaw_position = 0.0
self.first_time = True
self.enable_checkbox.setChecked(False)
self.yaw_slider.setEnabled(False)
self.roll_slider.setEnabled(False)
self.pitch_slider.setEnabled(False)
self.forward_slider.setEnabled(False)
self.lateral_slider.setEnabled(False)
self.height_slider.setEnabled(False)
self.pub_robot = rospy.Publisher("/flor/controller/bdi_desired_pelvis_pose", PoseStamped, queue_size=10)
# self.stateSubscriber = rospy.Subscriber('/flor/pelvis_controller/current_states',JointState, self.stateCallbackFnc)
# self.pub_bdi_pelvis = rospy.Publisher('/bdi_manipulate_pelvis_pose_rpy',PoseStamped)
self.pelvis_trajectory_pub = rospy.Publisher(
"/robot_controllers/pelvis_traj_controller/command", JointTrajectory, queue_size=10
)
self.stateSubscriber = rospy.Subscriber(
"/robot_controllers/pelvis_traj_controller/state", JointTrajectoryControllerState, self.stateCallbackFnc
)
def stateCallbackFnc(self, jointState_msg):
self.updateStateSignal.emit(jointState_msg)
def shutdown_plugin(self):
# Just make sure to stop timers and publishers, unsubscribe from Topics etc in the shutdown_plugin method.
print "Shutdown BDI pelvis height "
self.pub_robot.unregister()
self.stateSubscriber.unregister()
def publishRobotPelvisPose(self):
print "publishing new pelvis pose ..."
bdi_pelvis_pose = PoseStamped()
bdi_pelvis_pose.header.stamp = rospy.Time.now()
bdi_pelvis_pose.pose.position.x = self.forward_position
bdi_pelvis_pose.pose.position.y = self.lateral_position
bdi_pelvis_pose.pose.position.z = self.height_position
# Use BDI yaw*roll*pitch concatenation
xaxis, yaxis, zaxis = (1, 0, 0), (0, 1, 0), (0, 0, 1)
Rx = tf.transformations.rotation_matrix(self.roll_position, xaxis)
Ry = tf.transformations.rotation_matrix(self.pitch_position, yaxis)
Rz = tf.transformations.rotation_matrix(self.yaw_position, zaxis)
R = tf.transformations.concatenate_matrices(Rz, Rx, Ry)
q = tf.transformations.quaternion_from_matrix(R)
bdi_pelvis_pose.pose.orientation.w = q[3]
bdi_pelvis_pose.pose.orientation.x = q[0]
bdi_pelvis_pose.pose.orientation.y = q[1]
bdi_pelvis_pose.pose.orientation.z = q[2]
# w = math.cos(self.yaw_position*0.5)
# bdi_pelvis_pose.pose.orientation.x = 0.0
# bdi_pelvis_pose.pose.orientation.y = 0.0
# bdi_pelvis_pose.pose.orientation.z = math.sin(self.yaw_position*0.5)
print bdi_pelvis_pose
print q
euler = tf.transformations.euler_from_quaternion(q)
print euler
self.pub_robot.publish(bdi_pelvis_pose)
# Now publish the trajectory form for the new controllers
trajectory = JointTrajectory()
trajectory.header.stamp = rospy.Time.now()
trajectory.joint_names = ["com_v1", "com_v0", "pelvis_height", "pelvis_roll", "pelvis_pitch", "pelvis_yaw"]
trajectory.points = [JointTrajectoryPoint()]
trajectory.points[0].positions = [
self.lateral_position,
self.forward_position,
self.height_position,
self.roll_position,
self.pitch_position,
self.yaw_position,
]
trajectory.points[0].velocities = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
trajectory.points[0].time_from_start = rospy.Duration(0.75)
self.pelvis_trajectory_pub.publish(trajectory)
###################################
def on_snapCurrentPressed(self):
print "Snap ", self.name, " values to current pelvis positions"
self.blockSignals(True)
self.resetCurrentPelvisSliders()
self.blockSignals(False)
def blockSignals(self, block):
self.yaw_slider.blockSignals(block)
self.roll_slider.blockSignals(block)
self.pitch_slider.blockSignals(block)
self.forward_slider.blockSignals(block)
self.lateral_slider.blockSignals(block)
self.height_slider.blockSignals(block)
def resetCurrentPelvisSliders(self):
self.yaw_slider.setValue((4.0 / math.pi) * self.currentYaw * 100)
self.roll_slider.setValue((6.0 / math.pi) * self.currentRoll * 100)
self.pitch_slider.setValue((6.0 / math.pi) * self.currentPitch * 100)
self.forward_slider.setValue((self.currentForward / 0.075) * 100)
self.lateral_slider.setValue((self.currentLateral / 0.15) * 100)
self.height_slider.setValue(self.currentHeight * 100)
self.yaw_label.setText("Yaw: " + str(self.currentYaw))
self.roll_label.setText("Roll: " + str(self.currentRoll))
self.pitch_label.setText("Pitch: " + str(self.currentPitch))
self.forward_label.setText("Forward: " + str(self.currentForward))
self.lateral_label.setText("Lateral: " + str(self.currentLateral))
self.height_label.setText("Height: " + str(self.currentHeight))
self.forward_position = self.currentForward
self.lateral_position = self.currentLateral
self.height_position = self.currentHeight
self.roll_position = self.currentRoll
self.pitch_position = self.currentPitch
self.yaw_position = self.currentYaw
def on_updateState(self, jointState_msg):
self.currentLateral = jointState_msg.actual.positions[0]
self.currentForward = jointState_msg.actual.positions[1]
self.currentHeight = jointState_msg.actual.positions[2]
self.currentRoll = jointState_msg.actual.positions[3]
self.currentPitch = jointState_msg.actual.positions[4]
self.currentYaw = jointState_msg.actual.positions[5]
self.yaw_progress_bar.setValue((4.0 / math.pi) * self.currentYaw * 100)
self.yaw_progress_bar.setFormat("%.6f" % self.currentYaw)
self.roll_progress_bar.setValue((6.0 / math.pi) * self.currentRoll * 100)
self.roll_progress_bar.setFormat("%.6f" % self.currentRoll)
self.pitch_progress_bar.setValue((6.0 / math.pi) * self.currentPitch * 100)
self.pitch_progress_bar.setFormat("%.6f" % self.currentPitch)
self.forward_progress_bar.setValue((self.currentForward / 0.075) * 100)
self.forward_progress_bar_label.setText("%.6f" % self.currentForward)
self.lateral_progress_bar.setValue((self.currentLateral / 0.15) * 100)
self.lateral_progress_bar.setFormat("%.6f" % self.currentYaw)
self.height_progress_bar.setValue(self.currentHeight * 100)
self.height_progress_bar_label.setText("%.6f" % self.currentHeight)
####################################
def on_enable_check(self, value):
print "Toggle the enabling checkbox - current state is ", value
self.yaw_slider.setEnabled(self.enable_checkbox.isChecked())
self.roll_slider.setEnabled(self.enable_checkbox.isChecked())
self.pitch_slider.setEnabled(self.enable_checkbox.isChecked())
self.forward_slider.setEnabled(self.enable_checkbox.isChecked())
self.lateral_slider.setEnabled(self.enable_checkbox.isChecked())
self.height_slider.setEnabled(self.enable_checkbox.isChecked())
def on_yawSliderMoved(self, value):
self.yaw_position = (value / 100.0) * math.pi / 4.0
# print "New yaw=",self.yaw_position
self.yaw_label.setText("Yaw: " + str(self.yaw_position))
if self.enable_checkbox.isChecked():
self.publishRobotPelvisPose()
def on_rollSliderMoved(self, value):
self.roll_position = (value / 100.0) * math.pi / 6.0
self.roll_label.setText("Roll: " + str(self.roll_position))
if self.enable_checkbox.isChecked():
self.publishRobotPelvisPose()
def on_pitchSliderMoved(self, value):
self.pitch_position = (value / 100.0) * math.pi / 6.0
self.pitch_label.setText("Pitch: " + str(self.pitch_position))
if self.enable_checkbox.isChecked():
self.publishRobotPelvisPose()
def on_forwardSliderMoved(self, value):
self.forward_position = (value / 100.0) * 0.075
# print "New forward=",self.forward_position
self.forward_label.setText("Forward: " + str(self.forward_position))
if self.enable_checkbox.isChecked():
self.publishRobotPelvisPose()
def on_lateralSliderMoved(self, value):
self.lateral_position = (value / 100.0) * 0.15
# print "New lateral=",self.lateral_position
self.lateral_label.setText("Lateral: " + str(self.lateral_position))
if self.enable_checkbox.isChecked():
self.publishRobotPelvisPose()
def on_heightSliderMoved(self, value):
self.height_position = value / 100.0
# print "New height=",self.height_position
self.height_label.setText("Height: " + str(self.height_position))
if self.enable_checkbox.isChecked():
self.publishRobotPelvisPose()
def __init__(self, context):
#super(FootstepParamControlDialog, self).__init__(context)
#self.setObjectName('FootstepParamControlDialog')
super(FootstepParamControlWidget, self).__init__()
self.name = 'FootstepParamControlWidget'
#self._widget = QWidget()
self._widget = context
vbox = QVBoxLayout()
### STEP_COST_ESTIMATOR ########
self.step_cost_vbox = QVBoxLayout()
self.step_cost_groupbox = QGroupBox( "Step Cost Estimator" )
self.step_cost_groupbox.setCheckable( True )
self.step_cost_groupbox.setChecked(False)
self.step_cost_combobox = QComboBox()
self.step_cost_combobox.addItem( "Euclidean" )
self.step_cost_combobox.addItem( "GPR" )
self.step_cost_combobox.addItem( "Map" )
self.step_cost_combobox.addItem( "Boundary" )
self.step_cost_combobox.addItem( "Dynamics" )
self.step_cost_vbox.addWidget( self.step_cost_combobox )
self.step_cost_groupbox.setLayout( self.step_cost_vbox )
vbox.addWidget( self.step_cost_groupbox )
# #HARD ### FOOTSTEP_SET ########
# # parameters for discret footstep planning mode
# vigir_atlas_control_msgs/VigirBehaviorStepData[] footstep_set # set of footsteps (displacement vectors (in meter / rad))
# float32[] footstep_cost # cost for each footstep given in footstep set
#
# #HARD ### LOAD_GPR_STEP_COST ########
# # map step cost file
# std_msgs/String map_step_cost_file
#
# #HARD ### LOAD_MAP_STEP_COST ########
# # destination of gpr file
# std_msgs/String gpr_step_cost_file
### COLLISION_CHECK_TYPE ########
self.collision_check_groupbox = QGroupBox( "Collision Check Type" )
self.collision_check_groupbox.setCheckable( True )
self.collision_check_groupbox.setChecked( False)
self.collision_check_vbox = QVBoxLayout()
self.collision_check_feet_checkbox = QCheckBox( "Feet" )
self.collision_check_vbox.addWidget( self.collision_check_feet_checkbox )
self.collision_check_ub_checkbox = QCheckBox( "Upper Body" )
self.collision_check_vbox.addWidget( self.collision_check_ub_checkbox )
self.collision_check_fc_checkbox = QCheckBox( "Foot Contact Support" )
self.collision_check_vbox.addWidget( self.collision_check_fc_checkbox )
self.collision_check_groupbox.setLayout( self.collision_check_vbox )
vbox.addWidget( self.collision_check_groupbox )
### FOOT_SIZE ########
self.foot_size_vbox = QVBoxLayout()
self.foot_size_groupbox = QGroupBox( "Foot Size" )
self.foot_size_groupbox.setCheckable( True )
self.foot_size_groupbox.setChecked( False )
self.foot_size_hbox = QHBoxLayout()
self.foot_size_label = QLabel("Foot Size")
self.foot_size_hbox.addWidget( self.foot_size_label )
self.foot_size_x = QDoubleSpinBox()
self.foot_size_x.setSingleStep(.01)
self.foot_size_hbox.addWidget(self.foot_size_x)
self.foot_size_y = QDoubleSpinBox()
self.foot_size_y.setSingleStep(.01)
self.foot_size_hbox.addWidget(self.foot_size_y)
self.foot_size_z = QDoubleSpinBox()
self.foot_size_z.setSingleStep(.01)
self.foot_size_hbox.addWidget(self.foot_size_z)
self.foot_size_vbox.addLayout( self.foot_size_hbox )
self.foot_shift_hbox = QHBoxLayout()
self.foot_shift_label = QLabel("Foot Origin Shift")
self.foot_shift_hbox.addWidget( self.foot_shift_label )
self.foot_shift_x = QDoubleSpinBox()
self.foot_shift_x.setRange(-1.0, 1.0)
self.foot_shift_x.setSingleStep(.01)
self.foot_shift_hbox.addWidget(self.foot_shift_x)
self.foot_shift_y = QDoubleSpinBox()
self.foot_shift_y.setRange(-1.0, 1.0)
self.foot_shift_y.setSingleStep(.01)
self.foot_shift_hbox.addWidget(self.foot_shift_y)
self.foot_shift_z = QDoubleSpinBox()
self.foot_shift_z.setRange(-1.0, 1.0)
self.foot_shift_z.setSingleStep(.01)
self.foot_shift_hbox.addWidget(self.foot_shift_z)
self.foot_size_vbox.addLayout( self.foot_shift_hbox )
self.foot_size_groupbox.setLayout( self.foot_size_vbox )
vbox.addWidget( self.foot_size_groupbox )
### UPPER_BODY_SIZE ########
self.upper_vbox = QVBoxLayout()
self.upper_groupbox = QGroupBox( "Upper Body Size" )
self.upper_groupbox.setCheckable( True )
self.upper_groupbox.setChecked( False )
self.upper_size_hbox = QHBoxLayout()
self.upper_size_label = QLabel("Upper Body Size")
self.upper_size_hbox.addWidget( self.upper_size_label )
self.upper_size_x = QDoubleSpinBox()
self.upper_size_x.setSingleStep(.01)
self.upper_size_hbox.addWidget(self.upper_size_x)
self.upper_size_y = QDoubleSpinBox()
self.upper_size_y.setSingleStep(.01)
self.upper_size_hbox.addWidget(self.upper_size_y)
self.upper_size_z = QDoubleSpinBox()
self.upper_size_z.setSingleStep(.01)
self.upper_size_hbox.addWidget(self.upper_size_z)
self.upper_vbox.addLayout( self.upper_size_hbox )
self.upper_origin_hbox = QHBoxLayout()
self.upper_origin_label = QLabel("Upper Body Origin")
self.upper_origin_hbox.addWidget( self.upper_origin_label )
self.upper_origin_x = QDoubleSpinBox()
self.upper_origin_x.setRange(-1.0, 1.0)
self.upper_origin_x.setSingleStep(.01)
self.upper_origin_hbox.addWidget(self.upper_origin_x)
self.upper_origin_y = QDoubleSpinBox()
self.upper_origin_y.setRange(-1.0, 1.0)
self.upper_origin_y.setSingleStep(.01)
self.upper_origin_hbox.addWidget(self.upper_origin_y)
self.upper_origin_z = QDoubleSpinBox()
self.upper_origin_z.setRange(-1.0, 1.0)
self.upper_origin_z.setSingleStep(.01)
self.upper_origin_hbox.addWidget(self.upper_origin_z)
self.upper_vbox.addLayout( self.upper_origin_hbox )
self.upper_groupbox.setLayout( self.upper_vbox )
vbox.addWidget( self.upper_groupbox )
### TERRAIN_MODEL ########
self.terrain_model_groupbox = QGroupBox( "Terrain Model" )
self.terrain_model_groupbox.setCheckable( True )
self.terrain_model_groupbox.setChecked( False )
self.terrain_model_vbox = QVBoxLayout()
self.terrain_model_checkbox = QCheckBox( "Use Terrain Model" )
self.terrain_model_vbox.addWidget( self.terrain_model_checkbox )
self.terrain_min_ssx_hbox = QHBoxLayout()
self.terrain_min_ssx_label = QLabel("Min Sampling Steps x")
self.terrain_min_ssx_hbox.addWidget( self.terrain_min_ssx_label )
self.terrain_min_ssx_val = QDoubleSpinBox()
self.terrain_min_ssx_val.setSingleStep(1)
self.terrain_min_ssx_val.setRange(0,100)
self.terrain_min_ssx_hbox.addWidget( self.terrain_min_ssx_val )
self.terrain_model_vbox.addLayout( self.terrain_min_ssx_hbox )
self.terrain_min_ssy_hbox = QHBoxLayout()
self.terrain_min_ssy_label = QLabel("Min Sampling Steps y")
self.terrain_min_ssy_hbox.addWidget( self.terrain_min_ssy_label )
self.terrain_min_ssy_val = QDoubleSpinBox()
self.terrain_min_ssy_val.setSingleStep(1)
self.terrain_min_ssy_val.setRange(0,100)
self.terrain_min_ssy_hbox.addWidget( self.terrain_min_ssy_val )
self.terrain_model_vbox.addLayout( self.terrain_min_ssy_hbox )
self.terrain_max_ssx_hbox = QHBoxLayout()
self.terrain_max_ssx_label = QLabel("Max Sampling Steps x")
self.terrain_max_ssx_hbox.addWidget( self.terrain_max_ssx_label )
self.terrain_max_ssx_val = QDoubleSpinBox()
self.terrain_max_ssx_val.setSingleStep(1)
self.terrain_max_ssx_val.setRange(0,100)
self.terrain_max_ssx_hbox.addWidget( self.terrain_max_ssx_val )
self.terrain_model_vbox.addLayout( self.terrain_max_ssx_hbox )
self.terrain_max_ssy_hbox = QHBoxLayout()
self.terrain_max_ssy_label = QLabel("Max Sampling Steps y")
self.terrain_max_ssy_hbox.addWidget( self.terrain_max_ssy_label )
self.terrain_max_ssy_val = QDoubleSpinBox()
self.terrain_max_ssy_val.setSingleStep(1)
self.terrain_max_ssy_val.setRange(0,100)
self.terrain_max_ssy_hbox.addWidget( self.terrain_max_ssy_val )
self.terrain_model_vbox.addLayout( self.terrain_max_ssy_hbox )
self.terrain_max_iz_hbox = QHBoxLayout()
self.terrain_max_iz_label = QLabel("Max Intrusion z")
self.terrain_max_iz_hbox.addWidget( self.terrain_max_iz_label )
self.terrain_max_iz_val = QDoubleSpinBox()
self.terrain_max_iz_val.setDecimals(4)
self.terrain_max_iz_val.setSingleStep(.0001)
self.terrain_max_iz_val.setRange(0,.25)
self.terrain_max_iz_hbox.addWidget( self.terrain_max_iz_val )
self.terrain_model_vbox.addLayout( self.terrain_max_iz_hbox )
self.terrain_max_gc_hbox = QHBoxLayout()
self.terrain_max_gc_label = QLabel("Max Ground Clearance")
self.terrain_max_gc_hbox.addWidget( self.terrain_max_gc_label )
self.terrain_max_gc_val = QDoubleSpinBox()
self.terrain_max_gc_val.setDecimals(4)
self.terrain_max_gc_val.setSingleStep(.0001)
self.terrain_max_gc_val.setRange(0,.25)
self.terrain_max_gc_hbox.addWidget( self.terrain_max_gc_val )
self.terrain_model_vbox.addLayout( self.terrain_max_gc_hbox )
self.terrain_ms_hbox = QHBoxLayout()
self.terrain_ms_label = QLabel("Minimal Support")
self.terrain_ms_hbox.addWidget( self.terrain_ms_label )
self.terrain_ms_val = QDoubleSpinBox()
self.terrain_ms_val.setDecimals(2)
self.terrain_ms_val.setSingleStep(.01)
self.terrain_ms_val.setRange(0,1)
self.terrain_ms_hbox.addWidget( self.terrain_ms_val )
self.terrain_model_vbox.addLayout( self.terrain_ms_hbox )
self.terrain_model_groupbox.setLayout( self.terrain_model_vbox )
vbox.addWidget( self.terrain_model_groupbox )
### STANDARD_STEP_PARAMS ########
self.std_step_vbox = QVBoxLayout()
self.std_step_groupbox = QGroupBox( "Standard Step Parameters" )
self.std_step_groupbox.setCheckable( True )
self.std_step_groupbox.setChecked( False )
self.foot_sep_hbox = QHBoxLayout()
self.foot_sep_label = QLabel("Foot Separation")
self.foot_sep_hbox.addWidget( self.foot_sep_label )
self.foot_sep_val = QDoubleSpinBox()
self.foot_sep_val.setSingleStep(.01)
self.foot_sep_hbox.addWidget(self.foot_sep_val)
self.std_step_vbox.addLayout( self.foot_sep_hbox )
self.std_step_step_duration_hbox = QHBoxLayout()
self.std_step_step_duration_label = QLabel("Step Duration")
self.std_step_step_duration_hbox.addWidget( self.std_step_step_duration_label )
self.std_step_step_duration_val = QDoubleSpinBox()
self.std_step_step_duration_val.setSingleStep(.01)
self.std_step_step_duration_hbox.addWidget( self.std_step_step_duration_val )
self.std_step_vbox.addLayout( self.std_step_step_duration_hbox )
self.std_step_sway_duration_hbox = QHBoxLayout()
self.std_step_sway_duration_label = QLabel("Sway Duration")
self.std_step_sway_duration_hbox.addWidget( self.std_step_sway_duration_label )
self.std_step_sway_duration_val = QDoubleSpinBox()
self.std_step_sway_duration_val.setSingleStep(.01)
self.std_step_sway_duration_hbox.addWidget( self.std_step_sway_duration_val )
self.std_step_vbox.addLayout( self.std_step_sway_duration_hbox )
self.std_step_swing_height_hbox = QHBoxLayout()
self.std_step_swing_height_label = QLabel("Swing Height")
self.std_step_swing_height_hbox.addWidget( self.std_step_swing_height_label )
self.std_step_swing_height_val = QDoubleSpinBox()
self.std_step_swing_height_val.setSingleStep(.01)
self.std_step_swing_height_hbox.addWidget( self.std_step_swing_height_val )
self.std_step_vbox.addLayout( self.std_step_swing_height_hbox )
self.std_step_lift_height_hbox = QHBoxLayout()
self.std_step_lift_height_label = QLabel("Lift Height")
self.std_step_lift_height_hbox.addWidget( self.std_step_lift_height_label )
self.std_step_lift_height_val = QDoubleSpinBox()
self.std_step_lift_height_val.setSingleStep(.01)
self.std_step_lift_height_hbox.addWidget( self.std_step_lift_height_val )
self.std_step_vbox.addLayout( self.std_step_lift_height_hbox )
self.std_step_groupbox.setLayout( self.std_step_vbox )
vbox.addWidget( self.std_step_groupbox )
button_hbox = QHBoxLayout()
button_get = QPushButton("Get Current Values")
button_hbox.addWidget( button_get )
button_submit = QPushButton("Send Values")
button_hbox.addWidget( button_submit)
vbox.addLayout( button_hbox )
vbox.addStretch(1)
self._widget.setLayout(vbox)
#context.add_widget(self._widget)
# publishers and subscribers
self.param_pub = rospy.Publisher('/flor/footstep_planner/set_params', FootstepPlannerParams, queue_size=10)
button_submit.pressed.connect(self.sendParams)
self.param_sub = self.stateSubscriber = rospy.Subscriber('/ros_footstep_planner/params', FootstepPlannerParams, self.getParamCallbackFcn)
button_get.pressed.connect(self.getParams)
def __init__(self, context):
self.control_mode = 0
self.mode_pub = rospy.Publisher('/flor/controller/mode_command', VigirControlModeCommand, queue_size=10)
self._widget = context
self.vbox = QVBoxLayout()
#Add input for setting the spindle speed
list_label = QLabel("Select Control Mode")
self.vbox.addWidget(list_label)
# Indexed list of allowed control modes from feedback
self.allowed_modes = rospy.get_param("/atlas_controller/allowed_control_modes")
self.allowed_modes=rospy.get_param("/atlas_controller/allowed_control_modes")
self.mode_ids={}
self.mode_ids
for ndx,txt in enumerate(self.allowed_modes):
self.mode_ids[txt] = ndx
# API 2.13 ordering
self.bdi_mode_names=['NONE ', 'FREEZE ', 'STAND_PREP ', \
'STAND ', 'WALK ', 'STEP ', 'MANIPULATE ', \
'USER ', 'CALIBRATE ']
self.list_box = QListWidget(None)
self.list_box.addItems(self.allowed_modes)
self.list_box.currentItemChanged.connect(self.handle_selection_change)
self.vbox.addWidget(self.list_box)
self.selection_label = QLabel("Flor Selected: "+self.allowed_modes[0]+"("+str(self.control_mode)+")")
self.vbox.addWidget(self.selection_label)
self.label = QLabel("Flor Command : "+self.allowed_modes[0]+"("+str(self.control_mode)+")")
self.vbox.addWidget(self.label)
self.flor_mode = 0
self.bdi_current_mode = 0
self.bdi_desired_mode = 0
self.flor_mode_label = QLabel("Flor Mode : "+self.allowed_modes[self.flor_mode]+"("+str(self.flor_mode)+")"+" BDI:("+str(self.bdi_current_mode)+", "+str(self.bdi_desired_mode)+")")
self.vbox.addWidget(self.flor_mode_label)
#Add combo box for available settings
self.available_modes = QComboBox();
self.available_modes.addItem("");
self.available_modes.addItem("BDI");
self.available_modes.addItem("Enable Upper Body");
self.available_modes.addItem("Enable Whole Body");
self.available_modes.currentIndexChanged.connect(self.handle_avail_modes_changed)
self.vbox.addWidget(self.available_modes);
self.vbox.addStretch(1)
#Add Button for sending the behavior mode command
self.push_button = QPushButton("Set Mode")
self.push_button.clicked.connect(self.handle_set_mode)
self.vbox.addWidget(self.push_button)
self.vbox.addStretch(1)
hbox = QHBoxLayout()
hbox.addStretch(1)
self.stop_enable= QCheckBox()
self.stop_enable.setChecked(False)
hbox.addWidget(self.stop_enable)
self.stop_enable.clicked.connect(self.handle_stop_enable)
self.stop_button = QPushButton("STOP!")
self.stop_button.clicked.connect(self.handle_stop)
self.stop_button.setStyleSheet('QPushButton {background-color: gray }')
hbox.addWidget(self.stop_button)
hbox.addStretch(1)
self.vbox.addLayout(hbox)
self._widget.setLayout(self.vbox)
#add stretch at end so all GUI elements are at top of dialog
self.vbox.addStretch(1)
self.flor_mode_cmd_sub = rospy.Subscriber("/flor/controller/mode", FlorControlMode, self.florModeCallback)
def __init__(self,
topic,
msg_type,
show_only_rate=False,
masteruri=None,
use_ssh=False,
parent=None):
'''
Creates an input dialog.
@param topic: the name of the topic
@type topic: C{str}
@param msg_type: the type of the topic
@type msg_type: C{str}
@raise Exception: if no topic class was found for the given type
'''
QDialog.__init__(self, parent=parent)
self._masteruri = masteruri
masteruri_str = '' if masteruri is None else '[%s]' % masteruri
self.setObjectName(' - '.join(['EchoDialog', topic, masteruri_str]))
self.setAttribute(Qt.WA_DeleteOnClose, True)
self.setWindowFlags(Qt.Window)
self.setWindowTitle('%s %s %s' %
('Echo --- ' if not show_only_rate else 'Hz --- ',
topic, masteruri_str))
self.resize(728, 512)
self.verticalLayout = QVBoxLayout(self)
self.verticalLayout.setObjectName("verticalLayout")
self.verticalLayout.setContentsMargins(1, 1, 1, 1)
self.mIcon = QIcon(":/icons/crystal_clear_prop_run_echo.png")
self.setWindowIcon(self.mIcon)
self.topic = topic
self.show_only_rate = show_only_rate
self.lock = threading.RLock()
self.last_printed_count = 0
self.msg_t0 = -1.
self.msg_tn = 0
self.times = []
self.message_count = 0
self._rate_message = ''
self._scrapped_msgs = 0
self._scrapped_msgs_sl = 0
self._last_received_ts = 0
self.receiving_hz = self.MESSAGE_HZ_LIMIT
self.line_limit = self.MESSAGE_LINE_LIMIT
self.field_filter_fn = None
options = QWidget(self)
if not show_only_rate:
hLayout = QHBoxLayout(options)
hLayout.setContentsMargins(1, 1, 1, 1)
self.no_str_checkbox = no_str_checkbox = QCheckBox('Hide strings')
no_str_checkbox.toggled.connect(self.on_no_str_checkbox_toggled)
hLayout.addWidget(no_str_checkbox)
self.no_arr_checkbox = no_arr_checkbox = QCheckBox('Hide arrays')
no_arr_checkbox.toggled.connect(self.on_no_arr_checkbox_toggled)
hLayout.addWidget(no_arr_checkbox)
self.combobox_reduce_ch = QComboBox(self)
self.combobox_reduce_ch.addItems(
[str(self.MESSAGE_LINE_LIMIT), '0', '80', '256', '1024'])
self.combobox_reduce_ch.activated[str].connect(
self.combobox_reduce_ch_activated)
self.combobox_reduce_ch.setEditable(True)
self.combobox_reduce_ch.setToolTip(
"Set maximum line width. 0 disables the limit.")
hLayout.addWidget(self.combobox_reduce_ch)
# reduce_ch_label = QLabel('ch', self)
# hLayout.addWidget(reduce_ch_label)
# add spacer
spacerItem = QSpacerItem(515, 20, QSizePolicy.Expanding,
QSizePolicy.Minimum)
hLayout.addItem(spacerItem)
# add combobox for displaying frequency of messages
self.combobox_displ_hz = QComboBox(self)
self.combobox_displ_hz.addItems([
str(self.MESSAGE_HZ_LIMIT), '0', '0.1', '1', '50', '100',
'1000'
])
self.combobox_displ_hz.activated[str].connect(
self.on_combobox_hz_activated)
self.combobox_displ_hz.setEditable(True)
hLayout.addWidget(self.combobox_displ_hz)
displ_hz_label = QLabel('Hz', self)
hLayout.addWidget(displ_hz_label)
# add combobox for count of displayed messages
self.combobox_msgs_count = QComboBox(self)
self.combobox_msgs_count.addItems(
[str(self.MAX_DISPLAY_MSGS), '0', '50', '100'])
self.combobox_msgs_count.activated[str].connect(
self.on_combobox_count_activated)
self.combobox_msgs_count.setEditable(True)
self.combobox_msgs_count.setToolTip(
"Set maximum displayed message count. 0 disables the limit.")
hLayout.addWidget(self.combobox_msgs_count)
displ_count_label = QLabel('#', self)
hLayout.addWidget(displ_count_label)
# add topic control button for unsubscribe and subscribe
self.topic_control_button = QToolButton(self)
self.topic_control_button.setText('stop')
self.topic_control_button.setIcon(
QIcon(':/icons/deleket_deviantart_stop.png'))
self.topic_control_button.clicked.connect(
self.on_topic_control_btn_clicked)
hLayout.addWidget(self.topic_control_button)
# add clear button
clearButton = QToolButton(self)
clearButton.setText('clear')
clearButton.clicked.connect(self.on_clear_btn_clicked)
hLayout.addWidget(clearButton)
self.verticalLayout.addWidget(options)
self.display = QTextBrowser(self)
self.display.setReadOnly(True)
self.verticalLayout.addWidget(self.display)
self.display.document().setMaximumBlockCount(500)
self.max_displayed_msgs = self.MAX_DISPLAY_MSGS
self._blocks_in_msg = None
self.display.setOpenLinks(False)
self.display.anchorClicked.connect(self._on_display_anchorClicked)
self.status_label = QLabel('0 messages', self)
self.verticalLayout.addWidget(self.status_label)
# subscribe to the topic
errmsg = ''
try:
self.__msg_class = message.get_message_class(msg_type)
if not self.__msg_class:
errmsg = "Cannot load message class for [%s]. Did you build messages?" % msg_type
# raise Exception("Cannot load message class for [%s]. Did you build messages?"%msg_type)
except Exception as e:
self.__msg_class = None
errmsg = "Cannot load message class for [%s]. Did you build messagest?\nError: %s" % (
msg_type, e)
# raise Exception("Cannot load message class for [%s]. Did you build messagest?\nError: %s"%(msg_type, e))
# variables for Subscriber
self.msg_signal.connect(self._append_message)
self.sub = None
# vairables for SSH connection
self.ssh_output_file = None
self.ssh_error_file = None
self.ssh_input_file = None
self.text_signal.connect(self._append_text)
self.text_hz_signal.connect(self._append_text_hz)
self._current_msg = ''
self._current_errmsg = ''
self.text_error_signal.connect(self._append_error_text)
# decide, which connection to open
if use_ssh:
self.__msg_class = None
self._on_display_anchorClicked(QUrl(self._masteruri))
elif self.__msg_class is None:
errtxt = '<pre style="color:red; font-family:Fixedsys,Courier,monospace; padding:10px;">\n%s</pre>' % (
errmsg)
self.display.setText('<a href="%s">open using SSH</a>' %
(masteruri))
self.display.append(errtxt)
else:
self.sub = rospy.Subscriber(self.topic, self.__msg_class,
self._msg_handle)
self.print_hz_timer = QTimer()
self.print_hz_timer.timeout.connect(self._on_calc_hz)
self.print_hz_timer.start(1000)
class TextSearchFrame(QDockWidget):
'''
A frame to find text in the Editor.
'''
search_result_signal = Signal(str, bool, str, int)
''' @ivar: A signal emitted after search_threaded was started.
(search text, found or not, file, position in text)
for each result a signal will be emitted.
'''
replace_signal = Signal(str, str, int, str)
''' @ivar: A signal emitted to replace string at given position.
(search text, file, position in text, replaced by text)
'''
def __init__(self, tabwidget, parent=None):
QDockWidget.__init__(self, "Find", parent)
self.setObjectName('SearchFrame')
self.setFeatures(QDockWidget.DockWidgetMovable | QDockWidget.DockWidgetFloatable)
self._dockwidget = QFrame(self)
self.vbox_layout = QVBoxLayout(self._dockwidget)
self.layout().setContentsMargins(0, 0, 0, 0)
self.layout().setSpacing(1)
# frame with two rows for find and replace
find_replace_frame = QFrame(self)
find_replace_vbox_layout = QVBoxLayout(find_replace_frame)
find_replace_vbox_layout.setContentsMargins(0, 0, 0, 0)
find_replace_vbox_layout.setSpacing(1)
# find_replace_vbox_layout.addSpacerItem(QSpacerItem(1, 1, QSizePolicy.Expanding, QSizePolicy.Expanding))
# create frame with find row
find_frame = self._create_find_frame()
find_replace_vbox_layout.addWidget(find_frame)
rplc_frame = self._create_replace_frame()
find_replace_vbox_layout.addWidget(rplc_frame)
# frame for find&replace and search results
self.vbox_layout.addWidget(find_replace_frame)
self.vbox_layout.addWidget(self._create_found_frame())
# self.vbox_layout.addStretch(2024)
self.setWidget(self._dockwidget)
# intern search parameters
self._tabwidget = tabwidget
self.current_search_text = ''
self.search_results = []
self.search_results_fileset = set()
self._search_result_index = -1
self._search_recursive = False
self._search_thread = None
def _create_find_frame(self):
find_frame = QFrame(self)
find_hbox_layout = QHBoxLayout(find_frame)
find_hbox_layout.setContentsMargins(0, 0, 0, 0)
find_hbox_layout.setSpacing(1)
self.search_field = EnchancedLineEdit(find_frame)
self.search_field.setPlaceholderText('search text')
self.search_field.textChanged.connect(self.on_search_text_changed)
self.search_field.returnPressed.connect(self.on_search)
find_hbox_layout.addWidget(self.search_field)
self.search_result_label = QLabel(find_frame)
self.search_result_label.setText(' ')
find_hbox_layout.addWidget(self.search_result_label)
self.find_button_back = QPushButton("<")
self.find_button_back.setFixedWidth(44)
self.find_button_back.clicked.connect(self.on_search_back)
find_hbox_layout.addWidget(self.find_button_back)
self.find_button = QPushButton(">")
self.find_button.setDefault(True)
# self.find_button.setFlat(True)
self.find_button.setFixedWidth(44)
self.find_button.clicked.connect(self.on_search)
find_hbox_layout.addWidget(self.find_button)
return find_frame
def _create_replace_frame(self):
# create frame with replace row
self.rplc_frame = rplc_frame = QFrame(self)
rplc_hbox_layout = QHBoxLayout(rplc_frame)
rplc_hbox_layout.setContentsMargins(0, 0, 0, 0)
rplc_hbox_layout.setSpacing(1)
self.replace_field = EnchancedLineEdit(rplc_frame)
self.replace_field.setPlaceholderText('replace text')
self.replace_field.returnPressed.connect(self.on_replace)
rplc_hbox_layout.addWidget(self.replace_field)
self.replace_result_label = QLabel(rplc_frame)
self.replace_result_label.setText(' ')
rplc_hbox_layout.addWidget(self.replace_result_label)
self.replace_button = replace_button = QPushButton("> &Replace >")
replace_button.setFixedWidth(90)
replace_button.clicked.connect(self.on_replace_click)
rplc_hbox_layout.addWidget(replace_button)
rplc_frame.setVisible(False)
return rplc_frame
def _create_found_frame(self):
ff_frame = QFrame(self)
self.found_files_vbox_layout = QVBoxLayout(ff_frame)
self.found_files_vbox_layout.setContentsMargins(0, 0, 0, 0)
self.recursive_search_box = QCheckBox("recursive search")
self.found_files_vbox_layout.addWidget(self.recursive_search_box)
self.found_files_list = QTreeWidget(ff_frame)
self.found_files_list.setColumnCount(1)
self.found_files_list.setFrameStyle(QFrame.StyledPanel)
self.found_files_list.setHeaderHidden(True)
self.found_files_list.itemActivated.connect(self.on_itemActivated)
self.found_files_list.setStyleSheet(
"QTreeWidget {"
"background-color:transparent;"
"}"
"QTreeWidget::item {"
"background-color:transparent;"
"}"
"QTreeWidget::item:selected {"
"background-color: darkgray;"
"}")
self.found_files_vbox_layout.addWidget(self.found_files_list)
self.recursive_search_box.setChecked(False)
return ff_frame
def keyPressEvent(self, event):
'''
Enable the shortcats for search and replace
'''
self.parent().keyPressEvent(event)
def on_search(self):
'''
Initiate the new search or request a next search result.
'''
if self.current_search_text != self.search_field.text() or self._search_recursive != self.recursive_search_box.isChecked():
# clear current search results
self._reset()
self.current_search_text = self.search_field.text()
if self.current_search_text:
path_text = {}
self._wait_for_result = True
for i in range(self._tabwidget.count()):
path_text[self._tabwidget.widget(i).filename] = self._tabwidget.widget(i).document().toPlainText()
self._search_recursive = self.recursive_search_box.isChecked()
self._search_thread = TextSearchThread(self.current_search_text, self._tabwidget.currentWidget().filename, path_text=path_text, recursive=self._search_recursive)
self._search_thread.search_result_signal.connect(self.on_search_result)
self._search_thread.warning_signal.connect(self.on_warning_result)
self._search_thread.start()
elif self.search_results:
self._check_position()
if self.search_results:
if self._search_result_index + 1 >= len(self.search_results):
self._search_result_index = -1
self._search_result_index += 1
(id, search_text, found, path, index, linenr, line) = self.search_results[self._search_result_index]
self.search_result_signal.emit(search_text, found, path, index)
self.replace_button.setEnabled(True)
self._update_label()
def on_search_back(self):
'''
Slot to handle the search back function.
'''
self._check_position(False)
if self.search_results:
self._search_result_index -= 1
if self._search_result_index < 0:
self._search_result_index = len(self.search_results) - 1
self._update_label()
(id, search_text, found, path, index, linenr, line) = self.search_results[self._search_result_index]
self.search_result_signal.emit(search_text, found, path, index)
self.replace_button.setEnabled(True)
def _check_position(self, forward=True):
try:
# if the position of the textCursor was changed by the user, move the search index
cur_pos = self._tabwidget.currentWidget().textCursor().position()
id, st, _f, pa, idx, lnr, ltxt = self.search_results[self._search_result_index]
sear_pos = idx + len(st)
if cur_pos != sear_pos:
first_idx = self._get_current_index_for_current_file()
if first_idx != -1:
id, st, _f, pa, idx, lnr, ltxt = self.search_results[first_idx]
sear_pos = idx + len(st)
while cur_pos > sear_pos and self._tabwidget.currentWidget().filename == pa:
first_idx += 1
id, st, _f, pa, idx, lnr, ltxt = self.search_results[first_idx]
sear_pos = idx + len(st)
self._search_result_index = first_idx
if forward:
self._search_result_index -= 1
else:
self._reset(True)
except:
pass
def _get_current_index_for_current_file(self):
for index in range(len(self.search_results)):
id, _st, _f, pa, _idx = self.search_results[index]
if self._tabwidget.currentWidget().filename == pa:
return index
return -1
def on_search_result(self, search_text, found, path, index, linenr, line):
'''
Slot to handle the signals for search result. This signals are forwarded used
search_result_signal.
'''
if found and search_text == self.current_search_text:
id = "%d:%s" % (index, path)
self.search_results_fileset.add(path)
item = (search_text, found, path, index)
if item not in self.search_results:
self.search_results.append((id, search_text, found, path, index, linenr, line))
if self._wait_for_result:
self._search_result_index += 1
if index >= self._tabwidget.currentWidget().textCursor().position() or self._tabwidget.currentWidget().filename != path:
self._wait_for_result = False
self.search_result_signal.emit(search_text, found, path, index)
self.replace_button.setEnabled(True)
pkg, rpath = package_name(os.path.dirname(path))
itemstr = '%s [%s]' % (os.path.basename(path), pkg)
if not self.found_files_list.findItems(itemstr, Qt.MatchExactly):
list_item = QTreeWidgetItem(self.found_files_list)
list_item.setText(0, itemstr)
list_item.setToolTip(0, path)
self.found_files_list.insertTopLevelItem(0, list_item)
self.found_files_list.expandAll()
for i in range(self.found_files_list.topLevelItemCount()):
top_item = self.found_files_list.topLevelItem(i)
if top_item.text(0) == itemstr:
sub_item_str = "%d: %s" % (linenr, line)
list_item2 = QTreeWidgetItem()
list_item2.setText(0, sub_item_str)
list_item2.setWhatsThis(0, id)
top_item.addChild(list_item2)
#self.found_files_list.setVisible(len(self.search_results_fileset) > 0)
self._update_label()
def on_warning_result(self, text):
rospy.logwarn(text)
def on_replace_click(self):
self.on_replace()
self.on_search()
def on_replace(self):
'''
Emits the replace signal, but only if currently selected text is equal to the searched one.
'''
if self.search_results:
try:
id, search_text, _found, path, index, linenr, line_text = self.search_results[self._search_result_index]
cursor = self._tabwidget.currentWidget().textCursor()
if cursor.selectedText() == search_text:
rptxt = self.replace_field.text()
for rindex in range(self._search_result_index + 1, len(self.search_results)):
iid, st, _f, pa, idx, lnr, ltxt = self.search_results[rindex]
if path == pa:
self.search_results.pop(rindex)
self.search_results.insert(rindex, (iid, st, _f, pa, idx + len(rptxt) - len(st), lnr, ltxt))
else:
break
self._remove_search_result(self._search_result_index)
self._search_result_index -= 1
self.replace_signal.emit(search_text, path, index, rptxt)
else:
self.replace_button.setEnabled(False)
except:
import traceback
print traceback.format_exc()
pass
def on_itemActivated(self, item):
'''
Go to the results for the selected file entry in the list.
'''
splits = item.whatsThis(0).split(':')
if len(splits) == 2:
item_index = int(splits[0])
item_path = splits[1]
new_search_index = -1
tmp_index = -1
search_index = -1
tmp_search_text = ''
for id, search_text, found, path, index, linenr, line_text in self.search_results:
new_search_index += 1
if item_path == path and item_index == index:
self._search_result_index = new_search_index
self.search_result_signal.emit(search_text, found, path, index)
self._update_label()
def on_search_text_changed(self, _text):
'''
Clear search result if the text was changed.
'''
self._reset()
def _update_label(self, clear_label=False):
'''
Updates the status label for search results. The info is created from search result lists.
'''
msg = ' '
if self.search_results:
count_files = len(self.search_results_fileset)
msg = '%d/%d' % (self._search_result_index + 1, len(self.search_results))
if count_files > 1:
msg = '%s(%d)' % (msg, count_files)
if self._search_thread is not None and self._search_thread.is_alive():
msg = 'searching..%s' % msg
elif not msg.strip() and self.current_search_text:
msg = '0 found'
self.current_search_text = ''
if clear_label:
msg = ' '
self.search_result_label.setText(msg)
self.find_button_back.setEnabled(len(self.search_results))
self._select_current_item_in_box(self._search_result_index)
def file_changed(self, path):
'''
Clears search results if for changed file are some search results are available
:param path: changed file path
:type path: str
'''
if path in self.search_results_fileset:
self._reset()
def set_replace_visible(self, value):
self.rplc_frame.setVisible(value)
self.raise_()
self.activateWindow()
if value:
self.replace_field.setFocus()
self.replace_field.selectAll()
self.setWindowTitle("Find / Replace")
else:
self.setWindowTitle("Find")
self.search_field.setFocus()
def is_replace_visible(self):
return self.rplc_frame.isVisible()
def _reset(self, force_new_search=False):
# clear current search results
if self._search_thread is not None:
self._search_thread.search_result_signal.disconnect()
self._search_thread.stop()
self._search_thread = None
self.current_search_text = ''
self.search_results = []
self.search_results_fileset = set()
self.found_files_list.clear()
# self.found_files_list.setVisible(False)
self._update_label(True)
self._search_result_index = -1
self.find_button_back.setEnabled(False)
if force_new_search:
self.on_search()
def enable(self):
self.setVisible(True)
# self.show()
self.raise_()
self.activateWindow()
self.search_field.setFocus()
self.search_field.selectAll()
def _select_current_item_in_box(self, index):
try:
(id, search_text, found, path, index, linenr, line) = self.search_results[index]
for topidx in range(self.found_files_list.topLevelItemCount()):
topitem = self.found_files_list.topLevelItem(topidx)
for childdx in range(topitem.childCount()):
child = topitem.child(childdx)
if child.whatsThis(0) == id:
child.setSelected(True)
elif child.isSelected():
child.setSelected(False)
except:
pass
def _remove_search_result(self, index):
try:
(id, search_text, found, path, index, linenr, line) = self.search_results.pop(index)
pkg, rpath = package_name(os.path.dirname(path))
itemstr = '%s [%s]' % (os.path.basename(path), pkg)
found_items = self.found_files_list.findItems(itemstr, Qt.MatchExactly)
for item in found_items:
for chi in range(item.childCount()):
child = item.child(chi)
if child.whatsThis(0) == id:
item.removeChild(child)
break
# delete top level item if it is now empty
for topidx in range(self.found_files_list.topLevelItemCount()):
if self.found_files_list.topLevelItem(topidx).childCount() == 0:
self.found_files_list.takeTopLevelItem(topidx)
break
# create new set with files contain the search text
new_path_set = set(path for _id, _st, _fd, path, _idx, lnr, lntxt in self.search_results)
self.search_results_fileset = new_path_set
# self.found_files_list.setVisible(len(self.search_results_fileset) > 0)
except:
import traceback
print traceback.format_exc()
def __init__(self, context, add_execute_widget=True):
super(StepInterfaceWidget, self).__init__()
# init signal mapper
self.command_mapper = QSignalMapper(self)
self.command_mapper.mapped.connect(self._publish_step_plan_request)
# start widget
widget = context
error_status_widget = QErrorStatusWidget()
self.logger = Logger(error_status_widget)
vbox = QVBoxLayout()
# start control box
controls_hbox = QHBoxLayout()
# left coloumn
left_controls_vbox = QVBoxLayout()
left_controls_vbox.setMargin(0)
self.add_command_button(left_controls_vbox, "Rotate Left",
PatternParameters.ROTATE_LEFT)
self.add_command_button(left_controls_vbox, "Strafe Left",
PatternParameters.STRAFE_LEFT)
self.add_command_button(left_controls_vbox, "Step Up",
PatternParameters.STEP_UP)
self.add_command_button(left_controls_vbox, "Center on Left",
PatternParameters.FEET_REALIGN_ON_LEFT)
left_controls_vbox.addStretch()
controls_hbox.addLayout(left_controls_vbox, 1)
# center coloumn
center_controls_vbox = QVBoxLayout()
center_controls_vbox.setMargin(0)
self.add_command_button(center_controls_vbox, "Forward",
PatternParameters.FORWARD)
self.add_command_button(center_controls_vbox, "Backward",
PatternParameters.BACKWARD)
self.add_command_button(center_controls_vbox, "Step Over",
PatternParameters.STEP_OVER)
self.add_command_button(center_controls_vbox, "Center Feet",
PatternParameters.FEET_REALIGN_ON_CENTER)
self.add_command_button(center_controls_vbox, "Wide Stance",
PatternParameters.WIDE_STANCE)
center_controls_vbox.addStretch()
controls_hbox.addLayout(center_controls_vbox, 1)
# right coloumn
right_controls_vbox = QVBoxLayout()
right_controls_vbox.setMargin(0)
self.add_command_button(right_controls_vbox, "Rotate Right",
PatternParameters.ROTATE_RIGHT)
self.add_command_button(right_controls_vbox, "Strafe Right",
PatternParameters.STRAFE_RIGHT)
self.add_command_button(right_controls_vbox, "Step Down",
PatternParameters.STEP_DOWN)
self.add_command_button(right_controls_vbox, "Center on Right",
PatternParameters.FEET_REALIGN_ON_RIGHT)
right_controls_vbox.addStretch()
controls_hbox.addLayout(right_controls_vbox, 1)
# end control box
add_layout_with_frame(vbox, controls_hbox, "Commands:")
# start settings
settings_hbox = QHBoxLayout()
settings_hbox.setMargin(0)
# start left column
left_settings_vbox = QVBoxLayout()
left_settings_vbox.setMargin(0)
# frame id
self.frame_id_line_edit = QLineEdit("/world")
add_widget_with_frame(left_settings_vbox, self.frame_id_line_edit,
"Frame ID:")
# do closing step
self.close_step_checkbox = QCheckBox()
self.close_step_checkbox.setText("Do closing step")
self.close_step_checkbox.setChecked(True)
left_settings_vbox.addWidget(self.close_step_checkbox)
# extra seperation
self.extra_seperation_checkbox = QCheckBox()
self.extra_seperation_checkbox.setText("Extra Seperation")
self.extra_seperation_checkbox.setChecked(False)
left_settings_vbox.addWidget(self.extra_seperation_checkbox)
left_settings_vbox.addStretch()
# number of steps
self.step_number = generate_q_double_spin_box(1, 1, 50, 0, 1.0)
add_widget_with_frame(left_settings_vbox, self.step_number,
"Number Steps:")
# start step index
self.start_step_index = generate_q_double_spin_box(0, 0, 1000, 0, 1.0)
add_widget_with_frame(left_settings_vbox, self.start_step_index,
"Start Step Index:")
# end left column
settings_hbox.addLayout(left_settings_vbox, 1)
# start center column
center_settings_vbox = QVBoxLayout()
center_settings_vbox.setMargin(0)
# start foot selection
self.start_foot_selection_combo_box = QComboBox()
self.start_foot_selection_combo_box.addItem("AUTO")
self.start_foot_selection_combo_box.addItem("LEFT")
self.start_foot_selection_combo_box.addItem("RIGHT")
add_widget_with_frame(center_settings_vbox,
self.start_foot_selection_combo_box,
"Start foot selection:")
center_settings_vbox.addStretch()
# step Distance
self.step_distance = generate_q_double_spin_box(0.0, 0.0, 0.5, 2, 0.01)
add_widget_with_frame(center_settings_vbox, self.step_distance,
"Step Distance (m):")
# side step distance
self.side_step = generate_q_double_spin_box(0.0, 0.0, 0.2, 2, 0.01)
add_widget_with_frame(center_settings_vbox, self.side_step,
"Side Step (m):")
# rotation per step
self.step_rotation = generate_q_double_spin_box(
0.0, -30.0, 30.0, 0, 1.0)
add_widget_with_frame(center_settings_vbox, self.step_rotation,
"Step Rotation (deg):")
# end center column
settings_hbox.addLayout(center_settings_vbox, 1)
# start right column
right_settings_vbox = QVBoxLayout()
right_settings_vbox.setMargin(0)
# roll
self.roll = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
add_widget_with_frame(right_settings_vbox, self.roll, "Roll (deg):")
# pitch
self.pitch = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
add_widget_with_frame(right_settings_vbox, self.pitch, "Pitch (deg):")
# use terrain model
self.use_terrain_model_checkbox = QCheckBox()
self.use_terrain_model_checkbox.setText("Use Terrain Model")
self.use_terrain_model_checkbox.setChecked(False)
self.use_terrain_model_checkbox.stateChanged.connect(
self.use_terrain_model_changed)
right_settings_vbox.addWidget(self.use_terrain_model_checkbox)
# override mode
self.override_checkbox = QCheckBox()
self.override_checkbox.setText("Override 3D")
self.override_checkbox.setChecked(False)
right_settings_vbox.addWidget(self.override_checkbox)
right_settings_vbox.addStretch()
# delta z
self.dz = generate_q_double_spin_box(0.0, -0.5, 0.5, 2, 0.01)
add_widget_with_frame(right_settings_vbox, self.dz,
"delta z per step (m):")
# end right column
settings_hbox.addLayout(right_settings_vbox, 1)
# end settings
add_layout_with_frame(vbox, settings_hbox, "Settings:")
# parameter set selection
self.parameter_set_widget = QParameterSetWidget(logger=self.logger)
add_widget_with_frame(vbox, self.parameter_set_widget,
"Parameter Set:")
# execute option
if add_execute_widget:
add_widget_with_frame(
vbox,
QExecuteStepPlanWidget(logger=self.logger,
step_plan_topic="step_plan"),
"Execute:")
# add error status widget
add_widget_with_frame(vbox, error_status_widget, "Status:")
# end widget
widget.setLayout(vbox)
#context.add_widget(widget)
# init widget
self.parameter_set_widget.param_cleared_signal.connect(
self.param_cleared)
self.parameter_set_widget.param_changed_signal.connect(
self.param_selected)
self.commands_set_enabled(False)
# subscriber
self.start_feet_sub = rospy.Subscriber("set_start_feet", Feet,
self.set_start_feet_callback)
# publisher
self.step_plan_pub = rospy.Publisher("step_plan",
StepPlan,
queue_size=1)
# action clients
self.step_plan_request_client = actionlib.SimpleActionClient(
"step_plan_request", StepPlanRequestAction)
class StepInterfaceWidget(QObject):
command_buttons = []
start_feet = Feet()
def __init__(self, context, add_execute_widget = True):
super(StepInterfaceWidget, self).__init__()
# init signal mapper
self.command_mapper = QSignalMapper(self)
self.command_mapper.mapped.connect(self._publish_step_plan_request)
# start widget
widget = context
error_status_widget = QErrorStatusWidget()
self.logger = Logger(error_status_widget)
vbox = QVBoxLayout()
# start control box
controls_hbox = QHBoxLayout()
# left coloumn
left_controls_vbox = QVBoxLayout()
left_controls_vbox.setMargin(0)
self.add_command_button(left_controls_vbox, "Rotate Left", PatternParameters.ROTATE_LEFT)
self.add_command_button(left_controls_vbox, "Strafe Left", PatternParameters.STRAFE_LEFT)
self.add_command_button(left_controls_vbox, "Step Up", PatternParameters.STEP_UP)
self.add_command_button(left_controls_vbox, "Center on Left", PatternParameters.FEET_REALIGN_ON_LEFT)
left_controls_vbox.addStretch()
controls_hbox.addLayout(left_controls_vbox, 1)
# center coloumn
center_controls_vbox = QVBoxLayout()
center_controls_vbox.setMargin(0)
self.add_command_button(center_controls_vbox, "Forward", PatternParameters.FORWARD)
self.add_command_button(center_controls_vbox, "Backward", PatternParameters.BACKWARD)
self.add_command_button(center_controls_vbox, "Step Over", PatternParameters.STEP_OVER)
self.add_command_button(center_controls_vbox, "Center Feet", PatternParameters.FEET_REALIGN_ON_CENTER)
self.add_command_button(center_controls_vbox, "Wide Stance", PatternParameters.WIDE_STANCE)
center_controls_vbox.addStretch()
controls_hbox.addLayout(center_controls_vbox, 1)
# right coloumn
right_controls_vbox = QVBoxLayout()
right_controls_vbox.setMargin(0)
self.add_command_button(right_controls_vbox, "Rotate Right", PatternParameters.ROTATE_RIGHT)
self.add_command_button(right_controls_vbox, "Strafe Right", PatternParameters.STRAFE_RIGHT)
self.add_command_button(right_controls_vbox, "Step Down", PatternParameters.STEP_DOWN)
self.add_command_button(right_controls_vbox, "Center on Right", PatternParameters.FEET_REALIGN_ON_RIGHT)
right_controls_vbox.addStretch()
controls_hbox.addLayout(right_controls_vbox, 1)
# end control box
add_layout_with_frame(vbox, controls_hbox, "Commands:")
# start settings
settings_hbox = QHBoxLayout()
settings_hbox.setMargin(0)
# start left column
left_settings_vbox = QVBoxLayout()
left_settings_vbox.setMargin(0)
# frame id
self.frame_id_line_edit = QLineEdit("/world")
add_widget_with_frame(left_settings_vbox, self.frame_id_line_edit, "Frame ID:")
# do closing step
self.close_step_checkbox = QCheckBox()
self.close_step_checkbox.setText("Do closing step")
self.close_step_checkbox.setChecked(True)
left_settings_vbox.addWidget(self.close_step_checkbox)
# extra seperation
self.extra_seperation_checkbox = QCheckBox()
self.extra_seperation_checkbox.setText("Extra Seperation")
self.extra_seperation_checkbox.setChecked(False)
left_settings_vbox.addWidget(self.extra_seperation_checkbox)
left_settings_vbox.addStretch()
# number of steps
self.step_number = generate_q_double_spin_box(1, 1, 50, 0, 1.0)
add_widget_with_frame(left_settings_vbox, self.step_number, "Number Steps:")
# start step index
self.start_step_index = generate_q_double_spin_box(0, 0, 1000, 0, 1.0)
add_widget_with_frame(left_settings_vbox, self.start_step_index, "Start Step Index:")
# end left column
settings_hbox.addLayout(left_settings_vbox, 1)
# start center column
center_settings_vbox = QVBoxLayout()
center_settings_vbox.setMargin(0)
# start foot selection
self.start_foot_selection_combo_box = QComboBox()
self.start_foot_selection_combo_box.addItem("AUTO")
self.start_foot_selection_combo_box.addItem("LEFT")
self.start_foot_selection_combo_box.addItem("RIGHT")
add_widget_with_frame(center_settings_vbox, self.start_foot_selection_combo_box, "Start foot selection:")
center_settings_vbox.addStretch()
# step Distance
self.step_distance = generate_q_double_spin_box(0.20, 0.0, 0.5, 2, 0.01)
add_widget_with_frame(center_settings_vbox, self.step_distance, "Step Distance (m):")
# side step distance
self.side_step = generate_q_double_spin_box(0.0, 0.0, 0.2, 2, 0.01)
add_widget_with_frame(center_settings_vbox, self.side_step, "Side Step (m):")
# rotation per step
self.step_rotation = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
add_widget_with_frame(center_settings_vbox, self.step_rotation, "Step Rotation (deg):")
# end center column
settings_hbox.addLayout(center_settings_vbox, 1)
# start right column
right_settings_vbox = QVBoxLayout()
right_settings_vbox.setMargin(0)
# roll
self.roll = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
add_widget_with_frame(right_settings_vbox, self.roll, "Roll (deg):")
# pitch
self.pitch = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
add_widget_with_frame(right_settings_vbox, self.pitch, "Pitch (deg):")
# use terrain model
self.use_terrain_model_checkbox = QCheckBox()
self.use_terrain_model_checkbox.setText("Use Terrain Model")
self.use_terrain_model_checkbox.setChecked(False)
self.use_terrain_model_checkbox.stateChanged.connect(self.use_terrain_model_changed)
right_settings_vbox.addWidget(self.use_terrain_model_checkbox)
# override mode
self.override_checkbox = QCheckBox()
self.override_checkbox.setText("Override 3D")
self.override_checkbox.setChecked(False)
right_settings_vbox.addWidget(self.override_checkbox)
right_settings_vbox.addStretch()
# delta z
self.dz = generate_q_double_spin_box(0.0, -0.5, 0.5, 2, 0.01)
add_widget_with_frame(right_settings_vbox, self.dz, "delta z per step (m):")
# end right column
settings_hbox.addLayout(right_settings_vbox, 1)
# end settings
add_layout_with_frame(vbox, settings_hbox, "Settings:")
# parameter set selection
self.parameter_set_widget = QParameterSetWidget(logger = self.logger)
add_widget_with_frame(vbox, self.parameter_set_widget, "Parameter Set:")
# execute option
if add_execute_widget:
add_widget_with_frame(vbox, QExecuteStepPlanWidget(logger = self.logger, step_plan_topic = "step_plan"), "Execute:")
# add error status widget
add_widget_with_frame(vbox, error_status_widget, "Status:")
# end widget
widget.setLayout(vbox)
#context.add_widget(widget)
# init widget
self.parameter_set_widget.param_cleared_signal.connect(self.param_cleared)
self.parameter_set_widget.param_changed_signal.connect(self.param_selected)
self.commands_set_enabled(False)
# subscriber
self.start_feet_sub = rospy.Subscriber("set_start_feet", Feet, self.set_start_feet_callback)
# publisher
self.step_plan_pub = rospy.Publisher("step_plan", StepPlan, queue_size=1)
# action clients
self.step_plan_request_client = actionlib.SimpleActionClient("step_plan_request", StepPlanRequestAction)
def shutdown_plugin(self):
print "Shutting down ..."
print "Done!"
def add_command_button(self, parent, text, command):
button = QPushButton(text)
self.command_mapper.setMapping(button, command)
button.clicked.connect(self.command_mapper.map)
parent.addWidget(button)
self.command_buttons.append(button)
return button
def set_start_feet_callback(self, feet):
self.start_feet = feet
# message publisher
def _publish_step_plan_request(self, walk_command):
params = PatternParameters()
params.steps = self.step_number.value()
params.mode = walk_command
params.close_step = self.close_step_checkbox.isChecked()
params.extra_seperation = self.extra_seperation_checkbox.isChecked()
params.use_terrain_model = self.use_terrain_model_checkbox.isChecked()
params.override = self.override_checkbox.isChecked() and not self.use_terrain_model_checkbox.isChecked()
params.roll = math.radians(self.roll.value())
params.pitch = math.radians(self.pitch.value())
params.dz = self.dz.value()
params.step_distance_forward = self.step_distance.value()
params.step_distance_sideward = self.side_step.value()
params.turn_angle = math.radians(self.step_rotation.value())
request = StepPlanRequest()
request.header = std_msgs.msg.Header()
request.header.stamp = rospy.Time.now()
request.header.frame_id = self.frame_id_line_edit.text()
request.start = self.start_feet
request.start_step_index = self.start_step_index.value()
if (self.start_foot_selection_combo_box.currentText() == "AUTO"):
request.start_foot_selection = StepPlanRequest.AUTO
elif (self.start_foot_selection_combo_box.currentText() == "LEFT"):
request.start_foot_selection = StepPlanRequest.LEFT
elif (self.start_foot_selection_combo_box.currentText() == "RIGHT"):
request.start_foot_selection = StepPlanRequest.RIGHT
else:
self.logger.log_error("Unknown start foot selection mode ('" + self.start_foot_selection_combo_box.currentText() + "')!")
return;
if (walk_command == PatternParameters.FORWARD):
params.mode = PatternParameters.SAMPLING
elif (walk_command == PatternParameters.BACKWARD):
params.mode = PatternParameters.SAMPLING
params.step_distance_forward *= -1;
params.step_distance_sideward *= -1;
params.turn_angle *= -1;
request.pattern_parameters = params
request.planning_mode = StepPlanRequest.PLANNING_MODE_PATTERN
request.parameter_set_name.data = self.parameter_set_widget.current_parameter_set_name()
print "Send request = ", request
# send request
if (self.step_plan_request_client.wait_for_server(rospy.Duration(0.5))):
self.logger.log_info("Sending footstep plan request...")
goal = StepPlanRequestGoal()
goal.plan_request = request;
self.step_plan_request_client.send_goal(goal)
if (self.step_plan_request_client.wait_for_result(rospy.Duration(5.0))):
self.logger.log_info("Received footstep plan!")
self.logger.log(self.step_plan_request_client.get_result().status)
self.step_plan_pub.publish(self.step_plan_request_client.get_result().step_plan)
else:
self.logger.log_error("Didn't received any results. Check communcation!")
else:
self.logger.log_error("Can't connect to footstep planner action server!")
def commands_set_enabled(self, enable):
for button in self.command_buttons:
button.setEnabled(enable)
@Slot()
def param_cleared(self):
self.commands_set_enabled(False)
@Slot(str)
def param_selected(self, name):
self.commands_set_enabled(True)
@Slot(int)
def use_terrain_model_changed(self, state):
enable_override = True
if state == Qt.Checked:
enable_override = False
self.roll.setEnabled(enable_override)
self.pitch.setEnabled(enable_override)
self.override_checkbox.setEnabled(enable_override)
self.dz.setEnabled(enable_override)
class SelectDialog(QDialog):
'''
This dialog creates an input mask for a string list and return selected entries.
'''
def __init__(self,
items=list(),
buttons=QDialogButtonBox.Cancel | QDialogButtonBox.Ok,
exclusive=False,
preselect_all=False,
title='',
description='',
icon='',
parent=None,
select_if_single=True,
checkitem1='',
checkitem2='',
closein=0):
'''
Creates an input dialog.
@param items: a list with strings
@type items: C{list()}
'''
QDialog.__init__(self, parent=parent)
self.setObjectName(' - '.join(['SelectDialog', utf8(items)]))
self.verticalLayout = QVBoxLayout(self)
self.verticalLayout.setObjectName("verticalLayout")
self.verticalLayout.setContentsMargins(1, 1, 1, 1)
# add filter row
self.filter_frame = QFrame(self)
filterLayout = QHBoxLayout(self.filter_frame)
filterLayout.setContentsMargins(1, 1, 1, 1)
label = QLabel("Filter:", self.filter_frame)
self.filter_field = EnchancedLineEdit(self.filter_frame)
filterLayout.addWidget(label)
filterLayout.addWidget(self.filter_field)
self.filter_field.textChanged.connect(self._on_filter_changed)
self.verticalLayout.addWidget(self.filter_frame)
if description:
self.description_frame = QFrame(self)
descriptionLayout = QHBoxLayout(self.description_frame)
# descriptionLayout.setContentsMargins(1, 1, 1, 1)
if icon:
self.icon_label = QLabel(self.description_frame)
self.icon_label.setSizePolicy(QSizePolicy.Fixed,
QSizePolicy.Fixed)
self.icon_label.setPixmap(
QPixmap(icon).scaled(30, 30, Qt.KeepAspectRatio))
descriptionLayout.addWidget(self.icon_label)
self.description_label = QLabel(self.description_frame)
self.description_label.setWordWrap(True)
self.description_label.setText(description)
descriptionLayout.addWidget(self.description_label)
self.verticalLayout.addWidget(self.description_frame)
# create area for the parameter
self.content = MainBox(self)
if items:
self.scroll_area = QScrollArea(self)
self.scroll_area.setFocusPolicy(Qt.NoFocus)
self.scroll_area.setObjectName("scroll_area")
self.scroll_area.setWidgetResizable(True)
self.scroll_area.setWidget(self.content)
self.verticalLayout.addWidget(self.scroll_area)
self.checkitem1 = checkitem1
self.checkitem1_result = False
self.checkitem2 = checkitem2
self.checkitem2_result = False
# add select all option
if not exclusive and items:
self._ignore_next_toggle = False
self.select_all_checkbox = QCheckBox('all entries')
self.select_all_checkbox.setTristate(True)
self.select_all_checkbox.stateChanged.connect(
self._on_select_all_checkbox_stateChanged)
self.verticalLayout.addWidget(self.select_all_checkbox)
self.content.toggled.connect(self._on_main_toggle)
if self.checkitem1:
self.checkitem1_checkbox = QCheckBox(self.checkitem1)
self.checkitem1_checkbox.stateChanged.connect(
self._on_select_checkitem1_checkbox_stateChanged)
self.verticalLayout.addWidget(self.checkitem1_checkbox)
if self.checkitem2:
self.checkitem2_checkbox = QCheckBox(self.checkitem2)
self.checkitem2_checkbox.stateChanged.connect(
self._on_select_checkitem2_checkbox_stateChanged)
self.verticalLayout.addWidget(self.checkitem2_checkbox)
if not items:
spacerItem = QSpacerItem(1, 1, QSizePolicy.Expanding,
QSizePolicy.Expanding)
self.verticalLayout.addItem(spacerItem)
self._close_timer = None
self._closein = closein - 1
if closein > 0:
self.closein_label = QLabel("OK in %d sec..." % closein)
self.closein_label.setAlignment(Qt.AlignRight)
self.verticalLayout.addWidget(self.closein_label)
self._close_timer = threading.Timer(1.0, self._on_close_timer)
self._close_timer.start()
# create buttons
self.buttonBox = QDialogButtonBox(self)
self.buttonBox.setObjectName("buttonBox")
self.buttonBox.setOrientation(Qt.Horizontal)
self.buttonBox.setStandardButtons(buttons)
self.buttonBox.accepted.connect(self.accept)
self.buttonBox.rejected.connect(self.reject)
self.verticalLayout.addWidget(self.buttonBox)
# set the input fields
if items:
self.content.createFieldsFromValues(items, exclusive)
if (select_if_single and len(items) == 1) or preselect_all:
self.select_all_checkbox.setCheckState(Qt.Checked)
if not items or len(items) < 7:
self.filter_frame.setVisible(False)
# print '=============== create', self.objectName()
#
# def __del__(self):
# print "************ destroy", self.objectName()
def _on_main_toggle(self, state):
self.cancel_autoclose()
self._ignore_next_toggle = state != self.select_all_checkbox.checkState(
)
self.select_all_checkbox.setCheckState(state)
def _on_select_all_checkbox_stateChanged(self, state):
self.cancel_autoclose()
if not self._ignore_next_toggle:
self.content.setState(state)
self._ignore_next_toggle = False
def _on_select_checkitem1_checkbox_stateChanged(self, state):
self.cancel_autoclose()
if state == Qt.Checked:
self.checkitem1_result = True
elif state == Qt.Unchecked:
self.checkitem1_result = False
def _on_select_checkitem2_checkbox_stateChanged(self, state):
self.cancel_autoclose()
if state == Qt.Checked:
self.checkitem2_result = True
elif state == Qt.Unchecked:
self.checkitem2_result = False
def _on_filter_changed(self):
self.content.filter(self.filter_field.text())
def _on_close_timer(self):
self.closein_label.setText("OK in %d sec..." % self._closein)
if self._closein == 0:
self.buttonBox.accepted.emit()
return
self._closein -= 1
self._close_timer = threading.Timer(1.0, self._on_close_timer)
self._close_timer.start()
def cancel_autoclose(self):
if self._close_timer is not None:
self._close_timer.cancel()
self.closein_label.setVisible(False)
def getSelected(self):
return self.content.getSelected()
@staticmethod
def getValue(title,
description='',
items=list(),
exclusive=False,
preselect_all=False,
icon='',
parent=None,
select_if_single=True,
checkitem1='',
checkitem2='',
closein=0):
selectDia = SelectDialog(items,
exclusive=exclusive,
preselect_all=preselect_all,
description=description,
icon=icon,
parent=parent,
select_if_single=select_if_single,
checkitem1=checkitem1,
checkitem2=checkitem2,
closein=closein)
selectDia.setWindowTitle(title)
selectDia.resize(480, 256)
if selectDia.exec_():
if selectDia.checkitem2:
return selectDia.getSelected(
), True, selectDia.checkitem1_result, selectDia.checkitem2_result
if selectDia.checkitem1:
return selectDia.getSelected(
), True, selectDia.checkitem1_result
return selectDia.getSelected(), True
if selectDia.checkitem2:
return list(), False, False, False
if selectDia.checkitem1:
return list(), False, False
return list(), False
# %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
# %%%%%%%%%%%%%%%%%% close handling %%%%%%%%%%%%%%%%%%%%%
# %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
def accept(self):
self.cancel_autoclose()
self.setResult(QDialog.Accepted)
self.hide()
def reject(self):
self.cancel_autoclose()
self.setResult(QDialog.Rejected)
self.hide()
def hideEvent(self, event):
self.close()
def closeEvent(self, event):
'''
Test the open files for changes and save this if needed.
'''
self.cancel_autoclose()
self.setAttribute(Qt.WA_DeleteOnClose, True)
QDialog.closeEvent(self, event)
def __init__(self, context, add_execute_widget = True):
super(StepInterfaceWidget, self).__init__()
# init signal mapper
self.command_mapper = QSignalMapper(self)
self.command_mapper.mapped.connect(self._publish_step_plan_request)
# start widget
widget = context
error_status_widget = QErrorStatusWidget()
self.logger = Logger(error_status_widget)
vbox = QVBoxLayout()
# start control box
controls_hbox = QHBoxLayout()
# left coloumn
left_controls_vbox = QVBoxLayout()
left_controls_vbox.setMargin(0)
self.add_command_button(left_controls_vbox, "Rotate Left", PatternParameters.ROTATE_LEFT)
self.add_command_button(left_controls_vbox, "Strafe Left", PatternParameters.STRAFE_LEFT)
self.add_command_button(left_controls_vbox, "Step Up", PatternParameters.STEP_UP)
self.add_command_button(left_controls_vbox, "Center on Left", PatternParameters.FEET_REALIGN_ON_LEFT)
left_controls_vbox.addStretch()
controls_hbox.addLayout(left_controls_vbox, 1)
# center coloumn
center_controls_vbox = QVBoxLayout()
center_controls_vbox.setMargin(0)
self.add_command_button(center_controls_vbox, "Forward", PatternParameters.FORWARD)
self.add_command_button(center_controls_vbox, "Backward", PatternParameters.BACKWARD)
self.add_command_button(center_controls_vbox, "Step Over", PatternParameters.STEP_OVER)
self.add_command_button(center_controls_vbox, "Center Feet", PatternParameters.FEET_REALIGN_ON_CENTER)
self.add_command_button(center_controls_vbox, "Wide Stance", PatternParameters.WIDE_STANCE)
center_controls_vbox.addStretch()
controls_hbox.addLayout(center_controls_vbox, 1)
# right coloumn
right_controls_vbox = QVBoxLayout()
right_controls_vbox.setMargin(0)
self.add_command_button(right_controls_vbox, "Rotate Right", PatternParameters.ROTATE_RIGHT)
self.add_command_button(right_controls_vbox, "Strafe Right", PatternParameters.STRAFE_RIGHT)
self.add_command_button(right_controls_vbox, "Step Down", PatternParameters.STEP_DOWN)
self.add_command_button(right_controls_vbox, "Center on Right", PatternParameters.FEET_REALIGN_ON_RIGHT)
right_controls_vbox.addStretch()
controls_hbox.addLayout(right_controls_vbox, 1)
# end control box
add_layout_with_frame(vbox, controls_hbox, "Commands:")
# start settings
settings_hbox = QHBoxLayout()
settings_hbox.setMargin(0)
# start left column
left_settings_vbox = QVBoxLayout()
left_settings_vbox.setMargin(0)
# frame id
self.frame_id_line_edit = QLineEdit("/world")
add_widget_with_frame(left_settings_vbox, self.frame_id_line_edit, "Frame ID:")
# do closing step
self.close_step_checkbox = QCheckBox()
self.close_step_checkbox.setText("Do closing step")
self.close_step_checkbox.setChecked(True)
left_settings_vbox.addWidget(self.close_step_checkbox)
# extra seperation
self.extra_seperation_checkbox = QCheckBox()
self.extra_seperation_checkbox.setText("Extra Seperation")
self.extra_seperation_checkbox.setChecked(False)
left_settings_vbox.addWidget(self.extra_seperation_checkbox)
left_settings_vbox.addStretch()
# number of steps
self.step_number = generate_q_double_spin_box(1, 1, 50, 0, 1.0)
add_widget_with_frame(left_settings_vbox, self.step_number, "Number Steps:")
# start step index
self.start_step_index = generate_q_double_spin_box(0, 0, 1000, 0, 1.0)
add_widget_with_frame(left_settings_vbox, self.start_step_index, "Start Step Index:")
# end left column
settings_hbox.addLayout(left_settings_vbox, 1)
# start center column
center_settings_vbox = QVBoxLayout()
center_settings_vbox.setMargin(0)
# start foot selection
self.start_foot_selection_combo_box = QComboBox()
self.start_foot_selection_combo_box.addItem("AUTO")
self.start_foot_selection_combo_box.addItem("LEFT")
self.start_foot_selection_combo_box.addItem("RIGHT")
add_widget_with_frame(center_settings_vbox, self.start_foot_selection_combo_box, "Start foot selection:")
center_settings_vbox.addStretch()
# step Distance
self.step_distance = generate_q_double_spin_box(0.20, 0.0, 0.5, 2, 0.01)
add_widget_with_frame(center_settings_vbox, self.step_distance, "Step Distance (m):")
# side step distance
self.side_step = generate_q_double_spin_box(0.0, 0.0, 0.2, 2, 0.01)
add_widget_with_frame(center_settings_vbox, self.side_step, "Side Step (m):")
# rotation per step
self.step_rotation = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
add_widget_with_frame(center_settings_vbox, self.step_rotation, "Step Rotation (deg):")
# end center column
settings_hbox.addLayout(center_settings_vbox, 1)
# start right column
right_settings_vbox = QVBoxLayout()
right_settings_vbox.setMargin(0)
# roll
self.roll = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
add_widget_with_frame(right_settings_vbox, self.roll, "Roll (deg):")
# pitch
self.pitch = generate_q_double_spin_box(0.0, -30.0, 30.0, 0, 1.0)
add_widget_with_frame(right_settings_vbox, self.pitch, "Pitch (deg):")
# use terrain model
self.use_terrain_model_checkbox = QCheckBox()
self.use_terrain_model_checkbox.setText("Use Terrain Model")
self.use_terrain_model_checkbox.setChecked(False)
self.use_terrain_model_checkbox.stateChanged.connect(self.use_terrain_model_changed)
right_settings_vbox.addWidget(self.use_terrain_model_checkbox)
# override mode
self.override_checkbox = QCheckBox()
self.override_checkbox.setText("Override 3D")
self.override_checkbox.setChecked(False)
right_settings_vbox.addWidget(self.override_checkbox)
right_settings_vbox.addStretch()
# delta z
self.dz = generate_q_double_spin_box(0.0, -0.5, 0.5, 2, 0.01)
add_widget_with_frame(right_settings_vbox, self.dz, "delta z per step (m):")
# end right column
settings_hbox.addLayout(right_settings_vbox, 1)
# end settings
add_layout_with_frame(vbox, settings_hbox, "Settings:")
# parameter set selection
self.parameter_set_widget = QParameterSetWidget(logger = self.logger)
add_widget_with_frame(vbox, self.parameter_set_widget, "Parameter Set:")
# execute option
if add_execute_widget:
add_widget_with_frame(vbox, QExecuteStepPlanWidget(logger = self.logger, step_plan_topic = "step_plan"), "Execute:")
# add error status widget
add_widget_with_frame(vbox, error_status_widget, "Status:")
# end widget
widget.setLayout(vbox)
#context.add_widget(widget)
# init widget
self.parameter_set_widget.param_cleared_signal.connect(self.param_cleared)
self.parameter_set_widget.param_changed_signal.connect(self.param_selected)
self.commands_set_enabled(False)
# subscriber
self.start_feet_sub = rospy.Subscriber("set_start_feet", Feet, self.set_start_feet_callback)
# publisher
self.step_plan_pub = rospy.Publisher("step_plan", StepPlan, queue_size=1)
# action clients
self.step_plan_request_client = actionlib.SimpleActionClient("step_plan_request", StepPlanRequestAction)
class Top(Plugin):
NODE_FIELDS = [ 'pid', 'get_cpu_percent', 'get_memory_percent', 'get_num_threads']
OUT_FIELDS = ['node_name', 'pid', 'cpu_percent', 'memory_percent', 'num_threads' ]
FORMAT_STRS = ['%s', '%s', '%0.2f', '%0.2f', '%s' ]
NODE_LABELS = ['Node', 'PID', 'CPU %', 'Mem %', 'Num Threads' ]
SORT_TYPE = [str, str, float, float, float ]
TOOLTIPS = {
0: ('cmdline', lambda x: '\n'.join(textwrap.wrap(' '.join(x)))),
3: ('memory_info', lambda x: ('Resident: %0.2f MiB, Virtual: %0.2f MiB' % (x[0]/2**20, x[1]/2**20)))
}
_node_info = NodeInfo()
name_filter = re.compile('')
def __init__(self, context):
super(Top, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('Top')
# 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
self._selected_node = ''
self._selected_node_lock = RLock()
# Setup the toolbar
self._toolbar = QToolBar()
self._filter_box = QLineEdit()
self._regex_box = QCheckBox()
self._regex_box.setText('regex')
self._toolbar.addWidget(QLabel('Filter'))
self._toolbar.addWidget(self._filter_box)
self._toolbar.addWidget(self._regex_box)
self._filter_box.returnPressed.connect(self.update_filter)
self._regex_box.stateChanged.connect(self.update_filter)
# Create a container widget and give it a layout
self._container = QWidget()
self._container.setWindowTitle('Process Monitor')
self._layout = QVBoxLayout()
self._container.setLayout(self._layout)
self._layout.addWidget(self._toolbar)
# Create the table widget
self._table_widget = QTreeWidget()
self._table_widget.setObjectName('TopTable')
self._table_widget.setColumnCount(len(self.NODE_LABELS))
self._table_widget.setHeaderLabels(self.NODE_LABELS)
self._table_widget.itemClicked.connect(self._tableItemClicked)
self._table_widget.setSortingEnabled(True)
self._table_widget.setAlternatingRowColors(True)
self._layout.addWidget(self._table_widget)
context.add_widget(self._container)
# Add a button for killing nodes
self._kill_button = QPushButton('Kill Node')
self._layout.addWidget(self._kill_button)
self._kill_button.clicked.connect(self._kill_node)
# Update twice since the first cpu% lookup will always return 0
self.update_table()
self.update_table()
self._table_widget.resizeColumnToContents(0)
# Start a timer to trigger updates
self._update_timer = QTimer()
self._update_timer.setInterval(1000)
self._update_timer.timeout.connect(self.update_table)
self._update_timer.start()
def _tableItemClicked(self, item, column):
with self._selected_node_lock:
self._selected_node = item.text(0)
def update_filter(self, *args):
if self._regex_box.isChecked():
expr = self._filter_box.text()
else:
expr = re.escape(self._filter_box.text())
self.name_filter = re.compile(expr)
self.update_table()
def _kill_node(self):
self._node_info.kill_node(self._selected_node)
def update_one_item(self, row, info):
twi = TopWidgetItem()
for col, field in enumerate(self.OUT_FIELDS):
val = info[field]
twi.setText(col, self.FORMAT_STRS[col] % val)
self._table_widget.insertTopLevelItem(row, twi)
for col, (key, func) in self.TOOLTIPS.iteritems():
twi.setToolTip(col, func(info[key]))
with self._selected_node_lock:
if twi.text(0) == self._selected_node:
twi.setSelected(True)
self._table_widget.setItemHidden(twi, len(self.name_filter.findall(info['node_name'])) == 0)
def update_table(self):
self._table_widget.clear()
infos = self._node_info.get_all_node_fields(self.NODE_FIELDS)
for nx, info in enumerate(infos):
self.update_one_item(nx, info)
def shutdown_plugin(self):
self._update_timer.stop()
def save_settings(self, plugin_settings, instance_settings):
instance_settings.set_value('filter_text', self._filter_box.text())
instance_settings.set_value('is_regex', int(self._regex_box.checkState()))
def restore_settings(self, plugin_settings, instance_settings):
self._filter_box.setText(instance_settings.value('filter_text'))
is_regex_int = instance_settings.value('is_regex')
if is_regex_int:
self._regex_box.setCheckState(Qt.CheckState(is_regex_int))
else:
self._regex_box.setCheckState(Qt.CheckState(0))
self.update_filter()
def __init__(self, context):
super(JointControlWidget, self).__init__()
self.updateStateSignal.connect(self.on_update_state)
self.updateGhostSignal.connect(self.on_update_ghost)
self.joint_states = JointState()
self.ghost_joint_states = JointState()
self._widget = context
vbox = QVBoxLayout()
# Define checkboxes
radios = QWidget()
hbox_radio = QHBoxLayout()
self.radioGroup = QButtonGroup()
self.radioGroup.setExclusive(True)
self.radio_ghost_target = QRadioButton()
self.radio_ghost_target.setText("Ghost")
self.radioGroup.addButton(self.radio_ghost_target, 0)
self.radio_ghost_target.setChecked(True)
self.radio_robot_target = QRadioButton()
self.radio_robot_target.setText("Robot")
self.radioGroup.addButton(self.radio_robot_target, 1)
hbox_radio.addStretch()
hbox_radio.addWidget(self.radio_ghost_target)
hbox_radio.addStretch()
hbox_radio.addWidget(self.radio_robot_target)
hbox_radio.addStretch()
radios.setLayout(hbox_radio)
vbox.addWidget(radios)
duration_box = QHBoxLayout()
duration_box.setAlignment(Qt.AlignLeft)
duration_box.addWidget(QLabel("Trajectory duration (s):"))
self.traj_duration_spin = QDoubleSpinBox()
self.traj_duration_spin.setValue(1.0)
self.traj_duration_spin.valueChanged.connect(self.on_traj_duration_changed)
duration_box.addWidget(self.traj_duration_spin)
self.update_controllers_buttonn = QPushButton("Update Controllers")
self.update_controllers_buttonn.pressed.connect(self.on_update_controllers)
duration_box.addWidget(self.update_controllers_buttonn)
vbox.addLayout(duration_box)
widget = QWidget()
hbox = QHBoxLayout()
# Left to right layout
self.joint_control = JointControl(self, hbox)
widget.setLayout(hbox)
vbox.addWidget(widget)
print "Add buttons to apply all ..."
all_widget = QWidget()
all_box = QHBoxLayout()
self.snap_to_ghost_button = QPushButton("SnapAllGhost")
self.snap_to_ghost_button.pressed.connect(self.on_snap_ghost_pressed)
all_box.addWidget(self.snap_to_ghost_button)
self.snap_to_current_button = QPushButton("SnapAllCurrent")
self.snap_to_current_button.pressed.connect(self.on_snap_current_pressed)
all_box.addWidget(self.snap_to_current_button)
self.apply_to_robot_button = QPushButton("ApplyAllRobot")
self.apply_to_robot_button.pressed.connect(self.on_apply_robot_pressed)
all_box.addWidget(self.apply_to_robot_button)
self.apply_to_robot_button = QPushButton("Apply WBC Robot")
self.apply_to_robot_button.pressed.connect(self.on_apply_wbc_robot_pressed)
all_box.addWidget(self.apply_to_robot_button)
all_widget.setLayout(all_box)
vbox.addWidget(all_widget)
override_box = QHBoxLayout()
self.override = QCheckBox()
self.override.setChecked(False)
self.override.stateChanged.connect(self.on_override_changed)
override_box.addWidget(self.override)
override_label = QLabel("SAFETY OVERRIDE")
override_label.setStyleSheet('QLabel { color: red }')
override_box.addWidget(override_label)
override_box.addStretch()
vbox.addLayout(override_box)
vbox.addStretch()
self._widget.setLayout(vbox)
self.first_time = True
self.stateSubscriber = rospy.Subscriber('/joint_states', JointState, self.state_callback_fnc)
self.ghostSubscriber = rospy.Subscriber('/flor/ghost/get_joint_states', JointState, self.ghost_callback_fnc)
self.wbc_robot_pub = rospy.Publisher('/flor/wbc_controller/joint_states', JointState, queue_size=10)
self.time_last_update_state = time.time()
self.time_last_update_ghost = time.time()
def createEditor(self, parent, option, index):
'''
Creates a editor in the TreeView depending on type of the settings data.
'''
item = self._itemFromIndex(index)
if item.edit_type() == SettingsValueItem.EDIT_TYPE_AUTODETECT:
if isinstance(item.value(), bool):
box = QCheckBox(parent)
box.setFocusPolicy(Qt.StrongFocus)
box.setAutoFillBackground(True)
box.stateChanged.connect(self.edit_finished)
return box
elif isinstance(item.value(), int):
box = QSpinBox(parent)
box.setValue(item.value())
if not item.value_min() is None:
box.setMinimum(item.value_min())
if not item.value_max() is None:
box.setMaximum(item.value_max())
if not item.value_step() is None:
box.setSingleStep(item.value_step())
return box
elif isinstance(item.value(), float):
box = QDoubleSpinBox(parent)
box.setValue(item.value())
if not item.value_min() is None:
box.setMinimum(item.value_min())
if not item.value_max() is None:
box.setMaximum(item.value_max())
if not item.value_step() is None:
box.setSingleStep(item.value_step())
box.setDecimals(3)
return box
elif item.edit_type() == SettingsValueItem.EDIT_TYPE_FOLDER:
editor = PathEditor(item.value(), parent)
editor.editing_finished_signal.connect(self.edit_finished)
return editor
elif item.edit_type() == SettingsValueItem.EDIT_TYPE_LIST:
box = QComboBox(parent)
box.addItems(item.value_list())
index = box.findText(item.value())
if index >= 0:
box.setCurrentIndex(index)
box.setEditable(False)
return box
return QStyledItemDelegate.createEditor(self, parent, option, index)
def addCheckBox(self, topic):
self.topic_list.append(topic)
item = QCheckBox(topic, self)
item.stateChanged.connect(lambda x: self.updateList(x,topic))
self.items_list.append(item)
self.selection_vlayout.addWidget(item)
class TextSearchFrame(QDockWidget):
'''
A frame to find text in the Editor.
'''
search_result_signal = Signal(str, bool, str, int)
''' @ivar: A signal emitted after search_threaded was started.
(search text, found or not, file, position in text)
for each result a signal will be emitted.
'''
replace_signal = Signal(str, str, int, str)
''' @ivar: A signal emitted to replace string at given position.
(search text, file, position in text, replaced by text)
'''
def __init__(self, tabwidget, parent=None):
QDockWidget.__init__(self, "Find", parent)
self.setObjectName('SearchFrame')
self.setFeatures(QDockWidget.DockWidgetMovable
| QDockWidget.DockWidgetFloatable)
self._dockwidget = QFrame(self)
self.vbox_layout = QVBoxLayout(self._dockwidget)
self.layout().setContentsMargins(0, 0, 0, 0)
self.layout().setSpacing(1)
# frame with two rows for find and replace
find_replace_frame = QFrame(self)
find_replace_vbox_layout = QVBoxLayout(find_replace_frame)
find_replace_vbox_layout.setContentsMargins(0, 0, 0, 0)
find_replace_vbox_layout.setSpacing(1)
# find_replace_vbox_layout.addSpacerItem(QSpacerItem(1, 1, QSizePolicy.Expanding, QSizePolicy.Expanding))
# create frame with find row
find_frame = self._create_find_frame()
find_replace_vbox_layout.addWidget(find_frame)
rplc_frame = self._create_replace_frame()
find_replace_vbox_layout.addWidget(rplc_frame)
# frame for find&replace and search results
self.vbox_layout.addWidget(find_replace_frame)
self.vbox_layout.addWidget(self._create_found_frame())
# self.vbox_layout.addStretch(2024)
self.setWidget(self._dockwidget)
# intern search parameters
self._tabwidget = tabwidget
self.current_search_text = ''
self.search_results = []
self.search_results_fileset = set()
self._search_result_index = -1
self._search_recursive = False
self._search_thread = None
def _create_find_frame(self):
find_frame = QFrame(self)
find_hbox_layout = QHBoxLayout(find_frame)
find_hbox_layout.setContentsMargins(0, 0, 0, 0)
find_hbox_layout.setSpacing(1)
self.search_field = EnchancedLineEdit(find_frame)
self.search_field.setPlaceholderText('search text')
self.search_field.textChanged.connect(self.on_search_text_changed)
self.search_field.returnPressed.connect(self.on_search)
find_hbox_layout.addWidget(self.search_field)
self.search_result_label = QLabel(find_frame)
self.search_result_label.setText(' ')
find_hbox_layout.addWidget(self.search_result_label)
self.find_button_back = QPushButton("<")
self.find_button_back.setFixedWidth(44)
self.find_button_back.clicked.connect(self.on_search_back)
find_hbox_layout.addWidget(self.find_button_back)
self.find_button = QPushButton(">")
self.find_button.setDefault(True)
# self.find_button.setFlat(True)
self.find_button.setFixedWidth(44)
self.find_button.clicked.connect(self.on_search)
find_hbox_layout.addWidget(self.find_button)
return find_frame
def _create_replace_frame(self):
# create frame with replace row
self.rplc_frame = rplc_frame = QFrame(self)
rplc_hbox_layout = QHBoxLayout(rplc_frame)
rplc_hbox_layout.setContentsMargins(0, 0, 0, 0)
rplc_hbox_layout.setSpacing(1)
self.replace_field = EnchancedLineEdit(rplc_frame)
self.replace_field.setPlaceholderText('replace text')
self.replace_field.returnPressed.connect(self.on_replace)
rplc_hbox_layout.addWidget(self.replace_field)
self.replace_result_label = QLabel(rplc_frame)
self.replace_result_label.setText(' ')
rplc_hbox_layout.addWidget(self.replace_result_label)
self.replace_button = replace_button = QPushButton("> &Replace >")
replace_button.setFixedWidth(90)
replace_button.clicked.connect(self.on_replace_click)
rplc_hbox_layout.addWidget(replace_button)
rplc_frame.setVisible(False)
return rplc_frame
def _create_found_frame(self):
ff_frame = QFrame(self)
self.found_files_vbox_layout = QVBoxLayout(ff_frame)
self.found_files_vbox_layout.setContentsMargins(0, 0, 0, 0)
self.recursive_search_box = QCheckBox("recursive search")
self.found_files_vbox_layout.addWidget(self.recursive_search_box)
self.found_files_list = QTreeWidget(ff_frame)
self.found_files_list.setColumnCount(1)
self.found_files_list.setFrameStyle(QFrame.StyledPanel)
self.found_files_list.setHeaderHidden(True)
self.found_files_list.itemActivated.connect(self.on_itemActivated)
self.found_files_list.setStyleSheet("QTreeWidget {"
"background-color:transparent;"
"}"
"QTreeWidget::item {"
"background-color:transparent;"
"}"
"QTreeWidget::item:selected {"
"background-color: darkgray;"
"}")
self.found_files_vbox_layout.addWidget(self.found_files_list)
self.recursive_search_box.setChecked(False)
return ff_frame
def keyPressEvent(self, event):
'''
Enable the shortcats for search and replace
'''
self.parent().keyPressEvent(event)
def on_search(self):
'''
Initiate the new search or request a next search result.
'''
if self.current_search_text != self.search_field.text(
) or self._search_recursive != self.recursive_search_box.isChecked():
# clear current search results
self._reset()
self.current_search_text = self.search_field.text()
if self.current_search_text:
path_text = {}
self._wait_for_result = True
for i in range(self._tabwidget.count()):
path_text[self._tabwidget.widget(
i).filename] = self._tabwidget.widget(
i).document().toPlainText()
self._search_recursive = self.recursive_search_box.isChecked()
self._search_thread = TextSearchThread(
self.current_search_text,
self._tabwidget.currentWidget().filename,
path_text=path_text,
recursive=self._search_recursive)
self._search_thread.search_result_signal.connect(
self.on_search_result)
self._search_thread.warning_signal.connect(
self.on_warning_result)
self._search_thread.start()
elif self.search_results:
self._check_position()
if self.search_results:
if self._search_result_index + 1 >= len(self.search_results):
self._search_result_index = -1
self._search_result_index += 1
(id, search_text, found, path, index, linenr,
line) = self.search_results[self._search_result_index]
self.search_result_signal.emit(search_text, found, path, index)
self.replace_button.setEnabled(True)
self._update_label()
def on_search_back(self):
'''
Slot to handle the search back function.
'''
self._check_position(False)
if self.search_results:
self._search_result_index -= 1
if self._search_result_index < 0:
self._search_result_index = len(self.search_results) - 1
self._update_label()
(id, search_text, found, path, index, linenr,
line) = self.search_results[self._search_result_index]
self.search_result_signal.emit(search_text, found, path, index)
self.replace_button.setEnabled(True)
def _check_position(self, forward=True):
try:
# if the position of the textCursor was changed by the user, move the search index
cur_pos = self._tabwidget.currentWidget().textCursor().position()
id, st, _f, pa, idx, lnr, ltxt = self.search_results[
self._search_result_index]
sear_pos = idx + len(st)
if cur_pos != sear_pos:
first_idx = self._get_current_index_for_current_file()
if first_idx != -1:
id, st, _f, pa, idx, lnr, ltxt = self.search_results[
first_idx]
sear_pos = idx + len(st)
while cur_pos > sear_pos and self._tabwidget.currentWidget(
).filename == pa:
first_idx += 1
id, st, _f, pa, idx, lnr, ltxt = self.search_results[
first_idx]
sear_pos = idx + len(st)
self._search_result_index = first_idx
if forward:
self._search_result_index -= 1
else:
self._reset(True)
except:
pass
def _get_current_index_for_current_file(self):
for index in range(len(self.search_results)):
id, _st, _f, pa, _idx = self.search_results[index]
if self._tabwidget.currentWidget().filename == pa:
return index
return -1
def on_search_result(self, search_text, found, path, index, linenr, line):
'''
Slot to handle the signals for search result. This signals are forwarded used
search_result_signal.
'''
if found and search_text == self.current_search_text:
id = "%d:%s" % (index, path)
self.search_results_fileset.add(path)
item = (search_text, found, path, index)
if item not in self.search_results:
self.search_results.append(
(id, search_text, found, path, index, linenr, line))
if self._wait_for_result:
self._search_result_index += 1
if index >= self._tabwidget.currentWidget().textCursor(
).position() or self._tabwidget.currentWidget(
).filename != path:
self._wait_for_result = False
self.search_result_signal.emit(search_text, found, path,
index)
self.replace_button.setEnabled(True)
pkg, rpath = package_name(os.path.dirname(path))
itemstr = '%s [%s]' % (os.path.basename(path), pkg)
if not self.found_files_list.findItems(itemstr, Qt.MatchExactly):
list_item = QTreeWidgetItem(self.found_files_list)
list_item.setText(0, itemstr)
list_item.setToolTip(0, path)
self.found_files_list.insertTopLevelItem(0, list_item)
self.found_files_list.expandAll()
for i in range(self.found_files_list.topLevelItemCount()):
top_item = self.found_files_list.topLevelItem(i)
if top_item.text(0) == itemstr:
sub_item_str = "%d: %s" % (linenr, line)
list_item2 = QTreeWidgetItem()
list_item2.setText(0, sub_item_str)
list_item2.setWhatsThis(0, id)
top_item.addChild(list_item2)
#self.found_files_list.setVisible(len(self.search_results_fileset) > 0)
self._update_label()
def on_warning_result(self, text):
rospy.logwarn(text)
def on_replace_click(self):
self.on_replace()
self.on_search()
def on_replace(self):
'''
Emits the replace signal, but only if currently selected text is equal to the searched one.
'''
if self.search_results:
try:
id, search_text, _found, path, index, linenr, line_text = self.search_results[
self._search_result_index]
cursor = self._tabwidget.currentWidget().textCursor()
if cursor.selectedText() == search_text:
rptxt = self.replace_field.text()
for rindex in range(self._search_result_index + 1,
len(self.search_results)):
iid, st, _f, pa, idx, lnr, ltxt = self.search_results[
rindex]
if path == pa:
self.search_results.pop(rindex)
self.search_results.insert(
rindex,
(iid, st, _f, pa, idx + len(rptxt) - len(st),
lnr, ltxt))
else:
break
self._remove_search_result(self._search_result_index)
self._search_result_index -= 1
self.replace_signal.emit(search_text, path, index, rptxt)
else:
self.replace_button.setEnabled(False)
except:
import traceback
print traceback.format_exc()
pass
def on_itemActivated(self, item):
'''
Go to the results for the selected file entry in the list.
'''
splits = item.whatsThis(0).split(':')
if len(splits) == 2:
item_index = int(splits[0])
item_path = splits[1]
new_search_index = -1
tmp_index = -1
search_index = -1
tmp_search_text = ''
for id, search_text, found, path, index, linenr, line_text in self.search_results:
new_search_index += 1
if item_path == path and item_index == index:
self._search_result_index = new_search_index
self.search_result_signal.emit(search_text, found, path,
index)
self._update_label()
def on_search_text_changed(self, _text):
'''
Clear search result if the text was changed.
'''
self._reset()
def _update_label(self, clear_label=False):
'''
Updates the status label for search results. The info is created from search result lists.
'''
msg = ' '
if self.search_results:
count_files = len(self.search_results_fileset)
msg = '%d/%d' % (self._search_result_index + 1,
len(self.search_results))
if count_files > 1:
msg = '%s(%d)' % (msg, count_files)
if self._search_thread is not None and self._search_thread.is_alive():
msg = 'searching..%s' % msg
elif not msg.strip() and self.current_search_text:
msg = '0 found'
self.current_search_text = ''
if clear_label:
msg = ' '
self.search_result_label.setText(msg)
self.find_button_back.setEnabled(len(self.search_results))
self._select_current_item_in_box(self._search_result_index)
def file_changed(self, path):
'''
Clears search results if for changed file are some search results are available
:param path: changed file path
:type path: str
'''
if path in self.search_results_fileset:
self._reset()
def set_replace_visible(self, value):
self.rplc_frame.setVisible(value)
self.raise_()
self.activateWindow()
if value:
self.replace_field.setFocus()
self.replace_field.selectAll()
self.setWindowTitle("Find / Replace")
else:
self.setWindowTitle("Find")
self.search_field.setFocus()
def is_replace_visible(self):
return self.rplc_frame.isVisible()
def _reset(self, force_new_search=False):
# clear current search results
if self._search_thread is not None:
self._search_thread.search_result_signal.disconnect()
self._search_thread.stop()
self._search_thread = None
self.current_search_text = ''
self.search_results = []
self.search_results_fileset = set()
self.found_files_list.clear()
# self.found_files_list.setVisible(False)
self._update_label(True)
self._search_result_index = -1
self.find_button_back.setEnabled(False)
if force_new_search:
self.on_search()
def enable(self):
self.setVisible(True)
# self.show()
self.raise_()
self.activateWindow()
self.search_field.setFocus()
self.search_field.selectAll()
def _select_current_item_in_box(self, index):
try:
(id, search_text, found, path, index, linenr,
line) = self.search_results[index]
for topidx in range(self.found_files_list.topLevelItemCount()):
topitem = self.found_files_list.topLevelItem(topidx)
for childdx in range(topitem.childCount()):
child = topitem.child(childdx)
if child.whatsThis(0) == id:
child.setSelected(True)
elif child.isSelected():
child.setSelected(False)
except:
pass
def _remove_search_result(self, index):
try:
(id, search_text, found, path, index, linenr,
line) = self.search_results.pop(index)
pkg, rpath = package_name(os.path.dirname(path))
itemstr = '%s [%s]' % (os.path.basename(path), pkg)
found_items = self.found_files_list.findItems(
itemstr, Qt.MatchExactly)
for item in found_items:
for chi in range(item.childCount()):
child = item.child(chi)
if child.whatsThis(0) == id:
item.removeChild(child)
break
# delete top level item if it is now empty
for topidx in range(self.found_files_list.topLevelItemCount()):
if self.found_files_list.topLevelItem(
topidx).childCount() == 0:
self.found_files_list.takeTopLevelItem(topidx)
break
# create new set with files contain the search text
new_path_set = set(path for _id, _st, _fd, path, _idx, lnr, lntxt
in self.search_results)
self.search_results_fileset = new_path_set
# self.found_files_list.setVisible(len(self.search_results_fileset) > 0)
except:
import traceback
print traceback.format_exc()
class ControlModeWidget:
def __init__(self, context):
self.control_mode = 0
self.mode_pub = rospy.Publisher('/flor/controller/mode_command', VigirControlModeCommand, queue_size=10)
self._widget = context
self.vbox = QVBoxLayout()
#Add input for setting the spindle speed
list_label = QLabel("Select Control Mode")
self.vbox.addWidget(list_label)
# Indexed list of allowed control modes from feedback
self.allowed_modes = rospy.get_param("/atlas_controller/allowed_control_modes")
self.allowed_modes=rospy.get_param("/atlas_controller/allowed_control_modes")
self.mode_ids={}
self.mode_ids
for ndx,txt in enumerate(self.allowed_modes):
self.mode_ids[txt] = ndx
# API 2.13 ordering
self.bdi_mode_names=['NONE ', 'FREEZE ', 'STAND_PREP ', \
'STAND ', 'WALK ', 'STEP ', 'MANIPULATE ', \
'USER ', 'CALIBRATE ']
self.list_box = QListWidget(None)
self.list_box.addItems(self.allowed_modes)
self.list_box.currentItemChanged.connect(self.handle_selection_change)
self.vbox.addWidget(self.list_box)
self.selection_label = QLabel("Flor Selected: "+self.allowed_modes[0]+"("+str(self.control_mode)+")")
self.vbox.addWidget(self.selection_label)
self.label = QLabel("Flor Command : "+self.allowed_modes[0]+"("+str(self.control_mode)+")")
self.vbox.addWidget(self.label)
self.flor_mode = 0
self.bdi_current_mode = 0
self.bdi_desired_mode = 0
self.flor_mode_label = QLabel("Flor Mode : "+self.allowed_modes[self.flor_mode]+"("+str(self.flor_mode)+")"+" BDI:("+str(self.bdi_current_mode)+", "+str(self.bdi_desired_mode)+")")
self.vbox.addWidget(self.flor_mode_label)
#Add combo box for available settings
self.available_modes = QComboBox();
self.available_modes.addItem("");
self.available_modes.addItem("BDI");
self.available_modes.addItem("Enable Upper Body");
self.available_modes.addItem("Enable Whole Body");
self.available_modes.currentIndexChanged.connect(self.handle_avail_modes_changed)
self.vbox.addWidget(self.available_modes);
self.vbox.addStretch(1)
#Add Button for sending the behavior mode command
self.push_button = QPushButton("Set Mode")
self.push_button.clicked.connect(self.handle_set_mode)
self.vbox.addWidget(self.push_button)
self.vbox.addStretch(1)
hbox = QHBoxLayout()
hbox.addStretch(1)
self.stop_enable= QCheckBox()
self.stop_enable.setChecked(False)
hbox.addWidget(self.stop_enable)
self.stop_enable.clicked.connect(self.handle_stop_enable)
self.stop_button = QPushButton("STOP!")
self.stop_button.clicked.connect(self.handle_stop)
self.stop_button.setStyleSheet('QPushButton {background-color: gray }')
hbox.addWidget(self.stop_button)
hbox.addStretch(1)
self.vbox.addLayout(hbox)
self._widget.setLayout(self.vbox)
#add stretch at end so all GUI elements are at top of dialog
self.vbox.addStretch(1)
self.flor_mode_cmd_sub = rospy.Subscriber("/flor/controller/mode", FlorControlMode, self.florModeCallback)
def shutdown_plugin(self):
print "Shutting down ..."
self.flor_mode_cmd_sub.unregister()
self.mode_pub.unregister()
print "Done!"
# Update BDI state
def simStateCallback(self, state):
if ((self.bdi_current_state != state.current_behavior) or (self.bdi_desired_state != state.desired_behavior) or (self.bdi_error_code != state.error_code) or (self.bdi_behavior_status != state.behavior_feedback.status_flags) ):
self.bdi_current_state = state.current_behavior
self.bdi_desired_state = state.desired_behavior
self.bdi_error_code = state.error_code
self.bdi_behavior_status = state.behavior_feedback.status_flags
if ((self.bdi_current_state < 0) or (self.bdi_current_state >= length(self.bdi_mode_names))):
self.bdi_state_label.setText( " BDI State : "+self.bdi_mode_names[0]+"("+str(self.bdi_current_state)+", " + str(self.bdi_desired_state) + ") EC:("+str(self.bdi_error_code)+", " + str(self.bdi_behavior_status) +")")
else:
self.bdi_state_label.setText( " BDI State : "+self.bdi_mode_names[self.bdi_current_state]+"("+str(self.bdi_current_state)+", " + str(self.bdi_desired_state) + ") EC:("+str(self.bdi_error_code)+", " + str(self.bdi_behavior_status) +")")
def simCommandCallback(self, bdi_cmd):
if (self.bdi_behavior != bdi_cmd.behavior) :
self.bdi_behavior = bdi_cmd.behavior
self.bdi_command_label.setText(" BDI Cmd : "+self.bdi_mode_names[self.bdi_behavior]+"("+str(self.bdi_behavior)+")")
def florModeCallback(self, cmd):
if ( (self.flor_mode != cmd.control_mode) or (self.bdi_current_mode != cmd.bdi_current_behavior) or (self.bdi_desired_mode != cmd.bdi_desired_behavior) ):
if (self.flor_mode != cmd.control_mode) and (self.control_mode != cmd.control_mode):
print "Flor mode changed externally - clear selection"
self.clearSelections()
self.selection_label.setText("Flor Selected : ")
self.label.setText("Flor Command : ")
self.flor_mode = cmd.control_mode
self.bdi_current_mode = cmd.bdi_current_behavior
self.bdi_desired_mode = cmd.bdi_desired_behavior
print "Flor mode: ", self.flor_mode, " BDI: ",self.bdi_current_mode,", ",self.bdi_desired_mode
if (self.flor_mode > 250):
print "Invalid control mode "
self.flor_mode = 0
#print "Allowed modes:"
#print self.allowed_modes
self.flor_mode_label.setText(" Flor Mode : "+self.allowed_modes[self.flor_mode]+"("+str(self.flor_mode)+") BDI:("+str(self.bdi_current_mode)+","+str(self.bdi_desired_mode)+")")
def clearSelections(self):
for i in range(self.list_box.count()):
item = self.list_box.item(i)
self.list_box.setItemSelected(item, False)
self.list_box.setCurrentRow(-1)
#Slot for selecting
def handle_selection_change(self, curr, prev):
if (curr != None):
self.control_mode = self.mode_ids[curr.text()]
self.selection_label.setText("Flor Selected : "+curr.text()+"("+str(self.control_mode)+")")
#else:
# print "NULL selection"
#self.label.setText(self.allowed_modes[selected])
#self.spindle_speed_pub.publish(data=math.radians(degree_per_sec))
@Slot(bool)
def handle_set_mode(self):
print "Selected=",self.allowed_modes[self.control_mode], ' id=', self.control_mode
self.label.setText("Flor Command : "+self.allowed_modes[self.control_mode]+"("+str(self.control_mode)+")")
mode_msg = VigirControlModeCommand()
mode_msg.header.stamp = rospy.Time.now()
mode_msg.requested_control_mode = self.control_mode
print mode_msg
self.mode_pub.publish(mode_msg)
@Slot(bool)
def handle_stop(self):
# FLOR_STOP command
if (self.stop_enable.isChecked()):
self.clearSelections()
self.selection_label.setText("Flor Selected : ")
self.control_mode = self.mode_ids['stop']
print "Selected=",self.allowed_modes[self.control_mode], ' id=', self.control_mode
#self.list_box
self.selection_label.setText("Flor Selected : "+self.allowed_modes[self.control_mode]+"("+str(self.control_mode)+")")
self.label.setText("Flor Command : "+self.allowed_modes[self.control_mode]+"("+str(self.control_mode)+")")
mode_msg = VigirControlModeCommand()
mode_msg.header.stamp = rospy.Time.now()
mode_msg.requested_control_mode = self.control_mode
print mode_msg
self.mode_pub.publish(mode_msg)
else:
print "Stop disabled!"
@Slot(bool)
def handle_stop_enable(self):
if (self.stop_enable.isChecked()):
self.stop_button.setStyleSheet('QPushButton {background-color: red }')
#self.stop_enable.setChecked(False)
else:
self.stop_button.setStyleSheet('QPushButton {background-color: gray }')
#self.stop_enable.setChecked(True)
@Slot(bool)
def handle_avail_modes_changed(self, index):
print 'no longer mapping allowable modes'
return
