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/point_indices_to_mask_image.h"
00037 #include <sensor_msgs/image_encodings.h>
00038 #include <cv_bridge/cv_bridge.h>
00039
00040 namespace jsk_pcl_ros
00041 {
00042 void PointIndicesToMaskImage::onInit()
00043 {
00044 DiagnosticNodelet::onInit();
00045 pnh_->param("approximate_sync", approximate_sync_, false);
00046 pub_ = advertise<sensor_msgs::Image>(*pnh_, "output", 1);
00047 }
00048
00049 void PointIndicesToMaskImage::subscribe()
00050 {
00051 sub_input_.subscribe(*pnh_, "input", 1);
00052 sub_image_.subscribe(*pnh_, "input/image", 1);
00053 if (approximate_sync_) {
00054 async_ = boost::make_shared<message_filters::Synchronizer<ApproximateSyncPolicy> >(100);
00055 async_->connectInput(sub_input_, sub_image_);
00056 async_->registerCallback(boost::bind(&PointIndicesToMaskImage::mask, this, _1, _2));
00057 }
00058 else {
00059 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
00060 sync_->connectInput(sub_input_, sub_image_);
00061 sync_->registerCallback(boost::bind(&PointIndicesToMaskImage::mask,
00062 this, _1, _2));
00063 }
00064 }
00065
00066 void PointIndicesToMaskImage::unsubscribe()
00067 {
00068 sub_input_.unsubscribe();
00069 sub_image_.unsubscribe();
00070 }
00071
00072 void PointIndicesToMaskImage::mask(
00073 const PCLIndicesMsg::ConstPtr& indices_msg,
00074 const sensor_msgs::Image::ConstPtr& image_msg)
00075 {
00076 vital_checker_->poke();
00077 int width = image_msg->width;
00078 int height = image_msg->height;
00079 cv::Mat mask_image = cv::Mat::zeros(height, width, CV_8UC1);
00080 for (size_t i = 0; i < indices_msg->indices.size(); i++) {
00081 int index = indices_msg->indices[i];
00082 int width_index = index % width;
00083 int height_index = index / width;
00084 mask_image.at<uchar>(height_index, width_index) = 255;
00085 }
00086 cv_bridge::CvImage mask_bridge(indices_msg->header,
00087 sensor_msgs::image_encodings::MONO8,
00088 mask_image);
00089 pub_.publish(mask_bridge.toImageMsg());
00090 }
00091
00092 void PointIndicesToMaskImage::updateDiagnostic(
00093 diagnostic_updater::DiagnosticStatusWrapper &stat)
00094 {
00095 if (vital_checker_->isAlive()) {
00096 stat.summary(diagnostic_msgs::DiagnosticStatus::OK,
00097 "PointIndicesToMaskImage running");
00098 }
00099 else {
00100 jsk_topic_tools::addDiagnosticErrorSummary(
00101 "PointIndicesToMaskImage", vital_checker_, stat);
00102 }
00103 }
00104 }
00105
00106 #include <pluginlib/class_list_macros.h>
00107 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::PointIndicesToMaskImage, nodelet::Nodelet);