create_pattern.cpp
Go to the documentation of this file.
00001 /* 
00002  * Copyright (c) 2010, Florian Zacherl <Florian.Zacherl1860@mytum.de>, 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 the Intelligent Autonomous Systems Group/
00014  *       Technische Universitaet Muenchen nor the names of its contributors 
00015  *       may be used to endorse or promote products derived from this software 
00016  *       without specific prior written permission.
00017  * 
00018  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00021  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00022  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00023  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00024  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00025  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00026  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00027  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00028  * POSSIBILITY OF SUCH DAMAGE.
00029  */
00030 
00031 #include <opencv/cv.h>
00032 #include "opencv/highgui.h"
00033 #include <iostream>
00034 #include <fstream>
00035 #include <math.h>
00036 #include <ros/ros.h>
00037 #include <dirent.h>
00038 #include "ias_projected_light/cp.h"
00039 //#include <gdk/gdk.h>
00040 
00041 
00042 using namespace std;
00043 
00044 class CreatePattern
00045 {
00046 
00047 private:
00048 
00049   int pwidth;
00050   int pheight;
00051   int awidth;
00052   int aheight;
00053   IplImage *pattern;
00054   int block_size; //The size of the smallest block in the pattern
00055   const static int num_patterns = 5; //number of patterns that are not created from files
00056 
00057   int pattern_index, pattern_count;
00058   vector<string> filepaths;
00059   bool service;
00060 
00061   ros::NodeHandle n;
00062   ros::ServiceServer serv;
00063 
00064   void setBlock(int posx, int posy, int col)
00065   {
00066     for (int x = 0; x < block_size; x++)
00067     {
00068       for (int y = 0; y < block_size; y++)
00069       {
00070         int ypix = posy * block_size + y;
00071         int xpix = posx * block_size + x;
00072         if (xpix < pwidth && ypix < pheight)
00073         {
00074           cvSet2D(pattern, ypix, xpix, cvScalar(col));
00075         }
00076       }
00077     }
00078   }
00079 
00080 
00081   void drawPattern()
00082   {
00083     switch (pattern_index)
00084     {
00085       case 0: //No (black) pattern
00086         for (int x = 0; x < ceil((float)pwidth / block_size); x++)
00087         {
00088           for (int y = 0; y < ceil((float)pheight / block_size); y++)
00089           {
00090             setBlock(x, y, 0);
00091           }
00092         }
00093         break;
00094 
00095       case 1: //White image
00096         for (int x = 0; x < ceil((float)pwidth / block_size); x++)
00097         {
00098           for (int y = 0; y < ceil((float)pheight / block_size); y++)
00099           {
00100             setBlock(x, y, 255);
00101           }
00102         }
00103         break;
00104 
00105       case 2: // Block pattern
00106         for (int x = 0; x < ceil((float)pwidth / block_size); x++)
00107         {
00108           for (int y = 0; y < ceil((float)pheight / block_size); y++)
00109           {
00110             if (x % 2 == y % 2)
00111             {
00112               setBlock(x, y, 255);
00113             }
00114             else
00115             {
00116               setBlock(x, y, 0);
00117             }
00118           }
00119         }
00120         break;
00121 
00122       case 3: //Stripe pattern
00123         for (int x = 0; x < ceil((float)pwidth / block_size); x++)
00124         {
00125           for (int y = 0; y < ceil((float)pheight / block_size); y++)
00126           {
00127             if (y % 2)
00128             {
00129               setBlock(x, y, 255);
00130             }
00131             else
00132             {
00133               setBlock(x, y, 0);
00134             }
00135           }
00136         }
00137         break;
00138 
00139       case 4: // Random pattern
00140         for (int x = 0; x < ceil((float)pwidth / block_size); x++)
00141         {
00142           for (int y = 0; y < ceil((float)pheight / block_size); y++)
00143           {
00144             if (rand() < RAND_MAX / 2)
00145             {
00146               setBlock(x, y, 0);
00147             }
00148             else
00149             {
00150               setBlock(x, y, 255);
00151             }
00152 
00153           }
00154         }
00155         break;
00156 
00157       default: //Pattern from textfile
00158 
00159         ifstream i(filepaths[pattern_index - num_patterns].c_str());
00160         if (!i)
00161         {
00162           ROS_INFO("Cannot open file.\n");
00163           return;
00164         }
00165 
00166         string line;
00167 
00168         int row = 0;
00169 
00170         while (!i.eof())
00171         {
00172           getline(i, line);
00173           if (line[0] == '0' || line[0] == '1') //Pattern data
00174           {
00175             for (int i = 0; i < aheight; i++)
00176             {
00177               for (int k = 0; k < ceil((float)pheight / aheight / block_size); k++)
00178               {
00179                 for (int g = 0; g < ceil((float)pwidth / awidth / block_size); g++)
00180                 {
00181 
00182                   int posx = row + g * awidth;
00183                   int posy = k * aheight + i;
00184                   setBlock(posx, posy, (line[i] == '0' ? 0 : 255));
00185                 }
00186               }
00187             }
00188             row++;
00189           }
00190           else //Pattern header (get size of pattern, ignore the rest of the header)
00191           {
00192             if (line.substr(0, 11) == "Array size:")
00193             {
00194               unsigned int pos = 12;
00195               string n = "";
00196               while (line[pos] != ' ')
00197               {
00198                 n += line[pos];
00199                 pos++;
00200               }
00201               awidth = atoi(n.c_str());
00202               pos += 3;
00203               n = "";
00204               while (pos < line.size())
00205               {
00206                 n += line[pos];
00207                 pos++;
00208               }
00209               aheight = atoi(n.c_str());
00210               //cout << "Array size: " << awidth << " x " << aheight << endl;
00211             }
00212           }
00213         }
00214 
00215     }
00216 
00217     cvShowImage("Window", pattern);
00218 
00219   }
00220 
00221   string pattern_name(int index)
00222   {
00223     switch (pattern_index)
00224     {
00225       case 0:
00226         return "black";
00227         break;
00228       case 1:
00229         return "white";
00230         break;
00231       case 2:
00232         return "block";
00233         break;
00234       case 3:
00235         return "stripe";
00236         break;
00237       case 4:
00238         return "random";
00239         break;
00240       default:
00241         return filepaths[pattern_index - num_patterns].c_str();
00242         break;
00243     }
00244   }
00245 
00246 public:
00247 
00248   bool change_pattern(ias_projected_light::cp::Request &req, ias_projected_light::cp::Response &res)
00249   {
00250     if (req.pattern < pattern_count)
00251     {
00252       pattern_index = req.pattern;
00253       block_size = req.block_size;
00254       res.name = pattern_name(pattern_index);
00255 
00256       drawPattern();
00257       cvWaitKey(100);
00258     }
00259     else
00260     {
00261       ROS_WARN("Wrong pattern number!");
00262       return false;
00263     }
00264     return true;
00265   }
00266 
00267   CreatePattern(ros::NodeHandle& n, int psize, char* p, bool s) :
00268     n(n), block_size(psize), pattern_index(0), service(s)
00269   {
00270     string path(p);
00271 
00272     pwidth = 1024;
00273     pheight = 768;
00274     //block_size = psize;
00275 
00276     DIR *pDIR;
00277     struct dirent *entry;
00278     if ((pDIR = opendir(path.c_str())))
00279     {
00280       while ((entry = readdir(pDIR)))
00281       {
00282         string file(entry->d_name);
00283         if (file.size() > 3 && strcmp(file.substr(file.size() - 3, 3).c_str(), "txt") == 0)
00284         {
00285           stringstream ss;
00286           ss << path << "/" << file;
00287           filepaths.push_back(ss.str());
00288           ROS_INFO("%s",ss.str().c_str());
00289         }
00290       }
00291     }
00292     else
00293     {
00294       ROS_ERROR("Couldn't find folder %s!", path.c_str());
00295       return;
00296     }
00297     pattern = cvCreateImage(cvSize(pwidth, pheight), IPL_DEPTH_8U, 1);
00298 
00299     pattern_count = num_patterns + filepaths.size();
00300 
00301     cvNamedWindow("Window");
00302     /*GdkWindow *w = gtk_widget_get_parent_window(cvGetWindowHandle("Window"));
00303     gdk_window_fullscreen(w);*/
00304 
00305 
00306     drawPattern();
00307 
00308     if (service)
00309     {
00310       serv = n.advertiseService("change_pattern", &CreatePattern::change_pattern, this);
00311       cvWaitKey(100);
00312     }
00313     else
00314     {
00315       while (true)
00316       {
00317         char c = cvWaitKey(0);
00318         switch (c)
00319         {
00320           case '+':
00321             block_size++;
00322             ROS_INFO("Block size: %d", block_size);
00323             break;
00324           case '-':
00325             if (block_size > 1)
00326               block_size--;
00327             ROS_INFO("Block size: %d", block_size);
00328             break;
00329           case 'p':
00330             pattern_index = (pattern_index + 1) % pattern_count;
00331             ROS_INFO("Switch to %s pattern", pattern_name(pattern_index).c_str());
00332             break;
00333 
00334             break;
00335           case 'q':
00336             return;
00337             break;
00338 
00339           case 'h':
00340             ROS_INFO("\nPress \"p\" to switch through patterns and \"+\" or \"-\" to increment or decrement the block size by one.");
00341 
00342         }
00343         drawPattern();
00344       }
00345     }
00346 
00347   }
00348 };
00349 
00350 int main(int argc, char** argv)
00351 {
00352   if (argc < 3)
00353   {
00354     ROS_INFO("Usage: %s <int: initial block size> <string: text file patterns folder> <bool: call pattern creator as ros service or change patterns with keybord>?", argv[0]);
00355     return 0;
00356   }
00357 
00358   ros::init(argc, argv, "create_pattern");
00359   ros::NodeHandle nh;
00360   bool s = false;
00361   if (argc > 3)
00362   {
00363     s = atoi(argv[3]);
00364   }
00365   CreatePattern cp(nh, atoi(argv[1]), argv[2], s);
00366 
00367   if(s) ros::spin();
00368 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


ias_projected_light
Author(s): Florian Zacherl, Dejan Pangercic
autogenerated on Sun Oct 6 2013 12:09:47