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 pnh_->param("use_multi_channels", use_multi_channels_, false);
00047 pnh_->param("target_channel", target_channel_, -1);
00048
00049 if (use_multi_channels_ && target_channel_ < 0) {
00050 pub_ = advertise<jsk_recognition_msgs::ClusterPointIndices>(*pnh_, "output/all_indices", 1);
00051 } else {
00052 pub_ = advertise<PCLIndicesMsg>(*pnh_, "output", 1);
00053 }
00054 onInitPostProcess();
00055 }
00056
00057 void MaskImageToPointIndices::subscribe()
00058 {
00059 sub_ = pnh_->subscribe("input", 1,
00060 &MaskImageToPointIndices::indices,
00061 this);
00062 }
00063
00064 void MaskImageToPointIndices::unsubscribe()
00065 {
00066 sub_.shutdown();
00067 }
00068
00069 void MaskImageToPointIndices::indices(
00070 const sensor_msgs::Image::ConstPtr& image_msg)
00071 {
00072 vital_checker_->poke();
00073 if (use_multi_channels_) {
00074 cv_bridge::CvImageConstPtr cv_ptr = cv_bridge::toCvShare(image_msg);
00075 cv::Mat image = cv_ptr->image;
00076 if (target_channel_ < 0) {
00077 jsk_recognition_msgs::ClusterPointIndices cluster_msg;
00078 cluster_msg.header = image_msg->header;
00079 cluster_msg.cluster_indices.resize(image.channels());
00080 for (size_t c = 0; c < image.channels(); ++c) {
00081 PCLIndicesMsg &indices_msg = cluster_msg.cluster_indices[c];
00082 indices_msg.header = image_msg->header;
00083 for (size_t j = 0; j < image.rows; j++) {
00084 for (size_t i = 0; i < image.cols; i++) {
00085 if (image.data[j * image.step + i * image.elemSize() + c] > 127) {
00086 indices_msg.indices.push_back(j * image.cols + i);
00087 }
00088 }
00089 }
00090 }
00091 pub_.publish(cluster_msg);
00092 } else {
00093 if (target_channel_ > image.channels() - 1) {
00094 NODELET_ERROR("target_channel_ is %d, but image has %d channels",
00095 target_channel_, image.channels());
00096 return;
00097 }
00098 PCLIndicesMsg indices_msg;
00099 indices_msg.header = image_msg->header;
00100 for (size_t j = 0; j < image.rows; j++) {
00101 for (size_t i = 0; i < image.cols; i++) {
00102 if (image.data[j * image.step + i * image.elemSize() + target_channel_] > 127) {
00103 indices_msg.indices.push_back(j * image.cols + i);
00104 }
00105 }
00106 }
00107 pub_.publish(indices_msg);
00108 }
00109 } else {
00110 cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(
00111 image_msg, sensor_msgs::image_encodings::TYPE_8UC1);
00112 cv::Mat image = cv_ptr->image;
00113 PCLIndicesMsg indices_msg;
00114 indices_msg.header = image_msg->header;
00115 for (size_t j = 0; j < image.rows; j++) {
00116 for (size_t i = 0; i < image.cols; i++) {
00117 if (image.at<uchar>(j, i) > 127) {
00118 indices_msg.indices.push_back(j * image.cols + i);
00119 }
00120 }
00121 }
00122 pub_.publish(indices_msg);
00123 }
00124 }
00125 }
00126
00127 #include <pluginlib/class_list_macros.h>
00128 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros_utils::MaskImageToPointIndices, nodelet::Nodelet);