diff --git a/perception/lidar_apollo_instance_segmentation/src/detector.cpp b/perception/lidar_apollo_instance_segmentation/src/detector.cpp index 236f0998ab21f..d0254c651eece 100644 --- a/perception/lidar_apollo_instance_segmentation/src/detector.cpp +++ b/perception/lidar_apollo_instance_segmentation/src/detector.cpp @@ -16,6 +16,8 @@ #include "lidar_apollo_instance_segmentation/feature_map.hpp" +#include + #include #include #include @@ -125,7 +127,27 @@ bool LidarApolloInstanceSegmentation::detectDynamicObjects( // convert from ros to pcl pcl::PointCloud::Ptr pcl_pointcloud_raw_ptr(new pcl::PointCloud); - pcl::fromROSMsg(transformed_cloud, *pcl_pointcloud_raw_ptr); + // pcl::fromROSMsg(transformed_cloud, *pcl_pointcloud_raw_ptr); + + auto pcl_pointcloud_raw = *pcl_pointcloud_raw_ptr; + pcl_pointcloud_raw.width = transformed_cloud.width; + pcl_pointcloud_raw.height = transformed_cloud.height; + pcl_pointcloud_raw.is_dense = transformed_cloud.is_dense == 1; + pcl_pointcloud_raw.resize(transformed_cloud.width * transformed_cloud.height); + + sensor_msgs::PointCloud2ConstIterator it_x(transformed_cloud, "x"); + sensor_msgs::PointCloud2ConstIterator it_y(transformed_cloud, "y"); + sensor_msgs::PointCloud2ConstIterator it_z(transformed_cloud, "z"); + sensor_msgs::PointCloud2ConstIterator it_intensity(transformed_cloud, "intensity"); + + for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_intensity) { + pcl::PointXYZI point; + point.x = *it_x; + point.y = *it_y; + point.z = *it_z; + point.intensity = static_cast(*it_intensity); + pcl_pointcloud_raw.emplace_back(std::move(point)); + } // generate feature map std::shared_ptr feature_map_ptr = diff --git a/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter.cpp b/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter.cpp index 5c47b61eaa0a3..96726b0087173 100644 --- a/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter.cpp +++ b/perception/raindrop_cluster_filter/src/low_intensity_cluster_filter.cpp @@ -123,9 +123,9 @@ bool LowIntensityClusterFilter::isValidatedCluster(const sensor_msgs::msg::Point RCLCPP_WARN(get_logger(), "Invalid point cloud data. point_step is less than 16."); return true; } - for (sensor_msgs::PointCloud2ConstIterator iter(cluster, "intensity"); iter != iter.end(); - ++iter) { - mean_intensity += *iter; + for (sensor_msgs::PointCloud2ConstIterator iter(cluster, "intensity"); + iter != iter.end(); ++iter) { + mean_intensity += static_cast(*iter); } const size_t num_points = cluster.width * cluster.height; mean_intensity /= static_cast(num_points); diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp index c871f1acbe5b9..95fb1f5cf3c8c 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/outlier_filter/ring_outlier_filter_nodelet.hpp @@ -33,6 +33,10 @@ using point_cloud_msg_wrapper::PointCloud2Modifier; class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter { protected: + using InputPointIndex = autoware_point_types::PointXYZIRCAEDTIndex; + using InputPointType = autoware_point_types::PointXYZIRCAEDT; + using OutputPointType = autoware_point_types::PointXYZIRC; + virtual void filter( const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output); @@ -75,8 +79,7 @@ class RingOutlierFilterComponent : public pointcloud_preprocessor::Filter } void setUpPointCloudFormat( - const PointCloud2ConstPtr & input, PointCloud2 & formatted_points, size_t points_size, - size_t num_fields); + const PointCloud2ConstPtr & input, PointCloud2 & formatted_points, size_t points_size); public: PCL_MAKE_ALIGNED_OPERATOR_NEW diff --git a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp index b90a64dc47c91..9c3eee9db677c 100644 --- a/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp +++ b/sensing/pointcloud_preprocessor/src/distortion_corrector/distortion_corrector.cpp @@ -251,9 +251,12 @@ bool DistortionCorrectorComponent::undistortPointCloud( // If there is a point that cannot be associated, record it to issue a warning bool twist_time_stamp_is_too_late = false; bool imu_time_stamp_is_too_late = false; + double global_point_stamp; for (; it_x != it_x.end(); ++it_x, ++it_y, ++it_z, ++it_time_stamp) { - while (twist_it != std::end(twist_queue_) - 1 && *it_time_stamp > twist_stamp) { + global_point_stamp = + points.header.stamp.sec + 1e-9 * (points.header.stamp.nanosec + *it_time_stamp); + while (twist_it != std::end(twist_queue_) - 1 && global_point_stamp > twist_stamp) { ++twist_it; twist_stamp = rclcpp::Time(twist_it->header.stamp).seconds(); } @@ -261,19 +264,19 @@ bool DistortionCorrectorComponent::undistortPointCloud( float v{static_cast(twist_it->twist.linear.x)}; float w{static_cast(twist_it->twist.angular.z)}; - if (std::abs(*it_time_stamp - twist_stamp) > 0.1) { + if (std::abs(global_point_stamp - twist_stamp) > 0.1) { twist_time_stamp_is_too_late = true; v = 0.0f; w = 0.0f; } if (use_imu_ && !angular_velocity_queue_.empty()) { - while (imu_it != std::end(angular_velocity_queue_) - 1 && *it_time_stamp > imu_stamp) { + while (imu_it != std::end(angular_velocity_queue_) - 1 && global_point_stamp > imu_stamp) { ++imu_it; imu_stamp = rclcpp::Time(imu_it->header.stamp).seconds(); } - if (std::abs(*it_time_stamp - imu_stamp) > 0.1) { + if (std::abs(global_point_stamp - imu_stamp) > 0.1) { imu_time_stamp_is_too_late = true; } else { w = static_cast(imu_it->vector.z); @@ -309,7 +312,7 @@ bool DistortionCorrectorComponent::undistortPointCloud( *it_y = static_cast(undistorted_point.getY()); *it_z = static_cast(undistorted_point.getZ()); - prev_time_stamp_sec = *it_time_stamp; + prev_time_stamp_sec = global_point_stamp; } if (twist_time_stamp_is_too_late) { diff --git a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp index 762c62dc03b39..2142b39c0e9f7 100644 --- a/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp +++ b/sensing/pointcloud_preprocessor/src/outlier_filter/ring_outlier_filter_nodelet.cpp @@ -18,6 +18,7 @@ #include +#include #include #include @@ -232,10 +233,10 @@ void RingOutlierFilterComponent::faster_filter( } } - setUpPointCloudFormat(input, output, output_size, /*num_fields=*/4); + setUpPointCloudFormat(input, output, output_size); if (publish_outlier_pointcloud_) { - setUpPointCloudFormat(input, outlier_points, outlier_points_size, /*num_fields=*/4); + setUpPointCloudFormat(input, outlier_points, outlier_points_size); outlier_pointcloud_publisher_->publish(outlier_points); } @@ -297,8 +298,7 @@ rcl_interfaces::msg::SetParametersResult RingOutlierFilterComponent::paramCallba } void RingOutlierFilterComponent::setUpPointCloudFormat( - const PointCloud2ConstPtr & input, PointCloud2 & formatted_points, size_t points_size, - size_t num_fields) + const PointCloud2ConstPtr & input, PointCloud2 & formatted_points, size_t points_size) { formatted_points.data.resize(points_size); // Note that `input->header.frame_id` is data before converted when `transform_info.need_transform @@ -312,11 +312,15 @@ void RingOutlierFilterComponent::setUpPointCloudFormat( formatted_points.is_bigendian = input->is_bigendian; formatted_points.is_dense = input->is_dense; - sensor_msgs::PointCloud2Modifier pcd_modifier(formatted_points); - pcd_modifier.setPointCloud2Fields( - num_fields, "x", 1, sensor_msgs::msg::PointField::FLOAT32, "y", 1, - sensor_msgs::msg::PointField::FLOAT32, "z", 1, sensor_msgs::msg::PointField::FLOAT32, - "intensity", 1, sensor_msgs::msg::PointField::FLOAT32); + // pcl::PCLPointCloud2 pcl_aux; + sensor_msgs::msg::PointCloud2 msg_aux; + // pcl_conversions::moveFromPCL(pcl_pc2, cloud); + pcl::toROSMsg(pcl::PointCloud(), msg_aux); + formatted_points.fields = msg_aux.fields; + // pcl::for_each_type::type> + // pcl::detail::FieldAdder(formatted_points.fields); pcl::for_each_type::type> + // (pcl::detail::FieldAdder(formatted_points.fields)); } } // namespace pointcloud_preprocessor