def paintEvent(self, event):
QWidget.paintEvent(self, event)
pmap = self._pixmap
if pmap.isNull():
return
w, h = pmap.width(), pmap.height()
cw, ch = self.rect().width(), self.rect().height()
scaled, nw, nh = fit_image(w, h, cw, ch)
if scaled:
pmap = pmap.scaled(nw, nh, Qt.IgnoreAspectRatio,
Qt.SmoothTransformation)
w, h = pmap.width(), pmap.height()
x = int(abs(cw - w) / 2.)
y = int(abs(ch - h) / 2.)
target = QRect(x, y, w, h)
p = QPainter(self)
p.setRenderHints(QPainter.Antialiasing
| QPainter.SmoothPixmapTransform)
p.drawPixmap(target, pmap)
pen = QPen()
pen.setWidth(self.BORDER_WIDTH)
p.setPen(pen)
if self.draw_border:
p.drawRect(target)
#p.drawRect(self.rect())
p.end()
def paintEvent(self, event):
QWidget.paintEvent(self, event)
pmap = self._pixmap
if pmap.isNull():
return
w, h = pmap.width(), pmap.height()
cw, ch = self.rect().width(), self.rect().height()
scaled, nw, nh = fit_image(w, h, cw, ch)
if scaled:
pmap = pmap.scaled(nw, nh, Qt.IgnoreAspectRatio,
Qt.SmoothTransformation)
w, h = pmap.width(), pmap.height()
x = int(abs(cw - w)/2.)
y = int(abs(ch - h)/2.)
target = QRect(x, y, w, h)
p = QPainter(self)
p.setRenderHints(QPainter.Antialiasing | QPainter.SmoothPixmapTransform)
p.drawPixmap(target, pmap)
pen = QPen()
pen.setWidth(self.BORDER_WIDTH)
p.setPen(pen)
if self.draw_border:
p.drawRect(target)
#p.drawRect(self.rect())
p.end()
def penMoveEvent(self, pos, pressure, liftedDeque):
pen_x = pos[0]
pen_y = pos[1]
pen_pressure = pressure
# print(liftedDeque)
# print(self.__lastPos)
if self.__lastPos is None:
self.__lastPos = QPoint(pen_x, pen_y)
self.__currentPos = QPoint(pen_x, pen_y)
self.__painter.begin(self.__board)
if self.EraserMode == False:
#Non-Eraser mode
self.__penColor = QColor("blue")
self.__painter.setPen(QPen(
self.__penColor, self.__thickness)) #Set pen color, thickness
else:
#Eraser mode: pen color is white, thickness is 6
self.__painter.setPen(QPen(Qt.white, 6))
self.__painter.drawLine(self.__lastPos, self.__currentPos)
self.__painter.end()
self.__lastPos = self.__currentPos
self.update() #Show updates
# If ever detected the pen is lifted, reset the __lastPos variable in order to reposition the pen
if (True in liftedDeque):
self.__lastPos = None
def penMoveEvent(self, pos, pressure):
pen_x = pos[0]
pen_y = pos[1]
pen_pressure = pressure
if self.__lastPos is None:
self.__lastPos = QPoint(pen_x, pen_y)
elif (abs(pen_x - self.__lastPos.x()) > 21
or abs(pen_y - self.__lastPos.y()) > 21):
self.__lastPos = QPoint(pen_x, pen_y)
self.__currentPos = QPoint(pen_x, pen_y)
self.__painter.begin(self.__board)
if self.EraserMode == False:
#Non-Eraser mode
self.__painter.setPen(QPen(
self.__penColor, self.__thickness)) #Set pen color, thickness
else:
#Eraser mode: pen color is white, thickness is 6
self.__painter.setPen(QPen(Qt.white, 6))
self.__painter.drawLine(self.__lastPos, self.__currentPos)
self.__painter.end()
self.__lastPos = self.__currentPos
self.update() #Show updates
def paintEvent(self, event):
QWidget.paintEvent(self, event)
pmap = self._pixmap
if pmap.isNull():
return
w, h = pmap.width(), pmap.height()
ow, oh = w, h
cw, ch = self.rect().width(), self.rect().height()
scaled, nw, nh = fit_image(w, h, cw, ch)
if scaled:
pmap = pmap.scaled(nw, nh, Qt.IgnoreAspectRatio, Qt.SmoothTransformation)
w, h = pmap.width(), pmap.height()
x = int(abs(cw - w) / 2.0)
y = int(abs(ch - h) / 2.0)
target = QRect(x, y, w, h)
p = QPainter(self)
p.setRenderHints(QPainter.Antialiasing | QPainter.SmoothPixmapTransform)
p.drawPixmap(target, pmap)
if self.draw_border:
pen = QPen()
pen.setWidth(self.BORDER_WIDTH)
p.setPen(pen)
p.drawRect(target)
if self.show_size:
sztgt = target.adjusted(0, 0, 0, -4)
f = p.font()
f.setBold(True)
p.setFont(f)
sz = u"\u00a0%d x %d\u00a0" % (ow, oh)
flags = Qt.AlignBottom | Qt.AlignRight | Qt.TextSingleLine
szrect = p.boundingRect(sztgt, flags, sz)
p.fillRect(szrect.adjusted(0, 0, 0, 4), QColor(0, 0, 0, 200))
p.setPen(QPen(QColor(255, 255, 255)))
p.drawText(sztgt, flags, sz)
p.end()
def paintArc(self, center_x, center_y, start_x, start_y, end_x, end_y):
radius = math.sqrt(
math.pow(center_x - start_x, 2) + math.pow(center_y - start_y, 2))
rect = QRectF(center_x - radius, center_y - radius, radius * 2,
radius * 2)
# start angle calculation
startAngle = math.degrees(
math.atan2(center_y - start_y, start_x - center_x))
# end angle calculation
endAngle = math.degrees(math.atan2(center_y - end_y, end_x - center_x))
# span angle calculation
spanAngle = endAngle - startAngle
# assume user always wants to draw clock-wise
if spanAngle > 0:
spanAngle = -1 * (360 - spanAngle)
#print("start angle is " + str(startAngle))
#print("span angle is " + str(spanAngle))
self.__painter.begin(self.__board)
self.__penColor = QColor("black")
self.__painter.setPen(QPen(self.__penColor, self.__thickness))
self.__painter.drawArc(rect, 16 * startAngle, 16 * spanAngle)
self.__painter.end()
self.update() #Show updates
def paintEvent(self, event):
pmap = self.blank if self.pixmap is None or self.pixmap.isNull(
) else self.pixmap
target = self.rect()
scaled, width, height = fit_image(pmap.width(), pmap.height(),
target.width(), target.height())
target.setRect(target.x(), target.y(), width, height)
p = QPainter(self)
p.setRenderHints(QPainter.Antialiasing
| QPainter.SmoothPixmapTransform)
p.drawPixmap(target, pmap)
if self.pixmap is not None and not self.pixmap.isNull():
sztgt = target.adjusted(0, 0, 0, -4)
f = p.font()
f.setBold(True)
p.setFont(f)
sz = u'\u00a0%d x %d\u00a0' % (self.pixmap.width(),
self.pixmap.height())
flags = Qt.AlignBottom | Qt.AlignRight | Qt.TextSingleLine
szrect = p.boundingRect(sztgt, flags, sz)
p.fillRect(szrect.adjusted(0, 0, 0, 4), QColor(0, 0, 0, 200))
p.setPen(QPen(QColor(255, 255, 255)))
p.drawText(sztgt, flags, sz)
p.end()
def __init__(self, spurset, fef, parent):
chart.__init__(self, spurset, fef, parent)
self.plot = QwtPlot(parent)
self.plot.setAxisScale(xaxis, self.spurset.RFmin, self.spurset.RFmax)
self.plot.setAxisScale(yaxis, -self.spurset.dspan / 2,
self.spurset.dspan / 2)
self.plot.setCanvasBackground(Qt.white)
grid = QwtPlotGrid()
grid.setMajPen(QPen(Qt.black, 1, Qt.DotLine))
grid.attach(self.plot)
self.plot.insertLegend(QwtLegend(self.parent), QwtPlot.ExternalLegend)
# a picker to be used for the front-end filter parallelogram
self.picker = QwtPlotPicker(xaxis, yaxis, QwtPicker.PointSelection,
QwtPlotPicker.NoRubberBand,
QwtPicker.AlwaysOff, self.plot.canvas())
# gonna need this to implement ondrop()
self._mouseRelease = self.picker.widgetMouseReleaseEvent
self._picked_obj = None
self.picker.connect(self.picker, SIGNAL('appended(const QPoint&)'),
self.onpick)
self.picker.connect(self.picker, SIGNAL('moved(const QPoint&)'),
self.ondrag)
# all about the monkey-patching
self.picker.widgetMouseReleaseEvent = self.ondrop
def drawScaleContents(self, painter, center, radius):
dir = 90 #360 - int(roud(self.origin() - self.value()))
arc = 90 #+ int(round(self.gradient * 90))
skyColor = QColor(38, 151, 221)
painter.save()
painter.setBrush(skyColor)
painter.drawChord(self.scaleContentsRect(), (dir - arc) * 16,
2 * arc * 16)
direction1 = self.value() * M_PI / 180.0
direction2 = self.value() * M_PI / 180.0 + M_PI_2
triangleSize = qRound(radius * 0.15)
p0 = (QPoint(center.x() + 0, center.y() + 0))
p1 = qwtPolar2Pos(p0, 2, direction2)
pa = QPolygon(3)
pa.setPoint(0, qwtPolar2Pos(p1, 2 * triangleSize, direction2))
pa.setPoint(1, qwtPolar2Pos(p1, triangleSize, direction2 + M_PI_2))
pa.setPoint(2, qwtPolar2Pos(p1, triangleSize, direction2 - M_PI_2))
color = self.palette().color(QPalette.Text)
painter.setBrush(color)
painter.drawPolygon(pa)
p0 = (QPoint(center.x() + 0, center.y() + 0))
color = self.palette().color(QPalette.Text)
painter.setBrush(color)
painter.setPen(QPen(color, 3))
painter.drawLine(qwtPolar2Pos(p0, radius - 3, direction1),
qwtPolar2Pos(p0, radius - 3, direction1 - M_PI))
painter.restore()
def paint_non_printing(self, painter, option, charcode):
text = self.np_pat.sub(r'\n\1', non_printing[charcode])
painter.drawText(
option.rect,
Qt.AlignCenter | Qt.TextWordWrap | Qt.TextWrapAnywhere, text)
painter.setPen(QPen(Qt.DashLine))
painter.drawRect(option.rect.adjusted(1, 1, -1, -1))
def paintEvent(self, event):
canvas_size = self.rect()
width = self.current_pixmap_size.width()
extrax = canvas_size.width() - width
if extrax < 0: extrax = 0
x = int(extrax / 2.)
height = self.current_pixmap_size.height()
extray = canvas_size.height() - height
if extray < 0: extray = 0
y = int(extray / 2.)
target = QRect(x, y, width, height)
p = QPainter(self)
p.setRenderHints(QPainter.Antialiasing
| QPainter.SmoothPixmapTransform)
p.drawPixmap(
target,
self.pixmap.scaled(target.size(), Qt.KeepAspectRatio,
Qt.SmoothTransformation))
if gprefs['bd_overlay_cover_size']:
sztgt = target.adjusted(0, 0, 0, -4)
f = p.font()
f.setBold(True)
p.setFont(f)
sz = u'\u00a0%d x %d\u00a0' % (self.pixmap.width(),
self.pixmap.height())
flags = Qt.AlignBottom | Qt.AlignRight | Qt.TextSingleLine
szrect = p.boundingRect(sztgt, flags, sz)
p.fillRect(szrect.adjusted(0, 0, 0, 4), QColor(0, 0, 0, 200))
p.setPen(QPen(QColor(255, 255, 255)))
p.drawText(sztgt, flags, sz)
p.end()
def paintPolyg(self, x1, y1, x2, y2, x3, y3, x4, y4):
self.__painter.begin(self.__board)
self.__penColor = QColor("black")
self.__painter.setPen(QPen(self.__penColor, self.__thickness))
self.__painter.drawPolygon(QPoint(x1, y1), QPoint(x2, y2),
QPoint(x3, y3), QPoint(x4, y4))
self.__painter.end()
self.update() #Show updates
def __init__(self, plot, label, color="green", mode=QwtPicker.PointSelection,
rubber_band=QwtPicker.VLineRubberBand, tracker_mode=QwtPicker.ActiveOnly, track=None):
QwtPlotPicker.__init__(self, QwtPlot.xBottom, QwtPlot.yRight, mode, rubber_band, tracker_mode,
plot.canvas())
self.plot = plot
self.label = label
self.track = track
self.color = QColor(color)
self.setRubberBandPen(QPen(self.color))
def paintTriangle(self, points):
self.__painter.begin(self.__board)
self.__penColor = QColor("black")
self.__painter.setPen(QPen(self.__penColor, self.__thickness))
self.__painter.drawPolygon(points)
self.__painter.end()
self.update() #Show updates
def failed_img(self):
if self._failed_img is None:
i = QImage(200, 150, QImage.Format_ARGB32)
i.fill(Qt.white)
p = QPainter(i)
r = i.rect().adjusted(10, 10, -10, -10)
n = QPen(Qt.DashLine)
n.setColor(Qt.black)
p.setPen(n)
p.drawRect(r)
p.setPen(Qt.black)
f = self.font()
f.setPixelSize(20)
p.setFont(f)
p.drawText(r.adjusted(10, 0, -10, 0), Qt.AlignCenter | Qt.TextWordWrap, _('Image could not be rendered'))
p.end()
self._failed_img = QPixmap.fromImage(i)
return self._failed_img
def addPlotBorder(self, border_pen, label, label_color=None, bg_brush=None):
# make plot items for image frame
# make curve for image borders
(l0, l1), (m0, m1) = self.image.getExtents()
self._border_pen = QPen(border_pen)
self._image_border.show()
self._image_border.setData([l0, l0, l1, l1, l0], [m0, m1, m1, m0, m0])
self._image_border.setPen(self._border_pen)
self._image_border.setZ(self.image.z() + 1 if self._z_markers is None else self._z_markers)
if label:
self._image_label.show()
self._image_label_text = text = QwtText(" %s " % label)
text.setColor(label_color)
text.setBackgroundBrush(bg_brush)
self._image_label.setValue(l1, m1)
self._image_label.setLabel(text)
self._image_label.setLabelAlignment(Qt.AlignRight | Qt.AlignVCenter)
self._image_label.setZ(self.image.z() + 2 if self._z_markers is None else self._z_markers)
def paintEllipse(self, center_x, center_y, radias1, radias2):
self.__painter.begin(self.__board)
self.__penColor = QColor("black")
self.__painter.setPen(QPen(self.__penColor, self.__thickness))
self.__painter.drawEllipse(QPoint(center_x, center_y), radias1,
radias2)
self.__painter.end()
self.update() #Show updates
def paintLine(self, P1_x, P1_y, P2_x, P2_y):
P1 = QPoint(P1_x, P1_y)
P2 = QPoint(P2_x, P2_y)
self.__painter.begin(self.__board)
self.__painter.setPen(QPen(self.__penColor, self.__thickness))
self.__painter.drawLine(P1, P2)
self.__painter.end()
self.update() #Show updates
def copy(self):
ans = GraphicsState()
ans.fill = QBrush(self.fill)
ans.stroke = QPen(self.stroke)
ans.opacity = self.opacity
ans.transform = self.transform * QTransform()
ans.brush_origin = QPointF(self.brush_origin)
ans.clip_updated = self.clip_updated
ans.do_fill, ans.do_stroke = self.do_fill, self.do_stroke
return ans
def __init__(self):
self.fill = QBrush(Qt.white)
self.stroke = QPen()
self.opacity = 1.0
self.transform = QTransform()
self.brush_origin = QPointF()
self.clip_updated = False
self.do_fill = False
self.do_stroke = True
self.qt_pattern_cache = {}
def failed_img(self):
if self._failed_img is None:
i = QImage(200, 150, QImage.Format_ARGB32)
i.fill(Qt.white)
p = QPainter(i)
r = i.rect().adjusted(10, 10, -10, -10)
n = QPen(Qt.DashLine)
n.setColor(Qt.black)
p.setPen(n)
p.drawRect(r)
p.setPen(Qt.black)
f = self.font()
f.setPixelSize(20)
p.setFont(f)
p.drawText(r.adjusted(10, 0, -10, 0),
Qt.AlignCenter | Qt.TextWordWrap,
_('Image could not be rendered'))
p.end()
self._failed_img = QPixmap.fromImage(i)
return self._failed_img
def paintRect(self, center_x, center_y, upper_left_x, upper_left_y):
width = abs(2 * (center_x - upper_left_x))
height = abs(2 * (center_y - upper_left_y))
self.__painter.begin(self.__board)
self.__penColor = QColor("black")
self.__painter.setPen(QPen(self.__penColor, self.__thickness))
self.__painter.drawRect(upper_left_x, upper_left_y, width, height)
self.__painter.end()
self.update() #Show updates
def paintEvent(self, event):
QWidget.paintEvent(self, event)
pmap = self._pixmap
if pmap.isNull():
return
w, h = pmap.width(), pmap.height()
ow, oh = w, h
cw, ch = self.rect().width(), self.rect().height()
scaled, nw, nh = fit_image(w, h, cw, ch)
if scaled:
pmap = pmap.scaled(nw, nh, Qt.IgnoreAspectRatio,
Qt.SmoothTransformation)
w, h = pmap.width(), pmap.height()
x = int(abs(cw - w) / 2.)
y = int(abs(ch - h) / 2.)
target = QRect(x, y, w, h)
p = QPainter(self)
p.setRenderHints(QPainter.Antialiasing
| QPainter.SmoothPixmapTransform)
p.drawPixmap(target, pmap)
if self.draw_border:
pen = QPen()
pen.setWidth(self.BORDER_WIDTH)
p.setPen(pen)
p.drawRect(target)
if self.show_size:
sztgt = target.adjusted(0, 0, 0, -4)
f = p.font()
f.setBold(True)
p.setFont(f)
sz = u'\u00a0%d x %d\u00a0' % (ow, oh)
flags = Qt.AlignBottom | Qt.AlignRight | Qt.TextSingleLine
szrect = p.boundingRect(sztgt, flags, sz)
p.fillRect(szrect.adjusted(0, 0, 0, 4), QColor(0, 0, 0, 200))
p.setPen(QPen(QColor(255, 255, 255)))
p.drawText(sztgt, flags, sz)
p.end()
def paintArc(self, center_x, center_y, start_x, start_y, end_x, end_y):
radius = math.sqrt(
math.pow(center_x - start_x, 2) + math.pow(center_y - start_y, 2))
rect = QRectF(center_x - radius, center_y - radius, radius * 2,
radius * 2)
startAngle = 16 * math.atan2(start_x - center_y,
start_x - center_x) * 180.0 / math.pi
endAngle = 16 * math.atan2(end_y - center_y,
end_x - center_x) * 180.0 / math.pi
spanAngle = endAngle - startAngle
self.__painter.begin(self.__board)
self.__painter.setPen(QPen(self.__penColor, self.__thickness))
self.__painter.drawArc(rect, startAngle, spanAngle)
self.__painter.end()
self.update() #Show updates
def initBumperGraphics(self):
# Pens
self.blue_pen = QPen(QColor(0,0,255))
self.blue_pen.setWidth(10)
self.bumper_lines = []
# Text state labels
self.bumper_state_labels = [QGraphicsTextItem() for i in range(0,4)]
for i in range(len(self.bumper_state_labels)):
self.bumper_state_labels[i].setFont(QFont('Ubuntu', 14, QFont.Bold))
self.bumper_state_labels[i].setPlainText('00')
self.bumper_state_labels[0].setPos(self.bumper_fl_x-10, self.bumper_fl_y + 55)
self.bumper_state_labels[1].setPos(self.bumper_fr_x-10, self.bumper_fr_y + 55)
self.bumper_state_labels[2].setPos(self.bumper_rl_x-10, self.bumper_rl_y - 80)
self.bumper_state_labels[3].setPos(self.bumper_rr_x-10, self.bumper_rr_y - 80)
# Bumper indicator lines
self.bumperLine(self.bumper_fl_x - 20, self.bumper_fl_y - self.bumper_dy, True)
self.bumperLine(self.bumper_fl_x - self.bumper_dx, self.bumper_fl_y - 20, False)
self.bumperLine(self.bumper_fl_x + self.bumper_dx, self.bumper_fl_y - 20, False)
self.bumperLine(self.bumper_fl_x - 20, self.bumper_fl_y + self.bumper_dy, True)
self.bumperLine(self.bumper_fr_x - 20, self.bumper_fr_y - self.bumper_dy, True)
self.bumperLine(self.bumper_fr_x - self.bumper_dx, self.bumper_fr_y - 20, False)
self.bumperLine(self.bumper_fr_x + self.bumper_dx, self.bumper_fr_y - 20, False)
self.bumperLine(self.bumper_fr_x - 20, self.bumper_fr_y + self.bumper_dy, True)
self.bumperLine(self.bumper_rl_x - 20, self.bumper_rl_y - self.bumper_dy, True)
self.bumperLine(self.bumper_rl_x - self.bumper_dx, self.bumper_rl_y - 20, False)
self.bumperLine(self.bumper_rl_x + self.bumper_dx, self.bumper_rl_y - 20, False)
self.bumperLine(self.bumper_rl_x - 20, self.bumper_rl_y + self.bumper_dy, True)
self.bumperLine(self.bumper_rr_x - 20, self.bumper_rr_y - self.bumper_dy, True)
self.bumperLine(self.bumper_rr_x - self.bumper_dx, self.bumper_rr_y - 20, False)
self.bumperLine(self.bumper_rr_x + self.bumper_dx, self.bumper_rr_y - 20, False)
self.bumperLine(self.bumper_rr_x - 20, self.bumper_rr_y + self.bumper_dy, True)
# Populate scene
graphics_scene = QGraphicsScene()
for bumper in self.bumper_lines:
graphics_scene.addItem(bumper)
for label in self.bumper_state_labels:
graphics_scene.addItem(label)
graphics_scene.setSceneRect(0, 0, self._widget.bumperGraphicsView.width() - 4, self._widget.bumperGraphicsView.height() - 4)
self._widget.bumperGraphicsView.setScene(graphics_scene)
self._widget.bumperGraphicsView.setBackgroundBrush(QBrush(QImage(os.path.join(rospkg.RosPack().get_path('mobility_base_tools'), 'images', 'mb_top.png'))))
self._widget.bumperGraphicsView.show()
def draw(self, painter, center, length, direction, cg):
direction1 = direction * M_PI / 180.0
triangleSize = qRound(length * 0.1)
painter.save()
p0 = (QPoint(center.x() + 0, center.y() + 0))
p1 = qwtPolar2Pos(p0, length - 2 * triangleSize - 2, direction1)
pa = QPolygon(3)
pa.setPoint(0, qwtPolar2Pos(p1, 2 * triangleSize, direction1))
pa.setPoint(1, qwtPolar2Pos(p1, triangleSize, direction1 + M_PI_2))
pa.setPoint(2, qwtPolar2Pos(p1, triangleSize, direction1 - M_PI_2))
color = self.palette().color(cg, QPalette.Text)
painter.setBrush(color)
painter.drawPolygon(pa)
painter.setPen(QPen(color, 3))
painter.drawLine(qwtPolar2Pos(p0, length - 2, direction1 + M_PI_2),
qwtPolar2Pos(p0, length - 2, direction1 - M_PI_2))
painter.restore()
def paintEvent(self, event):
res = QToolButton.paintEvent(self, event)
if _enable_hacked_QToolButton_paintEvent and self.isChecked():
r = event.rect()
x,y = r.left(), r.top()
w,h = r.width(), r.height()
# There are at least three rect sizes: 30x35, 30x36, 32x33 (see debug-print below).
# Not sure if this sometimes includes some padding around the icon.
## print r.top(),r.bottom(),r.height(), r.left(),r.right(),r.width()
## 0 29 30 0 34 35
## 0 29 30 0 35 36
## 0 31 32 0 32 33
p = QPainter(self, True) # True claims to mean un-clipped -- not sure if this is good.
## Qt4 error: TypeError: too many arguments to QPainter(), 1 at most expected
color = toolbutton_highlight_color
p.setPen(QPen(color, 3)) # try 3 and 2
p.drawRect(x+2,y+2,w-4,h-4) #e could also try drawRoundRect(r, xroundedness, yroundedness)
return res
def paintBezierSpline(self, pointListX, pointListY):
P1 = QPoint(int(pointListX[0]), int(pointListY[0]))
path = QtGui.QPainterPath()
path.moveTo(P1)
self.__painter.begin(self.__board)
self.__painter.setPen(QPen(self.__penColor, self.__thickness))
i = 0
while i < len(pointListX) - 3:
P2 = QPoint(int(pointListX[i + 1]), int(pointListY[i + 1]))
P3 = QPoint(int(pointListX[i + 2]), int(pointListY[i + 2]))
P4 = QPoint(int(pointListX[i + 3]), int(pointListY[i + 3]))
path.cubicTo(P2, P3, P4)
self.__painter.drawPath(path)
i += 3
self.__painter.end()
self.update() #Show updates
def paintAuxArc(self, center_x, center_y, start_x, start_y, end_x, end_y):
radius = math.sqrt(
math.pow(center_x - start_x, 2) + math.pow(center_y - start_y, 2))
rect = QRectF(center_x - radius, center_y - radius, radius * 2,
radius * 2)
# start angle calculation
startAngle = math.degrees(
math.atan2(center_y - start_y, start_x - center_x))
# end angle calculation
endAngle = math.degrees(math.atan2(center_y - end_y, end_x - center_x))
# span angle calculation
spanAngle = 0
if (self._isAngleInThirdQuadrant(startAngle)
and self._isAngleInSecondQuadrant(endAngle)
) or (self._isAngleInThirdQuadrant(endAngle)
and self._isAngleInSecondQuadrant(startAngle)) or (
startAngle == 180
and self._isAngleInThirdQuadrant(endAngle)
) or (startAngle == -180
and self._isAngleInSecondQuadrant(endAngle)) or (
endAngle == 180
and self._isAngleInThirdQuadrant(startAngle)) or (
endAngle == -180
and self._isAngleInSecondQuadrant(startAngle)):
if startAngle == 180 or self._isAngleInThirdQuadrant(startAngle):
spanAngle = endAngle - (startAngle + 360)
else:
spanAngle = (endAngle + 360) - startAngle
else:
spanAngle = endAngle - startAngle
#print("start angle is " + str(startAngle))
#print("span angle is " + str(spanAngle))
self.__painter.begin(self.__board)
self.__penColor = QColor("red")
self.__painter.setPen(QPen(self.__penColor, self.__thickness))
self.__painter.drawArc(rect, 16 * startAngle, 16 * spanAngle)
self.__painter.end()
self.update() #Show updates
def background(painter, bkgimg):
maxx = painter.device().width()
maxy = painter.device().height()
rimg = QRectF(0, 0, maxx, maxy * .9)
#
painter.fillRect(0, 0, maxx, maxy, QBrush(Qt.red, Qt.SolidPattern))
painter.drawImage(rimg, bkgimg)
wwh = QColor(255, 255, 255, 128)
painter.fillRect(0, 2 * maxy / 10, maxx, 4 * maxy / 10,
QBrush(wwh, Qt.SolidPattern))
u = QRectF(0, 9 * maxy / 10, maxx, maxy / 10)
penHText = QPen(Qt.white)
painter.setPen(penHText)
painter.setFont(QFont("Arial", 16, italic=True))
painter.drawText(
u, Qt.AlignLeft | Qt.TextIncludeTrailingSpaces | Qt.AlignVCenter,
" ekskursja.pl/flashcards")
def __init__(self, plot, color="black", linestyle=Qt.DotLine, align=Qt.AlignBottom | Qt.AlignRight, z=90,
label="", zlabel=None, linewidth=1, spacing=2,
yaxis=QwtPlot.yRight):
self.line = TiggerPlotCurve()
self.color = color = color if isinstance(color, QColor) else QColor(color)
self.line.setPen(QPen(color, linewidth, linestyle))
self.marker = TiggerPlotMarker()
self.marker.setLabelAlignment(align)
try:
self.marker.setSpacing(spacing)
except AttributeError:
pass
self.setText(label)
self.line.setZ(z)
self.marker.setZ(zlabel if zlabel is not None else z)
# set axes -- using yRight, since that is the "markup" z-axis
self.line.setAxis(QwtPlot.xBottom, yaxis)
self.marker.setAxis(QwtPlot.xBottom, yaxis)
# attach to plot
self.line.attach(plot)
self.marker.attach(plot)
def paintPCircle(self, center_x, center_y, start_x, start_y, pers_x,
pers_y):
point = [start_x, start_y]
center = [center_x, center_y]
distance = [(point[0] - center[0]), (point[1] - center[1])]
phi = math.atan2(distance[1], distance[0])
r = math.sqrt(distance[0] * distance[0] + distance[1] * distance[1])
points = self.PointsInCircum(r, 300, phi)
#Drawing the circle in perspective
dist_x = center_x - pers_x
print("dist_x", dist_x, pers_x, pers_y, center_x, center_y)
points = [(j[0], (j[1] + center_x - pers_y) *
((j[0] + center_x) / dist_x) + pers_y - center_y)
for j in points]
self.__painter.begin(self.__board)
self.__penColor = QColor("blue")
self.__painter.setPen(QPen(self.__penColor, self.__thickness))
for x in points:
self.__painter.drawPoint(round(x[0] + center[0], 3),
round(x[1] / 1.00 + center[1], 3))
self.__painter.end()
def do_paint(self, painter, option, index):
text = unicode(index.data(Qt.DisplayRole).toString())
font = QFont(option.font)
font.setPointSize(QFontInfo(font).pointSize() * 1.5)
font2 = QFont(font)
font2.setFamily(text)
system, has_latin = writing_system_for_font(font2)
if has_latin:
font = font2
r = option.rect
if option.state & QStyle.State_Selected:
painter.setPen(QPen(option.palette.highlightedText(), 0))
if (option.direction == Qt.RightToLeft):
r.setRight(r.right() - 4)
else:
r.setLeft(r.left() + 4)
painter.setFont(font)
painter.drawText(r,
Qt.AlignVCenter | Qt.AlignLeading | Qt.TextSingleLine,
text)
if (system != QFontDatabase.Any):
w = painter.fontMetrics().width(text + " ")
painter.setFont(font2)
sample = QFontDatabase().writingSystemSample(system)
if (option.direction == Qt.RightToLeft):
r.setRight(r.right() - w)
else:
r.setLeft(r.left() + w)
painter.drawText(
r, Qt.AlignVCenter | Qt.AlignLeading | Qt.TextSingleLine,
sample)
class ImageController(QFrame):
"""An ImageController is a widget for controlling the display of one image.
It can emit the following signals from the image:
raise raise button was clicked
center center-on-image option was selected
unload unload option was selected
slice image slice has changed, need to redraw (emitted by SkyImage automatically)
repaint image display range or colormap has changed, need to redraw (emitted by SkyImage automatically)
"""
def __init__(self, image, parent, imgman, name=None, save=False):
QFrame.__init__(self, parent)
self.setFrameStyle(QFrame.StyledPanel | QFrame.Raised)
# init state
self.image = image
self._imgman = imgman
self._currier = PersistentCurrier()
self._control_dialog = None
# create widgets
self._lo = lo = QHBoxLayout(self)
lo.setContentsMargins(0, 0, 0, 0)
lo.setSpacing(2)
# raise button
self._wraise = QToolButton(self)
lo.addWidget(self._wraise)
self._wraise.setIcon(pixmaps.raise_up.icon())
self._wraise.setAutoRaise(True)
self._can_raise = False
QObject.connect(self._wraise, SIGNAL("clicked()"), self._raiseButtonPressed)
self._wraise.setToolTip("""<P>Click here to raise this image above other images. Hold the button down briefly to
show a menu of image operations.</P>""")
# center label
self._wcenter = QLabel(self)
self._wcenter.setPixmap(pixmaps.center_image.pm())
self._wcenter.setToolTip(
"<P>The plot is currently centered on (the reference pixel %d,%d) of this image.</P>" % self.image.referencePixel())
lo.addWidget(self._wcenter)
# name/filename label
self.name = image.name
self._wlabel = QLabel(self.name, self)
self._number = 0
self.setName(self.name)
self._wlabel.setToolTip("%s %s" % (image.filename, "\u00D7".join(map(str, image.data().shape))))
lo.addWidget(self._wlabel, 1)
# if 'save' is specified, create a "save" button
if save:
self._wsave = QToolButton(self)
lo.addWidget(self._wsave)
self._wsave.setText("save")
self._wsave.setAutoRaise(True)
self._save_dir = save if isinstance(save, str) else "."
QObject.connect(self._wsave, SIGNAL("clicked()"), self._saveImage)
self._wsave.setToolTip("""<P>Click here to write this image to a FITS file.</P>""")
# render control
dprint(2, "creating RenderControl")
self._rc = RenderControl(image, self)
dprint(2, "done")
# selectors for extra axes
self._wslicers = []
curslice = self._rc.currentSlice(); # this may be loaded from config, so not necessarily 0
for iextra, axisname, labels in self._rc.slicedAxes():
if axisname.upper() not in ["STOKES", "COMPLEX"]:
lbl = QLabel("%s:" % axisname, self)
lo.addWidget(lbl)
else:
lbl = None
slicer = QComboBox(self)
self._wslicers.append(slicer)
lo.addWidget(slicer)
slicer.addItems(labels)
slicer.setToolTip("""<P>Selects current slice along the %s axis.</P>""" % axisname)
slicer.setCurrentIndex(curslice[iextra])
QObject.connect(slicer, SIGNAL("activated(int)"), self._currier.curry(self._rc.changeSlice, iextra))
# min/max display ranges
lo.addSpacing(5)
self._wrangelbl = QLabel(self)
lo.addWidget(self._wrangelbl)
self._minmaxvalidator = FloatValidator(self)
self._wmin = QLineEdit(self)
self._wmax = QLineEdit(self)
width = self._wmin.fontMetrics().width("1.234567e-05")
for w in self._wmin, self._wmax:
lo.addWidget(w, 0)
w.setValidator(self._minmaxvalidator)
w.setMaximumWidth(width)
w.setMinimumWidth(width)
QObject.connect(w, SIGNAL("editingFinished()"), self._changeDisplayRange)
# full-range button
self._wfullrange = QToolButton(self)
lo.addWidget(self._wfullrange, 0)
self._wfullrange.setIcon(pixmaps.zoom_range.icon())
self._wfullrange.setAutoRaise(True)
QObject.connect(self._wfullrange, SIGNAL("clicked()"), self.renderControl().resetSubsetDisplayRange)
rangemenu = QMenu(self)
rangemenu.addAction(pixmaps.full_range.icon(), "Full subset", self.renderControl().resetSubsetDisplayRange)
for percent in (99.99, 99.9, 99.5, 99, 98, 95):
rangemenu.addAction("%g%%" % percent, self._currier.curry(self._changeDisplayRangeToPercent, percent))
self._wfullrange.setPopupMode(QToolButton.DelayedPopup)
self._wfullrange.setMenu(rangemenu)
# update widgets from current display range
self._updateDisplayRange(*self._rc.displayRange())
# lock button
self._wlock = QToolButton(self)
self._wlock.setIcon(pixmaps.unlocked.icon())
self._wlock.setAutoRaise(True)
self._wlock.setToolTip("""<P>Click to lock or unlock the intensity range. When the intensity range is locked across multiple images, any changes in the intensity
range of one are propagated to the others. Hold the button down briefly for additional options.</P>""")
lo.addWidget(self._wlock)
QObject.connect(self._wlock, SIGNAL("clicked()"), self._toggleDisplayRangeLock)
QObject.connect(self.renderControl(), SIGNAL("displayRangeLocked"), self._setDisplayRangeLock)
QObject.connect(self.renderControl(), SIGNAL("dataSubsetChanged"), self._dataSubsetChanged)
lockmenu = QMenu(self)
lockmenu.addAction(pixmaps.locked.icon(), "Lock all to this",
self._currier.curry(imgman.lockAllDisplayRanges, self.renderControl()))
lockmenu.addAction(pixmaps.unlocked.icon(), "Unlock all", imgman.unlockAllDisplayRanges)
self._wlock.setPopupMode(QToolButton.DelayedPopup)
self._wlock.setMenu(lockmenu)
self._setDisplayRangeLock(self.renderControl().isDisplayRangeLocked())
# dialog button
self._wshowdialog = QToolButton(self)
lo.addWidget(self._wshowdialog)
self._wshowdialog.setIcon(pixmaps.colours.icon())
self._wshowdialog.setAutoRaise(True)
self._wshowdialog.setToolTip("""<P>Click for colourmap and intensity policy options.</P>""")
QObject.connect(self._wshowdialog, SIGNAL("clicked()"), self.showRenderControls)
tooltip = """<P>You can change the currently displayed intensity range by entering low and high limits here.</P>
<TABLE>
<TR><TD><NOBR>Image min:</NOBR></TD><TD>%g</TD><TD>max:</TD><TD>%g</TD></TR>
</TABLE>""" % self.image.imageMinMax()
for w in self._wmin, self._wmax, self._wrangelbl:
w.setToolTip(tooltip)
# create image operations menu
self._menu = QMenu(self.name, self)
self._qa_raise = self._menu.addAction(pixmaps.raise_up.icon(), "Raise image",
self._currier.curry(self.image.emit, SIGNAL("raise")))
self._qa_center = self._menu.addAction(pixmaps.center_image.icon(), "Center plot on image",
self._currier.curry(self.image.emit, SIGNAL("center")))
self._qa_show_rc = self._menu.addAction(pixmaps.colours.icon(), "Colours && Intensities...",
self.showRenderControls)
if save:
self._qa_save = self._menu.addAction("Save image...", self._saveImage)
self._menu.addAction("Export image to PNG file...", self._exportImageToPNG)
self._export_png_dialog = None
self._menu.addAction("Unload image", self._currier.curry(self.image.emit, SIGNAL("unload")))
self._wraise.setMenu(self._menu)
self._wraise.setPopupMode(QToolButton.DelayedPopup)
# connect updates from renderControl and image
self.image.connect(SIGNAL("slice"), self._updateImageSlice)
QObject.connect(self._rc, SIGNAL("displayRangeChanged"), self._updateDisplayRange)
# default plot depth of image markers
self._z_markers = None
# and the markers themselves
self._image_border = QwtPlotCurve()
self._image_label = QwtPlotMarker()
# subset markers
self._subset_pen = QPen(QColor("Light Blue"))
self._subset_border = QwtPlotCurve()
self._subset_border.setPen(self._subset_pen)
self._subset_border.setVisible(False)
self._subset_label = QwtPlotMarker()
text = QwtText("subset")
text.setColor(self._subset_pen.color())
self._subset_label.setLabel(text)
self._subset_label.setLabelAlignment(Qt.AlignRight | Qt.AlignBottom)
self._subset_label.setVisible(False)
self._setting_lmrect = False
self._all_markers = [self._image_border, self._image_label, self._subset_border, self._subset_label]
def close(self):
if self._control_dialog:
self._control_dialog.close()
self._control_dialog = None
def __del__(self):
self.close()
def __eq__(self, other):
return self is other
def renderControl(self):
return self._rc
def getMenu(self):
return self._menu
def getFilename(self):
return self.image.filename
def setName(self, name):
self.name = name
self._wlabel.setText("%s: %s" % (chr(ord('a') + self._number), self.name))
def setNumber(self, num):
self._number = num
self._menu.menuAction().setText("%s: %s" % (chr(ord('a') + self._number), self.name))
self._qa_raise.setShortcut(QKeySequence("Alt+" + chr(ord('A') + num)))
self.setName(self.name)
def getNumber(self):
return self._number
def setPlotProjection(self, proj):
self.image.setPlotProjection(proj)
sameproj = proj == self.image.projection
self._wcenter.setVisible(sameproj)
self._qa_center.setVisible(not sameproj)
if self._image_border:
(l0, l1), (m0, m1) = self.image.getExtents()
path = numpy.array([l0, l0, l1, l1, l0]), numpy.array([m0, m1, m1, m0, m0])
self._image_border.setData(*path)
if self._image_label:
self._image_label.setValue(path[0][2], path[1][2])
def addPlotBorder(self, border_pen, label, label_color=None, bg_brush=None):
# make plot items for image frame
# make curve for image borders
(l0, l1), (m0, m1) = self.image.getExtents()
self._border_pen = QPen(border_pen)
self._image_border.show()
self._image_border.setData([l0, l0, l1, l1, l0], [m0, m1, m1, m0, m0])
self._image_border.setPen(self._border_pen)
self._image_border.setZ(self.image.z() + 1 if self._z_markers is None else self._z_markers)
if label:
self._image_label.show()
self._image_label_text = text = QwtText(" %s " % label)
text.setColor(label_color)
text.setBackgroundBrush(bg_brush)
self._image_label.setValue(l1, m1)
self._image_label.setLabel(text)
self._image_label.setLabelAlignment(Qt.AlignRight | Qt.AlignVCenter)
self._image_label.setZ(self.image.z() + 2 if self._z_markers is None else self._z_markers)
def setPlotBorderStyle(self, border_color=None, label_color=None):
if border_color:
self._border_pen.setColor(border_color)
self._image_border.setPen(self._border_pen)
if label_color:
self._image_label_text.setColor(label_color)
self._image_label.setLabel(self._image_label_text)
def showPlotBorder(self, show=True):
self._image_border.setVisible(show)
self._image_label.setVisible(show)
def attachToPlot(self, plot, z_markers=None):
for item in [self.image] + self._all_markers:
if item.plot() != plot:
item.attach(plot)
def setImageVisible(self, visible):
self.image.setVisible(visible)
def showRenderControls(self):
if not self._control_dialog:
dprint(1, "creating control dialog")
self._control_dialog = ImageControlDialog(self, self._rc, self._imgman)
dprint(1, "done")
if not self._control_dialog.isVisible():
dprint(1, "showing control dialog")
self._control_dialog.show()
else:
self._control_dialog.hide()
def _changeDisplayRangeToPercent(self, percent):
if not self._control_dialog:
self._control_dialog = ImageControlDialog(self, self._rc, self._imgman)
self._control_dialog._changeDisplayRangeToPercent(percent)
def _updateDisplayRange(self, dmin, dmax):
"""Updates display range widgets."""
self._wmin.setText("%.4g" % dmin)
self._wmax.setText("%.4g" % dmax)
self._updateFullRangeIcon()
def _changeDisplayRange(self):
"""Gets display range from widgets and updates the image with it."""
try:
newrange = float(str(self._wmin.text())), float(str(self._wmax.text()))
except ValueError:
return
self._rc.setDisplayRange(*newrange)
def _dataSubsetChanged(self, subset, minmax, desc, subset_type):
"""Called when the data subset changes (or is reset)"""
# hide the subset indicator -- unless we're invoked while we're actually setting the subset itself
if not self._setting_lmrect:
self._subset = None
self._subset_border.setVisible(False)
self._subset_label.setVisible(False)
def setLMRectSubset(self, rect):
self._subset = rect
l0, m0, l1, m1 = rect.getCoords()
self._subset_border.setData([l0, l0, l1, l1, l0], [m0, m1, m1, m0, m0])
self._subset_border.setVisible(True)
self._subset_label.setValue(max(l0, l1), max(m0, m1))
self._subset_label.setVisible(True)
self._setting_lmrect = True
self.renderControl().setLMRectSubset(rect)
self._setting_lmrect = False
def currentSlice(self):
return self._rc.currentSlice()
def _updateImageSlice(self, slice):
dprint(2, slice)
for i, (iextra, name, labels) in enumerate(self._rc.slicedAxes()):
slicer = self._wslicers[i]
if slicer.currentIndex() != slice[iextra]:
dprint(3, "setting widget", i, "to", slice[iextra])
slicer.setCurrentIndex(slice[iextra])
def setMarkersZ(self, z):
self._z_markers = z
for i, elem in enumerate(self._all_markers):
elem.setZ(z + i)
def setZ(self, z, top=False, depthlabel=None, can_raise=True):
self.image.setZ(z)
if self._z_markers is None:
for i, elem in enumerate(self._all_markers):
elem.setZ(z + i + i)
# set the depth label, if any
label = "%s: %s" % (chr(ord('a') + self._number), self.name)
# label = "%s %s"%(depthlabel,self.name) if depthlabel else self.name
if top:
label = "%s: <B>%s</B>" % (chr(ord('a') + self._number), self.name)
self._wlabel.setText(label)
# set hotkey
self._qa_show_rc.setShortcut(Qt.Key_F9 if top else QKeySequence())
# set raise control
self._can_raise = can_raise
self._qa_raise.setVisible(can_raise)
self._wlock.setVisible(can_raise)
if can_raise:
self._wraise.setToolTip(
"<P>Click here to raise this image to the top. Click on the down-arrow to access the image menu.</P>")
else:
self._wraise.setToolTip("<P>Click to access the image menu.</P>")
def _raiseButtonPressed(self):
if self._can_raise:
self.image.emit(SIGNAL("raise"))
else:
self._wraise.showMenu()
def _saveImage(self):
filename = QFileDialog.getSaveFileName(self, "Save FITS file", self._save_dir,
"FITS files(*.fits *.FITS *fts *FTS)")
filename = str(filename)
if not filename:
return
busy = BusyIndicator()
self._imgman.showMessage("""Writing FITS image %s""" % filename, 3000)
QApplication.flush()
try:
self.image.save(filename)
except Exception as exc:
busy = None
traceback.print_exc()
self._imgman.showErrorMessage("""Error writing FITS image %s: %s""" % (filename, str(sys.exc_info()[1])))
return None
self.renderControl().startSavingConfig(filename)
self.setName(self.image.name)
self._qa_save.setVisible(False)
self._wsave.hide()
busy = None
def _exportImageToPNG(self, filename=None):
if not filename:
if not self._export_png_dialog:
dialog = self._export_png_dialog = QFileDialog(self, "Export image to PNG", ".", "*.png")
dialog.setDefaultSuffix("png")
dialog.setFileMode(QFileDialog.AnyFile)
dialog.setAcceptMode(QFileDialog.AcceptSave)
dialog.setModal(True)
QObject.connect(dialog, SIGNAL("filesSelected(const QStringList &)"), self._exportImageToPNG)
return self._export_png_dialog.exec_() == QDialog.Accepted
busy = BusyIndicator()
if isinstance(filename, QStringList):
filename = filename[0]
filename = str(filename)
# make QPixmap
nx, ny = self.image.imageDims()
(l0, l1), (m0, m1) = self.image.getExtents()
pixmap = QPixmap(nx, ny)
painter = QPainter(pixmap)
# use QwtPlot implementation of draw canvas, since we want to avoid caching
xmap = QwtScaleMap()
xmap.setPaintInterval(0, nx)
xmap.setScaleInterval(l1, l0)
ymap = QwtScaleMap()
ymap.setPaintInterval(ny, 0)
ymap.setScaleInterval(m0, m1)
self.image.draw(painter, xmap, ymap, pixmap.rect())
painter.end()
# save to file
try:
pixmap.save(filename, "PNG")
except Exception as exc:
self.emit(SIGNAL("showErrorMessage"), "Error writing %s: %s" % (filename, str(exc)))
return
self.emit(SIGNAL("showMessage"), "Exported image to file %s" % filename)
def _toggleDisplayRangeLock(self):
self.renderControl().lockDisplayRange(not self.renderControl().isDisplayRangeLocked())
def _setDisplayRangeLock(self, locked):
self._wlock.setIcon(pixmaps.locked.icon() if locked else pixmaps.unlocked.icon())
def _updateFullRangeIcon(self):
if self._rc.isSubsetDisplayRange():
self._wfullrange.setIcon(pixmaps.zoom_range.icon())
self._wfullrange.setToolTip(
"""<P>The current intensity range is the full range. Hold this button down briefly for additional options.</P>""")
else:
self._wfullrange.setIcon(pixmaps.full_range.icon())
self._wfullrange.setToolTip(
"""<P>Click to reset to a full intensity range. Hold the button down briefly for additional options.</P>""")
def paintEvent(self, event):
QSplitterHandle.paintEvent(self, event)
left, right = self.parent().left, self.parent().right
painter = QPainter(self)
painter.setClipRect(event.rect())
w = self.width()
h = self.height()
painter.setRenderHints(QPainter.Antialiasing, True)
C = 16 # Curve factor.
def create_line(ly, ry, right_to_left=False):
' Create path that represents upper or lower line of change marker '
line = QPainterPath()
if not right_to_left:
line.moveTo(0, ly)
line.cubicTo(C, ly, w - C, ry, w, ry)
else:
line.moveTo(w, ry)
line.cubicTo(w - C, ry, C, ly, 0, ly)
return line
ldoc, rdoc = left.document(), right.document()
lorigin, rorigin = left.contentOffset(), right.contentOffset()
lfv, rfv = left.firstVisibleBlock().blockNumber(
), right.firstVisibleBlock().blockNumber()
lines = []
for (ltop, lbot, kind), (rtop, rbot,
kind) in zip(left.changes, right.changes):
if lbot < lfv and rbot < rfv:
continue
ly_top = left.blockBoundingGeometry(
ldoc.findBlockByNumber(ltop)).translated(lorigin).y()
ly_bot = left.blockBoundingGeometry(
ldoc.findBlockByNumber(lbot)).translated(lorigin).y()
ry_top = right.blockBoundingGeometry(
rdoc.findBlockByNumber(rtop)).translated(rorigin).y()
ry_bot = right.blockBoundingGeometry(
rdoc.findBlockByNumber(rbot)).translated(rorigin).y()
if max(ly_top, ly_bot, ry_top, ry_bot) < 0:
continue
if min(ly_top, ly_bot, ry_top, ry_bot) > h:
break
upper_line = create_line(ly_top, ry_top)
lower_line = create_line(ly_bot, ry_bot, True)
region = QPainterPath()
region.moveTo(0, ly_top)
region.connectPath(upper_line)
region.lineTo(w, ry_bot)
region.connectPath(lower_line)
region.closeSubpath()
painter.fillPath(region, left.diff_backgrounds[kind])
for path, aa in zip((upper_line, lower_line),
(ly_top != ry_top, ly_bot != ry_bot)):
lines.append((kind, path, aa))
for kind, path, aa in sorted(
lines, key=lambda x: {'replace': 0}.get(x[0], 1)):
painter.setPen(left.diff_foregrounds[kind])
painter.setRenderHints(QPainter.Antialiasing, aa)
painter.drawPath(path)
painter.setFont(left.heading_font)
for (lnum, text), (rnum, text) in zip(left.headers, right.headers):
ltop, lbot, rtop, rbot = lnum, lnum + 3, rnum, rnum + 3
if lbot < lfv and rbot < rfv:
continue
ly_top = left.blockBoundingGeometry(
ldoc.findBlockByNumber(ltop)).translated(lorigin).y()
ly_bot = left.blockBoundingGeometry(
ldoc.findBlockByNumber(lbot)).translated(lorigin).y()
ry_top = right.blockBoundingGeometry(
rdoc.findBlockByNumber(rtop)).translated(rorigin).y()
ry_bot = right.blockBoundingGeometry(
rdoc.findBlockByNumber(rbot)).translated(rorigin).y()
if max(ly_top, ly_bot, ry_top, ry_bot) < 0:
continue
if min(ly_top, ly_bot, ry_top, ry_bot) > h:
break
ly = painter.boundingRect(3, ly_top, left.width(),
ly_bot - ly_top - 5, Qt.TextSingleLine,
text).bottom() + 3
ry = painter.boundingRect(3, ry_top, right.width(),
ry_bot - ry_top - 5, Qt.TextSingleLine,
text).bottom() + 3
line = create_line(ly, ry)
painter.setPen(QPen(left.palette().text(), 2))
painter.setRenderHints(QPainter.Antialiasing, ly != ry)
painter.drawPath(line)
painter.end()
# Paint the splitter without the change lines if the mouse is over the
# splitter
if getattr(self, 'hover', False):
QSplitterHandle.paintEvent(self, event)
def __init__(self, image, parent, imgman, name=None, save=False):
QFrame.__init__(self, parent)
self.setFrameStyle(QFrame.StyledPanel | QFrame.Raised)
# init state
self.image = image
self._imgman = imgman
self._currier = PersistentCurrier()
self._control_dialog = None
# create widgets
self._lo = lo = QHBoxLayout(self)
lo.setContentsMargins(0, 0, 0, 0)
lo.setSpacing(2)
# raise button
self._wraise = QToolButton(self)
lo.addWidget(self._wraise)
self._wraise.setIcon(pixmaps.raise_up.icon())
self._wraise.setAutoRaise(True)
self._can_raise = False
QObject.connect(self._wraise, SIGNAL("clicked()"), self._raiseButtonPressed)
self._wraise.setToolTip("""<P>Click here to raise this image above other images. Hold the button down briefly to
show a menu of image operations.</P>""")
# center label
self._wcenter = QLabel(self)
self._wcenter.setPixmap(pixmaps.center_image.pm())
self._wcenter.setToolTip(
"<P>The plot is currently centered on (the reference pixel %d,%d) of this image.</P>" % self.image.referencePixel())
lo.addWidget(self._wcenter)
# name/filename label
self.name = image.name
self._wlabel = QLabel(self.name, self)
self._number = 0
self.setName(self.name)
self._wlabel.setToolTip("%s %s" % (image.filename, "\u00D7".join(map(str, image.data().shape))))
lo.addWidget(self._wlabel, 1)
# if 'save' is specified, create a "save" button
if save:
self._wsave = QToolButton(self)
lo.addWidget(self._wsave)
self._wsave.setText("save")
self._wsave.setAutoRaise(True)
self._save_dir = save if isinstance(save, str) else "."
QObject.connect(self._wsave, SIGNAL("clicked()"), self._saveImage)
self._wsave.setToolTip("""<P>Click here to write this image to a FITS file.</P>""")
# render control
dprint(2, "creating RenderControl")
self._rc = RenderControl(image, self)
dprint(2, "done")
# selectors for extra axes
self._wslicers = []
curslice = self._rc.currentSlice(); # this may be loaded from config, so not necessarily 0
for iextra, axisname, labels in self._rc.slicedAxes():
if axisname.upper() not in ["STOKES", "COMPLEX"]:
lbl = QLabel("%s:" % axisname, self)
lo.addWidget(lbl)
else:
lbl = None
slicer = QComboBox(self)
self._wslicers.append(slicer)
lo.addWidget(slicer)
slicer.addItems(labels)
slicer.setToolTip("""<P>Selects current slice along the %s axis.</P>""" % axisname)
slicer.setCurrentIndex(curslice[iextra])
QObject.connect(slicer, SIGNAL("activated(int)"), self._currier.curry(self._rc.changeSlice, iextra))
# min/max display ranges
lo.addSpacing(5)
self._wrangelbl = QLabel(self)
lo.addWidget(self._wrangelbl)
self._minmaxvalidator = FloatValidator(self)
self._wmin = QLineEdit(self)
self._wmax = QLineEdit(self)
width = self._wmin.fontMetrics().width("1.234567e-05")
for w in self._wmin, self._wmax:
lo.addWidget(w, 0)
w.setValidator(self._minmaxvalidator)
w.setMaximumWidth(width)
w.setMinimumWidth(width)
QObject.connect(w, SIGNAL("editingFinished()"), self._changeDisplayRange)
# full-range button
self._wfullrange = QToolButton(self)
lo.addWidget(self._wfullrange, 0)
self._wfullrange.setIcon(pixmaps.zoom_range.icon())
self._wfullrange.setAutoRaise(True)
QObject.connect(self._wfullrange, SIGNAL("clicked()"), self.renderControl().resetSubsetDisplayRange)
rangemenu = QMenu(self)
rangemenu.addAction(pixmaps.full_range.icon(), "Full subset", self.renderControl().resetSubsetDisplayRange)
for percent in (99.99, 99.9, 99.5, 99, 98, 95):
rangemenu.addAction("%g%%" % percent, self._currier.curry(self._changeDisplayRangeToPercent, percent))
self._wfullrange.setPopupMode(QToolButton.DelayedPopup)
self._wfullrange.setMenu(rangemenu)
# update widgets from current display range
self._updateDisplayRange(*self._rc.displayRange())
# lock button
self._wlock = QToolButton(self)
self._wlock.setIcon(pixmaps.unlocked.icon())
self._wlock.setAutoRaise(True)
self._wlock.setToolTip("""<P>Click to lock or unlock the intensity range. When the intensity range is locked across multiple images, any changes in the intensity
range of one are propagated to the others. Hold the button down briefly for additional options.</P>""")
lo.addWidget(self._wlock)
QObject.connect(self._wlock, SIGNAL("clicked()"), self._toggleDisplayRangeLock)
QObject.connect(self.renderControl(), SIGNAL("displayRangeLocked"), self._setDisplayRangeLock)
QObject.connect(self.renderControl(), SIGNAL("dataSubsetChanged"), self._dataSubsetChanged)
lockmenu = QMenu(self)
lockmenu.addAction(pixmaps.locked.icon(), "Lock all to this",
self._currier.curry(imgman.lockAllDisplayRanges, self.renderControl()))
lockmenu.addAction(pixmaps.unlocked.icon(), "Unlock all", imgman.unlockAllDisplayRanges)
self._wlock.setPopupMode(QToolButton.DelayedPopup)
self._wlock.setMenu(lockmenu)
self._setDisplayRangeLock(self.renderControl().isDisplayRangeLocked())
# dialog button
self._wshowdialog = QToolButton(self)
lo.addWidget(self._wshowdialog)
self._wshowdialog.setIcon(pixmaps.colours.icon())
self._wshowdialog.setAutoRaise(True)
self._wshowdialog.setToolTip("""<P>Click for colourmap and intensity policy options.</P>""")
QObject.connect(self._wshowdialog, SIGNAL("clicked()"), self.showRenderControls)
tooltip = """<P>You can change the currently displayed intensity range by entering low and high limits here.</P>
<TABLE>
<TR><TD><NOBR>Image min:</NOBR></TD><TD>%g</TD><TD>max:</TD><TD>%g</TD></TR>
</TABLE>""" % self.image.imageMinMax()
for w in self._wmin, self._wmax, self._wrangelbl:
w.setToolTip(tooltip)
# create image operations menu
self._menu = QMenu(self.name, self)
self._qa_raise = self._menu.addAction(pixmaps.raise_up.icon(), "Raise image",
self._currier.curry(self.image.emit, SIGNAL("raise")))
self._qa_center = self._menu.addAction(pixmaps.center_image.icon(), "Center plot on image",
self._currier.curry(self.image.emit, SIGNAL("center")))
self._qa_show_rc = self._menu.addAction(pixmaps.colours.icon(), "Colours && Intensities...",
self.showRenderControls)
if save:
self._qa_save = self._menu.addAction("Save image...", self._saveImage)
self._menu.addAction("Export image to PNG file...", self._exportImageToPNG)
self._export_png_dialog = None
self._menu.addAction("Unload image", self._currier.curry(self.image.emit, SIGNAL("unload")))
self._wraise.setMenu(self._menu)
self._wraise.setPopupMode(QToolButton.DelayedPopup)
# connect updates from renderControl and image
self.image.connect(SIGNAL("slice"), self._updateImageSlice)
QObject.connect(self._rc, SIGNAL("displayRangeChanged"), self._updateDisplayRange)
# default plot depth of image markers
self._z_markers = None
# and the markers themselves
self._image_border = QwtPlotCurve()
self._image_label = QwtPlotMarker()
# subset markers
self._subset_pen = QPen(QColor("Light Blue"))
self._subset_border = QwtPlotCurve()
self._subset_border.setPen(self._subset_pen)
self._subset_border.setVisible(False)
self._subset_label = QwtPlotMarker()
text = QwtText("subset")
text.setColor(self._subset_pen.color())
self._subset_label.setLabel(text)
self._subset_label.setLabelAlignment(Qt.AlignRight | Qt.AlignBottom)
self._subset_label.setVisible(False)
self._setting_lmrect = False
self._all_markers = [self._image_border, self._image_label, self._subset_border, self._subset_label]
def _setupHistogramPlot(self):
self._histplot.setCanvasBackground(QColor("lightgray"))
self._histplot.setAxisFont(QwtPlot.yLeft, QApplication.font())
self._histplot.setAxisFont(QwtPlot.xBottom, QApplication.font())
# add histogram curves
self._histcurve1 = TiggerPlotCurve()
self._histcurve2 = TiggerPlotCurve()
self._histcurve1.setStyle(QwtPlotCurve.Steps)
self._histcurve2.setStyle(QwtPlotCurve.Steps)
self._histcurve1.setPen(QPen(Qt.NoPen))
self._histcurve1.setBrush(QBrush(QColor("slategrey")))
pen = QPen(QColor("red"))
pen.setWidth(1)
self._histcurve2.setPen(pen)
self._histcurve1.setZ(0)
self._histcurve2.setZ(100)
# self._histcurve1.attach(self._histplot)
self._histcurve2.attach(self._histplot)
# add maxbin and half-max curves
self._line_0 = self.HistogramLineMarker(self._histplot, color="grey50", linestyle=Qt.SolidLine,
align=Qt.AlignTop | Qt.AlignLeft, z=90)
self._line_mean = self.HistogramLineMarker(self._histplot, color="black", linestyle=Qt.SolidLine,
align=Qt.AlignBottom | Qt.AlignRight, z=91,
label="mean", zlabel=151)
self._line_std = self.HistogramLineMarker(self._histplot, color="black", linestyle=Qt.SolidLine,
align=Qt.AlignTop | Qt.AlignRight, z=91,
label="std", zlabel=151)
sym = QwtSymbol()
sym.setStyle(QwtSymbol.VLine)
sym.setSize(8)
self._line_std.line.setSymbol(sym)
self._line_maxbin = self.HistogramLineMarker(self._histplot, color="green", linestyle=Qt.DotLine,
align=Qt.AlignTop | Qt.AlignRight, z=92,
label="max bin", zlabel=150)
self._line_halfmax = self.HistogramLineMarker(self._histplot, color="green", linestyle=Qt.DotLine,
align=Qt.AlignBottom | Qt.AlignRight, z=90,
label="half-max", yaxis=QwtPlot.yLeft)
# add current range
self._rangebox = TiggerPlotCurve()
self._rangebox.setStyle(QwtPlotCurve.Steps)
self._rangebox.setYAxis(QwtPlot.yRight)
self._rangebox.setPen(QPen(Qt.NoPen))
self._rangebox.setBrush(QBrush(QColor("darkgray")))
self._rangebox.setZ(50)
self._rangebox.attach(self._histplot)
self._rangebox2 = TiggerPlotCurve()
self._rangebox2.setStyle(QwtPlotCurve.Sticks)
self._rangebox2.setYAxis(QwtPlot.yRight)
self._rangebox2.setZ(60)
# self._rangebox2.attach(self._histplot)
# add intensity transfer function
self._itfcurve = TiggerPlotCurve()
self._itfcurve.setStyle(QwtPlotCurve.Lines)
self._itfcurve.setPen(QPen(QColor("blue")))
self._itfcurve.setYAxis(QwtPlot.yRight)
self._itfcurve.setZ(120)
self._itfcurve.attach(self._histplot)
self._itfmarker = TiggerPlotMarker()
label = QwtText("ITF")
label.setColor(QColor("blue"))
self._itfmarker.setLabel(label)
try:
self._itfmarker.setSpacing(0)
except AttributeError:
pass
self._itfmarker.setLabelAlignment(Qt.AlignTop | Qt.AlignRight)
self._itfmarker.setZ(120)
self._itfmarker.attach(self._histplot)
# add colorbar
self._cb_item = self.ColorBarPlotItem(1, 1 + self.ColorBarHeight)
self._cb_item.setYAxis(QwtPlot.yRight)
self._cb_item.attach(self._histplot)
# add pickers
self._hist_minpicker = self.HistLimitPicker(self._histplot, "low: %(x).4g")
self._hist_minpicker.setMousePattern(QwtEventPattern.MouseSelect1, Qt.LeftButton)
QObject.connect(self._hist_minpicker, SIGNAL("selected(const QwtDoublePoint &)"), self._selectLowLimit)
self._hist_maxpicker = self.HistLimitPicker(self._histplot, "high: %(x).4g")
self._hist_maxpicker.setMousePattern(QwtEventPattern.MouseSelect1, Qt.RightButton)
QObject.connect(self._hist_maxpicker, SIGNAL("selected(const QwtDoublePoint &)"), self._selectHighLimit)
self._hist_maxpicker1 = self.HistLimitPicker(self._histplot, "high: %(x).4g")
self._hist_maxpicker1.setMousePattern(QwtEventPattern.MouseSelect1, Qt.LeftButton, Qt.CTRL)
QObject.connect(self._hist_maxpicker1, SIGNAL("selected(const QwtDoublePoint &)"), self._selectHighLimit)
self._hist_zoompicker = self.HistLimitPicker(self._histplot, label="zoom",
tracker_mode=QwtPicker.AlwaysOn, track=self._trackHistCoordinates,
color="black",
mode=QwtPicker.RectSelection,
rubber_band=QwtPicker.RectRubberBand)
self._hist_zoompicker.setMousePattern(QwtEventPattern.MouseSelect1, Qt.LeftButton, Qt.SHIFT)
QObject.connect(self._hist_zoompicker, SIGNAL("selected(const QwtDoubleRect &)"), self._zoomHistogramIntoRect)
def initJoystickGraphics(self):
# Pens
self.cyan_pen = QPen(QColor(0, 255, 255))
self.magenta_pen = QPen(QColor(255, 0, 255))
self.red_pen = QPen(QColor(255, 0, 0))
self.cyan_pen.setWidth(3)
self.magenta_pen.setWidth(3)
self.red_pen.setWidth(3)
self.stick_ind_l = QGraphicsEllipseItem()
self.stick_ind_r = QGraphicsEllipseItem()
self.stick_line_l = QGraphicsLineItem()
self.stick_line_r = QGraphicsLineItem()
self.mode_ind = QGraphicsLineItem()
# Left joystick indicator circle
px_l = self.stick_ind_lox - self.stick_ind_radius
py_l = self.stick_ind_loy - self.stick_ind_radius
self.stick_ind_l.setRect(px_l, py_l, 2 * self.stick_ind_radius, 2 * self.stick_ind_radius)
self.stick_ind_l.setBrush(QBrush(QColor(255, 0, 0)))
self.stick_ind_l.setPen(QPen(QColor(0, 0, 0)))
# Right joystick indicator circle
px_r = self.stick_ind_rox - self.stick_ind_radius
py_r = self.stick_ind_roy - self.stick_ind_radius
self.stick_ind_r.setRect(px_r, py_r, 2 * self.stick_ind_radius, 2 * self.stick_ind_radius)
self.stick_ind_r.setBrush(QBrush(QColor(255, 0, 0)))
self.stick_ind_r.setPen(QPen(QColor(0, 0, 0)))
# Left joystick indicator line
line_pen = QPen(QColor(255,0,0))
line_pen.setWidth(4)
self.stick_line_l.setLine(self.stick_ind_lox, self.stick_ind_loy, self.stick_ind_lox, self.stick_ind_loy)
self.stick_line_l.setPen(line_pen)
# Right joystick indicator line
self.stick_line_r.setLine(self.stick_ind_rox, self.stick_ind_roy, self.stick_ind_rox, self.stick_ind_roy)
self.stick_line_r.setPen(line_pen)
# Mode indicator line
self.mode_ind.setLine(self.mode_ind_x1, self.mode_ind_y1, self.mode_ind_x2, self.mode_ind_y2)
self.mode_ind.setPen(self.cyan_pen)
# Joystick power indicator
self.joystick_power_ind = []
self.joystick_power_ind.append(QGraphicsLineItem(self.power_ind_x1, self.power_ind_y + 20, self.power_ind_x1, self.power_ind_y - 20))
self.joystick_power_ind.append(QGraphicsLineItem(self.power_ind_x1, self.power_ind_y - 20, self.power_ind_x1 + 50, self.power_ind_y - 20))
self.joystick_power_ind.append(QGraphicsLineItem(self.power_ind_x1+50, self.power_ind_y - 20, self.power_ind_x1+50, self.power_ind_y + 20))
self.joystick_power_ind.append(QGraphicsLineItem(self.power_ind_x1+50, self.power_ind_y + 20, self.power_ind_x1, self.power_ind_y + 20))
# Populate scene
graphics_scene = QGraphicsScene()
graphics_scene.addItem(self.stick_ind_l)
graphics_scene.addItem(self.stick_ind_r)
graphics_scene.addItem(self.stick_line_l)
graphics_scene.addItem(self.stick_line_r)
graphics_scene.addItem(self.mode_ind)
for l in self.joystick_power_ind:
l.setPen(self.red_pen)
graphics_scene.addItem(l)
graphics_scene.setSceneRect(0, 0, self._widget.joystickGraphicsView.width() - 4, self._widget.joystickGraphicsView.height() - 4)
self._widget.joystickGraphicsView.setScene(graphics_scene)
self._widget.joystickGraphicsView.setBackgroundBrush(QBrush(QImage(os.path.join(rospkg.RosPack().get_path('mobility_base_tools'), 'images', 'dx6ilabels.jpg'))))
self._widget.joystickGraphicsView.show()
class DiagnosticGui(Plugin):
last_suppress_time = Time()
last_twist_time = Time()
gui_update_timer = QTimer()
# Raw joystick data
joystick_data = []
joystick_table_vals = []
joystick_channel_text = ['CHAN_ENABLE: ', 'CHAN_ROTATE: ', 'CHAN_FORWARD: ', 'CHAN_LATERAL: ', 'CHAN_MODE: ', 'CHAN_EXTRA: ']
joystick_bind_status = False
joystick_bind_dot_counter = 0
# Mode
current_mode = -1
# Topic pub timeouts
JOYSTICK_SUPPRESS_PERIOD = 0.2 # 5 Hz
CMD_VEL_TIMEOUT_PERIOD = 0.2 # 5 Hz
joystick_suppressed = False
command_received = False
current_cmd = Twist()
battery_percent = 0
battery_voltage = 0
# Checklist icons
bad_icon = QPixmap()
good_icon = QPixmap()
none_icon = QPixmap()
# Checklist status
GOOD = 0
BAD = 1
NONE = 2
# Checklist variables
checklist_status = []
BATT_MAN = 0
ESTOP_MAN = 1
DISABLE_MAN = 2
JOYSTICK_MAN = 3
SUPPRESS_MAN = 4
BATT_COMP = 5
ESTOP_COMP = 6
DISABLE_COMP = 7
JOYSTICK_COMP = 8
SUPPRESS_COMP = 9
CMD_COMP = 10
# Bumpers
bumper_front_left = 0
bumper_front_right = 0
bumper_rear_left = 0
bumper_rear_right = 0
# Gyro cal
gyro_cal_status = False
gyro_x = 0.0
gyro_y = 0.0
gyro_z = 0.0
cal_enabled = True
cal_time = rospy.Time(0)
# Max speed config
max_speed_known = False
max_speed_dirty = True
max_linear_actual = 0.0
max_angular_actual = 0.0
max_linear_setting = 0.0
max_angular_setting = 0.0
# Wake time
current_wake_time = rospy.Time(0)
rel_wake_days = 0
rel_wake_hours = 0
rel_wake_minutes = 0
rel_wake_secs = 0
# Switching between tabs and full GUI
is_currently_tab = False
widget_count = 0
current_tab_idx = -1
raw_data_tab_idx = 5
# Check connection to base
base_connected = False
last_joystick_time = rospy.Time(0)
# Constants
stick_ind_lox = 80
stick_ind_loy = 136
stick_ind_rox = 286
stick_ind_roy = 135
stick_ind_range_pix = 88.0
stick_ind_range_max = JoystickRaw.MAX
stick_ind_range_min = JoystickRaw.MIN
stick_ind_range_mid = JoystickRaw.CENTER
stick_ind_range_factor = stick_ind_range_pix / (stick_ind_range_max - stick_ind_range_min)
stick_ind_radius = 7
mode_ind_x1 = 52
mode_ind_y1 = 37
mode_ind_x2 = 44
mode_ind_y2 = 13
power_ind_x1 = 160
power_ind_x2 = 206
power_ind_y = 213
bumper_fl_x = 70
bumper_fl_y = 60
bumper_fr_x = 293
bumper_fr_y = 60
bumper_rl_x = 70
bumper_rl_y = 282
bumper_rr_x = 293
bumper_rr_y = 282
bumper_dx = 62
bumper_dy = 54
joystick_table_left_edge = 440
joystick_table_top_edge = 525
twist_table_left_edge = 700
twist_table_top_edge = 580
battery_table_left_edge = 700
battery_table_top_edge = 730
def __init__(self, context):
# Qt setup
self.context_ = context
self.initGui('full')
# ROS setup
topic_timeout_timer = rospy.Timer(rospy.Duration(0.02), self.topicTimeoutCallback)
self.subscribeTopics()
self.advertiseTopics()
def initGui(self, gui_type):
if gui_type == 'full':
self.spawnFullGui()
self.initTables(self._widget, self.joystick_table_left_edge, self.joystick_table_top_edge)
else:
self.spawnTabGui()
self.initTables(self._widget.gui_tabs.widget(self.raw_data_tab_idx), 20, 20)
self._widget.gui_tabs.setCurrentIndex(self.current_tab_idx)
self.resetGuiTimer()
self.initJoystickGraphics()
self.initBumperGraphics()
self.initChecklists()
self.bindCallbacks()
self.refreshMaxSpeed()
# Initialize absolute wake time setter to current time
datetime_now = QDateTime(QDate.currentDate(), QTime.currentTime())
self._widget.absolute_wake_time_obj.setDateTime(datetime_now)
temp_time = self._widget.absolute_wake_time_obj.time()
temp_time = QTime(temp_time.hour(), temp_time.minute())
self._widget.absolute_wake_time_obj.setTime(temp_time)
# Set connection label text
if self.base_connected:
self._widget.disconnected_lbl.setVisible(False)
else:
self._widget.disconnected_lbl.setVisible(True)
self._widget.disconnected_lbl.setText('<font color=#FF0000>NOT CONNECTED</font>')
def topicTimeoutCallback(self, event):
# Joystick suppression
if (event.current_real - self.last_suppress_time).to_sec() < self.JOYSTICK_SUPPRESS_PERIOD and self.suppress_dt.to_sec() <= 1.0/9.0:
self.joystick_suppressed = True
else:
self.joystick_suppressed = False
# Command message
if (event.current_real - self.last_twist_time).to_sec() < self.CMD_VEL_TIMEOUT_PERIOD and self.twist_dt.to_sec() <= 1.0/6.0:
self.command_received = True
else:
self.command_received = False
def initChecklists(self):
self.bad_icon.load(os.path.join(rospkg.RosPack().get_path('mobility_base_tools'), 'images', 'bad.png'))
self.good_icon.load(os.path.join(rospkg.RosPack().get_path('mobility_base_tools'), 'images', 'good.png'))
self.none_icon.load(os.path.join(rospkg.RosPack().get_path('mobility_base_tools'), 'images', 'none.png'))
self.checklist_status=[0 for i in range(self.CMD_COMP + 1)]
self.checklist_status[self.BATT_MAN] = self.NONE
self.checklist_status[self.ESTOP_MAN] = self.NONE
self.checklist_status[self.DISABLE_MAN] = self.NONE
self.checklist_status[self.JOYSTICK_MAN] = self.NONE
self.checklist_status[self.SUPPRESS_MAN] = self.NONE
self.checklist_status[self.BATT_COMP] = self.NONE
self.checklist_status[self.ESTOP_COMP] = self.NONE
self.checklist_status[self.DISABLE_COMP] = self.NONE
self.checklist_status[self.JOYSTICK_COMP] = self.NONE
self.checklist_status[self.SUPPRESS_COMP] = self.NONE
self.checklist_status[self.CMD_COMP] = self.NONE
def initTabTables(self):
self.joystick_table_vals = [QLabel(self._widget.gui_tabs.widget(self.raw_data_tab_idx)) for i in range(ChannelIndex.CHAN_EXTRA+1)]
self.joystick_table_labels = [QLabel(self._widget.gui_tabs.widget(self.raw_data_tab_idx)) for i in range(ChannelIndex.CHAN_EXTRA+1)]
self.joystick_table_heading = QLabel(self._widget.gui_tabs.widget(self.raw_data_tab_idx))
def initTables(self, widget, left, top):
# Joystick data table
self.joystick_data = [0 for i in range(ChannelIndex.CHAN_EXTRA+1)]
self.joystick_table_vals = [QLabel(widget) for i in range(ChannelIndex.CHAN_EXTRA+1)]
self.joystick_table_labels = [QLabel(widget) for i in range(ChannelIndex.CHAN_EXTRA+1)]
self.joystick_table_heading = QLabel(widget)
self.joystick_table_heading.setText('Raw Joystick Data')
self.joystick_table_heading.setFont(QFont('Ubuntu', 11, QFont.Bold))
self.joystick_table_heading.move(left, top)
for i in range(len(self.joystick_table_vals)):
self.joystick_table_vals[i].move(left + 150, top + 30 * (i+1))
self.joystick_table_vals[i].setText('0000')
self.joystick_table_vals[i].setFixedWidth(200)
self.joystick_table_labels[i].move(left, top + 30 * (i+1))
self.joystick_table_labels[i].setText(self.joystick_channel_text[i])
# Twist data table
self.twist_table_heading = QLabel(widget)
self.twist_table_heading.setText('Current Twist Command')
self.twist_table_heading.setFont(QFont('Ubuntu', 11, QFont.Bold))
self.twist_table_heading.move(left + 260, top)
self.twist_table_labels = [QLabel(widget) for i in range(0, 3)]
self.twist_table_vals = [QLabel(widget) for i in range(0, 3)]
for i in range(len(self.twist_table_vals)):
self.twist_table_vals[i].move(left + 260 + 150, top + 30 * (i+1))
self.twist_table_vals[i].setText('Not Published')
self.twist_table_vals[i].setFixedWidth(200)
self.twist_table_labels[i].move(left + 260, top + 30 * (i+1))
self.twist_table_labels[0].setText('Forward (m/s):')
self.twist_table_labels[1].setText('Lateral (m/s):')
self.twist_table_labels[2].setText('Rotation (rad/s):')
# Battery percentage
self.battery_heading = QLabel(widget)
self.battery_heading.setText('Current Battery State')
self.battery_heading.setFont(QFont('Ubuntu', 11, QFont.Bold))
self.battery_heading.move(left + 260, top + 150)
self.battery_label = QLabel(widget)
self.battery_label.move(left + 260, top + 150 +30)
self.battery_label.setText('000 %')
self.battery_voltage_label = QLabel(widget)
self.battery_voltage_label.move(left + 260, top + 150 +60)
self.battery_voltage_label.setText('00.00 V')
self.battery_voltage_label.setFixedWidth(200)
# Mode
self.mode_heading = QLabel(widget)
self.mode_heading.setFont(QFont('Ubuntu', 11, QFont.Bold))
self.mode_heading.move(left, top + 225)
self.mode_heading.setText('Current Mode')
self.mode_label = QLabel(widget)
self.mode_label.move(left + 120, top + 225)
self.mode_label.setText('XXXXXXXXXXXXXXXXXXXXXX')
def bindCallbacks(self):
self._widget.start_bind_btn.clicked.connect(self.startBind)
self._widget.stop_bind_btn.clicked.connect(self.stopBind)
self._widget.gyro_cal_btn.clicked.connect(self.calGyro)
self._widget.clear_cal_btn.clicked.connect(self.clearCal)
self._widget.max_linear_txt.editingFinished.connect(self.maxLinearChanged)
self._widget.max_angular_txt.editingFinished.connect(self.maxAngularChanged)
self._widget.set_speed_btn.clicked.connect(self.setMaxSpeed)
self._widget.clear_speed_btn.clicked.connect(self.clearMaxSpeed)
self._widget.wake_time_days_txt.editingFinished.connect(self.wakeDaysChanged)
self._widget.wake_time_hours_txt.editingFinished.connect(self.wakeHoursChanged)
self._widget.wake_time_minutes_txt.editingFinished.connect(self.wakeMinutesChanged)
self._widget.wake_time_secs_txt.editingFinished.connect(self.wakeSecsChanged)
self._widget.set_relative_wake_time_btn.clicked.connect(self.setRelativeWakeTime)
self._widget.set_absolute_wake_time_btn.clicked.connect(self.setAbsoluteWakeTime)
self._widget.clear_wake_time_btn.clicked.connect(self.clearWakeTime)
if self.is_currently_tab:
self._widget.gui_tabs.currentChanged.connect(self.setCurrentTab)
def setCurrentTab(self, idx):
self.current_tab_idx = self._widget.gui_tabs.currentIndex()
def startBind(self):
self.pub_start_bind.publish(std_msgs.msg.Empty())
def stopBind(self):
self.pub_stop_bind.publish(std_msgs.msg.Empty())
def calGyro(self):
gyro_cal_srv = rospy.ServiceProxy('/mobility_base/imu/calibrate', std_srvs.srv.Empty)
try:
gyro_cal_srv()
self.cal_enabled = False
self.cal_time = rospy.Time.now()
except:
pass
def clearCal(self):
clear_cal_srv = rospy.ServiceProxy('/mobility_base/imu/clear_cal', std_srvs.srv.Empty)
try:
clear_cal_srv()
except:
pass
def maxLinearChanged(self):
try:
float_val = float(self._widget.max_linear_txt.text())
self.max_linear_setting = float_val;
except ValueError:
if self.max_linear_actual <= 0 or not isfinite(self.max_linear_actual):
self.max_linear_setting = 0.0
else:
self.max_linear_setting = self.max_linear_actual
self.max_speed_dirty = True
def maxAngularChanged(self):
try:
float_val = float(self._widget.max_angular_txt.text())
self.max_angular_setting = float_val;
except ValueError:
if self.max_angular_actual <= 0 or not isfinite(self.max_angular_actual):
self.max_angular_setting = 0.0
else:
self.max_angular_setting = self.max_angular_actual
self.max_speed_dirty = True
def setMaxSpeed(self):
set_max_speed_srv = rospy.ServiceProxy('/mobility_base/set_max_speed', SetMaxSpeed)
req = SetMaxSpeedRequest()
req.linear = self.max_linear_setting
req.angular = self.max_angular_setting
try:
set_max_speed_srv(req)
self.max_speed_known = False
except:
pass
def clearMaxSpeed(self):
set_max_speed_srv = rospy.ServiceProxy('/mobility_base/set_max_speed', SetMaxSpeed)
req = SetMaxSpeedRequest()
req.linear = float('nan')
req.angular = float('nan')
try:
set_max_speed_srv(req)
self.max_speed_known = False
except:
pass
def wakeDaysChanged(self):
try:
self.rel_wake_days = float(self._widget.wake_time_days_txt.text())
except:
self._widget.wake_time_days_txt.setText(str(self.rel_wake_days))
def wakeHoursChanged(self):
try:
self.rel_wake_hours = float(self._widget.wake_time_hours_txt.text())
except:
self._widget.wake_time_hours_txt.setText(str(self.rel_wake_hours))
def wakeMinutesChanged(self):
try:
self.rel_wake_minutes = float(self._widget.wake_time_minutes_txt.text())
except:
self._widget.wake_time_minutes_txt.setText(str(self.rel_wake_minutes))
def wakeSecsChanged(self):
try:
self.rel_wake_secs = float(self._widget.wake_time_secs_txt.text())
except:
self._widget.wake_time_secs_txt.setText(str(self.rel_wake_secs))
def setRelativeWakeTime(self):
new_wake_time = std_msgs.msg.Time()
rel_wake_time = 86400 * self.rel_wake_days + 3600 * self.rel_wake_hours + 60 * self.rel_wake_minutes + self.rel_wake_secs
new_wake_time.data = rospy.Time.now() + rospy.Duration(rel_wake_time)
self.pub_set_wake_time.publish(new_wake_time)
def setAbsoluteWakeTime(self):
self.pub_set_wake_time.publish(rospy.Time(self._widget.absolute_wake_time_obj.dateTime().toTime_t()))
def clearWakeTime(self):
self.pub_set_wake_time.publish(rospy.Time(0))
def refreshMaxSpeed(self):
self.max_speed_dirty = True
self.max_speed_known = False
def updateGuiCallback(self):
# Switch between tabs and full GUI
if self.is_currently_tab:
if self._widget.height() > 830 and self._widget.width() > 1205:
self.is_currently_tab = False
self.context_.remove_widget(self._widget)
self.initGui('full')
else:
if self._widget.height() < 810 or self._widget.width() < 1185:
self.is_currently_tab = True
self.context_.remove_widget(self._widget)
self.initGui('tab')
# Check connection to base
if self.base_connected and (rospy.Time.now() - self.last_joystick_time).to_sec() > 1.0:
self.base_connected = False
self._widget.disconnected_lbl.setText('<font color=#FF0000>NOT CONNECTED</font>')
self._widget.disconnected_lbl.setVisible(True)
if not self.base_connected and (rospy.Time.now() - self.last_joystick_time).to_sec() < 1.0:
self.base_connected = True
# self._widget.disconnected_lbl.setText('')
self._widget.disconnected_lbl.setVisible(False)
# Update checklists
self.updateChecklist();
# Manage 5 second disable of gyro calibration button
if not self.cal_enabled:
if (rospy.Time.now() - self.cal_time).to_sec() > 5.0:
self._widget.gyro_cal_btn.setEnabled(True)
self._widget.gyro_cal_btn.setText('Calibrate')
self.cal_enabled = True
else:
self._widget.gyro_cal_btn.setEnabled(False)
self._widget.gyro_cal_btn.setText(str(5 - 0.1*floor(10*(rospy.Time.now() - self.cal_time).to_sec())))
# Update joystick graphics
if not self.checkJoystickValid():
self.updateCheckStatus(self._widget.joystick_bind_chk, self.NONE)
if not self.joystick_bind_status:
self._widget.joystick_bind_lbl.setText('')
for l in self.joystick_power_ind:
l.setVisible(True)
self.updateRightStickIndicator(self.stick_ind_range_mid, self.stick_ind_range_mid)
self.updateLeftStickIndicator(self.stick_ind_range_mid, self.stick_ind_range_mid)
else:
self.updateCheckStatus(self._widget.joystick_bind_chk, self.GOOD)
self._widget.joystick_bind_lbl.setText('Joystick bound')
for l in self.joystick_power_ind:
l.setVisible(False)
self.updateRightStickIndicator(self.joystick_data[ChannelIndex.CHAN_LATERAL], self.joystick_data[ChannelIndex.CHAN_FORWARD])
self.updateLeftStickIndicator(self.joystick_data[ChannelIndex.CHAN_ROTATE], self.joystick_data[ChannelIndex.CHAN_ENABLE])
if self.joystick_data[ChannelIndex.CHAN_MODE] < self.stick_ind_range_mid:
self.mode_ind.setPen(self.cyan_pen)
self._widget.modeLabel.setText("0 (Computer)")
else:
self.mode_ind.setPen(self.magenta_pen)
self._widget.modeLabel.setText("1 (Manual)")
# Update joystick data table
for i in range(len(self.joystick_table_vals)):
self.joystick_table_vals[i].setText(str(self.joystick_data[i]))
# Update twist data table
if self.command_received:
self.twist_table_vals[0].setText(str(0.01 * floor(100 * self.current_cmd.linear.x)))
self.twist_table_vals[1].setText(str(0.01 * floor(100 * self.current_cmd.linear.y)))
self.twist_table_vals[2].setText(str(0.01 * floor(100 * self.current_cmd.angular.z)))
else:
self.twist_table_vals[0].setText('Not Published')
self.twist_table_vals[1].setText('Not Published')
self.twist_table_vals[2].setText('Not Published')
# Update battery percentage
self.battery_label.setText(str(self.battery_percent) + ' %')
self.battery_voltage_label.setText(str(0.01 * floor(100 * self.battery_voltage)) + ' V')
# Update mode
self.mode_label.setText(self.getModeString(self.current_mode))
# Update bumper graphics
self.bumperVisibleSwitch(BumperIndex.BUMPER_1F, self.bumper_front_left)
self.bumperVisibleSwitch(BumperIndex.BUMPER_2F, self.bumper_front_right)
self.bumperVisibleSwitch(BumperIndex.BUMPER_3F, self.bumper_rear_left)
self.bumperVisibleSwitch(BumperIndex.BUMPER_4F, self.bumper_rear_right)
self.bumper_state_labels[0].setPlainText(str(self.bumper_front_left))
self.bumper_state_labels[1].setPlainText(str(self.bumper_front_right))
self.bumper_state_labels[2].setPlainText(str(self.bumper_rear_left))
self.bumper_state_labels[3].setPlainText(str(self.bumper_rear_right))
# Update gyro cal graphics
if self.gyro_cal_status:
self.updateCheckStatus(self._widget.gyro_cal_chk, self.GOOD)
self._widget.gyro_cal_lbl.setText('Gyro calibrated')
else:
self.updateCheckStatus(self._widget.gyro_cal_chk, self.BAD)
self._widget.gyro_cal_lbl.setText('Gyro NOT calibrated')
self._widget.gyro_x_lbl.setText('x: ' + str(1e-5*floor(1e5*self.gyro_x)))
self._widget.gyro_y_lbl.setText('y: ' + str(1e-5*floor(1e5*self.gyro_y)))
self._widget.gyro_z_lbl.setText('z: ' + str(1e-5*floor(1e5*self.gyro_z)))
# Update max speed configuration
if not self.max_speed_known:
service_name = '/mobility_base/get_max_speed'
get_max_speed_srv = rospy.ServiceProxy(service_name, GetMaxSpeed)
try:
resp = get_max_speed_srv()
self.max_speed_known = True
self.max_linear_actual = resp.linear
self.max_angular_actual = resp.angular
if self.max_linear_actual <= 0 or not isfinite(self.max_linear_actual):
self._widget.max_linear_lbl.setText('Unlimited')
else:
self._widget.max_linear_lbl.setText(str(1e-2*round(1e2*self.max_linear_actual)))
if self.max_angular_actual <= 0 or not isfinite(self.max_angular_actual):
self._widget.max_angular_lbl.setText('Unlimited')
else:
self._widget.max_angular_lbl.setText(str(1e-2*round(1e2*self.max_angular_actual)))
except:
pass
# print service_name + " doesn't exist"
if self.max_speed_dirty:
self._widget.max_linear_txt.setText(str(self.max_linear_setting))
self._widget.max_angular_txt.setText(str(self.max_angular_setting))
self.max_speed_dirty = False
# Wake time
if self.current_wake_time == rospy.Time(0):
self._widget.wake_time_lbl.setText('Not Set')
else:
self._widget.wake_time_lbl.setText(time.strftime('%m/%d/%Y, %H:%M:%S', time.localtime(self.current_wake_time.to_sec())))
def checkJoystickValid(self):
return self.joystick_data[ChannelIndex.CHAN_FORWARD] > 0 or self.joystick_data[ChannelIndex.CHAN_LATERAL] > 0 or self.joystick_data[ChannelIndex.CHAN_ROTATE] > 0 or self.joystick_data[ChannelIndex.CHAN_MODE] > 0
def getModeString(self, mode_num):
if mode_num == Mode.MODE_ESTOP:
return 'MODE_ESTOP'
elif mode_num == Mode.MODE_DISABLED:
return 'MODE_DISABLED'
elif mode_num == Mode.MODE_BATTERY_LIMP_HOME:
return 'MODE_BATTERY_LIMP_HOME'
elif mode_num == Mode.MODE_BATTERY_CRITICAL:
return 'MODE_BATTERY_CRITICAL'
elif mode_num == Mode.MODE_WIRELESS:
return 'MODE_WIRELESS'
elif mode_num == Mode.MODE_TIMEOUT:
return 'MODE_TIMEOUT'
elif mode_num == Mode.MODE_VELOCITY:
return 'MODE_VELOCITY'
elif mode_num == Mode.MODE_VELOCITY_RAW:
return 'MODE_VELOCITY_RAW'
else:
return ''
def updateChecklist(self):
if self.current_mode >= Mode.MODE_TIMEOUT:
self.checklist_status[self.BATT_MAN] = self.GOOD
self.checklist_status[self.ESTOP_MAN] = self.GOOD
self.checklist_status[self.DISABLE_MAN] = self.GOOD
self.checklist_status[self.JOYSTICK_MAN] = self.BAD
self.checklist_status[self.BATT_COMP] = self.GOOD
self.checklist_status[self.ESTOP_COMP] = self.GOOD
self.checklist_status[self.DISABLE_COMP] = self.GOOD
self.checklist_status[self.JOYSTICK_COMP] = self.GOOD
elif self.current_mode == Mode.MODE_WIRELESS:
self.checklist_status[self.BATT_MAN] = self.GOOD
self.checklist_status[self.ESTOP_MAN] = self.GOOD
self.checklist_status[self.DISABLE_MAN] = self.GOOD
self.checklist_status[self.JOYSTICK_MAN] = self.GOOD
self.checklist_status[self.BATT_COMP] = self.GOOD
self.checklist_status[self.ESTOP_COMP] = self.GOOD
self.checklist_status[self.DISABLE_COMP] = self.GOOD
self.checklist_status[self.JOYSTICK_COMP] = self.BAD
elif self.current_mode == Mode.MODE_DISABLED:
self.checklist_status[self.BATT_MAN] = self.NONE
self.checklist_status[self.ESTOP_MAN] = self.GOOD
self.checklist_status[self.DISABLE_MAN] = self.BAD
self.checklist_status[self.JOYSTICK_MAN] = self.NONE
self.checklist_status[self.BATT_COMP] = self.NONE
self.checklist_status[self.ESTOP_COMP] = self.GOOD
self.checklist_status[self.DISABLE_COMP] = self.BAD
self.checklist_status[self.JOYSTICK_COMP] = self.NONE
elif self.current_mode == Mode.MODE_ESTOP:
self.checklist_status[self.BATT_MAN] = self.NONE
self.checklist_status[self.ESTOP_MAN] = self.BAD
self.checklist_status[self.DISABLE_MAN] = self.NONE
self.checklist_status[self.JOYSTICK_MAN] = self.NONE
self.checklist_status[self.BATT_COMP] = self.NONE
self.checklist_status[self.ESTOP_COMP] = self.BAD
self.checklist_status[self.DISABLE_COMP] = self.NONE
self.checklist_status[self.JOYSTICK_COMP] = self.NONE
elif self.current_mode == Mode.MODE_BATTERY_CRITICAL:
self.checklist_status[self.BATT_MAN] = self.BAD
self.checklist_status[self.ESTOP_MAN] = self.GOOD
self.checklist_status[self.DISABLE_MAN] = self.GOOD
self.checklist_status[self.JOYSTICK_MAN] = self.NONE
self.checklist_status[self.BATT_COMP] = self.BAD
self.checklist_status[self.ESTOP_COMP] = self.GOOD
self.checklist_status[self.DISABLE_COMP] = self.GOOD
self.checklist_status[self.JOYSTICK_COMP] = self.NONE
elif self.current_mode == Mode.MODE_BATTERY_LIMP_HOME:
self.checklist_status[self.BATT_MAN] = self.GOOD
self.checklist_status[self.ESTOP_MAN] = self.GOOD
self.checklist_status[self.DISABLE_MAN] = self.GOOD
self.checklist_status[self.JOYSTICK_MAN] = self.NONE
self.checklist_status[self.BATT_COMP] = self.BAD
self.checklist_status[self.ESTOP_COMP] = self.GOOD
self.checklist_status[self.DISABLE_COMP] = self.GOOD
self.checklist_status[self.JOYSTICK_COMP] = self.NONE
# Check if joystick is suppressed by topic
if self.joystick_suppressed:
self.checklist_status[self.SUPPRESS_COMP] = self.GOOD
self.checklist_status[self.DISABLE_COMP] = self.NONE
self.checklist_status[self.JOYSTICK_COMP] = self.NONE
else:
self.checklist_status[self.SUPPRESS_COMP] = self.NONE
if (rospy.Time.now() - self.last_suppress_time).to_sec() > self.JOYSTICK_SUPPRESS_PERIOD:
self.checklist_status[self.SUPPRESS_MAN] = self.GOOD
else:
self.checklist_status[self.SUPPRESS_MAN] = self.BAD
# Command message received
if self.command_received:
self.checklist_status[self.CMD_COMP] = self.GOOD
else:
self.checklist_status[self.CMD_COMP] = self.BAD
# Override twist checkbox if mode is in TIMEOUT
if self.current_mode == Mode.MODE_TIMEOUT:
self.checklist_status[self.CMD_COMP] = self.BAD
# Update checklist graphics
self.updateCheckStatus(self._widget.batt_man_chk, self.checklist_status[self.BATT_MAN])
self.updateCheckStatus(self._widget.estop_man_chk, self.checklist_status[self.ESTOP_MAN])
self.updateCheckStatus(self._widget.disable_man_chk, self.checklist_status[self.DISABLE_MAN])
self.updateCheckStatus(self._widget.joystick_man_chk, self.checklist_status[self.JOYSTICK_MAN])
self.updateCheckStatus(self._widget.suppress_man_chk, self.checklist_status[self.SUPPRESS_MAN])
self.updateCheckStatus(self._widget.batt_comp_chk, self.checklist_status[self.BATT_COMP])
self.updateCheckStatus(self._widget.estop_comp_chk, self.checklist_status[self.ESTOP_COMP])
self.updateCheckStatus(self._widget.disable_comp_chk, self.checklist_status[self.DISABLE_COMP])
self.updateCheckStatus(self._widget.joystick_comp_chk, self.checklist_status[self.JOYSTICK_COMP])
self.updateCheckStatus(self._widget.suppress_comp_chk, self.checklist_status[self.SUPPRESS_COMP])
self.updateCheckStatus(self._widget.cmd_comp_chk, self.checklist_status[self.CMD_COMP])
self.updateCheckStatus(self._widget.joystick_bind_chk, self.NONE)
def updateCheckStatus(self, obj, icon_type):
if icon_type == self.GOOD:
obj.setPixmap(self.good_icon)
elif icon_type == self.BAD:
obj.setPixmap(self.bad_icon)
else:
obj.setPixmap(self.none_icon)
def updateTable(self):
for i in range(0, len(self.joystick_table_vals)):
self.joystick_table_vals[i].setText(55)
def recvMode(self, mode_msg):
self.current_mode = mode_msg.mode
def recvSuppress(self, suppress_msg):
self.suppress_dt = rospy.Time.now() - self.last_suppress_time
self.last_suppress_time = rospy.Time.now()
def recvTwist(self, twist_msg):
self.twist_dt = rospy.Time.now() - self.last_twist_time
self.last_twist_time = rospy.Time.now()
self.current_cmd = twist_msg
def recvJoystick(self, joystick_msg):
self.joystick_data = joystick_msg.channels;
self.last_joystick_time = rospy.Time.now()
def recvBumpers(self, bumper_msg):
self.bumper_front_left = bumper_msg.front_left
self.bumper_front_right = bumper_msg.front_right
self.bumper_rear_left = bumper_msg.rear_left
self.bumper_rear_right = bumper_msg.rear_right
def recvBattery(self, battery_msg):
self.battery_percent = battery_msg.percent
self.battery_voltage = battery_msg.voltage
def recvBindStatus(self, bind_msg):
self.joystick_bind_status = bind_msg.data
if bind_msg.data:
if self.joystick_bind_dot_counter == 0:
self._widget.joystick_bind_lbl.setText('Binding.')
elif self.joystick_bind_dot_counter == 1:
self._widget.joystick_bind_lbl.setText('Binding..')
elif self.joystick_bind_dot_counter == 2:
self._widget.joystick_bind_lbl.setText('Binding...')
self.joystick_bind_dot_counter = (self.joystick_bind_dot_counter + 1) % 3
def recvGyroCalibrated(self, cal_msg):
self.gyro_cal_status = cal_msg.data
def recvImu(self, imu_msg):
self.gyro_x = imu_msg.angular_velocity.x
self.gyro_y = imu_msg.angular_velocity.y
self.gyro_z = imu_msg.angular_velocity.z
def recvWakeTime(self, time_msg):
self.current_wake_time = time_msg.data
def subscribeTopics(self):
sub_joystick = rospy.Subscriber('/mobility_base/joystick_raw', JoystickRaw, self.recvJoystick)
sub_suppress = rospy.Subscriber('/mobility_base/suppress_wireless', std_msgs.msg.Empty, self.recvSuppress)
sub_twist = rospy.Subscriber('/mobility_base/cmd_vel', Twist, self.recvTwist)
sub_mode = rospy.Subscriber('/mobility_base/mode', Mode, self.recvMode)
sub_bumpers = rospy.Subscriber('/mobility_base/bumper_states', BumperState, self.recvBumpers)
sub_battery = rospy.Subscriber('/mobility_base/battery', BatteryState, self.recvBattery)
sub_bind = rospy.Subscriber('/mobility_base/bind_status', std_msgs.msg.Bool, self.recvBindStatus)
sub_gyro_calibrated = rospy.Subscriber('/mobility_base/imu/is_calibrated', std_msgs.msg.Bool, self.recvGyroCalibrated)
sub_imu = rospy.Subscriber('/mobility_base/imu/data_raw', Imu, self.recvImu)
sub_wake_time = rospy.Subscriber('/mobility_base/wake_time', std_msgs.msg.Time, self.recvWakeTime)
def advertiseTopics(self):
self.pub_start_bind = rospy.Publisher('/mobility_base/bind_start', std_msgs.msg.Empty, queue_size=1)
self.pub_stop_bind = rospy.Publisher('/mobility_base/bind_stop', std_msgs.msg.Empty, queue_size=1)
self.pub_set_wake_time = rospy.Publisher('/mobility_base/set_wake_time', std_msgs.msg.Time, queue_size=1)
def initJoystickGraphics(self):
# Pens
self.cyan_pen = QPen(QColor(0, 255, 255))
self.magenta_pen = QPen(QColor(255, 0, 255))
self.red_pen = QPen(QColor(255, 0, 0))
self.cyan_pen.setWidth(3)
self.magenta_pen.setWidth(3)
self.red_pen.setWidth(3)
self.stick_ind_l = QGraphicsEllipseItem()
self.stick_ind_r = QGraphicsEllipseItem()
self.stick_line_l = QGraphicsLineItem()
self.stick_line_r = QGraphicsLineItem()
self.mode_ind = QGraphicsLineItem()
# Left joystick indicator circle
px_l = self.stick_ind_lox - self.stick_ind_radius
py_l = self.stick_ind_loy - self.stick_ind_radius
self.stick_ind_l.setRect(px_l, py_l, 2 * self.stick_ind_radius, 2 * self.stick_ind_radius)
self.stick_ind_l.setBrush(QBrush(QColor(255, 0, 0)))
self.stick_ind_l.setPen(QPen(QColor(0, 0, 0)))
# Right joystick indicator circle
px_r = self.stick_ind_rox - self.stick_ind_radius
py_r = self.stick_ind_roy - self.stick_ind_radius
self.stick_ind_r.setRect(px_r, py_r, 2 * self.stick_ind_radius, 2 * self.stick_ind_radius)
self.stick_ind_r.setBrush(QBrush(QColor(255, 0, 0)))
self.stick_ind_r.setPen(QPen(QColor(0, 0, 0)))
# Left joystick indicator line
line_pen = QPen(QColor(255,0,0))
line_pen.setWidth(4)
self.stick_line_l.setLine(self.stick_ind_lox, self.stick_ind_loy, self.stick_ind_lox, self.stick_ind_loy)
self.stick_line_l.setPen(line_pen)
# Right joystick indicator line
self.stick_line_r.setLine(self.stick_ind_rox, self.stick_ind_roy, self.stick_ind_rox, self.stick_ind_roy)
self.stick_line_r.setPen(line_pen)
# Mode indicator line
self.mode_ind.setLine(self.mode_ind_x1, self.mode_ind_y1, self.mode_ind_x2, self.mode_ind_y2)
self.mode_ind.setPen(self.cyan_pen)
# Joystick power indicator
self.joystick_power_ind = []
self.joystick_power_ind.append(QGraphicsLineItem(self.power_ind_x1, self.power_ind_y + 20, self.power_ind_x1, self.power_ind_y - 20))
self.joystick_power_ind.append(QGraphicsLineItem(self.power_ind_x1, self.power_ind_y - 20, self.power_ind_x1 + 50, self.power_ind_y - 20))
self.joystick_power_ind.append(QGraphicsLineItem(self.power_ind_x1+50, self.power_ind_y - 20, self.power_ind_x1+50, self.power_ind_y + 20))
self.joystick_power_ind.append(QGraphicsLineItem(self.power_ind_x1+50, self.power_ind_y + 20, self.power_ind_x1, self.power_ind_y + 20))
# Populate scene
graphics_scene = QGraphicsScene()
graphics_scene.addItem(self.stick_ind_l)
graphics_scene.addItem(self.stick_ind_r)
graphics_scene.addItem(self.stick_line_l)
graphics_scene.addItem(self.stick_line_r)
graphics_scene.addItem(self.mode_ind)
for l in self.joystick_power_ind:
l.setPen(self.red_pen)
graphics_scene.addItem(l)
graphics_scene.setSceneRect(0, 0, self._widget.joystickGraphicsView.width() - 4, self._widget.joystickGraphicsView.height() - 4)
self._widget.joystickGraphicsView.setScene(graphics_scene)
self._widget.joystickGraphicsView.setBackgroundBrush(QBrush(QImage(os.path.join(rospkg.RosPack().get_path('mobility_base_tools'), 'images', 'dx6ilabels.jpg'))))
self._widget.joystickGraphicsView.show()
def initBumperGraphics(self):
# Pens
self.blue_pen = QPen(QColor(0,0,255))
self.blue_pen.setWidth(10)
self.bumper_lines = []
# Text state labels
self.bumper_state_labels = [QGraphicsTextItem() for i in range(0,4)]
for i in range(len(self.bumper_state_labels)):
self.bumper_state_labels[i].setFont(QFont('Ubuntu', 14, QFont.Bold))
self.bumper_state_labels[i].setPlainText('00')
self.bumper_state_labels[0].setPos(self.bumper_fl_x-10, self.bumper_fl_y + 55)
self.bumper_state_labels[1].setPos(self.bumper_fr_x-10, self.bumper_fr_y + 55)
self.bumper_state_labels[2].setPos(self.bumper_rl_x-10, self.bumper_rl_y - 80)
self.bumper_state_labels[3].setPos(self.bumper_rr_x-10, self.bumper_rr_y - 80)
# Bumper indicator lines
self.bumperLine(self.bumper_fl_x - 20, self.bumper_fl_y - self.bumper_dy, True)
self.bumperLine(self.bumper_fl_x - self.bumper_dx, self.bumper_fl_y - 20, False)
self.bumperLine(self.bumper_fl_x + self.bumper_dx, self.bumper_fl_y - 20, False)
self.bumperLine(self.bumper_fl_x - 20, self.bumper_fl_y + self.bumper_dy, True)
self.bumperLine(self.bumper_fr_x - 20, self.bumper_fr_y - self.bumper_dy, True)
self.bumperLine(self.bumper_fr_x - self.bumper_dx, self.bumper_fr_y - 20, False)
self.bumperLine(self.bumper_fr_x + self.bumper_dx, self.bumper_fr_y - 20, False)
self.bumperLine(self.bumper_fr_x - 20, self.bumper_fr_y + self.bumper_dy, True)
self.bumperLine(self.bumper_rl_x - 20, self.bumper_rl_y - self.bumper_dy, True)
self.bumperLine(self.bumper_rl_x - self.bumper_dx, self.bumper_rl_y - 20, False)
self.bumperLine(self.bumper_rl_x + self.bumper_dx, self.bumper_rl_y - 20, False)
self.bumperLine(self.bumper_rl_x - 20, self.bumper_rl_y + self.bumper_dy, True)
self.bumperLine(self.bumper_rr_x - 20, self.bumper_rr_y - self.bumper_dy, True)
self.bumperLine(self.bumper_rr_x - self.bumper_dx, self.bumper_rr_y - 20, False)
self.bumperLine(self.bumper_rr_x + self.bumper_dx, self.bumper_rr_y - 20, False)
self.bumperLine(self.bumper_rr_x - 20, self.bumper_rr_y + self.bumper_dy, True)
# Populate scene
graphics_scene = QGraphicsScene()
for bumper in self.bumper_lines:
graphics_scene.addItem(bumper)
for label in self.bumper_state_labels:
graphics_scene.addItem(label)
graphics_scene.setSceneRect(0, 0, self._widget.bumperGraphicsView.width() - 4, self._widget.bumperGraphicsView.height() - 4)
self._widget.bumperGraphicsView.setScene(graphics_scene)
self._widget.bumperGraphicsView.setBackgroundBrush(QBrush(QImage(os.path.join(rospkg.RosPack().get_path('mobility_base_tools'), 'images', 'mb_top.png'))))
self._widget.bumperGraphicsView.show()
def bumperLine(self, x, y, front):
new_line = QGraphicsLineItem()
if front:
new_line.setLine(x, y, x+40, y)
else:
new_line.setLine(x, y, x, y+40)
new_line.setPen(self.blue_pen)
new_line.setVisible(False)
self.bumper_lines.append(new_line)
def bumperVisibleSwitch(self, idx, bumper_state):
if (bumper_state & BumperState.BUMPER_FRONT):
self.bumper_lines[idx].setVisible(True)
else:
self.bumper_lines[idx].setVisible(False)
if (bumper_state & BumperState.BUMPER_LEFT):
self.bumper_lines[idx+1].setVisible(True)
else:
self.bumper_lines[idx+1].setVisible(False)
if (bumper_state & BumperState.BUMPER_RIGHT):
self.bumper_lines[idx+2].setVisible(True)
else:
self.bumper_lines[idx+2].setVisible(False)
if (bumper_state & BumperState.BUMPER_REAR):
self.bumper_lines[idx+3].setVisible(True)
else:
self.bumper_lines[idx+3].setVisible(False)
def resetGuiTimer(self):
# del self.gui_update_timer
self.gui_update_timer = QTimer(self._widget)
self.gui_update_timer.setInterval(100)
self.gui_update_timer.setSingleShot(False)
self.gui_update_timer.timeout.connect(lambda:self.updateGuiCallback())
self.gui_update_timer.start()
def spawnFullGui(self):
super(DiagnosticGui, self).__init__(self.context_)
# Give QObjects reasonable names
self.setObjectName('DiagnosticGui')
# 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('mobility_base_tools'), 'resource', 'DiagnosticGui.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('DiagnosticGui' + str(self.widget_count))
self.widget_count += 1
# Add widget to the user interface
self.context_.add_widget(self._widget)
def spawnTabGui(self):
super(DiagnosticGui, self).__init__(self.context_)
# Give QObjects reasonable names
self.setObjectName('DiagnosticGui')
# 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('mobility_base_tools'), 'resource', 'DiagnosticGuiTabs.ui')
# Extend the widget with all attributes and children from UI file
loadUi(ui_file, self._widget)
# Give QObjects reasonable names
self._widget.setObjectName('DiagnosticGui' + str(self.widget_count))
self.widget_count += 1
# Add widget to the user interface
self.context_.add_widget(self._widget)
def updateRightStickIndicator(self, lat_val, forward_val):
horiz_val = -self.stick_ind_range_factor * (lat_val - self.stick_ind_range_mid)
vert_val = -self.stick_ind_range_factor * (forward_val - self.stick_ind_range_mid)
r = sqrt(horiz_val * horiz_val + vert_val * vert_val)
if r > self.stick_ind_range_pix / 2:
r = self.stick_ind_range_pix / 2
ang = atan2(vert_val, horiz_val)
px = r * cos(ang)
py = r * sin(ang)
self.stick_ind_r.setPos(QPoint(px, py))
self.stick_line_r.setLine(self.stick_ind_rox, self.stick_ind_roy, self.stick_ind_rox + px, self.stick_ind_roy + py)
def updateLeftStickIndicator(self, yaw_val, enable_val):
horiz_val = -self.stick_ind_range_factor * (yaw_val - self.stick_ind_range_mid)
vert_val = -self.stick_ind_range_factor * (enable_val - self.stick_ind_range_mid)
r = sqrt(horiz_val * horiz_val + vert_val * vert_val)
if r > self.stick_ind_range_pix / 2:
r = self.stick_ind_range_pix / 2
ang = atan2(vert_val, horiz_val)
px = r * cos(ang)
py = r * sin(ang)
self.stick_ind_l.setPos(QPoint(px, py))
self.stick_line_l.setLine(self.stick_ind_lox, self.stick_ind_loy, self.stick_ind_lox + px, self.stick_ind_loy + py)
def shutdown_plugin(self):
pass
