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)