#!/usr/bin/env python

import rospy
import numpy
import threading #timer
from nav_msgs.msg import Odometry #position (souscrire)
from geometry_msgs.msg import Twist #position (publier)
from std_msgs.msg import Empty #takeoff et land
from time import sleep

def takeoff():
	pub = rospy.Publisher('/bebop/takeoff', Empty, queue_size=1,latch=True)	
	pub.publish(Empty())
	sleep(6.)
		
def land():
	pub = rospy.Publisher('/bebop/land', Empty, queue_size=1, latch=True)	
	pub.publish(Empty())
	sleep(4.)


def fleche_gauche(): 
	rate = rospy.Rate(10)
	for i in range(30):
		pubgauche = rospy.Publisher('/bebop/cmd_vel', Twist, queue_size=1)
		gauche=Twist()
		gauche.angular.x=0
		gauche.angular.y=0
		gauche.angular.z=0
		gauche.linear.x=0.4
		gauche.linear.y=0
		gauche.linear.z=0
		pubgauche.publish(gauche)
		rate.sleep()


if __name__ == '__main__':
	rospy.init_node('takeoffland', anonymous=True)
	rate = rospy.Rate(10)
	print "Decollage"
	takeoff()

	print "Deplacement"
	fleche_gauche()
	print "Atterissage"
	land()
