Beispiel #1
0
    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()
Beispiel #3
0
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
Beispiel #4
0
    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']))
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()
Beispiel #6
0
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 )
Beispiel #8
0
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