def main():
global local_Pose, local_Vel #, read_Position
#Use ENU coords
#allow loiter mode to be enabled by pressing the enter key twice
loiterThread = threading.Thread(name='emergency_Loiter',
target=emergency_Loiter)
loiterThread.start()
#rospy.Subscriber("/mavros/global_position/global", NavSatFix, position_callback)
rospy.Subscriber("/dGPS/Position", PoseStamped, local_Pos_Callback)
rospy.Subscriber("/dGPS/Velocity", TwistStamped, local_Vel_Callback)
#allow mode change to LOITER so that the quadrotor maintains its final position.
rospy.wait_for_service("mavros/set_mode")
modeSet = rospy.ServiceProxy("mavros/set_mode", SetMode)
pub_Position = rospy.Publisher("/mavros/setpoint_raw/local",
PositionTarget,
queue_size=0)
time.sleep(.1)
list_Heights = []
list_Norths = []
list_Easts = []
while len(list_Heights) < 10:
time.sleep(0.11)
if local_Pose.header.frame_id == 1 and local_Pose.pose.position.z != list_Heights[
len(list_Heights) - 1]:
list_Heights.append(local_Pose.pose.position.z)
list_Norths.append(local_Pose.pose.position.y)
list_Easts.append(local_Pose.pose.position.x)
else:
print "Non-Fixed or non unique solution. Waiting for fix"
list_Heights = []
list_Norths = []
list_Easts = []
#Set home position
home_Position = PoseStamped()
home_Position.pose.position.z = numpy.mean(list_Heights)
home_Position.pose.position.y = numpy.mean(list_Norths)
home_Position.pose.position.x = numpy.mean(list_Easts)
travel_Height = 20
max_North = 130
#Assign tuple with maximum allowable altitude
ground_Level = (home_Position.pose.position.z, )
print "The home position is: ", home_Position
#Display takeoff waypoint to user for 1 second
takeoff_Waypoint = set_Local_Waypoint(0, 0, travel_Height, 0, 0, 2, 0)
print takeoff_Waypoint
time.sleep(1)
#Waypoint has to be sent to FCU before mode can be changed to OFFBOARD.
i = 0
while i < 100:
takeoff_Waypoint = set_Local_Waypoint(0, 0, travel_Height, 0, 0, 2, 0)
pub_Position.publish(takeoff_Waypoint)
time.sleep(0.01)
i = i + 1
print i
#Set mode variable to OFFBOARD
mode_List = [0, "OFFBOARD"]
#Call function to set mode and arm motor
quad_Command(mode_List, True)
#take off to requested height
#change to use FCU position and velocity. Will add delay, but is safer
while (local_Pose.pose.position.z - ground_Level[0]
) < .95 * travel_Height or (local_Pose.pose.position.z -
ground_Level[0]) > 1.05 * travel_Height:
if local_Pose.header.frame_id == 1:
if exitFlag == 0:
modeSet(0, "OFFBOARD")
time.sleep(0.1)
else:
time.sleep(3)
sys.exit()
takeoff_Waypoint = set_Local_Waypoint(0, 0, travel_Height, 0, 0, 2,
0)
pub_Position.publish(takeoff_Waypoint)
time.sleep(0.2)
height = local_Pose.pose.position.z - ground_Level[0]
print "Taking off. The height is: ", height
else:
print "Lost fixed connection"
modeSet(0, "AUTO.LOITER")
time.sleep(0.2)
print "The desired height has been reached: ", local_Pose.pose.position.z - ground_Level[
0]
time.sleep(0.4)
#Set first waypoint and send to quadrotor at 10 Hz
time0 = time.time()
#fly for 15 seconds to build up speed
#while abs(local_Vel.twist.linear.x) < 6.5 and time.time() - time0 < 10:
while time.time() - time0 < 15:
first_Waypoint = set_Local_Waypoint(0, max_North, travel_Height, 0, 8,
0, 0)
pub_Position.publish(first_Waypoint)
time.sleep(0.1)
print "Distance North of home.", local_Pose.pose.position.y - home_Position.pose.position.y
time1 = time.time()
#check to allow turn. if none, don't turn. it !none, turn
north_Pos = None
while time.time() - time1 < 5:
#Commented due to faulty dGPS data
#desired_North = local_Pose.pose.position.x - home_Position.pose.position.x + 10
if local_Pose.header.frame_id == 1:
north_Pos = local_Pose.pose.position.y - home_Position.pose.position.y
print "Fixed RTK solution. Turning enabled!!"
break
else:
first_Waypoint = set_Local_Waypoint(0, max_North, travel_Height, 0,
8, 0, 0)
pub_Position.publish(first_Waypoint)
time.sleep(0.1)
print "Distance North of home.", local_Pose.pose.position.y - home_Position.pose.position.y
#if frame_id == 1, execute turn. Otherwise, go to loiter mode and exit the program.
#north_Pos will have a numerical value if the turn should be executed
if north_Pos == None:
print "Turn failed. Execute RTL"
modeSet(0, "AUTO.LOITER")
time.sleep(5)
sys.exit()
#RTH can be enabled by using the GCS or the RC controller.
else:
desired_East = 15
turn_Waypoint = set_Local_Waypoint(desired_East, north_Pos,
travel_Height, 8, 0, 0, 0)
pub_Position.publish(turn_Waypoint)
time.sleep(0.1)
time2 = time.time()
while time.time() - time2 < 12:
desired_East = desired_East + 0.75
turn_Waypoint = set_Local_Waypoint(desired_East, north_Pos,
travel_Height, 8, 0, 0, 0)
pub_Position.publish(turn_Waypoint)
time.sleep(0.1)
print "Distance East: ", local_Pose.pose.position.x
modeSet(0, "AUTO.LOITER")
time.sleep(5)
def main():
global local_Pose, local_Vel
rospy.Subscriber("/dGPS/Position", PoseStamped, local_Pos_Callback)
rospy.Subscriber("/dGPS/Velocity", TwistStamped, local_Vel_Callback)
pub_Position = rospy.Publisher("/mavros/setpoint_raw/local",
PositionTarget,
queue_size=0)
time.sleep(.2)
#Set home position
home_Position = local_Pose
travel_Height = 20
#Assign tuple with maximum allowable altitude
max_Height = (home_Position.pose.position.z + 50, )
ground_Level = (local_Pose.pose.position.z, )
print "The home position is: ", home_Position
#Display takeoff waypoint to user for 1 second
takeoff_Waypoint = set_Local_Waypoint(0, 0, 10, 0.01, 0.01, 2, 0)
print takeoff_Waypoint
time.sleep(1)
#Waypoint has to be sent to FCU before mode can be changed to OFFBOARD.
i = 0
while i < 100:
takeoff_Waypoint = set_Local_Waypoint(0, 0, 10, 0.01, 0.01, 2, 0)
pub_Position.publish(takeoff_Waypoint)
time.sleep(0.01)
i = i + 1
print i
#Set mode variable to OFFBOARD
mode_List = [0, "OFFBOARD"]
#Call function to set mode and arm motor
quad_Command(mode_List, True)
#take off to requested height
while (local_Pose.pose.position.z -
ground_Level[0]) < .95 * int(travel_Height):
takeoff_Waypoint = set_Local_Waypoint(0, 0, travel_Height, 0.01, 0.01,
2, 0)
pub_Position.publish(takeoff_Waypoint)
time.sleep(0.1)
height = local_Pose.pose.position.z - ground_Level[0]
print "Taking off. The height is: ", height
print "The desired height has been reached: ", local_Pose.pose.position.z - ground_Level[
0]
time.sleep(0.4)
#set value for takeoff position - might be slightly off from iris home position
zero_East = local_Pose.pose.position.y - home_Position.pose.position.y
#Set first waypoint and send to quadrotor at 10 Hz
time0 = time.time()
while local_Vel.twist.linear.x < 4.5 and time.time() - time0 < 10:
first_Waypoint = set_Local_Waypoint(zero_North, 250, 10, 0, 10, 0, 0)
pub_Position.publish(first_Waypoint)
time.sleep(0.1)
print "Distance North of home.", (local_Pose.pose.position.x -
home_Position.pose.position.x)
#print "Recording data."
#Need to add calls to programs for collecting data here.
time1 = time.time()
while time.time() - time1 < 5:
desired_X = local_Pose.pose.position.x - home_Position.pose.position.x + 10
first_Waypoint = set_Local_Waypoint(zero_East, desired_X, 10, 0, 5, 0,
0)
pub_Position.publish(first_Waypoint)
time.sleep(0.1)
print "Distance North of home.", (local_Pose.pose.position.x -
home_Position.pose.position.x)
pos_X = (local_Pose.pose.position.x - home_Position.pose.position.x)
distance = 100
time2 = time.time()
#Set waypoint off to the side, and send at 10 Hz
while (local_Pose.pose.position.y - home_Position.pose.position.y
) < .95 * distance and time.time() - time2 < 10:
desired_Y = local_Pose.pose.position.y - home_Position.pose.position.y + 10
final_Waypoint = set_Local_Waypoint(desired_Y, pos_X, 10, 5, 0, 0,
4.71)
if i < 111:
print final_Waypoint
i = i + 1
print "Second Flight Waypoint Has Been Set."
pub_Position.publish(final_Waypoint)
time.sleep(0.1)
print "Distance North of home.", (local_Pose.pose.position.x -
home_Position.pose.position.x)
#Change mode to LOITER so that the quadrotor maintains its final position.
rospy.wait_for_service("mavros/set_mode")
modeSet = rospy.ServiceProxy("mavros/set_mode", SetMode)
modeSet(0, "AUTO.LOITER")
time.sleep(5)
def main():
global local_Pose, local_Vel, fcu_Pose, fcu_Vel, exitFlag #, read_Position
#Use ENU coords
#allow loiter mode to be enabled by pressing the enter key twice
loiterThread = threading.Thread(name='emergency_Loiter',
target=emergency_Loiter)
loiterThread.start()
fcu_Pose = PoseStamped()
rospy.Subscriber("/mavros/local_position/pose", PoseStamped,
fcu_Pos_Callback)
rospy.Subscriber("/mavros/local_position/velocity", TwistStamped,
fcu_Vel_Callback)
#allow mode change to LOITER so that the quadrotor maintains its final position.
rospy.wait_for_service("mavros/set_mode")
modeSet = rospy.ServiceProxy("mavros/set_mode", SetMode)
pub_Position = rospy.Publisher("/mavros/setpoint_raw/local",
PositionTarget,
queue_size=0)
time.sleep(.1)
travel_Height = 20
max_North = 130
#wait for initial fcu update.
while fcu_Pose.pose.position.z == 0.0:
time.sleep(0.1)
print "Waiting for FCU update"
fcu_ground_Level = (fcu_Pose.pose.position.z, )
#Display takeoff waypoint to user for 1 second
takeoff_Waypoint = set_Local_Waypoint(0, 0, travel_Height, 0, 0, 2, 0)
print takeoff_Waypoint
time.sleep(1)
#Waypoint has to be sent to FCU before mode can be changed to OFFBOARD.
i = 0
while i < 100:
takeoff_Waypoint = set_Local_Waypoint(0, 0, travel_Height, 0, 0, 2, 0)
pub_Position.publish(takeoff_Waypoint)
time.sleep(0.01)
i = i + 1
print i
#Set mode variable to OFFBOARD
mode_List = [0, "OFFBOARD"]
#Call function to set mode and arm motor
quad_Command(mode_List, True)
#take off to requested height
#change to use FCU position and velocity. Will add delay, but is safer
#while (local_Pose.pose.position.z-ground_Level[0]) < .95 * travel_Height or (local_Pose.pose.position.z - ground_Level[0]) > 1.05*travel_Height:
while (fcu_Pose.pose.position.z -
fcu_ground_Level[0]) < .9 * travel_Height or (
fcu_Pose.pose.position.z - fcu_ground_Level[0]
) > 1.1 * travel_Height or abs(fcu_Vel.twist.linear.z) > 0.3:
if exitFlag != 0:
time.sleep(3)
sys.exit()
takeoff_Waypoint = set_Local_Waypoint(0, 0, travel_Height, 0, 0, 2, 0)
pub_Position.publish(takeoff_Waypoint)
time.sleep(0.2)
height = fcu_Pose.pose.position.z - fcu_ground_Level[0]
print "Taking off. The height is: ", height
print "The desired height has been reached: ", fcu_Pose.pose.position.z - fcu_ground_Level[
0]
time.sleep(0.4)
#Set first waypoint and send to quadrotor at 10 Hz
time0 = time.time()
#fly for 10 seconds to build up speed
#while abs(local_Vel.twist.linear.x) < 6.5 and time.time() - time0 < 10:
while time.time() - time0 < 15:
first_Waypoint = set_Local_Waypoint(0, max_North, travel_Height, 0, 8,
0, 0)
pub_Position.publish(first_Waypoint)
time.sleep(0.1)
print "Distance North of home.", fcu_Pose.pose.position.y
time1 = time.time()
check_Pos = fcu_Pose
#check to allow turn. if none, don't turn. it !none, turn
north_Pos = None
while time.time() - time1 < 10:
#Commented due to faulty dGPS data
if check_Pos.pose.position.y != fcu_Pose.pose.position.y:
north_Pos = fcu_Pose.pose.position.y
print "Received fcu pose update. Turning enabled!!"
break
else:
first_Waypoint = set_Local_Waypoint(0, max_North, travel_Height, 0,
8, 0, 0)
pub_Position.publish(first_Waypoint)
time.sleep(0.1)
print "Distance North of home.", fcu_Pose.pose.position.y
#if frame_id == 1, execute turn. Otherwise, go to loiter mode and exit the program.
#north_Pos will have a numerical value if the turn should be executed
if north_Pos == None:
print "Turn failed. Execute RTL"
modeSet(0, "AUTO.LOITER")
time.sleep(5)
sys.exit()
#RTH can be enabled by using the GCS or the RC controller.
else:
desired_East = 15
turn_Waypoint = set_Local_Waypoint(desired_East, north_Pos,
travel_Height, 8, 0, 0, 0)
pub_Position.publish(turn_Waypoint)
time.sleep(0.1)
time2 = time.time()
while time.time() - time2 < 15:
desired_East = desired_East + 0.75
turn_Waypoint = set_Local_Waypoint(desired_East, north_Pos,
travel_Height, 8, 0, 0, 0)
pub_Position.publish(turn_Waypoint)
time.sleep(0.1)
print "Distance East: ", fcu_Pose.pose.position.x
modeSet(0, "AUTO.LOITER")
time.sleep(5)
def main():
global local_Pose, local_Vel, fcu_Pose, fcu_Vel, exitFlag#, read_Position
#Use ENU coords
#allow loiter mode to be enabled by pressing the enter key twice
loiterThread = threading.Thread(name='emergency_Loiter', target=emergency_Loiter)
loiterThread.start()
fcu_Pose = PoseStamped()
rospy.Subscriber("/mavros/local_position/pose", PoseStamped, fcu_Pos_Callback)
rospy.Subscriber("/mavros/local_position/velocity", TwistStamped, fcu_Vel_Callback)
#allow mode change to LOITER so that the quadrotor maintains its final position.
rospy.wait_for_service("mavros/set_mode")
modeSet = rospy.ServiceProxy("mavros/set_mode", SetMode)
pub_Position = rospy.Publisher("/mavros/setpoint_raw/local", PositionTarget, queue_size = 0)
time.sleep(.1)
travel_Height = 20
max_North = 70
#wait for initial fcu update.
while fcu_Pose.pose.position.z == 0.0:
time.sleep(0.1)
print "Waiting for FCU update"
fcu_ground_Level = (fcu_Pose.pose.position.z,)
#Display takeoff waypoint to user for 1 second
takeoff_Waypoint = set_Local_Waypoint(0,0,travel_Height,0,0,2, 0)
print takeoff_Waypoint
time.sleep(1)
#Waypoint has to be sent to FCU before mode can be changed to OFFBOARD.
i = 0
while i < 100:
takeoff_Waypoint = set_Local_Waypoint(0,0,travel_Height,0,0,2, 0)
pub_Position.publish(takeoff_Waypoint)
time.sleep(0.01)
i = i + 1
print i
#Set mode variable to OFFBOARD
mode_List = [0, "OFFBOARD"]
#Call function to set mode and arm motor
quad_Command(mode_List, True)
#take off to requested height
#change to use FCU position and velocity. Will add delay, but is safer
#while (local_Pose.pose.position.z-ground_Level[0]) < .95 * travel_Height or (local_Pose.pose.position.z - ground_Level[0]) > 1.05*travel_Height:
while (fcu_Pose.pose.position.z-fcu_ground_Level[0]) < .9 * travel_Height or (fcu_Pose.pose.position.z - fcu_ground_Level[0]) > 1.1*travel_Height or abs(fcu_Vel.twist.linear.z) > 0.3:
if exitFlag != 0:
time.sleep(3)
sys.exit()
takeoff_Waypoint = set_Local_Waypoint(0,0,travel_Height,0,0,2, 0)
pub_Position.publish(takeoff_Waypoint)
time.sleep(0.2)
height = fcu_Pose.pose.position.z - fcu_ground_Level[0]
print "Taking off. The height is: ", height
print "The desired height has been reached: ", fcu_Pose.pose.position.z - fcu_ground_Level[0]
time.sleep(0.4)
#Set first waypoint and send to quadrotor at 10 Hz
time0 = time.time()
#fly for 10 seconds to build up speed
#while abs(local_Vel.twist.linear.x) < 6.5 and time.time() - time0 < 10:
while time.time() - time0 < 15:
first_Waypoint = set_Local_Waypoint(0,max_North,travel_Height, 0, 8, 0, 0)
pub_Position.publish(first_Waypoint)
time.sleep(0.1)
print "Distance North of home.", fcu_Pose.pose.position.y
modeSet(0, "AUTO.LOITER")
time.sleep(5)
def main():
global local_Pose, local_Vel #, read_Position
#rospy.Subscriber("/mavros/global_position/global", NavSatFix, position_callback)
rospy.Subscriber("/dGPS/Position", PoseStamped, local_Pos_Callback)
rospy.Subscriber("/dGPS/Velocity", TwistStamped, local_Vel_Callback)
pub_Position = rospy.Publisher("/mavros/setpoint_raw/local",
PositionTarget,
queue_size=0)
time.sleep(.1)
#Set home position
home_Position = local_Pose
#home_North = (local_Pose.pose.position.x)
#home_East = (local_Pose.pose.position.y)
travel_Height = 1.5
#Assign tuple with maximum allowable altitude
max_Height = (local_Pose.pose.position.z + 50, )
ground_Level = (local_Pose.pose.position.z, )
print "The home position is: ", home_Position
#Display takeoff waypoint to user for 1 second
takeoff_Waypoint = set_Local_Waypoint(0, 0, 10, 0.01, 0.01, 2, 0)
print takeoff_Waypoint
time.sleep(1)
#Waypoint has to be sent to FCU before mode can be changed to OFFBOARD.
i = 0
while i < 100:
takeoff_Waypoint = set_Local_Waypoint(0, 0, 10, 0.01, 0.01, 2, 0)
pub_Position.publish(takeoff_Waypoint)
time.sleep(0.01)
i = i + 1
print i
#Set mode variable to OFFBOARD
mode_List = [0, "OFFBOARD"]
#Call function to set mode and arm motor
quad_Command(mode_List, True)
#take off to requested height
while (local_Pose.pose.position.z -
ground_Level[0]) < .95 * int(travel_Height):
takeoff_Waypoint = set_Local_Waypoint(0, 0, travel_Height, 0.01, 0.01,
2, 0)
pub_Position.publish(takeoff_Waypoint)
time.sleep(0.1)
height = local_Pose.pose.position.z - ground_Level[0]
print "Taking off. The height is: ", height
print "The desired height has been reached: ", local_Pose.pose.position.z - ground_Level[
0]
time.sleep(0.4)
#Set first waypoint and send to quadrotor at 10 Hz
while local_Vel.twist.linear.y < 0.5:
first_Waypoint = set_Local_Waypoint(0, 250, 10, 0, 10, 0, 0)
pub_Position.publish(first_Waypoint)
time.sleep(0.1)
print "Distance North of home.", local_Pose.pose.position.x - home_Position.pose.position.x
#print "Record data."
#Need to add calls to programs for collecting data here.
time1 = time.time()
while time.time() - time1 < 12.5:
desired_North = local_Pose.pose.position.x - home_Position.pose.position.x + 10
first_Waypoint = set_Local_Waypoint(0, desired_North, 10, 0, 5, 0, 0)
pub_Position.publish(first_Waypoint)
time.sleep(0.1)
print "Distance North of home.", local_Pose.pose.position.x - home_Position.pose.position.x
pos_N = local_Pose.pose.position.x - home_Position.pose.position.x
#Change mode to LOITER so that the quadrotor maintains its final position.
rospy.wait_for_service("mavros/set_mode")
modeSet = rospy.ServiceProxy("mavros/set_mode", SetMode)
modeSet(0, "AUTO.LOITER")
time.sleep(5)
def main():
global read_Position, local_Pose, local_Vel
rospy.Subscriber("/mavros/global_position/global", NavSatFix,
position_callback)
rospy.Subscriber("/mavros/local_position/pose", PoseStamped,
local_Pos_Callback)
rospy.Subscriber("/mavros/local_position/velocity", TwistStamped,
local_Vel_Callback)
pub_Position = rospy.Publisher("/mavros/setpoint_raw/local",
PositionTarget,
queue_size=0)
time.sleep(.1)
#Set home position
home_Position = read_Position
travel_Height = 10
#Assign tuple with maximum allowable altitude
max_Height = (read_Position.altitude + 50, )
ground_Level = (read_Position.altitude, )
print "The home position is: ", home_Position
#Display takeoff waypoint to user for 1 second
takeoff_Waypoint = set_Local_Waypoint(0, 0, 10, 0.01, 0.01, 2, 0)
print takeoff_Waypoint
time.sleep(1)
#Waypoint has to be sent to FCU before mode can be changed to OFFBOARD.
i = 0
while i < 100:
takeoff_Waypoint = set_Local_Waypoint(0, 0, 10, 0.01, 0.01, 2, 0)
pub_Position.publish(takeoff_Waypoint)
time.sleep(0.01)
i = i + 1
print i
#Set mode variable to OFFBOARD
mode_List = [0, "OFFBOARD"]
#Call function to set mode and arm motor
quad_Command(mode_List, True)
#take off to requested height
while local_Pose.pose.position.z < .95 * int(travel_Height):
takeoff_Waypoint = set_Local_Waypoint(0, 0, 10, 0.01, 0.01, 2, 0)
pub_Position.publish(takeoff_Waypoint)
time.sleep(0.1)
height = read_Position.altitude - ground_Level[0]
print "Taking off. The height is: ", height
print "The desired height has been reached: ", read_Position.altitude - ground_Level[
0]
time.sleep(0.4)
#Set first waypoint and send to quadrotor at 10 Hz
while local_Vel.twist.linear.y < 7.5:
first_Waypoint = set_Local_Waypoint(0, 250, 10, 0, 10, 0, 0)
pub_Position.publish(first_Waypoint)
time.sleep(0.1)
print "Distance east of home.", local_Pose.pose.position.x
print "Recording data."
#Need to add calls to programs for collecting data here.
time1 = time.time()
while time.time() - time1 < 10:
desired_Y = local_Pose.pose.position.y + 15
first_Waypoint = set_Local_Waypoint(0, desired_Y, 10, 0, 10, 0, 0)
pub_Position.publish(first_Waypoint)
time.sleep(0.1)
print "Distance east of home.", local_Pose.pose.position.x
rospy.wait_for_service("/gazebo/apply_body_wrench")
apply_Force = rospy.ServiceProxy("gazebo/apply_body_wrench",
ApplyBodyWrench)
#apply_Force('body_name: iris::base_link' ,'reference_frame: iris::base_link', 'reference_point: {x: 0, y: 0, z: 0}', 'wrench: { force: {x: 500, y: 0, z: 0}, torque: { x: 0, y: 0 , z: 0 } }', 'start_time: 0', 'duration: 10' )
r_Point = Point()
r_Point.x = 0
r_Point.y = 0
r_Point.z = 0
applied_wrench = Wrench()
applied_wrench.force.x = 5
applied_wrench.force.y = 0
applied_wrench.force.z = 0
applied_wrench.torque.x = 0
applied_wrench.torque.y = 0
applied_wrench.torque.z = 0
startTime = rospy.Time()
startTime.secs = 0
startTime.nsecs = 1
timeDuration = rospy.Time()
timeDuration.secs = 30
timeDuration.nsecs = 0
print "Applying force"
apply_Force("iris::base_link", "iris::base_link", r_Point, applied_wrench,
startTime, timeDuration)
time2 = time.time()
while time.time() - time2 < 30:
desired_Y = local_Pose.pose.position.y + 15
first_Waypoint = set_Local_Waypoint(0, desired_Y, 10, 0, 10, 0, 0)
pub_Position.publish(first_Waypoint)
time.sleep(0.1)
print "Distance east of home.", local_Pose.pose.position.x
#Change mode to LOITER so that the quadrotor maintains its final position.
rospy.wait_for_service("mavros/set_mode")
modeSet = rospy.ServiceProxy("mavros/set_mode", SetMode)
modeSet(0, "AUTO.LOITER")
time.sleep(5)