Commit 5759ff6f authored by Jeremy Swerdlow's avatar Jeremy Swerdlow
Browse files

tries to update collect-urg-lidar.c to add specific time stamps

parent 134031bb
......@@ -18,7 +18,7 @@
static int print_data(urg_t *urg, long data[], long time_stamp,
unsigned short intensity[], int data_n,
int num_rotations) {
int num_rotations, int offset) {
(void)urg;
(void)time_stamp;
......@@ -43,10 +43,11 @@ static int print_data(urg_t *urg, long data[], long time_stamp,
long y;
// this for loop prints each sample from the data array
int i;
for (i = 0; i < data_n; ++i) {
length = data[i];
radian = urg_index2rad(urg, i);
int sample_num;
double sample_time;
for (sample_num = 0; sample_num < data_n; ++sample_num) {
length = data[sample_num];
radian = urg_index2rad(urg, sample_num);
// check to make sure the data is within an acceptable range for measurement
if ((length <= min_distance) || (length >= max_distance)) {
continue;
......@@ -54,10 +55,13 @@ static int print_data(urg_t *urg, long data[], long time_stamp,
// populate z and y distance from the sensor
z = (long)(length * cos(radian));
y = (long)(length * sin(radian));
// calculate the proper split_time value
sample_time = 25 / 1081 * (offset + sample_num); // 25 ms / 1081 samples per scan * which sample it is.
// print out the results
fprintf(stderr, "%ld,%ld,%ld,%d,%ld,%f,%d,",
z, y, time_stamp, intensity[i], length, radian, num_rotations, i);
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, "%ld,%ld,%ld,%d,%ld,%f,%d,%d,",
z, y, time_stamp, intensity[sample_num], length, radian, num_rotations, sample_num);
fprintf(stderr, "%02d.%02d.%02d.%03f\n", tm_info->tm_hour, tm_info->tm_min,
tm_info->tm_sec, (double)tp.tv_usec/1000 + sample_time);
}
return num_rotations;
}
......@@ -102,20 +106,24 @@ int main(int argc, char *argv[]) {
urg_deg2step(&urg, -1 * set_angle),
urg_deg2step(&urg, +1 * set_angle), 0);
*/
int deg = 45;
float cutoff = (135 - deg) / 270;
int offset = cutoff * 1080;
if(urg_set_scanning_parameter(&urg,
urg_deg2step(&urg, -45), // limiting to 45 degrees
urg_deg2step(&urg, 45), // on either side of bottom
urg_deg2step(&urg, -deg), // limiting to 45 degrees
urg_deg2step(&urg, deg), // on either side of bottom
1) != 0) { // step, TODO: look at increasing this to reduce num samples
perror("urg_set_scanning_parameter failed.");
return 1;
}
urg_start_measurement(&urg, URG_DISTANCE_INTENSITY, URG_SCAN_INFINITY, 0);
fprintf(stderr, "z,y,time stamp from lidar,intensity,length,radian,");
fprintf(stderr, "number of rotations,sample number from data,time stamp from print_data\n");
fprintf(stderr, "z,y,lidar time stamp,intensity,length,radian,");
fprintf(stderr, "number of rotations,sample number,time stamp from print_data\n");
do {
n = urg_get_distance_intensity(&urg, data, intensity, &time_stamp);
num_rotations = print_data(&urg, data, time_stamp, intensity, n, num_rotations);
num_rotations = print_data(&urg, data, time_stamp, intensity, n, num_rotations, offset);
} while (n > 0);
// Disconnects
......
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