00001 #include <ros/ros.h>
00002 #include <sensor_msgs/PointCloud2.h>
00003
00004 #include <pcl/pcl_base.h>
00005 #include <pcl/point_types.h>
00006 #include <pcl/ros/conversions.h>
00007 #include <pcl/filters/extract_indices.h>
00008
00009 #include <pcl_ros/pcl_nodelet.h>
00010 #include <pluginlib/class_list_macros.h>
00011
00012 #include <message_filters/subscriber.h>
00013 #include <message_filters/time_synchronizer.h>
00014 #include <message_filters/synchronizer.h>
00015
00016 #include "jsk_pcl_ros/pcl_conversion_util.h"
00017 #include <jsk_topic_tools/connection_based_nodelet.h>
00018
00019 #include <dynamic_reconfigure/server.h>
00020 #include <jsk_pcl_ros/ResizePointsPublisherConfig.h>
00021
00022 #include <cv_bridge/cv_bridge.h>
00023 #include <sensor_msgs/image_encodings.h>
00024
00025 namespace jsk_pcl_ros
00026 {
00027 class ResizePointsPublisher : public jsk_topic_tools::ConnectionBasedNodelet
00028 {
00029 typedef message_filters::sync_policies::ExactTime<sensor_msgs::PointCloud2,
00030 PCLIndicesMsg> SyncPolicy;
00031 typedef jsk_pcl_ros::ResizePointsPublisherConfig Config;
00032
00033 private:
00034 int step_x_, step_y_;
00035 message_filters::Subscriber<sensor_msgs::PointCloud2> sub_input_;
00036 message_filters::Subscriber<PCLIndicesMsg> sub_indices_;
00037 boost::shared_ptr <dynamic_reconfigure::Server<Config> > srv_;
00038 ros::Subscriber sub_;
00039 ros::Subscriber resizedmask_sub_;
00040 boost::shared_ptr<message_filters::Synchronizer<SyncPolicy> >sync_;
00041 ros::Publisher pub_;
00042 bool not_use_rgb_;
00043 boost::mutex mutex_;
00044 bool use_indices_;
00045 void onInit () {
00046 ConnectionBasedNodelet::onInit();
00047 pnh_->param("use_indices", use_indices_, false);
00048 pnh_->param("not_use_rgb", not_use_rgb_, false);
00049 srv_ = boost::make_shared <dynamic_reconfigure::Server<Config> > (*pnh_);
00050 dynamic_reconfigure::Server<Config>::CallbackType f =
00051 boost::bind (
00052 &ResizePointsPublisher::configCallback, this, _1, _2);
00053 srv_->setCallback (f);
00054 pub_ = advertise<sensor_msgs::PointCloud2>(*pnh_, "output", 1);
00055 resizedmask_sub_ = pnh_->subscribe("input/mask", 1, &ResizePointsPublisher::resizedmaskCallback, this);
00056 }
00057
00058 void configCallback(Config &config, uint32_t level) {
00059 boost::mutex::scoped_lock lock(mutex_);
00060 step_x_ = config.step_x;
00061 step_y_ = config.step_y;
00062 }
00063
00064 void resizedmaskCallback (const sensor_msgs::Image::ConstPtr& msg) {
00065 boost::mutex::scoped_lock lock(mutex_);
00066 cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy
00067 (msg, sensor_msgs::image_encodings::MONO8);
00068 cv::Mat mask = cv_ptr->image;
00069 int maskwidth = mask.cols;
00070 int maskheight = mask.rows;
00071 int cnt = 0;
00072 for (size_t j = 0; j < maskheight; j++){
00073 for (size_t i = 0; i < maskwidth; i++){
00074 if (mask.at<uchar>(j, i) != 0){
00075 cnt++;
00076 }
00077 }
00078 }
00079 int surface_per = ((double) cnt) / (maskwidth * maskheight) * 100;
00080
00081 step_x_ = sqrt(surface_per);
00082 if (step_x_ < 1) {
00083 step_x_ = 1;
00084 }
00085 step_y_ = step_x_;
00086 }
00087
00088 void subscribe()
00089 {
00090
00091 if (use_indices_) {
00092 sub_input_.subscribe(*pnh_, "input", 1);
00093 sub_indices_.subscribe(*pnh_, "indices", 1);
00094 sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(10);
00095 sync_->connectInput(sub_input_, sub_indices_);
00096 if (!not_use_rgb_) {
00097 sync_->registerCallback(boost::bind(&ResizePointsPublisher::filter<pcl::PointXYZRGB>, this, _1, _2));
00098 }
00099 else {
00100 sync_->registerCallback(boost::bind(&ResizePointsPublisher::filter<pcl::PointXYZ>, this, _1, _2));
00101 }
00102 }
00103 else {
00104 if (!not_use_rgb_) {
00105 sub_ = pnh_->subscribe(
00106 "input", 1,
00107 &ResizePointsPublisher::filter<pcl::PointXYZRGB>, this);
00108 }
00109 else {
00110 sub_ = pnh_->subscribe(
00111 "input", 1,
00112 &ResizePointsPublisher::filter<pcl::PointXYZ>, this);
00113 }
00114 }
00115 }
00116
00117 void unsubscribe()
00118 {
00119 if (use_indices_) {
00120 sub_input_.unsubscribe();
00121 sub_indices_.unsubscribe();
00122 }
00123 else {
00124 sub_.shutdown();
00125 }
00126 }
00127
00128 ~ResizePointsPublisher() { }
00129
00130 template<class T> void filter (const sensor_msgs::PointCloud2::ConstPtr &input) {
00131 filter<T>(input, PCLIndicesMsg::ConstPtr());
00132 }
00133
00134 template<class T> void filter (const sensor_msgs::PointCloud2::ConstPtr &input,
00135 const PCLIndicesMsg::ConstPtr &indices) {
00136 pcl::PointCloud<T> pcl_input_cloud, output;
00137 fromROSMsg(*input, pcl_input_cloud);
00138 boost::mutex::scoped_lock lock (mutex_);
00139 std::vector<int> ex_indices;
00140 ex_indices.resize(0);
00141
00142 int width = input->width;
00143 int height = input->height;
00144 int ox, oy, sx, sy;
00145
00146 sx = step_x_;
00147 ox = sx/2;
00148 if(height == 1) {
00149 sy = 1;
00150 oy = 0;
00151 } else {
00152 sy = step_y_;
00153 oy = sy/2;
00154 }
00155
00156 if (indices) {
00157 std::vector<int> flags;
00158 flags.resize(width*height);
00159
00160
00161
00162
00163 for(unsigned int i = 0; i < indices->indices.size(); i++) {
00164 flags[indices->indices.at(i)] = 1;
00165 }
00166 for(int y = oy; y < height; y += sy) {
00167 for(int x = ox; x < width; x += sx) {
00168 if (flags[y*width + x] == 1) {
00169 ex_indices.push_back(y*width + x);
00170 }
00171 }
00172 }
00173 } else {
00174 for(int y = oy; y < height; y += sy) {
00175 for(int x = ox; x < width; x += sx) {
00176 ex_indices.push_back(y*width + x);
00177 }
00178 }
00179 }
00180 pcl::ExtractIndices<T> extract;
00181 extract.setInputCloud (pcl_input_cloud.makeShared());
00182 extract.setIndices (boost::make_shared <std::vector<int> > (ex_indices));
00183 extract.setNegative (false);
00184 extract.filter (output);
00185
00186 if (output.points.size() > 0) {
00187 sensor_msgs::PointCloud2 ros_out;
00188 toROSMsg(output, ros_out);
00189 ros_out.header = input->header;
00190 ros_out.width = (width - ox)/sx;
00191 if((width - ox)%sx) ros_out.width += 1;
00192 ros_out.height = (height - oy)/sy;
00193 if((height - oy)%sy) ros_out.height += 1;
00194 ros_out.row_step = ros_out.point_step * ros_out.width;
00195 ros_out.is_dense = input->is_dense;
00196 #if DEBUG
00197 JSK_NODELET_INFO("%dx%d (%d %d)(%d %d) -> %dx%d %d", width,height, ox, oy, sx, sy,
00198 ros_out.width, ros_out.height, ex_indices.size());
00199 #endif
00200 pub_.publish(ros_out);
00201 JSK_NODELET_INFO("%s:: input header stamp is [%f]", getName().c_str(),
00202 input->header.stamp.toSec());
00203 JSK_NODELET_INFO("%s:: output header stamp is [%f]", getName().c_str(),
00204 ros_out.header.stamp.toSec());
00205 }
00206
00207 }
00208
00209 };
00210 }
00211
00212 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::ResizePointsPublisher, nodelet::Nodelet);