crop_foremost.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 <ros/ros.h>
35 #include <nodelet/nodelet.h>
37 #include <boost/thread.hpp>
38 #include <cv_bridge/cv_bridge.h>
39 #include <opencv2/imgproc/imgproc.hpp>
40 
41 namespace depth_image_proc {
42 
44 
46 {
47  // Subscriptions
50 
51  // Publications
52  boost::mutex connect_mutex_;
54 
55  virtual void onInit();
56 
57  void connectCb();
58 
59  void depthCb(const sensor_msgs::ImageConstPtr& raw_msg);
60 
61  double distance_;
62 };
63 
65 {
67  ros::NodeHandle& private_nh = getPrivateNodeHandle();
68  private_nh.getParam("distance", distance_);
69  it_.reset(new image_transport::ImageTransport(nh));
70 
71  // Monitor whether anyone is subscribed to the output
73  // Make sure we don't enter connectCb() between advertising and assigning to pub_depth_
74  boost::lock_guard<boost::mutex> lock(connect_mutex_);
75  pub_depth_ = it_->advertise("image", 1, connect_cb, connect_cb);
76 }
77 
78 // Handles (un)subscribing when clients (un)subscribe
80 {
81  boost::lock_guard<boost::mutex> lock(connect_mutex_);
82  if (pub_depth_.getNumSubscribers() == 0)
83  {
85  }
86  else if (!sub_raw_)
87  {
89  sub_raw_ = it_->subscribe("image_raw", 1, &CropForemostNodelet::depthCb, this, hints);
90  }
91 }
92 
93 void CropForemostNodelet::depthCb(const sensor_msgs::ImageConstPtr& raw_msg)
94 {
95  cv_bridge::CvImagePtr cv_ptr;
96  try
97  {
98  cv_ptr = cv_bridge::toCvCopy(raw_msg);
99  }
100  catch (cv_bridge::Exception& e)
101  {
102  ROS_ERROR("cv_bridge exception: %s", e.what());
103  return;
104  }
105 
106  // Check the number of channels
107  if(sensor_msgs::image_encodings::numChannels(raw_msg->encoding) != 1){
108  NODELET_ERROR_THROTTLE(2, "Only grayscale image is acceptable, got [%s]", raw_msg->encoding.c_str());
109  return;
110  }
111 
112  // search the min value without invalid value "0"
113  double minVal;
114  cv::minMaxIdx(cv_ptr->image, &minVal, 0, 0, 0, cv_ptr->image != 0);
115 
116  int imtype = cv_bridge::getCvType(raw_msg->encoding);
117  switch (imtype){
118  case CV_8UC1:
119  case CV_8SC1:
120  case CV_32F:
121  cv::threshold(cv_ptr->image, cv_ptr->image, minVal + distance_, 0, CV_THRESH_TOZERO_INV);
122  break;
123  case CV_16UC1:
124  case CV_16SC1:
125  case CV_32SC1:
126  case CV_64F:
127  // 8 bit or 32 bit floating array is required to use cv::threshold
128  cv_ptr->image.convertTo(cv_ptr->image, CV_32F);
129  cv::threshold(cv_ptr->image, cv_ptr->image, minVal + distance_, 1, CV_THRESH_TOZERO_INV);
130 
131  cv_ptr->image.convertTo(cv_ptr->image, imtype);
132  break;
133  }
134 
135  pub_depth_.publish(cv_ptr->toImageMsg());
136 }
137 
138 } // namespace depth_image_proc
139 
140 // Register as nodelet
#define NODELET_ERROR_THROTTLE(rate,...)
image_transport::Publisher pub_depth_
boost::function< void(const SingleSubscriberPublisher &)> SubscriberStatusCallback
boost::shared_ptr< image_transport::ImageTransport > it_
uint32_t getNumSubscribers() const
ros::NodeHandle & getPrivateNodeHandle() const
void publish(const sensor_msgs::Image &message) const
PLUGINLIB_EXPORT_CLASS(depth_image_proc::CropForemostNodelet, nodelet::Nodelet)
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
ros::NodeHandle & getNodeHandle() const
int getCvType(const std::string &encoding)
static int numChannels(const std::string &encoding)
bool getParam(const std::string &key, std::string &s) const
#define ROS_ERROR(...)
image_transport::Subscriber sub_raw_
void depthCb(const sensor_msgs::ImageConstPtr &raw_msg)


depth_image_proc
Author(s): Patrick Mihelich
autogenerated on Thu Nov 7 2019 03:44:56