class DragDropList(QListWidget):
''' QListWidget with an event that is called when a drag and drop action was performed.'''
keyPressed = pyqtSignal()
def __init__(self, parent, ui):
super(DragDropList, self).__init__(parent)
self.ui = ui
self.setAcceptDrops(True)
def dropEvent(self, e):
super(DragDropList, self).dropEvent(e)
items = []
for i in range(0, self.count()):
items.append(self.item(i).text())
self.ui.change_frame_order(items)
def keyPressEvent(self, event):
if event.key() == Qt.Key_Delete:
super(DragDropList, self).keyPressEvent(event)
self.keyPressed.emit()
elif event.key() == Qt.Key_Up and self.currentRow()-1 >= 0:
self.setCurrentRow(self.currentRow()-1)
elif event.key() == Qt.Key_Down and self.currentRow()+1 < self.count():
self.setCurrentRow(self.currentRow()+1)
class ROS_Subscriber(QObject):
received = pyqtSignal(int)
def __init__(self, topic, type, cb):
super(ROS_Subscriber, self).__init__()
self.subscriber = rospy.Subscriber(topic, type, cb, topic)
self.topic = topic
class Calculator(QObject):
aChanged = pyqtSignal(float)
bChanged = pyqtSignal(float)
sumChanged = pyqtSignal(float)
def __init__(self, parent=None):
super(Calculator, self).__init__(parent)
self._a = 0.0
self._b = 0.0
self._sum = 0.0
self.aChanged.connect(lambda _: self._calculate())
self.bChanged.connect(lambda _: self._calculate())
@pyqtProperty(float, notify=aChanged)
def a(self):
return self._a
@a.setter
def a(self, value):
if value == self._a:
return
self._a = value
self.aChanged.emit(value)
@pyqtProperty(float, notify=bChanged)
def b(self):
return self._b
@b.setter
def b(self, value):
if value == self._b:
return
self._b = value
self.bChanged.emit(value)
@pyqtProperty(float, notify=sumChanged)
def sum(self):
return self._sum
def _calculate(self):
self._sum = self._a + self._b
self.sumChanged.emit(self._sum)
class CustomPushButton(QPushButton):
signalPushedButton = pyqtSignal(int)
def __init__(self, id):
super(CustomPushButton, self).__init__()
self.id = id
self.clicked.connect(self.send_id)
def send_id(self)
class ActuatorPlugin(Plugin):
switches_changed = pyqtSignal(Switches)
def __init__(self, context):
super(ActuatorPlugin, self).__init__(context)
self.setObjectName('ActuatorPlugin')
self._widget = QWidget()
loadUi(os.path.join(uipath, 'actuatorplugin.ui'), self._widget)
context.add_widget(self._widget)
self._setvalve = rospy.ServiceProxy('actuator_driver/set_valve', SetValve)
self._pulsevalve = rospy.ServiceProxy('actuator_driver/pulse_valve', PulseValve)
self._switches_sub = rospy.Subscriber('actuator_driver/switches', Switches,
lambda msg: self.switches_changed.emit(msg))
self.switches_changed.connect(self._on_switches_changed)
self._widget.findChild(QPushButton, 'shootLeftButton').clicked.connect(
lambda: self._pulsevalve(VALVE_SHOOTER_LEFT, rospy.Duration(.3)))
self._widget.findChild(QPushButton, 'shootRightButton').clicked.connect(
lambda: self._pulsevalve(VALVE_SHOOTER_RIGHT, rospy.Duration(.3)))
self._widget.findChild(QPushButton, 'extendButton').clicked.connect(
lambda: self._setvalve(VALVE_GAS_POWERED_STICK, True))
self._widget.findChild(QPushButton, 'retractButton').clicked.connect(
lambda: self._setvalve(VALVE_GAS_POWERED_STICK, False))
self._widget.findChild(QPushButton, 'openButton').clicked.connect(self._open_grabber)
self._widget.findChild(QPushButton, 'closeButton').clicked.connect(self._close_grabber)
self._widget.findChild(QPushButton, 'dropButton').clicked.connect(
lambda: self._pulsevalve(VALVE_DROPPER, rospy.Duration(.5)))
self._widget.findChild(QPushButton, 'shutAllValvesButton').clicked.connect(self._shut_valves)
def shutdown_plugin(self):
self._switches_sub.unregister()
def _open_grabber(self):
self._setvalve(VALVE_GRABBER_CLOSE, False)
self._setvalve(VALVE_GRABBER_OPEN, True)
def _close_grabber(self):
self._setvalve(VALVE_GRABBER_OPEN, False)
self._setvalve(VALVE_GRABBER_CLOSE, True)
def _shut_valves(self):
for i in xrange(6):
self._setvalve(i, False)
def _on_switches_changed(self, msg):
if all(msg.pressed):
msg = '<span style="color: red">Pressed</span>'
elif any(msg.pressed):
msg = '<span style="color: yellow">Single pressed</span>'
else:
msg = '<span style="color: green">Released</span>'
self._widget.findChild(QLabel, 'switchLabel').setText(msg)
class CustomComboBox(QComboBox):
signalIndexChanged = pyqtSignal(int, int)
def __init__(self, id):
super(CustomComboBox, self).__init__()
self.id = id
self.currentIndexChanged.connect(self.send_index_and_id)
def send_index_and_id(self, index):
self.signalIndexChanged.emit(index, self.id)
class CustomCheckBox(QCheckBox):
signalStateChanged = pyqtSignal(int, int, int)
def __init__(self, label, row, column):
super(CustomCheckBox, self).__init__(label)
self.row = row
self.column = column
self.stateChanged.connect(self.send_row_and_col)
def send_row_and_col(self, state):
self.signalStateChanged.emit(state, self.row, self.column)
class Subscriber(QQuickItem, QQmlParserStatus):
on_message = pyqtSignal('QVariantMap',
name='messageChanged',
arguments=['msg'])
def __init__(self, parent=None):
super(Subscriber, self).__init__(parent)
self._topic = None
self._queue_size = None
self._sub = None
self._msg_dict = dict()
@pyqtProperty(Topic)
def topic(self):
return self._topic
@topic.setter
def topic(self, topic):
self._topic = topic
@pyqtProperty(int)
def queueSize(self):
return self._queue_size
@queueSize.setter
def queueSize(self, queue_size):
self._queue_size = queue_size
@pyqtProperty('QVariantMap', notify=on_message)
def message(self):
return self._msg_dict
@message.setter
def message(self, msg):
self._msg_dict = msg
self.on_message.emit(self._msg_dict)
def componentComplete(self):
if not self.topic:
rospy.logfatal('QML property Subscriber.topic is not set')
sys.exit(1)
def message_callback(self, data):
msg_dict = message_converter.convert_ros_message_to_dictionary(data)
self.message = msg_dict
def subscribe(self):
message_class = roslib.message.get_message_class(self.topic.type)
self._sub = rospy.Subscriber(self._topic.name,
message_class,
self.message_callback,
queue_size=self._queue_size)
class StatusDialog(QObject):
status_signal = pyqtSignal(object)
def __init__(self):
super(StatusDialog, self).__init__()
self.status = ''
self.joints = ''
self.position = ''
self.orientation = ''
self.velocity = 0
rospy.Subscriber('iris_sami/arm_status', ArmInfo, self.status_subscriber)
def status_subscriber(self, data):
if data.success:
self.status = data.feedback
self.joints = [round(x, 3) for x in data.joints]
pos = data.pose.position
self.position = [round(x, 5) for x in [pos.x, pos.y, pos.z]]
ori = data.pose.orientation
self.orientation = [round(x, 5) for x in euler_from_quaternion([ori.x, ori.y, ori.z, ori.w])]
self.velocity = data.velocity
def get_status(self):
# Call status service to obtain information about the robot
rate = rospy.Rate(10)
while not rospy.is_shutdown():
joints_str = '[' + ', '.join(['%.3f (<b>%.1f°</b>)' % (x, degrees(x)) for x in self.joints]) + ']'
status_text = ("<html> \
<b>Status:</b> %s<br>\
<b>Controller:</b> Positional<br>\
<b>Joints:</b> %s<br>\
<b>Position:</b> %s<br>\
<b>Orientation:</b> %s<br>\
<b>Velocity:</b> %.1f \
</html>" % (self.status, joints_str, str(self.position),
str(self.orientation), self.velocity))
self.status_signal.emit(status_text)
rate.sleep()
class ProblemViewerWidget(QWidget):
_problem_text = ""
_update_signal = pyqtSignal()
def __init__(self, plugin=None):
super(ProblemViewerWidget, self).__init__()
# Create QWidget
ui_file = os.path.join(rospkg.RosPack().get_path('rosplan_rqt'),
'resource', 'problem_viewer.ui')
loadUi(ui_file, self)
self.setObjectName('ROSPlanProblemViewer')
self._plugin = plugin
rospy.Subscriber("/kcl_rosplan/problem", String, self.problem_received)
self._update_signal.connect(self.update_problem)
"""
updating problem view
"""
def problem_received(self, msg):
self._problem_text = msg.data
self._update_signal.emit()
def update_problem(self):
self.textEdit.setPlainText(self._problem_text)
"""
Qt methods
"""
def shutdown_plugin(self):
pass
def save_settings(self, plugin_settings, instance_settings):
pass
def restore_settings(self, plugin_settings, instance_settings):
pass
class ResponsiveWidget(QWidget):
press_enter = pyqtSignal()
def __init__(self, plugin):
super(ResponsiveWidget, self).__init__()
self.gui = plugin
def keyPressEvent(self, event):
k = event.key()
rospy.loginfo("Key pressed: {}".format(k))
if k == Qt.Key_Left or k == Qt.Key_Right or k == Qt.Key_Down:
self.gui.keyboard_lock.lock()
self.gui.last_key_pressed = k
self.gui.keyboard_lock.unlock()
rospy.loginfo("Arrow key pressed")
self.gui.keyboard_condition.wakeAll()
if k == Qt.Key_Enter or k == Qt.Key_Return:
rospy.loginfo("Enter key pressed")
self.gui.keyboard_lock.lock()
self.gui.last_key_pressed = k
self.gui.keyboard_lock.unlock()
self.press_enter.emit()
class LearnerThread(QThread):
show_question = pyqtSignal(str)
show_fixed = pyqtSignal(str)
show_tab = pyqtSignal(int)
show_score = pyqtSignal(str)
show_score_outro = pyqtSignal(str)
flash_question = pyqtSignal(int)
flash_label = pyqtSignal(int)
def __init__(self, plugin):
super(LearnerThread, self).__init__()
self.gui = plugin
self.current_phase = None
self.learners = None
self.nao_talk = rospy.ServiceProxy('nao_speech', speech)
self.nao_led = rospy.ServiceProxy('nao_led', led)
# Initialize log dictionary
self.experiment_data = dict()
# Time - to name the log folder
self.ts = time.time()
# Initialize learner
self._initialize_learner()
def run(self):
rospy.loginfo("Thread started")
while True:
self.gui.phase_lock.lock()
try:
rospy.loginfo("Wait for new phase")
self.gui.phase_condition.wait(self.gui.phase_lock)
rospy.loginfo("New phase!")
self.current_phase = self.gui.sm
self.current_learner = self.gui.training_counter
finally:
self.gui.phase_lock.unlock()
if self.current_phase == 1:
self._training_phase()
elif self.current_phase == 4:
self._learning_phase(self.current_learner)
elif self.current_phase == 6:
self._experiment_log()
rospy.loginfo("Exit learner thread")
break
def _training_phase(self):
fixed_question = "Are these animals MAMMALS?"
if self.gui.finnish_help:
fixed_question += '\n' + 'Ovatko nämä eläimet NISÄKKÄITÄ?'
mixed_animals = ['Zebra', 'Lion', 'Squirrel', 'Dolphin', 'Blue whale',\
'Penguin', 'Python', 'Hawk', 'Salmon', 'Frog']
mixed_animals_fi = ['Seepra', 'Leijona', 'Orava', 'Delfiini', \
'Sinivalas', 'Pingviini', 'Python', 'Haukka', 'Lohi', 'Sammakko']
# randomize these questions
combined = list(zip(mixed_animals, mixed_animals_fi))
random.shuffle(combined)
mixed_animals[:], mixed_animals_fi[:] = zip(*combined)
# add training question to log, create empty answer list
self.experiment_data['training_questions'] = mixed_animals
self.experiment_data['training_answers'] = list()
self.show_fixed.emit("")
self.show_question.emit("Training task")
time.sleep(2)
resp = self.nao_talk("Let's start the training!\\eos=1\\ " + \
"Please reply to my questions by pressing the arrow keys on the" +\
" keyboard.\\pau=800\\" +\
"Right arrow for Yes,\\eos=1\\Left arrow for No\\eos=1\\and" +\
"\\eos=1\\Down arrow \\eos=1\\ if you don't know." \
, True, True)
self.show_question.emit("")
self.show_fixed.emit(fixed_question)
resp = self.nao_talk("Let's start!", False, True)
time.sleep(2)
verbose_starting = ['What about ', 'How about ', 'And ']
for i, animale in enumerate(mixed_animals):
# Ask question
if i == 0:
question = verbose_starting[0] +\
animale + '?'
self.nao_talk(question, False, True)
if self.gui.finnish_help:
question += '\n' '(' + mixed_animals_fi[i] + ')'
first_question = False
else:
r = np.argmax(np.random.multinomial(1,\
[0.2, 0.2, 0.6]))
question = verbose_starting[r] +\
animale + '?'
self.nao_talk(question, False, True)
if self.gui.finnish_help:
question += '\n' '(' + mixed_animals_fi[i] + ')'
self.show_question.emit(question)
self.gui.keyboard_lock.lock()
try:
rospy.loginfo("Wait for keystroke")
self.gui.keyboard_condition.wait(self.gui.keyboard_lock)
rospy.loginfo("Received keystroke!")
answer = self.gui.last_key_pressed
# Decode the answer
if answer == Qt.Key_Right:
answer = 1
elif answer == Qt.Key_Left:
answer = 0
elif answer == Qt.Key_Down:
answer = -1
else:
answer = -1
rospy.logwarn("Received invalid answer")
finally:
self.gui.keyboard_lock.unlock()
# log the answer
self.experiment_data['training_answers'].append(answer)
self.answer_feedback(answer)
self.show_question.emit("")
self.show_question.emit("")
self.show_fixed.emit("")
self.nao_talk("Cool, that ends the training session.\\eos=1\\" +\
" I hope now it is clear how to answer my questions.\\eos=1\\" +\
" If you have some questions, please ask the experimenter.\\eos=1\\" +\
" Press Enter when you are ready to continue.", True, True)
# Partial log after training session
self._experiment_log(partial=True)
self.show_question.emit("Training completed. Press Enter to continue.")
self.gui.phase_lock.lock()
self.gui.sm = 2
rospy.logwarn('SM: --> 2')
self.gui.phase_lock.unlock()
def _initialize_learner(self):
# list of names following the wn nomenclature
self.entities = list()
# list of names and ids following the awa nomenclature
self.entities_awa = list()
self.entities_id = list()
# list of names in finnish
self.entities_fin = list()
# list of names and ids of the attributes
self.attributes = list()
self.attributes_id = list()
# Path of the dataset
if rospy.has_param('/dataset_path'):
dataset_path = rospy.get_param('/dataset_path')
# Collect the entities of AwA2: ids and names
with open(dataset_path + '/classes_wn.txt', 'r') as f:
for line in f:
entity = line.split()[1].replace('+', '_')
entity_id = int(line.split()[0]) - 1
self.entities.append(entity)
self.entities_id.append(entity_id)
with open(dataset_path + '/classes.txt', 'r') as f:
for line in f:
entity = line.split()[1].replace('+', ' ')
entity_id = int(line.split()[0]) - 1
self.entities_awa.append(entity)
with open(dataset_path + '/classes_finnish.txt', 'r') as f:
for line in f:
entity = line.split()[1].replace('+', ' ')
entity_id = int(line.split()[0]) - 1
self.entities_fin.append(entity)
# Build the category tree
self.ct = al.CategoryTree('mammal.n.01', similarity_tree_gamma=0.7)
self.ct.add_leaves(self.entities)
self.ct.simplify_tree()
# Collect the attributes of AwA2
with open(dataset_path + '/predicates.txt', 'r') as f:
for line in f:
attribute = line.split()[1]
attribute_id = int(line.split()[0]) - 1
self.attributes.append(attribute)
self.attributes_id.append(attribute_id)
full_table = np.loadtxt(open(dataset_path +\
'/predicate-matrix-binary.txt', 'r'))
binary_table = full_table[np.asarray(self.entities_id, dtype=int), :]
self.binary_table = binary_table[:, np.asarray(self.attributes_id,\
dtype=int)]
# Compute distance for the hybrid learner
w = at.Walker()
paths = list()
for entity in self.entities:
for other in self.entities:
if entity is not other:
paths.append(w.walk(self.ct.node_dictionary[\
self.ct.leaves_to_wn[entity]],self.ct.node_dictionary\
[self.ct.leaves_to_wn[other]]))
distances = [(len(path[0]) + len(path[2])) for path in paths]
self.min_distance = min(distances)
self.max_distance = max(distances)
# Experiment parameters from rosparam
parameters_available = \
rospy.has_param('/time_bag')
if parameters_available:
self.time_bag = rospy.get_param('/time_bag')
else:
rospy.logerr('Parameters needed are not available')
exit(5)
self.selected_attributes = np.array([[22, 51], [30, 52], [31, 54]])
self.verbose_attributes = np.array([\
['Do these animals have PAWS?','Do these animals EAT FISH?'],\
['Do these animals have HORNS?', 'Do these animals EAT MEAT?'],\
['Do these animals have CLAWS?','Are these animals HERBIVORE?']])
self.verbose_attributes_fi = np.array([\
['Onko näillä eläimillä TASSUT?','Syövätkö nämä eläimet KALAA?'],\
['Onko näillä eläimillä SARVET?', 'Syövätkö nämä eläimet LIHAA?'],\
['Onko näillä eläimillä KYNNET?','Ovatko nämä eläimet KASVISSYÖJIÄ?']])
self.verbose_attributes_nao = np.array([\
['Do these animals have\\pau=200\\ paws?','Do these animals\\pau=200\\ eat fish?'],\
['Do these animals have\\pau=200\\ horns?', 'Do these animals\\pau=200\\ eat meat?'],\
['Do these animals have\\pau=200\\ claws?','Are these animals\\pau=200\\ herbivore?']])
# Initialize data for logging
self.experiment_data['attributes'] = self.selected_attributes
self.experiment_data['time_budget'] = self.time_bag
def _experiment_log(self, partial=False):
if partial:
rospy.logwarn('PARTIAL LOGGING')
else:
rospy.logwarn('COMPLETE LOGGING')
directory_name = \
datetime.datetime.fromtimestamp(self.ts).strftime('%m%d_%H%M')
# add name to directory_name
directory_name += self.gui.experiment_name
# path for the log
if rospy.has_param('/log_path'):
folderpath = os.path.join(rospy.get_param('/log_path'), \
directory_name)
if not os.path.exists(folderpath):
os.makedirs(folderpath)
if partial:
partial_folderpath = os.path.join(folderpath, 'partial')
if not os.path.exists(partial_folderpath):
os.makedirs(partial_folderpath)
filepath = os.path.join(partial_folderpath, 'partial.pkl')
else:
filepath = os.path.join(folderpath, 'experiment.pkl')
f = open(filepath, 'wb')
pickle.dump(self.experiment_data, f)
f.close()
rospy.logwarn('LOGGING DONE')
def _learning_phase(self, id_learner):
# Initialize variables
self.experiment_data['learners'] = self.learners
learner = self.learners[id_learner]
number_of_questions = len(self.entities)
hard_performance_threshold = 0.49
verbose_starting = ['What about ', 'How about ', 'And ']
if id_learner == 0:
self.nao_talk("Let us start with the first teaching session"+\
"\\eos=1\\ I will ask two sets of questions.", True, True)
else:
self.nao_talk("As before, I will ask two sets of questions"\
+"\\pau=1000\\ Remember, \\eos=1\\ pay attention to how "\
+"I choose my questions, \\eos=1\\ because I will use "\
+"a different strategy now!", True, True)
self.show_question.emit("")
self.show_fixed.emit("")
time.sleep(5)
rospy.logwarn('Starting with {} out of {}'.format(\
learner, self.learners))
for id_attribute, attribute in \
enumerate(self.selected_attributes[id_learner,:]):
self.nao_talk("I will now ask you:\\eos=1\\{}".format(\
self.verbose_attributes_nao[id_learner][id_attribute]), False, True)
question = self.verbose_attributes[id_learner][id_attribute]
if self.gui.finnish_help:
question += '\n' + self.verbose_attributes_fi[id_learner][
id_attribute]
self.show_fixed.emit(question)
time.sleep(1)
self.nao_talk("We have {} seconds of time.\\eos=1\\ Let's start!"\
.format(self.time_bag), False, True)
time.sleep(3)
# initialize attribute-learner specific logging data
self.experiment_data['question_h', learner, attribute] = list()
self.experiment_data['answer_h', learner, attribute] = list()
self.experiment_data['response_h', learner, attribute] = list()
self.experiment_data['response_only_h', learner,
attribute] = list()
self.experiment_data['testset_p', learner, attribute] = list()
self.experiment_data['dunno_p', learner, attribute] = list()
self.experiment_data['correct_answers_p', learner, attribute] = 0
# re-initialize the learner
previous_index = None
self.ct.reset_learning()
shuffled_entities = copy(self.entities)
dunno_entities = list()
random.shuffle(shuffled_entities)
# vector of the learned attributes from user, ordered as AwA2
learned_user = -1 * np.ones([len(self.entities)])
# vector of the learned attributes from dataset, ordered as AwA2
learned_truth = -1 * np.ones([len(self.entities)])
# intialize time budget
time_budget = self.time_bag
### LEARNING PHASE ###
first_question = True
while time_budget > 0 and len(shuffled_entities) > 0:
# select question
if learner == 'greedy':
rospy.loginfo('greedy')
awa_index, tilde = self.ct.select_greedy_query\
(shuffled_entities)
elif learner == 'similar':
rospy.loginfo('similar')
awa_index, tilde = self.ct.select_closest_query\
(shuffled_entities, previous_index)
elif learner == 'hybrid':
rospy.loginfo('hybrid')
awa_index, tilde = self.ct.select_hybrid_query(\
shuffled_entities, previous_index, -1.6, 0,\
self.min_distance, self.max_distance, 0.7)
elif learner == 'random':
awa_index = self.ct.leaves.index(shuffled_entities[-1])
shuffled_entities.pop()
else:
rospy.logerr('ERROR: unknown learner')
exit(10)
# store the picked question (for similar and hybrid learners)
previous_index = awa_index
# log the picked question
self.experiment_data['question_h',learner,attribute].append(\
awa_index)
# Ask question
if first_question:
question = verbose_starting[0] +\
self.entities_awa[awa_index] + '?'
first_question = False
else:
r = np.argmax(np.random.multinomial(1,\
[0.2, 0.2, 0.6]))
question = verbose_starting[r] +\
self.entities_awa[awa_index] + '?'
# Start the timer
text_question = copy(question)
if self.gui.finnish_help:
text_question += ' (' + self.entities_fin[awa_index] + ')'
t0 = time.time()
self.nao_talk(question, False, True)
self.show_question.emit(text_question)
t1 = time.time()
# Wait for an answer
self.gui.keyboard_lock.lock()
try:
rospy.loginfo("Wait for keystroke")
self.gui.keyboard_condition.wait(self.gui.keyboard_lock)
answer = self.gui.last_key_pressed
rospy.loginfo("Received keystroke!")
finally:
self.gui.keyboard_lock.unlock()
# Stop the timer
t2 = time.time()
# Decode the answer
if answer == Qt.Key_Right:
answer = 1
elif answer == Qt.Key_Left:
answer = 0
elif answer == Qt.Key_Down:
answer = -1
dunno_entities.append(awa_index)
else:
answer = -1
rospy.logwarn("Received invalid answer")
# Flash the label of question with the answer
self.answer_feedback(answer)
# Store answer
learned_user[awa_index] = answer
self.experiment_data['response_h',learner,\
attribute].append(t2-t0)
self.experiment_data['response_only_h',learner,\
attribute].append(t2-t1)
self.experiment_data['answer_h',learner,attribute].append(\
learned_user[awa_index])
# Update the time budget
time_budget -= (t2 - t0)
learned_truth[awa_index] = self.binary_table[awa_index,\
attribute]
if learned_truth[awa_index] == learned_user[awa_index]:
self.experiment_data['correct_answers_p',learner,\
attribute] += 1
# Update the model with the answer
if answer != -1:
self.ct.node_dictionary[self.ct.leaves_to_wn[\
self.entities[awa_index]]].push_information(\
learned_user[awa_index] == 1)
### EVALUATION PHASE ###
# performance on the unseen entities
performance = -1 * np.ones([len(shuffled_entities)])
for i, entity in enumerate(shuffled_entities):
theta = self.ct.node_dictionary[\
self.ct.leaves_to_wn[entity]].theta
if theta > 1 - hard_performance_threshold or \
theta < hard_performance_threshold:
if theta > 1 - hard_performance_threshold:
performance[i] = 1 if \
self.binary_table[self.entities.index(entity), \
attribute] == 1 else 0
else:
performance[i] = 1 if \
self.binary_table[self.entities.index(entity), \
attribute] == 0 else 0
else:
performance[i] = 0
# performance on the unknown entities
dunno_performance = -1 * np.ones([len(dunno_entities)])
for i, entity in enumerate(dunno_entities):
theta = self.ct.node_dictionary[self.ct.leaves_to_wn\
[self.entities[entity]]].theta
if theta > 1 - hard_performance_threshold or \
theta < hard_performance_threshold:
if theta > 1 - hard_performance_threshold:
dunno_performance[i] = 1 if \
self.binary_table[entity, attribute] == 1 else 0
else:
dunno_performance[i] = 1 if \
self.binary_table[entity, attribute] == 0 else 0
else:
dunno_performance[i] = 0
# log the performance
self.experiment_data['testset_p',learner,attribute].append(\
np.sum(performance))
self.experiment_data['dunno_p',learner,attribute].append(\
np.sum(dunno_performance))
self.show_question.emit("")
self.show_fixed.emit("")
if id_attribute == 0:
self.nao_talk("Time is over!\\eos=1\\"+\
" We are done with this set of questions!"+\
"\\eos=1\\ Let's have a small break!", True, True)
self.show_question.emit("Small Break")
time.sleep(10)
else:
time.sleep(2)
self.nao_talk('This teaching session is over!\\eos=1\\Thank you'+\
' for your patience!', True ,True)
self.show_score.emit("")
self.show_score_outro.emit("")
self.show_tab.emit(2)
if id_learner == 0:
self.nao_talk("I will take a test now, to check if"+\
" I learned something.\\eos=1\\ "+\
"Let's see how well I learned with this technique"+\
"\\pau=1000\\Wish me luck!", True, True)
else:
self.nao_talk("I will take the test again.\\eos=1\\ "+\
"Let's see how well I learned with this different technique"+\
"\\pau=1000\\Wish me luck!", True, True)
# Robot "thinking" led animations
self.nao_led(1, 0, 5, True)
self.nao_talk("I am done with the test!", True, True)
time.sleep(1)
score = 0
total = 0
for a in self.selected_attributes[id_learner, :]:
score += self.experiment_data['testset_p', learner, a][0]
score += self.experiment_data['dunno_p', learner, a][0]
score += self.experiment_data[
'correct_answers_p', learner,
attribute] # is this suppose to be just 'a'?
total += number_of_questions
self.nao_talk(
"I replied correctly to {0:.1f} % of the questions!".format(
100 * score / total), True, True)
self.show_score.emit("Percentage \n {0:.1f}%".format(100 * score /
total))
time.sleep(3)
self.nao_talk('Please now fill the questionnaires. \\eos=1\\'+\
'When you have filled them, please continue by pressing ENTER',\
True, True)
self.show_score_outro.emit(
"Fill the questionnaire now. \n Then, press ENTER to continue")
## PARTIAL LOG
self._experiment_log(partial=True)
## Pause to avoid skipping questionnaire
time.sleep(10)
### ADVANCE THE STATE MACHINE
self.gui.phase_lock.lock()
self.gui.sm = 5
rospy.logwarn('SM: --> 5')
self.gui.phase_lock.unlock()
def answer_feedback(self, answer):
# the led feedback is disabled (because creepy)
# self.nao_led(2, answer, 0.3, False)
self.flash_question.emit(answer + 1)
self.flash_label.emit(answer + 1)
time.sleep(0.3)
self.flash_question.emit(-1)
self.flash_label.emit(-1)
class DiffDriveControlWidget(QWidget):
updateLinearGoalState = pyqtSignal(float)
updateAngularGoalState = pyqtSignal(float)
updateLinearCurrentState = pyqtSignal(str)
updateAngularCurrentState = pyqtSignal(str)
def __init__(self, parent, context):
super(DiffDriveControlWidget, self).__init__()
self._max_linear_vel = rospy.get_param('~max_linear_vel', 0.17)
self._max_angular_vel = rospy.get_param('~max_angular_vel', 2.0)
self.twist_msg = Twist()
# load from ui
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('hector_generic_rqt_widgets'),
'resource', 'diff_drive_control.ui')
loadUi(ui_file, self, {'QWidget': QWidget})
self.setObjectName('DiffDriveControlUi')
# init elements
self.linearGoalDoubleSpinBox.setMinimum(-self._max_linear_vel)
self.linearGoalDoubleSpinBox.setMaximum(self._max_linear_vel)
self.angularGoalDoubleSpinBox.setMinimum(-self._max_angular_vel)
self.angularGoalDoubleSpinBox.setMaximum(self._max_angular_vel)
# connect to signals
self.linearVerticalSlider.valueChanged[int].connect(
lambda value: self.linearGoalDoubleSpinBox.setValue(
float(self._max_linear_vel * value / 100.0)))
self.linearGoalDoubleSpinBox.valueChanged[float].connect(
lambda value: self.linearVerticalSlider.setValue(
int(value / self._max_linear_vel * 100.0)))
self.linearGoalDoubleSpinBox.valueChanged[float].connect(
lambda value: self._linear_cmd_update(value, 'ui'))
self.angularDial.valueChanged[int].connect(
lambda value: self.angularGoalDoubleSpinBox.setValue(
float(self._max_angular_vel * -value / 100.0)))
self.angularGoalDoubleSpinBox.valueChanged[float].connect(
lambda value: self.angularDial.setValue(
int(-value / self._max_angular_vel * 100.0)))
self.angularGoalDoubleSpinBox.valueChanged[float].connect(
lambda value: self._angular_cmd_update(value, 'ui'))
self.stopPushButton.clicked[bool].connect(
lambda: self.linearVerticalSlider.setValue(0))
self.stopPushButton.clicked[bool].connect(
lambda: self.angularDial.setValue(0))
# Qt signals
self.updateLinearGoalState.connect(
lambda cmd: self._linear_cmd_update(cmd, 'msg'))
self.updateAngularGoalState.connect(
lambda cmd: self._angular_cmd_update(cmd, 'msg'))
self.updateLinearCurrentState.connect(
self.linearCurrentLineEdit.setText)
self.updateAngularCurrentState.connect(
self.angularCurrentLineEdit.setText)
# init subscribers
self.odom_sub = rospy.Subscriber('odom', Odometry, self._odom_cb)
self.cmd_vel_sub = rospy.Subscriber('cmd_vel', Twist, self._cmd_vel_cb)
# init publisher
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
def shutdown_plugin(self):
rospy.loginfo('Shutting down ...')
self.odom_sub.unregister()
self.cmd_vel_sub.unregister()
rospy.loginfo('Done!')
def _linear_cmd_update(self, cmd, source):
if source == 'ui':
self.twist_msg.linear.x = cmd
self.cmd_vel_pub.publish(self.twist_msg)
if source == 'msg':
self.linearVerticalSlider.blockSignals(True)
self.linearVerticalSlider.setValue(
int(cmd / self._max_linear_vel * 100.0))
self.linearVerticalSlider.blockSignals(False)
self.linearGoalDoubleSpinBox.blockSignals(True)
self.linearGoalDoubleSpinBox.setValue(cmd)
self.linearGoalDoubleSpinBox.blockSignals(False)
def _angular_cmd_update(self, cmd, source):
if source == 'ui':
self.twist_msg.angular.z = cmd
self.cmd_vel_pub.publish(self.twist_msg)
if source == 'msg':
self.angularDial.blockSignals(True)
self.angularDial.setValue(int(-cmd / self._max_angular_vel *
100.0))
self.angularDial.blockSignals(False)
self.angularGoalDoubleSpinBox.blockSignals(True)
self.angularGoalDoubleSpinBox.setValue(cmd)
self.angularGoalDoubleSpinBox.blockSignals(False)
def _cmd_vel_cb(self, msg):
if msg._connection_header['callerid'] != rospy.get_name():
self.twist_msg = msg
self.updateLinearGoalState.emit(msg.linear.x)
self.updateAngularGoalState.emit(msg.angular.z)
def _odom_cb(self, odom):
self.updateLinearCurrentState.emit(
str('%.2f' % odom.twist.twist.linear.x))
self.updateAngularCurrentState.emit(
str('%.2f' % odom.twist.twist.angular.z))
class QuestionDialogPlugin(Plugin):
timeout_sig = pyqtSignal()
update_sig = pyqtSignal()
def __init__(self, context):
super(QuestionDialogPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('QuestionDialogPlugin')
font_size = rospy.get_param("~font_size", 40)
# Create QWidget
self._widget = QWidget()
self._widget.setFont(QFont("Times", font_size, QFont.Bold))
self._layout = QVBoxLayout(self._widget)
self._text_browser = QTextBrowser(self._widget)
self._layout.addWidget(self._text_browser)
self._button_layout = QGridLayout()
self._layout.addLayout(self._button_layout)
# layout = QVBoxLayout(self._widget)
# layout.addWidget(self.button)
self._widget.setObjectName('QuestionDialogPluginUI')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
context.add_widget(self._widget)
# Setup service provider
self.service = rospy.Service('question_dialog', QuestionDialog,
self.service_callback)
self.request = None
self.response_ready = False
self.response = None
self.buttons = []
self.text_label = None
self.text_input = None
self.update_sig.connect(self.update)
self.timeout_sig.connect(self.timeout)
def shutdown_plugin(self):
self.service.shutdown()
def service_callback(self, req):
self.response_ready = False
self.request = req
self.update_sig.emit()
# Start timer against wall clock here instead of the ros clock.
start_time = time.time()
while not self.response_ready:
if self.request != req:
# The request got preempted by a new request.
return QuestionDialogResponse(QuestionDialogRequest.PREEMPTED, "")
if req.timeout != QuestionDialogRequest.NO_TIMEOUT:
current_time = time.time()
if current_time - start_time > req.timeout:
self.timeout_sig.emit()
return QuestionDialogResponse(
QuestionDialogRequest.TIMED_OUT, "")
time.sleep(0.2)
return self.response
def update(self):
self.clean()
if not self.request:
rospy.logwarn('question dialog: no request, update ignored')
return
req = self.request
self._text_browser.setText(req.message)
if req.type == QuestionDialogRequest.DISPLAY:
# All done, nothing more too see here.
self.response = QuestionDialogResponse(
QuestionDialogRequest.NO_RESPONSE, "")
self.response_ready = True
elif req.type == QuestionDialogRequest.CHOICE_QUESTION:
for index, options in enumerate(req.options):
button = QPushButton(options, self._widget)
button.clicked.connect(partial(self.handle_button, index))
row = index / 3
col = index % 3
self._button_layout.addWidget(button, row, col)
self.buttons.append(button)
elif req.type == QuestionDialogRequest.TEXT_QUESTION:
self.text_label = QLabel("Enter here: ", self._widget)
self._button_layout.addWidget(self.text_label, 0, 0)
self.text_input = QLineEdit(self._widget)
self.text_input.editingFinished.connect(self.handle_text)
self._button_layout.addWidget(self.text_input, 0, 1)
def timeout(self):
self._text_browser.setText("Oh no! The request timed out.")
self.clean()
def clean(self):
while self._button_layout.count():
item = self._button_layout.takeAt(0)
item.widget().deleteLater()
self.buttons = []
self.text_input = None
self.text_label = None
def handle_button(self, index):
self.response = QuestionDialogResponse(index, "")
self.clean()
self.response_ready = True
def handle_text(self):
self.response = QuestionDialogResponse(
QuestionDialogRequest.TEXT_RESPONSE,
self.text_input.text())
self.clean()
self.response_ready = True
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
class RosEventTrigger(QObject):
"""
Trigger events based on event name. Create event name list
based on event file
"""
## Send quad sensor/state data as string
status_signal = pyqtSignal(str, name='systemStatus')
## Send pose command received from Rviz
pose_command_signal = pyqtSignal(PoseStamped, name='poseCommand')
def __init__(self, event_file_path):
"""
Class that loads an event file from a parameter
It provides method to triggerEvents using ros publisher
This class separates the GUI part of plugin from ros interface
"""
super(RosEventTrigger, self).__init__()
# Create ros node
if not event_file_path:
# Get default event file name
rospack = rospkg.RosPack()
aerial_autonomy_path = rospack.get_path('aerial_autonomy')
default_path = os.path.join(aerial_autonomy_path,
'events/uav_basic_events')
if rospy.has_param("event_file"):
print "Found event file: ", rospy.get_param('event_file')
event_file_path = rospy.get_param('event_file', default_path)
print "Event file name: ", event_file_path
event_file = file(event_file_path, 'r')
# Parse event file to save event list and event manager name
event_line_list = [event_name.strip()
for event_name in event_file.read().splitlines()]
event_file.close()
## Define event manager name
self.event_manager_name = event_line_list[0][:-1]
## Event names used to create buttons
self.event_names_list = []
event_folder, event_file_name = event_file_path.rsplit('/', 1)
parse_event_file(event_folder, event_file_name, self.event_names_list)
## Ros publisher to trigger events based on name
self.event_pub = rospy.Publisher('~event_trigger',
String, queue_size=1)
## Ros publisher to trigger pose event
self.pose_command_pub = rospy.Publisher('~pose_command_combined',
PoseStamped, queue_size=1)
## Ros publisher to trigger pose event
self.vel_command_pub = rospy.Publisher('~velocity_yaw_command',
VelocityYaw, queue_size=1)
# Define partial callbacks
statusCallback = partial(
self.statusCallback, signal=self.status_signal)
# Subscribers for quad arm and state machine updates
rospy.Subscriber("~system_status", String, statusCallback)
# Subscribe to position commands (from Rviz)
rospy.Subscriber("/move_base_simple/goal",
PoseStamped, self.pose_command_signal.emit)
def statusCallback(self, msg, signal):
"""
Ros callback for status data from quad, arm, state machine
"""
signal.emit(str(msg.data))
def triggerPoseCommand(self, pose):
"""
Publish pose command event for state machine
"""
if not rospy.is_shutdown():
self.pose_command_pub.publish(pose)
def triggerVelocityCommand(self, velocity_yaw):
"""
Publish pose command event for state machine
"""
if not rospy.is_shutdown():
self.vel_command_pub.publish(velocity_yaw)
def triggerEvent(self, event_name):
"""
Function to publish ros event based on event name
"""
if not rospy.is_shutdown():
msg = String()
msg.data = event_name
self.event_pub.publish(msg)
class HelpingHandGUI(Plugin):
# Various member variables
# _topic_subscriber = None
# _topic_good = False
# _digitalinput_subscriber = None
# _namespace = None
_available_ns = []
_conf_yaml = None
_trigger_sources = []
_data_buffer = {}
_refresh_data_table_contents_sig = pyqtSignal()
# _pattern = re.compile(r'\/[^/]+\/')
# _digitalinput = None
# _gravcomp_service = None
# _dmp_action_server_client = None
# _topic_name = None
# _num_weights = 0
# _recorded_dmp = None
# _msg_store = None
# _database_service = None
# _dmp_save_name = None
# _pyaudio = None
# _stream = None
# _beep = None
# _joint_space = None
# _cartesian_space = None
# _available_frames = []
# DMP trajectory buffer
_joint_traj = []
# DMP encoding service
_dmp_ecoder = None
# Logging
_init_status_report = {
'status': 'idle',
'last_entry': 'None',
'last_type': 'None',
}
# Logics
_old_gravbutton = 0
_old_rec_button = 0
_recording = False
_gravComp = False
# Some signal
_dmp_recording_started_signal = pyqtSignal()
_dmp_recording_stopped_signal = pyqtSignal()
# TF2
_tf2_buffer = tf2_ros.Buffer()
_tf2_listener = tf2_ros.TransformListener(_tf2_buffer)
_tf_listener = tf.TransformListener()
# Available topics
_available_topics_array = []
_available_topics_dict = dict()
_old_index = 0
def __init__(self, context):
super(HelpingHandGUI, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('HelpingHandGUI')
# 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:
# rospy.logdebug 'arguments: ', args
# rospy.logdebug 'unknowns: ', unknowns
# Create QWidget
self._widget = QWidget()
# Get path to UI file which should be in the "resource" folder of this package
ui_file = os.path.join(rospkg.RosPack().get_path('helping_hand_gui'), 'qt', 'helpinghand.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
# self._widget.setObjectName('DMPRecordToolGUIUi')
# Show _widget.windowTitle on left-top of each plugin (when
# it's set in _widget). This is useful when you open multiple
# plugins at once. Also if you open multiple instances of your
# plugin at once, these lines add number to make it easy to
# tell from pane to pane.
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
# Populate configuration table's headers
self._conf_table = self._widget.findChild(QTableWidget, 'configTable')
self._conf_table.setHorizontalHeaderLabels((
'Trigger Name',
'Robot Namespace',
'Trigger Topic',
'Trigger Type',
'Trigger Value',
'Trigger Callback'))
# Populate data table's headers
self._data_table = self._widget.findChild(QTableWidget, 'data_table')
self._data_table.setHorizontalHeaderLabels((
'Timestamp',
'Trigger Conf',
'Trigger Reason',
'Data Type'))
header = self._data_table.horizontalHeader()
header.setSectionResizeMode(QHeaderView.ResizeToContents)
# Hide the 5th column that contains data identifiers
self._data_table.setColumnCount(self._data_table.columnCount()+1)
self._data_table.setColumnHidden(self._data_table.columnCount()-1, True)
# Connect the refresh signal with the update function
self._refresh_data_table_contents_sig.connect(self._update_data_table)
# Connect the item selected signal to the display details
self._data_table.itemSelectionChanged.connect(self._display_item_details)
# Parse config
self._parse_yaml_config()
# Show conf in table
self._write_conf_to_table()
# Display what we are listening to
self._display_addresses()
# Print initial status report
self.status_report = StatusReport(self._init_status_report, self)
self._print_status()
# DMP Encoding service
self._dmp_ecoder = rospy.ServiceProxy('/encode_traj_to_dmp', EncodeTrajectoryToDMP)
# Save button widget
save_button = self._widget.findChild(QPushButton, 'save_button')
save_button.clicked.connect(partial(self._save_button_pressed, save_button))
# Create database proxy
self._msg_store = MessageStoreProxy()
# Create the service proxies for into saving databse
self._tf_capture_srv = rospy.ServiceProxy('tf_capture', CaptureTF)
self._joint_capture_srv = rospy.ServiceProxy('joint_capture', CaptureJoint)
self._dmp_capture_srv = rospy.ServiceProxy('dmp_capture', CaptureDMP)
# Make sure the services are running !!
try:
self._tf_capture_srv.wait_for_service(0.5)
self._joint_capture_srv.wait_for_service(0.5)
self._dmp_capture_srv.wait_for_service(0.5)
except Exception as e:
rospy.logerr("Could not establish a connection to services. See exception:\n{}".format(e))
exit()
def _print_status(self):
status_text_widget = self._widget.findChild(QTextEdit, 'statusWindow')
status_text = \
"Status: {0}\n"\
"Last saved entry: {1}\n"\
"Last entry type: {2}".format(
self.status_report['status'],
self.status_report['last_entry'],
self.status_report['last_type']
)
status_text_widget.setText(status_text)
# YAML config parser
def _parse_yaml_config(self):
conf_file = os.path.join(rospkg.RosPack().get_path('helping_hand_gui'), 'conf', 'triggers_conf.yml')
self._conf_yaml = yaml.load(open(conf_file), Loader=yaml.FullLoader)
tmp_ns = []
for conf_key, conf_val in self._conf_yaml.items():
tmp_ns.append(conf_val['robot_ns'])
if conf_val['trig_type'] == 'rising_edge':
conf_val['subscriber'] = rospy.Subscriber(conf_val['trig_topic'], Bool, callback=self._rising_edge_cb, callback_args=(conf_key))
elif conf_val['trig_type'] == 'falling_edge':
conf_val['subscriber'] = rospy.Subscriber(conf_val['trig_topic'], Bool, callback=self._falling_edge_cb, callback_args=(conf_key))
elif conf_val['trig_type'] == 'hold':
conf_val['subscriber'] = rospy.Subscriber(conf_val['trig_topic'], Bool, callback=self._hold_cb, callback_args=(conf_key))
else:
self.status_report['status'] = "Error parsing trigger types."
if conf_val['trig_callback'] == 'joint_save':
joint_topic_addr = conf_val['robot_ns'] + conf_val['joint_topic']
conf_val['subscriber'] = rospy.Subscriber(joint_topic_addr, JointState, callback=self._joint_state_cb, callback_args=(conf_key))
if conf_val['trig_callback'] == 'joint_pose_save':
joint_topic_addr = conf_val['robot_ns'] + conf_val['joint_topic']
conf_val['subscriber'] = rospy.Subscriber(joint_topic_addr, JointState, callback=self._joint_state_cb, callback_args=(conf_key))
if conf_val['trig_callback'] == 'joint_dmp_save':
joint_topic_addr = conf_val['robot_ns'] + conf_val['joint_topic']
conf_val['subscriber'] = rospy.Subscriber(joint_topic_addr, JointState, callback=self._joint_state_cb, callback_args=(conf_key))
# Add TF
# Remove duplicate namespaces
self._available_ns = list(dict.fromkeys(tmp_ns))
def _write_conf_to_table(self):
self._conf_table.setRowCount(len(self._conf_yaml))
for row, conf_key in enumerate(self._conf_yaml.items()):
conf = conf_key[1]
self._conf_table.setItem(row, 0, QTableWidgetItem(conf['trig_name']))
self._conf_table.setItem(row, 1, QTableWidgetItem(conf['robot_ns']))
self._conf_table.setItem(row, 2, QTableWidgetItem(conf['trig_topic']))
self._conf_table.setItem(row, 3, QTableWidgetItem(conf['trig_type']))
self._conf_table.setItem(row, 4, QTableWidgetItem(str(conf['trig_value'])))
self._conf_table.setItem(row, 5, QTableWidgetItem(conf['trig_callback']))
def _update_data_table(self):
self._data_table.setRowCount(0)
for data_key, data_dict in self._data_buffer.items():
current_row = self._data_table.rowCount()
self._data_table.insertRow(current_row)
self._data_table.setItem(current_row, 0, QTableWidgetItem(data_dict['time']))
self._data_table.setItem(current_row, 1, QTableWidgetItem(data_dict['trig_name']))
self._data_table.setItem(current_row, 2, QTableWidgetItem(data_dict['trig_type']))
self._data_table.setItem(current_row, 3, QTableWidgetItem(data_dict['data']._type))
self._data_table.setItem(current_row, 4, QTableWidgetItem(data_key))
self._data_table.sortItems(0, Qt.DescendingOrder)
def _display_item_details(self):
text_box = self._widget.findChild(QTextEdit, 'entry_details')
try:
curent_item = self._data_table.currentItem()
current_row = curent_item.row()
entry_identifier = self._data_table.item(current_row, 4).text()
self.status_report['status'] = 'Item selected'
data_details = self._data_buffer[entry_identifier]['data'],
text_box.setText(format(data_details))
except Exception as e:
pass
def _display_addresses(self):
self.discover_available_topics_array()
addr_list_widget = self._widget.findChild(QListWidget, 'addr_list')
for topic in self._available_topics_array:
addr_list_widget.addItem(QListWidgetItem(topic))
# topic = '/ur10_2/joint_states'
# self._available_ns
# Get a list of topics that match a certain type
def discover_available_topics_array(self):
all_topics = rospy.get_published_topics()
for topic, msg_type in all_topics:
if msg_type in DESIRED_MSGS:
self._available_topics_array.append(topic)
self._available_topics_dict[topic] = msg_type
self._available_topics_array.sort()
# The following line removes possible duplicate entries from the array
self._available_topics_array = list(set(self._available_topics_array))
# Discover available robots
def discover_frames(self):
# Define a search pattern, it will look for all namespaces that contain the 'received_control_mode' topic
search_pattern = re.compile(r'^(?!.*link).*')
# search_pattern = re.compile(r'^((?!.*link)|(.*base)).*$')
# Get the list of all currently availabel frames in TF1 (this is a temporary solution until a better one is found)
time.sleep(1) # Give tf time to start working
# all_frames = self._tf_listener.getFrameStrings()
yaml_frames = yaml.load(self._tf2_buffer.all_frames_as_yaml())
# Look through all avaiable topics
for entry in yaml_frames:
namespace = search_pattern.findall(entry)
if len(namespace):
self._available_frames.append(namespace[0])
# Sort the namespaces alphabetically
self._available_frames.sort()
# rospy.logdebug out the result of the search
# rospy.loginfo("Found robots with namespace: \n%s", self._available_frames)
# Add robots namespaces to combobox
self._combobox = self._widget.findChildren(QComboBox, QRegExp('frameSelector'))[0]
self._combobox.insertItems(0, self._available_frames)
self._namespace = self._combobox.currentText()
# Quit if no robots are found
if len(self._available_frames) == 0:
rospy.logerr("[%s]: No robots available !!", rospy.get_name())
sys.exit()
def _save_button_pressed(self, save_button):
save_name_widget = self._widget.findChild(QLineEdit, 'save_name')
save_name = save_name_widget.text()
# Check if a name was entered
if not(len(save_name)):
self.status_report['status'] = 'No name provided !!'
return -1
# Get selected item
curent_item = self._data_table.currentItem()
if curent_item is None:
self.status_report['status'] = 'No item selected !!'
return -1
# Get selected item row
current_row = curent_item.row()
if current_row == -1:
self.status_report['status'] = 'No item selected !!'
return -1
# Get the item identifier and retrieve the data
entry_identifier = self._data_table.item(current_row, 4).text()
data = self._data_buffer[entry_identifier]['data']
# Update the status box
self.status_report['status'] = 'Save button pressed'
rospy.logdebug("Save button pressed !!")
# Handle transform stamped
if data._type == 'geometry_msgs/TransformStamped':
trig_name = self._data_buffer[entry_identifier]['trig_name']
for keys, vals in self._data_buffer.items():
if (vals['trig_name'] == trig_name) and (vals['data']._type == 'sensor_msgs/JointState'):
pass
# Handle Joint space configuration
elif data._type == 'sensor_msgs/JointState':
self._joint_capture_srv.call(data, save_name)
# Handle Joint space configuration
elif data._type == 'robot_module_msgs/JointSpaceDMP':
self._dmp_capture_srv.call(data, CartesianSpaceDMP(), save_name)
# Handle Joint space DMP
# # Saving to database according to the text in the saveName field
# if self._joint_space:
# existingDMP = self._msg_store.query_named(saveName, JointSpaceDMP._type)[0]
# if existingDMP == None:
# rospy.loginfo("Saving new DMP with ID = " + self._dmp_save_name + " to database!")
# self._msg_store.insert_named(saveName, self._recorded_dmp)
# else:
# rospy.loginfo("Updating DMP with ID = " + self._dmp_save_name + " in database!")
# self._msg_store.update_named(saveName, self._recorded_dmp)
# else:
# existingDMP = self._msg_store.query_named(saveName, CartesianSpaceDMP._type)[0]
# if existingDMP == None:
# rospy.loginfo("Saving new DMP with ID = " + self._dmp_save_name + " to database!")
# self._msg_store.insert_named(saveName, self._recorded_dmp)
# else:
# rospy.loginfo("Updating DMP with ID = " + self._dmp_save_name + " in database!")
# self._msg_store.update_named(saveName, self._recorded_dmp)
# Write to report
self.status_report['status'] = 'Data saved'
self.status_report['last_entry'] = save_name
self.status_report['last_type'] = data._type
# Update text fields
save_name_widget.setText('')
details_text_box = self._widget.findChild(QTextEdit, 'entry_details')
details_text_box.setText('')
# Clean up table
self._data_table.clearSelection()
self._data_table.removeRow(current_row)
# # Remove data from buffer
# self._data_buffer.pop(entry_identifier)
def shutdown_plugin(self):
# TODO unregister all publishers here
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
# Topic subscribers
def _joint_state_cb(self, data, conf_name):
self._conf_yaml[conf_name]['joint_data'] = data
# def save_joint_conf_sig_h(self):
# rospy.loginfo('Saving joint conf !!')
def _rising_edge_cb(self, data, conf_name):
if not(self._conf_yaml[conf_name].has_key('sig_val')):
self._conf_yaml[conf_name]['sig_val'] = data.data
old_val = self._conf_yaml[conf_name]['sig_val']
new_val = data.data
if old_val - new_val == -1:
rospy.logdebug('Rising edge!!')
self._handle_trigger(self._conf_yaml[conf_name])
self._conf_yaml[conf_name]['sig_val'] = new_val
def _falling_edge_cb(self, data, conf_name):
if not(self._conf_yaml[conf_name].has_key('sig_val')):
self._conf_yaml[conf_name]['sig_val'] = data.data
old_val = self._conf_yaml[conf_name]['sig_val']
new_val = data.data
if old_val - new_val == 1:
rospy.logdebug('Falling edge!!')
self._handle_trigger(self._conf_yaml[conf_name])
self._conf_yaml[conf_name]['sig_val'] = new_val
def _hold_cb(self, data, conf_name):
if not('sig_val' in self._conf_yaml[conf_name]):
self._conf_yaml[conf_name]['sig_val'] = data.data
old_val = self._conf_yaml[conf_name]['sig_val']
new_val = data.data
if falling_edge(old_val, new_val):
rospy.logdebug("[_hold_cb] Falling edge")
if len(self._joint_traj):
self._handle_trigger(self._conf_yaml[conf_name])
if rising_edge(old_val, new_val):
pass
rospy.logdebug("[_hold_cb] Rising edge")
if old_val + new_val == 2:
rospy.logdebug("[_hold_cb] Holding")
trig_type = self._conf_yaml[conf_name]['trig_callback']
if trig_type == "joint_dmp_save":
rospy.logdebug("Appending !!")
self._joint_traj.append(self._conf_yaml[conf_name]['joint_data'])
self._conf_yaml[conf_name]['sig_val'] = new_val
def _handle_trigger(self, trigg_conf):
trigg_conf['robot_ns']
trig_type = trigg_conf['trig_callback']
save_data = []
if trig_type == "joint_save":
rospy.logdebug('joint_save')
save_data = [trigg_conf['joint_data']]
elif trig_type == "joint_pose_save":
joint_data = trigg_conf['joint_data']
rospy.logdebug('joint_pose_save')
save_data = [joint_data, TransformStamped()] # , tf_data]
elif trig_type == "joint_dmp_save":
rospy.logdebug('joint_dmp_save')
traj_dur = traj_duration_sec(self._joint_traj)
if traj_dur < 1:
rospy.logwarn("Trajectory too short to store")
else:
dmp_request = EncodeTrajectoryToDMPRequest()
dmp_request.demonstratedTrajectory = self._joint_traj
dmp_request.dmpParameters.N = trigg_conf['joint_dmp_N']
result_dmp = self._dmp_ecoder.call(dmp_request).encodedDMP
save_data = [result_dmp]
self._joint_traj = []
else:
rospy.logerr("Unknown saving type !!")
rospy.logerr("trig_type = {}".format(trig_type))
# self.status_report['status'] = "Error. See terminal"
for data in save_data:
now = datetime.now()
entry_identifier = now.strftime("%H%M%S%f")
current_time = now.strftime("%H:%M:%S")
self._data_buffer[entry_identifier] = {
'data': data,
'time': current_time,
'trig_name': trigg_conf['trig_name'],
'trig_type': trigg_conf['trig_type']
}
self._refresh_data_table_contents_sig.emit()
class MyXObject(QObject):
""" Sub-classing to define custom signals """
custom_signal = pyqtSignal(str)
class ParamWidget(QWidget):
"""
main class inherits from the ui window class.
You can specify the topics that the topic pane.
ParamWidget.start must be called in order to update topic pane.
"""
SELECT_BY_NAME = 0
SELECT_BY_MSGTYPE = 1
# _column_names = ['topic', 'type', 'bandwidth', 'rate', 'value', 'checkbox']
_column_names = ['topic', 'type', 'min', 'value', 'max', 'checkbox']
selectionChanged = pyqtSignal(dict, name='selectionChanged')
def keyPressEvent(self, event):
# super(QWidget, self).keyPressEvent(event)
event.ignore()
print "key pressed"
return
current = self.topics_tree_widget.currentItem()
column = self.topics_tree_widget.currentColumn()
if event.key() == Qt.Key_Right:
self.topics_tree_widget.setCurrentItem(current, column+1)
print "right is pressed"
event.ignore()
return
if event.key() == Qt.Key_Left:
self.topics_tree_widget.setCurrentItem(current, column-1)
print "left is pressed"
return
# event.accept()
def keyReleaseEvent(self, event):
print "key released"
event.ignore()
return
current = self.topics_tree_widget.currentItem()
column = self.topics_tree_widget.currentColumn()
# print "current is ", current, " at ", column
# self.topics_tree_widget.editItem(current, column)
print event.key()
event.accept()
# if event.key() == Qt.Key_Escape:
# self.close()
def __init__(self, plugin=None, selected_topics=None, select_topic_type=SELECT_BY_NAME):
"""
@type selected_topics: list of tuples.
@param selected_topics: [($NAME_TOPIC$, $TYPE_TOPIC$), ...]
@type select_topic_type: int
@param select_topic_type: Can specify either the name of topics or by
the type of topic, to filter the topics to
show. If 'select_topic_type' argument is
None, this arg shouldn't be meaningful.
"""
super(ParamWidget, self).__init__()
self._select_topic_type = select_topic_type
self.setFocusPolicy(Qt.StrongFocus)
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('rqt_topic'), 'resource', 'ParamWidget.ui')
loadUi(ui_file, self)
self._plugin = plugin
self.topics_tree_widget.sortByColumn(0, Qt.AscendingOrder)
header = self.topics_tree_widget.header()
try:
setSectionResizeMode = header.setSectionResizeMode # Qt5
except AttributeError:
setSectionResizeMode = header.setResizeMode # Qt4
setSectionResizeMode(QHeaderView.ResizeToContents)
header.customContextMenuRequested.connect(self.handle_header_view_customContextMenuRequested)
header.setContextMenuPolicy(Qt.CustomContextMenu)
# for i in range(len(self._column_names)):
# setSectionResizeMode(i, QHeaderView.Stretch)
header.setStretchLastSection(False)
setSectionResizeMode(0, QHeaderView.Stretch)
setSectionResizeMode(self._column_names.index("value"), QHeaderView.Stretch)
setSectionResizeMode(self._column_names.index("checkbox"), QHeaderView.Fixed)
# Whether to get all topics or only the topics that are set in advance.
# Can be also set by the setter method "set_selected_topics".
self._selected_topics = selected_topics
self._selected_items = []
self._current_topic_list = []
self._topics = {}
self._tree_items = {}
self._column_index = {}
for column_name in self._column_names:
self._column_index[column_name] = len(self._column_index)
self.topics_tree_widget.itemExpanded.connect(self.expanded)
self.topics_tree_widget.itemCollapsed.connect(self.collapsed)
self.topics_tree_widget.itemChanged.connect(self.itemChanged)
self.topics_tree_widget.setAlternatingRowColors(True)
delegate = EditorDelegate()
self.topics_tree_widget.setItemDelegate(delegate)
# self.topics_tree_widget.setItemDelegateForColumn(1, delegate)
# print(self.topics_tree_widget.itemDelegate())
# print(self.topics_tree_widget.itemDelegate(3))
# print(self.topics_tree_widget.itemDelegateForColumn(1))
self.topics_tree_widget.setEditTriggers(QAbstractItemView.AnyKeyPressed | QAbstractItemView.DoubleClicked )
@Slot(QTreeWidgetItem, int)
def currentChanged(current, column):
# column = self.topics_tree_widget.currentColumn()
print "current is ", current, " at ", column
# print "previous is ", previous,
# self.topics_tree_widget.editItem(current, column)
# self.topics_tree_widget.currentItemChanged.connect(currentChanged)
# self.topics_tree_widget.itemActivated.connect(currentChanged)
hitem = self.topics_tree_widget.headerItem()
hitem.setTextAlignment(self._column_names.index("min"), Qt.AlignRight | Qt.AlignVCenter)
hitem.setTextAlignment(self._column_names.index("max"), Qt.AlignLeft | Qt.AlignVCenter)
hitem.setTextAlignment(self._column_names.index("value"), Qt.AlignHCenter | Qt.AlignVCenter)
hitem.setTextAlignment(self._column_names.index("type"), Qt.AlignHCenter | Qt.AlignVCenter)
# self.refresh_topics()
# init and start update timer
self._timer_refresh_topics = QTimer(self)
self._timer_refresh_topics.timeout.connect(self.refresh_topics)
# @Slot()
# def test():
# print "@@@@@@@@@@@@@@@@@@@@@@@@@@2 data changed @@@@@@@@@@@@@@@@@@@@@@2222@@@@@@@"
# self.topics_tree_widget.model().dataChanged.connect(test)
# print dir(self.selectionChanged)
@Slot(dict)
def selection_changed(data):
print "#\n#\n#\nthe selcted items are:", data, "\n\n"
self.selectionChanged.connect(selection_changed)
def set_topic_specifier(self, specifier):
self._select_topic_type = specifier
def start(self):
"""
This method needs to be called to start updating topic pane.
"""
self._timer_refresh_topics.start(1000)
@Slot('QTreeWidgetItem', int)
def itemChanged(self, item, column):
# print "<<<<<<<<<<<<<<<<<<<<<<<<<< item changed >>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>"
selected = self.get_selected()
if item._topic_name in selected and self._column_names[column] in ["min", "max", "value"]:
self.selectionChanged.emit(selected)
@Slot('QTreeWidgetItem')
def expanded(self, item):
name = item.data(0, Qt.UserRole)
if not isinstance(item, TreeWidgetItem) or not item._is_topic:
return
print "expanded", name
self._topics[item._topic_name]['info'].start_monitoring()
@Slot('QTreeWidgetItem')
def collapsed(self, item):
name = item.data(0, Qt.UserRole)
if not isinstance(item, TreeWidgetItem) or not item._is_topic:
return
print "collapsed", name
self._topics[item._topic_name]['info'].stop_monitoring()
_items_param = {}
def get_desc(self, item):
desc = {}
if item._topic_name in self._current_params:
desc = self._current_params[item._topic_name]
vmin = item.data(self._column_names.index("min"), Qt.EditRole)
vmax = item.data(self._column_names.index("max"), Qt.EditRole)
val = item.data(self._column_names.index("value"), Qt.EditRole)
def tryFloat(s):
try:
return float(s)
except:
return None
vmin = tryFloat(vmin)
vmax = tryFloat(vmax)
val = tryFloat(val)
desc["min"] = vmin
desc["max"] = vmax
desc["default"] = val
return desc
def get_selected(self):
# param_name:desc
return {param:self.get_desc(self._tree_items[param]) for param in self._selected_items if param in self._current_params}
pass
def insert_param(self, param_name, param_desc, parent = None):
if parent == None:
parent = self.topics_tree_widget
pnames = param_name.split("/")
print pnames
ns = ""
item = parent
for name in pnames:
if name == "":
continue
_name = "/" + name
ns = ns + _name
if ns not in self._tree_items:
# print ">>>> ", ns
_item = TreeWidgetItem(self._toggle_monitoring, ns, item)
_item.setText(self._column_index['topic'], _name)
_item.setData(0, Qt.UserRole, "name_space")
_item.setFlags(Qt.ItemIsEnabled | Qt.ItemIsUserCheckable | Qt.ItemIsTristate)
self._tree_items[ns] = _item
# _item.setText(self._column_index['type'], type_name)
# _item.setData(0, Qt.UserRole, name_space)
item = self._tree_items[ns]
item.setFlags(Qt.ItemIsEditable | Qt.ItemIsEnabled | Qt.ItemIsSelectable | Qt.ItemIsUserCheckable )
item.setData(self._column_index['min'], Qt.EditRole, str(param_desc["min"]))
item.setData(self._column_index['max'], Qt.EditRole, str(param_desc["max"]))
item.setData(self._column_index['value'], Qt.EditRole, str(param_desc["default"]))
item.setData(self._column_index['type'], Qt.EditRole, str(param_desc["type"]))
self._items_param[item] = param_name
print param_name, " added"
pass
_current_params = {}
# TODO: implement the delete mechanism
def delete_param(self, param_name, parent = None):
pass
@Slot()
def refresh_topics(self):
"""
refresh tree view items
"""
# print "is refreshing"
nodes = dynamic_reconfigure.find_reconfigure_services()
# dparams = []
dparams = {}
for node in nodes:
client = dynamic_reconfigure.client.Client(node, timeout=3)
gdesc = client.get_group_descriptions()
for pdesc in gdesc["parameters"]:
param = node + "/" + pdesc["name"]
dparams[param] = pdesc
# dparams = dparams.items()
for param, desc in self._current_params.items():
if param not in dparams:
del self._current_params[param]
# TODO: delete the tree widget item
for param, desc in dparams.items():
if param not in self._current_params:
self._current_params[param] = desc
self.insert_param(param, desc)
# print self._current_params
# print self._tree_items
# dparams = rospy.get_published_topics()
# print self._items_param
# selected_dict = { self._selected_items[index] : index for index in range(len(self._selected_items)) }
# print selected_dict
# print self._selected_items
# print self.get_selected()
return
try:
if self._selected_topics is None:
topic_list = dparams
if topic_list is None:
rospy.logerr('Not even a single published topic found. Check network configuration')
return
except IOError as e:
rospy.logerr("Communication with rosmaster failed: {0}".format(e.strerror))
return
if self._current_topic_list != topic_list:
self._current_topic_list = topic_list
print "is updating"
# start new topic dict
new_topics = {}
for topic_name, topic_type in topic_list:
# if topic is new or has changed its type
if topic_name not in self._topics or \
self._topics[topic_name]['type'] != topic_type:
# create new TopicInfo
topic_info = TopicInfo(topic_name, topic_type)
message_instance = None
if topic_info.message_class is not None:
message_instance = topic_info.message_class()
# add it to the dict and tree view
topic_item = self._recursive_create_widget_items(self.topics_tree_widget, topic_name, topic_type, message_instance)
new_topics[topic_name] = {
'item': topic_item,
'info': topic_info,
'type': topic_type,
}
else:
# if topic has been seen before, copy it to new dict and
# remove it from the old one
new_topics[topic_name] = self._topics[topic_name]
del self._topics[topic_name]
# clean up old topics
for topic_name in self._topics.keys():
self._topics[topic_name]['info'].stop_monitoring()
index = self.topics_tree_widget.indexOfTopLevelItem(
self._topics[topic_name]['item'])
self.topics_tree_widget.takeTopLevelItem(index)
del self._topics[topic_name]
# switch to new topic dict
self._topics = new_topics
# self._update_topics_data()
def _update_topics_data(self):
selected_dict = { self._selected_items[index] : index for index in range(len(self._selected_items)) }
print selected_dict
for topic in self._topics.values():
topic_info = topic['info']
if topic_info.monitoring:
# update rate
rate, _, _, _ = topic_info.get_hz()
rate_text = '%1.2f' % rate if rate != None else 'unknown'
# update bandwidth
bytes_per_s, _, _, _ = topic_info.get_bw()
if bytes_per_s is None:
bandwidth_text = 'unknown'
elif bytes_per_s < 1000:
bandwidth_text = '%.2fB/s' % bytes_per_s
elif bytes_per_s < 1000000:
bandwidth_text = '%.2fKB/s' % (bytes_per_s / 1000.)
else:
bandwidth_text = '%.2fMB/s' % (bytes_per_s / 1000000.)
# update values
value_text = ''
self.update_value(topic_info._topic_name, topic_info.last_message)
else:
rate_text = ''
bandwidth_text = ''
value_text = 'not monitored' if topic_info.error is None else topic_info.error
# self._tree_items[topic_info._topic_name].setText(self._column_index['rate'], rate_text)
# self._tree_items[topic_info._topic_name].setText(self._column_index['bandwidth'], bandwidth_text)
self._tree_items[topic_info._topic_name].setText(self._column_index['value'], value_text)
def update_value(self, topic_name, message):
if hasattr(message, '__slots__') and hasattr(message, '_slot_types'):
for slot_name in message.__slots__:
self.update_value(topic_name + '/' + slot_name, getattr(message, slot_name))
elif type(message) in (list, tuple) and (len(message) > 0) and hasattr(message[0], '__slots__'):
for index, slot in enumerate(message):
if topic_name + '[%d]' % index in self._tree_items:
self.update_value(topic_name + '[%d]' % index, slot)
else:
base_type_str, _ = self._extract_array_info(self._tree_items[topic_name].text(self._column_index['type']))
self._recursive_create_widget_items(self._tree_items[topic_name], topic_name + '[%d]' % index, base_type_str, slot)
# remove obsolete children
if len(message) < self._tree_items[topic_name].childCount():
for i in range(len(message), self._tree_items[topic_name].childCount()):
item_topic_name = topic_name + '[%d]' % i
self._recursive_delete_widget_items(self._tree_items[item_topic_name])
else:
if topic_name in self._tree_items:
self._tree_items[topic_name].setText(self._column_index['value'], repr(message))
def _extract_array_info(self, type_str):
array_size = None
if '[' in type_str and type_str[-1] == ']':
type_str, array_size_str = type_str.split('[', 1)
array_size_str = array_size_str[:-1]
if len(array_size_str) > 0:
array_size = int(array_size_str)
else:
array_size = 0
return type_str, array_size
def _recursive_create_widget_items(self, parent, topic_name, type_name, message):
if parent is self.topics_tree_widget:
# show full topic name with preceding namespace on toplevel item
topic_text = topic_name
# item = TreeWidgetItem(self._toggle_monitoring, topic_name, parent)
_item = parent
topic_names = topic_name.split('/')
name_space = ""
for name in topic_names:
if name == "":
continue
_name = "/" + name
name_space = name_space + _name
if name_space not in self._tree_items:
print ">>>"+name_space
is_topic = False
if name_space == topic_name:
is_topic = True
_item = TreeWidgetItem(self._toggle_monitoring, name_space, _item, is_topic = is_topic)
_item.setText(self._column_index['topic'], _name)
_item.setText(self._column_index['type'], type_name)
_item.setData(0, Qt.UserRole, name_space)
self._tree_items[name_space] = _item
_item = self._tree_items[name_space]
item = _item
else:
topic_text = topic_name.split('/')[-1]
if '[' in topic_text:
topic_text = topic_text[topic_text.index('['):]
item = TreeWidgetItem(self._toggle_monitoring, topic_name, parent)
# item = QTreeWidgetItem(parent)
# item.setCheckState(0, Qt.Unchecked)
item.setText(self._column_index['topic'], topic_text)
item.setText(self._column_index['type'], type_name)
item.setData(0, Qt.UserRole, topic_name)
self._tree_items[topic_name] = item
if hasattr(message, '__slots__') and hasattr(message, '_slot_types'):
for slot_name, type_name in zip(message.__slots__, message._slot_types):
self._recursive_create_widget_items(item, topic_name + '/' + slot_name, type_name, getattr(message, slot_name))
else:
base_type_str, array_size = self._extract_array_info(type_name)
try:
base_instance = roslib.message.get_message_class(base_type_str)()
except (ValueError, TypeError):
base_instance = None
if array_size is not None and hasattr(base_instance, '__slots__'):
for index in range(array_size):
self._recursive_create_widget_items(item, topic_name + '[%d]' % index, base_type_str, base_instance)
return item
def _toggle_monitoring(self, topic_name):
item = self._tree_items[topic_name]
if item.checkState(CHECK_COLUMN):
print "start %s" % topic_name
if topic_name not in self._selected_items and topic_name in self._current_params:
self._selected_items.append(topic_name)
# item.setText(CHECK_COLUMN, str(self._selected_items.index(topic_name)))
# self._topics[topic_name]['info'].start_monitoring()
else:
print "stop %s" % topic_name
if topic_name in self._selected_items:
self._selected_items.remove(topic_name)
item.setText(CHECK_COLUMN,'')
# sel = self._selected_items
# for i in range(len(sel)):
# if sel[i] in self._current_params:
# _item = self._tree_items[sel[i]]
# _item.setText(CHECK_COLUMN, '{%d}' % i )
self.selectionChanged.emit(self.get_selected())
# self._topics[topic_name]['info'].stop_monitoring()
def _recursive_delete_widget_items(self, item):
def _recursive_remove_items_from_tree(item):
for index in reversed(range(item.childCount())):
_recursive_remove_items_from_tree(item.child(index))
topic_name = item.data(0, Qt.UserRole)
del self._tree_items[topic_name]
_recursive_remove_items_from_tree(item)
item.parent().removeChild(item)
@Slot('QPoint')
def handle_header_view_customContextMenuRequested(self, pos):
header = self.topics_tree_widget.header()
# show context menu
menu = QMenu(self)
action_toggle_auto_resize = menu.addAction('Toggle Auto-Resize')
action = menu.exec_(header.mapToGlobal(pos))
# evaluate user action
if action is action_toggle_auto_resize:
if header.resizeMode(0) == QHeaderView.ResizeToContents:
header.setResizeMode(QHeaderView.Interactive)
else:
header.setResizeMode(QHeaderView.ResizeToContents)
@Slot('QPoint')
def on_topics_tree_widget_customContextMenuRequested(self, pos):
item = self.topics_tree_widget.itemAt(pos)
if item is None:
return
# show context menu
menu = QMenu(self)
action_item_expand = menu.addAction(QIcon.fromTheme('zoom-in'), 'Expand All Children')
action_item_collapse = menu.addAction(QIcon.fromTheme('zoom-out'), 'Collapse All Children')
action = menu.exec_(self.topics_tree_widget.mapToGlobal(pos))
# evaluate user action
if action in (action_item_expand, action_item_collapse):
expanded = (action is action_item_expand)
def recursive_set_expanded(item):
item.setExpanded(expanded)
for index in range(item.childCount()):
recursive_set_expanded(item.child(index))
recursive_set_expanded(item)
def shutdown_plugin(self):
for topic in self._topics.values():
topic['info'].stop_monitoring()
self._timer_refresh_topics.stop()
def set_selected_topics(self, selected_topics):
"""
@param selected_topics: list of tuple. [(topic_name, topic_type)]
@type selected_topics: []
"""
rospy.logdebug('set_selected_topics topics={}'.format(
len(selected_topics)))
self._selected_topics = selected_topics
# TODO(Enhancement) Save/Restore tree expansion state
def save_settings(self, plugin_settings, instance_settings):
header_state = self.topics_tree_widget.header().saveState()
instance_settings.set_value('tree_widget_header_state', header_state)
def restore_settings(self, pluggin_settings, instance_settings):
if instance_settings.contains('tree_widget_header_state'):
header_state = instance_settings.value('tree_widget_header_state')
if not self.topics_tree_widget.header().restoreState(header_state):
rospy.logwarn("rqt_topic: Failed to restore header state.")
class BenchmarkGui(Plugin):
set_timer_signal = pyqtSignal(int, bool)
set_benchmark_info_signal = pyqtSignal(str)
def __init__(self, context):
super(BenchmarkGui, self).__init__(context)
self.setObjectName('EUROBENCH Benchmark Control')
self._widget = QWidget()
ui_file = path.join(path.dirname(path.realpath(__file__)),
'benchmark_gui.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('EUROBENCH Benchmark Control')
self._widget.setWindowTitle('EUROBENCH Benchmark Control')
self.benchmark_group = rospy.get_param('benchmark_group')
# UI elements
self.robot_spinbox = self._widget.findChild(QSpinBox, 'robot_spinbox')
self.run_spinbox = self._widget.findChild(QSpinBox, 'run_spinbox')
self.clock_value = self._widget.findChild(QLabel, 'clock_value')
self.benchmark_status_value = self._widget.findChild(
QLabel, 'benchmark_status_value')
self.core_status_value = self._widget.findChild(
QLabel, 'core_status_value')
self.testbed_status_value = self._widget.findChild(
QLabel, 'testbed_status_value')
self.rosbag_controller_status_value = self._widget.findChild(
QLabel, 'rosbag_controller_status_value')
self.start_button = self._widget.findChild(QPushButton, 'start_button')
self.stop_button = self._widget.findChild(QPushButton, 'stop_button')
# UI callbacks
self.start_button.clicked.connect(self.on_startbutton_click)
self.stop_button.clicked.connect(self.on_stopbutton_click)
# Disable start buttons, set their listeners
self.start_button.setEnabled(False)
self.stop_button.setEnabled(False)
# Status of required nodes
self.benchmark_core_available = False
self.rosbag_controller_available = False
self.testbed_node_available = False
self.core_status = None
# Subscribe to benchmark state
rospy.Subscriber('bmcore/state',
BenchmarkCoreState,
self.state_callback,
queue_size=1)
# Run heartbeat checks for required nodes (every second)
rospy.Timer(rospy.Duration(1), self.check_required_nodes)
# Update status labels
rospy.Timer(rospy.Duration(0.1), self.update_status_labels)
context.add_widget(self._widget)
def on_startbutton_click(self):
robot_name = self.robot_spinbox.value()
run_number = self.run_spinbox.value()
try:
start_benchmark = rospy.ServiceProxy('bmcore/start_benchmark',
StartBenchmark)
start_request = StartBenchmarkRequest()
start_request.robot_name = robot_name
start_request.run_number = run_number
start_benchmark(start_request)
except rospy.ServiceException as e:
rospy.logerr(
"bmcore/start_benchmark couldn't be called: {ex_val}".format(
ex_val=str(e)))
@staticmethod
def on_stopbutton_click():
try:
stop_benchmark = rospy.ServiceProxy('bmcore/stop_benchmark',
StopBenchmark)
stop_benchmark()
except rospy.ServiceException as e:
rospy.logerr(
"bmcore/stop_benchmark couldn't be called: {ex_val}".format(
ex_val=str(e)))
def state_callback(self, core_status_msg):
self.core_status = core_status_msg
def update_status_labels(self, _):
all_required_nodes_available = self.benchmark_core_available and self.rosbag_controller_available and self.testbed_node_available
benchmark_running = self.core_status is not None and self.core_status.status == BenchmarkCoreState.RUNNING_BENCHMARK
# ready_to_start
if all_required_nodes_available:
if not benchmark_running:
self.benchmark_status_value.setText('Ready to start')
self.run_spinbox.setEnabled(True)
self.robot_spinbox.setEnabled(True)
self.start_button.setEnabled(True)
self.stop_button.setEnabled(False)
if benchmark_running:
self.benchmark_status_value.setText('Running benchmark')
self.run_spinbox.setEnabled(False)
self.robot_spinbox.setEnabled(False)
self.start_button.setEnabled(False)
self.stop_button.setEnabled(True)
else:
self.benchmark_status_value.setText(
'Cannot start (required nodes not running)')
self.run_spinbox.setEnabled(False)
self.robot_spinbox.setEnabled(False)
self.start_button.setEnabled(False)
self.stop_button.setEnabled(False)
# Set clock timer
if benchmark_running:
seconds_passed = self.core_status.current_benchmark_seconds_passed
if seconds_passed >= 0:
minutes_passed = seconds_passed // 60
seconds_passed = seconds_passed % 60
self.clock_value.setText('%02d:%02d' %
(minutes_passed, seconds_passed))
else:
self.clock_value.setText('%02d' % seconds_passed)
else:
self.clock_value.setText('--:--')
def shutdown_plugin(self):
rospy.signal_shutdown('Shutting down')
def save_settings(self, plugin_settings, instance_settings):
# Save intrinsic configuration
pass
def restore_settings(self, plugin_settings, instance_settings):
# Restore intrinsic configuration
pass
def check_required_nodes(self, _):
# Benchmark core
try:
rospy.wait_for_service('bmcore/start_benchmark', timeout=0.5)
self.benchmark_core_available = True
except rospy.ROSException:
self.benchmark_core_available = False
# Rosbag controller
try:
rospy.wait_for_service(
'/eurobench_rosbag_controller/start_recording', timeout=0.5)
self.rosbag_controller_available = True
except rospy.ROSException:
self.rosbag_controller_available = False
# Testbed
if self.benchmark_group == 'MADROB':
set_mode_service_name = '/madrob/door/set_mode'
try:
rospy.wait_for_service(set_mode_service_name, timeout=0.5)
self.testbed_node_available = True
except rospy.ROSException:
self.testbed_node_available = False
if self.benchmark_group == 'BEAST':
some_service_name = '/beast_cart/reset_encoders'
try:
rospy.wait_for_service(some_service_name, timeout=0.5)
self.testbed_node_available = True
except rospy.ROSException:
self.testbed_node_available = False
if self.benchmark_core_available:
self.core_status_value.setText('OK')
else:
self.core_status_value.setText('Off')
if self.rosbag_controller_available:
self.rosbag_controller_status_value.setText('OK')
else:
self.rosbag_controller_status_value.setText('Off')
if self.testbed_node_available:
self.testbed_status_value.setText('OK')
else:
self.testbed_status_value.setText('Off')
class TelloPlugin(Plugin):
STATE_DISCONNECTED = state.State('disconnected')
STATE_CONNECTING = state.State('connecting')
STATE_CONNECTED = state.State('connected')
STATE_FLYING = state.State('flying')
STATE_QUIT = state.State('quit')
# define signals ROS updates
sig_connection_changed = pyqtSignal()
sig_update_position = pyqtSignal()
sig_battery_percentage = pyqtSignal(int)
sig_tello_link = pyqtSignal(int)
sig_wifi_sig_link = pyqtSignal(int)
sig_cpu = pyqtSignal(float)
sig_memory = pyqtSignal(float)
sig_temperature = pyqtSignal(float)
sig_height = pyqtSignal(str)
sig_speed = pyqtSignal(str)
sig_flight_time = pyqtSignal(str)
sig_remaining_time = pyqtSignal(str)
sig_battery_low = pyqtSignal(str)
sig_fly_mode = pyqtSignal(str)
sig_fast_mode = pyqtSignal(str)
sig_is_flying = pyqtSignal(str)
sig_remaining_time = pyqtSignal(str)
sig_foto_info = pyqtSignal(str)
sig_image = pyqtSignal(object)
# sig_mission_state = pyqtSignal(str)
def __init__(self, context):
super(TelloPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('TelloPlugin')
# 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)
# load main widget
self._widget = QWidget()
self.ui = Ui_TelloPlugin()
self.ui.setupUi(self._widget)
self.ui.tabWidget.setCurrentIndex(0)
self._widget.setObjectName('TelloPluginUi')
# Add left and right key arrow blocking
filter = KeySuppress()
self.ui.tabWidget.installEventFilter(filter)
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
# Add widget to the user interface
context.add_widget(self._widget)
print("Tello widget added")
# Tello node
self.bridge = CvBridge()
self.pos_listener = TransformListener()
# Add recorder and Recording Timer
self.recording = False
self.record_timer = QTimer()
self.record_timer.timeout.connect(self.update_record_time)
self.record_time = QTime(00,00,00)
self.video_recorder = None
self.VIDEO_TYPE = {'avi': cv2.VideoWriter_fourcc(*'XVID'), 'mp4': cv2.VideoWriter_fourcc(*'MP4V')}
# System, process and Wifi interface info
interface = "wlp2s0"
ssid_names = "TELLO-FD143A"
self.wifi_info = WifiInfo(interface, ssid_names)
self.utility = GetUtility()
# Callbacks GUI items
self.ui.connect_button.clicked[bool].connect(self.handle_connect_clicked)
self.ui.record_button.clicked[bool].connect(self.handle_record_clicked)
self.ui.foto_button.clicked[bool].connect(self.handle_foto_clicked)
self.ui.stop_button.clicked[bool].connect(self.handle_stop_clicked)
self.ui.start_button.clicked[bool].connect(self.handle_start_clicked)
self.ui.land_button.clicked[bool].connect(self.handle_land_clicked)
self.ui.target_button.clicked[bool].connect(self.handle_target_clicked)
self.ui.calibrate_button.clicked[bool].connect(self.handle_calibrate_clicked)
# Set icons
# get an instance of RosPack with the default search paths
rospack = rospkg.RosPack()
icon_path = rospack.get_path('tello_rqt_gui') + '/src/tello_rqt/resource/icons/'
self.ui.record_button.setText('') # override text with icon
self.ui.record_button.setIcon(QIcon(QPixmap(icon_path + 'video.png')))
self.ui.record_button.setIconSize(QSize(25,25))
self.ui.foto_button.setText('') # override text with icon
self.ui.foto_button.setIcon(QIcon(QPixmap(icon_path + 'foto.png')))
self.ui.foto_button.setIconSize(QSize(25,25))
self.ui.label_record_time.setText('record')
# Set bar values to 0
self.ui.link_bar.setValue(0)
self.ui.battery_bar.setValue(0)
self.ui.cpu_bar.setValue(0)
self.ui.memory_bar.setValue(0)
self.ui.temperature_bar.setValue(0)
# create signals and slots for updates
# never access widgets and GUI related things directly from a thread other than the main thread
self.sig_connection_changed.connect(self.update_ui_state)
self.sig_update_position.connect(self.update_position_data)
self.sig_battery_percentage.connect(self.ui.battery_bar.setValue)
self.sig_tello_link.connect(self.ui.link_bar.setValue)
self.sig_wifi_sig_link.connect(self.ui.wifi_signal_bar.setValue)
self.sig_cpu.connect(self.ui.cpu_bar.setValue)
self.sig_memory.connect(self.ui.memory_bar.setValue)
self.sig_temperature.connect(self.ui.temperature_bar.setValue)
self.sig_height.connect(self.ui.label_height.setText)
self.sig_speed.connect(self.ui.label_speed.setText)
self.sig_flight_time.connect(self.ui.label_flight_time.setText)
self.sig_image.connect(self.set_image)
self.sig_battery_low.connect(self.ui.label_battery_low.setText)
self.sig_is_flying.connect(self.ui.label_is_flying.setText)
self.sig_fly_mode.connect(self.ui.label_fly_mode.setText)
self.sig_fast_mode.connect(self.ui.label_fast_mode.setText)
self.sig_remaining_time.connect(self.ui.label_remaining_time.setText)
self.sig_foto_info.connect(self.ui.label_foto.setText)
#self.sig_mission_state.connect(self.ui.label_mission_state.setText)
# Connect signals from Tello, Controller und SLAM to GUI
self.ns_tello = 'tello/'
self.ns_controller = 'tello_controller/'
self.topic_slam_pose = '/orb_slam2_mono/pose'
self.topic_slam_scale = 'tello_controller/slam_real_world_scale'
self.tello_state = self.STATE_DISCONNECTED.getname()
self.pub_connect = rospy.Publisher(self.ns_tello+'connect', Empty, queue_size=1, latch=True)
self.pub_disconnect = rospy.Publisher(self.ns_tello+'disconnect', Empty, queue_size=1, latch=True)
self.pub_take_picture = rospy.Publisher(self.ns_tello+'take_picture', Empty, queue_size=1, latch=True)
self.pub_mission_command = rospy.Publisher(self.ns_controller+'mission_command', String, queue_size=1, latch=True)
self.sub_picture_update = rospy.Subscriber(self.ns_tello+'picture_update', String, self.cb_picture_update)
self.sub_status = rospy.Subscriber(self.ns_tello+'status', TelloStatus, self.cb_status)
self.sub_connection = rospy.Subscriber(self.ns_tello+'connection_state', String, self.cb_connection)
self.sub_video = rospy.Subscriber('tello/camera/image_raw', Image, self.cb_video)
self.sub_odom = rospy.Subscriber(self.ns_tello+'odom', Odometry, self.cb_odom, queue_size=1)
self.sub_slam_pose = rospy.Subscriber(self.topic_slam_pose, PoseStamped, self.cb_slam_pose, queue_size=1)
self.sub_slam_scale = rospy.Subscriber(self.topic_slam_scale, Float32, self.cb_slam_scale, queue_size=1)
self.sub_mission = rospy.Subscriber(self.ns_controller+'mission_state', String, self.cb_mission_state)
self.cv_image = None
# create additional thread for system und process utility
self.stop_threads = False
self.thread = threading.Thread(target=self.update_sys_util, args=())
self.thread.start()
print('init done')
def update_ui_state(self):
if self.tello_state == self.STATE_QUIT.getname() or self.tello_state == self.STATE_DISCONNECTED.getname():
self.ui.connect_button.setText("Connect")
# self.ui.connect_button.setStyleSheet("background-color: None")
self.ui.link_bar.setValue(0)
self.ui.battery_bar.setValue(0)
self.ui.wifi_signal_bar.setValue(-90)
elif self.tello_state == self.STATE_CONNECTED.getname():
self.ui.connect_button.setText("Disconnect")
# self.ui.connect_button.setStyleSheet("background-color: lightgreen")
# print('end if..')
elif self.tello_state == self.STATE_CONNECTING.getname():
self.ui.connect_button.setText("Connecting...")
# Set bar values to 0
self.ui.link_bar.setValue(0)
self.ui.battery_bar.setValue(0)
self.ui.wifi_signal_bar.setValue(-90)
# self.ui.connect_button.setStyleSheet("background-color: None")
def update_sys_util(self):
# do next iteration of not shutdown
if not rospy.is_shutdown() and not self.stop_threads:
# get cpu, memory percentage and temperature
cpu, memory, temp = self.utility.get_data()
# get wifi ling quality
sig, qual = self.wifi_info.get_info()
self.sig_cpu.emit(cpu)
self.sig_memory.emit(memory)
self.sig_temperature.emit(temp)
self.sig_wifi_sig_link.emit(sig)
# start new timer
timer = 2.0 # seconds
threading.Timer(timer, self.update_sys_util).start()
def handle_connect_clicked(self):
"""
User pressed connect button
"""
print(self.tello_state)
if self.tello_state == self.STATE_CONNECTED.getname():
self.pub_disconnect.publish()
else:
self.pub_connect.publish()
def handle_record_clicked(self):
# Create a file in ~/Pictures/ to receive image data from the drone.
print('Recording: ', self.recording)
if self.recording:
# close and save video
self.recording = False
self.record_time = QTime(00,00,00)
self.ui.label_record_time.setText('record')
else:
self.record_timer.start(1000) # update every second
# self.stop_event.clear() # clear event
self.path = '%s/Videos/tello-%s.avi' % (
os.getenv('HOME'),
time.strftime("%Y-%m-%d_%H:%M:%S"))
self.video_recorder = cv2.VideoWriter(self.path, self.VIDEO_TYPE['avi'], 25, (960, 720))
self.recording = True
def handle_foto_clicked(self):
self.ui.label_foto.setText("Taking picture")
self.pub_take_picture.publish(Empty())
def handle_stop_clicked(self):
self.pub_mission_command.publish('stop')
def handle_start_clicked(self):
self.pub_mission_command.publish('start')
def handle_land_clicked(self):
self.pub_mission_command.publish('land')
def handle_target_clicked(self):
self.pub_mission_command.publish('target')
def handle_calibrate_clicked(self):
self.pub_mission_command.publish('calibrate')
def update_record_time(self):
# self.record_time = QtCore.QTime(00,00,00)
# print("Update Recording time")
if self.recording:
self.record_time = self.record_time.addSecs(1)
print(self.record_time.toString())
self.ui.label_record_time.setText(self.record_time.toString())
def update_position_data(self):
# try to look up current position
try:
(tello_trans, tello_rot) = self.pos_listener.lookupTransform('odom', 'base_link', rospy.Time(0))
# print('trans: ', tello_trans, ' | rot: ', tello_rot)
"""
euler_angles = euler_from_quaternion(tello_rot)
self.ui.label_internal_x.setText('%.2f' % tello_trans[0])
self.ui.label_internal_y.setText('%.2f' % tello_trans[1])
self.ui.label_internal_z.setText('%.2f' % tello_trans[2])
self.ui.label_internal_yaw.setText('%.2f' % euler_angles[2])
"""
except (LookupException, ConnectivityException, ExtrapolationException):
print('No Position Data available')
self.ui.label_internal_x.setText('wait')
self.ui.label_internal_y.setText('wait')
self.ui.label_internal_z.setText('wait')
self.ui.label_internal_yaw.setText('wait')
self.ui.label_slam_x.setText('wait')
self.ui.label_slam_y.setText('wait')
self.ui.label_slam_z.setText('wait')
self.ui.label_slam_yaw.setText('wait')
def cb_odom(self, msg):
current_orientation_list = [msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z, msg.pose.pose.orientation.w]
current_angles = euler_from_quaternion(current_orientation_list)
self.ui.label_internal_x.setText('%.2f' % msg.pose.pose.position.x)
self.ui.label_internal_y.setText('%.2f' % msg.pose.pose.position.y)
self.ui.label_internal_z.setText('%.2f' % msg.pose.pose.position.z)
self.ui.label_internal_yaw.setText('%.2f' % current_angles[2])
def cb_slam_pose(self, msg):
current_orientation_list = [msg.pose.orientation.x, msg.pose.orientation.y,
msg.pose.orientation.z, msg.pose.orientation.w]
current_angles = euler_from_quaternion(current_orientation_list)
self.ui.label_slam_x.setText('%.2f' % msg.pose.position.x)
self.ui.label_slam_y.setText('%.2f' % msg.pose.position.y)
self.ui.label_slam_z.setText('%.2f' % msg.pose.position.z)
self.ui.label_slam_yaw.setText('%.2f' % current_angles[2])
def cb_slam_scale(self, msg):
self.ui.label_slam_scaling.setText('%.2f' % msg.data)
def cb_picture_update(self, msg):
self.ui.label_foto.setText(msg.data)
def cb_mission_state(self, msg):
self.ui.label_mission_state.setText(msg.data)
def cb_status(self, msg):
self.sig_battery_percentage.emit(msg.battery_percentage)
self.sig_tello_link.emit(msg.wifi_strength)
self.sig_height.emit("%.2f" % msg.height_m)
self.sig_speed.emit("%.2f" % msg.speed_horizontal_mps)
self.sig_flight_time.emit("%.2f" % msg.flight_time_sec)
self.sig_battery_low.emit(str(msg.is_battery_lower))
self.sig_fly_mode.emit(str(msg.fly_mode))
self.sig_fast_mode.emit(str(msg.cmd_fast_mode))
self.sig_is_flying.emit(str(msg.is_flying))
m, s = divmod(-msg.drone_fly_time_left_sec/10., 60)
# print("Remaining: ",m,"min, ",s,"s")
self.sig_remaining_time.emit("%d:%d" % (m, s))
# check position
self.sig_update_position.emit()
def cb_connection(self, msg):
print(self.tello_state)
self.tello_state = msg.data
self.sig_connection_changed.emit()
def cb_video(self, msg):
"""
callback to record video and display video
"""
try:
self.cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8")
except CvBridgeError as e:
print(e)
if self.recording:
print('recording.... ')
self.video_recorder.write(self.cv_image)
# show Video in GUI
self.sig_image.emit(self.cv_image)
def set_image(self, img_bgr):
img_rgb = cv2.cvtColor(img_bgr, cv2.COLOR_BGR2RGB) # switch b and r channel
# Qt Image
qt_image = QImage(img_rgb, img_rgb.shape[1], img_rgb.shape[0], QImage.Format_RGB888)
self.ui.label_video.setPixmap(QPixmap(qt_image)) # set pixmap to label
def shutdown_plugin(self):
# TODO unregister all publishers here
# wait to terminate Threads
# self._ros_process.close()
print(os.path.dirname(os.path.realpath(__file__)))
self.pub_disconnect.publish(Empty())
self.stop_threads = True
self.thread.join()
pass
def save_settings(self, plugin_settings, instance_settings):
# TODO save intrinsic configuration, usually using:
# instance_settings.set_value(k, v)
pass
def restore_settings(self, plugin_settings, instance_settings):
# TODO restore intrinsic configuration, usually using:
# v = instance_settings.value(k)
pass
class ImageView2Widget(QWidget):
"""
Qt widget to communicate with image_view2
"""
cv_image = None
pixmap = None
repaint_trigger = pyqtSignal()
def __init__(self):
super(ImageView2Widget, self).__init__()
self.left_button_clicked = False
self.repaint_trigger.connect(self.redraw)
self.lock = Lock()
self.need_to_rewrite = False
self.bridge = CvBridge()
self.image_sub = None
self.event_pub = None
self.label = ScaledLabel()
self.label.setAlignment(Qt.AlignCenter)
self.label.setSizePolicy(
QSizePolicy(QSizePolicy.Ignored, QSizePolicy.Ignored))
#self.label.installEventFilter(self)
vbox = QtGui.QVBoxLayout(self)
vbox.addWidget(self.label)
self.setLayout(vbox)
self._image_topics = []
self._update_topic_thread = Thread(target=self.updateTopics)
self._update_topic_thread.start()
self._active_topic = None
self.setMouseTracking(True)
self.label.setMouseTracking(True)
self._dialog = ComboBoxDialog()
self.show()
def trigger_configuration(self):
self._dialog.exec_()
self.setupSubscriber(self._image_topics[self._dialog.number])
def setupSubscriber(self, topic):
if self.image_sub:
self.image_sub.unregister()
rospy.loginfo("Subscribing %s" % (topic + "/marked"))
self.image_sub = rospy.Subscriber(topic + "/marked", Image,
self.imageCallback)
self.event_pub = rospy.Publisher(topic + "/event", MouseEvent)
self._active_topic = topic
def onActivated(self, number):
self.setupSubscriber(self._image_topics[number])
def imageCallback(self, msg):
with self.lock:
if msg.width == 0 or msg.height == 0:
rospy.logdebug("Looks input images is invalid")
return
cv_image = self.bridge.imgmsg_to_cv2(msg, msg.encoding)
if msg.encoding == "bgr8":
self.cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB)
elif msg.encoding == "rgb8":
self.cv_image = cv_image
self.numpy_image = np.asarray(self.cv_image)
self.need_to_rewrite = True
self.repaint_trigger.emit()
def updateTopics(self):
need_to_update = False
for (topic, topic_type) in rospy.get_published_topics():
if topic_type == "sensor_msgs/Image":
with self.lock:
if not topic in self._image_topics:
self._image_topics.append(topic)
need_to_update = True
if need_to_update:
with self.lock:
self._image_topics = sorted(self._image_topics)
self._dialog.combo_box.clear()
for topic in self._image_topics:
self._dialog.combo_box.addItem(topic)
if self._active_topic:
self._dialog.combo_box.setCurrentIndex(
self._image_topics.index(self._active_topic))
time.sleep(1)
@pyqtSlot()
def redraw(self):
with self.lock:
if not self.need_to_rewrite:
return
if self.cv_image != None:
size = self.cv_image.shape
img = QImage(self.cv_image.data, size[1], size[0],
size[2] * size[1], QImage.Format_RGB888)
# convert to QPixmap
self.pixmap = QPixmap(size[1], size[0])
self.pixmap.convertFromImage(img)
self.label.setPixmap(
self.pixmap.scaled(self.label.width(), self.label.height(),
QtCore.Qt.KeepAspectRatio))
#self.label.setPixmap(self.pixmap)
def mousePosition(self, e):
label_x = self.label.x()
label_y = self.label.y()
label_width = self.label.width()
label_height = self.label.height()
pixmap_width = self.label.pixmap().width()
pixmap_height = self.label.pixmap().height()
x_offset = (label_width - pixmap_width) / 2.0 + label_x
y_offset = (label_height - pixmap_height) / 2.0 + label_y
return (e.x() - x_offset, e.y() - y_offset)
def mouseMoveEvent(self, e):
msg = MouseEvent()
msg.header.stamp = rospy.Time.now()
msg.type = MouseEvent.MOUSE_MOVE
msg.x, msg.y = self.mousePosition(e)
msg.width = self.label.pixmap().width()
msg.height = self.label.pixmap().height()
if self.event_pub:
self.event_pub.publish(msg)
def mousePressEvent(self, e):
msg = MouseEvent()
msg.header.stamp = rospy.Time.now()
if e.button() == Qt.LeftButton:
msg.type = MouseEvent.MOUSE_LEFT_DOWN
self.left_button_clicked = True
elif e.button() == Qt.RightButton:
msg.type = MouseEvent.MOUSE_RIGHT_DOWN
msg.width = self.label.pixmap().width()
msg.height = self.label.pixmap().height()
msg.x, msg.y = self.mousePosition(e)
if self.event_pub:
self.event_pub.publish(msg)
def mouseReleaseEvent(self, e):
if e.button() == Qt.LeftButton:
self.left_button_clicked = False
msg = MouseEvent()
msg.header.stamp = rospy.Time.now()
msg.width = self.label.pixmap().width()
msg.height = self.label.pixmap().height()
msg.type = MouseEvent.MOUSE_LEFT_UP
msg.x, msg.y = self.mousePosition(e)
if self.event_pub:
self.event_pub.publish(msg)
def save_settings(self, plugin_settings, instance_settings):
if self._active_topic:
instance_settings.set_value("active_topic", self._active_topic)
def restore_settings(self, plugin_settings, instance_settings):
if instance_settings.value("active_topic"):
topic = instance_settings.value("active_topic")
self._dialog.combo_box.addItem(topic)
self.setupSubscriber(topic)
class SignalRender(QObject):
renderSignal = pyqtSignal([bool])
class WristValuesSignal(QObject):
signal = pyqtSignal(list)
class FlexBeWidget(QWidget):
updateCurrentBehavior = pyqtSignal(str)
updateCurrentState = pyqtSignal(str)
updateStateLog = pyqtSignal(str)
clearStateLog = pyqtSignal()
setPauseButtonText = pyqtSignal(str)
def __init__(self, parent, context):
super(FlexBeWidget, self).__init__()
self._attached = False
self._paused = False
# load from ui
rp = rospkg.RosPack()
ui_file = os.path.join(rp.get_path('hector_flexbe_rqt_widget'),
'resource', 'flexbe_interface.ui')
loadUi(ui_file, self, {'QWidget': QWidget})
self.setObjectName('FlexBeInterfaceUi')
# init table widgets
self._argument_table_widget = TableWidget(self.argumentTableWidget)
self._userdata_table_widget = TableWidget(self.userdataTableWidget)
# connect to signals
self.executePushButton.clicked[bool].connect(
self._execute_button_pressed)
self.pausePushButton.clicked[bool].connect(self._pause_button_pressed)
self.abortPushButton.clicked[bool].connect(self._abort_button_pressed)
self.attachPushButton.clicked[bool].connect(
self._attach_button_pressed)
self.autonomyComboBox.currentIndexChanged[int].connect(
lambda index: self._autonomy_pub.publish(index))
self.clearArgumentPushButton.clicked[bool].connect(
self._argument_table_widget.clear)
self.clearUserdataPushButton.clicked[bool].connect(
self._userdata_table_widget.clear)
self.clearLogPushButton.clicked[bool].connect(self.logTextEdit.clear)
self.saveLogPushButton.clicked[bool].connect(self._save_log)
# Qt signals
self.updateCurrentBehavior.connect(self.behaviorLineEdit.setText)
self.updateCurrentState.connect(self.currentStateLineEdit.setText)
self.setPauseButtonText.connect(self.pausePushButton.setText)
self.updateStateLog.connect(self.logTextEdit.append)
self.clearStateLog.connect(self.logTextEdit.clear)
# init behavior list
self._lib = BehaviorLibrary()
self._retrieve_behaviors()
# init subscribers
self._status_sub = rospy.Subscriber('flexbe/status', BEStatus,
self._status_cb)
self._state_update_sub = rospy.Subscriber('flexbe/debug/current_state',
String,
self._state_update_cb)
self._behavior_update_sub = rospy.Subscriber('flexbe/behavior_update',
String,
self._behavior_update_cb)
# init publisher
self._attach_pub = rospy.Publisher('flexbe/command/attach',
UInt8,
queue_size=1)
self._autonomy_pub = rospy.Publisher('flexbe/command/autonomy',
UInt8,
queue_size=1)
self._command_pause_pub = rospy.Publisher('flexbe/command/pause',
Bool,
queue_size=1)
self._command_preempt_pub = rospy.Publisher('flexbe/command/preempt',
Empty,
queue_size=1)
# init action client
self._execute_behavior_client = actionlib.SimpleActionClient(
'flexbe/execute_behavior', BehaviorExecutionAction)
def shutdown_plugin(self):
print('Shutting down ...')
self._status_sub.unregister()
self._state_update_sub.unregister()
self._behavior_update_sub.unregister()
print('Done!')
def _status_cb(self, status):
if status.code in [
BEStatus.FINISHED, BEStatus.FAILED, BEStatus.READY,
BEStatus.ERROR
]:
self.executePushButton.setEnabled(True)
self.setPauseButtonText.emit('Pause')
self.abortPushButton.setEnabled(False)
self.attachPushButton.setEnabled(False)
self._detach()
self._paused = False
else:
self._retrieve_behavior_name(status.behavior_id)
self.executePushButton.setEnabled(False)
self.abortPushButton.setEnabled(True)
self.attachPushButton.setEnabled(True)
if status.code == BEStatus.STARTED:
self.updateCurrentState.emit('Started')
if status.code == BEStatus.FINISHED:
self.updateCurrentState.emit('Finished')
if status.code == BEStatus.FAILED:
self.updateCurrentState.emit('Failed')
if status.code == BEStatus.LOCKED:
self.updateCurrentState.emit('Locked')
if status.code == BEStatus.WAITING:
self.updateCurrentState.emit('Waiting')
if status.code == BEStatus.SWITCHING:
self.updateCurrentState.emit('Switching')
if status.code == BEStatus.WARNING:
self.updateCurrentState.emit('Warning')
if status.code == BEStatus.ERROR:
self.updateCurrentState.emit('Error')
if status.code == BEStatus.READY:
self.updateCurrentState.emit('Ready')
def _state_update_cb(self, msg):
if self._attached:
self.updateStateLog.emit(msg.data)
def _behavior_update_cb(self, msg):
if self._attached:
self.updateCurrentState.emit(msg.data)
def _execute_behavior_active_cb(self):
self.clearStateLog.emit()
def _retrieve_behaviors(self):
names = []
for item in self._lib._behavior_lib.values():
names.append(item['name'])
self._update_behaviors(names)
def _retrieve_behavior_name(self, id):
#behavior = self._lib.get_behavior(id) # TODO: How to get name?
self.updateCurrentBehavior.emit('N/A')
def _execute_button_pressed(self):
if not self.behaviorsListWidget.currentItem():
return
if not self._execute_behavior_client.wait_for_server(
rospy.Duration(1.0)):
rospy.logerr('FlexBE action server not available!')
self.updateCurrentState.emit('FlexBE action server not available!')
return
goal = BehaviorExecutionGoal()
goal.behavior_name = self.behaviorsListWidget.currentItem().text()
# add behavior arguments
args = self._argument_table_widget.get_entries()
for key in args:
value = args[key]
if key and value:
goal.arg_keys.append(key)
goal.arg_values.append(value)
# add initial userdata
args = self._userdata_table_widget.get_entries()
for key in args:
value = args[key]
if key and value:
goal.input_keys.append(key)
goal.input_values.append(value)
self._execute_behavior_client.send_goal(
goal, active_cb=self._execute_behavior_active_cb)
def _pause_button_pressed(self):
self._paused = not self._paused
self._command_pause_pub.publish(self._paused)
if self._paused:
self.updateCurrentState.emit('Paused')
self.setPauseButtonText.emit('Resume')
else:
self.updateCurrentState.emit('N/A')
self.setPauseButtonText.emit('Pause')
def _abort_button_pressed(self):
self._execute_behavior_client.cancel_goal()
self._command_preempt_pub.publish()
self.updateCurrentState.emit('Canceled')
def _attach_button_pressed(self):
if self._attached:
self._detach()
else:
self._attach()
def _attach(self):
self._attached = True
self.pausePushButton.setEnabled(True)
self.attachPushButton.setText('Detach')
self.autonomyLabel.setEnabled(True)
self.autonomyComboBox.setEnabled(True)
self._attach_pub.publish(1) # attach to state machine
self._autonomy_pub.publish(self.autonomyComboBox.currentIndex())
def _detach(self):
self._attached = False
self.pausePushButton.setEnabled(False)
self.attachPushButton.setText('Attach')
self.autonomyLabel.setEnabled(False)
self.autonomyComboBox.setEnabled(False)
self.updateCurrentBehavior.emit('N/A')
self.updateCurrentState.emit('N/A')
#self._attach_pub.publish(0) # detach from state machine; TODO: Seems not to work
def _update_behaviors(self, names):
enabled = self.executePushButton.isEnabled()
self.executePushButton.setEnabled(False)
self.behaviorsListWidget.clear()
self.behaviorsListWidget.addItems(names)
self.behaviorsListWidget.sortItems()
self.executePushButton.setEnabled(
enabled and self.behaviorsListWidget.count() > 0)
def _save_log(self):
dialog = QFileDialog()
dialog.setAcceptMode(QFileDialog.AcceptSave)
dialog.setNameFilter('Text file (*.txt)')
dialog.setDefaultSuffix('.txt')
dialog.setDirectory(os.getenv('HOME'))
if dialog.exec_():
file = open(dialog.selectedFiles()[0], 'w')
file.write(self.logTextEdit.toPlainText())
file.close()
class ConversationViewPlugin(Plugin):
signalAddItem = pyqtSignal(str, int)
def __init__(self, context):
super(ConversationViewPlugin, self).__init__(context)
self.setObjectName('ConversationViewPlugin')
self._widget = QWidget()
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() +
(' (%d)' % context.serial_number()))
ui_file = os.path.join(
rospkg.RosPack().get_path('rqt_conversation_view'), 'resource',
'conversation_view.ui')
loadUi(ui_file, self._widget)
context.add_widget(self._widget)
self.send_shortcut = QShortcut(QKeySequence("Ctrl+Return"),
self._widget, self.handle_button_send)
self._widget.buttonSend.clicked.connect(self.handle_button_send)
self._widget.textInput.setFocus()
self.signalAddItem.connect(self.add_item_to_conversation_view)
self.pub_input = rospy.Publisher('recognized_word',
RecognizedWord,
queue_size=10)
rospy.Subscriber('raising_events', RaisingEvents,
self.handle_raising_events)
rospy.Subscriber('reply', Reply, self.handle_reply)
def add_item_to_conversation_view(self, msg, type=0):
label_msg = QLabel(msg)
label_msg.setWordWrap(True)
label_msg.setStyleSheet('font-size:10pt;')
inner_text_layout = QHBoxLayout()
horizonalSpacer1 = QSpacerItem(0, 0, QSizePolicy.MinimumExpanding,
QSizePolicy.MinimumExpanding)
if type == 0:
inner_text_layout.addWidget(label_msg)
inner_text_layout.addItem(horizonalSpacer1)
elif type == 1:
inner_text_layout.addItem(horizonalSpacer1)
inner_text_layout.addWidget(label_msg)
inner_layout = QVBoxLayout()
time_msg = QLabel(str(time.asctime(time.localtime(time.time()))))
time_msg.setStyleSheet('font-size:8pt;')
inner_layout.addItem(inner_text_layout)
inner_layout.addWidget(time_msg)
inner_layout.setSizeConstraint(QLayout.SetFixedSize)
outer_layout = QHBoxLayout()
horizonalSpacer2 = QSpacerItem(0, 0, QSizePolicy.MinimumExpanding,
QSizePolicy.MinimumExpanding)
if type == 0:
label_msg.setStyleSheet(
'background: #e5e5ea; padding: 6px; border-radius: 8px;')
time_msg.setAlignment(Qt.AlignLeft)
outer_layout.addItem(inner_layout)
outer_layout.addItem(horizonalSpacer2)
elif type == 1:
label_msg.setStyleSheet(
'background: #1d86f4; padding: 6px; border-radius: 8px; color:#fff;'
)
time_msg.setAlignment(Qt.AlignRight)
outer_layout.addItem(horizonalSpacer2)
outer_layout.addItem(inner_layout)
outer_layout.setSizeConstraint(QLayout.SetMinimumSize)
widget = QWidget()
widget.setLayout(outer_layout)
widget.resize(widget.sizeHint())
item = QListWidgetItem()
item.setSizeHint(widget.sizeHint())
self._widget.listWidget.addItem(item)
self._widget.listWidget.setItemWidget(item, widget)
self._widget.listWidget.scrollToBottom()
def handle_raising_events(self, msg):
event_text = msg.recognized_word
for event in msg.events:
event_text = event_text + '\n -%s' % event
self.signalAddItem.emit(event_text, 1)
def handle_reply(self, msg):
event_text = msg.reply
self.signalAddItem.emit(event_text, 0)
def handle_button_send(self):
input_text = self._widget.textInput.toPlainText()
if input_text == '':
return
msg = RecognizedWord()
msg.recognized_word = input_text
msg.confidence = 1.0
self.pub_input.publish(msg)
self._widget.textInput.clear()
def shutdown_plugin(self):
pass
def save_settings(self, plugin_settings, instance_settings):
pass
def restore_settings(self, plugin_settings, instance_settings):
pass
class MapGraphicsScene(QGraphicsScene):
signalMousePressedPos = pyqtSignal(QPointF)
signalMouseReleasedPos = pyqtSignal(QPointF)
signalMouseMovePos = pyqtSignal(QPointF)
def __init__(self):
super(MapGraphicsScene, self).__init__()
self.regionCounter = 0
self.line_dict = {}
self.items_dict = {}
# Transfrom pixel coordinates to world coordinates
# Input: pixel coordinates QPointF
# Output: world coordinates (x, y, z)
def pixelToWorld(self, pixel_coords=QPointF()):
world_coords = ((pixel_coords.x() - self.worldOrigin.x()) *
self.map_resolution,
-(pixel_coords.y() - self.worldOrigin.y()) *
self.map_resolution, 0.0)
return world_coords
# Transform world coordinates to pixel coordinates
# Input: world coordinates (x, y, z)
# Output: pixel coordinates QPointF
def worldToPixel(self, world_coords):
pixel_coords = QPointF(
world_coords[0] / self.map_resolution + self.worldOrigin.x(),
-world_coords[1] / self.map_resolution + self.worldOrigin.y())
return pixel_coords
# Add ROI to graphics scene
# Input: pixel coordinates QPointF
# Update GraphicsScene
def add_ROI(self, pixel_coords):
self.regionCounter += 1
markerSize = 25
ellipse_item = QGraphicsEllipseItem(
QRectF(
QPointF(pixel_coords.x() - markerSize / 2,
pixel_coords.y() - markerSize / 2),
QSizeF(markerSize, markerSize)))
ellipse_item.setBrush(QBrush(QColor('red')))
self.addItem(ellipse_item)
label_font = QFont()
label_font.setPointSize(15)
region_string = 'r' + str(self.regionCounter).zfill(2)
ellipse_item_label = QGraphicsTextItem(region_string)
ellipse_item_label.setPos(pixel_coords)
ellipse_item_label.setFont(label_font)
self.addItem(ellipse_item_label)
self.items_dict.update({
region_string: {
'ellipse_item': ellipse_item,
'ellipse_item_label': ellipse_item_label,
'pixel_coords': pixel_coords,
'ap_item_label': {}
}
})
# Remove las added ROI
# Update GraphicsScene
def remove_ROI(self):
region_string = 'r' + str(self.regionCounter).zfill(2)
self.removeItem(self.items_dict[region_string]['ellipse_item'])
self.removeItem(self.items_dict[region_string]['ellipse_item_label'])
self.removeArrow(self.items_dict[region_string]['arrow'])
for i in range(
0,
len(self.items_dict[region_string]['ap_item_label'].keys())):
self.remove_ap(
region_string,
self.items_dict[region_string]['ap_item_label'].keys()[i])
del self.items_dict[region_string]
self.regionCounter = self.regionCounter - 1
# Add line between to ROI's
# Input: number ROI 1 int
# number ROI 2 int
# Update GraphicsScene
def add_edge(self, roi_num_1, roi_num_2):
pixel_coords_1 = self.items_dict[
'r' + str(roi_num_1).zfill(2)]['pixel_coords']
pixel_coords_2 = self.items_dict[
'r' + str(roi_num_2).zfill(2)]['pixel_coords']
self.line_dict[(str(roi_num_1) + '-' +
str(roi_num_2))] = QGraphicsLineItem(
QLineF(pixel_coords_2, pixel_coords_1))
self.addItem(self.line_dict[(str(roi_num_1) + '-' + str(roi_num_2))])
# Remove line between to ROI's
# Input: edge label string e.g. 1-2
# Update GraphicsScene
def remove_edge(self, edge):
self.removeItem(self.line_dict[edge])
del self.line_dict[edge]
# Add general atomic proposition label
# Input: region label string
# ap label string
# Update GraphicsScene
def add_ap(self, region, ap):
label_font = QFont()
label_font.setPointSize(15)
ap_item_label = QGraphicsTextItem(ap)
ap_item_label.setPos(
QPointF(self.items_dict[region]['pixel_coords'].x() - 25,
self.items_dict[region]['pixel_coords'].y()))
ap_item_label.setFont(label_font)
self.addItem(ap_item_label)
self.items_dict[region]['ap_item_label'].update({ap: ap_item_label})
# Remove general atomic proposition label
# Input: region label string
# ap label string
# Update GraphicsScene
def remove_ap(self, region, ap):
self.removeItem(self.items_dict[region]['ap_item_label'][ap])
# Reset graphics scene
# Update GraphicsScene
def reset(self):
for i in range(0, self.regionCounter):
self.remove_ROI()
for i in range(0, len(self.line_dict)):
self.remove_edge(self.line_dict.keys()[0])
self.regionCounter = 0
self.ellipse_items = []
self.ellipse_items_labels = []
self.pixel_coords_list = []
self.line_dict = {}
# Load map
# Input: scenario name string
# Update GraphicsScene
def load_map(self, scenario):
self.scenario = scenario
map_yaml = os.path.join(rospkg.RosPack().get_path('rqt_simulation'),
'scenarios', scenario, 'map.yaml')
self.loadConfig(map_yaml)
map = 'map.png'
map_file = os.path.join(rospkg.RosPack().get_path('rqt_simulation'),
'scenarios', scenario, map)
pixmap = QPixmap(map_file)
self.mapSize = pixmap.size()
self.addPixmap(pixmap)
# Add world origin
self.worldOrigin = QPointF(
-self.map_origin[0] / self.map_resolution,
self.map_origin[1] / self.map_resolution + self.mapSize.height())
self.addCoordinateSystem(self.worldOrigin, 0.0)
# Send signal if mouse button is pressed
# Input: click event
# Output: pixel coordinates QPointF
def mousePressEvent(self, event):
pos = event.lastScenePos()
self.signalMousePressedPos.emit(pos)
# Send signal if mouse button is released
# Input: release event
# Output: pixel coordinates QPointF
def mouseReleaseEvent(self, event):
pos = event.lastScenePos()
self.signalMouseReleasedPos.emit(pos)
# Send signal if mouse is moving in graphic scene
# Input: move event
# Output: pixel coordinates QPointF
def mouseMoveEvent(self, event):
pos = event.lastScenePos()
self.signalMouseMovePos.emit(pos)
# Add arrow to graphic scene
# Input: start point QPointF
# end point QPointF
# drawing options QPen
# Output: arrow list with 3 QGraphicsLineItem
# Update GraphicsScene
def addArrow(self, startPoint=QPointF(), endPoint=QPointF(), pen=QPen()):
alpha = 5 * pi / 6
arrowLength = 10
theta = atan2((endPoint.y() - startPoint.y()),
(endPoint.x() - startPoint.x()))
gamma1 = theta + alpha
gamma2 = theta - alpha
arrowPoint_1 = QPointF(endPoint.x() + arrowLength * cos(gamma1),
endPoint.y() + arrowLength * sin(gamma1))
arrowPoint_2 = QPointF(endPoint.x() + arrowLength * cos(gamma2),
endPoint.y() + arrowLength * sin(gamma2))
line_0 = QLineF(startPoint, endPoint)
line_1 = QLineF(endPoint, arrowPoint_1)
line_2 = QLineF(endPoint, arrowPoint_2)
line_item_0 = QGraphicsLineItem(line_0)
line_item_0.setPen(pen)
line_item_1 = QGraphicsLineItem(line_1)
line_item_1.setPen(pen)
line_item_2 = QGraphicsLineItem(line_2)
line_item_2.setPen(pen)
arrowItems = [line_item_0, line_item_1, line_item_2]
self.addItem(line_item_0)
self.addItem(line_item_1)
self.addItem(line_item_2)
return arrowItems
# Remove arrow from graphics scene
# Input: arrow list with 3 QGraphicsLineItem
# Update GraphicsScene
def removeArrow(self, arrow):
for n in arrow:
self.removeItem(n)
def scale_map(self, graphicsView, scenario):
rectF = graphicsView.geometry()
if (float(rectF.width()) / self.mapSize.width() <
float(rectF.height()) / self.mapSize.height()):
scale = float(rectF.width()) / self.mapSize.width()
elif scenario == 'pal_office' or scenario == 'sml':
scale = 0.7
else:
scale = float(rectF.height()) / self.mapSize.height()
transform = QTransform(scale, 0, 0.0, scale, 0, 0)
return transform
# Load ROI's and edges from a FTS
# Input: FTS dict
# Update GraphicsScene
def load_graphic_from_FTS(self, FTS):
sorted_keys = FTS.region_of_interest.keys()
sorted_keys.sort()
arrow_length = 50
# Add all the ROI's and edges
for i in range(0, len(FTS.region_of_interest)):
region_string = 'r' + str(i + 1).zfill(2)
pixel_coords = self.worldToPixel(
FTS.region_of_interest[sorted_keys[i]]['pose']['position'])
self.add_ROI(pixel_coords)
for j in range(
0, len(FTS.region_of_interest[sorted_keys[i]]['propos'])):
if sorted_keys[i] != FTS.region_of_interest[
sorted_keys[i]]['propos'][j]:
self.add_ap(
sorted_keys[i],
FTS.region_of_interest[sorted_keys[i]]['propos'][j])
quaternion = Quaternion(
FTS.region_of_interest[sorted_keys[i]]['pose']['orientation'])
rot_axis = quaternion.axis
theta = quaternion.angle * rot_axis[2]
end_point = QPointF(pixel_coords.x() + arrow_length * cos(theta),
pixel_coords.y() - arrow_length * sin(theta))
arrow = self.addArrow(pixel_coords, end_point)
self.items_dict[region_string]['arrow'] = arrow
# Add all edges to graphics scene
for i in range(0, len(FTS.region_of_interest)):
for j in range(
0, len(FTS.region_of_interest[sorted_keys[i]]['edges'])):
index = sorted_keys.index(FTS.region_of_interest[
sorted_keys[i]]['edges'][j]['target'])
if i < index:
if (str(i + 1) + '-' +
str(index + 1)) not in self.line_dict.keys():
self.add_edge(i + 1, index + 1)
else:
if (str(index + 1) + '-' +
str(i + 1)) not in self.line_dict.keys():
self.add_edge(index + 1, i + 1)
# Add coordinate system
# Input: origin QPointF
# angle Float
# Update GraphicsScene
def addCoordinateSystem(self, origin=QPointF(), angle=0.0):
XAxis = QPointF(origin.x() + 100, origin.y())
YAxis = QPointF(origin.x(), origin.y() - 100)
self.addArrow(origin, XAxis)
self.addArrow(origin, YAxis)
XLabel = self.addText('X', QFont())
XLabel.setPos(XAxis)
YLabel = self.addText('Y', QFont())
YLabel.setPos(YAxis)
# Load the data from map.yaml file
# Input: file string
def loadConfig(self, filename):
stream = file(filename, 'r')
data = yaml.load(stream)
stream.close()
self.map_image = data['image']
self.map_resolution = data['resolution']
self.map_origin = tuple(data['origin'])
self.map_negate = data['negate']
self.map_occupied_thresh = data['occupied_thresh']
self.map_free_thresh = data['free_thresh']
qualisys = data['qualisys']
if qualisys:
self.tf_qualisys_to_map = data['tf_qualisys_to_map']
rospy.loginfo('rqt_simulation map tf to qualisys : %s' %
(self.tf_qualisys_to_map))
rospy.loginfo('rqt_simulation map : %s' % (self.scenario))
rospy.loginfo('rqt_simulation map resolution : %.6f' %
(self.map_resolution))
rospy.loginfo('rqt_simulation map origin : %s' % (self.map_origin, ))
rospy.loginfo('rqt_simulation map negate : %s' % (self.map_negate))
rospy.loginfo('rqt_simulation map occupied threshold : %s' %
(self.map_occupied_thresh))
rospy.loginfo('rqt_simulation map free threshold : %s' %
(self.map_free_thresh))
class RobotTab(QWidget):
signalRobotNameChanged = pyqtSignal(int)
def __init__(self, num_robots, robots):
super(RobotTab, self).__init__()
# Available robots from gui_config.yaml
self.robots = robots
# Set robot type by default to ground
self.agent_type = 'ground'
# Number of robots
self.num_robots = num_robots
# Set the robot name by default
self.robot_name = 'robot' + str(self.num_robots)
# Set layout for tab
self.layout = QVBoxLayout()
# Robot tab title
self.robot_label_name = QLabel(('Robot ' + str(self.num_robots)))
font = QFont()
font.setPointSize(14)
font.setBold(True)
self.robot_label_name.setFont(font)
self.layout.addWidget(self.robot_label_name)
# Robot name label and input line
self.robot_label = QLabel('Robot name')
self.layout.addWidget(self.robot_label)
self.robot_name_input = QLineEdit('robot' + str(self.num_robots))
#self.robot_name_input.editingFinished.connect(self.robot_name_changed)
self.layout.addWidget(self.robot_name_input)
# Robot model label and comboBox
self.robot_model_label = QLabel('Robot model')
self.layout.addWidget(self.robot_model_label)
self.robot_comboBox = CustomComboBox(self.num_robots - 1)
self.robot_comboBox.addItems(self.robots['Models'].keys())
self.layout.addWidget(self.robot_comboBox)
self.robot_comboBox.signalIndexChanged.connect(self.set_agent_type)
# Initial pose
self.robot_label_init = QLabel('Initial pose')
self.layout.addWidget(self.robot_label_init)
self.robot_comboBox_init = CustomComboBox(self.num_robots)
self.layout.addWidget(self.robot_comboBox_init)
# Add initial pose text item to graphic
initial_pose_textItem = QGraphicsTextItem(
'start_' + str(self.num_robots).zfill(2))
self.initial_pose = {
'start_' + str(self.num_robots).zfill(2): {
'label': 'r01',
'text_item': initial_pose_textItem
}
}
# Use Qualisys
self.robot_localization_label = QLabel('Localization')
self.layout.addWidget(self.robot_localization_label)
self.robot_localization_checkBox = QCheckBox('Use Qualisys')
self.layout.addWidget(self.robot_localization_checkBox)
# Task specifications
self.robot_label_task_title = QLabel('Task robot ' +
str(self.num_robots))
self.robot_label_task_title.setFont(font)
self.layout.addWidget(self.robot_label_task_title)
self.robot_label_hard_task = QLabel('Hard tasks')
self.layout.addWidget(self.robot_label_hard_task)
self.robot_hard_task_input = QLineEdit('([]<> r01) && ([]<> r02)')
self.layout.addWidget(self.robot_hard_task_input)
self.robot_label_soft_task = QLabel('Soft tasks')
self.layout.addWidget(self.robot_label_soft_task)
self.robot_soft_task_input = QLineEdit()
self.layout.addWidget(self.robot_soft_task_input)
# Plan display
self.robot_label_prefix = QLabel('Planner prefix robot ' +
str(self.num_robots))
self.layout.addWidget(self.robot_label_prefix)
self.robot_prefix_textbox = QTextBrowser()
self.layout.addWidget(self.robot_prefix_textbox)
self.robot_label_sufix = QLabel('Planner sufix robot ' +
str(self.num_robots))
self.layout.addWidget(self.robot_label_sufix)
self.robot_sufix_textbox = QTextBrowser()
self.layout.addWidget(self.robot_sufix_textbox)
# Current goal display
self.robot_label_current_goal = QLabel('Current goal robot ' +
str(self.num_robots))
self.layout.addWidget(self.robot_label_current_goal)
self.robot_current_goal_textbox = QTextBrowser()
self.layout.addWidget(self.robot_current_goal_textbox)
# Clear costmap button
self.robot_resend_goal_button = QPushButton('Clear costmap')
self.layout.addWidget(self.robot_resend_goal_button)
# Temporary task button
self.robot_temporary_task_button = QPushButton('Temporary task')
self.layout.addWidget(self.robot_temporary_task_button)
self.robot_temporary_task_button.clicked.connect(
self.temporary_task_button_pressed)
self.setLayout(self.layout)
# Messages for publishing pose, soft-task, hard-task and clear_costmap
self.init_pose_msg = Pose()
self.soft_task_msg = String()
self.hard_task_msg = String()
self.prefix_string = ''
self.sufix_string = ''
# Marker displaying robots name
self.label_marker_msg = Marker()
self.label_marker_msg.pose = self.init_pose_msg
self.label_marker_msg.pose.position.z = 1.0
self.label_marker_msg.text = self.robot_name
self.label_marker_msg.type = self.label_marker_msg.TEXT_VIEW_FACING
self.label_marker_msg.id = self.num_robots
self.label_marker_msg.action = self.label_marker_msg.ADD
self.label_marker_msg.scale.z = 0.5
self.label_marker_msg.color.a = 1.0
self.label_marker_msg.color.r = 0.0
self.label_marker_msg.color.g = 0.0
self.label_marker_msg.color.b = 0.0
self.label_marker_msg.header.frame_id = '/map'
# Pose msg published for planner
self.pose_msg = PoseWithCovarianceStamped()
# Init pose msg for planner
self.pose_msg.pose.pose = self.init_pose_msg
# Clear costmap and resend current goal
self.robot_resend_goal_button.clicked.connect(
self.call_clear_costmap_srvs)
self.robot_current_goal = MoveBaseActionGoal()
# Start all publisher and Subscriber
#self.start_publisher_and_subscriber()
# AMCL pose callback
def current_pose_amcl_callback(self, msg):
if self.robot_localization_checkBox.checkState() != 2:
self.pose_msg.pose.pose = deepcopy(msg.pose.pose)
self.pose_msg.header = deepcopy(msg.header)
self.pose_msg.header.stamp = rospy.Time.now()
if self.agent_type == 'ground':
self.label_marker_msg.header = msg.header
self.label_marker_msg.pose = deepcopy(msg.pose.pose)
self.label_marker_msg.pose.position.z = deepcopy(
msg.pose.pose.position.z) + 1
# Localization with Qualisys
def current_pose_qualisys_callback(self, msg):
if self.robot_localization_checkBox.checkState() == 2:
self.pose_msg.header = deepcopy(msg.header)
self.pose_msg.pose.pose = deepcopy(msg.pose)
self.pose_msg.header.stamp = rospy.Time.now()
if self.agent_type == 'ground':
self.label_marker_msg.header = msg.header
self.label_marker_msg.pose = msg.pose
self.label_marker_msg.pose.position.z = deepcopy(
msg.pose.position.z) + 1
# Localization with Gazebo ground truth
def current_pose_gazebo_ground_truth_callback(self, msg):
if self.agent_type == 'aerial':
self.label_marker_msg.header = msg.header
self.label_marker_msg.pose = msg.pose.pose
self.label_marker_msg.pose.position.z = deepcopy(
msg.pose.pose.position.z) + 1.0
# Sends topic to planner to clear costmap manual
@Slot(bool)
def call_clear_costmap_srvs(self):
if self.agent_type == 'ground':
bool = Bool()
bool.data = True
self.clear_costmap_publisher.publish(bool)
# Opens dialog for temporary task
@Slot(bool)
def temporary_task_button_pressed(self):
temporary_task_dialog = TemporaryTask_dialog()
temporary_task_dialog.exec_()
if temporary_task_dialog.atomic_propositions:
temporary_task_msg = TemporaryTask()
temporary_task_msg.header.stamp = rospy.Time.now()
for i in range(0, len(temporary_task_dialog.atomic_propositions)):
string_msg = String()
string_msg.data = temporary_task_dialog.atomic_propositions[i]
temporary_task_msg.task.append(string_msg)
temporary_task_msg.T_des.data = temporary_task_dialog.T_des
self.temporary_task_publisher.publish(temporary_task_msg)
# If robot name in tab has changed the subscriber and publisher topics are updated
def robot_name_changed(self):
self.remove_publisher_and_subscriber()
self.robot_name = self.robot_name_input.text()
self.start_publisher_and_subscriber()
self.label_marker_msg.text = self.robot_name
self.signalRobotNameChanged.emit(self.num_robots)
# Start all publisher and subscriber from robot tab
def start_publisher_and_subscriber(self):
self.robot_name = self.robot_name_input.text()
self.ros_publisher = ROS_Publisher()
self.ros_publisher.add_publisher('/' + self.robot_name + '/init_pose',
Pose, 1.0, self.init_pose_msg)
self.ros_publisher.add_publisher('/' + self.robot_name + '/soft_task',
String, 1.0, self.soft_task_msg)
self.ros_publisher.add_publisher('/' + self.robot_name + '/hard_task',
String, 1.0, self.hard_task_msg)
self.ros_publisher.add_publisher(
'/' + self.robot_name + '/label_marker', Marker, 5.0,
self.label_marker_msg)
self.temporary_task_publisher = rospy.Publisher('/' + self.robot_name +
'/temporary_task',
TemporaryTask,
queue_size=1)
self.clear_costmap_publisher = rospy.Publisher('/' + self.robot_name +
'/clear_costmap',
Bool,
queue_size=1)
self.current_pose_amcl_subscriber = rospy.Subscriber(
'/' + self.robot_name + '/amcl_pose', PoseWithCovarianceStamped,
self.current_pose_amcl_callback)
self.current_pose_qualisys_subscriber = rospy.Subscriber(
'/' + self.robot_name + '/qualisys_pose_map', PoseStamped,
self.current_pose_qualisys_callback)
self.current_pose_gazebo_ground_truth_subscriber = rospy.Subscriber(
'/' + self.robot_name + '/ground_truth/pose_with_covariance',
PoseWithCovarianceStamped,
self.current_pose_gazebo_ground_truth_callback)
self.ros_publisher.add_publisher('/' + self.robot_name + '/pose_gui',
PoseWithCovarianceStamped, 15.0,
self.pose_msg)
self.label_marker_msg.text = self.robot_name
# Remove all publisher and subcriber from robot tab
def remove_publisher_and_subscriber(self):
del self.ros_publisher
del self.clear_costmap_publisher
del self.temporary_task_publisher
del self.current_pose_amcl_subscriber
del self.current_pose_qualisys_subscriber
del self.current_pose_gazebo_ground_truth_subscriber
# ADD FUNCTION THAT JUST CHANGES SOFT AND HARD TASK!!!!
# DELETE OLD ONE, AND ADD NEW ONE! pass as argument the planner or previous class such that I call it here
# Updates robot type if model checkbox was changed
def set_agent_type(self):
if self.robot_comboBox.currentText(
) in self.robots['robot_types']['aerial']:
self.agent_type = 'aerial'
else:
self.agent_type = 'ground'
def build_init_pose_msg(self, id):
self.init_pose_msg.position.x = self.initial_pose[
'start_' + str(id).zfill(2)]['pose']['position'][0]
self.init_pose_msg.position.y = self.initial_pose[
'start_' + str(id).zfill(2)]['pose']['position'][1]
self.init_pose_msg.position.z = self.initial_pose[
'start_' + str(id).zfill(2)]['pose']['position'][2]
self.init_pose_msg.orientation.w = self.initial_pose[
'start_' + str(id).zfill(2)]['pose']['orientation'][0]
self.init_pose_msg.orientation.x = self.initial_pose[
'start_' + str(id).zfill(2)]['pose']['orientation'][1]
self.init_pose_msg.orientation.y = self.initial_pose[
'start_' + str(id).zfill(2)]['pose']['orientation'][2]
self.init_pose_msg.orientation.z = self.initial_pose[
'start_' + str(id).zfill(2)]['pose']['orientation'][3]
class LevelSelectorPlugin(Plugin):
button_signal = pyqtSignal()
button_status_signal = pyqtSignal()
def __init__(self, context):
super(LevelSelectorPlugin, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('LevelSelectorPlugin')
# Create QWidget
self._widget = QWidget()
self._widget.setFont(QFont("Times", 15, QFont.Bold))
self._button_layout = QVBoxLayout(self._widget)
self.buttons = []
self.text_label = QLabel("Waiting for MultiLevelMapData...", self._widget)
self._button_layout.addWidget(self.text_label)
self._widget.setObjectName('LevelSelectorPluginUI')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle()
+ (' (%d)' % context.serial_number()))
context.add_widget(self._widget)
self.button_signal.connect(self.update_buttons)
self.button_status_signal.connect(self.update_button_status)
# Subscribe to the multi level map data to get information about all the maps.
self.multimap_subscriber = rospy.Subscriber("map_metadata", MultiLevelMapData,
self.process_multimap)
self.levels = []
self.current_level = None
rospy.loginfo('level selector: subscribed to maps')
# Subscribe to the current level we are on.
self.status_subscriber = None
# Create a service proxy to change the current level.
self.level_selector_proxy = rospy.ServiceProxy("level_mux/change_current_level",
ChangeCurrentLevel)
self.level_selector_proxy.wait_for_service()
rospy.loginfo('level selector: found "level_mux/change_current_level" service')
def process_multimap(self, msg):
""" Map metadata topic callback. """
rospy.loginfo('level selector: map metadata received.')
self.levels = msg.levels
# update level buttons in the selection window
self.button_signal.emit()
def update_buttons(self):
""" Update buttons in Qt window. """
rospy.loginfo('level selector: update_buttons called')
self.clean()
for index, level in enumerate(self.levels):
button = QPushButton(level.level_id, self._widget)
button.clicked[bool].connect(self.handle_button)
button.setCheckable(True)
self._button_layout.addWidget(button)
self.buttons.append(button)
# Subscribe to the current level we are on.
if self.status_subscriber is None:
self.status_subscriber = rospy.Subscriber("level_mux/current_level",
LevelMetaData,
self.process_level_status)
def update_button_status(self):
""" Update button status Qt push button callback. """
rospy.loginfo('level selector: update_button_status')
if not self.buttons or not self.current_level:
rospy.logwarn('level selector: current level not available')
return
rospy.logdebug('buttons: ' + str(self.buttons))
for index, level in enumerate(self.levels):
rospy.logdebug('level[' + str(index) + ']: ' + str(level.level_id))
if self.current_level == level.level_id:
self.buttons[index].setChecked(True)
else:
self.buttons[index].setChecked(False)
def process_level_status(self, msg):
""" level_mux/current_level topic callback. """
rospy.loginfo('level selector: current_level topic message received')
level_found = False
for level in self.levels:
if msg.level_id == level.level_id:
self.current_level = level.level_id
level_found = True
break
if not level_found:
self.current_level = None
self.button_status_signal.emit()
def handle_button(self):
source = self.sender()
if source.text() == self.current_level:
source.setChecked(True)
return
# Construct a identity pose. The level selector cannot be used
# to choose the initialpose, as it does not have the interface
# for specifying the position. The position should be
# specified via rviz.
origin_pose = PoseWithCovarianceStamped()
origin_pose.header.frame_id = frameIdFromLevelId(source.text())
origin_pose.pose.pose.orientation.w = 1 # Makes the origin quaternion valid.
origin_pose.pose.covariance[0] = 1.0
origin_pose.pose.covariance[7] = 1.0
origin_pose.pose.covariance[14] = 1.0
origin_pose.pose.covariance[21] = 1.0
origin_pose.pose.covariance[28] = 1.0
origin_pose.pose.covariance[35] = 1.0
# Don't actually publish the initial pose via the level
# selector. It doesn't know any better.
self.level_selector_proxy(source.text(), False, origin_pose)
def clean(self):
while self._button_layout.count():
item = self._button_layout.takeAt(0)
item.widget().deleteLater()
def save_settings(self, plugin_settings, instance_settings):
pass
def restore_settings(self, plugin_settings, instance_settings):
pass
class PlexilPlanSelectionGUI(Plugin):
#sets up our signal for the sub callback
monitor_signal = pyqtSignal(['QString'])
def __init__(self, context):
'''init initializes the widget and sets up our subscriber, publisher and event handlers'''
super(PlexilPlanSelectionGUI, self).__init__(context)
self.setObjectName('PlexilPlanSelectionGUI')
# Process standalone plugin command-line arguments
parser = ArgumentParser()
parser.add_argument("-q", "--quiet", action="store_true", dest="quiet", help="Put plugin in silent mode")
args, unknowns = parser.parse_known_args(context.argv())
# Find resources and Create QWidget
self._widget = QWidget()
ui_file = os.path.join(rospkg.RosPack().get_path('ow_plexil'), 'rqt_plexil_plan_selection', 'resource', 'plexil_plan_selection.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('PlexilPlanSelectionUI')
if context.serial_number() > 1:
self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
context.add_widget(self._widget)
#set window icon
icon = os.path.join(rospkg.RosPack().get_path('ow_plexil'), 'rqt_plexil_plan_selection', 'resource', 'icon.png')
self._widget.setWindowIcon(QIcon(icon))
#pub and sub setup
self.status_publisher = rospy.Publisher('plexil_plan_selection_status', String, queue_size=20)
self.status_subscriber = rospy.Subscriber('plexil_plan_selection_status', String, self.status_callback)
#client placeholder
self.plan_select_client = None
#Qt signal to modify GUI from callback
self.monitor_signal[str].connect(self.monitor_status)
#populates the plan list, shows different plans based off of what launch file is running
if rospy.get_param('owlat_flag', False):
owlat_plan_dir = os.path.join(rospkg.RosPack().get_path('ow_plexil'), 'src', 'plans', 'owlat_plans')
self.populate_plan_list(owlat_plan_dir)
else:
ow_plan_dir = os.path.join(rospkg.RosPack().get_path('ow_plexil'), 'src', 'plans')
self.populate_plan_list(ow_plan_dir)
#sets up tables
self._widget.sentPlansTable.setSizeAdjustPolicy(QAbstractScrollArea.AdjustToContents)
self._widget.planQueueTable.setSizeAdjustPolicy(QAbstractScrollArea.AdjustToContents)
self._widget.sentPlansTable.insertColumn(0)
self._widget.sentPlansTable.insertColumn(1)
self._widget.planQueueTable.insertColumn(0)
#sets up event listeners
self._widget.addButton.clicked[bool].connect(self.handle_add_button_clicked)
self._widget.removeButton.clicked[bool].connect(self.handle_remove_button_clicked)
self._widget.upButton.clicked[bool].connect(self.handle_up_button_clicked)
self._widget.downButton.clicked[bool].connect(self.handle_down_button_clicked)
self._widget.sendButton.clicked[bool].connect(self.handle_send_button_clicked)
self._widget.resetButton.clicked[bool].connect(self.handle_reset_button_clicked)
def populate_plan_list(self, plan_dir):
'''Finds all .ple plexil plans in the plans directory and stores them in the widget list.'''
plan_list = []
#get all plans with extension .ple
for p in os.listdir(plan_dir):
if p.endswith(".ple"):
plan_list.append(p.rsplit(".")[0])
#add to list
self._widget.planList.addItems(plan_list)
self._widget.planList.sortItems()
def monitor_status(self, feedback):
'''Signal from callback calls this function to do the work to avoid threading issued with the GUI,
changes the status on the sent plans table to match the current plan status. Plan can be Pending,
Running, Finished or Failed.'''
num_rows = self._widget.sentPlansTable.rowCount()
#if completed and previously running we set status as finished
if feedback == "COMPLETE":
for i in range(num_rows):
current_status = self._widget.sentPlansTable.item(i,1).text()
if current_status == "Running...":
self._widget.sentPlansTable.item(i, 1).setText("Finished")
self._widget.sentPlansTable.item(i, 1).setBackground(QColor(0,128,0))
break
else:
#splitting into plan name and status
status = feedback.rsplit(":")[0]
running_plan = feedback.rsplit(":")[1].rsplit(".")[0]
#we set status to running or Failed depending on status
for i in range(num_rows):
plan_name = self._widget.sentPlansTable.item(i,0).text()
current_status = self._widget.sentPlansTable.item(i,1).text()
if plan_name == running_plan and current_status == "Pending...":
if status == "SUCCESS":
self._widget.sentPlansTable.item(i, 1).setText("Running...")
break
else:
self._widget.sentPlansTable.item(i, 1).setText("Failed")
self._widget.sentPlansTable.item(i, 1).setBackground(QColor(230,38,0))
break
return
def status_callback(self, msg):
'''Callback from status subscriber. Sends the msg to the function monitor_signal for further
processing in order to prevent threading issues in the GUI.'''
#have to use a signal here or else GUI wont update
feedback_string = str(msg.data)
self.monitor_signal.emit(feedback_string)
def handle_add_button_clicked(self, checked):
'''When the add button is clicked this function moves any selected items to the plan queue table.'''
#get selected items
selected_plans = self._widget.planList.selectedItems()
#create a new row in the queue table for each selection and insert it
for i in selected_plans:
row_position = self._widget.planQueueTable.rowCount()
self._widget.planQueueTable.insertRow(row_position)
self._widget.planQueueTable.setItem(row_position-1, 1, QTableWidgetItem(i.text()))
self._widget.planList.selectionModel().clear()
self._widget.planQueueTable.resizeColumnsToContents()
return
def handle_remove_button_clicked(self, checked):
'''When the remove button is clicked this function deletes any selected items from the plan queue table.'''
selected_rows = self._widget.planQueueTable.selectedItems()
for i in selected_rows:
self._widget.planQueueTable.removeRow(self._widget.planQueueTable.row(i))
self._widget.planQueueTable.selectionModel().clear()
return
def handle_up_button_clicked(self, checked):
'''When up button is clicked this function moves the selected item up in the queue table.'''
selected_row = self._widget.planQueueTable.currentRow()
#checks we are not at top of list
if selected_row <= 0:
return
#switches the two rows and puts selection on previously selected row
else:
belowTxt = self._widget.planQueueTable.item(selected_row,0).text()
aboveTxt = self._widget.planQueueTable.item(selected_row-1,0).text()
self._widget.planQueueTable.item(selected_row, 0).setText(aboveTxt)
self._widget.planQueueTable.item(selected_row-1, 0).setText(belowTxt)
self._widget.planQueueTable.selectionModel().clear()
self._widget.planQueueTable.setCurrentItem(self._widget.planQueueTable.item(selected_row-1,0))
return
def handle_down_button_clicked(self, checked):
'''When down button is clicked this function moves the selected item down in the queue table.'''
selected_row = self._widget.planQueueTable.currentRow()
num_rows = self._widget.planQueueTable.rowCount()
#checks we are not at bottom of list
if selected_row >= num_rows-1 or selected_row < 0:
return
#switches the two rows and puts selection on previously selected row
else:
belowTxt = self._widget.planQueueTable.item(selected_row+1,0).text()
aboveTxt = self._widget.planQueueTable.item(selected_row,0).text()
self._widget.planQueueTable.item(selected_row+1, 0).setText(aboveTxt)
self._widget.planQueueTable.item(selected_row, 0).setText(belowTxt)
self._widget.planQueueTable.selectionModel().clear()
self._widget.planQueueTable.setCurrentItem(self._widget.planQueueTable.item(selected_row+1,0))
return
def handle_send_button_clicked(self, checked):
'''When send button is clicked any items in the plan queue table are sent (in order) to the sent plans table.
It then publishes a string array of these plans so that the ow_plexil_plan_selection node can run them
sequentially. If the subscriber is not connected a popup box reminding the user to run the ow_plexil node will show up.'''
if self.check_client_set_up() == False:
return
num_rows = self._widget.planQueueTable.rowCount()
plans_sent = []
#creates a new row in the sent table for each item in the queue table and inserts it
for i in range(num_rows):
plan_name = self._widget.planQueueTable.item(0,0).text()
plans_sent.append(str(plan_name + ".plx"))
self._widget.planQueueTable.removeRow(0)
row_position = self._widget.sentPlansTable.rowCount()
self._widget.sentPlansTable.insertRow(row_position)
self._widget.sentPlansTable.setItem(row_position, 0, QTableWidgetItem(plan_name))
self._widget.sentPlansTable.setItem(row_position, 1, QTableWidgetItem("Pending..."))
self._widget.sentPlansTable.resizeColumnsToContents()
# Create msg and send to subscriber for plan execution
self.plan_select_client("ADD", plans_sent)
return
def handle_reset_button_clicked(self, checked):
'''When reset button is clicked all plans in the sent plans table are deleted. It also publishes a string command
RESET letting the ow_plexil_plan_selection node know that any plans in its queue should also be deleted.'''
if self.check_client_set_up() == False:
return
num_rows = self._widget.sentPlansTable.rowCount()
#deletes each row pressent in sentplanstable
for i in range(num_rows):
self._widget.sentPlansTable.removeRow(0)
self.plan_select_client("RESET", [])
return
def check_client_set_up(self):
'''Checks to see if the client is initialized, if not we check if the server is running before initializing the client.
If server is not yet running we send a popup informing the user and return False.'''
#checks to see if we have connected to service yet
if self.plan_select_client == None:
#we get list of services and see if any match plexil_plan_selection
services = rosservice.get_service_list()
service_running = [i for i in services if "plexil_plan_selection" in i]
#if none exist we send a popup and return
if len(service_running) == 0:
popup = QMessageBox()
popup.setWindowTitle("ow_plexil service not yet connected")
popup.setText("ow_plexil plan selection service not connected yet, please make sure the ow_plexil launch file is running.")
popup.exec_()
return False
else:
#client setup
rospy.wait_for_service('plexil_plan_selection')
self.plan_select_client = rospy.ServiceProxy('plexil_plan_selection', PlanSelection)
return True
else:
return True
def shutdown_plugin(self):
'''handles shutdown procedures for the plugin, unregisters the publisher and subscriber.'''
self.status_subscriber.unregister()
pass
class MetricsRefboxWidget(QWidget):
status_signal = pyqtSignal(object)
timeout_signal = pyqtSignal(object)
result_signal = pyqtSignal(object)
bagfile_signal = pyqtSignal(object)
def __init__(self):
super(MetricsRefboxWidget, self).__init__()
self.status_signal.connect(self.update_status)
self.timeout_signal.connect(self._handle_timeout)
self.result_signal.connect(self.update_result)
self.bagfile_signal.connect(self._update_bagfile_name)
self.timer = QElapsedTimer()
self.timer_interrupt = QTimer(self)
self.timer_interrupt.timeout.connect(self.update_timer)
self.timer_interrupt.start(100)
self.metrics_refbox = metrics_refbox.MetricsRefbox(
self._status_cb, self._start_cb, self._result_cb)
self.trial_configs = {}
self.current_benchmark = None
self.current_benchmark_enum = -1
# once we receive confirmation from robot
self.benchmark_running = False
# once we send the start message to robot
self.benchmark_started = False
# set to True if we're not recording individual trials
# but rather continuously recording all data (hence no timeouts)
self.continuous_recording = False
self.timeout = False
self.stopped = False
self.result_msg = None
self.config_locked = True
self.config = self.metrics_refbox.get_config()
self.setup()
self.show()
def setup(self):
layout = QGridLayout()
# Sidebar
self.benchmark_combo_box = QComboBox()
for key in self.config['benchmarks'].keys():
self.benchmark_combo_box.addItem(
self.config['benchmarks'][key]['name'])
self.benchmark_combo_box.currentIndexChanged.connect(
self._handle_benchmark_selected)
self.benchmark_combo_box.setMaximumWidth(SIDEBAR_WIDTH)
self.set_current_benchmark()
self.team_combo_box = QComboBox()
for key in self.config['teams']:
self.team_combo_box.addItem(key)
self.team_combo_box.setMaximumWidth(SIDEBAR_WIDTH)
self.test_communication_button = QPushButton('Test communication')
self.test_communication_button.clicked.connect(self._handle_test_comm)
self.trial_list_widget = QListWidget()
self.trial_list_widget.currentItemChanged.connect(
self._handle_trial_change)
self.trial_list_widget.setMaximumWidth(SIDEBAR_WIDTH)
sidebar_layout = QVBoxLayout()
sidebar_layout.addWidget(QLabel("Team"))
sidebar_layout.addWidget(self.team_combo_box)
sidebar_layout.addWidget(self.test_communication_button)
sidebar_layout.addWidget(QLabel("Benchmark"))
sidebar_layout.addWidget(self.benchmark_combo_box)
sidebar_layout.addWidget(QLabel("Trial"))
sidebar_layout.addWidget(self.trial_list_widget)
self.generate_button = QPushButton('Generate')
self.generate_button.clicked.connect(self._handle_generate)
self.delete_button = QPushButton('Delete')
self.delete_button.clicked.connect(self._handle_delete)
self.save_trials_button = QPushButton('Save')
self.save_trials_button.clicked.connect(self._handle_save_trial_config)
self.lock_button = QPushButton('Lock')
if self.config_locked:
self.lock_button.setText('Unlock')
self.lock_button.clicked.connect(self._handle_lock)
sidebar_button_layout = QGridLayout()
sidebar_button_layout.addWidget(self.generate_button, 0, 0)
sidebar_button_layout.addWidget(self.delete_button, 0, 1)
sidebar_button_layout.addWidget(self.save_trials_button, 1, 0)
sidebar_button_layout.addWidget(self.lock_button, 1, 1)
sidebar_layout.addLayout(sidebar_button_layout)
layout.addLayout(sidebar_layout, 0, 0)
# Status box
self.status = QPlainTextEdit()
self.status.setReadOnly(True)
self.status.setMaximumHeight(200)
# row 1, col 0, rowspan 1, colspan 2
layout.addWidget(self.status, 1, 0, 1, 2)
# trial config and results
trial_layout = QVBoxLayout()
self.trial_config_layout = QHBoxLayout()
self.trial_results_layout = QVBoxLayout()
self.setup_trial_config()
# benchmark trial controls
benchmark_controls_group_box = QGroupBox('Controls')
benchmark_controls_layout = QHBoxLayout()
self.start_trial_button = QPushButton('Start')
self.start_trial_button.clicked.connect(self._handle_start_trial)
self.stop_trial_button = QPushButton('Stop')
self.stop_trial_button.clicked.connect(self._handle_stop_trial)
self.prev_trial_button = QPushButton('Previous')
self.prev_trial_button.clicked.connect(self._handle_prev_trial)
self.next_trial_button = QPushButton('Next')
self.next_trial_button.clicked.connect(self._handle_next_trial)
self.start_continuous_recording_button = QPushButton(
'Start continuous recording')
self.start_continuous_recording_button.clicked.connect(
self._handle_continuous_recording)
self.timer_field = QLabel()
font = QFont("Arial", 20, QFont.Bold)
self.timer_field.setFont(font)
self.timer_field.setAutoFillBackground(True)
benchmark_controls_layout.addWidget(self.start_trial_button)
benchmark_controls_layout.addWidget(self.stop_trial_button)
benchmark_controls_layout.addWidget(self.prev_trial_button)
benchmark_controls_layout.addWidget(self.next_trial_button)
benchmark_controls_layout.addWidget(
self.start_continuous_recording_button)
benchmark_controls_layout.addWidget(self.timer_field)
benchmark_controls_group_box.setLayout(benchmark_controls_layout)
trial_layout.addLayout(self.trial_config_layout)
trial_layout.addWidget(benchmark_controls_group_box)
trial_layout.addLayout(self.trial_results_layout)
self.save_results_button = QPushButton('Save results')
self.save_results_button.clicked.connect(self._handle_save_result)
trial_layout.addWidget(self.save_results_button)
layout.addLayout(trial_layout, 0, 1)
self.setLayout(layout)
self.show()
self.show_env_var('ROS_MASTER_URI')
self.show_env_var('ROS_IP')
self.show_env_var('ROS_HOSTNAME')
def show_env_var(self, var):
env_str = os.getenv(var) if os.getenv(var) is not None else ''
msg = var + ': ' + env_str + '\n'
self.update_status(msg)
def set_current_benchmark(self):
'''
Sets the current benchmark type based on user selection
Load config file for selected benchmark and setup GUI to show config and results based on that
'''
benchmark_index = self.benchmark_combo_box.currentIndex()
benchmark_name = list(
self.config['benchmarks'].keys())[benchmark_index]
benchmark_result_type = getattr(
metrics_refbox_msgs.msg,
self.config['benchmarks'][benchmark_name]['type'])
config_file_name = benchmark_name + '.json'
config_file = os.path.join(self.metrics_refbox.get_config_file_path(),
config_file_name)
stream = open(config_file, 'r')
benchmark_config = json.load(stream)
module = importlib.import_module(benchmark_config['module'])
self.current_benchmark = getattr(module, benchmark_config['class'])(
benchmark_config, self.metrics_refbox.get_config_file_path(),
benchmark_name, benchmark_result_type)
self.current_benchmark_enum = self.config['benchmarks'][
benchmark_name]['enum']
def setup_trial_config(self):
'''
set up the benchmark specific part of the GUI related to the configuration of
the benchmark (for example, which objects will be used for object detection)
the specific configuration is loaded based on the selected trial_index
'''
for i in reversed(range(self.trial_config_layout.count())):
self.trial_config_layout.itemAt(i).widget().setParent(None)
for i in reversed(range(self.trial_results_layout.count())):
self.trial_results_layout.itemAt(i).widget().setParent(None)
self.current_benchmark.setup(self.trial_config_layout,
self.trial_results_layout,
self.config_locked)
self.trial_list_widget.clear()
self.trial_configs.clear()
path = os.path.join(self.metrics_refbox.get_config_file_path(),
'trials')
trial_config_file = os.path.join(
path, self.current_benchmark.config['trial_config'])
if os.path.exists(trial_config_file):
with open(trial_config_file, "r") as fp:
trial_config = json.load(fp)
for key in sorted(trial_config.keys()):
trial_item = QListWidgetItem(key)
trial_item.setFlags(trial_item.flags() | Qt.ItemIsEditable)
self.trial_list_widget.addItem(trial_item)
self.trial_configs[key] = trial_config[key]
self.trial_list_widget.setCurrentRow(0)
def _handle_test_comm(self):
'''
'Test communication' button
'''
self.metrics_refbox.test_communication()
self.status_signal.emit('Sent test message to all clients\n')
def _handle_benchmark_selected(self, idx):
'''
benchmark selection has changed
'''
self._handle_save_trial_config(None)
self.set_current_benchmark()
self.setup_trial_config()
def _handle_trial_change(self, current_item, previous_item):
'''
Trial selection has changed
'''
if previous_item is not None:
self.trial_configs[previous_item.text(
)] = self.current_benchmark.get_current_selections()
self.current_benchmark.clear_results()
if current_item is not None and current_item.text(
) in self.trial_configs.keys() and self.trial_configs[
current_item.text()] is not None:
self.current_benchmark.apply_selections(
self.trial_configs[current_item.text()])
else:
self.current_benchmark.clear_selections()
def _handle_generate(self, button):
'''
generate a new trial config
'''
trial_config = self.current_benchmark.generate()
names = []
for key in self.trial_configs.keys():
names.append(key)
msg = 'Select a name for this trial configuration'
text = ''
while True:
text, ok = QInputDialog.getText(self, 'Trial name', msg)
if text not in names:
break
QMessageBox.critical(self, "Error", "Name exists already")
if ok:
trial_item = QListWidgetItem(text)
trial_item.setFlags(trial_item.flags() | Qt.ItemIsEditable)
self.trial_list_widget.addItem(trial_item)
self.trial_configs[text] = trial_config
self.trial_list_widget.setCurrentItem(trial_item)
def _handle_delete(self, button):
'''
delete current trial config
'''
selected_items = self.trial_list_widget.selectedItems()
for item in selected_items:
self.trial_list_widget.takeItem(self.trial_list_widget.row(item))
del self.trial_configs[item.text()]
def _handle_save_trial_config(self, button):
'''
save trial config in case it has been edited
'''
if self.trial_list_widget.currentItem() is not None:
self.trial_configs[self.trial_list_widget.currentItem().text(
)] = self.current_benchmark.get_current_selections()
path = os.path.join(self.metrics_refbox.get_config_file_path(),
'trials')
trial_config_file = os.path.join(
path, self.current_benchmark.config['trial_config'])
with open(trial_config_file, "w") as fp:
json.dump(self.trial_configs, fp)
def _handle_lock(self, button):
'''
Lock trial config settings so they are not changed accidentally
'''
if self.config_locked:
self.current_benchmark.unlock_config()
self.config_locked = False
self.lock_button.setText('Lock')
else:
self.current_benchmark.lock_config()
self.config_locked = True
self.lock_button.setText('Unlock')
def _handle_start_trial(self, button):
'''
Trial start button; send command to refbox client via ROS node
'''
if self.benchmark_running or self.benchmark_started:
self.update_status(
"Benchmark has already started. Stop and start the benchmark if you want to restart it\n"
)
else:
self.metrics_refbox.start(
self.current_benchmark_enum,
self.current_benchmark.get_task_info_for_robot())
self.benchmark_started = True
self.current_benchmark.clear_results()
self.update_status("Sent start command\n")
self.timer_field.setText('')
self.disable_buttons()
def _handle_stop_trial(self, button):
'''
Trial stop button
'''
self.metrics_refbox.stop()
self.elapsed_time = self.timer.elapsed()
self.stopped = True
self.benchmark_running = False
self.benchmark_started = False
self.status_signal.emit("Sent stop command\n")
self.result_signal.emit(None)
self.enable_buttons()
def _handle_next_trial(self, button):
'''
Select next trial
'''
row = self.trial_list_widget.currentRow()
if (row + 1 < self.trial_list_widget.count()):
self.trial_list_widget.setCurrentRow(row + 1)
def _handle_prev_trial(self, button):
'''
Select previous trial
'''
row = self.trial_list_widget.currentRow()
if (row - 1 >= 0):
self.trial_list_widget.setCurrentRow(row - 1)
def _handle_continuous_recording(self, button):
'''
Start recording button, not tied to any benchmark, and requires user to stop recording
'''
if self.benchmark_running or self.benchmark_started:
self.metrics_refbox.stop()
self.elapsed_time = self.timer.elapsed()
self.stopped = True
self.benchmark_running = False
self.benchmark_started = False
self.status_signal.emit("Sent stop command\n")
self.start_continuous_recording_button.setText(
'Start continuous recording')
self.enable_buttons()
self.stop_trial_button.setEnabled(True)
self.continuous_recording = False
else:
self.metrics_refbox.start_recording()
self.continuous_recording = True
self.start_continuous_recording_button.setText('Stop recording')
self.benchmark_started = True
self.update_status("Sent start command\n")
self.timer_field.setText('')
self.disable_buttons()
self.stop_trial_button.setEnabled(False)
def _handle_save_result(self, button):
'''
Save results again in case notes were added
'''
self.result_signal.emit(self.result_msg)
def _start_cb(self, msg):
'''
called when we get confirmation that the robot received the start message;
official start of trial time
'''
if msg.data == True:
self.elapsed_time = 0.0
self.timer.start()
self.benchmark_running = True
self.timeout = False
self.stopped = False
self.result_msg = None
if msg.rosbag_filename == '':
self.status_signal.emit(
'Error: rosbag filename not specified although start message confirmed\n'
)
self._handle_stop_trial(None)
return
self.status_signal.emit('Started trial and recording to %s\n' %
msg.rosbag_filename)
self.bagfile_signal.emit(msg.rosbag_filename)
else:
self.status_signal.emit(
'Error: Benchmark did not start, probably because the rosbag recorder is not running\n'
)
self._handle_stop_trial(None)
def _result_cb(self, msg):
'''
called when we get a result from the robot
official end of trial time
'''
if self.benchmark_running:
self.elapsed_time = self.timer.elapsed()
self.benchmark_running = False
self.benchmark_started = False
self.status_signal.emit("Trial completed in %.2f seconds\n" %
(self.elapsed_time / 1000.0))
self.result_msg = msg
self.result_signal.emit(msg)
self.enable_buttons()
def _status_cb(self, msg):
'''
callback to send text to status box
'''
self.status_signal.emit(msg)
def update_status(self, msg):
'''
signal handler for status box
'''
timestamp = datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S')
m = '[' + timestamp + '] ' + msg
self.status.insertPlainText(m)
self.status.ensureCursorVisible()
def _update_bagfile_name(self, name):
self.current_benchmark.set_bagfile_name(name)
def update_result(self, msg):
'''
signal handler for result message; process and save result
'''
self.current_benchmark.show_results(msg, self.timeout, self.stopped)
current_trial_name = self.trial_list_widget.currentItem().text()
current_team_name = self.team_combo_box.currentText()
results_dict = self.current_benchmark.get_trial_result_dict(
msg, current_trial_name, current_team_name, self.timeout,
self.stopped, self.elapsed_time / 1000.0)
filename = self.current_benchmark.get_bagfile_name(
)[:-4] + '_' + self.current_benchmark.benchmark_name + '.json'
path = os.path.join(self.metrics_refbox.get_results_file_path(),
filename)
with open(path, "w") as fp:
json.dump(results_dict, fp)
def update_timer(self):
'''
Timer callback; update GUI, and also check if timeout has expired
'''
if not self.benchmark_running:
return
self.timer_field.setText("Time: %.1f s" %
(self.timer.elapsed() / 1000.0))
# if we're in continuous recording mode, no need to check timeout
if self.continuous_recording:
return
if self.timer.hasExpired(self.current_benchmark.get_timeout() *
1000.0):
self.status_signal.emit(
"Trial timeout of %.2f seconds has expired\n" %
self.current_benchmark.get_timeout())
self.timeout_signal.emit('timeout expired')
def _handle_timeout(self, msg):
'''
timeout has expired, so stop the trial
'''
self.timeout = True
self._handle_stop_trial(None)
def closeEvent(self, event):
'''
make sure we save trial configs when window is closed
'''
self.metrics_refbox.stop()
self._handle_save_trial_config(None)
event.accept()
def disable_buttons(self):
'''
disable buttons when trial is running
'''
self.team_combo_box.setEnabled(False)
self.benchmark_combo_box.setEnabled(False)
self.trial_list_widget.setEnabled(False)
self.next_trial_button.setEnabled(False)
self.prev_trial_button.setEnabled(False)
self.delete_button.setEnabled(False)
self.generate_button.setEnabled(False)
self.save_trials_button.setEnabled(False)
self.lock_button.setEnabled(False)
self.start_trial_button.setEnabled(False)
def enable_buttons(self):
self.team_combo_box.setEnabled(True)
self.benchmark_combo_box.setEnabled(True)
self.trial_list_widget.setEnabled(True)
self.next_trial_button.setEnabled(True)
self.prev_trial_button.setEnabled(True)
self.delete_button.setEnabled(True)
self.generate_button.setEnabled(True)
self.save_trials_button.setEnabled(True)
self.lock_button.setEnabled(True)
self.start_trial_button.setEnabled(True)
