disparity.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>
46 #include <stereo_msgs/DisparityImage.h>
48 
50 
52 
54 {
61 
62  boost::mutex connect_mutex_;
64  double min_range_;
65  double max_range_;
66  double delta_d_;
67 
68  virtual void onInit();
69 
70  void connectCb();
71 
72  void depthCb(const sensor_msgs::ImageConstPtr& depth_msg,
73  const sensor_msgs::CameraInfoConstPtr& info_msg);
74 
75  template<typename T>
76  void convert(const sensor_msgs::ImageConstPtr& depth_msg,
77  stereo_msgs::DisparityImagePtr& disp_msg);
78 };
79 
81 {
83  ros::NodeHandle &private_nh = getPrivateNodeHandle();
84  ros::NodeHandle left_nh(nh, "left");
85  left_it_.reset(new image_transport::ImageTransport(left_nh));
86  right_nh_.reset( new ros::NodeHandle(nh, "right") );
87 
88  // Read parameters
89  int queue_size;
90  private_nh.param("queue_size", queue_size, 5);
91  private_nh.param("min_range", min_range_, 0.0);
92  private_nh.param("max_range", max_range_, std::numeric_limits<double>::infinity());
93  private_nh.param("delta_d", delta_d_, 0.125);
94 
95  // Synchronize inputs. Topic subscriptions happen on demand in the connection callback.
96  sync_.reset( new Sync(sub_depth_image_, sub_info_, queue_size) );
97  sync_->registerCallback(boost::bind(&DisparityNodelet::depthCb, this, _1, _2));
98 
99  // Monitor whether anyone is subscribed to the output
100  ros::SubscriberStatusCallback connect_cb = boost::bind(&DisparityNodelet::connectCb, this);
101  // Make sure we don't enter connectCb() between advertising and assigning to pub_disparity_
102  boost::lock_guard<boost::mutex> lock(connect_mutex_);
103  pub_disparity_ = left_nh.advertise<stereo_msgs::DisparityImage>("disparity", 1, connect_cb, connect_cb);
104 }
105 
106 // Handles (un)subscribing when clients (un)subscribe
108 {
109  boost::lock_guard<boost::mutex> lock(connect_mutex_);
111  {
114  }
115  else if (!sub_depth_image_.getSubscriber())
116  {
118  sub_depth_image_.subscribe(*left_it_, "image_rect", 1, hints);
119  sub_info_.subscribe(*right_nh_, "camera_info", 1);
120  }
121 }
122 
123 void DisparityNodelet::depthCb(const sensor_msgs::ImageConstPtr& depth_msg,
124  const sensor_msgs::CameraInfoConstPtr& info_msg)
125 {
126  // Allocate new DisparityImage message
127  stereo_msgs::DisparityImagePtr disp_msg( new stereo_msgs::DisparityImage );
128  disp_msg->header = depth_msg->header;
129  disp_msg->image.header = disp_msg->header;
130  disp_msg->image.encoding = enc::TYPE_32FC1;
131  disp_msg->image.height = depth_msg->height;
132  disp_msg->image.width = depth_msg->width;
133  disp_msg->image.step = disp_msg->image.width * sizeof (float);
134  disp_msg->image.data.resize( disp_msg->image.height * disp_msg->image.step, 0.0f );
135  double fx = info_msg->P[0];
136  disp_msg->T = -info_msg->P[3] / fx;
137  disp_msg->f = fx;
138  // Remaining fields depend on device characteristics, so rely on user input
139  disp_msg->min_disparity = disp_msg->f * disp_msg->T / max_range_;
140  disp_msg->max_disparity = disp_msg->f * disp_msg->T / min_range_;
141  disp_msg->delta_d = delta_d_;
142 
143  if (depth_msg->encoding == enc::TYPE_16UC1)
144  {
145  convert<uint16_t>(depth_msg, disp_msg);
146  }
147  else if (depth_msg->encoding == enc::TYPE_32FC1)
148  {
149  convert<float>(depth_msg, disp_msg);
150  }
151  else
152  {
153  NODELET_ERROR_THROTTLE(5, "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str());
154  return;
155  }
156 
157  pub_disparity_.publish(disp_msg);
158 }
159 
160 template<typename T>
161 void DisparityNodelet::convert(const sensor_msgs::ImageConstPtr& depth_msg,
162  stereo_msgs::DisparityImagePtr& disp_msg)
163 {
164  // For each depth Z, disparity d = fT / Z
165  float unit_scaling = DepthTraits<T>::toMeters( T(1) );
166  float constant = disp_msg->f * disp_msg->T / unit_scaling;
167 
168  const T* depth_row = reinterpret_cast<const T*>(&depth_msg->data[0]);
169  int row_step = depth_msg->step / sizeof(T);
170  float* disp_data = reinterpret_cast<float*>(&disp_msg->image.data[0]);
171  for (int v = 0; v < (int)depth_msg->height; ++v)
172  {
173  for (int u = 0; u < (int)depth_msg->width; ++u)
174  {
175  T depth = depth_row[u];
176  if (DepthTraits<T>::valid(depth))
177  *disp_data = constant / depth;
178  ++disp_data;
179  }
180 
181  depth_row += row_step;
182  }
183 }
184 
185 } // namespace xiaoqiang_depth_image_proc
186 
187 // Register as nodelet
image_transport::SubscriberFilter sub_depth_image_
Definition: disparity.cpp:57
message_filters::TimeSynchronizer< sensor_msgs::Image, sensor_msgs::CameraInfo > Sync
Definition: disparity.cpp:59
boost::shared_ptr< image_transport::ImageTransport > left_it_
Definition: disparity.cpp:55
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
void publish(const boost::shared_ptr< M > &message) const
#define NODELET_ERROR_THROTTLE(rate,...)
void depthCb(const sensor_msgs::ImageConstPtr &depth_msg, const sensor_msgs::CameraInfoConstPtr &info_msg)
Definition: disparity.cpp:123
PLUGINLIB_EXPORT_CLASS(xiaoqiang_depth_image_proc::DisparityNodelet, nodelet::Nodelet)
ros::NodeHandle & getPrivateNodeHandle() const
bool param(const std::string &param_name, T &param_val, const T &default_val) const
const Subscriber & getSubscriber() const
message_filters::Subscriber< sensor_msgs::CameraInfo > sub_info_
Definition: disparity.cpp:58
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::NodeHandle & getNodeHandle() const
void subscribe(ImageTransport &it, const std::string &base_topic, uint32_t queue_size, const TransportHints &transport_hints=TransportHints())
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)
void convert(const sensor_msgs::ImageConstPtr &depth_msg, stereo_msgs::DisparityImagePtr &disp_msg)
Definition: disparity.cpp:161


xiaoqiang_depth_image_proc
Author(s): Xie fusheng
autogenerated on Mon Jun 10 2019 15:53:04