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 jsk_pcl_ros_utils
00042 {
00043 void PointCloudToMaskImage::onInit()
00044 {
00045 DiagnosticNodelet::onInit();
00046 pub_ = advertise<sensor_msgs::Image>(*pnh_, "output", 1);
00047 onInitPostProcess();
00048 }
00049
00050 void PointCloudToMaskImage::subscribe()
00051 {
00052 sub_ = pnh_->subscribe("input", 1, &PointCloudToMaskImage::convert, this);
00053 }
00054
00055 void PointCloudToMaskImage::unsubscribe()
00056 {
00057 sub_.shutdown();
00058 }
00059
00060 void PointCloudToMaskImage::updateDiagnostic(diagnostic_updater::DiagnosticStatusWrapper &stat)
00061 {
00062 if (vital_checker_->isAlive())
00063 {
00064 stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "PointCloudToMaskImage running");
00065 }
00066 else
00067 {
00068 jsk_topic_tools::addDiagnosticErrorSummary("PointCloudToMaskImage", vital_checker_, stat);
00069 }
00070 }
00071
00072 void PointCloudToMaskImage::convert(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
00073 {
00074 vital_checker_->poke();
00075 pcl::PointCloud<pcl::PointXYZ>::Ptr pc(new pcl::PointCloud<pcl::PointXYZ>);
00076 pcl::fromROSMsg(*cloud_msg, *pc);
00077
00078 if (!pc->isOrganized())
00079 {
00080 NODELET_FATAL("Input point cloud is not organized.");
00081 return;
00082 }
00083
00084 cv::Mat mask_image = cv::Mat::zeros(cloud_msg->height, cloud_msg->width, CV_8UC1);
00085 for (size_t index = 0; index < pc->points.size(); index++)
00086 {
00087 if (isnan(pc->points[index].x) || isnan(pc->points[index].y) || isnan(pc->points[index].z))
00088 {
00089 continue;
00090 }
00091 int width_index = index % cloud_msg->width;
00092 int height_index = index / cloud_msg->width;
00093 mask_image.at<uchar>(height_index, width_index) = 255;
00094 }
00095 cv_bridge::CvImage mask_bridge(cloud_msg->header,
00096 sensor_msgs::image_encodings::MONO8,
00097 mask_image);
00098 pub_.publish(mask_bridge.toImageMsg());
00099 }
00100 }
00101
00102 #include <pluginlib/class_list_macros.h>
00103 PLUGINLIB_EXPORT_CLASS(jsk_pcl_ros_utils::PointCloudToMaskImage, nodelet::Nodelet);