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