def simulateMovement(self, x, y, nextAngle, direction):
ligne = Ligne("", self.x, self.y, x, y, "purple")
# simulate movement
movingAngle = self.normalizeAngle(ligne.getAngle() +
(180 if direction < 0 else 0))
x = 0
y = 0
for p in range(0, (int)(ligne.getlongeur())):
ratio = (1.0 / ligne.getlongeur()) * p
ratioDest = 1 - ratio
x = ligne.x1 * ratioDest + ligne.x2 * ratio
y = ligne.y1 * ratioDest + ligne.y2 * ratio
self.setPosition(x, y, movingAngle, direction)
if webInterface.instance and webInterface.instance.runningState == RunningState.STOP:
return False
if type(self.collisionDetected) != bool:
collisionX, collisionY = self.collisionDetected
print "\t \t Obstacle, stopping robot"
errorObstacle = True
break
elif self.collisionDetected:
print "\t \t Obstacle, stopping robot"
errorObstacle = True
break
time.sleep(self.simulationSpeed)
self.setPosition(x, y, nextAngle, 0)
def collisionRectangleLigne(self,rectangle,ligne):
haut=Ligne("haut",rectangle.x1,rectangle.y1,rectangle.x2,rectangle.y1,"black")
bas=Ligne("bas",rectangle.x1,rectangle.y2,rectangle.x2,rectangle.y2,"black")
gauche=Ligne("gauche",rectangle.x1,rectangle.y1,rectangle.x1,rectangle.y2,"black")
droite=Ligne("droite",rectangle.x2,rectangle.y1,rectangle.x2,rectangle.y2,"black")
if self.collisionEntre(ligne,haut) or self.collisionEntre(ligne,bas) or self.collisionEntre(ligne,gauche) or self.collisionEntre(ligne,droite):
return True
return False
def contenuDans(self,x,y,forme):
if isinstance(forme, Ligne):
return False
if isinstance(forme, Cercle):
return forme.distanceAvec(Ligne("", x, y, x, y)) <= forme.rayon
if isinstance(forme, Rectangle):
return forme.x1 <= x and forme.x2 >= x and forme.y1 <= y and forme.y2 >= y
ligne = Ligne("", x, y, forme.x, forme.y)
return not self.collisionEntre(ligne, forme)
def toJson(self):
line = Ligne("", self.x, self.y, self.x + 100, self.y, "blue")
line.rotate(self.angle)
str = u'{'
str += u'"type":"line",'
str += u'"name":"{}",'.format("")
str += u'"x1":{},'.format(self.x)
str += u'"y1":{},'.format(self.y)
str += u'"x2":{},'.format(line.x2)
str += u'"y2":{},'.format(line.y2)
str += u'"color":"{}"'.format("blue")
str += u'}'
return str
def simulateMovement(self, x, y, nextAngle, direction):
ligne = Ligne("", self.x, self.y, x, y, "purple")
# simulate movement
movingAngle = self.normalizeAngle(ligne.getAngle() +
(180 if direction < 0 else 0))
for p in range(0, (int)(ligne.getlongeur())):
ratio = (1.0 / ligne.getlongeur()) * p
ratioDest = 1 - ratio
self.setPosition(ligne.x1 * ratioDest + ligne.x2 * ratio,
ligne.y1 * ratioDest + ligne.y2 * ratio,
movingAngle)
if webInterface.instance and webInterface.instance.runningState == RunningState.STOP:
return False
time.sleep(self.simulationSpeed)
self.setPosition(ligne.x2, ligne.y2, nextAngle)
def eviterObstacle(self, absoluteObstacleAngle, direction=1):
print("\t \tEscape from A=" + str(absoluteObstacleAngle))
#Get opposed angle
if absoluteObstacleAngle > 0:
newAngle = absoluteObstacleAngle - 180
else:
newAngle = absoluteObstacleAngle + 180
#newAngle = absoluteObstacleAngle
xprev = self.x
yprev = self.y
newx = xprev + 300 * math.cos(newAngle * 0.0174532925) #rad
newy = yprev + 300 * math.sin(newAngle * 0.0174532925) #rad
ligne = Ligne("", xprev, yprev, newx, newy, "purple")
distance = -1 * direction * ligne.getlongeur()
#self.aveugler()
time.sleep(0.2)
self.seDeplacerDistanceAngle(distance, 0, 0.4, 0)
time.sleep(0.2)
#self.rendreVue()
time.sleep(0.2)
return True
#search for a free opposed movment
"""escapePointInteretList = list(self.listPointInteret)
escapeObstacles = self.chercher.pointContenuListe(xprev, yprev, escapePointInteretList)
for elem in escapeObstacles:
escapePointInteretList.remove(elem)
for size in [400, 300, 250]:
for a in range(0, 41, 10):
for side in [-1, 1]:
lineTest = Ligne("", xprev, yprev, newx, newy, "purple")
lineTest.resize(size)
testAngle = lineTest.getAngle()+a*side
lineTest.rotate(lineTest.getAngle()+a*side)
if not self.chercher.enCollisionCarte(lineTest, escapePointInteretList):
print("\t \tFound collision escape")
print("\t \tEscape angle=" + str(lineTest.getAngle()))
distance = -1*direction*lineTest.getlongeur()
angleToDo = self.getAngleToDo(lineTest.getAngle())
if angleToDo>0:
angleToDo-=180
else:
angleToDo+=180
self.seDeplacerDistanceAngle(distance, angleToDo, 0.2, 0)
return True
elif (self.fenetre):
lineTest.dessiner(self.fenetre)
self.fenetre.win.redraw()"""
return False
def updatePositionRelative(self, distance, angle):
angleDiff = (angle + self.angle) * 0.0174532925 #rad
xprev = self.x
yprev = self.y
self.x += distance * math.cos(angleDiff)
self.y += distance * math.sin(angleDiff)
self.angle += angle
if self.angle > 180:
self.angle = -360 + self.angle
if self.angle < -180:
self.angle = 360 + self.angle
if self.fenetre != None:
ligne = Ligne("", xprev, yprev, self.x, self.y, "green")
ligne.dessiner(self.fenetre)
self.fenetre.win.redraw()
self.setPosition(self.x, self.y, self.angle) #update interface
def updateNodesRemovingElement(self, element, listePointInteret):
tmpList = list(listePointInteret) # copy
tmpList.remove(element)
for key, noeud in self.graph.listeNoeud.iteritems():
if element in noeud.colisionObject:
noeud.colisionObject.remove(
element) #line added during cup 2017
tester = Collision(self.fenetre)
line = Ligne("", noeud.x, noeud.y, noeud.x, noeud.y)
tmpList = sorted(tmpList,
key=lambda pointInteret: line.distanceAvec(
pointInteret.forme))
for point in listePointInteret:
if tester.collisionEntre(line, point.zoneEvitement.forme):
noeud.colisionObject = point
break
def collisionPolygoneCercle(self, polygone, cercle):
count=0
firstPoint = polygone.pointList[0]
lastPoint = polygone.pointList[0]
for point in polygone.pointList:
count+=1
if count == 1:
continue
currentLine = Ligne("", float(point["x"]), float(point["y"]), float(lastPoint["x"]), float(lastPoint["y"]))
if self.collisionLigneCercle(currentLine, cercle):
return True
lastPoint = point
currentLine = Ligne("", float(firstPoint["x"]), float(firstPoint["y"]), float(lastPoint["x"]), float(lastPoint["y"]))
if self.collisionLigneCercle(currentLine, cercle):
return True
return False
def collisionCercleRectangle(self,cercle,rectangle):
listCote = []
listCote.append(Ligne("haut", rectangle.x1, rectangle.y1, rectangle.x2, rectangle.y1, "black"))
listCote.append(Ligne("bas", rectangle.x1, rectangle.y2, rectangle.x2, rectangle.y2, "black"))
listCote.append(Ligne("gauche", rectangle.x1, rectangle.y1, rectangle.x1, rectangle.y2, "black"))
listCote.append(Ligne("droite", rectangle.x2, rectangle.y1, rectangle.x2, rectangle.y2, "black"))
listeRect = []
nbrect = 7
for i in numpy.arange(0, math.pi, math.pi / (nbrect + 1)):
x = cercle.rayon * 1. * math.sin(i)
y = cercle.rayon * 1. * math.cos(i)
rect = Rectangle("", cercle.x - x, cercle.y - y, cercle.x + x, cercle.y + y, "red")
listeRect.append(rect)
for cote in listCote:
for rect in listeRect:
if self.collisionEntre(rect,cote):
return True
return False
def seDeplacerDistanceAngle(self,
distance,
angle,
vitesse=1.0,
retry=1,
forceLine=False):
print "\t \tDeplacement: distance:", str(distance), " angle:", str(
angle)
if self.movingBase.isXYSupported:
lineTarget = Ligne("", self.x, self.y, self.x * 2, self.y * 2,
"purple")
lineTarget.resize(distance)
lineTarget.rotate(self.angle + angle)
return self.seDeplacerXY(lineTarget.x2, lineTarget.y2,
self.angle + angle, vitesse, forceLine)
if distance == 0 or not self.telemetreDetectCollision(distance):
if not self.isSimulated:
self.movingBase.startMovementDistanceAngle(
distance, angle, vitesse)
direction = 1
if distance < 0:
direction = -1
self.speed = vitesse * direction
self.movingDA = True
self.movingDALastDist = 0
self.movingDALastAngle = 0
result = self.__waitForMovementFinished(False, distance == 0)
self.movingDA = False
return result
else:
self.updatePositionRelative(distance, angle)
return True
else:
return False
def enCollisionGraph(self, ligne, blockingElements=None):
self.callCount["enCollisionGraph"] = self.callCount[
"enCollisionGraph"] + 1 if "enCollisionGraph" in self.callCount else 1
resolution = self.step
for i in range(resolution,
int(ligne.getlongeur()) + resolution, resolution):
subLine = Ligne("", ligne.x1, ligne.y1, ligne.x2, ligne.y2)
subLine.resize(float(i))
node = self.graph.trouverPointProche(subLine.x2, subLine.y2)
if len(node.colisionObject) > 0:
if blockingElements is None:
return True
else:
for elem in node.colisionObject:
if not elem in blockingElements:
return True
"""if self.fenetre:
subCircle=Cercle("", subLine.x2, subLine.y2,resolution/2, "blue")
subCircle.dessiner(self.fenetre)
subLine.setCouleur("violet")
subLine.dessiner(self.fenetre)
ligne.setCouleur("orange")
ligne.dessiner(self.fenetre)"""
return False
def collisionCercleLigne(self,cercle,ligne):
listeLigne = []
nbrect = 8
x = cercle.x + cercle.rayon *1.*math.sin(0)
y = cercle.y + cercle.rayon *1. *math.cos(0)
for i in numpy.arange(0,math.pi*2,math.pi/(nbrect+1)*2):
x1 = cercle.x + cercle.rayon *1.*math.sin(i)
y1 = cercle.y + cercle.rayon *1. *math.cos(i)
ligne1 = Ligne("", x, y, x1, y1, "red")
listeLigne.append(ligne1)
x = x1
y = y1
x1 = cercle.x + cercle.rayon *1.*math.sin(0)
y1 = cercle.y + cercle.rayon *1. *math.cos(0)
listeLigne.append(Ligne("", x, y, x1, y1, "red"))
for ligne1 in listeLigne:
"""if self.fenetre != None:
ligne.setCouleur("blue")
ligne.dessiner(self.fenetre)"""
if self.collisionEntre(ligne1,ligne):
return True
return False
def simplifierChemin(self, tabchemin, listPointInteret):
i = -1
while i < len(tabchemin) - 2:
i += 1
l1 = tabchemin[i]
l2 = tabchemin[i + 1]
line = Ligne("", l1.x1, l1.y1, l2.x2, l2.y2, "")
#if self.fenetre:
# line.setCouleur("red")
# line.dessiner(self.fenetre)
if not self.enCollisionCarte(line, listPointInteret):
tabchemin.insert(i, line)
tabchemin.remove(l1)
tabchemin.remove(l2)
i = -1
return tabchemin
def enCollisionCarte(self,
ligne,
_listePointInteret,
ignoreEvitmentZone=False):
collisionEntre = self.collider.collisionEntre #local var perf optim
listePointInteret = sorted(_listePointInteret,
key=lambda pointInteret: Ligne(
"", ligne.x1, ligne.y1, ligne.x1, ligne.
y1).distanceAvec(pointInteret.forme))
for point in listePointInteret:
if ignoreEvitmentZone:
if collisionEntre(ligne, point.forme):
return point
else:
if collisionEntre(ligne, point.zoneEvitement.forme):
return point
return False
def enCollisionCarte(self,
ligne,
_listePointInteret,
ignoreEvitmentZone=False):
tester = Collision(self.fenetre)
listePointInteret = sorted(_listePointInteret,
key=lambda pointInteret: Ligne(
"", ligne.x1, ligne.y1, ligne.x1, ligne.
y1).distanceAvec(pointInteret.forme))
for point in listePointInteret:
if ignoreEvitmentZone:
if tester.collisionEntre(ligne, point.forme):
return point
else:
if tester.collisionEntre(ligne, point.zoneEvitement.forme):
return point
return False
def simplifierChemin(self,
tabchemin,
listPointInteret,
blockingElements=None):
i = -1
while i < len(tabchemin) - 2:
i += 1
l1 = tabchemin[i]
l2 = tabchemin[i + 1]
line = Ligne("", l1.x1, l1.y1, l2.x2, l2.y2, "")
"""line.setCouleur("blue")
line.dessiner(self.fenetre)"""
if not self.enCollisionCarte(line, listPointInteret):
#if not self.enCollisionGraph(line, blockingElements):
tabchemin.insert(i, line)
tabchemin.remove(l1)
tabchemin.remove(l2)
i = -1
return tabchemin
def enCollisionCarte(self,
ligne,
_listePointInteret,
ignoreEvitmentZone=False):
self.callCount["enCollisionCarte"] = self.callCount[
"enCollisionCarte"] + 1 if "enCollisionCarte" in self.callCount else 1
listePointInteret = sorted(_listePointInteret,
key=lambda pointInteret: Ligne(
"", ligne.x1, ligne.y1, ligne.x1, ligne.
y1).distanceAvec(pointInteret.forme))
for point in listePointInteret:
if ignoreEvitmentZone:
if self.collider.collisionEntre(ligne, point.forme):
return point
else:
if self.collider.collisionEntre(ligne,
point.zoneEvitement.forme):
return point
return False
def setPosition(self, x, y, angle):
self.x = x
self.y = y
self.angle = angle
if webInterface.instance:
self.forme = Ligne("robot", self.x, self.y, self.x + self.largeur,
self.y, "violet")
self.forme.rotate(self.angle)
webInterface.instance.addDynamicElement(self)
for telemetre in self.listTelemetre:
line = Ligne("", self.x, self.y, self.x - telemetre.x,
self.y + telemetre.y, "green")
line.rotate(line.getAngle() + self.angle - 90)
lineTarget = Ligne("", line.x2, line.y2, line.x2 * 2,
line.y2 * 2, "green")
lineTarget.resize(
telemetre.value * 1.25
) # extending the line by 1.25, see telemetreDetectCollision()
lineTarget.rotate(self.angle + telemetre.angle)
lineTarget.couleur = telemetre.color
telemetre.forme = lineTarget
webInterface.instance.addDynamicElement(telemetre)
def getMovmentAngle(self):
line = Ligne("", self.last_x, self.last_y, self.x, self.y)
return line.getAngle()
def trouverChemin(self, x1, y1, x2, y2, _listePointInteret):
collisionEntre = self.collider.collisionEntre #local var perf optim
startElements = self.pointContenuListe(x1, y1, _listePointInteret)
endElements = self.pointContenuListe(x2, y2, _listePointInteret)
blockingElements = startElements + list(
set(endElements) - set(startElements)) #merge removing duplicates
t = time.time()
#self.computeElementsCollisonList(_listePointInteret)
#print "Init Collisions: " + str(time.time() - t)
listePointInteret = sorted(
_listePointInteret,
key=lambda pointInteret: Ligne("", x1, y1, x1, y1).distanceAvec(
pointInteret.forme))
self.resetCollisonLists(listePointInteret)
#self.fenetre = None
initLine = Ligne("", x1, y1, x2, y2)
if self.fenetre:
initLine.setCouleur("violet")
initLine.dessiner(self.fenetre)
pathList = [[initLine]]
countTest = 0
tl = time.time()
while len(pathList) > 0:
#print("{} loop: {}".format(time.time()-tl, countTest))
tl = time.time()
countTest += 1
#print(pathList)
path = pathList.pop(0) #get firt path
if self.fenetre:
#self.fenetre.clear()
for l in path:
l.setCouleur("blue")
l.dessiner(self.fenetre)
self.fenetre.win.redraw()
#time.sleep(0.02)
ligne = path.pop() #get last line
collidePoint = self.enCollisionCarte(ligne, listePointInteret,
False)
if not collidePoint:
path.append(ligne)
#print countTest
t = time.time()
self.simplifierChemin(path, listePointInteret,
blockingElements)
#print "\tSimplifyPathTime: " + str(time.time() - t)
#print "Chemin Simplified len ", len(path)
return path
else:
collidedObjects = [collidePoint]
self.computeCollisonList(collidePoint, listePointInteret)
collidedObjects += collidePoint.collisionList
#find objects coliding current object
"""for obj in listePointInteret:
if obj == collidePoint:
continue
formeCollidePoint = collidePoint.zoneEvitement.forme if collidePoint.zoneEvitement else collidePoint.forme
formeObj = obj.zoneEvitement.forme if obj.zoneEvitement else obj.forme
if self.collider.collisionEntre(formeCollidePoint, formeObj):
collidedObjects.append(obj)"""
points = []
for collidedObj in collidedObjects:
#List collided object's points
objectPoints = []
forme = collidedObj.zoneEvitement.forme if collidedObj.zoneEvitement else collidedObj.forme
#print("colided with {} {}".format(forme.__class__.__name__, collidedObj.nom ))
if isinstance(forme, Ligne):
objectPoints.append(
self.extendLinePoint(forme, forme.x1, forme.y1))
objectPoints.append(
self.extendLinePoint(forme, forme.x2, forme.y2))
if isinstance(forme, Rectangle):
objectPoints.append(
self.extendLinePoint(forme, forme.x1, forme.y1))
objectPoints.append(
self.extendLinePoint(forme, forme.x2, forme.y1))
objectPoints.append(
self.extendLinePoint(forme, forme.x1, forme.y2))
objectPoints.append(
self.extendLinePoint(forme, forme.x2, forme.y2))
if isinstance(forme, Polygone):
for p in forme.pointList:
objectPoints.append(
self.extendLinePoint(forme, p["x"], p["y"]))
if isinstance(forme, Cercle):
nbpoint = 8
for i in numpy.arange(0, math.pi * 2,
math.pi * 2 / (nbpoint + 1)):
x = forme.x + forme.rayon * 1. * math.sin(i)
y = forme.y + forme.rayon * 1. * math.cos(i)
objectPoints.append(
self.extendLinePoint(forme, x, y))
"""c = Cercle("", x, y, 10, "white")
c.dessiner(self.fenetre)
self.fenetre.win.redraw()"""
#Exclude points colliding with direct line
for p in objectPoints:
#Exclude out of table
if p["x"] < 0 or p["x"] > self.largeur or p[
"y"] < 0 or p["y"] > self.longueur:
continue
#Exclude same point
if ligne.x1 == p["x"] and ligne.y1 == p["y"]:
continue
#Exclude point already in path
found = False
for l in path:
if (l.x1 == p["x"] and l.y1 == p["y"]) or (
l.x2 == p["x"] and l.y2 == p["y"]):
found = True
break
if found:
continue
#Exculde from collisions
directLine = Ligne("", ligne.x1, ligne.y1, p["x"],
p["y"])
if collisionEntre(directLine, forme):
continue
if self.enCollisionCarte(directLine, listePointInteret,
False):
continue
if self.fenetre:
c = Cercle("", p["x"], p["y"], 10, "black")
c.dessiner(self.fenetre)
self.fenetre.win.redraw()
points.append(p)
for p in points:
"""c = Cercle("", p["x"], p["y"], 10, "orange")
c.dessiner(self.fenetre)
self.fenetre.win.redraw()"""
newPath = list(path)
newPath.append(
Ligne("", ligne.x1, ligne.y1, p["x"], p["y"]))
newPath.append(
Ligne("", p["x"], p["y"], ligne.x2, ligne.y2))
pathList.append(newPath)
#Explore Paths with points
#print("points: {}".format(len(points)))
#Remove path ending at same point
pai = 0
while pai < len(pathList):
pa = pathList[pai]
#Look if other path reach same line
pa_length = 0
stop = False
lai = 0
while lai < len(pa) - 1:
la = pa[lai]
pa_length += la.getlongeur()
#look for same line in other path
pbi = 0
while pbi < len(pathList):
if pai == pbi:
pbi += 1
continue
pb = pathList[pbi]
pb_length = 0
lbi = 0
while lbi < len(pb) - 1:
lb = pb[lbi]
pb_length += lb.getlongeur()
if la.x1 == lb.x1 and la.y1 == lb.y1 and la.x2 == lb.x2 and la.y2 == lb.y2:
if pa_length < pb_length:
pbi -= 1
pathList.pop(pbi)
break
else:
pathList.pop(pai)
pai -= 1
stop = True
break
lbi += 1
if stop:
break
pbi += 1
lai += 1
if stop:
break
pai += 1
shortestPathMap = {}
conservedPath = []
"""for currentPath in pathList:
length = 0
endLine = None
for l in range(1, len(currentPath)):
if l == len(currentPath)-1:
endLine = currentPath[l]
else:
length += currentPath[l].getlongeur()
if not endLine:
conservedPath.append(currentPath)
continue
x = endLine.x1
y = endLine.y1
key = str(x)+","+str(y)
if (key in shortestPathMap and shortestPathMap[key]["length"] > length) or not key in shortestPathMap:
shortestPathMap[key] = {"length": length, "path": currentPath}
for key, noeud in shortestPathMap.iteritems():
conservedPath.append(noeud["path"])
pathList = conservedPath"""
return []
"""
# Validate checksum
if checksum == cs:
return result
return []
if __name__ == "__main__":
import time
from affichage.fenetre import Fenetre
from cartographie.ligne import Ligne
from threading import Thread
displaySize=600
fenetre = Fenetre(displaySize, displaySize, 1, 0)
lidar = LidarX2("/dev/cu.SLAB_USBtoUART")
if (lidar.open()):
t = time.time() # start time
while (time.time() - t) < 30: # scan for 30 seconds
measures = lidar.getMeasures()
print len(measures)
fenetre.clear()
for measure in measures:
line = Ligne("", displaySize/2, displaySize/2,displaySize/2+measure.distance/2,displaySize/2, "green")
line.rotate(measure.angle)
line.dessiner(fenetre)
fenetre.win.redraw()
time.sleep(0.01)
lidar.close()
else:
print("Cannot open lidar")
def telemetreDetectCollision(self,
speed=None,
rotationOnly=False,
movementAngle=None):
#return False
if self.isSimulated or not self.collisionDetector:
return False
if rotationOnly:
return False
if speed is None:
speed = self.speed
self.collisionDetector.updateTelemetre(self.listTelemetre)
if movementAngle is None:
movementAngle = self.getMovmentAngle()
movementAngle -= 360 if movementAngle >= 360 else 0
movementAngle += 360 if movementAngle < 0 else 0
for telemetre in self.listTelemetre:
telemetre.color = "black"
#Reject invalid telemeter (too far, too close measures)
if not telemetre.isValid():
telemetre.color = "transparent"
continue
if speed == 0:
continue
#Reject telemeters away from movement direction
minAngle = movementAngle - 60
minAngle -= 360 if minAngle >= 360 else 0
minAngle += 360 if minAngle < 0 else 0
maxAngle = movementAngle + 60
maxAngle -= 360 if maxAngle >= 360 else 0
maxAngle += 360 if maxAngle < 0 else 0
line = Ligne("", self.x, self.y, self.x - telemetre.x,
self.y + telemetre.y, "green")
line.rotate(line.getAngle() + self.angle - 90)
telemetreAngle = telemetre.angle + self.angle
telemetreAngle -= 360 if telemetreAngle >= 360 else 0
telemetreAngle += 360 if telemetreAngle < 0 else 0
inRange = False
if maxAngle > minAngle:
inRange = minAngle <= telemetreAngle and telemetreAngle <= maxAngle
else:
inRange = (minAngle <= telemetreAngle and telemetreAngle <= 360
) or (0 <= telemetreAngle
and telemetreAngle <= maxAngle)
if not inRange:
telemetre.color = "grey"
continue
telemetre.color = "blue"
#print telemetre.nom, telemetre.value
line = Ligne("", self.x, self.y, self.x - telemetre.x,
self.y + telemetre.y, "green")
line.rotate(line.getAngle() + self.angle - 90)
lineTarget = Ligne("", line.x2, line.y2, line.x2 * 2, line.y2 * 2,
"purple")
lineTarget.resize(telemetre.value * 1.25)
#extending the line by 1.25 makes more sure it detects colliding objects (not reported as unknown object)
lineTarget.rotate(self.angle + telemetre.angle)
if not 0 < lineTarget.x2 < self.chercher.largeur or not 0 < lineTarget.y2 < self.chercher.longueur:
telemetre.color = "white"
continue # Rejecting detections out of the map
#lineTarget.dessiner(self.fenetre)
#return True
objectInRange = False
objectDetectedLine = None
for i in range(1, 3):
containingElements = self.chercher.pointContenuListe(
lineTarget.x1, lineTarget.y1, self.listPointInteret)
listPointDetection = list(self.listPointInteret)
for point in containingElements:
listPointDetection.remove(point)
lineTest = Ligne("", lineTarget.x1, lineTarget.y1,
lineTarget.x2 / i, lineTarget.y2 / i,
"purple")
collision = self.chercher.enCollisionCarte(
lineTest, listPointDetection, False)
if not collision:
#circle = Cercle(telemetre.nom, line.x2, line.y2, 20, "purple")
#circle.dessiner(self.fenetre)
#lineTarget.dessiner(self.fenetre)
telemetre.color = "purple"
objectDetectedLine = lineTest
else:
objectInRange = True
telemetre.color = "blue"
#print(telemetre.nom + " detected " + collision.nom)
if objectInRange:
pass
elif objectDetectedLine:
#Collision detected
valueRange = telemetre.maxValue - telemetre.minValue
rangePercent = 100 / valueRange * telemetre.value
if rangePercent > 40:
#Slow down
if self.collisionSpeedModifier > rangePercent / 100:
self.collisionSpeedModifier = rangePercent / 100
telemetre.color = "purple"
else:
#Stop
telemetre.color = "red"
print("/!\\" + telemetre.nom +
" detected Unkown object. Position " +
str(objectDetectedLine.x2) + "," +
str(objectDetectedLine.y2) + ", angle " +
str(objectDetectedLine.getAngle()) +
" at distance " + str(telemetre.value))
return [objectDetectedLine.x2, objectDetectedLine.y2]
self.collisionSpeedModifier = 1
return False
def trouverChemin(self, x1, y1, x2, y2, _listePointInteret):
self.graph.nettoyer()
#Point contained in objects
startElements = self.pointContenuListe(x1, y1, _listePointInteret)
endElements = self.pointContenuListe(x2, y2, _listePointInteret)
blockingElements = startElements + list(
set(endElements) - set(startElements)) #merge removing duplicates
notBlockingElements = list(
set(_listePointInteret) - set(blockingElements))
directLine = Ligne("", x1, y1, x2, y2)
if not self.enCollisionCarte(directLine, notBlockingElements):
return [directLine]
elif self.fenetre:
directLine.setCouleur("violet")
directLine.dessiner(self.fenetre)
startNode = self.graph.trouverPointProche(x1, y1)
endNode = self.graph.trouverPointProche(x2, y2)
if startNode == None or endNode == None:
print "Start or end node not found"
return None
listnoeud = deque()
listnoeud.append(startNode)
self.graph.marquer(startNode)
while len(listnoeud) > 0:
currentNode = listnoeud.popleft()
if currentNode == endNode:
break
for noeud in self.graph.getVoisin(currentNode):
if not self.graph.estMaquer(noeud):
addNode = True
if len(noeud.colisionObject) != 0:
for elem in noeud.colisionObject:
if not elem in blockingElements:
self.graph.marquer(noeud)
addNode = False
break
if (addNode):
self.graph.setPere(noeud, currentNode)
self.graph.marquer(noeud)
listnoeud.append(noeud)
listPoint = []
lastNode = endNode
currentNode = self.graph.getPere(endNode)
if currentNode == None:
pass
elif self.graph.getPere(currentNode) == None:
#from endNode
listPoint.append([x2, y2])
#trip
listPoint.append([lastNode.x, lastNode.y])
listPoint.append([currentNode.x, currentNode.y])
#to startNode
listPoint.append([x1, y1])
else:
#from endNode
listPoint.append([x2, y2])
listPoint.append([lastNode.x, lastNode.y])
#trip
while self.graph.getPere(currentNode) != None:
listPoint.append([currentNode.x, currentNode.y])
currentNode = self.graph.getPere(currentNode)
# to startNode
listPoint.append([x1, y1])
tmpList = list(_listePointInteret)
for elem in blockingElements:
try:
tmpList.remove(elem)
except ValueError:
print "ChercheurChemin Err:" + elem.nom #shouldn't pop anymore
listPoint.reverse()
listChemin = []
for i in range(1, len(listPoint)):
p1 = listPoint[i - 1]
p2 = listPoint[i]
line = Ligne("", p1[0], p1[1], p2[0], p2[1])
listChemin.append(line)
print "Chemin len ", len(listChemin)
self.simplifierChemin(listChemin, tmpList)
print "Chemin Simplified len ", len(listChemin)
return listChemin
def dessiner(self, fenetre):
self.forme.dessiner(fenetre)
line = Ligne("", self.x, self.y, self.x + 100, self.y, "blue")
line.rotate(self.angle)
line.dessiner(fenetre)
class Robot:
def __init__(self, nom, largeur):
self.movingBase = None
self.controlPanel = None
self.collisionDetector = None
self.nom = nom
self.fenetre = None
self.largeur = largeur
self.chercher = None
self.listPointInteret = None
self.listVariables = []
self.listPosition = []
self.listTelemetre = []
self.listBoard = []
self.couleur = ""
self.countTest = 0
self.isSimulated = False
self.x = 0
self.y = 0
self.angle = 0
self.speed = 0
self.movingDA = False
self.movingDALastDist = 0
self.movingDALastAngle = 0
self.movingXY = False
self.startTime = time.time()
self.matchDuration = 0
self.objectifEnCours = None
self.forme = None
self.simulationSpeed = 0.002
if webInterface.instance:
webInterface.instance.addMapElement(self)
def __del__(self):
self.closeConnections()
def updateInterfaceMap(self):
if webInterface.instance:
for p in self.listPointInteret:
webInterface.instance.addMapElement(p)
def initialiser(self,
chercher,
listPointInteret,
fenetre=None,
simulate=False):
self.isSimulated = simulate
self.fenetre = fenetre
self.chercher = chercher
self.listPointInteret = listPointInteret
self.updateInterfaceMap()
if not self.isSimulated:
for board in self.listBoard:
if not board.connect():
print("ERROR: Unable to connect " + board.nom)
self.isSimulated = True
if webInterface.instance and webInterface.instance.runningState == RunningState.STOP:
self.isSimulated = True
return False
for board in self.listBoard:
if board.fonction == "movingBase":
self.movingBase = board
elif board.fonction == "controlPanel":
self.controlPanel = board
elif board.fonction == "collisionDetector":
self.collisionDetector = board
if not self.controlPanel and any(board.fonction == "controlPanel"
for board in self.listBoard):
print("ERROR: No controlPanel found")
self.isSimulated = True
elif webInterface.instance and self.controlPanel:
webInterface.instance.addCallableObject(self.controlPanel)
if not self.movingBase and any(board.fonction == "movingBase"
for board in self.listBoard):
print("ERROR: No movingBase found")
self.isSimulated = True
elif webInterface.instance and self.movingBase:
webInterface.instance.addCallableObject(self.movingBase)
if not self.collisionDetector and any(
board.fonction == "collisionDetector"
for board in self.listBoard):
print("ERROR: No collisionDetector found")
self.isSimulated = True
elif webInterface.instance and self.collisionDetector:
webInterface.instance.addCallableObject(self.collisionDetector)
if (simulate != self.isSimulated):
if (self.controlPanel):
self.controlPanel.displayMessage("ERROR: Boards not found.")
time.sleep(0.2)
self.controlPanel.displayMessage("Stopping robot...")
print "HALT: Stopping"
self.isSimulated = True
return self.isSimulated
if self.isSimulated:
self.movingBase = MovingBase("dummyBase", "movingBase", "")
self.movingBase._isXYSupported = True
return self.isSimulated
def setPosition(self, x, y, angle):
self.x = x
self.y = y
self.angle = angle
if webInterface.instance:
self.forme = Ligne("robot", self.x, self.y, self.x + self.largeur,
self.y, "violet")
self.forme.rotate(self.angle)
webInterface.instance.addDynamicElement(self)
for telemetre in self.listTelemetre:
line = Ligne("", self.x, self.y, self.x - telemetre.x,
self.y + telemetre.y, "green")
line.rotate(line.getAngle() + self.angle - 90)
lineTarget = Ligne("", line.x2, line.y2, line.x2 * 2,
line.y2 * 2, "green")
lineTarget.resize(
telemetre.value * 1.25
) # extending the line by 1.25, see telemetreDetectCollision()
lineTarget.rotate(self.angle + telemetre.angle)
lineTarget.couleur = telemetre.color
telemetre.forme = lineTarget
webInterface.instance.addDynamicElement(telemetre)
def closeConnections(self):
for board in self.listBoard:
if board:
board.disconnect()
def dessiner(self):
from cartographie.cercle import Cercle
circle = Cercle(self.nom, self.x, self.y, self.largeur, "white")
circle.dessiner(self.fenetre)
line = Ligne("", self.x, self.y, self.x + self.largeur, self.y, "blue")
line.rotate(self.angle)
line.dessiner(self.fenetre)
for telemetre in self.listTelemetre:
line = Ligne("", self.x, self.y, self.x - telemetre.x,
self.y + telemetre.y, "green")
line.rotate(line.getAngle() + self.angle - 90)
line.dessiner(self.fenetre)
circle = Cercle("", line.x2, line.y2, 10, "green")
circle.dessiner(self.fenetre)
lineTarget = Ligne("", line.x2, line.y2, line.x2 * 2, line.y2 * 2,
"orange")
lineTarget.resize(75)
lineTarget.rotate(self.angle + telemetre.angle)
lineTarget.dessiner(self.fenetre)
def attendreDepart(self):
if self.isSimulated:
self.startTime = time.time()
defaultValues = self.listPosition[1]
if self.couleur != "":
if self.couleur == self.listPosition[0].couleur:
defaultValues = self.listPosition[0]
elif self.couleur == self.listPosition[1].couleur:
defaultValues = self.listPosition[1]
self.couleur = defaultValues.couleur
self.setPosition(defaultValues.x, defaultValues.y,
defaultValues.angle) #0 degres =3 heures
if self.fenetre != None:
self.dessiner()
print("Le robot virtuel est " + self.couleur +
" a la position x:" + str(self.x) + " y:" + str(self.y) +
" angle:" + str(self.angle))
return True
else:
oldColor = None
self.displayScore(0)
if webInterface.instance and webInterface.instance.runningState == RunningState.MANUAL:
self.startTime = time.time()
color = self.controlPanel.getColor() # get the color
if color is not None:
print "Color", self.listPosition[
color].couleur, "(", color, ")", "at X", self.listPosition[
color].x, " Y", self.listPosition[
color].y, " A:", self.listPosition[color].angle
else:
while (self.controlPanel.getStartSignal()):
time.sleep(0.2)
while (not self.controlPanel.getStartSignal()):
self.startTime = time.time()
color = self.controlPanel.getColor() #get the color
if color is not None:
print "Color", self.listPosition[
color].couleur, "(", color, ")", "at X", self.listPosition[
color].x, " Y", self.listPosition[
color].y, " A:", self.listPosition[
color].angle
#for board in self.listBoard:
# print board.nom, board.getId()
self.couleur = self.listPosition[color].couleur
self.setPosition(self.listPosition[color].x,
self.listPosition[color].y,
self.listPosition[color].angle)
if self.movingBase and self.movingBase.isXYSupported():
self.movingBase.setPosition(self.x, self.y, self.angle)
print("Le robot est " + self.couleur)
self.controlPanel.displayMessage("Color: " + self.couleur)
oldColor = color
time.sleep(0.2)
#Set the initial positions
print("Le robot est " + self.couleur + " a la position x:" +
str(self.x) + " y:" + str(self.y) + " angle:" +
str(self.angle))
self.startTime = time.time()
self.controlPanel.displayMessage("Start")
if self.movingBase:
self.movingBase.enableMovements() #authorize the robot to move
return True
def getRunningTime(self):
return time.time() - self.startTime
def attendreMilliseconde(self, duree):
time.sleep(float(duree) / 1000.0)
return True
def getVariable(self, nom):
for variable in self.listVariables:
if variable.nom == nom:
return variable
return None
def displayScore(self, score):
if (self.controlPanel):
self.controlPanel.setScore(score)
print("Score= " + str(score))
def testTelemeters(self):
self.testingTelemeters = True
while self.testingTelemeters:
self.telemetreDetectCollision(0)
self.updatePosition()
time.sleep(0.3)
def stopTestTelemeters(self):
self.testingTelemeters = False
def telemetreDetectCollision(self, speed=None):
#return False
if self.isSimulated:
return False
if speed is None:
speed = self.speed
self.collisionDetector.updateTelemetre(self.listTelemetre)
for telemetre in self.listTelemetre:
telemetre.color = "black"
if speed < 0:
if -80 < telemetre.angle < 80:
telemetre.color = "grey"
continue
elif speed > 0 and not -80 < telemetre.angle < 80:
telemetre.color = "grey"
continue
if not telemetre.isValid():
telemetre.color = "black"
continue
#print telemetre.nom, telemetre.value
line = Ligne("", self.x, self.y, self.x - telemetre.x,
self.y + telemetre.y, "green")
line.rotate(line.getAngle() + self.angle - 90)
lineTarget = Ligne("", line.x2, line.y2, line.x2 * 2, line.y2 * 2,
"purple")
lineTarget.resize(telemetre.value * 1.25)
#extending the line by 1.25 makes more sure it detects colliding objects (not reported as unknown object)
lineTarget.rotate(self.angle + telemetre.angle)
if not 0 < lineTarget.x2 < self.chercher.largeur or not 0 < lineTarget.y2 < self.chercher.longueur:
telemetre.color = "blue"
continue # Rejecting detections out of the map
#lineTarget.dessiner(self.fenetre)
if 5 < telemetre.value < 500:
#return True
#Only test telemeter if the reported value is bellow 500mm
containingElements = self.chercher.pointContenuListe(
lineTarget.x1, lineTarget.y1, self.listPointInteret)
listPointDetection = list(self.listPointInteret)
for point in containingElements:
listPointDetection.remove(point)
collision = self.chercher.enCollisionCarte(
lineTarget, listPointDetection, True)
if not collision:
#circle = Cercle(telemetre.nom, line.x2, line.y2, 20, "purple")
#circle.dessiner(self.fenetre)
#lineTarget.dessiner(self.fenetre)
telemetre.color = "purple"
print("/!\\" + telemetre.nom +
" detected Unkown object. Position " +
str(lineTarget.x2) + "," + str(lineTarget.y2) +
", angle " + str(lineTarget.getAngle()) +
" at distance " + str(telemetre.value))
return [lineTarget.x2, lineTarget.y2]
else:
telemetre.color = "blue"
print(telemetre.nom + " detected " + collision.nom)
return False
def executer(self, action):
tabParam = []
for param in action.tabParametres:
tabParam.append(param.getValue())
if hasattr(self, action.methode):
return getattr(self, action.methode)(
*tabParam) #Run the requested method
else:
print "ERREUR: La methode", action.methode, "n'existe pas!!!"
return False
def positionAtteinte(self, x, y, angle, x1, y1, angle1, erreurPos,
erreurAngle):
print "Check Pos:", x, y, angle, x1, y1, angle1, erreurPos, erreurAngle
if ((abs(x - x1) <= erreurPos) and (abs(y - y1) <= erreurPos) and
(abs(self.normalizeAngle(angle) - self.normalizeAngle(angle1)) <=
erreurAngle)):
return True
return False
def distanceAtteinte(self, dist1, angle1, dist2, angle2, erreurDist,
erreurAngle):
if ((abs(dist1 - dist2) <= erreurDist) and
(abs(self.normalizeAngle(angle1) - self.normalizeAngle(angle2)) <=
erreurAngle)):
return True
return False
def normalizeAngle(self, angle):
if angle > 180:
angle = -360 + angle
if angle < -180:
angle = 360 + angle
return angle
def getAngleToDo(self, angle):
res = angle - self.angle
self.normalizeAngle(res)
return res
def rectifyPosition(self, x=-1, y=-1, angle=-1):
if x != -1:
self.x = x
if y != -1:
self.y = y
if angle != -1:
self.angle = angle
self.setPosition(self.x, self.y, self.angle) #update interface
if not self.isSimulated and self.movingBase and self.movingBase.isXYSupported(
):
self.movingBase.setPosition(self.x, self.y, self.angle)
print "\t \t New rectified position: x:", self.x, " y:", self.y, " angle:", self.angle
return True
def simulateMovement(self, x, y, nextAngle, direction):
ligne = Ligne("", self.x, self.y, x, y, "purple")
# simulate movement
movingAngle = self.normalizeAngle(ligne.getAngle() +
(180 if direction < 0 else 0))
for p in range(0, (int)(ligne.getlongeur())):
ratio = (1.0 / ligne.getlongeur()) * p
ratioDest = 1 - ratio
self.setPosition(ligne.x1 * ratioDest + ligne.x2 * ratio,
ligne.y1 * ratioDest + ligne.y2 * ratio,
movingAngle)
if webInterface.instance and webInterface.instance.runningState == RunningState.STOP:
return False
time.sleep(self.simulationSpeed)
self.setPosition(ligne.x2, ligne.y2, nextAngle)
def seDeplacerXY(self,
x,
y,
absoluteAngle,
vitesse=1.0,
forceStraight=False):
self.updatePosition()
print "\t \t From: x:{:.2f} y:{:.2f} angel:{:.2f}".format(
self.x, self.y, self.angle)
print "\t \t Deplacement: x:{:.2f} y:{:.2f} angel:{:.2f}".format(
x, y, absoluteAngle)
chemin = None
if not forceStraight:
chemin = self.chercher.trouverChemin(self.x, self.y, x, y,
self.listPointInteret)
else:
chemin = []
chemin.append(Ligne("", self.x, self.y, x, y))
if chemin == None or len(chemin) == 0:
print "\t \t Chemin non trouve"
#if self.fenetre:
# Ligne("", self.x, self.y, x, y, "red").dessiner(self.fenetre)
return False
for i in range(0, len(chemin)):
ligne = chemin[i]
if self.fenetre:
ligne.setCouleur("green")
ligne.dessiner(self.fenetre)
print "\t \t path {}/{} from {:.2f},{:.2f} to {:.2f} {:.2f} at angle {:.2f}".format(
i + 1, len(chemin), ligne.x1, ligne.y1, ligne.x2, ligne.y2,
ligne.getAngle())
result = False
if self.movingBase and not self.movingBase.isXYSupported:
result = self.seDeplacerDistanceAngle(
ligne.getlongeur(), self.getAngleToDo(ligne.getAngle()))
if not self.seDeplacerDistanceAngle(
0, self.getAngleToDo(absoluteAngle), vitesse):
print "\t \t Distance-Angle failed"
return False
else:
dirLine = Ligne("", self.x, self.y, ligne.x2, ligne.y2,
"purple")
direction = 1
if abs(dirLine.getAngle() - self.angle) > 90:
direction = -1
#print "Direction", direction
if not self.telemetreDetectCollision(direction):
if not self.isSimulated:
nextAngle = absoluteAngle
if i < len(chemin) - 1:
nextAngle = chemin[i + 1].getAngle()
self.movingBase.startMovementXY(
ligne.x2, ligne.y2, nextAngle, vitesse)
result = self.__waitForMovementFinished(True)
else:
#if self.fenetre:
# dirLine.dessiner(self.fenetre)
nextAngle = absoluteAngle
if i < len(chemin) - 1:
nextAngle = chemin[i + 1].getAngle()
self.simulateMovement(ligne.x2, ligne.y2, nextAngle,
direction)
self.setPosition(ligne.x2, ligne.y2, nextAngle)
result = True
else:
result = False
if not result:
print "\t \t DeplacementXY failed"
return False
if not self.isSimulated:
res = self.positionAtteinte(x, y, absoluteAngle, self.x, self.y,
self.angle, 50, 5)
if not res:
print "\t \t Position non atteinte"
return res
else:
return True
def seDeplacerDistanceAngle(self,
distance,
angle,
vitesse=1.0,
retry=1,
forceLine=False):
print "\t \tDeplacement: distance:", str(distance), " angle:", str(
angle)
if self.movingBase.isXYSupported:
lineTarget = Ligne("", self.x, self.y, self.x * 2, self.y * 2,
"purple")
lineTarget.resize(distance)
lineTarget.rotate(self.angle + angle)
return self.seDeplacerXY(lineTarget.x2, lineTarget.y2,
self.angle + angle, vitesse, forceLine)
if distance == 0 or not self.telemetreDetectCollision(distance):
if not self.isSimulated:
self.movingBase.startMovementDistanceAngle(
distance, angle, vitesse)
direction = 1
if distance < 0:
direction = -1
self.speed = vitesse * direction
self.movingDA = True
self.movingDALastDist = 0
self.movingDALastAngle = 0
result = self.__waitForMovementFinished(False, distance == 0)
self.movingDA = False
return result
else:
self.updatePositionRelative(distance, angle)
return True
else:
return False
def __waitForMovementFinished(self,
xyMove,
rotationOnly=False,
doNotAvoid=False):
errorObstacle = False
errorStuck = False
errorOutOfTime = False
time.sleep(0.1) #wait 100ms before getting information on the movment
print "\t \t waiting"
status = self.movingBase.getMovementStatus()
print "\t \t " + status
while "running" in status:
self.updatePosition()
status = self.movingBase.getMovementStatus()
if not rotationOnly:
collision = self.telemetreDetectCollision()
if type(collision) != bool:
collisionX, collisionY = collision
print "\t \t Obstacle, stopping robot"
self.movingBase.emergencyBreak()
errorObstacle = True
break
elif collision:
print "\t \t Obstacle, stopping robot"
self.movingBase.emergencyBreak()
errorObstacle = True
break
if webInterface.instance and webInterface.instance.runningState == RunningState.STOP:
print "\t \t Stop requested"
self.movingBase.emergencyBreak()
return False
if self.getRunningTime() > self.matchDuration:
self.movingBase.emergencyBreak()
errorOutOfTime = True
break
time.sleep(0.1)
if "stuck" in status:
errorStuck = True
print "\t \t Stuck, stopping movement"
self.movingBase.emergencyBreak()
if xyMove:
newX, newY, newAngle, speed = self.movingBase.getPositionXY()
self.setPosition(newX, newY, newAngle)
else:
distanceDone, angleDone, currentSpeed = self.movingBase.getPositionDistanceAngle(
)
self.updatePositionRelative(distanceDone, angleDone)
if errorObstacle or errorStuck:
if not doNotAvoid:
#self.eviterObstacle(self.angle, direction)
pass
print "\t \t Movement error"
return False
if errorOutOfTime:
print "\t \t Movement out of time"
return False
print "\t \t Movement finished"
return True
def eviterObstacle(self, absoluteObstacleAngle, direction=1):
print("\t \tEscape from A=" + str(absoluteObstacleAngle))
#Get opposed angle
if absoluteObstacleAngle > 0:
newAngle = absoluteObstacleAngle - 180
else:
newAngle = absoluteObstacleAngle + 180
#newAngle = absoluteObstacleAngle
xprev = self.x
yprev = self.y
newx = xprev + 300 * math.cos(newAngle * 0.0174532925) #rad
newy = yprev + 300 * math.sin(newAngle * 0.0174532925) #rad
ligne = Ligne("", xprev, yprev, newx, newy, "purple")
distance = -1 * direction * ligne.getlongeur()
#self.aveugler()
time.sleep(0.2)
self.seDeplacerDistanceAngle(distance, 0, 0.4, 0)
time.sleep(0.2)
#self.rendreVue()
time.sleep(0.2)
return True
#search for a free opposed movment
"""escapePointInteretList = list(self.listPointInteret)
escapeObstacles = self.chercher.pointContenuListe(xprev, yprev, escapePointInteretList)
for elem in escapeObstacles:
escapePointInteretList.remove(elem)
for size in [400, 300, 250]:
for a in range(0, 41, 10):
for side in [-1, 1]:
lineTest = Ligne("", xprev, yprev, newx, newy, "purple")
lineTest.resize(size)
testAngle = lineTest.getAngle()+a*side
lineTest.rotate(lineTest.getAngle()+a*side)
if not self.chercher.enCollisionCarte(lineTest, escapePointInteretList):
print("\t \tFound collision escape")
print("\t \tEscape angle=" + str(lineTest.getAngle()))
distance = -1*direction*lineTest.getlongeur()
angleToDo = self.getAngleToDo(lineTest.getAngle())
if angleToDo>0:
angleToDo-=180
else:
angleToDo+=180
self.seDeplacerDistanceAngle(distance, angleToDo, 0.2, 0)
return True
elif (self.fenetre):
lineTest.dessiner(self.fenetre)
self.fenetre.win.redraw()"""
return False
def updatePositionRelative(self, distance, angle):
angleDiff = (angle + self.angle) * 0.0174532925 #rad
xprev = self.x
yprev = self.y
self.x += distance * math.cos(angleDiff)
self.y += distance * math.sin(angleDiff)
self.angle += angle
if self.angle > 180:
self.angle = -360 + self.angle
if self.angle < -180:
self.angle = 360 + self.angle
if self.fenetre != None:
ligne = Ligne("", xprev, yprev, self.x, self.y, "green")
ligne.dessiner(self.fenetre)
self.fenetre.win.redraw()
self.setPosition(self.x, self.y, self.angle) #update interface
def updatePosition(self):
if self.isSimulated:
return
if self.movingBase.isXYSupported:
newX, newY, newAngle, speed = self.movingBase.getPositionXY()
self.setPosition(newX, newY, newAngle)
self.speed = speed
elif self.movingDA:
distance, angle, speed = self.movingBase.getPositionDistanceAngle()
self.speed = speed
self.updatePositionRelative(distance - self.movingDALastDist,
angle - self.movingDALastAngle)
self.movingDALastDist = distance
self.movingDALastAngle = angle
else:
self.speed = self.movingBase.getSpeed()
def seDeplacerVersUnElement(self, type, vitesse=1, couleur=None):
element = None
if couleur == None:
couleur = self.couleur
#recherche de l'objet
for obj in self.listPointInteret:
if obj.type == type and (obj.couleur == couleur
or obj.couleur not in [
self.listPosition[0].couleur,
self.listPosition[1].couleur
]):
element = obj
break
if element == None:
print "\t \tElement non trouve!!!!" + type
return False
print "\t \tDeplacement vers " + type
zoneAcces = element.zoneAcces
if zoneAcces == None:
print "\t \tL'element \"" + element.nom + "\" n'a pas de zone d'acces"
return False
return self.seDeplacerXY(zoneAcces.x, zoneAcces.y, zoneAcces.angle,
vitesse)
def retirerElementCarte(self, type, couleur=None):
element = None
if couleur == None:
couleur = self.couleur
#recherche de l'objet
for obj in self.listPointInteret:
if obj.type == type and (obj.couleur == couleur
or obj.couleur not in [
self.listPosition[0].couleur,
self.listPosition[1].couleur
]):
element = obj
break
if element == None:
print "Element", type, " non trouve!!!!"
return True
self.chercher.updateNodesRemovingElement(element,
self.listPointInteret)
self.listPointInteret.remove(element)
if webInterface.instance:
webInterface.instance.removeMapElement(element)
if self.fenetre:
element.zoneEvitement.forme.couleur = "white"
element.zoneEvitement.dessiner(self.fenetre)
self.fenetre.win.redraw()
return True
def avancer(self, distance, vitesse=0.5):
#return self.seDeplacerDistanceAngle(distance,0, vitesse,1, True)
newx = self.x + distance * math.cos(self.angle * 0.0174532925) # rad
newy = self.y + distance * math.sin(self.angle * 0.0174532925) # rad
return self.seDeplacerXY(newx, newy, self.angle, vitesse, True)
def reculer(self, distance, vitesse=0.5):
#return self.seDeplacerDistanceAngle(-distance,0, vitesse, 1, True)
newx = self.x + -distance * math.cos(self.angle * 0.0174532925) # rad
newy = self.y + -distance * math.sin(self.angle * 0.0174532925) # rad
return self.seDeplacerXY(newx, newy, self.angle, vitesse, True)
def tournerAbsolue(self, angle, vitesse=0.5):
return self.seDeplacerXY(self.x, self.y, angle, vitesse)
def recaler(self,
distance,
axe,
coordinate,
angle,
vitesse=0.2,
coordinate2=None):
success = True
if not self.isSimulated:
self.movingBase.startRepositioningMovement(distance, vitesse)
direction = 1
if distance < 0:
direction = -1
result = self.__waitForMovementFinished(False, direction, True)
if result:
print(
"ERROR: Movement wasn't stuck during the repositioning movement, the known angle should have a big error."
)
success = False
if axe == "X":
self.x = coordinate
elif axe == "Y":
self.y = coordinate
elif axe == "XY":
self.x = coordinate
self.y = coordinate2
self.angle = angle
self.setPosition(self.x, self.y, self.angle) # update interface
return success
def incrementerVariable(self, variable):
var = self.getVariable(variable)
if var:
var.incrementer()
else:
print("ERROR: Variable not found: " + str(variable))
def decrementerVariable(self, variable):
var = self.getVariable(variable)
if var:
var.decrementer()
else:
print("ERROR: Variable not found: " + str(variable))
def resetVariable(self, variable):
var = self.getVariable(variable)
if var:
var.set(0)
else:
print("ERROR: Variable not found: " + str(variable))
def telemetreDetectCollision(self, speed=None):
#return False
if self.isSimulated:
return False
if speed is None:
speed = self.speed
self.collisionDetector.updateTelemetre(self.listTelemetre)
for telemetre in self.listTelemetre:
telemetre.color = "black"
if speed < 0:
if -80 < telemetre.angle < 80:
telemetre.color = "grey"
continue
elif speed > 0 and not -80 < telemetre.angle < 80:
telemetre.color = "grey"
continue
if not telemetre.isValid():
telemetre.color = "black"
continue
#print telemetre.nom, telemetre.value
line = Ligne("", self.x, self.y, self.x - telemetre.x,
self.y + telemetre.y, "green")
line.rotate(line.getAngle() + self.angle - 90)
lineTarget = Ligne("", line.x2, line.y2, line.x2 * 2, line.y2 * 2,
"purple")
lineTarget.resize(telemetre.value * 1.25)
#extending the line by 1.25 makes more sure it detects colliding objects (not reported as unknown object)
lineTarget.rotate(self.angle + telemetre.angle)
if not 0 < lineTarget.x2 < self.chercher.largeur or not 0 < lineTarget.y2 < self.chercher.longueur:
telemetre.color = "blue"
continue # Rejecting detections out of the map
#lineTarget.dessiner(self.fenetre)
if 5 < telemetre.value < 500:
#return True
#Only test telemeter if the reported value is bellow 500mm
containingElements = self.chercher.pointContenuListe(
lineTarget.x1, lineTarget.y1, self.listPointInteret)
listPointDetection = list(self.listPointInteret)
for point in containingElements:
listPointDetection.remove(point)
collision = self.chercher.enCollisionCarte(
lineTarget, listPointDetection, True)
if not collision:
#circle = Cercle(telemetre.nom, line.x2, line.y2, 20, "purple")
#circle.dessiner(self.fenetre)
#lineTarget.dessiner(self.fenetre)
telemetre.color = "purple"
print("/!\\" + telemetre.nom +
" detected Unkown object. Position " +
str(lineTarget.x2) + "," + str(lineTarget.y2) +
", angle " + str(lineTarget.getAngle()) +
" at distance " + str(telemetre.value))
return [lineTarget.x2, lineTarget.y2]
else:
telemetre.color = "blue"
print(telemetre.nom + " detected " + collision.nom)
return False
def seDeplacerXY(self,
x,
y,
absoluteAngle,
vitesse=1.0,
forceStraight=False):
self.updatePosition()
print "\t \t From: x:{:.2f} y:{:.2f} angel:{:.2f}".format(
self.x, self.y, self.angle)
print "\t \t Deplacement: x:{:.2f} y:{:.2f} angel:{:.2f}".format(
x, y, absoluteAngle)
chemin = None
if not forceStraight:
chemin = self.chercher.trouverChemin(self.x, self.y, x, y,
self.listPointInteret)
else:
chemin = []
chemin.append(Ligne("", self.x, self.y, x, y))
if chemin == None or len(chemin) == 0:
print "\t \t Chemin non trouve"
#if self.fenetre:
# Ligne("", self.x, self.y, x, y, "red").dessiner(self.fenetre)
return False
for i in range(0, len(chemin)):
ligne = chemin[i]
if self.fenetre:
ligne.setCouleur("green")
ligne.dessiner(self.fenetre)
print "\t \t path {}/{} from {:.2f},{:.2f} to {:.2f} {:.2f} at angle {:.2f}".format(
i + 1, len(chemin), ligne.x1, ligne.y1, ligne.x2, ligne.y2,
ligne.getAngle())
result = False
if self.movingBase and not self.movingBase.isXYSupported:
result = self.seDeplacerDistanceAngle(
ligne.getlongeur(), self.getAngleToDo(ligne.getAngle()))
if not self.seDeplacerDistanceAngle(
0, self.getAngleToDo(absoluteAngle), vitesse):
print "\t \t Distance-Angle failed"
return False
else:
dirLine = Ligne("", self.x, self.y, ligne.x2, ligne.y2,
"purple")
direction = 1
if abs(dirLine.getAngle() - self.angle) > 90:
direction = -1
#print "Direction", direction
if not self.telemetreDetectCollision(direction):
if not self.isSimulated:
nextAngle = absoluteAngle
if i < len(chemin) - 1:
nextAngle = chemin[i + 1].getAngle()
self.movingBase.startMovementXY(
ligne.x2, ligne.y2, nextAngle, vitesse)
result = self.__waitForMovementFinished(True)
else:
#if self.fenetre:
# dirLine.dessiner(self.fenetre)
nextAngle = absoluteAngle
if i < len(chemin) - 1:
nextAngle = chemin[i + 1].getAngle()
self.simulateMovement(ligne.x2, ligne.y2, nextAngle,
direction)
self.setPosition(ligne.x2, ligne.y2, nextAngle)
result = True
else:
result = False
if not result:
print "\t \t DeplacementXY failed"
return False
if not self.isSimulated:
res = self.positionAtteinte(x, y, absoluteAngle, self.x, self.y,
self.angle, 50, 5)
if not res:
print "\t \t Position non atteinte"
return res
else:
return True
def extendLinePoint(self, forme, x, y):
ligne = Ligne("", forme.x, forme.y, x, y)
ligne.resize(ligne.getlongeur() + 30)
point = {"x": ligne.x2, "y": ligne.y2}
return point
def dessiner(self):
from cartographie.cercle import Cercle
circle = Cercle(self.nom, self.x, self.y, self.largeur, "white")
circle.dessiner(self.fenetre)
line = Ligne("", self.x, self.y, self.x + self.largeur, self.y, "blue")
line.rotate(self.angle)
line.dessiner(self.fenetre)
for telemetre in self.listTelemetre:
line = Ligne("", self.x, self.y, self.x - telemetre.x,
self.y + telemetre.y, "green")
line.rotate(line.getAngle() + self.angle - 90)
line.dessiner(self.fenetre)
circle = Cercle("", line.x2, line.y2, 10, "green")
circle.dessiner(self.fenetre)
lineTarget = Ligne("", line.x2, line.y2, line.x2 * 2, line.y2 * 2,
"orange")
lineTarget.resize(75)
lineTarget.rotate(self.angle + telemetre.angle)
lineTarget.dessiner(self.fenetre)
