#!/usr/bin/env python
import rospy
from drone import Drone
from vr_object import VRController
import numpy as np
initialized = False
pos_coef = 2.0
if __name__ == '__main__':
rospy.init_node('drone_control', anonymous=True)
drone = Drone()
human = VRController()
drone.arm()
drone.takeoff(1.0)
drone.hover(1.0)
print('Following human movements...')
while not rospy.is_shutdown():
if not initialized:
print('Initial position is defined')
human_pose_init = human.position()
drone_pose_init = drone.pose
initialized = True
dx, dy, dz = human.position() - human_pose_init
# tmp = dx; dx = -dy; dy = tmp # switch coordinate axes for correct mapping
drone.sp = np.array([
drone_pose_init[0] + pos_coef * dx,
drone_pose_init[1] + pos_coef * dy,
# main program, runs through basic algorith
from drone import Drone
import time
middle = 1680
if __name__ == "__main__":
# create drone object
d = Drone()
try:
# arming sequence
d.move([900, 1500, 1500])
d.arm(2)
d.move([middle - 250, 1500, 1500])
time.sleep(1.5)
# loop through pictures and move based off of outputs
while True:
throt, yaw, pit = d.process()
print(throt, yaw, pit)
d.move([throt, yaw, pit])
except:
# stop the drone if program shut off
d.move([900, 1500, 1500])
