auto.py

# -*- coding: utf8 -*-

"""
    auto.py - The view shown when monitoring the car's autonomous movements
    on the map or doing simulations.
"""

from PySide import QtSvg
from PySide.QtGui import *
from PySide.QtCore import *

from widgets import NotificationTooltip, GraphicsCarItem, Waypoint, GraphicalParticleFilter

from probability import ParticleFilter
from collections import deque
from math import atan2, pi, radians, sqrt
import random

from geometry import simplifyPath


class AutoScene(QGraphicsScene):

    def __init__(self, car, parent=None):
        super(AutoScene, self).__init__(parent)


        self.x = 0
        self.y = 0

        # Car object model only
        self.car = car

        # Car graphic representation
        self.graphicCar = None

        # Map generated by parsing an svg file
        self.map = None

        # The last generated path
        self.path = None

        # Graphical representation of the last generated path
        self.graphicalPath = None

        # Heatmap, should be used for probabilities [WIP]
        self.particleFilter = None
        # ( initialized when pressing 'H' )

        # List of the (graphical) waypoints
        self.waypoints = list()

        self.Ynotif = 20
        self.notifications = list()

    def clearNotification(self):
        self.notifications.pop(-1)
        if len(self.notifications) == 0:
            self.Ynotif = 20

    def notify(self, notifText, type=NotificationTooltip.normal):
        tooltip = NotificationTooltip(text=notifText, type=type)
        tooltip.animation.finished.connect(self.clearNotification)

        x = self.width - tooltip.boundingRect().width() - 20
        tooltip.setPos(x, self.Ynotif)

        self.Ynotif += tooltip.boundingRect().height() + 20

        self.notifications.append(tooltip)
        self.addItem(tooltip)

    def pathfinding(self, x, y):

        if not self.map.isReachable(x,y):
            self.notify("The chosen goal is unreachable.", type=NotificationTooltip.error)
        elif not self.map.isReachable(self.car.x, self.car.y):
            self.notify("Can't move the car from its current position.", type=NotificationTooltip.error)
        else:
            # We generate a path from the car to where we clicked and show it on the UI
            # We get the path from our 'map' object
            self.path = self.map.search((self.car.x, self.car.y), (x, y))

            if len(self.path) == 0:
                return

            # And a simple version of the path (to be sent to the car)
            self.sPath = simplifyPath(self.path)

            # If the car is currently on a path, we end it
            self.pathFinished()

            # We build a polyline graphic item
            painterPath = QPainterPath()
            totalPath = QPainterPath()

            self.waypoints = list()

            painterPath.moveTo(self.sPath[0].x, self.sPath[0].y)
            totalPath.moveTo(self.path[0].x, self.path[0].y)

            waypoint = Waypoint(self.path[0].x, self.path[0].y)
            self.addItem(waypoint)
            self.waypoints.append(waypoint)

            # We draw a line (and waypoints) of the simplified path
            for i in xrange(1, len(self.sPath)):
                x, y = self.sPath[i].x, self.sPath[i].y

                painterPath.lineTo(x, y)

                waypoint = Waypoint(x, y)
                self.addItem(waypoint)
                self.waypoints.append(waypoint)

            # We draw a path (mainly just for the total distance) of the original path
            for i in xrange(1, len(self.path)):
                x, y = self.path[i].x, self.path[i].y
                totalPath.lineTo(x, y)

            # We update the path shown on screen
            self.graphicalPath.setPath(painterPath)

            # Animating the car on the path
            self.animation = QParallelAnimationGroup()

            posAnim = QPropertyAnimation(self.car, "positionProperty")
            rotAnim = QPropertyAnimation(self.car, "angleProperty")

            # Calculating the animation's duration
            totalLength = totalPath.length()
            pixelsPerMS = 200. / 1000.
            totalDuration = totalLength / pixelsPerMS

            posAnim.setDuration(totalDuration)
            rotAnim.setDuration(totalDuration)

            posAnim.setKeyValueAt(0, QPointF(self.car.x, self.car.y))
            rotAnim.setKeyValueAt(0, self.car.angle)

            angles = deque()
            angles.append(self.car.angle)
            curAngle = self.car.angle

            t = 0.

            for i in xrange(1, len(self.path) - 1):
                #This loop describes going from path[i-1] to path[i]

                pt = self.path[i]
                lastPt = self.path[i-1]

                distance = sqrt( (pt.x - lastPt.x)**2 + (pt.y - lastPt.y)**2 )

                # Time's evolution (distance / speed)
                t += distance/pixelsPerMS

                posAnim.setKeyValueAt(t/totalDuration, QPointF(pt.x, pt.y))

                # Calculation of the 'new' angle
                newAngle = pi/2 + radians(self.car.map.north_angle) - atan2(lastPt.y - pt.y, lastPt.x - pt.x)

                if abs(2*pi + newAngle - curAngle) < abs(newAngle - curAngle):
                    curAngle = 2*pi + newAngle
                else:
                    curAngle = newAngle

                angles.append( curAngle )
                rotAnim.setKeyValueAt(t/totalDuration, sum(angles)/len(angles))

                if len(angles) > 10:
                    angles.popleft()


            posAnim.setEndValue(QPointF(self.path[-1].x, self.path[-1].y))
            rotAnim.setEndValue(angles[-1])

            posAnim.setEasingCurve(QEasingCurve.InOutQuad)
            rotAnim.setEasingCurve(QEasingCurve.InOutQuad)

            self.animation.addAnimation(rotAnim)
            self.animation.addAnimation(posAnim)

            self.animation.finished.connect(self.pathFinished)

            self.animation.start(QAbstractAnimation.DeleteWhenStopped)
            self.car.setMoving(True)

    def pathFinished(self):
    # Called when the car has arrived to the path's end
        self.car.setMoving(False)
        self.graphicalPath.setPath(QPainterPath())

        for point in self.waypoints:
            self.removeItem(point)

        self.waypoints = list()

    def mousePressEvent(self, event):
        x, y = event.scenePos().x(), event.scenePos().y()

        self.pathfinding(x, y)

        super(AutoScene, self).mousePressEvent(event)

    def mouseMoveEvent(self, event):
        x, y = event.scenePos().x(), event.scenePos().y()

        if not self.car.moving:
            # We calculate the angle (in radians)
            angle = pi/2 + radians(self.car.map.north_angle) - atan2(self.car.y - y, self.car.x - x)
            self.car.setAngle(angle)

    def keyPressEvent(self, event):

        if event.key() == Qt.Key_H:
            # Hiding/showing the particle filter
            self.heatmap.setVisible(not self.heatmap.isVisible())
        elif event.key() == Qt.Key_R:
            # Reseting the particle filter
            self.particleFilter.reset()
            self.heatmap.update()
        elif not self.car.moving:
            # Moving the car

            speed = 0
            deltaAngle = 0

            if event.key() == Qt.Key_Up or event.key() == Qt.Key_Z:
                speed = 20
            elif event.key() == Qt.Key_Down or event.key() == Qt.Key_S:
                speed = -20
            elif event.key() == Qt.Key_Right or event.key() == Qt.Key_D:
                deltaAngle = -pi/32
            elif event.key() == Qt.Key_Left or event.key() == Qt.Key_Q:
                deltaAngle = pi/32

            if speed != 0 or deltaAngle != 0:
                self.car.setAngle(self.car.angle + deltaAngle)

                # Adding some noise to the displacement
                nSpeed = speed + random.gauss(0.0, (self.car.displacement_noise/100.)*speed)
                if speed != 0:
                    # Simulating car's deviation
                    deltaAngle += random.gauss(0.0, radians(self.car.rotation_noise))
                    self.car.move(nSpeed)


                if self.heatmap.isVisible():
                    # Noise on the car's current angle
                    noisyCarAngle = self.car.angle + random.gauss(0.0, radians(self.car.rotation_noise))
                    self.particleFilter.setAngle(noisyCarAngle)
                    self.particleFilter.move(speed)
                    self.particleFilter.sense(self.car.distance, noisyCarAngle)
                    self.particleFilter.resample()
                    self.heatmap.update()

                    relevance = self.particleFilter.relevance
                    if relevance >= ParticleFilter.DecentRelevance and not self.car.localized:
                        self.notify("Car localized with a {}% relevance rate".format(int(100*relevance)),
                                    type=NotificationTooltip.ok)
                        self.car.localized = True
                    elif self.car.localized and relevance < ParticleFilter.DecentRelevance - 0.10:
                        self.notify("Lost car's localization !")

                        self.car.localized = False

                # Putting back the car into the map if it got out
                # x = min(max(0, self.car.x), self.map.width - 1)
                # y = min(max(0, self.car.y), self.map.height - 1)
                # self.car.setPosition(QPointF(x, y))

    def setMapScale(self):
        ok = False
        while not ok:
            # We ask for the scale in 'mm per px' as it's easier to imagine, but convert it to px per mm
            curValue = 1. / self.map.pixel_per_mm if self.map.pixel_per_mm is not None else 1.
            mm_per_px, ok = QInputDialog.getDouble(self.views()[0], "Map's scale", "Enter the map's scale (mm per px):",
                                                      value=curValue)

        self.map.setScale(1. / mm_per_px)

    def setMapNorthAngle(self):
        ok = False
        while not ok:
            curValue = self.map.north_angle if self.map.north_angle is not None else 0.
            north_angle, ok = QInputDialog.getDouble(self.views()[0], "Map's north angle",
                "Enter the angle corresponding to the map's top :", value=curValue)
        self.map.setNorthAngle(north_angle)

class AutoView(QGraphicsView):
    Native, OpenGL, Image = range(3)

    def __init__(self, car, parent=None):
        super(AutoView, self).__init__(parent)

        self.setRenderHints(QPainter.Antialiasing | QPainter.SmoothPixmapTransform)

        self.renderer = AutoView.OpenGL
        self.svgItem = None
        self.backgroundItem = None

        self.setScene(AutoScene(car=car, parent=self))
        self.setTransformationAnchor(QGraphicsView.AnchorUnderMouse)
        self.setDragMode(QGraphicsView.ScrollHandDrag)

        self.setBackgroundBrush(QImage("img/blueprintDark.png"))
        self.setCacheMode(QGraphicsView.CacheBackground)

        # Disabling scrollbars
        self.setVerticalScrollBarPolicy(Qt.ScrollBarAlwaysOff)
        self.setHorizontalScrollBarPolicy(Qt.ScrollBarAlwaysOff)


    def openMap(self, svg_map):
        s = self.scene()

        s.map = svg_map
        # If the map doesn't contain information about the map's scale or north angle
        if s.map.pixel_per_mm is None:
            s.setMapScale()

        if s.map.north_angle is None:
            s.setMapNorthAngle()

        s.path = None
        s.graphicalPath = None

        # We remove the current view from the car's model
        s.car.removeView(s.graphicCar)

        if self.backgroundItem:
            drawBackground = self.backgroundItem.isVisible()
        else:
            drawBackground = True

        s.clear()

        # Graphic visualization of the SVG map
        self.svgItem = QtSvg.QGraphicsSvgItem(svg_map.path)
        self.svgItem.setFlags(QGraphicsItem.ItemClipsToShape)
        self.svgItem.setZValue(0)
        s.addItem(self.svgItem)

        # The svg item's height:
        s.width = self.svgItem.boundingRect().width()
        s.height = self.svgItem.boundingRect().height()

        # Background (blueprint image)
        self.backgroundItem = QGraphicsRectItem(self.svgItem.boundingRect())
        self.backgroundItem.setBrush(QImage("img/blueprint.png"))
        self.backgroundItem.setPen(QPen())
        self.backgroundItem.setVisible(drawBackground)
        self.backgroundItem.setZValue(-1)
        self.backgroundItem.setCacheMode(QGraphicsItem.ItemCoordinateCache)
        s.addItem(self.backgroundItem)

        # Shadow effect on the background
        # TODO : See why this is *so* slow when we zoom in ?
        # self.shadow = QGraphicsDropShadowEffect()
        # self.shadow.setBlurRadius(50)
        # self.shadow.setColor( QColor(20, 20, 40) )
        # self.shadow.setOffset(0, 0)
        # self.backgroundItem.setGraphicsEffect( self.shadow )

        # Title text
        self.titleItem = QGraphicsTextItem("Autonomee visualization UI")
        self.titleItem.setFont(QFont("Ubuntu-L.ttf", 35, QFont.Light))
        # 'Dirty' centering of the text
        self.titleItem.setPos(s.width/2 - self.titleItem.boundingRect().width()/2, 5)
        self.titleItem.setDefaultTextColor(QColor(210, 220, 250))
        s.addItem(self.titleItem)
        # Drop shadow on the text
        self.textShadow = QGraphicsDropShadowEffect()
        self.textShadow.setBlurRadius(3)
        self.textShadow.setColor(QColor(20, 20, 40))
        self.textShadow.setOffset(1, 1)
        self.titleItem.setGraphicsEffect(self.textShadow)

        # Compass showing the map's orientation
        # angle = s.map.north_angle if s.map.north_angle is not None else 0.
        # s.graphicCompass = MapCompass(angle)
        # s.graphicCompass.setPos(s.width - 80, 60)
        # s.addItem(s.graphicCompass)

        # We initialize the path's visualization
        s.graphicalPath = QGraphicsPathItem()

        pen = QPen()
        pen.setColor(QColor(180, 200, 240))
        pen.setWidth(3)
        pen.setMiterLimit(10)
        pen.setJoinStyle(Qt.RoundJoin)
        space = 4
        pen.setDashPattern([8, space, 1, space])

        s.graphicalPath.setPen(pen)
        s.graphicalPath.setOpacity(0.8)
        s.graphicalPath.setZValue(-1)

        s.addItem(s.graphicalPath)

        # Car visualization
        s.car.map = s.map
        s.graphicCar = GraphicsCarItem(s.car)
        s.addItem(s.graphicCar)

        # Heatmap
        if s.particleFilter is None:
            s.particleFilter = ParticleFilter(car=s.car, map=s.map)
        else:
            s.particleFilter.reset()
            s.particleFilter.setMap(s.map)
        s.heatmap = GraphicalParticleFilter(s.particleFilter)
        s.heatmap.setVisible(False)
        s.addItem(s.heatmap)

        # Waypoints
        s.waypoints = list()

        self.x = 0
        self.y = 0

        self.updateScene()

    def updateScene(self):
        self.scene().setSceneRect(self.svgItem.boundingRect().adjusted(self.x-10, self.y-10, self.x+10, self.y+10))

    def setRenderer(self, renderer):
        self.renderer = renderer
        self.setViewport(QWidget())

    def setViewBackground(self, enable):
        if self.backgroundItem:
            self.backgroundItem.setVisible(enable)

    def setViewOutline(self, enable):
        if self.outlineItem:
            self.outlineItem.setVisible(enable)

    def wheelEvent(self, event):
        factor = 1.2**(event.delta() / 240.0)

        self.scale(factor, factor)

        event.accept()