def _findAngleToRotate2(self, line, cameraAngle): x1, y1 = (213, 213 * line.slope + line.ordinateToOrigin) x2, y2 = (426, 426 * line.slope + line.ordinateToOrigin) robotToPixelMovement = RobotToPixelMovement() x1, y1 = robotToPixelMovement.getMovementToPixel(None, (x1, y1), TRANSFORMATIONS_FROM_YAWPITCH[(0, -45)]) x2, y2 = robotToPixelMovement.getMovementToPixel(None, (x2, y2), TRANSFORMATIONS_FROM_YAWPITCH[(0, -45)]) return numpy.arctan(float(y2 - y1)/(x2 - x1))
def getRotationToCommandPanel(self): horizontalLines = list(self.horizontalLines) horizontalLine = horizontalLines[0] deltaX = horizontalLine.x2 - horizontalLine.x1 deltaY = horizontalLine.y2 -horizontalLine.y1 robotToPixelMovement = RobotToPixelMovement() x1, y1 = robotToPixelMovement.getMovementToPixel(None, (horizontalLine.x1, horizontalLine.y1), TRANSFORMATIONS_FROM_YAWPITCH[(0, -45)]) x2, y2 = robotToPixelMovement.getMovementToPixel(None, (horizontalLine.x2, horizontalLine.y2), TRANSFORMATIONS_FROM_YAWPITCH[(0, -45)]) return np.arctan(float(deltaY)/deltaX)
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()
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)