traj = [] with SyncCrazyflie(drone.uri, cf=Crazyflie(rw_cache='./cache')) as scf: drone.scf = scf reset_estimator(drone) drone.start_position_reading() # 20 Hz time.sleep(0.5) drone.pose_home = drone.pose print('Home position:', drone.pose_home) rate = rospy.Rate(10) trajectory = [] stop_recording = [] thread.start_new_thread(input_thread, (stop_recording, )) print('Started trajectory recoridng') while not stop_recording: drone.publish_path() trajectory.append(drone.pose) rate.sleep() # Execute recorded trajectory drone.sp = np.zeros(4) print('Executing recorded trajectory') for i in range(len(trajectory)): drone.sp[:3] = trajectory[i] if toFly: drone.fly() drone.publish_sp() drone.publish_path() if toFly else drone.publish_path_sp() time.sleep(0.1)
elif abs(dz) > dz_thresh[1]: z_input = np.sign(dz) * dz_thresh[1] else: z_input = dz cmd_vel = vel_koef * (np.array([x_input, y_input, z_input])) yaw_input = yaw_koef * yaw_input # print 'cmd_vel', cmd_vel # np.putmask(cmd_vel, abs(cmd_vel) <= (vel_koef*0.035), 0) # np.putmask(cmd_vel, abs(cmd_vel) <= (0.20), 0) time_now = time.time() drone.sp += cmd_vel * (time.time() - time_prev) time_prev = time_now # define flight area to operate inside of if put_limits: np.putmask(drone.sp, drone.sp >= limits_up, limits_up) np.putmask(drone.sp, drone.sp <= limits_down, limits_down) # send pose flight commands to the Crazyflie drone drone.fly(yaw=yaw_input) # visualization: RVIZ drone.publish_sp(orient=np.array([0, 0, yaw_input])) drone.publish_path_sp() # define landing condition if controller.position()[2] < 0.5: landing() break rate.sleep()
dx = -dz dz = tmp_y drone.sp = np.array([ drone_pose_init[0] + pos_coef * dx, drone_pose_init[1] + pos_coef * dy, drone_pose_init[2] + pos_coef * dz ]) # define flight area to operate inside of if put_limits: np.putmask(drone.sp, drone.sp >= limits_up, limits_up) np.putmask(drone.sp, drone.sp <= limits_down, limits_down) # send pose flight commands to the Crazyflie drone drone.fly() # yaw=yaw_input # visualization: RVIZ drone.publish_sp() drone.publish_path_sp() # define landing condition # if controller.position()[2] < 0.5: # landing() # break if dz < -0.5: landing() break rate.sleep()