diff --git a/perception/autoware_detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml b/perception/autoware_detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml index cddee782e8af0..37b77eb436c7d 100644 --- a/perception/autoware_detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml +++ b/perception/autoware_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/autoware_detected_object_validation/schema/obstacle_pointcloud_based_validator.schema.json b/perception/autoware_detected_object_validation/schema/obstacle_pointcloud_based_validator.schema.json index d7aa970993ca1..95c83a90f075f 100644 --- a/perception/autoware_detected_object_validation/schema/obstacle_pointcloud_based_validator.schema.json +++ b/perception/autoware_detected_object_validation/schema/obstacle_pointcloud_based_validator.schema.json @@ -30,6 +30,11 @@ "default": [800, 800, 800, 800, 800, 800, 800, 800], "description": "Threshold value of the number of point clouds per object when the distance from baselink is 1m, because the number of point clouds varies with the distance from baselink." }, + "validate_max_distance_m": { + "type": "number", + "default": 70.0, + "description": "The maximum distance from the baselink to the object to be validated" + }, "using_2d_validator": { "type": "boolean", "default": false, diff --git a/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp index 194f333acd87e..979cdd3909f14 100644 --- a/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp +++ b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp @@ -295,6 +295,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"); @@ -346,6 +348,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_) { diff --git a/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp index 6d9a11634ea08..f10dbb7531d36 100644 --- a/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp +++ b/perception/autoware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp @@ -152,6 +152,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_;