#!/usr/bin/python
import rospy
from std_msgs.msg import String
from geometry_msgs.msg import Twist
from std_msgs.msg import Empty

land = rospy.Publisher("/tello/land", Empty, queue_size=1)


def talker(xi, yi, zi, anglei):
    pub = rospy.Publisher('tello/command', Twist, queue_size=10)
    point = Twist()
    point.linear.x = xi
    point.linear.y = yi
    point.linear.z = zi
    point.angular.z = anglei
    pub.publish(point)


rospy.init_node('command', anonymous=False)

if __name__ == '__main__':
    x = 0.0
    y = 1.5
    z = 1.5
    angle = 0.0
    talker(x, y, z, angle)
    while not rospy.is_shutdown():
        try:
            print("x:")
            x = float(input())

            print("y:")
            y = float(input())

            print("z:")
            z = float(input())

            print("angle:")
            angle = float(input())
        except:
            print("Not a valid point. Landing.")
            x = 0.0
            y = 0.0
            z = 0.0
            angle = 0.0
            land.publish(Empty())
            pass
        talker(x, y, z, angle)
        # rospy.loginfo(str(x) + " + " + str(y) + " + " + str(z) + " + " + str(angle))
