Skip to content

Commit

Permalink
Undo code formatting
Browse files Browse the repository at this point in the history
  • Loading branch information
@mebasoglu
mebasoglu committed Sep 1, 2023
1 parent d6bcc14 commit bd7d9f4
Show file tree
Hide file tree
Showing 6 changed files with 50 additions and 73 deletions.
2 changes: 1 addition & 1 deletion 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 @@ -14,7 +14,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, public ParallelUnpack
{
Expand Down
2 changes: 0 additions & 2 deletions nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/velodyne_driver.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,8 +8,6 @@
#include "nebula_decoders/nebula_decoders_common/nebula_driver_base.hpp"
#include "nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp"

#include <range/v3/view/enumerate.hpp>

#include "velodyne_msgs/msg/velodyne_packet.hpp"
#include "velodyne_msgs/msg/velodyne_scan.hpp"

Expand Down
41 changes: 18 additions & 23 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,34 +30,31 @@ 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()
{
Expand All @@ -68,14 +65,12 @@ 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 Down Expand Up @@ -237,7 +232,7 @@ void Vlp16Decoder::unpack(
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 @@ -249,10 +244,9 @@ void Vlp16Decoder::unpack(
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 @@ -288,8 +282,9 @@ void Vlp16Decoder::unpack(
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;

Expand Down
37 changes: 16 additions & 21 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,33 +31,30 @@ 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()
{
Expand All @@ -68,12 +65,12 @@ 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 @@ -82,10 +79,7 @@ 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)
{
Expand Down Expand Up @@ -315,8 +309,9 @@ void Vlp32Decoder::unpack(
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;

Expand Down
36 changes: 14 additions & 22 deletions nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,28 +36,25 @@ Vls128Decoder::Vls128Decoder(
}
// 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)
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) {
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;
timing_offsets_[x][y] = (full_firing_cycle_s * sequence_index) + (single_firing_s * firing_group_index) - offset_packet_time;
}
}
}

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

std::tuple<drivers::NebulaPointCloudPtr, double> Vls128Decoder::get_pointcloud()
{
Expand All @@ -68,14 +65,12 @@ std::tuple<drivers::NebulaPointCloudPtr, double> 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<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 @@ -84,10 +79,7 @@ std::tuple<drivers::NebulaPointCloudPtr, double> Vls128Decoder::get_pointcloud()
return std::make_tuple(scan_pc_, scan_timestamp_);
}

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

void Vls128Decoder::reset_pointcloud(size_t n_pts)
{
Expand Down Expand Up @@ -261,8 +253,7 @@ void Vls128Decoder::unpack(
if (scan_timestamp_ < 0) {
scan_timestamp_ = block_timestamp;
}
double point_time_offset =
timing_offsets_[block / 4][firing_order + laser_number / 64];
double point_time_offset = timing_offsets_[block / 4][firing_order + laser_number / 64];

// Determine return type.
uint8_t return_type;
Expand Down Expand Up @@ -313,8 +304,9 @@ void Vls128Decoder::unpack(
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<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;

append_point(current_point, packet_index);
Expand Down
5 changes: 1 addition & 4 deletions nebula_decoders/src/nebula_decoders_velodyne/velodyne_driver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,10 +69,7 @@ std::tuple<drivers::NebulaPointCloudPtr, double> VelodyneDriver::ConvertScanToPo
}
return pointcloud;
}
Status VelodyneDriver::GetStatus()
{
return driver_status_;
}
Status VelodyneDriver::GetStatus() { return driver_status_; }

} // namespace drivers
} // namespace nebula

0 comments on commit bd7d9f4

Please sign in to comment.