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()
