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
00040
00041 #ifndef KINECT_NODE_KINECT_H_
00042 #define KINECT_NODE_KINECT_H_
00043
00044 #include <libusb.h>
00045 #include <boost/thread/mutex.hpp>
00046
00047
00048 #include <sensor_msgs/PointCloud.h>
00049 #include <sensor_msgs/PointCloud2.h>
00050 #include <sensor_msgs/Image.h>
00051 #include <sensor_msgs/CameraInfo.h>
00052 #include <sensor_msgs/Imu.h>
00053
00054 #include <ros/ros.h>
00055 #include <ros/package.h>
00056 #include <camera_info_manager/camera_info_manager.h>
00057 #include <image_transport/image_transport.h>
00058 #include <dynamic_reconfigure/server.h>
00059 #include <kinect_camera/KinectConfig.h>
00060
00061
00062 #include <image_geometry/pinhole_camera_model.h>
00063
00064 #include <Eigen/Core>
00065
00066 extern "C"
00067 {
00068 #include "libfreenect.h"
00069 }
00070
00071 namespace kinect_camera
00072 {
00073 class KinectDriver
00074 {
00075 public:
00077 KinectDriver (ros::NodeHandle comm_nh, ros::NodeHandle param_nh);
00078 virtual ~KinectDriver ();
00079
00085 virtual void depthCb (freenect_device *dev, freenect_depth *buf, uint32_t timestamp);
00086
00092 virtual void rgbCb (freenect_device *dev, freenect_pixel *rgb, uint32_t timestamp);
00093
00099 virtual void irCb (freenect_device *dev, freenect_pixel_ir *rgb, uint32_t timestamp);
00100
00102
00103 void start ();
00105 void stop ();
00106
00110 bool init (int index);
00111
00115 inline bool
00116 ok ()
00117 {
00118 freenect_raw_device_state *state;
00119 freenect_update_device_state (f_dev_);
00120 state = freenect_get_device_state (f_dev_);
00121 freenect_get_mks_accel (state, &accel_x_, &accel_y_, &accel_z_);
00122 tilt_angle_ = freenect_get_tilt_degs(state);
00123
00124 publishImu();
00125 return (freenect_process_events (f_ctx_) >= 0);
00126 }
00127
00128 void processRgbAndDepth();
00129
00130 protected:
00132 void publish ();
00133
00134 void publishImu();
00135
00136 private:
00138 boost::mutex buffer_mutex_;
00139
00141 image_transport::CameraPublisher pub_rgb_, pub_depth_, pub_ir_;
00142 image_transport::Publisher pub_rgb_rect_;
00143 ros::Publisher pub_depth_points_, pub_depth_points2_;
00144 ros::Publisher pub_rgb_points_, pub_rgb_points2_;
00145 ros::Publisher pub_imu_;
00146
00148 boost::shared_ptr<CameraInfoManager> rgb_info_manager_, depth_info_manager_;
00149 image_geometry::PinholeCameraModel rgb_model_, depth_model_;
00150
00152 typedef kinect_camera::KinectConfig Config;
00153 typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
00154 ReconfigureServer reconfigure_server_;
00155 Config config_;
00156
00158 int width_;
00159 int height_;
00160 double shift_offset_;
00161 double baseline_;
00162 Eigen::Matrix<double, 3, 4> depth_to_rgb_;
00163 Eigen::Matrix4d S;
00164 static const double SHIFT_SCALE;
00165
00167 freenect_context *f_ctx_;
00168
00170 freenect_device *f_dev_;
00171
00173 bool started_;
00174
00176
00177 sensor_msgs::Image rgb_image_, depth_image_;
00179 sensor_msgs::PointCloud cloud_, cloud_rgb_;
00181 sensor_msgs::PointCloud2 cloud2_, cloud2_rgb_;
00183 sensor_msgs::CameraInfo rgb_info_, depth_info_;
00185 sensor_msgs::Imu imu_msg_;
00186 cv::Mat rgb_rect_;
00187
00188 bool depth_sent_;
00189 bool rgb_sent_;
00190 freenect_depth *depth_buf_;
00191 freenect_pixel *rgb_buf_;
00192
00194 double accel_x_, accel_y_, accel_z_;
00195
00197 double tilt_angle_;
00198
00200 void configCb (Config &config, uint32_t level);
00201
00202 void updateDeviceSettings();
00203
00204 static void depthCbInternal (freenect_device *dev, void *buf, uint32_t timestamp);
00205
00206 static void rgbCbInternal (freenect_device *dev, freenect_pixel *buf, uint32_t timestamp);
00207
00208 static void irCbInternal (freenect_device *dev, freenect_pixel_ir *buf, uint32_t timestamp);
00209
00214 void depthBufferTo8BitImage(const freenect_depth * buf);
00215 public:
00216 EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
00217 };
00218
00219 }
00220
00221 #endif //KINECT_NODE_KINECT_H_