point_cloud_xyzrgb.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 #include <boost/version.hpp>
35 #if ((BOOST_VERSION / 100) % 1000) >= 53
36 #include <boost/thread/lock_guard.hpp>
37 #endif
38 
39 #include <ros/ros.h>
40 #include <nodelet/nodelet.h>
49 #include <sensor_msgs/PointCloud2.h>
52 #include <cv_bridge/cv_bridge.h>
53 #include <opencv2/imgproc/imgproc.hpp>
54 
55 namespace depth_image_proc {
56 
57 using namespace message_filters::sync_policies;
59 
61 {
64 
65  // Subscriptions
74 
75  // Publications
76  boost::mutex connect_mutex_;
77  typedef sensor_msgs::PointCloud2 PointCloud;
79 
81 
82  virtual void onInit();
83 
84  void connectCb();
85 
86  void imageCb(const sensor_msgs::ImageConstPtr& depth_msg,
87  const sensor_msgs::ImageConstPtr& rgb_msg,
88  const sensor_msgs::CameraInfoConstPtr& info_msg);
89 
90  template<typename T>
91  void convert(const sensor_msgs::ImageConstPtr& depth_msg,
92  const sensor_msgs::ImageConstPtr& rgb_msg,
93  const PointCloud::Ptr& cloud_msg,
94  int red_offset, int green_offset, int blue_offset, int color_step);
95 };
96 
98 {
99  ros::NodeHandle& nh = getNodeHandle();
100  ros::NodeHandle& private_nh = getPrivateNodeHandle();
101  rgb_nh_.reset( new ros::NodeHandle(nh, "rgb") );
102  ros::NodeHandle depth_nh(nh, "depth_registered");
103  rgb_it_ .reset( new image_transport::ImageTransport(*rgb_nh_) );
104  depth_it_.reset( new image_transport::ImageTransport(depth_nh) );
105 
106  // Read parameters
107  int queue_size;
108  private_nh.param("queue_size", queue_size, 5);
109  bool use_exact_sync;
110  private_nh.param("exact_sync", use_exact_sync, false);
111 
112  // Synchronize inputs. Topic subscriptions happen on demand in the connection callback.
113  if (use_exact_sync)
114  {
115  exact_sync_.reset( new ExactSynchronizer(ExactSyncPolicy(queue_size), sub_depth_, sub_rgb_, sub_info_) );
116  exact_sync_->registerCallback(boost::bind(&PointCloudXyzrgbNodelet::imageCb, this, _1, _2, _3));
117  }
118  else
119  {
120  sync_.reset( new Synchronizer(SyncPolicy(queue_size), sub_depth_, sub_rgb_, sub_info_) );
121  sync_->registerCallback(boost::bind(&PointCloudXyzrgbNodelet::imageCb, this, _1, _2, _3));
122  }
123 
124  // Monitor whether anyone is subscribed to the output
125  ros::SubscriberStatusCallback connect_cb = boost::bind(&PointCloudXyzrgbNodelet::connectCb, this);
126  // Make sure we don't enter connectCb() between advertising and assigning to pub_point_cloud_
127  boost::lock_guard<boost::mutex> lock(connect_mutex_);
128  pub_point_cloud_ = depth_nh.advertise<PointCloud>("points", 1, connect_cb, connect_cb);
129 }
130 
131 // Handles (un)subscribing when clients (un)subscribe
133 {
134  boost::lock_guard<boost::mutex> lock(connect_mutex_);
135  if (pub_point_cloud_.getNumSubscribers() == 0)
136  {
137  sub_depth_.unsubscribe();
138  sub_rgb_ .unsubscribe();
139  sub_info_ .unsubscribe();
140  }
141  else if (!sub_depth_.getSubscriber())
142  {
143  ros::NodeHandle& private_nh = getPrivateNodeHandle();
144  // parameter for depth_image_transport hint
145  std::string depth_image_transport_param = "depth_image_transport";
146 
147  // depth image can use different transport.(e.g. compressedDepth)
148  image_transport::TransportHints depth_hints("raw",ros::TransportHints(), private_nh, depth_image_transport_param);
149  sub_depth_.subscribe(*depth_it_, "image_rect", 1, depth_hints);
150 
151  // rgb uses normal ros transport hints.
152  image_transport::TransportHints hints("raw", ros::TransportHints(), private_nh);
153  sub_rgb_ .subscribe(*rgb_it_, "image_rect_color", 1, hints);
154  sub_info_ .subscribe(*rgb_nh_, "camera_info", 1);
155  }
156 }
157 
158 void PointCloudXyzrgbNodelet::imageCb(const sensor_msgs::ImageConstPtr& depth_msg,
159  const sensor_msgs::ImageConstPtr& rgb_msg_in,
160  const sensor_msgs::CameraInfoConstPtr& info_msg)
161 {
162  // Check for bad inputs
163  if (depth_msg->header.frame_id != rgb_msg_in->header.frame_id)
164  {
165  NODELET_ERROR_THROTTLE(5, "Depth image frame id [%s] doesn't match RGB image frame id [%s]",
166  depth_msg->header.frame_id.c_str(), rgb_msg_in->header.frame_id.c_str());
167  return;
168  }
169 
170  // Update camera model
171  model_.fromCameraInfo(info_msg);
172 
173  // Check if the input image has to be resized
174  sensor_msgs::ImageConstPtr rgb_msg = rgb_msg_in;
175  if (depth_msg->width != rgb_msg->width || depth_msg->height != rgb_msg->height)
176  {
177  sensor_msgs::CameraInfo info_msg_tmp = *info_msg;
178  info_msg_tmp.width = depth_msg->width;
179  info_msg_tmp.height = depth_msg->height;
180  float ratio = float(depth_msg->width)/float(rgb_msg->width);
181  info_msg_tmp.K[0] *= ratio;
182  info_msg_tmp.K[2] *= ratio;
183  info_msg_tmp.K[4] *= ratio;
184  info_msg_tmp.K[5] *= ratio;
185  info_msg_tmp.P[0] *= ratio;
186  info_msg_tmp.P[2] *= ratio;
187  info_msg_tmp.P[5] *= ratio;
188  info_msg_tmp.P[6] *= ratio;
189  model_.fromCameraInfo(info_msg_tmp);
190 
192  try
193  {
194  cv_ptr = cv_bridge::toCvShare(rgb_msg, rgb_msg->encoding);
195  }
196  catch (cv_bridge::Exception& e)
197  {
198  ROS_ERROR("cv_bridge exception: %s", e.what());
199  return;
200  }
201  cv_bridge::CvImage cv_rsz;
202  cv_rsz.header = cv_ptr->header;
203  cv_rsz.encoding = cv_ptr->encoding;
204  cv::resize(cv_ptr->image.rowRange(0,depth_msg->height/ratio), cv_rsz.image, cv::Size(depth_msg->width, depth_msg->height));
205  if ((rgb_msg->encoding == enc::RGB8) || (rgb_msg->encoding == enc::BGR8) || (rgb_msg->encoding == enc::MONO8))
206  rgb_msg = cv_rsz.toImageMsg();
207  else
208  rgb_msg = cv_bridge::toCvCopy(cv_rsz.toImageMsg(), enc::RGB8)->toImageMsg();
209 
210  //NODELET_ERROR_THROTTLE(5, "Depth resolution (%ux%u) does not match RGB resolution (%ux%u)",
211  // depth_msg->width, depth_msg->height, rgb_msg->width, rgb_msg->height);
212  //return;
213  } else
214  rgb_msg = rgb_msg_in;
215 
216  // Supported color encodings: RGB8, BGR8, MONO8
217  int red_offset, green_offset, blue_offset, color_step;
218  if (rgb_msg->encoding == enc::RGB8)
219  {
220  red_offset = 0;
221  green_offset = 1;
222  blue_offset = 2;
223  color_step = 3;
224  }
225  if (rgb_msg->encoding == enc::RGBA8)
226  {
227  red_offset = 0;
228  green_offset = 1;
229  blue_offset = 2;
230  color_step = 4;
231  }
232  else if (rgb_msg->encoding == enc::BGR8)
233  {
234  red_offset = 2;
235  green_offset = 1;
236  blue_offset = 0;
237  color_step = 3;
238  }
239  else if (rgb_msg->encoding == enc::BGRA8)
240  {
241  red_offset = 2;
242  green_offset = 1;
243  blue_offset = 0;
244  color_step = 4;
245  }
246  else if (rgb_msg->encoding == enc::MONO8)
247  {
248  red_offset = 0;
249  green_offset = 0;
250  blue_offset = 0;
251  color_step = 1;
252  }
253  else
254  {
255  try
256  {
257  rgb_msg = cv_bridge::toCvCopy(rgb_msg, enc::RGB8)->toImageMsg();
258  }
259  catch (cv_bridge::Exception& e)
260  {
261  NODELET_ERROR_THROTTLE(5, "Unsupported encoding [%s]: %s", rgb_msg->encoding.c_str(), e.what());
262  return;
263  }
264  red_offset = 0;
265  green_offset = 1;
266  blue_offset = 2;
267  color_step = 3;
268  }
269 
270  // Allocate new point cloud message
271  PointCloud::Ptr cloud_msg (new PointCloud);
272  cloud_msg->header = depth_msg->header; // Use depth image time stamp
273  cloud_msg->height = depth_msg->height;
274  cloud_msg->width = depth_msg->width;
275  cloud_msg->is_dense = false;
276  cloud_msg->is_bigendian = false;
277 
278  sensor_msgs::PointCloud2Modifier pcd_modifier(*cloud_msg);
279  pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");
280 
281  if (depth_msg->encoding == enc::TYPE_16UC1)
282  {
283  convert<uint16_t>(depth_msg, rgb_msg, cloud_msg, red_offset, green_offset, blue_offset, color_step);
284  }
285  else if (depth_msg->encoding == enc::TYPE_32FC1)
286  {
287  convert<float>(depth_msg, rgb_msg, cloud_msg, red_offset, green_offset, blue_offset, color_step);
288  }
289  else
290  {
291  NODELET_ERROR_THROTTLE(5, "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str());
292  return;
293  }
294 
295  pub_point_cloud_.publish (cloud_msg);
296 }
297 
298 template<typename T>
299 void PointCloudXyzrgbNodelet::convert(const sensor_msgs::ImageConstPtr& depth_msg,
300  const sensor_msgs::ImageConstPtr& rgb_msg,
301  const PointCloud::Ptr& cloud_msg,
302  int red_offset, int green_offset, int blue_offset, int color_step)
303 {
304  // Use correct principal point from calibration
305  float center_x = model_.cx();
306  float center_y = model_.cy();
307 
308  // Combine unit conversion (if necessary) with scaling by focal length for computing (X,Y)
309  double unit_scaling = DepthTraits<T>::toMeters( T(1) );
310  float constant_x = unit_scaling / model_.fx();
311  float constant_y = unit_scaling / model_.fy();
312  float bad_point = std::numeric_limits<float>::quiet_NaN ();
313 
314  const T* depth_row = reinterpret_cast<const T*>(&depth_msg->data[0]);
315  int row_step = depth_msg->step / sizeof(T);
316  const uint8_t* rgb = &rgb_msg->data[0];
317  int rgb_skip = rgb_msg->step - rgb_msg->width * color_step;
318 
319  sensor_msgs::PointCloud2Iterator<float> iter_x(*cloud_msg, "x");
320  sensor_msgs::PointCloud2Iterator<float> iter_y(*cloud_msg, "y");
321  sensor_msgs::PointCloud2Iterator<float> iter_z(*cloud_msg, "z");
322  sensor_msgs::PointCloud2Iterator<uint8_t> iter_r(*cloud_msg, "r");
323  sensor_msgs::PointCloud2Iterator<uint8_t> iter_g(*cloud_msg, "g");
324  sensor_msgs::PointCloud2Iterator<uint8_t> iter_b(*cloud_msg, "b");
325  sensor_msgs::PointCloud2Iterator<uint8_t> iter_a(*cloud_msg, "a");
326 
327  for (int v = 0; v < int(cloud_msg->height); ++v, depth_row += row_step, rgb += rgb_skip)
328  {
329  for (int u = 0; u < int(cloud_msg->width); ++u, rgb += color_step, ++iter_x, ++iter_y, ++iter_z, ++iter_a, ++iter_r, ++iter_g, ++iter_b)
330  {
331  T depth = depth_row[u];
332 
333  // Check for invalid measurements
334  if (!DepthTraits<T>::valid(depth))
335  {
336  *iter_x = *iter_y = *iter_z = bad_point;
337  }
338  else
339  {
340  // Fill in XYZ
341  *iter_x = (u - center_x) * depth * constant_x;
342  *iter_y = (v - center_y) * depth * constant_y;
343  *iter_z = DepthTraits<T>::toMeters(depth);
344  }
345 
346  // Fill in color
347  *iter_a = 255;
348  *iter_r = rgb[red_offset];
349  *iter_g = rgb[green_offset];
350  *iter_b = rgb[blue_offset];
351  }
352  }
353 }
354 
355 } // namespace depth_image_proc
356 
357 // Register as nodelet
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
void convert(const sensor_msgs::ImageConstPtr &depth_msg, PointCloud::Ptr &cloud_msg, const image_geometry::PinholeCameraModel &model, double range_max=0.0)
boost::shared_ptr< Synchronizer > sync_
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
#define NODELET_ERROR_THROTTLE(rate,...)
ExactTime< sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo > SyncPolicy
std::string encoding
image_transport::SubscriberFilter sub_rgb_
void imageCb(const sensor_msgs::ImageConstPtr &depth_msg, const sensor_msgs::ImageConstPtr &rgb_msg, const sensor_msgs::CameraInfoConstPtr &info_msg)
message_filters::Synchronizer< ExactSyncPolicy > ExactSynchronizer
sensor_msgs::ImagePtr toImageMsg() const
bool param(const std::string &param_name, T &param_val, const T &default_val) const
image_geometry::PinholeCameraModel model_
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
message_filters::Synchronizer< SyncPolicy > Synchronizer
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
boost::shared_ptr< ExactSynchronizer > exact_sync_
PLUGINLIB_EXPORT_CLASS(depth_image_proc::PointCloudXyzrgbNodelet, nodelet::Nodelet)
boost::shared_ptr< image_transport::ImageTransport > rgb_it_
ExactTime< sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo > ExactSyncPolicy
message_filters::Subscriber< sensor_msgs::CameraInfo > sub_info_
void setPointCloud2FieldsByString(int n_fields,...)
ApproximateTime< sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo > SyncPolicy
#define ROS_ERROR(...)
void convert(const sensor_msgs::ImageConstPtr &depth_msg, const sensor_msgs::ImageConstPtr &rgb_msg, const PointCloud::Ptr &cloud_msg, int red_offset, int green_offset, int blue_offset, int color_step)
std_msgs::Header header


depth_image_proc
Author(s): Patrick Mihelich
autogenerated on Wed Dec 7 2022 03:25:21