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/point_indices_to_mask_image.h"
00037 #include <jsk_topic_tools/log_utils.h>
00038 #include <sensor_msgs/image_encodings.h>
00039 #include <cv_bridge/cv_bridge.h>
00040
00041 namespace jsk_pcl_ros_utils
00042 {
00043 void PointIndicesToMaskImage::onInit()
00044 {
00045 DiagnosticNodelet::onInit();
00046 pnh_->param("approximate_sync", approximate_sync_, false);
00047 pnh_->param("queue_size", queue_size_, 100);
00048 pnh_->param("static_image_size", static_image_size_, false);
00049 pub_ = advertise<sensor_msgs::Image>(*pnh_, "output", 1);
00050 onInitPostProcess();
00051 }
00052
00053 void PointIndicesToMaskImage::subscribe()
00054 {
00055 if (static_image_size_) {
00056 pnh_->getParam("width", width_);
00057 pnh_->getParam("height", height_);
00058 sub_input_static_ = pnh_->subscribe("input", 1, &PointIndicesToMaskImage::mask, this);
00059 }
00060 else {
00061 sub_input_.subscribe(*pnh_, "input", 1);
00062 sub_image_.subscribe(*pnh_, "input/image", 1);
00063 if (approximate_sync_) {
00064 async_ = boost::make_shared<message_filters::Synchronizer<ApproximateSyncPolicy> >(queue_size_);
00065 async_->connectInput(sub_input_, sub_image_);
00066 async_->registerCallback(boost::bind(&PointIndicesToMaskImage::mask, this, _1, _2));
00067 }
00068 else {
00069 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(queue_size_);
00070 sync_->connectInput(sub_input_, sub_image_);
00071 sync_->registerCallback(boost::bind(&PointIndicesToMaskImage::mask,
00072 this, _1, _2));
00073 }
00074 }
00075 }
00076
00077 void PointIndicesToMaskImage::unsubscribe()
00078 {
00079 sub_input_.unsubscribe();
00080 if (!static_image_size_) {
00081 sub_image_.unsubscribe();
00082 }
00083 }
00084
00085 void PointIndicesToMaskImage::convertAndPublish(
00086 const PCLIndicesMsg::ConstPtr& indices_msg,
00087 const int width,
00088 const int height)
00089 {
00090 cv::Mat mask_image = cv::Mat::zeros(height, width, CV_8UC1);
00091 for (size_t i = 0; i < indices_msg->indices.size(); i++) {
00092 int index = indices_msg->indices[i];
00093 if (index >= height * width || index < 0) {
00094 ROS_ERROR("Input index is out of expected mask size.");
00095 return;
00096 }
00097 int width_index = index % width;
00098 int height_index = index / width;
00099 mask_image.at<uchar>(height_index, width_index) = 255;
00100 }
00101 cv_bridge::CvImage mask_bridge(indices_msg->header,
00102 sensor_msgs::image_encodings::MONO8,
00103 mask_image);
00104 pub_.publish(mask_bridge.toImageMsg());
00105 }
00106
00107 void PointIndicesToMaskImage::mask(
00108 const PCLIndicesMsg::ConstPtr& indices_msg)
00109 {
00110 vital_checker_->poke();
00111 convertAndPublish(indices_msg, width_, height_);
00112 }
00113
00114 void PointIndicesToMaskImage::mask(
00115 const PCLIndicesMsg::ConstPtr& indices_msg,
00116 const sensor_msgs::Image::ConstPtr& image_msg)
00117 {
00118 vital_checker_->poke();
00119 convertAndPublish(indices_msg, image_msg->width, image_msg->height);
00120 }
00121
00122 void PointIndicesToMaskImage::updateDiagnostic(
00123 diagnostic_updater::DiagnosticStatusWrapper &stat)
00124 {
00125 if (vital_checker_->isAlive()) {
00126 stat.summary(diagnostic_msgs::DiagnosticStatus::OK,
00127 "PointIndicesToMaskImage running");
00128 }
00129 else {
00130 jsk_topic_tools::addDiagnosticErrorSummary(
00131 "PointIndicesToMaskImage", vital_checker_, stat);
00132 }
00133 }
00134 }
00135
00136 #include <pluginlib/class_list_macros.h>
00137 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros_utils::PointIndicesToMaskImage, nodelet::Nodelet);