adding_images_nodelet.cpp
Go to the documentation of this file.
1 // -*- mode: c++ -*-
2 /*********************************************************************
3  * Software License Agreement (BSD License)
4  *
5  * Copyright (c) JSK, 2016 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/tree/2.4/samples/cpp/tutorial_code/ImgProc/AddingImages.cpp
41 #include <opencv2/opencv.hpp>
42 #include <cv_bridge/cv_bridge.h>
44 #include <sensor_msgs/CameraInfo.h>
45 #include <dynamic_reconfigure/server.h>
46 
49 
54 
55 #include "opencv_apps/AddingImagesConfig.h"
56 #include "opencv_apps/nodelet.h"
57 
58 namespace opencv_apps
59 {
61 {
62 private:
64 
67  typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image,
68  sensor_msgs::CameraInfo>
72 
74  // Dynamic Reconfigure
76  typedef opencv_apps::AddingImagesConfig Config;
77  typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
78  Config config_;
80 
84 
85  std::string window_name_;
86 
94  boost::mutex mutex_;
95 
97  double alpha_;
98  double beta_;
99  double gamma_;
100 
101  void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr& msg1, const sensor_msgs::ImageConstPtr& msg2,
102  const sensor_msgs::CameraInfoConstPtr& cam_info)
103  {
104  doWork(msg1, msg2, cam_info->header.frame_id);
105  }
106 
107  void imageCallback(const sensor_msgs::ImageConstPtr& msg1, const sensor_msgs::ImageConstPtr& msg2)
108  {
109  doWork(msg1, msg2, msg1->header.frame_id);
110  }
111 
112  void subscribe() // NOLINT(modernize-use-override)
113  {
114  NODELET_DEBUG("Subscribing to image topic.");
115  sub_image1_.subscribe(*it_, "image1", 3);
116  sub_image2_.subscribe(*it_, "image2", 3);
117  sub_camera_info_.subscribe(*nh_, "camera_info", 3);
118  if (config_.use_camera_info)
119  {
120  if (approximate_sync_)
121  {
122  async_with_info_ =
123  boost::make_shared<message_filters::Synchronizer<ApproxSyncPolicyWithCameraInfo> >(queue_size_);
124  async_with_info_->connectInput(sub_image1_, sub_image2_, sub_camera_info_);
125  async_with_info_->registerCallback(boost::bind(&AddingImagesNodelet::imageCallbackWithInfo, this, _1, _2, _3));
126  }
127  else
128  {
129  sync_with_info_ = boost::make_shared<message_filters::Synchronizer<SyncPolicyWithCameraInfo> >(queue_size_);
130  sync_with_info_->connectInput(sub_image1_, sub_image2_, sub_camera_info_);
131  sync_with_info_->registerCallback(boost::bind(&AddingImagesNodelet::imageCallbackWithInfo, this, _1, _2, _3));
132  }
133  }
134  else
135  {
136  if (approximate_sync_)
137  {
138  async_ = boost::make_shared<message_filters::Synchronizer<ApproxSyncPolicy> >(queue_size_);
139  async_->connectInput(sub_image1_, sub_image2_);
140  async_->registerCallback(boost::bind(&AddingImagesNodelet::imageCallback, this, _1, _2));
141  }
142  else
143  {
144  sync_ = boost::make_shared<message_filters::Synchronizer<SyncPolicy> >(queue_size_);
145  sync_->connectInput(sub_image1_, sub_image2_);
146  sync_->registerCallback(boost::bind(&AddingImagesNodelet::imageCallback, this, _1, _2));
147  }
148  }
149  }
150 
151  void unsubscribe() // NOLINT(modernize-use-override)
152  {
153  NODELET_DEBUG("Unsubscribing from image topic.");
154  sub_image1_.unsubscribe();
155  sub_image2_.unsubscribe();
156  sub_camera_info_.unsubscribe();
157  }
158 
159  void reconfigureCallback(Config& config, uint32_t level)
160  {
161  boost::mutex::scoped_lock lock(mutex_);
162  config_ = config;
163  alpha_ = config.alpha;
164  if (config.auto_beta)
165  {
166  beta_ = 1.0 - alpha_;
167  config.beta = beta_;
168  }
169  else
170  {
171  beta_ = config.beta;
172  }
173  gamma_ = config.gamma;
174  }
175 
176  void doWork(const sensor_msgs::Image::ConstPtr& image_msg1, const sensor_msgs::Image::ConstPtr& image_msg2,
177  const std::string& input_frame_from_msg)
178  {
179  boost::mutex::scoped_lock lock(mutex_);
180  // Work on the image.
181  try
182  {
183  cv::Mat image1 = cv_bridge::toCvCopy(image_msg1, image_msg1->encoding)->image;
184  cv::Mat image2 = cv_bridge::toCvCopy(image_msg2, image_msg1->encoding)->image;
185  if (cv_bridge::getCvType(image_msg1->encoding) != cv_bridge::getCvType(image_msg2->encoding))
186  {
187  NODELET_ERROR("Encoding of input images must be same type: %s, %s", image_msg1->encoding.c_str(),
188  image_msg2->encoding.c_str());
189  return;
190  }
191 
192  cv::Mat result_image;
193  if (image1.rows != image2.rows || image1.cols != image2.cols)
194  {
195  int new_rows = std::max(image1.rows, image2.rows);
196  int new_cols = std::max(image1.cols, image2.cols);
197  // if ( new_rows != image1.rows || new_cols != image1.cols ) {
198  cv::Mat image1 = cv::Mat(new_rows, new_cols, image1.type());
199  image1.copyTo(image1(cv::Rect(0, 0, image1.cols, image1.rows)));
200  image1 = image1.clone(); // need clone becuase toCvShare??
201 
202  // if ( new_rows != image2.rows || new_cols != image2.cols ) {
203  cv::Mat image2 = cv::Mat(new_rows, new_cols, image2.type());
204  image2.copyTo(image2(cv::Rect(0, 0, image2.cols, image2.rows)));
205  image2 = image2.clone();
206  }
207  cv::addWeighted(image1, alpha_, image2, beta_, gamma_, result_image);
208  //-- Show what you got
209  sensor_msgs::ImagePtr image_msg3 =
210  cv_bridge::CvImage(image_msg1->header, image_msg1->encoding, result_image).toImageMsg();
211  if (debug_view_)
212  {
213  cv::namedWindow(window_name_, cv::WINDOW_AUTOSIZE);
214 #ifdef CV_BRIDGE_CVT_COLOR_FOR_DISPLAY_IS_NOT_SUPPORTED
215  cv::imshow(window_name_, result_image);
216 #else
217 #ifdef CV_BRIDGE_CVT_COLOR_FOR_DISPLAY_OPTION_IS_NOT_SUPPORTED
218  cv::imshow(window_name_,
219  cv_bridge::cvtColorForDisplay(cv_bridge::toCvShare(image_msg3, image_msg3->encoding))->image);
220 #else
222  if (sensor_msgs::image_encodings::bitDepth(image_msg1->encoding) == 32 ||
223  sensor_msgs::image_encodings::bitDepth(image_msg1->encoding) == 64)
224  {
225  // float or double image
226  options.do_dynamic_scaling = true;
227  }
228  cv::imshow(window_name_, cv_bridge::cvtColorForDisplay(cv_bridge::toCvShare(image_msg3), "", options)->image);
229 #endif
230 #endif
231  int c = cv::waitKey(1);
232  }
233  img_pub_.publish(image_msg3);
234  }
235  catch (cv::Exception& e)
236  {
237  NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
238  }
239 
240  prev_stamp_ = image_msg1->header.stamp;
241  }
242 
243 public:
244  virtual void onInit() // NOLINT(modernize-use-override)
245  {
246  Nodelet::onInit();
248 
249  pnh_->param("debug_view", debug_view_, false);
250  if (debug_view_)
251  {
252  always_subscribe_ = true;
253  }
254  prev_stamp_ = ros::Time(0, 0);
255 
256  window_name_ = "AddingImages Demo";
258  // Dynamic Reconfigure
260  reconfigure_server_ = boost::make_shared<dynamic_reconfigure::Server<Config> >(*pnh_);
261  dynamic_reconfigure::Server<Config>::CallbackType f =
262  boost::bind(&AddingImagesNodelet::reconfigureCallback, this, _1, _2);
263  reconfigure_server_->setCallback(f);
264 
265  pnh_->param("approximate_sync", approximate_sync_, true);
266  pnh_->param("queue_size", queue_size_, 100);
267  img_pub_ = advertiseImage(*pnh_, "image", 1);
269  }
270 };
271 } // namespace opencv_apps
272 
273 namespace adding_images
274 {
276 {
277 public:
278  virtual void onInit() // NOLINT(modernize-use-override)
279  {
280  ROS_WARN("DeprecationWarning: Nodelet adding_images/adding_images is deprecated, "
281  "and renamed to opencv_apps/adding_images.");
283  }
284 };
285 } // namespace adding_images
286 
CvImageConstPtr toCvShare(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
#define NODELET_ERROR(...)
f
boost::shared_ptr< ReconfigureServer > reconfigure_server_
Nodelet to automatically subscribe/unsubscribe topics according to subscription of advertised topics...
Definition: nodelet.h:70
message_filters::sync_policies::ExactTime< sensor_msgs::Image, sensor_msgs::Image > SyncPolicy
image_transport::Publisher img_pub_
Demo code to calculate moments.
Definition: nodelet.h:48
void reconfigureCallback(Config &config, uint32_t level)
message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::Image > ApproxSyncPolicy
boost::shared_ptr< message_filters::Synchronizer< ApproxSyncPolicyWithCameraInfo > > async_with_info_
boost::shared_ptr< ros::NodeHandle > pnh_
Shared pointer to private nodehandle.
Definition: nodelet.h:250
boost::shared_ptr< image_transport::ImageTransport > it_
#define ROS_WARN(...)
boost::shared_ptr< message_filters::Synchronizer< SyncPolicy > > sync_
message_filters::sync_policies::ExactTime< sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo > SyncPolicyWithCameraInfo
bool always_subscribe_
A flag to disable watching mechanism and always subscribe input topics. It can be specified via ~alwa...
Definition: nodelet.h:273
void imageCallback(const sensor_msgs::ImageConstPtr &msg1, const sensor_msgs::ImageConstPtr &msg2)
void imageCallbackWithInfo(const sensor_msgs::ImageConstPtr &msg1, const sensor_msgs::ImageConstPtr &msg2, const sensor_msgs::CameraInfoConstPtr &cam_info)
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
dynamic_reconfigure::Server< Config > ReconfigureServer
void subscribe()
This method is called when publisher is subscribed by other nodes. Set up subscribers in this method...
message_filters::sync_policies::ApproximateTime< sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo > ApproxSyncPolicyWithCameraInfo
boost::shared_ptr< ros::NodeHandle > nh_
Shared pointer to nodehandle.
Definition: nodelet.h:245
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
void unsubscribe()
This method is called when publisher is unsubscribed by other nodes. Shut down subscribers in this me...
void subscribe(ImageTransport &it, const std::string &base_topic, uint32_t queue_size, const TransportHints &transport_hints=TransportHints())
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
int getCvType(const std::string &encoding)
PLUGINLIB_EXPORT_CLASS(opencv_apps::AddingImagesNodelet, nodelet::Nodelet)
image_transport::SubscriberFilter sub_image1_
void subscribe(ros::NodeHandle &nh, const std::string &topic, uint32_t queue_size, const ros::TransportHints &transport_hints=ros::TransportHints(), ros::CallbackQueueInterface *callback_queue=0)
virtual void onInit()
Initialize nodehandles nh_ and pnh_. Subclass should call this method in its onInit method...
CvImageConstPtr cvtColorForDisplay(const CvImageConstPtr &source, const std::string &encoding=std::string(), const CvtColorForDisplayOptions options=CvtColorForDisplayOptions())
image_transport::SubscriberFilter sub_image2_
opencv_apps::AddingImagesConfig Config
static int bitDepth(const std::string &encoding)
void doWork(const sensor_msgs::Image::ConstPtr &image_msg1, const sensor_msgs::Image::ConstPtr &image_msg2, const std::string &input_frame_from_msg)
message_filters::Subscriber< sensor_msgs::CameraInfo > sub_camera_info_
boost::shared_ptr< message_filters::Synchronizer< ApproxSyncPolicy > > async_
#define NODELET_DEBUG(...)
sensor_msgs::ImagePtr toImageMsg() const
boost::shared_ptr< message_filters::Synchronizer< SyncPolicyWithCameraInfo > > sync_with_info_
virtual void onInit()
Initialize nodehandles nh_ and pnh_. Subclass should call this method in its onInit method...


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