refactor: remove pandarScan pub/sub, decode one packet at a time

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_decoder.hpp

@@ -52,17 +52,17 @@ class HesaiDecoder : public HesaiScanDecoder
     block_firing_offset_ns_;
 
   /// @brief Validates and parse PandarPacket. Currently only checks size, not checksums etc.
-  /// @param pandar_packet The incoming PandarPacket
+  /// @param packet The incoming PandarPacket
   /// @return Whether the packet was parsed successfully
-  bool parsePacket(const pandar_msgs::msg::PandarPacket & pandar_packet)
+  bool parsePacket(const std::vector<uint8_t> & packet)
   {
-    if (pandar_packet.size < sizeof(typename SensorT::packet_t)) {
+    if (packet.size() < sizeof(typename SensorT::packet_t)) {
       RCLCPP_ERROR_STREAM(
-        logger_, "Packet size mismatch:" << pandar_packet.size << " | Expected at least:"
+        logger_, "Packet size mismatch:" << packet.size() << " | Expected at least:"
                                          << sizeof(typename SensorT::packet_t));
       return false;
     }
-    if (std::memcpy(&packet_, pandar_packet.data.data(), sizeof(typename SensorT::packet_t))) {
+    if (std::memcpy(&packet_, packet.data(), sizeof(typename SensorT::packet_t))) {
       // FIXME(mojomex) do validation?
       // RCLCPP_DEBUG(logger_, "Packet parsed successfully");
       return true;
@@ -222,9 +222,9 @@ class HesaiDecoder : public HesaiScanDecoder
     output_pc_->reserve(SensorT::MAX_SCAN_BUFFER_POINTS);
   }
 
-  int unpack(const pandar_msgs::msg::PandarPacket & pandar_packet) override
+  int unpack(const std::vector<uint8_t> & packet) override
   {
-    if (!parsePacket(pandar_packet)) {
+    if (!parsePacket(packet)) {
       return -1;
     }
 

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/decoders/hesai_scan_decoder.hpp

@@ -26,9 +26,9 @@ class HesaiScanDecoder
   HesaiScanDecoder() = default;
 
   /// @brief Parses PandarPacket and add its points to the point cloud
-  /// @param pandar_packet The incoming PandarPacket
+  /// @param packet The incoming PandarPacket
   /// @return The last azimuth processed
-  virtual int unpack(const pandar_msgs::msg::PandarPacket & pandar_packet) = 0;
+  virtual int unpack(const std::vector<uint8_t> & packet) = 0;
 
   /// @brief Indicates whether one full scan is ready
   /// @return Whether a scan is ready

nebula_decoders/include/nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp

@@ -54,8 +54,8 @@ class HesaiDriver : NebulaDriverBase
   /// @brief Convert PandarScan message to point cloud
   /// @param pandar_scan Message
   /// @return tuple of Point cloud and timestamp
-  std::tuple<drivers::NebulaPointCloudPtr, double> ConvertScanToPointcloud(
-    const std::shared_ptr<pandar_msgs::msg::PandarScan> & pandar_scan);
+  std::tuple<drivers::NebulaPointCloudPtr, double> ParseCloudPacket(
+    const std::vector<uint8_t> & packet);
 };
 
 }  // namespace drivers

nebula_decoders/src/nebula_decoders_hesai/hesai_driver.cpp

@@ -72,8 +72,8 @@ HesaiDriver::HesaiDriver(
   }
 }
 
-std::tuple<drivers::NebulaPointCloudPtr, double> HesaiDriver::ConvertScanToPointcloud(
-  const std::shared_ptr<pandar_msgs::msg::PandarScan> & pandar_scan)
+std::tuple<drivers::NebulaPointCloudPtr, double> HesaiDriver::ParseCloudPacket(
+  const std::vector<uint8_t> & packet)
 {
   std::tuple<drivers::NebulaPointCloudPtr, double> pointcloud;
   auto logger = rclcpp::get_logger("HesaiDriver");
@@ -83,20 +83,17 @@ std::tuple<drivers::NebulaPointCloudPtr, double> HesaiDriver::ConvertScanToPoint
     return pointcloud;
   }
 
-  int cnt = 0, last_azimuth = 0;
-  for (auto & packet : pandar_scan->packets) {
-    last_azimuth = scan_decoder_->unpack(packet);
-    if (scan_decoder_->hasScanned()) {
-      pointcloud = scan_decoder_->getPointcloud();
-      cnt++;
-    }
+  scan_decoder_->unpack(packet);
+  if (scan_decoder_->hasScanned()) {
+    pointcloud = scan_decoder_->getPointcloud();
   }
 
-  if (cnt == 0) {
-    RCLCPP_ERROR_STREAM(
-      logger, "Scanned " << pandar_scan->packets.size() << " packets, but no "
-                         << "pointclouds were generated. Last azimuth: " << last_azimuth);
-  }
+  // todo
+  // if (cnt == 0) {
+  //   RCLCPP_ERROR_STREAM(
+  //     logger, "Scanned " << pandar_scan->packets.size() << " packets, but no "
+  //                        << "pointclouds were generated. Last azimuth: " << last_azimuth);
+  // }
 
   return pointcloud;
 }

nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp

@@ -102,8 +102,8 @@ class HesaiHwInterface : NebulaHwInterfaceBase
   std::unique_ptr<pandar_msgs::msg::PandarScan> scan_cloud_ptr_;
   std::function<bool(size_t)>
     is_valid_packet_; /*Lambda Function Array to verify proper packet size*/
-  std::function<void(std::unique_ptr<pandar_msgs::msg::PandarScan> buffer)>
-    scan_reception_callback_; /**This function pointer is called when the scan is complete*/
+  std::function<void(const std::vector<uint8_t> & buffer)>
+    cloud_packet_callback_; /**This function pointer is called when the scan is complete*/
 
   int prev_phase_{};
 
@@ -192,7 +192,7 @@ class HesaiHwInterface : NebulaHwInterfaceBase
   /// @param scan_callback Callback function
   /// @return Resulting status
   Status RegisterScanCallback(
-    std::function<void(std::unique_ptr<pandar_msgs::msg::PandarScan>)> scan_callback);
+    std::function<void(const std::vector<uint8_t> &)> scan_callback);
   /// @brief Getting data with PTC_COMMAND_GET_LIDAR_CALIBRATION
   /// @return Resulting status
   std::string GetLidarCalibrationString();

nebula_hw_interfaces/src/nebula_hesai_hw_interfaces/hesai_hw_interface.cpp

@@ -218,61 +218,19 @@ Status HesaiHwInterface::SensorInterfaceStart()
 }
 
 Status HesaiHwInterface::RegisterScanCallback(
-  std::function<void(std::unique_ptr<pandar_msgs::msg::PandarScan>)> scan_callback)
+  std::function<void(const std::vector<uint8_t> &)> scan_callback)
 {
-  scan_reception_callback_ = std::move(scan_callback);
+  cloud_packet_callback_ = std::move(scan_callback);
   return Status::OK;
 }
 
 void HesaiHwInterface::ReceiveSensorPacketCallback(const std::vector<uint8_t> & buffer)
 {
-  int scan_phase = static_cast<int>(sensor_configuration_->scan_phase * 100.0);
   if (!is_valid_packet_(buffer.size())) {
     PrintDebug("Invalid Packet: " + std::to_string(buffer.size()));
     return;
   }
-  const uint32_t buffer_size = buffer.size();
-  pandar_msgs::msg::PandarPacket pandar_packet;
-  std::copy_n(std::make_move_iterator(buffer.begin()), buffer_size, pandar_packet.data.begin());
-  pandar_packet.size = buffer_size;
-  auto now = std::chrono::system_clock::now();
-  auto now_secs = std::chrono::duration_cast<std::chrono::seconds>(now.time_since_epoch()).count();
-  auto now_nanosecs =
-    std::chrono::duration_cast<std::chrono::nanoseconds>(now.time_since_epoch()).count();
-  pandar_packet.stamp.sec = static_cast<int>(now_secs);
-  pandar_packet.stamp.nanosec = static_cast<std::uint32_t>(now_nanosecs % 1'000'000'000);
-  scan_cloud_ptr_->packets.emplace_back(pandar_packet);
-
-  int current_phase = 0;
-  bool comp_flg = false;
-
-  const auto & data = scan_cloud_ptr_->packets.back().data;
-  current_phase = (data[azimuth_index_] & 0xff) + ((data[azimuth_index_ + 1] & 0xff) << 8);
-  if (is_solid_state) {
-    current_phase = (static_cast<int>(current_phase) + 36000 - 0) % 12000;
-    if (current_phase >= prev_phase_ || scan_cloud_ptr_->packets.size() < 2) {
-      prev_phase_ = current_phase;
-    } else {
-      comp_flg = true;
-    }
-  } else {
-    current_phase = (static_cast<int>(current_phase) + 36000 - scan_phase) % 36000;
-
-    if (current_phase >= prev_phase_ || scan_cloud_ptr_->packets.size() < 2) {
-      prev_phase_ = current_phase;
-    } else {
-      comp_flg = true;
-    }
-  }
-
-  if (comp_flg) {  // Scan complete
-    if (scan_reception_callback_) {
-      scan_cloud_ptr_->header.stamp = scan_cloud_ptr_->packets.front().stamp;
-      // Callback
-      scan_reception_callback_(std::move(scan_cloud_ptr_));
-      scan_cloud_ptr_ = std::make_unique<pandar_msgs::msg::PandarScan>();
-    }
-  }
+  cloud_packet_callback_(buffer);
 }
 Status HesaiHwInterface::SensorInterfaceStop()
 {

nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp

@@ -57,10 +57,6 @@ class HesaiRosWrapper final : public rclcpp::Node, NebulaRosWrapperBase
   explicit HesaiRosWrapper(const rclcpp::NodeOptions & options);
   ~HesaiRosWrapper();
 
-  /// @brief Callback for PandarScan subscriber
-  /// @param scan_msg Received PandarScan
-  void ReceiveScanMsgCallback(const pandar_msgs::msg::PandarScan::SharedPtr scan_msg);
-
   /// @brief Get current status of this driver
   /// @return Current status
   Status GetStatus();
@@ -133,7 +129,7 @@ class HesaiRosWrapper final : public rclcpp::Node, NebulaRosWrapperBase
     const drivers::SensorConfigurationBase & sensor_configuration) override;
   /// @brief Callback for receiving PandarScan
   /// @param scan_buffer Received PandarScan
-  void ReceiveScanDataCallback(std::unique_ptr<pandar_msgs::msg::PandarScan> scan_buffer);
+  void ReceiveCloudPacketCallback(const std::vector<uint8_t> & scan_buffer);
 
   /// @brief rclcpp parameter callback
   /// @param parameters Received parameters

nebula_ros/src/hesai/hesai_ros_wrapper.cpp

@@ -69,7 +69,7 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options)
 #endif
 
   hw_interface_.RegisterScanCallback(
-    std::bind(&HesaiRosWrapper::ReceiveScanDataCallback, this, std::placeholders::_1));
+    std::bind(&HesaiRosWrapper::ReceiveCloudPacketCallback, this, std::placeholders::_1));
   pandar_scan_pub_ =
     this->create_publisher<pandar_msgs::msg::PandarScan>("pandar_packets", rclcpp::SensorDataQoS());
 
@@ -117,9 +117,7 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options)
   rmw_qos_profile_t qos_profile = rmw_qos_profile_sensor_data;
   auto qos = rclcpp::QoS(rclcpp::QoSInitialization(qos_profile.history, 10),
                          qos_profile);
-  pandar_scan_sub_ = create_subscription<pandar_msgs::msg::PandarScan>(
-    "pandar_packets", qos,
-    std::bind(&HesaiRosWrapper::ReceiveScanMsgCallback, this, std::placeholders::_1));
+
   nebula_points_pub_ =
     this->create_publisher<sensor_msgs::msg::PointCloud2>("pandar_points", rclcpp::SensorDataQoS());
   aw_points_base_pub_ =
@@ -198,16 +196,22 @@ cbg_r_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant);
 }
 }
 
-void HesaiRosWrapper::ReceiveScanMsgCallback(
-  const pandar_msgs::msg::PandarScan::SharedPtr scan_msg)
+void HesaiRosWrapper::ReceiveCloudPacketCallback(
+  const std::vector<uint8_t> & packet)
 {
+  // Driver is not initialized yet
+  if (!driver_ptr_) {
+    return;
+  }
+
   auto t_start = std::chrono::high_resolution_clock::now();
   std::tuple<nebula::drivers::NebulaPointCloudPtr, double> pointcloud_ts =
-    driver_ptr_->ConvertScanToPointcloud(scan_msg);
+    driver_ptr_->ParseCloudPacket(packet);
   nebula::drivers::NebulaPointCloudPtr pointcloud = std::get<0>(pointcloud_ts);
 
   if (pointcloud == nullptr) {
-    RCLCPP_WARN_STREAM(get_logger(), "Empty cloud parsed.");
+    // todo
+    // RCLCPP_WARN_STREAM(get_logger(), "Empty cloud parsed.");
     return;
   };
   if (
@@ -845,15 +849,6 @@ Status HesaiRosWrapper::InitializeHwMonitor(  // todo: don't think this is neede
   return Status::OK;
 }
 
-void HesaiRosWrapper::ReceiveScanDataCallback(
-  std::unique_ptr<pandar_msgs::msg::PandarScan> scan_buffer)
-{
-  // Publish
-  scan_buffer->header.frame_id = sensor_cfg_ptr_->frame_id;
-  scan_buffer->header.stamp = scan_buffer->packets.front().stamp;
-  pandar_scan_pub_->publish(std::move(scan_buffer));
-}
-
 rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::paramCallback(
   const std::vector<rclcpp::Parameter> & p)
 {