def setState(self, ev):
pnt = QPoint(ev.x(), ev.y())
# Since (0,0) is in the upper left corner, that means that increasingly negative Tilt values will be closer to the top of the push button; However,
# However, it makes visual sense to flip this so that increasingly negative Tilt values are closer to the bottom of the push button; thus, the funky math... :)
if (pnt.x() < self.pan_limits[0] or pnt.y() <
(self.size - self.tilt_limits[1]) or pnt.x() > self.pan_limits[1]
or pnt.y() > (self.size - self.tilt_limits[0])):
return
if self.state == pnt:
return
self.state = pnt
self.update()
if self.isChecked():
cmd_x = self.pxl2Deg(self.state.x(), self.pan_deg_limits[0],
self.pan_deg_limits[1])
cmd_y = -self.pxl2Deg(self.state.y(), self.tilt_deg_limits[0],
self.tilt_deg_limits[1])
self.gui.name_map["pan"]["display"].setText("%.1f" % cmd_x)
self.gui.name_map["tilt"]["display"].setText("%.1f" % cmd_y)
self.gui.update_slider_bar("pan")
self.gui.update_slider_bar("tilt")
class GLWidget(QGLWidget):
def __init__(self, parent=None):
glformat = QGLFormat()
glformat.setSampleBuffers(True)
super(GLWidget, self).__init__(glformat, parent)
self.setCursor(Qt.OpenHandCursor)
self.setMouseTracking(True)
self._modelview_matrix = numpy.identity(4)
self._near = 0.1
self._far = 4000.0
self._fovy = 45.0
self._radius = 50.0
self._last_point_2d = QPoint()
self._last_point_3d = [0.0, 0.0, 0.0]
self._last_point_3d_ok = False
def initializeGL(self):
glClearColor(0.65, 0.65, 0.65, 0.0)
glEnable(GL_DEPTH_TEST)
def resizeGL(self, width, height):
glViewport(0, 0, width, height)
self.set_projection(self._near, self._far, self._fovy)
self.updateGL()
def paintGL(self):
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
glMatrixMode(GL_MODELVIEW)
glLoadMatrixd(self._modelview_matrix)
def get_view_matrix(self):
return self._modelview_matrix.tolist()
def set_view_matrix(self, matrix):
self._modelview_matrix = numpy.array(matrix)
def load_view_matrix(self,view_matrix):
self.makeCurrent()
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
glLoadMatrixd(view_matrix)
self._modelview_matrix = glGetDoublev(GL_MODELVIEW_MATRIX)
def set_projection(self, near, far, fovy):
self._near = near
self._far = far
self._fovy = fovy
self.makeCurrent()
glMatrixMode(GL_PROJECTION)
glLoadIdentity()
height = max(self.height(), 1)
gluPerspective(self._fovy, float(self.width()) / float(height), self._near, self._far)
self.updateGL()
def reset_view(self):
# scene pos and size
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
self._modelview_matrix = glGetDoublev(GL_MODELVIEW_MATRIX)
self.view_all()
def reset_rotation(self):
self._modelview_matrix[0] = [1.0, 0.0, 0.0, 0.0]
self._modelview_matrix[1] = [0.0, 1.0, 0.0, 0.0]
self._modelview_matrix[2] = [0.0, 0.0, 1.0, 0.0]
glMatrixMode(GL_MODELVIEW)
glLoadMatrixd(self._modelview_matrix)
def translate(self, trans):
# translate the object
self.makeCurrent()
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
glTranslated(trans[0], trans[1], trans[2])
glMultMatrixd(self._modelview_matrix)
# update _modelview_matrix
self._modelview_matrix = glGetDoublev(GL_MODELVIEW_MATRIX)
def translate_absolute(self, trans):
# translate the object
self.makeCurrent()
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
glTranslated(trans[0], trans[1], trans[2])
self._modelview_matrix[3] = [0, 0, 0, self._modelview_matrix[3][3]]
glMultMatrixd(self._modelview_matrix)
# update _modelview_matrix
self._modelview_matrix = glGetDoublev(GL_MODELVIEW_MATRIX)
def rotate(self, axis, angle):
# rotate the object
self.makeCurrent()
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
t = [self._modelview_matrix[3][0], self._modelview_matrix[3][1], self._modelview_matrix[3][2]]
glTranslatef(t[0], t[1], t[2])
glRotated(angle, axis[0], axis[1], axis[2])
glTranslatef(-t[0], -t[1], -t[2])
glMultMatrixd(self._modelview_matrix)
# update _modelview_matrix
self._modelview_matrix = glGetDoublev(GL_MODELVIEW_MATRIX)
def rotate_absolute(self,axis,angle):
# rotate the object
self.makeCurrent()
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
t = [self._modelview_matrix[3][0], self._modelview_matrix[3][1], self._modelview_matrix[3][2]]
glTranslatef(t[0], t[1], t[2])
glRotated(angle, axis[0], axis[1], axis[2])
glTranslatef(-t[0], -t[1], -t[2])
matrix = glGetDoublev(GL_MODELVIEW_MATRIX)
matrix[3][0], matrix[3][1],matrix[3][2] = t
glLoadMatrixd(matrix)
# update _modelview_matrix
self._modelview_matrix = glGetDoublev(GL_MODELVIEW_MATRIX)
def view_all(self):
self.translate([-self._modelview_matrix[0][3], -self._modelview_matrix[1][3], -self._modelview_matrix[2][3] - self._radius / 2.0])
def wheelEvent(self, event):
# only zoom when no mouse buttons are pressed, to prevent interference with other user interactions
if event.buttons() == Qt.NoButton:
try:
delta = event.angleDelta().y()
except AttributeError:
delta = event.delta()
d = float(delta) / 200.0 * self._radius
self.translate([0.0, 0.0, d])
self.updateGL()
event.accept()
def mousePressEvent(self, event):
self._last_point_2d = event.pos()
self._last_point_3d_ok, self._last_point_3d = self._map_to_sphere(self._last_point_2d)
def mouseMoveEvent(self, event):
new_point_2d = event.pos()
if not self.rect().contains(new_point_2d):
return
new_point_3d_ok, new_point_3d = self._map_to_sphere(new_point_2d)
dy = float(new_point_2d.y() - self._last_point_2d.y())
h = float(self.height())
# left button: move in x-y-direction
if event.buttons() == Qt.LeftButton and event.modifiers() == Qt.NoModifier:
dx = float(new_point_2d.x() - self._last_point_2d.x())
w = float(self.width())
z = -self._modelview_matrix[3][2] / self._modelview_matrix[3][3]
n = 0.01 * self._radius
up = math.tan(self._fovy / 2.0 * math.pi / 180.0) * n
right = up * w / h
self.translate([2.0 * dx / w * right / n * z, -2.0 * dy / h * up / n * z, 0.0])
# left and middle button (or left + ctrl): rotate around center
elif event.buttons() == (Qt.LeftButton | Qt.MidButton) or (event.buttons() == Qt.LeftButton and event.modifiers() == Qt.ControlModifier):
if self._last_point_3d_ok and new_point_3d_ok:
cos_angle = numpy.dot(self._last_point_3d, new_point_3d)
if abs(cos_angle) < 1.0:
axis = numpy.cross(self._last_point_3d, new_point_3d)
angle = 2.0 * math.acos(cos_angle) * 180.0 / math.pi
self.rotate(axis, angle)
# middle button (or left + shift): move in z-direction
elif event.buttons() == Qt.MidButton or (event.buttons() == Qt.LeftButton and event.modifiers() == Qt.ShiftModifier):
delta_z = self._radius * dy * 20.0 / h
self.translate([0.0, 0.0, delta_z])
# remember the new points and flag
self._last_point_2d = new_point_2d
self._last_point_3d = new_point_3d
self._last_point_3d_ok = new_point_3d_ok
# trigger redraw
self.updateGL()
def mouseReleaseEvent(self, _event):
self._last_point_3d_ok = False
def _map_to_sphere(self, pos):
v = [0.0, 0.0, 0.0]
# check if inside widget
if self.rect().contains(pos):
# map widget coordinates to the centered unit square [-0.5..0.5] x [-0.5..0.5]
v[0] = float(pos.x() - 0.5 * self.width()) / self.width()
v[1] = float(0.5 * self.height() - pos.y()) / self.height()
# use Pythagoras to compute z (the sphere has radius sqrt(2.0*0.5*0.5))
v[2] = math.sqrt(max(0.5 - v[0] * v[0] - v[1] * v[1], 0.0))
# normalize direction to unit sphere
v = numpy.array(v) / numpy.linalg.norm(v)
return True, v
else:
return False, v
def unproject_mouse_on_scene(self,pos):
start_x, start_y, start_z = gluUnProject(pos.x(), pos.y(), 1, model=self._modelview_matrix,
proj=glGetDoublev(GL_PROJECTION_MATRIX))
end_x, end_y, end_z = gluUnProject(pos.x(), pos.y(), 0, model=self._modelview_matrix,
proj=glGetDoublev(GL_PROJECTION_MATRIX))
diff_x = end_x - start_x
diff_y = end_y - start_y
diff_z = end_z - start_z
t = (0 - start_z) / diff_z
x = start_x + (diff_x * t)
y = start_y + (diff_y * t)
return x, y,0
class GLWidget(QGLWidget):
def __init__(self, parent=None):
glformat = QGLFormat()
glformat.setSampleBuffers(True)
super(GLWidget, self).__init__(glformat, parent)
self.setCursor(Qt.OpenHandCursor)
self.setMouseTracking(True)
self._modelview_matrix = numpy.identity(4)
self._near = 0.001
self._far = 100000.0
self._fovy = 45.0
self._radius = 5.0
self._last_point_2d = QPoint()
self._last_point_3d = [0.0, 0.0, 0.0]
self._last_point_3d_ok = False
## ============================================
## callbacks for QGLWidget
def initializeGL(self):
glClearColor(1.0, 0.0, 0.0, 0.0)
glEnable(GL_DEPTH_TEST)
def resizeGL(self, width, height):
glViewport(0, 0, width, height)
self.set_projection(self._near, self._far, self._fovy)
self.updateGL()
def paintGL(self):
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT)
glMatrixMode(GL_MODELVIEW)
glLoadMatrixd(self._modelview_matrix)
def get_view_matrix(self):
return self._modelview_matrix.tolist()
def set_view_matrix(self, matrix):
self._modelview_matrix = numpy.array(matrix)
def set_projection(self, near, far, fovy):
self._near = near
self._far = far
self._fovy = fovy
self.makeCurrent()
glMatrixMode(GL_PROJECTION)
glLoadIdentity()
height = max(self.height(), 1)
gluPerspective(self._fovy,
float(self.width()) / float(height), self._near,
self._far)
self.updateGL()
def reset_view(self):
# scene pos and size
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
self._modelview_matrix = glGetDoublev(GL_MODELVIEW_MATRIX)
self.view_all()
def reset_rotation(self):
self._modelview_matrix[0] = [1.0, 0.0, 0.0, 0.0]
self._modelview_matrix[1] = [0.0, 1.0, 0.0, 0.0]
self._modelview_matrix[2] = [0.0, 0.0, 1.0, 0.0]
glMatrixMode(GL_MODELVIEW)
glLoadMatrixd(self._modelview_matrix)
def translate(self, trans):
# translate the object
self.makeCurrent()
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
glTranslated(trans[0], trans[1], trans[2])
glMultMatrixd(self._modelview_matrix)
# update _modelview_matrix
self._modelview_matrix = glGetDoublev(GL_MODELVIEW_MATRIX)
def rotate(self, axis, angle):
# rotate the object
self.makeCurrent()
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
t = [
self._modelview_matrix[3][0], self._modelview_matrix[3][1],
self._modelview_matrix[3][2]
]
glTranslatef(t[0], t[1], t[2])
glRotated(angle, axis[0], axis[1], axis[2])
glTranslatef(-t[0], -t[1], -t[2])
glMultMatrixd(self._modelview_matrix)
# update _modelview_matrix
self._modelview_matrix = glGetDoublev(GL_MODELVIEW_MATRIX)
def view_all(self):
self.translate([
-self._modelview_matrix[0][3], -self._modelview_matrix[1][3],
-self._modelview_matrix[2][3] - self._radius / 2.0
])
## ============================================
## Qt's event(?)
def wheelEvent(self, event):
# only zoom when no mouse buttons are pressed, to prevent interference with other user interactions
if event.buttons() == Qt.NoButton:
d = float(event.delta()) / 200.0 * self._radius
self.translate([0.0, 0.0, d])
self.updateGL()
event.accept()
def mousePressEvent(self, event):
self._last_point_2d = event.pos()
self._last_point_3d_ok, self._last_point_3d = self._map_to_sphere(
self._last_point_2d)
def mouseMoveEvent(self, event):
new_point_2d = event.pos()
if not self.rect().contains(new_point_2d):
return
new_point_3d_ok, new_point_3d = self._map_to_sphere(new_point_2d)
dy = float(new_point_2d.y() - self._last_point_2d.y())
h = float(self.height())
# left button: rotate around center
if event.buttons() == Qt.LeftButton and event.modifiers(
) == Qt.NoModifier:
if self._last_point_3d_ok and new_point_3d_ok:
cos_angle = numpy.dot(self._last_point_3d, new_point_3d)
if abs(cos_angle) < 1.0:
axis = numpy.cross(self._last_point_3d, new_point_3d)
angle = 2.0 * math.acos(cos_angle) * 180.0 / math.pi
self.rotate(axis, angle)
# middle button (or left + shift): move in x-y-direction
elif event.buttons() == Qt.MidButton or (
event.buttons() == Qt.LeftButton
and event.modifiers() == Qt.ShiftModifier):
dx = float(new_point_2d.x() - self._last_point_2d.x())
w = float(self.width())
z = -self._modelview_matrix[3][2] / self._modelview_matrix[3][3]
n = 0.01 * self._radius
up = math.tan(self._fovy / 2.0 * math.pi / 180.0) * n
right = up * w / h
self.translate([
2.0 * dx / w * right / n * z, -2.0 * dy / h * up / n * z, 0.0
])
# left and middle button (or left + ctrl): move in z-direction
elif event.buttons() == (Qt.LeftButton | Qt.MidButton) or (
event.buttons() == Qt.LeftButton
and event.modifiers() == Qt.ControlModifier):
delta_z = self._radius * dy * 2.0 / h
self.translate([0.0, 0.0, delta_z])
# remember the new points and flag
self._last_point_2d = new_point_2d
self._last_point_3d = new_point_3d
self._last_point_3d_ok = new_point_3d_ok
# trigger redraw
self.updateGL()
def mouseReleaseEvent(self, _event):
self._last_point_3d_ok = False
def _map_to_sphere(self, pos):
v = [0.0, 0.0, 0.0]
# check if inside widget
if self.rect().contains(pos):
# map widget coordinates to the centered unit square [-0.5..0.5] x [-0.5..0.5]
v[0] = float(pos.x() - 0.5 * self.width()) / self.width()
v[1] = float(0.5 * self.height() - pos.y()) / self.height()
# use Pythagoras to compute z (the sphere has radius sqrt(2.0*0.5*0.5))
v[2] = math.sqrt(max(0.5 - v[0] * v[0] - v[1] * v[1], 0.0))
# normalize direction to unit sphere
v = numpy.array(v) / numpy.linalg.norm(v)
return True, v
else:
return False, v
def get_texture(self, qimage):
return self.bindTexture(qimage)
class JoystickButton(QPushButton):
### @brief Initialization of the JoystickButton class; it builds up from a QPushButton
### @param gui - the parent widget
### @details - code inspired from the JoystickButton class created in 'pyqtgraph'
def __init__(self, gui):
super(JoystickButton, self).__init__()
self.gui = gui # parent widget
self.size = 360 # size in pixels of the push button
self.state = None # a QPoint representing the position of the crosshair in pixels
self.pan_limits = (0, 360) # width of the push button in pixels
self.tilt_limits = (0, 360) # height of the push button in pixels
self.setCheckable(True) # allow the push button to be 'checked'
self.setFixedWidth(self.size)
self.setFixedHeight(self.size)
self.center = QPoint(
self.size / 2.0, self.size / 2.0
) # (0,0) in pixels start from the upper left corner of the push button, so set the center to the middle (QPoint type)
self.current_pos = QPoint(
self.size / 2.0, self.size / 2.0
) # a QPoint representing the current position of the turret in pixels
self.pan_deg_limits = [
self.gui.name_map["pan"]["min_lower_limit"],
self.gui.name_map["pan"]["max_upper_limit"]
] # Pan joint limits in degrees
self.tilt_deg_limits = [
self.gui.name_map["tilt"]["min_lower_limit"],
self.gui.name_map["tilt"]["max_upper_limit"]
] # Tilt joint limits in degrees
self.setJointCommands([
float(self.gui.name_map["pan"]["display"].text()),
float(self.gui.name_map["tilt"]["display"].text())
]) # set the default crosshair position to the current turret position
### @brief Returns whether the joystick button is currently pressed
### @param <boolean> [out] - current button state
def isActive(self):
if self.isChecked():
return True
else:
return False
### @brief Redefines what happens when the Joystick button is pressed
### @param ev - QMouseEvent
def mousePressEvent(self, ev):
ev.accept()
self.setChecked(True)
self.setState(ev)
### @brief Redefines what happens when the mouse is moved
### @param ev - QMouseEvent
def mouseMoveEvent(self, ev):
ev.accept()
self.setState(ev)
### @brief Redefines what happens when the mouse is released
### @param ev - QMouseEvent
def mouseReleaseEvent(self, ev):
ev.accept()
self.setChecked(False)
### @brief Draws a circle in the QPushButton
### @param pnt - QPoint specifying where the circle should be drawn
### @param color - QColor specifying the circle color
### @param fill - whether the inside of the circle should be colored
def drawCircle(self, pnt, color, fill=True):
p = QPainter(self)
p.setPen(color)
if (fill):
p.setBrush(QBrush(color))
p.drawEllipse(pnt, 3, 3)
### @brief Draws a crosshair centered at the specified point
### @param pnt - QPoint specifying the point at which to draw the crosshair
def drawCrosshair(self, pnt):
p = QPainter(self)
horz_line = QLine(0, pnt.y(), self.size, pnt.y())
vert_line = QLine(pnt.x(), 0, pnt.x(), self.size)
p.drawLines(horz_line, vert_line)
### @brief Draws a green rectangle showing the valid positions that the pan and tilt motors can be commanded
def drawRect(self):
p = QPainter(self)
in_bounds = QRect(self.pan_limits[0], self.size - self.tilt_limits[1],
self.pan_limits[1] - self.pan_limits[0],
self.tilt_limits[1] - self.tilt_limits[0])
p.fillRect(in_bounds, QColor(0, 200, 0, 128))
p.drawRect(in_bounds)
### @brief Redefines a paint event (essentially what is drawn on the QPushButton)
### @param ev - QPaintEvent
def paintEvent(self, ev):
QPushButton.paintEvent(self, ev)
self.drawRect()
self.drawCircle(self.state, QColor(0, 0, 0), False)
self.drawCircle(self.center, QColor(0, 0, 0))
self.drawCircle(self.current_pos, QColor(255, 0, 0))
self.drawCrosshair(self.state)
### @brief Converts from degrees to pixels
### @param value - values in degrees to convert to pixels
### @param min_lower_limit - absolute minimum angle that the motor can reach in degrees
### @param max_upper_limit - absolute maximum angle that the motor can reach in degrees
### @param new_val - value converted to pixels
def deg2Pxl(self, value, min_lower_limit, max_upper_limit):
degree_span = max_upper_limit - min_lower_limit
value_scaled = float(value - min_lower_limit) / degree_span
new_val = value_scaled * self.size
return new_val
### @brief Converts from pixels to degrees
### @param value - values in pixels to convert to degrees
### @param min_lower_limit - absolute minimum angle that the motor can reach in degrees
### @param max_upper_limit - absolute maximum angle that the motor can reach in degrees
### @param new_val - value converted to degrees
def pxl2Deg(self, value, min_lower_limit, max_upper_limit):
value_scaled = float(value) / self.size
new_val = min_lower_limit + value_scaled * (max_upper_limit -
min_lower_limit)
return new_val
### @brief Sets the state of the crosshair in pixels
### @param ev - QMouseEvent containing the position of the cursor in pixels
### @details - checks to make sure that the desired crosshair position is within the pan and tilt limits before setting it; it
### also converts the cursor position from pixels to degrees and updates the parent widget's slider bars accordingly
def setState(self, ev):
pnt = QPoint(ev.x(), ev.y())
# Since (0,0) is in the upper left corner, that means that increasingly negative Tilt values will be closer to the top of the push button; However,
# However, it makes visual sense to flip this so that increasingly negative Tilt values are closer to the bottom of the push button; thus, the funky math... :)
if (pnt.x() < self.pan_limits[0] or pnt.y() <
(self.size - self.tilt_limits[1]) or pnt.x() > self.pan_limits[1]
or pnt.y() > (self.size - self.tilt_limits[0])):
return
if self.state == pnt:
return
self.state = pnt
self.update()
if self.isChecked():
cmd_x = self.pxl2Deg(self.state.x(), self.pan_deg_limits[0],
self.pan_deg_limits[1])
cmd_y = -self.pxl2Deg(self.state.y(), self.tilt_deg_limits[0],
self.tilt_deg_limits[1])
self.gui.name_map["pan"]["display"].setText("%.1f" % cmd_x)
self.gui.name_map["tilt"]["display"].setText("%.1f" % cmd_y)
self.gui.update_slider_bar("pan")
self.gui.update_slider_bar("tilt")
### @brief If the user changes the default Pan min/max values, this function converts the values from degrees to pixels
### @param min_value - new lower limit in degrees
### @param max_value - new upper limit in degrees
def setPanMinMax(self, min_value, max_value):
new_min = round(
self.deg2Pxl(min_value, self.pan_deg_limits[0],
self.pan_deg_limits[1]))
new_max = round(
self.deg2Pxl(max_value, self.pan_deg_limits[0],
self.pan_deg_limits[1]))
self.pan_limits = (new_min, new_max)
self.update()
### @brief If the user changes the default Tilt min/max values, this function converts the values from degrees to pixels
### @param min_value - new lower limit in degrees
### @param max_value - new upper limit in degrees
def setTiltMinMax(self, min_value, max_value):
new_min = round(
self.deg2Pxl(min_value, self.tilt_deg_limits[0],
self.tilt_deg_limits[1]))
new_max = round(
self.deg2Pxl(max_value, self.tilt_deg_limits[0],
self.tilt_deg_limits[1]))
self.tilt_limits = (new_min, new_max)
self.update()
### @brief Converts the current joint positions from degrees to pixels
### @param positions - list containing the pan and tilt values in degrees
def setJointStates(self, positions):
new_pan = round(
self.deg2Pxl(positions[0], self.pan_deg_limits[0],
self.pan_deg_limits[1]))
new_tilt = round(self.size - self.deg2Pxl(
positions[1], self.tilt_deg_limits[0], self.tilt_deg_limits[1]))
# Since this function is called at around 100 Hz, only update the QPushButton if the current position has significantly changed
if (abs(new_pan - self.current_pos.x()) >= 1
or abs(new_tilt - self.current_pos.y()) >= 1):
self.current_pos = QPoint(new_pan, new_tilt)
self.update()
### @brief Converts the current joint commands from degrees to pixels
### @param commands - list containing the pan and tilt values in degrees
def setJointCommands(self, commands):
new_pan = round(
self.deg2Pxl(commands[0], self.pan_deg_limits[0],
self.pan_deg_limits[1]))
new_tilt = round(self.size - self.deg2Pxl(
commands[1], self.tilt_deg_limits[0], self.tilt_deg_limits[1]))
self.setState(QPoint(new_pan, new_tilt))
self.update()
