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)