00001 from rqt_gui_py.plugin import Plugin
00002 import python_qt_binding.QtGui as QtGui
00003 from python_qt_binding.QtGui import (QAction, QIcon, QMenu, QWidget,
00004 QPainter, QColor, QFont, QBrush,
00005 QPen, QMessageBox, QSizePolicy,
00006 QImage, QPixmap, qRgb, QComboBox,
00007 QDialog, QPushButton)
00008 from python_qt_binding.QtCore import (Qt, QTimer, qWarning, Slot,
00009 QEvent, QSize, pyqtSignal,
00010 pyqtSlot)
00011 from threading import Lock, Thread
00012 import rospy
00013 import python_qt_binding.QtCore as QtCore
00014 from std_msgs.msg import Bool, Time
00015 import time
00016 import math
00017 from resource_retriever import get_filename
00018 import yaml
00019 import os, sys
00020
00021 import numpy as np
00022 import cv2, cv
00023 from cv_bridge import CvBridge, CvBridgeError
00024 from image_view2.msg import MouseEvent
00025 from sensor_msgs.msg import Image
00026
00027
00028 class ComboBoxDialog(QDialog):
00029 def __init__(self, parent=None):
00030 super(ComboBoxDialog, self).__init__()
00031 self.number = 0
00032 vbox = QtGui.QVBoxLayout(self)
00033 self.combo_box = QComboBox(self)
00034 self.combo_box.activated.connect(self.onActivated)
00035 vbox.addWidget(self.combo_box)
00036 button = QPushButton()
00037 button.setText("Done")
00038 button.clicked.connect(self.buttonCallback)
00039 vbox.addWidget(button)
00040 self.setLayout(vbox)
00041 def buttonCallback(self, event):
00042 self.close()
00043 def onActivated(self, number):
00044 self.number = number
00045 class ImageView2Plugin(Plugin):
00046 """
00047 rqt wrapper for image_view2
00048 """
00049 def __init__(self, context):
00050 super(ImageView2Plugin, self).__init__(context)
00051 self.setObjectName("ImageView2Plugin")
00052 self._widget = ImageView2Widget()
00053 context.add_widget(self._widget)
00054 def save_settings(self, plugin_settings, instance_settings):
00055 self._widget.save_settings(plugin_settings, instance_settings)
00056 def restore_settings(self, plugin_settings, instance_settings):
00057 self._widget.restore_settings(plugin_settings, instance_settings)
00058 def trigger_configuration(self):
00059 self._widget.trigger_configuration()
00060
00061
00062 class ScaledLabel(QtGui.QLabel):
00063 def __init__(self, *args, **kwargs):
00064 QtGui.QLabel.__init__(self)
00065 self._pixmap = QtGui.QPixmap(self.pixmap())
00066 def resizeEvent(self, event):
00067 self.setPixmap(self._pixmap.scaled(
00068 self.width(), self.height(),
00069 QtCore.Qt.KeepAspectRatio))
00070
00071 class ImageView2Widget(QWidget):
00072 """
00073 Qt widget to communicate with image_view2
00074 """
00075 cv_image = None
00076 pixmap = None
00077 repaint_trigger = pyqtSignal()
00078 def __init__(self):
00079 super(ImageView2Widget, self).__init__()
00080 self.left_button_clicked = False
00081
00082 self.repaint_trigger.connect(self.redraw)
00083 self.lock = Lock()
00084 self.need_to_rewrite = False
00085 self.bridge = CvBridge()
00086 self.image_sub = None
00087 self.event_pub = None
00088 self.label = ScaledLabel()
00089 self.label.setAlignment(Qt.AlignCenter)
00090 self.label.setSizePolicy(QSizePolicy(QSizePolicy.Ignored, QSizePolicy.Ignored))
00091
00092 vbox = QtGui.QVBoxLayout(self)
00093 vbox.addWidget(self.label)
00094 self.setLayout(vbox)
00095
00096 self._image_topics = []
00097 self._update_topic_thread = Thread(target=self.updateTopics)
00098 self._update_topic_thread.start()
00099
00100 self._active_topic = None
00101 self.setMouseTracking(True)
00102 self.label.setMouseTracking(True)
00103 self._dialog = ComboBoxDialog()
00104 self.show()
00105 def trigger_configuration(self):
00106 self._dialog.exec_()
00107 self.setupSubscriber(self._image_topics[self._dialog.number])
00108 def setupSubscriber(self, topic):
00109 if self.image_sub:
00110 self.image_sub.unregister()
00111 rospy.loginfo("Subscribing %s" % (topic + "/marked"))
00112 self.image_sub = rospy.Subscriber(topic + "/marked",
00113 Image,
00114 self.imageCallback)
00115 self.event_pub = rospy.Publisher(topic + "/event", MouseEvent)
00116 self._active_topic = topic
00117 def onActivated(self, number):
00118 self.setupSubscriber(self._image_topics[number])
00119 def imageCallback(self, msg):
00120 with self.lock:
00121 if msg.width == 0 or msg.height == 0:
00122 rospy.logdebug("Looks input images is invalid")
00123 return
00124 cv_image = self.bridge.imgmsg_to_cv2(msg, msg.encoding)
00125 if msg.encoding == "bgr8":
00126 self.cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB)
00127 elif msg.encoding == "rgb8":
00128 self.cv_image = cv_image
00129 self.numpy_image = np.asarray(self.cv_image)
00130 self.need_to_rewrite = True
00131 self.repaint_trigger.emit()
00132 def updateTopics(self):
00133 need_to_update = False
00134 for (topic, topic_type) in rospy.get_published_topics():
00135 if topic_type == "sensor_msgs/Image":
00136 with self.lock:
00137 if not topic in self._image_topics:
00138 self._image_topics.append(topic)
00139 need_to_update = True
00140 if need_to_update:
00141 with self.lock:
00142 self._image_topics = sorted(self._image_topics)
00143 self._dialog.combo_box.clear()
00144 for topic in self._image_topics:
00145 self._dialog.combo_box.addItem(topic)
00146 if self._active_topic:
00147 self._dialog.combo_box.setCurrentIndex(self._image_topics.index(self._active_topic))
00148 time.sleep(1)
00149 @pyqtSlot()
00150 def redraw(self):
00151 with self.lock:
00152 if not self.need_to_rewrite:
00153 return
00154 if self.cv_image != None:
00155 size = self.cv_image.shape
00156 img = QImage(self.cv_image.data,
00157 size[1], size[0], size[2] * size[1],
00158 QImage.Format_RGB888)
00159
00160 self.pixmap = QPixmap(size[1], size[0])
00161 self.pixmap.convertFromImage(img)
00162 self.label.setPixmap(self.pixmap.scaled(
00163 self.label.width(), self.label.height(),
00164 QtCore.Qt.KeepAspectRatio))
00165
00166 def mousePosition(self, e):
00167 label_x = self.label.x()
00168 label_y = self.label.y()
00169 label_width = self.label.width()
00170 label_height = self.label.height()
00171 pixmap_width = self.label.pixmap().width()
00172 pixmap_height = self.label.pixmap().height()
00173 x_offset = (label_width - pixmap_width) / 2.0 + label_x
00174 y_offset = (label_height - pixmap_height) / 2.0 + label_y
00175 return (e.x() - x_offset, e.y()- y_offset)
00176 def mouseMoveEvent(self, e):
00177 msg = MouseEvent()
00178 msg.header.stamp = rospy.Time.now()
00179 msg.type = MouseEvent.MOUSE_MOVE
00180 msg.x, msg.y = self.mousePosition(e)
00181 msg.width = self.label.pixmap().width()
00182 msg.height = self.label.pixmap().height()
00183 if self.event_pub:
00184 self.event_pub.publish(msg)
00185 def mousePressEvent(self, e):
00186 msg = MouseEvent()
00187 msg.header.stamp = rospy.Time.now()
00188 if e.button() == Qt.LeftButton:
00189 msg.type = MouseEvent.MOUSE_LEFT_DOWN
00190 self.left_button_clicked = True
00191 elif e.button() == Qt.RightButton:
00192 msg.type = MouseEvent.MOUSE_RIGHT_DOWN
00193 msg.width = self.label.pixmap().width()
00194 msg.height = self.label.pixmap().height()
00195 msg.x, msg.y = self.mousePosition(e)
00196 if self.event_pub:
00197 self.event_pub.publish(msg)
00198 def mouseReleaseEvent(self, e):
00199 if e.button() == Qt.LeftButton:
00200 self.left_button_clicked = False
00201 msg = MouseEvent()
00202 msg.header.stamp = rospy.Time.now()
00203 msg.width = self.label.pixmap().width()
00204 msg.height = self.label.pixmap().height()
00205 msg.type = MouseEvent.MOUSE_LEFT_UP
00206 msg.x, msg.y = self.mousePosition(e)
00207 if self.event_pub:
00208 self.event_pub.publish(msg)
00209 def save_settings(self, plugin_settings, instance_settings):
00210 if self._active_topic:
00211 instance_settings.set_value("active_topic", self._active_topic)
00212 def restore_settings(self, plugin_settings, instance_settings):
00213 if instance_settings.value("active_topic"):
00214 topic = instance_settings.value("active_topic")
00215 self._dialog.combo_box.addItem(topic)
00216 self.setupSubscriber(topic)