fixed rain problem in perception

config/perception/vehicle.yaml

@@ -13,11 +13,13 @@ perception:
 
   #FOV trimming
   apply_rotation: false
-  rotation: 2.736
+  rotation: 90.0
   apply_fov_trimming: false
   fov: 180.0    #ATTENTION: If fov trimming not applied, leave at 180 degrees
   min_range: 2.0
   max_height: 3.0
+  is_raining: false
+  minimum_intensity: 1.0  # Only aplied when is_raining is true
 
   #TRACKDRIVE/AUTOCROSS TRIMMING
   max_range: 40.0
@@ -28,7 +30,6 @@ perception:
 
   #SKIDPAD TRIMMING
   skid_max_range: 40.0
-
   #Ground Grid
   ground_grid_range: 40.0  #ATTENTION: Should be >= all max_ranges
   ground_grid_angle: 4.5
@@ -72,7 +73,7 @@ perception:
   max_distance_from_ground_max: 0.90 # max distance form the ground of the highest point in the cluster
   n_points_intial_max: 50
   n_points_intial_min: 4
-  n_points_max_distance_reduction: 1.5
+  n_points_max_distance_reduction: 1.0
   n_points_min_distance_reduction: 0.15
 
   #ICP

src/common_lib/include/common_lib/communication/marker.hpp

@@ -7,12 +7,12 @@
 
 #include "common_lib/structures/cone.hpp"
 #include "common_lib/structures/path_point.hpp"
+#include "common_lib/structures/position.hpp"
 #include "rclcpp/clock.hpp"
 #include "rclcpp/rclcpp.hpp"
 #include "std_msgs/msg/color_rgba.hpp"
 #include "visualization_msgs/msg/marker.hpp"
 #include "visualization_msgs/msg/marker_array.hpp"
-#include "common_lib/structures/position.hpp"
 
 using K = CGAL::Exact_predicates_inexact_constructions_kernel;
 using Point = K::Point_2;
@@ -250,9 +250,6 @@ visualization_msgs::msg::Marker lines_marker_from_triangulations(
  * @return visualization_msgs::msg::MarkerArray
  */
 visualization_msgs::msg::MarkerArray velocity_hover_markers(
-    const std::vector<common_lib::structures::PathPoint>& path_array, 
-    const std::string& name_space,
-    const std::string& frame_id,
-    float scale = 0.2f,
-    int every_nth = 1);
+    const std::vector<common_lib::structures::PathPoint>& path_array, const std::string& name_space,
+    const std::string& frame_id, float scale = 0.2f, int every_nth = 1);
 }  // namespace common_lib::communication

src/perception/include/fov_trimming/fov_trimming.hpp

@@ -21,7 +21,7 @@ class FovTrimming {
   virtual void fov_trimming(const sensor_msgs::msg::PointCloud2::SharedPtr& point_cloud,
                             sensor_msgs::msg::PointCloud2::SharedPtr& trimmed_cloud) const = 0;
 
-  bool within_limits(float x, float y, float z, const TrimmingParameters& params,
+  bool within_limits(float x, float y, float z, float intensity, const TrimmingParameters& params,
                      const double max_range) const;
 
 protected:

src/perception/include/utils/trimming_parameters.hpp

@@ -10,14 +10,16 @@ struct TrimmingParameters {
   double lidar_vertical_resolution;    ///< LIDAR vertical resolution.
   bool apply_rotation;                 ///< Whether to apply rotation to the point cloud.
   double rotation;                     ///< Rotation angle to be applied to the point cloud.
-  bool apply_fov_trimming;            ///< Whether to apply field of view trimming.
+  bool apply_fov_trimming;             ///< Whether to apply field of view trimming.
   double fov;                          ///< Field of view.
-  double max_height;                   ///< Maximum point cloud height after trimming.
-  double min_range;                    ///< Maximum point cloud distance after trimming.
-  double max_range;                    ///< Minimum point cloud distance after trimming.
-  double acc_max_range;                ///< (Acceleration Only) Maximum distance after trimming.
-  double acc_max_y;       ///< (Acceleration Only) Maximum lateral distance after trimming.
-  double skid_max_range;  ///< (Skidpad Only) Maximum distance after trimming.
+  bool is_raining;                     ///< Whether it is raining.
+  float minimum_intensity;  ///< Minimum intensity for point to be considered valid in rain.
+  double max_height;        ///< Maximum point cloud height after trimming.
+  double min_range;         ///< Maximum point cloud distance after trimming.
+  double max_range;         ///< Minimum point cloud distance after trimming.
+  double acc_max_range;     ///< (Acceleration Only) Maximum distance after trimming.
+  double acc_max_y;         ///< (Acceleration Only) Maximum lateral distance after trimming.
+  double skid_max_range;    ///< (Skidpad Only) Maximum distance after trimming.
 
   TrimmingParameters() = default;
 };

src/perception/src/fov_trimming/acceleration_trimming.cpp

@@ -35,6 +35,7 @@ void AccelerationTrimming::fov_trimming(
     const float x = *reinterpret_cast<const float*>(&data[LidarPoint::PointX(i)]);
     const float y = *reinterpret_cast<const float*>(&data[LidarPoint::PointY(i)]);
     const float z = *reinterpret_cast<const float*>(&data[LidarPoint::PointZ(i)]);
+    const float intensity = *reinterpret_cast<const float*>(&data[LidarPoint::PointIntensity(i)]);
 
     if (x == 0.0f && y == 0.0f && z == 0.0f) {
       continue;
@@ -49,10 +50,10 @@ void AccelerationTrimming::fov_trimming(
     }
 
     if (ry < params_.acc_max_y && ry > -params_.acc_max_y &&
-        within_limits(rx, ry, z, params_, params_.acc_max_range)) {
+        within_limits(rx, ry, z, intensity, params_, params_.acc_max_range)) {
       uint8_t* out = &trimmed_cloud->data[trimmed_cloud->width * LidarPoint::POINT_STEP];
 
-      std::memcpy(out, &data[LidarPoint::PointX(i)], LidarPoint::POINT_STEP);
+      static_cast<void>(std::memcpy(out, &data[LidarPoint::PointX(i)], LidarPoint::POINT_STEP));
 
       // Write rotated coords if rotation applies
       if (rotate) {

src/perception/src/fov_trimming/cut_trimming.cpp

@@ -33,6 +33,7 @@ void CutTrimming::fov_trimming(const sensor_msgs::msg::PointCloud2::SharedPtr& c
     const float x = *reinterpret_cast<const float*>(&data[LidarPoint::PointX(i)]);
     const float y = *reinterpret_cast<const float*>(&data[LidarPoint::PointY(i)]);
     const float z = *reinterpret_cast<const float*>(&data[LidarPoint::PointZ(i)]);
+    const float intensity = *reinterpret_cast<const float*>(&data[LidarPoint::PointIntensity(i)]);
 
     // Skip invalid points
     if (x == 0.0f && y == 0.0f && z == 0.0f) {
@@ -47,10 +48,10 @@ void CutTrimming::fov_trimming(const sensor_msgs::msg::PointCloud2::SharedPtr& c
       ry = static_cast<float>(x * sin_t + y * cos_t);
     }
 
-    if (within_limits(rx, ry, z, params_, params_.max_range)) {
+    if (within_limits(rx, ry, z, intensity, params_, params_.max_range)) {
       uint8_t* out = &trimmed_cloud->data[trimmed_cloud->width * LidarPoint::POINT_STEP];
 
-      std::memcpy(out, &data[LidarPoint::PointX(i)], LidarPoint::POINT_STEP);
+      static_cast<void>(std::memcpy(out, &data[LidarPoint::PointX(i)], LidarPoint::POINT_STEP));
 
       // Overwrite X/Y only if rotation is active
       if (rotate) {

src/perception/src/fov_trimming/fov_trimming.cpp

@@ -1,7 +1,7 @@
 #include "fov_trimming/fov_trimming.hpp"
 
-bool FovTrimming::within_limits(float x, float y, float z, const TrimmingParameters& params,
-                                const double max_range) const {
+bool FovTrimming::within_limits(float x, float y, float z, float intensity,
+                                const TrimmingParameters& params, const double max_range) const {
   bool ret = true;
 
   if (params.apply_fov_trimming) {
@@ -18,5 +18,12 @@ bool FovTrimming::within_limits(float x, float y, float z, const TrimmingParamet
   ret &= (distance_squared > params.min_range * params.min_range) &&
          (distance_squared <= max_range * max_range);
 
+  if (params.is_raining) {
+    // In rainy conditions, the intensity must be at greater or equal to 1 to be considered valid,
+    // as the water droplets cause 0 intensity points. It is not applied in normal conditions to
+    // preserve more points, as a lot of cone points have low intensity.
+    ret &= intensity >= params.minimum_intensity;
+  }
+
   return ret;
 }

src/perception/src/fov_trimming/skidpad_trimming.cpp

@@ -31,6 +31,7 @@ void SkidpadTrimming::fov_trimming(const sensor_msgs::msg::PointCloud2::SharedPt
     const float x = *reinterpret_cast<const float*>(&data[LidarPoint::PointX(i)]);
     const float y = *reinterpret_cast<const float*>(&data[LidarPoint::PointY(i)]);
     const float z = *reinterpret_cast<const float*>(&data[LidarPoint::PointZ(i)]);
+    const float intensity = *reinterpret_cast<const float*>(&data[LidarPoint::PointIntensity(i)]);
 
     // Skip invalid points
     if (x == 0.0f && y == 0.0f && z == 0.0f) {
@@ -45,10 +46,10 @@ void SkidpadTrimming::fov_trimming(const sensor_msgs::msg::PointCloud2::SharedPt
       ry = static_cast<float>(x * sin_t + y * cos_t);
     }
 
-    if (within_limits(rx, ry, z, params_, params_.skid_max_range)) {
+    if (within_limits(rx, ry, z, intensity, params_, params_.skid_max_range)) {
       uint8_t* out = &trimmed_cloud->data[trimmed_cloud->width * LidarPoint::POINT_STEP];
 
-      std::memcpy(out, &data[LidarPoint::PointX(i)], LidarPoint::POINT_STEP);
+      static_cast<void>(std::memcpy(out, &data[LidarPoint::PointX(i)], LidarPoint::POINT_STEP));
 
       // Overwrite rotated X/Y if needed
       if (rotate) {

src/perception/src/ground_removal/himmelsbach.cpp

@@ -63,7 +63,7 @@ void Himmelsbach::process_slice(
   std::vector<double> ground_distances;
 
   // For each ring in the slice, compare points to previous ground point
-  for (int ring_idx = NUM_RINGS - 1; ring_idx >= 0; --ring_idx) {
+  for (int ring_idx = ::NUM_RINGS - 1; ring_idx >= 0; --ring_idx) {
     const auto& ring = slices_->at(slice_idx).rings[ring_idx];
     if (ring.indices.empty()) {
       continue;
@@ -80,17 +80,19 @@ void Himmelsbach::process_slice(
 
       float current_ground_point_distance = std::hypot(pt_x, pt_y);
 
-      if (current_ground_point_distance - previous_ground_point_distance <
-              (-5 - (0.15 * ring_idx)) ||
-          current_ground_point_distance - previous_ground_point_distance >
-              (5 + (0.15 * ring_idx))) {
+      if (!this->trim_params_.is_raining &&
+          (current_ground_point_distance - previous_ground_point_distance <
+               (-5 - (0.15 * ring_idx)) ||
+           current_ground_point_distance - previous_ground_point_distance >
+               (5 + (0.15 * ring_idx)))) {
         // It is noise, ignore
         continue;
       } else if (current_ground_point_distance <= previous_ground_point_distance) {
         // Write to the output cloud
         size_t write_idx = ground_removed_point_cloud->width;
-        std::memcpy(&output_data[write_idx * LidarPoint::POINT_STEP],
-                    &cloud_data[idx * LidarPoint::POINT_STEP], LidarPoint::POINT_STEP);
+        static_cast<void>(std::memcpy(&output_data[write_idx * LidarPoint::POINT_STEP],
+                                      &cloud_data[idx * LidarPoint::POINT_STEP],
+                                      LidarPoint::POINT_STEP));
         ground_removed_point_cloud->width++;
       } else {
         // Compute slope to previous ground point
@@ -109,8 +111,9 @@ void Himmelsbach::process_slice(
         } else {
           // Point is non-ground, write to output cloud
           size_t write_idx = ground_removed_point_cloud->width;
-          std::memcpy(&output_data[write_idx * LidarPoint::POINT_STEP],
-                      &cloud_data[idx * LidarPoint::POINT_STEP], LidarPoint::POINT_STEP);
+          static_cast<void>(std::memcpy(&output_data[write_idx * LidarPoint::POINT_STEP],
+                                        &cloud_data[idx * LidarPoint::POINT_STEP],
+                                        LidarPoint::POINT_STEP));
           ground_removed_point_cloud->width++;
         }
       }
@@ -146,8 +149,9 @@ void Himmelsbach::process_slice(
         if (r < r_ref - threshold) {
           // too far from reference, non-ground
           size_t write_idx = ground_removed_point_cloud->width;
-          std::memcpy(&output_data[write_idx * LidarPoint::POINT_STEP],
-                      &cloud_data[idx * LidarPoint::POINT_STEP], LidarPoint::POINT_STEP);
+          static_cast<void>(std::memcpy(&output_data[write_idx * LidarPoint::POINT_STEP],
+                                        &cloud_data[idx * LidarPoint::POINT_STEP],
+                                        LidarPoint::POINT_STEP));
           ground_removed_point_cloud->width++;
         } else {
           // Ground, update the ground grid and lowest ground point

src/perception/src/perception/perception_node.cpp

@@ -62,6 +62,8 @@ PerceptionParameters Perception::load_config() {
   trim_params.rotation = perception_config["rotation"].as<double>();
   trim_params.apply_fov_trimming = perception_config["apply_fov_trimming"].as<bool>();
   trim_params.fov = perception_config["fov"].as<double>();
+  trim_params.is_raining = perception_config["is_raining"].as<bool>();
+  trim_params.minimum_intensity = perception_config["minimum_intensity"].as<float>();
 
   auto acceleration_trimming = std::make_shared<AccelerationTrimming>(trim_params);
   auto skidpad_trimming = std::make_shared<SkidpadTrimming>(trim_params);