Skip to content

Commit

Permalink
refactor(autoware_compare_map_segmentation): resolve clang-tidy error…
Browse files Browse the repository at this point in the history
… in autoware_compare_map_segmentation (autowarefoundation#9162)

* refactor(autoware_compare_map_segmentation): resolve clang-tidy error in autoware_compare_map_segmentation

Signed-off-by: Y.Hisaki <[email protected]>

* style(pre-commit): autofix

* include message_filters as SYSTEM

Signed-off-by: Y.Hisaki <[email protected]>

* style(pre-commit): autofix

---------

Signed-off-by: Y.Hisaki <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
  • Loading branch information
yhisaki and pre-commit-ci[bot] authored Oct 29, 2024
1 parent e2dc86e commit c5c0642
Show file tree
Hide file tree
Showing 13 changed files with 36 additions and 47 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -29,9 +29,7 @@
#include <pcl_conversions/pcl_conversions.h>
#include <rcutils/filesystem.h> // To be replaced by std::filesystem in C++17

#include <memory>
#include <string>
#include <utility>

namespace autoware::compare_map_segmentation
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -24,11 +24,7 @@

#include <grid_map_msgs/msg/grid_map.hpp>

#include <iostream>
#include <memory>
#include <mutex>
#include <string>
#include <vector>
namespace autoware::compare_map_segmentation
{
class CompareElevationMapFilterComponent : public autoware::pointcloud_preprocessor::Filter
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -51,10 +51,10 @@ bool DistanceBasedStaticMapLoader::is_close_to_map(
if (!is_initialized_.load(std::memory_order_acquire)) {
return false;
}
if (map_ptr_ == NULL) {
if (map_ptr_ == nullptr) {
return false;
}
if (tree_ == NULL) {
if (tree_ == nullptr) {
return false;
}

Expand Down Expand Up @@ -89,8 +89,8 @@ bool DistanceBasedDynamicMapLoader::is_close_to_map(
if (static_cast<size_t>(map_grid_index) >= current_voxel_grid_array_.size()) {
return false;
}
if (current_voxel_grid_array_.at(map_grid_index) != NULL) {
if (current_voxel_grid_array_.at(map_grid_index)->map_cell_kdtree == NULL) {
if (current_voxel_grid_array_.at(map_grid_index) != nullptr) {
if (current_voxel_grid_array_.at(map_grid_index)->map_cell_kdtree == nullptr) {
return false;
}
std::vector<int> nn_indices(1);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -24,14 +24,13 @@

#include <memory>
#include <string>
#include <vector>

namespace autoware::compare_map_segmentation
{

typedef typename pcl::Filter<pcl::PointXYZ>::PointCloud PointCloud;
typedef typename PointCloud::Ptr PointCloudPtr;
typedef typename PointCloud::ConstPtr PointCloudConstPtr;
using PointCloud = typename pcl::Filter<pcl::PointXYZ>::PointCloud;
using PointCloudPtr = typename PointCloud::Ptr;
using PointCloudConstPtr = typename PointCloud::ConstPtr;

class DistanceBasedStaticMapLoader : public VoxelGridStaticMapLoader
{
Expand Down Expand Up @@ -102,8 +101,8 @@ class DistanceBasedDynamicMapLoader : public VoxelGridDynamicMapLoader
class DistanceBasedCompareMapFilterComponent : public autoware::pointcloud_preprocessor::Filter
{
protected:
virtual void filter(
const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output);
void filter(
const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override;

private:
double distance_threshold_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@ namespace autoware::compare_map_segmentation
bool VoxelBasedApproximateStaticMapLoader::is_close_to_map(
const pcl::PointXYZ & point, [[maybe_unused]] const double distance_threshold)
{
if (voxel_map_ptr_ == NULL) {
if (voxel_map_ptr_ == nullptr) {
return false;
}
const int index =
Expand All @@ -55,7 +55,7 @@ bool VoxelBasedApproximateDynamicMapLoader::is_close_to_map(
if (static_cast<size_t>(map_grid_index) >= current_voxel_grid_array_.size()) {
return false;
}
if (current_voxel_grid_array_.at(map_grid_index) != NULL) {
if (current_voxel_grid_array_.at(map_grid_index) != nullptr) {
const int index = current_voxel_grid_array_.at(map_grid_index)
->map_cell_voxel_grid.getCentroidIndexAt(
current_voxel_grid_array_.at(map_grid_index)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,6 @@

#include <memory>
#include <string>
#include <vector>

namespace autoware::compare_map_segmentation
{
Expand Down Expand Up @@ -59,8 +58,8 @@ class VoxelBasedApproximateCompareMapFilterComponent
: public autoware::pointcloud_preprocessor::Filter
{
protected:
virtual void filter(
const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output);
void filter(
const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override;

private:
double distance_threshold_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -22,11 +22,9 @@
#include <pcl/segmentation/segment_differences.h>

#include <string>
#include <vector>

namespace autoware::compare_map_segmentation
{
using autoware::pointcloud_preprocessor::get_param;

VoxelBasedCompareMapFilterComponent::VoxelBasedCompareMapFilterComponent(
const rclcpp::NodeOptions & options)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -22,15 +22,14 @@
#include <pcl/search/pcl_search.h>

#include <memory>
#include <vector>

namespace autoware::compare_map_segmentation
{
class VoxelBasedCompareMapFilterComponent : public autoware::pointcloud_preprocessor::Filter
{
protected:
virtual void filter(
const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output);
void filter(
const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override;

private:
// pcl::SegmentDifferences<pcl::PointXYZ> impl_;
Expand All @@ -39,8 +38,6 @@ class VoxelBasedCompareMapFilterComponent : public autoware::pointcloud_preproce
double distance_threshold_;
bool set_map_in_voxel_grid_;

bool dynamic_map_load_enable_;

public:
PCL_MAKE_ALIGNED_OPERATOR_NEW
explicit VoxelBasedCompareMapFilterComponent(const rclcpp::NodeOptions & options);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -61,13 +61,13 @@ bool VoxelDistanceBasedStaticMapLoader::is_close_to_map(
if (!is_initialized_.load(std::memory_order_acquire)) {
return false;
}
if (voxel_map_ptr_ == NULL) {
if (voxel_map_ptr_ == nullptr) {
return false;
}
if (map_ptr_ == NULL) {
if (map_ptr_ == nullptr) {
return false;
}
if (tree_ == NULL) {
if (tree_ == nullptr) {
return false;
}
if (is_close_to_neighbor_voxels(point, distance_threshold, voxel_grid_, tree_)) {
Expand All @@ -91,7 +91,7 @@ bool VoxelDistanceBasedDynamicMapLoader::is_close_to_map(
return false;
}
if (
current_voxel_grid_array_.at(map_grid_index) != NULL &&
current_voxel_grid_array_.at(map_grid_index) != nullptr &&
is_close_to_neighbor_voxels(
point, distance_threshold, current_voxel_grid_array_.at(map_grid_index)->map_cell_voxel_grid,
current_voxel_grid_array_.at(map_grid_index)->map_cell_kdtree)) {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -25,14 +25,13 @@
#include <memory>
#include <string>
#include <utility>
#include <vector>

namespace autoware::compare_map_segmentation
{

typedef typename pcl::Filter<pcl::PointXYZ>::PointCloud PointCloud;
typedef typename PointCloud::Ptr PointCloudPtr;
typedef typename PointCloud::ConstPtr PointCloudConstPtr;
using PointCloud = typename pcl::Filter<pcl::PointXYZ>::PointCloud;
using PointCloudPtr = typename PointCloud::Ptr;
using PointCloudConstPtr = typename PointCloud::ConstPtr;

class VoxelDistanceBasedStaticMapLoader : public VoxelGridStaticMapLoader
{
Expand Down Expand Up @@ -124,8 +123,8 @@ class VoxelDistanceBasedDynamicMapLoader : public VoxelGridDynamicMapLoader
class VoxelDistanceBasedCompareMapFilterComponent : public autoware::pointcloud_preprocessor::Filter
{
protected:
virtual void filter(
const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output);
void filter(
const PointCloud2ConstPtr & input, const IndicesPtr & indices, PointCloud2 & output) override;

void input_target_callback(const PointCloud2ConstPtr map);

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,7 @@ bool VoxelGridMapLoader::is_close_to_neighbor_voxels(
if (index != -1) {
return true;
}
if (tree == NULL) {
if (tree == nullptr) {
return false;
}
std::vector<int> nn_indices(1);
Expand All @@ -64,7 +64,7 @@ bool VoxelGridMapLoader::is_close_to_neighbor_voxels(
{
// check map downsampled pc
double distance_threshold_z = downsize_ratio_z_axis_ * distance_threshold;
if (map == NULL) {
if (map == nullptr) {
return false;
}
if (is_in_voxel(
Expand Down Expand Up @@ -331,7 +331,7 @@ bool VoxelGridDynamicMapLoader::is_close_to_next_map_grid(
if (
static_cast<size_t>(neighbor_map_grid_index) >= current_voxel_grid_array_.size() ||
neighbor_map_grid_index == current_map_grid_index ||
current_voxel_grid_array_.at(neighbor_map_grid_index) != NULL) {
current_voxel_grid_array_.at(neighbor_map_grid_index) != nullptr) {
return false;
}
if (is_close_to_neighbor_voxels(
Expand Down Expand Up @@ -360,7 +360,7 @@ bool VoxelGridDynamicMapLoader::is_close_to_map(
return false;
}
if (
current_voxel_grid_array_.at(map_grid_index) != NULL &&
current_voxel_grid_array_.at(map_grid_index) != nullptr &&
is_close_to_neighbor_voxels(
point, distance_threshold, current_voxel_grid_array_.at(map_grid_index)->map_cell_pc_ptr,
current_voxel_grid_array_.at(map_grid_index)->map_cell_voxel_grid)) {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,6 @@
#include <map>
#include <memory>
#include <string>
#include <thread>
#include <utility>
#include <vector>

Expand Down Expand Up @@ -93,13 +92,15 @@ class VoxelGridMapLoader
bool debug_ = false;

public:
typedef VoxelGridEx<pcl::PointXYZ> VoxelGridPointXYZ;
typedef typename pcl::Filter<pcl::PointXYZ>::PointCloud PointCloud;
typedef typename PointCloud::Ptr PointCloudPtr;
using VoxelGridPointXYZ = VoxelGridEx<pcl::PointXYZ>;
using PointCloud = typename pcl::Filter<pcl::PointXYZ>::PointCloud;
using PointCloudPtr = typename PointCloud::Ptr;
explicit VoxelGridMapLoader(
rclcpp::Node * node, double leaf_size, double downsize_ratio_z_axis,
std::string * tf_map_input_frame);

virtual ~VoxelGridMapLoader() = default;

virtual bool is_close_to_map(const pcl::PointXYZ & point, const double distance_threshold) = 0;
static bool is_close_to_neighbor_voxels(
const pcl::PointXYZ & point, const double distance_threshold, VoxelGridPointXYZ & voxel,
Expand Down Expand Up @@ -142,7 +143,7 @@ class VoxelGridDynamicMapLoader : public VoxelGridMapLoader
pcl::search::Search<pcl::PointXYZ>::Ptr map_cell_kdtree;
};

typedef typename std::map<std::string, struct MapGridVoxelInfo> VoxelGridDict;
using VoxelGridDict = typename std::map<std::string, struct MapGridVoxelInfo>;

/** \brief Map to hold loaded map grid id and it's voxel filter */
VoxelGridDict current_voxel_grid_dict_;
Expand Down
2 changes: 2 additions & 0 deletions sensing/autoware_pointcloud_preprocessor/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,7 @@ target_include_directories(pointcloud_preprocessor_filter_base PUBLIC
)

ament_target_dependencies(pointcloud_preprocessor_filter_base
SYSTEM
message_filters
pcl_conversions
rclcpp
Expand All @@ -56,6 +57,7 @@ target_include_directories(faster_voxel_grid_downsample_filter PUBLIC
)

ament_target_dependencies(faster_voxel_grid_downsample_filter
SYSTEM
pcl_conversions
rclcpp
sensor_msgs
Expand Down

0 comments on commit c5c0642

Please sign in to comment.