threshold_nodelet.cpp
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) 2016, JSK Lab
6  * All rights reserved.
7  *
8  * Redistribution and use in source and binary forms, with or without
9  * modification, are permitted provided that the following conditions
10  * are met:
11  *
12  * * Redistributions of source code must retain the above copyright
13  * notice, this list of conditions and the following disclaimer.
14  * * Redistributions in binary form must reproduce the above
15  * copyright notice, this list of conditions and the following
16  * disclaimer in the documentation and/or other materials provided
17  * with the distribution.
18  * * Neither the name of the JSK Lab nor the names of its
19  * contributors may be used to endorse or promote products derived
20  * from this software without specific prior written permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 // https://github.com/opencv/opencv/blob/2.4/samples/cpp/tutorial_code/ImgProc/Threshold.cpp
43 #include <cv_bridge/cv_bridge.h>
45 
46 #include <opencv2/highgui/highgui.hpp>
47 #include "opencv2/imgproc/imgproc.hpp"
48 
49 #include "opencv_apps/nodelet.h"
50 #include "opencv_apps/ThresholdConfig.h"
51 
52 #include <dynamic_reconfigure/server.h>
53 
54 namespace opencv_apps
55 {
57 {
59  // Dynamic Reconfigure
61  typedef opencv_apps::ThresholdConfig Config;
62  typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
63  Config config_;
65 
68 
69  std::string window_name_;
70 
74 
76 
77  boost::mutex mutex_;
82 
83  void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info)
84  {
85  doWork(msg, cam_info->header.frame_id);
86  }
87 
88  void imageCallback(const sensor_msgs::ImageConstPtr& msg)
89  {
90  doWork(msg, msg->header.frame_id);
91  }
92 
93  void subscribe() // NOLINT(modernize-use-override)
94  {
95  NODELET_DEBUG("Subscribing to image topic.");
96  if (config_.use_camera_info)
97  cam_sub_ = it_->subscribeCamera("image", queue_size_, &ThresholdNodelet::imageCallbackWithInfo, this);
98  else
99  img_sub_ = it_->subscribe("image", queue_size_, &ThresholdNodelet::imageCallback, this);
100  }
101 
102  void unsubscribe() // NOLINT(modernize-use-override)
103  {
104  NODELET_DEBUG("Unsubscribing from image topic.");
105  img_sub_.shutdown();
106  cam_sub_.shutdown();
107  }
108 
109  void reconfigureCallback(Config& config, uint32_t level)
110  {
111  boost::mutex::scoped_lock lock(mutex_);
112  config_ = config;
113  threshold_value_ = config.threshold;
114  threshold_type_ = config.threshold_type;
115  max_binary_value_ = config.max_binary;
116  apply_otsu_ = config.apply_otsu;
117  }
118 
119  void doWork(const sensor_msgs::Image::ConstPtr& image_msg, const std::string& input_frame_from_msg)
120  {
121  try
122  {
123  cv::Mat src_image = cv_bridge::toCvShare(image_msg, sensor_msgs::image_encodings::BGR8)->image;
124  cv::Mat gray_image;
125  cv::cvtColor(src_image, gray_image, cv::COLOR_BGR2GRAY);
126  cv::Mat result_image;
127 
128  if (apply_otsu_)
129  {
130  threshold_type_ |= CV_THRESH_OTSU;
131  }
132  cv::threshold(gray_image, result_image, threshold_value_, max_binary_value_, threshold_type_);
133  //-- Show what you got
134  if (debug_view_)
135  {
136  cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE);
137  cv::imshow(window_name_, result_image);
138  int c = cv::waitKey(1);
139  }
140  img_pub_.publish(
141  cv_bridge::CvImage(image_msg->header, sensor_msgs::image_encodings::MONO8, result_image).toImageMsg());
142  }
143  catch (cv::Exception& e)
144  {
145  NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
146  }
147  }
148 
149 public:
150  virtual void onInit() // NOLINT(modernize-use-override)
151  {
152  Nodelet::onInit();
154 
155  pnh_->param("queue_size", queue_size_, 3);
156  pnh_->param("debug_view", debug_view_, false);
157  if (debug_view_)
158  {
159  always_subscribe_ = true;
160  }
162  // Dynamic Reconfigure
164  reconfigure_server_ = boost::make_shared<dynamic_reconfigure::Server<Config> >(*pnh_);
165  dynamic_reconfigure::Server<Config>::CallbackType f =
166  boost::bind(&ThresholdNodelet::reconfigureCallback, this, _1, _2);
167  reconfigure_server_->setCallback(f);
168 
169  img_pub_ = advertiseImage(*pnh_, "image", 1);
171  }
172 };
173 } // namespace opencv_apps
174 
175 namespace threshold
176 {
178 {
179 public:
180  virtual void onInit() // NOLINT(modernize-use-override)
181  {
182  ROS_WARN("DeprecationWarning: Nodelet threshold/threshold is deprecated, "
183  "and renamed to opencv_apps/threshold.");
185  }
186 };
187 } // namespace threshold
188 
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
#define NODELET_ERROR(...)
image_transport::CameraSubscriber cam_sub_
PLUGINLIB_EXPORT_CLASS(opencv_apps::ThresholdNodelet, nodelet::Nodelet)
f
void subscribe()
This method is called when publisher is subscribed by other nodes. Set up subscribers in this method...
image_transport::Publisher img_pub_
Nodelet to automatically subscribe/unsubscribe topics according to subscription of advertised topics...
Definition: nodelet.h:70
Demo code to calculate moments.
Definition: nodelet.h:48
virtual void onInit()
Initialize nodehandles nh_ and pnh_. Subclass should call this method in its onInit method...
boost::shared_ptr< ros::NodeHandle > pnh_
Shared pointer to private nodehandle.
Definition: nodelet.h:250
virtual void onInit()
Initialize nodehandles nh_ and pnh_. Subclass should call this method in its onInit method...
#define ROS_WARN(...)
dynamic_reconfigure::Server< Config > ReconfigureServer
bool always_subscribe_
A flag to disable watching mechanism and always subscribe input topics. It can be specified via ~alwa...
Definition: nodelet.h:273
image_transport::Subscriber img_sub_
virtual void onInitPostProcess()
Post processing of initialization of nodelet. You need to call this method in order to use always_sub...
Definition: nodelet.cpp:57
virtual void onInit()
Initialize nodehandles nh_ and pnh_. Subclass should call this method in its onInit method...
Definition: nodelet.cpp:40
void publish(const sensor_msgs::Image &message) const
boost::shared_ptr< ros::NodeHandle > nh_
Shared pointer to nodehandle.
Definition: nodelet.h:245
void imageCallback(const sensor_msgs::ImageConstPtr &msg)
void doWork(const sensor_msgs::Image::ConstPtr &image_msg, const std::string &input_frame_from_msg)
void reconfigureCallback(Config &config, uint32_t level)
image_transport::Publisher advertiseImage(ros::NodeHandle &nh, const std::string &topic, int queue_size)
Advertise an image topic and watch the publisher. Publishers which are created by this method...
Definition: nodelet.h:180
boost::shared_ptr< image_transport::ImageTransport > it_
void unsubscribe()
This method is called when publisher is unsubscribed by other nodes. Shut down subscribers in this me...
opencv_apps::ThresholdConfig Config
#define NODELET_DEBUG(...)
boost::shared_ptr< ReconfigureServer > reconfigure_server_
sensor_msgs::ImagePtr toImageMsg() const
void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr &msg, const sensor_msgs::CameraInfoConstPtr &cam_info)


opencv_apps
Author(s): Kei Okada
autogenerated on Wed Apr 24 2019 03:00:17