diff --git a/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml b/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml index cddee782e8af0..37b77eb436c7d 100644 --- a/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml +++ b/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml @@ -12,5 +12,7 @@ #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN [800.0, 800.0, 800.0, 800.0, 800.0, 800.0, 800.0, 800.0] + validate_max_distance_m: 70.0 # [m] + using_2d_validator: false enable_debugger: false diff --git a/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp b/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp index 47685dec7dbdc..ce7386bbf4f91 100644 --- a/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp +++ b/perception/detected_object_validation/include/detected_object_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp @@ -141,6 +141,7 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node typedef message_filters::Synchronizer Sync; Sync sync_; PointsNumThresholdParam points_num_threshold_param_; + double validate_max_distance_sq_; // maximum object distance to validate, squared [m^2] std::shared_ptr debugger_; bool using_2d_validator_; diff --git a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp index fbb3f863be7c4..831e8f0b6033d 100644 --- a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp +++ b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp @@ -288,6 +288,8 @@ ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator( declare_parameter>("max_points_num"); points_num_threshold_param_.min_points_and_distance_ratio = declare_parameter>("min_points_and_distance_ratio"); + const double validate_max_distance = declare_parameter("validate_max_distance_m"); + validate_max_distance_sq_ = validate_max_distance * validate_max_distance; using_2d_validator_ = declare_parameter("using_2d_validator"); @@ -339,6 +341,18 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud( for (size_t i = 0; i < transformed_objects.objects.size(); ++i) { const auto & transformed_object = transformed_objects.objects.at(i); const auto & object = input_objects->objects.at(i); + // check object distance + const double distance_sq = + transformed_object.kinematics.pose_with_covariance.pose.position.x * + transformed_object.kinematics.pose_with_covariance.pose.position.x + + transformed_object.kinematics.pose_with_covariance.pose.position.y * + transformed_object.kinematics.pose_with_covariance.pose.position.y; + if (distance_sq > validate_max_distance_sq_) { + // pass to output + output.objects.push_back(object); + continue; + } + const auto validated = validation_is_ready ? validator_->validate_object(transformed_object) : false; if (debugger_) {