depth_image_creator_nodelet.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2014, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 #include <cv_bridge/cv_bridge.h>
00037 #include <jsk_recognition_utils/pcl_ros_util.h>
00038 #include <jsk_topic_tools/rosparam_utils.h>
00039 #include <sensor_msgs/image_encodings.h>
00040 #include <sensor_msgs/point_cloud2_iterator.h>
00041 
00042 #include "jsk_pcl_ros/depth_image_creator.h"
00043 
00044 void jsk_pcl_ros::DepthImageCreator::onInit () {
00045   NODELET_INFO("[%s::onInit]", getName().c_str());
00046   ConnectionBasedNodelet::onInit();
00047   tf_listener_ = TfListenerSingleton::getInstance();
00048   // scale_depth
00049   pnh_->param("scale_depth", scale_depth, 1.0);
00050   ROS_INFO("scale_depth : %f", scale_depth);
00051 
00052   // use fixed transform
00053   pnh_->param("use_fixed_transform", use_fixed_transform, false);
00054   ROS_INFO("use_fixed_transform : %d", use_fixed_transform);
00055 
00056   pnh_->param("use_service", use_service, false);
00057   ROS_INFO("use_service : %d", use_service);
00058 
00059   pnh_->param("use_asynchronous", use_asynchronous, false);
00060   ROS_INFO("use_asynchronous : %d", use_asynchronous);
00061 
00062   pnh_->param("use_approximate", use_approximate, false);
00063   ROS_INFO("use_approximate : %d", use_approximate);
00064 
00065   pnh_->param("info_throttle", info_throttle_, 0);
00066   info_counter_ = 0;
00067   pnh_->param("max_queue_size", max_queue_size_, 3);
00068   // maybe below queue_size can always be 1,
00069   // but we set max_queue_size_ for backward compatibility.
00070   pnh_->param("max_pub_queue_size", max_pub_queue_size_, max_queue_size_);
00071   pnh_->param("max_sub_queue_size", max_sub_queue_size_, max_queue_size_);
00072   // set transformation
00073   std::vector<double> trans_pos(3, 0);
00074   std::vector<double> trans_quat(4, 0); trans_quat[3] = 1.0;
00075   if (pnh_->hasParam("translation")) {
00076     jsk_topic_tools::readVectorParameter(*pnh_, "translation", trans_pos);
00077   }
00078   if (pnh_->hasParam("rotation")) {
00079     jsk_topic_tools::readVectorParameter(*pnh_, "rotation", trans_quat);
00080   }
00081   tf::Quaternion btq(trans_quat[0], trans_quat[1], trans_quat[2], trans_quat[3]);
00082   tf::Vector3 btp(trans_pos[0], trans_pos[1], trans_pos[2]);
00083   fixed_transform.setOrigin(btp);
00084   fixed_transform.setRotation(btq);
00085 
00086   pub_depth_ = advertise<sensor_msgs::Image> (*pnh_, "output", max_pub_queue_size_);
00087   pub_image_ = advertise<sensor_msgs::Image> (*pnh_, "output_image", max_pub_queue_size_);
00088   pub_cloud_ = advertise<PointCloud>(*pnh_, "output_cloud", max_pub_queue_size_);
00089   pub_disp_image_ = advertise<stereo_msgs::DisparityImage> (*pnh_, "output_disp", max_pub_queue_size_);
00090   
00091   if (use_service) {
00092     service_ = pnh_->advertiseService("set_point_cloud",
00093                                       &DepthImageCreator::service_cb, this);
00094   }
00095   onInitPostProcess();
00096 }
00097 
00098 void jsk_pcl_ros::DepthImageCreator::subscribe() {
00099   if (!use_service) {
00100     if (use_asynchronous) {
00101       sub_as_info_ = pnh_->subscribe<sensor_msgs::CameraInfo> ("info", max_sub_queue_size_,
00102                                                                &DepthImageCreator::callback_info, this);
00103       sub_as_cloud_ = pnh_->subscribe<sensor_msgs::PointCloud2> ("input", max_sub_queue_size_,
00104                                                                  &DepthImageCreator::callback_cloud, this);
00105     } else {
00106       sub_info_.subscribe(*pnh_, "info", max_sub_queue_size_);
00107       sub_cloud_.subscribe(*pnh_, "input", max_sub_queue_size_);
00108 
00109       if (use_approximate) {
00110         sync_inputs_a_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<sensor_msgs::CameraInfo, sensor_msgs::PointCloud2> > > (max_queue_size_);
00111         sync_inputs_a_->connectInput (sub_info_, sub_cloud_);
00112         sync_inputs_a_->registerCallback (bind (&DepthImageCreator::callback_sync, this, _1, _2));
00113       } else {
00114         sync_inputs_e_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ExactTime<sensor_msgs::CameraInfo, sensor_msgs::PointCloud2> > > (max_queue_size_);
00115         sync_inputs_e_->connectInput (sub_info_, sub_cloud_);
00116         sync_inputs_e_->registerCallback (bind (&DepthImageCreator::callback_sync, this, _1, _2));
00117       }
00118     }
00119   } else {
00120     // not continuous
00121     sub_as_info_ = pnh_->subscribe<sensor_msgs::CameraInfo> ("info", max_sub_queue_size_,
00122                                                              &DepthImageCreator::callback_info, this);
00123    
00124   }
00125 }
00126 
00127 void jsk_pcl_ros::DepthImageCreator::unsubscribe() {
00128   if (!use_service) {
00129     if (use_asynchronous) {
00130       sub_as_info_.shutdown();
00131       sub_as_cloud_.shutdown();
00132     }
00133     else {
00134       sub_info_.unsubscribe();
00135       sub_cloud_.unsubscribe();
00136     }
00137   } else {
00138     // not continuous
00139     sub_as_info_.shutdown();
00140   }
00141 }
00142 
00143 bool jsk_pcl_ros::DepthImageCreator::service_cb (std_srvs::Empty::Request &req,
00144                                                  std_srvs::Empty::Response &res) {
00145   return true;
00146 }
00147 
00148 void jsk_pcl_ros::DepthImageCreator::callback_sync(const sensor_msgs::CameraInfoConstPtr& info,
00149                                                    const sensor_msgs::PointCloud2ConstPtr& pcloud2) {
00150   ROS_DEBUG("DepthImageCreator::callback_sync");
00151   publish_points(info, pcloud2);
00152 }
00153 
00154 void jsk_pcl_ros::DepthImageCreator::callback_cloud(const sensor_msgs::PointCloud2ConstPtr& pcloud2) {
00155   ROS_DEBUG("DepthImageCreator::callback_cloud");
00156   boost::mutex::scoped_lock lock(this->mutex_points);
00157   points_ptr_ = pcloud2;
00158 }
00159 
00160 void jsk_pcl_ros::DepthImageCreator::callback_info(const sensor_msgs::CameraInfoConstPtr& info) {
00161   ROS_DEBUG("DepthImageCreator::callback_info");
00162   boost::mutex::scoped_lock lock(this->mutex_points);
00163   if( info_counter_++ >= info_throttle_ ) {
00164     info_counter_ = 0;
00165   } else {
00166     return;
00167   }
00168   publish_points(info, points_ptr_);
00169 }
00170 
00171 void jsk_pcl_ros::DepthImageCreator::publish_points(const sensor_msgs::CameraInfoConstPtr& info,
00172                                                     const sensor_msgs::PointCloud2ConstPtr& pcloud2) {
00173   sensor_msgs::PointCloud2Ptr pcloud2_ptr(new sensor_msgs::PointCloud2(*pcloud2));
00174   if (!jsk_recognition_utils::hasField("rgb", *pcloud2_ptr)) {
00175     sensor_msgs::PointCloud2Modifier pc_mod(*pcloud2_ptr);
00176     pc_mod.setPointCloud2Fields(4, "x", 1, sensor_msgs::PointField::FLOAT32,
00177                                    "y", 1, sensor_msgs::PointField::FLOAT32,
00178                                    "z", 1, sensor_msgs::PointField::FLOAT32,
00179                                    "rgb", 1, sensor_msgs::PointField::FLOAT32);
00180   }
00181 
00182   ROS_DEBUG("DepthImageCreator::publish_points");
00183   if (!pcloud2_ptr)  return;
00184   bool proc_cloud = true, proc_depth = true, proc_image = true, proc_disp = true;
00185   if ( pub_cloud_.getNumSubscribers()==0 ) {
00186     proc_cloud = false;
00187   }
00188   if ( pub_depth_.getNumSubscribers()==0 ) {
00189     proc_depth = false;
00190   }
00191   if ( pub_image_.getNumSubscribers()==0 ) {
00192     proc_image = false;
00193   }
00194   if ( pub_disp_image_.getNumSubscribers()==0 ) {
00195     proc_disp = false;
00196   }
00197   if( !proc_cloud && !proc_depth && !proc_image && !proc_disp) return;
00198 
00199   int width = info->width;
00200   int height = info->height;
00201   float fx = info->P[0];
00202   float cx = info->P[2];
00203   float tx = info->P[3];
00204   float fy = info->P[5];
00205   float cy = info->P[6];
00206 
00207   Eigen::Affine3f sensorPose;
00208   {
00209     tf::StampedTransform transform;
00210     if(use_fixed_transform) {
00211       transform = fixed_transform;
00212     } else {
00213       try {
00214         tf_listener_->waitForTransform(pcloud2_ptr->header.frame_id,
00215                                        info->header.frame_id,
00216                                        info->header.stamp,
00217                                        ros::Duration(0.001));
00218         tf_listener_->lookupTransform(pcloud2_ptr->header.frame_id,
00219                                       info->header.frame_id,
00220                                       info->header.stamp, transform);
00221       }
00222       catch ( std::runtime_error e ) {
00223         ROS_ERROR("%s",e.what());
00224         return;
00225       }
00226     }
00227     tf::Vector3 p = transform.getOrigin();
00228     tf::Quaternion q = transform.getRotation();
00229     sensorPose = (Eigen::Affine3f)Eigen::Translation3f(p.getX(), p.getY(), p.getZ());
00230     Eigen::Quaternion<float> rot(q.getW(), q.getX(), q.getY(), q.getZ());
00231     sensorPose = sensorPose * rot;
00232 
00233     if (tx != 0.0) {
00234       Eigen::Affine3f trans = (Eigen::Affine3f)Eigen::Translation3f(-tx/fx , 0, 0);
00235       sensorPose = sensorPose * trans;
00236     }
00237 #if 0 // debug print
00238     ROS_INFO("%f %f %f %f %f %f %f %f %f, %f %f %f",
00239              sensorPose(0,0), sensorPose(0,1), sensorPose(0,2),
00240              sensorPose(1,0), sensorPose(1,1), sensorPose(1,2),
00241              sensorPose(2,0), sensorPose(2,1), sensorPose(2,2),
00242              sensorPose(0,3), sensorPose(1,3), sensorPose(2,3));
00243 #endif
00244   }
00245 
00246   PointCloud pointCloud;
00247   pcl::RangeImagePlanar rangeImageP;
00248   {
00249     // code here is dirty, some bag is in RangeImagePlanar
00250     PointCloud tpc;
00251     pcl::fromROSMsg(*pcloud2_ptr, tpc);
00252 
00253     Eigen::Affine3f inv;
00254 #if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 5 )
00255     inv = sensorPose.inverse();
00256     pcl::transformPointCloud< Point > (tpc, pointCloud, inv);
00257 #else
00258     pcl::getInverse(sensorPose, inv);
00259     pcl::getTransformedPointCloud<PointCloud> (tpc, inv, pointCloud);
00260 #endif
00261 
00262     Eigen::Affine3f dummytrans;
00263     dummytrans.setIdentity();
00264     rangeImageP.createFromPointCloudWithFixedSize (pointCloud,
00265                                                    width/scale_depth, height/scale_depth,
00266                                                    cx/scale_depth, cy/scale_depth,
00267                                                    fx/scale_depth, fy/scale_depth,
00268                                                    dummytrans); //sensorPose);
00269   }
00270 
00271   // Create color and depth image from point cloud
00272   cv::Mat color_mat = cv::Mat::zeros(rangeImageP.height, rangeImageP.width, CV_8UC3);
00273   cv::Mat depth_mat(rangeImageP.height, rangeImageP.width, CV_32FC1);
00274   depth_mat.setTo(std::numeric_limits<float>::quiet_NaN());
00275   for (size_t i=0; i<pointCloud.size(); i++) {
00276     Point pt = pointCloud[i];
00277 
00278     int image_x;
00279     int image_y;
00280     rangeImageP.getImagePoint(pt.x, pt.y, pt.z, image_x, image_y);
00281 
00282     if (!rangeImageP.isInImage(image_x, image_y)) {
00283       continue;
00284     }
00285 
00286     pcl::PointWithRange pt_with_range = rangeImageP.getPoint(image_x, image_y);
00287     depth_mat.at<float>(image_y, image_x) = pt_with_range.z;
00288 
00289     if (!std::isnan(pt.x) && !std::isnan(pt.y) && !std::isnan(pt.z)) {
00290       color_mat.at<cv::Vec3b>(image_y, image_x) = cv::Vec3b(pt.r, pt.g, pt.b);
00291     }
00292   }
00293 
00294   if (scale_depth != 1.0) {
00295     // LINEAR
00296     cv::resize(color_mat, color_mat, cv::Size(info->width, info->height));
00297     cv::resize(depth_mat, depth_mat, cv::Size(info->width, info->height));
00298   }
00299 
00300   if (proc_image) {
00301     pub_image_.publish(cv_bridge::CvImage(info->header,
00302                                           sensor_msgs::image_encodings::RGB8,
00303                                           color_mat).toImageMsg());
00304   }
00305   if (proc_depth) {
00306     pub_depth_.publish(cv_bridge::CvImage(info->header,
00307                                           sensor_msgs::image_encodings::TYPE_32FC1,
00308                                           depth_mat).toImageMsg());
00309   }
00310 
00311   if(proc_cloud || proc_disp) {
00312     // publish point cloud
00313     pcl::RangeImagePlanar rangeImagePP;
00314     rangeImagePP.setDepthImage ((float *)depth_mat.ptr(),
00315                                 width, height,
00316                                 cx, cy, fx, fy);
00317 #if PCL_MAJOR_VERSION == 1 && PCL_MINOR_VERSION >= 7
00318     rangeImagePP.header = pcl_conversions::toPCL(info->header);
00319 #else
00320     rangeImagePP.header = info->header;
00321 #endif
00322     if (proc_cloud) {
00323       PointCloud cloud_out;
00324       cloud_out.header = rangeImagePP.header;
00325       cloud_out.resize(rangeImagePP.points.size());
00326       for (int y = 0; y < (int)rangeImagePP.height; y++ ) {
00327         for (int x = 0; x < (int)rangeImagePP.width; x++ ) {
00328           pcl::PointWithRange pt_from = rangeImagePP.points[rangeImagePP.width * y + x];
00329           cv::Vec3b rgb = color_mat.at<cv::Vec3b>(y, x);
00330           Point pt_to = cloud_out[rangeImagePP.width * y + x];
00331           pt_to.x = pt_from.x;
00332           pt_to.y = pt_from.y;
00333           pt_to.z = pt_from.z;
00334           pt_to.r = rgb[0];
00335           pt_to.g = rgb[1];
00336           pt_to.b = rgb[2];
00337         }
00338       }
00339       pub_cloud_.publish(boost::make_shared<PointCloud>(cloud_out));
00340     }
00341 
00342     if(proc_disp) {
00343       stereo_msgs::DisparityImage disp;
00344 #if PCL_MAJOR_VERSION == 1 && PCL_MINOR_VERSION >= 7
00345       disp.header = pcl_conversions::fromPCL(rangeImagePP.header);
00346 #else
00347       disp.header = rangeImagePP.header;
00348 #endif
00349       disp.image.encoding  = sensor_msgs::image_encodings::TYPE_32FC1;
00350       disp.image.height    = rangeImagePP.height;
00351       disp.image.width     = rangeImagePP.width;
00352       disp.image.step      = disp.image.width * sizeof(float);
00353       disp.f = fx; disp.T = 0.075;
00354       disp.min_disparity = 0;
00355       disp.max_disparity = disp.T * disp.f / 0.3;
00356       disp.delta_d = 0.125;
00357 
00358       disp.image.data.resize (disp.image.height * disp.image.step);
00359       float *data = reinterpret_cast<float*> (&disp.image.data[0]);
00360 
00361       float normalization_factor = disp.f * disp.T;
00362       for (int y = 0; y < (int)rangeImagePP.height; y++ ) {
00363         for (int x = 0; x < (int)rangeImagePP.width; x++ ) {
00364           pcl::PointWithRange p = rangeImagePP.getPoint(x,y);
00365           data[y*disp.image.width+x] = normalization_factor / p.z;
00366         }
00367       }
00368       pub_disp_image_.publish(boost::make_shared<stereo_msgs::DisparityImage> (disp));
00369     }
00370   }
00371 }  // namespace jsk_pcl_ros
00372 
00373 #include <pluginlib/class_list_macros.h>
00374 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::DepthImageCreator, nodelet::Nodelet);


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Tue Jul 2 2019 19:41:42