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 #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
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;
00055 const static int num_patterns = 5;
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:
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:
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:
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:
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:
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:
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')
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
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
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
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
00303
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 }