crop_non_zero.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2008, Willow Garage, Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Willow Garage nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 #include <nodelet/nodelet.h>
36 #include <boost/thread.hpp>
37 #include <cv_bridge/cv_bridge.h>
38 //#include <algorithm> // for std::max_element
39 
40 namespace image_proc {
41 
43 
45 {
46  // Subscriptions
49 
50  // Publications
51  boost::mutex connect_mutex_;
53 
54  virtual void onInit();
55 
56  void connectCb();
57 
58  void imageCb(const sensor_msgs::ImageConstPtr& raw_msg);
59 };
60 
62 {
64  it_.reset(new image_transport::ImageTransport(nh));
65 
66  // Monitor whether anyone is subscribed to the output
68  // Make sure we don't enter connectCb() between advertising and assigning to pub_depth_
69  boost::lock_guard<boost::mutex> lock(connect_mutex_);
70  pub_ = it_->advertise("image", 1, connect_cb, connect_cb);
71 }
72 
73 // Handles (un)subscribing when clients (un)subscribe
75 {
76  boost::lock_guard<boost::mutex> lock(connect_mutex_);
77  if (pub_.getNumSubscribers() == 0)
78  {
80  }
81  else if (!sub_raw_)
82  {
84  sub_raw_ = it_->subscribe("image_raw", 1, &CropNonZeroNodelet::imageCb, this, hints);
85  }
86 }
87 
88 void CropNonZeroNodelet::imageCb(const sensor_msgs::ImageConstPtr& raw_msg)
89 {
90  cv_bridge::CvImagePtr cv_ptr;
91  try
92  {
93  cv_ptr = cv_bridge::toCvCopy(raw_msg);
94  }
95  catch (cv_bridge::Exception& e)
96  {
97  ROS_ERROR("cv_bridge exception: %s", e.what());
98  return;
99  }
100 
101  // Check the number of channels
102  if(sensor_msgs::image_encodings::numChannels(raw_msg->encoding) != 1){
103  NODELET_ERROR_THROTTLE(2, "Only grayscale image is acceptable, got [%s]", raw_msg->encoding.c_str());
104  return;
105  }
106 
107  std::vector<std::vector<cv::Point> >cnt;
108  cv::Mat1b m(raw_msg->width, raw_msg->height);
109 
110  if (raw_msg->encoding == enc::TYPE_8UC1){
111  m = cv_ptr->image;
112  }else{
113  double minVal, maxVal;
114  cv::minMaxIdx(cv_ptr->image, &minVal, &maxVal, 0, 0, cv_ptr->image != 0.);
115  double ra = maxVal - minVal;
116 
117  cv_ptr->image.convertTo(m, CV_8U, 255./ra, -minVal*255./ra);
118  }
119 
120  cv::findContours(m, cnt, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
121 
122  // search the largest area
123  /* // -std=c++11
124  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) {
125  return a.size() < b.size();
126  });
127  */
128  std::vector<std::vector<cv::Point> >::iterator it = cnt.begin();
129  for(std::vector<std::vector<cv::Point> >::iterator i=cnt.begin();i!=cnt.end();++i){
130  it = (*it).size() < (*i).size() ? i : it;
131  }
132  cv::Rect r = cv::boundingRect(cnt[std::distance(cnt.begin(), it)]);
133 
134  cv_bridge::CvImage out_msg;
135  out_msg.header = raw_msg->header;
136  out_msg.encoding = raw_msg->encoding;
137  out_msg.image = cv_ptr->image(r);
138 
139  pub_.publish(out_msg.toImageMsg());
140 }
141 
142 } // namespace image_proc
143 
144 // Register as nodelet
uint32_t getNumSubscribers() const
ros::NodeHandle & getNodeHandle() const
#define NODELET_ERROR_THROTTLE(rate,...)
std::string encoding
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
ros::NodeHandle & getPrivateNodeHandle() const
image_transport::Publisher pub_
sensor_msgs::ImagePtr toImageMsg() const
PLUGINLIB_EXPORT_CLASS(image_proc::CropNonZeroNodelet, nodelet::Nodelet)
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
void publish(const sensor_msgs::Image &message) const
static int numChannels(const std::string &encoding)
void imageCb(const sensor_msgs::ImageConstPtr &raw_msg)
boost::shared_ptr< image_transport::ImageTransport > it_
#define ROS_ERROR(...)
std_msgs::Header header
image_transport::Subscriber sub_raw_


image_proc
Author(s): Patrick Mihelich, Kurt Konolige, Jeremy Leibs
autogenerated on Wed Dec 7 2022 03:25:23