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)
def __init__(self, title, jsp, num_rows=0): super(JointStatePublisherGui, self).__init__() self.jsp = jsp self.joint_map = {} self.vlayout = QVBoxLayout(self) self.scrollable = QWidget() self.gridlayout = QGridLayout() self.scroll = QScrollArea() self.scroll.setWidgetResizable(True) font = QFont("Helvetica", 9, QFont.Bold) ### Generate sliders ### sliders = [] for name in self.jsp.joint_list: if name not in self.jsp.free_joints: continue joint = self.jsp.free_joints[name] if joint['min'] == joint['max']: continue joint_layout = QVBoxLayout() row_layout = QHBoxLayout() label = QLabel(name) label.setFont(font) row_layout.addWidget(label) display = QLineEdit("0.00") display.setAlignment(Qt.AlignRight) display.setFont(font) display.setReadOnly(True) row_layout.addWidget(display) joint_layout.addLayout(row_layout) slider = QSlider(Qt.Horizontal) slider.setFont(font) slider.setRange(0, RANGE) slider.setValue(RANGE / 2) joint_layout.addWidget(slider) self.joint_map[name] = { 'slidervalue': 0, 'display': display, 'slider': slider, 'joint': joint } # Connect to the signal provided by QSignal slider.valueChanged.connect( lambda event, name=name: self.onValueChangedOne(name)) sliders.append(joint_layout) # Determine number of rows to be used in grid self.num_rows = num_rows # if desired num of rows wasn't set, default behaviour is a vertical layout if self.num_rows == 0: self.num_rows = len(sliders) # equals VBoxLayout # Generate positions in grid and place sliders there self.positions = self.generate_grid_positions(len(sliders), self.num_rows) for item, pos in zip(sliders, self.positions): self.gridlayout.addLayout(item, *pos) # Set zero positions read from parameters self.center() # Synchronize slider and displayed value self.sliderUpdate(None) # Set up a signal for updating the sliders based on external joint info self.sliderUpdateTrigger.connect(self.updateSliders) self.scrollable.setLayout(self.gridlayout) self.scroll.setWidget(self.scrollable) self.vlayout.addWidget(self.scroll) # Buttons for randomizing and centering sliders and # Spinbox for on-the-fly selecting number of rows self.randbutton = QPushButton('Randomize', self) self.randbutton.clicked.connect(self.randomize_event) self.vlayout.addWidget(self.randbutton) self.ctrbutton = QPushButton('Center', self) self.ctrbutton.clicked.connect(self.center_event) self.vlayout.addWidget(self.ctrbutton) self.maxrowsupdown = QSpinBox() self.maxrowsupdown.setMinimum(1) self.maxrowsupdown.setMaximum(len(sliders)) self.maxrowsupdown.setValue(self.num_rows) self.maxrowsupdown.valueChanged.connect(self.reorggrid_event) self.vlayout.addWidget(self.maxrowsupdown) self.setLayout(self.vlayout)
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() '''
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()
class RobotTab(QWidget): signalRobotNameChanged = pyqtSignal(int) def __init__(self, num_robots, robots): super(RobotTab, self).__init__() # Available robots from gui_config.yaml self.robots = robots # Set robot type by default to ground self.agent_type = 'ground' # Number of robots self.num_robots = num_robots # Set the robot name by default self.robot_name = 'robot' + str(self.num_robots) # Set layout for tab self.layout = QVBoxLayout() # Robot tab title self.robot_label_name = QLabel(('Robot ' + str(self.num_robots))) font = QFont() font.setPointSize(14) font.setBold(True) self.robot_label_name.setFont(font) self.layout.addWidget(self.robot_label_name) # Robot name label and input line self.robot_label = QLabel('Robot name') self.layout.addWidget(self.robot_label) self.robot_name_input = QLineEdit('robot' + str(self.num_robots)) #self.robot_name_input.editingFinished.connect(self.robot_name_changed) self.layout.addWidget(self.robot_name_input) # Robot model label and comboBox self.robot_model_label = QLabel('Robot model') self.layout.addWidget(self.robot_model_label) self.robot_comboBox = CustomComboBox(self.num_robots - 1) self.robot_comboBox.addItems(self.robots['Models'].keys()) self.layout.addWidget(self.robot_comboBox) self.robot_comboBox.signalIndexChanged.connect(self.set_agent_type) # Initial pose self.robot_label_init = QLabel('Initial pose') self.layout.addWidget(self.robot_label_init) self.robot_comboBox_init = CustomComboBox(self.num_robots) self.layout.addWidget(self.robot_comboBox_init) # Add initial pose text item to graphic initial_pose_textItem = QGraphicsTextItem( 'start_' + str(self.num_robots).zfill(2)) self.initial_pose = { 'start_' + str(self.num_robots).zfill(2): { 'label': 'r01', 'text_item': initial_pose_textItem } } # Use Qualisys self.robot_localization_label = QLabel('Localization') self.layout.addWidget(self.robot_localization_label) self.robot_localization_checkBox = QCheckBox('Use Qualisys') self.layout.addWidget(self.robot_localization_checkBox) # Task specifications self.robot_label_task_title = QLabel('Task robot ' + str(self.num_robots)) self.robot_label_task_title.setFont(font) self.layout.addWidget(self.robot_label_task_title) self.robot_label_hard_task = QLabel('Hard tasks') self.layout.addWidget(self.robot_label_hard_task) self.robot_hard_task_input = QLineEdit('([]<> r01) && ([]<> r02)') self.layout.addWidget(self.robot_hard_task_input) self.robot_label_soft_task = QLabel('Soft tasks') self.layout.addWidget(self.robot_label_soft_task) self.robot_soft_task_input = QLineEdit() self.layout.addWidget(self.robot_soft_task_input) # Plan display self.robot_label_prefix = QLabel('Planner prefix robot ' + str(self.num_robots)) self.layout.addWidget(self.robot_label_prefix) self.robot_prefix_textbox = QTextBrowser() self.layout.addWidget(self.robot_prefix_textbox) self.robot_label_sufix = QLabel('Planner sufix robot ' + str(self.num_robots)) self.layout.addWidget(self.robot_label_sufix) self.robot_sufix_textbox = QTextBrowser() self.layout.addWidget(self.robot_sufix_textbox) # Current goal display self.robot_label_current_goal = QLabel('Current goal robot ' + str(self.num_robots)) self.layout.addWidget(self.robot_label_current_goal) self.robot_current_goal_textbox = QTextBrowser() self.layout.addWidget(self.robot_current_goal_textbox) # Clear costmap button self.robot_resend_goal_button = QPushButton('Clear costmap') self.layout.addWidget(self.robot_resend_goal_button) # Temporary task button self.robot_temporary_task_button = QPushButton('Temporary task') self.layout.addWidget(self.robot_temporary_task_button) self.robot_temporary_task_button.clicked.connect( self.temporary_task_button_pressed) self.setLayout(self.layout) # Messages for publishing pose, soft-task, hard-task and clear_costmap self.init_pose_msg = Pose() self.soft_task_msg = String() self.hard_task_msg = String() self.prefix_string = '' self.sufix_string = '' # Marker displaying robots name self.label_marker_msg = Marker() self.label_marker_msg.pose = self.init_pose_msg self.label_marker_msg.pose.position.z = 1.0 self.label_marker_msg.text = self.robot_name self.label_marker_msg.type = self.label_marker_msg.TEXT_VIEW_FACING self.label_marker_msg.id = self.num_robots self.label_marker_msg.action = self.label_marker_msg.ADD self.label_marker_msg.scale.z = 0.5 self.label_marker_msg.color.a = 1.0 self.label_marker_msg.color.r = 0.0 self.label_marker_msg.color.g = 0.0 self.label_marker_msg.color.b = 0.0 self.label_marker_msg.header.frame_id = '/map' # Pose msg published for planner self.pose_msg = PoseWithCovarianceStamped() # Init pose msg for planner self.pose_msg.pose.pose = self.init_pose_msg # Clear costmap and resend current goal self.robot_resend_goal_button.clicked.connect( self.call_clear_costmap_srvs) self.robot_current_goal = MoveBaseActionGoal() # Start all publisher and Subscriber #self.start_publisher_and_subscriber() # AMCL pose callback def current_pose_amcl_callback(self, msg): if self.robot_localization_checkBox.checkState() != 2: self.pose_msg.pose.pose = deepcopy(msg.pose.pose) self.pose_msg.header = deepcopy(msg.header) self.pose_msg.header.stamp = rospy.Time.now() if self.agent_type == 'ground': self.label_marker_msg.header = msg.header self.label_marker_msg.pose = deepcopy(msg.pose.pose) self.label_marker_msg.pose.position.z = deepcopy( msg.pose.pose.position.z) + 1 # Localization with Qualisys def current_pose_qualisys_callback(self, msg): if self.robot_localization_checkBox.checkState() == 2: self.pose_msg.header = deepcopy(msg.header) self.pose_msg.pose.pose = deepcopy(msg.pose) self.pose_msg.header.stamp = rospy.Time.now() if self.agent_type == 'ground': self.label_marker_msg.header = msg.header self.label_marker_msg.pose = msg.pose self.label_marker_msg.pose.position.z = deepcopy( msg.pose.position.z) + 1 # Localization with Gazebo ground truth def current_pose_gazebo_ground_truth_callback(self, msg): if self.agent_type == 'aerial': self.label_marker_msg.header = msg.header self.label_marker_msg.pose = msg.pose.pose self.label_marker_msg.pose.position.z = deepcopy( msg.pose.pose.position.z) + 1.0 # Sends topic to planner to clear costmap manual @Slot(bool) def call_clear_costmap_srvs(self): if self.agent_type == 'ground': bool = Bool() bool.data = True self.clear_costmap_publisher.publish(bool) # Opens dialog for temporary task @Slot(bool) def temporary_task_button_pressed(self): temporary_task_dialog = TemporaryTask_dialog() temporary_task_dialog.exec_() if temporary_task_dialog.atomic_propositions: temporary_task_msg = TemporaryTask() temporary_task_msg.header.stamp = rospy.Time.now() for i in range(0, len(temporary_task_dialog.atomic_propositions)): string_msg = String() string_msg.data = temporary_task_dialog.atomic_propositions[i] temporary_task_msg.task.append(string_msg) temporary_task_msg.T_des.data = temporary_task_dialog.T_des self.temporary_task_publisher.publish(temporary_task_msg) # If robot name in tab has changed the subscriber and publisher topics are updated def robot_name_changed(self): self.remove_publisher_and_subscriber() self.robot_name = self.robot_name_input.text() self.start_publisher_and_subscriber() self.label_marker_msg.text = self.robot_name self.signalRobotNameChanged.emit(self.num_robots) # Start all publisher and subscriber from robot tab def start_publisher_and_subscriber(self): self.robot_name = self.robot_name_input.text() self.ros_publisher = ROS_Publisher() self.ros_publisher.add_publisher('/' + self.robot_name + '/init_pose', Pose, 1.0, self.init_pose_msg) self.ros_publisher.add_publisher('/' + self.robot_name + '/soft_task', String, 1.0, self.soft_task_msg) self.ros_publisher.add_publisher('/' + self.robot_name + '/hard_task', String, 1.0, self.hard_task_msg) self.ros_publisher.add_publisher( '/' + self.robot_name + '/label_marker', Marker, 5.0, self.label_marker_msg) self.temporary_task_publisher = rospy.Publisher('/' + self.robot_name + '/temporary_task', TemporaryTask, queue_size=1) self.clear_costmap_publisher = rospy.Publisher('/' + self.robot_name + '/clear_costmap', Bool, queue_size=1) self.current_pose_amcl_subscriber = rospy.Subscriber( '/' + self.robot_name + '/amcl_pose', PoseWithCovarianceStamped, self.current_pose_amcl_callback) self.current_pose_qualisys_subscriber = rospy.Subscriber( '/' + self.robot_name + '/qualisys_pose_map', PoseStamped, self.current_pose_qualisys_callback) self.current_pose_gazebo_ground_truth_subscriber = rospy.Subscriber( '/' + self.robot_name + '/ground_truth/pose_with_covariance', PoseWithCovarianceStamped, self.current_pose_gazebo_ground_truth_callback) self.ros_publisher.add_publisher('/' + self.robot_name + '/pose_gui', PoseWithCovarianceStamped, 15.0, self.pose_msg) self.label_marker_msg.text = self.robot_name # Remove all publisher and subcriber from robot tab def remove_publisher_and_subscriber(self): del self.ros_publisher del self.clear_costmap_publisher del self.temporary_task_publisher del self.current_pose_amcl_subscriber del self.current_pose_qualisys_subscriber del self.current_pose_gazebo_ground_truth_subscriber # ADD FUNCTION THAT JUST CHANGES SOFT AND HARD TASK!!!! # DELETE OLD ONE, AND ADD NEW ONE! pass as argument the planner or previous class such that I call it here # Updates robot type if model checkbox was changed def set_agent_type(self): if self.robot_comboBox.currentText( ) in self.robots['robot_types']['aerial']: self.agent_type = 'aerial' else: self.agent_type = 'ground' def build_init_pose_msg(self, id): self.init_pose_msg.position.x = self.initial_pose[ 'start_' + str(id).zfill(2)]['pose']['position'][0] self.init_pose_msg.position.y = self.initial_pose[ 'start_' + str(id).zfill(2)]['pose']['position'][1] self.init_pose_msg.position.z = self.initial_pose[ 'start_' + str(id).zfill(2)]['pose']['position'][2] self.init_pose_msg.orientation.w = self.initial_pose[ 'start_' + str(id).zfill(2)]['pose']['orientation'][0] self.init_pose_msg.orientation.x = self.initial_pose[ 'start_' + str(id).zfill(2)]['pose']['orientation'][1] self.init_pose_msg.orientation.y = self.initial_pose[ 'start_' + str(id).zfill(2)]['pose']['orientation'][2] self.init_pose_msg.orientation.z = self.initial_pose[ 'start_' + str(id).zfill(2)]['pose']['orientation'][3]
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)
def __init__(self, title, jsp, num_rows=0): super(JointStatePublisherGui, self).__init__() self.jsp = jsp self.joint_map = {} self.vlayout = QVBoxLayout(self) self.gridlayout = QGridLayout() font = QFont("Helvetica", 9, QFont.Bold) ### Generate sliders ### sliders = [] for name in self.jsp.joint_list: if name not in self.jsp.free_joints: continue joint = self.jsp.free_joints[name] if joint['min'] == joint['max']: continue joint_layout = QVBoxLayout() row_layout = QHBoxLayout() label = QLabel(name) label.setFont(font) row_layout.addWidget(label) display = QLineEdit("0.00") display.setAlignment(Qt.AlignRight) display.setFont(font) display.setReadOnly(True) row_layout.addWidget(display) joint_layout.addLayout(row_layout) slider = QSlider(Qt.Horizontal) slider.setFont(font) slider.setRange(0, RANGE) slider.setValue(RANGE/2) joint_layout.addWidget(slider) self.joint_map[name] = {'slidervalue': 0, 'display': display, 'slider': slider, 'joint': joint} # Connect to the signal provided by QSignal slider.valueChanged.connect(self.onValueChanged) sliders.append(joint_layout) # Determine number of rows to be used in grid self.num_rows = num_rows # if desired num of rows wasn't set, default behaviour is a vertical layout if self.num_rows == 0: self.num_rows = len(sliders) # equals VBoxLayout # Generate positions in grid and place sliders there self.positions = self.generate_grid_positions(len(sliders), self.num_rows) for item, pos in zip(sliders, self.positions): self.gridlayout.addLayout(item, *pos) # Set zero positions read from parameters self.center() # Synchronize slider and displayed value self.sliderUpdate(None) # Set up a signal for updating the sliders based on external joint info self.sliderUpdateTrigger.connect(self.updateSliders) self.vlayout.addLayout(self.gridlayout) # Buttons for randomizing and centering sliders and # Spinbox for on-the-fly selecting number of rows self.randbutton = QPushButton('Randomize', self) self.randbutton.clicked.connect(self.randomize_event) self.vlayout.addWidget(self.randbutton) self.ctrbutton = QPushButton('Center', self) self.ctrbutton.clicked.connect(self.center_event) self.vlayout.addWidget(self.ctrbutton) self.maxrowsupdown = QSpinBox() self.maxrowsupdown.setMinimum(1) self.maxrowsupdown.setMaximum(len(sliders)) self.maxrowsupdown.setValue(self.num_rows) self.maxrowsupdown.lineEdit().setReadOnly(True) # don't edit it by hand to avoid weird resizing of window self.maxrowsupdown.valueChanged.connect(self.reorggrid_event) self.vlayout.addWidget(self.maxrowsupdown)
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)
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)
def __init__(self, content): super(SliderPublisher, self).__init__() content = content.replace('\t', ' ') # get message types self.publishers = {} self.values = {} pkgs = [] # to keep track of key ordering in the yaml file order = [] old = [] for topic, info in yaml.load(content).items(): pkg, msg = info['type'].split('/') pkgs.append(__import__(pkg, globals(), locals(), ['msg'])) self.publishers[topic] = Publisher(topic, getattr(pkgs[-1].msg, msg), info) order.append((topic, [])) for key in info: self.values[key] = info[key] order[-1][1].append((content.find(' ' + key + ':'), key)) old.append((content.find(' ' + key + ':'), key)) for bound in ['min', 'max']: self.values[key][bound] = float(self.values[key][bound]) self.values[key]['val'] = 0 order[-1][1].sort() order.sort(key=lambda x: x[1][0][0]) # build sliders - thanks joint_state_publisher sliderUpdateTrigger = Signal() self.vlayout = QVBoxLayout(self) self.gridlayout = QGridLayout() font = QFont("Helvetica", 9, QFont.Bold) topic_font = QFont("Helvetica", 10, QFont.Bold) sliders = [] self.key_map = {} y = 0 for topic, keys in order: topic_layout = QVBoxLayout() label = QLabel(topic) label.setFont(topic_font) topic_layout.addWidget(label) self.gridlayout.addLayout(topic_layout, *(y, 0)) y += 1 for idx, key in keys: key_layout = QVBoxLayout() row_layout = QHBoxLayout() label = QLabel(key) label.setFont(font) row_layout.addWidget(label) display = QLineEdit("0.00") display.setAlignment(Qt.AlignRight) display.setFont(font) display.setReadOnly(True) row_layout.addWidget(display) key_layout.addLayout(row_layout) slider = QSlider(Qt.Horizontal) slider.setFont(font) slider.setRange(0, RANGE) slider.setValue(RANGE / 2) key_layout.addWidget(slider) self.key_map[key] = { 'slidervalue': 0, 'display': display, 'slider': slider } slider.valueChanged.connect(self.onValueChanged) self.gridlayout.addLayout(key_layout, *(y, 0)) y += 1 #sliders.append(key_layout) # Generate positions in grid and place sliders there #self.positions = [(y,0) for y in range(len(sliders))] #for item, pos in zip(sliders, self.positions): # self.gridlayout.addLayout(item, *pos) self.vlayout.addLayout(self.gridlayout) self.ctrbutton = QPushButton('Center', self) self.ctrbutton.clicked.connect(self.center) self.vlayout.addWidget(self.ctrbutton) self.center(1)
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)
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)
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)