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 index 94df198de..7297a33e1 100644 --- 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 @@ -3,6 +3,7 @@ #include "nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp" #include "nebula_decoders/nebula_decoders_velodyne/decoders/vlp_16.hpp" #include "nebula_decoders/nebula_decoders_velodyne/decoders/vls_128.hpp" +#include "nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_packet.hpp" #include #include @@ -35,8 +36,8 @@ class VelodyneDecoder : public VelodyneScanDecoder 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 < ROTATION_MAX_UNITS; ++rot_index) { - float rotation = angles::from_degrees(ROTATION_RESOLUTION * rot_index); + for (uint16_t rot_index = 0; rot_index < velodyne_packet::ROTATION_MAX_UNITS; ++rot_index) { + float rotation = angles::from_degrees(velodyne_packet::ROTATION_RESOLUTION * rot_index); rotation_radians_[rot_index] = rotation; cos_rot_table_[rot_index] = cosf(rotation); sin_rot_table_[rot_index] = sinf(rotation); @@ -48,10 +49,10 @@ class VelodyneDecoder : public VelodyneScanDecoder sensor_.fillAzimuthCache(); // TODO: predefine azimuth cache and remove this for performance? // timing table calculation, from velodyne user manual p.64 - timing_offsets_.resize(BLOCKS_PER_PACKET); // x dir size + timing_offsets_.resize(velodyne_packet::BLOCKS_PER_PACKET); // x dir size for (size_t i = 0; i < timing_offsets_.size(); ++i) { timing_offsets_[i].resize( - CHANNELS_PER_BLOCK + SensorT::num_maintenance_periods); // y dir size + velodyne_packet::CHANNELS_PER_BLOCK + SensorT::num_maintenance_periods); // y dir size } double full_firing_cycle_s = SensorT::full_firing_cycle_s; @@ -107,7 +108,7 @@ class VelodyneDecoder : public VelodyneScanDecoder { // scan_pc_.reset(new NebulaPointCloud); scan_pc_->points.clear(); - max_pts_ = n_pts * POINTS_PER_PACKET; + max_pts_ = n_pts * velodyne_packet::POINTS_PER_PACKET; scan_pc_->points.reserve(max_pts_); reset_overflow(time_stamp); // transfer existing overflow points to the cleared pointcloud } @@ -158,21 +159,21 @@ class VelodyneDecoder : public VelodyneScanDecoder void unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) { // const raw_packet_t * raw = (const raw_packet_t *)&velodyne_packet.data[0]; - raw_packet_t raw_instance; - raw_packet_t * raw = &raw_instance; - std::memcpy(raw, &velodyne_packet.data[0], sizeof(raw_packet_t)); + velodyne_packet::raw_packet_t raw_instance; + velodyne_packet::raw_packet_t * raw = &raw_instance; + std::memcpy(raw, &velodyne_packet.data[0], sizeof(velodyne_packet::raw_packet_t)); float last_azimuth_diff = 0; uint16_t azimuth_next; - const uint8_t return_mode = velodyne_packet.data[RETURN_MODE_INDEX]; - const bool dual_return = (return_mode == RETURN_MODE_DUAL); + const uint8_t return_mode = velodyne_packet.data[velodyne_packet::RETURN_MODE_INDEX]; + const bool dual_return = (return_mode == velodyne_packet::RETURN_MODE_DUAL); int num_padding_blocks = sensor_.getNumPaddingBlocks(dual_return); - for (uint block = 0; block < static_cast(BLOCKS_PER_PACKET - num_padding_blocks); + for (uint block = 0; block < static_cast(velodyne_packet::BLOCKS_PER_PACKET - num_padding_blocks); block++) { // Cache block for use. - const raw_block_t & current_block = raw->blocks[block]; + const velodyne_packet::raw_block_t & current_block = raw->blocks[block]; uint bank_origin = 0; // Used to detect which bank of 32 lasers is in this block. @@ -205,7 +206,7 @@ class VelodyneDecoder : public VelodyneScanDecoder } else { azimuth = azimuth_next; } - if (block < static_cast(BLOCKS_PER_PACKET - (1 + dual_return))) { + if (block < static_cast(velodyne_packet::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; @@ -218,7 +219,7 @@ class VelodyneDecoder : public VelodyneScanDecoder // 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(BLOCKS_PER_PACKET - (4 * dual_return) - 1)) + azimuth_diff = (block == static_cast(velodyne_packet::BLOCKS_PER_PACKET - (4 * dual_return) - 1)) ? 0 : last_azimuth_diff; } @@ -239,10 +240,10 @@ class VelodyneDecoder : public VelodyneScanDecoder std::max(static_cast(SensorT::firing_sequences_per_block), static_cast(1)); firing_seq++) { for (int channel = 0; - channel < CHANNELS_PER_BLOCK && channel < SensorT::channels_per_firing_sequence; - channel++, k += RAW_CHANNEL_SIZE) { - union two_bytes current_return; - union two_bytes other_return; + channel < velodyne_packet::CHANNELS_PER_BLOCK && channel < SensorT::channels_per_firing_sequence; + channel++, k += velodyne_packet::RAW_CHANNEL_SIZE) { + union velodyne_packet::two_bytes current_return; + union velodyne_packet::two_bytes other_return; // Distance extraction. current_return.bytes[0] = current_block.data[k]; @@ -322,7 +323,7 @@ class VelodyneDecoder : public VelodyneScanDecoder // Determine return type. uint8_t return_type; switch (return_mode) { - case RETURN_MODE_DUAL: + case velodyne_packet::RETURN_MODE_DUAL: if ( (other_return.bytes[0] == 0 && other_return.bytes[1] == 0) || (other_return.bytes[0] == current_return.bytes[0] && @@ -354,10 +355,10 @@ class VelodyneDecoder : public VelodyneScanDecoder } } break; - case RETURN_MODE_STRONGEST: + case velodyne_packet::RETURN_MODE_STRONGEST: return_type = static_cast(drivers::ReturnType::STRONGEST); break; - case RETURN_MODE_LAST: + case velodyne_packet::RETURN_MODE_LAST: return_type = static_cast(drivers::ReturnType::LAST); break; default: @@ -394,9 +395,9 @@ class VelodyneDecoder : public VelodyneScanDecoder /// @return Resulting flag // params used by all velodyne decoders - float sin_rot_table_[ROTATION_MAX_UNITS]; - float cos_rot_table_[ROTATION_MAX_UNITS]; - float rotation_radians_[ROTATION_MAX_UNITS]; + float sin_rot_table_[velodyne_packet::ROTATION_MAX_UNITS]; + float cos_rot_table_[velodyne_packet::ROTATION_MAX_UNITS]; + float rotation_radians_[velodyne_packet::ROTATION_MAX_UNITS]; int phase_; int max_pts_; double last_block_timestamp_; 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..ff53dd206 --- /dev/null +++ b/nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_packet.hpp @@ -0,0 +1,103 @@ +#pragma once + +#include +#include +#include + +namespace nebula +{ +namespace drivers +{ +namespace velodyne_packet +{ + +/** + * Raw Velodyne packet constants and structures. + */ +static const int SIZE_BLOCK = 100; +static const int RAW_SCAN_SIZE = 3; // TODO: remove +static const int RAW_CHANNEL_SIZE = 3; +static const int SCANS_PER_BLOCK = 32; // TODO: remove +static const int CHANNELS_PER_BLOCK = 32; +static const int BLOCK_DATA_SIZE = (SCANS_PER_BLOCK * RAW_SCAN_SIZE); + +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; + +/** @todo make this work for both big and little-endian machines */ +static const uint16_t UPPER_BANK = 0xeeff; +static const uint16_t LOWER_BANK = 0xddff; + +/** Return Modes **/ +static const uint16_t RETURN_MODE_STRONGEST = 55; +static const uint16_t RETURN_MODE_LAST = 56; +static const uint16_t RETURN_MODE_DUAL = 57; + +const int PACKET_SIZE = 1206; +const int BLOCKS_PER_PACKET = 12; +const int PACKET_STATUS_SIZE = 4; +const int SCANS_PER_PACKET = (SCANS_PER_BLOCK * BLOCKS_PER_PACKET); +const int POINTS_PER_PACKET = (SCANS_PER_PACKET * 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 + */ +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[BLOCK_DATA_SIZE]; +} raw_block_t; + +/** \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[BLOCKS_PER_PACKET]; + uint16_t revolution; + uint8_t status[PACKET_STATUS_SIZE]; +} raw_packet_t; +#pragma pack(pop) + +/** 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]; +}; + +/** \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 hesai_packet +} // namespace drivers +} // namespace nebula 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 2f55b260d..c38b0c362 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 @@ -24,91 +24,6 @@ namespace nebula { namespace drivers { -/** - * Raw Velodyne packet constants and structures. - */ -static const int SIZE_BLOCK = 100; -static const int RAW_SCAN_SIZE = 3; // TODO: remove -static const int RAW_CHANNEL_SIZE = 3; -static const int SCANS_PER_BLOCK = 32; // TODO: remove -static const int CHANNELS_PER_BLOCK = 32; -static const int BLOCK_DATA_SIZE = (SCANS_PER_BLOCK * RAW_SCAN_SIZE); - -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; - -/** @todo make this work for both big and little-endian machines */ -static const uint16_t UPPER_BANK = 0xeeff; -static const uint16_t LOWER_BANK = 0xddff; - -/** Return Modes **/ -static const uint16_t RETURN_MODE_STRONGEST = 55; -static const uint16_t RETURN_MODE_LAST = 56; -static const uint16_t RETURN_MODE_DUAL = 57; - -/** \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[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 PACKET_SIZE = 1206; -static const int BLOCKS_PER_PACKET = 12; -static const int PACKET_STATUS_SIZE = 4; -static const int SCANS_PER_PACKET = (SCANS_PER_BLOCK * BLOCKS_PER_PACKET); -static const int POINTS_PER_PACKET = (SCANS_PER_PACKET * RAW_SCAN_SIZE); - -/** \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[BLOCKS_PER_PACKET]; - uint16_t revolution; - uint8_t status[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 {