resize.cpp
Go to the documentation of this file.
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2017, Kentaro Wada.
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 Kentaro Wada 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 <cv_bridge/cv_bridge.h>
35 #include <dynamic_reconfigure/server.h>
36 #include <nodelet/nodelet.h>
38 #include <ros/ros.h>
39 #include <sensor_msgs/CameraInfo.h>
40 #include <sensor_msgs/Image.h>
41 
42 #include "image_proc/ResizeConfig.h"
43 
44 namespace image_proc
45 {
46 
48 {
49 protected:
50  // ROS communication
55 
56  // Dynamic reconfigure
57  boost::recursive_mutex config_mutex_;
58  typedef image_proc::ResizeConfig Config;
59  typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
61  Config config_;
62 
63  virtual void onInit();
64  virtual void subscribe();
65  virtual void unsubscribe();
66 
67  void imageCb(const sensor_msgs::ImageConstPtr& image_msg);
68  void infoCb(const sensor_msgs::CameraInfoConstPtr& info_msg);
69 
70  void configCb(Config &config, uint32_t level);
71 };
72 
74 {
76 
77  // Set up dynamic reconfigure
79  ReconfigureServer::CallbackType f = boost::bind(&ResizeNodelet::configCb, this, _1, _2);
80  reconfigure_server_->setCallback(f);
81 
82  pub_info_ = advertise<sensor_msgs::CameraInfo>(*pnh_, "camera_info", 1);
83  pub_image_ = advertise<sensor_msgs::Image>(*pnh_, "image", 1);
84 
86 }
87 
88 void ResizeNodelet::configCb(Config &config, uint32_t level)
89 {
90  config_ = config;
91 }
92 
94 {
95  sub_info_ = nh_->subscribe("camera_info", 1, &ResizeNodelet::infoCb, this);
96  sub_image_ = nh_->subscribe("image", 1, &ResizeNodelet::imageCb, this);
97 }
98 
100 {
103 }
104 
105 void ResizeNodelet::infoCb(const sensor_msgs::CameraInfoConstPtr& info_msg)
106 {
107  Config config;
108  {
109  boost::lock_guard<boost::recursive_mutex> lock(config_mutex_);
110  config = config_;
111  }
112 
113  sensor_msgs::CameraInfo dst_info_msg = *info_msg;
114 
115  double scale_y;
116  double scale_x;
117  if (config.use_scale)
118  {
119  scale_y = config.scale_height;
120  scale_x = config.scale_width;
121  dst_info_msg.height = static_cast<int>(info_msg->height * config.scale_height);
122  dst_info_msg.width = static_cast<int>(info_msg->width * config.scale_width);
123  }
124  else
125  {
126  scale_y = static_cast<double>(config.height) / info_msg->height;
127  scale_x = static_cast<double>(config.width) / info_msg->width;
128  dst_info_msg.height = config.height;
129  dst_info_msg.width = config.width;
130  }
131 
132  dst_info_msg.K[0] = dst_info_msg.K[0] * scale_x; // fx
133  dst_info_msg.K[2] = dst_info_msg.K[2] * scale_x; // cx
134  dst_info_msg.K[4] = dst_info_msg.K[4] * scale_y; // fy
135  dst_info_msg.K[5] = dst_info_msg.K[5] * scale_y; // cy
136 
137  dst_info_msg.P[0] = dst_info_msg.P[0] * scale_x; // fx
138  dst_info_msg.P[2] = dst_info_msg.P[2] * scale_x; // cx
139  dst_info_msg.P[3] = dst_info_msg.P[3] * scale_x; // T
140  dst_info_msg.P[5] = dst_info_msg.P[5] * scale_y; // fy
141  dst_info_msg.P[6] = dst_info_msg.P[6] * scale_y; // cy
142 
143  pub_info_.publish(dst_info_msg);
144 }
145 
146 void ResizeNodelet::imageCb(const sensor_msgs::ImageConstPtr& image_msg)
147 {
148  Config config;
149  {
150  boost::lock_guard<boost::recursive_mutex> lock(config_mutex_);
151  config = config_;
152  }
153 
154  cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(image_msg);
155 
156  if (config.use_scale)
157  {
158  cv::resize(cv_ptr->image, cv_ptr->image, cv::Size(0, 0),
159  config.scale_width, config.scale_height, config.interpolation);
160  }
161  else
162  {
163  int height = config.height == -1 ? image_msg->height : config.height;
164  int width = config.width == -1 ? image_msg->width : config.width;
165  cv::resize(cv_ptr->image, cv_ptr->image, cv::Size(width, height), 0, 0, config.interpolation);
166  }
167 
168  pub_image_.publish(cv_ptr->toImageMsg());
169 }
170 
171 } // namespace image_proc
172 
void imageCb(const sensor_msgs::ImageConstPtr &image_msg)
Definition: resize.cpp:146
void publish(const boost::shared_ptr< M > &message) const
f
boost::shared_ptr< ReconfigureServer > reconfigure_server_
Definition: resize.cpp:60
virtual void subscribe()
Definition: resize.cpp:93
ros::Publisher pub_info_
Definition: resize.cpp:52
void infoCb(const sensor_msgs::CameraInfoConstPtr &info_msg)
Definition: resize.cpp:105
virtual void onInit()
Definition: resize.cpp:73
void configCb(Config &config, uint32_t level)
Definition: resize.cpp:88
dynamic_reconfigure::Server< Config > ReconfigureServer
Definition: resize.cpp:59
ros::Publisher pub_image_
Definition: resize.cpp:51
boost::shared_ptr< ros::NodeHandle > pnh_
PLUGINLIB_EXPORT_CLASS(image_proc::CropNonZeroNodelet, nodelet::Nodelet)
ros::Subscriber sub_info_
Definition: resize.cpp:53
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
image_proc::ResizeConfig Config
Definition: resize.cpp:58
boost::recursive_mutex config_mutex_
Definition: resize.cpp:57
virtual void unsubscribe()
Definition: resize.cpp:99
boost::shared_ptr< ros::NodeHandle > nh_
ros::Subscriber sub_image_
Definition: resize.cpp:54


image_proc
Author(s): Patrick Mihelich, Kurt Konolige, Jeremy Leibs
autogenerated on Fri May 3 2019 02:59:51