get_multiecho_intensity.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[], unsigned short intensity[],
00018                             int index)
00019 {
00020     int i;
00021 
00022     // [mm]
00023     for (i = 0; i < URG_MAX_ECHO; ++i) {
00024         printf("%ld, ", data[(URG_MAX_ECHO * index) + i]);
00025     }
00026 
00027     // [1]
00028     for (i = 0; i < URG_MAX_ECHO; ++i) {
00029         printf("%d, ", intensity[(URG_MAX_ECHO * index) + i]);
00030     }
00031 }
00032 
00033 
00034 // \~japanese 距離、強度のデータを表示する
00035 static void print_data(urg_t *urg, long data[],
00036                        unsigned short intensity[], int data_n, long time_stamp)
00037 {
00038 #if 1
00039     int front_index;
00040 
00041     (void)data_n;
00042 
00043     // \~japanese 前方のデータのみを表示
00044     front_index = urg_step2index(urg, 0);
00045     print_echo_data(data, intensity, front_index);
00046     printf("%ld\n", time_stamp);
00047 
00048 #else
00049     (void)urg;
00050     int i;
00051 
00052     // \~japanese 全てのデータを表示
00053     printf("# n = %d, time_stamp = %ld\n", data_n, time_stamp);
00054     for (i = 0; i < data_n; ++i) {
00055         print_echo_data(data, intensity, i);
00056         printf("\n");
00057     }
00058 #endif
00059 }
00060 
00061 
00062 int main(int argc, char *argv[])
00063 {
00064     enum {
00065         CAPTURE_TIMES = 10,
00066     };
00067     urg_t urg;
00068     int max_data_size;
00069     long *data = NULL;
00070     unsigned short *intensity = NULL;
00071     long time_stamp;
00072     unsigned long long system_time_stamp;
00073     int n;
00074     int i;
00075 
00076     if (open_urg_sensor(&urg, argc, argv) < 0) {
00077         return 1;
00078     }
00079 
00080     max_data_size = urg_max_data_size(&urg);
00081     data = (long *)malloc(max_data_size * 3 * sizeof(data[0]));
00082     intensity = (unsigned short *)malloc(max_data_size * 3 *
00083                                          sizeof(intensity[0]));
00084 
00085     if (!data) {
00086         perror("urg_max_index()");
00087         return 1;
00088     }
00089 
00090     // \~japanese データ取得
00091     urg_start_measurement(&urg, URG_MULTIECHO_INTENSITY, CAPTURE_TIMES, 0);
00092     for (i = 0; i < CAPTURE_TIMES; ++i) {
00093         n = urg_get_multiecho_intensity(&urg, data, intensity, &time_stamp, &system_time_stamp);
00094         if (n <= 0) {
00095             printf("urg_get_multiecho_intensity: %s\n", urg_error(&urg));
00096             free(data);
00097             free(intensity);
00098             urg_close(&urg);
00099             return 1;
00100         }
00101         print_data(&urg, data, intensity, n, time_stamp);
00102     }
00103 
00104     // \~japanese 切断
00105     free(data);
00106     free(intensity);
00107     urg_close(&urg);
00108 
00109 #if defined(URG_MSC)
00110     getchar();
00111 #endif
00112     return 0;
00113 }


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