Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
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
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() ) {
00140 try {
00141 int num = boost::lexical_cast<int>(filename_);
00142 cap_.open(num);
00143 } catch(boost::bad_lexical_cast &) {
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
00166
00167 flip_image_ = true;
00168 if (flip_horizontal && flip_vertical)
00169 flip_value_ = 0;
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);