def __collisionThread(self):
while not self.stopCollisionThread:
self.collisionDetected = self.telemetreDetectCollision()
#Display telemeters and robot
if webInterface and 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)
if webInterface and webInterface.instance:
line = Ligne("direction", self.x, self.y,
self.x + self.largeur * 2, self.y, "green")
line.rotate(self.getMovmentAngle())
self.movementDirectionObj.forme = line
webInterface.instance.addDynamicElement(
self.movementDirectionObj)
time.sleep(0.1)
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 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 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 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 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 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 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
