35 #define BOOST_PARAMETER_MAX_ARITY 7 47 DiagnosticNodelet::onInit();
49 srv_ = boost::make_shared<dynamic_reconfigure::Server<Config> >(*pnh_);
50 typename dynamic_reconfigure::Server<Config>::CallbackType
f =
54 pub_ = advertise<sensor_msgs::Image>(*
pnh_,
"output", 1);
75 z_near_ = std::min(config.z_near, config.z_far);
76 z_far_ = std::max(config.z_near, config.z_far);
86 pcl::PointCloud<pcl::PointXYZ>::Ptr pc(
new pcl::PointCloud<pcl::PointXYZ>);
89 if (!pc->isOrganized())
95 cv::Mat mask_image = cv::Mat::zeros(cloud_msg->height, cloud_msg->width, CV_8UC1);
98 if (isnan(pc->points[
index].x) || isnan(pc->points[
index].y) || isnan(pc->points[
index].z))
106 int width_index =
index % cloud_msg->width;
107 int height_index =
index / cloud_msg->width;
108 mask_image.at<uchar>(height_index, width_index) = 255;
130 mask_img->header = depth_img->header;
131 mask_img->encoding =
"mono8";
132 mask_img->image = cv::Mat(depth_img->image.rows, depth_img->image.cols, CV_8UC1);
134 cv::MatIterator_<uint8_t>
135 mask_it = mask_img->image.begin<uint8_t>(),
136 mask_end = mask_img->image.end<uint8_t>();
138 if (depth_img->encoding == enc::TYPE_32FC1) {
139 cv::MatConstIterator_<float>
140 depth_it = depth_img->image.begin<
float>(),
141 depth_end = depth_img->image.end<
float>();
143 for (; (depth_it != depth_end) && (mask_it != mask_end); ++depth_it, ++mask_it)
145 if (
z_near_ < *depth_it && *depth_it <
z_far_) *mask_it = -1;
148 }
else if (depth_img->encoding == enc::TYPE_16UC1) {
149 uint16_t z_near16 = (uint16_t) (
z_near_ * 1000.0), z_far16 = (uint16_t) (
z_far_ * 1000.0);
150 cv::MatConstIterator_<uint16_t>
151 depth_it = depth_img->image.begin<uint16_t>(),
152 depth_end = depth_img->image.end<uint16_t>();
154 for (; (depth_it != depth_end) && (mask_it != mask_end); ++depth_it, ++mask_it)
156 if (z_near16 < *depth_it && *depth_it < z_far16) *mask_it = -1;
PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros_utils::PointCloudToMaskImage, nodelet::Nodelet)
boost::shared_ptr< dynamic_reconfigure::Server< Config > > srv_
void publish(const boost::shared_ptr< M > &message) const
virtual void convert(const sensor_msgs::Image::ConstPtr &image_msg)
ros::Subscriber sub_image_
void fromROSMsg(const sensor_msgs::PointCloud2 &cloud, pcl::PointCloud< T > &pcl_cloud)
PointCloudToMaskImageConfig Config
#define NODELET_ERROR_STREAM(...)
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
ros::Subscriber sub_cloud_
virtual void unsubscribe()
#define NODELET_FATAL(...)
virtual void configCallback(Config &config, uint32_t level)