Go to the documentation of this file.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
00035 #define BOOST_PARAMETER_MAX_ARITY 7
00036 #include "jsk_pcl_ros_utils/pointcloud_to_mask_image.h"
00037 #include <jsk_recognition_utils/pcl_conversion_util.h>
00038 #include <cv_bridge/cv_bridge.h>
00039 #include <sensor_msgs/image_encodings.h>
00040
00041 namespace enc = sensor_msgs::image_encodings;
00042
00043 namespace jsk_pcl_ros_utils
00044 {
00045 void PointCloudToMaskImage::onInit()
00046 {
00047 DiagnosticNodelet::onInit();
00048
00049 srv_ = boost::make_shared<dynamic_reconfigure::Server<Config> >(*pnh_);
00050 typename dynamic_reconfigure::Server<Config>::CallbackType f =
00051 boost::bind(&PointCloudToMaskImage::configCallback, this, _1, _2);
00052 srv_->setCallback(f);
00053
00054 pub_ = advertise<sensor_msgs::Image>(*pnh_, "output", 1);
00055 onInitPostProcess();
00056 }
00057
00058 void PointCloudToMaskImage::subscribe()
00059 {
00060 sub_cloud_ = pnh_->subscribe<sensor_msgs::PointCloud2>(
00061 "input", 1, &PointCloudToMaskImage::convert, this);
00062 sub_image_ = pnh_->subscribe<sensor_msgs::Image>(
00063 "input/depth", 1, &PointCloudToMaskImage::convert, this);
00064 }
00065
00066 void PointCloudToMaskImage::unsubscribe()
00067 {
00068 sub_cloud_.shutdown();
00069 sub_image_.shutdown();
00070 }
00071
00072 void PointCloudToMaskImage::configCallback(Config &config, uint32_t level)
00073 {
00074 boost::mutex::scoped_lock lock(mutex_);
00075 z_near_ = std::min(config.z_near, config.z_far);
00076 z_far_ = std::max(config.z_near, config.z_far);
00077 config.z_near = z_near_;
00078 config.z_far = z_far_;
00079 }
00080
00081 void PointCloudToMaskImage::convert(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
00082 {
00083 boost::mutex::scoped_lock lock(mutex_);
00084
00085 vital_checker_->poke();
00086 pcl::PointCloud<pcl::PointXYZ>::Ptr pc(new pcl::PointCloud<pcl::PointXYZ>);
00087 pcl::fromROSMsg(*cloud_msg, *pc);
00088
00089 if (!pc->isOrganized())
00090 {
00091 NODELET_FATAL("Input point cloud is not organized.");
00092 return;
00093 }
00094
00095 cv::Mat mask_image = cv::Mat::zeros(cloud_msg->height, cloud_msg->width, CV_8UC1);
00096 for (size_t index = 0; index < pc->points.size(); index++)
00097 {
00098 if (isnan(pc->points[index].x) || isnan(pc->points[index].y) || isnan(pc->points[index].z))
00099 {
00100 continue;
00101 }
00102 if (pc->points[index].z < z_near_ || pc->points[index].z > z_far_)
00103 {
00104 continue;
00105 }
00106 int width_index = index % cloud_msg->width;
00107 int height_index = index / cloud_msg->width;
00108 mask_image.at<uchar>(height_index, width_index) = 255;
00109 }
00110 cv_bridge::CvImage mask_bridge(cloud_msg->header,
00111 sensor_msgs::image_encodings::MONO8,
00112 mask_image);
00113 pub_.publish(mask_bridge.toImageMsg());
00114 }
00115
00116 void PointCloudToMaskImage::convert(const sensor_msgs::Image::ConstPtr& image_msg)
00117 {
00118 boost::mutex::scoped_lock lock(mutex_);
00119 vital_checker_->poke();
00120
00121 cv_bridge::CvImage::Ptr depth_img;
00122 try {
00123 depth_img = cv_bridge::toCvCopy(image_msg);
00124 } catch (cv_bridge::Exception &e) {
00125 NODELET_ERROR_STREAM("Failed to convert image: " << e.what());
00126 return;
00127 }
00128
00129 cv_bridge::CvImage::Ptr mask_img(new cv_bridge::CvImage);
00130 mask_img->header = depth_img->header;
00131 mask_img->encoding = "mono8";
00132 mask_img->image = cv::Mat(depth_img->image.rows, depth_img->image.cols, CV_8UC1);
00133
00134 cv::MatIterator_<uint8_t>
00135 mask_it = mask_img->image.begin<uint8_t>(),
00136 mask_end = mask_img->image.end<uint8_t>();
00137
00138 if (depth_img->encoding == enc::TYPE_32FC1) {
00139 cv::MatConstIterator_<float>
00140 depth_it = depth_img->image.begin<float>(),
00141 depth_end = depth_img->image.end<float>();
00142
00143 for (; (depth_it != depth_end) && (mask_it != mask_end); ++depth_it, ++mask_it)
00144 {
00145 if (z_near_ < *depth_it && *depth_it < z_far_) *mask_it = -1;
00146 else *mask_it = 0;
00147 }
00148 } else if (depth_img->encoding == enc::TYPE_16UC1) {
00149 uint16_t z_near16 = (uint16_t) (z_near_ * 1000.0), z_far16 = (uint16_t) (z_far_ * 1000.0);
00150 cv::MatConstIterator_<uint16_t>
00151 depth_it = depth_img->image.begin<uint16_t>(),
00152 depth_end = depth_img->image.end<uint16_t>();
00153
00154 for (; (depth_it != depth_end) && (mask_it != mask_end); ++depth_it, ++mask_it)
00155 {
00156 if (z_near16 < *depth_it && *depth_it < z_far16) *mask_it = -1;
00157 else *mask_it = 0;
00158 }
00159
00160 } else {
00161 NODELET_ERROR_STREAM("Invalid encoding:" << depth_img->encoding);
00162 return;
00163 }
00164
00165 pub_.publish(mask_img->toImageMsg());
00166 }
00167 }
00168
00169 #include <pluginlib/class_list_macros.h>
00170 PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros_utils::PointCloudToMaskImage, nodelet::Nodelet);