Skip to content

Commit

Permalink
fix(nebula): adapt velodyne decoders azimuth and timestamp to AWF def…
Browse files Browse the repository at this point in the history 
…inition. (autowarefoundation#36)

* decoders. add elevation, fix point attribute calculation

Signed-off-by: amc-nu <abraham.monrroy@gmail.com>

* velodyne decoders. fix azimuth and ts

Signed-off-by: amc-nu <abraham.monrroy@gmail.com>

* cspell. fix typos for ci

Signed-off-by: amc-nu <abraham.monrroy@gmail.com>

* tests. update tests pcd to match new decoding

Signed-off-by: amc-nu <abraham.monrroy@gmail.com>

* feat: add view direction parameter for Velodyne

Signed-off-by: David Wong <david.wong@tier4.jp>

* fix: change view direcction parameter to radians

Signed-off-by: David Wong <david.wong@tier4.jp>

* fix: timestamping for empty clouds with limited view angle settings

Signed-off-by: David Wong <david.wong@tier4.jp>

---------

Signed-off-by: amc-nu <abraham.monrroy@gmail.com>
Signed-off-by: David Wong <david.wong@tier4.jp>
Co-authored-by: David Wong <david.wong@tier4.jp>
  • Loading branch information
@amc-nu @drwnz
amc-nu and drwnz authored Jul 27, 2023
1 parent a3d2788 commit 09805ef
Show file tree
Hide file tree
Showing 12 changed files with 178 additions and 73 deletions.
1 change: 0 additions & 1 deletion nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,6 @@
#include "nebula_decoders/nebula_decoders_common/nebula_driver_base.hpp"
#include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp"

#include "pandar_msgs/msg/pandar_jumbo_packet.hpp"
#include "pandar_msgs/msg/pandar_packet.hpp"
#include "pandar_msgs/msg/pandar_scan.hpp"

Expand Down
4 changes: 2 additions & 2 deletions ...oders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,7 @@ static const int RAW_SCAN_SIZE = 3;
static const int SCANS_PER_BLOCK = 32;
static const int BLOCK_DATA_SIZE = (SCANS_PER_BLOCK * RAW_SCAN_SIZE);

static const float ROTATION_RESOLUTION = 0.01f; // [deg]
static const double ROTATION_RESOLUTION = 0.01; // [deg]
static const uint16_t ROTATION_MAX_UNITS = 36000u; // [deg/100]

static const size_t RETURN_MODE_INDEX = 1204;
Expand Down Expand Up @@ -149,7 +149,7 @@ class VelodyneScanDecoder
bool has_scanned_ = true;
double dual_return_distance_threshold_{}; // Velodyne does this internally, this will not be
// implemented here
double first_timestamp{};
double scan_timestamp_{};

/// @brief SensorConfiguration for this decoder
std::shared_ptr<drivers::VelodyneSensorConfiguration> sensor_configuration_;
Expand Down
3 changes: 3 additions & 0 deletions nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp16_decoder.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@ namespace drivers
{
namespace vlp16
{
constexpr uint32_t MAX_POINTS = 300000;
/// @brief Velodyne LiDAR decoder (VLP16)
class Vlp16Decoder : public VelodyneScanDecoder
{
Expand Down Expand Up @@ -48,8 +49,10 @@ class Vlp16Decoder : public VelodyneScanDecoder
bool parsePacket(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) override;
float sin_rot_table_[ROTATION_MAX_UNITS];
float cos_rot_table_[ROTATION_MAX_UNITS];
float rotation_radians_[ROTATION_MAX_UNITS];
int phase_;
int max_pts_;
std::vector<std::vector<float>> timing_offsets_;
};

} // namespace vlp16
Expand Down
2 changes: 2 additions & 0 deletions nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp32_decoder.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,8 @@ class Vlp32Decoder : public VelodyneScanDecoder
bool parsePacket(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) override;
float sin_rot_table_[ROTATION_MAX_UNITS];
float cos_rot_table_[ROTATION_MAX_UNITS];
float rotation_radians_[ROTATION_MAX_UNITS];
std::vector<std::vector<float>> timing_offsets_;
int phase_;
int max_pts_;
};
Expand Down
2 changes: 2 additions & 0 deletions nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vls128_decoder.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,9 +48,11 @@ class Vls128Decoder : public VelodyneScanDecoder
bool parsePacket(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) override;
float sin_rot_table_[ROTATION_MAX_UNITS];
float cos_rot_table_[ROTATION_MAX_UNITS];
float rotation_radians_[ROTATION_MAX_UNITS];
float vls_128_laser_azimuth_cache_[16];
int phase_;
int max_pts_;
std::vector<std::vector<float>> timing_offsets_;
};

} // namespace vls128
Expand Down
73 changes: 51 additions & 22 deletions nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,17 +16,40 @@ Vlp16Decoder::Vlp16Decoder(
sensor_configuration_ = sensor_configuration;
calibration_configuration_ = calibration_configuration;

first_timestamp = std::numeric_limits<double>::max();
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 < ROTATION_MAX_UNITS; ++rot_index) {
float rotation = angles::from_degrees(ROTATION_RESOLUTION * rot_index);
rotation_radians_[rot_index] = rotation;
cos_rot_table_[rot_index] = cosf(rotation);
sin_rot_table_[rot_index] = sinf(rotation);
}
// timing table calculation, from velodyne user manual p.64
timing_offsets_.resize(BLOCKS_PER_PACKET);
for (size_t i=0; i < timing_offsets_.size(); ++i){
timing_offsets_[i].resize(32);
}
double full_firing_cycle_s = 55.296 * 1e-6;
double single_firing_s = 2.304 * 1e-6;
double data_block_index, data_point_index;
bool dual_mode = sensor_configuration_->return_mode==ReturnMode::DUAL;
// compute timing offsets
for (size_t x = 0; x < timing_offsets_.size(); ++x){
for (size_t y = 0; y < timing_offsets_[x].size(); ++y){
if (dual_mode){
data_block_index = (x - (x % 2)) + (y / 16);
}
else{
data_block_index = (x * 2) + (y / 16);
}
data_point_index = y % 16;
timing_offsets_[x][y] = (full_firing_cycle_s * data_block_index) + (single_firing_s * data_point_index);
}
}

phase_ = (uint16_t)round(sensor_configuration_->scan_phase * 100);
}
Expand All @@ -35,19 +58,21 @@ bool Vlp16Decoder::hasScanned() { return has_scanned_; }

std::tuple<drivers::NebulaPointCloudPtr, double> Vlp16Decoder::get_pointcloud()
{
int phase = (uint16_t)round(sensor_configuration_->scan_phase * 100);
double phase = angles::from_degrees(sensor_configuration_->scan_phase);
if (!scan_pc_->points.empty()) {
uint16_t current_azimuth = (int)scan_pc_->points.back().azimuth * 100;
uint16_t phase_diff = (36000 + current_azimuth - phase) % 36000;
while (phase_diff < 18000 && scan_pc_->points.size() > 0) {
auto current_azimuth = scan_pc_->points.back().azimuth;
auto phase_diff = static_cast<size_t>(angles::to_degrees(2*M_PI + current_azimuth - phase)) % 360;
while (phase_diff < M_PI_2 && !scan_pc_->points.empty()) {
overflow_pc_->points.push_back(scan_pc_->points.back());
scan_pc_->points.pop_back();
current_azimuth = (int)scan_pc_->points.back().azimuth * 100;
phase_diff = (36000 + current_azimuth - phase) % 36000;
current_azimuth = scan_pc_->points.back().azimuth;
phase_diff = static_cast<size_t>(angles::to_degrees(2*M_PI + current_azimuth - phase)) % 360;
}
overflow_pc_->width = overflow_pc_->points.size();
scan_pc_->width = scan_pc_->points.size();
scan_pc_->height = 1;
}
return std::make_tuple(scan_pc_, first_timestamp);
return std::make_tuple(scan_pc_, scan_timestamp_);
}

int Vlp16Decoder::pointsPerPacket()
Expand All @@ -61,7 +86,7 @@ void Vlp16Decoder::reset_pointcloud(size_t n_pts)
max_pts_ = n_pts * pointsPerPacket();
scan_pc_->points.reserve(max_pts_);
reset_overflow(); // transfer existing overflow points to the cleared pointcloud
first_timestamp = std::numeric_limits<double>::max();
scan_timestamp_ = -1;
}

void Vlp16Decoder::reset_overflow()
Expand Down Expand Up @@ -138,6 +163,11 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
other_return.bytes[1] =
block % 2 ? raw->blocks[block - 1].data[k + 1] : raw->blocks[block + 1].data[k + 1];
}
// Apply timestamp if this is the first new packet in the scan.
auto block_timestamp = rclcpp::Time(velodyne_packet.stamp).seconds();
if (scan_timestamp_ < 0) {
scan_timestamp_ = block_timestamp;
}
// Do not process if there is no return, or in dual return mode and the first and last
// echos are the same.
if (
Expand Down Expand Up @@ -193,14 +223,9 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
const float x_coord = xy_distance * cos_rot_angle; // velodyne y
const float y_coord = -(xy_distance * sin_rot_angle); // velodyne x
const float z_coord = distance * sin_vert_angle; // velodyne z
const float intensity = current_block.data[k + 2];
const uint8_t intensity = current_block.data[k + 2];

const double time_stamp =
(block * 2 + firing) * 55.296 / 1000.0 / 1000.0 + dsr * 2.304 / 1000.0 / 1000.0;
auto ts = rclcpp::Time(velodyne_packet.stamp).seconds();
if (ts < first_timestamp) {
first_timestamp = ts;
}
double point_time_offset = timing_offsets_[block][firing * 16 + dsr];

// Determine return type.
uint8_t return_type;
Expand All @@ -212,12 +237,12 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
other_return.bytes[1] == current_return.bytes[1])) {
return_type = static_cast<uint8_t>(drivers::ReturnType::IDENTICAL);
} else {
const float other_intensity = block % 2 ? raw->blocks[block - 1].data[k + 2]
const uint8_t other_intensity = block % 2 ? raw->blocks[block - 1].data[k + 2]
: raw->blocks[block + 1].data[k + 2];
bool first = other_return.uint < current_return.uint ? 0 : 1;
bool strongest = other_intensity < intensity ? 1 : 0;
bool first = current_return.uint > other_return.uint ;
bool strongest = intensity > other_intensity;
if (other_intensity == intensity) {
strongest = first ? 0 : 1;
strongest = !first;
}
if (first && strongest) {
return_type = static_cast<uint8_t>(drivers::ReturnType::FIRST_STRONGEST);
Expand Down Expand Up @@ -247,8 +272,12 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
current_point.z = z_coord;
current_point.return_type = return_type;
current_point.channel = corrections.laser_ring;
current_point.azimuth = azimuth_corrected;
current_point.time_stamp = time_stamp;
current_point.azimuth = rotation_radians_[azimuth_corrected];
current_point.elevation = sin_vert_angle;
auto point_ts = block_timestamp - scan_timestamp_ + point_time_offset;
if (point_ts < 0)
point_ts = 0;
current_point.time_stamp = static_cast<uint32_t>(point_ts*1e9);
current_point.intensity = intensity;
current_point.distance = distance;
scan_pc_->points.emplace_back(current_point);
Expand Down
75 changes: 53 additions & 22 deletions nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,37 +16,63 @@ Vlp32Decoder::Vlp32Decoder(
sensor_configuration_ = sensor_configuration;
calibration_configuration_ = calibration_configuration;

first_timestamp = std::numeric_limits<double>::max();
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 < ROTATION_MAX_UNITS; ++rot_index) {
float rotation = angles::from_degrees(ROTATION_RESOLUTION * rot_index);
rotation_radians_[rot_index] = rotation;
cos_rot_table_[rot_index] = cosf(rotation);
sin_rot_table_[rot_index] = sinf(rotation);
}
phase_ = (uint16_t)round(sensor_configuration_->scan_phase * 100);

timing_offsets_.resize(12);
for (size_t i=0; i < timing_offsets_.size(); ++i){
timing_offsets_[i].resize(32);
}
// constants
double full_firing_cycle = 55.296 * 1e-6; // seconds
double single_firing = 2.304 * 1e-6; // seconds
double dataBlockIndex, dataPointIndex;
bool dual_mode = sensor_configuration_->return_mode == ReturnMode::DUAL;
// compute timing offsets
for (size_t x = 0; x < timing_offsets_.size(); ++x){
for (size_t y = 0; y < timing_offsets_[x].size(); ++y){
if (dual_mode){
dataBlockIndex = x / 2;
}
else{
dataBlockIndex = x;
}
dataPointIndex = y / 2;
timing_offsets_[x][y] = (full_firing_cycle * dataBlockIndex) + (single_firing * dataPointIndex);
}
}
}

bool Vlp32Decoder::hasScanned() { return has_scanned_; }

std::tuple<drivers::NebulaPointCloudPtr, double> Vlp32Decoder::get_pointcloud()
{
int phase = (uint16_t)round(sensor_configuration_->scan_phase * 100);
double phase = angles::from_degrees(sensor_configuration_->scan_phase);
if (!scan_pc_->points.empty()) {
uint16_t current_azimuth = (int)scan_pc_->points.back().azimuth * 100;
uint16_t phase_diff = (36000 + current_azimuth - phase) % 36000;
while (phase_diff < 18000 && scan_pc_->points.size() > 0) {
auto current_azimuth = scan_pc_->points.back().azimuth;
auto phase_diff = (2*M_PI + current_azimuth - phase);
while (phase_diff < M_PI_2 && !scan_pc_->points.empty()) {
overflow_pc_->points.push_back(scan_pc_->points.back());
scan_pc_->points.pop_back();
current_azimuth = (int)scan_pc_->points.back().azimuth * 100;
phase_diff = (36000 + current_azimuth - phase) % 36000;
current_azimuth = scan_pc_->points.back().azimuth;
phase_diff = (2*M_PI + current_azimuth - phase);
}
overflow_pc_->width = overflow_pc_->points.size();
scan_pc_->width = scan_pc_->points.size();
scan_pc_->height = 1;
}
return std::make_tuple(scan_pc_, first_timestamp);
return std::make_tuple(scan_pc_, scan_timestamp_);
}

int Vlp32Decoder::pointsPerPacket() { return BLOCKS_PER_PACKET * SCANS_PER_BLOCK; }
Expand All @@ -58,7 +84,7 @@ void Vlp32Decoder::reset_pointcloud(size_t n_pts)
max_pts_ = n_pts * pointsPerPacket();
scan_pc_->points.reserve(max_pts_);
reset_overflow(); // transfer existing overflow points to the cleared pointcloud
first_timestamp = std::numeric_limits<double>::max();
scan_timestamp_ = -1;
}

void Vlp32Decoder::reset_overflow()
Expand All @@ -85,7 +111,7 @@ void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
}
for (int j = 0, k = 0; j < SCANS_PER_BLOCK; j++, k += RAW_SCAN_SIZE) {
float x, y, z;
float intensity;
uint8_t intensity;
const uint8_t laser_number = j + bank_origin;

const VelodyneLaserCorrection & corrections =
Expand All @@ -103,6 +129,11 @@ void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
other_return.bytes[1] =
i % 2 ? raw->blocks[i - 1].data[k + 1] : raw->blocks[i + 1].data[k + 1];
}
// Apply timestamp if this is the first new packet in the scan.
auto block_timestamp = rclcpp::Time(velodyne_packet.stamp).seconds();
if (scan_timestamp_ < 0) {
scan_timestamp_ = block_timestamp;
}
// Do not process if there is no return, or in dual return mode and the first and last echos
// are the same.
if (
Expand Down Expand Up @@ -145,7 +176,7 @@ void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa

// Compute the distance in the xy plane (w/o accounting for rotation)
/**the new term of 'vert_offset * sin_vert_angle'
* was added to the expression due to the mathemathical
* was added to the expression due to the mathematical
* model we used.
*/
float xy_distance = distance * cos_vert_angle - vert_offset * sin_vert_angle;
Expand Down Expand Up @@ -179,25 +210,25 @@ void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa

const float distance_x = distance + distance_corr_x;
/**the new term of 'vert_offset * sin_vert_angle'
* was added to the expression due to the mathemathical
* was added to the expression due to the mathematical
* model we used.
*/
xy_distance = distance_x * cos_vert_angle - vert_offset * sin_vert_angle;
/// the expression wiht '-' is proved to be better than the one with '+'
/// the expression with '-' is proved to be better than the one with '+'
x = xy_distance * sin_rot_angle - horiz_offset * cos_rot_angle;

const float distance_y = distance + distance_corr_y;
xy_distance = distance_y * cos_vert_angle - vert_offset * sin_vert_angle;
/**the new term of 'vert_offset * sin_vert_angle'
* was added to the expression due to the mathemathical
* was added to the expression due to the mathematical
* model we used.
*/
y = xy_distance * cos_rot_angle + horiz_offset * sin_rot_angle;

// Using distance_y is not symmetric, but the velodyne manual
// does this.
/**the new term of 'vert_offset * cos_vert_angle'
* was added to the expression due to the mathemathical
* was added to the expression due to the mathematical
* model we used.
*/
z = distance_y * sin_vert_angle + vert_offset * cos_vert_angle;
Expand All @@ -222,11 +253,7 @@ void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
intensity = (intensity < min_intensity) ? min_intensity : intensity;
intensity = (intensity > max_intensity) ? max_intensity : intensity;

double time_stamp = i * 55.296 / 1000.0 / 1000.0 + j * 2.304 / 1000.0 / 1000.0; // +
auto ts = rclcpp::Time(velodyne_packet.stamp).seconds();
if (ts < first_timestamp) {
first_timestamp = ts;
}
double point_time_offset = timing_offsets_[i][j];

nebula::drivers::ReturnType return_type;
switch (return_mode) {
Expand Down Expand Up @@ -272,9 +299,13 @@ void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
current_point.z = z_coord;
current_point.return_type = static_cast<uint8_t>(return_type);
current_point.channel = corrections.laser_ring;
current_point.azimuth = raw->blocks[i].rotation;
current_point.azimuth = rotation_radians_[block.rotation];
current_point.elevation = sin_vert_angle;
auto point_ts = block_timestamp - scan_timestamp_ + point_time_offset;
if (point_ts < 0)
point_ts = 0;
current_point.time_stamp = static_cast<uint32_t>(point_ts*1e9);
current_point.distance = distance;
current_point.time_stamp = time_stamp;
current_point.intensity = intensity;
scan_pc_->points.emplace_back(current_point);
}
Expand Down
Loading

0 comments on commit 09805ef

Please sign in to comment.