camera.h
Go to the documentation of this file.
00001 
00034 #ifndef MULTISENSE_ROS_CAMERA_H
00035 #define MULTISENSE_ROS_CAMERA_H
00036 
00037 #include <boost/shared_ptr.hpp>
00038 #include <boost/thread.hpp>
00039 #include <ros/ros.h>
00040 #include <multisense_ros/RawCamData.h>
00041 #include <sensor_msgs/PointCloud2.h>
00042 #include <stereo_msgs/DisparityImage.h>
00043 #include <image_geometry/stereo_camera_model.h>
00044 #include <image_transport/image_transport.h>
00045 #include <image_transport/camera_publisher.h>
00046 #include <sensor_msgs/distortion_models.h>
00047 
00048 
00049 #include <multisense_lib/MultiSenseChannel.hh>
00050 
00051 namespace multisense_ros {
00052 
00053 class Camera {
00054 public:
00055     Camera(crl::multisense::Channel* driver,
00056            const std::string& tf_prefix);
00057     ~Camera();
00058 
00059     void resolutionChanged() { queryConfig(); };
00060 
00061     void monoCallback(const crl::multisense::image::Header& header);
00062     void rectCallback(const crl::multisense::image::Header& header);
00063     void depthCallback(const crl::multisense::image::Header& header);
00064     void pointCloudCallback(const crl::multisense::image::Header& header);
00065     void rawCamDataCallback(const crl::multisense::image::Header& header);
00066     void colorImageCallback(const crl::multisense::image::Header& header);
00067     void disparityImageCallback(const crl::multisense::image::Header& header);
00068     void jpegImageCallback(const crl::multisense::image::Header& header);
00069     void histogramCallback(const crl::multisense::image::Header& header);
00070 
00071     void borderClipChanged(int borderClipType, double borderClipValue);
00072 
00073 private:
00074 
00075     //
00076     // Device stream control
00077 
00078     void connectStream(crl::multisense::DataSource enableMask);
00079     void disconnectStream(crl::multisense::DataSource disableMask);
00080     void stop();
00081 
00082     //
00083     // Query sensor status and calibration
00084 
00085     void queryConfig();
00086 
00087     //
00088     // Update a specific camera info topic. This is used once the camera resolution has changed.
00089     // Scale factors in x and y are applied to M and P camera matrices. M and P
00090     // matrices are assumed to be full resolution
00091 
00092     void updateCameraInfo(sensor_msgs::CameraInfo& cameraInfo,
00093                           const float M[3][3],
00094                           const float R[3][3],
00095                           const float P[3][4],
00096                           const float D[8],
00097                           double xScale=1,
00098                           double yScale=1);
00099 
00100     //
00101     // Republish camera info messages by publishing the current messages
00102     // Used whenever the resolution of the camera changes
00103 
00104     void publishAllCameraInfo();
00105 
00106     //
00107     // Generate border clips for point clouds
00108 
00109     void generateBorderClip(int borderClipType, double borderClipValue, uint32_t width, uint32_t height);
00110 
00111 
00112     //
00113     // CRL sensor API
00114 
00115     crl::multisense::Channel* driver_;
00116 
00117     //
00118     // Driver nodes
00119 
00120     ros::NodeHandle device_nh_;
00121     ros::NodeHandle left_nh_;
00122     ros::NodeHandle right_nh_;
00123 
00124     //
00125     // Image transports
00126 
00127     image_transport::ImageTransport  left_mono_transport_;
00128     image_transport::ImageTransport  right_mono_transport_;
00129     image_transport::ImageTransport  left_rect_transport_;
00130     image_transport::ImageTransport  right_rect_transport_;
00131     image_transport::ImageTransport  left_rgb_transport_;
00132     image_transport::ImageTransport  left_rgb_rect_transport_;
00133     image_transport::ImageTransport  depth_transport_;
00134     image_transport::ImageTransport  ni_depth_transport_;
00135     image_transport::ImageTransport  disparity_left_transport_;
00136     image_transport::ImageTransport  disparity_right_transport_;
00137     image_transport::ImageTransport  disparity_cost_transport_;
00138 
00139     //
00140     // Data publishers
00141 
00142     sensor_msgs::CameraInfo          left_mono_cam_info_;
00143     sensor_msgs::CameraInfo          right_mono_cam_info_;
00144     sensor_msgs::CameraInfo          left_rect_cam_info_;
00145     sensor_msgs::CameraInfo          right_rect_cam_info_;
00146     sensor_msgs::CameraInfo          left_rgb_rect_cam_info_;
00147     sensor_msgs::CameraInfo          left_disp_cam_info_;
00148     sensor_msgs::CameraInfo          right_disp_cam_info_;
00149     sensor_msgs::CameraInfo          left_cost_cam_info_;
00150     sensor_msgs::CameraInfo          left_rgb_cam_info_;
00151     sensor_msgs::CameraInfo          depth_cam_info_;
00152 
00153     image_transport::Publisher       left_mono_cam_pub_;
00154     image_transport::Publisher       right_mono_cam_pub_;
00155     image_transport::CameraPublisher left_rect_cam_pub_;
00156     image_transport::CameraPublisher right_rect_cam_pub_;
00157     image_transport::Publisher       depth_cam_pub_;
00158     image_transport::Publisher       ni_depth_cam_pub_; // publish depth infomation in the openNI format
00159     image_transport::Publisher       left_rgb_cam_pub_;
00160     image_transport::CameraPublisher left_rgb_rect_cam_pub_;
00161 
00162     ros::Publisher                   left_mono_cam_info_pub_;
00163     ros::Publisher                   right_mono_cam_info_pub_;
00164     ros::Publisher                   left_rect_cam_info_pub_;
00165     ros::Publisher                   right_rect_cam_info_pub_;
00166     ros::Publisher                   left_disp_cam_info_pub_;
00167     ros::Publisher                   right_disp_cam_info_pub_;
00168     ros::Publisher                   left_cost_cam_info_pub_;
00169     ros::Publisher                   left_rgb_cam_info_pub_;
00170     ros::Publisher                   left_rgb_rect_cam_info_pub_;
00171     ros::Publisher                   depth_cam_info_pub_;
00172 
00173     ros::Publisher                   luma_point_cloud_pub_;
00174     ros::Publisher                   color_point_cloud_pub_;
00175 
00176     ros::Publisher                   luma_organized_point_cloud_pub_;
00177     ros::Publisher                   color_organized_point_cloud_pub_;
00178 
00179     image_transport::Publisher       left_disparity_pub_;
00180     image_transport::Publisher       right_disparity_pub_;
00181     image_transport::Publisher       left_disparity_cost_pub_;
00182 
00183     ros::Publisher                   left_stereo_disparity_pub_;
00184     ros::Publisher                   right_stereo_disparity_pub_;
00185 
00186     //
00187     // Raw data publishers
00188 
00189     ros::Publisher raw_cam_data_pub_;
00190     ros::Publisher raw_cam_config_pub_;
00191     ros::Publisher raw_cam_cal_pub_;
00192     ros::Publisher device_info_pub_;
00193     ros::Publisher histogram_pub_;
00194 
00195     //
00196     // Store outgoing messages for efficiency
00197 
00198     sensor_msgs::Image         left_mono_image_;
00199     sensor_msgs::Image         right_mono_image_;
00200     sensor_msgs::Image         left_rect_image_;
00201     sensor_msgs::Image         right_rect_image_;
00202     sensor_msgs::Image         depth_image_;
00203     sensor_msgs::Image         ni_depth_image_;
00204     sensor_msgs::PointCloud2   luma_point_cloud_;
00205     sensor_msgs::PointCloud2   color_point_cloud_;
00206     sensor_msgs::PointCloud2   luma_organized_point_cloud_;
00207     sensor_msgs::PointCloud2   color_organized_point_cloud_;
00208 
00209     sensor_msgs::Image         left_luma_image_;
00210     sensor_msgs::Image         left_rgb_image_;
00211     sensor_msgs::Image         left_rgb_rect_image_;
00212 
00213     sensor_msgs::Image         left_disparity_image_;
00214     sensor_msgs::Image         left_disparity_cost_image_;
00215     sensor_msgs::Image         right_disparity_image_;
00216 
00217     stereo_msgs::DisparityImage left_stereo_disparity_;
00218     stereo_msgs::DisparityImage right_stereo_disparity_;
00219 
00220     bool                       got_raw_cam_left_;
00221     bool                       got_left_luma_;
00222     int64_t                    left_luma_frame_id_;
00223     int64_t                    left_rect_frame_id_;
00224     int64_t                    left_rgb_rect_frame_id_;
00225     int64_t                    luma_point_cloud_frame_id_;
00226     int64_t                    luma_organized_point_cloud_frame_id_;
00227     int64_t                    color_point_cloud_frame_id_;
00228     int64_t                    color_organized_point_cloud_frame_id_;
00229     multisense_ros::RawCamData raw_cam_data_;
00230 
00231     //
00232     // Calibration from sensor
00233 
00234     crl::multisense::system::VersionInfo version_info_;
00235     crl::multisense::system::DeviceInfo  device_info_;
00236     crl::multisense::image::Config       image_config_;
00237     crl::multisense::image::Calibration  image_calibration_;
00238 
00239     //
00240     // For local rectification of color images
00241 
00242     boost::mutex cal_lock_;
00243     CvMat *calibration_map_left_1_;
00244     CvMat *calibration_map_left_2_;
00245 
00246     //
00247     // The frame IDs
00248 
00249     std::string frame_id_left_;
00250     std::string frame_id_right_;
00251 
00252     //
00253     // For pointcloud generation
00254 
00255     std::vector<float>            disparity_buff_;
00256     std::vector<cv::Vec3f>        points_buff_;
00257     int64_t                       points_buff_frame_id_;
00258     cv::Mat_<double>              q_matrix_;
00259     float                         pc_max_range_;
00260     bool                          pc_color_frame_sync_;
00261 
00262 
00263     //
00264     // Current maximum number of disparities
00265 
00266     uint32_t                      disparities_;
00267 
00268     //
00269     // Stream subscriptions
00270 
00271     typedef std::map<crl::multisense::DataSource, int32_t> StreamMapType;
00272     boost::mutex stream_lock_;
00273     StreamMapType stream_map_;
00274 
00275     //
00276     // Histogram tracking
00277 
00278     int64_t last_frame_id_;
00279 
00280     //
00281     // Luma Color Depth
00282 
00283     uint8_t luma_color_depth_;
00284 
00285     //
00286     // If the color pointcloud data should be written packed. If false
00287     // it will be static_cast to a flat and interpreted literally
00288 
00289     bool write_pc_color_packed_;
00290 
00291     //
00292     // Enum to determine border clipping types
00293 
00294     enum clip_type_ {RECTANGULAR, CIRCULAR};
00295 
00296     int border_clip_type_;
00297     double border_clip_value_;
00298 
00299     //
00300     // The mask used to perform the border clipping of the disparity image
00301 
00302     cv::Mat_<uint8_t> border_clip_mask_;
00303 
00304     boost::mutex border_clip_lock_;
00305 };
00306 
00307 }
00308 
00309 #endif


multisense_ros
Author(s):
autogenerated on Thu Aug 27 2015 14:01:22