Ground mesh

include/lidar_processor/lidar_processor.hpp

@@ -30,9 +30,9 @@
 #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 <pcl_conversions/pcl_conversions.h>
-#include <pcl/point_types.h>
 #include <pcl/conversions.h>
 
 #include <tf2/exceptions.h>
@@ -53,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<pcl::PointXYZI>::Ptr cloud);
-  void ground_segmentation(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud, pcl::PointCloud<pcl::PointXYZI>::Ptr ground);
-  void project_to_laserscan(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud);
+  void passthrough_stage(pcl::PointCloud<pcl::PointXYZIR>::Ptr cloud);
+  void ground_segmentation(pcl::PointCloud<pcl::PointXYZIR>::Ptr cloud, pcl::PointCloud<pcl::PointXYZIR>::Ptr ground);
+  void project_to_laserscan(pcl::PointCloud<pcl::PointXYZIR>::Ptr cloud);
 
   void update_params();
   void raw_pc_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg);

include/lidar_processor/point_type.hpp

@@ -0,0 +1,28 @@
+#define PCL_NO_PRECOMPILE
+#include <pcl/pcl_macros.h>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+
+#ifndef LIDAR_PROCESSOR_PCL_POINT_TYPES_H
+#define LIDAR_PROCESSOR_PCL_POINT_TYPES_H
+
+namespace pcl 
+{
+    struct EIGEN_ALIGN16 PointXYZIR
+    {
+        PCL_ADD_POINT4D;
+        float intensity;
+        std::uint16_t ring;
+        PCL_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

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<Model::Plane>(&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<float>().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<pcl::PointXYZI>::Ptr in_pcl, pcl::PointCloud<pcl::PointXYZI>::Ptr inliers, const float& threshold)
+inline void naive_fit(const RModel& model, pcl::PointCloud<pcl::PointXYZIR>::Ptr in_pcl, pcl::PointCloud<pcl::PointXYZIR>::Ptr inliers, const float& threshold)
 {   
     // Iterate over PointCloud with it[0] = x, it[1] = y, it[2] = z.
-    pcl::PointCloud<pcl::PointXYZI>::iterator it = in_pcl->begin();
+    pcl::PointCloud<pcl::PointXYZIR>::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

src/lidar_processor.cpp

@@ -22,12 +22,14 @@
 
 #include "lidar_processor/lidar_processor.hpp"
 #include <lidar_processor/utils.hpp>
-
-#include <pcl/filters/crop_box.h>
 #include <pcl/filters/passthrough.h>
+#include <pcl/filters/impl/passthrough.hpp>
+#include <pcl/filters/crop_box.h>
+#include <pcl/filters/impl/crop_box.hpp>
 #include <pcl/filters/radius_outlier_removal.h>
+#include <pcl/filters/impl/radius_outlier_removal.hpp>
 #include <pcl/filters/voxel_grid.h>
-
+#include <pcl/filters/impl/voxel_grid.hpp>
 #include <memory>
 #include <functional>
 
@@ -79,17 +81,16 @@ void LidarProcessor::imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg)
   last_imu_ = msg;
 }
 
-void LidarProcessor::passthrough_stage(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud)
+void LidarProcessor::passthrough_stage(pcl::PointCloud<pcl::PointXYZIR>::Ptr cloud)
 {
   // Crop out robot body
-  pcl::CropBox<pcl::PointXYZI> crop_box;
+  pcl::CropBox<pcl::PointXYZIR> 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<float>().max();
   float max_intensity = 0.0f;
@@ -117,7 +118,7 @@ void LidarProcessor::passthrough_stage(pcl::PointCloud<pcl::PointXYZI>::Ptr clou
   double threshold = mean_intensity - (2 * std_dev_intensity);
 
   // Filter out points
-  pcl::PointCloud<pcl::PointXYZI>::iterator it = cloud->begin();
+  pcl::PointCloud<pcl::PointXYZIR>::iterator it = cloud->begin();
   while (it != cloud->end())
   {
     if (it->intensity < threshold)
@@ -131,7 +132,7 @@ void LidarProcessor::passthrough_stage(pcl::PointCloud<pcl::PointXYZI>::Ptr clou
   }
 }
 
-void LidarProcessor::ground_segmentation(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud, pcl::PointCloud<pcl::PointXYZI>::Ptr ground)
+void LidarProcessor::ground_segmentation(pcl::PointCloud<pcl::PointXYZIR>::Ptr cloud, pcl::PointCloud<pcl::PointXYZIR>::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<pcl::PointXYZI>::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<pcl::PointXYZI>::Ptr cl
   naive_fit(fit_model, cloud, ground, ground_point_model_threshold_);
 }
 
-void LidarProcessor::project_to_laserscan(pcl::PointCloud<pcl::PointXYZI>::Ptr cloud)
+void LidarProcessor::project_to_laserscan(pcl::PointCloud<pcl::PointXYZIR>::Ptr cloud)
 {
   // fill out configuration information
   // TODO: PARAMETERIZE
@@ -180,7 +181,7 @@ void LidarProcessor::project_to_laserscan(pcl::PointCloud<pcl::PointXYZI>::Ptr c
   // Note:  A simple method is just converting cartesian to polar coordinates.
   // x = rcos(theta)
   // y = rsin(theta)
-  for (pcl::PointCloud<pcl::PointXYZI>::iterator it = cloud->begin(); it != cloud->end(); it++)
+  for (pcl::PointCloud<pcl::PointXYZIR>::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
@@ -218,8 +219,8 @@ void LidarProcessor::raw_pc_callback(const sensor_msgs::msg::PointCloud2::Shared
   }
 
   // Container for original & filtered data
-  pcl::PointCloud<pcl::PointXYZI>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZI>);
-  pcl::PointCloud<pcl::PointXYZI>::Ptr ground_points(new pcl::PointCloud<pcl::PointXYZI>);
+  pcl::PointCloud<pcl::PointXYZIR>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZIR>);
+  pcl::PointCloud<pcl::PointXYZIR>::Ptr ground_points(new pcl::PointCloud<pcl::PointXYZIR>);
 
   // Convert to PCL data type
   pcl::fromROSMsg(*msg, *cloud);
@@ -229,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;