Skip to content
Permalink
Branch: master
Find file Copy path
Find file Copy path
Fetching contributors…
Cannot retrieve contributors at this time
49 lines (32 sloc) 1.2 KB
#!/usr/bin/env python
import rospy
import crazyflie
import time
import uav_trajectory
if __name__ == '__main__':
rospy.init_node('test_high_level')
cf = crazyflie.Crazyflie("crazyflie", "/vicon/crazyflie/crazyflie")
cf.setParam("commander/enHighLevel", 1)
cf.setParam("stabilizer/estimator", 2) # Use EKF
cf.setParam("stabilizer/controller", 2) # Use mellinger controller
# reset kalman
cf.setParam("kalman/resetEstimation", 1)
# cf.takeoff(targetHeight = 0.5, duration = 2.0)
# time.sleep(3.0)
# cf.goTo(goal = [0.5, 0.0, 0.0], yaw=0.2, duration = 2.0, relative = True)
# time.sleep(3.0)
# cf.land(targetHeight = 0.0, duration = 2.0)
traj1 = uav_trajectory.Trajectory()
traj1.loadcsv("takeoff.csv")
traj2 = uav_trajectory.Trajectory()
traj2.loadcsv("figure8.csv")
print(traj1.duration)
cf.uploadTrajectory(0, 0, traj1)
cf.uploadTrajectory(1, len(traj1.polynomials), traj2)
cf.startTrajectory(0, timescale=1.0)
time.sleep(traj1.duration * 2.0)
cf.startTrajectory(1, timescale=2.0)
time.sleep(traj2.duration * 2.0)
cf.startTrajectory(0, timescale=1.0, reverse=True)
time.sleep(traj1.duration * 1.0)
cf.stop()
You can’t perform that action at this time.