resize.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2017, Kentaro Wada.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Kentaro Wada nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 #include <cv_bridge/cv_bridge.h>
00035 #include <dynamic_reconfigure/server.h>
00036 #include <nodelet/nodelet.h>
00037 #include <nodelet_topic_tools/nodelet_lazy.h>
00038 #include <ros/ros.h>
00039 #include <sensor_msgs/CameraInfo.h>
00040 #include <sensor_msgs/Image.h>
00041 
00042 #include "image_proc/ResizeConfig.h"
00043 
00044 namespace image_proc
00045 {
00046 
00047 class ResizeNodelet : public nodelet_topic_tools::NodeletLazy
00048 {
00049 protected:
00050   // ROS communication
00051   ros::Publisher pub_image_;
00052   ros::Publisher pub_info_;
00053   ros::Subscriber sub_info_;
00054   ros::Subscriber sub_image_;
00055 
00056   // Dynamic reconfigure
00057   boost::recursive_mutex config_mutex_;
00058   typedef image_proc::ResizeConfig Config;
00059   typedef dynamic_reconfigure::Server<Config> ReconfigureServer;
00060   boost::shared_ptr<ReconfigureServer> reconfigure_server_;
00061   Config config_;
00062 
00063   virtual void onInit();
00064   virtual void subscribe();
00065   virtual void unsubscribe();
00066 
00067   void imageCb(const sensor_msgs::ImageConstPtr& image_msg);
00068   void infoCb(const sensor_msgs::CameraInfoConstPtr& info_msg);
00069 
00070   void configCb(Config &config, uint32_t level);
00071 };
00072 
00073 void ResizeNodelet::onInit()
00074 {
00075   nodelet_topic_tools::NodeletLazy::onInit();
00076 
00077   // Set up dynamic reconfigure
00078   reconfigure_server_.reset(new ReconfigureServer(config_mutex_, *pnh_));
00079   ReconfigureServer::CallbackType f = boost::bind(&ResizeNodelet::configCb, this, _1, _2);
00080   reconfigure_server_->setCallback(f);
00081 
00082   pub_info_ = advertise<sensor_msgs::CameraInfo>(*pnh_, "camera_info", 1);
00083   pub_image_ = advertise<sensor_msgs::Image>(*pnh_, "image", 1);
00084 
00085   onInitPostProcess();
00086 }
00087 
00088 void ResizeNodelet::configCb(Config &config, uint32_t level)
00089 {
00090   config_ = config;
00091 }
00092 
00093 void ResizeNodelet::subscribe()
00094 {
00095   sub_info_ = nh_->subscribe("camera_info", 1, &ResizeNodelet::infoCb, this);
00096   sub_image_ = nh_->subscribe("image", 1, &ResizeNodelet::imageCb, this);
00097 }
00098 
00099 void ResizeNodelet::unsubscribe()
00100 {
00101   sub_info_.shutdown();
00102   sub_image_.shutdown();
00103 }
00104 
00105 void ResizeNodelet::infoCb(const sensor_msgs::CameraInfoConstPtr& info_msg)
00106 {
00107   Config config;
00108   {
00109     boost::lock_guard<boost::recursive_mutex> lock(config_mutex_);
00110     config = config_;
00111   }
00112 
00113   sensor_msgs::CameraInfo dst_info_msg = *info_msg;
00114 
00115   double scale_y;
00116   double scale_x;
00117   if (config.use_scale)
00118   {
00119     scale_y = config.scale_height;
00120     scale_x = config.scale_width;
00121     dst_info_msg.height = static_cast<int>(info_msg->height * config.scale_height);
00122     dst_info_msg.width = static_cast<int>(info_msg->width * config.scale_width);
00123   }
00124   else
00125   {
00126     scale_y = static_cast<double>(config.height) / info_msg->height;
00127     scale_x = static_cast<double>(config.width) / info_msg->width;
00128     dst_info_msg.height = config.height;
00129     dst_info_msg.width = config.width;
00130   }
00131 
00132   dst_info_msg.K[0] = dst_info_msg.K[0] * scale_x;  // fx
00133   dst_info_msg.K[2] = dst_info_msg.K[2] * scale_x;  // cx
00134   dst_info_msg.K[4] = dst_info_msg.K[4] * scale_y;  // fy
00135   dst_info_msg.K[5] = dst_info_msg.K[5] * scale_y;  // cy
00136 
00137   dst_info_msg.P[0] = dst_info_msg.P[0] * scale_x;  // fx
00138   dst_info_msg.P[2] = dst_info_msg.P[2] * scale_x;  // cx
00139   dst_info_msg.P[3] = dst_info_msg.P[3] * scale_x;  // T
00140   dst_info_msg.P[5] = dst_info_msg.P[5] * scale_y;  // fy
00141   dst_info_msg.P[6] = dst_info_msg.P[6] * scale_y;  // cy
00142 
00143   pub_info_.publish(dst_info_msg);
00144 }
00145 
00146 void ResizeNodelet::imageCb(const sensor_msgs::ImageConstPtr& image_msg)
00147 {
00148   Config config;
00149   {
00150     boost::lock_guard<boost::recursive_mutex> lock(config_mutex_);
00151     config = config_;
00152   }
00153 
00154   cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(image_msg);
00155 
00156   if (config.use_scale)
00157   {
00158     cv::resize(cv_ptr->image, cv_ptr->image, cv::Size(0, 0),
00159                config.scale_width, config.scale_height, config.interpolation);
00160   }
00161   else
00162   {
00163     int height = config.height == -1 ? image_msg->height : config.height;
00164     int width = config.width == -1 ? image_msg->width : config.width;
00165     cv::resize(cv_ptr->image, cv_ptr->image, cv::Size(width, height), 0, 0, config.interpolation);
00166   }
00167 
00168   pub_image_.publish(cv_ptr->toImageMsg());
00169 }
00170 
00171 }  // namespace image_proc
00172 
00173 #include <pluginlib/class_list_macros.h>
00174 PLUGINLIB_EXPORT_CLASS(image_proc::ResizeNodelet, nodelet::Nodelet)


image_proc
Author(s): Patrick Mihelich, Kurt Konolige, Jeremy Leibs
autogenerated on Wed May 1 2019 02:51:47