Skip to content

Commit

Permalink
style(pre-commit): autofix
Browse files Browse the repository at this point in the history
Signed-off-by: Taiga Takano <[email protected]>
  • Loading branch information
pre-commit-ci[bot] authored and takanotaiga committed Dec 27, 2023
1 parent dbf7f93 commit 7726ae6
Showing 1 changed file with 21 additions and 11 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -135,7 +135,8 @@ void RadiusSearch2dFilter::filter(
const int min_points_threshold = std::min(
std::max(static_cast<int>(min_points_and_distance_ratio_ / distance + 0.5f), min_points_),
max_points_);
const int points_num = kd_tree_->nearestKSearch(i, min_points_threshold, k_indices, k_distances);
const int points_num =
kd_tree_->nearestKSearch(i, min_points_threshold, k_indices, k_distances);

Check warning on line 139 in perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp

View check run for this annotation

Codecov / codecov/patch

perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp#L139

Added line #L139 was not covered by tests

if (min_points_threshold <= points_num) {
output.points.push_back(xyz_cloud.points.at(i));
Expand Down Expand Up @@ -169,20 +170,25 @@ void RadiusSearch2dFilter::filter(
std::vector<float> k_distances_low(low_conf_xy_cloud->points.size());
kd_tree_->setInputCloud(low_conf_xy_cloud);
for (size_t i = 0; i < low_conf_input.points.size(); ++i) {
const float distance = std::hypot(low_conf_xy_cloud->points[i].x - pose.position.x, low_conf_xy_cloud->points[i].y - pose.position.y);
const float distance = std::hypot(
low_conf_xy_cloud->points[i].x - pose.position.x,
low_conf_xy_cloud->points[i].y - pose.position.y);

Check warning on line 175 in perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp

View check run for this annotation

Codecov / codecov/patch

perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp#L169-L175

Added lines #L169 - L175 were not covered by tests
const int min_points_threshold = std::min(
std::max(static_cast<int>(min_points_and_distance_ratio_ / distance + 0.5f), min_points_),
max_points_);

Check warning on line 178 in perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp

View check run for this annotation

Codecov / codecov/patch

perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp#L177-L178

Added lines #L177 - L178 were not covered by tests
const int points_num = kd_tree_->nearestKSearch(i, min_points_threshold, k_indices_low, k_distances_low);
const int points_num =
kd_tree_->nearestKSearch(i, min_points_threshold, k_indices_low, k_distances_low);
if (min_points_threshold <= points_num) {
output.points.push_back(low_conf_input.points.at(i));

Check warning on line 182 in perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp

View check run for this annotation

Codecov / codecov/patch

perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp#L180-L182

Added lines #L180 - L182 were not covered by tests
} else {
outlier.points.push_back(low_conf_input.points.at(i));

Check warning on line 184 in perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp

View check run for this annotation

Codecov / codecov/patch

perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp#L184

Added line #L184 was not covered by tests
}
}

// High conf cloud check
if(outlier.points.size() == 0){return;}
// High conf cloud check
if (outlier.points.size() == 0) {

Check warning on line 189 in perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp

View check run for this annotation

Codecov / codecov/patch

perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp#L189

Added line #L189 was not covered by tests
return;
}

pcl::PointCloud<pcl::PointXY>::Ptr high_conf_xy_cloud(new pcl::PointCloud<pcl::PointXY>);
high_conf_xy_cloud->points.resize(high_conf_input.points.size());
Expand All @@ -195,11 +201,14 @@ void RadiusSearch2dFilter::filter(
std::vector<float> k_distances_high(high_conf_xy_cloud->points.size());
kd_tree_->setInputCloud(high_conf_xy_cloud);
for (size_t i = 0; i < outlier.points.size(); ++i) {
const float distance = std::hypot(high_conf_xy_cloud->points[i].x - pose.position.x, high_conf_xy_cloud->points[i].y - pose.position.y);
const float distance = std::hypot(
high_conf_xy_cloud->points[i].x - pose.position.x,
high_conf_xy_cloud->points[i].y - pose.position.y);

Check warning on line 206 in perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp

View check run for this annotation

Codecov / codecov/patch

perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp#L200-L206

Added lines #L200 - L206 were not covered by tests
const int min_points_threshold = std::min(
std::max(static_cast<int>(min_points_and_distance_ratio_ / distance + 0.5f), min_points_),
max_points_);
const int points_num = kd_tree_->nearestKSearch(i, min_points_threshold, k_indices_high, k_distances_high);
const int points_num =
kd_tree_->nearestKSearch(i, min_points_threshold, k_indices_high, k_distances_high);

Check warning on line 211 in perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp

View check run for this annotation

Codecov / codecov/patch

perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp#L211

Added line #L211 was not covered by tests

if (min_points_threshold <= points_num) {
output.points.push_back(outlier.points.at(i));

Check warning on line 214 in perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp

View check run for this annotation

Codecov / codecov/patch

perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp#L214

Added line #L214 was not covered by tests
Expand Down Expand Up @@ -259,7 +268,7 @@ void OccupancyGridMapOutlierFilterComponent::splitPointCloudFrontBack(
{
size_t front_count = 0;
size_t behind_count = 0;

for (sensor_msgs::PointCloud2ConstIterator<float> x(*input_pc, "x"); x != x.end(); ++x) {

Check warning on line 272 in perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp

View check run for this annotation

Codecov / codecov/patch

perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp#L272

Added line #L272 was not covered by tests
if (*x < 0.0) {
behind_count++;

Check warning on line 274 in perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp

View check run for this annotation

Codecov / codecov/patch

perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp#L274

Added line #L274 was not covered by tests
Expand All @@ -270,15 +279,16 @@ void OccupancyGridMapOutlierFilterComponent::splitPointCloudFrontBack(

sensor_msgs::PointCloud2Modifier front_pc_modfier(front_pc);

Check warning on line 280 in perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp

View workflow job for this annotation

GitHub Actions / spell-check-differential

Unknown word (modfier)
sensor_msgs::PointCloud2Modifier behind_pc_modfier(behind_pc);

Check warning on line 281 in perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp

View workflow job for this annotation

GitHub Actions / spell-check-differential

Unknown word (modfier)
front_pc_modfier.setPointCloud2FieldsByString(1,"xyz");
behind_pc_modfier.setPointCloud2FieldsByString(1,"xyz");
front_pc_modfier.setPointCloud2FieldsByString(1, "xyz");

Check warning on line 282 in perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp

View workflow job for this annotation

GitHub Actions / spell-check-differential

Unknown word (modfier)
behind_pc_modfier.setPointCloud2FieldsByString(1, "xyz");

Check warning on line 283 in perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp

View workflow job for this annotation

GitHub Actions / spell-check-differential

Unknown word (modfier)
front_pc_modfier.resize(front_count);

Check warning on line 284 in perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp

View workflow job for this annotation

GitHub Actions / spell-check-differential

Unknown word (modfier)
behind_pc_modfier.resize(behind_count);

Check warning on line 285 in perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp

View check run for this annotation

Codecov / codecov/patch

perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp#L282-L285

Added lines #L282 - L285 were not covered by tests

sensor_msgs::PointCloud2Iterator<float> fr_iter(front_pc, "x");
sensor_msgs::PointCloud2Iterator<float> be_iter(behind_pc, "x");

Check warning on line 288 in perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp

View check run for this annotation

Codecov / codecov/patch

perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp#L287-L288

Added lines #L287 - L288 were not covered by tests

for (sensor_msgs::PointCloud2ConstIterator<float> in_iter(*input_pc, "x"); in_iter != in_iter.end(); ++in_iter) {
for (sensor_msgs::PointCloud2ConstIterator<float> in_iter(*input_pc, "x");
in_iter != in_iter.end(); ++in_iter) {
if (*in_iter < 0.0) {
*be_iter = in_iter[0];
be_iter[1] = in_iter[1];
Expand Down

0 comments on commit 7726ae6

Please sign in to comment.