get_distance.c
Go to the documentation of this file.
00001 
00011 #include "urg_c/urg_sensor.h"
00012 #include "urg_c/urg_utils.h"
00013 #include "open_urg_sensor.h"
00014 #include <stdlib.h>
00015 #include <stdio.h>
00016 
00017 
00018 static void print_data(urg_t *urg, long data[], int data_n, long time_stamp)
00019 {
00020 #if 1
00021     int front_index;
00022 
00023     (void)data_n;
00024 
00025     // \~japanese 前方のデータのみを表示
00026     front_index = urg_step2index(urg, 0);
00027     printf("%ld [mm], (%ld [msec])\n", data[front_index], time_stamp);
00028 
00029 #else
00030     (void)time_stamp;
00031 
00032     int i;
00033     long min_distance;
00034     long max_distance;
00035 
00036     // \~japanese 全てのデータの X-Y の位置を表示
00037     urg_distance_min_max(urg, &min_distance, &max_distance);
00038     for (i = 0; i < data_n; ++i) {
00039         long l = data[i];
00040         double radian;
00041         long x;
00042         long y;
00043 
00044         if ((l <= min_distance) || (l >= max_distance)) {
00045             continue;
00046         }
00047         radian = urg_index2rad(urg, i);
00048         x = (long)(l * cos(radian));
00049         y = (long)(l * sin(radian));
00050         printf("(%ld, %ld), ", x, y);
00051     }
00052     printf("\n");
00053 #endif
00054 }
00055 
00056 
00057 int main(int argc, char *argv[])
00058 {
00059     enum {
00060         CAPTURE_TIMES = 10,
00061     };
00062     urg_t urg;
00063     long *data = NULL;
00064     long time_stamp;
00065     unsigned long long system_time_stamp;
00066     int n;
00067     int i;
00068 
00069     if (open_urg_sensor(&urg, argc, argv) < 0) {
00070         return 1;
00071     }
00072 
00073     data = (long *)malloc(urg_max_data_size(&urg) * sizeof(data[0]));
00074     if (!data) {
00075         perror("urg_max_index()");
00076         return 1;
00077     }
00078 
00079     // \~japanese データ取得
00080 #if 0
00081     // \~japanese データの取得範囲を変更する場合
00082     urg_set_scanning_parameter(&urg,
00083                                urg_deg2step(&urg, -90),
00084                                urg_deg2step(&urg, +90), 0);
00085 #endif
00086 
00087     urg_start_measurement(&urg, URG_DISTANCE, CAPTURE_TIMES, 0);
00088     for (i = 0; i < CAPTURE_TIMES; ++i) {
00089         n = urg_get_distance(&urg, data, &time_stamp, &system_time_stamp);
00090         if (n <= 0) {
00091             printf("urg_get_distance: %s\n", urg_error(&urg));
00092             free(data);
00093             urg_close(&urg);
00094             return 1;
00095         }
00096         print_data(&urg, data, n, time_stamp);
00097     }
00098 
00099     // \~japanese 切断
00100     free(data);
00101     urg_close(&urg);
00102 
00103 #if defined(URG_MSC)
00104     getchar();
00105 #endif
00106     return 0;
00107 }


urg_c
Author(s): Satofumi Kamimura , Katsumi Kimoto, Adrian Boeing
autogenerated on Thu Jun 6 2019 19:06:57