diff --git a/nebula_decoders/CMakeLists.txt b/nebula_decoders/CMakeLists.txt index e94afcf2a..b49723525 100644 --- a/nebula_decoders/CMakeLists.txt +++ b/nebula_decoders/CMakeLists.txt @@ -61,10 +61,8 @@ target_include_directories(nebula_decoders_hesai PUBLIC # Velodyne add_library(nebula_decoders_velodyne SHARED src/nebula_decoders_velodyne/velodyne_driver.cpp - src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp - src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp - src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp ) + target_link_libraries(nebula_decoders_velodyne PUBLIC ${angles_msgs_TARGETS} ${velodyne_msgs_TARGETS} diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_generic_decoder.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_generic_decoder.hpp new file mode 100644 index 000000000..15df52049 --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_generic_decoder.hpp @@ -0,0 +1,389 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include "nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_packet.hpp" +#include "nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp" + +#include + +#include +#include + +#include + +#include +#include +#include +#include + +namespace nebula::drivers +{ +/// @brief Velodyne LiDAR decoder +template +class VelodyneDecoder : public VelodyneScanDecoder +{ +public: + explicit VelodyneDecoder( + const std::shared_ptr & sensor_configuration, + const std::shared_ptr & + calibration_configuration) + { + sensor_configuration_ = sensor_configuration; + calibration_configuration_ = calibration_configuration; + + scan_timestamp_ = -1; + + scan_pc_.reset(new NebulaPointCloud); + overflow_pc_.reset(new NebulaPointCloud); + + // Set up cached values for sin and cos of all the possible headings + for (uint16_t rot_index = 0; rot_index < velodyne_packet::g_rotation_max_units; ++rot_index) { + float rotation = angles::from_degrees(velodyne_packet::g_rotation_resolution * rot_index); + rotation_radians_[rot_index] = rotation; + cos_rot_table_[rot_index] = cosf(rotation); + sin_rot_table_[rot_index] = sinf(rotation); + } + + phase_ = static_cast(round(sensor_configuration_->scan_phase * 100)); + + // fill vls128 laser azimuth cache + sensor_.fill_azimuth_cache(); // TODO(ike-kazu): predefine azimuth cache and remove this for + // performance? + + // timing table calculation, from velodyne user manual p.64 + timing_offsets_.resize(velodyne_packet::g_blocks_per_packet); // x dir size + for (auto & timing_offset : timing_offsets_) { + timing_offset.resize( + velodyne_packet::g_channels_per_block + SensorT::num_maintenance_periods); // y dir size + } + + double full_firing_cycle_s = SensorT::full_firing_cycle_s; + double single_firing_s = SensorT::single_firing_s; + double offset_packet_time = SensorT::offset_packet_time; + 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) { + double firing_sequence_index; + if (dual_mode) { + // cast to long to make double multiplication integer division + firing_sequence_index = + static_cast(x * SensorT::firing_sequences_per_block / 2) + + (y / SensorT::channels_per_firing_sequence); + } else { + firing_sequence_index = static_cast(x * SensorT::firing_sequences_per_block) + + (y / SensorT::channels_per_firing_sequence); + } + double data_point_index = + (y % SensorT::channels_per_firing_sequence) / SensorT::num_simultaneous_firings - + offset_packet_time; + timing_offsets_[x][y] = + (full_firing_cycle_s * firing_sequence_index) + (single_firing_s * data_point_index); + } + } + } + + bool has_scanned() override { return has_scanned_; } + + std::tuple get_pointcloud() override + { + float phase = angles::from_degrees(sensor_configuration_->scan_phase); + if (!scan_pc_->points.empty()) { + 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 = 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_, scan_timestamp_); + } + + void reset_pointcloud(double time_stamp) override + { + // scan_pc_.reset(new NebulaPointCloud); + scan_pc_->points.clear(); + scan_pc_->points.reserve(max_pts_); + reset_overflow(time_stamp); // transfer existing overflow points to the cleared pointcloud + } + + void reset_overflow(double time_stamp) override + { + if (overflow_pc_->points.size() == 0) { + scan_timestamp_ = -1; + overflow_pc_->points.reserve(max_pts_); + return; + } + + // Compute the absolute time stamp of the last point of the overflow pointcloud + const double last_overflow_time_stamp = + scan_timestamp_ + 1e-9 * overflow_pc_->points.back().time_stamp; + + // Detect cases where there is an unacceptable time difference between the last overflow point + // and the first point of the next packet. In that case, there was probably a packet drop so it + // is better to ignore the overflow pointcloud + if (time_stamp - last_overflow_time_stamp > 0.05) { + scan_timestamp_ = -1; + overflow_pc_->points.clear(); + overflow_pc_->points.reserve(max_pts_); + return; + } + + // Add the overflow buffer points + while (overflow_pc_->points.size() > 0) { + auto overflow_point = overflow_pc_->points.back(); + + // The overflow points had the stamps from the previous pointcloud. These need to be changed + // to be relative to the overflow's packet timestamp + double new_timestamp_seconds = + scan_timestamp_ + 1e-9 * overflow_point.time_stamp - last_block_timestamp_; + overflow_point.time_stamp = + static_cast(new_timestamp_seconds < 0.0 ? 0.0 : 1e9 * new_timestamp_seconds); + + scan_pc_->points.emplace_back(overflow_point); + overflow_pc_->points.pop_back(); + } + + // When there is overflow, the timestamp becomes the overflow packets' one + scan_timestamp_ = last_block_timestamp_; + overflow_pc_->points.clear(); + overflow_pc_->points.reserve(max_pts_); + } + + void unpack(const std::vector & velodyne_packet, double packet_seconds) override + { + velodyne_packet::raw_packet_t raw_instance{}; + std::memcpy(&raw_instance, velodyne_packet.data(), sizeof(velodyne_packet::raw_packet_t)); + + float last_azimuth_diff = 0; + uint16_t azimuth_next; + const uint8_t return_mode = velodyne_packet[velodyne_packet::g_return_mode_index]; + const bool dual_return = (return_mode == velodyne_packet::g_return_mode_dual); + + int num_padding_blocks = sensor_.get_num_padding_blocks(dual_return); + + for (uint block = 0; + block < static_cast(velodyne_packet::g_blocks_per_packet - num_padding_blocks); + block++) { + // Cache block for use. + const velodyne_packet::raw_block_t & current_block = raw_instance.blocks[block]; + + uint bank_origin = 0; + bank_origin = sensor_.get_bank(bank_origin, current_block.flag); + + // Calculate difference between current and next block's azimuth angle. + uint16_t azimuth = (block == 0) ? current_block.rotation : azimuth_next; + + float azimuth_diff = NAN; + if (block < static_cast(velodyne_packet::g_blocks_per_packet - (1 + dual_return))) { + // Get the next block rotation to calculate how far we rotate between blocks + azimuth_next = raw_instance.blocks[block + (1 + dual_return)].rotation; + + // Finds the difference between two successive blocks + azimuth_diff = static_cast((36000 + azimuth_next - azimuth) % 36000); + + // This is used when the last block is next to predict rotation amount + last_azimuth_diff = azimuth_diff; + } else { + // This makes the assumption the difference between the last block and the next packet is + // the same as the last to the second to last. Assumes RPM doesn't change much between + // blocks. + azimuth_diff = (block == static_cast( + velodyne_packet::g_blocks_per_packet - (4 * dual_return) - 1)) + ? 0 + : last_azimuth_diff; + } + assert(!std::isnan(azimuth_diff)); + + // Condition added to avoid calculating points which are not in the interesting defined area + // (cloud_min_angle <= area <= cloud_max_angle). + if ( + (sensor_configuration_->cloud_min_angle >= sensor_configuration_->cloud_max_angle || + azimuth < sensor_configuration_->cloud_min_angle * 100 || + azimuth > sensor_configuration_->cloud_max_angle * 100) && + (sensor_configuration_->cloud_min_angle <= sensor_configuration_->cloud_max_angle || + (azimuth > sensor_configuration_->cloud_max_angle * 100 && + azimuth < sensor_configuration_->cloud_min_angle * 100))) { + continue; + } + + for (int unit_idx = 0; unit_idx < velodyne_packet::g_channels_per_block; ++unit_idx) { + uint16_t other_distance = 0; + int firing_seq = sensor_.get_firing_order(unit_idx, SensorT::channels_per_firing_sequence); + int channel = sensor_.get_channel_number(unit_idx); + + // Distance extraction. + uint16_t current_distance = current_block.units[unit_idx].distance; + if (dual_return) { + other_distance = block % 2 ? raw_instance.blocks[block - 1].units[unit_idx].distance + : raw_instance.blocks[block + 1].units[unit_idx].distance; + } + + // Apply timestamp if this is the first new packet in the scan. + auto block_timestamp = packet_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 ( + (current_distance == 0) || + (dual_return && block % 2 && current_distance == other_distance)) { + continue; + } + + const uint laser_number = + channel + bank_origin; // offset the laser in this block by which block it's in + const uint firing_order = + laser_number / SensorT::num_simultaneous_firings; // VLS-128 fires 8 lasers at a + // time, VLP32 = 2, VLP16 = 1 + + const VelodyneLaserCorrection & corrections = + calibration_configuration_->velodyne_calibration.laser_corrections[laser_number]; + + float distance = current_distance * SensorT::distance_resolution_m; + if (distance > 1e-6) { + distance += corrections.dist_correction; + } + + if ( + distance <= sensor_configuration_->min_range || + distance >= sensor_configuration_->max_range) { + continue; + } + // Correct for the laser rotation as a function of timing during the firings. + uint16_t azimuth_corrected = + sensor_.get_azimuth_corrected(azimuth, azimuth_diff, firing_seq, firing_order); + + // convert polar coordinates to Euclidean XYZ. + const float cos_vert_angle = corrections.cos_vert_correction; + const float sin_vert_angle = corrections.sin_vert_correction; + const float cos_rot_correction = corrections.cos_rot_correction; + const float sin_rot_correction = corrections.sin_rot_correction; + + // select correct azimuth if vlp32 current_block.rotation, if vlp128 and vlp16 + // azimuth_corrected + azimuth_corrected = sensor_.get_true_rotation(azimuth_corrected, current_block.rotation); + const float cos_rot_angle = cos_rot_table_[azimuth_corrected] * cos_rot_correction + + sin_rot_table_[azimuth_corrected] * sin_rot_correction; + const float sin_rot_angle = sin_rot_table_[azimuth_corrected] * cos_rot_correction - + cos_rot_table_[azimuth_corrected] * sin_rot_correction; + + // Compute the distance in the xy plane (w/o accounting for rotation). + const float xy_distance = distance * cos_vert_angle; + + // Use standard ROS coordinate system (right-hand rule). + 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 uint8_t intensity = current_block.units[unit_idx].reflectivity; + + last_block_timestamp_ = block_timestamp; + + double point_time_offset = timing_offsets_[block][channel]; + + // Determine return type. + auto return_type = static_cast(ReturnType::UNKNOWN); + switch (return_mode) { + case velodyne_packet::g_return_mode_dual: + if ((other_distance == 0) || (other_distance == current_distance)) { + return_type = static_cast(drivers::ReturnType::IDENTICAL); + } else { + const uint8_t other_intensity = + block % 2 ? raw_instance.blocks[block - 1].units[unit_idx].reflectivity + : raw_instance.blocks[block + 1].units[unit_idx].reflectivity; + + // TODO(ike-kazu): understand why this differs from VLP16 original decoder + bool first = other_distance >= current_distance; + + bool strongest = other_intensity < intensity; + if (other_intensity == intensity) { + strongest = !first; + } + if (first && strongest) { + return_type = static_cast(drivers::ReturnType::FIRST_STRONGEST); + } else if (!first && strongest) { + return_type = static_cast(drivers::ReturnType::LAST_STRONGEST); + } else if (first && !strongest) { + return_type = static_cast(drivers::ReturnType::FIRST_WEAK); + } else if (!first && !strongest) { + return_type = static_cast(drivers::ReturnType::LAST_WEAK); + } else { + return_type = static_cast(drivers::ReturnType::UNKNOWN); + } + } + break; + case velodyne_packet::g_return_mode_strongest: + return_type = static_cast(drivers::ReturnType::STRONGEST); + break; + case velodyne_packet::g_return_mode_last: + return_type = static_cast(drivers::ReturnType::LAST); + break; + default: + return_type = static_cast(drivers::ReturnType::UNKNOWN); + } + + drivers::NebulaPoint current_point{}; + current_point.x = x_coord; + current_point.y = y_coord; + current_point.z = z_coord; + current_point.return_type = return_type; + current_point.channel = corrections.laser_ring; + current_point.azimuth = rotation_radians_[azimuth_corrected]; + current_point.elevation = sin_vert_angle; + current_point.distance = distance; + 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); + } + } + } + + bool parse_packet( + [[maybe_unused]] const velodyne_msgs::msg::VelodynePacket & velodyne_packet) override + { + return false; + } + +private: + /// @brief Parsing VelodynePacket based on packet structure + /// @param velodyne_packet + /// @return Resulting flag + + // params used by all velodyne decoders + float sin_rot_table_[velodyne_packet::g_rotation_max_units]{}; + float cos_rot_table_[velodyne_packet::g_rotation_max_units]{}; + float rotation_radians_[velodyne_packet::g_rotation_max_units]{}; + int phase_; + int max_pts_{}; + double last_block_timestamp_{}; + std::vector> timing_offsets_; + + SensorT sensor_; +}; + +} // namespace nebula::drivers diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_packet.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_packet.hpp new file mode 100644 index 000000000..1de243460 --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_packet.hpp @@ -0,0 +1,103 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include + +namespace nebula::drivers::velodyne_packet +{ + +/** + * Raw Velodyne packet constants and structures. + */ +static const int g_raw_scan_size = 3; // TODO(ike-kazu): remove +static const int g_scans_per_block = 32; // TODO(ike-kazu): remove +static const int g_channels_per_block = 32; + +static const double g_rotation_resolution = 0.01; // [deg] +static const uint16_t g_rotation_max_units = 360 * 100u; // [deg/100] + +static const size_t g_return_mode_index = 1204; + +/** @todo make this work for both big and little-endian machines */ +static const uint16_t g_upper_bank = 0xeeff; +static const uint16_t g_lower_bank = 0xddff; + +/** Return Modes **/ +static const uint16_t g_return_mode_strongest = 55; +static const uint16_t g_return_mode_last = 56; +static const uint16_t g_return_mode_dual = 57; + +const int g_blocks_per_packet = 12; +const int g_packet_status_size = 4; +const int g_scans_per_packet = (g_scans_per_block * g_blocks_per_packet); +const int g_points_per_packet = (g_scans_per_packet * g_raw_scan_size); + +#pragma pack(push, 1) +/** \brief Raw Velodyne data block. + * + * Each block contains data from either the upper or lower laser + * bank. The device returns three times as many upper bank blocks. + * + * use stdint.h types, so things work with both 64 and 32-bit machines + */ +struct raw_units +{ + uint16_t distance; + uint8_t reflectivity; +}; + +struct raw_block_t +{ + uint16_t flag; ///< UPPER_BANK or LOWER_BANK + uint16_t rotation; ///< 0-35999, divide by 100 to get degrees + raw_units units[g_scans_per_block]; +}; + +/** \brief Raw Velodyne packet. + * + * revolution is described in the device manual as incrementing + * (mod 65536) for each physical turn of the device. Our device + * seems to alternate between two different values every third + * packet. One value increases, the other decreases. + * + * \todo figure out if revolution is only present for one of the + * two types of status fields + * + * status has either a temperature encoding or the microcode level + */ +struct raw_packet_t +{ + raw_block_t blocks[g_blocks_per_packet]; + uint32_t timestamp; + uint16_t factory; +}; +#pragma pack(pop) + +/** \brief Velodyne echo types */ +enum RETURN_TYPE { + INVALID = 0, + SINGLE_STRONGEST = 1, + SINGLE_LAST = 2, + DUAL_STRONGEST_FIRST = 3, + DUAL_STRONGEST_LAST = 4, + DUAL_WEAK_FIRST = 5, + DUAL_WEAK_LAST = 6, + DUAL_ONLY = 7 +}; + +} // namespace nebula::drivers::velodyne_packet 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 416eca3c3..9c36fdbe3 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 @@ -32,133 +32,24 @@ #include #include #include -#include #include #include namespace nebula::drivers { -/** - * Raw Velodyne packet constants and structures. - */ -static const int g_size_block = 100; -static const int g_raw_scan_size = 3; -static const int g_scans_per_block = 32; -static const int g_block_data_size = (g_scans_per_block * g_raw_scan_size); - -static const double g_rotation_resolution = 0.01; // [deg] -static const uint16_t g_rotation_max_units = 36000u; // [deg/100] - -static const size_t g_return_mode_index = 1204; - -/** @todo make this work for both big and little-endian machines */ -static const uint16_t g_upper_bank = 0xeeff; -static const uint16_t g_lower_bank = 0xddff; - -/** Return Modes **/ -static const uint16_t g_return_mode_strongest = 55; -static const uint16_t g_return_mode_last = 56; -static const uint16_t g_return_mode_dual = 57; - -/** Special Defines for VLP16 support **/ -static const int g_vlp16_firings_per_block = 2; -static const int g_vlp16_scans_per_firing = 16; -static const float g_vlp16_block_duration = 110.592f; // [µs] -static const float g_vlp16_dsr_toffset = 2.304f; // [µs] -static const float g_vlp16_firing_toffset = 55.296f; // [µs] - -/** Special Defines for VLP32 support **/ -static const float g_vlp32_channel_duration = 2.304f; // [µs] -static const float g_vlp32_seq_duration = 55.296f; // [µs] - -/** Special Definitions for VLS128 support **/ -static const float g_vlp128_distance_resolution = 0.004f; // [m] - -/** Special Definitions for VLS128 support **/ -// These are used to detect which bank of 32 lasers is in this block -static const uint16_t g_vls128_bank_1 = 0xeeff; -static const uint16_t g_vls128_bank_2 = 0xddff; -static const uint16_t g_vls128_bank_3 = 0xccff; -static const uint16_t g_vls128_bank_4 = 0xbbff; - -static const float g_vls128_channel_duration = - 2.665f; // [µs] Channels corresponds to one laser firing -static const float g_vls128_seq_duration = - 53.3f; // [µs] Sequence is a set of laser firings including recharging - -static const size_t g_offset_first_azimuth = 2; -static const size_t g_offset_last_azimuth = 1102; -static const uint32_t g_degree_subdivisions = 100; - -/** \brief Raw Velodyne data block. - * - * Each block contains data from either the upper or lower laser - * bank. The device returns three times as many upper bank blocks. - * - * use stdint.h types, so things work with both 64 and 32-bit machines - */ -typedef struct raw_block -{ - uint16_t header; ///< UPPER_BANK or LOWER_BANK - uint16_t rotation; ///< 0-35999, divide by 100 to get degrees - uint8_t data[g_block_data_size]; -} raw_block_t; - -/** used for unpacking the first two data bytes in a block - * - * They are packed into the actual data stream misaligned. I doubt - * this works on big endian machines. - */ -union two_bytes { - uint16_t uint; - uint8_t bytes[2]; -}; - -static const int g_packet_size = 1206; -static const int g_blocks_per_packet = 12; -static const int g_packet_status_size = 4; -static const int g_scans_per_packet = (g_scans_per_block * g_blocks_per_packet); - -/** \brief Raw Velodyne packet. - * - * revolution is described in the device manual as incrementing - * (mod 65536) for each physical turn of the device. Our device - * seems to alternate between two different values every third - * packet. One value increases, the other decreases. - * - * \todo figure out if revolution is only present for one of the - * two types of status fields - * - * status has either a temperature encoding or the microcode level - */ -typedef struct raw_packet -{ - raw_block_t blocks[g_blocks_per_packet]; - uint16_t revolution; - uint8_t status[g_packet_status_size]; -} raw_packet_t; - -/** \brief Velodyne echo types */ -enum RETURN_TYPE { - INVALID = 0, - SINGLE_STRONGEST = 1, - SINGLE_LAST = 2, - DUAL_STRONGEST_FIRST = 3, - DUAL_STRONGEST_LAST = 4, - DUAL_WEAK_FIRST = 5, - DUAL_WEAK_LAST = 6, - DUAL_ONLY = 7 -}; - /// @brief Base class for Velodyne LiDAR decoder class VelodyneScanDecoder { private: size_t processed_packets_{0}; uint32_t prev_packet_first_azm_phased_{0}; - bool has_scanned_{false}; + + static const size_t g_offset_first_azimuth = 2; + static const size_t g_offset_last_azimuth = 1102; + static const uint32_t g_degree_subdivisions = 100; protected: + bool has_scanned_{false}; /// @brief Checks if the current packet completes the ongoing scan. /// TODO: this has been moved from velodyne_hw_interface.cpp and is a temporary solution until /// the Velodyne decoder uses the same structure as Hesai/Robosense @@ -227,11 +118,7 @@ class VelodyneScanDecoder /// @brief Virtual function for getting the flag indicating whether one cycle is ready /// @return Readied - bool has_scanned() { return has_scanned_; } - - /// @brief Calculation of points in each packet - /// @return # of points - virtual int points_per_packet() = 0; + virtual bool has_scanned() = 0; /// @brief Virtual function for getting the constructed point cloud /// @return tuple of Point cloud and timestamp diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_sensor.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_sensor.hpp new file mode 100644 index 000000000..8b28cfb4e --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_sensor.hpp @@ -0,0 +1,60 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include + +namespace nebula::drivers +{ +class VelodyneSensor +{ +public: + VelodyneSensor() = default; + VelodyneSensor(const VelodyneSensor &) = default; + VelodyneSensor(VelodyneSensor &&) = default; + VelodyneSensor & operator=(const VelodyneSensor &) = default; + VelodyneSensor & operator=(VelodyneSensor &&) = default; + virtual ~VelodyneSensor() = default; + + /// @brief each VLP lidars packat structure in user manual. If you know details, see commens in + /// each .hpp file. To ignore an empty data blocks which is created by only VLS128 dual + /// return mode case + virtual int get_num_padding_blocks(bool /* dual_return */) { return 0; } + + /// @brief each VLP lidar laser timing in user manual. If you know details, see commens in each + /// .hpp file. calculate and stack the firing timing for each laser timeing used in + /// getAzimuthCorrected to calculate the corrected azimuth + virtual bool fill_azimuth_cache() { return false; } + + /// @brief VSL128User manual p. Packet structure + virtual uint32_t get_bank(uint32_t bank, uint32_t /* header */) { return bank; } + + /// @brief each VLP calculating sample code and formula in user manual. If you know details, see + /// commens in each .hpp file. calculate the corrected azimuth from each firing timing. + virtual uint16_t get_azimuth_corrected( + uint16_t azimuth, float azimuth_diff, int firing_sequence, int firing_order) = 0; + + /// @brief each VLP calculating sample code and formula in user manual. Check packet structure. + /// Get a correct firing order + virtual int get_firing_order(int /* channels_per_block */, int /* scans_per_firing */) + { + return 0; + } + + /// @brief each VLP calculating sample code and formula in user manual. Check packet structure. + /// Get a correct channel number + virtual int get_channel_number(int /* unit_idx */) { return 0; } +}; +} // namespace nebula::drivers 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 deleted file mode 100644 index 78c963618..000000000 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp16_decoder.hpp +++ /dev/null @@ -1,69 +0,0 @@ -// Copyright 2024 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#pragma once - -#include "nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp" - -#include -#include - -#include -#include -#include -#include - -namespace nebula::drivers::vlp16 -{ -constexpr uint32_t max_points = 300000; -/// @brief Velodyne LiDAR decoder (VLP16) -class Vlp16Decoder : public VelodyneScanDecoder -{ -public: - /// @brief Constructor - /// @param sensor_configuration SensorConfiguration for this decoder - /// @param calibration_configuration Calibration for this decoder - explicit Vlp16Decoder( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & - calibration_configuration); - /// @brief Parsing and shaping VelodynePacket - /// @param velodyne_packet - void unpack(const std::vector & packet, double packet_seconds) override; - /// @brief Calculation of points in each packet - /// @return # of points - int points_per_packet() override; - /// @brief Get the constructed point cloud - /// @return tuple of Point cloud and timestamp - std::tuple get_pointcloud() override; - /// @brief Resetting point cloud buffer - void reset_pointcloud(double time_stamp) override; - /// @brief Resetting overflowed point cloud buffer - void reset_overflow(double time_stamp) override; - -private: - /// @brief Parsing VelodynePacket based on packet structure - /// @param velodyne_packet - /// @return Resulting flag - bool parse_packet(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) override; - float sin_rot_table_[g_rotation_max_units]; - float cos_rot_table_[g_rotation_max_units]; - float rotation_radians_[g_rotation_max_units]; - int phase_; - int max_pts_; - double last_block_timestamp_; - std::vector> timing_offsets_; -}; - -} // namespace nebula::drivers::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 deleted file mode 100644 index 727111321..000000000 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp32_decoder.hpp +++ /dev/null @@ -1,68 +0,0 @@ -// Copyright 2024 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#pragma once - -#include "nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp" - -#include -#include - -#include -#include -#include -#include - -namespace nebula::drivers::vlp32 -{ -/// @brief Velodyne LiDAR decoder (VLP32) -class Vlp32Decoder : public VelodyneScanDecoder -{ -public: - /// @brief Constructor - /// @param sensor_configuration SensorConfiguration for this decoder - /// @param calibration_configuration Calibration for this decoder - explicit Vlp32Decoder( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & - calibration_configuration); - /// @brief Parsing and shaping VelodynePacket - /// @param velodyne_packet - void unpack(const std::vector & packet, double packet_seconds) override; - /// @brief Calculation of points in each packet - /// @return # of points - int points_per_packet() override; - /// @brief Get the constructed point cloud - /// @return tuple of Point cloud and timestamp - std::tuple get_pointcloud() override; - /// @brief Resetting point cloud buffer - void reset_pointcloud(double time_stamp) override; - /// @brief Resetting overflowed point cloud buffer - void reset_overflow(double time_stamp) override; - -private: - /// @brief Parsing VelodynePacket based on packet structure - /// @param velodyne_packet - /// @return Resulting flag - bool parse_packet(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) override; - float sin_rot_table_[g_rotation_max_units]; - float cos_rot_table_[g_rotation_max_units]; - float rotation_radians_[g_rotation_max_units]; - std::vector> timing_offsets_; - int phase_; - int max_pts_; - double last_block_timestamp_; -}; - -} // namespace nebula::drivers::vlp32 diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp_16.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp_16.hpp new file mode 100644 index 000000000..c634201df --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp_16.hpp @@ -0,0 +1,82 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include "nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_sensor.hpp" + +#include + +namespace nebula::drivers +{ + +class VLP16 : public VelodyneSensor +{ +public: + /// @brief formula from VLP16 User manual in p.64 + /// @param azimuth Azimuth angle + /// @param azimuth_diff Azimuth difference + /// @param firing_sequence Firing sequence + /// @param firing_order Firing order + /// @return Corrected azimuth + uint16_t get_azimuth_corrected( + uint16_t azimuth, float azimuth_diff, int firing_sequence, int firing_order) override + { + float azimuth_corrected = + azimuth + (azimuth_diff * + ((firing_order * vlp16_dsr_toffset) + (firing_sequence * vlp16_firing_toffset)) / + vlp16_block_duration); + + return static_cast(round(azimuth_corrected)) % 36000; + } + + // Not succeed nebula_test on only VLP32 so add this function + // Choose the correct azimuth from the 2 azimuths + static uint16_t get_true_rotation( + uint16_t azimuth_corrected, uint16_t /* current_block_rotation */) + { + return azimuth_corrected; + } + + int get_firing_order(int channels, int scans_per_firing) override + { + return channels / scans_per_firing; + } + + int get_channel_number(int unit_idx) override { return unit_idx % channels_per_firing_sequence; } + + constexpr static int num_maintenance_periods = 0; + + constexpr static int num_simultaneous_firings = 1; + + constexpr static double firing_sequences_per_block = 2.0; + + constexpr static int channels_per_firing_sequence = 16; + + constexpr static float distance_resolution_m = 0.002f; + + constexpr static double full_firing_cycle_s = 55.296 * 1e-6; + + constexpr static double single_firing_s = 2.304 * 1e-6; + + constexpr static double offset_packet_time = 0; + + /** Special Defines for VLP16 support **/ + constexpr static const int vlp16_firings_per_block = 2; + constexpr static const int vlp16_scans_per_firing = 16; + constexpr static const float vlp16_block_duration = 110.592f; // [µs] + constexpr static const float vlp16_dsr_toffset = 2.304f; // [µs] + constexpr static const float vlp16_firing_toffset = 55.296f; // [µs] +}; +} // namespace nebula::drivers diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp_32.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp_32.hpp new file mode 100644 index 000000000..9062dbfdf --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp_32.hpp @@ -0,0 +1,88 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include "nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_sensor.hpp" + +#include + +namespace nebula::drivers +{ +class VLP32 : public VelodyneSensor +{ +public: + // calculate and stack the firing timing for each laser timeing + /// @brief laser timing for VLP32 from VLP32 User manual in p.61 + bool fill_azimuth_cache() override + { + for (uint8_t i = 0; i < 16; i++) { + laser_azimuth_cache_[i] = (vlp32_channel_duration / vlp32_seq_duration) * (i + i / 2); + } + return true; + } + + /// @brief formula from VLP32 User manual in p.62 + /// @param azimuth Azimuth angle + /// @param azimuth_diff Azimuth difference between a current azimuth and a next azimuth + /// @param firing_order Firing order + /// @return Corrected azimuth + uint16_t get_azimuth_corrected( + uint16_t azimuth, float azimuth_diff, int /* firing_sequence */, int firing_order) override + { + float azimuth_corrected = azimuth + (azimuth_diff * laser_azimuth_cache_[firing_order]); + + return static_cast(round(azimuth_corrected)) % 36000; + } + + // Not succeed nebula_test on only VLP32 so add this function + // Choose the correct azimuth from the 2 azimuths + uint16_t get_true_rotation(uint16_t /* azimuth_corrected */, uint16_t current_block_rotation) + { + return current_block_rotation; + } + + int get_firing_order(int channels, int scans_per_firing) override + { + return channels / scans_per_firing; + } + + int get_channel_number(int unit_idx) override { return unit_idx % channels_per_firing_sequence; } + + constexpr static int num_maintenance_periods = 0; + + constexpr static int num_simultaneous_firings = 2; + + constexpr static double firing_sequences_per_block = 1.0; + + constexpr static int channels_per_firing_sequence = 32; + + constexpr static float distance_resolution_m = 0.004f; + + constexpr static double full_firing_cycle_s = 55.296 * 1e-6; + + constexpr static double single_firing_s = 2.304 * 1e-6; + + constexpr static double offset_packet_time = 0; + + /** Special Definitions for VLS32 support **/ + constexpr static const float vlp32_channel_duration = + 2.304f; // [µs] Channels corresponds to one laser firing + constexpr static const float vlp32_seq_duration = + 55.296f; // [µs] Sequence is a set of laser firings including recharging + +private: + float laser_azimuth_cache_[16]; +}; +} // namespace nebula::drivers 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 deleted file mode 100644 index fe1a81975..000000000 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vls128_decoder.hpp +++ /dev/null @@ -1,69 +0,0 @@ -// Copyright 2024 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#pragma once - -#include "nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp" - -#include -#include - -#include -#include -#include -#include - -namespace nebula::drivers::vls128 -{ -/// @brief Velodyne LiDAR decoder (VLS128) -class Vls128Decoder : public VelodyneScanDecoder -{ -public: - /// @brief Constructor - /// @param sensor_configuration SensorConfiguration for this decoder - /// @param calibration_configuration Calibration for this decoder - explicit Vls128Decoder( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & - calibration_configuration); - /// @brief Parsing and shaping VelodynePacket - /// @param velodyne_packet - void unpack(const std::vector & packet, double packet_seconds) override; - /// @brief Calculation of points in each packet - /// @return # of points - int points_per_packet() override; - /// @brief Get the constructed point cloud - /// @return tuple of Point cloud and timestamp - std::tuple get_pointcloud() override; - /// @brief Resetting point cloud buffer - void reset_pointcloud(double time_stamp) override; - /// @brief Resetting overflowed point cloud buffer - void reset_overflow(double time_stamp) override; - -private: - /// @brief Parsing VelodynePacket based on packet structure - /// @param velodyne_packet - /// @return Resulting flag - bool parse_packet(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) override; - float sin_rot_table_[g_rotation_max_units]; - float cos_rot_table_[g_rotation_max_units]; - float rotation_radians_[g_rotation_max_units]; - float vls_128_laser_azimuth_cache_[16]; - int phase_; - int max_pts_; - double last_block_timestamp_; - std::vector> timing_offsets_; -}; - -} // namespace nebula::drivers::vls128 diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vls_128.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vls_128.hpp new file mode 100644 index 000000000..f29c7fb73 --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vls_128.hpp @@ -0,0 +1,134 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include "nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_sensor.hpp" + +#include +#include + +#include + +namespace nebula::drivers +{ + +class VLS128 : public VelodyneSensor +{ +public: + // To ignore an empty data blocks in VLS128 case + /// @brief VLS128 Dual return mode data structure in VLS128 User manual p.57 + int get_num_padding_blocks(bool dual_return) override + { + if (dual_return) return 4; + return 0; + } + + // calculate and stack the firing timing for each laser timeing + /// @brief laser timing for VLS128 from VLS128 User manual in p.61 + bool fill_azimuth_cache() override + { + for (uint8_t i = 0; i < 16; i++) { + laser_azimuth_cache_[i] = (vls128_channel_duration / vls128_seq_duration) * (i + i / 8); + } + return true; + } + + /// @brief formula from VLS128 User manual in p.65 + /// @param azimuth Azimuth angle + /// @param azimuth_diff Azimuth difference + /// @param firing_order Firing order + /// @return Corrected azimuth + uint16_t get_azimuth_corrected( + uint16_t azimuth, float azimuth_diff, int /* firing_sequence */, int firing_order) override + { + float azimuth_corrected = azimuth + (azimuth_diff * laser_azimuth_cache_[firing_order]); + + return static_cast(round(azimuth_corrected)) % 36000; + } + + // Not succeed nebula_test on only VLP32 so add this function + // Choose the correct azimuth from the 2 azimuths + static uint16_t get_true_rotation( + uint16_t azimuth_corrected, uint16_t /* current_block_rotation */) + { + return azimuth_corrected; + } + + uint32_t get_bank(uint32_t bank, uint32_t header) override + { + // Used to detect which bank of 32 lasers is in this block. + switch (header) { + case bank_1: + bank = 0; + break; + case bank_2: + bank = 32; + break; + case bank_3: + bank = 64; + break; + case bank_4: + bank = 96; + break; + default: + RCLCPP_ERROR( + rclcpp::get_logger("VelodyneDecoder"), + "Invalid bank origin detected in packet. Skipping packet."); + return 0; // bad packet: skip the rest + } + return bank; + } + + int get_firing_order(int channels, int scans_per_firing) override + { + return channels / scans_per_firing; + } + + int get_channel_number(int unit_idx) override { return unit_idx % channels_per_firing_sequence; } + + constexpr static int num_maintenance_periods = 1; + + constexpr static int num_simultaneous_firings = 8; + + constexpr static double firing_sequences_per_block = 0.25; + + constexpr static int channels_per_firing_sequence = 128; + + constexpr static float distance_resolution_m = 0.004f; + + constexpr static double full_firing_cycle_s = 53.3 * 1e-6; + + constexpr static double single_firing_s = 2.665 * 1e-6; + + constexpr static double offset_packet_time = 8.7 * 1e-6; + + /** Special Definitions for VLS128 support **/ + constexpr static const float vls128_distance_resolution = 0.004f; // [m] + + constexpr static const float vls128_channel_duration = + 2.665f; // [µs] Channels corresponds to one laser firing + + constexpr static const float vls128_seq_duration = + 53.3f; // [µs] Sequence is a set of laser firings including recharging + // These are used to detect which bank of 32 lasers is in this block + constexpr static const uint16_t bank_1 = 0xeeff; + constexpr static const uint16_t bank_2 = 0xddff; + constexpr static const uint16_t bank_3 = 0xccff; + constexpr static const uint16_t bank_4 = 0xbbff; + +private: + float laser_azimuth_cache_[16]{}; +}; +} // namespace nebula::drivers diff --git a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/velodyne_driver.hpp b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/velodyne_driver.hpp index d2b4ca3d0..d36096ab4 100644 --- a/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/velodyne_driver.hpp +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/velodyne_driver.hpp @@ -19,7 +19,6 @@ #include "nebula_common/nebula_status.hpp" #include "nebula_common/point_types.hpp" #include "nebula_common/velodyne/velodyne_common.hpp" -#include "nebula_decoders/nebula_decoders_common/nebula_driver_base.hpp" #include "nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp" #include @@ -27,17 +26,14 @@ #include -#include #include -#include -#include #include #include namespace nebula::drivers { /// @brief Velodyne driver -class VelodyneDriver : NebulaDriverBase +class VelodyneDriver { private: /// @brief Current driver status @@ -59,7 +55,7 @@ class VelodyneDriver : NebulaDriverBase /// @param calibration_configuration /// @return Resulting status Status set_calibration_configuration( - const CalibrationConfigurationBase & calibration_configuration) override; + const CalibrationConfigurationBase & calibration_configuration); /// @brief Get current status of this driver /// @return Current status diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp deleted file mode 100644 index e6dea54e4..000000000 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp +++ /dev/null @@ -1,337 +0,0 @@ -// Copyright 2024 TIER IV, Inc. - -#include "nebula_decoders/nebula_decoders_velodyne/decoders/vlp16_decoder.hpp" - -#include - -#include -#include - -namespace nebula -{ -namespace drivers::vlp16 -{ -Vlp16Decoder::Vlp16Decoder( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & - calibration_configuration) -{ - sensor_configuration_ = sensor_configuration; - calibration_configuration_ = calibration_configuration; - - scan_timestamp_ = -1; - - scan_pc_.reset(new NebulaPointCloud); - overflow_pc_.reset(new NebulaPointCloud); - - // Set up cached values for sin and cos of all the possible headings - for (uint16_t rot_index = 0; rot_index < g_rotation_max_units; ++rot_index) { - float rotation = angles::from_degrees(g_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(g_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); -} - -std::tuple Vlp16Decoder::get_pointcloud() -{ - double phase = angles::from_degrees(sensor_configuration_->scan_phase); - if (!scan_pc_->points.empty()) { - 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 = 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_, scan_timestamp_); -} - -int Vlp16Decoder::points_per_packet() -{ - return g_blocks_per_packet * g_vlp16_firings_per_block * g_vlp16_scans_per_firing; -} - -void Vlp16Decoder::reset_pointcloud(double time_stamp) -{ - scan_pc_->clear(); - reset_overflow(time_stamp); // transfer existing overflow points to the cleared pointcloud -} - -void Vlp16Decoder::reset_overflow(double time_stamp) -{ - if (overflow_pc_->points.size() == 0) { - scan_timestamp_ = -1; - overflow_pc_->points.reserve(max_pts_); - return; - } - - // Compute the absolute time stamp of the last point of the overflow pointcloud - const double last_overflow_time_stamp = - scan_timestamp_ + 1e-9 * overflow_pc_->points.back().time_stamp; - - // Detect cases where there is an unacceptable time difference between the last overflow point and - // the first point of the next packet. In that case, there was probably a packet drop so it is - // better to ignore the overflow pointcloud - if (time_stamp - last_overflow_time_stamp > 0.05) { - scan_timestamp_ = -1; - overflow_pc_->points.clear(); - overflow_pc_->points.reserve(max_pts_); - return; - } - - // Add the overflow buffer points - while (overflow_pc_->points.size() > 0) { - auto overflow_point = overflow_pc_->points.back(); - - // The overflow points had the stamps from the previous pointcloud. These need to be changed to - // be relative to the overflow's packet timestamp - double new_timestamp_seconds = - scan_timestamp_ + 1e-9 * overflow_point.time_stamp - last_block_timestamp_; - overflow_point.time_stamp = - static_cast(new_timestamp_seconds < 0.0 ? 0.0 : 1e9 * new_timestamp_seconds); - - scan_pc_->points.emplace_back(overflow_point); - overflow_pc_->points.pop_back(); - } - - // When there is overflow, the timestamp becomes the overflow packets' one - scan_timestamp_ = last_block_timestamp_; - overflow_pc_->points.clear(); - overflow_pc_->points.reserve(max_pts_); -} - -void Vlp16Decoder::unpack(const std::vector & packet, double packet_seconds) -{ - check_and_handle_scan_complete(packet, packet_seconds, phase_); - - const raw_packet_t * raw = (const raw_packet_t *)packet.data(); - float last_azimuth_diff = 0; - uint16_t azimuth_next; - const uint8_t return_mode = packet[g_return_mode_index]; - const bool dual_return = (return_mode == g_return_mode_dual); - - for (uint block = 0; block < g_blocks_per_packet; block++) { - // Cache block for use. - const raw_block_t & current_block = raw->blocks[block]; - if (g_upper_bank != raw->blocks[block].header) { - // Do not flood the log with messages, only issue at most one - // of these warnings per minute. - return; // bad packet: skip the rest - } - - float azimuth_diff; - uint16_t azimuth; - - // Calculate difference between current and next block's azimuth angle. - if (block == 0) { - azimuth = current_block.rotation; - } else { - azimuth = azimuth_next; - } - if (block < static_cast(g_blocks_per_packet - (1 + dual_return))) { - // Get the next block rotation to calculate how far we rotate between blocks. - azimuth_next = raw->blocks[block + (1 + dual_return)].rotation; - - // Finds the difference between two successive blocks. - azimuth_diff = static_cast((36000 + azimuth_next - azimuth) % 36000); - - // This is used when the last block is next to predict rotation amount - last_azimuth_diff = azimuth_diff; - } else { - // This makes the assumption the difference between the last block and the next packet is the - // same as the last to the second to last. - // Assumes RPM doesn't change much between blocks. - azimuth_diff = - (block == static_cast(g_blocks_per_packet - dual_return - 1) ? 0 : last_azimuth_diff); - } - - // Condition added to avoid calculating points which are not in the interesting defined area - // (min_angle < area < max_angle). - if ( - (sensor_configuration_->cloud_min_angle < sensor_configuration_->cloud_max_angle && - azimuth >= sensor_configuration_->cloud_min_angle * 100 && - azimuth <= sensor_configuration_->cloud_max_angle * 100) || - (sensor_configuration_->cloud_min_angle > sensor_configuration_->cloud_max_angle)) { - for (int firing = 0, k = 0; firing < g_vlp16_firings_per_block; ++firing) { - for (int dsr = 0; dsr < g_vlp16_scans_per_firing; dsr++, k += g_raw_scan_size) { - union two_bytes current_return; - union two_bytes other_return; - // Distance extraction. - current_return.bytes[0] = current_block.data[k]; - current_return.bytes[1] = current_block.data[k + 1]; - - if (dual_return) { - other_return.bytes[0] = - block % 2 ? raw->blocks[block - 1].data[k] : raw->blocks[block + 1].data[k]; - 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 = packet_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 ( - (current_return.bytes[0] == 0 && current_return.bytes[1] == 0) || - (dual_return && block % 2 && other_return.bytes[0] == current_return.bytes[0] && - other_return.bytes[1] == current_return.bytes[1])) { - continue; - } - { - const VelodyneLaserCorrection & corrections = - calibration_configuration_->velodyne_calibration.laser_corrections[dsr]; - float distance = current_return.uint * - calibration_configuration_->velodyne_calibration.distance_resolution_m; - if (distance > 1e-6) { - distance += corrections.dist_correction; - } - - if ( - distance > sensor_configuration_->min_range && - distance < sensor_configuration_->max_range) { - // Correct for the laser rotation as a function of timing during the firings. - float azimuth_corrected_f = - azimuth + - (azimuth_diff * ((dsr * g_vlp16_dsr_toffset) + (firing * g_vlp16_firing_toffset)) / - g_vlp16_block_duration) - - corrections.rot_correction * 180.0 / M_PI * 100; - - if (azimuth_corrected_f < 0.0) { - azimuth_corrected_f += 36000.0; - } - const uint16_t azimuth_corrected = - (static_cast(round(azimuth_corrected_f))) % 36000; - - // Condition added to avoid calculating points which are not in the interesting - // defined area (min_angle < area < max_angle). - if ( - (azimuth_corrected >= sensor_configuration_->cloud_min_angle * 100 && - azimuth_corrected <= sensor_configuration_->cloud_max_angle * 100 && - sensor_configuration_->cloud_min_angle < sensor_configuration_->cloud_max_angle) || - (sensor_configuration_->cloud_min_angle > sensor_configuration_->cloud_max_angle && - (azimuth_corrected <= sensor_configuration_->cloud_max_angle * 100 || - azimuth_corrected >= sensor_configuration_->cloud_min_angle * 100))) { - // Convert polar coordinates to Euclidean XYZ. - const float cos_vert_angle = corrections.cos_vert_correction; - const float sin_vert_angle = corrections.sin_vert_correction; - const float cos_rot_angle = cos_rot_table_[azimuth_corrected]; - const float sin_rot_angle = sin_rot_table_[azimuth_corrected]; - - // Compute the distance in the xy plane (w/o accounting for rotation). - const float xy_distance = distance * cos_vert_angle; - - // Use standard ROS coordinate system (right-hand rule). - 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 uint8_t intensity = current_block.data[k + 2]; - - last_block_timestamp_ = block_timestamp; - - double point_time_offset = timing_offsets_[block][firing * 16 + dsr]; - - // Determine return type. - uint8_t return_type; - switch (return_mode) { - case g_return_mode_dual: - if ( - (other_return.bytes[0] == 0 && other_return.bytes[1] == 0) || - (other_return.bytes[0] == current_return.bytes[0] && - other_return.bytes[1] == current_return.bytes[1])) { - return_type = static_cast(drivers::ReturnType::IDENTICAL); - } else { - const uint8_t other_intensity = block % 2 - ? raw->blocks[block - 1].data[k + 2] - : raw->blocks[block + 1].data[k + 2]; - bool first = current_return.uint > other_return.uint; - bool strongest = intensity > other_intensity; - if (other_intensity == intensity) { - strongest = !first; - } - if (first && strongest) { - return_type = static_cast(drivers::ReturnType::FIRST_STRONGEST); - } else if (!first && strongest) { - return_type = static_cast(drivers::ReturnType::LAST_STRONGEST); - } else if (first && !strongest) { - return_type = static_cast(drivers::ReturnType::FIRST_WEAK); - } else if (!first && !strongest) { - return_type = static_cast(drivers::ReturnType::LAST_WEAK); - } else { - return_type = static_cast(drivers::ReturnType::UNKNOWN); - } - } - break; - case g_return_mode_strongest: - return_type = static_cast(drivers::ReturnType::STRONGEST); - break; - case g_return_mode_last: - return_type = static_cast(drivers::ReturnType::LAST); - break; - default: - return_type = static_cast(drivers::ReturnType::UNKNOWN); - } - drivers::NebulaPoint current_point{}; - current_point.x = x_coord; - current_point.y = y_coord; - current_point.z = z_coord; - current_point.return_type = return_type; - current_point.channel = corrections.laser_ring; - 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); - } - } - } - } - } - } - } -} - -bool Vlp16Decoder::parse_packet( - [[maybe_unused]] const velodyne_msgs::msg::VelodynePacket & velodyne_packet) -{ - return 0; -} - -} // namespace drivers::vlp16 -} // namespace nebula diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp deleted file mode 100644 index d97b6edab..000000000 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp +++ /dev/null @@ -1,393 +0,0 @@ -// Copyright 2024 TIER IV, Inc. - -#include "nebula_decoders/nebula_decoders_velodyne/decoders/vlp32_decoder.hpp" - -#include - -#include -#include - -namespace nebula -{ -namespace drivers::vlp32 -{ -Vlp32Decoder::Vlp32Decoder( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & - calibration_configuration) -{ - sensor_configuration_ = sensor_configuration; - calibration_configuration_ = calibration_configuration; - - scan_timestamp_ = -1; - - scan_pc_.reset(new NebulaPointCloud); - overflow_pc_.reset(new NebulaPointCloud); - - // Set up cached values for sin and cos of all the possible headings - for (uint16_t rot_index = 0; rot_index < g_rotation_max_units; ++rot_index) { - float rotation = angles::from_degrees(g_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); - } - } -} - -std::tuple Vlp32Decoder::get_pointcloud() -{ - double phase = angles::from_degrees(sensor_configuration_->scan_phase); - if (!scan_pc_->points.empty()) { - 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 = 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_, scan_timestamp_); -} - -int Vlp32Decoder::points_per_packet() -{ - return g_blocks_per_packet * g_scans_per_block; -} - -void Vlp32Decoder::reset_pointcloud(double time_stamp) -{ - scan_pc_->clear(); - reset_overflow(time_stamp); // transfer existing overflow points to the cleared pointcloud -} - -void Vlp32Decoder::reset_overflow(double time_stamp) -{ - if (overflow_pc_->points.size() == 0) { - scan_timestamp_ = -1; - overflow_pc_->points.reserve(max_pts_); - return; - } - - // Compute the absolute time stamp of the last point of the overflow pointcloud - const double last_overflow_time_stamp = - scan_timestamp_ + 1e-9 * overflow_pc_->points.back().time_stamp; - - // Detect cases where there is an unacceptable time difference between the last overflow point and - // the first point of the next packet. In that case, there was probably a packet drop so it is - // better to ignore the overflow pointcloud - if (time_stamp - last_overflow_time_stamp > 0.05) { - scan_timestamp_ = -1; - overflow_pc_->points.clear(); - overflow_pc_->points.reserve(max_pts_); - return; - } - - // Add the overflow buffer points - while (overflow_pc_->points.size() > 0) { - auto overflow_point = overflow_pc_->points.back(); - - // The overflow points had the stamps from the previous pointcloud. These need to be changed to - // be relative to the overflow's packet timestamp - double new_timestamp_seconds = - scan_timestamp_ + 1e-9 * overflow_point.time_stamp - last_block_timestamp_; - overflow_point.time_stamp = - static_cast(new_timestamp_seconds < 0.0 ? 0.0 : 1e9 * new_timestamp_seconds); - - scan_pc_->points.emplace_back(overflow_point); - overflow_pc_->points.pop_back(); - } - - // When there is overflow, the timestamp becomes the overflow packets' one - scan_timestamp_ = last_block_timestamp_; - overflow_pc_->points.clear(); - overflow_pc_->points.reserve(max_pts_); -} - -void Vlp32Decoder::unpack(const std::vector & packet, double packet_seconds) -{ - check_and_handle_scan_complete(packet, packet_seconds, phase_); - - const raw_packet_t * raw = (const raw_packet_t *)packet.data(); - float last_azimuth_diff = 0; - uint16_t azimuth_next; - uint8_t return_mode = packet[g_return_mode_index]; - const bool dual_return = (return_mode == g_return_mode_dual); - - for (uint i = 0; i < g_blocks_per_packet; i++) { - int bank_origin = 0; - if (raw->blocks[i].header == g_lower_bank) { - // lower bank lasers are [32..63] - bank_origin = 32; - } - float azimuth_diff; - uint16_t azimuth; - - // Calculate difference between current and next block's azimuth angle. - if (i == 0) { - azimuth = raw->blocks[i].rotation; - } else { - azimuth = azimuth_next; - } - if (i < static_cast(g_blocks_per_packet - (1 + dual_return))) { - // Get the next block rotation to calculate how far we rotate between blocks - azimuth_next = raw->blocks[i + (1 + dual_return)].rotation; - - // Finds the difference between two successive blocks - azimuth_diff = static_cast((36000 + azimuth_next - azimuth) % 36000); - - // This is used when the last block is next to predict rotation amount - last_azimuth_diff = azimuth_diff; - } else { - // This makes the assumption the difference between the last block and the next packet is the - // same as the last to the second to last. - // Assumes RPM doesn't change much between blocks. - azimuth_diff = (i == static_cast(g_blocks_per_packet - (4 * dual_return) - 1)) - ? 0 - : last_azimuth_diff; - } - - for (uint j = 0, k = 0; j < g_scans_per_block; j++, k += g_raw_scan_size) { - float x, y, z; - uint8_t intensity; - const uint8_t laser_number = j + bank_origin; - - const VelodyneLaserCorrection & corrections = - calibration_configuration_->velodyne_calibration.laser_corrections[laser_number]; - - /** Position Calculation */ - const raw_block_t & block = raw->blocks[i]; - union two_bytes current_return; - current_return.bytes[0] = block.data[k]; - current_return.bytes[1] = block.data[k + 1]; - - union two_bytes other_return; - if (dual_return) { - other_return.bytes[0] = i % 2 ? raw->blocks[i - 1].data[k] : raw->blocks[i + 1].data[k]; - 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 = packet_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 ( - (current_return.bytes[0] == 0 && current_return.bytes[1] == 0) || - (dual_return && i % 2 && other_return.bytes[0] == current_return.bytes[0] && - other_return.bytes[1] == current_return.bytes[1])) { - continue; - } - - float distance = current_return.uint * - calibration_configuration_->velodyne_calibration.distance_resolution_m; - if (distance > 1e-6) { - distance += corrections.dist_correction; - } - - if ( - distance > sensor_configuration_->min_range && - distance < sensor_configuration_->max_range) { - /*condition added to avoid calculating points which are not - in the interesting defined area (min_angle < area < max_angle)*/ - if ( - (block.rotation >= sensor_configuration_->cloud_min_angle * 100 && - block.rotation <= sensor_configuration_->cloud_max_angle * 100 && - sensor_configuration_->cloud_min_angle < sensor_configuration_->cloud_max_angle) || - (sensor_configuration_->cloud_min_angle > sensor_configuration_->cloud_max_angle && - (raw->blocks[i].rotation <= sensor_configuration_->cloud_max_angle * 100 || - raw->blocks[i].rotation >= sensor_configuration_->cloud_min_angle * 100))) { - const float cos_vert_angle = corrections.cos_vert_correction; - const float sin_vert_angle = corrections.sin_vert_correction; - float azimuth_corrected_f = - azimuth + (azimuth_diff * g_vlp32_channel_duration / g_vlp32_seq_duration * j) - - corrections.rot_correction * 180.0 / M_PI * 100; - if (azimuth_corrected_f < 0) { - azimuth_corrected_f += 36000; - } - const uint16_t azimuth_corrected = - (static_cast(std::round(azimuth_corrected_f))) % 36000; - - const float cos_rot_angle = cos_rot_table_[azimuth_corrected]; - const float sin_rot_angle = sin_rot_table_[azimuth_corrected]; - - const float horiz_offset = corrections.horiz_offset_correction; - const float vert_offset = corrections.vert_offset_correction; - - // 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 mathematical - * model we used. - */ - float xy_distance = distance * cos_vert_angle - vert_offset * sin_vert_angle; - - // Calculate temporal X, use absolute value. - float xx = xy_distance * sin_rot_angle - horiz_offset * cos_rot_angle; - // Calculate temporal Y, use absolute value - float yy = xy_distance * cos_rot_angle + horiz_offset * sin_rot_angle; - if (xx < 0) { - xx = -xx; - } - if (yy < 0) { - yy = -yy; - } - - // Get 2points calibration values,Linear interpolation to get distance - // correction for X and Y, that means distance correction use - // different value at different distance - float distance_corr_x = 0; - float distance_corr_y = 0; - if (corrections.two_pt_correction_available) { - distance_corr_x = (corrections.dist_correction - corrections.dist_correction_x) * - (xx - 2.4) / (25.04 - 2.4) + - corrections.dist_correction_x; - distance_corr_x -= corrections.dist_correction; - distance_corr_y = (corrections.dist_correction - corrections.dist_correction_y) * - (yy - 1.93) / (25.04 - 1.93) + - corrections.dist_correction_y; - distance_corr_y -= corrections.dist_correction; - } - - 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 mathematical - * model we used. - */ - xy_distance = distance_x * cos_vert_angle - vert_offset * sin_vert_angle; - /// 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 mathematical - * model we used. - */ - y = xy_distance * cos_rot_angle + horiz_offset * sin_rot_angle; - - // 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 mathematical - * model we used. - */ - z = distance_y * sin_vert_angle + vert_offset * cos_vert_angle; - - /** Use standard ROS coordinate system (right-hand rule) */ - const float x_coord = y; - const float y_coord = -x; - const float z_coord = z; - - /** Intensity Calculation */ - const float min_intensity = corrections.min_intensity; - const float max_intensity = corrections.max_intensity; - - intensity = raw->blocks[i].data[k + 2]; - - last_block_timestamp_ = block_timestamp; - - const float focal_offset = 256 * (1 - corrections.focal_distance / 13100) * - (1 - corrections.focal_distance / 13100); - const float focal_slope = corrections.focal_slope; - float sqr = (1 - static_cast(current_return.uint) / 65535) * - (1 - static_cast(current_return.uint) / 65535); - intensity += focal_slope * (std::abs(focal_offset - 256 * sqr)); - intensity = (intensity < min_intensity) ? min_intensity : intensity; - intensity = (intensity > max_intensity) ? max_intensity : intensity; - - double point_time_offset = timing_offsets_[i][j]; - - nebula::drivers::ReturnType return_type; - switch (return_mode) { - case g_return_mode_dual: - if ( - (other_return.bytes[0] == 0 && other_return.bytes[1] == 0) || - (other_return.bytes[0] == current_return.bytes[0] && - other_return.bytes[1] == current_return.bytes[1])) { - return_type = drivers::ReturnType::IDENTICAL; - } else { - const float other_intensity = - i % 2 ? raw->blocks[i - 1].data[k + 2] : raw->blocks[i + 1].data[k + 2]; - bool first = other_return.uint < current_return.uint ? 0 : 1; - bool strongest = other_intensity < intensity ? 1 : 0; - if (other_intensity == intensity) { - strongest = first ? 0 : 1; - } - if (first && strongest) { - return_type = drivers::ReturnType::FIRST_STRONGEST; - } else if (!first && strongest) { - return_type = drivers::ReturnType::LAST_STRONGEST; - } else if (first && !strongest) { - return_type = drivers::ReturnType::FIRST_WEAK; - } else if (!first && !strongest) { - return_type = drivers::ReturnType::LAST_WEAK; - } else { - return_type = drivers::ReturnType::UNKNOWN; - } - } - break; - case g_return_mode_strongest: - return_type = drivers::ReturnType::STRONGEST; - break; - case g_return_mode_last: - return_type = drivers::ReturnType::LAST; - break; - default: - return_type = drivers::ReturnType::UNKNOWN; - } - drivers::NebulaPoint current_point{}; - current_point.x = x_coord; - current_point.y = y_coord; - current_point.z = z_coord; - current_point.return_type = static_cast(return_type); - current_point.channel = corrections.laser_ring; - 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.intensity = intensity; - scan_pc_->points.emplace_back(current_point); - } - } - } - } -} - -bool Vlp32Decoder::parse_packet( - [[maybe_unused]] const velodyne_msgs::msg::VelodynePacket & velodyne_packet) -{ - return 0; -} - -} // namespace drivers::vlp32 -} // namespace nebula diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp deleted file mode 100644 index cb2555f99..000000000 --- a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp +++ /dev/null @@ -1,357 +0,0 @@ -// Copyright 2024 TIER IV, Inc. - -#include "nebula_decoders/nebula_decoders_velodyne/decoders/vls128_decoder.hpp" - -#include - -#include -#include - -namespace nebula -{ -namespace drivers::vls128 -{ -Vls128Decoder::Vls128Decoder( - const std::shared_ptr & sensor_configuration, - const std::shared_ptr & - calibration_configuration) -{ - sensor_configuration_ = sensor_configuration; - calibration_configuration_ = calibration_configuration; - - scan_timestamp_ = -1; - - scan_pc_.reset(new NebulaPointCloud); - overflow_pc_.reset(new NebulaPointCloud); - - // Set up cached values for sin and cos of all the possible headings - for (uint16_t rot_index = 0; rot_index < g_rotation_max_units; ++rot_index) { - float rotation = angles::from_degrees(g_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); - - for (uint8_t i = 0; i < 16; i++) { - vls_128_laser_azimuth_cache_[i] = - (g_vls128_channel_duration / g_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; - } - } -} - -std::tuple Vls128Decoder::get_pointcloud() -{ - double phase = angles::from_degrees(sensor_configuration_->scan_phase); - if (!scan_pc_->points.empty()) { - 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 = 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_, scan_timestamp_); -} - -int Vls128Decoder::points_per_packet() -{ - return g_blocks_per_packet * g_scans_per_block; -} - -void Vls128Decoder::reset_pointcloud(double time_stamp) -{ - scan_pc_->clear(); - reset_overflow(time_stamp); // transfer existing overflow points to the cleared pointcloud -} - -void Vls128Decoder::reset_overflow(double time_stamp) -{ - if (overflow_pc_->points.size() == 0) { - scan_timestamp_ = -1; - overflow_pc_->points.reserve(max_pts_); - return; - } - - // Compute the absolute time stamp of the last point of the overflow pointcloud - const double last_overflow_time_stamp = - scan_timestamp_ + 1e-9 * overflow_pc_->points.back().time_stamp; - - // Detect cases where there is an unacceptable time difference between the last overflow point and - // the first point of the next packet. In that case, there was probably a packet drop so it is - // better to ignore the overflow pointcloud - if (time_stamp - last_overflow_time_stamp > 0.05) { - scan_timestamp_ = -1; - overflow_pc_->points.clear(); - overflow_pc_->points.reserve(max_pts_); - return; - } - - // Add the overflow buffer points - while (overflow_pc_->points.size() > 0) { - auto overflow_point = overflow_pc_->points.back(); - - // The overflow points had the stamps from the previous pointcloud. These need to be changed to - // be relative to the overflow's packet timestamp - double new_timestamp_seconds = - scan_timestamp_ + 1e-9 * overflow_point.time_stamp - last_block_timestamp_; - overflow_point.time_stamp = - static_cast(new_timestamp_seconds < 0.0 ? 0.0 : 1e9 * new_timestamp_seconds); - - scan_pc_->points.emplace_back(overflow_point); - overflow_pc_->points.pop_back(); - } - - // When there is overflow, the timestamp becomes the overflow packets' one - scan_timestamp_ = last_block_timestamp_; - overflow_pc_->points.clear(); - overflow_pc_->points.reserve(max_pts_); -} - -void Vls128Decoder::unpack(const std::vector & packet, double packet_seconds) -{ - check_and_handle_scan_complete(packet, packet_seconds, phase_); - - const raw_packet_t * raw = (const raw_packet_t *)packet.data(); - float last_azimuth_diff = 0; - uint16_t azimuth_next; - const uint8_t return_mode = packet[g_return_mode_index]; - const bool dual_return = (return_mode == g_return_mode_dual); - - for (uint block = 0; block < static_cast(g_blocks_per_packet - (4 * dual_return)); - block++) { - // Cache block for use. - const raw_block_t & current_block = raw->blocks[block]; - - uint bank_origin = 0; - // Used to detect which bank of 32 lasers is in this block. - switch (current_block.header) { - case g_vls128_bank_1: - bank_origin = 0; - break; - case g_vls128_bank_2: - bank_origin = 32; - break; - case g_vls128_bank_3: - bank_origin = 64; - break; - case g_vls128_bank_4: - bank_origin = 96; - break; - default: - // Do not flood the log with messages, only issue at most one - // of these warnings per minute. - return; // bad packet: skip the rest - } - - float azimuth_diff; - uint16_t azimuth; - - // Calculate difference between current and next block's azimuth angle. - if (block == 0) { - azimuth = current_block.rotation; - } else { - azimuth = azimuth_next; - } - if (block < static_cast(g_blocks_per_packet - (1 + dual_return))) { - // Get the next block rotation to calculate how far we rotate between blocks - azimuth_next = raw->blocks[block + (1 + dual_return)].rotation; - - // Finds the difference between two successive blocks - azimuth_diff = static_cast((36000 + azimuth_next - azimuth) % 36000); - - // This is used when the last block is next to predict rotation amount - last_azimuth_diff = azimuth_diff; - } else { - // This makes the assumption the difference between the last block and the next packet is the - // same as the last to the second to last. - // Assumes RPM doesn't change much between blocks. - azimuth_diff = (block == static_cast(g_blocks_per_packet - (4 * dual_return) - 1)) - ? 0 - : last_azimuth_diff; - } - - // Condition added to avoid calculating points which are not in the interesting defined area - // (cloud_min_angle < area < cloud_max_angle). - if ( - (sensor_configuration_->cloud_min_angle < sensor_configuration_->cloud_max_angle && - // azimuth >= sensor_configuration_->cloud_min_angle && - azimuth >= sensor_configuration_->cloud_min_angle * 100 && - azimuth <= sensor_configuration_->cloud_max_angle * 100) || - // azimuth <= sensor_configuration_->cloud_max_angle) || - (sensor_configuration_->cloud_min_angle > sensor_configuration_->cloud_max_angle)) { - for (size_t j = 0, k = 0; j < g_scans_per_block; j++, k += g_raw_scan_size) { - union two_bytes current_return { - }; - union two_bytes other_return { - }; - // Distance extraction. - current_return.bytes[0] = current_block.data[k]; - current_return.bytes[1] = current_block.data[k + 1]; - if (dual_return) { - other_return.bytes[0] = - block % 2 ? raw->blocks[block - 1].data[k] : raw->blocks[block + 1].data[k]; - 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 = packet_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 ( - (current_return.bytes[0] == 0 && current_return.bytes[1] == 0) || - (dual_return && block % 2 && other_return.bytes[0] == current_return.bytes[0] && - other_return.bytes[1] == current_return.bytes[1])) { - continue; - } - { - const uint laser_number = - j + bank_origin; // offset the laser in this block by which block it's in - const uint firing_order = laser_number / 8; // VLS-128 fires 8 lasers at a time - - const VelodyneLaserCorrection & corrections = - calibration_configuration_->velodyne_calibration.laser_corrections[laser_number]; - - float distance = current_return.uint * g_vlp128_distance_resolution; - if (distance > 1e-6) { - distance += corrections.dist_correction; - } - - // Correct for the laser rotation as a function of timing during the firings. - float azimuth_corrected_f = azimuth + - (azimuth_diff * vls_128_laser_azimuth_cache_[firing_order]) - - corrections.rot_correction * 180.0 / M_PI * 100; - - if (azimuth_corrected_f < 0.0) { - azimuth_corrected_f += 36000.0; - } - const uint16_t azimuth_corrected = ((uint16_t)std::round(azimuth_corrected_f)) % 36000; - - if ( - distance > sensor_configuration_->min_range && - distance < sensor_configuration_->max_range) { - // Condition added to avoid calculating points which are not in the interesting defined - // area (cloud_min_angle < area < cloud_max_angle). - if ( - (azimuth_corrected >= sensor_configuration_->cloud_min_angle * 100 && - azimuth_corrected <= sensor_configuration_->cloud_max_angle * 100 && - sensor_configuration_->cloud_min_angle < sensor_configuration_->cloud_max_angle) || - (sensor_configuration_->cloud_min_angle > sensor_configuration_->cloud_max_angle && - (azimuth_corrected <= sensor_configuration_->cloud_max_angle * 100 || - azimuth_corrected >= sensor_configuration_->cloud_min_angle * 100))) { - // convert polar coordinates to Euclidean XYZ. - const float cos_vert_angle = corrections.cos_vert_correction; - const float sin_vert_angle = corrections.sin_vert_correction; - const float cos_rot_angle = cos_rot_table_[azimuth_corrected]; - const float sin_rot_angle = sin_rot_table_[azimuth_corrected]; - - // Compute the distance in the xy plane (w/o accounting for rotation). - const float xy_distance = distance * cos_vert_angle; - - // Use standard ROS coordinate system (right-hand rule). - 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 uint8_t intensity = current_block.data[k + 2]; - last_block_timestamp_ = block_timestamp; - double point_time_offset = - timing_offsets_[block / 4][firing_order + laser_number / 64]; - - // Determine return type. - uint8_t return_type; - switch (return_mode) { - case g_return_mode_dual: - if ( - (other_return.bytes[0] == 0 && other_return.bytes[1] == 0) || - (other_return.bytes[0] == current_return.bytes[0] && - 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] - : raw->blocks[block + 1].data[k + 2]; - bool first = other_return.uint >= current_return.uint; - bool strongest = other_intensity < intensity; - if (other_intensity == intensity) { - strongest = !first; - } - if (first && strongest) { - return_type = static_cast(drivers::ReturnType::FIRST_STRONGEST); - } else if (!first && strongest) { - return_type = static_cast(drivers::ReturnType::LAST_STRONGEST); - } else if (first && !strongest) { - return_type = static_cast(drivers::ReturnType::FIRST_WEAK); - } else if (!first && !strongest) { - return_type = static_cast(drivers::ReturnType::LAST_WEAK); - } else { - return_type = static_cast(drivers::ReturnType::UNKNOWN); - } - } - break; - case g_return_mode_strongest: - return_type = static_cast(drivers::ReturnType::STRONGEST); - break; - case g_return_mode_last: - return_type = static_cast(drivers::ReturnType::LAST); - break; - default: - return_type = static_cast(drivers::ReturnType::UNKNOWN); - } - drivers::NebulaPoint current_point{}; - current_point.x = x_coord; - current_point.y = y_coord; - current_point.z = z_coord; - current_point.return_type = return_type; - current_point.channel = corrections.laser_ring; - current_point.azimuth = rotation_radians_[azimuth_corrected]; - current_point.elevation = sin_vert_angle; - current_point.distance = distance; - 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 - } // distance condition - } // empty "else" - } // (uint j = 0, k = 0; j < SCANS_PER_BLOCK; j++, k += RAW_SCAN_SIZE) - } // scan area condition - } // for (uint block = 0; block < static_cast < uint > (BLOCKS_PER_PACKET - (4 * dual_return)); - // block++) -} - -bool Vls128Decoder::parse_packet( - [[maybe_unused]] const velodyne_msgs::msg::VelodynePacket & velodyne_packet) -{ - return 0; -} - -} // namespace drivers::vls128 -} // namespace nebula diff --git a/nebula_decoders/src/nebula_decoders_velodyne/velodyne_driver.cpp b/nebula_decoders/src/nebula_decoders_velodyne/velodyne_driver.cpp index 191c904f4..30c398caa 100644 --- a/nebula_decoders/src/nebula_decoders_velodyne/velodyne_driver.cpp +++ b/nebula_decoders/src/nebula_decoders_velodyne/velodyne_driver.cpp @@ -2,9 +2,11 @@ #include "nebula_decoders/nebula_decoders_velodyne/velodyne_driver.hpp" -#include "nebula_decoders/nebula_decoders_velodyne/decoders/vlp16_decoder.hpp" -#include "nebula_decoders/nebula_decoders_velodyne/decoders/vlp32_decoder.hpp" -#include "nebula_decoders/nebula_decoders_velodyne/decoders/vls128_decoder.hpp" +#include "memory" +#include "nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_generic_decoder.hpp" +#include "nebula_decoders/nebula_decoders_velodyne/decoders/vlp_16.hpp" +#include "nebula_decoders/nebula_decoders_velodyne/decoders/vlp_32.hpp" +#include "nebula_decoders/nebula_decoders_velodyne/decoders/vls_128.hpp" namespace nebula::drivers { @@ -20,18 +22,18 @@ VelodyneDriver::VelodyneDriver( driver_status_ = nebula::Status::INVALID_SENSOR_MODEL; break; case SensorModel::VELODYNE_VLS128: - scan_decoder_.reset( - new drivers::vls128::Vls128Decoder(sensor_configuration, calibration_configuration)); + scan_decoder_ = + std::make_shared>(sensor_configuration, calibration_configuration); break; case SensorModel::VELODYNE_VLP32: case SensorModel::VELODYNE_HDL64: case SensorModel::VELODYNE_HDL32: - scan_decoder_.reset( - new drivers::vlp32::Vlp32Decoder(sensor_configuration, calibration_configuration)); + scan_decoder_ = + std::make_shared>(sensor_configuration, calibration_configuration); break; case SensorModel::VELODYNE_VLP16: - scan_decoder_.reset( - new drivers::vlp16::Vlp16Decoder(sensor_configuration, calibration_configuration)); + scan_decoder_ = + std::make_shared>(sensor_configuration, calibration_configuration); break; default: driver_status_ = nebula::Status::INVALID_SENSOR_MODEL; diff --git a/src/transport_drivers b/src/transport_drivers new file mode 160000 index 000000000..e7e25ed52 --- /dev/null +++ b/src/transport_drivers @@ -0,0 +1 @@ +Subproject commit e7e25ed52b58b3556fa8b0f75a87f7c5d42fbef1