def __init__(self,
options,
title='Exclusive Options',
selected_index=None):
super(ExclusiveOptionGroup, self).__init__()
self.setTitle(title)
self.setLayout(QVBoxLayout())
self._button_group = QButtonGroup()
self._button_group.setExclusive(True)
self._options = options
button_id = 0
for option in self._options:
button_id += 1
radio_button = QRadioButton(
option.get('title', 'option %d' % button_id))
radio_button.setEnabled(option.get('enabled', True))
radio_button.setChecked(
option.get('selected', False)
or (button_id - 1) == selected_index)
radio_button.setToolTip(option.get('tooltip', ''))
widget = QWidget()
widget.setLayout(QVBoxLayout())
widget.layout().addWidget(radio_button)
if 'description' in option:
widget.layout().addWidget(QLabel(option['description']))
self._button_group.addButton(radio_button, button_id)
self.layout().addWidget(widget)
def __init__(self,
options,
title='Checkboxes',
selected_indexes=[],
parent=None):
super(CheckBoxGroup, self).__init__()
self.setTitle(title)
self.setLayout(QVBoxLayout())
self._button_group = QButtonGroup()
self._button_group.setExclusive(False)
self._options = options
if parent == None:
parent = self
for (button_id, option) in enumerate(self._options):
checkbox = QCheckBox(option.get('title', 'option %d' % button_id))
checkbox.setEnabled(option.get('enabled', True))
checkbox.setChecked(button_id in selected_indexes)
checkbox.setToolTip(option.get('tooltip', ''))
self._button_group.addButton(checkbox, button_id)
parent.layout().addWidget(checkbox)
if 'description' in option:
parent.layout().addWidget(QLabel(option['description']))
class ExclusiveOptionGroup(QGroupBox):
"""
Creates a button group of exclusive radio options.
Options must be a dict with following keys: 'enabled','selected','title','description','tooltip'
"""
def __init__(self, options, title='Exclusive Options', selected_index=None, parent=None):
super(ExclusiveOptionGroup, self).__init__()
self.setTitle(title)
self.setLayout(QVBoxLayout())
self._button_group = QButtonGroup()
self._button_group.setExclusive(True)
self._options = options
if parent == None:
parent = self
for (button_id, option) in enumerate(self._options):
radio_button = QRadioButton(option.get('title', 'option %d' % button_id))
radio_button.setEnabled(option.get('enabled', True))
radio_button.setChecked(option.get('selected', False) or button_id == selected_index)
radio_button.setToolTip(option.get('tooltip', ''))
self._button_group.addButton(radio_button, button_id)
parent.layout().addWidget(radio_button)
if 'description' in option:
parent.layout().addWidget(QLabel(option['description']))
def get_settings(self):
"""Returns dictionary with selected_index (int) and selected_option (dict) keys."""
selected_index = self._button_group.checkedId()
if selected_index >= 0:
return {'selected_index': selected_index, 'selected_option': self._options[selected_index]}
return {'selected_index': None, 'selected_option': None}
def __init__(self,
options,
title='Exclusive Options',
selected_index=None,
parent=None):
super(ExclusiveOptionGroup, self).__init__()
self.setTitle(title)
self.setLayout(QVBoxLayout())
self._button_group = QButtonGroup()
self._button_group.setExclusive(True)
self._options = options
if parent == None:
parent = self
for (button_id, option) in enumerate(self._options):
radio_button = QRadioButton(
option.get('title', 'option %d' % button_id))
radio_button.setEnabled(option.get('enabled', True))
radio_button.setChecked(
option.get('selected', False) or button_id == selected_index)
radio_button.setToolTip(option.get('tooltip', ''))
self._button_group.addButton(radio_button, button_id)
parent.layout().addWidget(radio_button)
if 'description' in option:
parent.layout().addWidget(QLabel(option['description']))
def setup_group_frame(self, group):
layout = QVBoxLayout()
# grid for buttons for named targets
grid = QGridLayout()
grid.setSpacing(1)
self.button_group = QButtonGroup(self)
row = 0
column = 0
named_targets = self.get_named_targets(group)
for target in named_targets:
button = QPushButton(target)
self.button_group.addButton(button)
grid.addWidget(button, row, column)
column += 1
if column >= self.MAX_COLUMNS:
row += 1
column = 0
self.button_group.buttonClicked.connect(self._handle_button_clicked)
# grid for show joint value and move arm buttons/text field
joint_values_grid = QGridLayout()
joint_values_grid.setSpacing(1)
button_show_joint_values = QPushButton('Current Joint Values')
button_show_joint_values.clicked[bool].connect(
self._handle_show_joint_values_clicked)
line_edit = QLineEdit()
self.text_joint_values.append(line_edit)
button_move_to = QPushButton('Move to')
button_move_to.clicked[bool].connect(self._handle_move_to_clicked)
joint_values_grid.addWidget(button_show_joint_values, 0, 1)
joint_values_grid.addWidget(line_edit, 0, 2)
joint_values_grid.addWidget(button_move_to, 0, 3)
layout.addLayout(grid)
line = QFrame()
line.setFrameShape(QFrame.HLine)
line.setFrameShadow(QFrame.Sunken)
layout.addWidget(line)
layout.addLayout(joint_values_grid)
frame = QFrame()
frame.setLayout(layout)
return frame
class ExclusiveOptionGroup(QGroupBox):
"""
Creates a button group of exclusive radio options.
Options must be a dict with following keys: 'enabled','selected','title','description','tooltip'
"""
def __init__(self,
options,
title='Exclusive Options',
selected_index=None,
parent=None):
super(ExclusiveOptionGroup, self).__init__()
self.setTitle(title)
self.setLayout(QVBoxLayout())
self._button_group = QButtonGroup()
self._button_group.setExclusive(True)
self._options = options
if parent == None:
parent = self
for (button_id, option) in enumerate(self._options):
radio_button = QRadioButton(
option.get('title', 'option %d' % button_id))
radio_button.setEnabled(option.get('enabled', True))
radio_button.setChecked(
option.get('selected', False) or button_id == selected_index)
radio_button.setToolTip(option.get('tooltip', ''))
self._button_group.addButton(radio_button, button_id)
parent.layout().addWidget(radio_button)
if 'description' in option:
parent.layout().addWidget(QLabel(option['description']))
def get_settings(self):
"""Returns dictionary with selected_index (int) and selected_option (dict) keys."""
selected_index = self._button_group.checkedId()
if selected_index >= 0:
return {
'selected_index': selected_index,
'selected_option': self._options[selected_index]
}
return {'selected_index': None, 'selected_option': None}
class CheckBoxGroup(QGroupBox):
"""
Creates a button group of non-exclusive checkbox options.
Options must be a dict with following keys: 'enabled','title','description','tooltip'
"""
def __init__(self,
options,
title='Checkboxes',
selected_indexes=[],
parent=None):
super(CheckBoxGroup, self).__init__()
self.setTitle(title)
self.setLayout(QVBoxLayout())
self._button_group = QButtonGroup()
self._button_group.setExclusive(False)
self._options = options
if parent == None:
parent = self
for (button_id, option) in enumerate(self._options):
checkbox = QCheckBox(option.get('title', 'option %d' % button_id))
checkbox.setEnabled(option.get('enabled', True))
checkbox.setChecked(button_id in selected_indexes)
checkbox.setToolTip(option.get('tooltip', ''))
self._button_group.addButton(checkbox, button_id)
parent.layout().addWidget(checkbox)
if 'description' in option:
parent.layout().addWidget(QLabel(option['description']))
def get_settings(self):
"""Returns dictionary with selected_indexes (array) and selected_options (array) keys."""
selected_indexes = []
selected_options = []
for button in self._button_group.buttons():
if button.isChecked():
selected_indexes.append(self._button_group.id(button))
selected_options.append(
self._options[self._button_group.id(button)])
return {
'selected_indexes': selected_indexes,
'selected_options': selected_options
}
def __init__(self, options, title='Exclusive Options', selected_index=None, parent=None):
super(ExclusiveOptionGroup, self).__init__()
self.setTitle(title)
self.setLayout(QVBoxLayout())
self._button_group = QButtonGroup()
self._button_group.setExclusive(True)
self._options = options
if parent == None:
parent = self
for (button_id, option) in enumerate(self._options):
radio_button = QRadioButton(option.get('title', 'option %d' % button_id))
radio_button.setEnabled(option.get('enabled', True))
radio_button.setChecked(option.get('selected', False) or button_id == selected_index)
radio_button.setToolTip(option.get('tooltip', ''))
self._button_group.addButton(radio_button, button_id)
parent.layout().addWidget(radio_button)
if 'description' in option:
parent.layout().addWidget(QLabel(option['description']))
def __init__(self, options, title='Checkboxes', selected_indexes=[], parent=None):
super(CheckBoxGroup, self).__init__()
self.setTitle(title)
self.setLayout(QVBoxLayout())
self._button_group = QButtonGroup()
self._button_group.setExclusive(False)
self._options = options
if parent == None:
parent = self
for (button_id, option) in enumerate(self._options):
checkbox = QCheckBox(option.get('title', 'option %d' % button_id))
checkbox.setEnabled(option.get('enabled', True))
checkbox.setChecked(button_id in selected_indexes)
checkbox.setToolTip(option.get('tooltip', ''))
self._button_group.addButton(checkbox, button_id)
parent.layout().addWidget(checkbox)
if 'description' in option:
parent.layout().addWidget(QLabel(option['description']))
class CheckBoxGroup(QGroupBox):
"""
Creates a button group of non-exclusive checkbox options.
Options must be a dict with following keys: 'enabled','title','description','tooltip'
"""
def __init__(self, options, title='Checkboxes', selected_indexes=[], parent=None):
super(CheckBoxGroup, self).__init__()
self.setTitle(title)
self.setLayout(QVBoxLayout())
self._button_group = QButtonGroup()
self._button_group.setExclusive(False)
self._options = options
if parent == None:
parent = self
for (button_id, option) in enumerate(self._options):
checkbox = QCheckBox(option.get('title', 'option %d' % button_id))
checkbox.setEnabled(option.get('enabled', True))
checkbox.setChecked(button_id in selected_indexes)
checkbox.setToolTip(option.get('tooltip', ''))
self._button_group.addButton(checkbox, button_id)
parent.layout().addWidget(checkbox)
if 'description' in option:
parent.layout().addWidget(QLabel(option['description']))
def get_settings(self):
"""Returns dictionary with selected_indexes (array) and selected_options (array) keys."""
selected_indexes = []
selected_options = []
for button in self._button_group.buttons():
if button.isChecked():
selected_indexes.append(self._button_group.id(button))
selected_options.append(self._options[self._button_group.id(button)])
return {'selected_indexes': selected_indexes, 'selected_options': selected_options}
class MoveitCommanderWidget(QWidget):
"""
Provides a UI for Moveit Commander
- parses rosparam 'robot_description_semantic' to get content of robot srdf
- there are tabs for each move_group group
- for each group, there is a set of buttons with the named targets
- for each group, there is a button to get current joint values and a button to move to specific joint values
"""
def __init__(self, context):
super(MoveitCommanderWidget, self).__init__()
self.setObjectName('MoveitCommanderWidget')
self.groups = self.get_groups()
self.commanders = [
moveit_commander.MoveGroupCommander(group) for group in self.groups
]
# list of QLineEdit fields used to display joint values
self.text_joint_values = []
self.MAX_COLUMNS = 4
self.tab_widget = QTabWidget()
for g in self.groups:
frame = self.setup_group_frame(g)
self.tab_widget.addTab(frame, g)
widget_layout = QVBoxLayout()
widget_layout.addWidget(self.tab_widget)
self.setLayout(widget_layout)
def setup_group_frame(self, group):
layout = QVBoxLayout()
# grid for buttons for named targets
grid = QGridLayout()
grid.setSpacing(1)
self.button_group = QButtonGroup(self)
row = 0
column = 0
named_targets = self.get_named_targets(group)
for target in named_targets:
button = QPushButton(target)
self.button_group.addButton(button)
grid.addWidget(button, row, column)
column += 1
if column >= self.MAX_COLUMNS:
row += 1
column = 0
self.button_group.buttonClicked.connect(self._handle_button_clicked)
# grid for show joint value and move arm buttons/text field
joint_values_grid = QGridLayout()
joint_values_grid.setSpacing(1)
button_show_joint_values = QPushButton('Current Joint Values')
button_show_joint_values.clicked[bool].connect(
self._handle_show_joint_values_clicked)
line_edit = QLineEdit()
self.text_joint_values.append(line_edit)
button_move_to = QPushButton('Move to')
button_move_to.clicked[bool].connect(self._handle_move_to_clicked)
joint_values_grid.addWidget(button_show_joint_values, 0, 1)
joint_values_grid.addWidget(line_edit, 0, 2)
joint_values_grid.addWidget(button_move_to, 0, 3)
layout.addLayout(grid)
line = QFrame()
line.setFrameShape(QFrame.HLine)
line.setFrameShadow(QFrame.Sunken)
layout.addWidget(line)
layout.addLayout(joint_values_grid)
frame = QFrame()
frame.setLayout(layout)
return frame
def _handle_button_clicked(self, button):
commander = self.commanders[self.tab_widget.currentIndex()]
rospy.loginfo('Moving ' + commander.get_name() + ' to: ' +
button.text())
try:
commander.set_named_target(str(button.text()))
except Exception as e:
rospy.logerr('Unable to set target: %s' % (str(e)))
return
error_code = commander.go(wait=True)
if error_code == moveit_msgs.msg.MoveItErrorCodes.SUCCESS:
rospy.loginfo("Succeeded")
else:
rospy.logerr("Movement failed with error code: %d", error_code)
def _handle_show_joint_values_clicked(self, checked):
commander = self.commanders[self.tab_widget.currentIndex()]
l = commander.get_current_joint_values()
l = [round(ll, 6) for ll in l]
self.text_joint_values[self.tab_widget.currentIndex()].setText(str(l))
def _handle_move_to_clicked(self, checked):
commander = self.commanders[self.tab_widget.currentIndex()]
# parse string as list
l = ast.literal_eval(
str(self.text_joint_values[self.tab_widget.currentIndex()].text()))
if all(isinstance(x, float) for x in l):
try:
commander.set_joint_value_target(l)
except Exception as e:
rospy.logerr('Unable to set target position: %s' % (str(e)))
return
error_code = commander.go(wait=True)
if error_code == moveit_msgs.msg.MoveItErrorCodes.SUCCESS:
rospy.loginfo("Succeeded")
else:
rospy.logerr("Movement failed with error code: %d", error_code)
else:
rospy.logerr("Malformed joint angle list")
def get_named_targets(self, group_name):
s = rospy.get_param('/robot_description_semantic')
rex = "group_state[ \\\\\n\r\f\v\t]*name=\"[a-zA-Z0-9_/\n\r ]*\"[ \\\\\n\r\f\v\t]*group=\"[ \\\\\n\r\f\v\t]*" \
+ group_name + "\""
p = re.compile(rex)
# returns a list of 'group_state name="named_target"' entries
l = p.findall(s)
pp = re.compile("name=\"[a-zA-Z0-9_/\n\r ]*\"")
named_targets = [pp.findall(ll)[0][6:-1].strip() for ll in l]
return named_targets
def get_groups(self):
s = rospy.get_param('/robot_description_semantic')
p = re.compile("group[ \\\\\n\r\f\v\t]*name=\"[a-zA-Z0-9_/\n\r ]*\"")
# returns a list of 'group name="group_name"' entries
l = p.findall(s)
pp = re.compile("name=\"[a-zA-Z0-9_/\n\r ]*\"")
groups = [pp.findall(ll)[0][6:-1].strip() for ll in l]
return groups
def __init__(self, context):
#super(PositionModeDialog, self).__init__(context)
#self.setObjectName('PositionModeDialog')
super(PositionModeWidget, self).__init__()
self.name = 'PositionModeWidget'
self.updateStateSignal.connect(self.on_updateState)
self.updatePelvisSignal.connect(self.on_updatePelvis)
self.updateGhostSignal.connect(self.on_updateGhost)
## Process standalone plugin command-line arguments
#from argparse import ArgumentParser
#parser = ArgumentParser()
## Add argument(s) to the parser.
#parser.add_argument("-f", "--file",
# dest="file",
# help="Input file name")
#args, unknowns = parser.parse_known_args(context.argv())
#print 'arguments: ', args
#print 'unknowns: ', unknowns
#
#if ((args.file == "") or (args.file == None)):
# print "No file specified - abort!"
# sys.exit(-1)
# Define how long the trajectories will take to execute
self.time_factor = rospy.get_param('~time_factor', 2.1)
print "Using ",self.time_factor, " as the time factor for all trajectories"
# Joint Name : Child Link :OSRF
self.joint_names=[]
self.joint_names.append('back_bkz') # : ltorso : 0
self.joint_names.append('back_bky') # : mtorso : 1
self.joint_names.append('back_bkx') # : utorso : 2
self.joint_names.append('neck_ry') # : head : 3
self.joint_names.append('l_leg_hpz') # : l_uglut : 4
self.joint_names.append('l_leg_hpx') # : l_lglut : 5
self.joint_names.append('l_leg_hpy') # : l_uleg : 6
self.joint_names.append('l_leg_kny') # : l_lleg : 7
self.joint_names.append('l_leg_aky') # : l_talus : 8
self.joint_names.append('l_leg_akx') # : l_foot : 9
self.joint_names.append('r_leg_hpz') # : r_uglut : 10
self.joint_names.append('r_leg_hpx') # : r_lglut : 11
self.joint_names.append('r_leg_hpy') # : r_uleg : 12
self.joint_names.append('r_leg_kny') # : r_lleg : 13
self.joint_names.append('r_leg_aky') # : r_talus : 14
self.joint_names.append('r_leg_akx') # : r_foot : 15
self.joint_names.append('l_arm_shz') # : l_clav : 16
self.joint_names.append('l_arm_shx') # : l_scap : 17
self.joint_names.append('l_arm_ely') # : l_uarm : 18
self.joint_names.append('l_arm_elx') # : l_larm : 19
self.joint_names.append('l_arm_wry') # : l_farm : 20
self.joint_names.append('l_arm_wrx') # : l_hand : 21
self.joint_names.append('l_arm_wry2') # : l_farm : 22
self.joint_names.append('r_arm_shz') # : r_clav : 23
self.joint_names.append('r_arm_shx') # : r_scap : 24
self.joint_names.append('r_arm_ely') # : r_uarm : 25
self.joint_names.append('r_arm_elx') # : r_larm : 26
self.joint_names.append('r_arm_wry') # : r_farm : 27
self.joint_names.append('r_arm_wrx') # : r_hand : 28
self.joint_names.append('r_arm_wry2') # : r_farm : 29
self.joint_states = JointState()
self.pelvis_names=[]
self.joint_names.append('forward') # : 30
self.joint_names.append('lateral') # : 31
self.joint_names.append('height') # : 32
self.joint_names.append('roll') # : 33
self.joint_names.append('pitch') # : 34
self.joint_names.append('yaw') # : 35
self.pelvis_states = JointState()
self.pelvis_states.position = [0.0, 0.0, 0.85, 0.0, 0.0, 0.0] # default pelvis pose
#self._widget = QWidget()
self._widget = context
vbox = QVBoxLayout()
# Define checkboxes
radios = QWidget();
hbox_radio = QHBoxLayout()
self.radioGroup = QButtonGroup()
self.radioGroup.setExclusive(True)
self.radio_ghost_target = QRadioButton()
self.radio_ghost_target.setText("Ghost")
self.radio_robot_target = QRadioButton()
self.radio_robot_target.setText("Robot")
self.radioGroup.addButton(self.radio_ghost_target,0)
self.radioGroup.addButton(self.radio_robot_target,1)
self.radio_ghost_target.setChecked(True)
self.commandGroup = QButtonGroup()
self.commandGroup.setExclusive(True)
self.radio_position_command = QRadioButton()
self.radio_position_command.setText("Position")
self.commandGroup.addButton(self.radio_position_command,0)
self.radio_trajectory_command = QRadioButton()
self.radio_trajectory_command.setText("Trajectory")
self.commandGroup.addButton(self.radio_trajectory_command,1)
self.radio_trajectory_command.setChecked(True)
#print "position button is checked: ",self.radio_position_command.isChecked()
#print "trajectory button is checked: ",self.radio_trajectory_command.isChecked()
print "Default group button checked is ",self.radioGroup.checkedId()
print "Default command button checked is ",self.commandGroup.checkedId()
hbox_radio.addStretch()
hbox_radio.addWidget(self.radio_ghost_target)
#hbox_radio.addWidget(QLabel("Ghost"))
hbox_radio.addStretch()
vbox_robot = QVBoxLayout()
widget_robot = QWidget()
widget_robot_sel = QWidget()
hbox_sel = QHBoxLayout()
hbox_radio.addStretch()
hbox_sel.addWidget(self.radio_robot_target)
#hbox_sel.addWidget(QLabel("Robot"))
hbox_radio.addStretch()
widget_robot_sel.setLayout(hbox_sel)
vbox_robot.addWidget(widget_robot_sel);
widget_cmd = QWidget()
hbox_cmd = QHBoxLayout()
hbox_radio.addStretch()
hbox_cmd.addWidget(self.radio_position_command)
#hbox_cmd.addWidget(QLabel("Position"))
hbox_radio.addStretch()
hbox_cmd.addWidget(self.radio_trajectory_command)
#hbox_cmd.addWidget(QLabel("Trajectory"))
hbox_radio.addStretch()
widget_cmd.setLayout(hbox_cmd)
vbox_robot.addWidget(widget_cmd);
widget_robot.setLayout(vbox_robot)
hbox_radio.addWidget(widget_robot)
radios.setLayout(hbox_radio)
vbox.addWidget(radios)
positions_widget = QWidget()
hbox = QHBoxLayout()
#self.robot = URDF.from_parameter_server()
self.chains=[]
# Subscribe to Joint States update /atlas/atlas_state
#print "Load parameters"
#print rospy.get_param_names()
#fileName = rospy.get_param('positions_file', '/opt/vigir/catkin_ws/src/vigir_ocs_eui/vigir_rqt/vigir_rqt_position_mode/launch/r_arm_positions.txt')
#print "FileName:",fileName
# Left to right layout
numFiles = rospy.get_param("~num_files");
for i in range(1,numFiles+1):
path = "~position_files/file_" + str(i)
foobar = str(rospy.get_param(path))
self.chains.append(ChainData(self, foobar, hbox))
#add stretch at end so all GUI elements are at left of dialog
hbox.addStretch(1)
positions_widget.setLayout(hbox)
vbox.addWidget(positions_widget)
vbox.addStretch(1)
self._widget.setLayout(vbox)
#context.add_widget(self._widget)
self.stateSubscriber = rospy.Subscriber('/atlas/joint_states', JointState, self.stateCallbackFnc)
self.ghostSubscriber = rospy.Subscriber('/flor/ghost/get_joint_states', JointState, self.ghostCallbackFnc)
self.pelvisSubscriber = rospy.Subscriber('/robot_controllers/pelvis_traj_controller/current_states',JointState, self.pelvisCallbackFnc)
def __init__(self, model):
"""
Initializes the Widget.
:param model: the model of the widget
:type model: ROSModel
"""
super(SelectionWidget, self).__init__()
self.setObjectName('selection_widget')
self.__model = model
# Get path to UI file which is a sibling of this file
self.rp = rospkg.RosPack()
ui_file = os.path.join(self.rp.get_path('arni_rqt_detail_plugin'), 'resources', 'SelectionWidget.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self)
self.setObjectName('SelectionWidgetUi')
self.__selected_item = None
self.__draw_graphs = True
self.__current_combo_box_index = 0
self.__last_update = rospy.Time.now()
try:
if rospy.get_param("/enable_statistics") == False:
raise KeyError('/enable_statistics')
except KeyError:
self.__overview_widget = None
raise EnvironmentError("/enable_statistics is either not set or set to false - arni gui would not work correctly. Please make sure to start "
"roscore with the neccesary parameters or to load these while running (see init_params.launch)")
self.__values_dict = {
"bandwidth_mean": 0,
"bandwidth_stddev": 0,
"bandwidth_max": 0,
}
self.__logger = self.__model.get_logger()
self.__log_filter_proxy = LogFilterProxy()
self.__log_filter_proxy.filter_by_item(self.__selected_item)
self.__log_filter_proxy.setDynamicSortFilter(True)
self.__log_filter_proxy.setSourceModel(self.__logger.get_representation())
self.log_tab_tree_view.setModel(self.__log_filter_proxy)
self.__log_delegate = LogDelegate()
self.log_tab_tree_view.setItemDelegate(self.__log_delegate)
self.__style_string = ".detailed_data {\n" \
" font-size: 12\n;" \
"}\n"
self.__style_string = ".erroneous_entry {\n" \
" color: red\n;" \
"}\n"
self.information_tab_text_browser.setStyleSheet(self.__style_string)
self.range_combo_box.clear()
self.range_combo_box.addItem("10 " + self.tr("Seconds"))
self.range_combo_box.addItem("30 " + self.tr("Seconds"))
self.range_combo_box.addItem("60 " + self.tr("Seconds"))
self.range_combo_box.setCurrentIndex(0)
#self.scrollAreaWidgetContents_2.setWidget(self.host_node_label)
self.tab_widget.setTabText(0, self.tr("Information"))
self.tab_widget.setTabText(1, self.tr("Graphs"))
self.tab_widget.setTabText(2, self.tr("Log"))
self.tab_widget.setTabText(3, self.tr("Actions"))
self.stop_push_button.setText(self.tr("Stop Node"))
self.restart_push_button.setText(self.tr("Restart Node"))
self.stop_push_button.setEnabled(False)
self.restart_push_button.setEnabled(False)
### CARSON ADDED ###
# set default values for throttle rate and window sliders
self.throttle_rate_slider.setFocusPolicy(Qt.StrongFocus)
self.throttle_rate_slider.setValue(5000)
self.throttle_window_slider.setFocusPolicy(Qt.StrongFocus)
self.throttle_window_slider.setValue(500)
# set up validator for throttle rate and window text fields
# only allows floating point numbers
regex = QRegExp(r'[0-9]*\.?[0-9]+')
validator = QRegExpValidator(regex)
self.throttle_rate.setValidator(validator)
self.throttle_window.setValidator(validator)
# set up QButtonGroup for message/bandwidth throttle type radio buttons
self.throttle_radio_group = QButtonGroup()
self.throttle_radio_group.addButton(self.throttle_message_radio)
self.throttle_radio_group.addButton(self.throttle_bandwidth_radio)
self.throttle_radio_group.buttonClicked.connect(self.__on_type_button_clicked)
### ###
self.selected_label.setText(self.tr("Selected") + ":")
self.range_label.setText(self.tr("Range") + ":")
self.log_tab_tree_view.setRootIsDecorated(False)
self.log_tab_tree_view.setAlternatingRowColors(True)
self.log_tab_tree_view.setSortingEnabled(True)
self.log_tab_tree_view.sortByColumn(1, Qt.AscendingOrder)
self.__current_range_combo_box_index = 0
self.__current_selected_combo_box_index = 0
self.set_selected_item(self.__selected_item)
self.__model.layoutChanged.connect(self.update)
self.__state = "ok"
self.__previous_state = "ok"
self.__selected_item_changed = True
self.__deleted = False
pg.setConfigOption('background', 'w')
pg.setConfigOption('foreground', 'k')
self.__graph_layout = ResizeableGraphicsLayoutWidget(self.__on_graph_window_size_changed)
self.graph_scroll_area.setWidget(self.__graph_layout)
self.__plotable_items = None#self.__selected_item.get_plotable_items()
self.__items_per_group = 1
self.__expected_items_per_group = 1
self.__number_of_groups = 1
self.__update_graphs_lock = Lock()
self.__first_update_pending = True
self.__graph_dict = {}
self.__plotted_curves = {}
#self.create_graphs()
self.__timer = Timer(Duration(secs=1.0), self.update_graphs)
class SelectionWidget(QWidget):
"""
The SelectionWidget of the ArniGuiDetail-Plugin.
"""
def __init__(self, model):
"""
Initializes the Widget.
:param model: the model of the widget
:type model: ROSModel
"""
super(SelectionWidget, self).__init__()
self.setObjectName('selection_widget')
self.__model = model
# Get path to UI file which is a sibling of this file
self.rp = rospkg.RosPack()
ui_file = os.path.join(self.rp.get_path('arni_rqt_detail_plugin'), 'resources', 'SelectionWidget.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self)
self.setObjectName('SelectionWidgetUi')
self.__selected_item = None
self.__draw_graphs = True
self.__current_combo_box_index = 0
self.__last_update = rospy.Time.now()
try:
if rospy.get_param("/enable_statistics") == False:
raise KeyError('/enable_statistics')
except KeyError:
self.__overview_widget = None
raise EnvironmentError("/enable_statistics is either not set or set to false - arni gui would not work correctly. Please make sure to start "
"roscore with the neccesary parameters or to load these while running (see init_params.launch)")
self.__values_dict = {
"bandwidth_mean": 0,
"bandwidth_stddev": 0,
"bandwidth_max": 0,
}
self.__logger = self.__model.get_logger()
self.__log_filter_proxy = LogFilterProxy()
self.__log_filter_proxy.filter_by_item(self.__selected_item)
self.__log_filter_proxy.setDynamicSortFilter(True)
self.__log_filter_proxy.setSourceModel(self.__logger.get_representation())
self.log_tab_tree_view.setModel(self.__log_filter_proxy)
self.__log_delegate = LogDelegate()
self.log_tab_tree_view.setItemDelegate(self.__log_delegate)
self.__style_string = ".detailed_data {\n" \
" font-size: 12\n;" \
"}\n"
self.__style_string = ".erroneous_entry {\n" \
" color: red\n;" \
"}\n"
self.information_tab_text_browser.setStyleSheet(self.__style_string)
self.range_combo_box.clear()
self.range_combo_box.addItem("10 " + self.tr("Seconds"))
self.range_combo_box.addItem("30 " + self.tr("Seconds"))
self.range_combo_box.addItem("60 " + self.tr("Seconds"))
self.range_combo_box.setCurrentIndex(0)
#self.scrollAreaWidgetContents_2.setWidget(self.host_node_label)
self.tab_widget.setTabText(0, self.tr("Information"))
self.tab_widget.setTabText(1, self.tr("Graphs"))
self.tab_widget.setTabText(2, self.tr("Log"))
self.tab_widget.setTabText(3, self.tr("Actions"))
self.stop_push_button.setText(self.tr("Stop Node"))
self.restart_push_button.setText(self.tr("Restart Node"))
self.stop_push_button.setEnabled(False)
self.restart_push_button.setEnabled(False)
### CARSON ADDED ###
# set default values for throttle rate and window sliders
self.throttle_rate_slider.setFocusPolicy(Qt.StrongFocus)
self.throttle_rate_slider.setValue(5000)
self.throttle_window_slider.setFocusPolicy(Qt.StrongFocus)
self.throttle_window_slider.setValue(500)
# set up validator for throttle rate and window text fields
# only allows floating point numbers
regex = QRegExp(r'[0-9]*\.?[0-9]+')
validator = QRegExpValidator(regex)
self.throttle_rate.setValidator(validator)
self.throttle_window.setValidator(validator)
# set up QButtonGroup for message/bandwidth throttle type radio buttons
self.throttle_radio_group = QButtonGroup()
self.throttle_radio_group.addButton(self.throttle_message_radio)
self.throttle_radio_group.addButton(self.throttle_bandwidth_radio)
self.throttle_radio_group.buttonClicked.connect(self.__on_type_button_clicked)
### ###
self.selected_label.setText(self.tr("Selected") + ":")
self.range_label.setText(self.tr("Range") + ":")
self.log_tab_tree_view.setRootIsDecorated(False)
self.log_tab_tree_view.setAlternatingRowColors(True)
self.log_tab_tree_view.setSortingEnabled(True)
self.log_tab_tree_view.sortByColumn(1, Qt.AscendingOrder)
self.__current_range_combo_box_index = 0
self.__current_selected_combo_box_index = 0
self.set_selected_item(self.__selected_item)
self.__model.layoutChanged.connect(self.update)
self.__state = "ok"
self.__previous_state = "ok"
self.__selected_item_changed = True
self.__deleted = False
pg.setConfigOption('background', 'w')
pg.setConfigOption('foreground', 'k')
self.__graph_layout = ResizeableGraphicsLayoutWidget(self.__on_graph_window_size_changed)
self.graph_scroll_area.setWidget(self.__graph_layout)
self.__plotable_items = None#self.__selected_item.get_plotable_items()
self.__items_per_group = 1
self.__expected_items_per_group = 1
self.__number_of_groups = 1
self.__update_graphs_lock = Lock()
self.__first_update_pending = True
self.__graph_dict = {}
self.__plotted_curves = {}
#self.create_graphs()
self.__timer = Timer(Duration(secs=1.0), self.update_graphs)
def __del__(self):
self.__deleted = True
def connect_slots(self):
"""
Connects the slots.
"""
# : tab_widget
self.tab_widget.currentChanged.connect(self.__on_current_tab_changed)
#: restart_push_button
self.restart_push_button.clicked.connect(self.__on_restart_push_button_clicked)
#: stop_push_button
self.stop_push_button.clicked.connect(self.__on_stop_push_button_clicked)
#: range_combo_box
self.range_combo_box.currentIndexChanged.connect(self.__on_range_combo_box_index_changed)
#pause button
self.pause_button.clicked.connect(self.__on_pause_button_clicked)
# selected combo box
self.selected_combo_box.currentIndexChanged.connect(self.__on_selected_combo_box_index_changed)
### CARSON ADDED ###
# connect sliders to text fields and update text fields for first time
self.throttle_rate_slider.valueChanged.connect(self.__on_throttle_rate_slider_changed)
self.throttle_window_slider.valueChanged.connect(self.__on_throttle_window_slider_changed)
self.__on_throttle_rate_slider_changed(self.throttle_rate_slider.value())
self.__on_throttle_window_slider_changed(self.throttle_window_slider.value())
# connect text fields to sliders
self.throttle_rate.editingFinished.connect(self.__on_throttle_rate_changed)
self.throttle_window.editingFinished.connect(self.__on_throttle_window_changed)
# connect buttons to actions
self.throttle_start_button.clicked.connect(self.__on_throttle_start_button_clicked)
self.throttle_stop_button.clicked.connect(self.__on_throttle_stop_button_clicked)
self.throttle_reset_button.clicked.connect(self.__on_throttle_reset_button_clicked)
### ###
### CARSON ADDED ###
def __on_throttle_rate_slider_changed(self, value):
"""Called whenever throttle rate slider changes value.
Updates the throttle rate text field to match the slider value.
Performs conversion if necessary.
Args:
value (int): new value of throttle rate slider
"""
if self.throttle_radio_group.checkedButton() is self.throttle_message_radio:
value = convert_from_slider(value, THROTTLE_RATE_SLIDER_DPI)
self.throttle_rate.setText(str(value))
def __on_throttle_rate_changed(self):
"""Called whenever throttle rate text field emits the "finishedEditing" signal.
Updates the throttle rate slider to match the throttle rate text field, using
necessary conversions and checking for endpoints.
"""
text = self.throttle_rate.text()
value = float(text)
if self.throttle_radio_group.checkedButton() is self.throttle_message_radio:
value = convert_to_slider(value, THROTTLE_RATE_SLIDER_DPI)
if value%1 != 0:
value = int(value)
else:
value = int(value)
# check for endpoints
if value > self.throttle_rate_slider.maximum():
value = self.throttle_rate_slider.maximum()
if value < self.throttle_rate_slider.minimum():
value = self.throttle_rate_slider.minimum()
# call slider update method manually if value is same (keeps text field in sync)
if self.throttle_rate_slider.value() == value:
self.__on_throttle_rate_slider_changed(value)
self.throttle_rate_slider.setValue(value)
def __on_throttle_window_slider_changed(self, value):
"""Called whenever throttle window slider changes value.
Updates the throttle window text field to match the slider value
using the proper conversion.
Args:
value (int): new value of throttle window slider
"""
self.throttle_window.setText(str(convert_from_slider(value, THROTTLE_WINDOW_SLIDER_DPI)))
def __on_throttle_window_changed(self):
"""Called whenever throttle window text field emits the "finishedEditing" signal.
Updates the throttle window slider to match the throttle window text field, using
necessary conversions and checking for endpoints.
"""
text = self.throttle_window.text()
value = convert_to_slider(float(text), THROTTLE_WINDOW_SLIDER_DPI)
# check for endpoints
if value > self.throttle_window_slider.maximum():
value = self.throttle_window_slider.maximum()
if value < self.throttle_window_slider.minimum():
value = self.throttle_window_slider.minimum()
# call slider update method manually if value is same (keeps text field in sync)
if self.throttle_window_slider.value() == value:
self.__on_throttle_window_slider_changed(value)
self.throttle_window_slider.setValue(value)
def __on_type_button_clicked(self, button):
"""Called whenever a throttle type radio button is clicked.
Performs the necessary GUI updates for the new throttle type.
Args:
button (QRadioButton): currently checked radio button.
"""
throttle = self.__selected_item.topic_item.throttle
if button is self.throttle_message_radio:
self.throttle_rate_label.setText('Rate (Hz)')
self.__update_throttle_window_gui(False)
self.throttle_rate_slider.setMaximum(convert_to_slider(1000, THROTTLE_RATE_SLIDER_DPI))
self.throttle_rate_slider.setTickInterval(convert_to_slider(100, THROTTLE_RATE_SLIDER_DPI))
value = 0
if throttle is None or not self.typeButtonMatchesThrottleType(throttle):
value = convert_to_slider(500, THROTTLE_RATE_SLIDER_DPI)
else:
value = convert_to_slider(throttle.rate, THROTTLE_RATE_SLIDER_DPI)
self.throttle_rate_slider.setValue(value)
self.throttle_rate.setMaxLength(5)
else:
self.throttle_rate_label.setText('Rate (Kb/s)')
self.__update_throttle_window_gui(True)
self.throttle_rate_slider.setMaximum(1e6)
self.throttle_rate_slider.setTickInterval(1e5)
value = 0
if throttle is None or not self.typeButtonMatchesThrottleType(throttle):
value = 5e5
else:
value = throttle.bandwidth
self.throttle_rate_slider.setValue(value)
self.throttle_rate.setMaxLength(7)
self.throttle_window_slider.setValue(convert_to_slider(5, THROTTLE_WINDOW_SLIDER_DPI))
def __on_throttle_start_button_clicked(self):
"""Called whenever the Start Throttle button is clicked.
Starts or updates the throttle as necessary according to which radio button
is active (message or bandwidth)
"""
# grab topic name and inner topic item from selected_item
# this is necessary because only the TopicItem class has the new throttle attribute
topic_name = self.__selected_item.seuid.split('/')[-1]
topic_item = self.__selected_item.topic_item
# convert rate field to number
throttle_rate = float(self.throttle_rate.text())
# determine if we need to make a new throttle or update an existing one
if topic_item.throttle is None or not self.typeButtonMatchesThrottleType(topic_item.throttle):
if topic_item.throttle is not None:
# stop existing throttle if there is one
print(topic_item.throttle.stop())
if self.throttle_radio_group.checkedButton() is self.throttle_message_radio:
topic_item.throttle = MessageThrottle(topic_name, topic_name + '_message_throttled', throttle_rate)
# self.throttle_message_radio.setPalette(self.active_label_palette)
print(topic_item.throttle.start())
print('started message throttler')
else:
# addtionally grab window value for a bandwidth throttle
throttle_window = float(self.throttle_window.text())
topic_item.throttle = BandwidthThrottle(topic_name, topic_name + '_bandwidth_throttled', kb_to_bytes(throttle_rate), throttle_window)
# self.throttle_window_radio.setPalette(self.active_label_palette)
print(topic_item.throttle.start())
print('started bandwidth throttler')
# enable stop button now that a throttle is active
self.throttle_stop_button.setEnabled(True)
else:
if self.throttle_radio_group.checkedButton() is self.throttle_message_radio:
if throttle_rate != topic_item.throttle.rate:
success = topic_item.throttle.update(rate=throttle_rate)
print(success)
if success == None:
print('no running throttle')
else:
print('no updates to throttle necessary')
else:
throttle_window = float(self.throttle_window.text())
if throttle_rate != topic_item.throttle.bandwidth or throttle_window != topic_item.throttle.window:
success = topic_item.throttle.update(bandwidth=throttle_rate, window=throttle_window)
print(success)
if success == None:
print('no running throttle')
else:
print('no updates to throttle necessary')
def __on_throttle_stop_button_clicked(self):
"""Called whenever the Stop Throttle button is clicked.
Stops the running throttle, resets the topic's throttle attribute, and disables
the Stop Throttle button.
"""
print(self.__selected_item.topic_item.throttle.stop())
self.__selected_item.topic_item.throttle = None
self.throttle_stop_button.setEnabled(False)
def __on_throttle_reset_button_clicked(self):
"""Called whenever the Reset Throttle button is clicked.
Resets the throttle parameters to current throttle values or defaults if no
throttle is running.
"""
throttle = self.__selected_item.topic_item.throttle
if throttle is None or not self.typeButtonMatchesThrottleType(throttle):
self.__on_type_button_clicked(self.throttle_radio_group.checkedButton())
else:
if self.throttle_radio_group.checkedButton() is self.throttle_message_radio:
self.throttle_rate_slider.setValue(convert_to_slider(throttle.rate, THROTTLE_RATE_SLIDER_DPI))
else:
self.throttle_rate_slider.setValue(bytes_to_kb(throttle.bandwidth))
self.throttle_window_slider.setValue(convert_to_slider(throttle.window, THROTTLE_WINDOW_SLIDER_DPI))
def typeButtonMatchesThrottleType(self, throttle):
"""Determines if the active type radio button matches the type of the
provided throttle.
Args:
throttle (rosthrottle.MessageThrottle or rosthrottle.BandwidthThrottle): throttle to compare with
"""
message_match = self.throttle_radio_group.checkedButton() is self.throttle_message_radio and isinstance(throttle, MessageThrottle)
bandwidth_match = self.throttle_radio_group.checkedButton() is self.throttle_bandwidth_radio and isinstance(throttle, BandwidthThrottle)
return message_match or bandwidth_match
def __update_throttle_window_gui(self, mode):
"""Updates the throttle window GUI components to the given mode.
Args:
mode (bool): true if enabling throttle window GUI components, false if disabling
"""
self.throttle_window_label.setEnabled(mode)
self.throttle_window_slider.setEnabled(mode)
self.throttle_window.setEnabled(mode)
def __update_throttle_gui(self, mode):
"""Updates the throttle GUI to the given mode.
Args:
mode (bool): true if enabling throttle GUI components, false if disabling
"""
if self.throttle_radio_group.checkedButton() is self.throttle_bandwidth_radio:
self.__update_throttle_window_gui(mode)
self.throttle_rate_slider.setEnabled(mode)
if self.__selected_item.can_execute_throttles():
if self.__selected_item.topic_item.throttle is not None:
self.throttle_stop_button.setEnabled(True)
self.__update_throttle_params(throttle=self.__selected_item.topic_item.throttle)
else:
self.throttle_stop_button.setEnabled(False)
self.__update_throttle_params()
else:
self.throttle_stop_button.setEnabled(mode)
self.throttle_start_button.setEnabled(mode)
self.throttle_rate_label.setEnabled(mode)
self.throttle_bandwidth_radio.setEnabled(mode)
self.throttle_message_radio.setEnabled(mode)
self.throttle_rate.setEnabled(mode)
self.throttle_reset_button.setEnabled(mode)
def __update_throttle_params(self, throttle=None):
"""Updates the throttle parameters based on the given throttle.
Args:
throttle (optional, rosthrottle.MessageThrottle or rosthrottle.BandwidthThrottle): throttle
to use as source of throttle parameters. If not provided, default values are used.
"""
if throttle is None:
self.throttle_message_radio.setChecked(True)
self.__on_type_button_clicked(self.throttle_message_radio)
else:
if isinstance(throttle, MessageThrottle):
self.throttle_message_radio.setChecked(True)
self.__on_type_button_clicked(self.throttle_message_radio)
self.throttle_rate_slider.setValue(convert_to_slider(throttle.rate, THROTTLE_RATE_SLIDER_DPI))
self.throttle_window_slider.setValue(500)
else:
self.throttle_bandwidth_radio.setChecked(True)
self.__on_type_button_clicked(self.throttle_bandwidth_radio)
self.throttle_rate_slider.setValue(bytes_to_kb(throttle.bandwidth))
self.throttle_window_slider.setValue(convert_to_slider(throttle.window, THROTTLE_WINDOW_SLIDER_DPI))
### ###
def create_graphs(self):
"""
Creates the graphs for the plot.
"""
self.__update_graphs_lock.acquire()
if self.__selected_item is not None:
first_iteration = True
first_view = None
i = 0
self.__expected_items_per_group = 0
self.__graph_layout.clear()
if len(self.__plotable_items) is 0:
raise UserWarning()
for key in self.__plotable_items[min(self.__current_selected_combo_box_index *
self.__items_per_group, len(self.__plotable_items)):
min((self.__current_selected_combo_box_index + 1)
* self.__items_per_group, len(self.__plotable_items))]:
plot_widget = None
if first_iteration:
first_iteration = False
date_axis = DateAxis(orientation="bottom")
first_view = pg.ViewBox()
plot_widget = self.__graph_layout.addPlot(title=self.tr(key), axisItems={'bottom': date_axis}, viewBox=first_view)
else:
date_axis = DateAxis(orientation="bottom")
view_box = pg.ViewBox()
plot_widget = self.__graph_layout.addPlot(title=self.tr(key), viewBox=view_box, axisItems={'bottom': date_axis})
view_box.setXLink(first_view)
#performance enhancements when only a short range of the plot is shown
#plot_widget.setClipToView(clip=True)
plot_widget.setYRange(-1, 1)
self.__graph_dict[key] = plot_widget
self.__graph_layout.nextRow()
plot_widget = self.__graph_dict[key]
plot_widget.showGrid(x=True, y=True)
plot_widget.setMenuEnabled(enableMenu=True)
plot_widget.enableAutoRange('xy', True)
x = np.array([1])
y = np.array([int(str(Time.now()))/1000000000])
self.__plotted_curves[key] = plot_widget.plot(x=x, y=y, fillLevel=0, brush=(50, 50, 200, 100),
pen=(255, 0, 0))
self.__expected_items_per_group += 1
self.__first_update_pending = True
self.__update_graphs_lock.release()
def __on_graph_window_size_changed(self):
# getting the size
if self.__selected_item is not None:
size = self.__graph_layout.size()
items_per_group = max(int(math.ceil((size.height() - 100) / 200 + 1)), 1)
if items_per_group is not self.__items_per_group or self.__selected_item_changed:
self.__graph_layout.set_blocked(True)
self.__items_per_group = items_per_group
self.__number_of_groups = int(math.ceil(len(self.__plotable_items) / float(self.__items_per_group)))
# change the groups in the widget
self.selected_combo_box.clear()
for group in range(0, self.__number_of_groups):
list = self.__plotable_items[min(group *
self.__items_per_group, len(self.__plotable_items)):min((group + 1)
* self.__items_per_group, len(self.__plotable_items))]
content = ""
for i in range(0, len(list) - 1):
content += self.tr(list[i])
content += ", "
content += list[len(list) - 1]
self.selected_combo_box.addItem(content)
# redraw
self.create_graphs()
self.update_graphs(None)
self.__graph_layout.set_blocked(False)
def __on_selected_combo_box_index_changed(self, index):
"""
Updates what is shown in the graphs
:param index: the index of the selected range
:type index: int
"""
if index is not -1:
self.__current_selected_combo_box_index = index
self.create_graphs()
self.update_graphs(None)
def __on_pause_button_clicked(self):
"""
To be called whenever the pause button is clicked. Stops the graphs from updating until the pause button
is clicked again and the other way.
"""
if self.__draw_graphs:
self.__draw_graphs = False
self.pause_button.setText(self.tr("Continue"))
else:
self.__draw_graphs = True
self.pause_button.setText(self.tr("Pause"))
def set_selected_item(self, index):
"""
Sets the selected item.
:param index: the index of the selected item
:type selected_item: QModelIndex
"""
if index is not None:
self.__update_graphs_lock.acquire()
src_index = index.model().mapToSource(index)
self.__selected_item = src_index.internalPointer()
self.__log_filter_proxy.filter_by_item(self.__selected_item)
if self.__selected_item is not None:
self.__plotable_items = self.__selected_item.get_plotable_items()
for key in self.__selected_item.get_list_items():
self.__plotable_items.remove(key)
# check if actions can be executed
if self.__selected_item.can_execute_actions():
self.stop_push_button.setEnabled(True)
self.restart_push_button.setEnabled(True)
else:
self.stop_push_button.setEnabled(False)
self.restart_push_button.setEnabled(False)
# check if throttles can be executed
if self.__selected_item.can_execute_throttles():
self.__update_throttle_gui(True)
else:
self.__update_throttle_gui(False)
self.__selected_item_changed = True
self.__update_graphs_lock.release()
self.update()
self.__on_graph_window_size_changed()
def __on_current_tab_changed(self, tab):
"""
Will be called when switching between tabs.
:param tab: index of the current tab
:type tab: int
"""
if tab is 1:
self.__draw_graphs = True
else:
self.__draw_graphs = False
def __on_restart_push_button_clicked(self):
"""
Handels the restart button and restarts a host or node.
"""
if self.__selected_item is not None:
self.__selected_item.execute_action("restart")
def __on_stop_push_button_clicked(self):
"""Handels the stop button and stops a host or node."""
if self.__selected_item is not None:
self.__selected_item.execute_action("stop")
def __on_range_combo_box_index_changed(self, index):
"""
Handels the change of the graph range.
:param index: the index of the selected range
:type index: int
"""
self.__current_combo_box_index = index
def update_graphs(self, event):
"""
Updates and redraws the graphs.
"""
self.__update_graphs_lock.acquire()
if self.__selected_item is not None:
if self.__draw_graphs or self.__first_update_pending:
#print("items per group: " + str(self.__items_per_group))
#print("combo index: " + str(self.__current_selected_combo_box_index))
#print("len plot " + str(len(self.__plotable_items)))
plotable_items = self.__plotable_items[min(self.__current_selected_combo_box_index *
self.__items_per_group, len(self.__plotable_items)):min((self.__current_selected_combo_box_index + 1)
* self.__items_per_group, len(self.__plotable_items))]
plotable_data = self.__selected_item.get_items_younger_than(
#Time.now() - Duration(secs=self.__combo_box_index_to_seconds(self.__current_range_combo_box_index)),
Time.now() - (Duration(secs=(self.__combo_box_index_to_seconds(self.__current_range_combo_box_index) + 10 )) if int(Duration(secs=self.__combo_box_index_to_seconds(self.__current_range_combo_box_index)).to_sec()) <= int(Time.now().to_sec()) else Time(0) ),
"window_stop", *plotable_items)
if "window_stop" in plotable_data:
if plotable_data["window_stop"]:
temp_time = []
temp_content = []
length = len(plotable_data["window_stop"])
modulo = (length / 200) + 1
for i in range(0, length, modulo):
# now having maximally 100 items to plot :)
temp_time.append(plotable_data["window_stop"][i].to_sec())
x = np.array(temp_time)
list_entries = self.__selected_item.get_list_items()
time_entries = self.__selected_item.get_time_items()
for key in plotable_items:
if key in list_entries:
pass
else:
if key in time_entries:
for i in range(0, length, modulo):
temp_content.append(float(str(plotable_data[key][i]))/1000000000)
else:
for i in range(0, length, modulo):
temp_content.append(plotable_data[key][i])
y = np.array(temp_content)
del temp_content[:]
self.__plotted_curves[key].setData(x=x, y=y)
else:
pass
self.__first_update_pending = False
self.__update_graphs_lock.release()
def update(self):
"""
Updates the widget.
"""
if not self.__deleted:
if self.__selected_item is not None:
data_dict = self.__selected_item.get_latest_data()
self.__state = data_dict["state"]
self.host_node_label.setText(self.tr(self.__selected_item.get_seuid()))
if self.__previous_state is not self.__state:
self.__previous_state = self.__state
if self.__state == "ok":
self.current_status_label.setText(self.tr("ok"))
#self.host_node_label.setText(self.tr("Current Status: Ok"))
pixmap = QPixmap(os.path.join(self.rp.get_path('arni_rqt_detail_plugin'), 'resources/graphics',
'block_green.png'))
elif self.__state == "warning":
self.current_status_label.setText(self.tr("warning"))
#self.host_node_label.setText(self.tr("Current Status: Warning"))
pixmap = QPixmap(os.path.join(self.rp.get_path('arni_rqt_detail_plugin'), 'resources/graphics',
'block_orange.png'))
elif self.__state == "unknown":
self.current_status_label.setText(self.tr("unkown"))
#self.host_node_label.setText(self.tr("Current Status: Unkown"))
pixmap = QPixmap(os.path.join(self.rp.get_path('arni_rqt_detail_plugin'), 'resources/graphics',
'block_grey.png'))
else: # error or offline
self.current_status_label.setText(self.tr(self.__state))
pixmap = QPixmap(os.path.join(self.rp.get_path('arni_rqt_detail_plugin'), 'resources/graphics',
'block_red.png'))
self.status_light_label.setPixmap(pixmap)
content = self.__selected_item.get_detailed_data()
scroll_value = self.information_tab_text_browser.verticalScrollBar().value()
self.information_tab_text_browser.setHtml(content)
self.information_tab_text_browser.verticalScrollBar().setSliderPosition(scroll_value)
else:
self.host_node_label.setText(self.tr("No item selected"))
self.current_status_label.setText(self.tr("Offline"))
self.information_tab_text_browser.setText(self.tr("Please select an item in the TreeView to get more information"
" about it"))
def __combo_box_index_to_seconds(self, index):
"""
Calculates the range from the combo-box index.
:param index: the index of teh combo-box
:type index: int
:returns: the seconds of the selected index
:rtype: int
"""
if self.__current_combo_box_index == 0:
return 10
elif self.__current_combo_box_index == 1:
return 30
else:
return 60
def get_current_tab(self):
"""
Returns the current tab.
:returns: the current tab
:rtype: int
"""
return self.tab_widget.currentIndex()
def set_current_tab(self, index=0):
"""
Sets the default tab.
:param index: the index of the tab
:type index: int
"""
if index is None:
index = 0
self.tab_widget.setCurrentIndex(index)
def get_range_combo_box_index(self):
"""
Returns the index of the combo-box.
:returns: the index
:rtype: int
"""
return self.range_combo_box.currentIndex()
def set_range_combo_box_index(self, index=0):
"""
Sets the default value of the combo-box.
:param index: the index of the combo-box
:type index: int
"""
if index is None:
index = 0
self.range_combo_box.setCurrentIndex(index)
def __init__(self, context):
super(JointControlWidget, self).__init__()
self.updateStateSignal.connect(self.on_update_state)
self.updateGhostSignal.connect(self.on_update_ghost)
self.joint_states = JointState()
self.ghost_joint_states = JointState()
self._widget = context
vbox = QVBoxLayout()
# Define checkboxes
radios = QWidget()
hbox_radio = QHBoxLayout()
self.radioGroup = QButtonGroup()
self.radioGroup.setExclusive(True)
self.radio_ghost_target = QRadioButton()
self.radio_ghost_target.setText("Ghost")
self.radioGroup.addButton(self.radio_ghost_target, 0)
self.radio_ghost_target.setChecked(True)
self.radio_robot_target = QRadioButton()
self.radio_robot_target.setText("Robot")
self.radioGroup.addButton(self.radio_robot_target, 1)
hbox_radio.addStretch()
hbox_radio.addWidget(self.radio_ghost_target)
hbox_radio.addStretch()
hbox_radio.addWidget(self.radio_robot_target)
hbox_radio.addStretch()
radios.setLayout(hbox_radio)
vbox.addWidget(radios)
duration_box = QHBoxLayout()
duration_box.setAlignment(Qt.AlignLeft)
duration_box.addWidget(QLabel("Trajectory duration (s):"))
self.traj_duration_spin = QDoubleSpinBox()
self.traj_duration_spin.setValue(1.0)
self.traj_duration_spin.valueChanged.connect(self.on_traj_duration_changed)
duration_box.addWidget(self.traj_duration_spin)
self.update_controllers_buttonn = QPushButton("Update Controllers")
self.update_controllers_buttonn.pressed.connect(self.on_update_controllers)
duration_box.addWidget(self.update_controllers_buttonn)
vbox.addLayout(duration_box)
widget = QWidget()
hbox = QHBoxLayout()
# Left to right layout
self.joint_control = JointControl(self, hbox)
widget.setLayout(hbox)
vbox.addWidget(widget)
print "Add buttons to apply all ..."
all_widget = QWidget()
all_box = QHBoxLayout()
self.snap_to_ghost_button = QPushButton("SnapAllGhost")
self.snap_to_ghost_button.pressed.connect(self.on_snap_ghost_pressed)
all_box.addWidget(self.snap_to_ghost_button)
self.snap_to_current_button = QPushButton("SnapAllCurrent")
self.snap_to_current_button.pressed.connect(self.on_snap_current_pressed)
all_box.addWidget(self.snap_to_current_button)
self.apply_to_robot_button = QPushButton("ApplyAllRobot")
self.apply_to_robot_button.pressed.connect(self.on_apply_robot_pressed)
all_box.addWidget(self.apply_to_robot_button)
self.apply_to_robot_button = QPushButton("Apply WBC Robot")
self.apply_to_robot_button.pressed.connect(self.on_apply_wbc_robot_pressed)
all_box.addWidget(self.apply_to_robot_button)
all_widget.setLayout(all_box)
vbox.addWidget(all_widget)
override_box = QHBoxLayout()
self.override = QCheckBox()
self.override.setChecked(False)
self.override.stateChanged.connect(self.on_override_changed)
override_box.addWidget(self.override)
override_label = QLabel("SAFETY OVERRIDE")
override_label.setStyleSheet('QLabel { color: red }')
override_box.addWidget(override_label)
override_box.addStretch()
vbox.addLayout(override_box)
vbox.addStretch()
self._widget.setLayout(vbox)
self.first_time = True
self.stateSubscriber = rospy.Subscriber('/joint_states', JointState, self.state_callback_fnc)
self.ghostSubscriber = rospy.Subscriber('/flor/ghost/get_joint_states', JointState, self.ghost_callback_fnc)
self.wbc_robot_pub = rospy.Publisher('/flor/wbc_controller/joint_states', JointState, queue_size=10)
self.time_last_update_state = time.time()
self.time_last_update_ghost = time.time()
class NoLimitJointControlDialog(Plugin):
updateStateSignal = Signal(object)
updateGhostSignal = Signal(object)
def __init__(self, context):
super(NoLimitJointControlDialog, self).__init__(context)
self.setObjectName('NoLimitJointControlDialog')
self.updateStateSignal.connect(self.on_updateState)
self.updateGhostSignal.connect(self.on_updateGhost)
self.joint_states = JointState()
self.ghost_joint_states = JointState()
self._widget = QWidget()
vbox = QVBoxLayout()
# Define checkboxes
radios = QWidget();
hbox_radio = QHBoxLayout()
self.radioGroup = QButtonGroup()
self.radioGroup.setExclusive(True)
self.radio_ghost_target = QRadioButton()
self.radio_ghost_target.setText("Ghost")
self.radioGroup.addButton(self.radio_ghost_target,0)
self.radio_ghost_target.setChecked(True)
self.radio_robot_target = QRadioButton()
self.radio_robot_target.setText("Robot")
self.radioGroup.addButton(self.radio_robot_target,1)
hbox_radio.addStretch()
hbox_radio.addWidget(self.radio_ghost_target)
#hbox_radio.addWidget(QLabel("Ghost"))
hbox_radio.addStretch()
hbox_radio.addWidget(self.radio_robot_target)
#hbox_radio.addWidget(QLabel("Robot"))
hbox_radio.addStretch()
radios.setLayout(hbox_radio)
vbox.addWidget(radios)
widget = QWidget()
hbox = QHBoxLayout()
# Left to right layout
self.joint_control = NoLimitJointControl(self, roslib.packages.get_pkg_dir('vigir_rqt_no_limit_joint_control') + '/launch/joints.txt',hbox)
widget.setLayout(hbox)
vbox.addWidget(widget)
print "Add buttons to apply all ..."
all_widget = QWidget()
all_box = QHBoxLayout()
self.snap_to_ghost_button = QPushButton("SnapAllGhost")
self.snap_to_ghost_button.pressed.connect(self.on_snapGhostPressed)
all_box.addWidget(self.snap_to_ghost_button)
self.snap_to_current_button = QPushButton("SnapAllCurrent")
self.snap_to_current_button.pressed.connect(self.on_snapCurrentPressed)
all_box.addWidget(self.snap_to_current_button)
self.apply_to_robot_button = QPushButton("ApplyAllRobot")
self.apply_to_robot_button.pressed.connect(self.on_applyRobotPressed)
all_box.addWidget(self.apply_to_robot_button)
self.apply_to_robot_button = QPushButton("Apply WBC Robot")
self.apply_to_robot_button.pressed.connect(self.on_applyWBCRobotPressed)
all_box.addWidget(self.apply_to_robot_button)
all_widget.setLayout(all_box)
vbox.addWidget(all_widget)
# all_hbox = QHBoxLayout()
# all_hbox.addStretch()
# all_hbox.addWidget(all_widget)
# all_hbox.addStretch()
# bottom_widget=QWidget()
# bottom_widget.setLayout(all_jbox)
# vbox.addWidget(bottom_widget)
vbox.addStretch()
self._widget.setLayout(vbox)
context.add_widget(self._widget)
self.first_time = True
self.stateSubscriber = rospy.Subscriber('/atlas/joint_states', JointState, self.stateCallbackFnc)
self.ghostSubscriber = rospy.Subscriber('/flor/ghost/get_joint_states', JointState, self.ghostCallbackFnc)
self.wbc_robot_pub = rospy.Publisher('/flor/wbc_controller/joint_states',JointState, queue_size=10)
def shutdown_plugin(self):
#Just make sure to stop timers and publishers, unsubscribe from Topics etc in the shutdown_plugin method.
print "Shutdown ..."
#rospy.sleep(0.1)
self.stateSubscriber.unregister()
self.ghostSubscriber.unregister()
self.wbc_robot_pub.unregister()
print "Done!"
def stateCallbackFnc(self, atlasState_msg):
self.updateStateSignal.emit(atlasState_msg)
def ghostCallbackFnc(self, ghost_joint_state_msg):
self.updateGhostSignal.emit(ghost_joint_state_msg)
def on_snapGhostPressed(self):
print "Snap all joint values to current ghost joint positions"
for controller in self.joint_control.controllers:
controller.on_snapGhostPressed()
def on_snapCurrentPressed(self):
print "Snap all joint values to current joint positions"
for controller in self.joint_control.controllers:
controller.on_snapCurrentPressed()
def on_applyRobotPressed(self):
print "Send all latest joint values directly to robot"
for controller in self.joint_control.controllers:
controller.on_applyRobotPressed()
def on_applyWBCRobotPressed(self):
print "Send all latest joint values directly to robot as a WBC command"
if self.first_time:
print "Uninitialized !"
else:
joint_state = JointState()
joint_state.header.stamp = rospy.Time.now()
joint_state.name = copy.deepcopy(self.joint_states.name)
joint_state.position = list(copy.deepcopy(self.joint_states.position))
joint_state.velocity = list(copy.deepcopy(self.joint_states.velocity))
#print "Positions: ",joint_state.position
for i,name in enumerate(joint_state.name):
# for each joint, retrieve the data from each controller to populate the WBC vectors
for controller in self.joint_control.controllers:
for joint in controller.joints:
if joint.name == self.joint_states.name[i]:
print " fetching value for "+name+" = "+str(joint.position)+" at ndx="+str(i)
joint_state.position[i] = joint.position
joint_state.velocity[i] = joint.velocity
#joint_state.effort[i] = joint.effort
self.wbc_robot_pub.publish(joint_state)
# this runs in the Qt GUI thread and can therefore update the GUI
def on_updateState(self, atlasState_msg):
self.joint_states = atlasState_msg
self.joint_control.updateJointPositions(self.joint_states,self.first_time)
if self.first_time:
self.joint_control.resetCurrentJointSliders()
self.first_time = False
self.ghost_joint_states = copy.deepcopy(atlasState_msg);
self.ghost_joint_states.position = list(self.ghost_joint_states.position)
#print "Initial Ghost Stored:",self.ghost_joint_states
# this runs in the Qt GUI thread and can therefore update the GUI
def on_updateGhost(self, ghost_joint_state_msg):
for i, new_joint_name in enumerate(list(ghost_joint_state_msg.name)):
name_found = False
for j,stored_joint_name in enumerate(self.ghost_joint_states.name):
if (new_joint_name == stored_joint_name):
name_found = True
#print "ghost_joint_msg[",str(i),"]=",ghost_joint_state_msg.position[i]
#print "ghost_joint_states[",str(j),"]=",self.ghost_joint_states.position[i]
self.ghost_joint_states.position[j] = ghost_joint_state_msg.position[i]
#self.ghost_joint_states.velocity[j] = ghost_joint_state_msg.velocity[i]
if (not name_found):
# Update the list without regard to order
self.ghost_joint_states.name.append(new_joint_name)
self.ghost_joint_states.position.append(ghost_joint_state_msg.position[i])
def __init__(self, context):
super(NoLimitJointControlDialog, self).__init__(context)
self.setObjectName('NoLimitJointControlDialog')
self.updateStateSignal.connect(self.on_updateState)
self.updateGhostSignal.connect(self.on_updateGhost)
self.joint_states = JointState()
self.ghost_joint_states = JointState()
self._widget = QWidget()
vbox = QVBoxLayout()
# Define checkboxes
radios = QWidget();
hbox_radio = QHBoxLayout()
self.radioGroup = QButtonGroup()
self.radioGroup.setExclusive(True)
self.radio_ghost_target = QRadioButton()
self.radio_ghost_target.setText("Ghost")
self.radioGroup.addButton(self.radio_ghost_target,0)
self.radio_ghost_target.setChecked(True)
self.radio_robot_target = QRadioButton()
self.radio_robot_target.setText("Robot")
self.radioGroup.addButton(self.radio_robot_target,1)
hbox_radio.addStretch()
hbox_radio.addWidget(self.radio_ghost_target)
#hbox_radio.addWidget(QLabel("Ghost"))
hbox_radio.addStretch()
hbox_radio.addWidget(self.radio_robot_target)
#hbox_radio.addWidget(QLabel("Robot"))
hbox_radio.addStretch()
radios.setLayout(hbox_radio)
vbox.addWidget(radios)
widget = QWidget()
hbox = QHBoxLayout()
# Left to right layout
self.joint_control = NoLimitJointControl(self, roslib.packages.get_pkg_dir('vigir_rqt_no_limit_joint_control') + '/launch/joints.txt',hbox)
widget.setLayout(hbox)
vbox.addWidget(widget)
print "Add buttons to apply all ..."
all_widget = QWidget()
all_box = QHBoxLayout()
self.snap_to_ghost_button = QPushButton("SnapAllGhost")
self.snap_to_ghost_button.pressed.connect(self.on_snapGhostPressed)
all_box.addWidget(self.snap_to_ghost_button)
self.snap_to_current_button = QPushButton("SnapAllCurrent")
self.snap_to_current_button.pressed.connect(self.on_snapCurrentPressed)
all_box.addWidget(self.snap_to_current_button)
self.apply_to_robot_button = QPushButton("ApplyAllRobot")
self.apply_to_robot_button.pressed.connect(self.on_applyRobotPressed)
all_box.addWidget(self.apply_to_robot_button)
self.apply_to_robot_button = QPushButton("Apply WBC Robot")
self.apply_to_robot_button.pressed.connect(self.on_applyWBCRobotPressed)
all_box.addWidget(self.apply_to_robot_button)
all_widget.setLayout(all_box)
vbox.addWidget(all_widget)
# all_hbox = QHBoxLayout()
# all_hbox.addStretch()
# all_hbox.addWidget(all_widget)
# all_hbox.addStretch()
# bottom_widget=QWidget()
# bottom_widget.setLayout(all_jbox)
# vbox.addWidget(bottom_widget)
vbox.addStretch()
self._widget.setLayout(vbox)
context.add_widget(self._widget)
self.first_time = True
self.stateSubscriber = rospy.Subscriber('/atlas/joint_states', JointState, self.stateCallbackFnc)
self.ghostSubscriber = rospy.Subscriber('/flor/ghost/get_joint_states', JointState, self.ghostCallbackFnc)
self.wbc_robot_pub = rospy.Publisher('/flor/wbc_controller/joint_states',JointState, queue_size=10)
def buildRadioButtons(self,
labelTextArray,
orientation,
alignment,
activeButtons=None,
behavior=CheckboxGroupBehavior.RADIO_BUTTONS):
'''
@param labelTextArray: Names of buttons
@type labelTextArray: [string]
@param orientation: Whether to arrange the buttons vertically or horizontally.
@type orientation: Orientation
@param alignment: whether buttons should be aligned Left/Center/Right for horizontal,
Top/Center/Bottom for vertical:/c
@type alignment: Alignment
@param activeButtons: Name of the buttons that is to be checked initially. Or None.
@type activeButtons: [string]
@param behavior: Indicates whether the button group is to behave like Radio Buttons, or like Checkboxes.
@type behavior: CheckboxGroupBehavior
@return
1. The button group that contains the related buttons. Caller: ensure that
this object does not go out of scope.
2. The button layout, which callers will need to add to
their own layouts.
3. and a dictionary mapping button names to button objects that
were created within this method. This dict is needed by the
controller.
@rtype (QButtonGroup, QLayout, dict<string,QRadioButton>).
'''
# Button control management group. It has no visible
# representation. It allows the radio button bahavior, for instance:
buttonGroup = QButtonGroup();
if behavior == CheckboxGroupBehavior.CHECKBOXES:
buttonGroup.setExclusive(False);
else:
buttonGroup.setExclusive(True);
if orientation == Orientation.VERTICAL:
buttonCompLayout = QVBoxLayout();
# Arrange for the alignment (Part 1):
if (alignment == Alignment.CENTER) or\
(alignment == Alignment.BOTTOM):
buttonCompLayout.addStretch(1);
else:
buttonCompLayout = QHBoxLayout();
# Arrange for the alignment (Part 1):
if (alignment == Alignment.CENTER) or\
(alignment == Alignment.RIGHT):
buttonCompLayout.addStretch(1);
# Build the buttons:
buttonDict = {};
for label in labelTextArray:
button = QRadioButton(label);
buttonDict[label] = button;
# Add button to the button management group:
buttonGroup.addButton(button);
# Add the button to the visual group:
buttonCompLayout.addWidget(button);
if label in activeButtons:
button.setChecked(True);
if orientation == Orientation.VERTICAL:
buttonCompLayout = QVBoxLayout();
# Arrange for the alignment (Part 2):
if (alignment == Alignment.CENTER) or\
(alignment == Alignment.TOP):
buttonCompLayout.addStretch(1);
else:
# Arrange for the alignment (Part 2):
if (alignment == Alignment.CENTER) or\
(alignment == Alignment.LEFT):
buttonCompLayout.addStretch(1);
return (buttonGroup, buttonCompLayout, buttonDict);
class JointControlWidget(QObject):
updateStateSignal = Signal(object)
updateGhostSignal = Signal(object)
def __init__(self, context):
super(JointControlWidget, self).__init__()
self.updateStateSignal.connect(self.on_update_state)
self.updateGhostSignal.connect(self.on_update_ghost)
self.joint_states = JointState()
self.ghost_joint_states = JointState()
self._widget = context
vbox = QVBoxLayout()
# Define checkboxes
radios = QWidget()
hbox_radio = QHBoxLayout()
self.radioGroup = QButtonGroup()
self.radioGroup.setExclusive(True)
self.radio_ghost_target = QRadioButton()
self.radio_ghost_target.setText("Ghost")
self.radioGroup.addButton(self.radio_ghost_target, 0)
self.radio_ghost_target.setChecked(True)
self.radio_robot_target = QRadioButton()
self.radio_robot_target.setText("Robot")
self.radioGroup.addButton(self.radio_robot_target, 1)
hbox_radio.addStretch()
hbox_radio.addWidget(self.radio_ghost_target)
hbox_radio.addStretch()
hbox_radio.addWidget(self.radio_robot_target)
hbox_radio.addStretch()
radios.setLayout(hbox_radio)
vbox.addWidget(radios)
duration_box = QHBoxLayout()
duration_box.setAlignment(Qt.AlignLeft)
duration_box.addWidget(QLabel("Trajectory duration (s):"))
self.traj_duration_spin = QDoubleSpinBox()
self.traj_duration_spin.setValue(1.0)
self.traj_duration_spin.valueChanged.connect(self.on_traj_duration_changed)
duration_box.addWidget(self.traj_duration_spin)
self.update_controllers_buttonn = QPushButton("Update Controllers")
self.update_controllers_buttonn.pressed.connect(self.on_update_controllers)
duration_box.addWidget(self.update_controllers_buttonn)
vbox.addLayout(duration_box)
widget = QWidget()
hbox = QHBoxLayout()
# Left to right layout
self.joint_control = JointControl(self, hbox)
widget.setLayout(hbox)
vbox.addWidget(widget)
print "Add buttons to apply all ..."
all_widget = QWidget()
all_box = QHBoxLayout()
self.snap_to_ghost_button = QPushButton("SnapAllGhost")
self.snap_to_ghost_button.pressed.connect(self.on_snap_ghost_pressed)
all_box.addWidget(self.snap_to_ghost_button)
self.snap_to_current_button = QPushButton("SnapAllCurrent")
self.snap_to_current_button.pressed.connect(self.on_snap_current_pressed)
all_box.addWidget(self.snap_to_current_button)
self.apply_to_robot_button = QPushButton("ApplyAllRobot")
self.apply_to_robot_button.pressed.connect(self.on_apply_robot_pressed)
all_box.addWidget(self.apply_to_robot_button)
self.apply_to_robot_button = QPushButton("Apply WBC Robot")
self.apply_to_robot_button.pressed.connect(self.on_apply_wbc_robot_pressed)
all_box.addWidget(self.apply_to_robot_button)
all_widget.setLayout(all_box)
vbox.addWidget(all_widget)
override_box = QHBoxLayout()
self.override = QCheckBox()
self.override.setChecked(False)
self.override.stateChanged.connect(self.on_override_changed)
override_box.addWidget(self.override)
override_label = QLabel("SAFETY OVERRIDE")
override_label.setStyleSheet('QLabel { color: red }')
override_box.addWidget(override_label)
override_box.addStretch()
vbox.addLayout(override_box)
vbox.addStretch()
self._widget.setLayout(vbox)
self.first_time = True
self.stateSubscriber = rospy.Subscriber('/joint_states', JointState, self.state_callback_fnc)
self.ghostSubscriber = rospy.Subscriber('/flor/ghost/get_joint_states', JointState, self.ghost_callback_fnc)
self.wbc_robot_pub = rospy.Publisher('/flor/wbc_controller/joint_states', JointState, queue_size=10)
self.time_last_update_state = time.time()
self.time_last_update_ghost = time.time()
def shutdown_plugin(self):
# Just make sure to stop timers and publishers, unsubscribe from Topics etc in the shutdown_plugin method.
print "Shutdown ..."
self.stateSubscriber.unregister()
self.ghostSubscriber.unregister()
self.wbc_robot_pub.unregister()
print "Done!"
def state_callback_fnc(self, atlas_state_msg):
self.updateStateSignal.emit(atlas_state_msg)
def command_callback_fnc(self, atlas_command_msg):
self.updateCommandSignal.emit(atlas_command_msg)
def ghost_callback_fnc(self, ghost_joint_state_msg):
self.updateGhostSignal.emit(ghost_joint_state_msg)
def on_update_controllers(self):
self.joint_control.update_controllers()
def on_override_changed(self):
if self.override.isChecked():
print "WARNING: TURNING OFF SAFETY"
for controller in self.joint_control.controllers:
controller.set_override(True)
else:
print "Enabling safety checks"
for controller in self.joint_control.controllers:
controller.set_override(False)
def on_snap_ghost_pressed(self):
print "Snap all joint values to current ghost joint positions"
for controller in self.joint_control.controllers:
controller.on_snap_ghost_pressed()
def on_snap_current_pressed(self):
print "Snap all joint values to current joint positions"
for controller in self.joint_control.controllers:
controller.on_snap_current_pressed()
def on_apply_robot_pressed(self):
print "Send all latest joint values directly to robot"
for controller in self.joint_control.controllers:
controller.on_apply_robot_pressed()
def on_traj_duration_changed(self, duration):
self.joint_control.update_traj_duration(duration)
def on_apply_wbc_robot_pressed(self):
print "Send all latest joint values directly to robot as a WBC command"
if self.first_time:
print "Uninitialized !"
else:
joint_state = JointState()
joint_state.header.stamp = rospy.Time.now()
joint_state.name = copy.deepcopy(self.joint_states.name)
joint_state.position = list(copy.deepcopy(self.joint_states.position))
joint_state.velocity = list(copy.deepcopy(self.joint_states.velocity))
# print "Positions: ",joint_state.position
for i, name in enumerate(joint_state.name):
# for each joint, retrieve the data from each controller to populate the WBC vectors
for controller in self.joint_control.controllers:
for joint in controller.joints:
if joint.name == self.joint_states.name[i]:
print " fetching value for " + name + " = " + str(joint.position) + " at ndx=" + str(i)
joint_state.position[i] = joint.position
joint_state.velocity[i] = joint.velocity
# joint_state.effort[i] = joint.effort
self.wbc_robot_pub.publish(joint_state)
def on_update_state(self, atlas_state_msg):
if time.time() - self.time_last_update_state >= 0.2:
self.joint_states = atlas_state_msg
self.joint_control.update_joint_positions(self.joint_states)
if self.first_time:
self.joint_control.reset_current_joint_sliders()
self.first_time = False
self.ghost_joint_states = copy.deepcopy(atlas_state_msg)
self.ghost_joint_states.position = list(self.ghost_joint_states.position)
self.time_last_update_state = time.time()
# this runs in the Qt GUI thread and can therefore update the GUI
def on_update_ghost(self, ghost_joint_state_msg):
if time.time() - self.time_last_update_ghost >= 0.2:
for i, new_joint_name in enumerate(list(ghost_joint_state_msg.name)):
name_found = False
for j, stored_joint_name in enumerate(self.ghost_joint_states.name):
if new_joint_name == stored_joint_name:
name_found = True
self.ghost_joint_states.position[j] = ghost_joint_state_msg.position[i]
if not name_found:
# Update the list without regard to order
self.ghost_joint_states.name.append(new_joint_name)
self.ghost_joint_states.position.append(ghost_joint_state_msg.position[i])
self.time_last_update_ghost = time.time()
