From 84e5ca1e1067086100b013a4897dd47f165519f8 Mon Sep 17 00:00:00 2001 From: FranciscoAlmeroRuiz Date: Sun, 22 Mar 2026 11:09:59 +0100 Subject: [PATCH 1/6] fix: adaptive limits to the point cloud --- .../launch/labeled_groundfiltering_launch.py | 34 ++++ .../src/labeled_groundfiltering_node.cpp | 149 ++++++++++++++++++ .../include/perception/ground_filtering.h | 28 +++- 3 files changed, 206 insertions(+), 5 deletions(-) create mode 100644 src/labeled_groundfiltering/launch/labeled_groundfiltering_launch.py create mode 100644 src/labeled_groundfiltering/src/labeled_groundfiltering_node.cpp diff --git a/src/labeled_groundfiltering/launch/labeled_groundfiltering_launch.py b/src/labeled_groundfiltering/launch/labeled_groundfiltering_launch.py new file mode 100644 index 00000000..fbf26bd1 --- /dev/null +++ b/src/labeled_groundfiltering/launch/labeled_groundfiltering_launch.py @@ -0,0 +1,34 @@ +import pandas as pd +import matplotlib.pyplot as plt + +# Leer los datos generados por el nodo C++ +try: + df = pd.read_csv("metrics_results.csv") +except FileNotFoundError: + print("No se encontró el archivo de métricas. Asegúrate de que el nodo C++ lo generó.") + exit() + +fig, axs = plt.subplots(2, 1, figsize=(10, 8), sharex=True) +fig.suptitle('Resultados de Evaluación de Percepción (SIL C++)', fontsize=16) + +# Gráfica 1: TP, FP, FN +axs[0].plot(df['Frame'], df['TP'], label='Verdaderos Positivos (TP)', color='green', marker='o') +axs[0].plot(df['Frame'], df['FP'], label='Falsos Positivos (FP)', color='red', marker='x') +axs[0].plot(df['Frame'], df['FN'], label='Falsos Negativos (FN)', color='orange', marker='s') +axs[0].set_ylabel('Cantidad de Puntos') +axs[0].set_title('Métricas de Error por Nube de Puntos') +axs[0].legend() +axs[0].grid(True, linestyle='--', alpha=0.7) + +# Gráfica 2: Precision & Recall +axs[1].plot(df['Frame'], df['Precision'], label='Precision (%)', color='blue', marker='o') +axs[1].plot(df['Frame'], df['Recall'], label='Recall (%)', color='purple', marker='x') +axs[1].set_xlabel('Índice de Nube (Frame)') +axs[1].set_ylabel('Porcentaje (%)') +axs[1].set_title('Rendimiento: Precisión vs Recall') +axs[1].set_ylim([-5, 105]) +axs[1].legend() +axs[1].grid(True, linestyle='--', alpha=0.7) + +plt.tight_layout() +plt.show() \ No newline at end of file diff --git a/src/labeled_groundfiltering/src/labeled_groundfiltering_node.cpp b/src/labeled_groundfiltering/src/labeled_groundfiltering_node.cpp new file mode 100644 index 00000000..382f9ddb --- /dev/null +++ b/src/labeled_groundfiltering/src/labeled_groundfiltering_node.cpp @@ -0,0 +1,149 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +class EvaluatorNode : public rclcpp::Node +{ +public: + EvaluatorNode() : Node("evaluator_node"), current_cloud_idx_(0), total_clouds_(10) + { + // Publica la nube cruda simulando el LiDAR + lidar_pub_ = this->create_publisher("/rslidar_points", 10); + + // Escucha el resultado de tu nodo de percepción (ej. el suelo detectado) + perception_sub_ = this->create_subscription( + "/perception/ground_cloud", 10, + std::bind(&EvaluatorNode::perception_callback, this, std::placeholders::_1)); + + // Preparamos el CSV + csv_file_.open("metrics_results.csv"); + csv_file_ << "Frame,TP,FP,FN,Precision,Recall\n"; + + RCLCPP_INFO(this->get_logger(), "Evaluador Iniciado. Mandando la primera nube..."); + + // Damos un segundo para que los topics se conecten y mandamos la primera nube + timer_ = this->create_wall_timer( + std::chrono::seconds(1), + [this]() { + this->send_next_cloud(); + this->timer_->cancel(); // Solo lo usamos para el retraso inicial + }); + } + + ~EvaluatorNode() + { + if (csv_file_.is_open()) csv_file_.close(); + } + +private: + int current_cloud_idx_; + int total_clouds_; + std::ofstream csv_file_; + rclcpp::Publisher::SharedPtr lidar_pub_; + rclcpp::Subscription::SharedPtr perception_sub_; + rclcpp::TimerBase::SharedPtr timer_; + + void send_next_cloud() + { + if (current_cloud_idx_ >= total_clouds_) { + RCLCPP_INFO(this->get_logger(), "Evaluacion terminada. Lanzando graficas..."); + csv_file_.close(); + // MAGIA: Ejecutamos el script de Python de forma asíncrona + std::system("python3 plot_results.py &"); + rclcpp::shutdown(); + return; + } + + std::string raw_pcd_path = "dataset/raw_cloud_" + std::to_string(current_cloud_idx_) + ".pcd"; + pcl::PointCloud::Ptr raw_cloud(new pcl::PointCloud); + + if (pcl::io::loadPCDFile(raw_pcd_path, *raw_cloud) == -1) { + RCLCPP_ERROR(this->get_logger(), "No se pudo leer %s", raw_pcd_path.c_str()); + return; + } + + sensor_msgs::msg::PointCloud2 ros_msg; + pcl::toROSMsg(*raw_cloud, ros_msg); + ros_msg.header.frame_id = "/rslidar"; + ros_msg.header.stamp = this->now(); + + RCLCPP_INFO(this->get_logger(), "-> Publicando nube cruda %d", current_cloud_idx_); + lidar_pub_->publish(ros_msg); + } + + void perception_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) + { + RCLCPP_INFO(this->get_logger(), "<- Recibida evaluacion de la nube %d", current_cloud_idx_); + + // 1. Convertir el resultado del nodo a PCL + pcl::PointCloud::Ptr test_cloud(new pcl::PointCloud); + pcl::fromROSMsg(*msg, *test_cloud); + + // 2. Cargar tu Ground Truth hecho a mano (ej: solo los puntos que DEBERÍAN ser suelo) + std::string gt_pcd_path = "dataset/gt_ground_" + std::to_string(current_cloud_idx_) + ".pcd"; + pcl::PointCloud::Ptr gt_cloud(new pcl::PointCloud); + + if (pcl::io::loadPCDFile(gt_pcd_path, *gt_cloud) == -1) { + RCLCPP_ERROR(this->get_logger(), "No se encontro el GT %s", gt_pcd_path.c_str()); + return; + } + + // 3. Evaluar usando KD-Tree (distancia menor a 1e-4 = mismo punto) + int tp = 0, fp = 0, fn = 0; + double tolerance = 1e-4; + + pcl::KdTreeFLANN kdtree_gt; + kdtree_gt.setInputCloud(gt_cloud); + std::vector pointIdxRadiusSearch; + std::vector pointRadiusSquaredDistance; + + // Calcular TP y FP + for (const auto& pt : test_cloud->points) { + if (kdtree_gt.radiusSearch(pt, tolerance, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0) { + tp++; + } else { + fp++; + } + } + + // Calcular FN + pcl::KdTreeFLANN kdtree_test; + if (!test_cloud->empty()) { + kdtree_test.setInputCloud(test_cloud); + for (const auto& pt : gt_cloud->points) { + if (kdtree_test.radiusSearch(pt, tolerance, pointIdxRadiusSearch, pointRadiusSquaredDistance) == 0) { + fn++; + } + } + } else { + fn = gt_cloud->size(); // Si el test devolvió vacío, todos son Falsos Negativos + } + + double precision = (tp + fp > 0) ? (double)tp / (tp + fp) * 100.0 : 0.0; + double recall = (tp + fn > 0) ? (double)tp / (tp + fn) * 100.0 : 0.0; + + // Guardar e imprimir resultados + RCLCPP_INFO(this->get_logger(), "[RESULTADOS %d] TP: %d | FP: %d | FN: %d | Prec: %.2f%% | Rec: %.2f%%", + current_cloud_idx_, tp, fp, fn, precision, recall); + + csv_file_ << current_cloud_idx_ << "," << tp << "," << fp << "," << fn << "," + << precision << "," << recall << "\n"; + + // Avanzar a la siguiente nube + current_cloud_idx_++; + send_next_cloud(); + } +}; + +int main(int argc, char **argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/src/perception/include/perception/ground_filtering.h b/src/perception/include/perception/ground_filtering.h index 4df5c18a..8f0c2c3f 100644 --- a/src/perception/include/perception/ground_filtering.h +++ b/src/perception/include/perception/ground_filtering.h @@ -141,12 +141,30 @@ 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; + double min_x = std::numeric_limits::max(); + double max_x = std::numeric_limits::lowest(); + double min_y = std::numeric_limits::max(); + double max_y = std::numeric_limits::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; + } + + if (min_x > max_x) return; + + 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::Ptr> grid_cells(total_cells); @@ -160,8 +178,8 @@ namespace GroundFiltering for (const auto& point : cloud->points) { if (point.z < min_z) continue; - int idx_i = static_cast(point.x / x_step); - int idx_j = static_cast((point.y + My) / y_step); + int idx_i = static_cast((point.x - min_x) / x_step); + int idx_j = static_cast((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) { From 354a92577c44a62d9df3d710429040764584d5aa Mon Sep 17 00:00:00 2001 From: FranciscoAlmeroRuiz Date: Thu, 26 Mar 2026 15:30:33 +0100 Subject: [PATCH 2/6] fix: add automatic adaptation of the ground filter limits to those of the point cloud --- src/perception/config/perception_config.yaml | 7 ++++++- src/perception/include/perception/ground_filtering.h | 12 ++++++++++++ .../src/trajectory_optimization_node.cpp | 1 + 3 files changed, 19 insertions(+), 1 deletion(-) diff --git a/src/perception/config/perception_config.yaml b/src/perception/config/perception_config.yaml index a9700455..5dd131a1 100644 --- a/src/perception/config/perception_config.yaml +++ b/src/perception/config/perception_config.yaml @@ -18,7 +18,7 @@ perception: use_artificial_timestamp: false # 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,8 +29,13 @@ perception: slow_straight_cropping: false # Leave only a straight section in front of the car when moving slowly # Clipping parameters +<<<<<<< Updated upstream clipping: true # Enable clipping of the point cloud max_x_clip: 50.0 +======= + clipping: false # Enable clipping of the point cloud + max_x_clip: 40.0 +>>>>>>> Stashed changes max_y_clip: 5.0 min_x_clip: -30.0 min_y_clip: -80.0 diff --git a/src/perception/include/perception/ground_filtering.h b/src/perception/include/perception/ground_filtering.h index 8f0c2c3f..8a7a5c62 100644 --- a/src/perception/include/perception/ground_filtering.h +++ b/src/perception/include/perception/ground_filtering.h @@ -144,6 +144,10 @@ namespace GroundFiltering if (cloud->empty()) return; +<<<<<<< Updated upstream +======= + // We associate our limits with the actual limits of the point cloud +>>>>>>> Stashed changes double min_x = std::numeric_limits::max(); double max_x = std::numeric_limits::lowest(); double min_y = std::numeric_limits::max(); @@ -157,8 +161,16 @@ namespace GroundFiltering if (point.y > max_y) max_y = point.y; } +<<<<<<< Updated upstream if (min_x > max_x) return; +======= + // 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 +>>>>>>> Stashed changes max_x += 0.001; max_y += 0.001; diff --git a/src/planning/trajectory_optimization/src/trajectory_optimization_node.cpp b/src/planning/trajectory_optimization/src/trajectory_optimization_node.cpp index 53bb47c7..de58505f 100644 --- a/src/planning/trajectory_optimization/src/trajectory_optimization_node.cpp +++ b/src/planning/trajectory_optimization/src/trajectory_optimization_node.cpp @@ -57,6 +57,7 @@ TrajectoryOptimization::TrajectoryOptimization() : Node("trajectory_optimization smoothing_sub_ = this->create_subscription( kTrajectoryTopic, 1, std::bind(&TrajectoryOptimization::smoothing_callback, this, std::placeholders::_1)); optimized_trajectory_pub_ = this->create_publisher(kOptimizedTrajectoryTopic, 10); + } From 0d614807157917e6eec5e0afbd0ddc2d4bfd62ef Mon Sep 17 00:00:00 2001 From: FranciscoAlmeroRuiz Date: Thu, 26 Mar 2026 15:35:18 +0100 Subject: [PATCH 3/6] fix: delete labeled_groundfiltering --- .../launch/labeled_groundfiltering_launch.py | 34 ---- .../src/labeled_groundfiltering_node.cpp | 149 ------------------ 2 files changed, 183 deletions(-) delete mode 100644 src/labeled_groundfiltering/launch/labeled_groundfiltering_launch.py delete mode 100644 src/labeled_groundfiltering/src/labeled_groundfiltering_node.cpp diff --git a/src/labeled_groundfiltering/launch/labeled_groundfiltering_launch.py b/src/labeled_groundfiltering/launch/labeled_groundfiltering_launch.py deleted file mode 100644 index fbf26bd1..00000000 --- a/src/labeled_groundfiltering/launch/labeled_groundfiltering_launch.py +++ /dev/null @@ -1,34 +0,0 @@ -import pandas as pd -import matplotlib.pyplot as plt - -# Leer los datos generados por el nodo C++ -try: - df = pd.read_csv("metrics_results.csv") -except FileNotFoundError: - print("No se encontró el archivo de métricas. Asegúrate de que el nodo C++ lo generó.") - exit() - -fig, axs = plt.subplots(2, 1, figsize=(10, 8), sharex=True) -fig.suptitle('Resultados de Evaluación de Percepción (SIL C++)', fontsize=16) - -# Gráfica 1: TP, FP, FN -axs[0].plot(df['Frame'], df['TP'], label='Verdaderos Positivos (TP)', color='green', marker='o') -axs[0].plot(df['Frame'], df['FP'], label='Falsos Positivos (FP)', color='red', marker='x') -axs[0].plot(df['Frame'], df['FN'], label='Falsos Negativos (FN)', color='orange', marker='s') -axs[0].set_ylabel('Cantidad de Puntos') -axs[0].set_title('Métricas de Error por Nube de Puntos') -axs[0].legend() -axs[0].grid(True, linestyle='--', alpha=0.7) - -# Gráfica 2: Precision & Recall -axs[1].plot(df['Frame'], df['Precision'], label='Precision (%)', color='blue', marker='o') -axs[1].plot(df['Frame'], df['Recall'], label='Recall (%)', color='purple', marker='x') -axs[1].set_xlabel('Índice de Nube (Frame)') -axs[1].set_ylabel('Porcentaje (%)') -axs[1].set_title('Rendimiento: Precisión vs Recall') -axs[1].set_ylim([-5, 105]) -axs[1].legend() -axs[1].grid(True, linestyle='--', alpha=0.7) - -plt.tight_layout() -plt.show() \ No newline at end of file diff --git a/src/labeled_groundfiltering/src/labeled_groundfiltering_node.cpp b/src/labeled_groundfiltering/src/labeled_groundfiltering_node.cpp deleted file mode 100644 index 382f9ddb..00000000 --- a/src/labeled_groundfiltering/src/labeled_groundfiltering_node.cpp +++ /dev/null @@ -1,149 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include - -class EvaluatorNode : public rclcpp::Node -{ -public: - EvaluatorNode() : Node("evaluator_node"), current_cloud_idx_(0), total_clouds_(10) - { - // Publica la nube cruda simulando el LiDAR - lidar_pub_ = this->create_publisher("/rslidar_points", 10); - - // Escucha el resultado de tu nodo de percepción (ej. el suelo detectado) - perception_sub_ = this->create_subscription( - "/perception/ground_cloud", 10, - std::bind(&EvaluatorNode::perception_callback, this, std::placeholders::_1)); - - // Preparamos el CSV - csv_file_.open("metrics_results.csv"); - csv_file_ << "Frame,TP,FP,FN,Precision,Recall\n"; - - RCLCPP_INFO(this->get_logger(), "Evaluador Iniciado. Mandando la primera nube..."); - - // Damos un segundo para que los topics se conecten y mandamos la primera nube - timer_ = this->create_wall_timer( - std::chrono::seconds(1), - [this]() { - this->send_next_cloud(); - this->timer_->cancel(); // Solo lo usamos para el retraso inicial - }); - } - - ~EvaluatorNode() - { - if (csv_file_.is_open()) csv_file_.close(); - } - -private: - int current_cloud_idx_; - int total_clouds_; - std::ofstream csv_file_; - rclcpp::Publisher::SharedPtr lidar_pub_; - rclcpp::Subscription::SharedPtr perception_sub_; - rclcpp::TimerBase::SharedPtr timer_; - - void send_next_cloud() - { - if (current_cloud_idx_ >= total_clouds_) { - RCLCPP_INFO(this->get_logger(), "Evaluacion terminada. Lanzando graficas..."); - csv_file_.close(); - // MAGIA: Ejecutamos el script de Python de forma asíncrona - std::system("python3 plot_results.py &"); - rclcpp::shutdown(); - return; - } - - std::string raw_pcd_path = "dataset/raw_cloud_" + std::to_string(current_cloud_idx_) + ".pcd"; - pcl::PointCloud::Ptr raw_cloud(new pcl::PointCloud); - - if (pcl::io::loadPCDFile(raw_pcd_path, *raw_cloud) == -1) { - RCLCPP_ERROR(this->get_logger(), "No se pudo leer %s", raw_pcd_path.c_str()); - return; - } - - sensor_msgs::msg::PointCloud2 ros_msg; - pcl::toROSMsg(*raw_cloud, ros_msg); - ros_msg.header.frame_id = "/rslidar"; - ros_msg.header.stamp = this->now(); - - RCLCPP_INFO(this->get_logger(), "-> Publicando nube cruda %d", current_cloud_idx_); - lidar_pub_->publish(ros_msg); - } - - void perception_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) - { - RCLCPP_INFO(this->get_logger(), "<- Recibida evaluacion de la nube %d", current_cloud_idx_); - - // 1. Convertir el resultado del nodo a PCL - pcl::PointCloud::Ptr test_cloud(new pcl::PointCloud); - pcl::fromROSMsg(*msg, *test_cloud); - - // 2. Cargar tu Ground Truth hecho a mano (ej: solo los puntos que DEBERÍAN ser suelo) - std::string gt_pcd_path = "dataset/gt_ground_" + std::to_string(current_cloud_idx_) + ".pcd"; - pcl::PointCloud::Ptr gt_cloud(new pcl::PointCloud); - - if (pcl::io::loadPCDFile(gt_pcd_path, *gt_cloud) == -1) { - RCLCPP_ERROR(this->get_logger(), "No se encontro el GT %s", gt_pcd_path.c_str()); - return; - } - - // 3. Evaluar usando KD-Tree (distancia menor a 1e-4 = mismo punto) - int tp = 0, fp = 0, fn = 0; - double tolerance = 1e-4; - - pcl::KdTreeFLANN kdtree_gt; - kdtree_gt.setInputCloud(gt_cloud); - std::vector pointIdxRadiusSearch; - std::vector pointRadiusSquaredDistance; - - // Calcular TP y FP - for (const auto& pt : test_cloud->points) { - if (kdtree_gt.radiusSearch(pt, tolerance, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0) { - tp++; - } else { - fp++; - } - } - - // Calcular FN - pcl::KdTreeFLANN kdtree_test; - if (!test_cloud->empty()) { - kdtree_test.setInputCloud(test_cloud); - for (const auto& pt : gt_cloud->points) { - if (kdtree_test.radiusSearch(pt, tolerance, pointIdxRadiusSearch, pointRadiusSquaredDistance) == 0) { - fn++; - } - } - } else { - fn = gt_cloud->size(); // Si el test devolvió vacío, todos son Falsos Negativos - } - - double precision = (tp + fp > 0) ? (double)tp / (tp + fp) * 100.0 : 0.0; - double recall = (tp + fn > 0) ? (double)tp / (tp + fn) * 100.0 : 0.0; - - // Guardar e imprimir resultados - RCLCPP_INFO(this->get_logger(), "[RESULTADOS %d] TP: %d | FP: %d | FN: %d | Prec: %.2f%% | Rec: %.2f%%", - current_cloud_idx_, tp, fp, fn, precision, recall); - - csv_file_ << current_cloud_idx_ << "," << tp << "," << fp << "," << fn << "," - << precision << "," << recall << "\n"; - - // Avanzar a la siguiente nube - current_cloud_idx_++; - send_next_cloud(); - } -}; - -int main(int argc, char **argv) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} \ No newline at end of file From 3327c9b1b38850007b37eddd5891019dd4ae505d Mon Sep 17 00:00:00 2001 From: FranciscoAlmeroRuiz Date: Fri, 27 Mar 2026 18:04:40 +0100 Subject: [PATCH 4/6] fix: delete unnecessary comments --- src/perception/include/perception/ground_filtering.h | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/perception/include/perception/ground_filtering.h b/src/perception/include/perception/ground_filtering.h index 8a7a5c62..e625d55f 100644 --- a/src/perception/include/perception/ground_filtering.h +++ b/src/perception/include/perception/ground_filtering.h @@ -144,10 +144,7 @@ namespace GroundFiltering if (cloud->empty()) return; -<<<<<<< Updated upstream -======= // We associate our limits with the actual limits of the point cloud ->>>>>>> Stashed changes double min_x = std::numeric_limits::max(); double max_x = std::numeric_limits::lowest(); double min_y = std::numeric_limits::max(); @@ -161,16 +158,13 @@ namespace GroundFiltering if (point.y > max_y) max_y = point.y; } -<<<<<<< Updated upstream if (min_x > max_x) return; -======= // 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 ->>>>>>> Stashed changes max_x += 0.001; max_y += 0.001; From 53e3283b0e12ab3d93893d096b8877d931ebe143 Mon Sep 17 00:00:00 2001 From: FranciscoAlmeroRuiz Date: Fri, 27 Mar 2026 18:06:42 +0100 Subject: [PATCH 5/6] fix: delete duplicate line --- src/perception/include/perception/ground_filtering.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/perception/include/perception/ground_filtering.h b/src/perception/include/perception/ground_filtering.h index e625d55f..d9075cff 100644 --- a/src/perception/include/perception/ground_filtering.h +++ b/src/perception/include/perception/ground_filtering.h @@ -158,8 +158,6 @@ namespace GroundFiltering if (point.y > max_y) max_y = point.y; } - if (min_x > max_x) return; - // Error prevention if no valid points remain if (min_x > max_x) return; From f5285305b3f6b9b24f7a7dda87efbf74d182c778 Mon Sep 17 00:00:00 2001 From: FranciscoAlmeroRuiz Date: Fri, 27 Mar 2026 18:11:21 +0100 Subject: [PATCH 6/6] fix: delete duplicate line --- src/perception/config/perception_config.yaml | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/perception/config/perception_config.yaml b/src/perception/config/perception_config.yaml index 5dd131a1..c99ec09b 100644 --- a/src/perception/config/perception_config.yaml +++ b/src/perception/config/perception_config.yaml @@ -29,13 +29,8 @@ perception: slow_straight_cropping: false # Leave only a straight section in front of the car when moving slowly # Clipping parameters -<<<<<<< Updated upstream - clipping: true # Enable clipping of the point cloud - max_x_clip: 50.0 -======= clipping: false # Enable clipping of the point cloud max_x_clip: 40.0 ->>>>>>> Stashed changes max_y_clip: 5.0 min_x_clip: -30.0 min_y_clip: -80.0