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
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
00164 long timestamp;
00165 int data_size;
00166
00167
00168
00169 parse_args(&mode, argc, argv);
00170
00171
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
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
00198 switch (mode.measurement_type) {
00199 case URG_DISTANCE:
00200 n = urg_get_distance(&urg, data, ×tamp);
00201 break;
00202
00203 case URG_DISTANCE_INTENSITY:
00204 n = urg_get_distance_intensity(&urg, data, intensity, ×tamp);
00205 break;
00206
00207 case URG_MULTIECHO:
00208 n = urg_get_multiecho(&urg, data, ×tamp);
00209 break;
00210
00211 case URG_MULTIECHO_INTENSITY:
00212 n = urg_get_multiecho_intensity(&urg, data, intensity, ×tamp);
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
00226
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 }