def main():
speed = 0.75 # m/s
altitude_goal = 2.5 # meters
tag_timeout = 10 # seconds
takeoff = Takeoff(speed, altitude_goal, tag_timeout)
rate = rospy.Rate(100) # 100Hz
# To let us know that the mode is working
rospy.loginfo("Started Takeoff Mode")
while ((takeoff.state != 'takeoff')):
# print takeoff.state
rate.sleep()
# To get this guy to take off! For some reason just calling this function
# once does not work. This value (50) was determined experimentally
i = 0
while i < 50:
takeoff.launch()
i += 1
rate.sleep()
while not rospy.is_shutdown():
# We only want to execute these manuevers if we're in takeoff mode
if takeoff.state == 'takeoff':
if (takeoff.altitude < altitude_goal):
rospy.loginfo("Go up!")
takeoff.change_altitude()
elif (takeoff.altitude >= altitude_goal):
speed = 0
rospy.loginfo("Stop!")
# takeoff.change_altitude(speed)
# To change states, we publish the fact that we've
# reached our takeoff altitude
rospy.loginfo("Going to follow mode")
takeoff.goto_follow()
# Let's change the launch file and then we can
# break
# out from the loop
# else:
# continue
rate.sleep()
def main():
speed = 0.75 # m/s
altitude_goal = 2.5 # meters
tag_timeout = 10 # seconds
takeoff = Takeoff(speed, altitude_goal, tag_timeout)
rate = rospy.Rate(200) # 100Hz
# To let us know that the mode is working
rospy.loginfo("Started Takeoff Mode")
while((takeoff.state != 'takeoff')):
# print takeoff.state
rate.sleep()
# To get this guy to take off! For some reason just calling this function
# once does not work. This value (50) was determined experimentally
i = 0
while i < 50:
takeoff.launch()
i += 1
rate.sleep()
while not rospy.is_shutdown():
# We only want to execute these manuevers if we're in takeoff mode
if takeoff.state == 'takeoff':
if(takeoff.altitude < altitude_goal):
rospy.loginfo("Go up!")
takeoff.change_altitude()
elif(takeoff.altitude >= altitude_goal):
speed = 0
rospy.loginfo("Stop!")
# takeoff.change_altitude(speed)
# To change states, we publish the fact that we've
# reached our takeoff altitude
rospy.loginfo("Going to follow mode")
takeoff.goto_follow()
# Let's change the launch file and then we can
# break
# out from the loop
# else:
# continue
rate.sleep()
if (state == 'takeoff'):
print "Takeoff Mode"
# NOTE: This was copy and pasted from the takeoff node
# To get this guy to take off! For some reason just calling this
# function once does not work. This value (50) was determined
# experimentally
# TODO: Turn on takeoff mode's timer and turn off every other mode's
# timers EXCEPT take picture
takeoff.turn_on_timer(takeoff.timer, 'Takeoff')
land.turn_off_timer(land.timer, 'Land')
reac.turn_off_timer(reac.prev_state_timer, 'Reac prev state')
reac.turn_off_timer(reac.land_timer, 'Reac land')
# print "Turned on Takeoff Timer"
# print "Turned off Land, Previous State, and Land timers"
while takeoff_counter < 50:
takeoff.launch()
takeoff_counter += 1
rate.sleep()
if(takeoff.altitude < takeoff.max_altitude):
rospy.loginfo("Go up!")
takeoff.change_altitude(takeoff_speed)
elif(takeoff.altitude >= takeoff.max_altitude):
takeoff_speed = 0
rospy.loginfo("Stop!")
# takeoff.change_altitude(takeoff_speed)
# To change states, we publish the fact that we've
# reached our takeoff altitude
rospy.loginfo("Going to follow mode")
transition = 'TAKEOFF_ALT_REACH'
# takeoff.goto_follow()
if (state == 'takeoff'):
print "Takeoff Mode"
# NOTE: This was copy and pasted from the takeoff node
# To get this guy to take off! For some reason just calling this
# function once does not work. This value (50) was determined
# experimentally
# TODO: Turn on takeoff mode's timer and turn off every other mode's
# timers EXCEPT take picture
takeoff.turn_on_timer(takeoff.timer, 'Takeoff')
land.turn_off_timer(land.timer, 'Land')
reac.turn_off_timer(reac.prev_state_timer, 'Reac prev state')
reac.turn_off_timer(reac.land_timer, 'Reac land')
# print "Turned on Takeoff Timer"
# print "Turned off Land, Previous State, and Land timers"
while takeoff_counter < 50:
takeoff.launch()
takeoff_counter += 1
rate.sleep()
if (takeoff.altitude < takeoff.max_altitude):
rospy.loginfo("Go up!")
takeoff.change_altitude(takeoff_speed)
elif (takeoff.altitude >= takeoff.max_altitude):
takeoff_speed = 0
rospy.loginfo("Stop!")
# takeoff.change_altitude(takeoff_speed)
# To change states, we publish the fact that we've
# reached our takeoff altitude
rospy.loginfo("Going to follow mode")
transition = 'TAKEOFF_ALT_REACH'
# takeoff.goto_follow()
