00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034 #include <boost/version.hpp>
00035 #if ((BOOST_VERSION / 100) % 1000) >= 53
00036 #include <boost/thread/lock_guard.hpp>
00037 #endif
00038
00039 #include <ros/ros.h>
00040 #include <nodelet/nodelet.h>
00041 #include <image_transport/image_transport.h>
00042 #include <image_transport/subscriber_filter.h>
00043 #include <message_filters/subscriber.h>
00044 #include <message_filters/synchronizer.h>
00045 #include <message_filters/sync_policies/approximate_time.h>
00046 #include <pcl_ros/point_cloud.h>
00047 #include <pcl/point_types.h>
00048 #include <sensor_msgs/image_encodings.h>
00049 #include <image_geometry/pinhole_camera_model.h>
00050 #include "depth_traits.h"
00051 #include <cv_bridge/cv_bridge.h>
00052 #include <opencv2/imgproc/imgproc.hpp>
00053
00054 namespace depth_image_proc {
00055
00056 using namespace message_filters::sync_policies;
00057 namespace enc = sensor_msgs::image_encodings;
00058
00059 typedef union
00060 {
00061 struct
00062 {
00063 unsigned char Blue;
00064 unsigned char Green;
00065 unsigned char Red;
00066 unsigned char Alpha;
00067 };
00068 float float_value;
00069 long long_value;
00070 } RGBValue;
00071
00072 class PointCloudXyzrgbNodelet : public nodelet::Nodelet
00073 {
00074 ros::NodeHandlePtr rgb_nh_;
00075 boost::shared_ptr<image_transport::ImageTransport> rgb_it_, depth_it_;
00076
00077
00078 image_transport::SubscriberFilter sub_depth_, sub_rgb_;
00079 message_filters::Subscriber<sensor_msgs::CameraInfo> sub_info_;
00080 typedef ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> SyncPolicy;
00081 typedef message_filters::Synchronizer<SyncPolicy> Synchronizer;
00082 boost::shared_ptr<Synchronizer> sync_;
00083
00084
00085 boost::mutex connect_mutex_;
00086 typedef pcl::PointCloud<pcl::PointXYZRGB> PointCloud;
00087 ros::Publisher pub_point_cloud_;
00088
00089 image_geometry::PinholeCameraModel model_;
00090
00091 virtual void onInit();
00092
00093 void connectCb();
00094
00095 void imageCb(const sensor_msgs::ImageConstPtr& depth_msg,
00096 const sensor_msgs::ImageConstPtr& rgb_msg,
00097 const sensor_msgs::CameraInfoConstPtr& info_msg);
00098
00099 template<typename T>
00100 void convert(const sensor_msgs::ImageConstPtr& depth_msg,
00101 const sensor_msgs::ImageConstPtr& rgb_msg,
00102 const PointCloud::Ptr& cloud_msg,
00103 int red_offset, int green_offset, int blue_offset, int color_step);
00104 };
00105
00106 void PointCloudXyzrgbNodelet::onInit()
00107 {
00108 ros::NodeHandle& nh = getNodeHandle();
00109 ros::NodeHandle& private_nh = getPrivateNodeHandle();
00110 rgb_nh_.reset( new ros::NodeHandle(nh, "rgb") );
00111 ros::NodeHandle depth_nh(nh, "depth_registered");
00112 rgb_it_ .reset( new image_transport::ImageTransport(*rgb_nh_) );
00113 depth_it_.reset( new image_transport::ImageTransport(depth_nh) );
00114
00115
00116 int queue_size;
00117 private_nh.param("queue_size", queue_size, 5);
00118
00119
00120 sync_.reset( new Synchronizer(SyncPolicy(queue_size), sub_depth_, sub_rgb_, sub_info_) );
00121 sync_->registerCallback(boost::bind(&PointCloudXyzrgbNodelet::imageCb, this, _1, _2, _3));
00122
00123
00124 ros::SubscriberStatusCallback connect_cb = boost::bind(&PointCloudXyzrgbNodelet::connectCb, this);
00125
00126 boost::lock_guard<boost::mutex> lock(connect_mutex_);
00127 pub_point_cloud_ = depth_nh.advertise<PointCloud>("points", 1, connect_cb, connect_cb);
00128 }
00129
00130
00131 void PointCloudXyzrgbNodelet::connectCb()
00132 {
00133 boost::lock_guard<boost::mutex> lock(connect_mutex_);
00134 if (pub_point_cloud_.getNumSubscribers() == 0)
00135 {
00136 sub_depth_.unsubscribe();
00137 sub_rgb_ .unsubscribe();
00138 sub_info_ .unsubscribe();
00139 }
00140 else if (!sub_depth_.getSubscriber())
00141 {
00142 ros::NodeHandle& private_nh = getPrivateNodeHandle();
00143
00144 std::string depth_image_transport_param = "depth_image_transport";
00145
00146
00147 image_transport::TransportHints depth_hints("raw",ros::TransportHints(), private_nh, depth_image_transport_param);
00148 sub_depth_.subscribe(*depth_it_, "image_rect", 1, depth_hints);
00149
00150
00151 image_transport::TransportHints hints("raw", ros::TransportHints(), private_nh);
00152 sub_rgb_ .subscribe(*rgb_it_, "image_rect_color", 1, hints);
00153 sub_info_ .subscribe(*rgb_nh_, "camera_info", 1);
00154 }
00155 }
00156
00157 void PointCloudXyzrgbNodelet::imageCb(const sensor_msgs::ImageConstPtr& depth_msg,
00158 const sensor_msgs::ImageConstPtr& rgb_msg_in,
00159 const sensor_msgs::CameraInfoConstPtr& info_msg)
00160 {
00161
00162 if (depth_msg->header.frame_id != rgb_msg_in->header.frame_id)
00163 {
00164 NODELET_ERROR_THROTTLE(5, "Depth image frame id [%s] doesn't match RGB image frame id [%s]",
00165 depth_msg->header.frame_id.c_str(), rgb_msg_in->header.frame_id.c_str());
00166 return;
00167 }
00168
00169
00170 model_.fromCameraInfo(info_msg);
00171
00172
00173 sensor_msgs::ImageConstPtr rgb_msg = rgb_msg_in;
00174 if (depth_msg->width != rgb_msg->width || depth_msg->height != rgb_msg->height)
00175 {
00176 sensor_msgs::CameraInfo info_msg_tmp = *info_msg;
00177 info_msg_tmp.width = depth_msg->width;
00178 info_msg_tmp.height = depth_msg->height;
00179 float ratio = float(depth_msg->width)/float(rgb_msg->width);
00180 info_msg_tmp.K[0] *= ratio;
00181 info_msg_tmp.K[2] *= ratio;
00182 info_msg_tmp.K[4] *= ratio;
00183 info_msg_tmp.K[5] *= ratio;
00184 info_msg_tmp.P[0] *= ratio;
00185 info_msg_tmp.P[2] *= ratio;
00186 info_msg_tmp.P[5] *= ratio;
00187 info_msg_tmp.P[6] *= ratio;
00188 model_.fromCameraInfo(info_msg_tmp);
00189
00190 cv_bridge::CvImageConstPtr cv_ptr;
00191 try
00192 {
00193 cv_ptr = cv_bridge::toCvShare(rgb_msg, rgb_msg->encoding);
00194 }
00195 catch (cv_bridge::Exception& e)
00196 {
00197 ROS_ERROR("cv_bridge exception: %s", e.what());
00198 return;
00199 }
00200 cv_bridge::CvImage cv_rsz;
00201 cv_rsz.header = cv_ptr->header;
00202 cv_rsz.encoding = cv_ptr->encoding;
00203 cv::resize(cv_ptr->image.rowRange(0,depth_msg->height/ratio), cv_rsz.image, cv::Size(depth_msg->width, depth_msg->height));
00204 if ((rgb_msg->encoding == enc::RGB8) || (rgb_msg->encoding == enc::BGR8) || (rgb_msg->encoding == enc::MONO8))
00205 rgb_msg = cv_rsz.toImageMsg();
00206 else
00207 rgb_msg = cv_bridge::toCvCopy(cv_rsz.toImageMsg(), enc::RGB8)->toImageMsg();
00208
00209
00210
00211
00212 } else
00213 rgb_msg = rgb_msg_in;
00214
00215
00216 int red_offset, green_offset, blue_offset, color_step;
00217 if (rgb_msg->encoding == enc::RGB8)
00218 {
00219 red_offset = 0;
00220 green_offset = 1;
00221 blue_offset = 2;
00222 color_step = 3;
00223 }
00224 else if (rgb_msg->encoding == enc::BGR8)
00225 {
00226 red_offset = 2;
00227 green_offset = 1;
00228 blue_offset = 0;
00229 color_step = 3;
00230 }
00231 else if (rgb_msg->encoding == enc::MONO8)
00232 {
00233 red_offset = 0;
00234 green_offset = 0;
00235 blue_offset = 0;
00236 color_step = 1;
00237 }
00238 else
00239 {
00240 try
00241 {
00242 rgb_msg = cv_bridge::toCvCopy(rgb_msg, enc::RGB8)->toImageMsg();
00243 }
00244 catch (cv_bridge::Exception& e)
00245 {
00246 NODELET_ERROR_THROTTLE(5, "Unsupported encoding [%s]: %s", rgb_msg->encoding.c_str(), e.what());
00247 return;
00248 }
00249 red_offset = 0;
00250 green_offset = 1;
00251 blue_offset = 2;
00252 color_step = 3;
00253 }
00254
00255
00256 PointCloud::Ptr cloud_msg (new PointCloud);
00257 cloud_msg->header = depth_msg->header;
00258 cloud_msg->height = depth_msg->height;
00259 cloud_msg->width = depth_msg->width;
00260 cloud_msg->is_dense = false;
00261 cloud_msg->points.resize (cloud_msg->height * cloud_msg->width);
00262
00263 if (depth_msg->encoding == enc::TYPE_16UC1)
00264 {
00265 convert<uint16_t>(depth_msg, rgb_msg, cloud_msg, red_offset, green_offset, blue_offset, color_step);
00266 }
00267 else if (depth_msg->encoding == enc::TYPE_32FC1)
00268 {
00269 convert<float>(depth_msg, rgb_msg, cloud_msg, red_offset, green_offset, blue_offset, color_step);
00270 }
00271 else
00272 {
00273 NODELET_ERROR_THROTTLE(5, "Depth image has unsupported encoding [%s]", depth_msg->encoding.c_str());
00274 return;
00275 }
00276
00277 pub_point_cloud_.publish (cloud_msg);
00278 }
00279
00280 template<typename T>
00281 void PointCloudXyzrgbNodelet::convert(const sensor_msgs::ImageConstPtr& depth_msg,
00282 const sensor_msgs::ImageConstPtr& rgb_msg,
00283 const PointCloud::Ptr& cloud_msg,
00284 int red_offset, int green_offset, int blue_offset, int color_step)
00285 {
00286
00287 float center_x = model_.cx();
00288 float center_y = model_.cy();
00289
00290
00291 double unit_scaling = DepthTraits<T>::toMeters( T(1) );
00292 float constant_x = unit_scaling / model_.fx();
00293 float constant_y = unit_scaling / model_.fy();
00294 float bad_point = std::numeric_limits<float>::quiet_NaN ();
00295
00296 const T* depth_row = reinterpret_cast<const T*>(&depth_msg->data[0]);
00297 int row_step = depth_msg->step / sizeof(T);
00298 const uint8_t* rgb = &rgb_msg->data[0];
00299 int rgb_skip = rgb_msg->step - rgb_msg->width * color_step;
00300 PointCloud::iterator pt_iter = cloud_msg->begin ();
00301
00302 for (int v = 0; v < (int)cloud_msg->height; ++v, depth_row += row_step, rgb += rgb_skip)
00303 {
00304 for (int u = 0; u < (int)cloud_msg->width; ++u, rgb += color_step)
00305 {
00306 pcl::PointXYZRGB& pt = *pt_iter++;
00307 T depth = depth_row[u];
00308
00309
00310 if (!DepthTraits<T>::valid(depth))
00311 {
00312 pt.x = pt.y = pt.z = bad_point;
00313 }
00314 else
00315 {
00316
00317 pt.x = (u - center_x) * depth * constant_x;
00318 pt.y = (v - center_y) * depth * constant_y;
00319 pt.z = DepthTraits<T>::toMeters(depth);
00320 }
00321
00322
00323 RGBValue color;
00324 color.Red = rgb[red_offset];
00325 color.Green = rgb[green_offset];
00326 color.Blue = rgb[blue_offset];
00327 color.Alpha = 0;
00328 pt.rgb = color.float_value;
00329 }
00330 }
00331 }
00332
00333 }
00334
00335
00336 #include <pluginlib/class_list_macros.h>
00337 PLUGINLIB_EXPORT_CLASS(depth_image_proc::PointCloudXyzrgbNodelet,nodelet::Nodelet);