From ccb9d3b5032ff6bebe6365aa8351fc5fde04ef9b Mon Sep 17 00:00:00 2001
From: Michal Sojka <sojkam1@fel.cvut.cz>
Date: Wed, 15 Dec 2010 11:28:44 +0100
Subject: [PATCH] shape-detect: Works with real hokuyo

---
 src/hokuyo/shape-detect/Makefile.omk |  2 +-
 src/hokuyo/shape-detect/main.cc      | 73 ++++++++++++----------------
 2 files changed, 32 insertions(+), 43 deletions(-)

diff --git a/src/hokuyo/shape-detect/Makefile.omk b/src/hokuyo/shape-detect/Makefile.omk
index e6b9bdd1..b65e39b7 100644
--- 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 8f9201a7..04c3dc09 100644
--- 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
-- 
2.39.2