image_publisher_nodelet.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2014, JSK Lab.
00005 *                2008, Willow Garage, Inc.
00006 *  All rights reserved.
00007 * 
00008 *  Redistribution and use in source and binary forms, with or without
00009 *  modification, are permitted provided that the following conditions
00010 *  are met:
00011 * 
00012 *   * Redistributions of source code must retain the above copyright
00013 *     notice, this list of conditions and the following disclaimer.
00014 *   * Redistributions in binary form must reproduce the above
00015 *     copyright notice, this list of conditions and the following
00016 *     disclaimer in the documentation and/or other materials provided
00017 *     with the distribution.
00018 *   * Neither the name of the Willow Garage nor the names of its
00019 *     contributors may be used to endorse or promote products derived
00020 *     from this software without specific prior written permission.
00021 * 
00022 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 *  POSSIBILITY OF SUCH DAMAGE.
00034 *********************************************************************/
00035 
00036 #include <ros/ros.h>
00037 #include <nodelet/nodelet.h>
00038 #include <cv_bridge/cv_bridge.h>
00039 #include <image_publisher/ImagePublisherConfig.h>
00040 #include <image_transport/image_transport.h>
00041 #include <sensor_msgs/CameraInfo.h>
00042 #include <camera_info_manager/camera_info_manager.h>
00043 #include <opencv2/highgui/highgui.hpp>
00044 #include <dynamic_reconfigure/server.h>
00045 #include <boost/assign.hpp>
00046 using namespace boost::assign;
00047 
00048 namespace image_publisher {
00049 class ImagePublisherNodelet : public nodelet::Nodelet
00050 {
00051   dynamic_reconfigure::Server<image_publisher::ImagePublisherConfig> srv;
00052 
00053   image_transport::CameraPublisher pub_;
00054 
00055   boost::shared_ptr<image_transport::ImageTransport> it_;
00056   ros::NodeHandle nh_;
00057 
00058   cv::VideoCapture cap_;
00059   cv::Mat image_;
00060   int subscriber_count_;
00061   ros::Timer timer_;
00062 
00063   std::string frame_id_;
00064   std::string filename_;
00065   bool flip_image_;
00066   int flip_value_;
00067   sensor_msgs::CameraInfo camera_info_;
00068   
00069 
00070   void reconfigureCallback(image_publisher::ImagePublisherConfig &new_config, uint32_t level)
00071   {
00072     frame_id_ = new_config.frame_id;
00073 
00074     timer_ = nh_.createTimer(ros::Duration(1.0/new_config.publish_rate), &ImagePublisherNodelet::do_work, this);
00075 
00076     camera_info_manager::CameraInfoManager c(nh_);
00077     if ( !new_config.camera_info_url.empty() ) {
00078       try {
00079         c.validateURL(new_config.camera_info_url);
00080         c.loadCameraInfo(new_config.camera_info_url);
00081         camera_info_ = c.getCameraInfo();
00082       } catch(cv::Exception &e) {
00083         NODELET_ERROR("camera calibration failed to load: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
00084       }
00085     }
00086     
00087 
00088   }
00089 
00090   void do_work(const ros::TimerEvent& event)
00091   {
00092     // Transform the image.
00093     try
00094     {
00095       if ( cap_.isOpened() ) {
00096         if ( ! cap_.read(image_) ) {
00097           cap_.set(CV_CAP_PROP_POS_FRAMES, 0);
00098         }
00099       }
00100       if (flip_image_)
00101         cv::flip(image_, image_, flip_value_);
00102 
00103       sensor_msgs::ImagePtr out_img = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_).toImageMsg();
00104       out_img->header.frame_id = frame_id_;
00105       out_img->header.stamp = ros::Time::now();
00106       camera_info_.header.frame_id = out_img->header.frame_id;
00107       camera_info_.header.stamp = out_img->header.stamp;
00108 
00109       pub_.publish(*out_img, camera_info_);
00110     }
00111     catch (cv::Exception &e)
00112     {
00113       NODELET_ERROR("Image processing error: %s %s %s %i", e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
00114     }
00115   }
00116 
00117   void connectCb(const image_transport::SingleSubscriberPublisher& ssp)
00118   {
00119     subscriber_count_++;
00120   }
00121 
00122   void disconnectCb(const image_transport::SingleSubscriberPublisher&)
00123   {
00124     subscriber_count_--;
00125   }
00126 
00127 public:
00128   virtual void onInit()
00129   {
00130     subscriber_count_ = 0;
00131     nh_ = getPrivateNodeHandle();
00132     it_ = boost::shared_ptr<image_transport::ImageTransport>(new image_transport::ImageTransport(nh_));
00133     pub_ = image_transport::ImageTransport(nh_).advertiseCamera("image_raw", 1);
00134 
00135     nh_.param("filename", filename_, std::string(""));
00136     NODELET_INFO("File name for publishing image is : %s", filename_.c_str());
00137     try {
00138       image_ = cv::imread(filename_, CV_LOAD_IMAGE_COLOR);
00139       if ( image_.empty() ) { // if filename is motion file or device file
00140         try {  // if filename is number
00141           int num = boost::lexical_cast<int>(filename_);//num is 1234798797
00142           cap_.open(num);
00143         } catch(boost::bad_lexical_cast &) { // if file name is string
00144           cap_.open(filename_);
00145         }
00146         CV_Assert(cap_.isOpened());
00147         cap_.read(image_);
00148         cap_.set(CV_CAP_PROP_POS_FRAMES, 0);
00149       }
00150       CV_Assert(!image_.empty());
00151     }
00152     catch (cv::Exception &e)
00153     {
00154       NODELET_ERROR("Failed to load image (%s): %s %s %s %i", filename_.c_str(), e.err.c_str(), e.func.c_str(), e.file.c_str(), e.line);
00155     }
00156 
00157     bool flip_horizontal;
00158     nh_.param("flip_horizontal", flip_horizontal, false);
00159     NODELET_INFO("Flip horizontal image is : %s",  ((flip_horizontal)?"true":"false"));
00160 
00161     bool flip_vertical;
00162     nh_.param("flip_vertical", flip_vertical, false);
00163     NODELET_INFO("Flip flip_vertical image is : %s", ((flip_vertical)?"true":"false"));
00164 
00165     // From http://docs.opencv.org/modules/core/doc/operations_on_arrays.html#void flip(InputArray src, OutputArray dst, int flipCode)
00166     // FLIP_HORIZONTAL == 1, FLIP_VERTICAL == 0 or FLIP_BOTH == -1
00167     flip_image_ = true;
00168     if (flip_horizontal && flip_vertical)
00169       flip_value_ = 0; // flip both, horizontal and vertical
00170     else if (flip_horizontal)
00171       flip_value_ = 1;
00172     else if (flip_vertical)
00173       flip_value_ = -1;
00174     else
00175       flip_image_ = false;
00176 
00177     camera_info_.width = image_.cols;
00178     camera_info_.height = image_.rows;
00179     camera_info_.distortion_model = "plumb_bob";
00180     camera_info_.D = list_of(0)(0)(0)(0)(0).convert_to_container<std::vector<double> >();
00181     camera_info_.K = list_of(1)(0)(camera_info_.width/2)(0)(1)(camera_info_.height/2)(0)(0)(1);
00182     camera_info_.R = list_of(1)(0)(0)(0)(1)(0)(0)(0)(1);
00183     camera_info_.P = list_of(1)(0)(camera_info_.width/2)(0)(0)(1)(camera_info_.height/2)(0)(0)(0)(1)(0);
00184 
00185     timer_ = nh_.createTimer(ros::Duration(1), &ImagePublisherNodelet::do_work, this);
00186 
00187     dynamic_reconfigure::Server<image_publisher::ImagePublisherConfig>::CallbackType f =
00188       boost::bind(&ImagePublisherNodelet::reconfigureCallback, this, _1, _2);
00189     srv.setCallback(f);
00190   }
00191 };
00192 }
00193 #include <pluginlib/class_list_macros.h>
00194 PLUGINLIB_EXPORT_CLASS(image_publisher::ImagePublisherNodelet, nodelet::Nodelet);


image_publisher
Author(s): Kei Okada
autogenerated on Tue Sep 19 2017 02:56:17