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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 [] """
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
# 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 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)
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 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