crop_foremost.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 <ros/ros.h>
00035 #include <nodelet/nodelet.h>
00036 #include <image_transport/image_transport.h>
00037 #include <boost/thread.hpp>
00038 #include <cv_bridge/cv_bridge.h>
00039 #include <opencv2/imgproc/imgproc.hpp>
00040 
00041 namespace depth_image_proc {
00042 
00043 namespace enc = sensor_msgs::image_encodings;
00044 
00045 class CropForemostNodelet : public nodelet::Nodelet
00046 {
00047   // Subscriptions
00048   boost::shared_ptr<image_transport::ImageTransport> it_;
00049   image_transport::Subscriber sub_raw_;
00050 
00051   // Publications
00052   boost::mutex connect_mutex_;
00053   image_transport::Publisher pub_depth_;
00054 
00055   virtual void onInit();
00056 
00057   void connectCb();
00058 
00059   void depthCb(const sensor_msgs::ImageConstPtr& raw_msg);
00060 
00061   double distance_;
00062 };
00063 
00064 void CropForemostNodelet::onInit()
00065 {
00066   ros::NodeHandle& nh = getNodeHandle();
00067   ros::NodeHandle& private_nh = getPrivateNodeHandle();
00068   private_nh.getParam("distance", distance_);
00069   it_.reset(new image_transport::ImageTransport(nh));
00070 
00071   // Monitor whether anyone is subscribed to the output
00072   image_transport::SubscriberStatusCallback connect_cb = boost::bind(&CropForemostNodelet::connectCb, this);
00073   // Make sure we don't enter connectCb() between advertising and assigning to pub_depth_
00074   boost::lock_guard<boost::mutex> lock(connect_mutex_);
00075   pub_depth_ = it_->advertise("image", 1, connect_cb, connect_cb);
00076 }
00077 
00078 // Handles (un)subscribing when clients (un)subscribe
00079 void CropForemostNodelet::connectCb()
00080 {
00081   boost::lock_guard<boost::mutex> lock(connect_mutex_);
00082   if (pub_depth_.getNumSubscribers() == 0)
00083   {
00084     sub_raw_.shutdown();
00085   }
00086   else if (!sub_raw_)
00087   {
00088     image_transport::TransportHints hints("raw", ros::TransportHints(), getPrivateNodeHandle());
00089     sub_raw_ = it_->subscribe("image_raw", 1, &CropForemostNodelet::depthCb, this, hints);
00090   }
00091 }
00092 
00093 void CropForemostNodelet::depthCb(const sensor_msgs::ImageConstPtr& raw_msg)
00094 {
00095   cv_bridge::CvImagePtr cv_ptr;
00096   try
00097   {
00098     cv_ptr = cv_bridge::toCvCopy(raw_msg);
00099   }
00100   catch (cv_bridge::Exception& e)
00101   {
00102     ROS_ERROR("cv_bridge exception: %s", e.what());
00103     return;
00104   }
00105 
00106   // Check the number of channels
00107   if(sensor_msgs::image_encodings::numChannels(raw_msg->encoding) != 1){
00108     NODELET_ERROR_THROTTLE(2, "Only grayscale image is acceptable, got [%s]", raw_msg->encoding.c_str());
00109     return;
00110   }
00111 
00112   // search the min value without invalid value "0"
00113   double minVal;
00114   cv::minMaxIdx(cv_ptr->image, &minVal, 0, 0, 0, cv_ptr->image != 0);
00115 
00116   int imtype = cv_bridge::getCvType(raw_msg->encoding);
00117   switch (imtype){
00118     case CV_8UC1:
00119     case CV_8SC1:
00120     case CV_32F:
00121       cv::threshold(cv_ptr->image, cv_ptr->image, minVal + distance_, 0, CV_THRESH_TOZERO_INV);
00122       break;
00123     case CV_16UC1:
00124     case CV_16SC1:
00125     case CV_32SC1:
00126     case CV_64F:
00127       // 8 bit or 32 bit floating array is required to use cv::threshold
00128       cv_ptr->image.convertTo(cv_ptr->image, CV_32F);
00129       cv::threshold(cv_ptr->image, cv_ptr->image, minVal + distance_, 1, CV_THRESH_TOZERO_INV);
00130 
00131       cv_ptr->image.convertTo(cv_ptr->image, imtype);
00132       break;
00133   }
00134 
00135   pub_depth_.publish(cv_ptr->toImageMsg());
00136 }
00137 
00138 } // namespace depth_image_proc
00139 
00140 // Register as nodelet
00141 #include <pluginlib/class_list_macros.h>
00142 PLUGINLIB_EXPORT_CLASS(depth_image_proc::CropForemostNodelet,nodelet::Nodelet);


depth_image_proc
Author(s): Patrick Mihelich
autogenerated on Wed May 1 2019 02:51:42