feat(nebula): add autoware point type publishers (#27)

* nebula_common. add aw point type conversions

Signed-off-by: amc-nu <abraham.monrroy@gmail.com>

* nebula_ros. add aw point type conversion/publishing

Signed-off-by: amc-nu <abraham.monrroy@gmail.com>

* style(pre-commit): autofix

---------

Signed-off-by: amc-nu <abraham.monrroy@gmail.com>
Co-authored-by: amc-nu <amc-nu@users.noreply.github.com>

nebula_common/include/nebula_common/nebula_common.hpp

@@ -490,6 +490,8 @@ inline ReturnMode ReturnModeFromString(const std::string & return_mode)
 [[maybe_unused]] pcl::PointCloud<PointXYZIR>::Ptr convertPointXYZIRCAEDTToPointXYZIR(
   const pcl::PointCloud<PointXYZIRCAEDT>::ConstPtr & input_pointcloud);
 
+pcl::PointCloud<PointXYZIRADT>::Ptr convertPointXYZIRCAEDTToPointXYZIRADT(
+  const pcl::PointCloud<PointXYZIRCAEDT>::ConstPtr & input_pointcloud);
 }  // namespace drivers
 }  // namespace nebula
 

nebula_common/src/nebula_common.cpp

@@ -36,6 +36,31 @@ pcl::PointCloud<PointXYZIR>::Ptr convertPointXYZIRCAEDTToPointXYZIR(
     point.y = p.y;
     point.z = p.z;
     point.intensity = p.intensity;
+    point.ring = p.channel;
+    output_pointcloud->points.emplace_back(point);
+  }
+
+  output_pointcloud->header = input_pointcloud->header;
+  output_pointcloud->height = 1;
+  output_pointcloud->width = output_pointcloud->points.size();
+  return output_pointcloud;
+}
+
+pcl::PointCloud<PointXYZIRADT>::Ptr convertPointXYZIRCAEDTToPointXYZIRADT(
+  const pcl::PointCloud<PointXYZIRCAEDT>::ConstPtr & input_pointcloud)
+{
+  pcl::PointCloud<PointXYZIRADT>::Ptr output_pointcloud(new pcl::PointCloud<PointXYZIRADT>);
+  output_pointcloud->reserve(input_pointcloud->points.size());
+  PointXYZIRADT point;
+  for (const auto & p : input_pointcloud->points) {
+    point.x = p.x;
+    point.y = p.y;
+    point.z = p.z;
+    point.intensity = p.intensity;
+    point.ring = p.channel;
+    point.azimuth = p.azimuth;
+    point.distance = p.distance;
+    point.time_stamp = p.time_stamp;
     output_pointcloud->points.emplace_back(point);
   }
 

nebula_ros/CMakeLists.txt

@@ -29,7 +29,7 @@ include_directories(
         ${PCL_INCLUDE_DIRS}
         ${PCL_COMMON_INCLUDE_DIRS}
 )
-link_libraries(${YAML_CPP_LIBRARIES})
+link_libraries(${YAML_CPP_LIBRARIES} ${PCL_LIBRARIES})
 ## Hesai
 # Hw Interface
 ament_auto_add_library(hesai_hw_ros_wrapper SHARED

nebula_ros/include/nebula_ros/hesai/hesai_decoder_ros_wrapper.hpp

@@ -26,7 +26,9 @@ class HesaiDriverRosWrapper final : public rclcpp::Node, NebulaDriverRosWrapperB
   std::shared_ptr<drivers::HesaiDriver> driver_ptr_;
   Status wrapper_status_;
   rclcpp::Subscription<pandar_msgs::msg::PandarScan>::SharedPtr pandar_scan_sub_;
-  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr pandar_points_pub_;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr nebula_points_pub_;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr aw_points_ex_pub_;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr aw_points_base_pub_;
 
   std::shared_ptr<drivers::HesaiCalibrationConfiguration> calibration_cfg_ptr_;
   std::shared_ptr<drivers::SensorConfigurationBase> sensor_cfg_ptr_;
@@ -71,6 +73,15 @@ class HesaiDriverRosWrapper final : public rclcpp::Node, NebulaDriverRosWrapperB
       std::chrono::duration<double>(seconds));
   }
 
+  /***
+   * Publishes a sensor_msgs::msg::PointCloud2 to the specified publisher
+   * @param pointcloud unique pointer containing the point cloud to publish
+   * @param publisher
+   */
+  void PublishCloud(
+    std::unique_ptr<sensor_msgs::msg::PointCloud2> pointcloud,
+    const rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr & publisher);
+
 public:
   explicit HesaiDriverRosWrapper(const rclcpp::NodeOptions & options);
 
@@ -83,7 +94,7 @@ class HesaiDriverRosWrapper final : public rclcpp::Node, NebulaDriverRosWrapperB
   Status GetStatus();
 
 private:
-  /// @brief File path of Correction data (Only here because because it is required only for AT)
+  /// @brief File path of Correction data (Only required only for AT)
   std::string correction_file_path;
 };
 

nebula_ros/include/nebula_ros/velodyne/velodyne_decoder_ros_wrapper.hpp

@@ -25,7 +25,9 @@ class VelodyneDriverRosWrapper final : public rclcpp::Node, NebulaDriverRosWrapp
   std::shared_ptr<drivers::VelodyneDriver> driver_ptr_;
   Status wrapper_status_;
   rclcpp::Subscription<velodyne_msgs::msg::VelodyneScan>::SharedPtr velodyne_scan_sub_;
-  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr velodyne_points_pub_;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr nebula_points_pub_;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr aw_points_ex_pub_;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr aw_points_base_pub_;
 
   std::shared_ptr<drivers::CalibrationConfigurationBase> calibration_cfg_ptr_;
   std::shared_ptr<drivers::SensorConfigurationBase> sensor_cfg_ptr_;
@@ -55,6 +57,15 @@ class VelodyneDriverRosWrapper final : public rclcpp::Node, NebulaDriverRosWrapp
       std::chrono::duration<double>(seconds));
   }
 
+  /***
+   * Publishes a sensor_msgs::msg::PointCloud2 to the specified publisher
+   * @param pointcloud unique pointer containing the point cloud to publish
+   * @param publisher
+   */
+  void PublishCloud(
+    std::unique_ptr<sensor_msgs::msg::PointCloud2> pointcloud,
+    const rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr & publisher);
+
 public:
   explicit VelodyneDriverRosWrapper(const rclcpp::NodeOptions & options);
 

nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp

@@ -45,8 +45,12 @@ HesaiDriverRosWrapper::HesaiDriverRosWrapper(const rclcpp::NodeOptions & options
   pandar_scan_sub_ = create_subscription<pandar_msgs::msg::PandarScan>(
     "pandar_packets", rclcpp::SensorDataQoS(),
     std::bind(&HesaiDriverRosWrapper::ReceiveScanMsgCallback, this, std::placeholders::_1));
-  pandar_points_pub_ =
+  nebula_points_pub_ =
     this->create_publisher<sensor_msgs::msg::PointCloud2>("pandar_points", rclcpp::SensorDataQoS());
+  aw_points_base_pub_ =
+    this->create_publisher<sensor_msgs::msg::PointCloud2>("aw_points", rclcpp::SensorDataQoS());
+  aw_points_ex_pub_ =
+    this->create_publisher<sensor_msgs::msg::PointCloud2>("aw_points_ex", rclcpp::SensorDataQoS());
 }
 
 void HesaiDriverRosWrapper::ReceiveScanMsgCallback(
@@ -63,20 +67,48 @@ void HesaiDriverRosWrapper::ReceiveScanMsgCallback(
     RCLCPP_WARN_STREAM(get_logger(), "Empty cloud parsed.");
     return;
   };
-  auto ros_pc_msg_ptr = std::make_unique<sensor_msgs::msg::PointCloud2>();
-  pcl::toROSMsg(*pointcloud, *ros_pc_msg_ptr);
-  if (!pointcloud->points.empty()) {
+  if (
+    nebula_points_pub_->get_subscription_count() > 0 ||
+    nebula_points_pub_->get_intra_process_subscription_count() > 0) {
+    auto ros_pc_msg_ptr = std::make_unique<sensor_msgs::msg::PointCloud2>();
+    pcl::toROSMsg(*pointcloud, *ros_pc_msg_ptr);
     ros_pc_msg_ptr->header.stamp =
       rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count());
-    if (ros_pc_msg_ptr->header.stamp.sec < 0)  // && false)
-    {
-      rclcpp::Clock system_clock(RCL_SYSTEM_TIME);
-      ros_pc_msg_ptr->header.stamp = system_clock.now();
-      RCLCPP_WARN_STREAM(this->get_logger(), "Timestamp error... use system_clock");
-    }
+    PublishCloud(std::move(ros_pc_msg_ptr), nebula_points_pub_);
+  }
+  if (
+    aw_points_base_pub_->get_subscription_count() > 0 ||
+    aw_points_base_pub_->get_intra_process_subscription_count() > 0) {
+    const auto autoware_cloud_xyzi =
+      nebula::drivers::convertPointXYZIRCAEDTToPointXYZIR(pointcloud);
+    auto ros_pc_msg_ptr = std::make_unique<sensor_msgs::msg::PointCloud2>();
+    pcl::toROSMsg(*autoware_cloud_xyzi, *ros_pc_msg_ptr);
+    ros_pc_msg_ptr->header.stamp =
+      rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count());
+    PublishCloud(std::move(ros_pc_msg_ptr), aw_points_base_pub_);
+  }
+  if (
+    aw_points_ex_pub_->get_subscription_count() > 0 ||
+    aw_points_ex_pub_->get_intra_process_subscription_count() > 0) {
+    const auto autoware_ex_cloud =
+      nebula::drivers::convertPointXYZIRCAEDTToPointXYZIRADT(pointcloud);
+    auto ros_pc_msg_ptr = std::make_unique<sensor_msgs::msg::PointCloud2>();
+    pcl::toROSMsg(*autoware_ex_cloud, *ros_pc_msg_ptr);
+    ros_pc_msg_ptr->header.stamp =
+      rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count());
+    PublishCloud(std::move(ros_pc_msg_ptr), aw_points_ex_pub_);
+  }
+}
+
+void HesaiDriverRosWrapper::PublishCloud(
+  std::unique_ptr<sensor_msgs::msg::PointCloud2> pointcloud,
+  const rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr & publisher)
+{
+  if (pointcloud->header.stamp.sec < 0) {
+    RCLCPP_WARN_STREAM(this->get_logger(), "Timestamp error, verify clock source.");
   }
-  ros_pc_msg_ptr->header.frame_id = sensor_cfg_ptr_->frame_id;
-  pandar_points_pub_->publish(std::move(ros_pc_msg_ptr));
+  pointcloud->header.frame_id = sensor_cfg_ptr_->frame_id;
+  publisher->publish(std::move(pointcloud));
 }
 
 Status HesaiDriverRosWrapper::InitializeDriver(

nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp

@@ -32,8 +32,12 @@ VelodyneDriverRosWrapper::VelodyneDriverRosWrapper(const rclcpp::NodeOptions & o
   velodyne_scan_sub_ = create_subscription<velodyne_msgs::msg::VelodyneScan>(
     "velodyne_packets", rclcpp::SensorDataQoS(),
     std::bind(&VelodyneDriverRosWrapper::ReceiveScanMsgCallback, this, std::placeholders::_1));
-  velodyne_points_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>(
+  nebula_points_pub_ = this->create_publisher<sensor_msgs::msg::PointCloud2>(
     "velodyne_points", rclcpp::SensorDataQoS());
+  aw_points_base_pub_ =
+    this->create_publisher<sensor_msgs::msg::PointCloud2>("aw_points", rclcpp::SensorDataQoS());
+  aw_points_ex_pub_ =
+    this->create_publisher<sensor_msgs::msg::PointCloud2>("aw_points_ex", rclcpp::SensorDataQoS());
 }
 
 void VelodyneDriverRosWrapper::ReceiveScanMsgCallback(
@@ -46,17 +50,48 @@ void VelodyneDriverRosWrapper::ReceiveScanMsgCallback(
     driver_ptr_->ConvertScanToPointcloud(scan_msg);
   nebula::drivers::NebulaPointCloudPtr pointcloud = std::get<0>(pointcloud_ts);
 
-  auto ros_pc_msg_ptr = std::make_unique<sensor_msgs::msg::PointCloud2>();
-  pcl::toROSMsg(*pointcloud, *ros_pc_msg_ptr);
-  if (!pointcloud->points.empty()) {
-    //    double first_point_timestamp = pointcloud->points.front().time_stamp;
-    //    ros_pc_msg_ptr->header.stamp =
-    //      rclcpp::Time(SecondsToChronoNanoSeconds(first_point_timestamp).count());
+  if (
+    nebula_points_pub_->get_subscription_count() > 0 ||
+    nebula_points_pub_->get_intra_process_subscription_count() > 0) {
+    auto ros_pc_msg_ptr = std::make_unique<sensor_msgs::msg::PointCloud2>();
+    pcl::toROSMsg(*pointcloud, *ros_pc_msg_ptr);
     ros_pc_msg_ptr->header.stamp =
       rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count());
+    PublishCloud(std::move(ros_pc_msg_ptr), nebula_points_pub_);
   }
-  ros_pc_msg_ptr->header.frame_id = sensor_cfg_ptr_->frame_id;
-  velodyne_points_pub_->publish(std::move(ros_pc_msg_ptr));
+  if (
+    aw_points_base_pub_->get_subscription_count() > 0 ||
+    aw_points_base_pub_->get_intra_process_subscription_count() > 0) {
+    const auto autoware_cloud_xyzi =
+      nebula::drivers::convertPointXYZIRCAEDTToPointXYZIR(pointcloud);
+    auto ros_pc_msg_ptr = std::make_unique<sensor_msgs::msg::PointCloud2>();
+    pcl::toROSMsg(*autoware_cloud_xyzi, *ros_pc_msg_ptr);
+    ros_pc_msg_ptr->header.stamp =
+      rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count());
+    PublishCloud(std::move(ros_pc_msg_ptr), aw_points_base_pub_);
+  }
+  if (
+    aw_points_ex_pub_->get_subscription_count() > 0 ||
+    aw_points_ex_pub_->get_intra_process_subscription_count() > 0) {
+    const auto autoware_ex_cloud =
+      nebula::drivers::convertPointXYZIRCAEDTToPointXYZIRADT(pointcloud);
+    auto ros_pc_msg_ptr = std::make_unique<sensor_msgs::msg::PointCloud2>();
+    pcl::toROSMsg(*autoware_ex_cloud, *ros_pc_msg_ptr);
+    ros_pc_msg_ptr->header.stamp =
+      rclcpp::Time(SecondsToChronoNanoSeconds(std::get<1>(pointcloud_ts)).count());
+    PublishCloud(std::move(ros_pc_msg_ptr), aw_points_ex_pub_);
+  }
+}
+
+void VelodyneDriverRosWrapper::PublishCloud(
+  std::unique_ptr<sensor_msgs::msg::PointCloud2> pointcloud,
+  const rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr & publisher)
+{
+  if (pointcloud->header.stamp.sec < 0) {
+    RCLCPP_WARN_STREAM(this->get_logger(), "Timestamp error, verify clock source.");
+  }
+  pointcloud->header.frame_id = sensor_cfg_ptr_->frame_id;
+  publisher->publish(std::move(pointcloud));
 }
 
 Status VelodyneDriverRosWrapper::InitializeDriver(