bebop.h
Go to the documentation of this file.
1 
25 #ifndef BEBOP_AUTONOMY_BEBOP_H
26 #define BEBOP_AUTONOMY_BEBOP_H
27 
28 #define BEBOP_ERR_STR_SZ 150
29 
30 #include <sys/syscall.h>
31 #include <sys/types.h>
32 
33 #include <string>
34 #include <vector>
35 #include <utility>
36 #include <map>
37 
38 #include <boost/shared_ptr.hpp>
39 #include <boost/atomic.hpp>
40 #include <boost/thread/mutex.hpp>
41 #include <boost/thread/condition_variable.hpp>
42 
43 extern "C"
44 {
45  #include "libARSAL/ARSAL.h"
46  #include "libARController/ARController.h"
47 }
48 
49 // Forward declarations
50 extern "C"
51 {
52  struct ARDISCOVERY_Device_t;
53 }
54 namespace ros
55 {
56  class NodeHandle;
57 }
58 
59 namespace bebop_driver
60 {
61 
62 namespace util
63 {
64 inline long int GetLWPId()
65 {
66  return (syscall(SYS_gettid));
67 }
68 
69 } // namespace util
70 
71 // Forward declarations
72 class BebopArdrone3Config;
73 class VideoDecoder;
74 namespace cb
75 {
76  class AbstractCommand;
77 } // namespace cb
78 #define FORWARD_DECLARATIONS
82 #undef FORWARD_DECLARATIONS
83 
84 class Bebop
85 {
86 private:
87  static const char* LOG_TAG;
88  boost::atomic<bool> is_connected_;
89  boost::atomic<bool> is_streaming_started_;
90  ARDISCOVERY_Device_t* device_ptr_;
91  ARCONTROLLER_Device_t* device_controller_ptr_;
92  eARCONTROLLER_ERROR error_;
93  eARCONTROLLER_DEVICE_STATE device_state_;
94  ARSAL_Sem_t state_sem_;
96  std::string bebop_ip_;
97 
98 // boost::mutex callback_map_mutex_;
99  typedef std::map<eARCONTROLLER_DICTIONARY_KEY, boost::shared_ptr<cb::AbstractCommand> > callback_map_t;
100  typedef std::pair<eARCONTROLLER_DICTIONARY_KEY, boost::shared_ptr<cb::AbstractCommand> > callback_map_pair_t;
101  callback_map_t callback_map_;
102 
103  // sync
105  mutable boost::mutex frame_avail_mutex_;
106  mutable bool is_frame_avail_;
107 
108  static void StateChangedCallback(
109  eARCONTROLLER_DEVICE_STATE new_state,
110  eARCONTROLLER_ERROR error,
111  void *bebop_void_ptr);
112 
113  static void CommandReceivedCallback(
114  eARCONTROLLER_DICTIONARY_KEY cmd_key,
115  ARCONTROLLER_DICTIONARY_ELEMENT_t* element_dict_ptr,
116  void* bebop_void_ptr);
117 
118  static eARCONTROLLER_ERROR FrameReceivedCallback(ARCONTROLLER_Frame_t *frame,
119  void *bebop_void_ptr);
120 
121  static eARCONTROLLER_ERROR DecoderConfigCallback(ARCONTROLLER_Stream_Codec_t codec,
122  void *bebop_void_ptr);
123 
124  // nothrow
125  void Cleanup();
126 
127  void ThrowOnInternalError(const std::string& message = std::string());
128  void ThrowOnCtrlError(const eARCONTROLLER_ERROR& error, const std::string& message = std::string());
129 
130 public:
131  // Navdata Callbacks
132 #define DEFINE_SHARED_PTRS
136 #undef DEFINE_SHARED_PTRS
137 
138  inline ARSAL_Sem_t* GetStateSemPtr() {return &state_sem_;}
139  inline const ARCONTROLLER_Device_t* GetControllerCstPtr() const {return device_controller_ptr_;}
140 
141  inline bool IsConnected() const {return is_connected_;}
142  inline bool IsStreamingStarted() const {return is_streaming_started_;}
143 
144  explicit Bebop(ARSAL_Print_Callback_t custom_print_callback = 0);
145  ~Bebop();
146 
147  void Connect(ros::NodeHandle& nh, ros::NodeHandle& priv_nh, const std::string& bebop_ip = "192.168.42.1");
148  void StartStreaming();
149 
150  // Disable all data callback and streaming (nothrow)
151  void StopStreaming();
152  // nothrow
153  void Disconnect();
154 
155  void SetDate(const std::string &date);
156  void RequestAllSettings();
157  void ResetAllSettings();
158  void UpdateSettings(const bebop_driver::BebopArdrone3Config& config);
159 
160  void Takeoff();
161  void Land();
162  void Emergency();
163  void FlatTrim();
164  // false: Stop, true: Start
165  void NavigateHome(const bool& start_stop);
166  void StartAutonomousFlight(const std::string &filepath);
167  void PauseAutonomousFlight();
168  void StopAutonomousFlight();
169  void AnimationFlip(const uint8_t& anim_id);
170 
171  // -1..1
172  void Move(const double& roll, const double& pitch, const double& gaz_speed, const double& yaw_speed);
173  void MoveCamera(const double& tilt, const double& pan);
174 
175  void TakeSnapshot();
176  // exposure should be between -3.0 and +3.0
177  void SetExposure(const float& exposure);
178  // true: start, false: stop
179  void ToggleVideoRecording(const bool start);
180 
181  // This function is blocking and runs in the caller's thread's context
182  // which is different from FrameReceivedCallback's context
183  bool GetFrontCameraFrame(std::vector<uint8_t>& buffer, uint32_t &width, uint32_t &height) const;
184  uint32_t GetFrontCameraFrameWidth() const;
185  uint32_t GetFrontCameraFrameHeight() const;
186 };
187 
188 } // namespace bebop_driver
189 
190 
191 #endif // BEBOP_AUTONOMY_BEBOP_H
bool is_frame_avail_
Definition: bebop.h:106
ROSCPP_DECL void start()
const ARCONTROLLER_Device_t * GetControllerCstPtr() const
Definition: bebop.h:139
callback_map_t callback_map_
Definition: bebop.h:101
boost::shared_ptr< VideoDecoder > video_decoder_ptr_
Definition: bebop.h:95
ARSAL_Sem_t * GetStateSemPtr()
Definition: bebop.h:138
std::string bebop_ip_
Definition: bebop.h:96
ARSAL_Sem_t state_sem_
Definition: bebop.h:94
boost::atomic< bool > is_connected_
Definition: bebop.h:88
static const char * LOG_TAG
Definition: bebop.h:87
bool IsConnected() const
Definition: bebop.h:141
boost::condition_variable frame_avail_cond_
Definition: bebop.h:104
long int GetLWPId()
Definition: bebop.h:64
ARCONTROLLER_Device_t * device_controller_ptr_
Definition: bebop.h:91
bool IsStreamingStarted() const
Definition: bebop.h:142
boost::mutex frame_avail_mutex_
Definition: bebop.h:105
eARCONTROLLER_DEVICE_STATE device_state_
Definition: bebop.h:93
ARDISCOVERY_Device_t * device_ptr_
Definition: bebop.h:90
std::map< eARCONTROLLER_DICTIONARY_KEY, boost::shared_ptr< cb::AbstractCommand > > callback_map_t
Definition: bebop.h:99
eARCONTROLLER_ERROR error_
Definition: bebop.h:92
std::pair< eARCONTROLLER_DICTIONARY_KEY, boost::shared_ptr< cb::AbstractCommand > > callback_map_pair_t
Definition: bebop.h:100
boost::atomic< bool > is_streaming_started_
Definition: bebop.h:89


bebop_driver
Author(s): Mani Monajjemi
autogenerated on Mon Jun 10 2019 12:58:56