# z_des = 4
# yaw_des = 1
try:
while not rospy.is_shutdown():
#get position nad orientation from vicon
currentPosition = ardrone.get_pos()
currentOrientation = ardrone.get_orient()
#compute desired pose
dt = max((ardrone.get_time() - ardrone.time_stamp) / pow(10, 9),
0.0001)
ardrone.time_stamp = ardrone.get_time()
x_des, y_des, z_des, yaw_des = ardrone.x_des, ardrone.y_des, ardrone.z_des, ardrone.yaw_des
traj = positionCtrl.getDesiredState(currentPosition,
currentOrientation, x_des,
y_des, z_des, yaw_des, dt)
#publish actuation commands
ardrone.set_vel(traj)
#spin
ardrone.rate.sleep()
except KeyboardInterrupt:
msg = Twist()
msg.linear.x = 0
msg.linear.y = 0
msg.linear.z = 0
msg.angular.z = 0
ardrone.set_vel(msg)
ardrone.land()
rospy.spin()
