Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039 #ifndef OPENNI_CAMERA_DRIVER_H
00040 #define OPENNI_CAMERA_DRIVER_H
00041
00042
00043 #include <ros/ros.h>
00044 #include <nodelet/nodelet.h>
00045 #include <image_transport/image_transport.h>
00046
00047
00048 #include <camera_info_manager/camera_info_manager.h>
00049 #include <dynamic_reconfigure/server.h>
00050 #include <openni_camera/OpenNIConfig.h>
00051
00052
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
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
00090 sensor_msgs::CameraInfoPtr getDefaultCameraInfo(int width, int height, double f) const;
00091 sensor_msgs::CameraInfoPtr getRgbCameraInfo(ros::Time time) const;
00092 sensor_msgs::CameraInfoPtr getIrCameraInfo(ros::Time time) const;
00093 sensor_msgs::CameraInfoPtr getDepthCameraInfo(ros::Time time) const;
00094 sensor_msgs::CameraInfoPtr getProjectorCameraInfo(ros::Time time) const;
00095
00096
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
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
00117 boost::shared_ptr<camera_info_manager::CameraInfoManager> rgb_info_manager_, ir_info_manager_;
00118 std::string rgb_frame_id_;
00119 std::string depth_frame_id_;
00120 double depth_ir_offset_x_;
00121 double depth_ir_offset_y_;
00122 int z_offset_mm_;
00123
00125 unsigned image_width_;
00126 unsigned image_height_;
00127 unsigned depth_width_;
00128 unsigned depth_height_;
00129
00130
00131 boost::mutex counter_mutex_;
00132 int rgb_frame_counter_;
00133 int depth_frame_counter_;
00134 int ir_frame_counter_;
00135 bool publish_rgb_;
00136 bool publish_ir_;
00137 bool publish_depth_;
00138 void checkFrameCounters();
00139
00140 void watchDog(const ros::TimerEvent& event);
00141
00143 double time_out_;
00144 ros::Time time_stamp_;
00145 ros::Timer watch_dog_timer_;
00146
00147 struct modeComp
00148 {
00149 bool operator () (const XnMapOutputMode& mode1, const XnMapOutputMode& mode2) const
00150 {
00151 if (mode1.nXRes < mode2.nXRes)
00152 return true;
00153 else if (mode1.nXRes > mode2.nXRes)
00154 return false;
00155 else if (mode1.nYRes < mode2.nYRes)
00156 return true;
00157 else if (mode1.nYRes > mode2.nYRes)
00158 return false;
00159 else if (mode1.nFPS < mode2.nFPS)
00160 return true;
00161 else
00162 return false;
00163 }
00164 };
00165 std::map<XnMapOutputMode, int, modeComp> xn2config_map_;
00166 std::map<int, XnMapOutputMode> config2xn_map_;
00167 };
00168 }
00169
00170 #endif