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 
00047 // Configuration
00048 #include <camera_info_manager/camera_info_manager.h>
00049 #include <dynamic_reconfigure/server.h>
00050 #include <openni_camera/OpenNIConfig.h>
00051 
00052 // OpenNI
00053 #include "openni_camera/openni_driver.h"
00054 
00055 namespace openni_camera
00056 {
00058   class DriverNodelet : public nodelet::Nodelet
00059   {
00060     public:
00061       virtual ~DriverNodelet ();
00062     private:
00063       typedef OpenNIConfig Config;
00064       typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
00065 
00067       virtual void onInit ();
00068       void onInitImpl ();
00069       void setupDevice ();
00070       void updateModeMaps ();
00071       void startSynchronization ();
00072       void stopSynchronization ();
00073       void setupDeviceModes (int image_mode, int depth_mode);
00074 
00076       int mapXnMode2ConfigMode (const XnMapOutputMode& output_mode) const;
00077       XnMapOutputMode mapConfigMode2XnMode (int mode) const;
00078 
00079       // Callback methods
00080       void rgbCb(boost::shared_ptr<openni_wrapper::Image> image, void* cookie);
00081       void depthCb(boost::shared_ptr<openni_wrapper::DepthImage> depth_image, void* cookie);
00082       void irCb(boost::shared_ptr<openni_wrapper::IRImage> ir_image, void* cookie);
00083       void configCb(Config &config, uint32_t level);
00084 
00085       void rgbConnectCb();
00086       void depthConnectCb();
00087       void irConnectCb();
00088 
00089       // Methods to get calibration parameters for the various cameras
00090       sensor_msgs::CameraInfoPtr getDefaultCameraInfo(int width, int height, double f) const;
00091       sensor_msgs::CameraInfoPtr getRgbCameraInfo(int width, int height, ros::Time time) const;
00092       sensor_msgs::CameraInfoPtr getIrCameraInfo(int width, int height, ros::Time time) const;
00093       sensor_msgs::CameraInfoPtr getDepthCameraInfo(int width, int height, ros::Time time) const;
00094       sensor_msgs::CameraInfoPtr getProjectorCameraInfo(int width, int height, ros::Time time) const;
00095 
00096       // published topics
00097       image_transport::CameraPublisher pub_rgb_;
00098       image_transport::CameraPublisher pub_depth_, pub_depth_registered_;
00099       image_transport::CameraPublisher pub_ir_;
00100       ros::Publisher pub_projector_info_;
00101 
00102       // publish methods
00103       void publishRgbImage(const openni_wrapper::Image& image, ros::Time time) const;
00104       void publishDepthImage(const openni_wrapper::DepthImage& depth, ros::Time time) const;
00105       void publishIrImage(const openni_wrapper::IRImage& ir, ros::Time time) const;
00106 
00108       boost::shared_ptr<openni_wrapper::OpenNIDevice> device_;
00109       boost::thread init_thread_;
00110       boost::mutex connect_mutex_;
00111 
00113       boost::shared_ptr<ReconfigureServer> reconfigure_server_;
00114       Config config_;
00115       bool config_init_;
00116 
00118       boost::shared_ptr<camera_info_manager::CameraInfoManager> rgb_info_manager_, ir_info_manager_;
00119       std::string rgb_frame_id_;
00120       std::string depth_frame_id_;
00121       double depth_ir_offset_x_;
00122       double depth_ir_offset_y_;
00123       int z_offset_mm_;
00124       double z_scaling_;
00125 
00126       // The desired image dimensions
00127       // (might be different from what the driver gives us).
00128       unsigned image_width_;
00129       unsigned image_height_;
00130       unsigned depth_width_;
00131       unsigned depth_height_;
00132 
00133       // Counters/flags for skipping frames
00134       boost::mutex counter_mutex_;
00135       int rgb_frame_counter_;
00136       int depth_frame_counter_;
00137       int ir_frame_counter_;
00138       bool publish_rgb_;
00139       bool publish_ir_;
00140       bool publish_depth_;
00141       void checkFrameCounters();
00142 
00143       void watchDog(const ros::TimerEvent& event);
00144 
00146       double time_out_;
00147       ros::Time time_stamp_;
00148       ros::Timer watch_dog_timer_;
00149 
00150       struct modeComp
00151       {
00152         bool operator () (const XnMapOutputMode& mode1, const XnMapOutputMode& mode2) const
00153         {
00154           if (mode1.nXRes < mode2.nXRes)
00155             return true;
00156           else if (mode1.nXRes > mode2.nXRes)
00157             return false;
00158           else if (mode1.nYRes < mode2.nYRes)
00159             return true;
00160           else if (mode1.nYRes > mode2.nYRes)
00161             return false;
00162           else if (mode1.nFPS < mode2.nFPS)
00163             return true;
00164           else
00165             return false;
00166         }
00167       };
00168       std::map<XnMapOutputMode, int, modeComp> xn2config_map_;
00169       std::map<int, XnMapOutputMode> config2xn_map_;
00170   };
00171 }
00172 
00173 #endif


openni_camera
Author(s): Patrick Mihelich, Suat Gedikli, Radu Bogdan Rusu
autogenerated on Wed Aug 26 2015 15:08:49