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 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 __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 __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)