class HrpsysLogMenu(MenuDashWidget):
"""
a button to download log of hrpsys
"""
def __init__(self):
base_icon = '/usr/share/icons/Humanity/actions/32/document-new.svg'
icons = [['bg-grey.svg', base_icon], ['bg-green.svg', base_icon],
['bg-red.svg', base_icon]]
super(HrpsysLogMenu, self).__init__('hrpsys log', icons)
self.update_state(0)
self.add_action('download rtm log', self.on_download)
self.setFixedSize(self._icons[0].actualSize(QSize(50, 30)))
self.setToolTip("Download RTM log")
def on_download(self):
try:
hrpsys_save = rospy.ServiceProxy(
"/DataLoggerServiceROSBridge/save",
OpenHRP_DataLoggerService_save)
name = "/tmp/rtclog-" + time.strftime("%Y%m%d%H%M%S")
print "Writing log data to ", name
hrpsys_save(OpenHRP_DataLoggerService_saveRequest(name))
print "Done writing", name
except rospy.ServiceException, e:
mb = QMessageBox(
QMessageBox.NoIcon, "Error",
"Failed to save rtcd log: service call failed with error: %s" %
(e), QMessageBox.Ok, self.parent())
mb.exec_()
except Exception, e:
mb = QMessageBox(QMessageBox.NoIcon, "Error", str(e),
QMessageBox.Ok, self.parent())
mb.exec_()
def on_servo_on(self):
try:
if self.start_command:
Popen(['bash', '-c', self.start_command])
elif self.use_hrpsys_configurator:
execHrpsysConfiguratorCommand("hcf.servoOn()")
else:
servo = rospy.ServiceProxy(
"/RobotHardwareServiceROSBridge/servo",
OpenHRP_RobotHardwareService_servo)
power = rospy.ServiceProxy(
"/RobotHardwareServiceROSBridge/power",
OpenHRP_RobotHardwareService_power)
actual = rospy.ServiceProxy(
"/StateHolderServiceROSBridge/goActual",
OpenHRP_StateHolderService_goActual)
power(OpenHRP_RobotHardwareService_powerRequest("all", 0))
time.sleep(1)
actual(OpenHRP_StateHolderService_goActualRequest())
time.sleep(2)
servo(OpenHRP_RobotHardwareService_servoRequest("all", 0))
except Exception, e:
mb = QMessageBox(QMessageBox.NoIcon, "Error",
"Failed to servo on: %s" % (e), QMessageBox.Ok,
self.parent())
mb.exec_()
def _check_for_master(self):
# check if master is available
try:
rospy.get_master().getSystemState()
return
except Exception:
pass
# spawn thread to detect when master becomes available
self._wait_for_master_thread = threading.Thread(
target=self._wait_for_master)
self._wait_for_master_thread.start()
self._wait_for_master_dialog = QMessageBox(
QMessageBox.Question, self.tr('Waiting for ROS master'),
self.
tr("Could not find ROS master. Either start a 'roscore' or abort loading the plugin."
), QMessageBox.Abort)
self._master_found_signal.connect(self._wait_for_master_dialog.done,
Qt.QueuedConnection)
button = self._wait_for_master_dialog.exec_()
# check if master existence was not detected by background thread
no_master = button != QMessageBox.Ok
self._wait_for_master_dialog.deleteLater()
self._wait_for_master_dialog = None
if no_master:
raise PluginLoadError(
'RosPyPluginProvider._init_node() could not find ROS master')
def __init__(self, context):
super(NodeManager, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('NodeManagerFKIE')
# Process standalone plugin command-line arguments
from argparse import ArgumentParser
parser = ArgumentParser()
# Add argument(s) to the parser.
parser.add_argument("-q", "--quiet", action="store_true",
dest="quiet",
help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
if not args.quiet:
print 'arguments: ', args
print 'unknowns: ', unknowns
node_manager_fkie.init_settings()
masteruri = node_manager_fkie.settings().masteruri()
node_manager_fkie.init_globals(masteruri)
# Create QWidget
try:
self._widget = MainWindow()
# self._widget.read_view_history()
except Exception, e:
msgBox = QMessageBox()
msgBox.setText(str(e))
msgBox.exec_()
raise
def on_power_off(self):
try:
power = rospy.ServiceProxy("/RobotHardwareServiceROSBridge/power", OpenHRP_RobotHardwareService_power )
power(OpenHRP_RobotHardwareService_powerRequest("all",1))
except Exception, e:
mb = QMessageBox(QMessageBox.NoIcon, "Error",
"Failed to power off: %s"%(e),
QMessageBox.Ok, self.parent())
mb.exec_()
def _export_region(self, path, topics, start_stamp, end_stamp):
"""
Starts a thread to save the current selection to a new bag file
:param path: filesystem path to write to, ''str''
:param topics: topics to write to the file, ''list(str)''
:param start_stamp: start of area to save, ''rospy.Time''
:param end_stamp: end of area to save, ''rospy.Time''
"""
if not self.start_background_task('Copying messages to "%s"' % path):
return
# TODO implement a status bar area with information on the current save status
bag_entries = list(
self.get_entries_with_bags(topics, start_stamp, end_stamp))
if self.background_task_cancel:
return
# Get the total number of messages to copy
total_messages = len(bag_entries)
# If no messages, prompt the user and return
if total_messages == 0:
QMessageBox(QMessageBox.Warning, 'rqt_bag', 'No messages found',
QMessageBox.Ok).exec_()
self.stop_background_task()
return
# Open the path for writing
try:
export_bag = rosbag.Bag(path, 'w')
except Exception:
QMessageBox(QMessageBox.Warning, 'rqt_bag',
'Error opening bag file [%s] for writing' % path,
QMessageBox.Ok).exec_()
self.stop_background_task()
return
# Run copying in a background thread
self._export_thread = threading.Thread(target=self._run_export_region,
args=(export_bag, topics,
start_stamp, end_stamp,
bag_entries))
self._export_thread.start()
def _showWarning(self, title, body):
if threading.current_thread().name != "MainThread":
rospy.logwarn("{}: {}".format(title, body))
else:
msg = QMessageBox()
msg.setIcon(QMessageBox.Warning)
msg.setWindowTitle(title)
msg.setText(body)
msg.setStandardButtons(QMessageBox.Ok)
msg.exec_()
def on_halt_motors(self, evt):
halt = rospy.ServiceProxy("pr2_etherCAT/halt_motors",
std_srvs.srv.Empty)
try:
halt()
except rospy.ServiceException, e:
QMessageBox(
self, 'Error',
'Failed to halt the motors: service call failed with error: %s'
% (e))
def on_download(self):
try:
hrpsys_save = rospy.ServiceProxy("/DataLoggerServiceROSBridge/save", OpenHRP_DataLoggerService_save )
name = "/tmp/rtclog-" + time.strftime("%Y%m%d%H%M%S")
print "Writing log data to ",name
hrpsys_save(OpenHRP_DataLoggerService_saveRequest(name))
print "Done writing",name
except rospy.ServiceException, e:
mb = QMessageBox(QMessageBox.NoIcon, "Error",
"Failed to save rtcd log: service call failed with error: %s"%(e),
QMessageBox.Ok, self.parent())
mb.exec_()
def on_sit_down(self):
self.bodyPoseClient.send_goal_and_wait(
BodyPoseGoal(pose_name='crouch'))
state = self.bodyPoseClient.get_state()
if state == actionlib.GoalStatus.SUCCEEDED:
self.stiffnessDisableClient.call()
else:
QMessageBox(
self, 'Error',
'crouch pose did not succeed: %s - cannot remove stiffness' %
self.bodyPoseClient.get_goal_status_text())
rospy.logerror("crouch pose did not succeed: %s",
self.bodyPoseClient.get_goal_status_text())
def apply_posture(self):
posture = self.currentText()
rospy.loginfo("go to posture: " + str(posture))
self.bodyPoseClient.send_goal_and_wait(
BodyPoseWithSpeedGoal(posture_name=posture, speed=0.7))
state = self.bodyPoseClient.get_state()
if not state == actionlib.GoalStatus.SUCCEEDED:
QMessageBox(
self, 'Error',
str(posture) +
' posture did not succeed: %s - cannot remove stiffness' %
self.bodyPoseClient.get_goal_status_text())
rospy.logerror("crouch pose did not succeed: %s",
self.bodyPoseClient.get_goal_status_text())
def _run_export_region(self, export_bag, topics, start_stamp, end_stamp,
bag_entries):
"""
Threaded function that saves the current selection to a new bag file
:param export_bag: bagfile to write to, ''rosbag.bag''
:param topics: topics to write to the file, ''list(str)''
:param start_stamp: start of area to save, ''rospy.Time''
:param end_stamp: end of area to save, ''rospy.Time''
"""
total_messages = len(bag_entries)
update_step = max(1, total_messages / 100)
message_num = 1
progress = 0
# Write out the messages
for bag, entry in bag_entries:
if self.background_task_cancel:
break
try:
topic, msg, t = self.read_message(bag, entry.position)
export_bag.write(topic, msg, t)
except Exception as ex:
qWarning('Error exporting message at position %s: %s' %
(str(entry.position), str(ex)))
export_bag.close()
self.stop_background_task()
return
if message_num % update_step == 0 or message_num == total_messages:
new_progress = int(100.0 *
(float(message_num) / total_messages))
if new_progress != progress:
progress = new_progress
if not self.background_task_cancel:
self.background_progress = progress
self.status_bar_changed_signal.emit()
message_num += 1
# Close the bag
try:
self.background_progress = 0
self.status_bar_changed_signal.emit()
export_bag.close()
except Exception as ex:
QMessageBox(
QMessageBox.Warning, 'rqt_bag',
'Error closing bag file [%s]: %s' %
(export_bag.filename, str(ex)), QMessageBox.Ok).exec_()
self.stop_background_task()
def __init__(self, rqt_plugin_name):
super(Switcher, self).__init__()
self._rqt_plugin_name = rqt_plugin_name
self._rqt_subprocess = None
self._no_ros_master_dialog = QMessageBox(
QMessageBox.Warning, self.tr(self._rqt_plugin_name),
self.tr("Waiting for the ROS Master to become available."))
self._no_ros_master_abort_button = self._no_ros_master_dialog.addButton(
QMessageBox.Abort)
self._no_ros_master_abort_button.clicked.connect(self.shutdown)
self.signal_master_changed_state.connect(self.switch_state,
type=Qt.QueuedConnection)
self._ros_master_monitor = RosMasterMonitor(
period=1, callback_function=self._master_changed_state)
self._rqt_subprocess_shutdown_by_us = False
def start_background_task(self, background_task):
"""
Verify that a background task is not currently running before starting a new one
:param background_task: name of the background task, ''str''
"""
if self.background_task is not None:
QMessageBox(
QMessageBox.Warning, 'Exclamation',
'Background operation already running:\n\n%s' %
self.background_task, QMessageBox.Ok).exec_()
return False
self.background_task = background_task
self.background_task_cancel = False
return True
def on_servo_off(self):
try:
if self.stop_command:
Popen(['bash', '-c', self.stop_command])
elif self.use_hrpsys_configurator:
execHrpsysConfiguratorCommand("hcf.servoOff()")
else:
power = rospy.ServiceProxy("/RobotHardwareServiceROSBridge/power", OpenHRP_RobotHardwareService_power )
servo = rospy.ServiceProxy("/RobotHardwareServiceROSBridge/servo", OpenHRP_RobotHardwareService_servo )
servo(OpenHRP_RobotHardwareService_servoRequest("all", 1))
time.sleep(1)
power(OpenHRP_RobotHardwareService_powerRequest("all",1))
except Exception, e:
mb = QMessageBox(QMessageBox.NoIcon, "Error",
"Failed to servo off: %s"%(e),
QMessageBox.Ok, self.parent())
mb.exec_()
def show(self):
# append folder of 'qt_gui_cpp/lib' to module search path
qt_gui_cpp_path = os.path.realpath(get_package_path('qt_gui_cpp'))
sys.path.append(os.path.join(qt_gui_cpp_path, 'lib'))
sys.path.append(os.path.join(qt_gui_cpp_path, 'src'))
from qt_gui_cpp.cpp_binding_helper import qt_gui_cpp
import rospkg
_rospkg_version = getattr(rospkg, '__version__', '< 0.2.4')
logo = os.path.join(self._qtgui_path, 'resource', 'ros_org_vertical.png')
text = '<img src="%s" width="56" height="200" style="float: left;"/>' % logo
text += '<h3 style="margin-top: 1px;">%s</h3>' % self.tr('rqt')
text += '<p>%s %s</p>' % (self.tr('rqt is a framework for graphical user interfaces.'), self.tr('It is extensible with plugins which can be written in either Python or C++.'))
text += '<p>%s</p>' % (self.tr('Please see the <a href="%s">Wiki</a> for more information on rqt and available plugins.' % 'http://wiki.ros.org/rqt'))
text += '<p>%s: ' % self.tr('Utilized libraries:')
text += 'Python %s, ' % platform.python_version()
text += 'rospkg %s, ' % _rospkg_version
if QT_BINDING == 'pyside':
text += 'PySide'
elif QT_BINDING == 'pyqt':
text += 'PyQt'
text += ' %s (%s), ' % (QT_BINDING_VERSION, ', '.join(sorted(QT_BINDING_MODULES)))
text += 'Qt %s, ' % qVersion()
if qt_gui_cpp is not None:
if QT_BINDING == 'pyside':
text += '%s' % (self.tr('%s C++ bindings available') % 'Shiboken')
elif QT_BINDING == 'pyqt':
text += '%s' % (self.tr('%s C++ bindings available') % 'SIP')
else:
text += '%s' % self.tr('C++ bindings available')
else:
text += '%s' % self.tr('no C++ bindings found - no C++ plugins available')
text += '.</p>'
mb = QMessageBox(QMessageBox.NoIcon, self.tr('About rqt'), text, QMessageBox.Ok, self.parent())
mb.exec_()
def dasherr(msg, obj, title='Error'):
"""
Logs a message with ``rospy.logerr`` and displays a ``QMessageBox`` to the user
:param msg: Message to display.
:type msg: str
:param obj: Parent object for the ``QMessageBox``
:type obj: QObject
:param title: An optional title for the `QMessageBox``
:type title: str
"""
rospy.logerr(msg)
box = QMessageBox()
box.setText(msg)
box.setWindowTitle(title)
box.show()
obj._message_box = box
def show_warning(self, text):
qWarning(text)
msgBox = QMessageBox()
msgBox.setText(text)
msgBox.exec_()
