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/mask_image_to_point_indices.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 MaskImageToPointIndices::onInit()
00044 {
00045 DiagnosticNodelet::onInit();
00046 pub_ = advertise<PCLIndicesMsg>(*pnh_, "output", 1);
00047 onInitPostProcess();
00048 }
00049
00050 void MaskImageToPointIndices::subscribe()
00051 {
00052 sub_ = pnh_->subscribe("input", 1,
00053 &MaskImageToPointIndices::indices,
00054 this);
00055 }
00056
00057 void MaskImageToPointIndices::unsubscribe()
00058 {
00059 sub_.shutdown();
00060 }
00061
00062 void MaskImageToPointIndices::updateDiagnostic(
00063 diagnostic_updater::DiagnosticStatusWrapper &stat)
00064 {
00065 if (vital_checker_->isAlive()) {
00066 stat.summary(diagnostic_msgs::DiagnosticStatus::OK,
00067 "MaskImageToPointIndices running");
00068 }
00069 else {
00070 jsk_topic_tools::addDiagnosticErrorSummary(
00071 "MaskImageToPointIndices", vital_checker_, stat);
00072 }
00073 }
00074
00075 void MaskImageToPointIndices::indices(
00076 const sensor_msgs::Image::ConstPtr& image_msg)
00077 {
00078 vital_checker_->poke();
00079 cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(
00080 image_msg, sensor_msgs::image_encodings::MONO8);
00081 cv::Mat image = cv_ptr->image;
00082 PCLIndicesMsg indices_msg;
00083 indices_msg.header = image_msg->header;
00084 for (size_t j = 0; j < image.rows; j++) {
00085 for (size_t i = 0; i < image.cols; i++) {
00086 if (image.at<uchar>(j, i) == 255) {
00087 indices_msg.indices.push_back(j * image.cols + i);
00088 }
00089 }
00090 }
00091 pub_.publish(indices_msg);
00092 }
00093 }
00094
00095 #include <pluginlib/class_list_macros.h>
00096 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros_utils::MaskImageToPointIndices, nodelet::Nodelet);