Feature/perception/fix ground filtering

src/perception/config/perception_config.yaml

@@ -18,7 +18,7 @@ perception:
     use_artificial_timestamp: true  # Use the node's current time as timestamp instead of the one from the LiDAR messages
 
     # Deskewing
-    deskewing: true               # Motion compensation within each LiDAR scan using point timestamps
+    deskewing: false               # Motion compensation within each LiDAR scan using point timestamps
     buffer_size: 100
 
     # Cropping parameters
@@ -29,7 +29,7 @@ perception:
     slow_straight_cropping: false    # Leave only a straight section in front of the car when moving slowly
 
     # Clipping parameters
-    clipping: true                 # Enable clipping of the point cloud
+    clipping: false                 # Enable clipping of the point cloud
     max_x_clip: 40.0
     max_y_clip: 5.0
     min_x_clip: -25.0

src/perception/include/perception/ground_filtering.h

@@ -141,12 +141,34 @@ namespace GroundFiltering
                             pcl::ModelCoefficients::Ptr& coefficients, double threshold, double Mx, double My, double min_z,
                             int number_sections, int max_iterations)
     {
-        // Initialize the variables
-        double x_step = (Mx - 0) / number_sections;
-        double y_step = (My - (-My)) / number_sections;
 
         if (cloud->empty()) return;
 
+        // We associate our limits with the actual limits of the point cloud
+        double min_x = std::numeric_limits<double>::max();
+        double max_x = std::numeric_limits<double>::lowest();
+        double min_y = std::numeric_limits<double>::max();
+        double max_y = std::numeric_limits<double>::lowest();
+
+        for (const auto& point : cloud->points) {
+            if (point.z < min_z) continue;
+            if (point.x < min_x) min_x = point.x;
+            if (point.x > max_x) max_x = point.x;
+            if (point.y < min_y) min_y = point.y;
+            if (point.y > max_y) max_y = point.y;
+        }
+
+        // Error prevention if no valid points remain
+        if (min_x > max_x) return;
+
+        // We expand the maximum by one millimeter so that the points
+        // that fall exactly on the edge do not give an out-of-range index
+        max_x += 0.001;
+        max_y += 0.001;
+
+        double x_step = (max_x - min_x) / number_sections;
+        double y_step = (max_y - min_y) / number_sections;
+
         int total_cells = number_sections * number_sections;
         std::vector<typename pcl::PointCloud<PointT>::Ptr> grid_cells(total_cells);
 
@@ -160,8 +182,8 @@ namespace GroundFiltering
         for (const auto& point : cloud->points) {
             if (point.z < min_z) continue;
 
-            int idx_i = static_cast<int>(point.x / x_step);
-            int idx_j = static_cast<int>((point.y + My) / y_step);
+            int idx_i = static_cast<int>((point.x - min_x) / x_step);
+            int idx_j = static_cast<int>((point.y - min_y) / y_step);
 
             // Double security check of indexes
             if (idx_i >= 0 && idx_i < number_sections && idx_j >= 0 && idx_j < number_sections) {

src/planning/trajectory_optimization/src/trajectory_optimization_node.cpp

@@ -57,6 +57,7 @@ TrajectoryOptimization::TrajectoryOptimization() : Node("trajectory_optimization
     smoothing_sub_ = this->create_subscription<common_msgs::msg::Trajectory>(
         kTrajectoryTopic, 1, std::bind(&TrajectoryOptimization::smoothing_callback, this, std::placeholders::_1));
     optimized_trajectory_pub_ = this->create_publisher<common_msgs::msg::Trajectory>(kOptimizedTrajectoryTopic, 10);
+
 }