hokuyo: Switch off laser Ctrl-C

diff --git a/src/hokuyo/hokuyo.c b/src/hokuyo/hokuyo.c
index 8a3cad761c849e70912c6e7cbec315c193d3a0f5..ea868a9a612aec25ee873d0c96697961c89c413f 100644 (file)
--- a/src/hokuyo/hokuyo.c
+++ b/src/hokuyo/hokuyo.c
@@ -18,7 +18,7 @@
 #include <roboorte_robottype.h>
 #include <unistd.h>
 #include <time.h>
-
+#include <signal.h>
 #define HOKUYO_DEVICE "/dev/ttyACM0"   // /dev/ttyACM0
 
 #define COMPILE_TIME_ASSERT(cond, msg) \
@@ -30,6 +30,7 @@ COMPILE_TIME_ASSERT(HOKUYO_FINAL_MEASUREMENT - HOKUYO_INITIAL_MEASUREMENT == HOK
 static urg_t urg;
 static long *scan;
 static int scan_max;
+int interrupt = 0;
 
 struct robottype_orte_data orte;
 
@@ -63,12 +64,21 @@ static inline void next_period(struct timespec *next, long long interval_ns)
        }
 }
 
+static void sig_handler(int sig)
+{
+       interrupt = 1;
+}
+
+
 int main(int argc, char **argv)
 {
   int ret;
   urg_parameter_t parameter;
   struct timespec next;
 
+  signal(SIGINT, sig_handler);
+  signal(SIGTERM, sig_handler);
+  
   ret = hokuyo_init(HOKUYO_DEVICE);
   if (ret < 0)
          return ret;
@@ -95,7 +105,7 @@ int main(int argc, char **argv)
 
   clock_gettime(CLOCK_MONOTONIC, &next);
        
-  while (1) {
+  while (!interrupt) {
          int i;
          int average = 0, errors[19];
          memset(errors, 0, sizeof(errors));
@@ -129,6 +139,7 @@ int main(int argc, char **argv)
          ORTEPublicationSend(orte.publication_hokuyo_scan);
   }
 
+  printf("Swithing off laser\n");
   urg_disconnect(&urg);
 
   robottype_roboorte_destroy(&orte);