Go to the documentation of this file.00001
00025 #ifndef BEBOP_AUTONOMY_BEBOP_DRIVER_NODELET_H
00026 #define BEBOP_AUTONOMY_BEBOP_DRIVER_NODELET_H
00027
00028
00029
00030 #include <string>
00031
00032 #include <ros/timer.h>
00033 #include <nodelet/nodelet.h>
00034 #include <geometry_msgs/Twist.h>
00035 #include <std_msgs/Empty.h>
00036 #include <std_msgs/UInt8.h>
00037 #include <std_msgs/Float32.h>
00038 #include <std_msgs/Bool.h>
00039 #include <std_msgs/String.h>
00040 #include <camera_info_manager/camera_info_manager.h>
00041 #include <image_transport/image_transport.h>
00042 #include <dynamic_reconfigure/server.h>
00043
00044 #include <boost/thread.hpp>
00045 #include <boost/shared_ptr.hpp>
00046
00047 #include "bebop_driver/bebop.h"
00048
00049 #define CLAMP(x, l, h) (((x) > (h)) ? (h) : (((x) < (l)) ? (l) : (x)))
00050
00051 namespace bebop_driver
00052 {
00053
00054
00055 namespace util
00056 {
00057 static const ros::console::levels::Level arsal_level_to_ros[ARSAL_PRINT_MAX] = {
00058 ros::console::levels::Fatal,
00059 ros::console::levels::Error,
00060 ros::console::levels::Warn,
00061 ros::console::levels::Info,
00062 ros::console::levels::Debug,
00063 ros::console::levels::Debug};
00064
00065 static const double eps = 1.0e-6;
00066 static const double deg2rad = 3.14159265 / 180.0;
00067
00068 static char bebop_err_str[BEBOP_ERR_STR_SZ];
00069
00070 void ResetTwist(geometry_msgs::Twist& t)
00071 {
00072 t.linear.x = 0.0;
00073 t.linear.y = 0.0;
00074 t.linear.z = 0.0;
00075 t.angular.x = 0.0;
00076 t.angular.y = 0.0;
00077 t.angular.z = 0.0;
00078 }
00079
00080
00081
00082 inline bool CompareTwists(const geometry_msgs::Twist& lhs, const geometry_msgs::Twist& rhs)
00083 {
00084 return (fabs(lhs.linear.x - rhs.linear.x) < eps) &&
00085 (fabs(lhs.linear.y - rhs.linear.y) < eps) &&
00086 (fabs(lhs.linear.z - rhs.linear.z) < eps) &&
00087 (fabs(lhs.angular.x - rhs.angular.x) < eps) &&
00088 (fabs(lhs.angular.y - rhs.angular.y) < eps) &&
00089 (fabs(lhs.angular.z - rhs.angular.z) < eps);
00090 }
00091
00092 int BebopPrintToROSLogCB(eARSAL_PRINT_LEVEL level, const char *tag, const char *format, va_list va);
00093 }
00094
00095
00096 class BebopArdrone3Config;
00097 class Bebop;
00098
00099 class BebopDriverNodelet : public nodelet::Nodelet
00100 {
00101 private:
00102 boost::shared_ptr<bebop_driver::Bebop> bebop_ptr_;
00103 boost::shared_ptr<boost::thread> camera_pub_thread_ptr_;
00104 boost::shared_ptr<boost::thread> aux_thread_ptr_;
00105
00106 geometry_msgs::Twist prev_bebop_twist_;
00107 ros::Time prev_twist_stamp_;
00108 boost::mutex twist_mutex_;
00109
00110 geometry_msgs::Twist camera_twist_;
00111 geometry_msgs::Twist prev_camera_twist_;
00112
00113 ros::Subscriber cmd_vel_sub_;
00114 ros::Subscriber camera_move_sub_;
00115 ros::Subscriber takeoff_sub_;
00116 ros::Subscriber land_sub_;
00117 ros::Subscriber reset_sub_;
00118 ros::Subscriber flattrim_sub_;
00119 ros::Subscriber navigatehome_sub_;
00120 ros::Subscriber start_autoflight_sub_;
00121 ros::Subscriber pause_autoflight_sub_;
00122 ros::Subscriber stop_autoflight_sub_;
00123 ros::Subscriber animation_sub_;
00124 ros::Subscriber snapshot_sub_;
00125 ros::Subscriber exposure_sub_;
00126 ros::Subscriber toggle_recording_sub_;
00127
00128 ros::Publisher odom_pub_;
00129 ros::Publisher camera_joint_pub_;
00130 ros::Publisher gps_fix_pub_;
00131
00132 boost::shared_ptr<camera_info_manager::CameraInfoManager> cinfo_manager_ptr_;
00133 boost::shared_ptr<image_transport::ImageTransport> image_transport_ptr_;
00134 image_transport::CameraPublisher image_transport_pub_;
00135
00136 sensor_msgs::CameraInfoPtr camera_info_msg_ptr_;
00137
00138
00139 boost::shared_ptr<dynamic_reconfigure::Server<bebop_driver::BebopArdrone3Config> > dynr_serv_ptr_;
00140
00141
00142 std::string param_camera_frame_id_;
00143 std::string param_odom_frame_id_;
00144 bool param_publish_odom_tf_;
00145 double param_cmd_vel_timeout_;
00146
00147
00148 void CameraPublisherThread();
00149
00150
00151 void AuxThread();
00152
00153 void CmdVelCallback(const geometry_msgs::TwistConstPtr& twist_ptr);
00154 void CameraMoveCallback(const geometry_msgs::TwistConstPtr& twist_ptr);
00155 void TakeoffCallback(const std_msgs::EmptyConstPtr& empty_ptr);
00156 void LandCallback(const std_msgs::EmptyConstPtr& empty_ptr);
00157 void EmergencyCallback(const std_msgs::EmptyConstPtr& empty_ptr);
00158 void FlatTrimCallback(const std_msgs::EmptyConstPtr& empty_ptr);
00159 void NavigateHomeCallback(const std_msgs::BoolConstPtr& start_stop_ptr);
00160 void StartAutonomousFlightCallback(const std_msgs::StringConstPtr& file_path_ptr);
00161 void PauseAutonomousFlightCallback(const std_msgs::EmptyConstPtr& empty_ptr);
00162 void StopAutonomousFlightCallback(const std_msgs::EmptyConstPtr& empty_ptr);
00163 void FlipAnimationCallback(const std_msgs::UInt8ConstPtr& animid_ptr);
00164 void TakeSnapshotCallback(const std_msgs::EmptyConstPtr& empty_ptr);
00165 void SetExposureCallback(const std_msgs::Float32ConstPtr& exposure_ptr);
00166 void ToggleRecordingCallback(const std_msgs::BoolConstPtr& toggle_ptr);
00167
00168 void ParamCallback(bebop_driver::BebopArdrone3Config &config, uint32_t level);
00169
00170 public:
00171 BebopDriverNodelet();
00172 virtual ~BebopDriverNodelet();
00173
00174 virtual void onInit();
00175 };
00176 }
00177
00178 #endif // BEBOP_AUTONOMY_BEBOP_DRIVER_NODELET_H