refactor(velodyne): generic decoder

nebula_decoders/CMakeLists.txt

@@ -61,10 +61,8 @@ target_include_directories(nebula_decoders_hesai PUBLIC
 # Velodyne
 add_library(nebula_decoders_velodyne SHARED
     src/nebula_decoders_velodyne/velodyne_driver.cpp
-    src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp
-    src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp
-    src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp
 )
+
 target_link_libraries(nebula_decoders_velodyne PUBLIC
     ${angles_msgs_TARGETS}
     ${velodyne_msgs_TARGETS}

nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_packet.hpp

@@ -0,0 +1,103 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include <cstddef>
+#include <cstdint>
+#include <ctime>
+
+namespace nebula::drivers::velodyne_packet
+{
+
+/**
+ * Raw Velodyne packet constants and structures.
+ */
+static const int g_raw_scan_size = 3;     // TODO(ike-kazu): remove
+static const int g_scans_per_block = 32;  // TODO(ike-kazu): remove
+static const int g_channels_per_block = 32;
+
+static const double g_rotation_resolution = 0.01;         // [deg]
+static const uint16_t g_rotation_max_units = 360 * 100u;  // [deg/100]
+
+static const size_t g_return_mode_index = 1204;
+
+/** @todo make this work for both big and little-endian machines */
+static const uint16_t g_upper_bank = 0xeeff;
+static const uint16_t g_lower_bank = 0xddff;
+
+/** Return Modes **/
+static const uint16_t g_return_mode_strongest = 55;
+static const uint16_t g_return_mode_last = 56;
+static const uint16_t g_return_mode_dual = 57;
+
+const int g_blocks_per_packet = 12;
+const int g_packet_status_size = 4;
+const int g_scans_per_packet = (g_scans_per_block * g_blocks_per_packet);
+const int g_points_per_packet = (g_scans_per_packet * g_raw_scan_size);
+
+#pragma pack(push, 1)
+/** \brief Raw Velodyne data block.
+ *
+ *  Each block contains data from either the upper or lower laser
+ *  bank.  The device returns three times as many upper bank blocks.
+ *
+ *  use stdint.h types, so things work with both 64 and 32-bit machines
+ */
+struct raw_units
+{
+  uint16_t distance;
+  uint8_t reflectivity;
+};
+
+struct raw_block_t
+{
+  uint16_t flag;      ///< UPPER_BANK or LOWER_BANK
+  uint16_t rotation;  ///< 0-35999, divide by 100 to get degrees
+  raw_units units[g_scans_per_block];
+};
+
+/** \brief Raw Velodyne packet.
+ *
+ *  revolution is described in the device manual as incrementing
+ *    (mod 65536) for each physical turn of the device.  Our device
+ *    seems to alternate between two different values every third
+ *    packet.  One value increases, the other decreases.
+ *
+ *  \todo figure out if revolution is only present for one of the
+ *  two types of status fields
+ *
+ *  status has either a temperature encoding or the microcode level
+ */
+struct raw_packet_t
+{
+  raw_block_t blocks[g_blocks_per_packet];
+  uint32_t timestamp;
+  uint16_t factory;
+};
+#pragma pack(pop)
+
+/** \brief Velodyne echo types */
+enum RETURN_TYPE {
+  INVALID = 0,
+  SINGLE_STRONGEST = 1,
+  SINGLE_LAST = 2,
+  DUAL_STRONGEST_FIRST = 3,
+  DUAL_STRONGEST_LAST = 4,
+  DUAL_WEAK_FIRST = 5,
+  DUAL_WEAK_LAST = 6,
+  DUAL_ONLY = 7
+};
+
+}  // namespace nebula::drivers::velodyne_packet

nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_scan_decoder.hpp

@@ -32,133 +32,24 @@
 #include <cmath>
 #include <cstdint>
 #include <memory>
-#include <string>
 #include <tuple>
 #include <vector>
 
 namespace nebula::drivers
 {
-/**
- * Raw Velodyne packet constants and structures.
- */
-static const int g_size_block = 100;
-static const int g_raw_scan_size = 3;
-static const int g_scans_per_block = 32;
-static const int g_block_data_size = (g_scans_per_block * g_raw_scan_size);
-
-static const double g_rotation_resolution = 0.01;     // [deg]
-static const uint16_t g_rotation_max_units = 36000u;  // [deg/100]
-
-static const size_t g_return_mode_index = 1204;
-
-/** @todo make this work for both big and little-endian machines */
-static const uint16_t g_upper_bank = 0xeeff;
-static const uint16_t g_lower_bank = 0xddff;
-
-/** Return Modes **/
-static const uint16_t g_return_mode_strongest = 55;
-static const uint16_t g_return_mode_last = 56;
-static const uint16_t g_return_mode_dual = 57;
-
-/** Special Defines for VLP16 support **/
-static const int g_vlp16_firings_per_block = 2;
-static const int g_vlp16_scans_per_firing = 16;
-static const float g_vlp16_block_duration = 110.592f;  // [µs]
-static const float g_vlp16_dsr_toffset = 2.304f;       // [µs]
-static const float g_vlp16_firing_toffset = 55.296f;   // [µs]
-
-/** Special Defines for VLP32 support **/
-static const float g_vlp32_channel_duration = 2.304f;  // [µs]
-static const float g_vlp32_seq_duration = 55.296f;     // [µs]
-
-/** Special Definitions for VLS128 support **/
-static const float g_vlp128_distance_resolution = 0.004f;  // [m]
-
-/** Special Definitions for VLS128 support **/
-// These are used to detect which bank of 32 lasers is in this block
-static const uint16_t g_vls128_bank_1 = 0xeeff;
-static const uint16_t g_vls128_bank_2 = 0xddff;
-static const uint16_t g_vls128_bank_3 = 0xccff;
-static const uint16_t g_vls128_bank_4 = 0xbbff;
-
-static const float g_vls128_channel_duration =
-  2.665f;  // [µs] Channels corresponds to one laser firing
-static const float g_vls128_seq_duration =
-  53.3f;  // [µs] Sequence is a set of laser firings including recharging
-
-static const size_t g_offset_first_azimuth = 2;
-static const size_t g_offset_last_azimuth = 1102;
-static const uint32_t g_degree_subdivisions = 100;
-
-/** \brief Raw Velodyne data block.
- *
- *  Each block contains data from either the upper or lower laser
- *  bank.  The device returns three times as many upper bank blocks.
- *
- *  use stdint.h types, so things work with both 64 and 32-bit machines
- */
-typedef struct raw_block
-{
-  uint16_t header;    ///< UPPER_BANK or LOWER_BANK
-  uint16_t rotation;  ///< 0-35999, divide by 100 to get degrees
-  uint8_t data[g_block_data_size];
-} raw_block_t;
-
-/** used for unpacking the first two data bytes in a block
- *
- *  They are packed into the actual data stream misaligned.  I doubt
- *  this works on big endian machines.
- */
-union two_bytes {
-  uint16_t uint;
-  uint8_t bytes[2];
-};
-
-static const int g_packet_size = 1206;
-static const int g_blocks_per_packet = 12;
-static const int g_packet_status_size = 4;
-static const int g_scans_per_packet = (g_scans_per_block * g_blocks_per_packet);
-
-/** \brief Raw Velodyne packet.
- *
- *  revolution is described in the device manual as incrementing
- *    (mod 65536) for each physical turn of the device.  Our device
- *    seems to alternate between two different values every third
- *    packet.  One value increases, the other decreases.
- *
- *  \todo figure out if revolution is only present for one of the
- *  two types of status fields
- *
- *  status has either a temperature encoding or the microcode level
- */
-typedef struct raw_packet
-{
-  raw_block_t blocks[g_blocks_per_packet];
-  uint16_t revolution;
-  uint8_t status[g_packet_status_size];
-} raw_packet_t;
-
-/** \brief Velodyne echo types */
-enum RETURN_TYPE {
-  INVALID = 0,
-  SINGLE_STRONGEST = 1,
-  SINGLE_LAST = 2,
-  DUAL_STRONGEST_FIRST = 3,
-  DUAL_STRONGEST_LAST = 4,
-  DUAL_WEAK_FIRST = 5,
-  DUAL_WEAK_LAST = 6,
-  DUAL_ONLY = 7
-};
-
 /// @brief Base class for Velodyne LiDAR decoder
 class VelodyneScanDecoder
 {
 private:
   size_t processed_packets_{0};
   uint32_t prev_packet_first_azm_phased_{0};
-  bool has_scanned_{false};
+
+  static const size_t g_offset_first_azimuth = 2;
+  static const size_t g_offset_last_azimuth = 1102;
+  static const uint32_t g_degree_subdivisions = 100;
 
 protected:
+  bool has_scanned_{false};
   /// @brief Checks if the current packet completes the ongoing scan.
   /// TODO: this has been moved from velodyne_hw_interface.cpp and is a temporary solution until
   /// the Velodyne decoder uses the same structure as Hesai/Robosense
@@ -227,11 +118,7 @@ class VelodyneScanDecoder
 
   /// @brief Virtual function for getting the flag indicating whether one cycle is ready
   /// @return Readied
-  bool has_scanned() { return has_scanned_; }
-
-  /// @brief Calculation of points in each packet
-  /// @return # of points
-  virtual int points_per_packet() = 0;
+  virtual bool has_scanned() = 0;
 
   /// @brief Virtual function for getting the constructed point cloud
   /// @return tuple of Point cloud and timestamp

nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/velodyne_sensor.hpp

@@ -0,0 +1,60 @@
+// Copyright 2024 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#pragma once
+
+#include <cstdint>
+
+namespace nebula::drivers
+{
+class VelodyneSensor
+{
+public:
+  VelodyneSensor() = default;
+  VelodyneSensor(const VelodyneSensor &) = default;
+  VelodyneSensor(VelodyneSensor &&) = default;
+  VelodyneSensor & operator=(const VelodyneSensor &) = default;
+  VelodyneSensor & operator=(VelodyneSensor &&) = default;
+  virtual ~VelodyneSensor() = default;
+
+  /// @brief each VLP lidars packat structure in user manual. If you know details, see commens in
+  /// each <vlp_list>.hpp file. To ignore an empty data blocks which is created by only VLS128 dual
+  /// return mode case
+  virtual int get_num_padding_blocks(bool /* dual_return */) { return 0; }
+
+  /// @brief each VLP lidar laser timing in user manual. If you know details, see commens in each
+  /// <vlp_list>.hpp file. calculate and stack the firing timing for each laser timeing used in
+  /// getAzimuthCorrected to calculate the corrected azimuth
+  virtual bool fill_azimuth_cache() { return false; }
+
+  /// @brief VSL128User manual p. Packet structure
+  virtual uint32_t get_bank(uint32_t bank, uint32_t /* header */) { return bank; }
+
+  /// @brief each VLP calculating sample code and formula in user manual. If you know details, see
+  /// commens in each <vlp_list>.hpp file. calculate the corrected azimuth from each firing timing.
+  virtual uint16_t get_azimuth_corrected(
+    uint16_t azimuth, float azimuth_diff, int firing_sequence, int firing_order) = 0;
+
+  /// @brief each VLP calculating sample code and formula in user manual. Check packet structure.
+  /// Get a correct firing order
+  virtual int get_firing_order(int /* channels_per_block */, int /* scans_per_firing */)
+  {
+    return 0;
+  }
+
+  /// @brief each VLP calculating sample code and formula in user manual. Check packet structure.
+  /// Get a correct channel number
+  virtual int get_channel_number(int /* unit_idx */) { return 0; }
+};
+}  // namespace nebula::drivers