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 <mutex>
35 #include <cv_bridge/cv_bridge.h>
36 #include <dynamic_reconfigure/server.h>
37 #include <nodelet/nodelet.h>
38 #include <ros/ros.h>
39 #include <sensor_msgs/CameraInfo.h>
40 #include <sensor_msgs/Image.h>
42 
43 #include "image_proc/ResizeConfig.h"
44 
45 namespace image_proc
46 {
47 class ResizeNodelet : public nodelet::Nodelet
48 {
49 protected:
50  // ROS communication
51  std::shared_ptr<ros::NodeHandle> nh_;
52  std::shared_ptr<ros::NodeHandle> pnh_;
57 
58  std::shared_ptr<image_transport::ImageTransport> it_, private_it_;
59  std::mutex connect_mutex_;
60 
61  // Dynamic reconfigure
62  std::mutex config_mutex_;
63  typedef image_proc::ResizeConfig Config;
64  typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
65  std::shared_ptr<ReconfigureServer> reconfigure_server_;
68 
69  virtual void onInit();
70  void connectCb();
71 
72  void imageCb(const sensor_msgs::ImageConstPtr& image_msg);
73  void infoCb(const sensor_msgs::CameraInfoConstPtr& info_msg);
74 
75  void configCb(Config& config, uint32_t level);
76 };
77 
79 {
80  nh_.reset(new ros::NodeHandle(getNodeHandle()));
84 
85  // Set up dynamic reconfigure
87  ReconfigureServer::CallbackType f =
88  std::bind(&ResizeNodelet::configCb, this, std::placeholders::_1, std::placeholders::_2);
89  reconfigure_server_->setCallback(f);
90 
91  // Monitor whether anyone is subscribed to the output
92  auto connect_cb = std::bind(&ResizeNodelet::connectCb, this);
93  // Make sure we don't enter connectCb() between advertising and assigning to
94  // pub_XXX
95  std::lock_guard<std::mutex> lock(connect_mutex_);
96  pub_image_ = private_it_->advertise("image", 1, connect_cb, connect_cb);
97  pub_info_ = pnh_->advertise<sensor_msgs::CameraInfo>("camera_info", 1, connect_cb, connect_cb);
98 }
99 
100 // Handles (un)subscribing when clients (un)subscribe
102 {
103  std::lock_guard<std::mutex> lock(connect_mutex_);
104  if (pub_image_.getNumSubscribers() == 0)
105  {
107  }
108  else if (!sub_image_)
109  {
110  sub_image_ = it_->subscribe("image", 1, &ResizeNodelet::imageCb, this);
111  }
112  if (pub_info_.getNumSubscribers() == 0)
113  {
115  }
116  else if (!sub_info_)
117  {
118  sub_info_ = nh_->subscribe<sensor_msgs::CameraInfo>("camera_info", 1, &ResizeNodelet::infoCb, this);
119  }
120 }
121 
122 void ResizeNodelet::configCb(Config& config, uint32_t level)
123 {
124  std::lock_guard<std::mutex> lock(config_mutex_);
125  config_ = config;
126 }
127 
128 void ResizeNodelet::infoCb(const sensor_msgs::CameraInfoConstPtr& info_msg)
129 {
130  Config config;
131  {
132  std::lock_guard<std::mutex> lock(config_mutex_);
133  config = config_;
134  }
135 
136  sensor_msgs::CameraInfoPtr dst_info_msg(new sensor_msgs::CameraInfo(*info_msg));
137 
138  double scale_y;
139  double scale_x;
140  if (config.use_scale)
141  {
142  scale_y = config.scale_height;
143  scale_x = config.scale_width;
144  dst_info_msg->height = static_cast<int>(info_msg->height * config.scale_height);
145  dst_info_msg->width = static_cast<int>(info_msg->width * config.scale_width);
146  }
147  else
148  {
149  scale_y = static_cast<double>(config.height) / info_msg->height;
150  scale_x = static_cast<double>(config.width) / info_msg->width;
151  dst_info_msg->height = config.height;
152  dst_info_msg->width = config.width;
153  }
154 
155  dst_info_msg->K[0] = dst_info_msg->K[0] * scale_x; // fx
156  dst_info_msg->K[2] = dst_info_msg->K[2] * scale_x; // cx
157  dst_info_msg->K[4] = dst_info_msg->K[4] * scale_y; // fy
158  dst_info_msg->K[5] = dst_info_msg->K[5] * scale_y; // cy
159 
160  dst_info_msg->P[0] = dst_info_msg->P[0] * scale_x; // fx
161  dst_info_msg->P[2] = dst_info_msg->P[2] * scale_x; // cx
162  dst_info_msg->P[3] = dst_info_msg->P[3] * scale_x; // T
163  dst_info_msg->P[5] = dst_info_msg->P[5] * scale_y; // fy
164  dst_info_msg->P[6] = dst_info_msg->P[6] * scale_y; // cy
165 
166  dst_info_msg->roi.x_offset = static_cast<int>(dst_info_msg->roi.x_offset * scale_x);
167  dst_info_msg->roi.y_offset = static_cast<int>(dst_info_msg->roi.y_offset * scale_y);
168  dst_info_msg->roi.width = static_cast<int>(dst_info_msg->roi.width * scale_x);
169  dst_info_msg->roi.height = static_cast<int>(dst_info_msg->roi.height * scale_y);
170 
171  pub_info_.publish(dst_info_msg);
172 }
173 
174 void ResizeNodelet::imageCb(const sensor_msgs::ImageConstPtr& image_msg)
175 {
176  Config config;
177  {
178  std::lock_guard<std::mutex> lock(config_mutex_);
179  config = config_;
180  }
181 
183  try
184  {
185  cv_ptr = cv_bridge::toCvShare(image_msg);
186  }
187  catch (cv_bridge::Exception& e)
188  {
189  ROS_ERROR("cv_bridge exception: %s", e.what());
190  return;
191  }
192 
193  if (config.use_scale)
194  {
195  cv::resize(cv_ptr->image, scaled_cv_.image, cv::Size(0, 0), config.scale_width, config.scale_height,
196  config.interpolation);
197  }
198  else
199  {
200  int height = config.height == -1 ? image_msg->height : config.height;
201  int width = config.width == -1 ? image_msg->width : config.width;
202  cv::resize(cv_ptr->image, scaled_cv_.image, cv::Size(width, height), 0, 0, config.interpolation);
203  }
204 
205  scaled_cv_.header = image_msg->header;
206  scaled_cv_.encoding = image_msg->encoding;
208 }
209 
210 } // namespace image_proc
211 
image_proc::ResizeNodelet::infoCb
void infoCb(const sensor_msgs::CameraInfoConstPtr &info_msg)
Definition: resize.cpp:160
ros::Publisher
nodelet::Nodelet::getNodeHandle
ros::NodeHandle & getNodeHandle() const
image_proc::ResizeNodelet::sub_info_
ros::Subscriber sub_info_
Definition: resize.cpp:120
image_transport::ImageTransport
image_proc::ResizeNodelet::pub_info_
ros::Publisher pub_info_
Definition: resize.cpp:118
boost::shared_ptr
cv_bridge::CvImage::toImageMsg
sensor_msgs::ImagePtr toImageMsg() const
image_proc::ResizeNodelet::imageCb
void imageCb(const sensor_msgs::ImageConstPtr &image_msg)
Definition: resize.cpp:206
ros.h
image_proc::ResizeNodelet::nh_
std::shared_ptr< ros::NodeHandle > nh_
Definition: resize.cpp:115
image_transport::Publisher::getNumSubscribers
uint32_t getNumSubscribers() const
image_proc::ResizeNodelet::pnh_
std::shared_ptr< ros::NodeHandle > pnh_
Definition: resize.cpp:116
ros::Subscriber::shutdown
void shutdown()
image_proc::ResizeNodelet::sub_image_
image_transport::Subscriber sub_image_
Definition: resize.cpp:119
cv_bridge::Exception
nodelet::Nodelet::getPrivateNodeHandle
ros::NodeHandle & getPrivateNodeHandle() const
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
image_proc::ResizeNodelet::connectCb
void connectCb()
Definition: resize.cpp:133
f
f
image_transport::Subscriber
class_list_macros.h
image_proc::ResizeNodelet::onInit
virtual void onInit()
Definition: resize.cpp:110
image_proc::ResizeNodelet::ReconfigureServer
dynamic_reconfigure::Server< Config > ReconfigureServer
Definition: resize.cpp:128
cv_bridge::CvImage::header
std_msgs::Header header
image_proc::ResizeNodelet::config_mutex_
std::mutex config_mutex_
Definition: resize.cpp:126
image_proc::ResizeNodelet
Definition: resize.cpp:79
image_proc::ResizeNodelet::config_
Config config_
Definition: resize.cpp:130
image_transport::Publisher::publish
void publish(const sensor_msgs::Image &message) const
image_proc::ResizeNodelet::reconfigure_server_
std::shared_ptr< ReconfigureServer > reconfigure_server_
Definition: resize.cpp:129
image_transport.h
nodelet::Nodelet
cv_bridge::CvImage::encoding
std::string encoding
image_proc::ResizeNodelet::Config
image_proc::ResizeConfig Config
Definition: resize.cpp:127
image_proc
Definition: advertisement_checker.h:39
PLUGINLIB_EXPORT_CLASS
PLUGINLIB_EXPORT_CLASS(image_proc::CropNonZeroNodelet, nodelet::Nodelet)
nodelet.h
image_transport::Publisher
cv_bridge.h
ROS_ERROR
#define ROS_ERROR(...)
cv_bridge::CvImage
image_proc::ResizeNodelet::connect_mutex_
std::mutex connect_mutex_
Definition: resize.cpp:123
image_proc::ResizeNodelet::pub_image_
image_transport::Publisher pub_image_
Definition: resize.cpp:117
ros::Publisher::getNumSubscribers
uint32_t getNumSubscribers() const
cv_bridge::toCvShare
CvImageConstPtr toCvShare(const sensor_msgs::Image &source, const boost::shared_ptr< void const > &tracked_object, const std::string &encoding=std::string())
image_proc::ResizeNodelet::scaled_cv_
cv_bridge::CvImage scaled_cv_
Definition: resize.cpp:131
image_proc::ResizeNodelet::configCb
void configCb(Config &config, uint32_t level)
Definition: resize.cpp:154
cv_bridge::CvImage::image
cv::Mat image
image_proc::ResizeNodelet::it_
std::shared_ptr< image_transport::ImageTransport > it_
Definition: resize.cpp:122
ros::NodeHandle
ros::Subscriber
image_proc::ResizeNodelet::private_it_
std::shared_ptr< image_transport::ImageTransport > private_it_
Definition: resize.cpp:122
image_transport::Subscriber::shutdown
void shutdown()


image_proc
Author(s): Patrick Mihelich, Kurt Konolige, Jeremy Leibs
autogenerated on Wed Mar 2 2022 00:26:54