viewer_sdl.c
Go to the documentation of this file.
00001 
00010 #include "urg_sensor.h"
00011 #include "urg_utils.h"
00012 #include "urg_connection.h"
00013 #include "plotter_sdl.h"
00014 #include <SDL.h>
00015 #include <math.h>
00016 
00017 
00018 #if defined(URG_WINDOWS_OS)
00019 static const char *serial_device = "COM4";
00020 #else
00021 static const char *serial_device = "/dev/ttyACM0";
00022 #endif
00023 static const char *ethernet_address = "192.168.0.10";
00024 //static const char *ethernet_address = "localhost";
00025 
00026 
00027 typedef struct
00028 {
00029     urg_connection_type_t connection_type;
00030     const char *device;
00031     long baudrate_or_port;
00032     urg_measurement_type_t measurement_type;
00033     bool is_intensity;
00034     bool is_multiecho;
00035 } scan_mode_t;
00036 
00037 
00038 static void help_exit(const char *program_name)
00039 {
00040     printf("URG simple data viewer\n"
00041            "usage:\n"
00042            "    %s [options]\n"
00043            "\n"
00044            "options:\n"
00045            "  -h, --help    display this help and exit\n"
00046            "  -i,           intensity mode\n"
00047            "  -m,           multiecho mode\n"
00048            "\n",
00049            program_name);
00050 }
00051 
00052 
00053 static void parse_args(scan_mode_t *mode, int argc, char *argv[])
00054 {
00055     int i;
00056 
00057     mode->connection_type = URG_SERIAL;
00058     mode->device = serial_device;
00059     mode->baudrate_or_port = 115200;
00060     mode->is_multiecho = false;
00061     mode->is_intensity = false;
00062 
00063     for (i = 1; i < argc; ++i) {
00064         const char *token = argv[i];
00065 
00066         if (!strcmp(token, "-h") || !strcmp(token, "--help")) {
00067             help_exit(argv[0]);
00068 
00069         } else if (!strcmp(token, "-e")) {
00070             mode->connection_type = URG_ETHERNET;
00071             mode->device = ethernet_address;
00072             mode->baudrate_or_port = 10940;
00073 
00074         } else if (!strcmp(token, "-m")) {
00075             mode->is_multiecho = true;
00076         } else if (!strcmp(token, "-i")) {
00077             mode->is_intensity = true;
00078         }
00079     }
00080 
00081     if (mode->is_multiecho) {
00082         mode->measurement_type =
00083             (mode->is_intensity) ? URG_MULTIECHO_INTENSITY : URG_MULTIECHO;
00084     } else {
00085         mode->measurement_type =
00086             (mode->is_intensity) ? URG_DISTANCE_INTENSITY : URG_DISTANCE;
00087     }
00088 }
00089 
00090 
00091 static void plot_data_point(urg_t *urg, long data[], unsigned short intensity[],
00092                             int data_n, bool is_multiecho, int offset)
00093 {
00094     long min_distance;
00095     long max_distance;
00096     const double radian_offset = M_PI / 2.0;
00097     int step = (is_multiecho) ? 3 : 1;
00098     int i;
00099 
00100     urg_distance_min_max(urg, &min_distance, &max_distance);
00101 
00102     for (i = 0; i < data_n; ++i) {
00103         int index = (step * i) + offset;
00104         long l = (data) ? data[index] : intensity[index];
00105         double rad;
00106         float x;
00107         float y;
00108 
00109         if ((l <= min_distance) || (l >= max_distance)) {
00110             continue;
00111         }
00112 
00113         rad = urg_index2rad(urg, i) + radian_offset;
00114         x = l * cos(rad);
00115         y = l * sin(rad);
00116         plotter_plot(x, y);
00117     }
00118 }
00119 
00120 
00121 static void plot_data(urg_t *urg,
00122                       long data[], unsigned short intensity[], int data_n,
00123                       bool is_multiecho)
00124 {
00125     plotter_clear();
00126 
00127     // 霍晞屬
00128     plotter_set_color(0x00, 0xff, 0xff);
00129     plot_data_point(urg, data, NULL, data_n, is_multiecho, 0);
00130 
00131     if (is_multiecho) {
00132         plotter_set_color(0xff, 0x00, 0xff);
00133         plot_data_point(urg, data, NULL, data_n, is_multiecho, 1);
00134 
00135         plotter_set_color(0x00, 0x00, 0xff);
00136         plot_data_point(urg, data, NULL, data_n, is_multiecho, 2);
00137     }
00138 
00139     if (intensity) {
00140         // 蠑キ蠎ヲ
00141         plotter_set_color(0xff, 0xff, 0x00);
00142         plot_data_point(urg, NULL, intensity, data_n, is_multiecho, 0);
00143 
00144         if (is_multiecho) {
00145             plotter_set_color(0xff, 0x00, 0x00);
00146             plot_data_point(urg, NULL, intensity, data_n, is_multiecho, 1);
00147 
00148             plotter_set_color(0x00, 0xff, 0x00);
00149             plot_data_point(urg, NULL, intensity, data_n, is_multiecho, 2);
00150         }
00151     }
00152 
00153     plotter_swap();
00154 }
00155 
00156 
00157 int main(int argc, char *argv[])
00158 {
00159     scan_mode_t mode;
00160     urg_t urg;
00161     long *data = NULL;
00162     unsigned short *intensity = NULL;
00163     //long previous_timestamp = 0;
00164     long timestamp;
00165     int data_size;
00166 
00167 
00168     // 蠑墓焚縺ョ隗」譫
00169     parse_args(&mode, argc, argv);
00170 
00171     // URG 縺ォ謗・邯
00172     if (urg_open(&urg, mode.connection_type,
00173                  mode.device, mode.baudrate_or_port)) {
00174         printf("urg_open: %s\n", urg_error(&urg));
00175         return 1;
00176     }
00177 
00178     // 繝繝シ繧ソ蜿門セ励ョ貅門y
00179     data_size = urg_max_data_size(&urg);
00180     if (mode.is_multiecho) {
00181         data_size *= 3;
00182     }
00183     data = malloc(data_size * sizeof(data[0]));
00184     if (mode.is_intensity) {
00185         intensity = malloc(data_size * sizeof(intensity[0]));
00186     }
00187 
00188     // 逕サ髱「縺ョ菴懈
00189     if (!plotter_initialize(data_size * ((mode.is_intensity) ? 2 : 1))) {
00190         return 1;
00191     }
00192 
00193     // 繝繝シ繧ソ縺ョ蜿門セ励→謠冗判
00194     urg_start_measurement(&urg, mode.measurement_type, URG_SCAN_INFINITY, 0);
00195     while (1) {
00196         int n;
00197         //urg_start_measurement(&urg, mode.measurement_type, 1, 0);
00198         switch (mode.measurement_type) {
00199         case URG_DISTANCE:
00200             n = urg_get_distance(&urg, data, &timestamp);
00201             break;
00202 
00203         case URG_DISTANCE_INTENSITY:
00204             n = urg_get_distance_intensity(&urg, data, intensity, &timestamp);
00205             break;
00206 
00207         case URG_MULTIECHO:
00208             n = urg_get_multiecho(&urg, data, &timestamp);
00209             break;
00210 
00211         case URG_MULTIECHO_INTENSITY:
00212             n = urg_get_multiecho_intensity(&urg, data, intensity, &timestamp);
00213             break;
00214 
00215         default:
00216             n = 0;
00217             break;
00218         }
00219 
00220         if (n <= 0) {
00221             printf("urg_get_function: %s\n", urg_error(&urg));
00222             break;
00223         }
00224 
00225         //fprintf(stderr, "%ld, ", timestamp - previous_timestamp);
00226         //previous_timestamp = timestamp;
00227 
00228         plot_data(&urg, data, intensity, n, mode.is_multiecho);
00229         if (plotter_is_quit()) {
00230             break;
00231         }
00232     }
00233 
00234     // 繝ェ繧ス繝シ繧ケ縺ョ隗」謾セ
00235     plotter_terminate();
00236     free(intensity);
00237     free(data);
00238     urg_close(&urg);
00239 
00240     return 0;
00241 }


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