src/hokuyo/hokuyo-dump.c

   1 #include <stdio.h>
   2 #include <stdlib.h>
   3 #include <string.h>
   4 #include "hokuyo.h"
   5 #include <robottype.h>
   6 #include <roboorte_robottype.h>
   7 #include <unistd.h>
   8 #include <time.h>
   9 #include <semaphore.h>
  10
  11 sem_t data_printed;
  12 struct robottype_orte_data orte;
  13
  14 void rcv_hokuyo_scan_cb(const ORTERecvInfo *info, void *vinstance,
  15                         void *recvCallBackParam)
  16 {
  17         struct hokuyo_scan_type *instance = (struct hokuyo_scan_type *)vinstance;
  18
  19         switch (info->status) {
  20                 case NEW_DATA: {
  21                         int i;
  22                         for (i = 0; i < HOKUYO_ARRAY_SIZE; i++)
  23                                 printf("%d\n", instance->data[i]);
  24                         sem_post(&data_printed);
  25                         break;
  26                 }
  27                 case DEADLINE:
  28                         fprintf(stderr, "%s: ORTE deadline occurred\n", __FUNCTION__);
  29                         break;
  30         }
  31 }
  32
  33
  34
  35 int main(int argc, char **argv)
  36 {
  37   int ret;
  38   sem_init(&data_printed, 0, 0);
  39
  40   ret = robottype_roboorte_init(&orte);
  41   if (ret < 0) {
  42           fprintf(stderr, "robottype_roboorte_init failed\n");
  43           return ret;
  44   }
  45   robottype_subscriber_hokuyo_scan_create(&orte, rcv_hokuyo_scan_cb, &orte);
  46
  47   sem_wait(&data_printed);
  48
  49   robottype_roboorte_destroy(&orte);
  50
  51   return 0;
  52 }