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()
# TODO instantiate these correctly i.e. correct parameters # Land Mode Instantiation land_speed = -1 # m/s land_min_alt = 1000 # mm land_max_alt = 2000 # mm land_height_diff = 0 #mm land_timeout = 3 # seconds land = Landing(land_speed, land_min_alt, land_height_diff, land_max_alt, land_timeout) # Takeoff Mode Instantiation takeoff_speed = 1 # percentage of max speed (kind of) takeoff_max_alt = 2500 # mm takeoff_timeout = 10 # seconds takeoff_counter = 0 # times takeoff_to_reac_timeout = 15 # seconds takeoff = Takeoff(takeoff_speed, takeoff_max_alt, takeoff_timeout) # Takeoff Mode Instantiation emergency = Emergency() # Take Picture Mode Instantiation takepicture_time = 300 # seconds # takepicture_max_time = 3 # seconds takepicture = TakePicture(takepicture_time) # Reacquisition Mode Instantiation reac_velocities = (0.3, 0.3, 0.3) # m/s reac_max_alt = 3000 # mm reac_tag_timeout = 15 # seconds reac_prev_state_timeout = 5 # seconds reac = Reacquisition(reac_velocities, reac_max_alt, reac_tag_timeout, reac_prev_state_timeout)
# Land Mode Instantiation land_speed = -1 # m/s land_min_alt = 1000 # mm land_max_alt = 2000 # mm land_height_diff = 0 #mm land_timeout = 3 # seconds land = Landing(land_speed, land_min_alt, land_height_diff, land_max_alt, land_timeout) # Takeoff Mode Instantiation takeoff_speed = 1 # percentage of max speed (kind of) takeoff_max_alt = 2500 # mm takeoff_timeout = 10 # seconds takeoff_counter = 0 # times takeoff_to_reac_timeout = 15 # seconds takeoff = Takeoff(takeoff_speed, takeoff_max_alt, takeoff_timeout) # Takeoff Mode Instantiation emergency = Emergency() # Take Picture Mode Instantiation takepicture_time = 300 # seconds # takepicture_max_time = 3 # seconds takepicture = TakePicture(takepicture_time) # Reacquisition Mode Instantiation reac_velocities = (0.3, 0.3, 0.3) # m/s reac_max_alt = 3000 # mm reac_tag_timeout = 15 # seconds reac_prev_state_timeout = 5 # seconds reac = Reacquisition(reac_velocities, reac_max_alt, reac_tag_timeout,