calculate_xy.c
Go to the documentation of this file.
00001 
00012 #include "urg_c/urg_sensor.h"
00013 #include "urg_c/urg_utils.h"
00014 #include "open_urg_sensor.h"
00015 #include <math.h>
00016 #include <stdio.h>
00017 #include <stdlib.h>
00018 
00019 
00020 int main(int argc, char *argv[])
00021 {
00022     urg_t urg;
00023     long *data;
00024     long max_distance;
00025     long min_distance;
00026     long time_stamp;
00027     unsigned long long system_time_stamp;
00028     int i;
00029     int n;
00030 
00031     if (open_urg_sensor(&urg, argc, argv) < 0) {
00032         return 1;
00033     }
00034 
00035     data = (long *)malloc(urg_max_data_size(&urg) * sizeof(data[0]));
00036     if (!data) {
00037         perror("urg_max_index()");
00038         return 1;
00039     }
00040 
00041     // \~japanese データ取得
00042     urg_start_measurement(&urg, URG_DISTANCE, 1, 0);
00043     n = urg_get_distance(&urg, data, &time_stamp, &system_time_stamp);
00044     if (n < 0) {
00045         printf("urg_get_distance: %s\n", urg_error(&urg));
00046         urg_close(&urg);
00047         return 1;
00048     }
00049 
00050     // \~japanese X-Y 座標系の値を出力
00051     urg_distance_min_max(&urg, &min_distance, &max_distance);
00052     for (i = 0; i < n; ++i) {
00053         long distance = data[i];
00054         double radian;
00055         long x;
00056         long y;
00057 
00058         if ((distance < min_distance) || (distance > max_distance)) {
00059             continue;
00060         }
00061 
00062         radian = urg_index2rad(&urg, i);
00063         x = (long)(distance * cos(radian));
00064         y = (long)(distance * sin(radian));
00065 
00066         printf("%ld, %ld\n", x, y);
00067     }
00068     printf("\n");
00069 
00070     // \~japanese 切断
00071     free(data);
00072     urg_close(&urg);
00073 
00074 #if defined(URG_MSC)
00075     getchar();
00076 #endif
00077     return 0;
00078 }


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