Skip to content

Commit d091381

Browse files
interimaddTakahisa.Ishikawa
andauthored
feat(crop_box_filter): simplify validation logic for input point cloud (#875)
* feat(crop_box_filter): simplify point cloud validation logic and remove redundant functions Signed-off-by: Takahisa.Ishikawa <takahisa.ishikawa@tier4.jp> * feat(crop_box_filter): remove dependency on rclcpp::Logger from validation logic Signed-off-by: Takahisa.Ishikawa <takahisa.ishikawa@tier4.jp> * refactor(crop_box_filter): rename validation function Signed-off-by: Takahisa.Ishikawa <takahisa.ishikawa@tier4.jp> * refactor(crop_box_filter): simplify point cloud validation tests and improve clarity Signed-off-by: Takahisa.Ishikawa <takahisa.ishikawa@tier4.jp> * refactor(crop_box_filter): simplify point cloud validation logic by removing redundant lambda function Signed-off-by: Takahisa.Ishikawa <takahisa.ishikawa@tier4.jp> * refactor(crop_box_filter): separate validation logic into its own files Signed-off-by: Takahisa.Ishikawa <takahisa.ishikawa@tier4.jp> * refactor(crop_box_filter): remove rclcpp dependency and simplify timestamp handling in validation logic Signed-off-by: Takahisa.Ishikawa <takahisa.ishikawa@tier4.jp> * chore(crop_box_filter): update copyright information to reflect current ownership Signed-off-by: Takahisa.Ishikawa <takahisa.ishikawa@tier4.jp> * refactor(crop_box_filter): restructure validation logic by separating xyz field and data size checks Signed-off-by: Takahisa.Ishikawa <takahisa.ishikawa@tier4.jp> * refactor(test): remove rclcpp dependency from test file Signed-off-by: Takahisa.Ishikawa <takahisa.ishikawa@tier4.jp> * refactor(crop_box_filter): update validation functions to accept PointCloud2 Signed-off-by: Takahisa.Ishikawa <takahisa.ishikawa@tier4.jp> * refactor(crop_box_filter): improve error message comment Signed-off-by: Takahisa.Ishikawa <takahisa.ishikawa@tier4.jp> * refactor(crop_box_filter): simplify PointCloud2 test cases by removing shared_ptr usage Signed-off-by: Takahisa.Ishikawa <takahisa.ishikawa@tier4.jp> * feat(crop_box_filter): update data size validation logic to use greater than comparison Signed-off-by: Takahisa.Ishikawa <takahisa.ishikawa@tier4.jp> * refactor(crop_box_filter): streamline validation logic for x, y, z fields in PointCloud2 Signed-off-by: Takahisa.Ishikawa <takahisa.ishikawa@tier4.jp> * refactor(crop_box_filter): remove unused memory header from crop_box_filter.cpp Signed-off-by: Takahisa.Ishikawa <takahisa.ishikawa@tier4.jp> --------- Signed-off-by: Takahisa.Ishikawa <takahisa.ishikawa@tier4.jp> Co-authored-by: Takahisa.Ishikawa <takahisa.ishikawa@tier4.jp>
1 parent e232aec commit d091381

File tree

6 files changed

+219
-255
lines changed

6 files changed

+219
-255
lines changed

sensing/autoware_crop_box_filter/CMakeLists.txt

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,7 @@ include_directories(
1212
)
1313

1414
ament_auto_add_library(crop_box_filter_node SHARED
15+
src/crop_box_filter.cpp
1516
src/crop_box_filter_node.cpp
1617
)
1718

@@ -21,6 +22,11 @@ rclcpp_components_register_node(crop_box_filter_node
2122

2223
if(BUILD_TESTING)
2324
find_package(ament_cmake_gtest REQUIRED)
25+
ament_auto_add_gtest(test_crop_box_filter
26+
test/test_crop_box_filter.cpp
27+
)
28+
target_include_directories(test_crop_box_filter PRIVATE src)
29+
2430
ament_auto_add_gtest(test_crop_box_filter_node
2531
test/test_crop_box_filter_node.cpp
2632
)
Lines changed: 78 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,78 @@
1+
// Copyright 2026 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "crop_box_filter.hpp"
16+
17+
#include <sensor_msgs/msg/point_field.hpp>
18+
19+
#include <sstream>
20+
21+
namespace autoware::crop_box_filter
22+
{
23+
24+
static ValidationResult validate_xyz_fields(const PointCloud2 & cloud)
25+
{
26+
bool has_x = false;
27+
bool has_y = false;
28+
bool has_z = false;
29+
30+
for (const auto & field : cloud.fields) {
31+
if (field.datatype != sensor_msgs::msg::PointField::FLOAT32) {
32+
continue;
33+
}
34+
35+
if (field.name == "x") {
36+
has_x = true;
37+
} else if (field.name == "y") {
38+
has_y = true;
39+
} else if (field.name == "z") {
40+
has_z = true;
41+
}
42+
43+
if (has_x && has_y && has_z) break;
44+
}
45+
46+
if (!has_x || !has_y || !has_z) {
47+
return {false, "The pointcloud does not have the required x, y, z FLOAT32 fields."};
48+
}
49+
return {true, ""};
50+
}
51+
52+
static ValidationResult validate_data_size(const PointCloud2 & cloud)
53+
{
54+
if (cloud.width * cloud.height * cloud.point_step > cloud.data.size()) {
55+
std::ostringstream oss;
56+
oss << "Invalid PointCloud (data.size = " << cloud.data.size() << ", width = " << cloud.width
57+
<< ", height = " << cloud.height << ", step = " << cloud.point_step << ") with stamp "
58+
<< cloud.header.stamp.sec + cloud.header.stamp.nanosec * 1e-9 << ", and frame "
59+
<< cloud.header.frame_id << " received!";
60+
return {false, oss.str()};
61+
}
62+
return {true, ""};
63+
}
64+
65+
ValidationResult validate_pointcloud2(const PointCloud2 & cloud)
66+
{
67+
const ValidationResult xyz_validation_result = validate_xyz_fields(cloud);
68+
if (!xyz_validation_result.is_valid) {
69+
return xyz_validation_result;
70+
}
71+
const ValidationResult data_size_validation_result = validate_data_size(cloud);
72+
if (!data_size_validation_result.is_valid) {
73+
return data_size_validation_result;
74+
}
75+
return {true, ""};
76+
}
77+
78+
} // namespace autoware::crop_box_filter
Lines changed: 38 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,38 @@
1+
// Copyright 2026 TIER IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#ifndef CROP_BOX_FILTER_HPP_
16+
#define CROP_BOX_FILTER_HPP_
17+
18+
#include <sensor_msgs/msg/point_cloud2.hpp>
19+
20+
#include <string>
21+
22+
using PointCloud2 = sensor_msgs::msg::PointCloud2;
23+
using PointCloud2ConstPtr = sensor_msgs::msg::PointCloud2::ConstSharedPtr;
24+
25+
namespace autoware::crop_box_filter
26+
{
27+
28+
struct ValidationResult
29+
{
30+
bool is_valid;
31+
std::string reason;
32+
};
33+
34+
ValidationResult validate_pointcloud2(const PointCloud2 & cloud);
35+
36+
} // namespace autoware::crop_box_filter
37+
38+
#endif // CROP_BOX_FILTER_HPP_

sensing/autoware_crop_box_filter/src/crop_box_filter_node.cpp

Lines changed: 3 additions & 232 deletions
Original file line numberDiff line numberDiff line change
@@ -230,8 +230,9 @@ void CropBoxFilter::filter_pointcloud(const PointCloud2ConstPtr & cloud, PointCl
230230
void CropBoxFilter::pointcloud_callback(const PointCloud2ConstPtr cloud)
231231
{
232232
// check whether the pointcloud is valid
233-
if (!is_valid(cloud)) {
234-
RCLCPP_ERROR(this->get_logger(), "[input_pointcloud_callback] Invalid input pointcloud!");
233+
const ValidationResult result = validate_pointcloud2(*cloud);
234+
if (!result.is_valid) {
235+
RCLCPP_ERROR(this->get_logger(), "[input_pointcloud_callback] %s", result.reason.c_str());
235236
return;
236237
}
237238

@@ -356,236 +357,6 @@ rcl_interfaces::msg::SetParametersResult CropBoxFilter::param_callback(
356357
return result;
357358
}
358359

359-
bool CropBoxFilter::is_data_layout_compatible_with_point_xyzi(const PointCloud2 & input)
360-
{
361-
using PointIndex = autoware::point_types::PointXYZIIndex;
362-
using autoware::point_types::PointXYZI;
363-
if (input.fields.size() < 4) {
364-
return false;
365-
}
366-
bool same_layout = true;
367-
const auto & field_x = input.fields.at(static_cast<size_t>(PointIndex::X));
368-
same_layout &= field_x.name == "x";
369-
same_layout &= field_x.offset == offsetof(PointXYZI, x);
370-
same_layout &= field_x.datatype == sensor_msgs::msg::PointField::FLOAT32;
371-
same_layout &= field_x.count == 1;
372-
const auto & field_y = input.fields.at(static_cast<size_t>(PointIndex::Y));
373-
same_layout &= field_y.name == "y";
374-
same_layout &= field_y.offset == offsetof(PointXYZI, y);
375-
same_layout &= field_y.datatype == sensor_msgs::msg::PointField::FLOAT32;
376-
same_layout &= field_y.count == 1;
377-
const auto & field_z = input.fields.at(static_cast<size_t>(PointIndex::Z));
378-
same_layout &= field_z.name == "z";
379-
same_layout &= field_z.offset == offsetof(PointXYZI, z);
380-
same_layout &= field_z.datatype == sensor_msgs::msg::PointField::FLOAT32;
381-
same_layout &= field_z.count == 1;
382-
const auto & field_intensity = input.fields.at(static_cast<size_t>(PointIndex::Intensity));
383-
same_layout &= field_intensity.name == "intensity";
384-
same_layout &= field_intensity.offset == offsetof(PointXYZI, intensity);
385-
same_layout &= field_intensity.datatype == sensor_msgs::msg::PointField::FLOAT32;
386-
same_layout &= field_intensity.count == 1;
387-
return same_layout;
388-
}
389-
390-
bool CropBoxFilter::is_data_layout_compatible_with_point_xyzirc(const PointCloud2 & input)
391-
{
392-
using PointIndex = autoware::point_types::PointXYZIRCIndex;
393-
using autoware::point_types::PointXYZIRC;
394-
if (input.fields.size() < 6) {
395-
return false;
396-
}
397-
bool same_layout = true;
398-
const auto & field_x = input.fields.at(static_cast<size_t>(PointIndex::X));
399-
same_layout &= field_x.name == "x";
400-
same_layout &= field_x.offset == offsetof(PointXYZIRC, x);
401-
same_layout &= field_x.datatype == sensor_msgs::msg::PointField::FLOAT32;
402-
same_layout &= field_x.count == 1;
403-
const auto & field_y = input.fields.at(static_cast<size_t>(PointIndex::Y));
404-
same_layout &= field_y.name == "y";
405-
same_layout &= field_y.offset == offsetof(PointXYZIRC, y);
406-
same_layout &= field_y.datatype == sensor_msgs::msg::PointField::FLOAT32;
407-
same_layout &= field_y.count == 1;
408-
const auto & field_z = input.fields.at(static_cast<size_t>(PointIndex::Z));
409-
same_layout &= field_z.name == "z";
410-
same_layout &= field_z.offset == offsetof(PointXYZIRC, z);
411-
same_layout &= field_z.datatype == sensor_msgs::msg::PointField::FLOAT32;
412-
same_layout &= field_z.count == 1;
413-
const auto & field_intensity = input.fields.at(static_cast<size_t>(PointIndex::Intensity));
414-
same_layout &= field_intensity.name == "intensity";
415-
same_layout &= field_intensity.offset == offsetof(PointXYZIRC, intensity);
416-
same_layout &= field_intensity.datatype == sensor_msgs::msg::PointField::UINT8;
417-
same_layout &= field_intensity.count == 1;
418-
const auto & field_return_type = input.fields.at(static_cast<size_t>(PointIndex::ReturnType));
419-
same_layout &= field_return_type.name == "return_type";
420-
same_layout &= field_return_type.offset == offsetof(PointXYZIRC, return_type);
421-
same_layout &= field_return_type.datatype == sensor_msgs::msg::PointField::UINT8;
422-
same_layout &= field_return_type.count == 1;
423-
const auto & field_ring = input.fields.at(static_cast<size_t>(PointIndex::Channel));
424-
same_layout &= field_ring.name == "channel";
425-
same_layout &= field_ring.offset == offsetof(PointXYZIRC, channel);
426-
same_layout &= field_ring.datatype == sensor_msgs::msg::PointField::UINT16;
427-
same_layout &= field_ring.count == 1;
428-
429-
return same_layout;
430-
}
431-
432-
bool CropBoxFilter::is_data_layout_compatible_with_point_xyziradrt(const PointCloud2 & input)
433-
{
434-
using PointIndex = autoware::point_types::PointXYZIRADRTIndex;
435-
using autoware::point_types::PointXYZIRADRT;
436-
if (input.fields.size() < 9) {
437-
return false;
438-
}
439-
bool same_layout = true;
440-
const auto & field_x = input.fields.at(static_cast<size_t>(PointIndex::X));
441-
same_layout &= field_x.name == "x";
442-
same_layout &= field_x.offset == offsetof(PointXYZIRADRT, x);
443-
same_layout &= field_x.datatype == sensor_msgs::msg::PointField::FLOAT32;
444-
same_layout &= field_x.count == 1;
445-
const auto & field_y = input.fields.at(static_cast<size_t>(PointIndex::Y));
446-
same_layout &= field_y.name == "y";
447-
same_layout &= field_y.offset == offsetof(PointXYZIRADRT, y);
448-
same_layout &= field_y.datatype == sensor_msgs::msg::PointField::FLOAT32;
449-
same_layout &= field_y.count == 1;
450-
const auto & field_z = input.fields.at(static_cast<size_t>(PointIndex::Z));
451-
same_layout &= field_z.name == "z";
452-
same_layout &= field_z.offset == offsetof(PointXYZIRADRT, z);
453-
same_layout &= field_z.datatype == sensor_msgs::msg::PointField::FLOAT32;
454-
same_layout &= field_z.count == 1;
455-
const auto & field_intensity = input.fields.at(static_cast<size_t>(PointIndex::Intensity));
456-
same_layout &= field_intensity.name == "intensity";
457-
same_layout &= field_intensity.offset == offsetof(PointXYZIRADRT, intensity);
458-
same_layout &= field_intensity.datatype == sensor_msgs::msg::PointField::FLOAT32;
459-
same_layout &= field_intensity.count == 1;
460-
const auto & field_ring = input.fields.at(static_cast<size_t>(PointIndex::Ring));
461-
same_layout &= field_ring.name == "ring";
462-
same_layout &= field_ring.offset == offsetof(PointXYZIRADRT, ring);
463-
same_layout &= field_ring.datatype == sensor_msgs::msg::PointField::UINT16;
464-
same_layout &= field_ring.count == 1;
465-
const auto & field_azimuth = input.fields.at(static_cast<size_t>(PointIndex::Azimuth));
466-
same_layout &= field_azimuth.name == "azimuth";
467-
same_layout &= field_azimuth.offset == offsetof(PointXYZIRADRT, azimuth);
468-
same_layout &= field_azimuth.datatype == sensor_msgs::msg::PointField::FLOAT32;
469-
same_layout &= field_azimuth.count == 1;
470-
const auto & field_distance = input.fields.at(static_cast<size_t>(PointIndex::Distance));
471-
same_layout &= field_distance.name == "distance";
472-
same_layout &= field_distance.offset == offsetof(PointXYZIRADRT, distance);
473-
same_layout &= field_distance.datatype == sensor_msgs::msg::PointField::FLOAT32;
474-
same_layout &= field_distance.count == 1;
475-
const auto & field_return_type = input.fields.at(static_cast<size_t>(PointIndex::ReturnType));
476-
same_layout &= field_return_type.name == "return_type";
477-
same_layout &= field_return_type.offset == offsetof(PointXYZIRADRT, return_type);
478-
same_layout &= field_return_type.datatype == sensor_msgs::msg::PointField::UINT8;
479-
same_layout &= field_return_type.count == 1;
480-
const auto & field_time_stamp = input.fields.at(static_cast<size_t>(PointIndex::TimeStamp));
481-
same_layout &= field_time_stamp.name == "time_stamp";
482-
same_layout &= field_time_stamp.offset == offsetof(PointXYZIRADRT, time_stamp);
483-
same_layout &= field_time_stamp.datatype == sensor_msgs::msg::PointField::FLOAT64;
484-
same_layout &= field_time_stamp.count == 1;
485-
return same_layout;
486-
}
487-
488-
bool CropBoxFilter::is_data_layout_compatible_with_point_xyzircaedt(const PointCloud2 & input)
489-
{
490-
using PointIndex = autoware::point_types::PointXYZIRCAEDTIndex;
491-
using autoware::point_types::PointXYZIRCAEDT;
492-
if (input.fields.size() != 10) {
493-
return false;
494-
}
495-
bool same_layout = true;
496-
const auto & field_x = input.fields.at(static_cast<size_t>(PointIndex::X));
497-
same_layout &= field_x.name == "x";
498-
same_layout &= field_x.offset == offsetof(PointXYZIRCAEDT, x);
499-
same_layout &= field_x.datatype == sensor_msgs::msg::PointField::FLOAT32;
500-
same_layout &= field_x.count == 1;
501-
const auto & field_y = input.fields.at(static_cast<size_t>(PointIndex::Y));
502-
same_layout &= field_y.name == "y";
503-
same_layout &= field_y.offset == offsetof(PointXYZIRCAEDT, y);
504-
same_layout &= field_y.datatype == sensor_msgs::msg::PointField::FLOAT32;
505-
same_layout &= field_y.count == 1;
506-
const auto & field_z = input.fields.at(static_cast<size_t>(PointIndex::Z));
507-
same_layout &= field_z.name == "z";
508-
same_layout &= field_z.offset == offsetof(PointXYZIRCAEDT, z);
509-
same_layout &= field_z.datatype == sensor_msgs::msg::PointField::FLOAT32;
510-
same_layout &= field_z.count == 1;
511-
const auto & field_intensity = input.fields.at(static_cast<size_t>(PointIndex::Intensity));
512-
same_layout &= field_intensity.name == "intensity";
513-
same_layout &= field_intensity.offset == offsetof(PointXYZIRCAEDT, intensity);
514-
same_layout &= field_intensity.datatype == sensor_msgs::msg::PointField::UINT8;
515-
same_layout &= field_intensity.count == 1;
516-
const auto & field_return_type = input.fields.at(static_cast<size_t>(PointIndex::ReturnType));
517-
same_layout &= field_return_type.name == "return_type";
518-
same_layout &= field_return_type.offset == offsetof(PointXYZIRCAEDT, return_type);
519-
same_layout &= field_return_type.datatype == sensor_msgs::msg::PointField::UINT8;
520-
same_layout &= field_return_type.count == 1;
521-
const auto & field_ring = input.fields.at(static_cast<size_t>(PointIndex::Channel));
522-
same_layout &= field_ring.name == "channel";
523-
same_layout &= field_ring.offset == offsetof(PointXYZIRCAEDT, channel);
524-
same_layout &= field_ring.datatype == sensor_msgs::msg::PointField::UINT16;
525-
same_layout &= field_ring.count == 1;
526-
const auto & field_azimuth = input.fields.at(static_cast<size_t>(PointIndex::Azimuth));
527-
same_layout &= field_azimuth.name == "azimuth";
528-
same_layout &= field_azimuth.offset == offsetof(PointXYZIRCAEDT, azimuth);
529-
same_layout &= field_azimuth.datatype == sensor_msgs::msg::PointField::FLOAT32;
530-
same_layout &= field_azimuth.count == 1;
531-
const auto & field_elevation = input.fields.at(static_cast<size_t>(PointIndex::Elevation));
532-
same_layout &= field_elevation.name == "elevation";
533-
same_layout &= field_elevation.offset == offsetof(PointXYZIRCAEDT, elevation);
534-
same_layout &= field_elevation.datatype == sensor_msgs::msg::PointField::FLOAT32;
535-
same_layout &= field_elevation.count == 1;
536-
const auto & field_distance = input.fields.at(static_cast<size_t>(PointIndex::Distance));
537-
same_layout &= field_distance.name == "distance";
538-
same_layout &= field_distance.offset == offsetof(PointXYZIRCAEDT, distance);
539-
same_layout &= field_distance.datatype == sensor_msgs::msg::PointField::FLOAT32;
540-
same_layout &= field_distance.count == 1;
541-
const auto & field_time_stamp = input.fields.at(static_cast<size_t>(PointIndex::TimeStamp));
542-
same_layout &= field_time_stamp.name == "time_stamp";
543-
same_layout &= field_time_stamp.offset == offsetof(PointXYZIRCAEDT, time_stamp);
544-
same_layout &= field_time_stamp.datatype == sensor_msgs::msg::PointField::UINT32;
545-
same_layout &= field_time_stamp.count == 1;
546-
return same_layout;
547-
}
548-
549-
bool CropBoxFilter::is_valid(const PointCloud2ConstPtr & cloud)
550-
{
551-
// firstly check the fields of the point cloud
552-
if (
553-
!is_data_layout_compatible_with_point_xyzircaedt(*cloud) &&
554-
!is_data_layout_compatible_with_point_xyzirc(*cloud)) {
555-
RCLCPP_ERROR(
556-
get_logger(),
557-
"The pointcloud layout is not compatible with PointXYZIRCAEDT or PointXYZIRC. Aborting");
558-
559-
if (is_data_layout_compatible_with_point_xyziradrt(*cloud)) {
560-
RCLCPP_ERROR(
561-
get_logger(),
562-
"The pointcloud layout is compatible with PointXYZIRADRT. You may be using legacy "
563-
"code/data");
564-
}
565-
566-
if (is_data_layout_compatible_with_point_xyzi(*cloud)) {
567-
RCLCPP_ERROR(
568-
get_logger(),
569-
"The pointcloud layout is compatible with PointXYZI. You may be using legacy "
570-
"code/data");
571-
}
572-
573-
return false;
574-
}
575-
576-
// secondly, verify the total size of the point cloud
577-
if (cloud->width * cloud->height * cloud->point_step != cloud->data.size()) {
578-
RCLCPP_WARN(
579-
this->get_logger(),
580-
"Invalid PointCloud (data = %zu, width = %d, height = %d, step = %d) with stamp %f, "
581-
"and frame %s received!",
582-
cloud->data.size(), cloud->width, cloud->height, cloud->point_step,
583-
rclcpp::Time(cloud->header.stamp).seconds(), cloud->header.frame_id.c_str());
584-
return false;
585-
}
586-
return true;
587-
}
588-
589360
} // namespace autoware::crop_box_filter
590361

591362
#include <rclcpp_components/register_node_macro.hpp>

0 commit comments

Comments
 (0)