From a161d3dfd15eb07f69ab71672c2d15ff43f1450d Mon Sep 17 00:00:00 2001 From: Shinnosuke Hirakawa <8327162+0x126@users.noreply.github.com> Date: Thu, 3 Feb 2022 15:16:19 +0900 Subject: [PATCH] fix: changed pointcloud conversion method (#336) Signed-off-by: Shinnosuke Hirakawa --- .../concatenate_data_nodelet.hpp | 25 ++++----- .../concatenate_data_nodelet.cpp | 52 ++++++++++++++----- 2 files changed, 48 insertions(+), 29 deletions(-) diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp index e9e22bf9c24d9..7083ab14e99d4 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/concatenate_data/concatenate_data_nodelet.hpp @@ -61,12 +61,16 @@ #include // ROS includes +#include "autoware_point_types/types.hpp" + #include +#include #include #include #include #include +#include #include #include @@ -75,12 +79,13 @@ #include #include #include -#include #include #include namespace pointcloud_preprocessor { +using autoware_point_types::PointXYZI; +using point_cloud_msg_wrapper::PointCloud2Modifier; /** \brief @b PointCloudConcatenateDataSynchronizerComponent is a special form of data * synchronizer: it listens for a set of input PointCloud messages on the same topic, * checks their timestamps, and concatenates their fields together into a single @@ -150,29 +155,17 @@ class PointCloudConcatenateDataSynchronizerComponent : public rclcpp::Node void publish(); void convertToXYZICloud( - const sensor_msgs::msg::PointCloud2 & input_cloud, - sensor_msgs::msg::PointCloud2 & output_cloud); + const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, + sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr); void setPeriod(const int64_t new_period); void cloud_callback( - const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, - const std::string & topic_name); + const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, const std::string & topic_name); void twist_callback(const autoware_auto_vehicle_msgs::msg::VelocityReport::ConstSharedPtr input); void timer_callback(); void checkConcatStatus(diagnostic_updater::DiagnosticStatusWrapper & stat); }; -struct PointXYZI -{ - PCL_ADD_POINT4D; - float intensity; - EIGEN_MAKE_ALIGNED_OPERATOR_NEW -} EIGEN_ALIGN16; - } // namespace pointcloud_preprocessor -POINT_CLOUD_REGISTER_POINT_STRUCT( - pointcloud_preprocessor::PointXYZI, - (float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)) - #endif // POINTCLOUD_PREPROCESSOR__CONCATENATE_DATA__CONCATENATE_DATA_NODELET_HPP_ diff --git a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp index 89707f4754b55..7ecd614daf1ff 100644 --- a/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/concatenate_data/concatenate_data_nodelet.cpp @@ -297,14 +297,42 @@ void PointCloudConcatenateDataSynchronizerComponent::publish() } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// - void PointCloudConcatenateDataSynchronizerComponent::convertToXYZICloud( - const sensor_msgs::msg::PointCloud2 & input_cloud, sensor_msgs::msg::PointCloud2 & output_cloud) + const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, + sensor_msgs::msg::PointCloud2::SharedPtr & output_ptr) { - pcl::PointCloud tmp_cloud; - pcl::fromROSMsg(input_cloud, tmp_cloud); - pcl::toROSMsg(tmp_cloud, output_cloud); - output_cloud.header = input_cloud.header; + output_ptr->header = input_ptr->header; + PointCloud2Modifier output_modifier{*output_ptr, input_ptr->header.frame_id}; + output_modifier.reserve(input_ptr->width); + + bool has_intensity = std::any_of( + input_ptr->fields.begin(), input_ptr->fields.end(), + [](auto & field) { return field.name == "intensity"; }); + + sensor_msgs::PointCloud2Iterator it_x(*input_ptr, "x"); + sensor_msgs::PointCloud2Iterator it_y(*input_ptr, "y"); + sensor_msgs::PointCloud2Iterator it_z(*input_ptr, "z"); + + if (has_intensity) { + sensor_msgs::PointCloud2Iterator it_i(*input_ptr, "intensity"); + for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_i) { + PointXYZI point; + point.x = *it_x; + point.y = *it_y; + point.z = *it_z; + point.intensity = *it_i; + output_modifier.push_back(std::move(point)); + } + } else { + for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z) { + PointXYZI point; + point.x = *it_x; + point.y = *it_y; + point.z = *it_z; + point.intensity = 0.0f; + output_modifier.push_back(std::move(point)); + } + } } void PointCloudConcatenateDataSynchronizerComponent::setPeriod(const int64_t new_period) @@ -324,14 +352,12 @@ void PointCloudConcatenateDataSynchronizerComponent::setPeriod(const int64_t new } void PointCloudConcatenateDataSynchronizerComponent::cloud_callback( - const sensor_msgs::msg::PointCloud2::ConstSharedPtr & input_ptr, const std::string & topic_name) + const sensor_msgs::msg::PointCloud2::SharedPtr & input_ptr, const std::string & topic_name) { std::lock_guard lock(mutex_); - sensor_msgs::msg::PointCloud2 xyz_cloud; - convertToXYZICloud(*input_ptr, xyz_cloud); - sensor_msgs::msg::PointCloud2::ConstSharedPtr xyz_input_ptr( - new sensor_msgs::msg::PointCloud2(xyz_cloud)); + sensor_msgs::msg::PointCloud2::SharedPtr xyzi_input_ptr(new sensor_msgs::msg::PointCloud2()); + convertToXYZICloud(input_ptr, xyzi_input_ptr); const bool is_already_subscribed_this = (cloud_stdmap_[topic_name] != nullptr); const bool is_already_subscribed_tmp = std::any_of( @@ -339,7 +365,7 @@ void PointCloudConcatenateDataSynchronizerComponent::cloud_callback( [](const auto & e) { return e.second != nullptr; }); if (is_already_subscribed_this) { - cloud_stdmap_tmp_[topic_name] = xyz_input_ptr; + cloud_stdmap_tmp_[topic_name] = xyzi_input_ptr; if (!is_already_subscribed_tmp) { auto period = std::chrono::duration_cast( @@ -352,7 +378,7 @@ void PointCloudConcatenateDataSynchronizerComponent::cloud_callback( timer_->reset(); } } else { - cloud_stdmap_[topic_name] = xyz_input_ptr; + cloud_stdmap_[topic_name] = xyzi_input_ptr; const bool is_subscribed_all = std::all_of( std::begin(cloud_stdmap_), std::end(cloud_stdmap_),