camera.cpp
Go to the documentation of this file.
00001 
00034 #include <multisense_ros/camera.h>
00035 #include <multisense_ros/RawCamConfig.h>
00036 #include <multisense_ros/RawCamCal.h>
00037 #include <multisense_ros/DeviceInfo.h>
00038 #include <multisense_ros/Histogram.h>
00039 
00040 #include <sensor_msgs/distortion_models.h>
00041 #include <sensor_msgs/image_encodings.h>
00042 
00043 #include <multisense_lib/MultiSenseChannel.hh>
00044 
00045 #include <opencv2/opencv.hpp>
00046 #include <arpa/inet.h>
00047 #include <fstream>
00048 #include <turbojpeg.h>
00049 
00050 using namespace crl::multisense;
00051 
00052 namespace multisense_ros {
00053 
00054 namespace { // anonymous
00055 
00056 //
00057 // All of the data sources that we control here
00058 
00059 const DataSource allImageSources = (Source_Luma_Left            |
00060                                     Source_Luma_Right           |
00061                                     Source_Luma_Rectified_Left  |
00062                                     Source_Luma_Rectified_Right |
00063                                     Source_Chroma_Left          |
00064                                     Source_Disparity            |
00065                                     Source_Disparity_Right      |
00066                                     Source_Disparity_Cost       |
00067                                     Source_Jpeg_Left);
00068 
00069 //
00070 // Packed size of point cloud structures
00071 
00072 const uint32_t luma_cloud_step  = 16;
00073 const uint32_t color_cloud_step = 16;
00074 
00075 //
00076 // Shims for C-style driver callbacks
00077 
00078 void monoCB(const image::Header& header, void* userDataP)
00079 { reinterpret_cast<Camera*>(userDataP)->monoCallback(header); }
00080 void rectCB(const image::Header& header, void* userDataP)
00081 { reinterpret_cast<Camera*>(userDataP)->rectCallback(header); }
00082 void depthCB(const image::Header& header, void* userDataP)
00083 { reinterpret_cast<Camera*>(userDataP)->depthCallback(header); }
00084 void pointCB(const image::Header& header, void* userDataP)
00085 { reinterpret_cast<Camera*>(userDataP)->pointCloudCallback(header); }
00086 void rawCB(const image::Header& header, void* userDataP)
00087 { reinterpret_cast<Camera*>(userDataP)->rawCamDataCallback(header); }
00088 void colorCB(const image::Header& header, void* userDataP)
00089 { reinterpret_cast<Camera*>(userDataP)->colorImageCallback(header); }
00090 void dispCB(const image::Header& header, void* userDataP)
00091 { reinterpret_cast<Camera*>(userDataP)->disparityImageCallback(header); }
00092 void jpegCB(const image::Header& header, void* userDataP)
00093 { reinterpret_cast<Camera*>(userDataP)->jpegImageCallback(header); }
00094 void histCB(const image::Header& header, void* userDataP)
00095 { reinterpret_cast<Camera*>(userDataP)->histogramCallback(header); }
00096 
00097 //
00098 // Check for valid range points coming out of OpenCV
00099 
00100 bool isValidPoint(const cv::Vec3f& pt,
00101                   const float&     maxRange)
00102 {
00103     //
00104     // Check both for disparities explicitly marked as invalid (where
00105     // OpenCV maps pt.z to MISSING_Z) and zero disparities (point
00106     // mapped to infinity).
00107 
00108     if (image_geometry::StereoCameraModel::MISSING_Z != pt[2] && std::isfinite(pt[2])) {
00109 
00110         //
00111         // Also filter on reasonable ranges
00112 
00113         const float mag = std::sqrt((pt[0]*pt[0])+(pt[1]*pt[1])+(pt[2]*pt[2]));
00114 
00115         if (mag < maxRange)
00116             return true;
00117     }
00118 
00119     return false;
00120 }
00121 
00122 //
00123 // Publish a point cloud, using the given storage, filtering the points,
00124 // and colorizing the cloud with all available color channels.
00125 //
00126 // Note that the dependencies for the point cloud will be generated in
00127 // different threads.  This function is called each time a dependency
00128 // becomes ready.
00129 //
00130 // The published frame ID for this point cloud type is tracked and
00131 // publishing is serialized here via a mutex to prevent race conditions.
00132 
00133 boost::mutex point_cloud_mutex;
00134 
00135 bool publishPointCloud(int64_t                       imageFrameId,
00136                        int64_t                       pointsFrameId,
00137                        int64_t&                      cloudFrameId,
00138                        ros::Publisher&               pub,
00139                        sensor_msgs::PointCloud2&     cloud,
00140                        const uint32_t                width,
00141                        const uint32_t                height,
00142                        const uint32_t                timeSeconds,
00143                        const uint32_t                timeMicroSeconds,
00144                        const uint32_t                cloudStep,
00145                        const std::vector<cv::Vec3f>& points,
00146                        const uint8_t*                imageP,
00147                        const uint32_t                colorChannels,
00148                        const float                   maxRange,
00149                        bool                          writeColorPacked,
00150                        bool                          organized)
00151 {
00152     boost::mutex::scoped_lock lock(point_cloud_mutex);
00153 
00154     if (0            == pub.getNumSubscribers()  ||
00155         imageFrameId != pointsFrameId            ||
00156         cloudFrameId >= imageFrameId) {
00157         return false;
00158     }
00159 
00160     cloudFrameId = imageFrameId;
00161     const uint32_t imageSize = height * width;
00162 
00163     if (points.size() != imageSize)
00164         return false;
00165 
00166     cloud.data.resize(imageSize * cloudStep);
00167 
00168     uint8_t       *cloudP      = reinterpret_cast<uint8_t*>(&cloud.data[0]);
00169     const uint32_t pointSize   = 3 * sizeof(float); // x, y, z
00170     uint32_t       validPoints = 0;
00171 
00172     cv::Vec3f nanPoint(std::numeric_limits<float>::quiet_NaN(),
00173                        std::numeric_limits<float>::quiet_NaN(),
00174                        std::numeric_limits<float>::quiet_NaN());
00175 
00176 
00177     for(uint32_t i=0; i<height; ++i)
00178         for(uint32_t j=0; j<width; ++j) {
00179 
00180             const uint32_t index = i * width + j;
00181 
00182             const uint32_t* pointP = reinterpret_cast<const uint32_t*>(&points[index]);
00183             uint32_t* targetCloudP = reinterpret_cast<uint32_t*>(cloudP);
00184 
00185 
00186             //
00187             // When creating an organized pointcloud replace invalid points
00188             // with NaN points
00189 
00190             if (false == isValidPoint(points[index], maxRange))
00191             {
00192                 if (organized)
00193                 {
00194                     pointP = reinterpret_cast<const uint32_t*>(&nanPoint[0]);
00195                 }
00196                 else
00197                 {
00198                     continue;
00199                 }
00200             }
00201 
00202 
00203             //
00204             // Directly copy points to eliminate memcpy
00205 
00206             targetCloudP[0] = pointP[0];
00207             targetCloudP[1] = pointP[1];
00208             targetCloudP[2] = pointP[2];
00209 
00210 
00211             const uint8_t *sourceColorP = &(imageP[colorChannels * index]);
00212             uint8_t       *cloudColorP  = (cloudP + pointSize);
00213 
00214             //
00215             // Write the poincloud packed if specified or the color image
00216             // is BGR. Copying is optimized to eliminate memcpy operations
00217             // increasing overall speed
00218 
00219             if (writeColorPacked || colorChannels > 2)
00220             {
00221                 switch(colorChannels)
00222                 {
00223                     case 4:
00224                         cloudColorP[3] = sourceColorP[3];
00225                     case 3:
00226                         cloudColorP[2] = sourceColorP[2];
00227                     case 2:
00228                         cloudColorP[1] = sourceColorP[1];
00229                     case 1:
00230                         cloudColorP[0] = sourceColorP[0];
00231                 }
00232 
00233             } else {
00234                 union
00235                 {
00236                     uint32_t value;
00237                     char bytes[sizeof(uint32_t)];
00238                 } color;
00239 
00240                 color.value = 0;
00241 
00242                 //
00243                 // We only need to copy 2 values since this case only
00244                 // applies for images with color channels of 1 or 2 bytes
00245 
00246                 color.bytes[0] = sourceColorP[0];
00247                 color.bytes[1] = sourceColorP[1] & ((colorChannels > 1) * 255);
00248 
00249                 float* floatCloudColorP = reinterpret_cast<float*>(cloudColorP);
00250                 floatCloudColorP[0] = static_cast<float>(color.value);
00251             }
00252 
00253             cloudP += cloudStep;
00254             validPoints ++;
00255         }
00256 
00257     if (!organized) {
00258         cloud.row_step     = validPoints * cloudStep;
00259         cloud.width        = validPoints;
00260         cloud.height       = 1;
00261     } else {
00262         cloud.width = width;
00263         cloud.height = height;
00264         cloud.row_step = width * cloudStep;
00265     }
00266 
00267     cloud.header.stamp = ros::Time(timeSeconds, 1000 * timeMicroSeconds);
00268     cloud.data.resize(validPoints * cloudStep);
00269     pub.publish(cloud);
00270 
00271     return true;
00272 }
00273 
00274 bool savePgm(const std::string& fileName,
00275              uint32_t           width,
00276              uint32_t           height,
00277              uint32_t           bitsPerPixel,
00278              const void        *dataP)
00279 {
00280     std::ofstream outputStream(fileName.c_str(), std::ios::binary | std::ios::out);
00281 
00282     if (false == outputStream.good()) {
00283         fprintf(stderr, "failed to open \"%s\"\n", fileName.c_str());
00284         return false;
00285     }
00286 
00287     const uint32_t imageSize = height * width;
00288 
00289     switch(bitsPerPixel) {
00290     case 8:
00291     {
00292 
00293         outputStream << "P5\n"
00294                      << width << " " << height << "\n"
00295                      << 0xFF << "\n";
00296 
00297         outputStream.write(reinterpret_cast<const char*>(dataP), imageSize);
00298 
00299         break;
00300     }
00301     case 16:
00302     {
00303         outputStream << "P5\n"
00304                      << width << " " << height << "\n"
00305                      << 0xFFFF << "\n";
00306 
00307         const uint16_t *imageP = reinterpret_cast<const uint16_t*>(dataP);
00308 
00309         for (uint32_t i=0; i<imageSize; ++i) {
00310             uint16_t o = htons(imageP[i]);
00311             outputStream.write(reinterpret_cast<const char*>(&o), sizeof(uint16_t));
00312         }
00313 
00314         break;
00315     }
00316     }
00317 
00318     outputStream.close();
00319     return true;
00320 }
00321 
00322 }; // anonymous
00323 
00324 Camera::Camera(Channel* driver,
00325                const std::string& tf_prefix) :
00326     driver_(driver),
00327     device_nh_(""),
00328     left_nh_(device_nh_, "left"),
00329     right_nh_(device_nh_, "right"),
00330     left_mono_transport_(left_nh_),
00331     right_mono_transport_(right_nh_),
00332     left_rect_transport_(left_nh_),
00333     right_rect_transport_(right_nh_),
00334     left_rgb_transport_(left_nh_),
00335     left_rgb_rect_transport_(left_nh_),
00336     depth_transport_(device_nh_),
00337     ni_depth_transport_(device_nh_),
00338     disparity_left_transport_(left_nh_),
00339     disparity_right_transport_(right_nh_),
00340     disparity_cost_transport_(left_nh_),
00341     left_mono_cam_info_(),
00342     right_mono_cam_info_(),
00343     left_rect_cam_info_(),
00344     right_rect_cam_info_(),
00345     left_rgb_rect_cam_info_(),
00346     left_disp_cam_info_(),
00347     right_disp_cam_info_(),
00348     left_cost_cam_info_(),
00349     left_rgb_cam_info_(),
00350     depth_cam_info_(),
00351     left_mono_cam_pub_(),
00352     right_mono_cam_pub_(),
00353     left_rect_cam_pub_(),
00354     right_rect_cam_pub_(),
00355     depth_cam_pub_(),
00356     ni_depth_cam_pub_(),
00357     left_rgb_cam_pub_(),
00358     left_rgb_rect_cam_pub_(),
00359     left_mono_cam_info_pub_(),
00360     right_mono_cam_info_pub_(),
00361     left_rect_cam_info_pub_(),
00362     right_rect_cam_info_pub_(),
00363     left_disp_cam_info_pub_(),
00364     right_disp_cam_info_pub_(),
00365     left_cost_cam_info_pub_(),
00366     left_rgb_cam_info_pub_(),
00367     left_rgb_rect_cam_info_pub_(),
00368     depth_cam_info_pub_(),
00369     luma_point_cloud_pub_(),
00370     color_point_cloud_pub_(),
00371     luma_organized_point_cloud_pub_(),
00372     color_organized_point_cloud_pub_(),
00373     left_disparity_pub_(),
00374     right_disparity_pub_(),
00375     left_disparity_cost_pub_(),
00376     left_stereo_disparity_pub_(),
00377     right_stereo_disparity_pub_(),
00378     raw_cam_data_pub_(),
00379     raw_cam_config_pub_(),
00380     raw_cam_cal_pub_(),
00381     device_info_pub_(),
00382     histogram_pub_(),
00383     left_mono_image_(),
00384     right_mono_image_(),
00385     left_rect_image_(),
00386     right_rect_image_(),
00387     depth_image_(),
00388     luma_point_cloud_(),
00389     color_point_cloud_(),
00390     luma_organized_point_cloud_(),
00391     color_organized_point_cloud_(),
00392     left_luma_image_(),
00393     left_rgb_image_(),
00394     left_rgb_rect_image_(),
00395     left_disparity_image_(),
00396     left_disparity_cost_image_(),
00397     right_disparity_image_(),
00398     left_stereo_disparity_(),
00399     right_stereo_disparity_(),
00400     got_raw_cam_left_(false),
00401     got_left_luma_(false),
00402     left_luma_frame_id_(0),
00403     left_rect_frame_id_(0),
00404     left_rgb_rect_frame_id_(-1),
00405     luma_point_cloud_frame_id_(-1),
00406     luma_organized_point_cloud_frame_id_(-1),
00407     color_point_cloud_frame_id_(-1),
00408     color_organized_point_cloud_frame_id_(-1),
00409     raw_cam_data_(),
00410     version_info_(),
00411     device_info_(),
00412     image_config_(),
00413     image_calibration_(),
00414     cal_lock_(),
00415     calibration_map_left_1_(NULL),
00416     calibration_map_left_2_(NULL),
00417     frame_id_left_(),
00418     frame_id_right_(),
00419     disparity_buff_(),
00420     points_buff_(),
00421     points_buff_frame_id_(-1),
00422     q_matrix_(4, 4, 0.0),
00423     pc_max_range_(15.0f),
00424     pc_color_frame_sync_(true),
00425     disparities_(0),
00426     stream_lock_(),
00427     stream_map_(),
00428     last_frame_id_(-1),
00429     luma_color_depth_(1),
00430     write_pc_color_packed_(false),
00431     border_clip_type_(0),
00432     border_clip_value_(0.0),
00433     border_clip_lock_()
00434 {
00435     //
00436     // Query device and version information from sensor
00437 
00438     Status status = driver_->getVersionInfo(version_info_);
00439     if (Status_Ok != status) {
00440         ROS_ERROR("Camera: failed to query version info: %s",
00441                   Channel::statusString(status));
00442         return;
00443     }
00444     status = driver_->getDeviceInfo(device_info_);
00445     if (Status_Ok != status) {
00446         ROS_ERROR("Camera: failed to query device info: %s",
00447                   Channel::statusString(status));
00448         return;
00449     }
00450 
00451     //
00452     // Set frame ID
00453 
00454     frame_id_left_  = tf_prefix + "/left_camera_optical_frame";
00455     frame_id_right_ = tf_prefix + "/right_camera_optical_frame";
00456     ROS_INFO("camera frame id: %s", frame_id_left_.c_str());
00457 
00458     //
00459     // Topics published for all device types
00460 
00461     ros::NodeHandle calibration_nh(device_nh_, "calibration");
00462     device_info_pub_    = calibration_nh.advertise<multisense_ros::DeviceInfo>("device_info", 1, true);
00463     raw_cam_cal_pub_    = calibration_nh.advertise<multisense_ros::RawCamCal>("raw_cam_cal", 1, true);
00464     raw_cam_config_pub_ = calibration_nh.advertise<multisense_ros::RawCamConfig>("raw_cam_config", 1, true);
00465     histogram_pub_      = device_nh_.advertise<multisense_ros::Histogram>("histogram", 5);
00466 
00467     //
00468     // Change the way the luma pointcloud is published for ST21 sensors
00469 
00470     if (system::DeviceInfo::HARDWARE_REV_MULTISENSE_ST21 == device_info_.hardwareRevision) {
00471 
00472         //
00473         // Luma images are 16 bit so when copying to the point cloud
00474         // structure copy 2 bytes
00475 
00476         luma_color_depth_ = 2;
00477     }
00478 
00479     //
00480     // Create topic publishers (TODO: color topics should not be advertised if the device can't support it)
00481 
00482     if (system::DeviceInfo::HARDWARE_REV_BCAM == device_info_.hardwareRevision) {
00483 
00484         left_mono_cam_pub_  = left_mono_transport_.advertise("image_mono", 5,
00485                               boost::bind(&Camera::connectStream, this, Source_Luma_Left),
00486                               boost::bind(&Camera::disconnectStream, this, Source_Luma_Left));
00487 
00488         left_rgb_cam_pub_   = left_rgb_transport_.advertise("image_color", 5,
00489                               boost::bind(&Camera::connectStream, this, Source_Jpeg_Left),
00490                               boost::bind(&Camera::disconnectStream, this, Source_Jpeg_Left));
00491 
00492         left_rgb_rect_cam_pub_ = left_rgb_rect_transport_.advertiseCamera("image_rect_color", 5,
00493                               boost::bind(&Camera::connectStream, this, Source_Jpeg_Left),
00494                               boost::bind(&Camera::disconnectStream, this, Source_Jpeg_Left));
00495 
00496         left_mono_cam_info_pub_  = left_nh_.advertise<sensor_msgs::CameraInfo>("image_mono/camera_info", 1, true);
00497         left_rgb_cam_info_pub_  = left_nh_.advertise<sensor_msgs::CameraInfo>("image_color/camera_info", 1, true);
00498         left_rgb_rect_cam_info_pub_  = left_nh_.advertise<sensor_msgs::CameraInfo>("image_rect_color/camera_info", 1, true);
00499 
00500 
00501     } else if (system::DeviceInfo::HARDWARE_REV_MULTISENSE_M == device_info_.hardwareRevision) {
00502 
00503         // monocular variation
00504 
00505         left_mono_cam_pub_  = left_mono_transport_.advertise("image_mono", 5,
00506                               boost::bind(&Camera::connectStream, this, Source_Luma_Left),
00507                               boost::bind(&Camera::disconnectStream, this, Source_Luma_Left));
00508         left_rect_cam_pub_  = left_rect_transport_.advertiseCamera("image_rect", 5,
00509                               boost::bind(&Camera::connectStream, this, Source_Luma_Rectified_Left),
00510                               boost::bind(&Camera::disconnectStream, this, Source_Luma_Rectified_Left));
00511         left_rgb_cam_pub_   = left_rgb_transport_.advertise("image_color", 5,
00512                               boost::bind(&Camera::connectStream, this, Source_Luma_Left | Source_Chroma_Left),
00513                               boost::bind(&Camera::disconnectStream, this, Source_Luma_Left | Source_Chroma_Left));
00514         left_rgb_rect_cam_pub_ = left_rgb_rect_transport_.advertiseCamera("image_rect_color", 5,
00515                                  boost::bind(&Camera::connectStream, this, Source_Luma_Left | Source_Chroma_Left),
00516                                  boost::bind(&Camera::disconnectStream, this, Source_Luma_Left | Source_Chroma_Left));
00517 
00518         left_mono_cam_info_pub_     = left_nh_.advertise<sensor_msgs::CameraInfo>("image_mono/camera_info", 1, true);
00519         left_rect_cam_info_pub_     = left_nh_.advertise<sensor_msgs::CameraInfo>("image_rect/camera_info", 1, true);
00520         left_rgb_cam_info_pub_      = left_nh_.advertise<sensor_msgs::CameraInfo>("image_color/camera_info", 1, true);
00521         left_rgb_rect_cam_info_pub_ = left_nh_.advertise<sensor_msgs::CameraInfo>("image_rect_color/camera_info", 1, true);
00522 
00523     } else {  // all other MultiSense-S* variations
00524 
00525 
00526         left_mono_cam_pub_  = left_mono_transport_.advertise("image_mono", 5,
00527                               boost::bind(&Camera::connectStream, this, Source_Luma_Left),
00528                               boost::bind(&Camera::disconnectStream, this, Source_Luma_Left));
00529         right_mono_cam_pub_ = right_mono_transport_.advertise("image_mono", 5,
00530                               boost::bind(&Camera::connectStream, this, Source_Luma_Right),
00531                               boost::bind(&Camera::disconnectStream, this, Source_Luma_Right));
00532         left_rect_cam_pub_  = left_rect_transport_.advertiseCamera("image_rect", 5,
00533                               boost::bind(&Camera::connectStream, this, Source_Luma_Rectified_Left),
00534                               boost::bind(&Camera::disconnectStream, this, Source_Luma_Rectified_Left));
00535         right_rect_cam_pub_ = right_rect_transport_.advertiseCamera("image_rect", 5,
00536                               boost::bind(&Camera::connectStream, this, Source_Luma_Rectified_Right),
00537                               boost::bind(&Camera::disconnectStream, this, Source_Luma_Rectified_Right));
00538         depth_cam_pub_      = depth_transport_.advertise("depth", 5,
00539                               boost::bind(&Camera::connectStream, this, Source_Disparity),
00540                               boost::bind(&Camera::disconnectStream, this, Source_Disparity));
00541         ni_depth_cam_pub_   = ni_depth_transport_.advertise("openni_depth", 5,
00542                               boost::bind(&Camera::connectStream, this, Source_Disparity),
00543                               boost::bind(&Camera::disconnectStream, this, Source_Disparity));
00544 
00545         if (system::DeviceInfo::HARDWARE_REV_MULTISENSE_ST21 != device_info_.hardwareRevision) {
00546 
00547             left_rgb_cam_pub_   = left_rgb_transport_.advertise("image_color", 5,
00548                                   boost::bind(&Camera::connectStream, this, Source_Luma_Left | Source_Chroma_Left),
00549                                   boost::bind(&Camera::disconnectStream, this, Source_Luma_Left | Source_Chroma_Left));
00550             left_rgb_rect_cam_pub_ = left_rgb_rect_transport_.advertiseCamera("image_rect_color", 5,
00551                                   boost::bind(&Camera::connectStream, this, Source_Luma_Left | Source_Chroma_Left),
00552                                   boost::bind(&Camera::disconnectStream, this, Source_Luma_Left | Source_Chroma_Left));
00553             color_point_cloud_pub_ = device_nh_.advertise<sensor_msgs::PointCloud2>("image_points2_color", 5,
00554                                   boost::bind(&Camera::connectStream, this,
00555                                   Source_Disparity | Source_Luma_Left | Source_Chroma_Left),
00556                                   boost::bind(&Camera::disconnectStream, this,
00557                                   Source_Disparity | Source_Luma_Left | Source_Chroma_Left));
00558             color_organized_point_cloud_pub_ = device_nh_.advertise<sensor_msgs::PointCloud2>("organized_image_points2_color", 5,
00559                                   boost::bind(&Camera::connectStream, this,
00560                                   Source_Disparity | Source_Luma_Left | Source_Chroma_Left),
00561                                   boost::bind(&Camera::disconnectStream, this,
00562                                   Source_Disparity | Source_Luma_Left | Source_Chroma_Left));
00563 
00564             left_rgb_cam_info_pub_  = left_nh_.advertise<sensor_msgs::CameraInfo>("image_color/camera_info", 1, true);
00565             left_rgb_rect_cam_info_pub_  = left_nh_.advertise<sensor_msgs::CameraInfo>("image_rect_color/camera_info", 1, true);
00566 
00567         }
00568 
00569         luma_point_cloud_pub_ = device_nh_.advertise<sensor_msgs::PointCloud2>("image_points2", 5,
00570                               boost::bind(&Camera::connectStream, this, Source_Luma_Rectified_Left | Source_Disparity),
00571                               boost::bind(&Camera::disconnectStream, this, Source_Luma_Rectified_Left | Source_Disparity));
00572 
00573         luma_organized_point_cloud_pub_ = device_nh_.advertise<sensor_msgs::PointCloud2>("organized_image_points2", 5,
00574                               boost::bind(&Camera::connectStream, this, Source_Luma_Rectified_Left | Source_Disparity),
00575                               boost::bind(&Camera::disconnectStream, this, Source_Luma_Rectified_Left | Source_Disparity));
00576 
00577         raw_cam_data_pub_   = calibration_nh.advertise<multisense_ros::RawCamData>("raw_cam_data", 5,
00578                               boost::bind(&Camera::connectStream, this, Source_Luma_Rectified_Left | Source_Disparity),
00579                               boost::bind(&Camera::disconnectStream, this, Source_Luma_Rectified_Left | Source_Disparity));
00580 
00581         left_disparity_pub_ = disparity_left_transport_.advertise("disparity", 5,
00582                               boost::bind(&Camera::connectStream, this, Source_Disparity),
00583                               boost::bind(&Camera::disconnectStream, this, Source_Disparity));
00584 
00585         left_stereo_disparity_pub_ = left_nh_.advertise<stereo_msgs::DisparityImage>("disparity_image", 5,
00586                               boost::bind(&Camera::connectStream, this, Source_Disparity),
00587                               boost::bind(&Camera::disconnectStream, this, Source_Disparity));
00588 
00589         if (version_info_.sensorFirmwareVersion >= 0x0300) {
00590 
00591             right_disparity_pub_ = disparity_right_transport_.advertise("disparity", 5,
00592                                    boost::bind(&Camera::connectStream, this, Source_Disparity_Right),
00593                                    boost::bind(&Camera::disconnectStream, this, Source_Disparity_Right));
00594 
00595             right_stereo_disparity_pub_ = right_nh_.advertise<stereo_msgs::DisparityImage>("disparity_image", 5,
00596                                   boost::bind(&Camera::connectStream, this, Source_Disparity_Right),
00597                                   boost::bind(&Camera::disconnectStream, this, Source_Disparity_Right));
00598 
00599             left_disparity_cost_pub_ = disparity_cost_transport_.advertise("cost", 5,
00600                                    boost::bind(&Camera::connectStream, this, Source_Disparity_Cost),
00601                                    boost::bind(&Camera::disconnectStream, this, Source_Disparity_Cost));
00602 
00603             right_disp_cam_info_pub_  = right_nh_.advertise<sensor_msgs::CameraInfo>("disparity/camera_info", 1, true);
00604             left_cost_cam_info_pub_  = left_nh_.advertise<sensor_msgs::CameraInfo>("cost/camera_info", 1, true);
00605         }
00606 
00607         //
00608         // Camera info topic publishers
00609         left_mono_cam_info_pub_  = left_nh_.advertise<sensor_msgs::CameraInfo>("image_mono/camera_info", 1, true);
00610         right_mono_cam_info_pub_ = right_nh_.advertise<sensor_msgs::CameraInfo>("image_mono/camera_info", 1, true);
00611         left_rect_cam_info_pub_  = left_nh_.advertise<sensor_msgs::CameraInfo>("image_rect/camera_info", 1, true);
00612         right_rect_cam_info_pub_ = right_nh_.advertise<sensor_msgs::CameraInfo>("image_rect/camera_info", 1, true);
00613         left_disp_cam_info_pub_  = left_nh_.advertise<sensor_msgs::CameraInfo>("disparity/camera_info", 1, true);
00614         depth_cam_info_pub_ = device_nh_.advertise<sensor_msgs::CameraInfo>("depth/camera_info", 1, true);
00615     }
00616 
00617 
00618     //
00619     // All image streams off
00620 
00621     stop();
00622 
00623     //
00624     // Publish device info
00625 
00626     multisense_ros::DeviceInfo msg;
00627 
00628     msg.deviceName     = device_info_.name;
00629     msg.buildDate      = device_info_.buildDate;
00630     msg.serialNumber   = device_info_.serialNumber;
00631     msg.deviceRevision = device_info_.hardwareRevision;
00632 
00633     msg.numberOfPcbs = device_info_.pcbs.size();
00634     std::vector<system::PcbInfo>::const_iterator it = device_info_.pcbs.begin();
00635     for(; it != device_info_.pcbs.end(); ++it) {
00636         msg.pcbSerialNumbers.push_back((*it).revision);
00637         msg.pcbNames.push_back((*it).name);
00638     }
00639 
00640     msg.imagerName              = device_info_.imagerName;
00641     msg.imagerType              = device_info_.imagerType;
00642     msg.imagerWidth             = device_info_.imagerWidth;
00643     msg.imagerHeight            = device_info_.imagerHeight;
00644 
00645     msg.lensName                = device_info_.lensName;
00646     msg.lensType                = device_info_.lensType;
00647     msg.nominalBaseline         = device_info_.nominalBaseline;
00648     msg.nominalFocalLength      = device_info_.nominalFocalLength;
00649     msg.nominalRelativeAperture = device_info_.nominalRelativeAperture;
00650 
00651     msg.lightingType            = device_info_.lightingType;
00652     msg.numberOfLights          = device_info_.numberOfLights;
00653 
00654     msg.laserName               = device_info_.laserName;
00655     msg.laserType               = device_info_.laserType;
00656 
00657     msg.motorName               = device_info_.motorName;
00658     msg.motorType               = device_info_.motorType;
00659     msg.motorGearReduction      = device_info_.motorGearReduction;
00660 
00661     msg.apiBuildDate            = version_info_.apiBuildDate;
00662     msg.apiVersion              = version_info_.apiVersion;
00663     msg.firmwareBuildDate       = version_info_.sensorFirmwareBuildDate;
00664     msg.firmwareVersion         = version_info_.sensorFirmwareVersion;
00665     msg.bitstreamVersion        = version_info_.sensorHardwareVersion;
00666     msg.bitstreamMagic          = version_info_.sensorHardwareMagic;
00667     msg.fpgaDna                 = version_info_.sensorFpgaDna;
00668 
00669     device_info_pub_.publish(msg);
00670 
00671     //
00672     // Publish image calibration
00673 
00674     status = driver_->getImageCalibration(image_calibration_);
00675     if (Status_Ok != status)
00676         ROS_ERROR("Camera: failed to query image calibration: %s",
00677                   Channel::statusString(status));
00678     else {
00679 
00680         multisense_ros::RawCamCal cal;
00681         const float              *cP;
00682 
00683         cP = reinterpret_cast<const float *>(&(image_calibration_.left.M[0][0]));
00684         for(uint32_t i=0; i<9; i++) cal.left_M[i] = cP[i];
00685         cP = reinterpret_cast<const float *>(&(image_calibration_.left.D[0]));
00686         for(uint32_t i=0; i<8; i++) cal.left_D[i] = cP[i];
00687         cP = reinterpret_cast<const float *>(&(image_calibration_.left.R[0][0]));
00688         for(uint32_t i=0; i<9; i++) cal.left_R[i] = cP[i];
00689         cP = reinterpret_cast<const float *>(&(image_calibration_.left.P[0][0]));
00690         for(uint32_t i=0; i<12; i++) cal.left_P[i] = cP[i];
00691 
00692         cP = reinterpret_cast<const float *>(&(image_calibration_.right.M[0][0]));
00693         for(uint32_t i=0; i<9; i++) cal.right_M[i] = cP[i];
00694         cP = reinterpret_cast<const float *>(&(image_calibration_.right.D[0]));
00695         for(uint32_t i=0; i<8; i++) cal.right_D[i] = cP[i];
00696         cP = reinterpret_cast<const float *>(&(image_calibration_.right.R[0][0]));
00697         for(uint32_t i=0; i<9; i++) cal.right_R[i] = cP[i];
00698         cP = reinterpret_cast<const float *>(&(image_calibration_.right.P[0][0]));
00699         for(uint32_t i=0; i<12; i++) cal.right_P[i] = cP[i];
00700 
00701         raw_cam_cal_pub_.publish(cal);
00702     }
00703 
00704     //
00705     // Get current sensor configuration
00706 
00707     q_matrix_(0,0) = q_matrix_(1,1) = 1.0;
00708     queryConfig();
00709 
00710     //
00711     // Initialze point cloud data structures
00712 
00713     luma_point_cloud_.is_bigendian    = (htonl(1) == 1);
00714     luma_point_cloud_.is_dense        = true;
00715     luma_point_cloud_.point_step      = luma_cloud_step;
00716     luma_point_cloud_.height          = 1;
00717     luma_point_cloud_.header.frame_id = frame_id_left_;
00718     luma_point_cloud_.fields.resize(4);
00719     luma_point_cloud_.fields[0].name     = "x";
00720     luma_point_cloud_.fields[0].offset   = 0;
00721     luma_point_cloud_.fields[0].count    = 1;
00722     luma_point_cloud_.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
00723     luma_point_cloud_.fields[1].name     = "y";
00724     luma_point_cloud_.fields[1].offset   = 4;
00725     luma_point_cloud_.fields[1].count    = 1;
00726     luma_point_cloud_.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
00727     luma_point_cloud_.fields[2].name     = "z";
00728     luma_point_cloud_.fields[2].offset   = 8;
00729     luma_point_cloud_.fields[2].count    = 1;
00730     luma_point_cloud_.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
00731     luma_point_cloud_.fields[3].name     = "luminance";
00732     luma_point_cloud_.fields[3].offset   = 12;
00733     luma_point_cloud_.fields[3].count    = 1;
00734     luma_point_cloud_.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
00735 
00736     color_point_cloud_.is_bigendian    = (htonl(1) == 1);
00737     color_point_cloud_.is_dense        = true;
00738     color_point_cloud_.point_step      = color_cloud_step;
00739     color_point_cloud_.height          = 1;
00740     color_point_cloud_.header.frame_id = frame_id_left_;
00741     color_point_cloud_.fields.resize(4);
00742     color_point_cloud_.fields[0].name     = "x";
00743     color_point_cloud_.fields[0].offset   = 0;
00744     color_point_cloud_.fields[0].count    = 1;
00745     color_point_cloud_.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
00746     color_point_cloud_.fields[1].name     = "y";
00747     color_point_cloud_.fields[1].offset   = 4;
00748     color_point_cloud_.fields[1].count    = 1;
00749     color_point_cloud_.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
00750     color_point_cloud_.fields[2].name     = "z";
00751     color_point_cloud_.fields[2].offset   = 8;
00752     color_point_cloud_.fields[2].count    = 1;
00753     color_point_cloud_.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
00754     color_point_cloud_.fields[3].name     = "rgb";
00755     color_point_cloud_.fields[3].offset   = 12;
00756     color_point_cloud_.fields[3].count    = 1;
00757     color_point_cloud_.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
00758 
00759     luma_organized_point_cloud_.is_bigendian    = (htonl(1) == 1);
00760     luma_organized_point_cloud_.is_dense        = false;
00761     luma_organized_point_cloud_.point_step      = luma_cloud_step;
00762     luma_organized_point_cloud_.header.frame_id = frame_id_left_;
00763     luma_organized_point_cloud_.fields.resize(4);
00764     luma_organized_point_cloud_.fields[0].name     = "x";
00765     luma_organized_point_cloud_.fields[0].offset   = 0;
00766     luma_organized_point_cloud_.fields[0].count    = 1;
00767     luma_organized_point_cloud_.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
00768     luma_organized_point_cloud_.fields[1].name     = "y";
00769     luma_organized_point_cloud_.fields[1].offset   = 4;
00770     luma_organized_point_cloud_.fields[1].count    = 1;
00771     luma_organized_point_cloud_.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
00772     luma_organized_point_cloud_.fields[2].name     = "z";
00773     luma_organized_point_cloud_.fields[2].offset   = 8;
00774     luma_organized_point_cloud_.fields[2].count    = 1;
00775     luma_organized_point_cloud_.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
00776     luma_organized_point_cloud_.fields[3].name     = "luminance";
00777     luma_organized_point_cloud_.fields[3].offset   = 12;
00778     luma_organized_point_cloud_.fields[3].count    = 1;
00779     luma_organized_point_cloud_.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
00780 
00781     color_organized_point_cloud_.is_bigendian    = (htonl(1) == 1);
00782     color_organized_point_cloud_.is_dense        = false;
00783     color_organized_point_cloud_.point_step      = color_cloud_step;
00784     color_organized_point_cloud_.header.frame_id = frame_id_left_;
00785     color_organized_point_cloud_.fields.resize(4);
00786     color_organized_point_cloud_.fields[0].name     = "x";
00787     color_organized_point_cloud_.fields[0].offset   = 0;
00788     color_organized_point_cloud_.fields[0].count    = 1;
00789     color_organized_point_cloud_.fields[0].datatype = sensor_msgs::PointField::FLOAT32;
00790     color_organized_point_cloud_.fields[1].name     = "y";
00791     color_organized_point_cloud_.fields[1].offset   = 4;
00792     color_organized_point_cloud_.fields[1].count    = 1;
00793     color_organized_point_cloud_.fields[1].datatype = sensor_msgs::PointField::FLOAT32;
00794     color_organized_point_cloud_.fields[2].name     = "z";
00795     color_organized_point_cloud_.fields[2].offset   = 8;
00796     color_organized_point_cloud_.fields[2].count    = 1;
00797     color_organized_point_cloud_.fields[2].datatype = sensor_msgs::PointField::FLOAT32;
00798     color_organized_point_cloud_.fields[3].name     = "rgb";
00799     color_organized_point_cloud_.fields[3].offset   = 12;
00800     color_organized_point_cloud_.fields[3].count    = 1;
00801     color_organized_point_cloud_.fields[3].datatype = sensor_msgs::PointField::FLOAT32;
00802 
00803     //
00804     // Add driver-level callbacks.
00805     //
00806     //    -Driver creates individual background thread for each callback.
00807     //    -Images are queued (depth=5) per callback, with oldest silently dropped if not keeping up.
00808     //    -All images presented are backed by a referenced buffer system (no copying of image data is done.)
00809 
00810     if (system::DeviceInfo::HARDWARE_REV_BCAM == device_info_.hardwareRevision) {
00811 
00812         driver_->addIsolatedCallback(monoCB, Source_Luma_Left, this);
00813         driver_->addIsolatedCallback(jpegCB, Source_Jpeg_Left, this);
00814 
00815     } else {
00816 
00817         driver_->addIsolatedCallback(monoCB,  Source_Luma_Left | Source_Luma_Right, this);
00818         driver_->addIsolatedCallback(rectCB,  Source_Luma_Rectified_Left | Source_Luma_Rectified_Right, this);
00819         driver_->addIsolatedCallback(depthCB, Source_Disparity, this);
00820         driver_->addIsolatedCallback(pointCB, Source_Disparity, this);
00821         driver_->addIsolatedCallback(rawCB,   Source_Disparity | Source_Luma_Rectified_Left, this);
00822         driver_->addIsolatedCallback(colorCB, Source_Luma_Left | Source_Chroma_Left, this);
00823         driver_->addIsolatedCallback(dispCB,  Source_Disparity | Source_Disparity_Right | Source_Disparity_Cost, this);
00824     }
00825 
00826     //
00827     // A common callback to publish histograms
00828 
00829     driver_->addIsolatedCallback(histCB, allImageSources, this);
00830 
00831 
00832     //
00833     // Disable color point cloud strict frame syncing, if desired
00834 
00835     const char *pcColorFrameSyncEnvStringP = getenv("MULTISENSE_ROS_PC_COLOR_FRAME_SYNC_OFF");
00836     if (NULL != pcColorFrameSyncEnvStringP) {
00837         pc_color_frame_sync_ = false;
00838         ROS_INFO("color point cloud frame sync is disabled");
00839     }
00840 }
00841 
00842 Camera::~Camera()
00843 {
00844     stop();
00845 
00846     if (system::DeviceInfo::HARDWARE_REV_BCAM == device_info_.hardwareRevision) {
00847 
00848         driver_->removeIsolatedCallback(monoCB);
00849         driver_->removeIsolatedCallback(jpegCB);
00850 
00851     } else {
00852 
00853         driver_->removeIsolatedCallback(monoCB);
00854         driver_->removeIsolatedCallback(rectCB);
00855         driver_->removeIsolatedCallback(depthCB);
00856         driver_->removeIsolatedCallback(pointCB);
00857         driver_->removeIsolatedCallback(rawCB);
00858         driver_->removeIsolatedCallback(colorCB);
00859         driver_->removeIsolatedCallback(dispCB);
00860     }
00861 }
00862 
00863 void Camera::histogramCallback(const image::Header& header)
00864 {
00865     if (last_frame_id_ >= header.frameId)
00866         return;
00867 
00868     last_frame_id_ = header.frameId;
00869 
00870     if (histogram_pub_.getNumSubscribers() > 0) {
00871         multisense_ros::Histogram rh;
00872         image::Histogram          mh;
00873 
00874         Status status = driver_->getImageHistogram(header.frameId, mh);
00875         if (Status_Ok == status) {
00876             rh.frame_count = header.frameId;
00877             rh.time_stamp  = ros::Time(header.timeSeconds,
00878                                        1000 * header.timeMicroSeconds);
00879             rh.width  = header.width;
00880             rh.height = header.height;
00881             switch(header.source) {
00882             case Source_Chroma_Left:
00883             case Source_Chroma_Right:
00884                 rh.width  *= 2;
00885                 rh.height *= 2;
00886             }
00887 
00888             rh.exposure_time = header.exposure;
00889             rh.gain          = header.gain;
00890             rh.fps           = header.framesPerSecond;
00891             rh.channels      = mh.channels;
00892             rh.bins          = mh.bins;
00893             rh.data          = mh.data;
00894             histogram_pub_.publish(rh);
00895         }
00896     }
00897 }
00898 
00899 void Camera::jpegImageCallback(const image::Header& header)
00900 {
00901     if (Source_Jpeg_Left != header.source)
00902         return;
00903 
00904     const uint32_t height    = header.height;
00905     const uint32_t width     = header.width;
00906     const uint32_t rgbLength = height * width * 3;
00907 
00908     left_rgb_image_.header.frame_id = frame_id_left_;
00909     left_rgb_image_.height          = height;
00910     left_rgb_image_.width           = width;
00911     left_rgb_image_.encoding        = "rgb8";
00912     left_rgb_image_.is_bigendian    = false;
00913     left_rgb_image_.step            = 3 * width;
00914     left_rgb_image_.header.stamp    = ros::Time(header.timeSeconds,
00915                                                 1000 * header.timeMicroSeconds);
00916 
00917     left_rgb_image_.data.resize(rgbLength);
00918 
00919     tjhandle jpegDecompressor = tjInitDecompress();
00920     tjDecompress2(jpegDecompressor,
00921                   reinterpret_cast<unsigned char*>(const_cast<void*>(header.imageDataP)),
00922                   header.imageLength,
00923                   &(left_rgb_image_.data[0]),
00924                   width, 0/*pitch*/, height, TJPF_RGB, 0);
00925     tjDestroy(jpegDecompressor);
00926 
00927     left_rgb_cam_pub_.publish(left_rgb_image_);
00928     left_rgb_cam_info_.header = left_rgb_image_.header;
00929     left_rgb_cam_info_pub_.publish(left_rgb_cam_info_);
00930 
00931     if (left_rgb_rect_cam_pub_.getNumSubscribers() > 0) {
00932         boost::mutex::scoped_lock lock(cal_lock_);
00933 
00934         if (width  != image_config_.width() ||
00935             height != image_config_.height()){
00936             //ROS_ERROR("calibration/image size mismatch: image=%dx%d, calibration=%dx%d",
00937             //width, height, image_config_.width(), image_config_.height());
00938             cal_lock_.unlock();
00939             queryConfig();
00940         }
00941         else if (NULL == calibration_map_left_1_ || NULL == calibration_map_left_2_)
00942             ROS_ERROR("Camera: undistort maps not initialized");
00943         else {
00944 
00945             const CvScalar outlierColor = {{0.0}};
00946 
00947             left_rgb_rect_image_.data.resize(rgbLength);
00948 
00949             IplImage *sourceImageP  = cvCreateImageHeader(cvSize(width, height), IPL_DEPTH_8U, 3);
00950             sourceImageP->imageData = reinterpret_cast<char*>(&(left_rgb_image_.data[0]));
00951             IplImage *destImageP    = cvCreateImageHeader(cvSize(width, height), IPL_DEPTH_8U, 3);
00952             destImageP->imageData   = reinterpret_cast<char*>(&(left_rgb_rect_image_.data[0]));
00953 
00954             cvRemap(sourceImageP, destImageP,
00955                     calibration_map_left_1_,
00956                     calibration_map_left_2_,
00957                     CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS,
00958                     outlierColor);
00959 
00960             cvReleaseImageHeader(&sourceImageP);
00961             cvReleaseImageHeader(&destImageP);
00962 
00963             left_rgb_rect_image_.header.frame_id = frame_id_left_;
00964             left_rgb_rect_image_.header.stamp    = ros::Time(header.timeSeconds,
00965                                                              1000 * header.timeMicroSeconds);
00966             left_rgb_rect_image_.height          = height;
00967             left_rgb_rect_image_.width           = width;
00968             left_rgb_rect_image_.encoding        = "rgb8";
00969             left_rgb_rect_image_.is_bigendian    = false;
00970             left_rgb_rect_image_.step            = 3 * width;
00971             left_rgb_rect_cam_info_.header       = left_rgb_rect_image_.header;
00972             left_rgb_rect_frame_id_              = header.frameId;
00973             left_rgb_rect_cam_pub_.publish(left_rgb_rect_image_, left_rgb_rect_cam_info_);
00974             left_rgb_rect_cam_info_pub_.publish(left_rgb_rect_cam_info_);
00975         }
00976     }
00977 }
00978 
00979 void Camera::disparityImageCallback(const image::Header& header)
00980 {
00981     if (!((Source_Disparity == header.source &&
00982            left_disparity_pub_.getNumSubscribers() > 0) ||
00983           (Source_Disparity_Right == header.source &&
00984            right_disparity_pub_.getNumSubscribers() > 0) ||
00985           (Source_Disparity_Cost == header.source &&
00986            left_disparity_cost_pub_.getNumSubscribers() > 0) ||
00987           (Source_Disparity == header.source &&
00988            left_stereo_disparity_pub_.getNumSubscribers() > 0) ||
00989           (Source_Disparity_Right == header.source &&
00990            right_stereo_disparity_pub_.getNumSubscribers() > 0) ))
00991         return;
00992 
00993     const uint32_t imageSize = (header.width * header.height * header.bitsPerPixel) / 8;
00994 
00995     ros::Time t = ros::Time(header.timeSeconds,
00996                             1000 * header.timeMicroSeconds);
00997 
00998     switch(header.source) {
00999     case Source_Disparity:
01000     case Source_Disparity_Right:
01001     {
01002         sensor_msgs::Image         *imageP   = NULL;
01003         sensor_msgs::CameraInfo    *camInfoP = NULL;
01004         image_transport::Publisher *pubP     = NULL;
01005         ros::Publisher *camInfoPubP          = NULL;
01006         ros::Publisher *stereoDisparityPubP  = NULL;
01007         stereo_msgs::DisparityImage *stereoDisparityImageP = NULL;
01008 
01009 
01010         if (Source_Disparity == header.source) {
01011             pubP                    = &left_disparity_pub_;
01012             imageP                  = &left_disparity_image_;
01013             imageP->header.frame_id = frame_id_left_;
01014             camInfoP                = &left_disp_cam_info_;
01015             camInfoPubP             = &left_disp_cam_info_pub_;
01016             stereoDisparityPubP     = &left_stereo_disparity_pub_;
01017             stereoDisparityImageP   = &left_stereo_disparity_;
01018             stereoDisparityImageP->header.frame_id = frame_id_left_;
01019         } else {
01020             pubP                    = &right_disparity_pub_;
01021             imageP                  = &right_disparity_image_;
01022             imageP->header.frame_id = frame_id_right_;
01023             camInfoP                = &right_disp_cam_info_;
01024             camInfoPubP             = &right_disp_cam_info_pub_;
01025             stereoDisparityPubP     = &right_stereo_disparity_pub_;
01026             stereoDisparityImageP   = &right_stereo_disparity_;
01027             stereoDisparityImageP->header.frame_id = frame_id_right_;
01028         }
01029 
01030 
01031 
01032         if (pubP->getNumSubscribers() > 0)
01033         {
01034             imageP->data.resize(imageSize);
01035             memcpy(&imageP->data[0], header.imageDataP, imageSize);
01036 
01037             imageP->header.stamp    = t;
01038             imageP->height          = header.height;
01039             imageP->width           = header.width;
01040             imageP->is_bigendian    = false;
01041 
01042             switch(header.bitsPerPixel) {
01043                 case 8:
01044                     imageP->encoding = sensor_msgs::image_encodings::TYPE_8UC1;
01045                     imageP->step     = header.width;
01046                     break;
01047                 case 16:
01048                     imageP->encoding = sensor_msgs::image_encodings::TYPE_16UC1;
01049                     imageP->step     = header.width * 2;
01050                     break;
01051             }
01052 
01053             pubP->publish(*imageP);
01054         }
01055 
01056         if (stereoDisparityPubP->getNumSubscribers() > 0)
01057         {
01058             //
01059             // If our current image resolution is using non-square pixels, i.e.
01060             // fx != fy then warn the user. This support is lacking in
01061             // stereo_msgs::DisparityImage and stereo_image_proc
01062 
01063             if (right_rect_cam_info_.P[0] != right_rect_cam_info_.P[5])
01064             {
01065                 std::stringstream warning;
01066                 warning << "Current camera configuration has non-square pixels (fx != fy).";
01067                 warning << "The stereo_msgs/DisparityImage does not account for";
01068                 warning << " this. Be careful when reprojecting to a pointcloud.";
01069                 ROS_WARN("%s", warning.str().c_str());
01070             }
01071 
01072             //
01073             // Our final floating point image will be serialized into uint8_t
01074             // meaning we need to allocate 4 bytes per pixel
01075 
01076             uint32_t floatingPointImageSize = header.width * header.height * 4;
01077             stereoDisparityImageP->image.data.resize(floatingPointImageSize);
01078 
01079             stereoDisparityImageP->header.stamp = t;
01080 
01081             stereoDisparityImageP->image.height = header.height;
01082             stereoDisparityImageP->image.width = header.width;
01083             stereoDisparityImageP->image.is_bigendian = false;
01084             stereoDisparityImageP->image.header.stamp = t;
01085             stereoDisparityImageP->image.header.frame_id = stereoDisparityImageP->header.frame_id;
01086             stereoDisparityImageP->image.encoding = "32FC1";
01087             stereoDisparityImageP->image.step = 4 * header.width;
01088 
01089 
01090             //
01091             // Fx is the same for both the right and left cameras
01092 
01093             stereoDisparityImageP->f = right_rect_cam_info_.P[0];
01094 
01095             //
01096             // Our Tx is negative. The DisparityImage message expects Tx to be
01097             // positive
01098 
01099             stereoDisparityImageP->T = fabs(right_rect_cam_info_.P[3] /
01100                                        right_rect_cam_info_.P[0]);
01101             stereoDisparityImageP->min_disparity = 0;
01102             stereoDisparityImageP->max_disparity = disparities_;
01103             stereoDisparityImageP->delta_d = 1./16.;
01104 
01105             //
01106             // The stereo_msgs::DisparityImage message expects the disparity
01107             // image to be floating point. We will use OpenCV to perform our
01108             // element-wise division
01109 
01110 
01111             cv::Mat_<uint16_t> tmpImage(header.height,
01112                                         header.width,
01113                                         reinterpret_cast<uint16_t*>(
01114                                         const_cast<void*>(header.imageDataP)));
01115 
01116             //
01117             // We will copy our data directly into our output message
01118 
01119             cv::Mat_<float> floatingPointImage(header.height,
01120                                                header.width,
01121                                                reinterpret_cast<float*>(&stereoDisparityImageP->image.data[0]));
01122 
01123             //
01124             // Convert our disparity to floating point by dividing by 16 and
01125             // copy the result to the output message
01126 
01127             floatingPointImage = tmpImage / 16.0;
01128 
01129             stereoDisparityPubP->publish(*stereoDisparityImageP);
01130         }
01131 
01132         camInfoP->header = imageP->header;
01133         camInfoP->header.stamp = t;
01134         camInfoPubP->publish(*camInfoP);
01135 
01136         break;
01137     }
01138     case Source_Disparity_Cost:
01139 
01140         left_disparity_cost_image_.data.resize(imageSize);
01141         memcpy(&left_disparity_cost_image_.data[0], header.imageDataP, imageSize);
01142 
01143         left_disparity_cost_image_.header.frame_id = frame_id_left_;
01144         left_disparity_cost_image_.header.stamp    = t;
01145         left_disparity_cost_image_.height          = header.height;
01146         left_disparity_cost_image_.width           = header.width;
01147 
01148         left_disparity_cost_image_.encoding        = sensor_msgs::image_encodings::TYPE_8UC1;
01149         left_disparity_cost_image_.is_bigendian    = false;
01150         left_disparity_cost_image_.step            = header.width;
01151 
01152         left_disparity_cost_pub_.publish(left_disparity_cost_image_);
01153 
01154         left_cost_cam_info_.header = left_disparity_cost_image_.header;
01155         left_cost_cam_info_pub_.publish(left_cost_cam_info_);
01156 
01157         break;
01158     }
01159 }
01160 
01161 void Camera::monoCallback(const image::Header& header)
01162 {
01163     if (Source_Luma_Left  != header.source &&
01164         Source_Luma_Right != header.source) {
01165 
01166         ROS_ERROR("Camera: unexpected image source: 0x%x", header.source);
01167         return;
01168     }
01169 
01170     ros::Time t = ros::Time(header.timeSeconds,
01171                             1000 * header.timeMicroSeconds);
01172 
01173     switch(header.source) {
01174     case Source_Luma_Left:
01175 
01176         left_mono_image_.data.resize(header.imageLength);
01177         memcpy(&left_mono_image_.data[0], header.imageDataP, header.imageLength);
01178 
01179         left_mono_image_.header.frame_id = frame_id_left_;
01180         left_mono_image_.header.stamp    = t;
01181         left_mono_image_.height          = header.height;
01182         left_mono_image_.width           = header.width;
01183 
01184         switch(header.bitsPerPixel) {
01185             case 8:
01186                 left_mono_image_.encoding = sensor_msgs::image_encodings::TYPE_8UC1;
01187                 left_mono_image_.step     = header.width;
01188                 break;
01189             case 16:
01190                 left_mono_image_.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
01191                 left_mono_image_.step     = header.width * 2;
01192                 break;
01193         }
01194 
01195         left_mono_image_.is_bigendian    = false;
01196 
01197         left_mono_cam_pub_.publish(left_mono_image_);
01198 
01199         //
01200         // Publish a specific camera info message for the left mono image
01201         left_mono_cam_info_.header= left_mono_image_.header;
01202         left_mono_cam_info_pub_.publish(left_mono_cam_info_);
01203 
01204         break;
01205     case Source_Luma_Right:
01206 
01207         right_mono_image_.data.resize(header.imageLength);
01208         memcpy(&right_mono_image_.data[0], header.imageDataP, header.imageLength);
01209 
01210         right_mono_image_.header.frame_id = frame_id_right_;
01211         right_mono_image_.header.stamp    = t;
01212         right_mono_image_.height          = header.height;
01213         right_mono_image_.width           = header.width;
01214 
01215         switch(header.bitsPerPixel) {
01216             case 8:
01217                 right_mono_image_.encoding = sensor_msgs::image_encodings::TYPE_8UC1;
01218                 right_mono_image_.step     = header.width;
01219                 break;
01220             case 16:
01221                 right_mono_image_.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
01222                 right_mono_image_.step     = header.width * 2;
01223                 break;
01224         }
01225         right_mono_image_.is_bigendian    = false;
01226 
01227         right_mono_cam_pub_.publish(right_mono_image_);
01228 
01229         //
01230         // Publish a specific camera info message for the left mono image
01231         right_mono_cam_info_.header= right_mono_image_.header;
01232         right_mono_cam_info_pub_.publish(right_mono_cam_info_);
01233 
01234         break;
01235     }
01236 }
01237 
01238 void Camera::rectCallback(const image::Header& header)
01239 {
01240     if (Source_Luma_Rectified_Left  != header.source &&
01241         Source_Luma_Rectified_Right != header.source) {
01242 
01243         ROS_ERROR("Camera: unexpected image source: 0x%x", header.source);
01244         return;
01245     }
01246 
01247 
01248     ros::Time t = ros::Time(header.timeSeconds,
01249                             1000 * header.timeMicroSeconds);
01250 
01251     switch(header.source) {
01252     case Source_Luma_Rectified_Left:
01253 
01254         left_rect_image_.data.resize(header.imageLength);
01255         memcpy(&left_rect_image_.data[0], header.imageDataP, header.imageLength);
01256 
01257         left_rect_image_.header.frame_id = frame_id_left_;
01258         left_rect_image_.header.stamp    = t;
01259         left_rect_image_.height          = header.height;
01260         left_rect_image_.width           = header.width;
01261 
01262         left_rect_frame_id_              = header.frameId;
01263 
01264 
01265         switch(header.bitsPerPixel) {
01266             case 8:
01267                 left_rect_image_.encoding = sensor_msgs::image_encodings::TYPE_8UC1;
01268                 left_rect_image_.step     = header.width;
01269 
01270                 break;
01271             case 16:
01272                 left_rect_image_.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
01273                 left_rect_image_.step     = header.width * 2;
01274 
01275                 break;
01276         }
01277 
01278         left_rect_image_.is_bigendian    = false;
01279 
01280         left_rect_cam_info_.header = left_rect_image_.header;
01281 
01282         //
01283         // Continue to publish the rect camera info on the
01284         // <namespace>/left/camera_info topic for backward compatibility with
01285         // older versions of the driver
01286         left_rect_cam_pub_.publish(left_rect_image_, left_rect_cam_info_);
01287 
01288         left_rect_cam_info_pub_.publish(left_rect_cam_info_);
01289 
01290         publishPointCloud(left_rect_frame_id_,
01291                           points_buff_frame_id_,
01292                           luma_point_cloud_frame_id_,
01293                           luma_point_cloud_pub_,
01294                           luma_point_cloud_,
01295                           header.width,
01296                           header.height,
01297                           header.timeSeconds,
01298                           header.timeMicroSeconds,
01299                           luma_cloud_step,
01300                           points_buff_,
01301                           &(left_rect_image_.data[0]), luma_color_depth_,
01302                           pc_max_range_,
01303                           write_pc_color_packed_,
01304                           false);
01305 
01306         publishPointCloud(left_rect_frame_id_,
01307                           points_buff_frame_id_,
01308                           luma_organized_point_cloud_frame_id_,
01309                           luma_organized_point_cloud_pub_,
01310                           luma_organized_point_cloud_,
01311                           header.width,
01312                           header.height,
01313                           header.timeSeconds,
01314                           header.timeMicroSeconds,
01315                           luma_cloud_step,
01316                           points_buff_,
01317                           &(left_rect_image_.data[0]), luma_color_depth_,
01318                           pc_max_range_,
01319                           write_pc_color_packed_,
01320                           true);
01321 
01322         break;
01323     case Source_Luma_Rectified_Right:
01324 
01325         right_rect_image_.data.resize(header.imageLength);
01326         memcpy(&right_rect_image_.data[0], header.imageDataP, header.imageLength);
01327 
01328         right_rect_image_.header.frame_id = frame_id_left_;
01329         right_rect_image_.header.stamp    = t;
01330         right_rect_image_.height          = header.height;
01331         right_rect_image_.width           = header.width;
01332 
01333         switch(header.bitsPerPixel) {
01334             case 8:
01335                 right_rect_image_.encoding = sensor_msgs::image_encodings::TYPE_8UC1;
01336                 right_rect_image_.step     = header.width;
01337                 break;
01338             case 16:
01339                 right_rect_image_.encoding = sensor_msgs::image_encodings::TYPE_16UC1;
01340                 right_rect_image_.step     = header.width * 2;
01341                 break;
01342         }
01343 
01344         right_rect_image_.is_bigendian    = false;
01345 
01346         right_rect_cam_info_.header = right_rect_image_.header;
01347 
01348         //
01349         // Continue to publish the rect camera info on the
01350         // <namespace>/right/camera_info topic for backward compatibility with
01351         // older versions of the driver
01352         right_rect_cam_pub_.publish(right_rect_image_, right_rect_cam_info_);
01353 
01354         right_rect_cam_info_pub_.publish(right_rect_cam_info_);
01355 
01356         break;
01357     }
01358 }
01359 
01360 void Camera::depthCallback(const image::Header& header)
01361 {
01362     if (Source_Disparity != header.source) {
01363 
01364         ROS_ERROR("Camera: unexpected image source: 0x%x", header.source);
01365         return;
01366     }
01367 
01368     uint32_t niDepthSubscribers = ni_depth_cam_pub_.getNumSubscribers();
01369     uint32_t depthSubscribers = depth_cam_pub_.getNumSubscribers();
01370     if (0 == niDepthSubscribers &&
01371         0 == depthSubscribers)
01372         return;
01373 
01374     const float    bad_point = std::numeric_limits<float>::quiet_NaN();
01375     const uint32_t depthSize = header.height * header.width * sizeof(float);
01376     const uint32_t niDepthSize = header.height * header.width * sizeof(uint16_t);
01377     const uint32_t imageSize = header.width * header.height;
01378 
01379     depth_image_.header.stamp    = ros::Time(header.timeSeconds,
01380                                              1000 * header.timeMicroSeconds);
01381     depth_image_.header.frame_id = frame_id_left_;
01382     depth_image_.height          = header.height;
01383     depth_image_.width           = header.width;
01384     depth_image_.is_bigendian    = (htonl(1) == 1);
01385 
01386     ni_depth_image_ = depth_image_;
01387 
01388     ni_depth_image_.encoding           = "16UC1";
01389     ni_depth_image_.step               = header.width * 2;
01390 
01391     depth_image_.encoding        = "32FC1";
01392     depth_image_.step            = header.width * 4;
01393 
01394     depth_image_.data.resize(depthSize);
01395     ni_depth_image_.data.resize(niDepthSize);
01396 
01397     float *depthImageP = reinterpret_cast<float*>(&depth_image_.data[0]);
01398     uint16_t *niDepthImageP = reinterpret_cast<uint16_t*>(&ni_depth_image_.data[0]);
01399 
01400     //
01401     // Disparity is in 32-bit floating point
01402 
01403     if (32 == header.bitsPerPixel) {
01404 
01405         //
01406         // Depth = focal_length*baseline/disparity
01407         // From the Q matrix used to reproject disparity images using non-isotropic
01408         // pixels we see that z = (fx*fy*Tx). Normalizing z so that
01409         // the scale factor on the homogeneous cartesian coordinate is 1 results
01410         // in z =  (fx*fy*Tx)/(-fy*d) or z = (fx*Tx)/(-d).
01411         // The 4th element of the right camera projection matrix is defined
01412         // as fx*Tx.
01413 
01414         const double scale = (-right_rect_cam_info_.P[3]);
01415 
01416         const float *disparityImageP = reinterpret_cast<const float*>(header.imageDataP);
01417 
01418         for (uint32_t i = 0 ; i < imageSize ; ++i)
01419         {
01420             if (0.0 >= disparityImageP[i])
01421             {
01422                 depthImageP[i] = bad_point;
01423                 niDepthImageP[i] = 0;
01424             }
01425             else
01426             {
01427                 depthImageP[i] = scale / disparityImageP[i];
01428                 niDepthImageP[i] = static_cast<uint16_t>(depthImageP[i] * 1000);
01429             }
01430         }
01431 
01432     //
01433     // Disparity is in 1/16th pixel, unsigned integer
01434 
01435     } else if (16 == header.bitsPerPixel) {
01436 
01437         //
01438         // Depth = focal_length*baseline/disparity
01439         // From the Q matrix used to reproject disparity images using non-isotropic
01440         // pixels we see that z = (fx*fy*Tx). Normalizing z so that
01441         // the scale factor on the homogeneous cartesian coordinate is 1 results
01442         // in z =  (fx*fy*Tx)/(-fy*d) or z = (fx*Tx)/(-d). Because our disparity
01443         // image is 16 bits we must also divide by 16 making z = (fx*Tx*16)/(-d)
01444         // The 4th element of the right camera projection matrix is defined
01445         // as fx*Tx.
01446 
01447 
01448         const float scale = (right_rect_cam_info_.P[3] * -16.0f);
01449 
01450         const uint16_t *disparityImageP = reinterpret_cast<const uint16_t*>(header.imageDataP);
01451 
01452         for (uint32_t i = 0 ; i < imageSize ; ++i)
01453         {
01454             if (0 == disparityImageP[i])
01455             {
01456                 depthImageP[i] = bad_point;
01457                 niDepthImageP[i] = 0;
01458             }
01459             else
01460             {
01461                 depthImageP[i] = scale / disparityImageP[i];
01462                 niDepthImageP[i] = static_cast<uint16_t>(depthImageP[i] * 1000);
01463             }
01464         }
01465 
01466     } else {
01467         ROS_ERROR("Camera: unsupported disparity bpp: %d", header.bitsPerPixel);
01468         return;
01469     }
01470 
01471     if (0 != niDepthSubscribers)
01472     {
01473         ni_depth_cam_pub_.publish(ni_depth_image_);
01474     }
01475 
01476     if (0 != depthSubscribers)
01477     {
01478         depth_cam_pub_.publish(depth_image_);
01479     }
01480 
01481     depth_cam_info_.header = depth_image_.header;
01482     depth_cam_info_pub_.publish(depth_cam_info_);
01483 }
01484 
01485 void Camera::pointCloudCallback(const image::Header& header)
01486 {
01487     if (Source_Disparity != header.source) {
01488 
01489         ROS_ERROR("Camera: unexpected image source: 0x%x", header.source);
01490         return;
01491     }
01492 
01493     if (0 == luma_point_cloud_pub_.getNumSubscribers() &&
01494         0 == color_point_cloud_pub_.getNumSubscribers() &&
01495         0 == luma_organized_point_cloud_pub_.getNumSubscribers() &&
01496         0 == color_organized_point_cloud_pub_.getNumSubscribers())
01497         return;
01498 
01499     boost::mutex::scoped_lock lock(border_clip_lock_);
01500 
01501     const bool      handle_missing = true;
01502     const uint32_t  imageSize      = header.height * header.width;
01503 
01504     //
01505     // Resize buffers
01506 
01507     points_buff_.resize(imageSize);
01508     disparity_buff_.resize(imageSize);
01509 
01510     //
01511     // Allocate buffer for reprojection output
01512 
01513     cv::Mat_<cv::Vec3f> points(header.height, header.width, &(points_buff_[0]));
01514 
01515     //
01516     // Image is already 32-bit floating point
01517 
01518     if (32 == header.bitsPerPixel) {
01519 
01520         cv::Mat_<float> disparity(header.height, header.width,
01521                                   const_cast<float*>(reinterpret_cast<const float*>(header.imageDataP)));
01522 
01523         cv::reprojectImageTo3D(disparity, points, q_matrix_, handle_missing);
01524 
01525     //
01526     // Convert CRL 1/16th pixel disparity to floating point
01527 
01528     } else if (16 == header.bitsPerPixel) {
01529 
01530         cv::Mat_<uint16_t> disparityOrigP(header.height, header.width,
01531                                           const_cast<uint16_t*>(reinterpret_cast<const uint16_t*>(header.imageDataP)));
01532         cv::Mat_<float>   disparity(header.height, header.width, &(disparity_buff_[0]));
01533         disparity = disparityOrigP / 16.0f;
01534 
01535         cv::reprojectImageTo3D(disparity, points, q_matrix_, handle_missing);
01536 
01537     } else {
01538         ROS_ERROR("Camera: unsupported disparity bpp: %d", header.bitsPerPixel);
01539         return;
01540     }
01541 
01542     //
01543     // Apply the border clip mask making all the points in the border clip region
01544     // invalid. Only do this if we have selected a border clip value
01545 
01546     if ( border_clip_value_ > 0)
01547     {
01548         points.setTo(cv::Vec3f(10000.0, 10000.0, 10000.0), border_clip_mask_);
01549     }
01550 
01551 
01552     //
01553     // Store the disparity frame ID
01554 
01555     points_buff_frame_id_ = header.frameId;
01556 
01557     //
01558     // Publish the point clouds if desired/possible
01559 
01560     publishPointCloud(left_rect_frame_id_,
01561                       points_buff_frame_id_,
01562                       luma_point_cloud_frame_id_,
01563                       luma_point_cloud_pub_,
01564                       luma_point_cloud_,
01565                       header.width,
01566                       header.height,
01567                       header.timeSeconds,
01568                       header.timeMicroSeconds,
01569                       luma_cloud_step,
01570                       points_buff_,
01571                       &(left_rect_image_.data[0]), luma_color_depth_,
01572                       pc_max_range_,
01573                       write_pc_color_packed_,
01574                       false);
01575 
01576     publishPointCloud(left_rgb_rect_frame_id_,
01577                       points_buff_frame_id_,
01578                       color_point_cloud_frame_id_,
01579                       color_point_cloud_pub_,
01580                       color_point_cloud_,
01581                       header.width,
01582                       header.height,
01583                       header.timeSeconds,
01584                       header.timeMicroSeconds,
01585                       color_cloud_step,
01586                       points_buff_,
01587                       &(left_rgb_rect_image_.data[0]), 3,
01588                       pc_max_range_,
01589                       write_pc_color_packed_,
01590                       false);
01591 
01592     publishPointCloud(left_rect_frame_id_,
01593                       points_buff_frame_id_,
01594                       luma_organized_point_cloud_frame_id_,
01595                       luma_organized_point_cloud_pub_,
01596                       luma_organized_point_cloud_,
01597                       header.width,
01598                       header.height,
01599                       header.timeSeconds,
01600                       header.timeMicroSeconds,
01601                       luma_cloud_step,
01602                       points_buff_,
01603                       &(left_rect_image_.data[0]), luma_color_depth_,
01604                       pc_max_range_,
01605                       write_pc_color_packed_,
01606                       true);
01607 
01608     publishPointCloud(left_rgb_rect_frame_id_,
01609                       points_buff_frame_id_,
01610                       color_organized_point_cloud_frame_id_,
01611                       color_organized_point_cloud_pub_,
01612                       color_organized_point_cloud_,
01613                       header.width,
01614                       header.height,
01615                       header.timeSeconds,
01616                       header.timeMicroSeconds,
01617                       color_cloud_step,
01618                       points_buff_,
01619                       &(left_rgb_rect_image_.data[0]), 3,
01620                       pc_max_range_,
01621                       write_pc_color_packed_,
01622                       true);
01623 }
01624 
01625 void Camera::rawCamDataCallback(const image::Header& header)
01626 {
01627     if (0 == raw_cam_data_pub_.getNumSubscribers()) {
01628         got_raw_cam_left_ = false;
01629         return;
01630     }
01631 
01632     const uint32_t imageSize = header.width * header.height;
01633 
01634     //
01635     // The left-rectified image is currently published before
01636     // the matching disparity image.
01637 
01638     if (false == got_raw_cam_left_) {
01639 
01640         if (Source_Luma_Rectified_Left == header.source) {
01641 
01642             raw_cam_data_.gray_scale_image.resize(imageSize);
01643             memcpy(&(raw_cam_data_.gray_scale_image[0]),
01644                    header.imageDataP,
01645                    imageSize * sizeof(uint8_t));
01646 
01647             raw_cam_data_.frames_per_second = header.framesPerSecond;
01648             raw_cam_data_.gain              = header.gain;
01649             raw_cam_data_.exposure_time     = header.exposure;
01650             raw_cam_data_.frame_count       = header.frameId;
01651             raw_cam_data_.time_stamp        = ros::Time(header.timeSeconds,
01652                                                         1000 * header.timeMicroSeconds);
01653             raw_cam_data_.width             = header.width;
01654             raw_cam_data_.height            = header.height;
01655 
01656             got_raw_cam_left_ = true;
01657         }
01658 
01659     } else if (Source_Disparity == header.source) {
01660 
01661         const uint32_t imageSize = header.width * header.height;
01662 
01663         if (header.frameId == raw_cam_data_.frame_count) {
01664 
01665             raw_cam_data_.disparity_image.resize(imageSize);
01666             memcpy(&(raw_cam_data_.disparity_image[0]),
01667                    header.imageDataP, imageSize * sizeof(uint16_t));
01668 
01669             raw_cam_data_pub_.publish(raw_cam_data_);
01670         }
01671 
01672         got_raw_cam_left_ = false;
01673     }
01674 }
01675 
01676 void Camera::colorImageCallback(const image::Header& header)
01677 {
01678     if (0 == left_rgb_cam_pub_.getNumSubscribers() &&
01679         0 == left_rgb_rect_cam_pub_.getNumSubscribers() &&
01680         0 == color_point_cloud_pub_.getNumSubscribers() &&
01681         0 == color_organized_point_cloud_pub_.getNumSubscribers()) {
01682         got_left_luma_ = false;
01683         return;
01684     }
01685 
01686     //
01687     // The left-luma image is currently published before
01688     // the matching chroma image.
01689 
01690     if (false == got_left_luma_) {
01691 
01692         if (Source_Luma_Left == header.source) {
01693 
01694             const uint32_t imageSize = header.width * header.height;
01695 
01696             left_luma_image_.data.resize(imageSize);
01697             memcpy(&left_luma_image_.data[0], header.imageDataP, imageSize);
01698 
01699             left_luma_image_.height = header.height;
01700             left_luma_image_.width  = header.width;
01701 
01702             left_luma_frame_id_ = header.frameId;
01703             got_left_luma_      = true;
01704         }
01705 
01706     } else if (Source_Chroma_Left == header.source) {
01707 
01708         if (header.frameId == left_luma_frame_id_) {
01709 
01710             const uint32_t height    = left_luma_image_.height;
01711             const uint32_t width     = left_luma_image_.width;
01712             const uint32_t imageSize = 3 * height * width;
01713 
01714             left_rgb_image_.data.resize(imageSize);
01715 
01716             left_rgb_image_.header.frame_id = frame_id_left_;
01717             left_rgb_image_.header.stamp    = ros::Time(header.timeSeconds,
01718                                                         1000 * header.timeMicroSeconds);
01719             left_rgb_image_.height          = height;
01720             left_rgb_image_.width           = width;
01721 
01722             left_rgb_image_.encoding        = "bgr8";
01723             left_rgb_image_.is_bigendian    = false;
01724             left_rgb_image_.step            = 3 * width;
01725 
01726             //
01727             // Convert YCbCr 4:2:0 to RGB
01728             // TODO: speed this up
01729 
01730             const uint8_t *lumaP     = reinterpret_cast<const uint8_t*>(&(left_luma_image_.data[0]));
01731             const uint8_t *chromaP   = reinterpret_cast<const uint8_t*>(header.imageDataP);
01732             uint8_t       *bgrP      = reinterpret_cast<uint8_t*>(&(left_rgb_image_.data[0]));
01733             const uint32_t rgbStride = width * 3;
01734 
01735             for(uint32_t y=0; y<height; y++) {
01736                 for(uint32_t x=0; x<width; x++) {
01737 
01738                     const uint32_t lumaOffset   = (y * width) + x;
01739                     const uint32_t chromaOffset = 2 * (((y/2) * (width/2)) + (x/2));
01740 
01741                     const float px_y  = static_cast<float>(lumaP[lumaOffset]);
01742                     const float px_cb = static_cast<float>(chromaP[chromaOffset+0]) - 128.0f;
01743                     const float px_cr = static_cast<float>(chromaP[chromaOffset+1]) - 128.0f;
01744 
01745                     float px_r  = px_y +                    1.402f   * px_cr;
01746                     float px_g  = px_y - 0.34414f * px_cb - 0.71414f * px_cr;
01747                     float px_b  = px_y + 1.772f   * px_cb;
01748 
01749                     if (px_r < 0.0f)        px_r = 0.0f;
01750                     else if (px_r > 255.0f) px_r = 255.0f;
01751                     if (px_g < 0.0f)        px_g = 0.0f;
01752                     else if (px_g > 255.0f) px_g = 255.0f;
01753                     if (px_b < 0.0f)        px_b = 0.0f;
01754                     else if (px_b > 255.0f) px_b = 255.0f;
01755 
01756                     const uint32_t rgbOffset = (y * rgbStride) + (3 * x);
01757 
01758                     bgrP[rgbOffset + 0] = static_cast<uint8_t>(px_b);
01759                     bgrP[rgbOffset + 1] = static_cast<uint8_t>(px_g);
01760                     bgrP[rgbOffset + 2] = static_cast<uint8_t>(px_r);
01761                 }
01762             }
01763 
01764             if (0 != left_rgb_cam_pub_.getNumSubscribers()) {
01765                 left_rgb_cam_pub_.publish(left_rgb_image_);
01766 
01767                 left_rgb_cam_info_.header = left_rgb_image_.header;
01768                 left_rgb_cam_info_pub_.publish(left_rgb_cam_info_);
01769             }
01770 
01771             if (left_rgb_rect_cam_pub_.getNumSubscribers() > 0 ||
01772                 color_point_cloud_pub_.getNumSubscribers() > 0 ||
01773                 color_organized_point_cloud_pub_.getNumSubscribers() > 0) {
01774                 boost::mutex::scoped_lock lock(cal_lock_);
01775 
01776                 if (width  != image_config_.width() ||
01777                     height != image_config_.height())
01778                     //ROS_ERROR("calibration/image size mismatch: image=%dx%d, calibration=%dx%d",
01779                     //width, height, image_config_.width(), image_config_.height());
01780                     ;
01781                 else if (NULL == calibration_map_left_1_ || NULL == calibration_map_left_2_)
01782                     ROS_ERROR("Camera: undistort maps not initialized");
01783                 else {
01784 
01785                     const CvScalar outlierColor = {{0.0}};
01786 
01787                     left_rgb_rect_image_.data.resize(imageSize);
01788 
01789                     IplImage *sourceImageP  = cvCreateImageHeader(cvSize(width, height), IPL_DEPTH_8U, 3);
01790                     sourceImageP->imageData = reinterpret_cast<char*>(&(left_rgb_image_.data[0]));
01791                     IplImage *destImageP    = cvCreateImageHeader(cvSize(width, height), IPL_DEPTH_8U, 3);
01792                     destImageP->imageData   = reinterpret_cast<char*>(&(left_rgb_rect_image_.data[0]));
01793 
01794                     cvRemap(sourceImageP, destImageP,
01795                             calibration_map_left_1_,
01796                             calibration_map_left_2_,
01797                             CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS,
01798                             outlierColor);
01799 
01800                     cvReleaseImageHeader(&sourceImageP);
01801                     cvReleaseImageHeader(&destImageP);
01802 
01803                     left_rgb_rect_image_.header.frame_id = frame_id_left_;
01804                     left_rgb_rect_image_.header.stamp    = ros::Time(header.timeSeconds,
01805                                                                      1000 * header.timeMicroSeconds);
01806                     left_rgb_rect_image_.height          = height;
01807                     left_rgb_rect_image_.width           = width;
01808 
01809                     left_rgb_rect_image_.encoding        = "bgr8";
01810                     left_rgb_rect_image_.is_bigendian    = false;
01811                     left_rgb_rect_image_.step            = 3 * width;
01812 
01813                     left_rgb_rect_cam_info_.header = left_rgb_rect_image_.header;
01814                     left_rgb_rect_frame_id_              = header.frameId;
01815 
01816                     if (left_rgb_rect_cam_pub_.getNumSubscribers() > 0) {
01817                         left_rgb_rect_cam_pub_.publish(left_rgb_rect_image_, left_rgb_rect_cam_info_);
01818 
01819                         left_rgb_rect_cam_info_.header = left_rgb_rect_image_.header;
01820                         left_rgb_rect_cam_info_pub_.publish(left_rgb_rect_cam_info_);
01821                     }
01822 
01823                     //
01824                     // Publish the color point cloud if desired/possible
01825 
01826                     publishPointCloud(left_rgb_rect_frame_id_,
01827                                       points_buff_frame_id_,
01828                                       color_point_cloud_frame_id_,
01829                                       color_point_cloud_pub_,
01830                                       color_point_cloud_,
01831                                       left_luma_image_.width,
01832                                       left_luma_image_.height,
01833                                       header.timeSeconds,
01834                                       header.timeMicroSeconds,
01835                                       color_cloud_step,
01836                                       points_buff_,
01837                                       &(left_rgb_rect_image_.data[0]), 3,
01838                                       pc_max_range_,
01839                                       write_pc_color_packed_,
01840                                       false);
01841 
01842                     publishPointCloud(left_rgb_rect_frame_id_,
01843                                       points_buff_frame_id_,
01844                                       color_organized_point_cloud_frame_id_,
01845                                       color_organized_point_cloud_pub_,
01846                                       color_organized_point_cloud_,
01847                                       left_luma_image_.width,
01848                                       left_luma_image_.height,
01849                                       header.timeSeconds,
01850                                       header.timeMicroSeconds,
01851                                       color_cloud_step,
01852                                       points_buff_,
01853                                       &(left_rgb_rect_image_.data[0]), 3,
01854                                       pc_max_range_,
01855                                       write_pc_color_packed_,
01856                                       true);
01857                 }
01858             }
01859         }
01860 
01861         got_left_luma_ = false;
01862     }
01863 }
01864 
01865 void Camera::queryConfig()
01866 {
01867     boost::mutex::scoped_lock lock(cal_lock_);
01868 
01869     //
01870     // Get the camera config
01871 
01872     Status status = driver_->getImageConfig(image_config_);
01873     if (Status_Ok != status) {
01874         ROS_ERROR("Camera: failed to query sensor configuration: %s",
01875                   Channel::statusString(status));
01876         return;
01877     }
01878 
01879     //
01880     // For convenience
01881 
01882     const image::Config& c = image_config_;
01883 
01884     disparities_ = c.disparities();
01885 
01886     //
01887     // Frame IDs must match for the rectified images
01888 
01889     left_rect_cam_info_.header.frame_id  = frame_id_left_;
01890     left_rect_cam_info_.header.stamp     = ros::Time::now();
01891     right_rect_cam_info_.header          = left_rect_cam_info_.header;
01892 
01893     left_rect_cam_info_.width  = c.width();
01894     left_rect_cam_info_.height = c.height();
01895 
01896     right_rect_cam_info_.width = c.width();
01897     right_rect_cam_info_.height = c.height();
01898 
01899     //
01900     // Calibration from sensor is for native resolution
01901 
01902     image::Calibration cal = image_calibration_;
01903 
01904     const float x_scale = 1.0f / ((static_cast<float>(device_info_.imagerWidth) /
01905                                    static_cast<float>(c.width())));
01906     const float y_scale = 1.0f / ((static_cast<float>(device_info_.imagerHeight) /
01907                                    static_cast<float>(c.height())));
01908 
01909     //
01910     // Populate our rectified camera info topics
01911 
01912     updateCameraInfo(left_rect_cam_info_, cal.left.M, cal.left.R, cal.left.P, cal.left.D, x_scale, y_scale);
01913     updateCameraInfo(right_rect_cam_info_, cal.right.M, cal.right.R, cal.right.P, cal.right.D, x_scale, y_scale);
01914 
01915     //
01916     // Copy our rectified camera info topics to populate the other camera info.
01917 
01918     left_disp_cam_info_ = left_rect_cam_info_;
01919     left_cost_cam_info_ = left_rect_cam_info_;
01920     left_rgb_rect_cam_info_ = left_rect_cam_info_;
01921     left_mono_cam_info_ = left_rect_cam_info_;
01922     left_rgb_cam_info_ = left_rect_cam_info_;
01923     depth_cam_info_ = left_rect_cam_info_;
01924 
01925     right_mono_cam_info_ = right_rect_cam_info_;
01926     right_disp_cam_info_ = right_rect_cam_info_;
01927 
01928     //
01929     // Update the frame ID's for the unrectified right image topics
01930 
01931     right_mono_cam_info_.header.frame_id = frame_id_right_;
01932     right_disp_cam_info_.header.frame_id = frame_id_right_;
01933 
01934     //
01935     // Compute the Q matrix here, as image_geometery::StereoCameraModel does
01936     // not allow for non-square pixels.
01937     //
01938     //  FyTx    0     0    -FyCxTx
01939     //   0     FxTx   0    -FxCyTx
01940     //   0      0     0     FxFyTx
01941     //   0      0    -Fy    Fy(Cx - Cx')
01942 
01943     q_matrix_(0,0) =  c.fy() * c.tx();
01944     q_matrix_(1,1) =  c.fx() * c.tx();
01945     q_matrix_(0,3) = -c.fy() * c.cx() * c.tx();
01946     q_matrix_(1,3) = -c.fx() * c.cy() * c.tx();
01947     q_matrix_(2,3) =  c.fx() * c.fy() * c.tx();
01948     q_matrix_(3,2) = -c.fy();
01949     q_matrix_(3,3) =  c.fy() * (right_rect_cam_info_.P[2] - left_rect_cam_info_.P[2]);
01950 
01951     //
01952     // Create rectification maps for local rectification of color images
01953 
01954     if (calibration_map_left_1_)
01955         cvReleaseMat(&calibration_map_left_1_);
01956     if (calibration_map_left_2_)
01957         cvReleaseMat(&calibration_map_left_2_);
01958 
01959     calibration_map_left_1_ = cvCreateMat(c.height(), c.width(), CV_32F);
01960     calibration_map_left_2_ = cvCreateMat(c.height(), c.width(), CV_32F);
01961 
01962     cal.left.M[0][0]  *= x_scale;  cal.left.M[1][1]  *= y_scale;
01963     cal.left.M[0][2]  *= x_scale;  cal.left.M[1][2]  *= y_scale;
01964     cal.right.M[0][0] *= x_scale;  cal.right.M[1][1] *= y_scale;
01965     cal.right.M[0][2] *= x_scale;  cal.right.M[1][2] *= y_scale;
01966     cal.left.P[0][0]  *= x_scale;  cal.left.P[1][1]  *= y_scale;
01967     cal.left.P[0][2]  *= x_scale;  cal.left.P[1][2]  *= y_scale;
01968     cal.left.P[0][3]  *= x_scale;  cal.left.P[1][3]  *= y_scale;
01969 
01970     CvMat M1 = cvMat(3, 3, CV_32F, &cal.left.M);
01971     CvMat D1 = cvMat(1, 8, CV_32F, &cal.left.D);
01972     CvMat R1 = cvMat(3, 3, CV_32F, &cal.left.R);
01973     CvMat P1 = cvMat(3, 4, CV_32F, &cal.left.P);
01974 
01975     cvInitUndistortRectifyMap(&M1, &D1, &R1, &P1,
01976                               calibration_map_left_1_,
01977                               calibration_map_left_2_);
01978 
01979 
01980     //
01981     // Publish the "raw" config message
01982 
01983     multisense_ros::RawCamConfig cfg;
01984 
01985     cfg.width             = c.width();
01986     cfg.height            = c.height();
01987     cfg.frames_per_second = c.fps();
01988     cfg.gain              = c.gain();
01989     cfg.exposure_time     = c.exposure();
01990 
01991     cfg.fx    = c.fx();
01992     cfg.fy    = c.fy();
01993     cfg.cx    = c.cx();
01994     cfg.cy    = c.cy();
01995     cfg.tx    = c.tx();
01996     cfg.ty    = c.ty();
01997     cfg.tz    = c.tz();
01998     cfg.roll  = c.roll();
01999     cfg.pitch = c.pitch();
02000     cfg.yaw   = c.yaw();
02001 
02002     raw_cam_config_pub_.publish(cfg);
02003 
02004     //
02005     // Update the border clipping mask since the resolution changed
02006 
02007     generateBorderClip(border_clip_type_, border_clip_value_, c.height(), c.width());
02008 
02009     //
02010     // Republish our camera info topics since the resolution changed
02011 
02012     publishAllCameraInfo();
02013 }
02014 
02015 void Camera::updateCameraInfo(sensor_msgs::CameraInfo& cameraInfo,
02016                               const float M[3][3],
02017                               const float R[3][3],
02018                               const float P[3][4],
02019                               const float D[8],
02020                               double xScale,
02021                               double yScale
02022 )
02023 {
02024     cameraInfo.P[0]   = P[0][0] * xScale;      cameraInfo.P[1]   = P[0][1];
02025     cameraInfo.P[2]   = P[0][2] * xScale;      cameraInfo.P[3]   = P[0][3] * xScale;
02026     cameraInfo.P[4]   = P[1][0];               cameraInfo.P[5]   = P[1][1] * yScale;
02027     cameraInfo.P[6]   = P[1][2] * yScale;      cameraInfo.P[7]   = P[1][3];
02028     cameraInfo.P[8]   = P[2][0];               cameraInfo.P[9]   = P[2][1];
02029     cameraInfo.P[10]  = P[2][2];               cameraInfo.P[11]  = P[2][3];
02030 
02031     cameraInfo.K[0]   = M[0][0] * xScale;      cameraInfo.K[1]   = M[0][1];
02032     cameraInfo.K[2]   = M[0][2] * xScale;      cameraInfo.K[3]   = M[1][0];
02033     cameraInfo.K[4]   = M[1][1] * yScale;      cameraInfo.K[5]   = M[1][2] * yScale;
02034     cameraInfo.K[6]   = M[2][0];               cameraInfo.K[7]   = M[2][1];
02035     cameraInfo.K[8]   = M[2][2];
02036 
02037     cameraInfo.R[0] = R[0][0];                 cameraInfo.R[1] = R[0][1];
02038     cameraInfo.R[2] = R[0][2];                 cameraInfo.R[3] = R[1][0];
02039     cameraInfo.R[4] = R[1][1];                 cameraInfo.R[5] = R[1][2];
02040     cameraInfo.R[6] = R[2][0];                 cameraInfo.R[7] = R[2][1];
02041     cameraInfo.R[8] = R[2][2];
02042 
02043     //
02044     // Distortion coefficients follow OpenCV's convention.
02045     // k1, k2, p1, p2, k3, k4, k5, k6
02046 
02047     cameraInfo.D.resize(8);
02048     for (uint8_t i=0 ; i < 8 ; ++i) {
02049         cameraInfo.D[i] = D[i];
02050     }
02051 
02052     //
02053     // MultiSense cameras support both the full 8 parameter rational_polynomial
02054     // model and the simplified 5 parameter plum_bob model. If the last 3
02055     // parameters of the distortion model are 0 then the camera is using
02056     // the simplified plumb_bob model
02057 
02058     if (D[7] == 0.0 && D[6] == 0.0 && D[5] == 0.0) {
02059         cameraInfo.distortion_model = sensor_msgs::distortion_models::PLUMB_BOB;
02060     } else {
02061         cameraInfo.distortion_model = sensor_msgs::distortion_models::RATIONAL_POLYNOMIAL;
02062     }
02063 
02064 }
02065 
02066 
02067 void Camera::publishAllCameraInfo()
02068 {
02069 
02070     //
02071     // Republish camera info messages outside of image callbacks.
02072     // The camera info publishers are latching so the messages
02073     // will persist until a new message is published in one of the image
02074     // callbacks. This makes it easier when a user is trying access a camera_info
02075     // for a topic which they are not subscribed to
02076 
02077     if (system::DeviceInfo::HARDWARE_REV_BCAM == device_info_.hardwareRevision) {
02078 
02079         left_mono_cam_info_pub_.publish(left_mono_cam_info_);
02080         left_rgb_cam_info_pub_.publish(left_rgb_cam_info_);
02081         left_rgb_rect_cam_info_pub_.publish(left_rgb_rect_cam_info_);
02082 
02083     } else if (system::DeviceInfo::HARDWARE_REV_MULTISENSE_M == device_info_.hardwareRevision) {
02084 
02085         left_mono_cam_info_pub_.publish(left_mono_cam_info_);
02086         left_rect_cam_info_pub_.publish(left_rect_cam_info_);
02087         left_rgb_cam_info_pub_.publish(left_rgb_cam_info_);
02088         left_rgb_rect_cam_info_pub_.publish(left_rgb_rect_cam_info_);
02089 
02090     } else {  // all other MultiSense-S* variations
02091 
02092         if (system::DeviceInfo::HARDWARE_REV_MULTISENSE_ST21 != device_info_.hardwareRevision) {
02093 
02094             left_rgb_cam_info_pub_.publish(left_rgb_cam_info_);
02095             left_rgb_rect_cam_info_pub_.publish(left_rgb_rect_cam_info_);
02096         }
02097 
02098         if (version_info_.sensorFirmwareVersion >= 0x0300) {
02099 
02100             right_disp_cam_info_pub_.publish(right_disp_cam_info_);
02101             left_cost_cam_info_pub_.publish(left_cost_cam_info_);
02102         }
02103 
02104         left_mono_cam_info_pub_.publish(left_mono_cam_info_);
02105         left_rect_cam_info_pub_.publish(left_rect_cam_info_);
02106         right_mono_cam_info_pub_.publish(right_mono_cam_info_);
02107         right_rect_cam_info_pub_.publish(right_rect_cam_info_);
02108         left_disp_cam_info_pub_.publish(left_disp_cam_info_);
02109         depth_cam_info_pub_.publish(depth_cam_info_);
02110 
02111     }
02112 }
02113 
02114 void Camera::borderClipChanged(int borderClipType, double borderClipValue)
02115 {
02116     //
02117     // This assumes the image resolution did not change and will just use
02118     // the current mask size as width and height arguments
02119 
02120     generateBorderClip(borderClipType, borderClipValue, border_clip_mask_.rows, border_clip_mask_.cols);
02121 
02122 }
02123 
02124 void Camera::generateBorderClip(int borderClipType, double borderClipValue, uint32_t height, uint32_t width)
02125 {
02126 
02127     boost::mutex::scoped_lock lock(border_clip_lock_);
02128 
02129     border_clip_type_ = borderClipType;
02130     border_clip_value_ = borderClipValue;
02131 
02132     //
02133     // Reset the border clip mask
02134 
02135     border_clip_mask_ = cv::Mat_<uint8_t>(height, width, static_cast<uint8_t>(255));
02136 
02137     //
02138     // Manually generate our disparity border clipping mask. Points with
02139     // a value of 255 are excluded from the pointcloud. Points with a value of 0
02140     // are included
02141 
02142     double halfWidth = static_cast<double>(width)/2.0;
02143     double halfHeight = static_cast<double>(height)/2.0;
02144 
02145     //
02146     // Precompute the maximum radius from the center of the image for a point
02147     // to be considered in the circle
02148 
02149     double radius = sqrt( pow( halfWidth, 2) + pow( halfHeight, 2) );
02150     radius -= borderClipValue;
02151 
02152     for (uint32_t u = 0 ; u < width ; ++u)
02153     {
02154         for (uint32_t v = 0 ; v < height ; ++v)
02155         {
02156             switch (borderClipType)
02157             {
02158                 case RECTANGULAR:
02159                 {
02160                     if ( u >= borderClipValue && u <= width - borderClipValue &&
02161                          v >= borderClipValue && v <= height - borderClipValue)
02162                     {
02163                         border_clip_mask_(v, u) = 0;
02164                     }
02165 
02166                     break;
02167                 }
02168                 case CIRCULAR:
02169                 {
02170                     double vector = sqrt( pow( halfWidth - u, 2) +
02171                                           pow( halfHeight - v, 2) );
02172 
02173                     if ( vector < radius)
02174                     {
02175                         border_clip_mask_(v, u) = 0;
02176                     }
02177 
02178                     break;
02179                 }
02180                 default:
02181                 {
02182                     ROS_WARN("Unknown border clip type.");
02183                     return;
02184                 }
02185             }
02186         }
02187     }
02188 }
02189 
02190 void Camera::stop()
02191 {
02192     boost::mutex::scoped_lock lock(stream_lock_);
02193 
02194     stream_map_.clear();
02195 
02196     Status status = driver_->stopStreams(allImageSources);
02197     if (Status_Ok != status)
02198         ROS_ERROR("Camera: failed to stop all streams: %s",
02199                   Channel::statusString(status));
02200 }
02201 
02202 void Camera::connectStream(DataSource enableMask)
02203 {
02204     boost::mutex::scoped_lock lock(stream_lock_);
02205 
02206     DataSource notStarted = 0;
02207 
02208     for(uint32_t i=0; i<32; i++)
02209         if ((1<<i) & enableMask && 0 == stream_map_[(1<<i)]++)
02210             notStarted |= (1<<i);
02211 
02212     if (0 != notStarted) {
02213 
02214         Status status = driver_->startStreams(notStarted);
02215         if (Status_Ok != status)
02216             ROS_ERROR("Camera: failed to start streams 0x%x: %s",
02217                       notStarted, Channel::statusString(status));
02218     }
02219 }
02220 
02221 void Camera::disconnectStream(DataSource disableMask)
02222 {
02223     boost::mutex::scoped_lock lock(stream_lock_);
02224 
02225     DataSource notStopped = 0;
02226 
02227     for(uint32_t i=0; i<32; i++)
02228         if ((1<<i) & disableMask && 0 == --stream_map_[(1<<i)])
02229             notStopped |= (1<<i);
02230 
02231     if (0 != notStopped) {
02232         Status status = driver_->stopStreams(notStopped);
02233         if (Status_Ok != status)
02234             ROS_ERROR("Camera: failed to stop streams 0x%x: %s\n",
02235                       notStopped, Channel::statusString(status));
02236     }
02237 }
02238 
02239 
02240 
02241 } // namespace


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