Beispiel #1
0
    def __init__(self, communicator, internCommunicator, locomotionSystem):
        self.communicator = communicator
        self.internCommunicator = internCommunicator
        self.locomotionSystem = LocomotionSystem(self.internCommunicator)
        self.cartographySystem = self._createBaseCartographySystem()
        self.pathfinder = Pathfinder(self.SQUARE_SIZE_IN_CENTIMETERS)
        self.imageTaker = LogitechCameraImageTaker(saveImages=True)
        self.letterCommandFinder = CommandFinder(self.LETTER_TEMPLATE_DIR, FirstCommandPanelLineFinderBuilder(), 1)
        self.directionCommandFinder = CommandFinder(self.DIRECTION_TEMPLATE_DIR, SecondCommandPanelLineFinderBuilder(), 2)
        self.puckCenterFinder = PuckCenterPixelFinder()
        self.robotToPixelMovement = RobotToPixelMovement()
        self.cornerFinder = CornerFinder()

        self._initializeDevices()
Beispiel #2
0
class iRondelle:
    STARTING_ZONE_CENTER = (55, 55)
    BOTTOM_LEFT = (20, 20)
    PUCK_ZONE = (30, 145)
    CENTER_PUCK_ZONE = (55, 180)
    SQUARE_SIZE_IN_CENTIMETERS = 2
    LETTER_TEMPLATE_DIR = "/root/design3/iRondelle/lib/static/images/letterTemplates"
    DIRECTION_TEMPLATE_DIR = "/root/design3/iRondelle/lib/static/images/directionTemplates"
    HALF_PUCK_SIZE_IN_MILLIMETERS = 55
    DISTANCE_FROM_CENTER_TO_CORNER = 295
    DISTANCE_FROM_ROBOT_CENTER_TO_ELECTROMAGNET = 30
    MILLIMETERS_PER_CENTIMETER = 10
    MAXIMUM_ROTATION_ERROR = 75
    NUMBER_OF_RETRIES = 3


    ANGLE_FOR_PUCK_DROP = {(22, 22): 135,
                           (22, 88): 45,
                           (88, 22): 225,
                           (88, 88): 315}

    def __init__(self, communicator, internCommunicator, locomotionSystem):
        self.communicator = communicator
        self.internCommunicator = internCommunicator
        self.locomotionSystem = LocomotionSystem(self.internCommunicator)
        self.cartographySystem = self._createBaseCartographySystem()
        self.pathfinder = Pathfinder(self.SQUARE_SIZE_IN_CENTIMETERS)
        self.imageTaker = LogitechCameraImageTaker(saveImages=True)
        self.letterCommandFinder = CommandFinder(self.LETTER_TEMPLATE_DIR, FirstCommandPanelLineFinderBuilder(), 1)
        self.directionCommandFinder = CommandFinder(self.DIRECTION_TEMPLATE_DIR, SecondCommandPanelLineFinderBuilder(), 2)
        self.puckCenterFinder = PuckCenterPixelFinder()
        self.robotToPixelMovement = RobotToPixelMovement()
        self.cornerFinder = CornerFinder()

        self._initializeDevices()
             
    def _initializeDevices(self):
        self.internCommunicator.raiseLever()
        self.internCommunicator.deactivateElectromagnet()
        self.internCommunicator.rotateCamera("yaw", 0)
        self.internCommunicator.rotateCamera("pitch", 0)

    def initializeVariables(self):
        self.internCommunicator.turnOffEndingLED()
        self.nextExpectedOrientation = None
        resistanceColorCode = None
        letterCommandPanelValue = None
        directionCommandPanelValue = None
        letterCommand = None
        directionCommand = None
        puckColors = None

    def startSequence(self):
        currentNumber = 1

        while True:
            # Wait until the correct signal is received
            self.waitUntilBaseStationIsReady()

            self.initializeVariables()

            # Rotate to be ready to measure the resistance value
            self.rotateForOhmmeter()

            if currentNumber == 1:
                self.findObstacles()

            resistanceColorCode = self.moveAndMeasureResistance()
            letterCommandPanelValue, _ = resistanceColorCode[0]
            directionCommandPanelValue, _ = resistanceColorCode[2]

            self._moveFromRobotToCoordinates(self.STARTING_ZONE_CENTER, dilatation=NO_DILATATION)

            # Find command from letter command panel
            self.moveToLetterCommandPanel()
            letterCommand = self.findCommand(self.letterCommandFinder, letterCommandPanelValue)
            self.communicator.sendFirstCommandPanelCommand(letterCommand.upper())
            print "Commande panneau lettre: ", letterCommand

            # Find command from direction command panel
            self.moveToDirectionCommandPanel()
            directionCommand = self.findCommand(self.directionCommandFinder, directionCommandPanelValue)

            if directionCommand == "_":
                print "Erreur lecture panneau"
                for _ in range(self.NUMBER_OF_RETRIES):
                    try:
                        self._move(0, -50)
                        image = self.imageTaker.getImage()
                        orientationFinder = LogitechOrientationFinder(image)
                        angleToRotate = orientationFinder.findRotation(45) * -1
                        print "Angle correction: ", angleToRotate
                        self.nextExpectedOrientation = 0
                        self.locomotionSystem.rotate(angleToRotate)
                        break
                    except (ValueError, TypeError):
                        self._move(0, -150)
                else:
                    exit("Échec de trouver une ligne")

                directionCommand = self.findCommand(self.directionCommandFinder, directionCommandPanelValue)

            self.communicator.sendSecondCommandPanelCommand(directionCommand.upper())
            print "Commande panneau direction: ", directionCommand

            # Find which puck to bring to which corner
            puckColors = [color for (value, color) in resistanceColorCode]
            finalPuckPositionFinder = FinalPucksPositionFinder()
            self.finalPucksPosition = finalPuckPositionFinder.findFinalPucksPosition(letterCommand, directionCommand, puckColors)

            print "Pucks: ", self.finalPucksPosition

            self.transformationToFindPucks = TRANSFORMATIONS_FROM_YAWPITCH[(0,-45)]
            self.prepareCameraForPucks()
            self.grabAllPucks(self.finalPucksPosition)

            time.sleep(1)

            self.internCommunicator.turnOnEndingLED()
            self.communicator.sendEndSignal()
            currentRun += 1
            print "Fin de la sequence"

            time.sleep(3)

    def findObstacles(self):
        # Move to the bottom left corner to see obstacles
        self._moveFromRobotToCoordinates(self.BOTTOM_LEFT, dilatation=NO_DILATATION)
        self.addObstaclesToMap()

    def moveAndMeasureResistance(self):
        self.moveToResistanceStation()
        resistanceColorCode = self.measureResistance()
        self._move(0, 200)
        return resistanceColorCode

    def findPuckCoordinates(self, puck, transformationToFindPucks):
        image = self.imageTaker.getImage()
        puckPixelPosition = self.puckCenterFinder.findCenterPixel(image, puck)
        return self.robotToPixelMovement.getMovementToPixel(image, puckPixelPosition, transformationToFindPucks)

    def findPucks(self, puck, transformationToFindPucks):
        leftMovements = [(90, (220, 0)),
                         (90, (220, 0)),
                         (90, None)]
        centerMovements = [(0, (200, 0)),
                           (0, (200, 0)),
                           (0, None)]
        rightMovements = [(270, (220, 0)),
                          (270, (220, 0)),
                          (270, None)]

        for angle, movement in leftMovements + centerMovements + rightMovements:
            self._rotateToAngle(angle)
            try:
                x, y = self.findPuckCoordinates(puck, transformationToFindPucks)
                return x, y
            except PuckNotFoundException:
                pass

            if movement:
                self._move(*movement)

    def waitUntilBaseStationIsReady(self):
        isStartSignalSent = False

        while not isStartSignalSent:
            isStartSignalSent = self.communicator.isStartSignalSent()
            time.sleep(1)

        return isStartSignalSent

    def askForPositionAndOrientation(self):
        validPositionsX = []
        validPositionsY = []

        for _ in range(self.NUMBER_OF_RETRIES):
            position = self.communicator.getRobotPositionAndOrientation()

            if (position.x, position.y) == (-1, -1):
                continue
            else:
                validPositionsX.append(position.x)
                validPositionsY.append(position.y)
        else:
            exit("Erreur position robot")

        positionNumber = len(validPositionsX)
        validPositionsX.sort()
        validPositionsY.sort()

        if positionNumber == 1:
            robotPosition = (validPositionsX[0], validPositionsY[0])
        elif positionNumber % 2 == 0:
            robotPosition = ((validPositionsX[positionNumber/2 - 1] + validPositionsX[position/2])/2,
                             (validPositionsY[positionNumber/2 - 1] + validPositionsY[position/2])/2)
        else:
            robotPosition = (validPositionsX[positionNumber//2], validPositionsY[positionNumber//2])
        

        robotPosition = (position.x, position.y)
        robotOrientation = position.theta * 180 / pi

        if self.nextExpectedOrientation is not None:
            minimumValidAngle = self._formatAngle(self.nextExpectedOrientation - self.MAXIMUM_ROTATION_ERROR)
            maximumValidAngle = self._formatAngle(self.nextExpectedOrientation + self.MAXIMUM_ROTATION_ERROR)

            print "Correction orientation: ", minimumValidAngle, maximumValidAngle, robotOrientation, self.nextExpectedOrientation

            if minimumValidAngle > maximumValidAngle:
                if not ((minimumValidAngle < robotOrientation <= 360) or (0 <= robotOrientation < maximumValidAngle)):
                    print "Fix pente négative 1: ", robotOrientation, self.nextExpectedOrientation
                    robotOrientation = self.nextExpectedOrientation
            else:
                if not (minimumValidAngle < robotOrientation < maximumValidAngle):
                    print "Fix pente négative 2: ", robotOrientation, self.nextExpectedOrientation
                    robotOrientation = self.nextExpectedOrientation
        else:
            self.nextExpectedOrientation = robotOrientation

        print "robotPosition, robotOrientation: ", robotPosition, robotOrientation
        return robotPosition, robotOrientation

    def rotateForOhmmeter(self):
        ohmmeterAngle = 95
        self._rotateToAngle(ohmmeterAngle)

    def askForObstaclePositions(self):
        obstaclePositions = self.communicator.getObstaclesPositions()
        return obstaclePositions

    def addObstaclesToMap(self):
        self.obstaclePositions = self.askForObstaclePositions()
        obstacleRadius = 7
        print "Nombre d'obstacles: {0}".format(len(self.obstaclePositions))

        for position in self.obstaclePositions:
            obstacle = CircularObject(obstacleRadius*2)
            i, j = convertCoordinatesToMapPoint(position, self.cartographySystem.rowNumber, self.SQUARE_SIZE_IN_CENTIMETERS)
            i, j = i - obstacleRadius, j - obstacleRadius
            try:
                self.cartographySystem.createObject(obstacle, i, j, walkable=False)
            except ValueError:
                print "Erreur obstacle"
                continue

    def moveToResistanceStation(self):
        self._moveFromRobotToCoordinates((85, 20), dilatation=NO_DILATATION)
        self.rotateForOhmmeter()
        self._move(0, -200)

    def measureResistance(self):
        for _ in range(self.NUMBER_OF_RETRIES):
            try:
                resistanceValue = int(self.internCommunicator.getResistance())
                resistance = Resistance(resistanceValue)
                break
            except ValueError:
                print "Erreur mesure résistance"
                self._move(0, 200)
                self._moveFromRobotToCoordinates((85, 20), dilatation=NO_DILATATION)
                self.rotateForOhmmeter()
                self._move(0, -200)

        self.communicator.sendResistanceValue(resistanceValue)
        return resistance.toColorCode()

    def rotateForCommandPanel(self):
        commandPanelAngle = 0
        self._rotateToAngle(commandPanelAngle)

    def moveToLetterCommandPanel(self):
        self._rotateToAngle(0)
        self.rotateForCommandPanel()
        self._moveFromRobotToCoordinates((38, 185))
        self.rotateCameraForCommandPanel()

        self.correctCommandPanelPosition(FirstCommandPanelLineFinderBuilder(), 1)

    def correctCommandPanelPosition(self, lineFinderBuilder, commandPanelNumber):
        for _ in range(self.NUMBER_OF_RETRIES):
            try:
                image = self.imageTaker.getImage()
                self.commandPanelRobotPositioning = CommandPanelRobotPositioning(image, lineFinderBuilder)
                xMovement = self.commandPanelRobotPositioning.getLateralMovement(commandPanelNumber)
                print "Déplacement correctif: ", xMovement
                self._move(xMovement * self.MILLIMETERS_PER_CENTIMETER, 0)
                angle = self.commandPanelRobotPositioning.getRotationToCommandPanel() * 180 / 3.141592
                print "Rotation corrective: ", angle
                self.nextExpectedOrientation += angle
                self.locomotionSystem.rotate(angle)
                break
            except ValueError:
                print "Erreur position"
                self._move(50, 0)

    def moveToDirectionCommandPanel(self):
        self._move(370, 0)
        self.correctCommandPanelPosition(SecondCommandPanelLineFinderBuilder(), 2)

    def rotateCameraForCommandPanel(self):
        self.internCommunicator.rotateCamera("pitch", -30)

    def findCommand(self, commandFinder, value):
        correctionAngles = [-15, 5, 5, 5, 5, 5, 5]
        i, j = self._calculatePositionInCommandPanel(value)

        for angle in correctionAngles:
            try:
                image = self.imageTaker.getImage()
                command = commandFinder.findCommand(image, i, j)
                break
            except IndexError:
                print "Erreur lignes"
                self.nextExpectedOrientation += angle
                self.internCommunicator.rotate(angle)

        return command

    def prepareCameraForPucks(self):
        self.internCommunicator.rotateCamera("pitch", -45)
        self.internCommunicator.rotateCamera("yaw", 0)

    def grabPuck(self):
        self.internCommunicator.lowerLever()
        self.internCommunicator.activateElectromagnet()
        self.internCommunicator.raiseLever()

    def grabAllPucks(self, finalPucksPosition):
        pucksToGrab = list(finalPucksPosition.keys())

        self._moveFromRobotToCoordinates(self.CENTER_PUCK_ZONE)
        puckOrientations = self.findPuckOrientations(pucksToGrab)

        print puckOrientations

        lastMovement = len(puckOrientations.items()) - 1

        pucksToGrabInOrder = self.getGrabOrder(finalPucksPosition)
        for i, puck in enumerate(pucksToGrabInOrder):
            orientation = puckOrientations[puck]
            print "Puck: ", str(puck.outerColor)

            for offset in (0, -1, 1):
                index = self.anglesToCheck.index(orientation) + offset
                index = max(index, 0)
                index = min(index, len(self.anglesToCheck) - 1)
                robotOrientation = self.anglesToCheck[index]
                self._rotateToAngle(robotOrientation)
                try:
                    x, y = self.findPuckCoordinates(puck, TRANSFORMATIONS_FROM_YAWPITCH[(0, -45)])
                    break
                except PuckNotFoundException:
                    pass
            else:
                exit("Échec lamentable")

            for _ in range(self.NUMBER_OF_RETRIES):
                self._move(x * self.MILLIMETERS_PER_CENTIMETER, 0)
                self._move(0, y * self.MILLIMETERS_PER_CENTIMETER)

                self.grabPuck()

                self._move(0, -(y * self.MILLIMETERS_PER_CENTIMETER - self.HALF_PUCK_SIZE_IN_MILLIMETERS))

                try:
                    x, y = self.findPuckCoordinates(puck, TRANSFORMATIONS_FROM_YAWPITCH[(0, -45)])
                    print "Puck not grabbed"
                    continue
                except PuckNotFoundException:
                    print "Puck grabbed"
                    break
            else:
                exit("Échec de prendre la rondelle")


            self._moveFromRobotToCoordinates(self.CENTER_PUCK_ZONE)
            self._rotateToAngle(0)
            self.internCommunicator.rotateCamera("pitch", -45)

            for _ in range(self.NUMBER_OF_RETRIES):
                try:
                    image = self.imageTaker.getImage()
                    orientationFinder = LogitechOrientationFinder(image)
                    angleToRotate = orientationFinder.findRotation(45) * -1
                    print "Angle correction: ", angleToRotate
                    self.nextExpectedOrientation = 0
                    self.locomotionSystem.rotate(angleToRotate)
                    break
                except (ValueError, TypeError):
                    self._move(0, -150)
            else:
                exit("Échec de trouver une ligne")

            self._moveFromRobotToCoordinates(self.STARTING_ZONE_CENTER, forceOrientation=0)
            self._moveFromRobotToCoordinates(self.STARTING_ZONE_CENTER)

            self.moveAndDropPuck(finalPucksPosition[puck], TRANSFORMATIONS_FROM_YAWPITCH[(0, -45)], lastMovement=(i == lastMovement))


    def getGrabOrder(self, finalPucksPosition):
        pucksToGrab = list(finalPucksPosition.keys())
        x = 0
        y = 1

        centerXInCentimeters = 56
        rightWallPosition = 112

        areObstaclesTooNearWall = True
        xDistanceFromWall = list()
        for obstaclePosition in self.obstaclePositions:
            if min(obstaclePosition[x], rightWallPosition - obstaclePosition[x]) > 30:
                areObstaclesTooNearWall = False

        if areObstaclesTooNearWall:
            positions = sorted(self.obstaclePositions, key=lambda position: position[y])
            nearestXValue = positions[0][x]
            pucksToGrab.sort(key=lambda puck: (finalPucksPosition[puck][y], abs(finalPucksPosition[puck][x] - nearestXValue)))
        else:
            averageX = sum([position[x] for position in self.obstaclePositions]) // len(self.obstaclePositions)
            pucksToGrab.sort(key=lambda puck: (finalPucksPosition[puck][y], abs(finalPucksPosition[puck][x] - averageX)))

        return pucksToGrab


    def placeRobotToGrabPuck(self, puckPosition):
        x, y = puckPosition
        if x < 20:
            return 30, y, 90
        elif x > 85:
            return 90, y, 270
        else:
            return x, 200, 0

    def findPuckOrientations(self, pucks):
        self.anglesToCheck = [105, 80, 45, 10, 350, 315, 280, 245]
        puckOrientations = {}

        while len(puckOrientations) < 3:
            for angle in self.anglesToCheck:
                self._rotateToAngle(angle)

                image = self.imageTaker.getImage()

                for puck in pucks:
                    try:
                        self.puckCenterFinder.findCenterPixel(image, puck)
                        orientationFound = {puck: angle}
                        puckOrientations = self.updatePucksOrientationDictionary(puckOrientations, orientationFound)
                    except PuckNotFoundException:
                        pass

                if len(puckOrientations) == 3:
                    break
            else:
                self._moveFromRobotToCoordinates(self.CENTER_PUCK_ZONE)

            print [str(puckOrientation.outerColor) for puckOrientation in puckOrientations.keys()]

        return puckOrientations

    def updatePucksOrientationDictionary(self, pucksAngle, puckOrientationFound):
        puckFound = puckOrientationFound.keys()[0]
        orientationFound = puckOrientationFound[puckFound]

        if puckFound not in pucksAngle.keys():
            pucksAngle.update(puckOrientationFound)
        elif pucksAngle[puckFound] % 45 != 0:
            pucksAngle.update(puckOrientationFound)

        return pucksAngle

    def moveAndDropPuck(self, puckPosition, transformationToFindCorner, lastMovement=False):
        self._rotateToAngle(self.ANGLE_FOR_PUCK_DROP[puckPosition])
        self.prepareCameraForPucks()
        image = self.imageTaker.getImage()
        cornerPosition = self.cornerFinder.findCornerPixel(image)
        x, y = self.robotToPixelMovement.getMovementToPixel(image, cornerPosition, transformationToFindCorner)
        print "CornerPosition, (x, y): ", cornerPosition, x, y
        self._move(x * self.MILLIMETERS_PER_CENTIMETER, 0)
        self._move(0, self.MILLIMETERS_PER_CENTIMETER * y - self.HALF_PUCK_SIZE_IN_MILLIMETERS)
        self.internCommunicator.deactivateElectromagnet()
        self._move(0, -(self.MILLIMETERS_PER_CENTIMETER * y - self.HALF_PUCK_SIZE_IN_MILLIMETERS))

        if not lastMovement:
            self._rotateToAngle(0)
            self._moveFromRobotToCoordinates(self.CENTER_PUCK_ZONE)
            self._moveFromRobotToCoordinates(self.CENTER_PUCK_ZONE)


    def _rotateToAngle(self, angle):
        _, robotOrientation = self.askForPositionAndOrientation()
        angleToRotate = angle - robotOrientation

        self.nextExpectedOrientation = angle
        print "_rotateToAngle.nextExpectedOrientation = ", self.nextExpectedOrientation

        if -360 <= angleToRotate <= -180:
            angleToRotate = 360 + angleToRotate
        elif 180 <= angleToRotate <= 360:
            angleToRotate = angleToRotate - 360

        print "Rotation: ", -angleToRotate
        self.internCommunicator.rotate(-angleToRotate)

    def _moveFromRobotToCoordinates(self, coordinates, dilatation=None, forceOrientation=None):
        if dilatation is None:
            dilatation = EXTRA_LARGE_DILATATION

        robotPosition, robotOrientation = self.askForPositionAndOrientation()
        self.robotPosition = robotPosition

        if forceOrientation is not None:
            robotOrientation = forceOrientation

        for currentDilatation in DILATATIONS[DILATATIONS.index(dilatation):]:
            try:
                path = self.pathfinder.findPath(self.cartographySystem.map, robotPosition, coordinates, dilatation=currentDilatation)
                print "Dilatation: ", currentDilatation
                break
            except NoPathFoundException:
                print "Erreur pathfinding: sans dilatation"
                robotPosition, robotOrientation = self.askForPositionAndOrientation()

        self.communicator.sendPlannedPath(path)
        pathToFollow = [(x * self.MILLIMETERS_PER_CENTIMETER, y * self.MILLIMETERS_PER_CENTIMETER) for x, y in path]
        self.locomotionSystem.followPath(pathToFollow, robotOrientation * pi / 180)

        print "RobotPosition, RobotOrientation: ", robotPosition, robotOrientation
        print "Déplacements: ", pathToFollow

    def _calculatePositionInCommandPanel(self, value):
        i = (value - 1)//3
        j = (value - 1) % 3
        return i, j

    def _move(self, x, y):
        robotX, robotY = self.robotPosition
        path = [self.robotPosition, (robotX + x//10, robotY + y//10)]
        self.communicator.sendPlannedPath(path)
        self.locomotionSystem.move(x, y)

    def _formatAngle(self, angle):
        if angle < 0:
            return 360 + angle
        elif angle > 360:
            return angle - 360
        else:
            return angle

    def _createBaseCartographySystem(self):
        rowNumber, columnNumber = 117, 56

        borders = [(RectangularObject(1, columnNumber), (0, 0), False),
                   (RectangularObject(1, columnNumber), (rowNumber - 1, 0), False),
                   (RectangularObject(rowNumber, 1), (0, 0), False),
                   (RectangularObject(rowNumber, 1), (0, columnNumber - 1), False)]
        startingZoneCorners = [(RectangularObject(1, 1), (106, 44), True),
                               (RectangularObject(1, 1), (73, 44), True),
                               (RectangularObject(1, 1), (106, 11), True),
                               (RectangularObject(1, 1), (73, 11), True)]

        puckZones = [(RectangularObject(3, 40), (1, 1), False),
                     (RectangularObject(54, 3), (1, 1), False),
                     (RectangularObject(3, 40), (1, 52), False)]
        cornerCommandPanel, cornerCommandPanelPosition = RectangularObject(1, 8), (0, 14)
        clockwiseCommandPanel, clockwiseCommandPanelPosition = RectangularObject(1, 8), (0, 32)
        orangePanel, orangePanelPosition = RectangularObject(7, 1), (103, columnNumber-1)

        cartographySystem = CartographySystem(rowNumber, columnNumber)
        for mapObject, coordinates, walkable in borders + startingZoneCorners + puckZones:
            cartographySystem.createObject(mapObject, *coordinates, walkable=walkable)

        cartographySystem.createObject(cornerCommandPanel, *cornerCommandPanelPosition, walkable=False)
        cartographySystem.createObject(clockwiseCommandPanel, *clockwiseCommandPanelPosition, walkable=False)
        cartographySystem.createObject(orangePanel, *orangePanelPosition, walkable=False)

        return cartographySystem

    def _saveImage(filepath, image):
        cv2.imwrite(filepath, image)