class Trajectoire(TestCase):
def setUp(self):
self.carte = Carte()
self.carte.ajouterElementCarto(self.analyseImageWorld.elementsCartographiques)
self.carte.trajectoire.initGrilleCellule([])
def test_trouverTrajet(self):
depart = (random.randrange(5,1595), random.randrange(5,845))
arrive = (random.randrange(5,1595), random.randrange(5,845))
self.carte.trajectoire.trouverTrajet(depart, arrive)
bool = self.trajetValide(self.carte.trajectoire.tra, depart, arrive)
self.assertTrue(bool)
def trajetValide(self, trajet, arrive, depart):
if (trajet == []):
return True
elif (depart[0]-6 < trajet[0][0] and trajet[0][0] < depart[0]+6) and (depart[1]-9 < trajet[1][1] and trajet[1][1] < depart[1]+9):
return True
else:
return False
def test_trouverLongueurTrajetCarre(self):
depart = (0, 0)
milieu = (0, 855)
arrive = (0, 1600)
trajet = [depart, milieu, arrive]
dist = self.carte.trajectoire.trouverLongueurTrajetCarre(trajet)
bool = 110**2 + 220**2 < dist < 120**2 + 230**2
self.assertTrue(bool)
def test_distanceAuCarre(self):
self.algo.distanceAuCarre(0, 0, 8, 8)
self.assertTrue(self.carte.trajectoire.grilleCellule.distanceAuCarre.called)
class TestAlgorithmeTrajectoire(TestCase):
def setUp(self):
self.carte = Carte()
self.carte.ajouterElementCarto(self.analyseImageWorld.elementsCartographiques)
self.carte.trajectoire.initGrilleCellule([])
self.algo = AlgorithmeTrajectoire(self.carte.trajectoire.grilleCellule)
def test_trouverTrajet(self):
depart = (random.randrange(5,1595), random.randrange(5,845))
arrive = (random.randrange(5,1595), random.randrange(5,845))
self.algo.trouverTrajet(depart, arrive)
bool = self.trajetValide(self.carte.trajectoire.tra, depart, arrive)
self.assertTrue(bool)
def trajetValide(self, trajet, arrive, depart):
if (trajet == []):
return True
elif (depart[0]-6 < trajet[0][0] and trajet[0][0] < depart[0]+6) and (depart[1]-9 < trajet[1][1] and trajet[1][1] < depart[1]+9):
return True
else:
return False
def test_simplifierTrajectoire(self):
depart = (20, 20)
milieuInutile = (20, 400)
arrive = (20, 820)
self.algo.trajet = [depart, milieuInutile, arrive]
self.algo.simplifierTrajet()
self.assertEqual(len(self.carte.trajectoire.trajectoire), 2)
def test_SectionnerTrajectoire(self):
depart = (20, 20)
arrive = (20, 820)
self.algo.trajet = [depart, arrive]
self.algo.sectionnerTrajet()
bool = len(self.algo.trajet) >= 2
self.assertTrue(bool)
def test_eliminerDetourInutile(self):
depart = (20, 20)
milieuInutile = (200, 400)
arrive = (20, 820)
self.algo.trajet = [depart, milieuInutile, arrive]
self.algo.eliminerDetourInutile()
bool = len(self.algo.trajet) == 2
self.assertTrue(bool)
def test_distanceAuCarre(self):
self.algo.distanceAuCarre(0, 0, 8, 8)
self.assertTrue(self.algo.grilleCellule.distanceAuCarre.called)
def __init__(self, numeroTable):
Thread.__init__(self)
self.startTimer = default_timer()
self.numeroTable = numeroTable
self.trajectoireReel = None
self.trajectoirePrevue = None
self.angleDesire = None
self.tensionCondensateur = "0"
self.descriptionIleCible = "?"
self.manchester = "?"
self.roundTerminee = False
self.rapport = 0.84
self.coordonneeXMilieu = 787
self.coordonneeYMilieu = 419
self.carte = Carte()
self.demarrerConnectionTCP()
self.demarrerFeedVideo()
self.demarrerAnalyseImageWorld()
self.demarrerImageVirtuelle()
def setUp(self):
self.carte = Carte()
self.carte.ajouterElementCarto(self.analyseImageWorld.elementsCartographiques)
self.carte.trajectoire.initGrilleCellule([])
class StationBase(Thread):
def __init__(self, numeroTable):
Thread.__init__(self)
self.startTimer = default_timer()
self.numeroTable = numeroTable
self.trajectoireReel = None
self.trajectoirePrevue = None
self.angleDesire = None
self.tensionCondensateur = "0"
self.descriptionIleCible = "?"
self.manchester = "?"
self.roundTerminee = False
self.rapport = 0.84
self.coordonneeXMilieu = 787
self.coordonneeYMilieu = 419
self.carte = Carte()
self.demarrerConnectionTCP()
self.demarrerFeedVideo()
self.demarrerAnalyseImageWorld()
self.demarrerImageVirtuelle()
def run(self):
self.attendreFinDeDetectionPrimaire()
self.initialisationTrajectoire()
self.attendreRobotPret()
self.demarerRoutine()
time.sleep(10)
def demarerRoutine(self):
self.deplacement(ETAPE_RECHARGE)
self.aligner(ETAPE_ALIGNEMENT_STATION)
self.trouverTresorEtCible()
self.attendreRobot()
self.attendreThreadCible()
while not self.threadCommunication.tresorTrouve:
self.deplacement(ETAPE_TRESOR)
self.aligner(ETAPE_ALIGNEMENT_TRESOR)
time.sleep(0.01)
self.deplacement(ETAPE_ILE)
self.aligner(ETAPE_ALIGNEMENT_ILE)
self.roundTerminee = True
def deplacement(self, type):
print '\n--------------------------------------------------'
print 'Etape de deplacement : %s' % type
print '--------------------------------------------------'
destination = self.identifierDestination(type)
self.trouverTrajectoirePrevu(destination, type)
while (self.trajectoireReel is not None) and (len(self.trajectoireReel) > MIN_TRAJECTOIRE):
self.orienter('deplacement')
self.deplacer()
self.correctionsFinales(type)
print '\n--------------------------------------------------'
print 'Arriver.'
print '--------------------------------------------------'
def identifierDestination(self, etape):
print '\nIndentifier destination'
destination = None
if etape == ETAPE_RECHARGE:
destination = self.carte.getStationRecharge().getCentre()
elif etape == ETAPE_TRESOR:
destination = self.carte.cible.tresorChoisi.getCentre()
print 'identifier destination tresor'
print destination
elif etape == ETAPE_ILE:
destination = self.carte.cible.ileChoisie.getCentre()
if destination is None:
print 'erreur! Aucune destination trouvee.'
return destination
def correctionsFinales(self, type):
if type == ETAPE_RECHARGE:
self.angleDesire = 90
self.orienter(type)
self.deplacementArriere(5)
self.deplacementDroit(10)
elif type == ETAPE_TRESOR:
if self.carte.getCible().getTresorCible().getCentre()[1] < MAX_CENTRE_TRESOR:
self.angleDesire = 90
self.carte.cible.conteur += 1
else:
self.angleDesire = 270
self.carte.cible.conteur += 1
self.orienter(type)
self.deplacementArriere(7)
elif type == ETAPE_ILE:
arriver = self.carte.getCible().getIleCible().getCentre()
debut = self.getPositionRobot()
print 'arrive: ', arriver
print 'robot: ', debut
debut2 = self.correctionCentre(debut)
print 'robot2: ', debut2
self.angleDesire = self.trouverOrientationDesire(debut2, arriver)
print self.angleDesire
self.orienter(type)
self.trajectoirePrevue = None
def aligner(self, type):
print '\n--------------------------------------------------'
print 'Debuter l''alignement.'
print '--------------------------------------------------'
int = 0
if type == ETAPE_ALIGNEMENT_ILE:
couleur = self.carte.getCible().getIleCible().getCouleur()
if couleur == COULEUR_VERT:
int = 0
elif couleur == COULEUR_BLEU:
int = 1
elif couleur == COULEUR_JAUNE:
int = 2
elif couleur == COULEUR_ROUGE:
int = 3
RequeteJSON(type, int)
self.threadCommunication.signalerEnvoyerCommande()
if not type == ETAPE_ALIGNEMENT_STATION:
self.attendreRobot()
print '\n--------------------------------------------------'
print 'Allignement termine.'
print '--------------------------------------------------'
def trouverTrajectoirePrevu(self, destination, type):
print '\nTrouve la trajectoire prevu...'
self.trajectoirePrevue = self.carte.trajectoire.trouverTrajet(self.getPositionRobot(), copy.deepcopy(destination), type)
if self.trajectoirePrevue is None:
print 'erreur! Aucun trajet trouve.'
else:
print 'trajet trouve.'
self.trajectoireReel = copy.deepcopy(self.trajectoirePrevue)
def orienter(self, type):
print '\nOrienter'
conteur = 0
while 1:
if self.angleDesire is None:
arriver = self.trajectoireReel[-2]
debut = self.getPositionRobot()
self.angleDesire = self.trouverOrientationDesire(debut, arriver)
angle = self.trouverDeplacementOrientation()
if -3 <= angle <= 3:
print '\nOrientation termine.'
break
if conteur >= 2:
if angle > 0:
angle = 1
else:
angle = -1
if angle >= 0:
RequeteJSON("rotateClockwise", angle)
else:
RequeteJSON("rotateAntiClockwise", abs(angle))
print 'Signaler que la comande est prete a envoyer.'
self.threadCommunication.signalerEnvoyerCommande()
time.sleep(0.5)
self.attendreRobot()
if type == 'deplacement':
self.angleDesire = None
conteur += 1
self.angleDesire = None
def trouverOrientationDesire(self, debut, arriver):
deltaX = arriver[0]-debut[0]
deltaY = -1*(arriver[1]-debut[1])
if not deltaX == 0:
pente = deltaY/deltaX
if deltaY == 0 and deltaX < 0:
angle = 180
elif deltaY == 0 and deltaX > 0:
angle = 0
elif deltaX == 0 and deltaY > 0:
angle = 90
elif deltaX == 0 and deltaY < 0:
angle = 270
elif deltaX > 0 and deltaY > 0:
angle = int(round(math.degrees(math.atan(pente))))
elif deltaX > 0 and deltaY < 0:
angle = 360 + int(round(math.degrees(math.atan(pente))))
elif deltaX < 0:
angle = 180 + int(round(math.degrees(math.atan(pente))))
return angle
def trouverDeplacementOrientation(self):
angleRobot = self.getOrientationRobot()
print 'angle du robot: ', angleRobot
print 'angle desire: ', self.angleDesire
depDegre = angleRobot - self.angleDesire
if depDegre < -180:
depDegre = depDegre + 360
elif depDegre > 180:
depDegre = depDegre - 360
print 'correction: ', depDegre
return depDegre
def deplacer(self):
print '\nDeplacer'
arriver = self.trajectoireReel[-2]
debut = self.getPositionRobot()
dep = self.distanceADestination(debut[0], debut[1], arriver[0], arriver[1])
dep = int(round(dep))
print 'deplacement: ', dep
RequeteJSON("forward", dep)
self.threadCommunication.signalerEnvoyerCommande()
self.attendreRobot()
debut = self.getPositionRobot()
dep = self.distanceAuCarre(debut[0], debut[1], arriver[0], arriver[1])
print '\nArriver.'
if len(self.trajectoireReel) == 2:
self.trajectoireReel = None
else:
self.trajectoireReel.pop(-1)
def deplacementDroit(self, dep):
print '\nDeplacer'
print 'deplacement: ', dep
RequeteJSON("right", dep)
self.threadCommunication.signalerEnvoyerCommande()
self.attendreRobot()
def deplacementArriere(self, dep):
print '\nDeplacer'
print 'deplacement: ', dep
RequeteJSON("backward", dep)
self.threadCommunication.signalerEnvoyerCommande()
self.attendreRobot()
def deplacementAvant(self, dep):
print '\nDeplacer'
print 'deplacement: ', dep
RequeteJSON("forward", dep)
self.threadCommunication.signalerEnvoyerCommande()
self.attendreRobot()
def decoderManchester(self):
RequeteJSON("decoderManchester", 0)
self.threadCommunication.signalerEnvoyerCommande()
self.attendreRobot()
def demarrerFeedVideo(self):
self.threadVideo = FeedVideoStation()
self.threadVideo.start()
def demarrerConnectionTCP(self):
self.threadCommunication = StationServeur(self)
self.threadCommunication.start()
def demarrerAnalyseImageWorld(self):
self.threadAnalyseImageWorld = AnalyseImageWorld(self)
self.threadAnalyseImageWorld.start()
def demarrerImageVirtuelle(self):
self.threadImageVirtuelle = ImageVirtuelle(self)
self.threadImageVirtuelle.start()
def initialisationTrajectoire(self):
self.carte.getTrajectoire().initGrilleCellule(self.carte.getIles())
def trouverTresorEtCible(self):
self.threadCible = TrouverTresorEtCible(self)
self.threadCible.start()
def attendreThreadCible(self):
while self.threadCible.isAlive():
time.sleep(0.01)
print 'Cible trouve'
def attendreRobotPret(self):
print '\nAttendre que le robot soit pret...'
while 1:
if self.threadCommunication.getRobotPret():
print 'Robot est pret'
break
time.sleep(0.01)
def attendreRobot(self):
self.threadCommunication.debuteAttenteDuRobot()
while self.threadCommunication.getAttenteDuRobot():
time.sleep(0.01)
time.sleep(0.1)
print 'Robot a fini.'
def attendreCible(self):
while self.carte.getCible() is None:
time.sleep(0.01)
def attendreFinDeDetectionPrimaire(self):
while not self.threadAnalyseImageWorld.detectionPrimaireFini:
time.sleep(0.01)
def attendreFeed(self):
while self.threadVideo.captureTable is None:
time.sleep(0.01)
def attendreImageVirtuelle(self):
while self.threadImageVirtuelle.imageVirtuelle is None:
time.sleep(0.01)
def distanceAuCarre(self, x, y, x2, y2):
return self.carte.getTrajectoire().distanceAuCarre(x, y, x2, y2)
def distanceADestination(self, x, y, x2, y2):
return math.sqrt(self.distanceAuCarre(x, y, x2, y2))
def getImage(self):
return copy.deepcopy(self.threadVideo.captureTable)
def setRobot(self, robot):
self.carte.robot = robot
def getCarte(self):
return self.carte
def getManchester(self):
return self.manchester
def setManchester(self, lettre):
self.manchester = lettre
def getTensionCondensateur(self):
return self.tensionCondensateur
def setTensionCondensateur(self, tension):
self.tensionCondensateur = tension
def getTrajectoirePrevue(self):
return self.trajectoirePrevue
def getPositionRobot(self):
centre = self.carte.getRobotValide().getCentre()
return centre
def getOrientationRobot(self):
return self.carte.getRobotValide().orientation
def getNumTable(self):
return self.numeroTable
def correctionCentre(self, centre):
xNonCorrige = centre[0]
deltaX = xNonCorrige - self.coordonneeXMilieu
xCorriger = int(round(self.coordonneeXMilieu + (deltaX * self.rapport)))
yNonCorrige = centre[1]
deltaY = yNonCorrige - self.coordonneeYMilieu
yCorriger = int(round(self.coordonneeYMilieu + (deltaY * self.rapport)))
return xCorriger, yCorriger