image_view2_wrapper.py
Go to the documentation of this file.
1 from distutils.version import LooseVersion
2 import math
3 import os
4 import sys
5 from threading import Lock, Thread
6 import time
7 
8 import cv2
9 import numpy as np
10 import python_qt_binding
11 from python_qt_binding.QtCore import pyqtSignal
12 from python_qt_binding.QtCore import pyqtSlot
13 from python_qt_binding.QtCore import QEvent
14 from python_qt_binding.QtCore import QSize
15 from python_qt_binding.QtCore import Qt
16 import python_qt_binding.QtCore as QtCore
17 from python_qt_binding.QtCore import QTimer
18 from python_qt_binding.QtCore import qWarning
19 from python_qt_binding.QtCore import Slot
20 from python_qt_binding.QtGui import QBrush
21 from python_qt_binding.QtGui import QColor
22 from python_qt_binding.QtGui import QFont
23 from python_qt_binding.QtGui import QIcon
24 from python_qt_binding.QtGui import QImage
25 from python_qt_binding.QtGui import QPainter
26 from python_qt_binding.QtGui import QPen
27 from python_qt_binding.QtGui import QPixmap
28 from python_qt_binding.QtGui import qRgb
29 import python_qt_binding.QtGui as QtGui
30 import yaml
31 
32 from cv_bridge import CvBridge
33 from cv_bridge import CvBridgeError
34 from image_view2.msg import MouseEvent
35 from resource_retriever import get_filename
36 import rospy
37 from rqt_gui_py.plugin import Plugin
38 from sensor_msgs.msg import Image
39 from std_msgs.msg import Bool
40 from std_msgs.msg import Time
41 
42 if LooseVersion(python_qt_binding.QT_BINDING_VERSION).version[0] >= 5:
43  from python_qt_binding.QtWidgets import QAction
44  from python_qt_binding.QtWidgets import QComboBox
45  from python_qt_binding.QtWidgets import QDialog
46  from python_qt_binding.QtWidgets import QMenu
47  from python_qt_binding.QtWidgets import QMessageBox
48  from python_qt_binding.QtWidgets import QLabel
49  from python_qt_binding.QtWidgets import QPushButton
50  from python_qt_binding.QtWidgets import QSizePolicy
51  from python_qt_binding.QtWidgets import QVBoxLayout
52  from python_qt_binding.QtWidgets import QWidget
53 
54 else:
55  from python_qt_binding.QtGui import QAction
56  from python_qt_binding.QtGui import QComboBox
57  from python_qt_binding.QtGui import QDialog
58  from python_qt_binding.QtGui import QMenu
59  from python_qt_binding.QtGui import QMessageBox
60  from python_qt_binding.QtGui import QLabel
61  from python_qt_binding.QtGui import QPushButton
62  from python_qt_binding.QtGui import QSizePolicy
63  from python_qt_binding.QtGui import QVBoxLayout
64  from python_qt_binding.QtGui import QWidget
65 
66 
67 class ComboBoxDialog(QDialog):
68  def __init__(self, parent=None):
69  super(ComboBoxDialog, self).__init__()
70  self.number = 0
71  vbox = QVBoxLayout(self)
72  self.combo_box = QComboBox(self)
73  self.combo_box.activated.connect(self.onActivated)
74  vbox.addWidget(self.combo_box)
75  button = QPushButton()
76  button.setText("Done")
77  button.clicked.connect(self.buttonCallback)
78  vbox.addWidget(button)
79  self.setLayout(vbox)
80 
81  def buttonCallback(self, event):
82  self.close()
83 
84  def onActivated(self, number):
85  self.number = number
86 
87 
89  """
90  rqt wrapper for image_view2
91  """
92  def __init__(self, context):
93  super(ImageView2Plugin, self).__init__(context)
94  self.setObjectName("ImageView2Plugin")
96  context.add_widget(self._widget)
97 
98  def save_settings(self, plugin_settings, instance_settings):
99  self._widget.save_settings(plugin_settings, instance_settings)
100 
101  def restore_settings(self, plugin_settings, instance_settings):
102  self._widget.restore_settings(plugin_settings, instance_settings)
103 
105  self._widget.trigger_configuration()
106 
107 
108 class ScaledLabel(QLabel):
109  def __init__(self, *args, **kwargs):
110  QLabel.__init__(self)
111  self._pixmap = QPixmap(self.pixmap())
112 
113  def resizeEvent(self, event):
114  self.setPixmap(self._pixmap.scaled(
115  self.width(), self.height(),
116  QtCore.Qt.KeepAspectRatio))
117 
118 
119 class ImageView2Widget(QWidget):
120  """
121  Qt widget to communicate with image_view2
122  """
123  cv_image = None
124  pixmap = None
125  repaint_trigger = pyqtSignal()
126 
127  def __init__(self):
128  super(ImageView2Widget, self).__init__()
129  self.left_button_clicked = False
130 
131  self.repaint_trigger.connect(self.redraw)
132  self.lock = Lock()
133  self.need_to_rewrite = False
134  self.bridge = CvBridge()
135  self.image_sub = None
136  self.event_pub = None
138  self.label.setAlignment(Qt.AlignCenter)
139  self.label.setSizePolicy(
140  QSizePolicy(QSizePolicy.Ignored, QSizePolicy.Ignored))
141  # self.label.installEventFilter(self)
142  vbox = QVBoxLayout(self)
143  vbox.addWidget(self.label)
144  self.setLayout(vbox)
145 
146  self._image_topics = []
147  self._update_topic_thread = Thread(target=self.updateTopics)
148  self._update_topic_thread.start()
149 
150  self._active_topic = None
151  self.setMouseTracking(True)
152  self.label.setMouseTracking(True)
154  self.show()
155 
157  self._dialog.exec_()
158  self.setupSubscriber(self._image_topics[self._dialog.number])
159 
160  def setupSubscriber(self, topic):
161  if self.image_sub:
162  self.image_sub.unregister()
163  rospy.loginfo("Subscribing %s" % (topic + "/marked"))
164  self.image_sub = rospy.Subscriber(topic + "/marked",
165  Image,
166  self.imageCallback)
167  self.event_pub = rospy.Publisher(
168  topic + "/event", MouseEvent, queue_size=1)
169  self._active_topic = topic
170 
171  def onActivated(self, number):
172  self.setupSubscriber(self._image_topics[number])
173 
174  def imageCallback(self, msg):
175  with self.lock:
176  if msg.width == 0 or msg.height == 0:
177  rospy.logdebug("Looks input images is invalid")
178  return
179  cv_image = self.bridge.imgmsg_to_cv2(msg, msg.encoding)
180  if msg.encoding == "bgr8":
181  self.cv_image = cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB)
182  elif msg.encoding == "rgb8":
183  self.cv_image = cv_image
184  self.numpy_image = np.asarray(self.cv_image)
185  self.need_to_rewrite = True
186  self.repaint_trigger.emit()
187 
188  def updateTopics(self):
189  need_to_update = False
190  for (topic, topic_type) in rospy.get_published_topics():
191  if topic_type == "sensor_msgs/Image":
192  with self.lock:
193  if not topic in self._image_topics:
194  self._image_topics.append(topic)
195  need_to_update = True
196  if need_to_update:
197  with self.lock:
198  self._image_topics = sorted(self._image_topics)
199  self._dialog.combo_box.clear()
200  for topic in self._image_topics:
201  self._dialog.combo_box.addItem(topic)
202  if self._active_topic:
203  self._dialog.combo_box.setCurrentIndex(
204  self._image_topics.index(self._active_topic))
205  time.sleep(1)
206 
207  @pyqtSlot()
208  def redraw(self):
209  with self.lock:
210  if not self.need_to_rewrite:
211  return
212  if np.all(self.cv_image) is not None:
213  size = self.cv_image.shape
214  img = QImage(self.cv_image.data,
215  size[1], size[0], size[2] * size[1],
216  QImage.Format_RGB888)
217  # convert to QPixmap
218  self.pixmap = QPixmap(size[1], size[0])
219  self.pixmap.convertFromImage(img)
220  self.label.setPixmap(self.pixmap.scaled(
221  self.label.width(), self.label.height(),
222  QtCore.Qt.KeepAspectRatio))
223  # self.label.setPixmap(self.pixmap)
224 
225  def mousePosition(self, e):
226  label_x = self.label.x()
227  label_y = self.label.y()
228  label_width = self.label.width()
229  label_height = self.label.height()
230  pixmap_width = self.label.pixmap().width()
231  pixmap_height = self.label.pixmap().height()
232  x_offset = (label_width - pixmap_width) / 2.0 + label_x
233  y_offset = (label_height - pixmap_height) / 2.0 + label_y
234  return (e.x() - x_offset, e.y() - y_offset)
235 
236  def mouseMoveEvent(self, e):
237  msg = MouseEvent()
238  msg.header.stamp = rospy.Time.now()
239  msg.type = MouseEvent.MOUSE_MOVE
240  msg.x, msg.y = self.mousePosition(e)
241  msg.width = self.label.pixmap().width()
242  msg.height = self.label.pixmap().height()
243  if self.event_pub:
244  self.event_pub.publish(msg)
245 
246  def mousePressEvent(self, e):
247  msg = MouseEvent()
248  msg.header.stamp = rospy.Time.now()
249  if e.button() == Qt.LeftButton:
250  msg.type = MouseEvent.MOUSE_LEFT_DOWN
251  self.left_button_clicked = True
252  elif e.button() == Qt.RightButton:
253  msg.type = MouseEvent.MOUSE_RIGHT_DOWN
254  msg.width = self.label.pixmap().width()
255  msg.height = self.label.pixmap().height()
256  msg.x, msg.y = self.mousePosition(e)
257  if self.event_pub:
258  self.event_pub.publish(msg)
259 
260  def mouseReleaseEvent(self, e):
261  if e.button() == Qt.LeftButton:
262  self.left_button_clicked = False
263  msg = MouseEvent()
264  msg.header.stamp = rospy.Time.now()
265  msg.width = self.label.pixmap().width()
266  msg.height = self.label.pixmap().height()
267  msg.type = MouseEvent.MOUSE_LEFT_UP
268  msg.x, msg.y = self.mousePosition(e)
269  if self.event_pub:
270  self.event_pub.publish(msg)
271 
272  def save_settings(self, plugin_settings, instance_settings):
273  if self._active_topic:
274  instance_settings.set_value("active_topic", self._active_topic)
275 
276  def restore_settings(self, plugin_settings, instance_settings):
277  if instance_settings.value("active_topic"):
278  topic = instance_settings.value("active_topic")
279  self._dialog.combo_box.addItem(topic)
280  self.setupSubscriber(topic)
def restore_settings(self, plugin_settings, instance_settings)
def restore_settings(self, plugin_settings, instance_settings)
def save_settings(self, plugin_settings, instance_settings)
def save_settings(self, plugin_settings, instance_settings)


jsk_rqt_plugins
Author(s):
autogenerated on Fri Feb 5 2021 04:03:20