def setVelocity(self, vx, vy, vz, yaw): cmd = jderobot.CMDVelData() cmd.linearX = vy cmd.linearY = vx cmd.linearZ = vz cmd.angularZ = yaw cmd.angularX = cmd.angularY = 1.0 self.CMDVelPrx.setCMDVelData(cmd)
def sendCMDVel(self, vx, vy, vz, yaw, roll, pitch): cmd = jderobot.CMDVelData() cmd.linearX = vy cmd.linearY = vx cmd.linearZ = vz cmd.angularZ = yaw cmd.angularX = cmd.angularY = 1.0 if self.cmdVelProxy: self.lock.acquire() self.cmdVelProxy.setCMDVelData(cmd) self.lock.release()
def sendCMDVel(self, vx, vy, vz, ax, ay, az): cmd = jderobot.CMDVelData() cmd.linearX = vx cmd.linearY = vy cmd.linearZ = vz cmd.angularZ = az cmd.angularX = ax cmd.angularY = ay if self.hasproxy(): self.lock.acquire() self.proxy.setCMDVelData(cmd) self.lock.release()
def getCMDVelData(self, current=None): time.sleep(0.05) # 20Hz (50ms) rate to rx CMDVel lock.acquire() data = jderobot.CMDVelData() data.linearX = self.linearX data.linearY = self.linearY data.linearZ = self.linearZ data.angularX = self.angularX data.angularY = self.angularY data.angularZ = self.angularZ lock.release() return data
def sendCMDVel(self, vx, vy, vz, yaw, roll, pitch): cmd = jderobot.CMDVelData() if self.isVirtual() == True: cmd.linearX = vy cmd.linearY = vx cmd.linearZ = vz cmd.angularZ = yaw else: if abs(vx) > self.MAX_LINX: if vx > 0: cmd.linearX = self.MAX_LINX else: cmd.linearX = -self.MAX_LINX else: cmd.linearX = vx if abs(vy) > self.MAX_LINY: if vy > 0: cmd.linearY = self.MAX_LINY else: cmd.linearY = -self.MAX_LINY else: cmd.linearY = vy if abs(vz) > self.MAX_LINZ: if vz > 0: cmd.linearZ = self.MAX_LINZ else: cmd.linearZ = -self.MAX_LINZ else: cmd.linearZ = vz if abs(yaw) > self.MAX_ANGZ: if yaw > 0: cmd.angularZ = self.MAX_ANGZ else: cmd.angularZ = -self.MAX_ANGZ else: cmd.angularZ = yaw cmd.angularX = roll cmd.angularY = pitch if self.cmdVelProxy: self.lock.acquire() self.cmdVelProxy.setCMDVelData(cmd) self.lock.release()
def __init__(self): self.lock = threading.Lock() self.playButton = False self.cmd = jderobot.CMDVelData() self.cmd.linearX = self.cmd.linearY = self.cmd.linearZ = 0 self.cmd.angularZ = 0 ''' With values distinct to 0 in the next fields, the ardrone not enter in hover mode''' self.cmd.angularX = 0.5 self.cmd.angularY = 1.0 try: ic = Ice.initialize(sys.argv) properties = ic.getProperties() basecamera = ic.propertyToProxy("Introrob.Camera.Proxy") self.cameraProxy = jderobot.CameraPrx.checkedCast(basecamera) if self.cameraProxy: self.image = self.cameraProxy.getImageData("RGB8") self.height = self.image.description.height self.width = self.image.description.width self.trackImage = np.zeros((self.height, self.width, 3), np.uint8) self.trackImage.shape = self.height, self.width, 3 self.thresoldImage = np.zeros((self.height, self.width, 1), np.uint8) self.thresoldImage.shape = self.height, self.width, 1 self.filterValues = ColorFilterValues() else: print 'Interface camera not connected' basecmdVel = ic.propertyToProxy("Introrob.CMDVel.Proxy") self.cmdVelProxy = jderobot.CMDVelPrx.checkedCast(basecmdVel) if not self.cmdVelProxy: print 'Interface cmdVel not connected' basenavdata = ic.propertyToProxy("Introrob.Navdata.Proxy") self.navdataProxy = jderobot.NavdataPrx.checkedCast(basenavdata) if self.navdataProxy: self.navdata = self.navdataProxy.getNavdata() if self.navdata.vehicle == self.ARDRONE_SIMULATED: self.virtualDrone = True else: self.virtualDrone = False else: print 'Interface navdata not connected' self.virtualDrone = True baseextra = ic.propertyToProxy("Introrob.Extra.Proxy") self.extraProxy = jderobot.ArDroneExtraPrx.checkedCast(baseextra) if not self.extraProxy: print 'Interface ardroneExtra not connected' basepose3D = ic.propertyToProxy("Introrob.Pose3D.Proxy") self.pose3DProxy = jderobot.Pose3DPrx.checkedCast(basepose3D) if self.pose3DProxy: self.pose = jderobot.Pose3DData() else: print 'Interface pose3D not connected' except: traceback.print_exc() exit() status = 1
def subautomata9(self): run = True cycle = 100 t_activated = False t_fin = 0 t_GoingToBottom_max = 3 t_StabilizingBottom_max = 2 t_GoneToBottom_max = 0.1 while (run): totala = time.time() * 1000000 if (self.sub5 == "DoingLeftSide"): if ((self.sub9 == "GoingToBottom_ghost") or (self.sub9 == "StabilizingBottom_ghost") or (self.sub9 == "GoneToBottom_ghost")): ghostStateIndex = self.StatesSub9.index(self.sub9) self.sub9 = self.StatesSub9[ghostStateIndex - 1] t_ini = time.time() # Evaluation if if (self.sub9 == "GoingToBottom"): if (not t_activated): t_ini = time.time() t_activated = True else: t_fin = time.time() secs = t_fin - t_ini if (secs > t_GoingToBottom_max): self.sub9 = "StabilizingBottom" t_activated = False if self.displayGui: self.automataGui.notifySetNodeAsActive( 'StabilizingBottom') t_GoingToBottom_max = 3 elif (self.sub9 == "StabilizingBottom"): if (not t_activated): t_ini = time.time() t_activated = True else: t_fin = time.time() secs = t_fin - t_ini if (secs > t_StabilizingBottom_max): self.sub9 = "GoneToBottom" t_activated = False if self.displayGui: self.automataGui.notifySetNodeAsActive( 'GoneToBottom') t_StabilizingBottom_max = 2 elif (self.sub9 == "GoneToBottom"): if (not t_activated): t_ini = time.time() t_activated = True else: t_fin = time.time() secs = t_fin - t_ini if (secs > t_GoneToBottom_max): self.sub9 = "GoingToBottom" t_activated = False self.goneToBottom = False if self.displayGui: self.automataGui.notifySetNodeAsActive( 'GoingToBottom') t_GoneToBottom_max = 0.1 # Actuation if if (self.sub9 == "GoingToBottom"): cmd = jderobot.CMDVelData() cmd.linearX = -0.75 cmd.linearY = 0 cmd.linearZ = 0 self.lock.acquire() self.cmdVelPrx.setCMDVelData(cmd) self.lock.release() elif (self.sub9 == "StabilizingBottom"): cmd = jderobot.CMDVelData() cmd.linearX = 0 cmd.linearY = 0 cmd.linearZ = 0 self.lock.acquire() self.cmdVelPrx.setCMDVelData(cmd) self.lock.release() elif (self.sub9 == "GoneToBottom"): self.goneToBottom = True else: if (self.sub9 == "GoingToBottom"): t_GoingToBottom_max = 3 - (t_fin - t_ini) ghostStateIndex = self.StatesSub9.index(self.sub9) + 1 self.sub9 = self.StatesSub9[ghostStateIndex] elif (self.sub9 == "StabilizingBottom"): t_StabilizingBottom_max = 2 - (t_fin - t_ini) ghostStateIndex = self.StatesSub9.index(self.sub9) + 1 self.sub9 = self.StatesSub9[ghostStateIndex] elif (self.sub9 == "GoneToBottom"): t_GoneToBottom_max = 0.1 - (t_fin - t_ini) ghostStateIndex = self.StatesSub9.index(self.sub9) + 1 self.sub9 = self.StatesSub9[ghostStateIndex] totalb = time.time() * 1000000 msecs = (totalb - totala) / 1000 if (msecs < 0 or msecs > cycle): msecs = cycle else: msecs = cycle - msecs time.sleep(msecs / 1000) if (msecs < 33): time.sleep(33 / 1000)
def subautomata3(self): run = True cycle = 100 t_activated = False t_fin = 0 t_Stabilizing2_max = 3 t_goneFront_max = 0.2 initPos = 0 pos = 0 while (run): totala = time.time() * 1000000 if (self.sub1 == "GoFront"): if ((self.sub3 == "GoingFront_ghost") or (self.sub3 == "Stabilizing2_ghost") or (self.sub3 == "goneFront_ghost")): ghostStateIndex = self.StatesSub3.index(self.sub3) self.sub3 = self.StatesSub3[ghostStateIndex - 1] t_ini = time.time() # Evaluation if if (self.sub3 == "GoingFront"): if (pos - initPos > 300): self.sub3 = "Stabilizing2" print "300m reached" if self.displayGui: self.automataGui.notifySetNodeAsActive( 'Stabilizing2') elif (self.sub3 == "Stabilizing2"): if (not t_activated): t_ini = time.time() t_activated = True else: t_fin = time.time() secs = t_fin - t_ini if (secs > t_Stabilizing2_max): self.sub3 = "goneFront" t_activated = False print "from stabilizing2 to GoneFront" if self.displayGui: self.automataGui.notifySetNodeAsActive( 'goneFront') t_Stabilizing2_max = 3 elif (self.sub3 == "goneFront"): if (not t_activated): t_ini = time.time() t_activated = True else: t_fin = time.time() secs = t_fin - t_ini if (secs > t_goneFront_max): self.sub3 = "GoingFront" t_activated = False initPos = 0 pos = 0 if self.displayGui: self.automataGui.notifySetNodeAsActive( 'GoingFront') t_goneFront_max = 0.2 # Actuation if if (self.sub3 == "GoingFront"): pos = self.pose3dPrx.getPose3DData().x * 100 if initPos == 0: initPos = self.pose3dPrx.getPose3DData().x * 100 print "pos:", pos, "init:", initPos print "distancia", pos - initPos cmd = jderobot.CMDVelData() cmd.linearX = 1 cmd.linearY = 0 cmd.linearZ = 0 self.lock.acquire() self.cmdVelPrx.setCMDVelData(cmd) self.lock.release() elif (self.sub3 == "Stabilizing2"): cmd.linearX = 0 cmd.linearY = 0 cmd.linearZ = 0 self.lock.acquire() self.cmdVelPrx.setCMDVelData(cmd) self.lock.release() elif (self.sub3 == "goneFront"): self.goneFront = True else: if (self.sub3 == "GoingFront"): ghostStateIndex = self.StatesSub3.index(self.sub3) + 1 self.sub3 = self.StatesSub3[ghostStateIndex] elif (self.sub3 == "Stabilizing2"): t_Stabilizing2_max = 3 - (t_fin - t_ini) ghostStateIndex = self.StatesSub3.index(self.sub3) + 1 self.sub3 = self.StatesSub3[ghostStateIndex] elif (self.sub3 == "goneFront"): t_goneFront_max = 0.2 - (t_fin - t_ini) ghostStateIndex = self.StatesSub3.index(self.sub3) + 1 self.sub3 = self.StatesSub3[ghostStateIndex] totalb = time.time() * 1000000 msecs = (totalb - totala) / 1000 if (msecs < 0 or msecs > cycle): msecs = cycle else: msecs = cycle - msecs time.sleep(msecs / 1000) if (msecs < 33): time.sleep(33 / 1000)
def subautomata3(self): run = True cycle = 100 t_activated = False t_GoingFront_max = 4 t_Stabilizing_max = 3 while(run): totala = time.time() * 1000000 if(self.sub1 == "GoFront"): if ((self.sub3 == "GoingFront_ghost") or (self.sub3 == "Stabilizing_ghost") or (self.sub3 == "ENDSUBAUTOMATA_ghost")): ghostStateIndex = self.StatesSub3.index(self.sub3) self.sub3 = self.StatesSub3[ghostStateIndex - 1] t_ini = time.time() # Evaluation if if(self.sub3 == "GoingFront"): if(not t_activated): t_ini = time.time() t_activated = True else: t_fin = time.time() secs = t_fin - t_ini if(secs > t_GoingFront_max): self.sub3 = "Stabilizing" t_activated = False t_GoingFront_max = 4 elif(self.sub3 == "Stabilizing"): if(not t_activated): t_ini = time.time() t_activated = True else: t_fin = time.time() secs = t_fin - t_ini if(secs > t_Stabilizing_max): self.sub3 = "ENDSUBAUTOMATA" t_activated = False t_Stabilizing_max = 3 # Actuation if if(self.sub3 == "GoingFront"): cmd = jderobot.CMDVelData() cmd.linearX = 1 cmd.linearY = 0 cmd.linearZ = 0 self.lock.acquire() self.cmdPrx.setCMDVelData(cmd) self.lock.release() elif(self.sub3 == "ENDSUBAUTOMATA"): self.endGoFront = True else: if(self.sub3 == "GoingFront"): t_GoingFront_max = 4 - (t_fin - t_ini) ghostStateIndex = self.StateSub3.index(self.sub3) + 1 sub3 = self.StatesSub3[ghostStateIndex] elif(self.sub3 == "Stabilizing"): t_Stabilizing_max = 3 - (t_fin - t_ini) ghostStateIndex = self.StateSub3.index(self.sub3) + 1 sub3 = self.StatesSub3[ghostStateIndex] totalb = time.time() * 1000000 msecs = (totalb - totala) / 1000; if(msecs < 0 or msecs > cycle): msecs = cycle else: msecs = cycle - msecs time.sleep(msecs / 1000) if(msecs < 33 ): time.sleep(33 / 1000);
def subautomata1(self): run = True cycle = 100 t_activated = False hasTakenOff = False hasLanded = False while (run): totala = time.time() * 1000000 # Evaluation if if (self.sub1 == "TakeOff"): if (hasTakenOff): self.sub1 = "Stabilizing1" print "Going to Stabilizing" self.automataGui.notifySetNodeAsActive('Stabilizing1') elif (self.sub1 == "Stabilizing1"): if (not t_activated): t_ini = time.time() t_activated = True else: t_fin = time.time() secs = t_fin - t_ini if (secs > 1.5): self.sub1 = "GoFront" t_activated = False print "going to GoFront" self.automataGui.notifySetNodeAsActive('GoFront') elif (self.sub1 == "GoFront"): if (not t_activated): t_ini = time.time() t_activated = True else: t_fin = time.time() secs = t_fin - t_ini if (secs > 3): self.sub1 = "Stoping" t_activated = False print "going to Stoping" self.automataGui.notifySetNodeAsActive('Stoping') elif (self.sub1 == "Stoping"): if (not t_activated): t_ini = time.time() t_activated = True else: t_fin = time.time() secs = t_fin - t_ini if (secs > 3.5): self.sub1 = "Landing" t_activated = False print "going to Land" self.automataGui.notifySetNodeAsActive('Landing') elif (self.sub1 == "Landing"): if (hasLanded): self.sub1 = "Stabilizing2" self.automataGui.notifySetNodeAsActive('Stabilizing2') elif (self.sub1 == "Stabilizing2"): if (not t_activated): t_ini = time.time() t_activated = True else: t_fin = time.time() secs = t_fin - t_ini if (secs > 3): self.sub1 = "TakeOff" t_activated = False print "Restarting" self.automataGui.notifySetNodeAsActive('TakeOff') # Actuation if if (self.sub1 == "TakeOff"): print "Taking Off" self.lock.acquire() self.extraPrx.takeoff() self.lock.release() hasTakenOff = True elif (self.sub1 == "Stabilizing1"): print "Stabilizing" elif (self.sub1 == "GoFront"): cmd = jderobot.CMDVelData() cmd.linearX = 1 cmd.linearY = 0 cmd.linearZ = 0 self.lock.acquire() self.cmdPrx.setCMDVelData(cmd) self.lock.release() elif (self.sub1 == "Stoping"): cmd = jderobot.CMDVelData() cmd.linearX = 0 cmd.linearY = 0 cmd.linearZ = 0 self.lock.acquire() self.cmdPrx.setCMDVelData(cmd) self.lock.release() print "Stoping" elif (self.sub1 == "Landing"): print "Landing" self.lock.acquire() self.extraPrx.land() self.lock.release() hasLanded = True elif (self.sub1 == "Stabilizing2"): finished() totalb = time.time() * 1000000 msecs = (totalb - totala) / 1000 if (msecs < 0 or msecs > cycle): msecs = cycle else: msecs = cycle - msecs time.sleep(msecs / 1000) if (msecs < 33): time.sleep(33 / 1000)
] # print cameraCentrePixels # print targetCentrePixels print targetPose ### Landing decision make ### print 'Landing error: %f' % landingError # Loiter if target is in the landing area # if (landingError < LandingPrecision) and (landingError != 0): print 'Landing decision = TRUE' targetCentreMetres = (0, 0) landDecision = True data = jderobot.CMDVelData() if landDecision: data.linearZ = -1 # Landing Velocity else: data.linearZ = 0 PH_CMDVel.setCMDVelData(data) ### send waypoint to server ### targetCentreMetresDiff = frameChange2D(targetCentreMetres, vehicleYaw) wayPoint = jderobot.Pose3DData() wayPoint.x = vehicleXYZ[0] + targetCentreMetres[0] wayPoint.y = vehicleXYZ[1] + targetCentreMetres[1] wayPoint.z = MissionHeight