Skip to content

Commit

Permalink
style(pre-commit): autofix
Browse files Browse the repository at this point in the history
  • Loading branch information
pre-commit-ci[bot] committed Mar 5, 2025
1 parent dc96513 commit 46b11ac
Show file tree
Hide file tree
Showing 6 changed files with 43 additions and 28 deletions.
2 changes: 1 addition & 1 deletion sensing/autoware_pointcloud_preprocessor/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -90,7 +90,7 @@ ament_auto_add_library(pointcloud_preprocessor_filter SHARED
src/downsample_filter/random_downsample_filter_node.cpp
src/downsample_filter/approximate_downsample_filter_node.cpp
src/downsample_filter/pickup_based_voxel_grid_downsample_filter_node.cpp
src/downsample_filter/roi_excluded_voxel_grid_downsample_filter_node.cpp
src/downsample_filter/roi_excluded_voxel_grid_downsample_filter_node.cpp
src/outlier_filter/ring_outlier_filter_node.cpp
src/outlier_filter/radius_search_2d_outlier_filter_node.cpp
src/outlier_filter/voxel_grid_outlier_filter_node.cpp
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -15,15 +15,18 @@
#pragma once

#include "autoware/pointcloud_preprocessor/transform_info.hpp"

#include <Eigen/Core>

#include <pcl/filters/voxel_grid.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/msg/point_cloud2.h>
#include <unordered_map>
#include <vector>
#include <limits>

#include <cfloat>
#include <cmath>
#include <Eigen/Core>
#include <limits>
#include <unordered_map>
#include <vector>

namespace autoware::pointcloud_preprocessor
{
Expand All @@ -39,8 +42,8 @@ class RoiExcludedFasterVoxelGridDownsampleFilter // renamed class
void set_field_offsets(const PointCloud2ConstPtr & input, const rclcpp::Logger & logger);
void set_roi(float x_min, float x_max, float y_min, float y_max);
void filter(
const PointCloud2ConstPtr & input, PointCloud2 & output,
const TransformInfo & transform_info, const rclcpp::Logger & logger);
const PointCloud2ConstPtr & input, PointCloud2 & output, const TransformInfo & transform_info,
const rclcpp::Logger & logger);

private:
struct Centroid
Expand All @@ -52,15 +55,21 @@ class RoiExcludedFasterVoxelGridDownsampleFilter // renamed class
uint32_t point_count_;
Centroid() : x(0), y(0), z(0), intensity(0), point_count_(0) {}
Centroid(float _x, float _y, float _z, float _intensity)
: x(_x), y(_y), z(_z), intensity(_intensity), point_count_(1) {}
: x(_x), y(_y), z(_z), intensity(_intensity), point_count_(1)
{
}
void add_point(float _x, float _y, float _z, float _intensity)
{
x += _x; y += _y; z += _z; intensity += _intensity;
x += _x;
y += _y;
z += _z;
intensity += _intensity;
point_count_++;
}
Eigen::Vector4f calc_centroid() const
{
return Eigen::Vector4f(x/point_count_, y/point_count_, z/point_count_, intensity/point_count_);
return Eigen::Vector4f(
x / point_count_, y / point_count_, z / point_count_, intensity / point_count_);
}
};

Expand Down Expand Up @@ -88,4 +97,4 @@ class RoiExcludedFasterVoxelGridDownsampleFilter // renamed class
const TransformInfo & transform_info);
};

} // namespace pointcloud_preprocessor
} // namespace autoware::pointcloud_preprocessor
Original file line number Diff line number Diff line change
Expand Up @@ -12,14 +12,14 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__ROI_EXCLUDED_VOXEL_GRID_DOWNSAMPLE_FILTER_NODELET_HPP_
#define AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__ROI_EXCLUDED_VOXEL_GRID_DOWNSAMPLE_FILTER_NODELET_HPP_
#ifndef AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__ROI_EXCLUDED_VOXEL_GRID_DOWNSAMPLE_FILTER_NODE_HPP_
#define AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__ROI_EXCLUDED_VOXEL_GRID_DOWNSAMPLE_FILTER_NODE_HPP_

#include "autoware/pointcloud_preprocessor/filter.hpp"
#include "autoware/pointcloud_preprocessor/transform_info.hpp"

#include <rclcpp/rclcpp.hpp>
#include <rcl_interfaces/msg/set_parameters_result.hpp>
#include <rclcpp/rclcpp.hpp>

#include <mutex>
#include <vector>
Expand All @@ -35,8 +35,8 @@ class RoiExcludedVoxelGridDownsampleFilterComponent : public Filter
void filter(
const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override;
void faster_filter(
const PointCloud2ConstPtr & input, const IndicesPtr & indices,
PointCloud2 & output, const TransformInfo & transform_info) override;
const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output,
const TransformInfo & transform_info) override;

private:
float voxel_size_x_;
Expand All @@ -52,6 +52,6 @@ class RoiExcludedVoxelGridDownsampleFilterComponent : public Filter
const std::vector<rclcpp::Parameter> & parameters);
};

} // namespace pointcloud_preprocessor
} // namespace autoware::pointcloud_preprocessor

#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__ROI_EXCLUDED_VOXEL_GRID_DOWNSAMPLE_FILTER_NODELET_HPP_
#endif // AUTOWARE__POINTCLOUD_PREPROCESSOR__DOWNSAMPLE_FILTER__ROI_EXCLUDED_VOXEL_GRID_DOWNSAMPLE_FILTER_NODE_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -14,10 +14,10 @@

#include "autoware/pointcloud_preprocessor/downsample_filter/roi_excluded_faster_voxel_grid_downsample_filter.hpp"

#include <Eigen/Dense>
#include <Eigen/Dense>
#include <rclcpp/rclcpp.hpp>

#include <cfloat>
#include <cfloat>

namespace autoware::pointcloud_preprocessor
{
Expand Down Expand Up @@ -89,7 +89,7 @@ bool RoiExcludedFasterVoxelGridDownsampleFilter::get_min_max_voxel(
max_point = max_point.cwiseMax(point.head<3>());
}
}

if (
((static_cast<std::int64_t>((max_point[0] - min_point[0]) * inverse_voxel_size_[0]) + 1) *
(static_cast<std::int64_t>((max_point[1] - min_point[1]) * inverse_voxel_size_[1]) + 1) *
Expand Down Expand Up @@ -235,4 +235,4 @@ void RoiExcludedFasterVoxelGridDownsampleFilter::filter(
output.data.begin() + downsampled_filtered.data.size());
}

Check warning on line 236 in sensing/autoware_pointcloud_preprocessor/src/downsample_filter/roi_excluded_faster_voxel_grid_downsample_filter.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Complex Method

RoiExcludedFasterVoxelGridDownsampleFilter::filter has a cyclomatic complexity of 9, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.

Check warning on line 236 in sensing/autoware_pointcloud_preprocessor/src/downsample_filter/roi_excluded_faster_voxel_grid_downsample_filter.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ New issue: Bumpy Road Ahead

RoiExcludedFasterVoxelGridDownsampleFilter::filter has 2 blocks with nested conditional logic. Any nesting of 2 or deeper is considered. Threshold is one single, nested block per function. The Bumpy Road code smell is a function that contains multiple chunks of nested conditional logic. The deeper the nesting and the more bumps, the lower the code health.

} // namespace pointcloud_preprocessor
} // namespace autoware::pointcloud_preprocessor
Original file line number Diff line number Diff line change
Expand Up @@ -13,8 +13,11 @@
// limitations under the License.

#include "autoware/pointcloud_preprocessor/downsample_filter/roi_excluded_voxel_grid_downsample_filter_node.hpp"

#include "autoware/pointcloud_preprocessor/downsample_filter/roi_excluded_faster_voxel_grid_downsample_filter.hpp"

#include <rclcpp/rclcpp.hpp>

#include <mutex>
#include <vector>

Expand Down Expand Up @@ -49,8 +52,8 @@ void RoiExcludedVoxelGridDownsampleFilterComponent::filter(
}

void RoiExcludedVoxelGridDownsampleFilterComponent::faster_filter(
const PointCloud2ConstPtr & input, const IndicesPtr & indices,
PointCloud2 & output, const TransformInfo & transform_info)
const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output,
const TransformInfo & transform_info)
{
std::scoped_lock lock(mutex_);
if (indices) {
Expand All @@ -63,7 +66,8 @@ void RoiExcludedVoxelGridDownsampleFilterComponent::faster_filter(
roi_excluded_filter.filter(input, output, transform_info, this->get_logger());
}

rcl_interfaces::msg::SetParametersResult RoiExcludedVoxelGridDownsampleFilterComponent::paramCallback(
rcl_interfaces::msg::SetParametersResult
RoiExcludedVoxelGridDownsampleFilterComponent::paramCallback(
const std::vector<rclcpp::Parameter> & parameters)
{
std::scoped_lock lock(mutex_);
Expand Down Expand Up @@ -94,7 +98,8 @@ rcl_interfaces::msg::SetParametersResult RoiExcludedVoxelGridDownsampleFilterCom
return result;
}

} // namespace pointcloud_preprocessor
} // namespace autoware::pointcloud_preprocessor

#include <rclcpp_components/register_node_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(autoware::pointcloud_preprocessor::RoiExcludedVoxelGridDownsampleFilterComponent)
RCLCPP_COMPONENTS_REGISTER_NODE(
autoware::pointcloud_preprocessor::RoiExcludedVoxelGridDownsampleFilterComponent)
3 changes: 2 additions & 1 deletion sensing/autoware_pointcloud_preprocessor/src/filter.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -140,7 +140,8 @@ void autoware::pointcloud_preprocessor::Filter::subscribe(const std::string & fi
// each time a child class supports the faster version.
// When all the child classes support the faster version, this workaround is deleted.
std::set<std::string> supported_nodes = {
"CropBoxFilter", "RingOutlierFilter", "VoxelGridDownsampleFilter", "ScanGroundFilter", "RoiExcludedVoxelGridDownsampleFilter"};
"CropBoxFilter", "RingOutlierFilter", "VoxelGridDownsampleFilter", "ScanGroundFilter",
"RoiExcludedVoxelGridDownsampleFilter"};
auto callback = supported_nodes.find(filter_name) != supported_nodes.end()
? &Filter::faster_input_indices_callback
: &Filter::input_indices_callback;
Expand Down

0 comments on commit 46b11ac

Please sign in to comment.