def __init__(self, flight=None):
'''
constructor
(optional parameter) flight is yudrone gui
'''
self.__facedTagNr = -1
self.__faceLostCounter = 20
self.__maxAltitude = 1500
self.__minAltitude = 100
self.__yawSpeed = 0.8
self.__hrzSpeed = 0.5
self.__searchState = -1
self.__searchTagNr = -1
self.__lockNr = -1
self.__lastFaceCorrectionSums = {0:0.5, 1:0.5, 2:0.5}
self.imgheight = 360
self.imgwidth = 640
self.__aim = {'ax':0.0, 'ay':0.0, 'az':0.0, 'lx':0.0, 'ly':0.0, 'lz':0.0}
self.navdata = None
self.pub_land = rospy.Publisher( "ardrone/land", Empty ) # ardrone land command
self.pub_takeoff = rospy.Publisher( "ardrone/takeoff", Empty ) # ardrone takeoff command
self.pub_emergency = rospy.Publisher( "ardrone/reset", Empty ) # ardrone emergency mode toggle
self.sub_nav = rospy.Subscriber( "ardrone/navdata", Navdata, self.updateNav )
self.sub_tags = rospy.Subscriber( "tags", Tags, self.updateTags ) # artoolkit
self.pub_cmdStatus = rospy.Publisher( "yudrone/cmdStatus", commandStatus ) # yudrone_cmd reports availability and status
self.sub_com = rospy.Subscriber( "yudrone/commands", commandsMsg, self.handle_command ) # yudrone_cmd listenes command messages
self.pub_twist = rospy.Publisher( "yudrone/cmd_vel", Twist ) # forwarded to ardrone navigation topic (except if joypad has control)
# init PIDs (reference, Kp, Ki, Kd, maxIntegral[=0.15/Ki])
self.PID_az = PID_controller(0.5, 0.4, 0.005, 1.0, 30) # yaw to tag controller
self.PID_lz = PID_controller(0.5, 0.4, 0.001, 1.0, 150) # elevate to tag controller
self.PID_ly = PID_controller(0.0, 0.1, 0.0005, 1.5, 300) # move perpendicular to tag controller
self.PID_lx = PID_controller(FACEDIST, 0.00005, 0.000005, 0.001, 500)# distance to tag controller
if flight == None:
# this code applies for usage WITHOUT gui
rospy.init_node('yudrone_cmd')
rospy.loginfo("init 'yudrone commands' stand-alone")
rospy.loginfo("make sure the following nodes are running: ardrone_driver, ar_recog")
self.hasGui = False
else:
# this code applies for usage WITH gui
rospy.loginfo("init 'yudrone commands' using gui")
self.hasGui = True
# stop moving and start naviagtion loop
self.__reset_twist(0.1)
self.navTimer = rospy.Timer(rospy.Duration(1.0/NAVRATE), self.__onNavigate)
import graph as gr
import time
from PID_controller import PID_controller
#PID controller test
x=0
vel=0
ar=[]
f=[]
t_graph=[]
v_graph=[]
dt=0.05
t=0
t0=time.time();
t_last=0
v_last=0
controller=PID_controller(0.5,0.1,0.1)
for i in range(0, 1000):
if i < 20:
ar.append(0)
else:
ar.append(5)
#t = time.time()-t0
t = t+dt
#dt = t - t_last
#control
dx=ar[i]-x
y=controller.getControl(dx)
class Commands:
'''****************************************************************************************************
* administrative methods
****************************************************************************************************'''
def __init__(self, flight=None):
'''
constructor
(optional parameter) flight is yudrone gui
'''
self.__facedTagNr = -1
self.__faceLostCounter = 20
self.__maxAltitude = 1500
self.__minAltitude = 100
self.__yawSpeed = 0.8
self.__hrzSpeed = 0.5
self.__searchState = -1
self.__searchTagNr = -1
self.__lockNr = -1
self.__lastFaceCorrectionSums = {0:0.5, 1:0.5, 2:0.5}
self.imgheight = 360
self.imgwidth = 640
self.__aim = {'ax':0.0, 'ay':0.0, 'az':0.0, 'lx':0.0, 'ly':0.0, 'lz':0.0}
self.navdata = None
self.pub_land = rospy.Publisher( "ardrone/land", Empty ) # ardrone land command
self.pub_takeoff = rospy.Publisher( "ardrone/takeoff", Empty ) # ardrone takeoff command
self.pub_emergency = rospy.Publisher( "ardrone/reset", Empty ) # ardrone emergency mode toggle
self.sub_nav = rospy.Subscriber( "ardrone/navdata", Navdata, self.updateNav )
self.sub_tags = rospy.Subscriber( "tags", Tags, self.updateTags ) # artoolkit
self.pub_cmdStatus = rospy.Publisher( "yudrone/cmdStatus", commandStatus ) # yudrone_cmd reports availability and status
self.sub_com = rospy.Subscriber( "yudrone/commands", commandsMsg, self.handle_command ) # yudrone_cmd listenes command messages
self.pub_twist = rospy.Publisher( "yudrone/cmd_vel", Twist ) # forwarded to ardrone navigation topic (except if joypad has control)
# init PIDs (reference, Kp, Ki, Kd, maxIntegral[=0.15/Ki])
self.PID_az = PID_controller(0.5, 0.4, 0.005, 1.0, 30) # yaw to tag controller
self.PID_lz = PID_controller(0.5, 0.4, 0.001, 1.0, 150) # elevate to tag controller
self.PID_ly = PID_controller(0.0, 0.1, 0.0005, 1.5, 300) # move perpendicular to tag controller
self.PID_lx = PID_controller(FACEDIST, 0.00005, 0.000005, 0.001, 500)# distance to tag controller
if flight == None:
# this code applies for usage WITHOUT gui
rospy.init_node('yudrone_cmd')
rospy.loginfo("init 'yudrone commands' stand-alone")
rospy.loginfo("make sure the following nodes are running: ardrone_driver, ar_recog")
self.hasGui = False
else:
# this code applies for usage WITH gui
rospy.loginfo("init 'yudrone commands' using gui")
self.hasGui = True
# stop moving and start naviagtion loop
self.__reset_twist(0.1)
self.navTimer = rospy.Timer(rospy.Duration(1.0/NAVRATE), self.__onNavigate)
def __onNavigate(self, event = None):
'''
this function is contigiously called by a timer
rate = NAVRATE
'''
# caculate values
twist = Twist()
twist.angular.x = self.__aim['ax']
twist.angular.y = self.__aim['ay']
twist.angular.z = self.__aim['az']
# p-controller
#if self.navdata.altd > 50:
#twist.linear.z = (self.__aim['lz'] - self.navdata.altd) /10
#rospy.loginfo('lz: ' + str(twist.linear.z))
twist.linear.z = self.__aim['lz']
twist.linear.x = self.__aim['lx']
twist.linear.y = self.__aim['ly']
# publish twist to ardrone
self.pub_twist.publish(twist)
def lockWarning(self):
'''
display log warning
anounce status on cmdStatus topic
Explanation: The node was locked and is supposed not to react on incomming naviagtion commands
'''
rospy.logwarn('Command was not executed, it is locked by (%i)'%self.__lockNr)
status = commandStatus(id=self.__lockNr, isLocked=True, status="running")
self.pub_cmdStatus.publish(status)
def __lock(self, lockNr):
'''
sets a lock flag, which blocks other commands to be executed
usage example:
from yudrone.msg import commandsMsg
pub=rospy.Publisher('yudrone_commands', commandsMsg)
m=commandsMsg()
m.hasYaw=True
m.yaw=500
pub.publish(m)
# Yaw 500
pub.publish(m) # after a second
# Yaw 500
pub.publish(m) # imediately after last command
# [WARN] [WallTime: 1363883870.378374] Command was not executed, it is locked by (2)
print(m.header.seq)
# 3 <-- this sequential number is set by publishing the message (not by instanciation!)
'''
if lockNr > 0:
if self.__lockNr == -1:
# if not already locked
self.__lockNr = lockNr
status = commandStatus(id=self.__lockNr, isLocked=True, status="running")
rospy.loginfo('CmdNode is locked by %i'%self.__lockNr)
self.pub_cmdStatus.publish(status)
else:
rospy.logerr('invalid lockNr: %i'%lockNr)
def __unlock(self, exitStatus=""):
'''
resets lock flag, allowing other commands to be executed
Parameter exitStatus:
if not empty, the current executed command is finished with the given status
'''
if exitStatus != "":
# announce exit of old command
status = commandStatus(id=self.__lockNr, isLocked=False, status=exitStatus)
self.__lockNr = -1
rospy.loginfo('Status turned to "%s"'%exitStatus)
self.pub_cmdStatus.publish(status)
elif self.__lockNr != -1:
status = commandStatus(id=self.__lockNr, isLocked=True, status="done")
rospy.loginfo('Status turned to "done" (%i)'%self.__lockNr)
# dissolve lock and announce availability
self.__lockNr = -1
status = commandStatus(id=-1, isLocked=False, status="available")
rospy.loginfo('Status turned to "available"')
self.pub_cmdStatus.publish(status)
def handle_command(self, msg):
'''
handle incomming commandsMsg
'''
# call command determined by hasXYZ
if msg.hasTwist == True:
rospy.loginfo('received twist command')
self.pub_twist.publish(msg.twist) # or better use funtion???
if msg.hasAltdDelta == True:
rospy.loginfo('received altitude command')
self.Altitude(delta=msg.altdDelta)
self.__lock(msg.header.seq) # lock instance
if msg.hasAltdAbs == True:
rospy.loginfo('received altitude command')
self.Altitude(absolute=msg.altdAbs)
self.__lock(msg.header.seq) # lock instance
if msg.hasMaxAltd == True:
rospy.loginfo('received max_altitude command')
self.MaxAltd(msg.MaxAltd)
if msg.hasMinAltd == True:
rospy.loginfo('received min_altitude command')
self.MinAltd(msg.MixAltd)
if msg.hasYaw == True:
rospy.loginfo('received yaw command (%i)'%msg.yaw)
self.Yaw(msg.yaw)
self.__lock(msg.header.seq) # lock instance
if msg.hasHorizontal == True:
rospy.loginfo('received horizontal command %i %i'%(msg.horizontalX, msg.horizontalY))
self.Horizontal(msg.horizontalX, msg.horizontalY)
self.__lock(msg.header.seq) # lock instance
if msg.hasFace == True:
rospy.loginfo('received face command (%i)'%msg.tagNr)
self.Face(msg.tagNr)
self.__lock(msg.header.seq) # lock instance
if msg.hasRelease == True:
rospy.loginfo('received release command')
self.Release()
if msg.hasSearch == True:
rospy.loginfo('received search command (%i)'%msg.tagNr)
self.Search(msg.tagNr, msg.searchStartAltd)
self.__lock(msg.header.seq) # lock instance
if msg.hasApproach == True:
rospy.loginfo('received approach command (%i)'%msg.tagNr)
self.Approach(msg.tagNr)
self.__lock(msg.header.seq) # lock instance
if msg.hasNavigate == True:
rospy.loginfo('received navigate command')
self.Navigate(msg.nav_XYZ_RxRyRz[0], msg.nav_XYZ_RxRyRz[1], msg.nav_XYZ_RxRyRz[2], msg.nav_XYZ_RxRyRz[3], msg.nav_XYZ_RxRyRz[4], msg.nav_XYZ_RxRyRz[5])
self.__lock(msg.header.seq) # lock instance
if msg.hasYawSpeed == True:
rospy.loginfo('received yawSpeed command')
self.YawSpeed(msg.yawSpeed)
if msg.hasHrzSpeed == True:
rospy.loginfo('received hrzSpeed command')
self.HrzSpeed(msg.hrzSpeed)
if msg.hasTakeoff == True:
rospy.loginfo('received takeoff command (%i)'%msg.tagNr)
self.TakeOff(msg.tagNr)
if msg.hasLand == True:
rospy.loginfo('received land command (%i)'%msg.tagNr)
self.Land(msg.tagNr)
# not implemented by now...
## ToggleEmerg(self)
## __reset_twist(delay = 0)
def updateNav(self, navdata):
'''
set navdata (from ardrone)
'''
self.navdata = navdata
def updateTags(self, tagMsg):
'''
set tags (from artoolkit)
'''
self.tagMsg = tagMsg
def updateResolution(self, height, width):
'''
change camera resolution
'''
rospy.loginfo('camera resolution set to %d x %d' %(width, height))
self.imgheight = height
self.imgwidth = width
'''****************************************************************************************************
* tresholds
****************************************************************************************************'''
def Altitude(self, delta=None, absolute=None, callback=None):
'''
Action:
In-/decreases the UAVs hovering altitude by a certain value.
The function uses the scale given by the IMUs altimeter.
Parameter:
delta [mm] (integer)
positive means up
'''
if self.__lockNr != -1:
self.lockWarning() # command is not executed, see function description
else:
# set designated altitude
if delta != None:
self.__aimedAltd = self.navdata.altd + delta
elif absolute != None:
self.__aimedAltd = absolute
delta = self.__aimedAltd - self.navdata.altd
else:
rospy.logerr('no altitude specified')
return
# adjust if altitude exceeds thresholds
if self.__aimedAltd < self.__minAltitude:
self.__aimedAltd = self.__minAltitude
elif self.__aimedAltd > self.__maxAltitude:
self.__aimedAltd = self.__maxAltitude
self.__altdDelta = delta
# lift ardrone by until in range of designated altitude
self.__aim['lz'] = 0.4 * math.fabs(delta)/delta
reading1 = self.navdata.altd
rospy.loginfo('current altitude = %i \t aimed altitude = %i' %(reading1, self.__aimedAltd))
# stop callback
if callback == None:
callback = self.__altdStop # default callback
rospy.Timer(rospy.Duration(1.0/CONTROLLRATE), callback, oneshot=True)
'''
this function is a callback to stop altitude changing after the designated value was reached
'''
def __altdStop(self, event=None):
# get two altitude readings
reading1 = self.navdata.altd
rospy.sleep(0.1)
reading2 = self.navdata.altd
rospy.loginfo('altitude %i'%reading1)
if ( (self.__altdDelta > 0 and reading1 > self.__aimedAltd) or # upwards
(self.__altdDelta <= 0 and reading1 < self.__aimedAltd) or # downwards
reading1 <= self.__minAltitude or # reached bottom
reading1 >= self.__maxAltitude ): # reached top
#stop lifting
rospy.loginfo('reached altitude @ %i'%reading1)
self.__onResetSilent()
self.__unlock("altitude command sucessfully")
else:
#call again
rospy.Timer(rospy.Duration(1.0/CONTROLLRATE), self.__altdStop, oneshot=True)
def __altdStopSearch(self, event=None):
# get two altitude readings
reading1 = self.navdata.altd
rospy.sleep(0.1)
reading2 = self.navdata.altd
rospy.loginfo('altitude %i'%reading1)
if ( (self.__altdDelta > 0 and reading1 > self.__aimedAltd) or # upwards
(self.__altdDelta <= 0 and reading1 < self.__aimedAltd) or # downwards
reading1 <= self.__minAltitude or # reached bottom
reading1 >= self.__maxAltitude ): # reached top
#stop lifting
rospy.loginfo('reached altitude @ %i'%reading1)
rospy.Timer(rospy.Duration(1.0/CONTROLLRATE), self.__onSearch, oneshot=True)
self.__onResetSilent()
else:
#call again if tag still lost
facedTag = self.getTag(self.__searchTagNr)
if facedTag.diameter != 0:
self.__onResetSilent()
self.__searchStepFinished = True
else:
rospy.Timer(rospy.Duration(1.0/CONTROLLRATE), self.__altdStopSearch, oneshot=True)
def MaxAltd(self, val):
'''
Action:
Sets threshold value. Once defined, thefunction will not exceed those values any more.
If not defined, the maximal altitude is limited by a firmware defined threshold.
Parameter:
val [mm]
'''
print('MaxAltd set to ' + str(val))
self.__maxAltitude = val
def MinAltd(self, val):
'''
Action:
Sets threshold values. Once defined, thefunction will not exceed those values any more.
If not defined, the maximal altitude is limited by the ground.
Parameter:
val [mm]
'''
print('MinAltd set to ' + str(val))
self.__minAltitude = val
'''****************************************************************************************************
* basic navigation
****************************************************************************************************'''
def __reset_twist(self, delay = 0):
'''
pushes a zero to yaw aim
'''
if delay == 0:
self.__onReset()
else:
self.__reset_twist_timer = rospy.Timer(rospy.Duration(delay), self.__onReset, oneshot=True)
def __onReset(self, event=None):
'''
resets twist (= stop moving)
+ loginfo and unlock yudrone/cmdStatus
'''
self.__aim['ax'] = self.__aim['ay'] = self.__aim['az'] = 0
self.__aim['lz'] = self.__aim['lx'] = self.__aim['ly'] = 0
self.PID_az.reset()
self.PID_lz.reset()
self.PID_ly.reset()
self.PID_lx.reset()
rospy.loginfo('Aim=(0,0,0,0,0,0)')
self.__unlock()
def __onResetSilent(self, event=None):
'''
resets twist (= stop moving)
without loginfo or unlock yudrone/cmdStatus
'''
self.__aim['ax'] = self.__aim['ay'] = self.__aim['az'] = 0
self.__aim['lz'] = self.__aim['lx'] = self.__aim['ly'] = 0
self.PID_az.reset()
self.PID_lz.reset()
self.PID_ly.reset()
self.PID_lx.reset()
self.__searchStepFinished = True
def Yaw(self, angle):
'''
Action:
Rotates the UAV by a specified angle (yaw).
The device will turn slowly until the designated angle is reached.
Therefore it accesses the rotation angle provided by the internal compass.
Parameter:
angle [deg] (integer) -180, ..., +180 negative is clockwise
'''
if self.__lockNr != -1:
self.lockWarning() # command is not executed, see function description
else:
print('Yaw ' + str(angle))
# set yaw parameter
self.__aim['az'] = angle/math.fabs(angle) * self.__yawSpeed
self.__reset_twist(math.fabs(angle)/(100*self.__yawSpeed) )
def YawSpeed(self, val):
'''
Action:
Sets the speed to be used performing Yaw().
Parameter:
val in [0..2]
'''
if val > 0.0 and val <=2.0:
self.__yawSpeed = val
print('YawSpeed set to ' + str(val))
else:
print('invalid YawSpeed ' + str(val))
def Horizontal(self, x, y):
'''
Action:
Moves the UAV a certain distance in a specified horizontal direction (see figure 1).
The distance cannot be determined exactly, but it is approximated by experienced data
and the horizontal speed.
Parameter:
x[cm], y[cm] (both integer)
'''
if self.__lockNr != -1:
self.lockWarning() # command is not executed, see function description
else:
print('Horizontal movement')
# set yaw parameter
vectLen = math.sqrt( x*x + y*y )
rospy.loginfo('vectlen' + str( vectLen))
self.__aim['lx'] = x/vectLen * self.__hrzSpeed
self.__aim['ly'] = y/vectLen * self.__hrzSpeed
rospy.loginfo('lx: ' + str(self.__aim['lx']))
self.__reset_twist(vectLen/100)
def HrzSpeed(self, val):
'''
Action:
Sets the speed to be used performing Horizontal().
Parameter:
val in [0..1]
'''
if val >0.0 and val <=1.0:
print('HrzSpeed set to ' + str(val))
self.__hrzSpeed = val
else:
print('invalid HrzSpeed ' + str(val))
def TakeOff(self, tagNr = None):
'''
Action:
Triggers built-in takeoff command.
'''
if tagNr == None:
print('TakeOff cmd')
self.pub_takeoff.publish( Empty() )
else:
print('Starting above tag %i __unimplemented'%tagNr)
# PID controller on lx and ly (propably different from used ones)
self.pub_takeoff.publish( Empty() )
def Land(self, tagNr = -1):
'''
Action:
Triggers built-in land command.
'''
if tagNr == -1:
print('Land cmd')
self.pub_land.publish( Empty() )
else:
print('Landing on tag %i __unimplemented'%tagNr)
# PID controller on lx and ly (propably different from used ones)
self.pub_land.publish( Empty() )
def ToggleEmerg(self):
'''
Action:
Toggles the emergency mode of Ardrone. Red LEDs indicate emergency mode, where all
rotors are stopped. Without emergency these LEDs show up green.
'''
print('Emegr cmd')
self.pub_emergency.publish( Empty() )
'''****************************************************************************************************
* Batch
****************************************************************************************************'''
def Pause(self):
'''
Action:
Interrupts the current command. Any incoming command will be ignored during pause.
'''
print('Pause cmd' + ' __unimplemented')
def Continue(self):
'''
Action:
Continues execution of current command and exits pause mode.
'''
print('Continue cmd' + ' __unimplemented')
def Break(self):
'''
Action:
Same as pause, but additionally the current action will be dismissed.
'''
print('Break cmd' + ' __unimplemented')
def Batch(self, fileName):
'''
Action:
Runs the specified batch-file.
Parameter:
fileName (string)
'''
print('Run ' + str(fileName) + ' __unimplemented')
def Exit(self):
'''
Action:
Cancels a batch run.
'''
print('Cancel cmd' + ' __unimplemented')
'''****************************************************************************************************
* high level navigation
****************************************************************************************************'''
def Face(self, tagNr, faceYaw = True, faceDist = True, faceAlt = False, facePerpend = True):
'''
Action:
Faces a specified tag. As long as this tag is visible in the field of view, the Ardrone will
automatically yaw towards the target. This function overrides yaw control, whereas
horizontal motion is not affected. Whenever the system loses the target, it will prompt a warning.
Parameter:
tagNr (integer)
'''
if self.__lockNr != -1:
self.lockWarning() # command is not executed, see function description
else:
print("Faceing %s"%PATTERNS[tagNr])
self.__facedTagNr = tagNr
self.__faceLostCounter = 0
self.__faceYaw = faceYaw
self.__facePerpend = facePerpend
self.__faceDist = faceDist
self.__faceAlt = faceAlt
#self.__faceState = 0 # do one step at a time
#self.__faceStateCounter = 0
# start facing
self.__onFace()
def getTag(self, tagNr):
# select tag
facedTag = Tag()
for tag in self.tagMsg.tags:
if (tag.id == tagNr):
facedTag = tag
return facedTag
def getCentre(self, tag):
# find tag centre in relation to screen centre
cx = 0; cy = 0
for i in [0,2,4,6]:
cx = cx + tag.cwCorners[i]
cy = cy + tag.cwCorners[i+1]
cx = cx / 4. / self.imgwidth
cy = cy / 4. / self.imgheight
return {'x':cx, 'y':cy}
def __onFace(self, event=None):
'''
callback for Face(..) [which only initializes facing]
performs face until 'Release()' or tag lost for longer time
'''
facedTag = self.getTag(self.__facedTagNr)
if facedTag.diameter == 0:
# designated tag is not in range
self.__onResetSilent(None) # stop moving (silent)
if self.__reset_twist_timer.isAlive():
# stop last running timer
self.__reset_twist_timer.shutdown()
self.__faceLostCounter += 1
if self.__faceLostCounter % int(CONTROLLRATE) == 0: # 10Hz
rospy.logwarn('tag not in range')
if self.__faceLostCounter > CONTROLLRATE*FACELOSTTIME: # after FACELOSTTIME seconds
rospy.logerr('tag is lost for %i seconds, faceing canceled'%FACELOSTTIME)
self.Release()
else: # designated tag is in range
# face
if self.__faceLostCounter > 3: # got lost tag back
rospy.loginfo('tag in range')
self.__searchState = -1# reset search state
self.__faceState = 0 # reset face state
self.__faceStateCounter = 0
self.__onSearch() # will stop search. because stage=-1
#rospy.loginfo('Face-state = ' + FACESTATES[self.__faceState])
self.__faceLostCounter = 0
# find tag centre in relation to screen centre
centre = self.getCentre(facedTag)
cx = centre['x']
cy = centre['y']
# rotate to face the tag
if self.__faceYaw == True:# and self.__faceState == 0:
self.__aim['az'] = self.PID_az.update(cx)
# elevate to face tag
if self.__faceAlt == True:# and self.__faceState == 1:
if self.__maxAltitude > self.navdata.altd and \
(self.__minAltitude < self.navdata.altd or \
self.navdata.altd == 0): # if altitude is within tresholds or UAV on ground (during dry tests)
self.__aim['lz'] = self.PID_lz.update(cy)
else:
self.__aim['lz'] = 0.0
# move perpendicular to tag [operates on two dimensions]
if self.__facePerpend == True:# and self.__faceState == 2:
self.__aim['ly'] = - self.PID_ly.update(facedTag.yRot)
#self.__aim['az'] = self.PID_az.update(cx)
else:
self.__aim['ly'] = 0.0
#self.__aim['az'] = 0.0
# move to certain distance of tag
if self.__faceDist == True:# and self.__faceState == 3:
#deltaDist = facedTag.distance - FACEDIST
#if abs(deltaDist) > 100:
self.__aim['lx'] = - self.PID_lx.update(facedTag.distance)
#else:
# self.__aim['lx'] = 0
# update facing state
#self.__faceStateCounter = self.__faceStateCounter + 1
#if abs(self.__aim['az']) < 0.02 and self.__faceState == 0 or \
#abs(self.__aim['lz']) < 0.02 and self.__faceState == 1 or \
#abs(self.__aim['ly']) < 0.02 and self.__faceState == 2 or \
#abs(self.__aim['lx']) < 0.02 and self.__faceState == 3 or \
#self.__faceStateCounter > int(CONTROLLRATE*STATEDURATION[0]) and self.__faceState == 0 or \
#self.__faceStateCounter > int(CONTROLLRATE*STATEDURATION[1]) and self.__faceState == 1 or \
#self.__faceStateCounter > int(CONTROLLRATE*STATEDURATION[2]) and self.__faceState == 2 or \
#self.__faceStateCounter > int(CONTROLLRATE*STATEDURATION[3]) and self.__faceState == 3:
## eiter if one of the states is done, or if too long in one state
##rospy.loginfo('Face-state = ' + FACESTATES[self.__faceState])
#self.__faceState = (self.__faceState+1) % 4
#self.__faceStateCounter = 0
#self.__onResetSilent(None) # stop moving (silent)
# sum up correction vectors
correctionSum = 0.0
for x in self.__aim.values():
correctionSum = correctionSum + abs(x)
rospy.loginfo('correctionSum (%3f)'%correctionSum)
if correctionSum < FACESTOPTHRESHOLD and \
self.__lastFaceCorrectionSums[0] < FACESTOPTHRESHOLD and \
self.__lastFaceCorrectionSums[1] < FACESTOPTHRESHOLD and \
self.__lastFaceCorrectionSums[2] < FACESTOPTHRESHOLD:
status = commandStatus(id=self.__lockNr, isLocked=True, status="face stable")
self.pub_cmdStatus.publish(status)
self.__onResetSilent()
self.__lastFaceCorrectionSums[2] = self.__lastFaceCorrectionSums[1]
self.__lastFaceCorrectionSums[1] = self.__lastFaceCorrectionSums[0]
self.__lastFaceCorrectionSums[0] = correctionSum
# // end designated tag in range
# keep faceing
if self.__facedTagNr > -1:
# call again
rospy.Timer(rospy.Duration(1.0/CONTROLLRATE), self.__onFace, oneshot=True)
def faceBelow(self, tagNr):
'''
Action:
Faces a specified tag with bottom camera
Parameter:
tagNr (integer)
'''
if self.__lockNr != -1:
self.lockWarning() # command is not executed, see function description
else:
print("Faceing %s"%PATTERNS[tagNr])
self.__facedTagNr = tagNr
self.__faceLostCounter = 0
# start facing
self.__onFaceBelow()
def __onFaceBelow(self, event=None):
'''
callback for FaceBelow(..)
performs face until 'Release()' or tag lost for longer time
'''
facedTag = self.getTag(self.__facedTagNr)
self.__aim['ly'] = 0.0
self.__aim['lx'] = 0.0
if facedTag.diameter == 0:
# designated tag is not in range
self.__onResetSilent(None) # stop moving (silent)
if self.__reset_twist_timer.isAlive():
# stop last running timer
self.__reset_twist_timer.shutdown()
self.__faceLostCounter += 1
if self.__faceLostCounter % int(CONTROLLRATE) == 0: # 10Hz
rospy.logwarn('tag not in range')
if self.__faceLostCounter > CONTROLLRATE*FACELOSTTIME: # after FACELOSTTIME seconds
rospy.logerr('tag is lost for %i seconds, faceing canceled'%FACELOSTTIME)
self.Release()
else: # designated tag is in range
# face
if self.__faceLostCounter > 3: # got lost tag back
rospy.loginfo('tag in range')
self.__faceLostCounter = 0
# find tag centre in relation to screen centre
centre = self.getCentre(facedTag)
cx = centre['x']
cy = centre['y']
self.__aim['ly'] = self.PID_az.update(cx)
self.__aim['lx'] = self.PID_lz.update(cy)
# keep faceing
if self.__facedTagNr > -1:
# call again
rospy.Timer(rospy.Duration(1.0/CONTROLLRATE), self.__onFaceBelow, oneshot=True)
def Release(self):
'''
Action:
stops faceing of tag (specified by Face(tagNr))
Parameter:
tagNr (integer)
'''
self.__unlock("released")
self.__searchState = -1
if self.__facedTagNr > -1:
print('released tag nr %i', self.__facedTagNr)
self.__facedTagNr = -1
def Search(self, tagNr, startAltd=0):
'''
Action:
Ardrone will yaw slowly, until the specified tag [direction (string)] appears. Overrides all movement controls.
If it rotated full 360 without finding the target, a warning is prompted.
[The optional parameter direction may be set "-",changing the direction to "clockwise"]
Parameter:
tagNr (integer)
startAltd (integer) if other than 0, drone will start search at given altitude
'''
if self.__lockNr != -1:
self.lockWarning() # command is not executed, see function description
else:
print('Searching ' + str(tagNr))
self.__searchTagNr = tagNr
self.__searchState = 0
self.__searchStepFinished = True
self.___searchTagNr = tagNr
if startAltd != 0:
print('approaching start altitude %i'%startAltd)
self.__searchStepFinished = False
self.Altitude(absolute = startAltd, callback = self.__altdStopSearch)
self.__searchRecallTimer = rospy.Timer(rospy.Duration(1.0/CONTROLLRATE), self.__onSearch, oneshot=True)
def __onSearch(self, event=None):
facedTag = self.getTag(self.__searchTagNr)
if facedTag.diameter != 0:
# search was successful
self.__unlock("search successful")
rospy.loginfo("search successful")
self.__searchState = -1
self.__searchTagNr = -1
if self.__searchState == -1 or self.__searchTagNr == -1 :
# no reason to search
self.__unlock()
self.__searchState = 0 # reset search parameter
elif self.__searchStepFinished == False or self.__searchRecallTimer.isAlive():
# wait for step to be done
rospy.Timer(rospy.Duration(1.0/CONTROLLRATE), self.__onSearch, oneshot=True)
else:
# keep searching
if self.__searchStepFinished == True: # only if last step finished
rospy.loginfo('performing search stage %i: %s'%(self.__searchState, SEARCH[self.__searchState]))
if self.__reset_twist_timer.isAlive(): # shutdown 'old' timer if still alive (to avoid __onReset by old timer)
self.__reset_twist_timer.shutdown()
# perform next move using searchstate values
yawAngle = SEARCH[self.__searchState][0]
altdDelta = SEARCH[self.__searchState][1]
if yawAngle != 0.0:
# yaw given angle
self.__aim['az'] = yawAngle/math.fabs(yawAngle) * self.__yawSpeed
self.__searchStepFinished = False
durationRecall = rospy.Duration(math.fabs(yawAngle)/(100*self.__yawSpeed) + 1.0) # waiting as long as turn lasts (same code as in Yaw +1.0s)
durationStop = rospy.Duration(math.fabs(yawAngle)/(100*self.__yawSpeed)) # waiting as long as turn lasts (same code as in Yaw)
# stop moving
rospy.Timer(durationStop, self.__onResetSilent, oneshot=True)
# call again
self.__searchRecallTimer = rospy.Timer(durationRecall, self.__onSearch, oneshot=True)
elif altdDelta != 0.0:
# lift given delta altitude
self.__searchStepFinished = False
self.Altitude(delta = altdDelta, callback = self.__altdStopSearch)
# update search state
self.__searchState = (self.__searchState + 1)
if self.__searchState >= SEARCHSTEPS:
# search was not successful
rospy.loginfo('search ended')
self.__unlock("search not successful")
self.__searchState = -1
self.__searchTagNr = -1
self.__onReset()
def Approach(self, tagNr):
'''
Action:
Ardrone will automatically search and approach the specified tag as described in 3.2.
Overrides all movement controls.
Parameter:
tagNr (integer)
'''
if self.__lockNr != -1:
self.lockWarning() # command is not executed, see function description
else:
print('Approaching ' + str(tagNr))
self.__approachTagNr = tagNr
self.__lastApproachTagDistance = 99999999
self.__approachTagLostCounter = 0
#self.__approachStep = 0
rospy.Timer(rospy.Duration(1.0/CONTROLLRATE), self.__onApproach, oneshot=True)
def __onApproach(self, event=None):
# step 0: gently comming closer ........................
#if self.__approachStep == 0:
facedTag = self.getTag(self.__approachTagNr)
if facedTag.diameter == 0:
# designated tag is not in range
self.__onResetSilent()
self.__approachTagLostCounter = self.__approachTagLostCounter + 1
rospy.loginfo("tag lost")
else:
# designated tag is in range
self.__approachTagLostCounter = 0
self.__lastApproachTagDistance = facedTag.distance
# find tag centre in relation to screen centre
centre = self.getCentre(facedTag)
# rotate to face the tag
self.__aim['az'] = self.PID_az.update(centre['x'])
self.__aim['ly'] = - self.PID_ly.update(facedTag.yRot)
self.__aim['lx'] = 0.04
if self.__approachTagLostCounter > 2:
# tag is lost for some frames
rospy.loginfo("Approach stopped. Eastimated distance is %i"%self.__lastApproachTagDistance)
#self.__approachStep = 1
self.__onResetSilent()
self.__unlock("approach done")
else:
# call again
rospy.Timer(rospy.Duration(1.0/CONTROLLRATE), self.__onApproach, oneshot=True)
# step 1: go on top of the box
#if self.__approachStep == 1:
# self.__aim['lx'] = 0.1
# self.__aim['']
def Navigate(self, x, y, z, xRot, yRot, zRot):
'''
Action:
Performs a graph-based navigation as described in 3.3. Overrides all movement controls.
Thisfunction it will require an initialization of the graph environment.
Parameter:
x, y, z,xRot, yRot, zRot (all integer)
'''
if self.__lockNr != -1:
self.lockWarning() # command is not executed, see function description
else:
print('Navigate cmd' + ' __unimplemented')
