예제 #1
0
    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'
예제 #2
0
 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
예제 #3
0
    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'
예제 #4
0
    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'
예제 #5
0
    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'
예제 #6
0
 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