image_algos_main.cpp
Go to the documentation of this file.
00001 // #include <unistd.h>
00002 
00003 #include <map>
00004 #include <ctime>
00005 #include <ros/node_handle.h>
00006 #include <ias_table_msgs/TableWithObjects.h>
00007 
00008 // image_algos plugin stuff
00009 #include <image_algos/image_algos.h>
00010 #include <image_algos/color_find_hsv.h>
00011 #include <image_algos/pcd_to_image_projector_algo.h>
00012 #include <pluginlib/class_loader.h>
00013 
00014 //openCV
00015 #include <opencv/cv.h>
00016 #include <opencv/highgui.h>
00017 #include <cv_bridge/CvBridge.h>
00018 
00019 //boost
00020 #include <boost/thread/mutex.hpp>
00021 
00022 using namespace image_algos;
00023 
00025 struct TableObject
00026 {
00027   TableObject (): name(""),  sensor_type(""), object_type(""), object_color(""), 
00028                   object_geometric_type(""), perception_method(""){ }
00029   geometry_msgs::Point32 center;
00030   sensor_msgs::PointCloud point_cluster;
00031   geometry_msgs::Point32 minP;
00032   geometry_msgs::Point32 maxP;
00033   //unsigned long long lo_id;
00034   std::vector <double> coeffs;
00035   double score;
00036   std::vector<int> triangles;
00037   std::string name;
00038   std::string sensor_type;
00039   std::string object_type;
00040   std::string object_color;
00041   std::string object_geometric_type;
00042   std::string perception_method;
00043 };
00044 
00046 struct TableStateInstance
00047 {
00048   ros::Time time_instance;
00049   std::vector<TableObject*> objects;
00050 };
00051 
00052 struct Table
00053 {
00054   geometry_msgs::Point32 center;
00055   geometry_msgs::Polygon polygon;
00056   //sensor_msgs::Image roi;
00057   IplImage *roi;
00058   std::vector<TableStateInstance*> inst;
00059 };
00060 
00061 class ImageAlgosMain
00062 {
00063 protected:
00064   ros::NodeHandle &nh_;
00065   
00066   // topic names
00067   std::string input_table_topic_;
00068   std::string input_image_topic_;
00069   std::string output_table_roi_topic_;
00070   
00071   // publishers and subscribers
00072   ros::Publisher table_image_pub_;
00073   ros::Subscriber table_sub_;
00074   ros::Subscriber image_sub_;
00075 
00076   IplImage* image_, *clone_;
00077   boost::mutex  image_lock_;
00078   sensor_msgs::CvBridge bridge_;
00079   // plugin loader
00080   pluginlib::ClassLoader<image_algos::ImageAlgo> *cl;
00081 
00082   typedef struct _NamedAlgorithm 
00083   {
00084     std::string name;
00085     ImageAlgo * algorithm;
00086     _NamedAlgorithm (std::string n) : name(n) {};
00087   } NamedAlgorithm;
00088   
00089   std::vector<NamedAlgorithm> algorithm_pool;
00090   
00091   //table object
00092   Table t;
00093   //utility params
00094   bool got_image_;
00095 public:
00096   ImageAlgosMain (ros::NodeHandle &anode) : nh_(anode)
00097   {
00098     nh_.param ("input_table_topic", input_table_topic_, std::string("/table_with_objects"));
00099     nh_.param ("input_image_topic", input_image_topic_, std::string("/cop/right/camera"));
00100     nh_.param ("output_table_roi_topic", output_table_roi_topic_, std::string("table_roi"));       // 15 degrees
00101     table_sub_ = nh_.subscribe (input_table_topic_, 1, &ImageAlgosMain::table_cb, this);
00102     image_sub_ = nh_.subscribe (input_image_topic_, 1, &ImageAlgosMain::image_cb, this);
00103     table_image_pub_ = nh_.advertise<sensor_msgs::Image> (output_table_roi_topic_, 1);
00104     algorithm_pool.push_back (NamedAlgorithm ("image_algos/ColorFindHSV"));
00105     algorithm_pool.push_back (NamedAlgorithm ("image_algos/PCDToImageProjector"));
00106     load_plugins ();
00107     got_image_ = false;
00108   }
00109 
00115   bool load_algorithm (std::string algo_name, ImageAlgo*& algorithm)
00116   {
00117     try
00118     {
00119       cl->loadLibraryForClass(algo_name);
00120       ROS_DEBUG("Loaded library with plugin %s inside", algo_name.c_str());
00121     }
00122     catch(pluginlib::PluginlibException &ex)
00123     {
00124       ROS_ERROR("Failed to load library with plugin %s inside. Exception: %s", algo_name.c_str(), ex.what());
00125     }
00126     
00127     if (cl->isClassLoaded(algo_name))
00128     {
00129       algorithm = cl->createClassInstance(algo_name);
00130       algorithm->init (nh_);
00131       return true;
00132     }
00133     else ROS_ERROR("Cannot create ImageAlgo Class of type %s", algo_name.c_str ());
00134     return false;
00135   }
00136   
00140   void load_plugins ()
00141   {
00142     cl = new pluginlib::ClassLoader <image_algos::ImageAlgo> ("image_algos", "image_algos::ImageAlgo");
00143     
00144     ROS_INFO("ClassLoader instantiated");
00145     std::vector<std::string> plugins = cl->getDeclaredClasses();
00146     
00147     for (std::vector<std::string>::iterator it = plugins.begin(); it != plugins.end() ; ++it)
00148     {
00149       ROS_INFO("%s is in package %s and is of type %s", it->c_str(), cl->getClassPackage(*it).c_str(), cl->getClassType(*it).c_str());
00150       ROS_INFO("It does \"%s\"", cl->getClassDescription(*it).c_str());
00151     }
00152     
00153     for (unsigned int i = 0; i < algorithm_pool.size(); i++)
00154       load_algorithm (algorithm_pool[i].name, algorithm_pool[i].algorithm);
00155   }
00156   
00157   ImageAlgo* find_algorithm (std::string name)
00158   {
00159     for (unsigned int i = 0; i < algorithm_pool.size(); i++)
00160       if (algorithm_pool[i].name == name)
00161         return algorithm_pool[i].algorithm;
00162     return NULL;
00163   }
00164   
00165   void color_find (const boost::shared_ptr<const sensor_msgs::Image> image)
00166   {
00167     ImageAlgo * color_find = find_algorithm ("ColorFindHSV");
00168     ros::Publisher pub_image = nh_.advertise <ColorFindHSV::OutputType>
00169       (((ColorFindHSV*)color_find)->default_output_topic (), 5);
00170     
00171     // call ColorFindHSV
00172     color_find->pre();
00173     //std::cerr << "[color_find] Calling Color with a PCD with " <<
00174     //to->point_cluster.points.size () << " points." << std::endl;
00175     std::string process_answer_color_find = ((ColorFindHSV*)color_find)->process(image);
00176     ROS_INFO("got response: %s", process_answer_color_find.c_str ());
00177     boost::shared_ptr <const ias_table_msgs::TableObject> color_image = (((ColorFindHSV*)color_find)->output ());
00178     //image_pub_.publish (color_image);
00179     color_find->post();
00180   }
00181 
00182 
00183   // incoming data...
00184   void table_cb (const ias_table_msgs::TableWithObjects::ConstPtr& table)
00185   {
00186     t.center.x = table->table_min.x + ((table->table_max.x - table->table_min.x) / 2.0);
00187     t.center.y = table->table_min.y + ((table->table_max.y - table->table_min.y) / 2.0);
00188     t.center.z = table->table_min.z + ((table->table_max.z - table->table_min.z) / 2.0);
00189     for (unsigned int i = 0; i < table->table.points.size(); i++)
00190       t.polygon.points.push_back (table->table.points.at(i));
00191     ROS_INFO("Table polygon with size %ld", t.polygon.points.size());
00192 
00193     
00194     ImageAlgo *proj_points = find_algorithm ("image_algos/PCDToImageProjector");
00195     proj_points->pre();
00196     //got_cloud_ = true;
00197     if (got_image_)
00198     {
00199       std::string process_answer_proj_points = ((PCDToImageProjector*)proj_points)->process (t.polygon, clone_);
00200       ROS_INFO("Got response for table: %s", process_answer_proj_points.c_str ());
00201       t.polygon.points.clear();
00202       PCDToImageProjector::OutputType table_roi = ((PCDToImageProjector*)proj_points)->output ();
00203       //table_image_pub_.publish(bridge_.cvToImgMsg(table_roi));
00204       ((PCDToImageProjector*)proj_points)->post ();
00205       for (unsigned int i = 0; i < table->objects.size(); i++)
00206       {
00207         sensor_msgs::PointCloud point_cluster = table->objects[i].points;
00208         std::string process_answer_proj_points = ((PCDToImageProjector*)proj_points)->process (point_cluster, clone_);
00209         ROS_INFO("Got response for cluster: %s", process_answer_proj_points.c_str ());
00210         PCDToImageProjector::OutputType cluster_roi = ((PCDToImageProjector*)proj_points)->output ();
00211         sleep(1);
00212         table_image_pub_.publish(bridge_.cvToImgMsg(cluster_roi));
00213         ((PCDToImageProjector*)proj_points)->post ();
00214       }
00215       cvReleaseImage(&clone_);
00216       got_image_ = false;
00217     }
00218     //proj_points->post();
00219 
00220     //color_find(image);
00221     //print_mem_stats (table_found);
00222   }
00223 
00224   void image_cb(const sensor_msgs::ImageConstPtr& image_msg)
00225   {
00226     if (!got_image_)
00227     {
00228       image_ = NULL;
00229       try 
00230       {
00231         image_ = bridge_.imgMsgToCv(image_msg, "bgr8");
00232         clone_ = cvCloneImage(image_);
00233         ROS_INFO("[ImageAlgosMain: ] got image.");
00234       }
00235       catch (sensor_msgs::CvBridgeException& ex) 
00236       {
00237         ROS_ERROR("[PointCloudToImageProjector:] Failed to convert image");
00238         return;
00239       }
00240       got_image_ = true;
00241     }
00242   }
00243 };
00244 
00245 int main (int argc, char* argv[])
00246 {
00247   ros::init (argc, argv, "image_algos_main");
00248   
00249   ros::NodeHandle nh("~");
00250   ImageAlgosMain n(nh);
00251   ros::spin ();
00252   
00253   return (0);
00254 }
00255 
 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