bebop_driver_nodelet.h
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 // TODO(mani-monaj): Use forward decl here if possible
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 // TODO(mani-monaj): Move these to util.h
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   // True: Equal, false otherwise
00081   // TODO(mani-monaj): refactor
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 }  // namespace util
00094 
00095 // Forward decl
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   // Dynamic Reconfigure
00139   boost::shared_ptr<dynamic_reconfigure::Server<bebop_driver::BebopArdrone3Config> > dynr_serv_ptr_;
00140 
00141   // Params (not dynamically reconfigurable, persistent)
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   // This runs in its own context
00148   void CameraPublisherThread();
00149 
00150   // Safety monitor + ROS specfic publishers (TF, Odom, etc)
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 }  // namespace bebop_driver
00177 
00178 #endif  // BEBOP_AUTONOMY_BEBOP_DRIVER_NODELET_H


bebop_driver
Author(s): Mani Monajjemi
autogenerated on Mon Aug 21 2017 02:36:39