edge_detection_nodelet.cpp
Go to the documentation of this file.
1 // -*- coding:utf-8-unix; mode: c++; indent-tabs-mode: nil; c-basic-offset: 2; -*-
2 /*********************************************************************
3 * Software License Agreement (BSD License)
4 *
5 * Copyright (c) 2014, Kei Okada.
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 Kei Okada 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/Itseez/opencv/blob/2.4/samples/cpp/tutorial_code/ImgTrans/
53 #include <ros/ros.h>
54 #include "opencv_apps/nodelet.h"
57 #include <cv_bridge/cv_bridge.h>
58 
59 #include <opencv2/highgui/highgui.hpp>
60 #include <opencv2/imgproc/imgproc.hpp>
61 
62 #include <dynamic_reconfigure/server.h>
63 #include "opencv_apps/EdgeDetectionConfig.h"
64 
65 namespace opencv_apps
66 {
68 {
73 
75 
76  typedef opencv_apps::EdgeDetectionConfig Config;
77  typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
78  Config config_;
80 
84 
93 
94  std::string window_name_;
95  static bool need_config_update_;
96 
97  void reconfigureCallback(Config& new_config, uint32_t level)
98  {
99  config_ = new_config;
100  canny_threshold1_ = config_.canny_threshold1;
101  canny_threshold2_ = config_.canny_threshold2;
102  apertureSize_ = 2 * ((config_.apertureSize / 2)) + 1;
103  L2gradient_ = config_.L2gradient;
104 
105  apply_blur_pre_ = config_.apply_blur_pre;
106  apply_blur_post_ = config_.apply_blur_post;
107  postBlurSize_ = 2 * ((config_.postBlurSize) / 2) + 1;
108  postBlurSigma_ = config_.postBlurSigma;
109  }
110 
111  const std::string& frameWithDefault(const std::string& frame, const std::string& image_frame)
112  {
113  if (frame.empty())
114  return image_frame;
115  return frame;
116  }
117 
118  void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info)
119  {
120  doWork(msg, cam_info->header.frame_id);
121  }
122 
123  void imageCallback(const sensor_msgs::ImageConstPtr& msg)
124  {
125  doWork(msg, msg->header.frame_id);
126  }
127 
128  static void trackbarCallback(int /*unused*/, void* /*unused*/)
129  {
130  need_config_update_ = true;
131  }
132 
133  void doWork(const sensor_msgs::ImageConstPtr& msg, const std::string& input_frame_from_msg)
134  {
135  // Work on the image.
136  try
137  {
138  // Convert the image into something opencv can handle.
139  cv::Mat frame = cv_bridge::toCvShare(msg, sensor_msgs::image_encodings::BGR8)->image;
140 
141  // Do the work
142  cv::Mat src_gray;
143  cv::GaussianBlur(frame, frame, cv::Size(3, 3), 0, 0, cv::BORDER_DEFAULT);
144 
146  if (frame.channels() > 1)
147  {
148  cv::cvtColor(frame, src_gray, cv::COLOR_RGB2GRAY);
149  }
150  else
151  {
152  src_gray = frame;
153  }
154 
156  if (debug_view_)
157  {
158  cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE);
159  }
160 
161  std::string new_window_name;
162  cv::Mat grad;
163  switch (config_.edge_type)
164  {
165  case opencv_apps::EdgeDetection_Sobel:
166  {
168  cv::Mat grad_x, grad_y;
169  cv::Mat abs_grad_x, abs_grad_y;
170 
171  int scale = 1;
172  int delta = 0;
173  int ddepth = CV_16S;
174 
176  // Scharr( src_gray, grad_x, ddepth, 1, 0, scale, delta, BORDER_DEFAULT );
177  cv::Sobel(src_gray, grad_x, ddepth, 1, 0, 3, scale, delta, cv::BORDER_DEFAULT);
178  cv::convertScaleAbs(grad_x, abs_grad_x);
179 
181  // Scharr( src_gray, grad_y, ddepth, 0, 1, scale, delta, BORDER_DEFAULT );
182  cv::Sobel(src_gray, grad_y, ddepth, 0, 1, 3, scale, delta, cv::BORDER_DEFAULT);
183  cv::convertScaleAbs(grad_y, abs_grad_y);
184 
186  cv::addWeighted(abs_grad_x, 0.5, abs_grad_y, 0.5, 0, grad);
187 
188  new_window_name = "Sobel Edge Detection Demo";
189  break;
190  }
191  case opencv_apps::EdgeDetection_Laplace:
192  {
193  cv::Mat dst;
194  int kernel_size = 3;
195  int scale = 1;
196  int delta = 0;
197  int ddepth = CV_16S;
199 
200  cv::Laplacian(src_gray, dst, ddepth, kernel_size, scale, delta, cv::BORDER_DEFAULT);
201  convertScaleAbs(dst, grad);
202 
203  new_window_name = "Laplace Edge Detection Demo";
204  break;
205  }
206  case opencv_apps::EdgeDetection_Canny:
207  {
208  int edge_thresh = 1;
209  int kernel_size = 3;
210  int const max_canny_threshold1 = 500;
211  int const max_canny_threshold2 = 500;
212  cv::Mat detected_edges;
213 
215  if (apply_blur_pre_)
216  {
217  cv::blur(src_gray, src_gray, cv::Size(apertureSize_, apertureSize_));
218  }
219 
221  cv::Canny(src_gray, grad, canny_threshold1_, canny_threshold2_, kernel_size, L2gradient_);
222  if (apply_blur_post_)
223  {
224  cv::GaussianBlur(grad, grad, cv::Size(postBlurSize_, postBlurSize_), postBlurSigma_,
225  postBlurSigma_); // 0.3*(ksize/2 - 1) + 0.8
226  }
227 
228  new_window_name = "Canny Edge Detection Demo";
229 
231  if (debug_view_)
232  {
233  if (need_config_update_)
234  {
235  config_.canny_threshold1 = canny_threshold1_;
236  config_.canny_threshold2 = canny_threshold2_;
237  reconfigure_server_->updateConfig(config_);
238  need_config_update_ = false;
239  }
240  if (window_name_ == new_window_name)
241  {
242  cv::createTrackbar("Min CannyThreshold1:", window_name_, &canny_threshold1_, max_canny_threshold1,
244  cv::createTrackbar("Min CannyThreshold2:", window_name_, &canny_threshold2_, max_canny_threshold2,
246  }
247  }
248  break;
249  }
250  }
251 
252  if (debug_view_)
253  {
254  if (window_name_ != new_window_name)
255  {
256  cv::destroyWindow(window_name_);
257  window_name_ = new_window_name;
258  }
259  cv::imshow(window_name_, grad);
260  int c = cv::waitKey(1);
261  }
262 
263  // Publish the image.
264  sensor_msgs::Image::Ptr out_img =
266  img_pub_.publish(out_img);
267  }
268  catch (cv::Exception& e)
269  {
270  NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
271  }
272 
273  prev_stamp_ = msg->header.stamp;
274  }
275 
276  void subscribe() // NOLINT(modernize-use-override)
277  {
278  NODELET_DEBUG("Subscribing to image topic.");
279  if (config_.use_camera_info)
280  cam_sub_ = it_->subscribeCamera("image", queue_size_, &EdgeDetectionNodelet::imageCallbackWithInfo, this);
281  else
282  img_sub_ = it_->subscribe("image", queue_size_, &EdgeDetectionNodelet::imageCallback, this);
283  }
284 
285  void unsubscribe() // NOLINT(modernize-use-override)
286  {
287  NODELET_DEBUG("Unsubscribing from image topic.");
288  img_sub_.shutdown();
289  cam_sub_.shutdown();
290  }
291 
292 public:
293  virtual void onInit() // NOLINT(modernize-use-override)
294  {
295  Nodelet::onInit();
297 
298  pnh_->param("queue_size", queue_size_, 3);
299  pnh_->param("debug_view", debug_view_, false);
300 
301  if (debug_view_)
302  {
303  always_subscribe_ = true;
304  }
305  prev_stamp_ = ros::Time(0, 0);
306 
307  window_name_ = "Edge Detection Demo";
308  canny_threshold1_ = 100; // only for canny
309  canny_threshold2_ = 200; // only for canny
310 
311  reconfigure_server_ = boost::make_shared<dynamic_reconfigure::Server<Config> >(*pnh_);
312  dynamic_reconfigure::Server<Config>::CallbackType f =
313  boost::bind(&EdgeDetectionNodelet::reconfigureCallback, this, _1, _2);
314  reconfigure_server_->setCallback(f);
315 
316  img_pub_ = advertiseImage(*pnh_, "image", 1);
317  // msg_pub_ = local_nh_.advertise<opencv_apps::LineArrayStamped>("lines", 1, msg_connect_cb, msg_disconnect_cb);
318 
320  }
321 };
323 } // namespace opencv_apps
324 
325 namespace edge_detection
326 {
328 {
329 public:
330  virtual void onInit() // NOLINT(modernize-use-override)
331  {
332  ROS_WARN("DeprecationWarning: Nodelet edge_detection/edge_detection is deprecated, "
333  "and renamed to opencv_apps/edge_detection.");
335  }
336 };
337 } // namespace edge_detection
338 
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
opencv_apps::EdgeDetectionConfig Config
void unsubscribe()
This method is called when publisher is unsubscribed by other nodes. Shut down subscribers in this me...
#define NODELET_ERROR(...)
void reconfigureCallback(Config &new_config, uint32_t level)
f
void subscribe()
This method is called when publisher is subscribed by other nodes. Set up subscribers in this method...
void imageCallback(const sensor_msgs::ImageConstPtr &msg)
Nodelet to automatically subscribe/unsubscribe topics according to subscription of advertised topics...
Definition: nodelet.h:70
boost::shared_ptr< image_transport::ImageTransport > it_
Demo code to calculate moments.
Definition: nodelet.h:48
dynamic_reconfigure::Server< Config > ReconfigureServer
image_transport::CameraSubscriber cam_sub_
void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr &msg, const sensor_msgs::CameraInfoConstPtr &cam_info)
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(...)
PLUGINLIB_EXPORT_CLASS(opencv_apps::EdgeDetectionNodelet, nodelet::Nodelet)
bool always_subscribe_
A flag to disable watching mechanism and always subscribe input topics. It can be specified via ~alwa...
Definition: nodelet.h:273
boost::shared_ptr< ReconfigureServer > reconfigure_server_
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 doWork(const sensor_msgs::ImageConstPtr &msg, const std::string &input_frame_from_msg)
virtual void onInit()
Initialize nodehandles nh_ and pnh_. Subclass should call this method in its onInit method...
const std::string & frameWithDefault(const std::string &frame, const std::string &image_frame)
image_transport::Subscriber img_sub_
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
#define NODELET_DEBUG(...)
sensor_msgs::ImagePtr toImageMsg() const


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