def execute(self, userdata):
userdata.tagNr = self.getTagNr()
userdata.lastCmdStatus = self.lastCmdStatus
self.checkCmdNode()
# execute face command
if self.faceIsRunning == False and self.isFacing_stable == False and self.isLost == False:
m=commandsMsg()
m.hasFace=True
m.tagNr=self.getTagNr()
self.pubCommands.publish(m)
userdata.lastCmd = m
self.commandID = m.header.seq
rospy.loginfo("face command submitted: id = %i"%self.commandID)
self.faceIsRunning = True
if self.faceIsRunning == True and self.isFacing_stable == False:
rospy.loginfo("still attempting to face tag nr %i"%self.getTagNr())
rospy.sleep(0.5)
if self.isLost == True:
self.clean()
return 'lost'
elif self.isFacing_stable == True:
#send out release message
m = commandsMsg()
m.hasRelease = True
self.pubCommands.publish(m)
userdata.lastCmd = m
self.clean()
return 'done'
else:
return 'control'
def __init__(self):
ydState.__init__(self)
smach.State.__init__(self, outcomes=['lost','done', 'control'], output_keys=['tagNr', 'lastCmd', 'lastCmdStatus'])
self.clean()
m = commandsMsg()
self.pubCommands.publish(m) # send empty message as initialization
def execute(self, userdata):
userdata.tagNr = self.getTagNr()
userdata.lastCmdStatus = self.lastCmdStatus
self.checkCmdNode()
self.hasAcquiredNode = False
self.giveUp = False
# execute search command
if self.searchIsRunning == False and self.hasAcquiredNode == False and self.giveUp == False:
rospy.sleep(0.5)
m=commandsMsg()
m.hasSearch=True
m.tagNr=self.getTagNr()
m.searchStartAltd = self.getTagAltd()
self.pubCommands.publish(m)
userdata.lastCmd = m
self.commandID = m.header.seq
rospy.loginfo("search command submitted: id = %i"%self.commandID)
self.searchIsRunning = True
if self.searchIsRunning == True and self.hasAcquiredNode == False:
rospy.loginfo("still searching for tag nr %i"%self.getTagNr())
rospy.sleep(0.5)
if self.hasAcquiredNode == True:
self.clean()
return 'acquired_node'
elif self.giveUp == True:
self.clean()
return 'gave_up'
else:
return 'still_lost'
def execute(self, userdata):
userdata.tagNr = self.getTagNr()
userdata.lastCmdStatus = self.lastCmdStatus
self.checkCmdNode()
if False:
m=commandsMsg()
m.hasLand=True
self.pubCommands.publish(m)
userdata.lastCmd = m
return 'landing'
else:
self.clean()
return 'landed'
def execute(self, userdata):
userdata.tagNr = self.getTagNr()
userdata.lastCmdStatus = self.lastCmdStatus
self.checkCmdNode()
if self.approachState == "initialized": # call approach
self.approachState = "approach running"
m=commandsMsg()
m.hasApproach=True
m.tagNr=self.getTagNr()
self.pubCommands.publish(m)
userdata.lastCmd = m
self.commandID = m.header.seq
rospy.loginfo("approach command submitted: id = %i"%self.commandID)
elif self.approachState == "approach done": # call altitude
self.approachState = "altitude running"
m=commandsMsg()
m.hasAltdDelta=True
m.altdDelta=400
self.pubCommands.publish(m)
userdata.lastCmd = m
self.commandID = m.header.seq
rospy.loginfo("altitude command submitted: id = %i"%self.commandID)
elif self.approachState == "altitude done": # call horizontal
self.approachState = "horizontal running"
m=commandsMsg()
m.hasHorizontal=True
m.horizontalX=50
m.horizontalY=0
self.pubCommands.publish(m)
userdata.lastCmd = m
self.commandID = m.header.seq
rospy.loginfo("horizontal command submitted: id = %i"%self.commandID)
elif self.approachState == "horizontal done":
self.clean()
return 'in_range'
rospy.loginfo("Approchestate = %s"%self.approachState)
#rospy.sleep(0.2)
return 'still_tracking'
def __init__(self):
ydState.__init__(self)
smach.State.__init__(self, outcomes=['still_lost','acquired_node', 'gave_up'], output_keys=['tagNr', 'lastCmd', 'lastCmdStatus'])
self.clean()
m = commandsMsg()
self.pubCommands.publish(m) # send empty message as initialization
