src/lidars/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 <lidar_params.h>
  17 #include <urg_ctrl.h>
  18 #include <robottype.h>
  19 #include <roboorte_robottype.h>
  20 #include <unistd.h>
  21 #include <time.h>
  22 #include <signal.h>
  23 #include <error.h>
  24 #include <errno.h>
  25
  26 #define HOKUYO_DEVICE "/dev/ttyACM0"    // /dev/ttyACM0
  27
  28 #define COMPILE_TIME_ASSERT(cond, msg) \
  29         typedef char msg[(cond) ? 1 : -1]
  30
  31 COMPILE_TIME_ASSERT(HOKUYO_FINAL_MEASUREMENT - HOKUYO_INITIAL_MEASUREMENT == HOKUYO_ARRAY_SIZE,
  32                     wrong_size_of_lidar_scan_type);
  33
  34 static urg_t urg;
  35 static long *scan;
  36 static int scan_max;
  37 int interrupt = 0;
  38
  39 struct robottype_orte_data orte;
  40
  41 int hokuyo_init(char *device)
  42 {
  43         int ret;
  44         ret = urg_connect(&urg, device, 115200);
  45         if (ret < 0) {
  46           error(0, errno, "hokuyo: urg_connect(%s) failed: %s errno",
  47                 device, urg_error(&urg));
  48           urg_disconnect(&urg);
  49           return -1;
  50         }
  51
  52         /* Reserve for reception data */
  53         scan_max = urg_dataMax(&urg);
  54         scan = (long*)malloc(sizeof(long) * scan_max);
  55         if (scan == NULL) {
  56                 fprintf(stderr, "hokuyo: get_scan - malloc failed!\n");
  57                 return -1;
  58         }
  59
  60         return 0;
  61 }
  62
  63 static inline void next_period(struct timespec *next, long long interval_ns)
  64 {
  65         next->tv_nsec += interval_ns;
  66         if (next->tv_nsec >= 1000000000) {
  67                 next->tv_sec++;
  68                 next->tv_nsec -= 1000000000;
  69         }
  70 }
  71
  72 static void sig_handler(int sig)
  73 {
  74         interrupt = 1;
  75 }
  76
  77
  78 int main(int argc, char **argv)
  79 {
  80   int ret;
  81   urg_parameter_t parameter;
  82   struct timespec next;
  83
  84   signal(SIGINT, sig_handler);
  85   signal(SIGTERM, sig_handler);
  86
  87   ret = hokuyo_init(HOKUYO_DEVICE);
  88   if (ret < 0)
  89           return ret;
  90
  91   printf("Detected model: %s\n", urg_model(&urg));
  92   printf("Distance min %ld mm, max %ld mm\n", urg_minDistance(&urg),
  93          urg_maxDistance(&urg));
  94   urg_parameters(&urg, &parameter);
  95
  96   ret = robottype_roboorte_init(&orte);
  97   if (ret < 0) {
  98           fprintf(stderr, "robottype_roboorte_init failed\n");
  99 #ifdef COMPETITION
 100           /* Sleep in order to that not be respawn too fast by init */
 101           sleep(3);
 102 #endif
 103           return ret;
 104   }
 105   robottype_publisher_hokuyo_scan_create(&orte, NULL, NULL);
 106
 107   /* Request for continuous scanning data */
 108   urg_setCaptureTimes(&urg, UrgInfinityTimes);
 109   ret = urg_requestData(&urg, URG_MD, URG_FIRST, URG_LAST);
 110   if (ret < 0) {
 111           fprintf(stderr, "hokuyo: get_scan - request data failed!\n");
 112 #ifdef COMPETITION
 113           /* Sleep in order to that not be respawn too fast by init */
 114           sleep(3);
 115 #endif
 116           return -1;
 117   }
 118
 119   clock_gettime(CLOCK_MONOTONIC, &next);
 120
 121   while (!interrupt) {
 122           int i;
 123           int average, errors[19];
 124 //        double ang;
 125           memset(errors, 0, sizeof(errors));
 126
 127           clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME,
 128                           &next, NULL);
 129           next_period(&next, 100/*ms*/*1000*1000);
 130
 131           /* Receive actual data */
 132           ret = urg_receiveData(&urg, scan, scan_max);
 133           if (ret < 0) {
 134                   fprintf(stderr, "urg_receiveData failed\n");
 135                   break;
 136           }
 137 /*        for(i=0; i < scan_max ; i++) */
 138 /*                printf("[%d %ld] ", i, scan[i]); */
 139 /*        printf("\n%d\n", scan_max); */
 140           average = 0;
 141           for(i=0; i < HOKUYO_ARRAY_SIZE; i++) {
 142                   int idx = i + HOKUYO_INITIAL_MEASUREMENT;
 143                   if (idx >= scan_max)
 144                           break;
 145                   // This is useful when you want to test which angle is scanning the robot inside
 146 //                ang = HOKUYO_INDEX_TO_RAD(i);
 147 //                if((ang<(-80.0/180.0*M_PI))||((ang>(75.0/180.0*M_PI))))
 148 //                {
 149 //                      orte.hokuyo_scan.data[i] = 0;
 150 //                } else {
 151 //                        orte.hokuyo_scan.data[i] = scan[idx];
 152 //                }
 153                   orte.hokuyo_scan.data[i] = scan[idx];
 154                   if (scan[idx] > 19)
 155                           average += scan[idx];
 156                   else if (scan[idx] > 0)
 157                           errors[scan[idx]]++;
 158           }
 159           average /= HOKUYO_ARRAY_SIZE;
 160           printf("Scan average: %d mm", average);
 161           for (i = 0; i < 19; i++)
 162                   if (errors[i])
 163                           printf(", error %d: %dx", i, errors[i]);
 164           printf("\n");
 165
 166           orte.hokuyo_scan.data_lenght = hokuyo_params.data_lenght;
 167           orte.hokuyo_scan.lidar_type = hokuyo_params.type;
 168           ORTEPublicationSend(orte.publication_hokuyo_scan);
 169   }
 170
 171   printf("Swithing off laser\n");
 172   urg_disconnect(&urg);
 173
 174   robottype_roboorte_destroy(&orte);
 175
 176   return 0;
 177 }