refactor(velodyne)!: unify wrapper nodes. Currently WIP but compiling

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

@@ -20,6 +20,7 @@
 #include <velodyne_msgs/msg/velodyne_scan.hpp>
 
 #include <tuple>
+#include <angles/angles.h> 
 
 namespace nebula
 {
@@ -138,17 +139,14 @@ class VelodyneScanDecoder
   /// @brief Point cloud overflowing from one cycle
   drivers::NebulaPointCloudPtr overflow_pc_;
 
-  uint16_t scan_phase_{};
-  uint16_t last_phase_{};
-  bool has_scanned_ = true;
   double dual_return_distance_threshold_{};  // Velodyne does this internally, this will not be
                                              // implemented here
   double scan_timestamp_{};
 
   /// @brief SensorConfiguration for this decoder
-  std::shared_ptr<drivers::VelodyneSensorConfiguration> sensor_configuration_;
+  std::shared_ptr<const drivers::VelodyneSensorConfiguration> sensor_configuration_;
   /// @brief Calibration for this decoder
-  std::shared_ptr<drivers::VelodyneCalibrationConfiguration> calibration_configuration_;
+  std::shared_ptr<const drivers::VelodyneCalibrationConfiguration> calibration_configuration_;
 
 public:
   VelodyneScanDecoder(VelodyneScanDecoder && c) = delete;
@@ -161,15 +159,34 @@ class VelodyneScanDecoder
 
   /// @brief Virtual function for parsing and shaping VelodynePacket
   /// @param pandar_packet
-  virtual void unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) = 0;
+  virtual void unpack(const std::vector<uint8_t> & packet, int32_t packet_seconds) = 0;
   /// @brief Virtual function for parsing VelodynePacket based on packet structure
   /// @param pandar_packet
   /// @return Resulting flag
   virtual bool parsePacket(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) = 0;
 
   /// @brief Virtual function for getting the flag indicating whether one cycle is ready
   /// @return Readied
-  virtual bool hasScanned() = 0;
+  virtual bool hasScanned() {
+    if (scan_pc_->size() < 2) {
+      return false;
+    }
+
+    float scan_phase = angles::from_degrees(sensor_configuration_->scan_phase);
+    float first_azimuth = scan_pc_->points.front().azimuth - scan_phase;
+    float last_azimuth = scan_pc_->points.back().azimuth - scan_phase;
+
+    if (first_azimuth < 0) {
+      first_azimuth += 2 * M_PI;
+    }
+
+    if (last_azimuth < 0) {
+      last_azimuth += 2 * M_PI;
+    }
+
+    return first_azimuth > last_azimuth;
+  }
+
   /// @brief Calculation of points in each packet
   /// @return # of points
   virtual int pointsPerPacket() = 0;

nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp16_decoder.hpp

@@ -25,14 +25,11 @@ class Vlp16Decoder : public VelodyneScanDecoder
   /// @param sensor_configuration SensorConfiguration for this decoder
   /// @param calibration_configuration Calibration for this decoder
   explicit Vlp16Decoder(
-    const std::shared_ptr<drivers::VelodyneSensorConfiguration> & sensor_configuration,
-    const std::shared_ptr<drivers::VelodyneCalibrationConfiguration> & calibration_configuration);
+    const std::shared_ptr<const drivers::VelodyneSensorConfiguration> & sensor_configuration,
+    const std::shared_ptr<const drivers::VelodyneCalibrationConfiguration> & calibration_configuration);
   /// @brief Parsing and shaping VelodynePacket
   /// @param velodyne_packet
-  void unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) override;
-  /// @brief Get the flag indicating whether one cycle is ready
-  /// @return Readied
-  bool hasScanned() override;
+  void unpack(const std::vector<uint8_t> & packet, int32_t packet_seconds) override;
   /// @brief Calculation of points in each packet
   /// @return # of points
   int pointsPerPacket() override;

nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vlp32_decoder.hpp

@@ -24,14 +24,11 @@ class Vlp32Decoder : public VelodyneScanDecoder
   /// @param sensor_configuration SensorConfiguration for this decoder
   /// @param calibration_configuration Calibration for this decoder
   explicit Vlp32Decoder(
-    const std::shared_ptr<drivers::VelodyneSensorConfiguration> & sensor_configuration,
-    const std::shared_ptr<drivers::VelodyneCalibrationConfiguration> & calibration_configuration);
+    const std::shared_ptr<const drivers::VelodyneSensorConfiguration> & sensor_configuration,
+    const std::shared_ptr<const drivers::VelodyneCalibrationConfiguration> & calibration_configuration);
   /// @brief Parsing and shaping VelodynePacket
   /// @param velodyne_packet
-  void unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) override;
-  /// @brief Get the flag indicating whether one cycle is ready
-  /// @return Readied
-  bool hasScanned() override;
+  void unpack(const std::vector<uint8_t> & packet, int32_t packet_seconds) override;
   /// @brief Calculation of points in each packet
   /// @return # of points
   int pointsPerPacket() override;

nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/decoders/vls128_decoder.hpp

@@ -21,14 +21,11 @@ class Vls128Decoder : public VelodyneScanDecoder
   /// @param sensor_configuration SensorConfiguration for this decoder
   /// @param calibration_configuration Calibration for this decoder
   explicit Vls128Decoder(
-    const std::shared_ptr<drivers::VelodyneSensorConfiguration> & sensor_configuration,
-    const std::shared_ptr<drivers::VelodyneCalibrationConfiguration> & calibration_configuration);
+    const std::shared_ptr<const drivers::VelodyneSensorConfiguration> & sensor_configuration,
+    const std::shared_ptr<const drivers::VelodyneCalibrationConfiguration> & calibration_configuration);
   /// @brief Parsing and shaping VelodynePacket
   /// @param velodyne_packet
-  void unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_packet) override;
-  /// @brief Get the flag indicating whether one cycle is ready
-  /// @return Readied
-  bool hasScanned() override;
+  void unpack(const std::vector<uint8_t> & packet, int32_t packet_seconds) override;
   /// @brief Calculation of points in each packet
   /// @return # of points
   int pointsPerPacket() override;

nebula_decoders/include/nebula_decoders/nebula_decoders_velodyne/velodyne_driver.hpp

@@ -36,8 +36,8 @@ class VelodyneDriver : NebulaDriverBase
   /// @param sensor_configuration SensorConfiguration for this driver
   /// @param calibration_configuration CalibrationConfiguration for this driver
   VelodyneDriver(
-    const std::shared_ptr<drivers::VelodyneSensorConfiguration> & sensor_configuration,
-    const std::shared_ptr<drivers::VelodyneCalibrationConfiguration> & calibration_configuration);
+    const std::shared_ptr<const drivers::VelodyneSensorConfiguration> & sensor_configuration,
+    const std::shared_ptr<const drivers::VelodyneCalibrationConfiguration> & calibration_configuration);
 
   /// @brief Setting CalibrationConfiguration (not used)
   /// @param calibration_configuration
@@ -52,8 +52,8 @@ class VelodyneDriver : NebulaDriverBase
   /// @brief Convert VelodyneScan message to point cloud
   /// @param velodyne_scan Message
   /// @return tuple of Point cloud and timestamp
-  std::tuple<drivers::NebulaPointCloudPtr, double> ConvertScanToPointcloud(
-    const std::shared_ptr<velodyne_msgs::msg::VelodyneScan> & velodyne_scan);
+  std::tuple<drivers::NebulaPointCloudPtr, double> ParseCloudPacket(
+    const std::vector<uint8_t> & packet, int32_t packet_seconds);
 };
 
 }  // namespace drivers

nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp16_decoder.cpp

@@ -12,8 +12,8 @@ namespace drivers
 namespace vlp16
 {
 Vlp16Decoder::Vlp16Decoder(
-  const std::shared_ptr<drivers::VelodyneSensorConfiguration> & sensor_configuration,
-  const std::shared_ptr<drivers::VelodyneCalibrationConfiguration> & calibration_configuration)
+  const std::shared_ptr<const drivers::VelodyneSensorConfiguration> & sensor_configuration,
+  const std::shared_ptr<const drivers::VelodyneCalibrationConfiguration> & calibration_configuration)
 {
   sensor_configuration_ = sensor_configuration;
   calibration_configuration_ = calibration_configuration;
@@ -56,11 +56,6 @@ Vlp16Decoder::Vlp16Decoder(
   phase_ = (uint16_t)round(sensor_configuration_->scan_phase * 100);
 }
 
-bool Vlp16Decoder::hasScanned()
-{
-  return has_scanned_;
-}
-
 std::tuple<drivers::NebulaPointCloudPtr, double> Vlp16Decoder::get_pointcloud()
 {
   double phase = angles::from_degrees(sensor_configuration_->scan_phase);
@@ -138,12 +133,12 @@ void Vlp16Decoder::reset_overflow(double time_stamp)
   overflow_pc_->points.reserve(max_pts_);
 }
 
-void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_packet)
+void Vlp16Decoder::unpack(const std::vector<uint8_t> & packet, int32_t packet_seconds)
 {
-  const raw_packet_t * raw = (const raw_packet_t *)&velodyne_packet.data[0];
+  const raw_packet_t * raw = (const raw_packet_t *)packet.data();
   float last_azimuth_diff = 0;
   uint16_t azimuth_next;
-  const uint8_t return_mode = velodyne_packet.data[RETURN_MODE_INDEX];
+  const uint8_t return_mode = packet[RETURN_MODE_INDEX];
   const bool dual_return = (return_mode == RETURN_MODE_DUAL);
 
   for (uint block = 0; block < BLOCKS_PER_PACKET; block++) {
@@ -203,7 +198,7 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
               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();
+          auto block_timestamp = packet_seconds;
           if (scan_timestamp_ < 0) {
             scan_timestamp_ = block_timestamp;
           }
@@ -216,7 +211,7 @@ void Vlp16Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
             continue;
           }
           {
-            VelodyneLaserCorrection & corrections =
+            const VelodyneLaserCorrection & corrections =
               calibration_configuration_->velodyne_calibration.laser_corrections[dsr];
             float distance = current_return.uint *
                              calibration_configuration_->velodyne_calibration.distance_resolution_m;

nebula_decoders/src/nebula_decoders_velodyne/decoders/vlp32_decoder.cpp

@@ -12,8 +12,8 @@ namespace drivers
 namespace vlp32
 {
 Vlp32Decoder::Vlp32Decoder(
-  const std::shared_ptr<drivers::VelodyneSensorConfiguration> & sensor_configuration,
-  const std::shared_ptr<drivers::VelodyneCalibrationConfiguration> & calibration_configuration)
+  const std::shared_ptr<const drivers::VelodyneSensorConfiguration> & sensor_configuration,
+  const std::shared_ptr<const drivers::VelodyneCalibrationConfiguration> & calibration_configuration)
 {
   sensor_configuration_ = sensor_configuration;
   calibration_configuration_ = calibration_configuration;
@@ -56,11 +56,6 @@ Vlp32Decoder::Vlp32Decoder(
   }
 }
 
-bool Vlp32Decoder::hasScanned()
-{
-  return has_scanned_;
-}
-
 std::tuple<drivers::NebulaPointCloudPtr, double> Vlp32Decoder::get_pointcloud()
 {
   double phase = angles::from_degrees(sensor_configuration_->scan_phase);
@@ -137,10 +132,10 @@ void Vlp32Decoder::reset_overflow(double time_stamp)
   overflow_pc_->points.reserve(max_pts_);
 }
 
-void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_packet)
+void Vlp32Decoder::unpack(const std::vector<uint8_t> & packet, int32_t packet_seconds)
 {
-  const raw_packet_t * raw = (const raw_packet_t *)&velodyne_packet.data[0];
-  uint8_t return_mode = velodyne_packet.data[RETURN_MODE_INDEX];
+  const raw_packet_t * raw = (const raw_packet_t *)packet.data();
+  uint8_t return_mode = packet[RETURN_MODE_INDEX];
   const bool dual_return = (return_mode == RETURN_MODE_DUAL);
 
   for (int i = 0; i < BLOCKS_PER_PACKET; i++) {
@@ -170,7 +165,7 @@ void Vlp32Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_pa
           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();
+      auto block_timestamp = packet_seconds;
       if (scan_timestamp_ < 0) {
         scan_timestamp_ = block_timestamp;
       }

nebula_decoders/src/nebula_decoders_velodyne/decoders/vls128_decoder.cpp

@@ -12,8 +12,8 @@ namespace drivers
 namespace vls128
 {
 Vls128Decoder::Vls128Decoder(
-  const std::shared_ptr<drivers::VelodyneSensorConfiguration> & sensor_configuration,
-  const std::shared_ptr<drivers::VelodyneCalibrationConfiguration> & calibration_configuration)
+  const std::shared_ptr<const drivers::VelodyneSensorConfiguration> & sensor_configuration,
+  const std::shared_ptr<const drivers::VelodyneCalibrationConfiguration> & calibration_configuration)
 {
   sensor_configuration_ = sensor_configuration;
   calibration_configuration_ = calibration_configuration;
@@ -56,11 +56,6 @@ Vls128Decoder::Vls128Decoder(
   }
 }
 
-bool Vls128Decoder::hasScanned()
-{
-  return has_scanned_;
-}
-
 std::tuple<drivers::NebulaPointCloudPtr, double> Vls128Decoder::get_pointcloud()
 {
   double phase = angles::from_degrees(sensor_configuration_->scan_phase);
@@ -139,12 +134,12 @@ void Vls128Decoder::reset_overflow(double time_stamp)
   overflow_pc_->points.reserve(max_pts_);
 }
 
-void Vls128Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_packet)
+void Vls128Decoder::unpack(const std::vector<uint8_t> & packet, int32_t packet_seconds)
 {
-  const raw_packet_t * raw = (const raw_packet_t *)&velodyne_packet.data[0];
+  const raw_packet_t * raw = (const raw_packet_t *)packet.data();
   float last_azimuth_diff = 0;
   uint16_t azimuth_next;
-  const uint8_t return_mode = velodyne_packet.data[RETURN_MODE_INDEX];
+  const uint8_t return_mode = packet[RETURN_MODE_INDEX];
   const bool dual_return = (return_mode == RETURN_MODE_DUAL);
 
   for (uint block = 0; block < static_cast<uint>(BLOCKS_PER_PACKET - (4 * dual_return)); block++) {
@@ -223,7 +218,7 @@ void Vls128Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_p
             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();
+        auto block_timestamp = packet_seconds;
         if (scan_timestamp_ < 0) {
           scan_timestamp_ = block_timestamp;
         }
@@ -240,7 +235,7 @@ void Vls128Decoder::unpack(const velodyne_msgs::msg::VelodynePacket & velodyne_p
             j + bank_origin;  // offset the laser in this block by which block it's in
           const uint firing_order = laser_number / 8;  // VLS-128 fires 8 lasers at a time
 
-          VelodyneLaserCorrection & corrections =
+          const VelodyneLaserCorrection & corrections =
             calibration_configuration_->velodyne_calibration.laser_corrections[laser_number];
 
           float distance = current_return.uint * VLP128_DISTANCE_RESOLUTION;

nebula_decoders/src/nebula_decoders_velodyne/velodyne_driver.cpp

@@ -9,8 +9,8 @@ namespace nebula
 namespace drivers
 {
 VelodyneDriver::VelodyneDriver(
-  const std::shared_ptr<drivers::VelodyneSensorConfiguration> & sensor_configuration,
-  const std::shared_ptr<drivers::VelodyneCalibrationConfiguration> & calibration_configuration)
+  const std::shared_ptr<const drivers::VelodyneSensorConfiguration> & sensor_configuration,
+  const std::shared_ptr<const drivers::VelodyneCalibrationConfiguration> & calibration_configuration)
 {
   // initialize proper parser from cloud config's model and echo mode
   driver_status_ = nebula::Status::OK;
@@ -46,20 +46,24 @@ Status VelodyneDriver::SetCalibrationConfiguration(
     calibration_configuration.calibration_file + ")");
 }
 
-std::tuple<drivers::NebulaPointCloudPtr, double> VelodyneDriver::ConvertScanToPointcloud(
-  const std::shared_ptr<velodyne_msgs::msg::VelodyneScan> & velodyne_scan)
+std::tuple<drivers::NebulaPointCloudPtr, double> VelodyneDriver::ParseCloudPacket(
+  const std::vector<uint8_t> & packet, int32_t packet_seconds)
 {
   std::tuple<drivers::NebulaPointCloudPtr, double> pointcloud;
-  if (driver_status_ == nebula::Status::OK) {
-    scan_decoder_->reset_pointcloud(
-      velodyne_scan->packets.size(), rclcpp::Time(velodyne_scan->packets.front().stamp).seconds());
-    for (auto & packet : velodyne_scan->packets) {
-      scan_decoder_->unpack(packet);
-    }
-    pointcloud = scan_decoder_->get_pointcloud();
-  } else {
-    std::cout << "not ok driver_status_ = " << driver_status_ << std::endl;
+
+  if (driver_status_ != nebula::Status::OK)
+  {
+    auto logger = rclcpp::get_logger("VelodyneDriver");
+    RCLCPP_ERROR(logger, "Driver not OK.");
+    return pointcloud;
   }
+
+  scan_decoder_->unpack(packet, packet_seconds);
+  if (scan_decoder_->hasScanned())
+  {
+    pointcloud = scan_decoder_->get_pointcloud();
+  }  
+
   return pointcloud;
 }
 Status VelodyneDriver::GetStatus()