コード例 #1
0
    def motor_controller(self):
        '''
        Sets up the GUI in the middle of the Screen to control the motors. 
        Uses self._motorValues to determine which motors are present.
        '''
        i = 0
        for k, v in sorted(self._currentGoals.items()):
            group = QGroupBox()
            slider = QSlider(Qt.Horizontal)
            slider.setTickInterval(1)
            slider.setMinimum(-181)
            slider.setMaximum(181)
            slider.valueChanged.connect(self.slider_update)
            self._sliders[k] = slider

            textfield = QLineEdit()
            textfield.setText('0')
            textfield.textEdited.connect(self.textfield_update)
            self._textFields[k] = textfield

            label = QLabel()
            label.setText(k)

            layout = QVBoxLayout()
            layout.addWidget(label)
            layout.addWidget(self._sliders[k])
            layout.addWidget(self._textFields[k])
            group.setLayout(layout)
            self._widget.motorControlLayout.addWidget(group, i / 5, i % 5)
            i = i+1
コード例 #2
0
class QualityDialog(QDialog):
    def __init__(self, bagFiles, parent=None):
        super(QualityDialog, self).__init__()
        self.parent = parent
        self.bagFiles = bagFiles
        self.setWindowTitle("Data Quality")

        ## Widgets:
        self.spinBox = QDoubleSpinBox()
        self.spinBox.setDecimals(2)
        self.spinBox.setMaximum(1.0)
        self.spinBox.setSingleStep(0.1)
        self.spinBox.setValue(0.5)

        self.fppiLbl = QLabel("FPPI = ")

        self.motaLbl = QLabel("MOTA = ")

        self.motpLbl = QLabel("MOTP = ")

        self.recalcBtn = QPushButton("Recalculate Quality")
        self.recalcBtn.clicked.connect(self.calculate)

        ## Layout:
        layout = QVBoxLayout()
        layout.addWidget(self.spinBox)
        layout.addWidget(self.fppiLbl)
        layout.addWidget(self.motaLbl)
        layout.addWidget(self.motpLbl)
        layout.addWidget(self.recalcBtn)

        self.setLayout(layout)

        self.resize(300, 200)

        # calculate the data quality:
        self.calculate()

    def calculate(self):

        if self.bagFiles[0] == "" or self.bagFiles[1] == "":
            message_module.showMessage(
                "Bag file missing! Please import bag file in the main interface."
            )
            return

        threshold = self.spinBox.value()

        fppi = Rosbag_Analysis.getFPPI(self.bagFiles[0], self.bagFiles[1],
                                       threshold)
        self.fppiLbl.setText("FPPI = " + str(fppi))

        mota = Rosbag_Analysis.getMOTA(self.bagFiles[0], self.bagFiles[1],
                                       threshold)
        self.motaLbl.setText("MOTA = " + str(mota))

        motp = Rosbag_Analysis.getMOTP(self.bagFiles[0], self.bagFiles[1],
                                       threshold)
        self.motpLbl.setText("MOTP = " + str(motp))
コード例 #3
0
class HistorySelection(QWidget):
    def __init__(self):
        super(HistorySelection, self).__init__()
        self.setWindowTitle("Extracting Window Features")
        self.resize(550, 440)

        per_title, pre = TS.get_time_series_pre_feature_options()
        glob_title, glob = TS.get_global_time_series_features_options()

        self.history_items = dict()
        self.history_items[per_title] = pre
        self.history_items[glob_title] = glob

        self.saved_dir = []

        self.group_selected_items = dict()
        self.group_areas = dict()
        self.group_main_widget = dict()
        self.group_selection_vlayout = dict()
        self.group_item_all = dict()
        self.main_vlayout = QVBoxLayout(self)
        self.group_label = dict()

        self.items_list = []
        self.selected_topics = []
        self.files = []

        # layout = QFormLayout()

        self.select_path = QLineEdit()
        self.save_path = QLineEdit()

        self.select_path.setEnabled(False)
        self.save_path.setEnabled(False)

        self.ok_button = QPushButton("Select CSV...", self)
        self.ok_button.clicked.connect(self.onButtonClicked)

        self.two_buttons1 = QHBoxLayout(self)

        self.two_buttons1.addWidget(self.ok_button)

        self.two_buttons1.addWidget(self.select_path)

        self.main_vlayout.addLayout(self.two_buttons1)

        # self.main_vlayout.addRow(self.ok_button, self.client_answers)

        # self.main_vlayout.addWidget(self.ok_button)

        # self.main_vlayout.addWidget(self.ok_button)

        for group_name in self.history_items:
            self.group_selected_items[group_name] = []
            self.group_areas[group_name] = QScrollArea(self)
            self.group_main_widget[group_name] = QWidget(
                self.group_areas[group_name])
            self.group_label[group_name] = QLabel(group_name, self)
            self.group_label[group_name].setAlignment(Qt.AlignCenter)
            self.main_vlayout.addWidget(self.group_label[group_name])
            self.main_vlayout.addWidget(self.group_areas[group_name])
            self.group_selection_vlayout[group_name] = QVBoxLayout(self)
            self.group_item_all[group_name] = MyQCheckBox(
                "All", self, self.group_selection_vlayout[group_name], None)
            self.MakeCheckBoxList(self.group_selection_vlayout[group_name],
                                  self.group_selected_items[group_name],
                                  self.history_items[group_name],
                                  self.group_item_all[group_name])
            self.group_main_widget[group_name].setLayout(
                self.group_selection_vlayout[group_name])
            self.group_areas[group_name].setWidget(
                self.group_main_widget[group_name])

        self.clear_button = QPushButton("Clear Selection", self)
        self.clear_button.clicked.connect(self.onClearClicked)

        self.choose_button = QPushButton("Get Last Export Choose", self)
        self.choose_button.clicked.connect(self.onButtonChooseCliked)

        # self.main_vlayout.addWidget(self.choose_button)

        self.label4 = QLabel("Window time", self)
        self.label4.setAlignment(Qt.AlignCenter)

        # self.main_vlayout.addWidget(self.label4)

        self.window = QLineEdit(self)
        # self.main_vlayout.addWidget(self.window)
        self.window.setText("3")

        self.label5 = QLabel("Duration Time:", self)
        self.label5.setAlignment(Qt.AlignCenter)

        self.label6 = QLabel("Step:", self)
        self.label6.setAlignment(Qt.AlignCenter)

        self.step = QLineEdit(self)
        self.step.setText("1")

        self.windows_time_3 = QHBoxLayout(self)

        self.windows_time_3.addWidget(self.label4)

        self.windows_time_3.addWidget(self.window)

        self.windows_time_3.addWidget(self.label5)

        self.main_vlayout.addLayout(self.windows_time_3)

        self.step_2 = QHBoxLayout(self)

        self.step_2.addWidget(self.label6)

        self.step_2.addWidget(self.step)

        self.main_vlayout.addLayout(self.step_2)

        # self.main_vlayout.addRow(self.label4, self.window)

        self.two_buttons = QHBoxLayout(self)

        self.two_buttons.addWidget(self.choose_button)

        self.two_buttons.addWidget(self.clear_button)

        self.main_vlayout.addLayout(self.two_buttons)

        # self.main_vlayout.addRow(self.clear_button, self.choose_button)

        # self.label5 = QLabel("Load CSV", self)
        # self.label5.setAlignment(Qt.AlignCenter)
        #
        # self.main_vlayout.addWidget(self.label5)

        # self.label6 = QLabel("output", self)
        # self.label6.setAlignment(Qt.AlignCenter)
        #
        # self.main_vlayout.addWidget(self.label6)

        self.save_button = QPushButton("Save CSV...", self)
        self.save_button.clicked.connect(self.onSaveClicked)

        self.submit_button = QPushButton("Sumbit", self)
        self.submit_button.clicked.connect(self.onSumbitClicked)

        # self.main_vlayout.addWidget(self.save_button)

        self.two_buttons2 = QHBoxLayout(self)

        self.two_buttons2.addWidget(self.save_button)

        self.two_buttons2.addWidget(self.save_path)

        self.main_vlayout.addLayout(self.two_buttons2)

        # self.main_vlayout.addRow(self.save_button, self.client_answers1)

        self.main_vlayout.addWidget(self.submit_button)

        self.show()

    def get_min_rows_csv(self, csv_files):
        import pandas as pd
        dfs = map(lambda x: pd.read_csv(x, header=0), csv_files)
        number = dfs[0].shape[0]
        number = reduce(lambda acc, curr: min(acc, curr.shape[0]), dfs, number)
        self.label5.setText("Duration Time: %s" % number)

    def onClearClicked(self):
        self.clearTopicCheckState()

    def clearTopicCheckState(self):
        for item in self.items_list:
            item.setCheckState(False)
        for item in self.group_item_all.values():
            item.setCheckState(False)

    def MakeCheckBoxList(self, selection_vlayout, selected, topics_Keys,
                         item_all):
        item_all.stateChanged.connect(
            lambda x: self.updateList(x, item_all, None))
        selection_vlayout.addWidget(item_all)
        topic_data_list = topics_Keys
        topic_data_list.sort()
        for topic in topic_data_list:
            self.addCheckBox(topic, selection_vlayout, selected)

    def addCheckBox(self, topic, selection_vlayout, selected_list):
        item = MyQCheckBox(topic, self, selection_vlayout, selected_list)
        item.stateChanged.connect(lambda x: self.updateList(x, item, topic))
        self.items_list.append(item)
        selection_vlayout.addWidget(item)

    def updateList(self,
                   state,
                   item_clicked,
                   topic=None,
                   force_update_state=False):
        if type(item_clicked) is str:
            item_clicked = self.get_item_by_name(item_clicked)
            if item_clicked is None:
                return
        if topic is None:  # The "All" checkbox was checked / unchecked
            # print "if topic is None"
            if state == Qt.Checked:
                # self.item_all.setTristate(False)
                for item in self.items_list:
                    if item.checkState() == Qt.Unchecked and \
                                    item.selection_vlayout == item_clicked.selection_vlayout:
                        item.setCheckState(Qt.Checked)
            elif state == Qt.Unchecked:
                # self.item_all.setTristate(False)
                for item in self.items_list:
                    if item.checkState() == Qt.Checked and \
                                    item.selection_vlayout == item_clicked.selection_vlayout:
                        item.setCheckState(Qt.Unchecked)
        else:
            if state == Qt.Checked:
                item_clicked.selected_list.append(topic)
                # print item_clicked.selected_list
            else:
                item_clicked.selected_list.remove(topic)
                # if self.item_all.checkState() == Qt.Checked:
                #    self.item_all.setCheckState(Qt.PartiallyChecked)

    def get_current_opened_directory(self, filepath):
        import os
        direc = "/"
        if os.path.isfile(filepath):
            with open(filepath, 'r') as f:
                direc = f.read()
        return direc

    def onButtonClicked(self):
        import inspect, os
        filepath = os.path.dirname(
            os.path.abspath(inspect.getfile(
                inspect.currentframe()))) + "/log/select_history.log"
        fd = QFileDialog(self)
        wc = "Csv files {.csv} (*.csv)"
        self.files = []
        current_directory = self.get_current_opened_directory(filepath)
        tmp_pathes, filter = fd.getOpenFileNamesAndFilter(
            filter=wc, initialFilter=('*.csv'), directory=current_directory)
        for path in tmp_pathes:
            self.files.append(path.encode("utf-8"))
        # print self.files
        if len(self.files) != 0:
            self.get_min_rows_csv(tmp_pathes)
            self.select_path.setText(self.files[0])
            with open(filepath, "w") as f:
                f.write(self.files[0])

                # handler = logging.FileHandler(filepath, mode='w')
                # logger_topic.addHandler(handler)
                # logger_topic.info(self.files[0])
        else:
            self.select_path.setText("")

    def check_int(self, number, condition, title, message_body):
        try:
            # print type(number)
            val = int(number)
            if val <= condition:
                QMessageBox.about(self, title,
                                  "The number should > %s" % condition)
                return False
        except ValueError:
            QMessageBox.about(self, title, message_body)
            return False
        return True

    def check_choose(self):
        flag = True
        temp = []
        for item in self.group_selected_items.values():
            if item:
                for i in item:
                    temp.append(i)
        if len(temp) == 0:
            QMessageBox.about(self, "Features",
                              "One feature at least should be chosen")
            flag = False
        return flag

    def check_files_amount(self):
        flag = True
        if len(self.files) <= 0:
            QMessageBox.about(self, "Load CSV",
                              "One file at least should be chosen")
            flag = False
        return flag

    def check_validation(self):
        flag = self.check_files_amount()
        flag = flag & self.check_choose()
        flag = flag & self.check_int(self.window.text(), 2,
                                     "Error in Window Time",
                                     "That's not a number!")
        flag = flag & self.check_int(self.step.text(), 0, "Error in Step",
                                     "That's not a integer!")
        # TODO selected topic not empty

        if self.saved_dir == []:
            QMessageBox.about(self, "Save CSV", "Select path for saving")
            flag = False
        return flag

    def onSumbitClicked(self):
        if not self.check_validation():
            return
        self.selected_topics = []
        for item in self.group_selected_items.values():
            if item:
                for i in item:
                    self.selected_topics.append(i)
        # Defined Logging
        # handler = logging.FileHandler('/var/tmp/logger_history.log', mode='a')
        # logger_topic.addHandler(handler)
        topics = self.selected_topics
        with open(get_path() + 'logger_history.log', "w") as f:
            for topic in topics:
                f.write(topic + "\n")
        self.createTimeSeriesFeatures(self.files, self.saved_dir,
                                      int(self.window.text()),
                                      self.group_selected_items,
                                      int(self.step.text()))

    def onSaveClicked(self):
        import inspect, os
        filepath = os.path.dirname(
            os.path.abspath(inspect.getfile(
                inspect.currentframe()))) + "/log/save_history.log"
        # print self.files
        # print self.window.text()
        # print self.group_selected_items.values()

        # print self.group_selected_items
        current_directory = self.get_current_opened_directory(filepath)
        # print current_directory
        # self._to_save_filename = QFileDialog.getSaveFileName(self, self.tr('csv File'), current_directory,
        #                                                      self.tr('csv (*.csv)'))
        self.saved_dir = str(
            QFileDialog.getExistingDirectory(self, "Select Directory",
                                             current_directory))
        # if self._to_save_filename[0] != "":
        #     with open(filepath, "w") as f:
        #         f.write(self._to_save_filename[0])
        if self.saved_dir != "":
            with open(filepath, "w") as f:
                f.write(self.saved_dir)

                # handler = logging.FileHandler(filepath, mode='w')
                # logger_topic.addHandler(handler)
                # print self._to_save_filename[0]
                # logger_topic.info()
        # self.save_path.setText(get_corrent_file_name(self._to_save_filename[0], ".csv"))
        self.save_path.setText(self.saved_dir)

    def createTimeSeriesFeatures(self, files, saved_dir, window,
                                 group_selected_items, step):
        import TimeSeriesFeatures as TS
        # saved_dir = saved_dir[0].encode('utf-8')
        for input_path in files:
            output_path = generate_csv_from_bag(saved_dir, input_path)
            print "++++++++++++++++++++++++++ ", output_path
            print "in = %s out = %s " % (input_path, output_path)
            print "step = %s" % step
            ts = TS.TimeSeries(input_path, output_path, window,
                               group_selected_items, step)
            ts.generate_time_series_features()
        QMessageBox.about(self, "csv save", "csv was saved successfuly")

    def onButtonChooseCliked(self):
        for checkbox in self.items_list:
            checkbox.setCheckState(Qt.Unchecked)
        with open(get_path() + "logger_history.log", 'r') as f:
            topics = f.read().splitlines()
        for checkbox in self.items_list:
            if checkbox.text() in topics:
                checkbox.setCheckState(Qt.Checked)
コード例 #4
0
class UAVStatePublisher(QMainWindow):
    def __init__(self):
        super(UAVStatePublisher, self).__init__()

        #Not working: how to set window dimension
        self.height = 300
        self.width = 300
        self.top = 50
        self.left = 50
        font = QFont("Helvetica", 9, QFont.Bold)

        rospy.init_node("uav_state_publisher")
        nodename = rospy.get_name()
        rospy.loginfo("%s started" % nodename)

        self.arm_disarm_req = rospy.ServiceProxy('/mavros/cmd/arming',
                                                 CommandBool)
        self.takeoff_req = rospy.ServiceProxy('/mavros/cmd/takeoff',
                                              CommandTOL)
        self.land_req = rospy.ServiceProxy('/mavros/cmd/land', CommandTOL)
        self.offboard_req = rospy.ServiceProxy('/mavros/set_mode', SetMode)

        rospy.Subscriber("/mavros/local_position/odom", Odometry, self.odomCb)
        rospy.Subscriber("/mavros/state", State, self.stateCb)
        self.state = "Idle"
        self.arm_state = False
        self.local_cmd = ([0.0, 0.0, 0.0])
        self.dx_cmd = 0.0
        self.dy_cmd = 0.0
        self.dz_cmd = 0.0
        self.yaw_cmd = 0.0

        self.ctrl_mode = "position"
        self.setpoint_raw_pub = rospy.Publisher('mavros/setpoint_raw/local',
                                                PositionTarget,
                                                queue_size=1)
        '''
        uint16 IGNORE_PX=1
        uint16 IGNORE_PY=2
        uint16 IGNORE_PZ=4
        uint16 IGNORE_VX=8
        uint16 IGNORE_VY=16
        uint16 IGNORE_VZ=32
        uint16 IGNORE_AFX=64
        uint16 IGNORE_AFY=128
        uint16 IGNORE_AFZ=256
        uint16 FORCE=512
        uint16 IGNORE_YAW=1024
        uint16 IGNORE_YAW_RATE=2048
        '''
        self.pos_raw_msg = PositionTarget()
        self.pos_raw_msg.coordinate_frame = PositionTarget.FRAME_LOCAL_NED
        self.pos_raw_msg.type_mask = PositionTarget.IGNORE_VX + PositionTarget.IGNORE_VY + PositionTarget.IGNORE_VZ + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ + PositionTarget.IGNORE_YAW + PositionTarget.FORCE

        self.vel_raw_msg = PositionTarget()
        self.vel_raw_msg.coordinate_frame = PositionTarget.FRAME_BODY_NED
        self.vel_raw_msg.type_mask = PositionTarget.IGNORE_PX + PositionTarget.IGNORE_PY + PositionTarget.IGNORE_PZ + PositionTarget.IGNORE_AFX + PositionTarget.IGNORE_AFY + PositionTarget.IGNORE_AFZ + PositionTarget.IGNORE_YAW + PositionTarget.FORCE

        window = QWidget()
        self.layout = QVBoxLayout()

        self.setWindowTitle("UAV State Publisher")

        #Position label
        self.textbox_state = QLabel(self)
        self.textbox_state.move(20, 20)
        self.textbox_state.resize(280, 40)
        self.textbox_state.setFont(font)

        #Position label
        self.textbox_pos = QLabel(self)
        self.textbox_pos.move(20, 20)
        self.textbox_pos.resize(280, 40)

        #Yaw Label
        self.textbox_yaw = QLabel(self)
        self.textbox_yaw.move(20, 20)
        self.textbox_yaw.resize(280, 40)

        #Command position label
        self.textbox_cmd_pos = QLabel(self)
        self.textbox_cmd_pos.move(20, 20)
        self.textbox_cmd_pos.resize(280, 40)

        #Command yaw label
        self.textbox_cmd_yaw = QLabel(self)
        self.textbox_cmd_yaw.move(20, 20)
        self.textbox_cmd_yaw.resize(280, 40)

        #Position control
        self.checkbox_pvcontrol = QCheckBox('Position/velocity control')
        #self.checkbox_vcontrol = QCheckBox('Velocity control')
        self.checkbox_pvcontrol.stateChanged.connect(
            self.select_chkbox_pvcontrol)
        #self.checkbox_vcontrol.stateChanged.connect(self.select_chkbox_vcontrol)

        self.arm_button = QPushButton('Arm / Disarm', self)
        self.arm_button.setToolTip('Start/Stop motor spinning')
        self.arm_button.move(100, 70)
        self.arm_button.clicked.connect(self.on_click_arm)

        self.toff_button = QPushButton('Take-off', self)
        self.toff_button.setToolTip('takeoff the UAV')
        self.toff_button.move(100, 70)
        self.toff_button.clicked.connect(self.on_click_toff)

        self.land_button = QPushButton('Land', self)
        self.land_button.setToolTip('Land the UAV')
        self.land_button.move(100, 70)
        self.land_button.clicked.connect(self.on_click_land)

        self.offboard_button = QPushButton('OFFBOARD', self)
        self.offboard_button.setToolTip('Start offboard mode')
        self.offboard_button.move(100, 70)
        self.offboard_button.clicked.connect(self.on_click_offboard)

        x_cmdlabel = QLabel("Slide to command the x rate (d/s)")
        x_cmdlabel.setFont(font)

        self.cmd_dx_slider = QSlider(Qt.Horizontal, self)
        self.cmd_dx_slider.setGeometry(30, 40, 200, 30)
        self.cmd_dx_slider.valueChanged[int].connect(self.changeValue_dx)
        self.cmd_dx_slider.setRange(-10, 10)
        self.cmd_dx_slider.setValue(0)

        y_cmdlabel = QLabel("Slide to command the y rate (d/s)")
        y_cmdlabel.setFont(font)

        self.cmd_dy_slider = QSlider(Qt.Horizontal, self)
        self.cmd_dy_slider.setGeometry(30, 40, 200, 30)
        self.cmd_dy_slider.valueChanged[int].connect(self.changeValue_dy)
        self.cmd_dy_slider.setRange(-10, 10)
        self.cmd_dy_slider.setValue(0)

        z_cmdlabel = QLabel("Slide to command the z rate (d/s)")
        z_cmdlabel.setFont(font)

        self.cmd_dz_slider = QSlider(Qt.Horizontal, self)
        self.cmd_dz_slider.setGeometry(30, 40, 200, 30)
        self.cmd_dz_slider.valueChanged[int].connect(self.changeValue_dz)
        self.cmd_dz_slider.setRange(-10, 10)
        self.cmd_dz_slider.setValue(0)

        self.center_slider_button = QPushButton('Center', self)
        self.center_slider_button.setToolTip('Put in the middel all sliders')
        self.center_slider_button.move(100, 70)
        self.center_slider_button.clicked.connect(self.on_click_center)

        yaw_cmdlabel = QLabel("Slide to command the yaw rate (d/s)")
        yaw_cmdlabel.setFont(font)

        self.cmd_dyaw_slider = QSlider(Qt.Horizontal, self)
        self.cmd_dyaw_slider.setGeometry(30, 40, 200, 30)
        self.cmd_dyaw_slider.valueChanged[int].connect(self.changeValue_dyaw)
        self.cmd_dyaw_slider.setRange(-60, 60)
        self.cmd_dyaw_slider.setValue(0)

        joy_cmdlabel = QLabel("Click and release buttons to command the UAV")
        joy_cmdlabel.setFont(font)

        self.forward_button = QPushButton('FORWARD', self)
        self.forward_button.setToolTip('Move robot in positive x direction')
        self.forward_button.move(100, 70)
        self.forward_button.pressed.connect(self.on_pres_forward)
        self.forward_button.released.connect(self.on_rel_forward)

        self.back_button = QPushButton('BACKWARD', self)
        self.back_button.setToolTip('Move robot in negative x direction')
        self.back_button.move(100, 70)
        self.back_button.pressed.connect(self.on_pres_back)
        self.back_button.released.connect(self.on_rel_back)

        self.left_button = QPushButton('LEFT', self)
        self.left_button.setToolTip('Move robot in positive y direction')
        self.left_button.move(100, 70)
        self.left_button.pressed.connect(self.on_pres_left)
        self.left_button.released.connect(self.on_rel_left)

        self.right_button = QPushButton('RIGHT', self)
        self.right_button.setToolTip('Move robot in negative y direction')
        self.right_button.move(100, 70)
        self.right_button.pressed.connect(self.on_pres_right)
        self.right_button.released.connect(self.on_rel_right)

        self.up_button = QPushButton('UP', self)
        self.up_button.setToolTip('Move robot in positive z direction')
        self.up_button.move(100, 70)
        self.up_button.pressed.connect(self.on_pres_up)
        self.up_button.released.connect(self.on_rel_up)

        self.down_button = QPushButton('DOWN', self)
        self.down_button.setToolTip('Move robot in negative z direction')
        self.down_button.move(100, 70)
        self.down_button.pressed.connect(self.on_pres_down)
        self.down_button.released.connect(self.on_rel_down)

        self.rot_left_button = QPushButton('ROTATE_LEFT', self)
        self.rot_left_button.setToolTip('rotate robot in left direction')
        self.rot_left_button.move(100, 70)
        self.rot_left_button.pressed.connect(self.on_pres_yawleft)
        self.rot_left_button.released.connect(self.on_rel_yawleft)

        self.rot_right_button = QPushButton('ROTATE_RIGHT', self)
        self.rot_right_button.setToolTip('rotate robot in right direction')
        self.rot_right_button.move(100, 70)
        self.rot_right_button.pressed.connect(self.on_pres_yawright)
        self.rot_right_button.released.connect(self.on_rel_yawright)

        self.layout.addWidget(self.textbox_state)
        self.layout.addWidget(self.textbox_pos)
        self.layout.addWidget(self.textbox_yaw)
        self.layout.addWidget(self.textbox_cmd_pos)
        self.layout.addWidget(self.textbox_cmd_yaw)
        self.layout.addWidget(self.arm_button)
        self.layout.addWidget(self.toff_button)
        self.layout.addWidget(self.land_button)
        self.layout.addWidget(self.offboard_button)

        self.layout.addWidget(self.checkbox_pvcontrol)
        #self.layout.addWidget(self.checkbox_vcontrol)

        self.layout.addWidget(x_cmdlabel)
        self.layout.addWidget(self.cmd_dx_slider)
        self.layout.addWidget(y_cmdlabel)
        self.layout.addWidget(self.cmd_dy_slider)
        self.layout.addWidget(z_cmdlabel)
        self.layout.addWidget(self.cmd_dz_slider)
        self.layout.addWidget(yaw_cmdlabel)
        self.layout.addWidget(self.cmd_dyaw_slider)
        self.layout.addWidget(self.center_slider_button)

        self.layout.addWidget(joy_cmdlabel)
        self.layout.addWidget(self.forward_button)
        self.layout.addWidget(self.back_button)
        self.layout.addWidget(self.left_button)
        self.layout.addWidget(self.right_button)
        self.layout.addWidget(self.up_button)
        self.layout.addWidget(self.down_button)
        self.layout.addWidget(self.rot_left_button)
        self.layout.addWidget(self.rot_right_button)

        self.checkbox_pvcontrol.setChecked(True)

        self.forward_button.setEnabled(False)
        self.back_button.setEnabled(False)
        self.left_button.setEnabled(False)
        self.right_button.setEnabled(False)
        self.up_button.setEnabled(False)
        self.down_button.setEnabled(False)
        self.rot_left_button.setEnabled(False)
        self.rot_right_button.setEnabled(False)

        window.setLayout(self.layout)
        window.show()
        app.exec_()

        sys.exit()
        '''
        self.setWindowTitle("UAV State Publisher")
        font = QFont("Helvetica", 9, QFont.Bold)
        label = QLabel("Yaw")
        label.setFont(font)
        addWidget(label)

        mySlider = QSlider(Qt.Horizontal, self)
        mySlider.setGeometry(30, 40, 200, 30)
        mySlider.valueChanged[int].connect(self.changeValue)

        #self.setGeometry(50,50,320,200)
        self.show()
        '''

    def select_chkbox_pvcontrol(self, state):
        if state == 0:
            self.cmd_dx_slider.setValue(0)
            self.cmd_dy_slider.setValue(0)
            self.cmd_dz_slider.setValue(0)
            self.cmd_dyaw_slider.setValue(0)

            self.center_slider_button.setEnabled(False)
            self.cmd_dx_slider.setEnabled(False)
            self.cmd_dy_slider.setEnabled(False)
            self.cmd_dz_slider.setEnabled(False)
            self.cmd_dyaw_slider.setEnabled(False)

            self.forward_button.setEnabled(True)
            self.back_button.setEnabled(True)
            self.left_button.setEnabled(True)
            self.right_button.setEnabled(True)
            self.up_button.setEnabled(True)
            self.down_button.setEnabled(True)
            self.rot_left_button.setEnabled(True)
            self.rot_right_button.setEnabled(True)
            self.ctrl_mode = "velocity"

        elif state == 2:
            self.cmd_dx_slider.setValue(0)
            self.cmd_dy_slider.setValue(0)
            self.cmd_dz_slider.setValue(0)
            self.cmd_dyaw_slider.setValue(0)

            self.center_slider_button.setEnabled(True)
            self.cmd_dx_slider.setEnabled(True)
            self.cmd_dy_slider.setEnabled(True)
            self.cmd_dz_slider.setEnabled(True)
            self.cmd_dyaw_slider.setEnabled(True)

            self.forward_button.setEnabled(False)
            self.back_button.setEnabled(False)
            self.left_button.setEnabled(False)
            self.right_button.setEnabled(False)
            self.up_button.setEnabled(False)
            self.down_button.setEnabled(False)
            self.rot_left_button.setEnabled(False)
            self.rot_right_button.setEnabled(False)
            self.ctrl_mode = "position"

    #def select_chkbox_pcontrol(self, state):
    #    print("select_chkbox_pcontrol state: ", state)
    #    self.checkbox_vcontrol.setChecked( False )
    #    self.checkbox_pcontrol.setChecked( True )

    #def select_chkbox_vcontrol( self, state ):
    #    print("select_chkbox_vcontrol state: ", state)
    #    self.checkbox_pcontrol.setChecked( False )
    #    self.checkbox_vcontrol.setChecked( True )

    def on_pres_yawleft(self):
        self.yaw_cmd = 0.3

    def on_rel_yawleft(self):
        self.yaw_cmd = 0.0

    def on_pres_yawright(self):
        self.yaw_cmd = -0.3

    def on_rel_yawright(self):
        self.yaw_cmd = 0.0

    def on_pres_forward(self):
        self.dy_cmd = 0.3

    def on_rel_forward(self):
        self.dy_cmd = 0.0

    def on_pres_back(self):
        self.dy_cmd = -0.3

    def on_rel_back(self):
        self.dy_cmd = 0.0

    def on_pres_left(self):
        self.dx_cmd = -0.3

    def on_rel_left(self):
        self.dx_cmd = 0.0

    def on_pres_right(self):
        self.dx_cmd = 0.3

    def on_rel_right(self):
        self.dx_cmd = 0.0

    def on_pres_up(self):
        self.dz_cmd = 0.3

    def on_rel_up(self):
        self.dz_cmd = 0.0

    def on_pres_down(self):
        self.dz_cmd = -0.3

    def on_rel_down(self):
        self.dz_cmd = 0.0

    def on_click_arm(self):
        if self.arm_state == True:
            self.arm_disarm_req(False)
        else:
            self.arm_disarm_req(True)

    def on_click_toff(self):
        print("Request Takeoff")  #Todo
        self.local_cmd[2] = self.local_cmd[2] + 2.0

    def on_click_land(self):
        print("Request Land")  #Todo
        self.land_req(altitude=0, latitude=0, longitude=0, min_pitch=0, yaw=0)

    def on_click_center(self):
        self.cmd_dx_slider.setValue(0)
        self.cmd_dy_slider.setValue(0)
        self.cmd_dz_slider.setValue(0)
        self.cmd_dyaw_slider.setValue(0)

    def on_click_offboard(self):
        self.offboard_req(base_mode=0, custom_mode="OFFBOARD")

    def changeValue_dyaw(self, value):
        self.yaw_cmd = (value * 3.1415) / 180.0

    def changeValue_dx(self, value):
        self.dx_cmd = value / 10.0

    def changeValue_dy(self, value):
        self.dy_cmd = value / 10.0

    def changeValue_dz(self, value):
        self.dz_cmd = value / 10.0

    def stateCb(self, msg):
        self.state = msg.mode
        self.arm_state = msg.armed

    def odomCb(self, msg):

        self.textbox_state.setText("State: " + self.state)

        str_x = str(round(msg.pose.pose.position.x, 2))
        str_y = str(round(msg.pose.pose.position.y, 2))
        str_z = str(round(msg.pose.pose.position.z, 2))
        coords = str_x + " " + str_y + " " + str_z
        self.textbox_pos.setText("Pos: " + coords)

        quaternion = (msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
                      msg.pose.pose.orientation.z, msg.pose.pose.orientation.w)
        euler = tf.transformations.euler_from_quaternion(quaternion)
        yaw = euler[2]
        self.textbox_yaw.setText("Yaw: " + str(round(yaw, 2)))
        self.pos_raw_msg.header.stamp = rospy.get_rostime()

        if (self.state == "OFFBOARD"):
            if self.ctrl_mode == "position":
                self.local_cmd[0] = self.local_cmd[0] + self.dx_cmd * 0.02
                self.local_cmd[1] = self.local_cmd[1] + self.dy_cmd * 0.02
                self.local_cmd[2] = self.local_cmd[2] + self.dz_cmd * 0.02
            else:
                self.local_cmd[0] = msg.pose.pose.position.x
                self.local_cmd[1] = msg.pose.pose.position.y
                self.local_cmd[2] = msg.pose.pose.position.z
                #self.yaw_cmd = 0.0

        else:
            self.yaw_cmd = yaw
            self.local_cmd[0] = msg.pose.pose.position.x
            self.local_cmd[1] = msg.pose.pose.position.y
            self.local_cmd[2] = msg.pose.pose.position.z

        if self.ctrl_mode == "position":
            self.pos_raw_msg.position.x = self.local_cmd[0]
            self.pos_raw_msg.position.y = self.local_cmd[1]
            self.pos_raw_msg.position.z = self.local_cmd[2]
            self.pos_raw_msg.yaw_rate = self.yaw_cmd
            self.textbox_cmd_pos.setText("Cmd Pos: " +
                                         str(round(self.local_cmd[0], 2)) +
                                         " " +
                                         str(round(self.local_cmd[1], 2)) +
                                         " " +
                                         str(round(self.local_cmd[2], 2)))
            self.textbox_cmd_yaw.setText("Cmd dYaw: " +
                                         str(round(self.yaw_cmd, 2)) +
                                         " rad/s")
            self.setpoint_raw_pub.publish(self.pos_raw_msg)

        elif self.ctrl_mode == "velocity":
            self.vel_raw_msg.velocity.x = self.dx_cmd
            self.vel_raw_msg.velocity.y = self.dy_cmd
            self.vel_raw_msg.velocity.z = self.dz_cmd
            self.vel_raw_msg.yaw_rate = self.yaw_cmd
            self.textbox_cmd_pos.setText("Cmd Vel: " + str(self.dx_cmd) + " " +
                                         str(self.dy_cmd) + " " +
                                         str(self.dz_cmd))
            self.textbox_cmd_yaw.setText("Cmd dYaw: " +
                                         str(round(self.yaw_cmd, 2)) +
                                         " rad/s")
            self.setpoint_raw_pub.publish(self.vel_raw_msg)
コード例 #5
0
    def init_sliders(self):
        sliderbox = self._widget.findChild(QLayout, 'Sliders')

        graph_button = QPushButton()
        graph_button.setCheckable(True)
        graph_button.setText("Graph Off")
        graph_button.toggle()
        graph_button.clicked.connect(self.set_graph_state)
        self.graph_button = graph_button

        firstCol = QVBoxLayout()
        firstCol.addWidget(graph_button)

        sliderbox.addLayout(firstCol)

        self.sliders = []

        all_rows_layout = QVBoxLayout()
        chan_idx = 0
        for num_channels_row in self.settings['num_channels']:
            row_layout = QHBoxLayout()
            for i in range(num_channels_row):
                idx = chan_idx * 1

                slider_group = {
                    'slider_p': None,
                    'number_p': None,
                    'slider_v': None,
                    'number_v': None,
                    'on_off': None
                }

                layout_cluster = QVBoxLayout()
                slider_cluster = QHBoxLayout()
                label = QLabel()
                label.setText("Chan. %d" % (idx + 1))
                label.setAlignment(Qt.AlignCenter)
                layout_cluster.addWidget(label)
                for j in range(2):
                    layout = QVBoxLayout()
                    layout.setAlignment(Qt.AlignHCenter)

                    slider = QSlider(Qt.Vertical)
                    slider.setMinimum(0)
                    slider.setMaximum(255)
                    slider.setValue(
                        self.settings['valve_offsets'][chan_idx][j])
                    slider.setTickPosition(QSlider.TicksRight)
                    slider.setTickInterval(5)

                    spinbox = QSpinBox()
                    spinbox.setRange(0, 255)
                    spinbox.setValue(
                        self.settings['valve_offsets'][chan_idx][j])

                    slider.valueChanged.connect(spinbox.setValue)
                    spinbox.valueChanged.connect(slider.setValue)

                    cb_function_curr = lambda value, idx=idx: self.send_slider_value(
                        idx, value)
                    slider.valueChanged.connect(cb_function_curr)

                    label = QLabel()
                    label.setAlignment(Qt.AlignCenter)

                    if j == 0:
                        slider_group['slider_p'] = slider
                        slider_group['number_p'] = spinbox
                        label.setText("P")
                    else:
                        slider_group['slider_v'] = slider
                        slider_group['number_v'] = spinbox
                        label.setText("V")

                    labelmax = QLabel()
                    labelmax.setAlignment(Qt.AlignCenter)
                    labelmax.setText("255")

                    labelmin = QLabel()
                    labelmin.setAlignment(Qt.AlignCenter)
                    labelmin.setText("0")

                    layout.addWidget(label)
                    layout.addWidget(labelmax)
                    layout.addWidget(slider, Qt.AlignHCenter)
                    layout.addWidget(labelmin)
                    layout.addWidget(spinbox, Qt.AlignHCenter)
                    layout.setAlignment(slider, Qt.AlignHCenter)
                    layout.setAlignment(spinbox, Qt.AlignHCenter)

                    slider_cluster.addLayout(layout)

                on_button = QPushButton()
                on_button.setCheckable(True)
                on_button.setText("Off")

                if self.settings['channel_states'][chan_idx]:
                    on_button.toggle()
                    on_button.setText("On")

                on_button.clicked.connect(
                    lambda state, idx=idx: self.send_channel_state(idx, state))

                slider_group['on_off'] = on_button

                layout_cluster.addLayout(slider_cluster)
                layout_cluster.addWidget(on_button)

                row_layout.addLayout(layout_cluster)
                row_layout.addSpacing(20)

                self.sliders.append(slider_group)
                chan_idx += 1

            all_rows_layout.addLayout(row_layout)
        sliderbox.addLayout(all_rows_layout)
コード例 #6
0
    def init_sliders(self):
        sliderbox = self._widget.findChild(QLayout, 'Sliders')

        firstCol = QVBoxLayout()
        graph_button = QPushButton()
        graph_button.setCheckable(True)
        graph_button.setText("Graph Off")
        graph_button.toggle()
        graph_button.clicked.connect(self.set_graph_state)

        reset_button = QPushButton()
        reset_button.setCheckable(False)
        reset_button.setText("Reset")
        reset_button.clicked.connect(self.set_reset)

        self.graph_button = graph_button
        self.reset_button = reset_button

        firstCol.addWidget(graph_button)
        firstCol.addWidget(reset_button)

        firstCol.setAlignment(graph_button, Qt.AlignVCenter)
        firstCol.setAlignment(reset_button, Qt.AlignVCenter)

        zero_button = QPushButton()
        zero_button.setCheckable(False)
        zero_button.setText("Set All Zero")
        zero_button.clicked.connect(self.set_pressure_zero)
        self.zero_button = zero_button
        firstCol.addWidget(zero_button)
        firstCol.setAlignment(zero_button, Qt.AlignVCenter)

        transition_box = QVBoxLayout()
        label = QLabel()
        label.setAlignment(Qt.AlignCenter)
        label.setText("Transition Time")

        spinbox = QDoubleSpinBox()
        spinbox.setMinimum(0)
        spinbox.setMaximum(10)
        spinbox.setValue(self.settings['transitions'])
        spinbox.setDecimals(1)
        spinbox.setSingleStep(0.1)
        spinbox.setSuffix(" sec")

        spinbox.valueChanged.connect(self.set_transition_value)

        transition_box.addWidget(label)
        transition_box.addWidget(spinbox)
        transition_box.setAlignment(label, Qt.AlignBottom)
        transition_box.setAlignment(spinbox, Qt.AlignTop)
        firstCol.addLayout(transition_box)

        self.sliders = []
        sliderbox.addLayout(firstCol)

        all_rows_layout = QVBoxLayout()

        chan_idx = 0
        for num_channels_row in self.settings['num_channels']:
            row_layout = QHBoxLayout()
            for i in range(num_channels_row):
                idx = chan_idx * 1

                slider_group = {'slider': None, 'number': None, 'on_off': None}
                layout_cluster = QVBoxLayout()

                layout = QVBoxLayout()
                layout.setAlignment(Qt.AlignHCenter)

                slider = QSlider(Qt.Vertical)
                slider.setMinimum(self.settings['min_pressure'][idx] * 10.0)
                slider.setMaximum(self.settings['max_pressure'][idx] * 10.0)
                slider.setValue(0)
                slider.setTickPosition(QSlider.TicksRight)
                slider.setTickInterval(20)

                spinbox = QDoubleSpinBox()
                spinbox.setMinimum(self.settings['min_pressure'][idx])
                spinbox.setMaximum(self.settings['max_pressure'][idx])
                spinbox.setValue(0)
                spinbox.setDecimals(1)
                spinbox.setSingleStep(0.1)

                cb_function_curr = lambda value, idx=idx, slider=False: self.send_slider_value(
                    idx, value, slider)
                cb_function_curr2 = lambda value, idx=idx, slider=True: self.send_slider_value(
                    idx, value, slider)
                slider.valueChanged.connect(cb_function_curr2)
                spinbox.valueChanged.connect(cb_function_curr)

                labelmax = QLabel()
                labelmax.setAlignment(Qt.AlignCenter)
                labelmax.setText("%0.1f" %
                                 (self.settings['max_pressure'][idx]))

                labelmin = QLabel()
                labelmin.setAlignment(Qt.AlignCenter)
                labelmin.setText("%0.1f" %
                                 (self.settings['min_pressure'][idx]))

                layout.addWidget(labelmax)
                layout.addWidget(slider)
                layout.addWidget(labelmin)
                layout.addWidget(spinbox)

                layout.setAlignment(slider, Qt.AlignHCenter)
                layout.setAlignment(spinbox, Qt.AlignHCenter)

                label = QLabel()
                label.setText("Chan. %d" % (chan_idx + 1))
                label.setAlignment(Qt.AlignCenter)
                layout_cluster.addWidget(label)
                layout_cluster.addLayout(layout)

                slider_group['slider'] = slider
                slider_group['number'] = spinbox

                on_button = QPushButton()
                on_button.setCheckable(True)
                on_button.setText("Off")

                if self.settings['channel_states'][idx]:
                    on_button.toggle()
                    on_button.setText("On")

                on_button.clicked.connect(
                    lambda state, idx=idx: self.send_channel_state(idx, state))

                slider_group['on_off'] = on_button

                layout_cluster.addWidget(on_button)

                row_layout.addLayout(layout_cluster)
                row_layout.addSpacing(20)

                self.sliders.append(slider_group)

                chan_idx += 1

            all_rows_layout.addLayout(row_layout)

        sliderbox.addLayout(all_rows_layout)
コード例 #7
0
class GroupWidget(QWidget):
    '''
    (Isaac's guess as of 12/13/2012)
    This class bonds multiple Editor instances that are associated with
    a single node as a group.
    '''

    # public signal
    sig_node_disabled_selected = Signal(str)
    sig_node_state_change = Signal(bool)

    def __init__(self, updater, config, nodename):
        '''
        :param config:
        :type config: Dictionary? defined in dynamic_reconfigure.client.Client
        :type nodename: str
        '''

        super(GroupWidget, self).__init__()
        self.state = config['state']
        self.param_name = config['name']
        self._toplevel_treenode_name = nodename

        # TODO: .ui file needs to be back into usage in later phase.
#        ui_file = os.path.join(rp.get_path('rqt_reconfigure'),
#                               'resource', 'singlenode_parameditor.ui')
#        loadUi(ui_file, self)

        verticalLayout = QVBoxLayout(self)
        verticalLayout.setContentsMargins(QMargins(0, 0, 0, 0))

        _widget_nodeheader = QWidget()
        _h_layout_nodeheader = QHBoxLayout(_widget_nodeheader)
        _h_layout_nodeheader.setContentsMargins(QMargins(0, 0, 0, 0))

        self.nodename_qlabel = QLabel(self)
        font = QFont('Trebuchet MS, Bold')
        font.setUnderline(True)
        font.setBold(True)

        # Button to close a node.
        _icon_disable_node = QIcon.fromTheme('window-close')
        _bt_disable_node = QPushButton(_icon_disable_node, '', self)
        _bt_disable_node.setToolTip('Hide this node')
        _bt_disable_node_size = QSize(36, 24)
        _bt_disable_node.setFixedSize(_bt_disable_node_size)
        _bt_disable_node.pressed.connect(self._node_disable_bt_clicked)

        _h_layout_nodeheader.addWidget(self.nodename_qlabel)
        _h_layout_nodeheader.addWidget(_bt_disable_node)

        self.nodename_qlabel.setAlignment(Qt.AlignCenter)
        font.setPointSize(10)
        self.nodename_qlabel.setFont(font)
        grid_widget = QWidget(self)
        self.grid = QFormLayout(grid_widget)
        verticalLayout.addWidget(_widget_nodeheader)
        verticalLayout.addWidget(grid_widget, 1)
        # Again, these UI operation above needs to happen in .ui file.

        self.tab_bar = None  # Every group can have one tab bar
        self.tab_bar_shown = False

        self.updater = updater

        self.editor_widgets = []
        self._param_names = []

        self._create_node_widgets(config)

        rospy.logdebug('Groups node name={}'.format(nodename))
        self.nodename_qlabel.setText(nodename)

        # Labels should not stretch
        #self.grid.setColumnStretch(1, 1)
        #self.setLayout(self.grid)

    def collect_paramnames(self, config):
        pass

    def _create_node_widgets(self, config):
        '''
        :type config: Dict?
        '''
        i_debug = 0
        for param in config['parameters']:
            begin = time.time() * 1000
            editor_type = '(none)'

            if param['edit_method']:
                widget = EnumEditor(self.updater, param)
            elif param['type'] in EDITOR_TYPES:
                rospy.logdebug('GroupWidget i_debug=%d param type =%s',
                               i_debug,
                               param['type'])
                editor_type = EDITOR_TYPES[param['type']]
                widget = eval(editor_type)(self.updater, param)

            self.editor_widgets.append(widget)
            self._param_names.append(param['name'])

            rospy.logdebug('groups._create_node_widgets num editors=%d',
                           i_debug)

            end = time.time() * 1000
            time_elap = end - begin
            rospy.logdebug('ParamG editor={} loop=#{} Time={}msec'.format(
                                              editor_type, i_debug, time_elap))
            i_debug += 1

        for name, group in config['groups'].items():
            if group['type'] == 'tab':
                widget = TabGroup(self, self.updater, group, self._toplevel_treenode_name)
            elif group['type'] in _GROUP_TYPES.keys():
                widget = eval(_GROUP_TYPES[group['type']])(self.updater, group, self._toplevel_treenode_name)
            else:
                widget = eval(_GROUP_TYPES[''])(self.updater, group, self._toplevel_treenode_name)

            self.editor_widgets.append(widget)
            rospy.logdebug('groups._create_node_widgets ' +
                           'name=%s',
                           name)

        for i, ed in enumerate(self.editor_widgets):
            ed.display(self.grid)

        rospy.logdebug('GroupWdgt._create_node_widgets len(editor_widgets)=%d',
                       len(self.editor_widgets))

    def display(self, grid):
        grid.addRow(self)

    def update_group(self, config):
        if 'state' in config:
            old_state = self.state
            self.state = config['state']
            if self.state != old_state:
                self.sig_node_state_change.emit(self.state)

        names = [name for name in config.keys()]

        for widget in self.editor_widgets:
            if isinstance(widget, EditorWidget):
                if widget.param_name in names:
                    widget.update_value(config[widget.param_name])
            elif isinstance(widget, GroupWidget):
                cfg = find_cfg(config, widget.param_name)
                widget.update_group(cfg)

    def close(self):
        for w in self.editor_widgets:
            w.close()

    def get_treenode_names(self):
        '''
        :rtype: str[]
        '''
        return self._param_names

    def _node_disable_bt_clicked(self):
        rospy.logdebug('param_gs _node_disable_bt_clicked')
        self.sig_node_disabled_selected.emit(self._toplevel_treenode_name)
コード例 #8
0
class GroupWidget(QWidget):
    '''
    (Isaac's guess as of 12/13/2012)
    This class bonds multiple Editor instances that are associated with
    a single node as a group.
    '''

    # public signal
    sig_node_disabled_selected = Signal(str)

    def __init__(self, updater, config, nodename):
        '''
        :param config:
        :type config: Dictionary? defined in dynamic_reconfigure.client.Client
        :type nodename: str
        '''

        #TODO figure out what data type 'config' is. It is afterall returned
        #     from dynamic_reconfigure.client.get_parameter_descriptions()
        # ros.org/doc/api/dynamic_reconfigure/html/dynamic_reconfigure.client-pysrc.html#Client

        super(GroupWidget, self).__init__()
        self.state = config['state']
        self.name = config['name']
        self._toplevel_treenode_name = nodename

        # TODO: .ui file needs to be back into usage in later phase.
#        ui_file = os.path.join(rp.get_path('rqt_reconfigure'),
#                               'resource', 'singlenode_parameditor.ui')
#        loadUi(ui_file, self)

        verticalLayout = QVBoxLayout(self)
        verticalLayout.setContentsMargins(QMargins(0, 0, 0, 0))

        _widget_nodeheader = QWidget()
        _h_layout_nodeheader = QHBoxLayout(_widget_nodeheader)
        _h_layout_nodeheader.setContentsMargins(QMargins(0, 0, 0, 0))

        self.nodename_qlabel = QLabel(self)
        font = QFont('Trebuchet MS, Bold')
        font.setUnderline(True)
        font.setBold(True)

        # Button to close a node.
        _icon_disable_node = QIcon.fromTheme('window-close')
        _bt_disable_node = QPushButton(_icon_disable_node, '', self)
        _bt_disable_node.setToolTip('Hide this node')
        _bt_disable_node_size = QSize(36, 24)
        _bt_disable_node.setFixedSize(_bt_disable_node_size)
        _bt_disable_node.pressed.connect(self._node_disable_bt_clicked)

        _h_layout_nodeheader.addWidget(self.nodename_qlabel)
        _h_layout_nodeheader.addWidget(_bt_disable_node)

        self.nodename_qlabel.setAlignment(Qt.AlignCenter)
        font.setPointSize(10)
        self.nodename_qlabel.setFont(font)
        grid_widget = QWidget(self)
        self.grid = QFormLayout(grid_widget)
        verticalLayout.addWidget(_widget_nodeheader)
        verticalLayout.addWidget(grid_widget, 1)
        # Again, these UI operation above needs to happen in .ui file.

        self.tab_bar = None  # Every group can have one tab bar
        self.tab_bar_shown = False

        self.updater = updater

        self.editor_widgets = []
        self._param_names = []

        self._create_node_widgets(config)

        rospy.logdebug('Groups node name={}'.format(nodename))
        self.nodename_qlabel.setText(nodename)

        # Labels should not stretch
        #self.grid.setColumnStretch(1, 1)
        #self.setLayout(self.grid)

    def collect_paramnames(self, config):
        pass

    def _create_node_widgets(self, config):
        '''
        :type config: Dict?
        '''
        i_debug = 0
        for param in config['parameters']:
            begin = time.time() * 1000
            editor_type = '(none)'

            if param['edit_method']:
                widget = EnumEditor(self.updater, param)
            elif param['type'] in EDITOR_TYPES:
                rospy.logdebug('GroupWidget i_debug=%d param type =%s',
                              i_debug,
                              param['type'])
                editor_type = EDITOR_TYPES[param['type']]
                widget = eval(editor_type)(self.updater, param)

            self.editor_widgets.append(widget)
            self._param_names.append(param['name'])

            rospy.logdebug('groups._create_node_widgets num editors=%d',
                           i_debug)

            end = time.time() * 1000
            time_elap = end - begin
            rospy.logdebug('ParamG editor={} loop=#{} Time={}msec'.format(
                                              editor_type, i_debug, time_elap))
            i_debug += 1

        for name, group in config['groups'].items():
            if group['type'] == 'tab':
                widget = TabGroup(self, self.updater, group)
            elif group['type'] in _GROUP_TYPES.keys():
                widget = eval(_GROUP_TYPES[group['type']])(self.updater, group)

            self.editor_widgets.append(widget)
            rospy.logdebug('groups._create_node_widgets ' +
                          #'num groups=%d' +
                          'name=%s',
                          name)

        for i, ed in enumerate(self.editor_widgets):
            ed.display(self.grid)

        rospy.logdebug('GroupWdgt._create_node_widgets len(editor_widgets)=%d',
                      len(self.editor_widgets))

    def display(self, grid, row):
        # groups span across all columns
        grid.addWidget(self, row, 0, 1, -1)

    def update_group(self, config):
        self.state = config['state']

        # TODO: should use config.keys but this method doesnt exist
        names = [name for name in config.items()]

        for widget in self.editor_widgets:
            if isinstance(widget, EditorWidget):
                if widget.name in names:
                    widget.update_value(config[widget.name])
            elif isinstance(widget, GroupWidget):
                cfg = find_cfg(config, widget.name)
                widget.update_group(cfg)

    def close(self):
        for w in self.editor_widgets:
            w.close()

    def get_treenode_names(self):
        '''
        :rtype: str[]
        '''
        return self._param_names

    def _node_disable_bt_clicked(self):
        rospy.logdebug('param_gs _node_disable_bt_clicked')
        self.sig_node_disabled_selected.emit(self._toplevel_treenode_name)
コード例 #9
0
ファイル: kill_counter.py プロジェクト: leonshaw06/ROS-Main
class KillCounterPlugin(Plugin):
    def __init__(self, context):
        super(KillCounterPlugin, self).__init__(context)
        #return
        # Give QObjects reasonable names
        self.setObjectName('KillCounterPlugin')
        rp = rospkg.RosPack()

        # Process standalone plugin command-line arguments
        #from argparse import ArgumentParser
        #parser = ArgumentParser()
        ## Add argument(s) to the parser.
        #parser.add_argument("-q", "--quiet", action="store_true",
        #              dest="quiet",
        #              help="Put plugin in silent mode")
        #args, unknowns = parser.parse_known_args(context.argv())
        #if not args.quiet:
        #    print 'arguments: ', args
        #    print 'unknowns: ', unknowns

        # Create QWidget
        self._widget = QWidget()

        self._container = QWidget()
        self._layout = QVBoxLayout()
        self._container.setLayout(self._layout)

        self._num_killed = 0

        self._kill_label = QLabel('Martians Killed: 0')
        #Killing Martians is Bad Tho, We are a dying species and killing us is mean and will destroy an awesome society with culture and life and happiness. I'm so disappointed in you Kimberly
        self._layout.addWidget(self._kill_label)

        MIN = 540
        MAX = 660
        self._chance = randint(MIN, MAX)

        context.add_widget(self._container)

        # Get path to UI file which is a sibling of this file
        # in this example the .ui and .py file are in the same folder
        #ui_file = os.path.join(rp.get_path('rover_ui_plugins'), 'resource', 'CameraSelection.ui')
        #ui_file = os.path.join(rp.get_path('rover_ui_plugins'), 'resource', 'CameraSelectionSimple.ui')
        # Extend the widget with all attributes and children from UI file
        #loadUi(ui_file, self._widget, {})
        # Give QObjects reasonable names
        self._widget.setObjectName('Kill Counter')
        # Show _widget.windowTitle on left-top of each plugin (when
        # it's set in _widget). This is useful when you open multiple
        # plugins at once. Also if you open multiple instances of your
        # plugin at once, these lines add number to make it easy to
        # tell from pane to pane.
        #if context.serial_number() > 1:
        #self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))

        self.running = True
        rate = rospy.Rate(1)

        def run():
            while self.running:
                num = randint(0, self._chance)
                if num == self._chance:
                    self._num_killed += 1
                    self._kill_label.setText('Martians Killed: ' +
                                             str(self._num_killed))
                rate.sleep()

        self.run_thread = threading.Thread(target=run)
        self.run_thread.start()

    def shutdown_plugin(self):
        # TODO unregister all publishers here`
        self.running = False
コード例 #10
0
    def __init__(self, context, node_name):
        """
        Initializaze things.

        :type node_name: str
        """
        super(ParamClientWidget, self).__init__()
        self._node_grn = node_name
        self._toplevel_treenode_name = node_name

        self._editor_widgets = {}

        self._param_client = create_param_client(
            context.node, node_name, self._handle_param_event
        )

        verticalLayout = QVBoxLayout(self)
        verticalLayout.setContentsMargins(QMargins(0, 0, 0, 0))

        widget_nodeheader = QWidget()
        h_layout_nodeheader = QHBoxLayout(widget_nodeheader)
        h_layout_nodeheader.setContentsMargins(QMargins(0, 0, 0, 0))

        nodename_qlabel = QLabel(self)
        font = QFont('Trebuchet MS, Bold')
        font.setUnderline(True)
        font.setBold(True)
        font.setPointSize(10)
        nodename_qlabel.setFont(font)
        nodename_qlabel.setAlignment(Qt.AlignCenter)
        nodename_qlabel.setText(node_name)
        h_layout_nodeheader.addWidget(nodename_qlabel)

        # Button to close a node.
        icon_disable_node = QIcon.fromTheme('window-close')
        bt_disable_node = QPushButton(icon_disable_node, '', self)
        bt_disable_node.setToolTip('Hide this node')
        bt_disable_node_size = QSize(36, 24)
        bt_disable_node.setFixedSize(bt_disable_node_size)
        bt_disable_node.pressed.connect(self._node_disable_bt_clicked)
        h_layout_nodeheader.addWidget(bt_disable_node)

        grid_widget = QWidget(self)
        self.grid = QFormLayout(grid_widget)
        verticalLayout.addWidget(widget_nodeheader)
        verticalLayout.addWidget(grid_widget, 1)
        # Again, these UI operation above needs to happen in .ui file.
        param_names = self._param_client.list_parameters()
        self.add_editor_widgets(
            self._param_client.get_parameters(param_names),
            self._param_client.describe_parameters(param_names)
        )

        # Save and load buttons
        button_widget = QWidget(self)
        button_header = QHBoxLayout(button_widget)
        button_header.setContentsMargins(QMargins(0, 0, 0, 0))

        load_button = QPushButton()
        save_button = QPushButton()

        load_button.setIcon(QIcon.fromTheme('document-open'))
        save_button.setIcon(QIcon.fromTheme('document-save'))

        load_button.clicked[bool].connect(self._handle_load_clicked)
        save_button.clicked[bool].connect(self._handle_save_clicked)

        button_header.addWidget(save_button)
        button_header.addWidget(load_button)

        self.setMinimumWidth(150)
コード例 #11
0
    def init_sliders(self):
        sliderbox = self._widget.findChild(QLayout,'Sliders')

        firstCol = QVBoxLayout()
        graph_button=QPushButton()
        graph_button.setCheckable(True)
        graph_button.setText("Graph Off")
        graph_button.toggle()
        graph_button.clicked.connect(self.set_graph_state)
        self.graph_button = graph_button

        reset_button=QPushButton()
        reset_button.setCheckable(False)
        reset_button.setText("Reset")
        reset_button.clicked.connect(self.set_reset)

        self.graph_button = graph_button
        self.reset_button = reset_button

        firstCol.addWidget(graph_button)
        firstCol.addWidget(reset_button)


        firstCol.addWidget(graph_button)
        firstCol.setAlignment(graph_button,Qt.AlignVCenter)

        zero_button=QPushButton()
        zero_button.setCheckable(False)
        zero_button.setText("Set All Zero")
        zero_button.clicked.connect(self.set_pressure_zero)
        self.zero_button = zero_button
        firstCol.addWidget(zero_button)
        firstCol.setAlignment(zero_button,Qt.AlignVCenter)



        transition_box = QVBoxLayout()
        label = QLabel()
        label.setAlignment(Qt.AlignCenter)
        label.setText("Transition Time")

        spinbox = QDoubleSpinBox()
        spinbox.setMinimum(0)
        spinbox.setMaximum(10)
        spinbox.setValue(self.settings['transitions'])
        spinbox.setDecimals(1)
        spinbox.setSingleStep(0.1)
        spinbox.setSuffix(" sec")

        spinbox.valueChanged.connect(self.set_transition_value)

        transition_box.addWidget(label)
        transition_box.addWidget(spinbox)
        transition_box.setAlignment(label,Qt.AlignBottom)
        transition_box.setAlignment(spinbox,Qt.AlignTop)
        firstCol.addLayout(transition_box)

        self.sliders = []
        sliderbox.addLayout(firstCol)

        all_rows_layout = QVBoxLayout()

        g_idx = 0

        for row in self.settings['gui_config']:
            num_groups_row = len(row)
            row_layout = QHBoxLayout()
            for gr_idx, s_group in enumerate(row): 
                g_channels = s_group['channels']
                g_layout = s_group['layout']

                if 'horiz' in g_layout:
                    group_layout = QHBoxLayout()
                else:
                    group_layout = QVBoxLayout()

                control_group = {'sliders': [], 'on_off': None}

                label = QLabel()
                label.setText("Group. %d"%(g_idx+1))
                label.setAlignment(Qt.AlignCenter)
                group_layout.addWidget(label)

                for c_idx, s_idx in enumerate(g_channels):
                    idx = s_idx*1
                    
                    slider_group={'slider':None, 'number':None}
                    
                    layout_cluster = QVBoxLayout()

                    labelfirst = QLabel()
                    labelfirst.setAlignment(Qt.AlignCenter)

                    labellast = QLabel()
                    labellast.setAlignment(Qt.AlignCenter)

                    layout = QVBoxLayout()

                    if 'diff' in g_layout and c_idx == 0:

                        sublayout=QHBoxLayout()

                        layout.setAlignment(Qt.AlignVCenter)

                        slider = QSlider(Qt.Horizontal)
                        slider.setMinimum(-100)
                        slider.setMaximum(100)
                        slider.setValue(0)
                        slider.setTickPosition(QSlider.TicksRight)
                        slider.setTickInterval(20)

                        spinbox = QDoubleSpinBox()
                        spinbox.setMinimum(-10)
                        spinbox.setMaximum(10)
                        spinbox.setValue(0)
                        spinbox.setDecimals(1)
                        spinbox.setSingleStep(0.1)

                        labellast.setText("%0.1f"%(10)) # These are flipped becasue of order
                        labelfirst.setText("%0.1f"%(-10))

                        max_label = labellast
                        min_label = labelfirst

                    else:
                        layout.setAlignment(Qt.AlignHCenter)

                        slider = QSlider(Qt.Vertical)
                        slider.setMinimum(self.settings['min_pressure'][idx]*10.0)
                        slider.setMaximum(self.settings['max_pressure'][idx]*10.0)
                        slider.setValue(0)
                        slider.setTickPosition(QSlider.TicksRight)
                        slider.setTickInterval(20)

                        spinbox = QDoubleSpinBox()
                        spinbox.setMinimum(self.settings['min_pressure'][idx])
                        spinbox.setMaximum(self.settings['max_pressure'][idx])
                        spinbox.setValue(0)
                        spinbox.setDecimals(1)
                        spinbox.setSingleStep(0.1)

                        labelfirst.setText("%0.1f"%(self.settings['max_pressure'][idx]))
                        labellast.setText("%0.1f"%(self.settings['min_pressure'][idx]))

                        max_label = labelfirst
                        min_label = labellast

                    
                    cb_function_number = lambda value, g_idx=g_idx, s_idx=c_idx, slider=False: self.send_slider_value(g_idx,s_idx,value,slider)
                    cb_function_slider = lambda value, g_idx=g_idx, s_idx=c_idx, slider=True: self.send_slider_value(g_idx,s_idx,value,slider)
                    slider.valueChanged.connect(cb_function_slider)
                    spinbox.valueChanged.connect(cb_function_number)
               

                    if 'diff' in g_layout and c_idx == 0:

                        sublayout.addWidget(labelfirst)
                        sublayout.addWidget(slider)
                        sublayout.addWidget(labellast)
                        layout.addWidget(spinbox)
                        layout.addLayout(sublayout)
                    else:
                        layout.addWidget(labelfirst)
                        layout.addWidget(slider)
                        layout.addWidget(labellast)
                        layout.addWidget(spinbox)

                    layout.setAlignment(slider, Qt.AlignHCenter)
                    layout.setAlignment(spinbox, Qt.AlignHCenter)

                    layout_cluster.addLayout(layout)

                    slider_group['slider'] = slider
                    slider_group['number'] = spinbox
                    slider_group['max_label'] = max_label
                    slider_group['min_label'] = min_label
                    control_group['sliders'].append(slider_group)

                    group_layout.addLayout(layout_cluster)

                on_button=QPushButton()
                on_button.setCheckable(True)
                on_button.setText("Off")

                if self.settings['channel_states'][idx]:
                    on_button.toggle()
                    on_button.setText("On")

                on_button.clicked.connect(lambda state, g_idx=g_idx: self.send_channel_state(g_idx,state))

                group_layout.addWidget(on_button)

                row_layout.addLayout(group_layout)
                row_layout.addSpacing(20)

                control_group['on_off'] = on_button
                self.sliders.append(control_group)

                g_idx+=1

            all_rows_layout.addLayout(row_layout)

        sliderbox.addLayout(all_rows_layout)
コード例 #12
0
class EventTransmissionGUI(Plugin):
    """
    GUI to send events from User to logic state machine
    """
    def __init__(self, context):
        """
        Create Qt GUI using the event file
        """
        super(EventTransmissionGUI, self).__init__(context)
        self.setObjectName('ManualEventTriggerGUI')
        parser = ArgumentParser()

        # Add argument(s) to the parser.
        args = self._parse_args(context.argv())

        ## Create Event trigger
        self.event_trigger = RosEventTrigger(args.event_file)

        ## Parent container to store buttons, textboxes
        self._container = QTabWidget()
        ## Tab to display robot system, state machine status
        ## and command buttons without any arguments
        self._status_tab = QWidget()
        ## Tab to display positionyaw and velocityyaw commands
        ## and sliders for them
        self._additional_commands_tab = QWidget()
        # Set title of the parent container window
        self._status_tab.setWindowTitle(self.event_trigger.event_manager_name)
        ## Layout for status tab
        self._layout = QVBoxLayout()
        self._status_tab.setLayout(self._layout)

        # Create Textboxes and add to Layout
        self._layout.addWidget(QLabel('State Machine State'))
        ## Textbox to show sytem status
        self.system_status_textbox = QTextEdit()
        self.system_status_textbox.setReadOnly(True)
        self._layout.addWidget(self.system_status_textbox)

        # Define and connect buttons
        self._layout.addWidget(QLabel('Event Triggers'))
        ## Continer to store event triggering buttons
        self.button_container = QWidget()
        ## List of push buttons to trigger events
        self.push_buttons = list()
        ## Layout for the push buttons
        self.button_layout = QGridLayout()
        self.button_container.setLayout(self.button_layout)
        button_index = 0
        for event_name in self.event_trigger.event_names_list:
            self.push_buttons.append(QPushButton(event_name))
            partial_fun = partial(self.event_trigger.triggerEvent,
                                  event_name=event_name)
            self.push_buttons[-1].clicked.connect(partial_fun)
            row, col = self.get_row_col(button_index, args.grid_cols)
            self.button_layout.addWidget(self.push_buttons[-1], row, col)
            button_index += 1
        self._layout.addWidget(self.button_container)

        ## Layout for additional commands tab
        self._additional_commands_layout = QVBoxLayout()
        self._additional_commands_tab.setLayout(
            self._additional_commands_layout)
        # Create height slider
        self._additional_commands_layout.addWidget(
            QLabel('Pose Command Height (m)'))
        ## Height slider to adjust z coordinate for pose command
        ## \todo Matt: Load slider settings from param file
        self.height_slider = self.createSlider(1.0, 20.0, 2.0, 1.0)
        self._additional_commands_layout.addWidget(self.height_slider)
        ## Add button for triggering pose command
        ## Container for pose event related objects: slide etc
        ## \todo Matt: Reset slider value based on current quad height
        self.pose_command_container = QWidget()
        ## Pose command layout
        self.pose_command_layout = QGridLayout()
        self.pose_command_container.setLayout(self.pose_command_layout)
        ## x pose label to display position command from rviz to user
        self.pose_x = QLabel('x: -')
        ## y pose label to display position command from rviz to user
        self.pose_y = QLabel('y: -')
        ## z pose label to display position command from rviz to user
        self.pose_z = QLabel("z: {0:.2f}".format(self.height_slider.value()))
        self.height_slider.valueChanged.connect(
            partial(self.updateLabel,
                    header="z",
                    label=self.pose_z,
                    slider=self.height_slider))
        self.pose_command_layout.addWidget(self.pose_x, 0, 0)
        self.pose_command_layout.addWidget(self.pose_y, 0, 1)
        self.pose_command_layout.addWidget(self.pose_z, 0, 2)
        ## Button to send the pose command to state machine as poseyaw event
        self.send_pose_command_button = QPushButton("Send Pose Command")
        self.send_pose_command_button.clicked.connect(
            self.poseCommandButtonCallback)
        self.pose_command_layout.addWidget(self.send_pose_command_button, 0, 3)
        self._additional_commands_layout.addWidget(self.pose_command_container)
        ## Pose command container to store pose from Rviz and send to state
        ## machine
        self.pose_command = None
        ## Sliders for setting vx,vy,vz, yaw
        self.velocity_sliders = []
        ## Labels vx,vy,vz,yaw as an array
        self.velocity_command_labels = []
        self._additional_commands_layout.addWidget(
            QLabel('Velocity Command (m/s), Yaw (deg)'))
        for i in range(3):
            self.velocity_sliders.append(
                self.createSlider(-20.0, 20.0, 0.0, 1.0))
            self._additional_commands_layout.addWidget(
                self.velocity_sliders[i])
        # Slider for yaw
        self.velocity_sliders.append(
            self.createSlider(-180.0, 180.0, 0.0, 10.0))
        self._additional_commands_layout.addWidget(self.velocity_sliders[-1])
        ## Add button for triggering velocity yaw
        self.velocity_command_container = QWidget()
        ## Velocity command layout
        self.velocity_command_layout = QGridLayout()
        self.velocity_command_container.setLayout(self.velocity_command_layout)
        for i, axis_label in enumerate(['x', 'y', 'z']):
            # label to display velocity command from rviz to user
            self.velocity_command_labels.append(
                QLabel("v{0}: {1:.2f}".format(
                    axis_label, self.velocity_sliders[i].value() / 10.0)))
            self.velocity_sliders[i].valueChanged.connect(
                partial(self.updateLabel,
                        header="v" + str(i),
                        label=self.velocity_command_labels[i],
                        slider=self.velocity_sliders[i],
                        scale=10.0))
            self.velocity_command_layout.addWidget(
                self.velocity_command_labels[i], 0, i)
        # Label for yaw
        self.velocity_command_labels.append(
            QLabel("Yaw: {0:.2f}".format(self.velocity_sliders[-1].value())))
        self.velocity_sliders[-1].valueChanged.connect(
            partial(self.updateLabel,
                    header="Yaw",
                    label=self.velocity_command_labels[-1],
                    slider=self.velocity_sliders[-1]))
        self.velocity_command_layout.addWidget(
            self.velocity_command_labels[-1], 0, 3)
        ## Button to send the pose command to state machine as poseyaw event
        self.send_velocity_command_button = QPushButton(
            "Send Velocity Command")
        self.send_velocity_command_button.clicked.connect(
            self.velocityCommandButtonCallback)
        self.velocity_command_layout.addWidget(
            self.send_velocity_command_button, 0, 4)
        self._additional_commands_layout.addWidget(
            self.velocity_command_container)
        self._additional_commands_layout.addStretch()

        self._container.addTab(self._status_tab, "StatusBasicCommands")
        self._container.addTab(self._additional_commands_tab,
                               "AdditionalCommands")
        context.add_widget(self._container)

        # Add textboxes to update hooks from eventTrigger class
        # Define Partial callbacks
        systemStatusCallback = partial(self.updateStatus,
                                       text_box=self.system_status_textbox)
        # Connect Event Triggers
        self.event_trigger.status_signal.connect(systemStatusCallback)
        self.event_trigger.pose_command_signal.connect(
            self.poseCommandCallback)

    def createSlider(self, minimum, maximum, default_value, tick_interval):
        """
        Create a QtSlider with specified properties

        Parameters:
        @param minimum Minimum value for slider
        @param maximum Maximum value for slider
        @param default_value Initial value the slider is set to
        @param tick_inverval Integer value specifying difference between
                        successive ticks
        @return Slider value
        """
        slider = QSlider(Qt.Horizontal)
        slider.setMinimum(minimum)
        slider.setMaximum(maximum)
        slider.setValue(default_value)
        slider.setTickPosition(QSlider.TicksBelow)
        slider.setTickInterval(tick_interval)
        return slider

    def _parse_args(self, argv):
        """
        Parse extra arguments when plugin is deployed in standalone mode
        """
        parser = argparse.ArgumentParser(prog='aerial_autonomy',
                                         add_help=False)
        EventTransmissionGUI.add_arguments(parser)
        return parser.parse_args(argv)

    @staticmethod
    def add_arguments(parser):
        """
        Notify rqt_gui that this plugin can parse these extra arguments
        """
        group = parser.add_argument_group(
            'Options for aerial autonomy gui plugin')
        group.add_argument("-e",
                           "--event_file",
                           type=str,
                           default='',
                           help="Event file")
        group.add_argument("-c",
                           "--grid_cols",
                           type=int,
                           default=3,
                           help="Number of columns in grid")

    def get_row_col(self, button_index, ncols):
        """
        Automatically find the row and col to add the button
        to in a grid based on index of the button
        """
        col_index = button_index % ncols
        row_index = int((button_index - col_index) / ncols)
        return (row_index, col_index)

    def poseCommandCallback(self, pose):
        """
        Saves pose command and updates command display
        """
        self.pose_command = pose
        self.pose_x.setText("x: {0:.2f}".format(
            self.pose_command.pose.position.x))
        self.pose_y.setText("y: {0:.2f}".format(
            self.pose_command.pose.position.y))

    def poseCommandButtonCallback(self):
        """
        Publishes stored pose command after setting height from slider
        """
        if self.pose_command:
            self.pose_command.pose.position.z = self.height_slider.value()
            self.event_trigger.triggerPoseCommand(self.pose_command)
            # Reset pose command to avoid accidental triggering
            self.pose_command = None
            self.pose_x.setText('x: -')
            self.pose_y.setText('y: -')
        else:
            print "No pose command to trigger"

    def velocityCommandButtonCallback(self):
        """
        Publishes stored velocity command
        """
        velocity_command = VelocityYaw()
        velocity_command.vx = self.velocity_sliders[0].value() / 10.0
        velocity_command.vy = self.velocity_sliders[1].value() / 10.0
        velocity_command.vz = self.velocity_sliders[2].value() / 10.0
        velocity_command.yaw = self.velocity_sliders[3].value() * np.pi / 180.0
        self.event_trigger.triggerVelocityCommand(velocity_command)

    def updateLabel(self, header, label, slider, scale=1):
        """
        Updates height label based on slider value
        """
        label.setText(header + ": {0:.2f}".format(slider.value() / scale))

    def updateStatus(self, status, text_box):
        """
        Generic placeholder function to update text box
        """
        if not sip.isdeleted(text_box):
            text_box.setHtml(status)
            doc_size = text_box.document().size()
            text_box.setFixedHeight(doc_size.height() + 10)
コード例 #13
0
ファイル: label.py プロジェクト: sundermann/jsk_visualization
class StringLabelWidget(QWidget):
    def __init__(self):
        super(StringLabelWidget, self).__init__()
        self.lock = Lock()
        vbox = QVBoxLayout(self)
        self.label = QLabel()
        self.label.setAlignment(Qt.AlignLeft)
        self.label.setSizePolicy(
            QSizePolicy(QSizePolicy.Ignored, QSizePolicy.Ignored))
        font = QFont("Helvetica", 14)
        self.label.setFont(font)
        self.label.setWordWrap(True)
        vbox.addWidget(self.label)
        self.string_sub = None
        self._string_topics = []
        self._update_topic_timer = QTimer(self)
        self._update_topic_timer.timeout.connect(self.updateTopics)
        self._update_topic_timer.start(1000)
        self._active_topic = None
        # to update label visualization
        self._dialog = LineEditDialog()
        self._rosdata = None
        self._start_time = rospy.get_time()
        self._update_label_timer = QTimer(self)
        self._update_label_timer.timeout.connect(self.updateLabel)
        self._update_label_timer.start(40)

    def trigger_configuration(self):
        self._dialog.exec_()
        self.setupSubscriber(self._dialog.value)

    def updateLabel(self):
        if not self._rosdata:
            return
        try:
            _, data_y = self._rosdata.next()
        except rqt_plot.rosplot.RosPlotException as e:
            self._rosdata = None
            return
        if len(data_y) == 0:
            return
        latest = data_y[-1]  # get latest data
        # supports std_msgs/String as well as string data nested in rosmsg
        if type(latest) == String:
            self.string = latest.data
        else:
            self.string = latest
        try:
            self.label.setText(self.string)
        except TypeError as e:
            rospy.logwarn(e)

    def updateTopics(self):
        need_to_update = False
        for (topic, topic_type) in rospy.get_published_topics():
            msg = roslib.message.get_message_class(topic_type)
            field_names = get_slot_type_field_names(msg, slot_type='string')
            for field in field_names:
                string_topic = topic + field
                if string_topic not in self._string_topics:
                    self._string_topics.append(string_topic)
                    need_to_update = True
        if need_to_update:
            self._string_topics = sorted(self._string_topics)
            self._dialog.combo_box.clear()
            for topic in self._string_topics:
                self._dialog.combo_box.addItem(topic)
            if self._active_topic:
                if self._active_topic not in self._string_topics:
                    self._string_topics.append(self._active_topic)
                    self._dialog.combo_box.addItem(self._active_topic)
                self._dialog.combo_box.setCurrentIndex(
                    self._string_topics.index(self._active_topic))

    def setupSubscriber(self, topic):
        if not self._rosdata:
            self._rosdata = ROSData(topic, self._start_time)
        else:
            if self._rosdata != topic:
                self._rosdata.close()
                self._rosdata = ROSData(topic, self._start_time)
            else:
                rospy.logwarn("%s is already subscribed", topic)
        self._active_topic = topic

    def onActivated(self, number):
        self.setupSubscriber(self._string_topics[number])

    def save_settings(self, plugin_settings, instance_settings):
        if self._active_topic:
            instance_settings.set_value("active_topic", self._active_topic)

    def restore_settings(self, plugin_settings, instance_settings):
        if instance_settings.value("active_topic"):
            topic = instance_settings.value("active_topic")
            self._dialog.combo_box.addItem(topic)
            self.setupSubscriber(topic)
コード例 #14
0
ファイル: gui.py プロジェクト: maximkulkin/rqt_joint_control
    def __init__(self, node_ns, controller, urdf=None, *args, **kwargs):
        super(JointPositionControllerWidget, self).__init__(*args, **kwargs)
        self._controller = controller
        self._urdf = urdf

        loadUi('Controller.ui', self)
        self.controller_name.setText(controller.name)
        self.controller_type.setText(controller.type)

        controller_ns = rospy.resolve_name(controller.name, node_ns)

        joint_name_widget = QLabel()

        joint_position_widget = QLineEdit()
        joint_position_widget.setReadOnly(True)
        joint_position_widget.setAlignment(Qt.AlignRight)
        joint_position_widget.setText(str(0.0))

        extras_layout = QGridLayout()
        extras_layout.addWidget(joint_name_widget, 0, 0)
        extras_layout.addWidget(joint_position_widget, 0, 1)

        extras = QWidget()
        extras.setLayout(extras_layout)
        self.layout().addWidget(extras)

        slider_range = (0, 10000)
        limits = {'min': 0.0, 'max': 3.14}

        if self._urdf is not None:
            joint_name = next(iter([
                resource
                for interface in controller.claimed_resources
                if interface.hardware_interface == \
                    'hardware_interface::PositionJointInterface'
                for resource in interface.resources
            ]), None)

            if joint_name is not None:
                joint_name_widget.setText(joint_name)

                joint_limit = self._urdf.xpath(
                    '/robot/joint[@name=$name]/limit', name=joint_name)
                if joint_limit:
                    l = joint_limit[0]
                    limits = {
                        'min': float(joint_limit[0].get('lower')),
                        'max': float(joint_limit[0].get('upper'))
                    }

        pub = rospy.Publisher(controller_ns + '/command',
                              std_msgs.msg.Float64,
                              queue_size=10)

        def on_value_changed(value):
            value = limits['min'] + value / float(
                slider_range[1] - slider_range[0]) * (limits['max'] -
                                                      limits['min'])
            joint_position_widget.setText(str(value))
            pub.publish(value)

        slider = QSlider(Qt.Horizontal)
        slider.setRange(slider_range[0], slider_range[1])
        slider.valueChanged.connect(on_value_changed)

        extras_layout.addWidget(slider, 1, 0, 1, 2)
コード例 #15
0
    def __init__(self, title, jsp, num_rows=0):
        super(JointStatePublisherGui, self).__init__()
        font = QFont("Helvetica", 9, QFont.Bold)
        self.hlayout = QHBoxLayout(self)
        vlayout = QVBoxLayout()
        glayout = QGridLayout()
        right_l_lauout = QVBoxLayout()
        self.listVeiw = QListWidget()
        self.checkbox = []
        self.value_line_edit = []
        self.sliders = []
        self.positions = []
        self.progressbars = []

        self.value_last = []

        speed_max = enviromnt_conf['joint_speed']
        slider_max = speed_max * 1000

        position_max = enviromnt_conf['joint_max_position']
        progress_max = position_max * 1000

        #create joints widget
        for i in range(0, num_rows):
            if config[i][0]:
                g_in_g = QGridLayout()
                checkbox = QCheckBox(config[i][1])
                checkbox.setFont(font)

                self.checkbox.append(checkbox)

                value_line_edit = QLineEdit()
                value_line_edit.setFont(font)
                value_line_edit.setText("0.0")

                self.value_line_edit.append(value_line_edit)

                display_lable = QLabel()
                display_lable.setFont(font)
                display_lable.setText("Position:")

                position_label = QLabel()
                position_label.setFont(font)
                position_label.setText("0.0")

                self.positions.append(position_label)

                position_progress_bar = QProgressBar()
                position_progress_bar.setMaximum(progress_max)
                position_progress_bar.setMinimum(-progress_max)
                position_progress_bar.setValue(0)

                self.progressbars.append(position_progress_bar)

                slider = QSlider()
                slider.setMaximum(slider_max)
                slider.setMinimum(-slider_max)
                slider.setOrientation(Qt.Horizontal)
                slider.valueChanged.connect(self.slider_value_changed)
                self.sliders.append(slider)

                g_in_g.addWidget(checkbox, 0, 0)
                g_in_g.addWidget(value_line_edit, 0, 1)
                g_in_g.addWidget(display_lable, 0, 2)
                g_in_g.addWidget(position_label, 0, 3)
                g_in_g.addWidget(slider, 1, 0, 1, 2)
                g_in_g.addWidget(position_progress_bar, 1, 2, 1, 2)

                glayout.addLayout(g_in_g, i, 0)

        #create v layout
        self.import_Btn = QPushButton('Import')
        self.import_Btn.setFont(font)
        self.import_Btn.clicked.connect(self.import_Btn_clecked)

        self.export_Btn = QPushButton('Export')
        self.export_Btn.setFont(font)
        self.export_Btn.clicked.connect(self.export_Btn_clicked)

        self.start_Btn = QPushButton("Start")
        self.start_Btn.setFont(font)
        self.start_Btn.clicked.connect(self.start_Btn_clicked)

        self.reset_Btn = QPushButton('Reset')
        self.reset_Btn.setFont(font)
        self.reset_Btn.clicked.connect(self.reset_Btn_clicked)

        self.record_Btn = QPushButton('Record')
        self.record_Btn.setFont(font)
        self.record_Btn.clicked.connect(self.record_Btn_clicked)

        self.replay_Btn = QPushButton('Repaly')
        self.replay_Btn.setFont(font)
        self.replay_Btn.clicked.connect(self.replay_Btn_clicked)

        self.delete_Btn = QPushButton("Delete")
        self.delete_Btn.setFont(font)
        self.delete_Btn.clicked.connect(self.delete_Btn_clicked)

        self.debug_Btn = QPushButton("Debug")
        self.debug_Btn.setFont(font)
        self.debug_Btn.clicked.connect(self.debug_Btn_clicked)

        vlayout.addWidget(self.import_Btn)
        vlayout.addWidget(self.export_Btn)
        vlayout.addWidget(self.start_Btn)
        vlayout.addWidget(self.reset_Btn)
        vlayout.addWidget(self.record_Btn)
        vlayout.addWidget(self.delete_Btn)
        vlayout.addWidget(self.replay_Btn)
        vlayout.addWidget(self.debug_Btn)

        self.master_url = QLineEdit("http://192.168.0.91:11311")
        self.master_url.setFont(font)

        self.master_ip = QLineEdit("192.168.0.91")
        self.master_ip.setFont(font)

        self.listVeiw.clicked.connect(self.listVeiw_clicked)
        self.listVeiw.currentRowChanged.connect(
            self.listVeiw_itemSelectionChanged)

        self.description = QTextEdit("")
        self.description.setFont(font)

        #self.description.setGeometry(0,100,100,500)

        right_l_lauout.addWidget(self.master_url)
        right_l_lauout.addWidget(self.master_ip)
        right_l_lauout.addWidget(self.listVeiw)
        right_l_lauout.addWidget(self.description)

        right_l_lauout.setStretch(0, 1)
        right_l_lauout.setStretch(1, 1)
        right_l_lauout.setStretch(2, 3)
        right_l_lauout.setStretch(3, 1)

        self.num_rows = len(self.checkbox)
        self.hlayout.addLayout(glayout)
        self.hlayout.addLayout(vlayout)
        self.hlayout.addLayout(right_l_lauout)
        self.setLayout(self.hlayout)

        self.callback_start = None
        self.callback_pause = None
        self.callback_record = None
        self.callback_reset = None
        self.callback_replay = None
        self.callback_replay_stop = None
        self.callback_delete = None
        self.callback_debug = None
        self.callback_import = None
        self.callback_export = None
        self.callback_list_clicked = None

        self.listVeiw_isClicked = False
        self.listVeiw_current_item = 0
        self.listVeiw_len = 0
        self.f = QFileDialog()
コード例 #16
0
class HandEyeCalibration(Plugin):
    PLUGIN_TITLE = ' Intel OTC Robotics: Hand-Eye Calibration'

    def __init__(self, context):
        super(HandEyeCalibration, self).__init__(context)
        self.context = context
        self.node = context.node
        self.widget = QWidget()
        self.widget.setObjectName(self.PLUGIN_TITLE)
        self.widget.setWindowTitle(self.PLUGIN_TITLE)

        # Data
        self.Tsamples = []

        # Toolbar
        _, path_pkg = get_resource('packages', 'handeye_dashboard')
        print("{}".format(path_pkg))

        self.snapshot_action = QAction(QIcon.fromTheme('camera-photo'),
                                       'Take a snapshot', self.widget)
        path = path_pkg + '/share/handeye_dashboard/images/capture.png'
        self.calibrate_action = QAction(QIcon(QPixmap.fromImage(QImage(path))),
                                        'Get the camera/robot transform',
                                        self.widget)
        self.clear_action = QAction(QIcon.fromTheme('edit-clear'),
                                    'Clear the record data.', self.widget)
        path = path_pkg + '/share/handeye_dashboard/images/UR5.png'
        self.execut_action = QAction(QIcon(QPixmap.fromImage(QImage(path))),
                                     'EStart the publishing the TF.',
                                     self.widget)
        self.toolbar = QToolBar()
        self.toolbar.addAction(self.snapshot_action)
        self.toolbar.addAction(self.calibrate_action)
        self.toolbar.addAction(self.clear_action)
        self.toolbar.addAction(self.execut_action)

        # Toolbar0
        self.l0 = QLabel(self.widget)
        self.l0.setText("Camera-Mount-Type: ")
        self.l0.setFixedWidth(150)
        self.l0.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignVCenter)
        self.combobox = QComboBox(self.widget)
        self.combobox.addItem('attached on robot')
        self.combobox.addItem('fixed beside robot')
        self.toolbar0 = QToolBar()
        self.toolbar0.addWidget(self.l0)
        self.toolbar0.addWidget(self.combobox)

        # Toolbar1
        self.l1 = QLabel(self.widget)
        self.l1.setText("Camera-Frame: ")
        self.l1.setFixedWidth(150)
        self.l1.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignVCenter)
        self.camera_frame = QLineEdit(self.widget)
        self.camera_frame.setText("camera_link")
        self.toolbar1 = QToolBar()
        self.toolbar1.addWidget(self.l1)
        self.toolbar1.addWidget(self.camera_frame)

        # Toolbar2
        self.l2 = QLabel(self.widget)
        self.l2.setText("Object-Frame: ")
        self.l2.setFixedWidth(150)
        self.l2.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignVCenter)
        self.object_frame = QLineEdit(self.widget)
        self.object_frame.setText("calib_board")
        self.toolbar2 = QToolBar()
        self.toolbar2.addWidget(self.l2)
        self.toolbar2.addWidget(self.object_frame)

        # Toolbar3
        self.l3 = QLabel(self.widget)
        self.l3.setText("Robot-Base-Frame: ")
        self.l3.setFixedWidth(150)
        self.l3.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignVCenter)
        self.base_frame = QLineEdit(self.widget)
        self.base_frame.setText("base")
        self.toolbar3 = QToolBar()
        self.toolbar3.addWidget(self.l3)
        self.toolbar3.addWidget(self.base_frame)

        # Toolbar4
        self.l4 = QLabel(self.widget)
        self.l4.setText("End-Effector-Frame: ")
        self.l4.setFixedWidth(150)
        self.l4.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignVCenter)
        self.endeffector_frame = QLineEdit(self.widget)
        self.endeffector_frame.setText("tool0")
        self.toolbar4 = QToolBar()
        self.toolbar4.addWidget(self.l4)
        self.toolbar4.addWidget(self.endeffector_frame)

        # Toolbar5
        self.l5 = QLabel(self.widget)
        self.l5.setText("Sample-Number: ")
        self.l5.setFixedWidth(150)
        self.l5.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignVCenter)
        self.le5 = QLineEdit(self.widget)
        self.le5.setValidator(QIntValidator())
        self.le5.setText('10')
        self.le5.setReadOnly(True)
        self.toolbar5 = QToolBar()
        self.toolbar5.addWidget(self.l5)
        self.toolbar5.addWidget(self.le5)

        # TreeView
        self.treeview = QTreeView()
        self.treeview.setAlternatingRowColors(True)
        self.model = QStandardItemModel(self.treeview)
        self.treeview.setModel(self.model)
        self.treeview.setHeaderHidden(True)

        # TextEdit
        self.textedit = QTextEdit(self.widget)
        self.textedit.setReadOnly(True)

        # Layout
        self.layout = QVBoxLayout()
        self.layout.addWidget(self.toolbar0)
        self.layout.addWidget(self.toolbar1)
        self.layout.addWidget(self.toolbar2)
        self.layout.addWidget(self.toolbar3)
        self.layout.addWidget(self.toolbar4)
        self.layout.addWidget(self.toolbar5)
        self.layout.addWidget(self.toolbar)
        self.layoutH = QHBoxLayout()
        self.layoutH.addWidget(self.treeview)
        self.layoutH.addWidget(self.textedit)
        self.layout.addLayout(self.layoutH)
        self.widget.setLayout(self.layout)
        # Add the widget to the user interface
        if context.serial_number() > 1:
            self.widget.setWindowTitle(self.widget.windowTitle() +
                                       (' (%d)' % context.serial_number()))
        context.add_widget(self.widget)
        # Make the connections
        self.snapshot_action.triggered.connect(self.take_snapshot)
        self.calibrate_action.triggered.connect(self.calibration)
        self.clear_action.triggered.connect(self.clear)
        self.execut_action.triggered.connect(self.execution)

        # Package path
        self.path_pkg = path_pkg

        # Set up TF
        self.cli = self.node.create_client(HandeyeTF, 'handeye_tf_service')
        while not self.cli.wait_for_service(timeout_sec=1.0):
            self.node.get_logger().info(
                'service not available, waiting again...')
        self.req = HandeyeTF.Request()

    def clear(self):
        # >>> Clear the recorded samples
        self.textedit.append('Clearing the recorded data ...')
        self.textedit.clear()
        self.Tsamples = []
        self.model.clear()

    def get_tf_transform(self, frame_id, child_frame_id):
        self.req.transform.header.frame_id = frame_id
        self.req.transform.child_frame_id = child_frame_id
        self.req.publish.data = False

        future = self.cli.call_async(self.req)
        rclpy.spin_until_future_complete(self.node, future)

        transform = TransformStamped()

        try:
            result = future.result()
        except Exception as e:
            self.node.get_logger().info('Service call failed %r' % (e, ))
        else:
            transform = result.tf_lookup_result

        return transform

    def publish_tf_transform(self, transform_to_publish):
        self.req.publish.data = True
        self.req.transform = transform_to_publish

        future = self.cli.call_async(self.req)
        rclpy.spin_until_future_complete(self.node, future)

        try:
            future.result()
        except Exception as e:
            self.node.get_logger().info('Service call failed %r' % (e, ))
        else:
            self.node.get_logger().info(
                'Send the camera-robot transform :\n\tfrom `{}` to `{}`.'.
                format(self.req.transform.header.frame_id,
                       self.req.transform.child_frame_id))

    def take_snapshot(self):
        # >>> Take the snapshot
        self.textedit.append('Taking snapshot ...')

        # Get the transform from `tool0` to `base_link`
        T = self.get_tf_transform(self.base_frame.text(),
                                  self.endeffector_frame.text())
        bTe = np.zeros((4, 4))
        q = [
            T.transform.rotation.w, T.transform.rotation.x,
            T.transform.rotation.y, T.transform.rotation.z
        ]
        bTe = br.quaternion.to_transform(q)
        bTe[:3, 3] = np.array([
            T.transform.translation.x, T.transform.translation.y,
            T.transform.translation.z
        ])
        self.textedit.append('Lookup transform: from `{}` to `{}`.'.format(
            self.base_frame.text(), self.endeffector_frame.text()))
        self.node.get_logger().info(bcolors.OKGREEN + 'bTe:' + bcolors.ENDC +
                                    '\n{}'.format(bTe))

        # Get the transform from `calib_board` to `camera_link`
        T = self.get_tf_transform(self.camera_frame.text(),
                                  self.object_frame.text())
        cTo = np.zeros((4, 4))
        q = [
            T.transform.rotation.w, T.transform.rotation.x,
            T.transform.rotation.y, T.transform.rotation.z
        ]
        cTo = br.quaternion.to_transform(q)
        cTo[:3, 3] = np.array([
            T.transform.translation.x, T.transform.translation.y,
            T.transform.translation.z
        ])
        self.textedit.append('Lookup transform: from `{}` to `{}`.'.format(
            self.camera_frame.text(), self.object_frame.text()))
        self.node.get_logger().info(bcolors.OKGREEN + 'cTo:' + bcolors.ENDC +
                                    '\n{}'.format(cTo))

        parent = QStandardItem('Snapshot {}'.format(len(self.Tsamples)))
        child_1 = QStandardItem('bTe:\n{}\n{}\n{}\n{}'.format(
            bTe[0, :], bTe[1, :], bTe[2, :], bTe[3, :]))
        child_2 = QStandardItem('cTo:\n{}\n{}\n{}\n{}'.format(
            cTo[0, :], cTo[1, :], cTo[2, :], cTo[3, :]))
        parent.appendRow(child_1)
        parent.appendRow(child_2)
        self.model.appendRow(parent)
        self.Tsamples.append((bTe, cTo))
        self.le5.setText(str(len(self.Tsamples)))

    def calibration(self):
        # >>> Compute the calibration
        self.textedit.append('Making the calibration ...')
        if len(self.Tsamples) == 0:
            self.textedit.append(
                'No transform recorded, please take snapshots.')
            return
        # save samples to `dataset.json` file
        save_samples_to_file(self.Tsamples)
        import handeye
        if self.combobox.currentIndex() == 0:
            solver_cri = handeye.calibrator.HandEyeCalibrator(setup='Moving')
        if self.combobox.currentIndex() == 1:
            solver_cri = handeye.calibrator.HandEyeCalibrator(setup='Fixed')
        for sample in self.Tsamples:
            solver_cri.add_sample(sample[0], sample[1])
        try:
            bTc = solver_cri.solve(method=handeye.solver.Daniilidis1999)
            # save the calibration result to 'camera-robot.json' file
            file_output = '/tmp/' + 'camera-robot.json'
            with open(file_output, 'w') as f:
                json.dump(bTc.tolist(), f)
        except Exception:
            self.textedit.append("Failed to solve the hand-eye calibration.")

    def execution(self):
        # >>> Publish the camera-robot transform
        self.textedit.append('Publishing the camera TF ...')
        file_input = '/tmp/' + 'camera-robot.json'
        with open(file_input, 'r') as f:
            datastore = json.load(f)

        to_frame = self.camera_frame.text()
        if self.combobox.currentIndex() == 0:
            from_frame = self.endeffector_frame.text()
        if self.combobox.currentIndex() == 1:
            from_frame = self.base_frame.text()

        bTc = np.array(datastore)
        static_transformStamped = TransformStamped()
        static_transformStamped.header.stamp = ROSClock().now().to_msg()
        static_transformStamped.header.frame_id = from_frame
        static_transformStamped.child_frame_id = to_frame

        static_transformStamped.transform.translation.x = bTc[0, 3]
        static_transformStamped.transform.translation.y = bTc[1, 3]
        static_transformStamped.transform.translation.z = bTc[2, 3]

        q = br.transform.to_quaternion(bTc)
        static_transformStamped.transform.rotation.x = q[1]
        static_transformStamped.transform.rotation.y = q[2]
        static_transformStamped.transform.rotation.z = q[3]
        static_transformStamped.transform.rotation.w = q[0]

        self.publish_tf_transform(static_transformStamped)

        output_string = "camera-robot pose:\n"
        output_string += "  Translation: [{}, {}, {}]\n".format(
            bTc[0, 3], bTc[1, 3], bTc[2, 3])
        output_string += "  Rotation: in Quaternion [{}, {}, {}, {}]".format(
            q[0], q[1], q[2], q[3])
        file_path = '/tmp/' + 'camera-robot.txt'
        with open(file_path, 'w') as f:
            f.write(output_string)

    def shutdown_plugin(self):
        """
    Unregister subscribers when the plugin shutdown
    """
        pass

    def save_settings(self, plugin_settings, instance_settings):
        # Nothing to be done here
        pass

    def restore_settings(self, plugin_settings, instance_settings):
        # Nothing to be done here
        pass
コード例 #17
0
    def init_sliders(self):
        sliderbox = self._widget.findChild(QLayout, 'Sliders')

        graph_button = QPushButton()
        graph_button.setCheckable(True)
        graph_button.setText("Graph Off")
        graph_button.toggle()
        graph_button.clicked.connect(self.set_graph_state)
        self.graph_button = graph_button

        firstCol = QVBoxLayout()
        firstCol.addWidget(graph_button)

        sliderbox.addLayout(firstCol)

        self.sliders = []

        all_rows_layout = QVBoxLayout()
        chan_idx = 0
        for num_channels_row in self.settings['num_channels']:
            row_layout = QHBoxLayout()
            for i in range(num_channels_row):
                idx = chan_idx * 1

                slider_group = {
                    'slider_p': None,
                    'number_p': None,
                    'slider_i': None,
                    'number_i': None,
                    'slider_d': None,
                    'number_d': None,
                    'on_off': None
                }

                layout_cluster = QVBoxLayout()
                slider_cluster = QHBoxLayout()
                label = QLabel()
                label.setText("Chan. %d" % (idx + 1))
                label.setAlignment(Qt.AlignCenter)
                layout_cluster.addWidget(label)
                for j in range(3):
                    layout = QVBoxLayout()
                    layout.setAlignment(Qt.AlignHCenter)

                    if j == 0:
                        maxrange = 1.0
                    elif j == 1:
                        maxrange = 10
                    elif j == 2:
                        maxrange = 10

                    slider = QSlider(Qt.Vertical)
                    slider.setMinimum(0)
                    slider.setMaximum(maxrange * 100)
                    slider.setValue(self.settings['pid_gains'][chan_idx][j] *
                                    100)
                    slider.setTickPosition(QSlider.TicksRight)
                    slider.setTickInterval(maxrange / 100.0)

                    spinbox = QDoubleSpinBox()
                    spinbox.setMinimum(0)
                    spinbox.setMaximum(maxrange)
                    spinbox.setValue(self.settings['pid_gains'][chan_idx][j])
                    spinbox.setDecimals(2)
                    spinbox.setSingleStep(maxrange / 100.0)

                    cb_function_number = lambda value, idx=idx, gain_idx=j, slider=False: self.send_slider_value(
                        idx, gain_idx, value, slider)
                    cb_function_slider = lambda value, idx=idx, gain_idx=j, slider=True: self.send_slider_value(
                        idx, gain_idx, value, slider)

                    slider.valueChanged.connect(cb_function_slider)
                    spinbox.valueChanged.connect(cb_function_number)

                    label = QLabel()
                    label.setAlignment(Qt.AlignCenter)

                    if j == 0:
                        slider_group['slider_p'] = slider
                        slider_group['number_p'] = spinbox
                        label.setText("P")
                    elif j == 1:
                        slider_group['slider_i'] = slider
                        slider_group['number_i'] = spinbox
                        label.setText("I")
                    elif j == 2:
                        slider_group['slider_d'] = slider
                        slider_group['number_d'] = spinbox
                        label.setText("D")

                    labelmax = QLabel()
                    labelmax.setAlignment(Qt.AlignCenter)
                    labelmax.setText("%0.1f" % (maxrange))

                    labelmin = QLabel()
                    labelmin.setAlignment(Qt.AlignCenter)
                    labelmin.setText("0")

                    layout.addWidget(label)
                    layout.addWidget(labelmax)
                    layout.addWidget(slider, Qt.AlignHCenter)
                    layout.addWidget(labelmin)
                    layout.addWidget(spinbox, Qt.AlignHCenter)
                    layout.setAlignment(slider, Qt.AlignHCenter)
                    layout.setAlignment(spinbox, Qt.AlignHCenter)

                    slider_cluster.addLayout(layout)

                on_button = QPushButton()
                on_button.setCheckable(True)
                on_button.setText("Off")

                if self.settings['channel_states'][chan_idx]:
                    on_button.toggle()
                    on_button.setText("On")

                on_button.clicked.connect(
                    lambda state, idx=idx: self.send_channel_state(idx, state))

                slider_group['on_off'] = on_button

                layout_cluster.addLayout(slider_cluster)
                layout_cluster.addWidget(on_button)

                row_layout.addLayout(layout_cluster)
                row_layout.addSpacing(20)

                self.sliders.append(slider_group)
                chan_idx += 1

            all_rows_layout.addLayout(row_layout)
        sliderbox.addLayout(all_rows_layout)
コード例 #18
0
ファイル: param_groups.py プロジェクト: Dieptranivsr/Ros_Diep
class GroupWidget(QWidget):
    """
    (Isaac's guess as of 12/13/2012)
    This class bonds multiple Editor instances that are associated with
    a single node as a group.
    """

    # public signal
    sig_node_disabled_selected = Signal(str)
    sig_node_state_change = Signal(bool)

    def __init__(self, updater, config, nodename):
        """
        :param config:
        :type config: Dictionary? defined in dynamic_reconfigure.client.Client
        :type nodename: str
        """
        super(GroupWidget, self).__init__()
        self.state = config['state']
        self.param_name = config['name']
        self._toplevel_treenode_name = nodename

        # TODO: .ui file needs to be back into usage in later phase.
        #        ui_file = os.path.join(rp.get_path('rqt_reconfigure'),
        #                               'resource', 'singlenode_parameditor.ui')
        #        loadUi(ui_file, self)

        verticalLayout = QVBoxLayout(self)
        verticalLayout.setContentsMargins(QMargins(0, 0, 0, 0))

        _widget_nodeheader = QWidget()
        _h_layout_nodeheader = QHBoxLayout(_widget_nodeheader)
        _h_layout_nodeheader.setContentsMargins(QMargins(0, 0, 0, 0))

        self.nodename_qlabel = QLabel(self)
        font = QFont('Trebuchet MS, Bold')
        font.setUnderline(True)
        font.setBold(True)

        # Button to close a node.
        _icon_disable_node = QIcon.fromTheme('window-close')
        _bt_disable_node = QPushButton(_icon_disable_node, '', self)
        _bt_disable_node.setToolTip('Hide this node')
        _bt_disable_node_size = QSize(36, 24)
        _bt_disable_node.setFixedSize(_bt_disable_node_size)
        _bt_disable_node.pressed.connect(self._node_disable_bt_clicked)

        _h_layout_nodeheader.addWidget(self.nodename_qlabel)
        _h_layout_nodeheader.addWidget(_bt_disable_node)

        self.nodename_qlabel.setAlignment(Qt.AlignCenter)
        font.setPointSize(10)
        self.nodename_qlabel.setFont(font)
        grid_widget = QWidget(self)
        self.grid = QFormLayout(grid_widget)
        verticalLayout.addWidget(_widget_nodeheader)
        verticalLayout.addWidget(grid_widget, 1)
        # Again, these UI operation above needs to happen in .ui file.

        self.tab_bar = None  # Every group can have one tab bar
        self.tab_bar_shown = False

        self.updater = updater

        self.editor_widgets = []
        self._param_names = []

        self._create_node_widgets(config)

        logging.debug('Groups node name={}'.format(nodename))
        self.nodename_qlabel.setText(nodename)

        # Labels should not stretch
        # self.grid.setColumnStretch(1, 1)
        # self.setLayout(self.grid)

    def collect_paramnames(self, config):
        pass

    def _create_node_widgets(self, config):
        """
        :type config: Dict?
        """
        i_debug = 0
        for param in config['parameters']:
            begin = time.time() * 1000
            editor_type = '(none)'

            if param['edit_method']:
                widget = EnumEditor(self.updater, param)
            elif param['type'] in EDITOR_TYPES:
                logging.debug('GroupWidget i_debug={} param type ={}'.format(
                    i_debug, param['type']))
                editor_type = EDITOR_TYPES[param['type']]
                widget = eval(editor_type)(self.updater, param)

            self.editor_widgets.append(widget)
            self._param_names.append(param['name'])

            logging.debug(
                'groups._create_node_widgets num editors={}'.format(i_debug))

            end = time.time() * 1000
            time_elap = end - begin
            logging.debug('ParamG editor={} loop=#{} Time={}msec'.format(
                editor_type, i_debug, time_elap))
            i_debug += 1

        for name, group in sorted(config['groups'].items()):
            if group['type'] == 'tab':
                widget = TabGroup(self, self.updater, group,
                                  self._toplevel_treenode_name)
            elif group['type'] in _GROUP_TYPES.keys():
                widget = eval(_GROUP_TYPES[group['type']])(
                    self.updater, group, self._toplevel_treenode_name)
            else:
                widget = eval(_GROUP_TYPES[''])(self.updater, group,
                                                self._toplevel_treenode_name)

            self.editor_widgets.append(widget)
            logging.debug('groups._create_node_widgets name={}'.format(name))

        for i, ed in enumerate(self.editor_widgets):
            ed.display(self.grid)

        logging.debug('GroupWdgt._create_node_widgets'
                      ' len(editor_widgets)={}'.format(len(
                          self.editor_widgets)))

    def display(self, grid):
        grid.addRow(self)

    def update_group(self, config):
        if 'state' in config:
            old_state = self.state
            self.state = config['state']
            if self.state != old_state:
                self.sig_node_state_change.emit(self.state)

        names = [name for name in config.keys()]

        for widget in self.editor_widgets:
            if isinstance(widget, EditorWidget):
                if widget.param_name in names:
                    widget.update_value(config[widget.param_name])
            elif isinstance(widget, GroupWidget):
                cfg = find_cfg(config, widget.param_name) or config
                widget.update_group(cfg)

    def close(self):
        for w in self.editor_widgets:
            w.close()

    def get_treenode_names(self):
        """
        :rtype: str[]
        """
        return self._param_names

    def _node_disable_bt_clicked(self):
        logging.debug('param_gs _node_disable_bt_clicked')
        self.sig_node_disabled_selected.emit(self._toplevel_treenode_name)
コード例 #19
0
class MetricsRefboxWidget(QWidget):

    status_signal = pyqtSignal(object)
    timeout_signal = pyqtSignal(object)
    result_signal = pyqtSignal(object)
    bagfile_signal = pyqtSignal(object)

    def __init__(self):
        super(MetricsRefboxWidget, self).__init__()
        self.status_signal.connect(self.update_status)
        self.timeout_signal.connect(self._handle_timeout)
        self.result_signal.connect(self.update_result)
        self.bagfile_signal.connect(self._update_bagfile_name)
        self.timer = QElapsedTimer()
        self.timer_interrupt = QTimer(self)
        self.timer_interrupt.timeout.connect(self.update_timer)
        self.timer_interrupt.start(100)

        self.metrics_refbox = metrics_refbox.MetricsRefbox(
            self._status_cb, self._start_cb, self._result_cb)
        self.trial_configs = {}
        self.current_benchmark = None
        self.current_benchmark_enum = -1
        # once we receive confirmation from robot
        self.benchmark_running = False
        # once we send the start message to robot
        self.benchmark_started = False
        # set to True if we're not recording individual trials
        # but rather continuously recording all data (hence no timeouts)
        self.continuous_recording = False
        self.timeout = False
        self.stopped = False
        self.result_msg = None
        self.config_locked = True
        self.config = self.metrics_refbox.get_config()
        self.setup()
        self.show()

    def setup(self):
        layout = QGridLayout()

        # Sidebar
        self.benchmark_combo_box = QComboBox()
        for key in self.config['benchmarks'].keys():
            self.benchmark_combo_box.addItem(
                self.config['benchmarks'][key]['name'])
        self.benchmark_combo_box.currentIndexChanged.connect(
            self._handle_benchmark_selected)
        self.benchmark_combo_box.setMaximumWidth(SIDEBAR_WIDTH)
        self.set_current_benchmark()

        self.team_combo_box = QComboBox()
        for key in self.config['teams']:
            self.team_combo_box.addItem(key)
        self.team_combo_box.setMaximumWidth(SIDEBAR_WIDTH)
        self.test_communication_button = QPushButton('Test communication')
        self.test_communication_button.clicked.connect(self._handle_test_comm)

        self.trial_list_widget = QListWidget()
        self.trial_list_widget.currentItemChanged.connect(
            self._handle_trial_change)
        self.trial_list_widget.setMaximumWidth(SIDEBAR_WIDTH)

        sidebar_layout = QVBoxLayout()
        sidebar_layout.addWidget(QLabel("Team"))
        sidebar_layout.addWidget(self.team_combo_box)
        sidebar_layout.addWidget(self.test_communication_button)
        sidebar_layout.addWidget(QLabel("Benchmark"))
        sidebar_layout.addWidget(self.benchmark_combo_box)
        sidebar_layout.addWidget(QLabel("Trial"))
        sidebar_layout.addWidget(self.trial_list_widget)

        self.generate_button = QPushButton('Generate')
        self.generate_button.clicked.connect(self._handle_generate)

        self.delete_button = QPushButton('Delete')
        self.delete_button.clicked.connect(self._handle_delete)

        self.save_trials_button = QPushButton('Save')
        self.save_trials_button.clicked.connect(self._handle_save_trial_config)

        self.lock_button = QPushButton('Lock')
        if self.config_locked:
            self.lock_button.setText('Unlock')
        self.lock_button.clicked.connect(self._handle_lock)

        sidebar_button_layout = QGridLayout()
        sidebar_button_layout.addWidget(self.generate_button, 0, 0)
        sidebar_button_layout.addWidget(self.delete_button, 0, 1)
        sidebar_button_layout.addWidget(self.save_trials_button, 1, 0)
        sidebar_button_layout.addWidget(self.lock_button, 1, 1)
        sidebar_layout.addLayout(sidebar_button_layout)

        layout.addLayout(sidebar_layout, 0, 0)

        # Status box
        self.status = QPlainTextEdit()
        self.status.setReadOnly(True)
        self.status.setMaximumHeight(200)

        # row 1, col 0, rowspan 1, colspan 2
        layout.addWidget(self.status, 1, 0, 1, 2)

        # trial config and results
        trial_layout = QVBoxLayout()
        self.trial_config_layout = QHBoxLayout()
        self.trial_results_layout = QVBoxLayout()
        self.setup_trial_config()

        # benchmark trial controls
        benchmark_controls_group_box = QGroupBox('Controls')
        benchmark_controls_layout = QHBoxLayout()
        self.start_trial_button = QPushButton('Start')
        self.start_trial_button.clicked.connect(self._handle_start_trial)
        self.stop_trial_button = QPushButton('Stop')
        self.stop_trial_button.clicked.connect(self._handle_stop_trial)
        self.prev_trial_button = QPushButton('Previous')
        self.prev_trial_button.clicked.connect(self._handle_prev_trial)
        self.next_trial_button = QPushButton('Next')
        self.next_trial_button.clicked.connect(self._handle_next_trial)
        self.start_continuous_recording_button = QPushButton(
            'Start continuous recording')
        self.start_continuous_recording_button.clicked.connect(
            self._handle_continuous_recording)

        self.timer_field = QLabel()
        font = QFont("Arial", 20, QFont.Bold)
        self.timer_field.setFont(font)
        self.timer_field.setAutoFillBackground(True)
        benchmark_controls_layout.addWidget(self.start_trial_button)
        benchmark_controls_layout.addWidget(self.stop_trial_button)
        benchmark_controls_layout.addWidget(self.prev_trial_button)
        benchmark_controls_layout.addWidget(self.next_trial_button)
        benchmark_controls_layout.addWidget(
            self.start_continuous_recording_button)
        benchmark_controls_layout.addWidget(self.timer_field)
        benchmark_controls_group_box.setLayout(benchmark_controls_layout)

        trial_layout.addLayout(self.trial_config_layout)
        trial_layout.addWidget(benchmark_controls_group_box)
        trial_layout.addLayout(self.trial_results_layout)
        self.save_results_button = QPushButton('Save results')
        self.save_results_button.clicked.connect(self._handle_save_result)
        trial_layout.addWidget(self.save_results_button)

        layout.addLayout(trial_layout, 0, 1)

        self.setLayout(layout)
        self.show()

        self.show_env_var('ROS_MASTER_URI')
        self.show_env_var('ROS_IP')
        self.show_env_var('ROS_HOSTNAME')

    def show_env_var(self, var):
        env_str = os.getenv(var) if os.getenv(var) is not None else ''
        msg = var + ': ' + env_str + '\n'
        self.update_status(msg)

    def set_current_benchmark(self):
        '''
        Sets the current benchmark type based on user selection
        Load config file for selected benchmark and setup GUI to show config and results based on that
        '''
        benchmark_index = self.benchmark_combo_box.currentIndex()
        benchmark_name = list(
            self.config['benchmarks'].keys())[benchmark_index]
        benchmark_result_type = getattr(
            metrics_refbox_msgs.msg,
            self.config['benchmarks'][benchmark_name]['type'])
        config_file_name = benchmark_name + '.json'
        config_file = os.path.join(self.metrics_refbox.get_config_file_path(),
                                   config_file_name)
        stream = open(config_file, 'r')
        benchmark_config = json.load(stream)
        module = importlib.import_module(benchmark_config['module'])
        self.current_benchmark = getattr(module, benchmark_config['class'])(
            benchmark_config, self.metrics_refbox.get_config_file_path(),
            benchmark_name, benchmark_result_type)
        self.current_benchmark_enum = self.config['benchmarks'][
            benchmark_name]['enum']

    def setup_trial_config(self):
        '''
        set up the benchmark specific part of the GUI related to the configuration of
        the benchmark (for example, which objects will be used for object detection)

        the specific configuration is loaded based on the selected trial_index
        '''

        for i in reversed(range(self.trial_config_layout.count())):
            self.trial_config_layout.itemAt(i).widget().setParent(None)

        for i in reversed(range(self.trial_results_layout.count())):
            self.trial_results_layout.itemAt(i).widget().setParent(None)

        self.current_benchmark.setup(self.trial_config_layout,
                                     self.trial_results_layout,
                                     self.config_locked)

        self.trial_list_widget.clear()
        self.trial_configs.clear()

        path = os.path.join(self.metrics_refbox.get_config_file_path(),
                            'trials')
        trial_config_file = os.path.join(
            path, self.current_benchmark.config['trial_config'])
        if os.path.exists(trial_config_file):
            with open(trial_config_file, "r") as fp:
                trial_config = json.load(fp)
            for key in sorted(trial_config.keys()):
                trial_item = QListWidgetItem(key)
                trial_item.setFlags(trial_item.flags() | Qt.ItemIsEditable)
                self.trial_list_widget.addItem(trial_item)
                self.trial_configs[key] = trial_config[key]
            self.trial_list_widget.setCurrentRow(0)

    def _handle_test_comm(self):
        '''
        'Test communication' button
        '''
        self.metrics_refbox.test_communication()
        self.status_signal.emit('Sent test message to all clients\n')

    def _handle_benchmark_selected(self, idx):
        '''
        benchmark selection has changed
        '''
        self._handle_save_trial_config(None)
        self.set_current_benchmark()
        self.setup_trial_config()

    def _handle_trial_change(self, current_item, previous_item):
        '''
        Trial selection has changed
        '''
        if previous_item is not None:
            self.trial_configs[previous_item.text(
            )] = self.current_benchmark.get_current_selections()
            self.current_benchmark.clear_results()
        if current_item is not None and current_item.text(
        ) in self.trial_configs.keys() and self.trial_configs[
                current_item.text()] is not None:
            self.current_benchmark.apply_selections(
                self.trial_configs[current_item.text()])
        else:
            self.current_benchmark.clear_selections()

    def _handle_generate(self, button):
        '''
        generate a new trial config
        '''
        trial_config = self.current_benchmark.generate()
        names = []
        for key in self.trial_configs.keys():
            names.append(key)
        msg = 'Select a name for this trial configuration'
        text = ''
        while True:
            text, ok = QInputDialog.getText(self, 'Trial name', msg)
            if text not in names:
                break
            QMessageBox.critical(self, "Error", "Name exists already")
        if ok:
            trial_item = QListWidgetItem(text)
            trial_item.setFlags(trial_item.flags() | Qt.ItemIsEditable)
            self.trial_list_widget.addItem(trial_item)
            self.trial_configs[text] = trial_config
            self.trial_list_widget.setCurrentItem(trial_item)

    def _handle_delete(self, button):
        '''
        delete current trial config
        '''
        selected_items = self.trial_list_widget.selectedItems()
        for item in selected_items:
            self.trial_list_widget.takeItem(self.trial_list_widget.row(item))
            del self.trial_configs[item.text()]

    def _handle_save_trial_config(self, button):
        '''
        save trial config in case it has been edited
        '''
        if self.trial_list_widget.currentItem() is not None:
            self.trial_configs[self.trial_list_widget.currentItem().text(
            )] = self.current_benchmark.get_current_selections()
        path = os.path.join(self.metrics_refbox.get_config_file_path(),
                            'trials')
        trial_config_file = os.path.join(
            path, self.current_benchmark.config['trial_config'])
        with open(trial_config_file, "w") as fp:
            json.dump(self.trial_configs, fp)

    def _handle_lock(self, button):
        '''
        Lock trial config settings so they are not changed accidentally
        '''
        if self.config_locked:
            self.current_benchmark.unlock_config()
            self.config_locked = False
            self.lock_button.setText('Lock')
        else:
            self.current_benchmark.lock_config()
            self.config_locked = True
            self.lock_button.setText('Unlock')

    def _handle_start_trial(self, button):
        '''
        Trial start button; send command to refbox client via ROS node
        '''
        if self.benchmark_running or self.benchmark_started:
            self.update_status(
                "Benchmark has already started. Stop and start the benchmark if you want to restart it\n"
            )
        else:
            self.metrics_refbox.start(
                self.current_benchmark_enum,
                self.current_benchmark.get_task_info_for_robot())
            self.benchmark_started = True
            self.current_benchmark.clear_results()
            self.update_status("Sent start command\n")
            self.timer_field.setText('')
            self.disable_buttons()

    def _handle_stop_trial(self, button):
        '''
        Trial stop button
        '''
        self.metrics_refbox.stop()
        self.elapsed_time = self.timer.elapsed()
        self.stopped = True
        self.benchmark_running = False
        self.benchmark_started = False
        self.status_signal.emit("Sent stop command\n")
        self.result_signal.emit(None)
        self.enable_buttons()

    def _handle_next_trial(self, button):
        '''
        Select next trial
        '''
        row = self.trial_list_widget.currentRow()
        if (row + 1 < self.trial_list_widget.count()):
            self.trial_list_widget.setCurrentRow(row + 1)

    def _handle_prev_trial(self, button):
        '''
        Select previous trial
        '''
        row = self.trial_list_widget.currentRow()
        if (row - 1 >= 0):
            self.trial_list_widget.setCurrentRow(row - 1)

    def _handle_continuous_recording(self, button):
        '''
        Start recording button, not tied to any benchmark, and requires user to stop recording
        '''
        if self.benchmark_running or self.benchmark_started:
            self.metrics_refbox.stop()
            self.elapsed_time = self.timer.elapsed()
            self.stopped = True
            self.benchmark_running = False
            self.benchmark_started = False
            self.status_signal.emit("Sent stop command\n")
            self.start_continuous_recording_button.setText(
                'Start continuous recording')
            self.enable_buttons()
            self.stop_trial_button.setEnabled(True)
            self.continuous_recording = False
        else:
            self.metrics_refbox.start_recording()
            self.continuous_recording = True
            self.start_continuous_recording_button.setText('Stop recording')
            self.benchmark_started = True
            self.update_status("Sent start command\n")
            self.timer_field.setText('')
            self.disable_buttons()
            self.stop_trial_button.setEnabled(False)

    def _handle_save_result(self, button):
        '''
        Save results again in case notes were added
        '''
        self.result_signal.emit(self.result_msg)

    def _start_cb(self, msg):
        '''
        called when we get confirmation that the robot received the start message;
        official start of trial time
        '''
        if msg.data == True:
            self.elapsed_time = 0.0
            self.timer.start()
            self.benchmark_running = True
            self.timeout = False
            self.stopped = False
            self.result_msg = None
            if msg.rosbag_filename == '':
                self.status_signal.emit(
                    'Error: rosbag filename not specified although start message confirmed\n'
                )
                self._handle_stop_trial(None)
                return
            self.status_signal.emit('Started trial and recording to %s\n' %
                                    msg.rosbag_filename)
            self.bagfile_signal.emit(msg.rosbag_filename)
        else:
            self.status_signal.emit(
                'Error: Benchmark did not start, probably because the rosbag recorder is not running\n'
            )
            self._handle_stop_trial(None)

    def _result_cb(self, msg):
        '''
        called when we get a result from the robot
        official end of trial time
        '''
        if self.benchmark_running:
            self.elapsed_time = self.timer.elapsed()
            self.benchmark_running = False
            self.benchmark_started = False

            self.status_signal.emit("Trial completed in %.2f seconds\n" %
                                    (self.elapsed_time / 1000.0))
            self.result_msg = msg
            self.result_signal.emit(msg)
            self.enable_buttons()

    def _status_cb(self, msg):
        '''
        callback to send text to status box
        '''
        self.status_signal.emit(msg)

    def update_status(self, msg):
        '''
        signal handler for status box
        '''
        timestamp = datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S')
        m = '[' + timestamp + '] ' + msg
        self.status.insertPlainText(m)
        self.status.ensureCursorVisible()

    def _update_bagfile_name(self, name):
        self.current_benchmark.set_bagfile_name(name)

    def update_result(self, msg):
        '''
        signal handler for result message; process and save result
        '''
        self.current_benchmark.show_results(msg, self.timeout, self.stopped)
        current_trial_name = self.trial_list_widget.currentItem().text()
        current_team_name = self.team_combo_box.currentText()
        results_dict = self.current_benchmark.get_trial_result_dict(
            msg, current_trial_name, current_team_name, self.timeout,
            self.stopped, self.elapsed_time / 1000.0)

        filename = self.current_benchmark.get_bagfile_name(
        )[:-4] + '_' + self.current_benchmark.benchmark_name + '.json'
        path = os.path.join(self.metrics_refbox.get_results_file_path(),
                            filename)
        with open(path, "w") as fp:
            json.dump(results_dict, fp)

    def update_timer(self):
        '''
        Timer callback; update GUI, and also check if timeout has expired
        '''
        if not self.benchmark_running:
            return
        self.timer_field.setText("Time: %.1f s" %
                                 (self.timer.elapsed() / 1000.0))
        # if we're in continuous recording mode, no need to check timeout
        if self.continuous_recording:
            return
        if self.timer.hasExpired(self.current_benchmark.get_timeout() *
                                 1000.0):
            self.status_signal.emit(
                "Trial timeout of %.2f seconds has expired\n" %
                self.current_benchmark.get_timeout())
            self.timeout_signal.emit('timeout expired')

    def _handle_timeout(self, msg):
        '''
        timeout has expired, so stop the trial
        '''
        self.timeout = True
        self._handle_stop_trial(None)

    def closeEvent(self, event):
        '''
        make sure we save trial configs when window is closed
        '''
        self.metrics_refbox.stop()
        self._handle_save_trial_config(None)
        event.accept()

    def disable_buttons(self):
        '''
        disable buttons when trial is running
        '''
        self.team_combo_box.setEnabled(False)
        self.benchmark_combo_box.setEnabled(False)
        self.trial_list_widget.setEnabled(False)
        self.next_trial_button.setEnabled(False)
        self.prev_trial_button.setEnabled(False)
        self.delete_button.setEnabled(False)
        self.generate_button.setEnabled(False)
        self.save_trials_button.setEnabled(False)
        self.lock_button.setEnabled(False)
        self.start_trial_button.setEnabled(False)

    def enable_buttons(self):
        self.team_combo_box.setEnabled(True)
        self.benchmark_combo_box.setEnabled(True)
        self.trial_list_widget.setEnabled(True)
        self.next_trial_button.setEnabled(True)
        self.prev_trial_button.setEnabled(True)
        self.delete_button.setEnabled(True)
        self.generate_button.setEnabled(True)
        self.save_trials_button.setEnabled(True)
        self.lock_button.setEnabled(True)
        self.start_trial_button.setEnabled(True)