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 _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 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
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 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)
class FootstepParamControlWidget(QObject): 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 shutdown_plugin(self): print "Shutdown ..." self.param_pub.unregister() print "Done!" def getParams( self ): msg = FootstepPlannerParams() msg.change_mask = 0 self.param_pub.publish( msg ) def sendParams( self ): msg = FootstepPlannerParams() msg.change_mask = 0 if self.step_cost_groupbox.isChecked(): msg.change_mask = msg.change_mask | 0x001 if self.collision_check_groupbox.isChecked(): msg.change_mask = msg.change_mask | 0x010 if self.foot_size_groupbox.isChecked(): msg.change_mask = msg.change_mask | 0x020 if self.upper_groupbox.isChecked(): msg.change_mask = msg.change_mask | 0x040 if self.std_step_groupbox.isChecked(): msg.change_mask = msg.change_mask | 0x080 if self.terrain_model_groupbox.isChecked(): msg.change_mask = msg.change_mask | 0x100 ### STEP_COST_ESTIMATOR ######## msg.step_cost_type = self.step_cost_combobox.currentIndex() # #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 ######## msg.collision_check_type = 0 if self.collision_check_feet_checkbox.isChecked(): msg.collision_check_type = msg.collision_check_type | 0x01 if self.collision_check_ub_checkbox.isChecked(): msg.collision_check_type = msg.collision_check_type | 0x02 if self.collision_check_fc_checkbox.isChecked(): msg.collision_check_type = msg.collision_check_type | 0x04 ### FOOT_SIZE ######## msg.foot_size.x = self.foot_size_x.value() msg.foot_size.y = self.foot_size_y.value() msg.foot_size.z = self.foot_size_z.value() msg.foot_origin_shift.x = self.foot_shift_x.value() msg.foot_origin_shift.y = self.foot_shift_y.value() msg.foot_origin_shift.z = self.foot_shift_z.value() ### UPPER_BODY_SIZE ######## msg.upper_body_size.x = self.upper_size_x.value() msg.upper_body_size.y = self.upper_size_y.value() msg.upper_body_size.z = self.upper_size_z.value() msg.upper_body_origin_shift.x = self.upper_origin_x.value() msg.upper_body_origin_shift.y = self.upper_origin_y.value() msg.upper_body_origin_shift.z = self.upper_origin_z.value() ### TERRAIN_MODEL ######## msg.use_terrain_model = self.terrain_model_checkbox.isChecked() msg.min_sampling_steps_x = self.terrain_min_ssx_val.value() msg.min_sampling_steps_y = self.terrain_min_ssy_val.value() msg.max_sampling_steps_x = self.terrain_max_ssx_val.value() msg.max_sampling_steps_y = self.terrain_max_ssy_val.value() msg.max_intrusion_z = self.terrain_max_iz_val.value() msg.max_ground_clearance = self.terrain_max_gc_val.value() msg.minimal_support = self.terrain_ms_val.value() ### STANDARD_STEP_PARAMS ######## msg.foot_seperation = self.foot_sep_val.value() msg.step_duration = self.std_step_step_duration_val.value() msg.sway_duration = self.std_step_sway_duration_val.value() msg.swing_height = self.std_step_swing_height_val.value() msg.lift_height = self.std_step_lift_height_val.value() print msg self.param_pub.publish( msg ) def getParamCallbackFcn(self, msg): ### STEP_COST_ESTIMATOR ######## if msg.change_mask & 0x001: self.step_cost_combobox.setCurrentIndex( msg.step_cost_type ) # #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 ######## if msg.change_mask & 0x010: self.collision_check_feet_checkbox.setChecked( msg.collision_check_type & 0x01 ) self.collision_check_ub_checkbox.setChecked( msg.collision_check_type & 0x02 ) self.collision_check_fc_checkbox.setChecked( msg.collision_check_type & 0x04 ) ### FOOT_SIZE ######## if msg.change_mask & 0x020: self.foot_size_x.setValue( msg.foot_size.x ) self.foot_size_y.setValue( msg.foot_size.y ) self.foot_size_z.setValue( msg.foot_size.z ) self.foot_shift_x.setValue( msg.foot_origin_shift.x ) self.foot_shift_y.setValue( msg.foot_origin_shift.y ) self.foot_shift_z.setValue( msg.foot_origin_shift.z ) ### UPPER_BODY_SIZE ######## if msg.change_mask & 0x040: self.upper_size_x.setValue( msg.upper_body_size.x ) self.upper_size_y.setValue( msg.upper_body_size.y ) self.upper_size_z.setValue( msg.upper_body_size.z ) self.upper_origin_x.setValue( msg.upper_body_origin_shift.x ) self.upper_origin_y.setValue( msg.upper_body_origin_shift.y ) self.upper_origin_z.setValue( msg.upper_body_origin_shift.z ) ### STANDARD_STEP_PARAMS ######## if msg.change_mask & 0x080: self.foot_sep_val.setValue( msg.foot_seperation ) self.std_step_step_duration_val.setValue( msg.step_duration ) self.std_step_sway_duration_val.setValue( msg.sway_duration ) self.std_step_swing_height_val.setValue( msg.swing_height ) self.std_step_lift_height_val.setValue( msg.lift_height ) ### TERRAIN_MODEL ######## if msg.change_mask & 0x100: self.terrain_model_checkbox.setChecked( msg.use_terrain_model ) self.terrain_min_ssx_val.setValue( msg.min_sampling_steps_x ) self.terrain_min_ssy_val.setValue( msg.min_sampling_steps_y ) self.terrain_max_ssx_val.setValue( msg.max_sampling_steps_x ) self.terrain_max_ssy_val.setValue( msg.max_sampling_steps_y ) self.terrain_max_iz_val.setValue( msg.max_intrusion_z ) self.terrain_max_gc_val.setValue( msg.max_ground_clearance ) self.terrain_ms_val.setValue( msg.minimal_support )
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 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()
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 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 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()
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()
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