Commit da38c691 authored by --replace-all's avatar --replace-all
Browse files

starting point for collecting and timestamping lidar readings from the URG

parent 4d0d0c28
// collect-urg-lidar.c
// This started-out as get_distance.c from the URG examples. It was lightly
// modified by Nic and Charlie and used in 2017. We're about to re-think
// the approach illustrated here and adapt it to our specific context.
// to run use ./collect-urg-lidar -e
#include <stdlib.h>
#include <sys/select.h>
#include <math.h>
#include <stdio.h>
#include <sys/time.h>
#include <time.h>
#include "urg_sensor.h"
#include "urg_utils.h"
#include "open_urg_sensor.h"
static void print_data(urg_t *urg, long data[], long time_stamp,
unsigned short intensity[], int data_n) {
(void)urg;
(void)time_stamp;
int i;
long min_distance;
long max_distance;
long time_stamp_offset;
long t;
// realtime timer
char buffer[26];
struct timeval tp;
gettimeofday(&tp, 0);
time_t timer = tp.tv_sec;
struct tm *tm_info = localtime(&timer);
// Prints the X-Y coordinates for all the measurement points
urg_distance_min_max(urg, &min_distance, &max_distance);
for (i = 0; i < data_n; ++i) {
long l = data[i];
double radian;
long z;
long y;
float k; // count of rotations
if (radian == 0) {
k++;
}
if ((l <= min_distance) || (l >= max_distance)) {
continue;
}
radian = urg_index2rad(urg, i);
z = (long)(l * cos(radian));
y = (long)(l * sin(radian));
fprintf(stderr,"%ld,%ld,%ld,%d,%ld,%f,%f,%s", z, y, time_stamp, intensity[i], l, radian, k, buffer);
fprintf(stderr,"%02d.%02d.%02d.%03d\n", tm_info->tm_hour, tm_info->tm_min, tm_info->tm_sec, tp.tv_usec/1000);
}
fprintf(stderr,"\n");
//#endif
}
int main(int argc, char *argv[]) {
urg_t urg;
unsigned short *intensity = NULL;
long time_stamp;
long *data = NULL;
long set_angle = 90;
int i;
int n;
// fprintf(stderr, "Waiting for Ethernet port to become available (30s)...");
// sleep(30);
if (open_urg_sensor(&urg, argc, argv) < 0) {
return 1;
}
data = (long *)malloc(urg_max_data_size(&urg) * sizeof(data[0]));
if (!data) {
perror("urg_max_index()");
return 1;
}
intensity = malloc(urg_max_data_size(&urg) * sizeof(intensity[0]));
if (!intensity) {
perror("urg_max_index()");
return 1;
}
// Gets measurement data
// xxx consider enabling onboard clipping to reduce the payload size (at no
// loss to the analysis)
// Case where the measurement range (start/end steps) is defined
//set_angle = strtol(argv[2], NULL, 3);
//if (argc < 2) printf("Set print angle. ex: 90 = +90deg,-90deg view \n");
/*
urg_set_scanning_parameter(&urg,
urg_deg2step(&urg, -1 * set_angle),
urg_deg2step(&urg, +1 * set_angle), 0);
*/
urg_start_measurement(&urg, URG_DISTANCE_INTENSITY, URG_SCAN_INFINITY, 0);
fprintf(stderr,"z,y,urg_time,i,pc,sensor,time\n");
while (1) {
n = urg_get_distance_intensity(&urg, data, intensity, &time_stamp);
if (n <= 0) {
printf("urg_get_distance: %s\n", urg_error(&urg));
free(data);
urg_close(&urg);
return 1;
}
print_data(&urg, data, time_stamp, intensity, n);
}
//printf("Process Stopped.\n");
// Disconnects
free(data);
free(intensity);
urg_close(&urg);
#if defined(URG_MSC)
getchar();
#endif
return 0;
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment