ardrone_driver.h
Go to the documentation of this file.
00001 
00025 #ifndef ARDRONE_AUTONOMY_ARDRONE_DRIVER_H
00026 #define ARDRONE_AUTONOMY_ARDRONE_DRIVER_H
00027 
00028 class ARDroneDriver;
00029 
00030 // ROS
00031 #include <ros/ros.h>
00032 #include <image_transport/image_transport.h>
00033 #include <tf/transform_broadcaster.h>
00034 #include <camera_info_manager/camera_info_manager.h>
00035 #include <sensor_msgs/Image.h>
00036 #include <sensor_msgs/Imu.h>
00037 #include <geometry_msgs/Vector3Stamped.h>
00038 #include <std_srvs/Empty.h>
00039 #include <nav_msgs/Odometry.h>
00040 #include <ardrone_autonomy/Navdata.h>
00041 #include <ardrone_autonomy/ardrone_sdk.h>
00042 
00043 // C/C++
00044 #include <stdint.h>
00045 #include <vector>
00046 #include <string>
00047 
00048 // ardronelib
00049 #include <utils/ardrone_gen_ids.h>
00050 #include <ardrone_tool/ardrone_version.h>
00051 #include <ardrone_tool/ardrone_tool.h>
00052 
00053 // Load auto-generated include files for full navdata
00054 #define NAVDATA_STRUCTS_INCLUDES
00055 #include <ardrone_autonomy/NavdataMessageDefinitions.h>
00056 #undef NAVDATA_STRUCTS_INCLUDES
00057 
00058 #define _DEG2RAD 0.01745331111
00059 #define _RAD2DEG 57.2957184819
00060 
00061 #define DRIVER_USERNAME "ardrone_driver"
00062 #define DRIVER_APPNAME "ardrone_driver"
00063 #define CAMERA_QUEUE_SIZE (30)
00064 #define NAVDATA_QUEUE_SIZE (200)
00065 
00066 class ARDroneDriver
00067 {
00068 public:
00069   ARDroneDriver();
00070   ~ARDroneDriver();
00071   void run();
00072   static bool ReadCovParams(const std::string& param_name, boost::array<double, 9> &cov_array);
00073   static double CalcAverage(const std::vector<double> &vec);
00074   void PublishVideo();
00075   void PublishNavdata(const navdata_unpacked_t &navdata_raw, const ros::Time &navdata_receive_time);
00076   void PublishOdometry(const navdata_unpacked_t &navdata_raw, const ros::Time &navdata_receive_time);
00077 
00078 #define NAVDATA_STRUCTS_HEADER_PUBLIC
00079 #include <ardrone_autonomy/NavdataMessageDefinitions.h>
00080 #undef NAVDATA_STRUCTS_HEADER_PUBLIC
00081 
00082 private:
00083   void PublishTF();
00084   void ConfigureDrone();
00085 
00086   ros::NodeHandle node_handle;
00087   ros::NodeHandle private_nh;
00088   ros::Subscriber cmd_vel_sub;
00089   ros::Subscriber takeoff_sub;
00090   ros::Subscriber reset_sub;
00091   ros::Subscriber land_sub;
00092   image_transport::ImageTransport image_transport;
00093   image_transport::CameraPublisher image_pub;
00094   image_transport::CameraPublisher hori_pub;
00095   image_transport::CameraPublisher vert_pub;
00096 
00097   camera_info_manager::CameraInfoManager *cinfo_hori;
00098   camera_info_manager::CameraInfoManager *cinfo_vert;
00099 
00100   ros::Publisher navdata_pub;
00101   ros::Publisher imu_pub;
00102   ros::Publisher mag_pub;
00103   ros::Publisher odo_pub;
00104 
00105   tf::TransformBroadcaster tf_broad;
00106 
00107   ros::ServiceServer toggle_cam_srv;
00108   ros::ServiceServer set_cam_channel_srv;
00109   ros::ServiceServer set_led_animation_srv;
00110   ros::ServiceServer flat_trim_srv;
00111   ros::ServiceServer set_flight_anim_srv;
00112   ros::ServiceServer set_record_srv;
00113 
00114   int32_t last_frame_id;
00115   int32_t last_navdata_id;
00116   int32_t copy_current_frame_id;
00117   int32_t copy_current_navdata_id;
00118 
00119   int16_t flying_state;
00120 
00121   bool is_inited;
00122   std::string drone_frame_id;
00123 
00124   // Load auto-generated declarations for full navdata
00125 #define NAVDATA_STRUCTS_HEADER_PRIVATE
00126 #include <ardrone_autonomy/NavdataMessageDefinitions.h>
00127 #undef NAVDATA_STRUCTS_HEADER_PRIVATE
00128 
00129   /*
00130    * TF Frames
00131    * Base: Should be COM
00132    */
00133   std::string tf_prefix, drone_frame_base, drone_frame_imu, drone_frame_front_cam, drone_frame_bottom_cam;
00134   tf::StampedTransform tf_base_front, tf_base_bottom, tf_odom;
00135 
00136   // Huge part of IMU message is constant, let's fill'em once.
00137   sensor_msgs::Imu imu_msg;
00138   geometry_msgs::Vector3Stamped mag_msg;
00139   ardrone_autonomy::Navdata legacynavdata_msg;
00140 
00141   // odometry (x,y)
00142   ros::Time last_receive_time;
00143   double odometry[2];
00144 };
00145 
00146 #endif  // ARDRONE_AUTONOMY_ARDRONE_DRIVER_H


ardrone_autonomy
Author(s): Mani Monajjemi, Mani Monajjemi
autogenerated on Fri Dec 9 2016 03:36:59