resized_image_transport: image_resizer

Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <nodelet/nodelet.h>
00003 #include <sensor_msgs/Image.h>
00004 #include <sensor_msgs/CameraInfo.h>
00005 
00006 #include <image_transport/image_transport.h>
00007 #include <opencv2/opencv.hpp>
00008 #include <cv_bridge/cv_bridge.h>
00009 #include <std_srvs/Empty.h>
00010 #include <std_msgs/Empty.h>
00011 
00012 #include <boost/thread/mutex.hpp>
00013 #include <boost/foreach.hpp>
00014 #include <boost/circular_buffer.hpp>
00015 #include <boost/lambda/lambda.hpp>
00016 
00017 // dynamic reconfigure
00018 #include <dynamic_reconfigure/server.h>
00019 #include "resized_image_transport/ImageResizerConfig.h"
00020 
00021 namespace resized_image_transport
00022 {
00023 class ImageResizer: public nodelet::Nodelet
00024 {
00025   // variables
00026 public:
00027 protected:
00028   ros::NodeHandle nh;
00029   ros::NodeHandle pnh;
00030 
00031   // dynamic reconfigure
00032   typedef dynamic_reconfigure::Server<resized_image_transport::ImageResizerConfig> ReconfigureServer;
00033   ReconfigureServer reconfigure_server_;
00034 
00035   image_transport::CameraSubscriber cs_;
00036   image_transport::CameraPublisher cp_;
00037   image_transport::ImageTransport *it_;
00038   ros::ServiceServer srv_;
00039   ros::Subscriber sub_;
00040 
00041   double resize_x_, resize_y_;
00042   int dst_width_, dst_height_;
00043   int max_queue_size_;
00044   bool use_snapshot_;
00045   bool publish_once_;
00046   bool use_messages_;
00047   bool use_bytes_;
00048   bool verbose_;
00049   ros::Time last_rosinfo_time_, last_subscribe_time_, last_publish_time_;
00050   ros::Duration period_;
00051   boost::mutex mutex_;
00052 
00053 private:
00054   
00055   
00056 public:
00057 protected:
00058   
00059   boost::circular_buffer<double> in_times;
00060   boost::circular_buffer<double> out_times;
00061   boost::circular_buffer<double> in_bytes;
00062   boost::circular_buffer<double> out_bytes;
00063 
00064   void onInit() {
00065     nh = getNodeHandle();
00066     pnh = getPrivateNodeHandle();
00067     publish_once_ = true;
00068     ReconfigureServer::CallbackType f
00069       = boost::bind(&ImageResizer::config_cb, this, _1, _2);
00070     reconfigure_server_.setCallback(f);
00071 
00072     pnh.param("resize_scale_x", resize_x_, 1.0);
00073     NODELET_INFO("resize_scale_x : %f", resize_x_);
00074     pnh.param("resize_scale_y", resize_y_, 1.0);
00075     NODELET_INFO("resize_scale_y : %f", resize_y_);
00076 
00077     pnh.param("width", dst_width_, 0);
00078     NODELET_INFO("width : %d", dst_width_);
00079     pnh.param("height", dst_height_, 0);
00080     NODELET_INFO("height : %d", dst_height_);
00081 
00082     pnh.param("max_queue_size", max_queue_size_, 5);
00083 
00084     pnh.param("use_snapshot", use_snapshot_, false);
00085     pnh.param("use_messages", use_messages_, false);
00086     if (use_messages_) {
00087         double d_period;
00088         pnh.param("period", d_period, 1.0);
00089         period_ = ros::Duration(d_period);
00090         NODELET_INFO("use_messages : %d", use_messages_);
00091         NODELET_INFO("message period : %f", period_.toSec());
00092     }
00093     pnh.param("use_bytes", use_bytes_, false);
00094 
00095     it_ = new image_transport::ImageTransport(pnh);
00096     std::string img = nh.resolveName("image");
00097     std::string cam = nh.resolveName("camera");
00098     if (img.at(0) == '/') {
00099       img.erase(0, 1);
00100     }
00101     NODELET_INFO("camera = %s", cam.c_str());
00102     NODELET_INFO("image = %s", img.c_str());
00103 
00104     if (use_snapshot_) {
00105       publish_once_ = false;
00106       srv_ = pnh.advertiseService("snapshot", &ImageResizer::snapshot_srv_cb, this);
00107       sub_ = pnh.subscribe("snapshot", 1, &ImageResizer::snapshot_msg_cb, this);
00108     }
00109 
00110     cp_ = it_->advertiseCamera("output/image", max_queue_size_);
00111 
00112     cs_ = it_->subscribeCamera("input/image", max_queue_size_,
00113                                &ImageResizer::callback, this);
00114 
00115 
00116   }
00117 public:
00118   ImageResizer():
00119     in_times(boost::circular_buffer<double>(100)),
00120     out_times(boost::circular_buffer<double>(100)),
00121     in_bytes(boost::circular_buffer<double>(100)),
00122     out_bytes(boost::circular_buffer<double>(100))
00123     { }
00124   ~ImageResizer() { }
00125 
00126 protected:
00127 
00128   void config_cb (resized_image_transport::ImageResizerConfig &config, uint32_t level) {
00129     NODELET_INFO("config_cb");
00130     resize_x_ = config.resize_scale_x;
00131     resize_y_ = config.resize_scale_y;
00132     period_ = ros::Duration(1.0/config.msg_par_second);
00133     verbose_ = config.verbose;
00134     NODELET_DEBUG("resize_scale_x : %f", resize_x_);
00135     NODELET_DEBUG("resize_scale_y : %f", resize_y_);
00136     NODELET_DEBUG("message period : %f", period_.toSec());
00137   }
00138 
00139   void snapshot_msg_cb (const std_msgs::EmptyConstPtr msg) {
00140     boost::mutex::scoped_lock lock(mutex_);
00141 
00142     publish_once_ = true;
00143   }
00144 
00145   bool snapshot_srv_cb (std_srvs::Empty::Request &req,
00146                         std_srvs::Empty::Response &res) {
00147     boost::mutex::scoped_lock lock(mutex_);
00148 
00149     publish_once_ = true;
00150     return true;
00151   }
00152 
00153   void callback(const sensor_msgs::ImageConstPtr &img,
00154                 const sensor_msgs::CameraInfoConstPtr &info) {
00155     boost::mutex::scoped_lock lock(mutex_);
00156     ros::Time now = ros::Time::now();
00157     ROS_DEBUG("resize: callback");
00158     if ( !publish_once_ || cp_.getNumSubscribers () == 0 ) {
00159       ROS_DEBUG("resize: number of subscribers is 0, ignoring image");
00160       return;
00161     }
00162     in_times.push_front((now - last_subscribe_time_).toSec());
00163     in_bytes.push_front(img->data.size());
00164     //
00165     try {
00166         int width = dst_width_ ? dst_width_ : (resize_x_ * info->width);
00167         int height = dst_height_ ? dst_height_ : (resize_y_ * info->height);
00168         double scale_x = dst_width_ ? ((double)dst_width_)/info->width : resize_x_;
00169         double scale_y = dst_height_ ? ((double)dst_height_)/info->height : resize_y_;
00170 
00171         cv_bridge::CvImagePtr cv_img = cv_bridge::toCvCopy(img);
00172 
00173         cv::Mat tmpmat(height, width, cv_img->image.type());
00174         cv::resize(cv_img->image, tmpmat, cv::Size(width, height));
00175         cv_img->image = tmpmat;
00176 
00177         sensor_msgs::CameraInfo tinfo = *info;
00178         tinfo.height = height;
00179         tinfo.width = width;
00180         tinfo.K[0] = tinfo.K[0] * scale_x; // fx
00181         tinfo.K[2] = tinfo.K[2] * scale_x; // cx
00182         tinfo.K[4] = tinfo.K[4] * scale_y; // fy
00183         tinfo.K[5] = tinfo.K[5] * scale_y; // cy
00184 
00185         tinfo.P[0] = tinfo.P[0] * scale_x; // fx
00186         tinfo.P[2] = tinfo.P[2] * scale_x; // cx
00187         tinfo.P[3] = tinfo.P[3] * scale_x; // T
00188         tinfo.P[5] = tinfo.P[5] * scale_y; // fy
00189         tinfo.P[6] = tinfo.P[6] * scale_y; // cy
00190 
00191         if ( !use_messages_ || now - last_publish_time_  > period_ ) {
00192             cp_.publish(cv_img->toImageMsg(),
00193                         boost::make_shared<sensor_msgs::CameraInfo> (tinfo));
00194 
00195             out_times.push_front((now - last_publish_time_).toSec());
00196             out_bytes.push_front(cv_img->image.total()*cv_img->image.elemSize());
00197 
00198             last_publish_time_ = now;
00199         }
00200     } catch( cv::Exception& e ) {
00201         ROS_ERROR("%s", e.what());
00202     }
00203 
00204 
00205     float duration =  (now - last_rosinfo_time_).toSec();
00206     if ( duration > 2 ) {
00207         int in_time_n = in_times.size();
00208         int out_time_n = out_times.size();
00209         double in_time_mean = 0, in_time_rate = 1.0, in_time_std_dev = 0.0, in_time_max_delta, in_time_min_delta;
00210         double out_time_mean = 0, out_time_rate = 1.0, out_time_std_dev = 0.0, out_time_max_delta, out_time_min_delta;
00211 
00212         std::for_each( in_times.begin(), in_times.end(), (in_time_mean += boost::lambda::_1) );
00213         in_time_mean /= in_time_n;
00214         in_time_rate /= in_time_mean;
00215         std::for_each( in_times.begin(), in_times.end(), (in_time_std_dev += (boost::lambda::_1 - in_time_mean)*(boost::lambda::_1 - in_time_mean) ) );
00216         in_time_std_dev = sqrt(in_time_std_dev/in_time_n);
00217         if ( in_time_n > 1 ) {
00218             in_time_min_delta = *std::min_element(in_times.begin(), in_times.end());
00219             in_time_max_delta = *std::max_element(in_times.begin(), in_times.end());
00220         }
00221 
00222         std::for_each( out_times.begin(), out_times.end(), (out_time_mean += boost::lambda::_1) );
00223         out_time_mean /= out_time_n;
00224         out_time_rate /= out_time_mean;
00225         std::for_each( out_times.begin(), out_times.end(), (out_time_std_dev += (boost::lambda::_1 - out_time_mean)*(boost::lambda::_1 - out_time_mean) ) );
00226         out_time_std_dev = sqrt(out_time_std_dev/out_time_n);
00227         if ( out_time_n > 1 ) {
00228             out_time_min_delta = *std::min_element(out_times.begin(), out_times.end());
00229             out_time_max_delta = *std::max_element(out_times.begin(), out_times.end());
00230         }
00231 
00232         double in_byte_mean = 0, out_byte_mean = 0;
00233         std::for_each( in_bytes.begin(), in_bytes.end(), (in_byte_mean += boost::lambda::_1) );
00234         std::for_each( out_bytes.begin(), out_bytes.end(), (out_byte_mean += boost::lambda::_1) );
00235         in_byte_mean /= duration;
00236         out_byte_mean /= duration;
00237 
00238         if (verbose_) {
00239           NODELET_INFO_STREAM(" in  bandwidth: " << std::fixed << std::setw(11) << std::setprecision(3)  << in_byte_mean/1000*8
00240                               << " Kbps rate:"   << std::fixed << std::setw(7) << std::setprecision(3) << in_time_rate
00241                               << " hz min:"      << std::fixed << std::setw(7) << std::setprecision(3) << in_time_min_delta
00242                               << " s max: "    << std::fixed << std::setw(7) << std::setprecision(3) << in_time_max_delta
00243                               << " s std_dev: "<< std::fixed << std::setw(7) << std::setprecision(3) << in_time_std_dev << "s n: " << in_time_n);
00244           NODELET_INFO_STREAM(" out bandwidth: " << std::fixed << std::setw(11) << std::setprecision(3)  << out_byte_mean/1000*8
00245                               << " kbps rate:"   << std::fixed << std::setw(7) << std::setprecision(3) << out_time_rate
00246                               << " hz min:"      << std::fixed << std::setw(7) << std::setprecision(3) << out_time_min_delta
00247                               << " s max: "    << std::fixed << std::setw(7) << std::setprecision(3) << out_time_max_delta
00248                               << " s std_dev: "<< std::fixed << std::setw(7) << std::setprecision(3) << out_time_std_dev << "s n: " << out_time_n);
00249         }
00250         in_times.clear();
00251         in_bytes.clear();
00252         out_times.clear();
00253         out_bytes.clear();
00254         last_rosinfo_time_ = now;
00255     }
00256 
00257     last_subscribe_time_ = now;
00258 
00259     if(use_snapshot_) {
00260       publish_once_ = false;
00261     }
00262   }
00263 };
00264 }
00265 
00266 #include <pluginlib/class_list_macros.h>
00267 typedef resized_image_transport::ImageResizer ImageResizer;
00268 PLUGINLIB_DECLARE_CLASS (resized_image_transport, ImageResizer, ImageResizer, nodelet::Nodelet);