diff --git a/nebula_common/include/nebula_common/nebula_common.hpp b/nebula_common/include/nebula_common/nebula_common.hpp index 69a94ce020110..5eaca07d965c5 100644 --- a/nebula_common/include/nebula_common/nebula_common.hpp +++ b/nebula_common/include/nebula_common/nebula_common.hpp @@ -490,6 +490,8 @@ inline ReturnMode ReturnModeFromString(const std::string & return_mode) [[maybe_unused]] pcl::PointCloud::Ptr convertPointXYZIRCAEDTToPointXYZIR( const pcl::PointCloud::ConstPtr & input_pointcloud); +pcl::PointCloud::Ptr convertPointXYZIRCAEDTToPointXYZIRADT( + const pcl::PointCloud::ConstPtr & input_pointcloud); } // namespace drivers } // namespace nebula diff --git a/nebula_common/src/nebula_common.cpp b/nebula_common/src/nebula_common.cpp index fe9400bdec804..9b0c6aaec73eb 100644 --- a/nebula_common/src/nebula_common.cpp +++ b/nebula_common/src/nebula_common.cpp @@ -36,6 +36,31 @@ pcl::PointCloud::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::Ptr convertPointXYZIRCAEDTToPointXYZIRADT( + const pcl::PointCloud::ConstPtr & input_pointcloud) +{ + pcl::PointCloud::Ptr output_pointcloud(new pcl::PointCloud); + 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); } diff --git a/nebula_ros/CMakeLists.txt b/nebula_ros/CMakeLists.txt index eb40cad9f4339..6848b9deca46a 100644 --- a/nebula_ros/CMakeLists.txt +++ b/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 diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_decoder_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_decoder_ros_wrapper.hpp index 28e610cc2e579..f42782b506f9f 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_decoder_ros_wrapper.hpp +++ b/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 driver_ptr_; Status wrapper_status_; rclcpp::Subscription::SharedPtr pandar_scan_sub_; - rclcpp::Publisher::SharedPtr pandar_points_pub_; + rclcpp::Publisher::SharedPtr nebula_points_pub_; + rclcpp::Publisher::SharedPtr aw_points_ex_pub_; + rclcpp::Publisher::SharedPtr aw_points_base_pub_; std::shared_ptr calibration_cfg_ptr_; std::shared_ptr sensor_cfg_ptr_; @@ -71,6 +73,15 @@ class HesaiDriverRosWrapper final : public rclcpp::Node, NebulaDriverRosWrapperB std::chrono::duration(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 pointcloud, + const rclcpp::Publisher::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; }; diff --git a/nebula_ros/include/nebula_ros/velodyne/velodyne_decoder_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/velodyne/velodyne_decoder_ros_wrapper.hpp index 4c56d104f2d88..d9a76e14ed178 100644 --- a/nebula_ros/include/nebula_ros/velodyne/velodyne_decoder_ros_wrapper.hpp +++ b/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 driver_ptr_; Status wrapper_status_; rclcpp::Subscription::SharedPtr velodyne_scan_sub_; - rclcpp::Publisher::SharedPtr velodyne_points_pub_; + rclcpp::Publisher::SharedPtr nebula_points_pub_; + rclcpp::Publisher::SharedPtr aw_points_ex_pub_; + rclcpp::Publisher::SharedPtr aw_points_base_pub_; std::shared_ptr calibration_cfg_ptr_; std::shared_ptr sensor_cfg_ptr_; @@ -55,6 +57,15 @@ class VelodyneDriverRosWrapper final : public rclcpp::Node, NebulaDriverRosWrapp std::chrono::duration(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 pointcloud, + const rclcpp::Publisher::SharedPtr & publisher); + public: explicit VelodyneDriverRosWrapper(const rclcpp::NodeOptions & options); diff --git a/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp index 17ca3163fe7dd..8fc0124898058 100644 --- a/nebula_ros/src/hesai/hesai_decoder_ros_wrapper.cpp +++ b/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_packets", rclcpp::SensorDataQoS(), std::bind(&HesaiDriverRosWrapper::ReceiveScanMsgCallback, this, std::placeholders::_1)); - pandar_points_pub_ = + nebula_points_pub_ = this->create_publisher("pandar_points", rclcpp::SensorDataQoS()); + aw_points_base_pub_ = + this->create_publisher("aw_points", rclcpp::SensorDataQoS()); + aw_points_ex_pub_ = + this->create_publisher("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(); - 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(); + 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(); + 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(); + 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 pointcloud, + const rclcpp::Publisher::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( diff --git a/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp b/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp index 34fc3b96ec483..a60f80743f51b 100644 --- a/nebula_ros/src/velodyne/velodyne_decoder_ros_wrapper.cpp +++ b/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_packets", rclcpp::SensorDataQoS(), std::bind(&VelodyneDriverRosWrapper::ReceiveScanMsgCallback, this, std::placeholders::_1)); - velodyne_points_pub_ = this->create_publisher( + nebula_points_pub_ = this->create_publisher( "velodyne_points", rclcpp::SensorDataQoS()); + aw_points_base_pub_ = + this->create_publisher("aw_points", rclcpp::SensorDataQoS()); + aw_points_ex_pub_ = + this->create_publisher("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(); - 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(); + 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(); + 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(); + 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 pointcloud, + const rclcpp::Publisher::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(