#!/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])