class ScaledLabel(QLabel):
def __init__(self, *args, **kwargs):
QLabel.__init__(self)
self._pixmap = QPixmap(self.pixmap())
def resizeEvent(self, event):
self.setPixmap(
self._pixmap.scaled(self.width(), self.height(),
QtCore.Qt.KeepAspectRatio))
def image_callback(self, ros_data):
np_arr = np.fromstring(ros_data.data, np.uint8)
image = cv.imdecode(np_arr, cv.IMREAD_COLOR)
height, width, _ = image.shape
bytes_per_line = 3 * width
qImage = QImage(image.data, width, height, bytes_per_line,
QImage.Format_RGB888).rgbSwapped()
pix = QPixmap(qImage)
h_label = self._widget.videoDisplay.height()
w_label = self._widget.videoDisplay.width()
self._widget.videoDisplay.setPixmap(
pix.scaled(w_label, h_label, Qt.KeepAspectRatio))
def __init__(self, context):
super(GroundRobotTeleop, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('GroundRobotTeleop')
# 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
# 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(
'rqt_ground_robot_teleop'), 'resource', 'GroundRobotTeleop.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('GroundRobotTeleopUi')
# 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 logo
pixmap = QPixmap(os.path.join(rospkg.RosPack().get_path(
'rqt_ground_robot_teleop'), 'resource', 'jderobot.png'))
self._widget.img_logo.setPixmap(pixmap.scaled(121, 121))
# Set Variables
self.linear_velocity_scaling_factor = 1
self.angular_velocity_scaling_factor = 0.5
# Set functions for each GUI Item
self._widget.stop_button.clicked.connect(self.stop_robot)
# Add Publishers
self.twist_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1)
#Add global variables
self.twist_msg = Twist()
self.stop_icon = QIcon()
self.stop_icon.addPixmap(QPixmap(os.path.join(rospkg.RosPack().get_path(
'rqt_ground_robot_teleop'), 'resource', 'stop.png')), QIcon.Normal, QIcon.Off)
self.play_icon = QIcon()
self.play_icon.addPixmap(QPixmap(os.path.join(rospkg.RosPack().get_path(
'rqt_ground_robot_teleop'), 'resource', 'play.png')), QIcon.Normal, QIcon.Off)
self.bridge = CvBridge()
self.teleop = TeleopWidget(self, 'set_twist', 311)
self._widget.teleop_layout.addWidget(self.teleop)
self.teleop.setVisible(True)
# Add widget to the user interface
context.add_widget(self._widget)
# Add Subscibers
rospy.Subscriber('camera/rgb/image_raw', Image, self.cam_cb)
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)
def __init__(self, context):
super(DroneTeleop, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('DroneTeleop')
# 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
# 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('rqt_drone_teleop'),
'resource', 'DroneTeleop.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('DroneTeleopUi')
# 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 logo
pixmap = QPixmap(
os.path.join(rospkg.RosPack().get_path('rqt_drone_teleop'),
'resource', 'jderobot.png'))
self._widget.img_logo.setPixmap(pixmap.scaled(121, 121))
# Set Variables
self.play_code_flag = False
self.takeoff = False
self.linear_velocity_scaling_factor = 1
self.vertical_velocity_scaling_factor = 0.8
self.angular_velocity_scaling_factor = 0.5
self._widget.term_out.setReadOnly(True)
self._widget.term_out.setLineWrapMode(self._widget.term_out.NoWrap)
# Set functions for each GUI Item
self._widget.takeoffButton.clicked.connect(self.call_takeoff_land)
self._widget.playButton.clicked.connect(self.call_play)
self._widget.stopButton.clicked.connect(self.stop_drone)
# Add Publishers
self.takeoff_pub = rospy.Publisher('gui/takeoff_land',
Bool,
queue_size=1)
self.play_stop_pub = rospy.Publisher('gui/play_stop',
Bool,
queue_size=1)
self.twist_pub = rospy.Publisher('gui/twist', Twist, queue_size=1)
#Add global variables
self.shared_twist_msg = Twist()
self.current_pose = Pose()
self.current_twist = Twist()
self.stop_icon = QIcon()
self.stop_icon.addPixmap(
QPixmap(
os.path.join(rospkg.RosPack().get_path('rqt_drone_teleop'),
'resource', 'stop.png')), QIcon.Normal, QIcon.Off)
self.play_icon = QIcon()
self.play_icon.addPixmap(
QPixmap(
os.path.join(rospkg.RosPack().get_path('rqt_drone_teleop'),
'resource', 'play.png')), QIcon.Normal, QIcon.Off)
self.bridge = CvBridge()
self.teleop_stick_1 = TeleopWidget(self, 'set_linear_xy', 151)
self._widget.tlLayout.addWidget(self.teleop_stick_1)
self.teleop_stick_1.setVisible(True)
self.teleop_stick_2 = TeleopWidget(self, 'set_alt_yawrate', 151)
self._widget.tlLayout_2.addWidget(self.teleop_stick_2)
self.teleop_stick_2.setVisible(True)
self.sensors_widget = SensorsWidget(self)
self._widget.sensorsCheck.stateChanged.connect(
self.show_sensors_widget)
# Add widget to the user interface
context.add_widget(self._widget)
# Add Subscibers
cam_frontal_topic = rospy.get_param('cam_frontal_topic',
'/iris/cam_frontal/image_raw')
cam_ventral_topic = rospy.get_param('cam_ventral_topic',
'/iris/cam_ventral/image_raw')
rospy.Subscriber(cam_frontal_topic, Image, self.cam_frontal_cb)
rospy.Subscriber(cam_ventral_topic, Image, self.cam_ventral_cb)
rospy.Subscriber('interface/filtered_img', Image, self.filtered_img_cb)
rospy.Subscriber('interface/threshed_img', Image, self.threshed_img_cb)
rospy.Subscriber('mavros/local_position/pose', PoseStamped,
self.pose_stamped_cb)
rospy.Subscriber('mavros/local_position/velocity_body', TwistStamped,
self.twist_stamped_cb)
class ImageView2Widget(QWidget):
"""
Qt widget to communicate with image_view2
"""
cv_image = None
pixmap = None
def __init__(self):
super(ImageView2Widget, self).__init__()
self.left_button_clicked = False
self.lock = Lock()
self.event_pub = rospy.Publisher("event", MouseEvent)
self.bridge = CvBridge()
self.label = ScaledLabel()
self.label.setAlignment(Qt.AlignCenter)
self.label.setSizePolicy(QSizePolicy(QSizePolicy.Ignored, QSizePolicy.Ignored))
#self.label.installEventFilter(self)
hbox = QtGui.QHBoxLayout(self)
hbox.addWidget(self.label)
self.setLayout(hbox)
self.image_sub = rospy.Subscriber("image_marked", Image,
self.imageCallback)
self._update_plot_timer = QTimer(self)
self._update_plot_timer.timeout.connect(self.redraw)
self._update_plot_timer.start(40)
self.setMouseTracking(True)
self.show()
def imageCallback(self, msg):
with self.lock:
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)
def redraw(self):
with self.lock:
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):
if self.left_button_clicked:
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()
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 msg.type == 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)
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)
self.event_pub.publish(msg)
def eventFilter(self, widget, event):
if not self.pixmap:
return QtGui.QMainWindow.eventFilter(self, widget, event)
if (event.type() == QtCore.QEvent.Resize and
widget is self.label):
self.label.setPixmap(self.pixmap.scaled(
self.label.width(), self.label.height(),
QtCore.Qt.KeepAspectRatio))
return True
return QtGui.QMainWindow.eventFilter(self, widget, event)
def __init__(self, context):
super(PosTeleop, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('PosTeleop')
# 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
# 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('rqt_drone_teleop'),
'resource', 'PositionTeleop.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('PosTeleopUi')
# 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 logo
pixmap = QPixmap(
os.path.join(rospkg.RosPack().get_path('rqt_drone_teleop'),
'resource', 'jderobot.png'))
self._widget.img_logo.setPixmap(pixmap.scaled(121, 121))
# Set Variables
self.play_code_flag = False
self.takeoff = False
self._widget.term_out.setReadOnly(True)
self._widget.term_out.setLineWrapMode(self._widget.term_out.NoWrap)
# Set functions for each GUI Item
self._widget.takeoffButton.clicked.connect(self.call_takeoff_land)
self._widget.stopButton.clicked.connect(self.stop_drone)
self._widget.playButton.clicked.connect(self.call_play)
self._widget.posControlButton.clicked.connect(self.set_pos)
# Add Publishers
self.play_stop_pub = rospy.Publisher('gui/play_stop',
Bool,
queue_size=1)
# Add global variables
self.extended_state = ExtendedState()
self.shared_pose_msg = Pose()
self.shared_twist_msg = Twist()
self.current_pose = Pose()
self.pose_frame = ''
self.current_twist = Twist()
self.twist_frame = ''
self.is_running = True
self.stop_icon = QIcon()
self.stop_icon.addPixmap(
QPixmap(
os.path.join(rospkg.RosPack().get_path('rqt_drone_teleop'),
'resource', 'stop.png')), QIcon.Normal, QIcon.Off)
self.play_icon = QIcon()
self.play_icon.addPixmap(
QPixmap(
os.path.join(rospkg.RosPack().get_path('rqt_drone_teleop'),
'resource', 'play.png')), QIcon.Normal, QIcon.Off)
self.sensors_widget = SensorsWidget(self)
self._widget.sensorsCheck.stateChanged.connect(
self.show_sensors_widget)
# Add widget to the user interface
context.add_widget(self._widget)
# Add Subscribers
rospy.Subscriber('drone_wrapper/local_position/pose', PoseStamped,
self.pose_stamped_cb)
rospy.Subscriber('drone_wrapper/local_position/velocity_body',
TwistStamped, self.twist_stamped_cb)
rospy.Subscriber('drone_wrapper/extended_state', ExtendedState,
self.extended_state_cb)
# Add Timer
self.update_status_info()
class ImageView2Widget(QWidget):
"""
Qt widget to communicate with image_view2
"""
cv_image = None
pixmap = None
def __init__(self):
super(ImageView2Widget, self).__init__()
self.left_button_clicked = False
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._update_plot_timer = QTimer(self)
self._update_plot_timer.timeout.connect(self.redraw)
self._update_plot_timer.start(40)
self._image_topics = []
self._update_topic_timer = QTimer(self)
self._update_topic_timer.timeout.connect(self.updateTopics)
self._update_topic_timer.start(1)
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
def updateTopics(self):
need_to_update = False
for (topic, topic_type) in rospy.get_published_topics():
if topic_type == "sensor_msgs/Image":
if not topic in self._image_topics:
self._image_topics.append(topic)
need_to_update = True
if need_to_update:
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))
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 eventFilter(self, widget, event):
if not self.pixmap:
return QtGui.QMainWindow.eventFilter(self, widget, event)
if (event.type() == QtCore.QEvent.Resize and widget is self.label):
self.label.setPixmap(
self.pixmap.scaled(self.label.width(), self.label.height(),
QtCore.Qt.KeepAspectRatio))
return True
return QtGui.QMainWindow.eventFilter(self, widget, event)
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 ImageView2Widget(QWidget):
"""
Qt widget to communicate with image_view2
"""
cv_image = None
pixmap = None
def __init__(self):
super(ImageView2Widget, self).__init__()
self.left_button_clicked = False
self.lock = Lock()
self.event_pub = rospy.Publisher("event", MouseEvent)
self.bridge = CvBridge()
self.label = ScaledLabel()
self.label.setAlignment(Qt.AlignCenter)
self.label.setSizePolicy(
QSizePolicy(QSizePolicy.Ignored, QSizePolicy.Ignored))
#self.label.installEventFilter(self)
hbox = QtGui.QHBoxLayout(self)
hbox.addWidget(self.label)
self.setLayout(hbox)
self.image_sub = rospy.Subscriber("image_marked", Image,
self.imageCallback)
self._update_plot_timer = QTimer(self)
self._update_plot_timer.timeout.connect(self.redraw)
self._update_plot_timer.start(40)
self.setMouseTracking(True)
self.show()
def imageCallback(self, msg):
with self.lock:
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)
def redraw(self):
with self.lock:
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):
if self.left_button_clicked:
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()
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 msg.type == 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)
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)
self.event_pub.publish(msg)
def eventFilter(self, widget, event):
if not self.pixmap:
return QtGui.QMainWindow.eventFilter(self, widget, event)
if (event.type() == QtCore.QEvent.Resize and widget is self.label):
self.label.setPixmap(
self.pixmap.scaled(self.label.width(), self.label.height(),
QtCore.Qt.KeepAspectRatio))
return True
return QtGui.QMainWindow.eventFilter(self, widget, event)
