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