pyramids_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) 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/master/samples/cpp/tutorial_code/ImgProc/Pyramids/Pyramids.cpp
37 // https://github.com/opencv/opencv/blob/2.4.13.4/samples/cpp/tutorial_code/ImgProc/Pyramids.cpp
44 #include "opencv_apps/nodelet.h"
47 #include <cv_bridge/cv_bridge.h>
48 
49 #include <opencv2/highgui/highgui.hpp>
50 #include "opencv2/imgproc/imgproc.hpp"
51 
52 #include "opencv_apps/PyramidsConfig.h"
53 #include <dynamic_reconfigure/server.h>
54 
55 namespace opencv_apps
56 {
58 {
63 
65 
66  typedef opencv_apps::PyramidsConfig Config;
67  typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
68  Config config_;
70 
73 
75 
76  std::string window_name_;
77 
78  void reconfigureCallback(Config& new_config, uint32_t level)
79  {
80  config_ = new_config;
81 
82  num_of_pyramids_ = config_.num_of_pyramids;
83  }
84 
85  const std::string& frameWithDefault(const std::string& frame, const std::string& image_frame)
86  {
87  if (frame.empty())
88  return image_frame;
89  return frame;
90  }
91 
92  void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg, const sensor_msgs::CameraInfoConstPtr& cam_info)
93  {
94  doWork(msg, cam_info->header.frame_id);
95  }
96 
97  void imageCallback(const sensor_msgs::ImageConstPtr& msg)
98  {
99  doWork(msg, msg->header.frame_id);
100  }
101 
102  void doWork(const sensor_msgs::ImageConstPtr& image_msg, const std::string& input_frame_from_msg)
103  {
104  // Work on the image.
105  try
106  {
107  // Convert the image into something opencv can handle.
108  cv::Mat src_image = cv_bridge::toCvShare(image_msg, image_msg->encoding)->image;
109 
110  // Do the work
111  int num = num_of_pyramids_;
112  switch (config_.pyramids_type)
113  {
114  case opencv_apps::Pyramids_Up:
115  {
116  while (num)
117  {
118  num--;
119  cv::pyrUp(src_image, src_image, cv::Size(src_image.cols * 2, src_image.rows * 2));
120  }
121  break;
122  }
123  case opencv_apps::Pyramids_Down:
124  {
125  while (num)
126  {
127  num--;
128  cv::pyrDown(src_image, src_image, cv::Size(src_image.cols / 2, src_image.rows / 2));
129  }
130  break;
131  }
132  }
133 
134  //-- Show what you got
135  if (debug_view_)
136  {
137  cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE);
138  cv::imshow(window_name_, src_image);
139  int c = cv::waitKey(1);
140  }
141 
142  // Publish the image.
143  img_pub_.publish(cv_bridge::CvImage(image_msg->header, image_msg->encoding, src_image).toImageMsg());
144  }
145  catch (cv::Exception& e)
146  {
147  NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
148  }
149  }
150 
151  void subscribe() // NOLINT(modernize-use-override)
152  {
153  NODELET_DEBUG("Subscribing to image topic.");
154  if (config_.use_camera_info)
155  cam_sub_ = it_->subscribeCamera("image", queue_size_, &PyramidsNodelet::imageCallbackWithInfo, this);
156  else
157  img_sub_ = it_->subscribe("image", queue_size_, &PyramidsNodelet::imageCallback, this);
158  }
159 
160  void unsubscribe() // NOLINT(modernize-use-override)
161  {
162  NODELET_DEBUG("Unsubscribing from image topic.");
163  img_sub_.shutdown();
164  cam_sub_.shutdown();
165  }
166 
167 public:
168  virtual void onInit() // NOLINT(modernize-use-override)
169  {
170  Nodelet::onInit();
172 
173  pnh_->param("queue_size", queue_size_, 3);
174  pnh_->param("debug_view", debug_view_, false);
175 
176  if (debug_view_)
177  {
178  always_subscribe_ = true;
179  }
180 
181  window_name_ = "Image Pyramids Demo";
182 
183  reconfigure_server_ = boost::make_shared<dynamic_reconfigure::Server<Config> >(*pnh_);
184  dynamic_reconfigure::Server<Config>::CallbackType f =
185  boost::bind(&PyramidsNodelet::reconfigureCallback, this, _1, _2);
186  reconfigure_server_->setCallback(f);
187 
188  img_pub_ = advertiseImage(*pnh_, "image", 1);
189 
191  }
192 };
193 } // namespace opencv_apps
194 
195 namespace pyramids
196 {
198 {
199 public:
200  virtual void onInit() // NOLINT(modernize-use-override)
201  {
202  ROS_WARN("DeprecationWarning: Nodelet pyramids/pyramids is deprecated, "
203  "and renamed to opencv_apps/pyramids.");
205  }
206 };
207 } // namespace pyramids
208 
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
image_transport::CameraSubscriber cam_sub_
#define NODELET_ERROR(...)
dynamic_reconfigure::Server< Config > ReconfigureServer
f
opencv_apps::PyramidsConfig Config
void doWork(const sensor_msgs::ImageConstPtr &image_msg, const std::string &input_frame_from_msg)
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
const std::string & frameWithDefault(const std::string &frame, const std::string &image_frame)
boost::shared_ptr< ros::NodeHandle > pnh_
Shared pointer to private nodehandle.
Definition: nodelet.h:250
boost::shared_ptr< image_transport::ImageTransport > it_
virtual void onInit()
Initialize nodehandles nh_ and pnh_. Subclass should call this method in its onInit method...
#define ROS_WARN(...)
void subscribe()
This method is called when publisher is subscribed by other nodes. Set up subscribers in this method...
boost::shared_ptr< ReconfigureServer > reconfigure_server_
void imageCallback(const sensor_msgs::ImageConstPtr &msg)
void reconfigureCallback(Config &new_config, uint32_t level)
void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr &msg, const sensor_msgs::CameraInfoConstPtr &cam_info)
bool always_subscribe_
A flag to disable watching mechanism and always subscribe input topics. It can be specified via ~alwa...
Definition: nodelet.h:273
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 unsubscribe()
This method is called when publisher is unsubscribed by other nodes. Shut down subscribers in this me...
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
virtual void onInit()
Initialize nodehandles nh_ and pnh_. Subclass should call this method in its onInit method...
PLUGINLIB_EXPORT_CLASS(opencv_apps::PyramidsNodelet, nodelet::Nodelet)
image_transport::Publisher img_pub_
image_transport::Subscriber img_sub_
#define NODELET_DEBUG(...)
sensor_msgs::ImagePtr toImageMsg() const


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