| |
| # Authors: |
| # * XYZ Code Spaceappchallengepty |
| # |
| importsys |
| fromstd_msgs.msgimportString |
| fromstd_msgs.msgimportEmpty |
| fromsensor_msgs.msgimportCompressedImage |
| fromgeometry_msgs.msgimportTwist |
| importrospy |
| fromar_drone_wrapper.msgimportNavdata |
| fromimutilsimportpaths |
| importnumpyasnp |
| importimutils |
| importcv2 |
| fromlibardroneimportardrone |
| frommultiprocessingimportProcess |
| importtime |
| |
| |
| drone=None |
| pub_nav_data=rospy.Publisher("/ardrone/navdata", Navdata, queue_size=1) |
| front_image_pub=rospy.Publisher("/ardrone/front_camera/raw_image_compressed", CompressedImage, queue_size=10) |
| |
| |
| defimages(drone): |
| rospy.loginfo("Image info thread") |
| ifdrone: |
| drone.set_camera_view(True) |
| msg=CompressedImage() |
| msg.header.stamp=rospy.Time.now() |
| msg.format="jpeg" |
| compressed_images=cv2.imencode('.jpg', drone.get_image()) |
| msg.data=np.array(compressed_images[1]).tostring() |
| front_image_pub.publish(msg) |
| |
| |
| datos=dict() |
| datos=nav_data(drone) |
| print(datos[0]) |
| |
| |
| else: |
| rospy.loginfo("No inicio") |
| |
| defnav_data(drone): |
| ifdrone: |
| |
| nav_data_drone=drone.get_navdata() |
| data=nav_data_drone |
| |
| else: |
| rospy.loginfo("No data to publish") |
| return (data) |
| |
| defcmd_vel(move_data): |
| globaldrone |
| |
| ifmove_data.linear.x==-1: |
| drone.move_right() |
| |
| elifmove_data.linear.y==1: |
| drone.move_forward() |
| |
| |
| elifmove_data.linear.z==1: |
| drone.move_up() |
| |
| |
| elifmove_data.linear.x==1: |
| drone.move_left() |
| |
| |
| elifmove_data.linear.y==-1: |
| drone.move_backward() |
| |
| |
| elifmove_data.linear.z==-1: |
| drone.move_down() |
| |
| |
| elifmove_data.angular.x==-1: |
| drone.turn_right() |
| |
| |
| elifmove_data.angular.x==1: |
| drone.turn_left() |
| |
| #nuevo agregado |
| elifmove_data.angular.y==2: |
| drone.takeoff() |
| |
| elifmove_data.angular.y==-2: |
| drone.land() |
| |
| elifmove_data.angular.z==2: |
| drone.reset() |
| |
| elifmove_data.angular.z==-2: |
| |
| drone.hover() |
| |
| |
| |
| images(drone) |
| |
| |
| deftakeoff(_data): |
| globaldrone |
| drone.takeoff() |
| drone.hover() |
| |
| defland(_data): |
| globaldrone |
| drone.land() |
| |
| defreset(_data): |
| globaldrone |
| drone.reset() |
| |
| defmain(args): |
| globaldrone |
| rospy.init_node('control_driver', anonymous=True) |
| rospy.loginfo("Starting drone connection") |
| drone=ardrone.ARDrone(True) |
| rospy.loginfo("Connection Success!!") |
| drone.reset() |
| cmd_vel_sub=rospy.Subscriber("/ardrone/cmd_vel", Twist, cmd_vel, queue_size=1) |
| takeoff_sub=rospy.Subscriber("/ardrone/takeoff", Empty, takeoff, queue_size=1) |
| land_sub=rospy.Subscriber("/ardrone/land", Empty, land, queue_size=1) |
| reset_sub=rospy.Subscriber("/ardrone/reset", Empty, reset, queue_size=1) |
| try: |
| rospy.spin() |
| exceptKeyboardInterrupt: |
| drone.halt() |
| nav_data_thread.terminate() |
| print"Shutting down ROS ARDRONE DRIVER module" |
| |
| if__name__=='__main__': |
| main(sys.argv) |