src/hokuyo/hokuyo.c

   1 /*!
   2    @File
   3    @Brief: This is a demo program to obtain 10 scan data from URG-Series and save them into the file.
   4    @Note : This sample source is a referece for the user when creating his/her program and does not gurantee for users program
   5    @Note : Hokuyo Automatic Co., Ltd does not bear responsibility for any loss or damage caused due to this demo program.
   6    @Note : Mention the publisher's name when distributing this source or its part to the third party
   7    @Publisher HOKUYO AUTOMATIC CO.,LTD.
   8    @author Tomoaki Yoshida
   9    @Date   2005/03/11
  10  */
  11 #include<stdio.h>
  12 #include<stdlib.h>
  13 #include<sys/termios.h>
  14 #include <string.h>
  15 #include "hokuyo.h"
  16 #include <urg_ctrl.h>
  17 #include <robottype.h>
  18 #include <roboorte_robottype.h>
  19 #include <unistd.h>
  20 #include <time.h>
  21 #include <signal.h>
  22 #include <error.h>
  23 #include <errno.h>
  24
  25 #define HOKUYO_DEVICE "/dev/ttyACM0"    // /dev/ttyACM0
  26
  27 #define COMPILE_TIME_ASSERT(cond, msg) \
  28         typedef char msg[(cond) ? 1 : -1]
  29
  30 COMPILE_TIME_ASSERT(HOKUYO_FINAL_MEASUREMENT - HOKUYO_INITIAL_MEASUREMENT == HOKUYO_ARRAY_SIZE,
  31                     wrong_size_of_hokuyo_scan_type);
  32
  33 static urg_t urg;
  34 static long *scan;
  35 static int scan_max;
  36 int interrupt = 0;
  37
  38 struct robottype_orte_data orte;
  39
  40 int hokuyo_init(char *device)
  41 {
  42         int ret;
  43         ret = urg_connect(&urg, device, 115200);
  44         if (ret < 0) {
  45           error(0, errno, "hokuyo: urg_connect(%s) failed: %s errno",
  46                 device, urg_error(&urg));
  47           urg_disconnect(&urg);
  48           return -1;
  49         }
  50
  51         /* Reserve for reception data */
  52         scan_max = urg_dataMax(&urg);
  53         scan = (long*)malloc(sizeof(long) * scan_max);
  54         if (scan == NULL) {
  55                 fprintf(stderr, "hokuyo: get_scan - malloc failed!\n");
  56                 return -1;
  57         }
  58
  59         return 0;
  60 }
  61
  62 static inline void next_period(struct timespec *next, long long interval_ns)
  63 {
  64         next->tv_nsec += interval_ns;
  65         if (next->tv_nsec >= 1000000000) {
  66                 next->tv_sec++;
  67                 next->tv_nsec -= 1000000000;
  68         }
  69 }
  70
  71 static void sig_handler(int sig)
  72 {
  73         interrupt = 1;
  74 }
  75
  76
  77 int main(int argc, char **argv)
  78 {
  79   int ret;
  80   urg_parameter_t parameter;
  81   struct timespec next;
  82
  83   signal(SIGINT, sig_handler);
  84   signal(SIGTERM, sig_handler);
  85
  86   ret = hokuyo_init(HOKUYO_DEVICE);
  87   if (ret < 0)
  88           return ret;
  89
  90   printf("Detected model: %s\n", urg_model(&urg));
  91   printf("Distance min %ld mm, max %ld mm\n", urg_minDistance(&urg),
  92          urg_maxDistance(&urg));
  93   urg_parameters(&urg, &parameter);
  94
  95   ret = robottype_roboorte_init(&orte);
  96   if (ret < 0) {
  97           fprintf(stderr, "robottype_roboorte_init failed\n");
  98 #ifdef COMPETITION
  99           /* Sleep in order to that not be respawn too fast by init */
 100           sleep(3);
 101 #endif
 102           return ret;
 103   }
 104   robottype_publisher_hokuyo_scan_create(&orte, NULL, NULL);
 105
 106   /* Request for continuous scanning data */
 107   urg_setCaptureTimes(&urg, UrgInfinityTimes);
 108   ret = urg_requestData(&urg, URG_MD, URG_FIRST, URG_LAST);
 109   if (ret < 0) {
 110           fprintf(stderr, "hokuyo: get_scan - request data failed!\n");
 111 #ifdef COMPETITION
 112           /* Sleep in order to that not be respawn too fast by init */
 113           sleep(3);
 114 #endif
 115           return -1;
 116   }
 117
 118   clock_gettime(CLOCK_MONOTONIC, &next);
 119
 120   while (!interrupt) {
 121           int i;
 122           int average, errors[19];
 123 //        double ang;
 124           memset(errors, 0, sizeof(errors));
 125
 126           clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME,
 127                           &next, NULL);
 128           next_period(&next, 100/*ms*/*1000*1000);
 129
 130           /* Receive actual data */
 131           ret = urg_receiveData(&urg, scan, scan_max);
 132           if (ret < 0) {
 133                   fprintf(stderr, "urg_receiveData failed\n");
 134                   break;
 135           }
 136 /*        for(i=0; i < scan_max ; i++) */
 137 /*                printf("[%d %ld] ", i, scan[i]); */
 138 /*        printf("\n%d\n", scan_max); */
 139           average = 0;
 140           for(i=0; i < HOKUYO_ARRAY_SIZE; i++) {
 141                   int idx = i + HOKUYO_INITIAL_MEASUREMENT;
 142                   if (idx >= scan_max)
 143                           break;
 144                   // This is useful when you want to test which angle is scanning the robot inside
 145 //                ang = HOKUYO_INDEX_TO_RAD(i);
 146 //                if((ang<(-80.0/180.0*M_PI))||((ang>(75.0/180.0*M_PI))))
 147 //                {
 148 //                      orte.hokuyo_scan.data[i] = 0;
 149 //                } else {
 150 //                        orte.hokuyo_scan.data[i] = scan[idx];
 151 //                }
 152                   orte.hokuyo_scan.data[i] = scan[idx];
 153                   if (scan[idx] > 19)
 154                           average += scan[idx];
 155                   else if (scan[idx] > 0)
 156                           errors[scan[idx]]++;
 157           }
 158           average /= HOKUYO_ARRAY_SIZE;
 159           printf("Scan average: %d mm", average);
 160           for (i = 0; i < 19; i++)
 161                   if (errors[i])
 162                           printf(", error %d: %dx", i, errors[i]);
 163           printf("\n");
 164           ORTEPublicationSend(orte.publication_hokuyo_scan);
 165   }
 166
 167   printf("Swithing off laser\n");
 168   urg_disconnect(&urg);
 169
 170   robottype_roboorte_destroy(&orte);
 171
 172   return 0;
 173 }