diff --git a/.github/workflows/build-and-test-differential.yaml b/.github/workflows/build-and-test-differential.yaml index dabe871d5..8eeb1de06 100644 --- a/.github/workflows/build-and-test-differential.yaml +++ b/.github/workflows/build-and-test-differential.yaml @@ -74,7 +74,7 @@ jobs: - name: Get modified files id: get-modified-files - uses: tj-actions/changed-files@v36 + uses: tj-actions/changed-files@v35 with: files: | **/*.cpp diff --git a/.github/workflows/build-and-test.yaml b/.github/workflows/build-and-test.yaml index 6459a9804..33096b753 100644 --- a/.github/workflows/build-and-test.yaml +++ b/.github/workflows/build-and-test.yaml @@ -24,6 +24,14 @@ jobs: - name: Check out repository uses: actions/checkout@v3 + - name: Free disk space (Ubuntu) + uses: jlumbroso/free-disk-space@v1.2.0 + with: + tool-cache: false + dotnet: false + swap-storage: false + large-packages: false + - name: Remove exec_depend uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1 diff --git a/README.md b/README.md index 7a70659e9..5c3124f88 100644 --- a/README.md +++ b/README.md @@ -164,9 +164,8 @@ Parameters shared by all supported models: | frequency_ms | uint16 | 100 | milliseconds, > 0 | Time per scan | | packet_mtu_size | uint16 | 1500 | | Packet MTU size | | rotation_speed | uint16 | 600 | | Rotation speed | -| rotation_speed | uint16 | 600 | | Rotation speed | | cloud_min_angle | uint16 | 0 | degrees [0, 360] | FoV start angle | -| cloud_max_angle | uint16 | 360 | degrees [0, 360] | FoV end angle | +| cloud_max_angle | uint16 | 359 | degrees [0, 360] | FoV end angle | | dual_return_distance_threshold | double | 0.1 | | Dual return distance threshold | | diag_span | uint16 | 1000 | milliseconds, > 0 | Diagnostic span | | setup_sensor | bool | True | True, False | Configure sensor settings | @@ -201,6 +200,8 @@ Parameters shared by all supported models: | gnss_port | uint16 | 2369 | | GNSS port | | frequency_ms | uint16 | 100 | milliseconds, > 0 | Time per scan | | packet_mtu_size | uint16 | 1500 | | Packet MTU size | +| cloud_min_angle | uint16 | 0 | degrees [0, 360] | FoV start angle | +| cloud_max_angle | uint16 | 359 | degrees [0, 360] | FoV end angle | #### Driver parameters @@ -210,7 +211,8 @@ Parameters shared by all supported models: | calibration_file | string | | | LiDAR calibration file | | min_range | double | 0.3 | meters, >= 0.3 | Minimum point range published | | max_range | double | 300.0 | meters, <= 300.0 | Maximum point range published | -| view_width | double | 360.0 | degrees [0.0, 360.0] | Horizontal FOV centered at `scan_phase` | +| cloud_min_angle | uint16 | 0 | degrees [0, 360] | FoV start angle | +| cloud_max_angle | uint16 | 359 | degrees [0, 360] | FoV end angle | ## Software design overview diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp index b77f7d72e..b52105ae9 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp @@ -8,7 +8,6 @@ #include "nebula_decoders/nebula_decoders_common/nebula_driver_base.hpp" #include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp" -#include "pandar_msgs/msg/pandar_jumbo_packet.hpp" #include "pandar_msgs/msg/pandar_packet.hpp" #include "pandar_msgs/msg/pandar_scan.hpp" diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp index 1aeabf87c..b96fac205 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp @@ -39,7 +39,7 @@ static const int RAW_SCAN_SIZE = 3; static const int SCANS_PER_BLOCK = 32; static const int BLOCK_DATA_SIZE = (SCANS_PER_BLOCK * RAW_SCAN_SIZE); -static const float ROTATION_RESOLUTION = 0.01f; // [deg] +static const double ROTATION_RESOLUTION = 0.01; // [deg] static const uint16_t ROTATION_MAX_UNITS = 36000u; // [deg/100] static const size_t RETURN_MODE_INDEX = 1204; @@ -149,7 +149,7 @@ class VelodyneScanDecoder bool has_scanned_ = true; double dual_return_distance_threshold_{}; // Velodyne does this internally, this will not be // implemented here - double first_timestamp{}; + double scan_timestamp_{}; /// @brief SensorConfiguration for this decoder std::shared_ptr sensor_configuration_; diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp16_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp16_decoder.hpp index 10671e890..0cfe33aa5 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp16_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp16_decoder.hpp @@ -13,6 +13,7 @@ namespace drivers { namespace vlp16 { + constexpr uint32_t MAX_POINTS = 300000; /// @brief Velodyne LiDAR decoder (VLP16) class Vlp16Decoder : public VelodyneScanDecoder { @@ -48,8 +49,10 @@ class Vlp16Decoder : public VelodyneScanDecoder bool parsePacket(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) override; float sin_rot_table_[ROTATION_MAX_UNITS]; float cos_rot_table_[ROTATION_MAX_UNITS]; + float rotation_radians_[ROTATION_MAX_UNITS]; int phase_; int max_pts_; + std::vector> timing_offsets_; }; } // namespace vlp16 diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp32_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp32_decoder.hpp index dd85ba358..4fd5f4c03 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp32_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp32_decoder.hpp @@ -48,6 +48,8 @@ class Vlp32Decoder : public VelodyneScanDecoder bool parsePacket(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) override; float sin_rot_table_[ROTATION_MAX_UNITS]; float cos_rot_table_[ROTATION_MAX_UNITS]; + float rotation_radians_[ROTATION_MAX_UNITS]; + std::vector> timing_offsets_; int phase_; int max_pts_; }; diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vls128_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vls128_decoder.hpp index 33e1d3e30..d0702371d 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vls128_decoder.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vls128_decoder.hpp @@ -48,9 +48,11 @@ class Vls128Decoder : public VelodyneScanDecoder bool parsePacket(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) override; float sin_rot_table_[ROTATION_MAX_UNITS]; float cos_rot_table_[ROTATION_MAX_UNITS]; + float rotation_radians_[ROTATION_MAX_UNITS]; float vls_128_laser_azimuth_cache_[16]; int phase_; int max_pts_; + std::vector> timing_offsets_; }; } // namespace vls128 diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp index c5fe1037f..2e877e523 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp @@ -16,7 +16,7 @@ Vlp16Decoder::Vlp16Decoder( sensor_configuration_ = sensor_configuration; calibration_configuration_ = calibration_configuration; - first_timestamp = std::numeric_limits::max(); + scan_timestamp_ = -1; scan_pc_.reset(new NebulaPointCloud); overflow_pc_.reset(new NebulaPointCloud); @@ -24,9 +24,32 @@ Vlp16Decoder::Vlp16Decoder( // Set up cached values for sin and cos of all the possible headings for (uint16_t rot_index = 0; rot_index < ROTATION_MAX_UNITS; ++rot_index) { float rotation = angles::from_degrees(ROTATION_RESOLUTION * rot_index); + rotation_radians_[rot_index] = rotation; cos_rot_table_[rot_index] = cosf(rotation); sin_rot_table_[rot_index] = sinf(rotation); } + // timing table calculation, from velodyne user manual p.64 + timing_offsets_.resize(BLOCKS_PER_PACKET); + for (size_t i=0; i < timing_offsets_.size(); ++i){ + timing_offsets_[i].resize(32); + } + double full_firing_cycle_s = 55.296 * 1e-6; + double single_firing_s = 2.304 * 1e-6; + double data_block_index, data_point_index; + bool dual_mode = sensor_configuration_->return_mode==ReturnMode::DUAL; + // compute timing offsets + for (size_t x = 0; x < timing_offsets_.size(); ++x){ + for (size_t y = 0; y < timing_offsets_[x].size(); ++y){ + if (dual_mode){ + data_block_index = (x - (x % 2)) + (y / 16); + } + else{ + data_block_index = (x * 2) + (y / 16); + } + data_point_index = y % 16; + timing_offsets_[x][y] = (full_firing_cycle_s * data_block_index) + (single_firing_s * data_point_index); + } + } phase_ = (uint16_t)round(sensor_configuration_->scan_phase * 100); } @@ -35,19 +58,21 @@ bool Vlp16Decoder::hasScanned() { return has_scanned_; } std::tuple Vlp16Decoder::get_pointcloud() { - int phase = (uint16_t)round(sensor_configuration_->scan_phase * 100); + double phase = angles::from_degrees(sensor_configuration_->scan_phase); if (!scan_pc_->points.empty()) { - uint16_t current_azimuth = (int)scan_pc_->points.back().azimuth * 100; - uint16_t phase_diff = (36000 + current_azimuth - phase) % 36000; - while (phase_diff < 18000 && scan_pc_->points.size() > 0) { + auto current_azimuth = scan_pc_->points.back().azimuth; + auto phase_diff = static_cast(angles::to_degrees(2*M_PI + current_azimuth - phase)) % 360; + while (phase_diff < M_PI_2 && !scan_pc_->points.empty()) { overflow_pc_->points.push_back(scan_pc_->points.back()); scan_pc_->points.pop_back(); - current_azimuth = (int)scan_pc_->points.back().azimuth * 100; - phase_diff = (36000 + current_azimuth - phase) % 36000; + current_azimuth = scan_pc_->points.back().azimuth; + phase_diff = static_cast(angles::to_degrees(2*M_PI + current_azimuth - phase)) % 360; } overflow_pc_->width = overflow_pc_->points.size(); + scan_pc_->width = scan_pc_->points.size(); + scan_pc_->height = 1; } - return std::make_tuple(scan_pc_, first_timestamp); + return std::make_tuple(scan_pc_, scan_timestamp_); } int Vlp16Decoder::pointsPerPacket() @@ -61,7 +86,7 @@ void Vlp16Decoder::reset_pointcloud(size_t n_pts) max_pts_ = n_pts * pointsPerPacket(); scan_pc_->points.reserve(max_pts_); reset_overflow(); // transfer existing overflow points to the cleared pointcloud - first_timestamp = std::numeric_limits::max(); + scan_timestamp_ = -1; } void Vlp16Decoder::reset_overflow() @@ -138,6 +163,11 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa other_return.bytes[1] = block % 2 ? raw->blocks[block - 1].data[k + 1] : raw->blocks[block + 1].data[k + 1]; } + // Apply timestamp if this is the first new packet in the scan. + auto block_timestamp = rclcpp::Time(velodyne_packet.stamp).seconds(); + if (scan_timestamp_ < 0) { + scan_timestamp_ = block_timestamp; + } // Do not process if there is no return, or in dual return mode and the first and last // echos are the same. if ( @@ -193,14 +223,9 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa const float x_coord = xy_distance * cos_rot_angle; // velodyne y const float y_coord = -(xy_distance * sin_rot_angle); // velodyne x const float z_coord = distance * sin_vert_angle; // velodyne z - const float intensity = current_block.data[k + 2]; + const uint8_t intensity = current_block.data[k + 2]; - const double time_stamp = - (block * 2 + firing) * 55.296 / 1000.0 / 1000.0 + dsr * 2.304 / 1000.0 / 1000.0; - auto ts = rclcpp::Time(velodyne_packet.stamp).seconds(); - if (ts < first_timestamp) { - first_timestamp = ts; - } + double point_time_offset = timing_offsets_[block][firing * 16 + dsr]; // Determine return type. uint8_t return_type; @@ -212,12 +237,12 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa other_return.bytes[1] == current_return.bytes[1])) { return_type = static_cast(drivers::ReturnType::IDENTICAL); } else { - const float other_intensity = block % 2 ? raw->blocks[block - 1].data[k + 2] + const uint8_t other_intensity = block % 2 ? raw->blocks[block - 1].data[k + 2] : raw->blocks[block + 1].data[k + 2]; - bool first = other_return.uint < current_return.uint ? 0 : 1; - bool strongest = other_intensity < intensity ? 1 : 0; + bool first = current_return.uint > other_return.uint ; + bool strongest = intensity > other_intensity; if (other_intensity == intensity) { - strongest = first ? 0 : 1; + strongest = !first; } if (first && strongest) { return_type = static_cast(drivers::ReturnType::FIRST_STRONGEST); @@ -247,8 +272,12 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa current_point.z = z_coord; current_point.return_type = return_type; current_point.channel = corrections.laser_ring; - current_point.azimuth = azimuth_corrected; - current_point.time_stamp = time_stamp; + current_point.azimuth = rotation_radians_[azimuth_corrected]; + current_point.elevation = sin_vert_angle; + auto point_ts = block_timestamp - scan_timestamp_ + point_time_offset; + if (point_ts < 0) + point_ts = 0; + current_point.time_stamp = static_cast(point_ts*1e9); current_point.intensity = intensity; current_point.distance = distance; scan_pc_->points.emplace_back(current_point); diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp index 89583ef63..285e14525 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp @@ -16,7 +16,7 @@ Vlp32Decoder::Vlp32Decoder( sensor_configuration_ = sensor_configuration; calibration_configuration_ = calibration_configuration; - first_timestamp = std::numeric_limits::max(); + scan_timestamp_ = -1; scan_pc_.reset(new NebulaPointCloud); overflow_pc_.reset(new NebulaPointCloud); @@ -24,29 +24,55 @@ Vlp32Decoder::Vlp32Decoder( // Set up cached values for sin and cos of all the possible headings for (uint16_t rot_index = 0; rot_index < ROTATION_MAX_UNITS; ++rot_index) { float rotation = angles::from_degrees(ROTATION_RESOLUTION * rot_index); + rotation_radians_[rot_index] = rotation; cos_rot_table_[rot_index] = cosf(rotation); sin_rot_table_[rot_index] = sinf(rotation); } phase_ = (uint16_t)round(sensor_configuration_->scan_phase * 100); + + timing_offsets_.resize(12); + for (size_t i=0; i < timing_offsets_.size(); ++i){ + timing_offsets_[i].resize(32); + } + // constants + double full_firing_cycle = 55.296 * 1e-6; // seconds + double single_firing = 2.304 * 1e-6; // seconds + double dataBlockIndex, dataPointIndex; + bool dual_mode = sensor_configuration_->return_mode == ReturnMode::DUAL; + // compute timing offsets + for (size_t x = 0; x < timing_offsets_.size(); ++x){ + for (size_t y = 0; y < timing_offsets_[x].size(); ++y){ + if (dual_mode){ + dataBlockIndex = x / 2; + } + else{ + dataBlockIndex = x; + } + dataPointIndex = y / 2; + timing_offsets_[x][y] = (full_firing_cycle * dataBlockIndex) + (single_firing * dataPointIndex); + } + } } bool Vlp32Decoder::hasScanned() { return has_scanned_; } std::tuple Vlp32Decoder::get_pointcloud() { - int phase = (uint16_t)round(sensor_configuration_->scan_phase * 100); + double phase = angles::from_degrees(sensor_configuration_->scan_phase); if (!scan_pc_->points.empty()) { - uint16_t current_azimuth = (int)scan_pc_->points.back().azimuth * 100; - uint16_t phase_diff = (36000 + current_azimuth - phase) % 36000; - while (phase_diff < 18000 && scan_pc_->points.size() > 0) { + auto current_azimuth = scan_pc_->points.back().azimuth; + auto phase_diff = (2*M_PI + current_azimuth - phase); + while (phase_diff < M_PI_2 && !scan_pc_->points.empty()) { overflow_pc_->points.push_back(scan_pc_->points.back()); scan_pc_->points.pop_back(); - current_azimuth = (int)scan_pc_->points.back().azimuth * 100; - phase_diff = (36000 + current_azimuth - phase) % 36000; + current_azimuth = scan_pc_->points.back().azimuth; + phase_diff = (2*M_PI + current_azimuth - phase); } overflow_pc_->width = overflow_pc_->points.size(); + scan_pc_->width = scan_pc_->points.size(); + scan_pc_->height = 1; } - return std::make_tuple(scan_pc_, first_timestamp); + return std::make_tuple(scan_pc_, scan_timestamp_); } int Vlp32Decoder::pointsPerPacket() { return BLOCKS_PER_PACKET * SCANS_PER_BLOCK; } @@ -58,7 +84,7 @@ void Vlp32Decoder::reset_pointcloud(size_t n_pts) max_pts_ = n_pts * pointsPerPacket(); scan_pc_->points.reserve(max_pts_); reset_overflow(); // transfer existing overflow points to the cleared pointcloud - first_timestamp = std::numeric_limits::max(); + scan_timestamp_ = -1; } void Vlp32Decoder::reset_overflow() @@ -85,7 +111,7 @@ void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa } for (int j = 0, k = 0; j < SCANS_PER_BLOCK; j++, k += RAW_SCAN_SIZE) { float x, y, z; - float intensity; + uint8_t intensity; const uint8_t laser_number = j + bank_origin; const VelodyneLaserCorrection & corrections = @@ -103,6 +129,11 @@ void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa other_return.bytes[1] = i % 2 ? raw->blocks[i - 1].data[k + 1] : raw->blocks[i + 1].data[k + 1]; } + // Apply timestamp if this is the first new packet in the scan. + auto block_timestamp = rclcpp::Time(velodyne_packet.stamp).seconds(); + if (scan_timestamp_ < 0) { + scan_timestamp_ = block_timestamp; + } // Do not process if there is no return, or in dual return mode and the first and last echos // are the same. if ( @@ -145,7 +176,7 @@ void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa // Compute the distance in the xy plane (w/o accounting for rotation) /**the new term of 'vert_offset * sin_vert_angle' - * was added to the expression due to the mathemathical + * was added to the expression due to the mathematical * model we used. */ float xy_distance = distance * cos_vert_angle - vert_offset * sin_vert_angle; @@ -179,17 +210,17 @@ void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa const float distance_x = distance + distance_corr_x; /**the new term of 'vert_offset * sin_vert_angle' - * was added to the expression due to the mathemathical + * was added to the expression due to the mathematical * model we used. */ xy_distance = distance_x * cos_vert_angle - vert_offset * sin_vert_angle; - /// the expression wiht '-' is proved to be better than the one with '+' + /// the expression with '-' is proved to be better than the one with '+' x = xy_distance * sin_rot_angle - horiz_offset * cos_rot_angle; const float distance_y = distance + distance_corr_y; xy_distance = distance_y * cos_vert_angle - vert_offset * sin_vert_angle; /**the new term of 'vert_offset * sin_vert_angle' - * was added to the expression due to the mathemathical + * was added to the expression due to the mathematical * model we used. */ y = xy_distance * cos_rot_angle + horiz_offset * sin_rot_angle; @@ -197,7 +228,7 @@ void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa // Using distance_y is not symmetric, but the velodyne manual // does this. /**the new term of 'vert_offset * cos_vert_angle' - * was added to the expression due to the mathemathical + * was added to the expression due to the mathematical * model we used. */ z = distance_y * sin_vert_angle + vert_offset * cos_vert_angle; @@ -222,11 +253,7 @@ void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa intensity = (intensity < min_intensity) ? min_intensity : intensity; intensity = (intensity > max_intensity) ? max_intensity : intensity; - double time_stamp = i * 55.296 / 1000.0 / 1000.0 + j * 2.304 / 1000.0 / 1000.0; // + - auto ts = rclcpp::Time(velodyne_packet.stamp).seconds(); - if (ts < first_timestamp) { - first_timestamp = ts; - } + double point_time_offset = timing_offsets_[i][j]; nebula::drivers::ReturnType return_type; switch (return_mode) { @@ -272,9 +299,13 @@ void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa current_point.z = z_coord; current_point.return_type = static_cast(return_type); current_point.channel = corrections.laser_ring; - current_point.azimuth = raw->blocks[i].rotation; + current_point.azimuth = rotation_radians_[block.rotation]; + current_point.elevation = sin_vert_angle; + auto point_ts = block_timestamp - scan_timestamp_ + point_time_offset; + if (point_ts < 0) + point_ts = 0; + current_point.time_stamp = static_cast(point_ts*1e9); current_point.distance = distance; - current_point.time_stamp = time_stamp; current_point.intensity = intensity; scan_pc_->points.emplace_back(current_point); } diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp index 024e8135d..19480c7bf 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp @@ -16,7 +16,7 @@ Vls128Decoder::Vls128Decoder( sensor_configuration_ = sensor_configuration; calibration_configuration_ = calibration_configuration; - first_timestamp = std::numeric_limits::max(); + scan_timestamp_ = -1; scan_pc_.reset(new NebulaPointCloud); overflow_pc_.reset(new NebulaPointCloud); @@ -24,6 +24,7 @@ Vls128Decoder::Vls128Decoder( // Set up cached values for sin and cos of all the possible headings for (uint16_t rot_index = 0; rot_index < ROTATION_MAX_UNITS; ++rot_index) { float rotation = angles::from_degrees(ROTATION_RESOLUTION * rot_index); + rotation_radians_[rot_index] = rotation; cos_rot_table_[rot_index] = cosf(rotation); sin_rot_table_[rot_index] = sinf(rotation); } @@ -33,25 +34,45 @@ Vls128Decoder::Vls128Decoder( for (uint8_t i = 0; i < 16; i++) { vls_128_laser_azimuth_cache_[i] = (VLS128_CHANNEL_DURATION / VLS128_SEQ_DURATION) * (i + i / 8); } + // timing table calculation, from velodyne user manual p.64 + timing_offsets_.resize(3); + for(size_t i=0; i < timing_offsets_.size(); ++i) + { + timing_offsets_[i].resize(17); // 17 (+1 for the maintenance time after firing group 8) + } + double full_firing_cycle_s = 53.3 * 1e-6; + double single_firing_s = 2.665 * 1e-6; + double offset_packet_time = 8.7 * 1e-6; + // compute timing offsets + for (size_t x = 0; x < timing_offsets_.size(); ++x){ + for (size_t y = 0; y < timing_offsets_[x].size(); ++y){ + double sequence_index, firing_group_index; + sequence_index = x; + firing_group_index = y; + timing_offsets_[x][y] = (full_firing_cycle_s * sequence_index) + (single_firing_s * firing_group_index) - offset_packet_time; + } + } } bool Vls128Decoder::hasScanned() { return has_scanned_; } std::tuple Vls128Decoder::get_pointcloud() { - int phase = (uint16_t)round(sensor_configuration_->scan_phase * 100); + double phase = angles::from_degrees(sensor_configuration_->scan_phase); if (!scan_pc_->points.empty()) { - uint16_t current_azimuth = (int)scan_pc_->points.back().azimuth * 100; - uint16_t phase_diff = (36000 + current_azimuth - phase) % 36000; - while (phase_diff < 18000 && scan_pc_->points.size() > 0) { + auto current_azimuth = scan_pc_->points.back().azimuth; + auto phase_diff = static_cast(angles::to_degrees(2*M_PI + current_azimuth - phase)) % 360; + while (phase_diff < M_PI_2 && !scan_pc_->points.empty()) { overflow_pc_->points.push_back(scan_pc_->points.back()); scan_pc_->points.pop_back(); - current_azimuth = (int)scan_pc_->points.back().azimuth * 100; - phase_diff = (36000 + current_azimuth - phase) % 36000; + current_azimuth = scan_pc_->points.back().azimuth; + phase_diff = static_cast(angles::to_degrees(2*M_PI + current_azimuth - phase)) % 360; } overflow_pc_->width = overflow_pc_->points.size(); + scan_pc_->width = scan_pc_->points.size(); + scan_pc_->height = 1; } - return std::make_tuple(scan_pc_, first_timestamp); + return std::make_tuple(scan_pc_, scan_timestamp_); } int Vls128Decoder::pointsPerPacket() { return BLOCKS_PER_PACKET * SCANS_PER_BLOCK; } @@ -63,7 +84,7 @@ void Vls128Decoder::reset_pointcloud(size_t n_pts) max_pts_ = n_pts * pointsPerPacket(); scan_pc_->points.reserve(max_pts_); reset_overflow(); // transfer existing overflow points to the cleared pointcloud - first_timestamp = std::numeric_limits::max(); + scan_timestamp_ = -1; } void Vls128Decoder::reset_overflow() @@ -159,6 +180,11 @@ void Vls128Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_p other_return.bytes[1] = block % 2 ? raw->blocks[block - 1].data[k + 1] : raw->blocks[block + 1].data[k + 1]; } + // Apply timestamp if this is the first new packet in the scan. + auto block_timestamp = rclcpp::Time(velodyne_packet.stamp).seconds(); + if (scan_timestamp_ < 0) { + scan_timestamp_ = block_timestamp; + } // Do not process if there is no return, or in dual return mode and the first and last echos // are the same. if ( @@ -215,14 +241,12 @@ void Vls128Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_p const float x_coord = xy_distance * cos_rot_angle; // velodyne y const float y_coord = -(xy_distance * sin_rot_angle); // velodyne x const float z_coord = distance * sin_vert_angle; // velodyne z - const float intensity = current_block.data[k + 2]; - - const double time_stamp = - block * 55.3 / 1000.0 / 1000.0 + j * 2.665 / 1000.0 / 1000.0; // + - auto ts = rclcpp::Time(velodyne_packet.stamp).seconds(); - if (ts < first_timestamp) { - first_timestamp = ts; + const uint8_t intensity = current_block.data[k + 2]; + auto block_timestamp = rclcpp::Time(velodyne_packet.stamp).seconds(); + if (scan_timestamp_ < 0) { + scan_timestamp_ = block_timestamp; } + double point_time_offset = timing_offsets_[block / 4][firing_order + laser_number / 64]; // Determine return type. uint8_t return_type; @@ -236,10 +260,10 @@ void Vls128Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_p } else { const float other_intensity = block % 2 ? raw->blocks[block - 1].data[k + 2] : raw->blocks[block + 1].data[k + 2]; - bool first = other_return.uint < current_return.uint ? 0 : 1; - bool strongest = other_intensity < intensity ? 1 : 0; + bool first = other_return.uint >= current_return.uint; + bool strongest = other_intensity < intensity; if (other_intensity == intensity) { - strongest = first ? 0 : 1; + strongest = !first; } if (first && strongest) { return_type = static_cast(drivers::ReturnType::FIRST_STRONGEST); @@ -269,9 +293,13 @@ void Vls128Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_p current_point.z = z_coord; current_point.return_type = return_type; current_point.channel = corrections.laser_ring; - current_point.azimuth = azimuth_corrected; + current_point.azimuth = rotation_radians_[azimuth_corrected]; + current_point.elevation = sin_vert_angle; current_point.distance = distance; - current_point.time_stamp = time_stamp; + auto point_ts = block_timestamp - scan_timestamp_ + point_time_offset; + if (point_ts < 0) + point_ts = 0; + current_point.time_stamp = static_cast(point_ts*1e9); current_point.intensity = intensity; scan_pc_->points.emplace_back(current_point); } // 2nd scan area condition diff --git a/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp b/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp index 4560456fe..f3e01eb42 100644 --- a/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp +++ b/nebula_hw_interfaces/src/nebula_velodyne_hw_interfaces/velodyne_hw_interface.cpp @@ -216,21 +216,31 @@ VelodyneStatus VelodyneHwInterface::CheckAndSetConfig( target_key = "config.fov.start"; auto current_cloud_min_angle = tree.get(target_key); - if (sensor_configuration->cloud_min_angle != current_cloud_min_angle) { - SetFovStartAsync(sensor_configuration->cloud_min_angle); + int setting_cloud_min_angle = sensor_configuration->cloud_min_angle; + // Velodyne only allows a maximum of 359 in the setting + if (setting_cloud_min_angle == 360) { + setting_cloud_min_angle = 359; + } + if (setting_cloud_min_angle != current_cloud_min_angle) { + SetFovStartAsync(setting_cloud_min_angle); std::cout << "VelodyneHwInterface::parse_json(" << target_key << "): " << current_cloud_min_angle << std::endl; - std::cout << "sensor_configuration->cloud_min_angle: " << sensor_configuration->cloud_min_angle + std::cout << "sensor_configuration->cloud_min_angle: " << setting_cloud_min_angle << std::endl; } target_key = "config.fov.end"; auto current_cloud_max_angle = tree.get(target_key); - if (sensor_configuration->cloud_max_angle != current_cloud_max_angle) { - SetFovEndAsync(sensor_configuration->cloud_max_angle); + int setting_cloud_max_angle = sensor_configuration->cloud_max_angle; + // Velodyne only allows a maximum of 359 in the setting + if (setting_cloud_max_angle == 360) { + setting_cloud_max_angle = 359; + } + if (setting_cloud_max_angle != current_cloud_max_angle) { + SetFovEndAsync(setting_cloud_max_angle); std::cout << "VelodyneHwInterface::parse_json(" << target_key << "): " << current_cloud_max_angle << std::endl; - std::cout << "sensor_configuration->cloud_max_angle: " << sensor_configuration->cloud_max_angle + std::cout << "sensor_configuration->cloud_max_angle: " << setting_cloud_max_angle << std::endl; } diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp index 309ec8c61..fe1e43c79 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_hw_interface_ros_wrapper.hpp @@ -11,7 +11,6 @@ #include #include -#include "pandar_msgs/msg/pandar_jumbo_packet.hpp" #include "pandar_msgs/msg/pandar_packet.hpp" #include "pandar_msgs/msg/pandar_scan.hpp" diff --git a/nebula_ros/launch/velodyne_launch_all_hw.xml b/nebula_ros/launch/velodyne_launch_all_hw.xml index 4e5df0817..c39437039 100644 --- a/nebula_ros/launch/velodyne_launch_all_hw.xml +++ b/nebula_ros/launch/velodyne_launch_all_hw.xml @@ -14,7 +14,7 @@ - + diff --git a/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp b/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp index e47812e95..c0632c6d8 100644 --- a/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp +++ b/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp @@ -49,7 +49,10 @@ void VelodyneDriverRosWrapper::ReceiveScanMsgCallback( std::tuple pointcloud_ts = driver_ptr_->ConvertScanToPointcloud(scan_msg); nebula::drivers::NebulaPointCloudPtr pointcloud = std::get<0>(pointcloud_ts); - + if (pointcloud == nullptr) { + RCLCPP_WARN_STREAM(get_logger(), "Empty cloud parsed."); + return; + }; if ( nebula_points_pub_->get_subscription_count() > 0 || nebula_points_pub_->get_intra_process_subscription_count() > 0) { @@ -182,16 +185,25 @@ Status VelodyneDriverRosWrapper::GetParameters( this->declare_parameter("max_range", 300., descriptor); sensor_configuration.max_range = this->get_parameter("max_range").as_double(); } - double view_direction = sensor_configuration.scan_phase * M_PI / 180; - double view_width = 360 * M_PI / 180; + double view_direction; + { + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; + descriptor.read_only = false; + descriptor.dynamic_typing = false; + descriptor.additional_constraints = ""; + this->declare_parameter("view_direction", 0., descriptor); + view_direction = this->get_parameter("view_direction").as_double(); + } + double view_width = 2.0 * M_PI; { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE; descriptor.read_only = false; descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; - this->declare_parameter("view_width", 300., descriptor); - view_width = this->get_parameter("view_width").as_double() * M_PI / 180; + this->declare_parameter("view_width", 2.0 * M_PI, descriptor); + view_width = this->get_parameter("view_width").as_double(); } if (sensor_configuration.sensor_model != nebula::drivers::SensorModel::VELODYNE_HDL64) { @@ -202,7 +214,7 @@ Status VelodyneDriverRosWrapper::GetParameters( descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; rcl_interfaces::msg::IntegerRange range; - range.set__from_value(0).set__to_value(359).set__step(1); + range.set__from_value(0).set__to_value(360).set__step(1); descriptor.integer_range = {range}; this->declare_parameter("cloud_min_angle", 0, descriptor); sensor_configuration.cloud_min_angle = this->get_parameter("cloud_min_angle").as_int(); @@ -214,9 +226,9 @@ Status VelodyneDriverRosWrapper::GetParameters( descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; rcl_interfaces::msg::IntegerRange range; - range.set__from_value(0).set__to_value(359).set__step(1); + range.set__from_value(0).set__to_value(360).set__step(1); descriptor.integer_range = {range}; - this->declare_parameter("cloud_max_angle", 359, descriptor); + this->declare_parameter("cloud_max_angle", 360, descriptor); sensor_configuration.cloud_max_angle = this->get_parameter("cloud_max_angle").as_int(); } } else { diff --git a/nebula_ros/src/velodyne/velodyne_hw_interface_ros_wrapper.cpp b/nebula_ros/src/velodyne/velodyne_hw_interface_ros_wrapper.cpp index 20523a857..400fec791 100644 --- a/nebula_ros/src/velodyne/velodyne_hw_interface_ros_wrapper.cpp +++ b/nebula_ros/src/velodyne/velodyne_hw_interface_ros_wrapper.cpp @@ -180,7 +180,7 @@ Status VelodyneHwInterfaceRosWrapper::GetParameters( descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; rcl_interfaces::msg::IntegerRange range; - range.set__from_value(0).set__to_value(359).set__step(1); + range.set__from_value(0).set__to_value(360).set__step(1); descriptor.integer_range = {range}; this->declare_parameter("cloud_min_angle", 0, descriptor); sensor_configuration.cloud_min_angle = this->get_parameter("cloud_min_angle").as_int(); @@ -192,9 +192,9 @@ Status VelodyneHwInterfaceRosWrapper::GetParameters( descriptor.dynamic_typing = false; descriptor.additional_constraints = ""; rcl_interfaces::msg::IntegerRange range; - range.set__from_value(0).set__to_value(359).set__step(1); + range.set__from_value(0).set__to_value(360).set__step(1); descriptor.integer_range = {range}; - this->declare_parameter("cloud_max_angle", 359, descriptor); + this->declare_parameter("cloud_max_angle", 360, descriptor); sensor_configuration.cloud_max_angle = this->get_parameter("cloud_max_angle").as_int(); } diff --git a/nebula_tests/data/velodyne/vlp16/1673400472138016708.pcd b/nebula_tests/data/velodyne/vlp16/1673400472138016708.pcd index 439bbe0dd..b33415371 100644 Binary files a/nebula_tests/data/velodyne/vlp16/1673400472138016708.pcd and b/nebula_tests/data/velodyne/vlp16/1673400472138016708.pcd differ diff --git a/nebula_tests/data/velodyne/vls128/1614315746748918706.pcd b/nebula_tests/data/velodyne/vls128/1614315746748918706.pcd index d26d9a0ec..80ed53270 100644 Binary files a/nebula_tests/data/velodyne/vls128/1614315746748918706.pcd and b/nebula_tests/data/velodyne/vls128/1614315746748918706.pcd differ