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 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 __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 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 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 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 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 getMovmentAngle(self):
line = Ligne("", self.last_x, self.last_y, self.x, self.y)
return line.getAngle()
