class TestPathfinder(unittest.TestCase):
SQUARE_SIZE_IN_CENTIMETERS = 2
ROW_NUMBER = 117
AN_INITIAL_POSITION = (50, 54)
A_FINAL_POSITION = (24, 160)
AN_UNREACHABLE_POSITION = (66, 184)
def setUp(self):
self.pathfinder = Pathfinder(self.SQUARE_SIZE_IN_CENTIMETERS)
self.simpleMap = createSimpleMapConfiguration()
def testWhenPathIsFoundThenTheReturnedPathDoesntTouchObstacles(self):
processedMap = self.pathfinder._processMap(self.simpleMap)
path = self.pathfinder.findPath(self.simpleMap, self.AN_INITIAL_POSITION, self.A_FINAL_POSITION)
for x, y in path:
i, j = convertCoordinatesToMapPoint((x, y), self.ROW_NUMBER, self.SQUARE_SIZE_IN_CENTIMETERS)
self.assertNotEqual(processedMap[i, j], self.pathfinder.INFINITY)
def testWhenPathIsFoundThenTheReturnedPathLeadsToGoal(self):
path = self.pathfinder.findPath(self.simpleMap, self.AN_INITIAL_POSITION, self.A_FINAL_POSITION)
self.assertEqual(path[-1], self.A_FINAL_POSITION)
def testWhenGoalIsUnreachableThenNoPathFoundExceptionIsRaised(self):
self.assertRaises(
NoPathFoundException,
self.pathfinder.findPath,
self.simpleMap,
self.AN_INITIAL_POSITION,
self.AN_UNREACHABLE_POSITION,
)
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)
def setUp(self):
self.pathfinder = Pathfinder(self.SQUARE_SIZE_IN_CENTIMETERS)
self.simpleMap = createSimpleMapConfiguration()