freenect_device.hpp
Go to the documentation of this file.
00001 #ifndef FREENECT_DEVICE_T01IELX0
00002 #define FREENECT_DEVICE_T01IELX0
00003 
00004 #include <boost/shared_ptr.hpp>
00005 #include <boost/noncopyable.hpp>
00006 #include <boost/thread/mutex.hpp>
00007 #include <boost/date_time/posix_time/ptime.hpp>
00008 #include <stdexcept>
00009 
00010 #include <libfreenect/libfreenect.h>
00011 #include <libfreenect/libfreenect-registration.h>
00012 #include <freenect_camera/image_buffer.hpp>
00013 
00014 namespace freenect_camera {
00015 
00016   static const std::string PRODUCT_NAME = "Xbox NUI Camera";
00017   static const unsigned PRODUCT_ID = 0x2ae;
00018   static const std::string VENDOR_NAME = "Microsoft";
00019   static const unsigned VENDOR_ID = 0x45e;
00020 
00021   typedef freenect_resolution OutputMode;
00022 
00023   bool isImageMode(const ImageBuffer& buffer) {
00024     return buffer.metadata.video_format == FREENECT_VIDEO_BAYER;
00025   }
00026 
00027   class FreenectDriver;
00028 
00029   class FreenectDevice : public boost::noncopyable {
00030 
00031     public:
00032 
00033       FreenectDevice(freenect_context* driver, std::string serial) {
00034         openDevice(driver, serial);
00035         flushDeviceStreams();
00036 
00037         //Initialize default variables
00038         streaming_video_ = should_stream_video_ = false;
00039         new_video_resolution_ = getDefaultImageMode();
00040         new_video_format_ = FREENECT_VIDEO_BAYER;
00041         video_buffer_.metadata.resolution = FREENECT_RESOLUTION_DUMMY;
00042         video_buffer_.metadata.video_format = FREENECT_VIDEO_DUMMY;
00043 
00044         streaming_depth_ = should_stream_depth_ = false;
00045         new_depth_resolution_ = getDefaultDepthMode();
00046         new_depth_format_ = FREENECT_DEPTH_MM;
00047         depth_buffer_.metadata.resolution = FREENECT_RESOLUTION_DUMMY;
00048         depth_buffer_.metadata.depth_format = FREENECT_DEPTH_DUMMY;
00049 
00050         publishers_ready_ = false;
00051       }
00052 
00053 
00054       ~FreenectDevice() {
00055         shutdown();
00056       }
00057 
00058       void flushDeviceStreams() {
00059         device_flush_start_time_ = boost::posix_time::microsec_clock::local_time();
00060         device_flush_enabled_ = true; 
00061         ROS_INFO("Starting a 3s RGB and Depth stream flush.");
00062       }
00063 
00064       void openDevice(freenect_context* driver, std::string serial) {
00065         if (freenect_open_device_by_camera_serial(driver, &device_, serial.c_str()) < 0) {
00066           throw std::runtime_error("[ERROR] Unable to open specified kinect");
00067         }
00068         freenect_set_user(device_, this);
00069         freenect_set_depth_callback(device_, freenectDepthCallback);
00070         freenect_set_video_callback(device_, freenectVideoCallback);
00071         driver_ = driver;
00072         device_serial_ = serial;
00073         registration_ = freenect_copy_registration(device_);
00074       }
00075 
00076       void shutdown() {
00077         freenect_close_device(device_);
00078         freenect_destroy_registration(&registration_);
00079       }
00080 
00081       /* DEVICE SPECIFIC FUNCTIONS */
00082 
00084       unsigned getBus() const {
00085         return 0;
00086       }
00087 
00089       unsigned getAddress() const {
00090         return 0;
00091       }
00092 
00093       const char* getProductName() const {
00094         return PRODUCT_NAME.c_str();
00095       }
00096 
00097       const char* getSerialNumber() const {
00098         return device_serial_.c_str();
00099       }
00100 
00101       bool hasImageStream() const {
00102         return true;
00103       }
00104 
00105       bool hasDepthStream() const {
00106         return true;
00107       }
00108 
00109       bool hasIRStream() const {
00110         return true;
00111       }
00112 
00113       bool isDepthRegistrationSupported() const {
00114         return true;
00115       }
00116 
00117       bool isSynchronizationSupported() const {
00118         return false;
00119       }
00120 
00121       bool isSynchronized() const {
00122         return false;
00123       }
00124 
00125       bool setSynchronization(bool on_off) const {
00126         throw std::runtime_error("[ERROR] The kinect does not support hardware synchronization");
00127       }
00128 
00132       float getBaseline() const {
00133         return 0.01 * registration_.zero_plane_info.dcmos_emitter_dist;
00134       }
00135 
00136       /* CALLBACK ASSIGNMENT FUNCTIONS */
00137 
00138       template<typename T> void registerImageCallback (
00139           void (T::*callback)(const ImageBuffer& image, void* cookie), 
00140           T& instance, void* cookie = NULL) {
00141         image_callback_ = boost::bind(callback, boost::ref(instance), _1, cookie);
00142       }
00143 
00144       template<typename T> void registerDepthCallback (
00145           void (T::*callback)(const ImageBuffer& depth_image, void* cookie), 
00146           T& instance, void* cookie = NULL) {
00147         depth_callback_ = boost::bind(callback, boost::ref(instance), _1, cookie);
00148       }
00149 
00150       template<typename T> void registerIRCallback (
00151           void (T::*callback)(const ImageBuffer& ir_image, void* cookie), 
00152           T& instance, void* cookie = NULL) {
00153         ir_callback_ = boost::bind(callback, boost::ref(instance), _1, cookie);
00154       }
00155 
00156       void publishersAreReady() {
00157         publishers_ready_ = true;
00158       }
00159 
00160       /* IMAGE SETTINGS FUNCTIONS */
00161 
00162       OutputMode getImageOutputMode() {
00163         boost::lock_guard<boost::recursive_mutex> lock(m_settings_);
00164         return video_buffer_.metadata.resolution;
00165       }
00166 
00167       void setImageOutputMode(OutputMode mode) {
00168         boost::lock_guard<boost::recursive_mutex> lock(m_settings_);
00169         new_video_resolution_ = mode;
00170       }
00171 
00172       OutputMode getDefaultImageMode() const {
00173         return FREENECT_RESOLUTION_MEDIUM;
00174       }
00175 
00176       bool findCompatibleImageMode(const OutputMode& mode, OutputMode& compatible_mode) const {
00177         freenect_frame_mode new_mode = 
00178           freenect_find_video_mode(mode, video_buffer_.metadata.video_format);
00179         if (!new_mode.is_valid) {
00180           compatible_mode = getDefaultImageMode();
00181           return false;
00182         }
00183         compatible_mode = mode;
00184         return true;
00185       }
00186 
00187       void stopImageStream() {
00188         boost::lock_guard<boost::recursive_mutex> lock(m_settings_);
00189         //std::cout << "STOP IMAGE STREAM" << std::endl;
00190         should_stream_video_ = 
00191           (isImageStreamRunning()) ? false : streaming_video_;
00192       }
00193 
00194       void startImageStream() {
00195         boost::lock_guard<boost::recursive_mutex> lock(m_settings_);
00196         //std::cout << "START IMAGE STREAM" << std::endl;
00197         new_video_format_ = FREENECT_VIDEO_BAYER;
00198         should_stream_video_ = true;
00199       }
00200 
00201       bool isImageStreamRunning() {
00202         boost::lock_guard<boost::recursive_mutex> lock(m_settings_);
00203         return streaming_video_ && _isImageModeEnabled() && !device_flush_enabled_;
00204       }
00205 
00206       void stopIRStream() {
00207         boost::lock_guard<boost::recursive_mutex> lock(m_settings_);
00208         should_stream_video_ = 
00209           (isIRStreamRunning()) ? false : streaming_video_;
00210       }
00211 
00212       void startIRStream() {
00213         boost::lock_guard<boost::recursive_mutex> lock(m_settings_);
00214         new_video_format_ = FREENECT_VIDEO_IR_10BIT;
00215         should_stream_video_ = true;
00216       }
00217 
00218       bool isIRStreamRunning() {
00219         boost::lock_guard<boost::recursive_mutex> lock(m_settings_);
00220         return streaming_video_ && !_isImageModeEnabled();
00221       }
00222 
00223       /* DEPTH SETTINGS FUNCTIONS */
00224 
00225       OutputMode getDepthOutputMode() {
00226         boost::lock_guard<boost::recursive_mutex> lock(m_settings_);
00227         return depth_buffer_.metadata.resolution;
00228       }
00229 
00230       void setDepthOutputMode(OutputMode mode) {
00231         boost::lock_guard<boost::recursive_mutex> lock(m_settings_);
00232         new_depth_resolution_ = mode;
00233       }
00234 
00235       OutputMode getDefaultDepthMode() const {
00236         return FREENECT_RESOLUTION_MEDIUM;
00237       }
00238 
00239       bool findCompatibleDepthMode(const OutputMode& mode, OutputMode& compatible_mode) const {
00240         freenect_frame_mode new_mode = 
00241           freenect_find_depth_mode(mode, depth_buffer_.metadata.depth_format);
00242         if (!new_mode.is_valid) {
00243           compatible_mode = getDefaultDepthMode();
00244           return false;
00245         }
00246         compatible_mode = mode;
00247         return true;
00248       }
00249 
00250       bool isDepthRegistered() {
00251         boost::lock_guard<boost::recursive_mutex> lock(m_settings_);
00252         return depth_buffer_.metadata.depth_format == FREENECT_DEPTH_REGISTERED;
00253       }
00254 
00255       void setDepthRegistration(bool enable) {
00256         boost::lock_guard<boost::recursive_mutex> lock(m_settings_);
00257         new_depth_format_ = 
00258           (enable) ? FREENECT_DEPTH_REGISTERED : FREENECT_DEPTH_MM;
00259       }
00260 
00261       void stopDepthStream() {
00262         boost::lock_guard<boost::recursive_mutex> lock(m_settings_);
00263         should_stream_depth_ = false;
00264       }
00265 
00266       void startDepthStream() {
00267         boost::lock_guard<boost::recursive_mutex> lock(m_settings_);
00268         should_stream_depth_ = true;
00269       }
00270 
00271       bool isDepthStreamRunning() {
00272         boost::lock_guard<boost::recursive_mutex> lock(m_settings_);
00273         return streaming_depth_ && !device_flush_enabled_;
00274       }
00275 
00276       /* LIBFREENECT ASYNC CALLBACKS */
00277 
00278       static void freenectDepthCallback(
00279           freenect_device *dev, void *depth, uint32_t timestamp) {
00280 
00281         FreenectDevice* device = 
00282             static_cast<FreenectDevice*>(freenect_get_user(dev));
00283         device->depthCallback(depth);
00284       }
00285 
00286       static void freenectVideoCallback(
00287           freenect_device *dev, void *video, uint32_t timestamp) {
00288 
00289         FreenectDevice* device = 
00290             static_cast<FreenectDevice*>(freenect_get_user(dev));
00291         device->videoCallback(video);
00292       }
00293 
00294     private:
00295 
00296       friend class FreenectDriver;
00297 
00298       freenect_context* driver_;
00299       freenect_device* device_;
00300       std::string device_serial_;
00301       freenect_registration registration_;
00302 
00303       boost::function<void(const ImageBuffer&)> image_callback_;
00304       boost::function<void(const ImageBuffer&)> depth_callback_;
00305       boost::function<void(const ImageBuffer&)> ir_callback_;
00306 
00307       ImageBuffer video_buffer_;
00308       bool streaming_video_;
00309       bool should_stream_video_;
00310       freenect_resolution new_video_resolution_;
00311       freenect_video_format new_video_format_; 
00312 
00313       ImageBuffer depth_buffer_;
00314       bool streaming_depth_;
00315       bool should_stream_depth_;
00316       freenect_resolution new_depth_resolution_;
00317       freenect_depth_format new_depth_format_;
00318 
00319       /* Prevents changing settings unless the freenect thread in the driver
00320        * is ready */
00321       boost::recursive_mutex m_settings_;
00322 
00323       boost::posix_time::ptime device_flush_start_time_;
00324       bool device_flush_enabled_;
00325       bool publishers_ready_;
00326 
00327       void executeChanges() {
00328         boost::lock_guard<boost::recursive_mutex> lock(m_settings_);
00329         //ROS_INFO_THROTTLE(1.0, "exec changes");
00330 
00331         bool stop_device_flush = false;
00332 
00333         if (device_flush_enabled_) {
00334           boost::posix_time::ptime current_time = 
00335               boost::posix_time::microsec_clock::local_time(); 
00336           if ((current_time - device_flush_start_time_).total_milliseconds() > 3000) {
00337             device_flush_enabled_ = false;
00338             stop_device_flush = true;
00339             ROS_INFO("Stopping device RGB and Depth stream flush.");
00340           }
00341         }
00342 
00343         bool change_video_settings = 
00344           video_buffer_.metadata.video_format != new_video_format_ ||
00345           video_buffer_.metadata.resolution != new_video_resolution_ ||
00346           ((streaming_video_ != should_stream_video_) && !device_flush_enabled_) ||
00347           device_flush_enabled_ && !streaming_video_ ||
00348           stop_device_flush;
00349 
00350         if (change_video_settings) {
00352           // Stop video stream
00353           if (streaming_video_) {
00354             //ROS_INFO("  stopping video images...");
00355             freenect_stop_video(device_);
00356             streaming_video_ = false;
00357             return;
00358           }
00359           // Allocate buffer for video if settings have changed
00360           if (video_buffer_.metadata.resolution != new_video_resolution_ ||
00361               video_buffer_.metadata.video_format != new_video_format_) {
00362             try {
00363               allocateBufferVideo(video_buffer_, new_video_format_, 
00364                  new_video_resolution_, registration_);
00365             } catch (std::runtime_error& e) {
00366               printf("[ERROR] Unsupported video format/resolution provided. %s\n",
00367                   e.what());
00368               printf("[INFO] Setting default settings (RGB/VGA)\n");
00369               allocateBufferVideo(video_buffer_, FREENECT_VIDEO_BAYER,
00370                   FREENECT_RESOLUTION_MEDIUM, registration_);
00371             }
00372             freenect_set_video_mode(device_, video_buffer_.metadata);
00373             freenect_set_video_buffer(device_, video_buffer_.image_buffer.get());
00374             new_video_resolution_ = video_buffer_.metadata.resolution;
00375             new_video_format_ = video_buffer_.metadata.video_format;
00376           }
00377           // Restart stream if required
00378           if (should_stream_video_ || device_flush_enabled_) {
00379             int rgb_out = freenect_start_video(device_);
00380             //ROS_INFO("  streaming rgb images... %i", rgb_out);
00381             streaming_video_ = true;
00382           }
00383 
00384           // Don't make more than 1 change at a time
00385           return;
00386         }
00387 
00388         bool change_depth_settings = 
00389           depth_buffer_.metadata.depth_format != new_depth_format_ ||
00390           depth_buffer_.metadata.resolution != new_depth_resolution_ ||
00391           ((streaming_depth_ != should_stream_depth_) && !device_flush_enabled_) ||
00392           device_flush_enabled_ && !streaming_depth_ ||
00393           stop_device_flush;
00394 
00395         if (change_depth_settings) {
00396           //ROS_INFO("change depth called");
00397           // Stop depth stream
00398           if (streaming_depth_) {
00399             //ROS_INFO("  stopping depth images...");
00400             freenect_stop_depth(device_);
00401             streaming_depth_ = false;
00402             return;
00403           }
00404           // Allocate buffer for depth if settings have changed
00405           if (depth_buffer_.metadata.resolution != new_depth_resolution_ ||
00406               depth_buffer_.metadata.depth_format != new_depth_format_) {
00407             try {
00408               allocateBufferDepth(depth_buffer_, new_depth_format_, 
00409                  new_depth_resolution_, registration_);
00410             } catch (std::runtime_error& e) {
00411               printf("[ERROR] Unsupported depth format/resolution provided. %s\n",
00412                   e.what());
00413               printf("[INFO] Setting default settings (depth registered/VGA)\n");
00414               allocateBufferDepth(depth_buffer_, FREENECT_DEPTH_MM,
00415                   FREENECT_RESOLUTION_MEDIUM, registration_);
00416             }
00417             freenect_set_depth_mode(device_, depth_buffer_.metadata);
00418             freenect_set_depth_buffer(device_, depth_buffer_.image_buffer.get());
00419             new_depth_resolution_ = depth_buffer_.metadata.resolution;
00420             new_depth_format_ = depth_buffer_.metadata.depth_format;
00421           }
00422           // Restart stream if required
00423           if (should_stream_depth_ || device_flush_enabled_) {
00424             int depth_out = freenect_start_depth(device_);
00425             //ROS_INFO("  streaming depth images... %i", depth_out);
00426             streaming_depth_ = true;
00427             return;
00428           }
00429         }
00430       }
00431 
00432       bool _isImageModeEnabled() {
00433         boost::lock_guard<boost::recursive_mutex> lock(m_settings_);
00434         return isImageMode(video_buffer_);
00435       }
00436 
00437       void depthCallback(void* depth) {
00438         boost::lock_guard<boost::mutex> buffer_lock(depth_buffer_.mutex);
00439         assert(depth == depth_buffer_.image_buffer.get());
00440         if (publishers_ready_) {
00441           depth_callback_.operator()(depth_buffer_);
00442         }
00443       }
00444 
00445       void videoCallback(void* video) {
00446         boost::lock_guard<boost::mutex> buffer_lock(video_buffer_.mutex);
00447         assert(video == video_buffer_.image_buffer.get());
00448         if (publishers_ready_) {
00449           if (isImageMode(video_buffer_)) {
00450             image_callback_.operator()(video_buffer_);
00451           } else {
00452             ir_callback_.operator()(video_buffer_);
00453           }
00454         }
00455       }
00456 
00457   };
00458 }
00459 
00460 #endif /* end of include guard: FREENECT_DEVICE_T01IELX0 */


freenect_camera
Author(s): Patrick Mihelich, Suat Gedikli, Radu Bogdan Rusu (original openni_camera driver)., Piyush Khandelwal (libfreenect port).
autogenerated on Mon Oct 6 2014 00:00:36