openCV_to_ros.cpp
Go to the documentation of this file.
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 
00040 using namespace std;
00041 
00042 class ImageConverter {
00043 
00044 public:
00045   string image_file_;
00046   IplImage *cv_image_;
00047   double rate_;
00048   boost::thread spin_thread_;
00049   ImageConverter(ros::NodeHandle &n) :
00050     n_(n), it_(n_)
00051   {
00052     image_pub_ = it_.advertise("image_topic_2",10);
00053     spin_thread_ = boost::thread (boost::bind (&ros::spin));
00054   }
00055   
00056 
00057   ~ImageConverter()
00058   {
00059   }
00060 
00062   // Spin (!)
00063   bool spin ()
00064   {
00065     //double interval = rate_ * 1e+6;
00066     ros::Rate loop_rate(rate_);
00067     while (n_.ok ())
00068       {     
00069         ROS_INFO ("Publishing data on topic %s.", n_.resolveName ("image_topic_2").c_str ());
00070         try
00071           {
00072             image_pub_.publish(bridge_.cvToImgMsg(cv_image_));
00073           }
00074         catch (sensor_msgs::CvBridgeException error)
00075           {
00076             ROS_ERROR("error");
00077           }
00078         
00079         if (rate_ == 0)  
00080           break;
00081         loop_rate.sleep();
00082         //ros::spinOnce ();
00083       }
00084     return (true);
00085   }
00086 
00087 protected:
00088 
00089   ros::NodeHandle n_;
00090   image_transport::ImageTransport it_;
00091   sensor_msgs::CvBridge bridge_;
00092   image_transport::Publisher image_pub_;
00093 
00094 };
00095 
00096 int main(int argc, char** argv)
00097 {
00098   if (argc < 3)
00099     {
00100       ROS_ERROR ("Syntax is: %s <image file> [publishing_rate (in Hz)]", argv[0]);
00101       return (-1);
00102     }
00103   ros::init(argc, argv, "openCv_to_ros");
00104   ros::NodeHandle n;
00105   ImageConverter ic(n);
00106   ic.image_file_ = string(argv[1]);
00107   ic.rate_ = atof (argv[2]);
00108   ROS_INFO ("Loading file %s...", ic.image_file_.c_str ());
00109   ic.cv_image_ = cvLoadImage(ic.image_file_.c_str());  
00110   if (ic.cv_image_->width > 60 && ic.cv_image_->height > 60)
00111     cvCircle(ic.cv_image_, cvPoint(50,50), 10, CV_RGB( 0, 255, 0 ), 5);
00112   ic.spin();
00113   return 0;
00114 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


image_algos
Author(s): Dejan Pangercic
autogenerated on Thu May 23 2013 18:41:31