depth_image_creator_nodelet.cpp
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2014, JSK Lab
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/o2r other materials provided
17  * with the distribution.
18  * * Neither the name of the JSK Lab nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 #include <cv_bridge/cv_bridge.h>
41 
43 
45  NODELET_INFO("[%s::onInit]", getName().c_str());
46  ConnectionBasedNodelet::onInit();
48  // scale_depth
49  pnh_->param("scale_depth", scale_depth, 1.0);
50  ROS_INFO("scale_depth : %f", scale_depth);
51 
52  // use fixed transform
53  pnh_->param("use_fixed_transform", use_fixed_transform, false);
54  ROS_INFO("use_fixed_transform : %d", use_fixed_transform);
55 
56  pnh_->param("use_service", use_service, false);
57  ROS_INFO("use_service : %d", use_service);
58 
59  pnh_->param("use_asynchronous", use_asynchronous, false);
60  ROS_INFO("use_asynchronous : %d", use_asynchronous);
61 
62  pnh_->param("use_approximate", use_approximate, false);
63  ROS_INFO("use_approximate : %d", use_approximate);
64 
65  pnh_->param("fill_value", fill_value, std::numeric_limits<float>::quiet_NaN());
66  ROS_INFO("fill_value : %f", fill_value);
67  pnh_->param("organize_cloud", organize_cloud_, false);
68  ROS_INFO("organize_cloud : %d", organize_cloud_);
69 
70  pnh_->param("info_throttle", info_throttle_, 0);
71  ROS_INFO("info_throttle : %d", info_throttle_);
72  info_counter_ = 0;
73  pnh_->param("max_queue_size", max_queue_size_, 100);
74  ROS_INFO("max_queue_size : %d", max_queue_size_);
75  // maybe below queue_size can always be 1,
76  // but we set max_queue_size_ for backward compatibility.
77  pnh_->param("max_pub_queue_size", max_pub_queue_size_, max_queue_size_);
78  pnh_->param("max_sub_queue_size", max_sub_queue_size_, max_queue_size_);
79  // set transformation
80  std::vector<double> trans_pos(3, 0);
81  std::vector<double> trans_quat(4, 0); trans_quat[3] = 1.0;
82  if (pnh_->hasParam("translation")) {
83  jsk_topic_tools::readVectorParameter(*pnh_, "translation", trans_pos);
84  }
85  ROS_INFO("translation : %f %f %f", trans_pos[0], trans_pos[1], trans_pos[2]);
86  if (pnh_->hasParam("rotation")) {
87  jsk_topic_tools::readVectorParameter(*pnh_, "rotation", trans_quat);
88  }
89  ROS_INFO("rotation : %f %f %f %f", trans_quat[0], trans_quat[1], trans_quat[2], trans_quat[3]);
90  tf::Quaternion btq(trans_quat[0], trans_quat[1], trans_quat[2], trans_quat[3]);
91  tf::Vector3 btp(trans_pos[0], trans_pos[1], trans_pos[2]);
94  pnh_->param("tf_duration", tf_duration_, 0.001);
95  ROS_INFO("tf_duration : %f", tf_duration_);
96 
97  pub_depth_ = advertise<sensor_msgs::Image> (*pnh_, "output", max_pub_queue_size_);
98  pub_image_ = advertise<sensor_msgs::Image> (*pnh_, "output_image", max_pub_queue_size_);
99  pub_cloud_ = advertise<PointCloud>(*pnh_, "output_cloud", max_pub_queue_size_);
100  pub_disp_image_ = advertise<stereo_msgs::DisparityImage> (*pnh_, "output_disp", max_pub_queue_size_);
101 
102  if (use_service) {
103  service_ = pnh_->advertiseService("set_point_cloud",
105  }
107 }
108 
110  if (!use_service) {
111  if (use_asynchronous) {
112  sub_as_info_ = pnh_->subscribe<sensor_msgs::CameraInfo> ("info", max_sub_queue_size_,
114  sub_as_cloud_ = pnh_->subscribe<sensor_msgs::PointCloud2> ("input", max_sub_queue_size_,
116  } else {
119 
120  if (use_approximate) {
121  sync_inputs_a_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ApproximateTime<sensor_msgs::CameraInfo, sensor_msgs::PointCloud2> > > (max_queue_size_);
122  sync_inputs_a_->connectInput (sub_info_, sub_cloud_);
123  sync_inputs_a_->registerCallback (bind (&DepthImageCreator::callback_sync, this, _1, _2));
124  } else {
125  sync_inputs_e_ = boost::make_shared <message_filters::Synchronizer<message_filters::sync_policies::ExactTime<sensor_msgs::CameraInfo, sensor_msgs::PointCloud2> > > (max_queue_size_);
126  sync_inputs_e_->connectInput (sub_info_, sub_cloud_);
127  sync_inputs_e_->registerCallback (bind (&DepthImageCreator::callback_sync, this, _1, _2));
128  }
129  }
130  } else {
131  // not continuous
132  sub_as_info_ = pnh_->subscribe<sensor_msgs::CameraInfo> ("info", max_sub_queue_size_,
134 
135  }
136 }
137 
139  if (!use_service) {
140  if (use_asynchronous) {
143  }
144  else {
147  }
148  } else {
149  // not continuous
151  }
152 }
153 
154 bool jsk_pcl_ros::DepthImageCreator::service_cb (std_srvs::Empty::Request &req,
155  std_srvs::Empty::Response &res) {
156  return true;
157 }
158 
159 void jsk_pcl_ros::DepthImageCreator::callback_sync(const sensor_msgs::CameraInfoConstPtr& info,
160  const sensor_msgs::PointCloud2ConstPtr& pcloud2) {
161  ROS_DEBUG("DepthImageCreator::callback_sync");
162  publish_points(info, pcloud2);
163 }
164 
165 void jsk_pcl_ros::DepthImageCreator::callback_cloud(const sensor_msgs::PointCloud2ConstPtr& pcloud2) {
166  ROS_DEBUG("DepthImageCreator::callback_cloud");
167  boost::mutex::scoped_lock lock(this->mutex_points);
168  points_ptr_ = pcloud2;
169 }
170 
171 void jsk_pcl_ros::DepthImageCreator::callback_info(const sensor_msgs::CameraInfoConstPtr& info) {
172  ROS_DEBUG("DepthImageCreator::callback_info");
173  boost::mutex::scoped_lock lock(this->mutex_points);
174  if( info_counter_++ >= info_throttle_ ) {
175  info_counter_ = 0;
176  } else {
177  return;
178  }
179  if (points_ptr_) {
181  }
182 }
183 
184 void jsk_pcl_ros::DepthImageCreator::publish_points(const sensor_msgs::CameraInfoConstPtr& info,
185  const sensor_msgs::PointCloud2ConstPtr& pcloud2) {
186  sensor_msgs::PointCloud2Ptr pcloud2_ptr(new sensor_msgs::PointCloud2(*pcloud2));
187  if (!jsk_recognition_utils::hasField("rgb", *pcloud2_ptr)) {
188  sensor_msgs::PointCloud2Modifier pc_mod(*pcloud2_ptr);
189  pc_mod.setPointCloud2Fields(4, "x", 1, sensor_msgs::PointField::FLOAT32,
190  "y", 1, sensor_msgs::PointField::FLOAT32,
191  "z", 1, sensor_msgs::PointField::FLOAT32,
192  "rgb", 1, sensor_msgs::PointField::FLOAT32);
193  }
194 
195  ROS_DEBUG("DepthImageCreator::publish_points");
196  if (!pcloud2_ptr) return;
197  bool proc_cloud = true, proc_depth = true, proc_image = true, proc_disp = true;
198  if ( pub_cloud_.getNumSubscribers()==0 ) {
199  proc_cloud = false;
200  }
201  if ( pub_depth_.getNumSubscribers()==0 ) {
202  proc_depth = false;
203  }
204  if ( pub_image_.getNumSubscribers()==0 ) {
205  proc_image = false;
206  }
207  if ( pub_disp_image_.getNumSubscribers()==0 ) {
208  proc_disp = false;
209  }
210  if( !proc_cloud && !proc_depth && !proc_image && !proc_disp) return;
211 
212  int width = info->width;
213  int height = info->height;
214  float fx = info->P[0];
215  float cx = info->P[2];
216  float tx = info->P[3];
217  float fy = info->P[5];
218  float cy = info->P[6];
219 
220  Eigen::Affine3f sensorPose;
221  {
222  tf::StampedTransform transform;
223  if(use_fixed_transform) {
224  transform = fixed_transform;
225  } else {
226  try {
227  tf_listener_->waitForTransform(pcloud2_ptr->header.frame_id,
228  info->header.frame_id,
229  info->header.stamp,
231  tf_listener_->lookupTransform(pcloud2_ptr->header.frame_id,
232  info->header.frame_id,
233  info->header.stamp, transform);
234  }
235  catch ( std::runtime_error e ) {
236  ROS_ERROR("%s",e.what());
237  return;
238  }
239  }
240  tf::Vector3 p = transform.getOrigin();
241  tf::Quaternion q = transform.getRotation();
242  sensorPose = (Eigen::Affine3f)Eigen::Translation3f(p.getX(), p.getY(), p.getZ());
243  Eigen::Quaternion<float> rot(q.getW(), q.getX(), q.getY(), q.getZ());
244  sensorPose = sensorPose * rot;
245 
246  if (tx != 0.0) {
247  Eigen::Affine3f trans = (Eigen::Affine3f)Eigen::Translation3f(-tx/fx , 0, 0);
248  sensorPose = sensorPose * trans;
249  }
250 #if 0 // debug print
251  ROS_INFO("%f %f %f %f %f %f %f %f %f, %f %f %f",
252  sensorPose(0,0), sensorPose(0,1), sensorPose(0,2),
253  sensorPose(1,0), sensorPose(1,1), sensorPose(1,2),
254  sensorPose(2,0), sensorPose(2,1), sensorPose(2,2),
255  sensorPose(0,3), sensorPose(1,3), sensorPose(2,3));
256 #endif
257  }
258 
259  PointCloud pointCloud;
260  pcl::RangeImagePlanar rangeImageP;
261  {
262  // code here is dirty, some bag is in RangeImagePlanar
263  PointCloud tpc;
264  pcl::fromROSMsg(*pcloud2_ptr, tpc);
265 
266  Eigen::Affine3f inv;
267 #if ( PCL_MAJOR_VERSION >= 1 && PCL_MINOR_VERSION >= 5 )
268  inv = sensorPose.inverse();
269  pcl::transformPointCloud< Point > (tpc, pointCloud, inv);
270 #else
271  pcl::getInverse(sensorPose, inv);
272  pcl::getTransformedPointCloud<PointCloud> (tpc, inv, pointCloud);
273 #endif
274 
275  Eigen::Affine3f dummytrans;
276  dummytrans.setIdentity();
277  rangeImageP.createFromPointCloudWithFixedSize (pointCloud,
278  width/scale_depth, height/scale_depth,
279  cx/scale_depth, cy/scale_depth,
280  fx/scale_depth, fy/scale_depth,
281  dummytrans); //sensorPose);
282  }
283 
284  // Create color and depth image from point cloud
285  cv::Mat color_mat = cv::Mat::zeros(rangeImageP.height, rangeImageP.width, CV_8UC3);
286  cv::Mat depth_mat(rangeImageP.height, rangeImageP.width, CV_32FC1);
287  depth_mat.setTo(fill_value);
288  for (size_t i=0; i<pointCloud.size(); i++) {
289  Point pt = pointCloud[i];
290 
291  int image_x;
292  int image_y;
293  rangeImageP.getImagePoint(pt.x, pt.y, pt.z, image_x, image_y);
294 
295  if (!rangeImageP.isInImage(image_x, image_y)) {
296  continue;
297  }
298 
299  pcl::PointWithRange pt_with_range = rangeImageP.getPoint(image_x, image_y);
300  depth_mat.at<float>(image_y, image_x) = pt_with_range.z;
301 
302  if (!std::isnan(pt.x) && !std::isnan(pt.y) && !std::isnan(pt.z)) {
303  color_mat.at<cv::Vec3b>(image_y, image_x) = cv::Vec3b(pt.r, pt.g, pt.b);
304  }
305  }
306 
307  if (scale_depth != 1.0) {
308  // LINEAR
309  cv::resize(color_mat, color_mat, cv::Size(info->width, info->height));
310  cv::resize(depth_mat, depth_mat, cv::Size(info->width, info->height));
311  }
312 
313  if (proc_image) {
316  color_mat).toImageMsg());
317  }
318  if (proc_depth) {
321  depth_mat).toImageMsg());
322  }
323 
324  if(proc_cloud || proc_disp) {
325  // publish point cloud
326  pcl::RangeImagePlanar rangeImagePP;
327  rangeImagePP.setDepthImage ((float *)depth_mat.ptr(),
328  width, height,
329  cx, cy, fx, fy);
330 #if PCL_MAJOR_VERSION == 1 && PCL_MINOR_VERSION >= 7
331  rangeImagePP.header = pcl_conversions::toPCL(info->header);
332 #else
333  rangeImagePP.header = info->header;
334 #endif
335  if (proc_cloud) {
336  PointCloud cloud_out;
337  cloud_out.header = rangeImagePP.header;
338  if (organize_cloud_) {
339  cloud_out.width = rangeImagePP.width;
340  cloud_out.height = rangeImagePP.height;
341  } else {
342  cloud_out.width = rangeImagePP.width * rangeImagePP.height;
343  cloud_out.height = 1;
344  }
345  cloud_out.resize(cloud_out.width * cloud_out.height);
346  cloud_out.is_dense = true;
347  for (int y = 0; y < (int)rangeImagePP.height; y++ ) {
348  for (int x = 0; x < (int)rangeImagePP.width; x++ ) {
349  pcl::PointWithRange pt_from = rangeImagePP.points[rangeImagePP.width * y + x];
350  cv::Vec3b rgb = color_mat.at<cv::Vec3b>(y, x);
351  Point pt_to;
352  pt_to.x = pt_from.x;
353  pt_to.y = pt_from.y;
354  pt_to.z = pt_from.z;
355  pt_to.r = rgb[0];
356  pt_to.g = rgb[1];
357  pt_to.b = rgb[2];
358  cloud_out.points[rangeImagePP.width * y + x] = pt_to;
359  if (std::isnan(pt_to.x) || std::isnan(pt_to.y) || std::isnan(pt_to.z)) {
360  cloud_out.is_dense = false;
361  }
362  }
363  }
364  pub_cloud_.publish(boost::make_shared<PointCloud>(cloud_out));
365  }
366 
367  if(proc_disp) {
368  stereo_msgs::DisparityImage disp;
369 #if PCL_MAJOR_VERSION == 1 && PCL_MINOR_VERSION >= 7
370  disp.header = pcl_conversions::fromPCL(rangeImagePP.header);
371 #else
372  disp.header = rangeImagePP.header;
373 #endif
374  disp.image.encoding = sensor_msgs::image_encodings::TYPE_32FC1;
375  disp.image.height = rangeImagePP.height;
376  disp.image.width = rangeImagePP.width;
377  disp.image.step = disp.image.width * sizeof(float);
378  disp.f = fx; disp.T = 0.075;
379  disp.min_disparity = 0;
380  disp.max_disparity = disp.T * disp.f / 0.3;
381  disp.delta_d = 0.125;
382 
383  disp.image.data.resize (disp.image.height * disp.image.step);
384  float *data = reinterpret_cast<float*> (&disp.image.data[0]);
385 
386  float normalization_factor = disp.f * disp.T;
387  for (int y = 0; y < (int)rangeImagePP.height; y++ ) {
388  for (int x = 0; x < (int)rangeImagePP.width; x++ ) {
389  pcl::PointWithRange p = rangeImagePP.getPoint(x,y);
390  data[y*disp.image.width+x] = normalization_factor / p.z;
391  }
392  }
393  pub_disp_image_.publish(boost::make_shared<stereo_msgs::DisparityImage> (disp));
394  }
395  }
396 } // namespace jsk_pcl_ros
397 
TFSIMD_FORCE_INLINE void setRotation(const Quaternion &q)
void publish(const boost::shared_ptr< M > &message) const
message_filters::Subscriber< sensor_msgs::CameraInfo > sub_info_
const std::string & getName() const
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
TFSIMD_FORCE_INLINE const tfScalar & getY() const
void setPointCloud2Fields(int n_fields,...)
bool service_cb(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
TFSIMD_FORCE_INLINE const tfScalar & getW() const
bool hasField(const std::string &field_name, const sensor_msgs::PointCloud2 &msg)
TFSIMD_FORCE_INLINE const tfScalar & getZ() const
bool readVectorParameter(ros::NodeHandle &nh, const std::string &param_name, std::vector< double > &result)
bool waitForTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration &timeout, const ros::Duration &polling_sleep_duration=ros::Duration(0.01), std::string *error_msg=NULL) const
q
void toPCL(const pcl_msgs::PointIndices &pi, pcl::PointIndices &pcl_pi)
float
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros::DepthImageCreator, nodelet::Nodelet)
#define ROS_INFO(...)
void callback_info(const sensor_msgs::CameraInfoConstPtr &info)
boost::shared_ptr< ros::NodeHandle > pnh_
void callback_cloud(const sensor_msgs::PointCloud2ConstPtr &pcloud2)
const std::string TYPE_32FC1
boost::shared_ptr< message_filters::Synchronizer< message_filters::sync_policies::ApproximateTime< sensor_msgs::CameraInfo, sensor_msgs::PointCloud2 > > > sync_inputs_a_
rgb
rot
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
void lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
sensor_msgs::PointCloud2ConstPtr points_ptr_
boost::shared_ptr< message_filters::Synchronizer< message_filters::sync_policies::ExactTime< sensor_msgs::CameraInfo, sensor_msgs::PointCloud2 > > > sync_inputs_e_
void publish_points(const sensor_msgs::CameraInfoConstPtr &info, const sensor_msgs::PointCloud2ConstPtr &pcloud2)
void callback_sync(const sensor_msgs::CameraInfoConstPtr &info, const sensor_msgs::PointCloud2ConstPtr &pcloud2)
#define NODELET_INFO(...)
uint32_t getNumSubscribers() const
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
height
Quaternion getRotation() const
p
message_filters::Subscriber< sensor_msgs::PointCloud2 > sub_cloud_
TFSIMD_FORCE_INLINE const tfScalar & getX() const
static tf::TransformListener * getInstance()
tf::TransformListener * tf_listener_
#define ROS_ERROR(...)
pcl::PointCloud< Point > PointCloud
TFSIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
sensor_msgs::ImagePtr toImageMsg() const
std_msgs::Header fromPCL(const pcl::PCLHeader &pcl_header)
if(n==1)
tf::StampedTransform fixed_transform
width
#define ROS_DEBUG(...)


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Mon May 3 2021 03:03:46