Commit d0cc0c22 authored by Jeremy Swerdlow's avatar Jeremy Swerdlow
Browse files

updates print_data and main for (hopefully) better data reception

parent 491c78c6
......@@ -16,52 +16,59 @@
#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) {
static int print_data(urg_t *urg, long data[], long time_stamp,
unsigned short intensity[], int data_n,
int num_rotations) {
(void)urg;
(void)time_stamp;
int i;
// populates min and max distance to filter results on
long min_distance;
long max_distance;
long time_stamp_offset;
long t;
urg_distance_min_max(urg, &min_distance, &max_distance);
// realtime timer
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);
// keeps track of which rotation a sample belongs to
num_rotations += 1;
// variables used within the for loop print
double radian;
long length;
long z;
long y;
// this for loop prints each sample from the data array
int i;
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)) {
length = data[i]
radian = urg_index2rad(urg, i)
// check to make sure the data is within an acceptable range for measurement
if ((length <= min_distance) || (length >= 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,", z, y, time_stamp, intensity[i], l, radian, k);
fprintf(stderr,"%02d.%02d.%02d.%03d\n", tm_info->tm_hour, tm_info->tm_min, tm_info->tm_sec, tp.tv_usec/1000);
// populate z and y distance from the sensor
z = (long)(length * cos(radian));
y = (long)(length * sin(radian));
// print out the results
fprintf(stderr, "%ld,%ld,%ld,%d,%ld,%f,%d,",
z, y, time_stamp, intensity[i], length, radian, num_rotations);
fprintf(stderr, "%02d.%02d.%02d.%03d\n", tm_info->tm_hour, tm_info->tm_min, tm_info->tm_sec, tp.tv_usec/1000);
}
return num_rotations;
}
int main(int argc, char *argv[]) {
urg_t urg;
long *data = NULL;
unsigned short *intensity = NULL;
long time_stamp;
long *data = NULL;
long set_angle = 90;
int i;
int n;
int num_rotations = 0;
// fprintf(stderr, "Waiting for Ethernet port to become available (30s)...");
// sleep(30);
......@@ -95,31 +102,22 @@ int main(int argc, char *argv[]) {
urg_deg2step(&urg, -1 * set_angle),
urg_deg2step(&urg, +1 * set_angle), 0);
*/
urg_set_scanning_parameter(&urg,
urg_deg2step(&urg, -135), // limiting to 45 degrees
urg_deg2step(&urg, 135), // on either side of bottom
1); // step, TODO: look at increasing this to reduce num samples
urg_start_measurement(&urg, URG_DISTANCE_INTENSITY, URG_SCAN_INFINITY, 0);
fprintf(stderr,"z,y,urg_time,i,pc,sensor,time\n");
while (1) {
fprintf(stderr, "z,y,time stamp from lidar,intensity,")
fprintf(stderr, "length,radian,time stamp from print_data\n", );
do {
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");
num_rotations = print_data(&urg, data, time_stamp, intensity, n, num_rotations);
} while (n > 0);
// 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