roi_clipper_nodelet.cpp
Go to the documentation of this file.
00001 // -*- mode: c++ -*-
00002 /*********************************************************************
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2014, JSK Lab
00006  *  All rights reserved.
00007  *
00008  *  Redistribution and use in source and binary forms, with or without
00009  *  modification, are permitted provided that the following conditions
00010  *  are met:
00011  *
00012  *   * Redistributions of source code must retain the above copyright
00013  *     notice, this list of conditions and the following disclaimer.
00014  *   * Redistributions in binary form must reproduce the above
00015  *     copyright notice, this list of conditions and the following
00016  *     disclaimer in the documentation and/o2r other materials provided
00017  *     with the distribution.
00018  *   * Neither the name of the JSK Lab nor the names of its
00019  *     contributors may be used to endorse or promote products derived
00020  *     from this software without specific prior written permission.
00021  *
00022  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033  *  POSSIBILITY OF SUCH DAMAGE.
00034  *********************************************************************/
00035 
00036 #define BOOST_PARAMETER_MAX_ARITY 7
00037 
00038 #include "jsk_pcl_ros/roi_clipper.h"
00039 #include <cv_bridge/cv_bridge.h>
00040 #include <sensor_msgs/image_encodings.h>
00041 #include <opencv2/opencv.hpp>
00042 #include <image_geometry/pinhole_camera_model.h>
00043 #include "jsk_recognition_utils/pcl_conversion_util.h"
00044 
00045 namespace jsk_pcl_ros
00046 {
00047   void ROIClipper::onInit()
00048   {
00049     DiagnosticNodelet::onInit();
00050     pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
00051     pnh_->param("not_sync", not_sync_, false);
00052     pnh_->param("keep_organized", keep_organized_, false);
00053     pub_image_ = advertise<sensor_msgs::Image>(*pnh_, "output", 1);
00054     if (not_sync_) {
00055       pub_cloud_ = advertise<sensor_msgs::PointCloud2>(*pnh_, "output/cloud", 1);
00056       pub_cloud_indices_ = advertise<PCLIndicesMsg>(*pnh_, "output/cloud_indices", 1);
00057     }
00058     onInitPostProcess();
00059   }
00060 
00061   void ROIClipper::subscribe()
00062   {
00063     if (!not_sync_) {
00064       sub_image_.subscribe(*pnh_, "input/image", 1);
00065       sub_info_.subscribe(*pnh_, "input/camera_info", 1);
00066       sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(100);
00067       sync_->connectInput(sub_image_, sub_info_);
00068       sync_->registerCallback(boost::bind(&ROIClipper::clip, this, _1, _2));
00069     }
00070     else {
00071       sub_image_no_sync_ = pnh_->subscribe(
00072         "input/image", 1,
00073         &ROIClipper::imageCallback, this);
00074       sub_info_no_sync_ = pnh_->subscribe(
00075         "input/camera_info", 1,
00076         &ROIClipper::infoCallback, this);
00077       sub_cloud_no_sync_ = pnh_->subscribe(
00078         "input/cloud", 1,
00079         &ROIClipper::cloudCallback, this);
00080     }
00081   }
00082   
00083   void ROIClipper::unsubscribe()
00084   {
00085     if (!not_sync_) {
00086       sub_image_.unsubscribe();
00087       sub_info_.unsubscribe();
00088     }
00089     else {
00090       sub_image_no_sync_.shutdown();
00091       sub_info_no_sync_.shutdown();
00092       sub_cloud_no_sync_.shutdown();
00093     }
00094   }
00095   void ROIClipper::clip(const sensor_msgs::Image::ConstPtr& image_msg,
00096                         const sensor_msgs::CameraInfo::ConstPtr& camera_info_msg)
00097   {
00098     vital_checker_->poke();
00099     try {
00100       cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::RGB8);
00101       cv::Mat image = cv_ptr->image;
00102       cv::Rect roi(camera_info_msg->roi.x_offset, camera_info_msg->roi.y_offset,
00103                    camera_info_msg->roi.width, camera_info_msg->roi.height);
00104       //NODELET_INFO("roi::(%d, %d, %d, %d)", roi.x, roi.y, roi.width, roi.height);
00105       cv::Mat image_roi = image(roi);
00106       // cv::imshow("roi", image_roi);
00107       // cv::waitKey(3);
00108       cv_bridge::CvImage roi_bridge(image_msg->header,
00109                                     sensor_msgs::image_encodings::RGB8,
00110                                     image_roi);
00111       pub_image_.publish(roi_bridge.toImageMsg());
00112     }
00113     catch (cv_bridge::Exception& e)
00114     {
00115       NODELET_ERROR("cv_bridge exception: %s", e.what());
00116       return;
00117     }
00118   }
00119 
00120   void ROIClipper::infoCallback(
00121     const sensor_msgs::CameraInfo::ConstPtr& info_msg)
00122   {
00123     boost::mutex::scoped_lock lock(mutex_);
00124     latest_camera_info_ = info_msg;
00125   }
00126 
00127   void ROIClipper::imageCallback(
00128     const sensor_msgs::Image::ConstPtr& image_msg)
00129   {
00130     boost::mutex::scoped_lock lock(mutex_);
00131     if (latest_camera_info_) {
00132       clip(image_msg, latest_camera_info_);
00133     }
00134   }
00135 
00136   void ROIClipper::cloudCallback(
00137     const sensor_msgs::PointCloud2::ConstPtr& cloud_msg)
00138   {
00139     boost::mutex::scoped_lock lock(mutex_);
00140     vital_checker_->poke();
00141     if (latest_camera_info_) {
00142       image_geometry::PinholeCameraModel model;
00143       PCLIndicesMsg indices;
00144       model.fromCameraInfo(latest_camera_info_);
00145       pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud
00146         (new pcl::PointCloud<pcl::PointXYZRGB>);
00147       pcl::fromROSMsg(*cloud_msg, *cloud);
00148       pcl::PointCloud<pcl::PointXYZRGB>::Ptr clipped_cloud
00149         (new pcl::PointCloud<pcl::PointXYZRGB>);
00150       cv::Rect region = model.rectifiedRoi();
00151       pcl::PointXYZRGB nan_point;
00152       nan_point.x = nan_point.y = nan_point.z
00153         = std::numeric_limits<float>::quiet_NaN();;
00154       for (size_t i = 0; i < cloud->points.size(); i++) {
00155         pcl::PointXYZRGB p = cloud->points[i];
00156         bool foundp = false;
00157         if (!std::isnan(p.x) && !std::isnan(p.y) && !std::isnan(p.z)) {
00158           cv::Point2d uv = model.project3dToPixel(cv::Point3d(p.x, p.y, p.z));
00159           if (uv.x >= 0 && uv.x <= region.width &&
00160               uv.y >= 0 && uv.y <= region.height) {
00161             indices.indices.push_back(i);
00162             clipped_cloud->points.push_back(p);
00163             foundp = true;
00164           }
00165         }
00166         if (!foundp && keep_organized_) {
00167           clipped_cloud->points.push_back(nan_point);
00168         }
00169       }
00170       if (keep_organized_) {
00171         clipped_cloud->width = cloud->width;
00172         clipped_cloud->height = cloud->height;
00173       }
00174       sensor_msgs::PointCloud2 ros_cloud;
00175       pcl::toROSMsg(*clipped_cloud, ros_cloud);
00176       ros_cloud.header = cloud_msg->header;
00177       pub_cloud_.publish(ros_cloud);
00178       indices.header = cloud_msg->header;
00179       pub_cloud_indices_.publish(indices);
00180     }
00181   }
00182   
00183   void ROIClipper::updateDiagnostic(
00184       diagnostic_updater::DiagnosticStatusWrapper &stat)
00185   {
00186     if (vital_checker_->isAlive()) {
00187       stat.summary(diagnostic_msgs::DiagnosticStatus::OK,
00188                    "ROIClipper running");
00189     }
00190     else {
00191       jsk_topic_tools::addDiagnosticErrorSummary(
00192         "ROIClipper", vital_checker_, stat);
00193     }
00194 
00195   }
00196 }
00197 
00198 #include <pluginlib/class_list_macros.h>
00199 PLUGINLIB_EXPORT_CLASS (jsk_pcl_ros::ROIClipper, nodelet::Nodelet);


jsk_pcl_ros
Author(s): Yohei Kakiuchi
autogenerated on Sun Oct 8 2017 02:43:50