fix(velodyne): reset pointcloud incl. width/height, not just points b…

…uffer Signed-off-by: Max SCHMELLER <[email protected]>
tier4 · Nov 5, 2024 · e81890c · e81890c
1 parent 18af521
commit e81890c
Show file tree

Hide file tree

Showing 3 changed files with 23 additions and 21 deletions.
diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp
@@ -1,10 +1,10 @@
 #include "nebula_decoders/nebula_decoders_velodyne/decoders/vlp16_decoder.hpp"
 
+#include <angles/angles.h>
+
 #include <cmath>
 #include <utility>
 
-#include <angles/angles.h> 
-
 namespace nebula
 {
 namespace drivers
@@ -89,7 +89,7 @@ int Vlp16Decoder::pointsPerPacket()
 
 void Vlp16Decoder::reset_pointcloud(size_t n_pts, double time_stamp)
 {
-  scan_pc_->points.clear();
+  scan_pc_->clear();
   max_pts_ = n_pts * pointsPerPacket();
   scan_pc_->points.reserve(max_pts_);
   reset_overflow(time_stamp);  // transfer existing overflow points to the cleared pointcloud
@@ -231,9 +231,10 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
               float azimuth_corrected_f =
                 azimuth +
                 (azimuth_diff * ((dsr * VLP16_DSR_TOFFSET) + (firing * VLP16_FIRING_TOFFSET)) /
-                 VLP16_BLOCK_DURATION) - corrections.rot_correction * 180.0 / M_PI * 100;
-
-              if (azimuth_corrected_f < 0.0){
+                 VLP16_BLOCK_DURATION) -
+                corrections.rot_correction * 180.0 / M_PI * 100;
+
+              if (azimuth_corrected_f < 0.0) {
                 azimuth_corrected_f += 36000.0;
               }
               const uint16_t azimuth_corrected =

diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp
@@ -1,10 +1,10 @@
 #include "nebula_decoders/nebula_decoders_velodyne/decoders/vlp32_decoder.hpp"
 
+#include <angles/angles.h>
+
 #include <cmath>
 #include <utility>
 
-#include <angles/angles.h> 
-
 namespace nebula
 {
 namespace drivers
@@ -87,8 +87,7 @@ int Vlp32Decoder::pointsPerPacket()
 
 void Vlp32Decoder::reset_pointcloud(size_t n_pts, double time_stamp)
 {
-  //  scan_pc_.reset(new NebulaPointCloud);
-  scan_pc_->points.clear();
+  scan_pc_->clear();
   max_pts_ = n_pts * pointsPerPacket();
   scan_pc_->points.reserve(max_pts_);
   reset_overflow(time_stamp);  // transfer existing overflow points to the cleared pointcloud
@@ -173,9 +172,8 @@ void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
       // 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 = (i == static_cast<uint>(BLOCKS_PER_PACKET - (4 * dual_return) - 1))
-                       ? 0
-                       : last_azimuth_diff;
+      azimuth_diff =
+        (i == static_cast<uint>(BLOCKS_PER_PACKET - (4 * dual_return) - 1)) ? 0 : last_azimuth_diff;
     }
 
     for (uint j = 0, k = 0; j < SCANS_PER_BLOCK; j++, k += RAW_SCAN_SIZE) {
@@ -232,7 +230,9 @@ void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
             raw->blocks[i].rotation >= sensor_configuration_->cloud_min_angle * 100))) {
           const float cos_vert_angle = corrections.cos_vert_correction;
           const float sin_vert_angle = corrections.sin_vert_correction;
-          float azimuth_corrected_f = azimuth + (azimuth_diff * VLP32_CHANNEL_DURATION / VLP32_SEQ_DURATION * j) - corrections.rot_correction * 180.0 / M_PI * 100;
+          float azimuth_corrected_f =
+            azimuth + (azimuth_diff * VLP32_CHANNEL_DURATION / VLP32_SEQ_DURATION * j) -
+            corrections.rot_correction * 180.0 / M_PI * 100;
           if (azimuth_corrected_f < 0) {
             azimuth_corrected_f += 36000;
           }

diff --git a/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp b/nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp
@@ -1,10 +1,10 @@
 #include "nebula_decoders/nebula_decoders_velodyne/decoders/vls128_decoder.hpp"
 
+#include <angles/angles.h>
+
 #include <cmath>
 #include <utility>
 
-#include <angles/angles.h> 
-
 namespace nebula
 {
 namespace drivers
@@ -89,8 +89,7 @@ int Vls128Decoder::pointsPerPacket()
 
 void Vls128Decoder::reset_pointcloud(size_t n_pts, double time_stamp)
 {
-  //  scan_pc_.reset(new NebulaPointCloud);
-  scan_pc_->points.clear();
+  scan_pc_->clear();
   max_pts_ = n_pts * pointsPerPacket();
   scan_pc_->points.reserve(max_pts_);
   reset_overflow(time_stamp);  // transfer existing overflow points to the cleared pointcloud
@@ -249,9 +248,11 @@ void Vls128Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_p
           }
 
           // Correct for the laser rotation as a function of timing during the firings.
-          float azimuth_corrected_f = azimuth + (azimuth_diff * vls_128_laser_azimuth_cache_[firing_order]) - corrections.rot_correction * 180.0 / M_PI * 100;
-
-          if (azimuth_corrected_f < 0.0){
+          float azimuth_corrected_f = azimuth +
+                                      (azimuth_diff * vls_128_laser_azimuth_cache_[firing_order]) -
+                                      corrections.rot_correction * 180.0 / M_PI * 100;
+
+          if (azimuth_corrected_f < 0.0) {
             azimuth_corrected_f += 36000.0;
           }
           const uint16_t azimuth_corrected = ((uint16_t)std::round(azimuth_corrected_f)) % 36000;