def writeLocationsToFile(self):
out_dict = {}
out_dict["locations"] = self.locations.keys()
out_dict["locations"] = sorted(out_dict["locations"])
out_dict["polygons"] = []
for index, key in enumerate(sorted(self.locations)):
out_dict["polygons"].append([])
polygon = self.locations[key]
for i in range(polygon.size()):
pt = polygon.point(i)
scaled_pt = scalePoint(pt, self.image_size, self.map_size)
out_dict["polygons"][index].append(scaled_pt.x())
out_dict["polygons"][index].append(scaled_pt.y())
data_directory = os.path.dirname(os.path.realpath(self.location_file))
image_file = getLocationsImageFileLocationFromDataDirectory(
data_directory)
# Create an image with the location data, so that C++ programs don't need to rely on determining regions using polygons.
out_dict["data"] = 'locations.pgm'
location_image = QImage(self.map_size, QImage.Format_RGB32)
location_image.fill(Qt.white)
painter = QPainter(location_image)
for index, key in enumerate(self.locations):
if index > 254:
rospy.logerr(
"You have more than 254 locations, which is unsupported by the bwi_planning_common C++ code!"
)
painter.setPen(Qt.NoPen)
painter.setBrush(QColor(index, index, index))
scaled_polygon = scalePolygon(self.locations[key], self.image_size,
self.map_size)
painter.drawPolygon(scaled_polygon)
painter.end()
location_image.save(image_file)
stream = open(self.location_file, 'w')
yaml.dump(out_dict, stream)
stream.close()
self.is_modified = False
def map_cb(self, msg):
self.resolution = msg.info.resolution
self.w = msg.info.width
self.h = msg.info.height
a = numpy.array(msg.data, dtype=numpy.uint8, copy=False, order='C')
a = a.reshape((self.h, self.w))
if self.w % 4:
e = numpy.empty((self.h, 4 - self.w % 4), dtype=a.dtype, order='C')
a = numpy.append(a, e, axis=1)
image = QImage(a.reshape((a.shape[0] * a.shape[1])), self.w, self.h,
QImage.Format_Indexed8)
for i in reversed(range(101)):
image.setColor(100 - i, qRgb(i * 2.55, i * 2.55, i * 2.55))
image.setColor(101, qRgb(255, 0, 0)) # not used indices
image.setColor(255, qRgb(200, 200, 200)) # color for unknown value -1
self._map = image
self.setSceneRect(0, 0, self.w, self.h)
self.map_changed.emit()
def _update_map(self):
"""
Method used in the rqt_nav_view plugin to read the image from the map server.
"""
a = numpy.array(self.map.data,
dtype=numpy.uint8,
copy=False,
order='C')
a = a.reshape((self.map.info.height, self.map.info.width))
if self.map.info.width % 4:
e = numpy.empty(
(self.map.info.height, 4 - self.map.info.width % 4),
dtype=a.dtype,
order='C')
a = numpy.append(a, e, axis=1)
image = QImage(a.reshape(
(a.shape[0] * a.shape[1])), self.map.info.width,
self.map.info.height, QImage.Format_Indexed8)
for i in reversed(range(101)):
image.setColor(100 - i, qRgb(i * 2.55, i * 2.55, i * 2.55))
image.setColor(101, qRgb(255, 0, 0)) # not used indices
image.setColor(255, qRgb(200, 200, 200)) # color for unknown value -1
self._map = image
self.graphics_view.setSceneRect(0, 0, self.map.info.width,
self.map.info.height)
pixmap = QPixmap.fromImage(self._map)
self._map_item = self._scene.addPixmap(pixmap)
# Everything must be mirrored
self._map_item.scale(-1, 1)
self._map_item.translate(-1 * self.map.info.width, 0)
# Add drag and drop functionality
# self.add_dragdrop(self._map_item)
self.graphics_view.centerOn(self._map_item)
self.graphics_view.show()
def initialize_glview(self):
# create GL view
self._glview = MyGLWidget()
self._glview.setAcceptDrops(True)
# backup and replace original paint method
# self.glView.paintGL is callbacked from QGLWidget
self._glview.paintGL_original = self._glview.paintGL
self._glview.paintGL = self.glview_paintGL
# backup and replace original mouse release method
self._glview.mouseReleaseEvent_original = self._glview.mouseReleaseEvent
self._glview.mouseReleaseEvent = self.glview_mouseReleaseEvent
# backup and replace original mouse release method
self._glview.wheelEvent_original = self._glview.wheelEvent
self._glview.wheelEvent = self.glview_wheelEvent
# add GL view to widget layout
# http://doc.qt.io/qt-4.8/qgridlayout.html
self.layout().addWidget(self._glview, 1, 0, 1, 4)
self.qimage = QImage(self.filename, 'JPEG') # GL_TEXTURE_2D
def both_callback(self, luv, hsv):
if self.view != BOTH:
self.label_hsv.hide()
self.label_lab.hide()
return
img_luv = self.brd.imgmsg_to_cv2(luv)
img_hsv = self.brd.imgmsg_to_cv2(hsv)
# cv2.cvtColor(img_luv, cv2.COLOR_BGR2RGB)
# cv2.cvtColor(img_hsv, cv2.COLOR_BGR2RGB)
h, w, c = img_luv.shape
img = np.zeros([h, w, c])
luv_2 = cv2.resize(img_luv, (0, 0), fx=0.5, fy=0.5)
hsv_2 = cv2.resize(img_hsv, (0, 0), fx=0.5, fy=0.5)
both = np.hstack((hsv_2, luv_2))
dif = (img.shape[0] - both.shape[0]) // 2
img[dif:img.shape[0] - dif, 0:img.shape[1]] = both
q_img = QImage(both.data, both.shape[1], both.shape[0],
3 * both.shape[1], QImage.Format_RGB888)
q = QPixmap.fromImage(q_img)
self.wdg_img.setFixedWidth(both.shape[1])
self.wdg_img.setFixedHeight(both.shape[0])
self.wdg_img.setPixmap(q)
def _set_image(self):
print("_set_image")
self._scene.clear()
img = cv2.imread("/home/gachiemchiep/Pictures/aaa.png", 1)
img_rz = cv2.resize(img, (640, 480))
# https://stackoverflow.com/questions/34232632/convert-python-opencv-image-numpy-array-to-pyqt-qpixmap-image
height, width, channel = img_rz.shape
print(img_rz.shape)
bytesPerLine = 3 * width
img_q = QImage(img_rz.data, width, height, bytesPerLine,
QImage.Format_RGB888).rgbSwapped()
# painter : this does not work
# painter = QPainter(img_q)
# painter.setRenderHint(QPainter.Antialiasing)
# self._scene.render(painter)
# painter.end()
# pixmap : this work
pixmap = QPixmap.fromImage(img_q)
self._scene.addPixmap(pixmap)
self._scene.setSceneRect(0, 0, 640, 480)
def msg_to_pixmap(self, msg): cv_img = self.bridge.imgmsg_to_cv2(msg) h, w, _ = cv_img.shape bytesPerLine = 3 * w q_img = QImage(cv_img.data, w, h, bytesPerLine, QImage.Format_RGB888) return QPixmap.fromImage(q_img).scaled(320, 240)
def __init__(self, context):
super(MyPlugin, self).__init__(context)
Black = np.zeros((300, 400, 3), np.uint8)
self.is_decay_auto = 0
self.is_frame_auto = 0
self.is_segsz_auto = 0
self.bridge = CvBridge()
self.d_image_sub = rospy.Subscriber("/fome/direction", Image,
self.fome_dir)
self.m_image_sub = rospy.Subscriber("/fome/magnitude", Image,
self.fome_mag)
self.c_image_sub = rospy.Subscriber("/fome/cluster", Image,
self.fome_cls)
# self.r_image_sub = rospy.Subscriber("/dvs_rendering",Image,self.fome_raw)
self.r_image_sub = rospy.Subscriber("/dvs/image_raw", Image,
self.fome_raw)
self.decay_pub = rospy.Publisher("/fome/decay_rate",
String,
queue_size=1)
self.frame_pub = rospy.Publisher("/fome/frame_rate",
String,
queue_size=1)
self.segsz_pub = rospy.Publisher("/fome/segment_size",
String,
queue_size=1)
self.setObjectName('fome_gui')
rp = rospkg.RosPack()
self._widget = QWidget()
ui_file = os.path.join(rp.get_path('fome_gui'), 'resource',
'MyPlugin.ui')
loadUi(ui_file, self._widget)
self._widget.setObjectName('fome_gui')
height, width, byteValue = Black.shape
byteValue = byteValue * width
self.d_image = QImage(Black, width, height, byteValue,
QImage.Format_RGB888)
self.m_image = QImage(Black, width, height, byteValue,
QImage.Format_RGB888)
self.r_image = QImage(Black, width, height, byteValue,
QImage.Format_RGB888)
self.c_image = QImage(Black, width, height, byteValue,
QImage.Format_RGB888)
self.d_label = QLabel(self._widget.graphicsView_1)
d_image = QPixmap.fromImage(self.d_image)
self.d_label.setPixmap(d_image)
self.m_label = QLabel(self._widget.graphicsView_2)
m_image = QPixmap.fromImage(self.m_image)
self.m_label.setPixmap(m_image)
self.r_label = QLabel(self._widget.graphicsView_3)
r_image = QPixmap.fromImage(self.r_image)
self.r_label.setPixmap(r_image)
self.c_label = QLabel(self._widget.graphicsView_4)
c_image = QPixmap.fromImage(self.c_image)
self.c_label.setPixmap(c_image)
@QtCore.Slot(int)
def decay_moved(r):
if self.is_decay_auto == 0:
self.decay_pub.publish(str(r))
self.decay_rate = SliderWithValue(QtCore.Qt.Horizontal, 1)
self.decay_rate.slidervalue.connect(decay_moved)
self.decay_rate.setMinimum(-20)
self.decay_rate.setMaximum(30)
self.decay_rate.setTickInterval(1)
self.decay_rate.setSingleStep(1)
self.decay_rate.setPageStep(1)
self.decay_rate.setTickPosition(QSlider.TicksBelow)
self.decay_rate.setSizePolicy(QSizePolicy.MinimumExpanding,
QSizePolicy.Fixed)
self.decay_rate.setValue(-10)
self.decay_pub.publish(str(self.decay_rate.slidervalue))
self._widget.decay_rate.addWidget(self.decay_rate)
@QtCore.Slot(int)
def decay_auto(args):
self.is_decay_auto = args
if args == 2:
self.decay_pub.publish("auto")
self.decay_pub.unregister
del self.decay_pub
if args == 0:
self.decay_pub = rospy.Publisher("/fome/decay_rate",
String,
queue_size=1)
self._widget.decay_auto.stateChanged.connect(decay_auto)
@QtCore.Slot(int)
def frame_moved(r):
if self.is_frame_auto == 0:
self.frame_pub.publish(str(r))
self.frame_rate = SliderWithValue(QtCore.Qt.Horizontal, 1)
self.frame_rate.slidervalue.connect(frame_moved)
self.frame_rate.setMinimum(10)
self.frame_rate.setMaximum(2000)
self.frame_rate.setTickInterval(1)
self.frame_rate.setSingleStep(1)
self.frame_rate.setPageStep(1)
self.frame_rate.setTickPosition(QSlider.TicksBelow)
self.frame_rate.setSizePolicy(QSizePolicy.MinimumExpanding,
QSizePolicy.Fixed)
self.frame_rate.setValue(100)
self._widget.frame_rate.addWidget(self.frame_rate)
@QtCore.Slot(int)
def frame_auto(args):
self.is_frame_auto = args
if args == 2:
self.frame_pub.publish("auto")
self.frame_pub.unregister
del self.frame_pub
if args == 0:
self.frame_pub = rospy.Publisher("/fome/frame_rate",
String,
queue_size=1)
self._widget.frame_auto.stateChanged.connect(frame_auto)
@QtCore.Slot(int)
def segsz_moved(r):
if self.is_segsz_auto == 0:
self.segsz_pub.publish(str(r))
self.segsz_rate = SliderWithValue(QtCore.Qt.Horizontal, 1)
self.segsz_rate.slidervalue.connect(segsz_moved)
self.segsz_rate.setMinimum(100)
self.segsz_rate.setMaximum(10000)
self.segsz_rate.setTickInterval(10)
self.segsz_rate.setSingleStep(10)
self.segsz_rate.setPageStep(10)
self.segsz_rate.setTickPosition(QSlider.TicksBelow)
self.segsz_rate.setSizePolicy(QSizePolicy.MinimumExpanding,
QSizePolicy.Fixed)
self.segsz_rate.setValue(4320)
self._widget.segsz_rate.addWidget(self.segsz_rate)
@QtCore.Slot(int)
def segsz_auto(args):
self.is_segsz_auto = args
if args == 2:
self.segsz_pub.publish("auto")
self.segsz_pub.unregister
del self.segsz_pub
if args == 0:
self.segsz_pub = rospy.Publisher("/fome/segsz_rate",
String,
queue_size=1)
self._widget.segsz_auto.stateChanged.connect(segsz_auto)
if context.serial_number() > 1:
self._widget.setWindowTitle('Estimated direction from events')
context.add_widget(self._widget)
def _scale_icons(self, icon_size):
self._icon_size = icon_size
params = (self._icon_size, self._icon_size, Qt.IgnoreAspectRatio,
Qt.SmoothTransformation)
self.IMAGES = {
'green':
QImage(":/icons/stock_connect_green.png").scaled(*params),
'yellow':
QImage(":/icons/stock_connect_yellow.png").scaled(*params),
'red':
QImage(":/icons/stock_connect_red.png").scaled(*params),
'grey':
QImage(":/icons/stock_connect.png").scaled(*params),
'disconnected':
QImage(":/icons/stock_disconnect.png").scaled(*params),
'warning':
QImage(':/icons/crystal_clear_warning.png').scaled(*params),
'clock_warn':
QImage(':/icons/crystal_clear_xclock_fail.png').scaled(*params),
'cpu_warn':
QImage(':/icons/hight_load.png').scaled(*params),
'cpu_temp_warn':
QImage(':/icons/temperatur_warn.png').scaled(*params),
'hdd_warn':
QImage(':/icons/crystal_clear_hdd_warn.png').scaled(*params),
'net_warn':
QImage(':/icons/sekkyumu_net_warn.png').scaled(*params),
'mem_warn':
QImage(':/icons/mem_warn.png').scaled(*params)
}
def open_FileDialog(self):
filename = QFileDialog.getOpenFileName(self.parent, 'Open file',
os.path.expanduser('~'))
self.parent.filename = filename[0]
self.parent.qimage = QImage(self.parent.filename, 'JPEG')
def parameter_connect(self):
self.client_param = self.node.create_client(
SetParameters, '/grasp_modbus_server/set_parameters')
self.parameter_action.setIcon(QIcon(QPixmap.fromImage(QImage(self.path_red_icon))) \
if not self.client_param.wait_for_service(timeout_sec=0.5) \
else QIcon(QPixmap.fromImage(QImage(self.path_green_icon))))
def tf_connect(self):
self.client_tf = self.node.create_client(HandeyeTF,
'handeye_tf_service')
self.tf_action.setIcon(QIcon(QPixmap.fromImage(QImage(self.path_red_icon))) \
if not self.client_tf.wait_for_service(timeout_sec=0.5) \
else QIcon(QPixmap.fromImage(QImage(self.path_green_icon))))
def cvimg2qimg(cvimg):
height, width = cvimg.shape
return QImage(cvimg.data, width, height, QImage.Format_Grayscale8)
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 __init__(self,
icon,
title,
text,
detailed_text="",
buttons=Cancel | Ok,
parent=None):
QDialog.__init__(self, parent=parent)
self.setWindowFlags(self.windowFlags() & ~Qt.WindowTitleHint)
self.setWindowFlags(self.windowFlags()
& ~Qt.WindowContextHelpButtonHint
& ~Qt.WindowMinimizeButtonHint)
self.setObjectName('MessageBox')
self._use_checkbox = True
self.text = text
self.verticalLayout = QVBoxLayout(self)
self.verticalLayout.setObjectName("verticalLayout")
self.verticalLayout.setContentsMargins(1, 1, 1, 1)
self.horizontalLayout = QHBoxLayout()
self.horizontalLayout.setObjectName("horizontalLayout")
self.horizontalLayout.setContentsMargins(1, 1, 1, 1)
# create icon
pixmap = None
if icon == self.NoIcon:
pass
elif icon == self.Question:
pixmap = QPixmap(
QImage(":icons/question.png").scaled(56, 56,
Qt.IgnoreAspectRatio,
Qt.SmoothTransformation))
elif icon == self.Information:
pixmap = QPixmap(
QImage(":icons/info.png").scaled(56, 56, Qt.IgnoreAspectRatio,
Qt.SmoothTransformation))
elif icon == self.Warning:
pixmap = QPixmap(
QImage(":icons/warning.png").scaled(56, 56,
Qt.IgnoreAspectRatio,
Qt.SmoothTransformation))
elif icon == self.Critical:
pixmap = QPixmap(
QImage(":icons/critical.png").scaled(56, 56,
Qt.IgnoreAspectRatio,
Qt.SmoothTransformation))
spacerItem = QSpacerItem(10, 60, QSizePolicy.Minimum,
QSizePolicy.Minimum)
self.horizontalLayout.addItem(spacerItem)
self.icon_label = QLabel()
if pixmap is not None:
self.icon_label.setPixmap(pixmap)
self.icon_label.setSizePolicy(QSizePolicy.Fixed, QSizePolicy.Fixed)
self.horizontalLayout.addWidget(self.icon_label)
spacerItem = QSpacerItem(10, 60, QSizePolicy.Minimum,
QSizePolicy.Minimum)
self.horizontalLayout.addItem(spacerItem)
# add message
self.message_label = QLabel(text)
self.message_label.setWordWrap(True)
self.message_label.setScaledContents(True)
self.message_label.setOpenExternalLinks(True)
self.horizontalLayout.addWidget(self.message_label)
self.verticalLayout.addLayout(self.horizontalLayout)
# create buttons
self.buttonBox = QDialogButtonBox(self)
self.buttonBox.setObjectName("buttonBox")
self.buttonBox.setOrientation(Qt.Horizontal)
self._accept_button = None
self._reject_button = None
self._buttons = buttons
self._create_buttons(buttons)
self.buttonBox.accepted.connect(self.accept)
self.buttonBox.rejected.connect(self.reject)
self.verticalLayout.addWidget(self.buttonBox)
if detailed_text:
self.btn_show_details = QPushButton(self.tr('Details...'))
self.btn_show_details.setCheckable(True)
self.btn_show_details.setChecked(True)
self.btn_show_details.toggled.connect(self.on_toggled_details)
self.buttonBox.addButton(self.btn_show_details,
QDialogButtonBox.ActionRole)
# create area for detailed text
self.textEdit = textEdit = QTextEdit(self)
textEdit.setObjectName("textEdit")
textEdit.setReadOnly(True)
textEdit.setText(detailed_text)
# textEdit.setVisible(False)
self.verticalLayout.addWidget(self.textEdit)
self.resize(480, self.verticalLayout.totalSizeHint().height())
buttons_in_box = self.buttonBox.buttons()
if buttons_in_box:
self.buttonBox.buttons()[0].setFocus()
def __init__(self, context):
super(HandEyeCalibration, self).__init__(context)
self.context = context
self.node = context.node
self.widget = QWidget()
self.widget.setObjectName(self.PLUGIN_TITLE)
self.widget.setWindowTitle(self.PLUGIN_TITLE)
# Data
self.Tsamples = []
# Toolbar
_, path_pkg = get_resource('packages', 'handeye_dashboard')
print("{}".format(path_pkg))
self.snapshot_action = QAction(QIcon.fromTheme('camera-photo'),
'Take a snapshot', self.widget)
path = path_pkg + '/share/handeye_dashboard/images/capture.png'
self.calibrate_action = QAction(QIcon(QPixmap.fromImage(QImage(path))),
'Get the camera/robot transform',
self.widget)
self.clear_action = QAction(QIcon.fromTheme('edit-clear'),
'Clear the record data.', self.widget)
path = path_pkg + '/share/handeye_dashboard/images/UR5.png'
self.execut_action = QAction(QIcon(QPixmap.fromImage(QImage(path))),
'EStart the publishing the TF.',
self.widget)
self.toolbar = QToolBar()
self.toolbar.addAction(self.snapshot_action)
self.toolbar.addAction(self.calibrate_action)
self.toolbar.addAction(self.clear_action)
self.toolbar.addAction(self.execut_action)
# Toolbar0
self.l0 = QLabel(self.widget)
self.l0.setText("Camera-Mount-Type: ")
self.l0.setFixedWidth(150)
self.l0.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignVCenter)
self.combobox = QComboBox(self.widget)
self.combobox.addItem('attached on robot')
self.combobox.addItem('fixed beside robot')
self.toolbar0 = QToolBar()
self.toolbar0.addWidget(self.l0)
self.toolbar0.addWidget(self.combobox)
# Toolbar1
self.l1 = QLabel(self.widget)
self.l1.setText("Camera-Frame: ")
self.l1.setFixedWidth(150)
self.l1.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignVCenter)
self.camera_frame = QLineEdit(self.widget)
self.camera_frame.setText("camera_link")
self.toolbar1 = QToolBar()
self.toolbar1.addWidget(self.l1)
self.toolbar1.addWidget(self.camera_frame)
# Toolbar2
self.l2 = QLabel(self.widget)
self.l2.setText("Object-Frame: ")
self.l2.setFixedWidth(150)
self.l2.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignVCenter)
self.object_frame = QLineEdit(self.widget)
self.object_frame.setText("calib_board")
self.toolbar2 = QToolBar()
self.toolbar2.addWidget(self.l2)
self.toolbar2.addWidget(self.object_frame)
# Toolbar3
self.l3 = QLabel(self.widget)
self.l3.setText("Robot-Base-Frame: ")
self.l3.setFixedWidth(150)
self.l3.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignVCenter)
self.base_frame = QLineEdit(self.widget)
self.base_frame.setText("base")
self.toolbar3 = QToolBar()
self.toolbar3.addWidget(self.l3)
self.toolbar3.addWidget(self.base_frame)
# Toolbar4
self.l4 = QLabel(self.widget)
self.l4.setText("End-Effector-Frame: ")
self.l4.setFixedWidth(150)
self.l4.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignVCenter)
self.endeffector_frame = QLineEdit(self.widget)
self.endeffector_frame.setText("tool0")
self.toolbar4 = QToolBar()
self.toolbar4.addWidget(self.l4)
self.toolbar4.addWidget(self.endeffector_frame)
# Toolbar5
self.l5 = QLabel(self.widget)
self.l5.setText("Sample-Number: ")
self.l5.setFixedWidth(150)
self.l5.setAlignment(QtCore.Qt.AlignRight | QtCore.Qt.AlignVCenter)
self.le5 = QLineEdit(self.widget)
self.le5.setValidator(QIntValidator())
self.le5.setText('10')
self.le5.setReadOnly(True)
self.toolbar5 = QToolBar()
self.toolbar5.addWidget(self.l5)
self.toolbar5.addWidget(self.le5)
# TreeView
self.treeview = QTreeView()
self.treeview.setAlternatingRowColors(True)
self.model = QStandardItemModel(self.treeview)
self.treeview.setModel(self.model)
self.treeview.setHeaderHidden(True)
# TextEdit
self.textedit = QTextEdit(self.widget)
self.textedit.setReadOnly(True)
# Layout
self.layout = QVBoxLayout()
self.layout.addWidget(self.toolbar0)
self.layout.addWidget(self.toolbar1)
self.layout.addWidget(self.toolbar2)
self.layout.addWidget(self.toolbar3)
self.layout.addWidget(self.toolbar4)
self.layout.addWidget(self.toolbar5)
self.layout.addWidget(self.toolbar)
self.layoutH = QHBoxLayout()
self.layoutH.addWidget(self.treeview)
self.layoutH.addWidget(self.textedit)
self.layout.addLayout(self.layoutH)
self.widget.setLayout(self.layout)
# Add the widget to the user interface
if context.serial_number() > 1:
self.widget.setWindowTitle(self.widget.windowTitle() +
(' (%d)' % context.serial_number()))
context.add_widget(self.widget)
# Make the connections
self.snapshot_action.triggered.connect(self.take_snapshot)
self.calibrate_action.triggered.connect(self.calibration)
self.clear_action.triggered.connect(self.clear)
self.execut_action.triggered.connect(self.execution)
# Package path
self.path_pkg = path_pkg
# Set up TF
self.cli = self.node.create_client(HandeyeTF, 'handeye_tf_service')
while not self.cli.wait_for_service(timeout_sec=1.0):
self.node.get_logger().info(
'service not available, waiting again...')
self.req = HandeyeTF.Request()
def image(self, name):
return QImage(self.icon_path(name))
def __init__(self, context):
super(SensorMonitor, self).__init__(context)
# Give QObjects reasonable names
self.setObjectName('SensorMonitor')
# 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('sensor_monitor'),
'resource', 'sensor_monitor.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('SensorMonitorUi')
# 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)
self._widget.record_btn.clicked[bool].connect(self.on_record_clicked)
img_file = os.path.join(rospkg.RosPack().get_path('sensor_monitor'),
'resource', 'red.png')
self.abnormal_image = QImage(img_file).scaled(QSize(15, 15))
img_file = os.path.join(rospkg.RosPack().get_path('sensor_monitor'),
'resource', 'green.png')
self.normal_image = QImage(img_file).scaled(QSize(15, 15))
self.sensors = {}
self.ts_list = {}
self.freq = {}
self.has_msg = {}
self.bag = None
self.write_queue = Queue.Queue()
# self.write_thread = threading.Thread(target=self.run_write)
self.write_thread = None
rospy.Timer(rospy.Duration(1), self.timer_callback)
self.add_sensor("PointGreyLeft",
"/pointgrey/left/image_raw/compressed",
CompressedImage, 16.0)
self.add_sensor("PointGreyRight",
"/pointgrey/right/image_raw/compressed",
CompressedImage, 16.0)
self.add_sensor("IMU", "/mti/sensor/imu", Imu, 400.0)
self.add_sensor("Velodyne", "/velodyne_points", PointCloud2, 10.0)
self.add_sensor("GPS", "/rtk_gps/gps", GPSFix, 5.0)
self.add_sensor("Velocity", "/thomas_velocity_controller/cmd_vel",
Twist, 20.0)
self.add_sensor("Odometry", "/thomas_robot/odom", Odometry, 50.0)
