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
