driver.h
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011 Willow Garage, Inc.
00005  *    Radu Bogdan Rusu <rusu@willowgarage.com>
00006  *    Suat Gedikli <gedikli@willowgarage.com>
00007  *    Patrick Mihelich <mihelich@willowgarage.com>
00008  *
00009  *  All rights reserved.
00010  *
00011  *  Redistribution and use in source and binary forms, with or without
00012  *  modification, are permitted provided that the following conditions
00013  *  are met:
00014  *
00015  *   * Redistributions of source code must retain the above copyright
00016  *     notice, this list of conditions and the following disclaimer.
00017  *   * Redistributions in binary form must reproduce the above
00018  *     copyright notice, this list of conditions and the following
00019  *     disclaimer in the documentation and/or other materials provided
00020  *     with the distribution.
00021  *   * Neither the name of Willow Garage, Inc. nor the names of its
00022  *     contributors may be used to endorse or promote products derived
00023  *     from this software without specific prior written permission.
00024  *
00025  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00026  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00027  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00028  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00029  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00030  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00031  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00032  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00033  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00034  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00035  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00036  *  POSSIBILITY OF SUCH DAMAGE.
00037  *
00038  */
00039 #ifndef OPENNI_CAMERA_DRIVER_H
00040 #define OPENNI_CAMERA_DRIVER_H
00041 
00042 // ROS communication
00043 #include <ros/ros.h>
00044 #include <nodelet/nodelet.h>
00045 #include <image_transport/image_transport.h>
00046 #include <boost/thread.hpp>
00047 
00048 // Configuration
00049 #include <camera_info_manager/camera_info_manager.h>
00050 #include <dynamic_reconfigure/server.h>
00051 #include <freenect_camera/FreenectConfig.h>
00052 
00053 // freenect wrapper
00054 #include <freenect_camera/freenect_driver.hpp>
00055 
00056 // diagnostics
00057 #include <diagnostic_updater/diagnostic_updater.h>
00058 #include <diagnostic_updater/publisher.h>
00059 
00060 namespace freenect_camera
00061 {
00063   class DriverNodelet : public nodelet::Nodelet
00064   {
00065     public:
00066       virtual ~DriverNodelet ();
00067     private:
00068       typedef FreenectConfig Config;
00069       typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
00070       typedef diagnostic_updater::FrequencyStatusParam FrequencyStatusParam;
00071       typedef diagnostic_updater::HeaderlessTopicDiagnostic TopicDiagnostic;
00072       typedef boost::shared_ptr<TopicDiagnostic> TopicDiagnosticPtr;
00073 
00075       virtual void onInit ();
00076       void onInitImpl ();
00077       void setupDevice ();
00078       void updateModeMaps ();
00079       void startSynchronization ();
00080       void stopSynchronization ();
00081 
00083       int mapMode2ConfigMode (const OutputMode& output_mode) const;
00084       OutputMode mapConfigMode2OutputMode (int mode) const;
00085 
00086       // Callback methods
00087       void rgbCb(const ImageBuffer& image, void* cookie);
00088       void depthCb(const ImageBuffer& depth_image, void* cookie);
00089       void irCb(const ImageBuffer&_image, void* cookie);
00090       void configCb(Config &config, uint32_t level);
00091 
00092       void rgbConnectCb();
00093       void depthConnectCb();
00094       void irConnectCb();
00095 
00096       // Methods to get calibration parameters for the various cameras
00097       sensor_msgs::CameraInfoPtr getDefaultCameraInfo(int width, int height, double f) const;
00098       sensor_msgs::CameraInfoPtr getRgbCameraInfo(const ImageBuffer& image, ros::Time time) const;
00099       sensor_msgs::CameraInfoPtr getIrCameraInfo(const ImageBuffer& image, ros::Time time) const;
00100       sensor_msgs::CameraInfoPtr getDepthCameraInfo(const ImageBuffer& image, ros::Time time) const;
00101       sensor_msgs::CameraInfoPtr getProjectorCameraInfo(const ImageBuffer& image, ros::Time time) const;
00102 
00103       // published topics
00104       image_transport::CameraPublisher pub_rgb_;
00105       image_transport::CameraPublisher pub_depth_, pub_depth_registered_;
00106       image_transport::CameraPublisher pub_ir_;
00107       ros::Publisher pub_projector_info_;
00108 
00109       // Maintain frequency diagnostics on all sensors
00110       boost::shared_ptr<diagnostic_updater::Updater> diagnostic_updater_;
00111       double pub_freq_max_, pub_freq_min_;
00112       TopicDiagnosticPtr pub_rgb_freq_;
00113       bool enable_rgb_diagnostics_;
00114       TopicDiagnosticPtr pub_depth_freq_;
00115       bool enable_depth_diagnostics_;
00116       TopicDiagnosticPtr pub_ir_freq_;
00117       bool enable_ir_diagnostics_;
00118       boost::thread diagnostics_thread_;
00119       void updateDiagnostics();
00120       bool close_diagnostics_;
00121 
00122       // publish methods
00123       void publishRgbImage(const ImageBuffer& image, ros::Time time) const;
00124       void publishDepthImage(const ImageBuffer& depth, ros::Time time) const;
00125       void publishIrImage(const ImageBuffer& ir, ros::Time time) const;
00126 
00128       boost::shared_ptr<FreenectDevice> device_;
00129       boost::thread init_thread_;
00130       boost::mutex connect_mutex_;
00131 
00133       boost::shared_ptr<ReconfigureServer> reconfigure_server_;
00134       Config config_;
00135 
00137       boost::shared_ptr<camera_info_manager::CameraInfoManager> rgb_info_manager_, ir_info_manager_;
00138       std::string rgb_frame_id_;
00139       std::string depth_frame_id_;
00140       double depth_ir_offset_x_;
00141       double depth_ir_offset_y_;
00142       int z_offset_mm_;
00143 
00144       // Counters/flags for skipping frames
00145       boost::mutex counter_mutex_;
00146       int rgb_frame_counter_;
00147       int depth_frame_counter_;
00148       int ir_frame_counter_;
00149       bool publish_rgb_;
00150       bool publish_ir_;
00151       bool publish_depth_;
00152       void checkFrameCounters();
00153 
00154       void watchDog(const ros::TimerEvent& event);
00155 
00157       double time_out_;
00158       ros::Time rgb_time_stamp_;
00159       ros::Time depth_time_stamp_;
00160       ros::Time ir_time_stamp_;
00161       ros::Time time_stamp_;
00162       ros::Timer watch_dog_timer_;
00163 
00165       bool libfreenect_debug_;
00166 
00167       std::map<OutputMode, int> mode2config_map_;
00168       std::map<int, OutputMode> config2mode_map_;
00169   };
00170 }
00171 
00172 #endif


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