shape-detect: Works with real hokuyo

diff --git a/src/hokuyo/shape-detect/Makefile.omk b/src/hokuyo/shape-detect/Makefile.omk
index e6b9bdd1d9fe210b0965dd024fe6e80fcacf43db..b65e39b71d26b325e1d9f27d52cd845df1f40e8f 100644 (file)
--- a/src/hokuyo/shape-detect/Makefile.omk
+++ b/src/hokuyo/shape-detect/Makefile.omk
@@ -3,4 +3,4 @@
 bin_PROGRAMS = shape_detect
 
 shape_detect_SOURCES = main.cc
-#shape_detect_LIBS = roboorte robottype orte
+shape_detect_LIBS = roboorte robottype orte rt

diff --git a/src/hokuyo/shape-detect/main.cc b/src/hokuyo/shape-detect/main.cc
index 8f9201a75aefcef31bf3f3723ea94957f52feb14..04c3dc096adf27e668f66fbcb56114e310610355 100644 (file)
--- a/src/hokuyo/shape-detect/main.cc
+++ b/src/hokuyo/shape-detect/main.cc
@@ -237,12 +237,7 @@ void shape_detect(const vector<int> &input_data, vector<Line> &lines) {
 
        int cartes_size = cartes.size();
 
-       gnuplot = popen("gnuplot", "w");
-       FILE *p1, *p2;
-       p1 = fopen("cartes1", "w");
-       p2 = fopen("cartes2", "w");
-       int i, end, start = 0;
-       int set = 0;
+       int end, start = 0;
        while (start < cartes_size) {
                end = start + 1;
 
@@ -251,66 +246,54 @@ void shape_detect(const vector<int> &input_data, vector<Line> &lines) {
 
                end--;
 
-               for (i = start; i <= end; i++) {
-                       fprintf(set == 0 ? p1 : p2, "%g %g\n", cartes[i].x, cartes[i].y);
-               }
-               fflush(p1);
-               fflush(p2);
-               set = 1 - set; 
-       
                line_fitting(start, end, cartes, lines);
                start = end + 1;
        }
-       fclose(p1);
-       fclose(p2);
-
-       
        fprintf(gnuplot, "set grid\n");
        fprintf(gnuplot, "set nokey\n");
        fprintf(gnuplot, "set style line 1 lt 2 lc rgb \"red\" lw 3\n");
-       fprintf(gnuplot, "plot 'cartes1' with points ls 2, 'cartes2' with points ls 3");
+       fprintf(gnuplot, "plot [-1000:+3000] [-3000:+3000] '-' with points ls 2");
 
        for (int i = 0; i < (int) lines.size(); i++) {
                fprintf(gnuplot, ", \"< echo \'%f %f \\n %f %f\'\" ls 1 with linespoints",lines[i].a.x, lines[i].a.y, lines[i].b.x, lines[i].b.y);
        }
-
        fprintf(gnuplot, "\n");
-       fflush(gnuplot);
 
-       pclose(gnuplot);
+       for (int i = 0; i < (int) cartes.size(); i++) {
+               fprintf(gnuplot, "%g %g\n",cartes[i].x, cartes[i].y);
+       }
+
+       fprintf(gnuplot, "e\n");
+       fflush(gnuplot);
 }
 
-//-----------------------------------------------
-/*
 struct robottype_orte_data orte;
 
 void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
                        void *recvCallBackParam)
 {
        struct hokuyo_scan_type *instance = (struct hokuyo_scan_type *)vinstance;
-       struct hokuyo_scan_type scan;
+       static int count = 0;
 
        switch (info->status) {
                case NEW_DATA: {
-                       scan = *instance;
-
+                       printf("Scan\n");
+                       if (++count >= 4) {
+                               printf("Detect\n");
+                               count = 0;
                        
-                       vector<int> input(velikost);
+                               vector<int> input(HOKUYO_ARRAY_SIZE);
 
-                       for(int i = 0; i < velikost; i++)
-                               input[i] = (int) scan.data[i];
+                               for(unsigned i = 0; i < HOKUYO_ARRAY_SIZE; i++)
+                                       input[i] = (int) instance->data[HOKUYO_ARRAY_SIZE-1-i];
 
-                       
-
-                       vector<Line> output;
-                       shape_detect(input, output);
-                       
+                               vector<Line> output;
+                               shape_detect(input, output);
+                       }
                        break;
                }
                case DEADLINE:
-                       robot.status[COMPONENT_HOKUYO] = STATUS_FAILED;
-                       //system("killall -9 hokuyo");
-                       //DBG("%s: ORTE deadline occurred\n", __FUNCTION__);
+                       printf("Deadline\n");
                        break;
        }
 }
@@ -318,7 +301,6 @@ void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
 int robot_init_orte()
 {
        int rv = 0;
-       robot.orte.strength = 20;
 
        rv = robottype_roboorte_init(&orte);
        if (rv) return rv;
@@ -326,10 +308,8 @@ int robot_init_orte()
        robottype_subscriber_hokuyo_scan_create(&orte, rcv_hokuyo_scan_cb, &orte);
        return rv;
 }
-*/
-//-----------------------------------------------
 
-// MAIN
+#if 1
 int main(int argc, char** argv) {
   
        if (argc < 2) {
@@ -369,4 +349,13 @@ int main(int argc, char** argv) {
        
        return 0;
 }
-
+#else
+int main()
+{
+       gnuplot = popen("gnuplot", "w");
+       robot_init_orte();
+       getchar();
+       pclose(gnuplot);
+       return 0;
+}
+#endif