point_cloud2.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>
48 
49 #include <stereo_msgs/DisparityImage.h>
50 #include <sensor_msgs/PointCloud2.h>
53 
54 namespace stereo_image_proc {
55 
56 using namespace sensor_msgs;
57 using namespace stereo_msgs;
58 using namespace message_filters::sync_policies;
59 
60 class PointCloud2Nodelet : public nodelet::Nodelet
61 {
63 
64  // Subscriptions
66  message_filters::Subscriber<CameraInfo> sub_l_info_, sub_r_info_;
72  boost::shared_ptr<ExactSync> exact_sync_;
73  boost::shared_ptr<ApproximateSync> approximate_sync_;
74 
75  // Publications
76  boost::mutex connect_mutex_;
77  ros::Publisher pub_points2_;
78 
79  // Processing state (note: only safe because we're single-threaded!)
81  cv::Mat_<cv::Vec3f> points_mat_; // scratch buffer
82 
83  virtual void onInit();
84 
85  void connectCb();
86 
87  void imageCb(const ImageConstPtr& l_image_msg,
88  const CameraInfoConstPtr& l_info_msg,
89  const CameraInfoConstPtr& r_info_msg,
90  const DisparityImageConstPtr& disp_msg);
91 };
92 
94 {
95  ros::NodeHandle &nh = getNodeHandle();
96  ros::NodeHandle &private_nh = getPrivateNodeHandle();
97  it_.reset(new image_transport::ImageTransport(nh));
98 
99  // Synchronize inputs. Topic subscriptions happen on demand in the connection
100  // callback. Optionally do approximate synchronization.
101  int queue_size;
102  private_nh.param("queue_size", queue_size, 5);
103  bool approx;
104  private_nh.param("approximate_sync", approx, false);
105  if (approx)
106  {
107  approximate_sync_.reset( new ApproximateSync(ApproximatePolicy(queue_size),
108  sub_l_image_, sub_l_info_,
109  sub_r_info_, sub_disparity_) );
110  approximate_sync_->registerCallback(boost::bind(&PointCloud2Nodelet::imageCb,
111  this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
112  }
113  else
114  {
115  exact_sync_.reset( new ExactSync(ExactPolicy(queue_size),
116  sub_l_image_, sub_l_info_,
117  sub_r_info_, sub_disparity_) );
118  exact_sync_->registerCallback(boost::bind(&PointCloud2Nodelet::imageCb,
119  this, boost::placeholders::_1, boost::placeholders::_2, boost::placeholders::_3, boost::placeholders::_4));
120  }
121 
122  // Monitor whether anyone is subscribed to the output
123  ros::SubscriberStatusCallback connect_cb = boost::bind(&PointCloud2Nodelet::connectCb, this);
124  // Make sure we don't enter connectCb() between advertising and assigning to pub_points2_
125  boost::lock_guard<boost::mutex> lock(connect_mutex_);
126  pub_points2_ = nh.advertise<PointCloud2>("points2", 1, connect_cb, connect_cb);
127 }
128 
129 // Handles (un)subscribing when clients (un)subscribe
131 {
132  boost::lock_guard<boost::mutex> lock(connect_mutex_);
133  if (pub_points2_.getNumSubscribers() == 0)
134  {
135  sub_l_image_ .unsubscribe();
136  sub_l_info_ .unsubscribe();
137  sub_r_info_ .unsubscribe();
138  sub_disparity_.unsubscribe();
139  }
140  else if (!sub_l_image_.getSubscriber())
141  {
142  ros::NodeHandle &nh = getNodeHandle();
143  // Queue size 1 should be OK; the one that matters is the synchronizer queue size.
144  image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle());
145  sub_l_image_ .subscribe(*it_, "left/image_rect_color", 1, hints);
146  sub_l_info_ .subscribe(nh, "left/camera_info", 1);
147  sub_r_info_ .subscribe(nh, "right/camera_info", 1);
148  sub_disparity_.subscribe(nh, "disparity", 1);
149  }
150 }
151 
152 inline bool isValidPoint(const cv::Vec3f& pt)
153 {
154  // Check both for disparities explicitly marked as invalid (where OpenCV maps pt.z to MISSING_Z)
155  // and zero disparities (point mapped to infinity).
156  return pt[2] != image_geometry::StereoCameraModel::MISSING_Z && !std::isinf(pt[2]);
157 }
158 
159 void PointCloud2Nodelet::imageCb(const ImageConstPtr& l_image_msg,
160  const CameraInfoConstPtr& l_info_msg,
161  const CameraInfoConstPtr& r_info_msg,
162  const DisparityImageConstPtr& disp_msg)
163 {
164  // Update the camera model
165  model_.fromCameraInfo(l_info_msg, r_info_msg);
166 
167  // Calculate point cloud
168  const Image& dimage = disp_msg->image;
169  const cv::Mat_<float> dmat(dimage.height, dimage.width, (float*)&dimage.data[0], dimage.step);
170  model_.projectDisparityImageTo3d(dmat, points_mat_, true);
171  cv::Mat_<cv::Vec3f> mat = points_mat_;
172 
173  // Fill in new PointCloud2 message (2D image-like layout)
174  PointCloud2Ptr points_msg = boost::make_shared<PointCloud2>();
175  points_msg->header = disp_msg->header;
176  points_msg->height = mat.rows;
177  points_msg->width = mat.cols;
178  points_msg->is_bigendian = false;
179  points_msg->is_dense = false; // there may be invalid points
180 
181  sensor_msgs::PointCloud2Modifier pcd_modifier(*points_msg);
182  pcd_modifier.setPointCloud2FieldsByString(2, "xyz", "rgb");
183 
184  sensor_msgs::PointCloud2Iterator<float> iter_x(*points_msg, "x");
185  sensor_msgs::PointCloud2Iterator<float> iter_y(*points_msg, "y");
186  sensor_msgs::PointCloud2Iterator<float> iter_z(*points_msg, "z");
187  sensor_msgs::PointCloud2Iterator<uint8_t> iter_r(*points_msg, "r");
188  sensor_msgs::PointCloud2Iterator<uint8_t> iter_g(*points_msg, "g");
189  sensor_msgs::PointCloud2Iterator<uint8_t> iter_b(*points_msg, "b");
190 
191  float bad_point = std::numeric_limits<float>::quiet_NaN ();
192  for (int v = 0; v < mat.rows; ++v)
193  {
194  for (int u = 0; u < mat.cols; ++u, ++iter_x, ++iter_y, ++iter_z)
195  {
196  if (isValidPoint(mat(v,u)))
197  {
198  // x,y,z
199  *iter_x = mat(v, u)[0];
200  *iter_y = mat(v, u)[1];
201  *iter_z = mat(v, u)[2];
202  }
203  else
204  {
205  *iter_x = *iter_y = *iter_z = bad_point;
206  }
207  }
208  }
209 
210  // Fill in color
211  namespace enc = sensor_msgs::image_encodings;
212  const std::string& encoding = l_image_msg->encoding;
213  if (encoding == enc::MONO8)
214  {
215  const cv::Mat_<uint8_t> color(l_image_msg->height, l_image_msg->width,
216  (uint8_t*)&l_image_msg->data[0],
217  l_image_msg->step);
218  for (int v = 0; v < mat.rows; ++v)
219  {
220  for (int u = 0; u < mat.cols; ++u, ++iter_r, ++iter_g, ++iter_b)
221  {
222  uint8_t g = color(v,u);
223  *iter_r = *iter_g = *iter_b = g;
224  }
225  }
226  }
227  else if (encoding == enc::RGB8)
228  {
229  const cv::Mat_<cv::Vec3b> color(l_image_msg->height, l_image_msg->width,
230  (cv::Vec3b*)&l_image_msg->data[0],
231  l_image_msg->step);
232  for (int v = 0; v < mat.rows; ++v)
233  {
234  for (int u = 0; u < mat.cols; ++u, ++iter_r, ++iter_g, ++iter_b)
235  {
236  const cv::Vec3b& rgb = color(v,u);
237  *iter_r = rgb[0];
238  *iter_g = rgb[1];
239  *iter_b = rgb[2];
240  }
241  }
242  }
243  else if (encoding == enc::RGBA8)
244  {
245  const cv::Mat_<cv::Vec4b> color(l_image_msg->height, l_image_msg->width,
246  (cv::Vec4b*)&l_image_msg->data[0],
247  l_image_msg->step);
248  for (int v = 0; v < mat.rows; ++v)
249  {
250  for (int u = 0; u < mat.cols; ++u, ++iter_r, ++iter_g, ++iter_b)
251  {
252  const cv::Vec4b& rgba = color(v,u);
253  *iter_r = rgba[0];
254  *iter_g = rgba[1];
255  *iter_b = rgba[2];
256  }
257  }
258  }
259  else if (encoding == enc::BGR8)
260  {
261  const cv::Mat_<cv::Vec3b> color(l_image_msg->height, l_image_msg->width,
262  (cv::Vec3b*)&l_image_msg->data[0],
263  l_image_msg->step);
264  for (int v = 0; v < mat.rows; ++v)
265  {
266  for (int u = 0; u < mat.cols; ++u, ++iter_r, ++iter_g, ++iter_b)
267  {
268  const cv::Vec3b& bgr = color(v,u);
269  *iter_r = bgr[2];
270  *iter_g = bgr[1];
271  *iter_b = bgr[0];
272  }
273  }
274  }
275  else if (encoding == enc::BGRA8)
276  {
277  const cv::Mat_<cv::Vec4b> color(l_image_msg->height, l_image_msg->width,
278  (cv::Vec4b*)&l_image_msg->data[0],
279  l_image_msg->step);
280  for (int v = 0; v < mat.rows; ++v)
281  {
282  for (int u = 0; u < mat.cols; ++u, ++iter_r, ++iter_g, ++iter_b)
283  {
284  const cv::Vec4b& bgra = color(v,u);
285  *iter_r = bgra[2];
286  *iter_g = bgra[1];
287  *iter_b = bgra[0];
288  }
289  }
290  }
291  else
292  {
293  NODELET_WARN_THROTTLE(30, "Could not fill color channel of the point cloud, "
294  "unsupported encoding '%s'", encoding.c_str());
295  }
296 
297  pub_points2_.publish(points_msg);
298 }
299 
300 } // namespace stereo_image_proc
301 
302 // Register nodelet
image_transport::SubscriberFilter
sensor_msgs::image_encodings
point_cloud2_iterator.h
ros::Publisher
image_encodings.h
image_transport::ImageTransport
message_filters::Synchronizer
boost::shared_ptr< image_transport::ImageTransport >
ros.h
ros::TransportHints
ros::SubscriberStatusCallback
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
stereo_camera_model.h
NODELET_WARN_THROTTLE
#define NODELET_WARN_THROTTLE(rate,...)
message_filters::Subscriber< CameraInfo >
message_filters::sync_policies::ApproximateTime
sensor_msgs::PointCloud2Iterator
subscriber_filter.h
subscriber.h
stereo_image_proc::PointCloud2Nodelet::onInit
virtual void onInit()
Definition: point_cloud2.cpp:125
image_geometry::StereoCameraModel
image_geometry::StereoCameraModel::MISSING_Z
static const double MISSING_Z
stereo_image_proc::PointCloud2Nodelet
Definition: point_cloud2.cpp:92
image_transport.h
exact_time.h
message_filters::sync_policies
stereo_image_proc
Definition: processor.h:44
nodelet::Nodelet
PLUGINLIB_EXPORT_CLASS
PLUGINLIB_EXPORT_CLASS(image_proc::CropNonZeroNodelet, nodelet::Nodelet)
nodelet.h
sensor_msgs::PointCloud2Modifier
class_list_macros.hpp
ros::NodeHandle::param
T param(const std::string &param_name, const T &default_val) const
synchronizer.h
approximate_time.h
stereo_image_proc::isValidPoint
bool isValidPoint(const cv::Vec3f &pt)
Definition: processor.cpp:161
image_transport::TransportHints
message_filters::sync_policies::ExactTime
sensor_msgs
stereo_image_proc::PointCloud2Nodelet::connectCb
void connectCb()
Definition: point_cloud2.cpp:162
stereo_image_proc::PointCloud2Nodelet::imageCb
void imageCb(const ImageConstPtr &l_image_msg, const CameraInfoConstPtr &l_info_msg, const CameraInfoConstPtr &r_info_msg, const DisparityImageConstPtr &disp_msg)
Definition: point_cloud2.cpp:191
sensor_msgs::PointCloud2Modifier::setPointCloud2FieldsByString
void setPointCloud2FieldsByString(int n_fields,...)
ros::NodeHandle


stereo_image_proc
Author(s): Patrick Mihelich, Kurt Konolige, Jeremy Leibs
autogenerated on Wed Jan 24 2024 03:57:24