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)
