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 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