Skip to content

Commit

Permalink
fix(velodyne_decoder): overflow handling in vls128 (#111)
Browse files Browse the repository at this point in the history 
* fix: overflow handling in vls128

The timestamps in the vls128 were still relative to the last pointcloud, which was producing some errors
Additionally, when entire scans were dropped, the overflow was not being handled correctly (this bug was also present in the awf velodyne driver)

Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]>

* chore: fixed spelling

Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]>

* fix: relative time was in seconds

Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]>

* fix: timestamp handling and overflow order. error happen then the computed time is higher than the next measured one...

Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]>

* Update nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp

Co-authored-by: Benjamin Gilby <[email protected]>

* Update nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp

Co-authored-by: Benjamin Gilby <[email protected]>

---------

Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]>
Co-authored-by: Benjamin Gilby <[email protected]>
  • Loading branch information
@knzo25 @bgilby59
knzo25 and bgilby59 authored Jan 9, 2024
1 parent 73a1e1b commit 04d8d38
Show file tree
Hide file tree
Showing 8 changed files with 125 additions and 71 deletions.
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 @@ -185,9 +185,9 @@ class VelodyneScanDecoder
virtual std::tuple<drivers::NebulaPointCloudPtr, double> get_pointcloud() = 0;
/// @brief Resetting point cloud buffer
/// @param n_pts # of points
virtual void reset_pointcloud(size_t n_pts) = 0;
virtual void reset_pointcloud(size_t n_pts, double time_stamp) = 0;
/// @brief Resetting overflowed point cloud buffer
virtual void reset_overflow() = 0;
virtual void reset_overflow(double time_stamp) = 0;
};

} // namespace drivers
Expand Down
6 changes: 3 additions & 3 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,7 +13,7 @@ namespace drivers
{
namespace vlp16
{
constexpr uint32_t MAX_POINTS = 300000;
constexpr uint32_t MAX_POINTS = 300000;
/// @brief Velodyne LiDAR decoder (VLP16)
class Vlp16Decoder : public VelodyneScanDecoder
{
Expand All @@ -38,9 +38,9 @@ class Vlp16Decoder : public VelodyneScanDecoder
std::tuple<drivers::NebulaPointCloudPtr, double> get_pointcloud() override;
/// @brief Resetting point cloud buffer
/// @param n_pts # of points
void reset_pointcloud(size_t n_pts) override;
void reset_pointcloud(size_t n_pts, double time_stamp) override;
/// @brief Resetting overflowed point cloud buffer
void reset_overflow() override;
void reset_overflow(double time_stamp) override;

private:
/// @brief Parsing VelodynePacket based on packet structure
Expand Down
4 changes: 2 additions & 2 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 @@ -37,9 +37,9 @@ class Vlp32Decoder : public VelodyneScanDecoder
std::tuple<drivers::NebulaPointCloudPtr, double> get_pointcloud() override;
/// @brief Resetting point cloud buffer
/// @param n_pts # of points
void reset_pointcloud(size_t n_pts) override;
void reset_pointcloud(size_t n_pts, double time_stamp) override;
/// @brief Resetting overflowed point cloud buffer
void reset_overflow() override;
void reset_overflow(double time_stamp) override;

private:
/// @brief Parsing VelodynePacket based on packet structure
Expand Down
5 changes: 3 additions & 2 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 @@ -37,9 +37,9 @@ class Vls128Decoder : public VelodyneScanDecoder
std::tuple<drivers::NebulaPointCloudPtr, double> get_pointcloud() override;
/// @brief Resetting point cloud buffer
/// @param n_pts # of points
void reset_pointcloud(size_t n_pts) override;
void reset_pointcloud(size_t n_pts, double time_stamp) override;
/// @brief Resetting overflowed point cloud buffer
void reset_overflow() override;
void reset_overflow(double time_stamp) override;

private:
/// @brief Parsing VelodynePacket based on packet structure
Expand All @@ -52,6 +52,7 @@ class Vls128Decoder : public VelodyneScanDecoder
float vls_128_laser_azimuth_cache_[16];
int phase_;
int max_pts_;
double last_block_timestamp_;
std::vector<std::vector<float>> timing_offsets_;
};

Expand Down
47 changes: 26 additions & 21 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 @@ -30,43 +30,48 @@ Vlp16Decoder::Vlp16Decoder(
}
// 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){
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;
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){
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{
} 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);
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);
}

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

std::tuple<drivers::NebulaPointCloudPtr, double> 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<size_t>(angles::to_degrees(2*M_PI + current_azimuth - phase)) % 360;
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 = scan_pc_->points.back().azimuth;
phase_diff = static_cast<size_t>(angles::to_degrees(2*M_PI + current_azimuth - phase)) % 360;
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();
Expand All @@ -80,16 +85,16 @@ int Vlp16Decoder::pointsPerPacket()
return BLOCKS_PER_PACKET * VLP16_FIRINGS_PER_BLOCK * VLP16_SCANS_PER_FIRING;
}

void Vlp16Decoder::reset_pointcloud(size_t n_pts)
void Vlp16Decoder::reset_pointcloud(size_t n_pts, [[maybe_unused]] double time_stamp)
{
scan_pc_->points.clear();
max_pts_ = n_pts * pointsPerPacket();
scan_pc_->points.reserve(max_pts_);
reset_overflow(); // transfer existing overflow points to the cleared pointcloud
reset_overflow(time_stamp); // transfer existing overflow points to the cleared pointcloud
scan_timestamp_ = -1;
}

void Vlp16Decoder::reset_overflow()
void Vlp16Decoder::reset_overflow([[maybe_unused]] double time_stamp)
{
// Add the overflow buffer points
for (size_t i = 0; i < overflow_pc_->points.size(); i++) {
Expand Down Expand Up @@ -225,7 +230,7 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
const float z_coord = distance * sin_vert_angle; // velodyne z
const uint8_t intensity = current_block.data[k + 2];

double point_time_offset = timing_offsets_[block][firing * 16 + dsr];
double point_time_offset = timing_offsets_[block][firing * 16 + dsr];

// Determine return type.
uint8_t return_type;
Expand All @@ -237,9 +242,10 @@ 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 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 ;
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;
Expand Down Expand Up @@ -275,9 +281,8 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
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);
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
43 changes: 24 additions & 19 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 @@ -31,42 +31,45 @@ Vlp32Decoder::Vlp32Decoder(
phase_ = (uint16_t)round(sensor_configuration_->scan_phase * 100);

timing_offsets_.resize(12);
for (size_t i=0; i < timing_offsets_.size(); ++i){
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 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){
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{
} else {
dataBlockIndex = x;
}
dataPointIndex = y / 2;
timing_offsets_[x][y] = (full_firing_cycle * dataBlockIndex) + (single_firing * dataPointIndex);
timing_offsets_[x][y] =
(full_firing_cycle * dataBlockIndex) + (single_firing * dataPointIndex);
}
}
}

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

std::tuple<drivers::NebulaPointCloudPtr, double> 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);
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);
phase_diff = (2 * M_PI + current_azimuth - phase);
}
overflow_pc_->width = overflow_pc_->points.size();
scan_pc_->width = scan_pc_->points.size();
Expand All @@ -75,19 +78,22 @@ std::tuple<drivers::NebulaPointCloudPtr, double> Vlp32Decoder::get_pointcloud()
return std::make_tuple(scan_pc_, scan_timestamp_);
}

int Vlp32Decoder::pointsPerPacket() { return BLOCKS_PER_PACKET * SCANS_PER_BLOCK; }
int Vlp32Decoder::pointsPerPacket()
{
return BLOCKS_PER_PACKET * SCANS_PER_BLOCK;
}

void Vlp32Decoder::reset_pointcloud(size_t n_pts)
void Vlp32Decoder::reset_pointcloud(size_t n_pts, [[maybe_unused]] double time_stamp)
{
// scan_pc_.reset(new NebulaPointCloud);
scan_pc_->points.clear();
max_pts_ = n_pts * pointsPerPacket();
scan_pc_->points.reserve(max_pts_);
reset_overflow(); // transfer existing overflow points to the cleared pointcloud
reset_overflow(time_stamp); // transfer existing overflow points to the cleared pointcloud
scan_timestamp_ = -1;
}

void Vlp32Decoder::reset_overflow()
void Vlp32Decoder::reset_overflow([[maybe_unused]] double time_stamp)
{
// Add the overflow buffer points
for (size_t i = 0; i < overflow_pc_->points.size(); i++) {
Expand Down Expand Up @@ -302,9 +308,8 @@ void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
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);
if (point_ts < 0) point_ts = 0;
current_point.time_stamp = static_cast<uint32_t>(point_ts * 1e9);
current_point.distance = distance;
current_point.intensity = intensity;
scan_pc_->points.emplace_back(current_point);
Expand Down
Loading

0 comments on commit 04d8d38

Please sign in to comment.