$search
00001 /* 00002 * Copyright (c) 2010, Dejan Pangercic <dejan.pangercic@cs.tum.edu> 00003 * All rights reserved. 00004 * 00005 * Redistribution and use in source and binary forms, with or without 00006 * modification, are permitted provided that the following conditions are met: 00007 * 00008 * * Redistributions of source code must retain the above copyright 00009 * notice, this list of conditions and the following disclaimer. 00010 * * Redistributions in binary form must reproduce the above copyright 00011 * notice, this list of conditions and the following disclaimer in the 00012 * documentation and/or other materials provided with the distribution. 00013 * * Neither the name of Willow Garage, Inc. nor the names of its 00014 * contributors may be used to endorse or promote products derived from 00015 * this software without specific prior written permission. 00016 * 00017 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00018 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00019 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00020 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00021 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00022 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00023 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00024 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00025 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00026 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00027 * POSSIBILITY OF SUCH DAMAGE. 00028 */ 00029 00030 #include <ros/ros.h> 00031 #include <ros/node_handle.h> 00032 #include "sensor_msgs/Image.h" 00033 #include "image_transport/image_transport.h" 00034 #include "cv_bridge/CvBridge.h" 00035 #include <opencv/cv.h> 00036 #include <opencv/highgui.h> 00037 #include <string.h> 00038 #include <boost/thread.hpp> 00039 #include <boost/filesystem/operations.hpp> 00040 #include <boost/filesystem/fstream.hpp> 00041 #include "boost/filesystem.hpp" 00042 00043 using namespace std; 00044 using namespace boost::filesystem; 00045 00046 00047 class ImageConverter { 00048 00049 public: 00050 string image_file_; 00051 IplImage *cv_image_; 00052 double rate_; 00053 boost::thread spin_thread_; 00054 std::vector<std::string> fileNames_; 00055 ImageConverter(ros::NodeHandle &n) : 00056 n_(n), it_(n_) 00057 { 00058 image_pub_ = it_.advertise("image_topic_2",10); 00059 spin_thread_ = boost::thread (boost::bind (&ros::spin)); 00060 } 00061 00062 ~ImageConverter() 00063 { 00064 } 00065 00066 void get_log_files ( const path & directory, vector <string> &file_list, string suffix=".pcd", 00067 bool recurse_into_subdirs = false ) 00068 { 00069 if( exists( directory ) ) 00070 { 00071 directory_iterator end ; 00072 for( directory_iterator iter(directory) ; iter != end ; ++iter ) 00073 if ( is_directory( *iter ) ) 00074 { 00075 if( recurse_into_subdirs ) get_log_files(*iter,file_list,suffix,recurse_into_subdirs) ; 00076 } 00077 else 00078 { 00079 int len = iter->string().length(); 00080 int npos = iter->string().rfind(suffix); 00081 if((unsigned long)(len - npos) == suffix.length()) 00082 file_list.push_back(iter->string()); 00083 } 00084 } 00085 } 00086 00087 00089 // Spin (!) 00090 bool spin () 00091 { 00092 ros::Rate loop_rate(1/rate_); 00093 while (n_.ok () && fileNames_.size()!= 0) 00094 { 00095 ROS_INFO ("Publishing data on topic %s.", n_.resolveName ("image_topic_2").c_str ()); 00096 00097 ROS_INFO ("Loading file %s...", fileNames_.back().c_str ()); 00098 cv_image_ = cvLoadImage(fileNames_.back().c_str()); 00099 fileNames_.pop_back(); 00100 try 00101 { 00102 image_pub_.publish(bridge_.cvToImgMsg(cv_image_)); 00103 } 00104 catch (sensor_msgs::CvBridgeException error) 00105 { 00106 ROS_ERROR("error"); 00107 } 00108 00109 if (rate_ == 0) 00110 break; 00111 loop_rate.sleep(); 00112 00113 } 00114 return (true); 00115 } 00116 00117 protected: 00118 00119 ros::NodeHandle n_; 00120 image_transport::ImageTransport it_; 00121 sensor_msgs::CvBridge bridge_; 00122 image_transport::Publisher image_pub_; 00123 00124 }; 00125 int main(int argc, char** argv) 00126 { 00127 if (argc < 3) 00128 { 00129 ROS_ERROR ("Syntax is: %s <path to directory> [time beetween 2 images (in sec)]", argv[0]); 00130 return (-1); 00131 } 00132 ros::init(argc, argv, "openCv_to_ros"); 00133 ros::NodeHandle n; 00134 ImageConverter ic(n); 00135 ic.get_log_files(argv[1],ic.fileNames_,".png",true); 00136 00137 for(unsigned int i=0;i<ic.fileNames_.size();i++){ 00138 00139 ROS_INFO ("fileNames %s..", ic.fileNames_[i].c_str()); 00140 } 00141 ic.rate_ = atof (argv[2]); 00142 00143 ic.spin(); 00144 00145 return 0; 00146 }