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='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 _add_radio_button(self, group, group_name, name):
radio_button = QRadioButton(name)
radio_button.toggled.connect(
lambda checked: self.on_radio_button_toggled(
group_name, name, checked))
group.layout().addWidget(radio_button)
return radio_button
def __init__(self,
nodename,
masteruri,
loggername,
level='INFO',
parent=None):
'''
Creates a new item.
'''
QFrame.__init__(self, parent)
self.setObjectName("LoggerItem")
self.nodename = nodename
self.masteruri = masteruri
self.loggername = loggername
self.current_level = None
layout = QHBoxLayout(self)
layout.setContentsMargins(1, 1, 1, 1)
self.debug = QRadioButton()
self.debug.setStyleSheet(
"QRadioButton{ background-color: #39B54A;}") # QColor(57, 181, 74)
self.debug.toggled.connect(self.toggled_debug)
layout.addWidget(self.debug)
self.info = QRadioButton()
self.info.setStyleSheet("QRadioButton{ background-color: #FFFAFA;}")
self.info.toggled.connect(self.toggled_info)
layout.addWidget(self.info)
self.warn = QRadioButton()
self.warn.setStyleSheet(
"QRadioButton{ background-color: #FFC706;}") # QColor(255, 199, 6)
self.warn.toggled.connect(self.toggled_warn)
layout.addWidget(self.warn)
self.error = QRadioButton()
self.error.setStyleSheet(
"QRadioButton{ background-color: #DE382B;}") # QColor(222, 56, 43)
self.error.toggled.connect(self.toggled_error)
layout.addWidget(self.error)
self.fatal = QRadioButton()
self.fatal.setStyleSheet("QRadioButton{ background-color: #FF0000;}")
self.fatal.toggled.connect(self.toggled_fatal)
layout.addWidget(self.fatal)
self.label = QLabel(loggername)
layout.addWidget(self.label)
layout.addStretch()
self._callback = None # used to set all logger
self.success_signal.connect(self.on_succes_update)
self.error_signal.connect(self.on_error_update)
self.set_level(level)
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']))
class LoggerItem(QFrame):
'''
Represents one ROS logger and offers methods to change the logger level.
'''
success_signal = Signal(str)
error_signal = Signal(str)
def __init__(self,
nodename,
masteruri,
loggername,
level='INFO',
parent=None):
'''
Creates a new item.
'''
QFrame.__init__(self, parent)
self.setObjectName("LoggerItem")
self.nodename = nodename
self.masteruri = masteruri
self.loggername = loggername
self.current_level = None
layout = QHBoxLayout(self)
layout.setContentsMargins(1, 1, 1, 1)
self.debug = QRadioButton()
self.debug.setStyleSheet(
"QRadioButton{ background-color: #39B54A;}") # QColor(57, 181, 74)
self.debug.toggled.connect(self.toggled_debug)
layout.addWidget(self.debug)
self.info = QRadioButton()
self.info.setStyleSheet("QRadioButton{ background-color: #FFFAFA;}")
self.info.toggled.connect(self.toggled_info)
layout.addWidget(self.info)
self.warn = QRadioButton()
self.warn.setStyleSheet(
"QRadioButton{ background-color: #FFC706;}") # QColor(255, 199, 6)
self.warn.toggled.connect(self.toggled_warn)
layout.addWidget(self.warn)
self.error = QRadioButton()
self.error.setStyleSheet(
"QRadioButton{ background-color: #DE382B;}") # QColor(222, 56, 43)
self.error.toggled.connect(self.toggled_error)
layout.addWidget(self.error)
self.fatal = QRadioButton()
self.fatal.setStyleSheet("QRadioButton{ background-color: #FF0000;}")
self.fatal.toggled.connect(self.toggled_fatal)
layout.addWidget(self.fatal)
self.label = QLabel(loggername)
layout.addWidget(self.label)
layout.addStretch()
self._callback = None # used to set all logger
self.success_signal.connect(self.on_succes_update)
self.error_signal.connect(self.on_error_update)
self.set_level(level)
def set_callback(self, callback):
self._callback = callback
def toggled_debug(self, state):
if state:
self.set_level('DEBUG')
def toggled_info(self, state):
if state:
self.set_level('INFO')
def toggled_warn(self, state):
if state:
self.set_level('WARN')
def toggled_error(self, state):
if state:
self.set_level('ERROR')
def toggled_fatal(self, state):
if state:
self.set_level('FATAL')
def on_succes_update(self, level):
if level.upper() == 'DEBUG':
self.debug.setChecked(True)
elif level.upper() == 'INFO':
self.info.setChecked(True)
elif level.upper() == 'WARN':
self.warn.setChecked(True)
elif level.upper() == 'ERROR':
self.error.setChecked(True)
elif level.upper() == 'FATAL':
self.fatal.setChecked(True)
elif level:
rospy.logwarn("loglevel not found '%s'" % (level))
return
self.current_level = level
def on_error_update(self, level):
self.on_succes_update(level)
def set_level(self, level):
if self.current_level is not None:
if self._callback is not None:
self._callback(level)
else:
# call set loglevel service
thread = threading.Thread(target=self._set_level,
kwargs={
'level': level,
'current_level':
self.current_level
})
thread.setDaemon(True)
thread.start()
pass
else:
self.on_succes_update(level)
def _set_level(self, level, current_level):
try:
backup_level = current_level
service_name = '%s/set_logger_level' % self.nodename
# get service URI from ROS-Master
master = xmlrpcclient.ServerProxy(self.masteruri)
code, _, serviceuri = master.lookupService(rospy.get_name(),
service_name)
if code == 1:
self.call_service_set_level(serviceuri, service_name,
self.loggername, level)
self.success_signal.emit(level)
except rospy.ServiceException as e:
rospy.logwarn("Set logger %s for %s to %s failed: %s" %
(self.loggername, self.nodename, level, e))
if backup_level is not None:
self.error_signal.emit(backup_level)
@classmethod
def call_service_set_level(cls, serviceuri, servicename, loggername,
level):
_req, _resp = nm.starter().callService(
serviceuri,
servicename,
SetLoggerLevel,
service_args=[loggername, level])
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 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);
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)
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(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 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()