Shape_detect: offline.c - Testing for sequence measured files from Hokuyo.

	- editation offline.c for testing sequance files.
	- add -f switch for name open file
	- add -n switch for number open files
	- add -h and --help for print help

diff --git a/src/hokuyo/shape-detect/offline.cc b/src/hokuyo/shape-detect/offline.cc
index 32c5490507526111ac5459edc69478511f503f9d..5fed719ca1bd8e48f30f564312d0d932dccdeff1 100644 (file)
--- a/src/hokuyo/shape-detect/offline.cc
+++ b/src/hokuyo/shape-detect/offline.cc
@@ -19,6 +19,7 @@
 
 #include <shape_detect.h>
 #include <iostream>
+#include <stdio.h>
 using namespace std;
 
 ostream& operator << (ostream& os, Shape_detect::Point& point)
@@ -115,38 +116,16 @@ void gnuplot(std::vector<Shape_detect::Point> &cartes,
        pclose(gnuplot);
 }
 
-/**
- * There are detected line segments in input file (laser scan data)
- * @param argv name input file with measured data
- * @ingroup shapedet
- */
-int main(int argc, char** argv)
+void get_laser_scan(unsigned short laser_scan[], const char *fname)
 {
-       Shape_detect sd;
-
-       if (argc < 2) {
-               std::cout << "Error: Invalid number of input parameters." << std::endl;
-               return 1;
-       }
 
-       char *fname = NULL;
-       bool plot = false;
-
-       for (int i=1; i<argc; i++) {
-               if (strcmp(argv[i], "-g") == 0)
-                       plot = true;
-               else fname = argv[i];
-       }
-
-       unsigned short laser_scan[HOKUYO_ARRAY_SIZE];
-  
        std::ifstream infile(fname, std::ios_base::in);
 
        // line input file
        std::string line;
 
        // number from input file
-       int number;
+       unsigned short number;
        
        int idx = 0;
     
@@ -159,26 +138,97 @@ int main(int argc, char** argv)
                laser_scan[idx] = number;
                idx++;
        }
-       
+}
+
+/**
+ * There are detected line segments in input file (laser scan data)
+ * @param argv name input file with measured data
+ * @ingroup shapedet
+ */
+int main(int argc, char** argv)
+{
+       if (argc < 2 || strcmp(argv[1], "-h") == 0 || strcmp(argv[1], "--help") == 0) {
+               std::cout << "Help:" << std::endl;
+               std::cout << "Example: ./shape_detect_offline -g -f file_name -n seq_length" << std::endl;
+               std::cout << "-g: Gnuplot output." << std::endl;
+               std::cout << "-f file_name example: seq-scan-<NUMBER>." << std::endl;
+               std::cout << "-n seq_length: Maximal length sequence file <NUMBER>." << std::endl;
+               std::cout << std::endl;
+
+               return 0;
+       }
+
+       Shape_detect sd;
+
+       bool plot = false;
+       char fname[50];
+       int number_file = 0;
+       unsigned short laser_scan[HOKUYO_ARRAY_SIZE];
+
+       for (int i = 1; i < argc; i++) {
+               if (strcmp(argv[i], "-g") == 0)
+                       plot = true;
+               else if (strcmp(argv[i], "-f") == 0) {
+                       i++;
+                       strcpy(fname, argv[i]);
+               } else if (strcmp(argv[i], "-n") == 0) {
+                       i++;
+                       number_file = atoi(argv[i]);
+               } else {
+                       std::cout << "Invalid input parameter: " << argv[i] << "." << std::endl;
+                       return 1;
+               }
+       }
+
        std::vector<Shape_detect::Line> lines;
        std::vector<Shape_detect::Arc> arcs;
+       std::vector<Shape_detect::Arc> result;
+
+       if (number_file == 0) {
+               std::cout << "Open file: " << fname << std::endl;
+               get_laser_scan(laser_scan, fname);
+
+               sd.prepare(laser_scan);
+               sd.line_detect(lines);
+               sd.arc_detect(result);
+       } else {
+               for (int i = 1; i <= number_file; i++) {
+                       char name[50];
+                       if (i < 10)
+                               sprintf(name, "%s0%d", fname, i);
+                       else
+                               sprintf(name, "%s%d", fname, i);
+
+                       std::cout << "Open file: " << name << std::endl;
+
+                       get_laser_scan(laser_scan, name);
+
+                       sd.prepare(laser_scan);
+                       
+                       if (i == 1) {
+                               sd.line_detect(lines);
+                               sd.arc_detect(result);  
+                               continue;
+                       }
 
-       // shape detect
-       sd.prepare(laser_scan);
-       sd.line_detect(lines);
-       sd.arc_detect(arcs);
+                       sd.arc_detect(arcs);
+                       result = sd.arcs_compare(result, arcs, 8);
+                       
+                       if (result.size() == 0)
+                               break;
+               }
+       }
+
+       std::cout << std::endl << "Result:" << std::endl;
 
-       for (unsigned i = 0; i < lines.size(); i++) {
+       for (unsigned i = 0; i < lines.size(); i++)
                cout << "Line: " << lines[i] << endl;
-       }
 
-       for (unsigned i = 0; i < arcs.size(); i++) {
-               cout << "Arc: " << arcs[i] << endl;
-       }
+       for (unsigned i = 0; i < result.size(); i++)
+               cout << "Arc: " << result[i] << endl;
 
-       if (plot) {
-               gnuplot(sd.getCartes(), lines, arcs);
-       }
+       if (plot)
+               gnuplot(sd.getCartes(), lines, result);
 
        return 0;
 }