diff --git a/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster.hpp b/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster.hpp index f1e275a919a8a..aec3f56936828 100644 --- a/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster.hpp +++ b/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster.hpp @@ -32,6 +32,10 @@ class EuclideanCluster : public EuclideanClusterInterface bool cluster( const pcl::PointCloud::ConstPtr & pointcloud, std::vector> & clusters) override; + + bool cluster( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & pointcloud, + tier4_perception_msgs::msg::DetectedObjectsWithFeature & clusters) override; void setTolerance(float tolerance) { tolerance_ = tolerance; } private: diff --git a/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster_interface.hpp b/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster_interface.hpp index 70c21daabcd66..65b907f747666 100644 --- a/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster_interface.hpp +++ b/perception/euclidean_cluster/include/euclidean_cluster/euclidean_cluster_interface.hpp @@ -16,6 +16,9 @@ #include +#include +#include + #include #include @@ -41,6 +44,10 @@ class EuclideanClusterInterface const pcl::PointCloud::ConstPtr & pointcloud, std::vector> & clusters) = 0; + virtual bool cluster( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_msg, + tier4_perception_msgs::msg::DetectedObjectsWithFeature & output_clusters) = 0; + protected: bool use_height_ = true; int min_cluster_size_; diff --git a/perception/euclidean_cluster/include/euclidean_cluster/utils.hpp b/perception/euclidean_cluster/include/euclidean_cluster/utils.hpp index 2ac77aebdea0c..50d2306d48f72 100644 --- a/perception/euclidean_cluster/include/euclidean_cluster/utils.hpp +++ b/perception/euclidean_cluster/include/euclidean_cluster/utils.hpp @@ -31,6 +31,9 @@ void convertPointCloudClusters2Msg( const std_msgs::msg::Header & header, const std::vector> & clusters, tier4_perception_msgs::msg::DetectedObjectsWithFeature & msg); +void convertPointCloudClusters2Msg( + const std_msgs::msg::Header & header, const std::vector & clusters, + tier4_perception_msgs::msg::DetectedObjectsWithFeature & msg); void convertObjectMsg2SensorMsg( const tier4_perception_msgs::msg::DetectedObjectsWithFeature & input, sensor_msgs::msg::PointCloud2 & output); diff --git a/perception/euclidean_cluster/include/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp b/perception/euclidean_cluster/include/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp index d6a07503c5ca5..375cc2d19a01f 100644 --- a/perception/euclidean_cluster/include/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp +++ b/perception/euclidean_cluster/include/euclidean_cluster/voxel_grid_based_euclidean_cluster.hpp @@ -35,6 +35,9 @@ class VoxelGridBasedEuclideanCluster : public EuclideanClusterInterface bool cluster( const pcl::PointCloud::ConstPtr & pointcloud, std::vector> & clusters) override; + bool cluster( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & pointcloud, + tier4_perception_msgs::msg::DetectedObjectsWithFeature & clusters) override; void setVoxelLeafSize(float voxel_leaf_size) { voxel_leaf_size_ = voxel_leaf_size; } void setTolerance(float tolerance) { tolerance_ = tolerance; } void setMinPointsNumberPerVoxel(int min_points_number_per_voxel) diff --git a/perception/euclidean_cluster/lib/euclidean_cluster.cpp b/perception/euclidean_cluster/lib/euclidean_cluster.cpp index bd8270bd6b881..5ba1b99403280 100644 --- a/perception/euclidean_cluster/lib/euclidean_cluster.cpp +++ b/perception/euclidean_cluster/lib/euclidean_cluster.cpp @@ -33,6 +33,15 @@ EuclideanCluster::EuclideanCluster( : EuclideanClusterInterface(use_height, min_cluster_size, max_cluster_size), tolerance_(tolerance) { } +// TODO(badai-nguyen): implement input field copy for euclidean_cluster.cpp +bool EuclideanCluster::cluster( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & pointcloud_msg, + tier4_perception_msgs::msg::DetectedObjectsWithFeature & clusters) +{ + (void)pointcloud_msg; + (void)clusters; + return false; +} bool EuclideanCluster::cluster( const pcl::PointCloud::ConstPtr & pointcloud, diff --git a/perception/euclidean_cluster/lib/utils.cpp b/perception/euclidean_cluster/lib/utils.cpp index 6ff563c157d00..3a906575e2c1e 100644 --- a/perception/euclidean_cluster/lib/utils.cpp +++ b/perception/euclidean_cluster/lib/utils.cpp @@ -62,6 +62,25 @@ void convertPointCloudClusters2Msg( msg.feature_objects.push_back(feature_object); } } + +void convertPointCloudClusters2Msg( + const std_msgs::msg::Header & header, const std::vector & clusters, + tier4_perception_msgs::msg::DetectedObjectsWithFeature & msg) +{ + msg.header = header; + for (const auto & ros_pointcloud : clusters) { + tier4_perception_msgs::msg::DetectedObjectWithFeature feature_object; + feature_object.feature.cluster = ros_pointcloud; + feature_object.object.kinematics.pose_with_covariance.pose.position = + getCentroid(ros_pointcloud); + autoware_auto_perception_msgs::msg::ObjectClassification classification; + classification.label = autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; + classification.probability = 1.0f; + feature_object.object.classification.emplace_back(classification); + msg.feature_objects.push_back(feature_object); + } +} + void convertObjectMsg2SensorMsg( const tier4_perception_msgs::msg::DetectedObjectsWithFeature & input, sensor_msgs::msg::PointCloud2 & output) diff --git a/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp b/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp index b8276296dde5f..d82ec021a5396 100644 --- a/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp +++ b/perception/euclidean_cluster/lib/voxel_grid_based_euclidean_cluster.cpp @@ -40,14 +40,27 @@ VoxelGridBasedEuclideanCluster::VoxelGridBasedEuclideanCluster( min_points_number_per_voxel_(min_points_number_per_voxel) { } - +// TODO(badai-nguyen): remove this function when field copying also implemented for +// euclidean_cluster.cpp bool VoxelGridBasedEuclideanCluster::cluster( const pcl::PointCloud::ConstPtr & pointcloud, std::vector> & clusters) +{ + (void)pointcloud; + (void)clusters; + return false; +} + +bool VoxelGridBasedEuclideanCluster::cluster( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr & pointcloud_msg, + tier4_perception_msgs::msg::DetectedObjectsWithFeature & objects) { // TODO(Saito) implement use_height is false version // create voxel + pcl::PointCloud::Ptr pointcloud(new pcl::PointCloud); + int point_step = pointcloud_msg->point_step; + pcl::fromROSMsg(*pointcloud_msg, *pointcloud); pcl::PointCloud::Ptr voxel_map_ptr(new pcl::PointCloud); voxel_grid_.setLeafSize(voxel_leaf_size_, voxel_leaf_size_, 100000.0); voxel_grid_.setMinimumPointsNumberPerVoxel(min_points_number_per_voxel_); @@ -81,36 +94,69 @@ bool VoxelGridBasedEuclideanCluster::cluster( // create map to search cluster index from voxel grid index std::unordered_map map; + std::vector temporary_clusters; // no check about cluster size + std::vector clusters_data_size; + temporary_clusters.resize(cluster_indices.size()); for (size_t cluster_idx = 0; cluster_idx < cluster_indices.size(); ++cluster_idx) { const auto & cluster = cluster_indices.at(cluster_idx); + auto & temporary_cluster = temporary_clusters.at(cluster_idx); for (const auto & point_idx : cluster.indices) { map[point_idx] = cluster_idx; } + temporary_cluster.height = pointcloud_msg->height; + temporary_cluster.fields = pointcloud_msg->fields; + temporary_cluster.point_step = point_step; + temporary_cluster.data.resize(max_cluster_size_ * point_step); + clusters_data_size.push_back(0); } // create vector of point cloud cluster. vector index is voxel grid index. - std::vector> temporary_clusters; // no check about cluster size - temporary_clusters.resize(cluster_indices.size()); - for (const auto & point : pointcloud->points) { + for (size_t i = 0; i < pointcloud->points.size(); ++i) { + const auto & point = pointcloud->points.at(i); const int index = voxel_grid_.getCentroidIndexAt(voxel_grid_.getGridCoordinates(point.x, point.y, point.z)); if (map.find(index) != map.end()) { - temporary_clusters.at(map[index]).points.push_back(point); + auto & cluster_data_size = clusters_data_size.at(map[index]); + if (cluster_data_size + point_step > std::size_t(max_cluster_size_ * point_step)) { + continue; + } + std::memcpy( + &temporary_clusters.at(map[index]).data[cluster_data_size], + &pointcloud_msg->data[i * point_step], point_step); + cluster_data_size += point_step; } } // build output and check cluster size { - for (const auto & cluster : temporary_clusters) { - if (!(min_cluster_size_ <= static_cast(cluster.points.size()) && - static_cast(cluster.points.size()) <= max_cluster_size_)) { + for (size_t i = 0; i < temporary_clusters.size(); ++i) { + auto & i_cluster_data_size = clusters_data_size.at(i); + if (!(min_cluster_size_ <= static_cast(i_cluster_data_size / point_step) && + static_cast(i_cluster_data_size / point_step) <= max_cluster_size_)) { continue; } - clusters.push_back(cluster); - clusters.back().width = cluster.points.size(); - clusters.back().height = 1; - clusters.back().is_dense = false; + const auto & cluster = temporary_clusters.at(i); + tier4_perception_msgs::msg::DetectedObjectWithFeature feature_object; + feature_object.feature.cluster = cluster; + feature_object.feature.cluster.data.resize(i_cluster_data_size); + feature_object.feature.cluster.header = pointcloud_msg->header; + feature_object.feature.cluster.is_bigendian = pointcloud_msg->is_bigendian; + feature_object.feature.cluster.is_dense = pointcloud_msg->is_dense; + feature_object.feature.cluster.point_step = point_step; + feature_object.feature.cluster.row_step = i_cluster_data_size / pointcloud_msg->height; + feature_object.feature.cluster.width = + i_cluster_data_size / point_step / pointcloud_msg->height; + + feature_object.object.kinematics.pose_with_covariance.pose.position = + getCentroid(feature_object.feature.cluster); + autoware_auto_perception_msgs::msg::ObjectClassification classification; + classification.label = autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; + classification.probability = 1.0f; + feature_object.object.classification.emplace_back(classification); + + objects.feature_objects.push_back(feature_object); } + objects.header = pointcloud_msg->header; } return true; diff --git a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp index f58d9ac6dcc48..7e6a456561900 100644 --- a/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp +++ b/perception/euclidean_cluster/src/voxel_grid_based_euclidean_cluster_node.cpp @@ -55,24 +55,15 @@ void VoxelGridBasedEuclideanClusterNode::onPointCloud( stop_watch_ptr_->toc("processing_time", true); // convert ros to pcl - pcl::PointCloud::Ptr raw_pointcloud_ptr(new pcl::PointCloud); if (input_msg->data.empty()) { // NOTE: prevent pcl log spam RCLCPP_WARN_STREAM_THROTTLE( this->get_logger(), *this->get_clock(), 1000, "Empty sensor points!"); - } else { - pcl::fromROSMsg(*input_msg, *raw_pointcloud_ptr); } - - // clustering - std::vector> clusters; - if (!raw_pointcloud_ptr->empty()) { - cluster_->cluster(raw_pointcloud_ptr, clusters); - } - - // build output msg + // cluster and build output msg tier4_perception_msgs::msg::DetectedObjectsWithFeature output; - convertPointCloudClusters2Msg(input_msg->header, clusters, output); + + cluster_->cluster(input_msg, output); cluster_pub_->publish(output); // build debug msg