From e13d29de9e6a044002d1f8c810762eab9263f46a Mon Sep 17 00:00:00 2001 From: girvenavery2022 Date: Wed, 9 Mar 2022 21:40:59 -0500 Subject: [PATCH 1/4] added PointXYZIR --- include/lidar_processor/point_type.hpp | 25 +++++++++++++++++++++++++ src/lidar_processor.cpp | 11 +++++++---- 2 files changed, 32 insertions(+), 4 deletions(-) create mode 100644 include/lidar_processor/point_type.hpp diff --git a/include/lidar_processor/point_type.hpp b/include/lidar_processor/point_type.hpp new file mode 100644 index 0000000..f1af2a5 --- /dev/null +++ b/include/lidar_processor/point_type.hpp @@ -0,0 +1,25 @@ +#ifndef LIDAR_PROCESSOR_PCL_POINT_TYPES_H +#define LIDAR_PROCESSOR_PCL_POINT_TYPES_H + +#include + +namespace pcl +{ + struct PointXYZIR + { + PCL_ADD_POINT4D; + float intensity; + std::uint16_t ring; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + }; +} // namespace pcl + +POINT_CLOUD_REGISTER_POINT_STRUCT( + pcl::PointXYZIR, + (float, x, x) + (float, y, y) + (float, z, z) + (float, intensity, intensity) + (std::uint16_t, ring, ring) +) +#endif // LIDAR_PROCESSOR_PCL_POINT_TYPES_H \ No newline at end of file diff --git a/src/lidar_processor.cpp b/src/lidar_processor.cpp index 0a3ba8f..bfaf82e 100644 --- a/src/lidar_processor.cpp +++ b/src/lidar_processor.cpp @@ -212,10 +212,13 @@ void LidarProcessor::project_to_laserscan(pcl::PointCloud::Ptr c void LidarProcessor::raw_pc_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) { msg->header.frame_id = "laser_link"; // fix weird pointcloud frame? - if (debug_cloud_) - { - unfiltered_pc_publisher_->publish(*msg); - } + pcl::PointCloud::Ptr cloud1(new pcl::PointCloud); + sensor_msgs::msg::PointCloud2 out; + pcl::fromROSMsg(*msg, *cloud1); + pcl::toROSMsg(*cloud1, out); + + unfiltered_pc_publisher_->publish(out); + // Container for original & filtered data pcl::PointCloud::Ptr cloud(new pcl::PointCloud); From 8cd5431cab589f3d760161ab5d0a0b461323cf06 Mon Sep 17 00:00:00 2001 From: girvenavery2022 Date: Wed, 9 Mar 2022 21:42:13 -0500 Subject: [PATCH 2/4] forgot to add header --- include/lidar_processor/lidar_processor.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/include/lidar_processor/lidar_processor.hpp b/include/lidar_processor/lidar_processor.hpp index a646166..6874a6f 100644 --- a/include/lidar_processor/lidar_processor.hpp +++ b/include/lidar_processor/lidar_processor.hpp @@ -38,6 +38,7 @@ #include #include #include +#include "lidar_processor/point_type.hpp" namespace LidarProcessor { From b4836d4fcbc1b73d8d4295b3af74bbd0fae64d2c Mon Sep 17 00:00:00 2001 From: girvenavery2022 Date: Thu, 10 Mar 2022 11:32:25 -0500 Subject: [PATCH 3/4] migrated to PointXYZIR --- include/lidar_processor/lidar_processor.hpp | 9 +++-- include/lidar_processor/point_type.hpp | 11 +++--- src/lidar_processor.cpp | 40 ++++++++++----------- 3 files changed, 30 insertions(+), 30 deletions(-) diff --git a/include/lidar_processor/lidar_processor.hpp b/include/lidar_processor/lidar_processor.hpp index 6874a6f..d32c7fe 100644 --- a/include/lidar_processor/lidar_processor.hpp +++ b/include/lidar_processor/lidar_processor.hpp @@ -30,15 +30,14 @@ #include "sensor_msgs/msg/point_cloud2.hpp" #include "sensor_msgs/msg/laser_scan.hpp" #include "sensor_msgs/msg/imu.hpp" +#include "lidar_processor/point_type.hpp" #include -#include #include #include #include #include -#include "lidar_processor/point_type.hpp" namespace LidarProcessor { @@ -54,9 +53,9 @@ class LidarProcessor : public rclcpp::Node sensor_msgs::msg::PointCloud2::SharedPtr last_pcl_{}; rclcpp::TimerBase::SharedPtr param_update_timer_; - void passthrough_stage(pcl::PointCloud::Ptr cloud); - void ground_segmentation(pcl::PointCloud::Ptr cloud, pcl::PointCloud::Ptr ground); - void project_to_laserscan(pcl::PointCloud::Ptr cloud); + void passthrough_stage(pcl::PointCloud::Ptr cloud); + void ground_segmentation(pcl::PointCloud::Ptr cloud, pcl::PointCloud::Ptr ground); + void project_to_laserscan(pcl::PointCloud::Ptr cloud); void update_params(); void raw_pc_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg); diff --git a/include/lidar_processor/point_type.hpp b/include/lidar_processor/point_type.hpp index f1af2a5..182d4e8 100644 --- a/include/lidar_processor/point_type.hpp +++ b/include/lidar_processor/point_type.hpp @@ -1,16 +1,19 @@ +#define PCL_NO_PRECOMPILE +#include +#include +#include + #ifndef LIDAR_PROCESSOR_PCL_POINT_TYPES_H #define LIDAR_PROCESSOR_PCL_POINT_TYPES_H -#include - namespace pcl { - struct PointXYZIR + struct EIGEN_ALIGN16 PointXYZIR { PCL_ADD_POINT4D; float intensity; std::uint16_t ring; - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + PCL_MAKE_ALIGNED_OPERATOR_NEW }; } // namespace pcl diff --git a/src/lidar_processor.cpp b/src/lidar_processor.cpp index bfaf82e..b5f9715 100644 --- a/src/lidar_processor.cpp +++ b/src/lidar_processor.cpp @@ -22,12 +22,14 @@ #include "lidar_processor/lidar_processor.hpp" #include - -#include #include +#include +#include +#include #include +#include #include - +#include #include #include @@ -79,17 +81,16 @@ void LidarProcessor::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg) last_imu_ = msg; } -void LidarProcessor::passthrough_stage(pcl::PointCloud::Ptr cloud) +void LidarProcessor::passthrough_stage(pcl::PointCloud::Ptr cloud) { // Crop out robot body - pcl::CropBox crop_box; + pcl::CropBox crop_box; crop_box.setInputCloud(cloud); crop_box.setMin(Eigen::Vector4f(-0.2f, -0.2f, -0.2f, 1)); crop_box.setMax(Eigen::Vector4f(0.2f, 0.2f, 0.2f, 1)); crop_box.setNegative(true); // filter out points in box crop_box.filter(*cloud); - // Perform intensity filtering float min_intensity = std::numeric_limits().max(); float max_intensity = 0.0f; @@ -117,7 +118,7 @@ void LidarProcessor::passthrough_stage(pcl::PointCloud::Ptr clou double threshold = mean_intensity - (2 * std_dev_intensity); // Filter out points - pcl::PointCloud::iterator it = cloud->begin(); + pcl::PointCloud::iterator it = cloud->begin(); while (it != cloud->end()) { if (it->intensity < threshold) @@ -131,7 +132,7 @@ void LidarProcessor::passthrough_stage(pcl::PointCloud::Ptr clou } } -void LidarProcessor::ground_segmentation(pcl::PointCloud::Ptr cloud, pcl::PointCloud::Ptr ground) +void LidarProcessor::ground_segmentation(pcl::PointCloud::Ptr cloud, pcl::PointCloud::Ptr ground) { // Find inlier points to plane model: Segment ground plane RModel fit_model = Model::Plane{}; @@ -142,7 +143,7 @@ void LidarProcessor::ground_segmentation(pcl::PointCloud::Ptr cl // Get point in center of robot base footprint auto trans = tf_buffer_->lookupTransform("base_footprint", "laser_link", tf2::TimePointZero); - pcl::PointXYZI point; + pcl::PointXYZIR point; point.x = trans.transform.translation.x; point.y = trans.transform.translation.y; point.z = trans.transform.translation.z; @@ -152,7 +153,7 @@ void LidarProcessor::ground_segmentation(pcl::PointCloud::Ptr cl naive_fit(fit_model, cloud, ground, ground_point_model_threshold_); } -void LidarProcessor::project_to_laserscan(pcl::PointCloud::Ptr cloud) +void LidarProcessor::project_to_laserscan(pcl::PointCloud::Ptr cloud) { // fill out configuration information // TODO: PARAMETERIZE @@ -180,7 +181,7 @@ void LidarProcessor::project_to_laserscan(pcl::PointCloud::Ptr c // Note: A simple method is just converting cartesian to polar coordinates. // x = rcos(theta) // y = rsin(theta) - for (pcl::PointCloud::iterator it = cloud->begin(); it != cloud->end(); it++) + for (pcl::PointCloud::iterator it = cloud->begin(); it != cloud->end(); it++) { float range = std::hypot(it->x, it->y); // sqrt of x^2 + y^2 float angle = std::atan(it->y / it->x); // tan inverse of y/x @@ -212,17 +213,14 @@ void LidarProcessor::project_to_laserscan(pcl::PointCloud::Ptr c void LidarProcessor::raw_pc_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) { msg->header.frame_id = "laser_link"; // fix weird pointcloud frame? - pcl::PointCloud::Ptr cloud1(new pcl::PointCloud); - sensor_msgs::msg::PointCloud2 out; - pcl::fromROSMsg(*msg, *cloud1); - pcl::toROSMsg(*cloud1, out); - - unfiltered_pc_publisher_->publish(out); - + if (debug_cloud_) + { + unfiltered_pc_publisher_->publish(*msg); + } // Container for original & filtered data - pcl::PointCloud::Ptr cloud(new pcl::PointCloud); - pcl::PointCloud::Ptr ground_points(new pcl::PointCloud); + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + pcl::PointCloud::Ptr ground_points(new pcl::PointCloud); // Convert to PCL data type pcl::fromROSMsg(*msg, *cloud); @@ -232,7 +230,7 @@ void LidarProcessor::raw_pc_callback(const sensor_msgs::msg::PointCloud2::Shared ground_segmentation(cloud, ground_points); // this is outputting a scan that already has ground points filtered out from above - project_to_laserscan(cloud); + //project_to_laserscan(cloud); // Convert to ROS data type sensor_msgs::msg::PointCloud2 output; From aceacb07c0f94494b4f697930effb1b5d594b216 Mon Sep 17 00:00:00 2001 From: girvenavery2022 Date: Thu, 10 Mar 2022 11:37:54 -0500 Subject: [PATCH 4/4] forgot about utils lmao --- include/lidar_processor/utils.hpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/include/lidar_processor/utils.hpp b/include/lidar_processor/utils.hpp index 7f0cb77..d718aa8 100644 --- a/include/lidar_processor/utils.hpp +++ b/include/lidar_processor/utils.hpp @@ -59,7 +59,7 @@ struct NormalVector float z; }; -inline void find_plane_coefficients(RModel& plane_model, const NormalVector& norm, const pcl::PointXYZI& point) noexcept +inline void find_plane_coefficients(RModel& plane_model, const NormalVector& norm, const pcl::PointXYZIR& point) noexcept { // See if the passed in Model is truly a plane model if (const auto* plane = std::get_if(&plane_model); plane != nullptr) @@ -77,7 +77,7 @@ inline void find_plane_coefficients(RModel& plane_model, const NormalVector& nor } } -inline float distance_from_plane(const RModel& plane_model, const pcl::PointXYZI& point) noexcept +inline float distance_from_plane(const RModel& plane_model, const pcl::PointXYZIR& point) noexcept { // Initialize to infinity float distance = std::numeric_limits().infinity(); @@ -105,18 +105,19 @@ inline float distance_from_plane(const RModel& plane_model, const pcl::PointXYZI return distance; } -inline void naive_fit(const RModel& model, pcl::PointCloud::Ptr in_pcl, pcl::PointCloud::Ptr inliers, const float& threshold) +inline void naive_fit(const RModel& model, pcl::PointCloud::Ptr in_pcl, pcl::PointCloud::Ptr inliers, const float& threshold) { // Iterate over PointCloud with it[0] = x, it[1] = y, it[2] = z. - pcl::PointCloud::iterator it = in_pcl->begin(); + pcl::PointCloud::iterator it = in_pcl->begin(); while (it != in_pcl->end()) { // check if point fits model - pcl::PointXYZI point; + pcl::PointXYZIR point; point.x = it->x; point.y = it->y; point.z = it->z; point.intensity = it->intensity; + point.ring = it->ring; if(distance_from_plane(model, point) < threshold) { // NOTE: FILTER OUT POINT HERE