get_multiecho.c
Go to the documentation of this file.
00001 
00010 #include "urg_c/urg_sensor.h"
00011 #include "urg_c/urg_utils.h"
00012 #include "open_urg_sensor.h"
00013 #include <stdio.h>
00014 #include <stdlib.h>
00015 
00016 
00017 static void print_echo_data(long data[], int index)
00018 {
00019     int i;
00020 
00021     // [mm]
00022     for (i = 0; i < URG_MAX_ECHO; ++i) {
00023         printf("%ld, ", data[(URG_MAX_ECHO * index) + i]);
00024     }
00025 }
00026 
00027 
00028 static void print_data(urg_t *urg, long data[], int data_n, long time_stamp)
00029 {
00030 #if 1
00031     int front_index;
00032 
00033     (void)data_n;
00034 
00035     // \~japanese 前方のデータのみを表示
00036     front_index = urg_step2index(urg, 0);
00037     print_echo_data(data, front_index);
00038     printf("%ld\n", time_stamp);
00039 
00040 #else
00041     (void)urg;
00042 
00043     int i;
00044 
00045     // \~japanese 全てのデータを表示
00046     printf("# n = %d, time_stamp = %ld\n", data_n, time_stamp);
00047     for (i = 0; i < data_n; ++i) {
00048         print_echo_data(data, i);
00049         printf("\n");
00050     }
00051 #endif
00052 }
00053 
00054 
00055 int main(int argc, char *argv[])
00056 {
00057     enum {
00058         CAPTURE_TIMES = 10,
00059     };
00060     urg_t urg;
00061     long *data = NULL;
00062     long time_stamp;
00063     unsigned long long system_time_stamp;
00064     int n;
00065     int i;
00066 
00067     if (open_urg_sensor(&urg, argc, argv) < 0) {
00068         return 1;
00069     }
00070 
00071     data = (long *)malloc(urg_max_data_size(&urg) * 3 * sizeof(data[0]));
00072     if (!data) {
00073         perror("urg_max_index()");
00074         return 1;
00075     }
00076 
00077     // \~japanese データ取得
00078     urg_start_measurement(&urg, URG_MULTIECHO, CAPTURE_TIMES, 0);
00079     for (i = 0; i < CAPTURE_TIMES; ++i) {
00080         n = urg_get_multiecho(&urg, data, &time_stamp, &system_time_stamp);
00081         if (n <= 0) {
00082             printf("urg_get_multiecho: %s\n", urg_error(&urg));
00083             free(data);
00084             urg_close(&urg);
00085             return 1;
00086         }
00087         print_data(&urg, data, n, time_stamp);
00088     }
00089 
00090     // \~japanese 切断
00091     free(data);
00092     urg_close(&urg);
00093 
00094 #if defined(URG_MSC)
00095     getchar();
00096 #endif
00097     return 0;
00098 }


urg_c
Author(s): Satofumi Kamimura , Katsumi Kimoto, Adrian Boeing
autogenerated on Wed Aug 26 2015 16:38:27