def _handle_save_clicked(self, checked):
     filename = QFileDialog.getSaveFileName(self, 'Save to File', '.', self.tr('rqt_console msg file {.csv} (*.csv)'))
     if filename[0] != '':
         filename = filename[0]
         if filename[-4:] != '.csv':
             filename += '.csv'
         try:
             handle = open(filename, 'w')
         except IOError as e:
             qWarning(str(e))
             return
         try:
             handle.write(';'.join(MessageDataModel.columns) + '\n')
             for index in range(self._proxy_model.rowCount()):
                 row = self._proxy_model.mapToSource(self._proxy_model.index(index, 0)).row()
                 msg = self._model._messages[row]
                 data = {}
                 data['message'] = msg.message.replace('"', '\\"')
                 data['severity'] = str(msg.severity)
                 data['node'] = msg.node
                 data['stamp'] = str(msg.stamp[0]) + '.' + str(msg.stamp[1]).zfill(9)
                 data['topics'] = ','.join(msg.topics)
                 data['location'] = msg.location
                 line = []
                 for column in MessageDataModel.columns:
                     line.append('"%s"' % data[column])
                 handle.write(';'.join(line) + '\n')
         except Exception as e:
             qWarning('File save failed: %s' % str(e))
             return False
         finally:
             handle.close()
         return True

    def send_logger_change_message(self, node, logger, level):
        """
        Sends a logger level change request to 'node'.
        :param node: name of the node to chaange, ''str''
        :param logger: name of the logger to change, ''str''
        :param level: name of the level to change, ''str''
        :returns: True if the response is valid, ''bool''
        :returns: False if the request raises an exception or would not change the cached state, ''bool''
        """
        servicename = node + '/set_logger_level'
        if self._current_levels[logger].lower() == level.lower():
            return False

        service = rosservice.get_service_class_by_name(servicename)
        request = service._request_class()
        setattr(request, 'logger', logger)
        setattr(request, 'level', level)
        proxy = rospy.ServiceProxy(str(servicename), service)
        try:
            proxy(request)
            self._current_levels[logger] = level.upper()
        except rospy.ServiceException as e:
            qWarning('SetupDialog.level_changed(): request:\n%r' % (request))
            qWarning('SetupDialog.level_changed() "%s":\n%s' % (servicename, e))
            return False
        return True

 def _add_dock_widget_to_main_window(self, dock_widget):
     if self._main_window is not None:
         # warn about dock_widget with same object name
         old_dock_widget = self._main_window.findChild(DockWidget, dock_widget.objectName())
         if old_dock_widget is not None:
             qWarning('PluginHandler._add_dock_widget_to_main_window() duplicate object name "%s", assign unique object names before adding widgets!' % dock_widget.objectName())
         self._main_window.addDockWidget(Qt.BottomDockWidgetArea, dock_widget)

 def gripper_click(self, button_name):
     grip_side = ''
     grip_side_text = ''
     
     if ('Left' in button_name):
         grip_side = 'l'
         grip_side_text = 'left'
     else:
         grip_side = 'r'
         grip_side_text = 'right'
         
     if ('Open' in button_name):
         grip_action = 20.0
         grip_action_text = 'close'
         qWarning('Robot opened %s gripper' % (grip_side_text))
     else:
         grip_action = 0.0
         grip_action_text = 'open'
         qWarning('Robot closed %s gripper' % (grip_side_text))
         
     
     self.show_text_in_rviz("%sing %s Gripper" % (grip_action_text.capitalize(), grip_side_text.capitalize()))
     self.gripper_action(grip_side, grip_action)
     
     self._widget.sender().setText('%s %s Gripper' % (grip_action_text.capitalize(), grip_side_text.capitalize()))

    def dragEnterEvent(self, event):
        if event.mimeData().hasText():
            topic_name = str(event.mimeData().text())
            if len(topic_name) == 0:
                qWarning('PoseViewWidget.dragEnterEvent(): event.mimeData() text is empty')
                return
        else:
            if not hasattr(event.source(), 'selectedItems') or len(event.source().selectedItems()) == 0:
                qWarning('PoseViewWidget.dragEnterEvent(): event.source() has no attribute selectedItems or length of selectedItems is 0')
                return
            item = event.source().selectedItems()[0]
            topic_name = item.data(0, Qt.UserRole)

            if topic_name is None:
                qWarning('PoseViewWidget.dragEnterEvent(): selectedItem has no UserRole data with a topic name')
                return

        # check for valid topic
        msg_class, self._topic_name, _ = get_topic_class(topic_name)
        if msg_class is None:
            qWarning('PoseViewWidget.dragEnterEvent(): No message class was found for topic "%s".' % topic_name)
            return

        # check for valid message class
        quaternion_slot_path, point_slot_path = self._get_slot_paths(msg_class)

        if quaternion_slot_path is None and point_slot_path is None:
            qWarning('PoseViewWidget.dragEnterEvent(): No Pose, Quaternion or Point data was found outside of arrays in "%s" on topic "%s".'
                     % (msg_class._type, topic_name))
            return

        event.acceptProposedAction()

    def load_bag(self, filename):
        qWarning("Loading %s" % filename)

        # QProgressBar can EITHER: show text or show a bouncing loading bar,
        #  but apparently the text is hidden when the bounding loading bar is
        #  shown
        #self.progress_bar.setRange(0, 0)
        self.set_status_text.emit("Loading %s" % filename)
        #progress_format = self.progress_bar.format()
        #progress_text_visible = self.progress_bar.isTextVisible()
        #self.progress_bar.setFormat("Loading %s" % filename)
        #self.progress_bar.setTextVisible(True)

        bag = rosbag.Bag(filename)
        self.play_button.setEnabled(True)
        self.thumbs_button.setEnabled(True)
        self.zoom_in_button.setEnabled(True)
        self.zoom_out_button.setEnabled(True)
        self.zoom_all_button.setEnabled(True)
        self.faster_button.setEnabled(True)
        self.slower_button.setEnabled(True)
        self.begin_button.setEnabled(True)
        self.end_button.setEnabled(True)
        self.save_button.setEnabled(True)
        self.record_button.setEnabled(False)
        self._timeline.add_bag(bag)
        qWarning("Done loading %s" % filename )
        # put the progress bar back the way it was
        self.set_status_text.emit("")

    def add_widget(self, widget):
        if widget in self._embed_widgets:
            qWarning('PluginHandlerXEmbedClient.add_widget() widget "%s" already added' % widget.objectName())
            return
        embed_widget = QX11EmbedWidget()
        layout = QVBoxLayout()
        layout.setContentsMargins(0, 0, 0, 0)
        layout.addWidget(widget)
        embed_widget.setLayout(layout)

        # close embed widget when container is closed
        # TODO necessary?
        #embed_widget.containerClosed.connect(embed_widget.close)

        embed_container_window_id = self._remote_container.embed_widget(os.getpid(), widget.objectName())
        embed_widget.embedInto(embed_container_window_id)

        signaler = WindowChangedSignaler(widget, widget)
        signaler.window_icon_changed_signal.connect(self._on_embed_widget_icon_changed)
        signaler.window_title_changed_signal.connect(self._on_embed_widget_title_changed)
        self._embed_widgets[widget] = embed_widget, signaler
        # trigger to update initial window icon and title
        signaler.window_icon_changed_signal.emit(widget)
        signaler.window_title_changed_signal.emit(widget)

        embed_widget.show()

    def _evaluate_expression(self, expression, slot_type):
        successful_eval = True

        try:
            # try to evaluate expression
            value = eval(expression, {}, self._eval_locals)
        except Exception:
            successful_eval = False

        if slot_type is str:
            if successful_eval:
                value = str(value)
            else:
                # for string slots just convert the expression to str, if it did not evaluate successfully
                value = str(expression)
            successful_eval = True

        elif successful_eval:
            type_set = set((slot_type, type(value)))
            # check if value's type and slot_type belong to the same type group, i.e. array types, numeric types
            # and if they do, make sure values's type is converted to the exact slot_type
            if type_set <= set((list, tuple)) or type_set <= set((int, float)):
                # convert to the right type
                value = slot_type(value)

        if successful_eval and isinstance(value, slot_type):
            return True, value
        else:
            qWarning('Publisher._evaluate_expression(): failed to evaluate expression: "%s" as Python type "%s"' % (expression, slot_type.__name__))

        return False, None

    def _evaluate_expression(self, expression, slot_type):
        successful_eval = True
        successful_conversion = True

        try:
            # try to evaluate expression
            value = eval(expression, {}, self._eval_locals)
        except Exception:
            # just use expression-string as value
            value = expression
            successful_eval = False

        try:
            # try to convert value to right type
            value = slot_type(value)
        except Exception:
            successful_conversion = False

        if successful_conversion:
            return value
        elif successful_eval:
            qWarning('ServiceCaller.fill_message_slots(): can not convert expression to slot type: %s -> %s' % (type(value), slot_type))
        else:
            qWarning('ServiceCaller.fill_message_slots(): failed to evaluate expression: %s' % (expression))

        return None

 def get_ylim(self):
     """get Y limits"""
     if self._data_plot_widget:
         return self._data_plot_widget.get_ylim()
     else:
         qWarning("No plot widget; returning default Y limits")
         return [0.0, 10.0]

    def _message_recorded(self, topic, msg, t):
        if self._timeline_frame._start_stamp is None:
            self._timeline_frame._start_stamp = t
            self._timeline_frame._end_stamp = t
            self._timeline_frame._playhead = t
        elif self._timeline_frame._end_stamp is None or t > self._timeline_frame._end_stamp:
            self._timeline_frame._end_stamp = t

        if not self._timeline_frame.topics or topic not in self._timeline_frame.topics:
            self._timeline_frame.topics = self._get_topics()
            self._timeline_frame._topics_by_datatype = self._get_topics_by_datatype()

            self._playhead_positions_cvs[topic] = threading.Condition()
            self._messages_cvs[topic] = threading.Condition()
            self._message_loaders[topic] = MessageLoaderThread(self, topic)

        if self._timeline_frame._stamp_left is None:
            self.reset_zoom()

        # Notify the index caching thread that it has work to do
        with self._timeline_frame.index_cache_cv:
            self._timeline_frame.invalidated_caches.add(topic)
            self._timeline_frame.index_cache_cv.notify()

        if topic in self._listeners:
            for listener in self._listeners[topic]:
                try:
                    listener.timeline_changed()
                except Exception as ex:
                    qWarning('Error calling timeline_changed on %s: %s' % (type(listener), str(ex)))

    def on_pushButton_calibrate_clicked(self):

        self._widget.pushButton_calibrate.setEnabled(False)

        status_icon = QIcon.fromTheme('stock_no')

        # Fill request information
        request = self._selected_service['service_class']._request_class()
        request.target_hue.data = self._widget.spinBox_hue.value()
        request.mult_diffs.data = self._widget.spinBox_diff.value()
        request.mult_sat.data = self._widget.spinBox_sat.value()
        try:
            # Call service
            response = self._selected_service['service_proxy'](request)
        except rospy.ServiceException as e:
            qWarning('on_pushButton_calibrate_clicked(): error calling service "%s":\n%s' % (self._selected_service['service_name'], e))
        else:
            # Get debug image and show
            img = response.image_debug
            qImage = QImage(img.data, img.width, img.height, img.step, QImage.Format_RGB888)
            qPixmap = QPixmap()
            qPixmap.convertFromImage(qImage)
            pixmapItem = QGraphicsPixmapItem(qPixmap)
            self._qGraphicsScene.clear()
            self._qGraphicsScene.addItem(pixmapItem)

            self._widget.graphicsView.fitInView(QRectF(qPixmap.rect()), Qt.KeepAspectRatio)

            if response.success.data == True:
                status_icon = QIcon.fromTheme('stock_yes')

        self._widget.label_status.setPixmap(status_icon.pixmap(status_icon.actualSize(QSize(24, 24))))

        self._widget.pushButton_calibrate.setEnabled(True)

 def command_cb(self):
     clickedButtonName = self._widget.sender().text()
     for key in self.commands.keys():
         if (self.commands[key] == clickedButtonName):
             qWarning('Sending speech command: '+ key)
             command = Command()
             command.command = key
             self.speech_cmd_publisher.publish(command)

 def update_topics(self):
     self.model().clear()
     topic_list = rospy.get_published_topics()
     for topic_path, topic_type in topic_list:
         topic_name = topic_path.strip('/')
         message_class = roslib.message.get_message_class(topic_type)
         if message_class is None:
             qWarning('TopicCompleter.update_topics(): could not get message class for topic type "%s" on topic "%s"' % (topic_type, topic_path))
             continue
         message_instance = message_class()
         self.model().add_message(message_instance, topic_name, topic_type, topic_path)

    def _create_player(self):
        if not self._player:
            try:
                self._player = Player(self)
                if self._publish_clock:
                    self._player.start_clock_publishing()
            except Exception as ex:
                qWarning('Error starting player; aborting publish: %s' % str(ex))
                return False

        return True

 def level_changed(self, row):
     """
     Handles the rowchanged event for the level_list widget
     Makes a service call to change the logger level for the indicated node/logger to the selected value
     :param row: the selected row in level_list, ''int''
     """
     if row == -1:
         return
     if row < 0 or row >= self.level_list.count():
         qWarning('Level row %s out of bounds. Current count: %s' % (row, self.level_list.count()))
         return
     self._caller.send_logger_change_message(self.node_list.currentItem().text(), self.logger_list.currentItem().text(), self.level_list.item(row).text())

    def on_refresh_services_button_clicked(self):
        service_names = rosservice.get_service_list()
        self._services = {}
        for service_name in service_names:
            try:
                self._services[service_name] = rosservice.get_service_class_by_name(service_name)
                #qDebug('ServiceCaller.on_refresh_services_button_clicked(): found service %s using class %s' % (service_name, self._services[service_name]))
            except (rosservice.ROSServiceException, rosservice.ROSServiceIOException) as e:
                qWarning('ServiceCaller.on_refresh_services_button_clicked(): could not get class of service %s:\n%s' % (service_name, e))

        self.service_combo_box.clear()
        self.service_combo_box.addItems(sorted(service_names))

    def rotate_head(self, button_name):
        if('Left' in button_name):
            #qWarning('x: %s, y: %s' % (self.head_x, self.head_y))
           
            if (self.head_x < -0.8 and self.head_y > 0.0):
                self.show_warning('Can\'t rotate anymore')

            elif (self.head_y < 0.0):
                self.head_x += 0.1
                self.head_y = -((1.0 - self.head_x ** 2.0) ** 0.5)
                self.show_text_in_rviz("Turning Head Left")

            else:
                self.head_x -= 0.1
                self.head_y = (1.0 - self.head_x ** 2.0) ** 0.5
                self.show_text_in_rviz("Turning Head Left")

            qWarning('x: %s, y: %s' % (self.head_x, self.head_y))
            self.head_action(self.head_x, self.head_y, self.head_z)
            
        elif('Up' in button_name):
            if (self.head_z <= 1.6):
                self.head_z += 0.1
                self.show_text_in_rviz("Moving Head Up")
                self.head_action(self.head_x, self.head_y, self.head_z)
            else:
                self.show_warning('Can\'t look up anymore')
        
        elif('Down' in button_name):
            if (self.head_z >= -2.2):
                self.head_z -= 0.1
                self.show_text_in_rviz("Moving Head Down")
                self.head_action(self.head_x, self.head_y, self.head_z)
            else:
                self.show_warning('Can\'t look down anymore') 
            
        else:
            #qWarning('x: %s, y: %s' % (self.head_x, self.head_y))
            if (self.head_x < -0.8 and self.head_y < 0.0):
                self.show_warning('Can\'t rotate anymore')

            elif (self.head_y > 0.0):
                self.head_x += 0.1
                self.head_y = (1.0 - self.head_x ** 2.0) ** 0.5
                self.show_text_in_rviz("Turning Head Right")
                
            else:
                self.head_x -= 0.1
                self.head_y = -((1.0 - self.head_x ** 2.0) ** 0.5)
                self.show_text_in_rviz("Turning Head Right")     
            
            #qWarning('x: %s, y: %s' % (self.head_x, self.head_y))
            self.head_action(self.head_x, self.head_y, self.head_z)

 def dragEnterEvent(self, event):
     if not event.mimeData().hasText():
         if not hasattr(event.source(), 'selectedItems') or len(event.source().selectedItems()) == 0:
             qWarning('Plot.dragEnterEvent(): not hasattr(event.source(), selectedItems) or len(event.source().selectedItems()) == 0')
             return
         item = event.source().selectedItems()[0]
         ros_topic_name = item.data(0, Qt.UserRole)
         if ros_topic_name is None:
             qWarning('Plot.dragEnterEvent(): not hasattr(item, ros_topic_name_)')
             return
     # TODO: do some checks for valid topic here
     event.acceptProposedAction()

 def _change_publisher_rate(self, publisher_info, topic_name, new_value):
     try:
         rate = float(new_value)
     except Exception:
         qWarning('Publisher._change_publisher_rate(): could not parse rate value: %s' % (new_value))
     else:
         publisher_info['rate'] = rate
         #qDebug('Publisher._change_publisher_rate(): %s rate changed: %fHz' % (publisher_info['topic_name'], publisher_info['rate']))
         publisher_info['timer'].stop()
         if publisher_info['enabled'] and publisher_info['rate'] > 0:
             publisher_info['timer'].start(int(1000.0 / publisher_info['rate']))
     # make sure the column value reflects the actual rate
     return '%.2f' % publisher_info['rate']