diff --git a/config/perception/vehicle.yaml b/config/perception/vehicle.yaml index 9a3cdaa64..70c236a95 100644 --- a/config/perception/vehicle.yaml +++ b/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 diff --git a/ext/as-integration b/ext/as-integration index 52b369f64..0d754371e 160000 --- a/ext/as-integration +++ b/ext/as-integration @@ -1 +1 @@ -Subproject commit 52b369f64c6b2899087e81f65f166e597844cdc0 +Subproject commit 0d754371e865eef3c6a506dcca9458f2e034c066 diff --git a/ext/gtsam b/ext/gtsam index 4f66a491f..3ad4b4c3c 160000 --- a/ext/gtsam +++ b/ext/gtsam @@ -1 +1 @@ -Subproject commit 4f66a491ffc83cf092d0d818b11dc35135521612 +Subproject commit 3ad4b4c3cb28394c9597f48fa02dad361c8450e3 diff --git a/src/common_lib/include/common_lib/communication/marker.hpp b/src/common_lib/include/common_lib/communication/marker.hpp index ef733b40b..7be13dbe6 100644 --- a/src/common_lib/include/common_lib/communication/marker.hpp +++ b/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& path_array, - const std::string& name_space, - const std::string& frame_id, - float scale = 0.2f, - int every_nth = 1); + const std::vector& path_array, const std::string& name_space, + const std::string& frame_id, float scale = 0.2f, int every_nth = 1); } // namespace common_lib::communication \ No newline at end of file diff --git a/src/perception/include/fov_trimming/fov_trimming.hpp b/src/perception/include/fov_trimming/fov_trimming.hpp index a70368523..9f4691b6e 100644 --- a/src/perception/include/fov_trimming/fov_trimming.hpp +++ b/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: diff --git a/src/perception/include/utils/trimming_parameters.hpp b/src/perception/include/utils/trimming_parameters.hpp index 287d3c152..d715860a1 100644 --- a/src/perception/include/utils/trimming_parameters.hpp +++ b/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; }; diff --git a/src/perception/src/fov_trimming/acceleration_trimming.cpp b/src/perception/src/fov_trimming/acceleration_trimming.cpp index 503f7fc7c..3d7a0b846 100644 --- a/src/perception/src/fov_trimming/acceleration_trimming.cpp +++ b/src/perception/src/fov_trimming/acceleration_trimming.cpp @@ -35,6 +35,7 @@ void AccelerationTrimming::fov_trimming( const float x = *reinterpret_cast(&data[LidarPoint::PointX(i)]); const float y = *reinterpret_cast(&data[LidarPoint::PointY(i)]); const float z = *reinterpret_cast(&data[LidarPoint::PointZ(i)]); + const float intensity = *reinterpret_cast(&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(std::memcpy(out, &data[LidarPoint::PointX(i)], LidarPoint::POINT_STEP)); // Write rotated coords if rotation applies if (rotate) { diff --git a/src/perception/src/fov_trimming/cut_trimming.cpp b/src/perception/src/fov_trimming/cut_trimming.cpp index 6e84a2f47..7576e1e09 100644 --- a/src/perception/src/fov_trimming/cut_trimming.cpp +++ b/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(&data[LidarPoint::PointX(i)]); const float y = *reinterpret_cast(&data[LidarPoint::PointY(i)]); const float z = *reinterpret_cast(&data[LidarPoint::PointZ(i)]); + const float intensity = *reinterpret_cast(&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(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(std::memcpy(out, &data[LidarPoint::PointX(i)], LidarPoint::POINT_STEP)); // Overwrite X/Y only if rotation is active if (rotate) { diff --git a/src/perception/src/fov_trimming/fov_trimming.cpp b/src/perception/src/fov_trimming/fov_trimming.cpp index e13d724bf..3a427f5be 100644 --- a/src/perception/src/fov_trimming/fov_trimming.cpp +++ b/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; } diff --git a/src/perception/src/fov_trimming/skidpad_trimming.cpp b/src/perception/src/fov_trimming/skidpad_trimming.cpp index 2160df423..c46f38e36 100644 --- a/src/perception/src/fov_trimming/skidpad_trimming.cpp +++ b/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(&data[LidarPoint::PointX(i)]); const float y = *reinterpret_cast(&data[LidarPoint::PointY(i)]); const float z = *reinterpret_cast(&data[LidarPoint::PointZ(i)]); + const float intensity = *reinterpret_cast(&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(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(std::memcpy(out, &data[LidarPoint::PointX(i)], LidarPoint::POINT_STEP)); // Overwrite rotated X/Y if needed if (rotate) { diff --git a/src/perception/src/ground_removal/himmelsbach.cpp b/src/perception/src/ground_removal/himmelsbach.cpp index 3856255ca..80180a51f 100644 --- a/src/perception/src/ground_removal/himmelsbach.cpp +++ b/src/perception/src/ground_removal/himmelsbach.cpp @@ -63,7 +63,7 @@ void Himmelsbach::process_slice( std::vector 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(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(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(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 diff --git a/src/perception/src/perception/perception_node.cpp b/src/perception/src/perception/perception_node.cpp index 7cbadca9b..3df1acc6d 100644 --- a/src/perception/src/perception/perception_node.cpp +++ b/src/perception/src/perception/perception_node.cpp @@ -62,6 +62,8 @@ PerceptionParameters Perception::load_config() { trim_params.rotation = perception_config["rotation"].as(); trim_params.apply_fov_trimming = perception_config["apply_fov_trimming"].as(); trim_params.fov = perception_config["fov"].as(); + trim_params.is_raining = perception_config["is_raining"].as(); + trim_params.minimum_intensity = perception_config["minimum_intensity"].as(); auto acceleration_trimming = std::make_shared(trim_params); auto skidpad_trimming = std::make_shared(trim_params);