def _rqt_shutdown(self):
'''
Shutdown handler coming from the rqt subprocess shutting down. This needs
to be handled differently depending on whether it's us shutting it down
because a ros master shut down (in which case we need to stay alive)
or otherwise.
'''
if not self._rqt_subprocess_shutdown_by_us:
self._ros_master_monitor.shutdown()
QCoreApplication.instance().quit()
else:
self._rqt_subprocess_shutdown_by_us = False
def shutdown(self):
'''
Shutdowns from external signals or the abort button while waiting for a master.
'''
print("Shutting down the ros master monitor")
self._ros_master_monitor.shutdown()
if self._rqt_subprocess is not None:
print("Terminating subprocess")
self._rqt_subprocess.send_signal(signal.SIGINT)
# self._rqt_subprocess.terminate() # this is SIGTERM
self._rqt_subprocess = None
QCoreApplication.instance().quit()
def timerEvent(self, event):
if rospy.is_shutdown():
QCoreApplication.instance().quit()
# needed to move qt calls out of the ros callback, otherwise qt crashes because of inter-thread communication
if self._set_window_visibility:
self._set_window_visibility = False
if not self.isVisible() and self._last_window_control_data == self._window:
self.show()
self.setGeometry(self._geometry)
elif self.isVisible() and (not self._last_window_control_data or self._last_window_control_data == -self._window):
self._geometry = self.geometry()
self.hide()
def timerEvent(self, event):
if rospy.is_shutdown():
QCoreApplication.instance().quit()
# needed to move qt calls out of the ros callback, otherwise qt crashes because of inter-thread communication
if self._set_window_visibility:
self._set_window_visibility = False
if not self.isVisible(
) and self._last_window_control_data == self._window:
self.show()
self.setGeometry(self._geometry)
elif self.isVisible() and (not self._last_window_control_data
or self._last_window_control_data
== -self._window):
self._geometry = self.geometry()
self.hide()
def update_ui(self):
if self.target_poses:
count_target_poses = len(self.target_poses)
else:
count_target_poses = 1
self.progress_bar.setMaximum(count_target_poses)
self.progress_bar.setValue(self.current_target_pose + 1)
self.pose_number_lbl.setText('{}/{}'.format(
self.current_target_pose + 1, count_target_poses))
if self.state == CalibrationMovementsGUI.BAD_PLAN:
self.bad_plan_lbl.setText('BAD plan!! Don\'t do it!!!!')
self.bad_plan_lbl.setStyleSheet('QLabel { background-color : red}')
elif self.state == CalibrationMovementsGUI.GOOD_PLAN:
self.bad_plan_lbl.setText('Good plan')
self.bad_plan_lbl.setStyleSheet(
'QLabel { background-color : green}')
else:
self.bad_plan_lbl.setText('No plan yet')
self.bad_plan_lbl.setStyleSheet('')
if self.state == CalibrationMovementsGUI.NOT_INITED_YET:
self.guide_lbl.setText(
'Bring the robot to a plausible position and check if it is a suitable starting pose'
)
elif self.state == CalibrationMovementsGUI.CHECKING_STARTING_POSITION:
self.guide_lbl.setText(
'Checking if the robot can translate and rotate in all directions from the current pose'
)
elif self.state == CalibrationMovementsGUI.BAD_STARTING_POSITION:
self.guide_lbl.setText('Cannot calibrate from current position')
elif self.state == CalibrationMovementsGUI.GOOD_STARTING_POSITION:
self.guide_lbl.setText('Ready to start: click on next pose')
elif self.state == CalibrationMovementsGUI.GOOD_PLAN:
self.guide_lbl.setText(
'The plan seems good: press execute to move the robot')
elif self.state == CalibrationMovementsGUI.BAD_PLAN:
self.guide_lbl.setText('Planning failed: try again')
elif self.state == CalibrationMovementsGUI.MOVED_TO_POSE:
self.guide_lbl.setText(
'Pose reached: take a sample and go on to next pose')
can_plan = self.state == CalibrationMovementsGUI.GOOD_STARTING_POSITION
self.plan_btn.setEnabled(can_plan)
can_move = self.state == CalibrationMovementsGUI.GOOD_PLAN
self.execute_btn.setEnabled(can_move)
QCoreApplication.processEvents()
def run(self):
"""
Thread body. loops and notifies the listener of new messages
"""
while not self._stop_flag:
# Wait for a new message
cv = self.timeline._messages_cvs[self.topic]
with cv:
while (self.topic not in self.timeline._messages) or (self.bag_msg_data == self.timeline._messages[self.topic]):
cv.wait()
if self._stop_flag:
return
bag_msg_data = self.timeline._messages[self.topic]
# View the message
self.bag_msg_data = bag_msg_data
try:
event = ListenerEvent(bag_msg_data)
QCoreApplication.postEvent(self.listener, event)
except Exception, ex:
qWarning('Error notifying listener %s: %s' % (type(self.listener), str(ex)))
def run(self):
"""
Thread body. loops and notifies the listener of new messages
"""
while not self._stop_flag:
# Wait for a new message
cv = self.timeline._messages_cvs[self.topic]
with cv:
while (self.topic not in self.timeline._messages) or (self.bag_msg_data == self.timeline._messages[self.topic]):
cv.wait()
if self._stop_flag:
return
bag_msg_data = self.timeline._messages[self.topic]
# View the message
self.bag_msg_data = bag_msg_data
try:
event = ListenerEvent(bag_msg_data)
QCoreApplication.postEvent(self.listener, event)
except Exception as ex:
qWarning('Error notifying listener %s: %s' % (type(self.listener), str(ex)))
class Message(QObject):
DEBUG = 10
INFO = 20
WARN = 30
ERROR = 40
FATAL = 50
SEVERITY_LABELS = {
DEBUG: QCoreApplication.translate('Message', 'Debug'),
INFO: QCoreApplication.translate('Message', 'Info'),
WARN: QCoreApplication.translate('Message', 'Warn'),
ERROR: QCoreApplication.translate('Message', 'Error'),
FATAL: QCoreApplication.translate('Message', 'Fatal'),
}
_next_id = 1
def __init__(self):
super(Message, self).__init__()
self.id = Message._next_id
Message._next_id += 1
self.message = None
self.severity = None
self.node = None
self.__stamp = (None, None)
self.location = None
self._stamp_compare = None
self._stamp_qdatetime = None
self._stamp_format = None
self._stamp_string = None
self.highlighted = True
def _get_stamp(self):
return self.__stamp
def _set_stamp(self, stamp):
"""
Update the string representation of the timestamp.
:param stamp: a tuple containing seconds and nano seconds
"""
assert len(stamp) == 2
self.__stamp = stamp
if None not in self.__stamp:
# shortest string representation to compare stamps
# floats do not provide enough precision
self._stamp_compare = '%08x%08x' % (self.__stamp[0],
self.__stamp[1])
else:
self._stamp_compare = None
self._stamp_qdatetime = self._get_stamp_as_qdatetime(self.__stamp)
if self._stamp_format:
s = self._stamp_qdatetime.toString(self._stamp_format)
if 'ZZZ' in s:
s = s.replace('ZZZ', str(self.__stamp[1]).zfill(9))
self._stamp_string = s
stamp = property(_get_stamp, _set_stamp)
def get_stamp_for_compare(self):
return self._stamp_compare
def get_stamp_as_qdatetime(self):
return self._stamp_qdatetime
def _get_stamp_as_qdatetime(self, stamp):
if None in self.__stamp:
return None
dt = QDateTime()
dt.setTime_t(stamp[0])
dt.addMSecs(int(float(stamp[1]) / 10**6))
return dt
def get_stamp_string(self):
return self._stamp_string
def set_stamp_format(self, stamp_format):
self._stamp_format = stamp_format
if None not in self.__stamp:
self.stamp = self.__stamp
def pretty_print(self):
text = self.tr('Node: ') + self.node + '\n'
text += self.tr('Time: ') + self.get_stamp_string() + '\n'
text += self.tr('Severity: ') + Message.SEVERITY_LABELS[
self.severity] + '\n'
text += '\n' + self.message + '\n'
text += '\n' + 'Location:'
text += '\n' + self.location + '\n\n'
text += '-' * 100 + '\n\n'
return text
