Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

fix(nebula): adapt velodyne decoders azimuth and timestamp to AWF definition. #36

Merged
merged 8 commits into from
Jul 27, 2023
Merged

fix(nebula): adapt velodyne decoders azimuth and timestamp to AWF definition. #36

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
d271c54
decoders. add elevation, fix point attribute calculation
amc-nu Jul 6, 2023
df8f292
velodyne decoders. fix azimuth and ts
amc-nu Jul 10, 2023
945e3bb
cspell. fix typos for ci
amc-nu Jul 10, 2023
841089b
tests. update tests pcd to match new decoding
amc-nu Jul 10, 2023
f069959
Merge branch 'main' into velodyne_fix
amc-nu Jul 19, 2023
993e7ab
feat: add view direction parameter for Velodyne
drwnz Jul 24, 2023
4426b56
fix: change view direcction parameter to radians
drwnz Jul 24, 2023
508ecfc
fix: timestamping for empty clouds with limited view angle settings
drwnz Jul 27, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 0 additions & 1 deletion nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Loading