crop_non_zero.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 * 
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 * 
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 * 
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 * 
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 #include <nodelet/nodelet.h>
00035 #include <image_transport/image_transport.h>
00036 #include <boost/thread.hpp>
00037 #include <cv_bridge/cv_bridge.h>
00038 //#include <algorithm> // for std::max_element
00039 
00040 namespace image_proc {
00041 
00042 namespace enc = sensor_msgs::image_encodings;
00043 
00044 class CropNonZeroNodelet : public nodelet::Nodelet
00045 {
00046   // Subscriptions
00047   boost::shared_ptr<image_transport::ImageTransport> it_;
00048   image_transport::Subscriber sub_raw_;
00049 
00050   // Publications
00051   boost::mutex connect_mutex_;
00052   image_transport::Publisher pub_;
00053 
00054   virtual void onInit();
00055 
00056   void connectCb();
00057 
00058   void imageCb(const sensor_msgs::ImageConstPtr& raw_msg);
00059 };
00060 
00061 void CropNonZeroNodelet::onInit()
00062 {
00063   ros::NodeHandle& nh = getNodeHandle();
00064   it_.reset(new image_transport::ImageTransport(nh));
00065 
00066   // Monitor whether anyone is subscribed to the output
00067   image_transport::SubscriberStatusCallback connect_cb = boost::bind(&CropNonZeroNodelet::connectCb, this);
00068   // Make sure we don't enter connectCb() between advertising and assigning to pub_depth_
00069   boost::lock_guard<boost::mutex> lock(connect_mutex_);
00070   pub_ = it_->advertise("image", 1, connect_cb, connect_cb);
00071 }
00072 
00073 // Handles (un)subscribing when clients (un)subscribe
00074 void CropNonZeroNodelet::connectCb()
00075 {
00076   boost::lock_guard<boost::mutex> lock(connect_mutex_);
00077   if (pub_.getNumSubscribers() == 0)
00078   {
00079     sub_raw_.shutdown();
00080   }
00081   else if (!sub_raw_)
00082   {
00083     image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle());
00084     sub_raw_ = it_->subscribe("image_raw", 1, &CropNonZeroNodelet::imageCb, this, hints);
00085   }
00086 }
00087 
00088 void CropNonZeroNodelet::imageCb(const sensor_msgs::ImageConstPtr& raw_msg)
00089 {
00090   cv_bridge::CvImagePtr cv_ptr;
00091   try
00092   {
00093     cv_ptr = cv_bridge::toCvCopy(raw_msg);
00094   }
00095   catch (cv_bridge::Exception& e)
00096   {
00097     ROS_ERROR("cv_bridge exception: %s", e.what());
00098     return;
00099   }
00100 
00101   // Check the number of channels
00102   if(sensor_msgs::image_encodings::numChannels(raw_msg->encoding) != 1){
00103     NODELET_ERROR_THROTTLE(2, "Only grayscale image is acceptable, got [%s]", raw_msg->encoding.c_str());
00104     return;
00105   }
00106 
00107   std::vector<std::vector<cv::Point> >cnt;
00108   cv::Mat1b m(raw_msg->width, raw_msg->height);
00109 
00110   if (raw_msg->encoding == enc::TYPE_8UC1){
00111     m = cv_ptr->image;
00112   }else{
00113     double minVal, maxVal;
00114     cv::minMaxIdx(cv_ptr->image, &minVal, &maxVal, 0, 0, cv_ptr->image != 0.);
00115     double ra = maxVal - minVal;
00116 
00117     cv_ptr->image.convertTo(m, CV_8U, 255./ra, -minVal*255./ra);
00118   }
00119 
00120   cv::findContours(m, cnt, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
00121 
00122   // search the largest area
00123   /* // -std=c++11
00124   std::vector<std::vector<cv::Point> >::iterator it = std::max_element(cnt.begin(), cnt.end(), [](std::vector<cv::Point> a, std::vector<cv::Point> b) {
00125     return a.size() < b.size();
00126   });
00127   */
00128   std::vector<std::vector<cv::Point> >::iterator it = cnt.begin();
00129   for(std::vector<std::vector<cv::Point> >::iterator i=cnt.begin();i!=cnt.end();++i){
00130     it = (*it).size() < (*i).size() ? i : it;
00131   }
00132   cv::Rect r = cv::boundingRect(cnt[std::distance(cnt.begin(), it)]);
00133 
00134   cv_bridge::CvImage out_msg;
00135   out_msg.header   = raw_msg->header;
00136   out_msg.encoding = raw_msg->encoding;
00137   out_msg.image    = cv_ptr->image(r);
00138 
00139   pub_.publish(out_msg.toImageMsg());
00140 }
00141 
00142 } // namespace image_proc
00143 
00144 // Register as nodelet
00145 #include <pluginlib/class_list_macros.h>
00146 PLUGINLIB_EXPORT_CLASS(image_proc::CropNonZeroNodelet,nodelet::Nodelet);


image_proc
Author(s): Patrick Mihelich, Kurt Konolige, Jeremy Leibs
autogenerated on Wed May 1 2019 02:51:47