41 inline float SQR(
float val) {
return val*val; }
54 double view_direction,
104 double full_firing_cycle = 55.296 * 1e-6;
105 double single_firing = 2.304 * 1e-6;
106 double dataBlockIndex, dataPointIndex;
107 bool dual_mode =
false;
112 dataBlockIndex = (x - (x % 2)) + (y / 16);
115 dataBlockIndex = (x * 2) + (y / 16);
117 dataPointIndex = y % 16;
119 timing_offsets[x][y] = (full_firing_cycle * dataBlockIndex) + (single_firing * dataPointIndex);
131 double full_firing_cycle = 55.296 * 1e-6;
132 double single_firing = 2.304 * 1e-6;
133 double dataBlockIndex, dataPointIndex;
134 bool dual_mode =
false;
139 dataBlockIndex = x / 2;
144 dataPointIndex = y / 2;
145 timing_offsets[x][y] = (full_firing_cycle * dataBlockIndex) + (single_firing * dataPointIndex);
157 double full_firing_cycle = 46.080 * 1e-6;
158 double single_firing = 1.152 * 1e-6;
159 double dataBlockIndex, dataPointIndex;
160 bool dual_mode =
false;
165 dataBlockIndex = x / 2;
170 dataPointIndex = y / 2;
171 timing_offsets[x][y] = (full_firing_cycle * dataBlockIndex) + (single_firing * dataPointIndex);
186 double sequenceIndex, firingGroupIndex;
192 firingGroupIndex = y;
193 timing_offsets[x][y] = (full_firing_cycle * sequenceIndex) + (single_firing * firingGroupIndex) - offset_paket_time;
214 ROS_WARN(
"NO TIMING OFFSETS CALCULATED. ARE YOU USING A SUPPORTED VELODYNE SENSOR?");
257 double max_range_,
double min_range_)
304 for (uint8_t i = 0; i < 16; i++) {
314 for (uint8_t i = 0; i < 16; i++) {
348 float time_diff_start_to_this_packet = (pkt.stamp - scan_start_time).toSec();
367 const uint8_t laser_number = j + bank_origin;
395 data.addPoint(nanf(
""), nanf(
""), nanf(
""), corrections.laser_ring, raw->
blocks[i].
rotation, nanf(
""), nanf(
""), time);
400 distance += corrections.dist_correction;
402 float cos_vert_angle = corrections.cos_vert_correction;
403 float sin_vert_angle = corrections.sin_vert_correction;
404 float cos_rot_correction = corrections.cos_rot_correction;
405 float sin_rot_correction = corrections.sin_rot_correction;
409 float cos_rot_angle =
412 float sin_rot_angle =
416 float horiz_offset = corrections.horiz_offset_correction;
417 float vert_offset = corrections.vert_offset_correction;
424 float xy_distance = distance * cos_vert_angle - vert_offset * sin_vert_angle;
427 float xx = xy_distance * sin_rot_angle - horiz_offset * cos_rot_angle;
429 float yy = xy_distance * cos_rot_angle + horiz_offset * sin_rot_angle;
436 float distance_corr_x = 0;
437 float distance_corr_y = 0;
438 if (corrections.two_pt_correction_available) {
440 (corrections.dist_correction - corrections.dist_correction_x)
441 * (xx - 2.4) / (25.04 - 2.4)
442 + corrections.dist_correction_x;
443 distance_corr_x -= corrections.dist_correction;
445 (corrections.dist_correction - corrections.dist_correction_y)
446 * (yy - 1.93) / (25.04 - 1.93)
447 + corrections.dist_correction_y;
448 distance_corr_y -= corrections.dist_correction;
451 float distance_x = distance + distance_corr_x;
456 xy_distance = distance_x * cos_vert_angle - vert_offset * sin_vert_angle ;
458 x = xy_distance * sin_rot_angle - horiz_offset * cos_rot_angle;
460 float distance_y = distance + distance_corr_y;
461 xy_distance = distance_y * cos_vert_angle - vert_offset * sin_vert_angle ;
466 y = xy_distance * cos_rot_angle + horiz_offset * sin_rot_angle;
474 z = distance_y * sin_vert_angle + vert_offset*cos_vert_angle;
483 float min_intensity = corrections.min_intensity;
484 float max_intensity = corrections.max_intensity;
488 float focal_offset = 256
489 * (1 - corrections.focal_distance / 13100)
490 * (1 - corrections.focal_distance / 13100);
491 float focal_slope = corrections.focal_slope;
492 intensity += focal_slope * (std::abs(focal_offset - 256 *
493 SQR(1 -
static_cast<float>(tmp.
uint)/65535)));
494 intensity = (intensity < min_intensity) ? min_intensity : intensity;
495 intensity = (intensity > max_intensity) ? max_intensity : intensity;
497 data.addPoint(x_coord, y_coord, z_coord, corrections.laser_ring, raw->
blocks[i].
rotation, distance, intensity, time);
511 float azimuth_diff, azimuth_corrected_f;
512 float last_azimuth_diff = 0;
513 uint16_t azimuth, azimuth_next, azimuth_corrected;
514 float x_coord, y_coord, z_coord;
519 float cos_vert_angle, sin_vert_angle, cos_rot_correction, sin_rot_correction;
520 float cos_rot_angle, sin_rot_angle;
523 float time_diff_start_to_this_packet = (pkt.stamp - scan_start_time).toSec();
525 uint8_t laser_number, firing_order;
526 bool dual_return = (pkt.data[1204] == 57);
535 switch (current_block.
header) {
553 << block <<
" header value is "
562 azimuth = azimuth_next;
566 azimuth_next = raw->
blocks[block + (1+dual_return)].rotation;
569 azimuth_diff = (float)((36000 + azimuth_next - azimuth) % 36000);
572 last_azimuth_diff = azimuth_diff;
577 azimuth_diff = (block ==
BLOCKS_PER_PACKET - (4*dual_return)-1) ? 0 : last_azimuth_diff;
584 tmp.bytes[0] = current_block.
data[k];
585 tmp.bytes[1] = current_block.
data[k + 1];
591 laser_number = j + bank_origin;
592 firing_order = laser_number / 8;
596 time =
timing_offsets[block/4][firing_order + laser_number/64] + time_diff_start_to_this_packet;
603 azimuth_corrected = ((uint16_t) round(azimuth_corrected_f)) % 36000;
621 xy_distance = distance * cos_vert_angle;
623 data.addPoint(xy_distance * cos_rot_angle,
624 -(xy_distance * sin_rot_angle),
625 distance * sin_vert_angle,
629 current_block.
data[k + 2],
652 int raw_azimuth_diff;
653 float last_azimuth_diff=0;
654 float azimuth_corrected_f;
655 int azimuth_corrected;
659 float time_diff_start_to_this_packet = (pkt.stamp - scan_start_time).toSec();
670 << block <<
" header value is "
679 azimuth_diff = (float)((36000 + raw_azimuth_diff)%36000);
681 if(raw_azimuth_diff < 0)
685 if(last_azimuth_diff > 0){
686 azimuth_diff = last_azimuth_diff;
694 last_azimuth_diff = azimuth_diff;
696 azimuth_diff = last_azimuth_diff;
710 azimuth_corrected = ((int)round(azimuth_corrected_f)) % 36000;
732 float cos_rot_angle =
735 float sin_rot_angle =
747 float xy_distance = distance * cos_vert_angle - vert_offset * sin_vert_angle;
750 float xx = xy_distance * sin_rot_angle - horiz_offset * cos_rot_angle;
752 float yy = xy_distance * cos_rot_angle + horiz_offset * sin_rot_angle;
759 float distance_corr_x = 0;
760 float distance_corr_y = 0;
764 * (xx - 2.4) / (25.04 - 2.4)
769 * (yy - 1.93) / (25.04 - 1.93)
774 float distance_x = distance + distance_corr_x;
779 xy_distance = distance_x * cos_vert_angle - vert_offset * sin_vert_angle ;
780 x = xy_distance * sin_rot_angle - horiz_offset * cos_rot_angle;
782 float distance_y = distance + distance_corr_y;
787 xy_distance = distance_y * cos_vert_angle - vert_offset * sin_vert_angle ;
788 y = xy_distance * cos_rot_angle + horiz_offset * sin_rot_angle;
796 z = distance_y * sin_vert_angle + vert_offset*cos_vert_angle;
812 intensity += focal_slope * (std::abs(focal_offset - 256 *
813 SQR(1 -
static_cast<float>(tmp.
uint) / 65535)));
814 intensity = (intensity < min_intensity) ? min_intensity : intensity;
815 intensity = (intensity > max_intensity) ? max_intensity : intensity;
819 time =
timing_offsets[block][firing * 16 + dsr] + time_diff_start_to_this_packet;
821 data.addPoint(x_coord, y_coord, z_coord, corrections.
laser_ring, azimuth_corrected, distance, intensity, time);