Merge pull request #1750 from saka1-s/feat/obst-point-validator/set-v…

…alidate-range/x2_gen2/v0.29.2 feat(autoware_detected_object_validation): set validate distance in t…
tier4 · Jan 10, 2025 · 1f2b4d3 · 1f2b4d3
2 parents 726c04e + ab61816
commit 1f2b4d3
Show file tree

Hide file tree

Showing 3 changed files with 17 additions and 0 deletions.
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
@@ -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/...ct_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp b/...ct_validation/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp
@@ -141,6 +141,7 @@ class ObstaclePointCloudBasedValidator : public rclcpp::Node
   typedef message_filters::Synchronizer<SyncPolicy> 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> 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
@@ -288,6 +288,8 @@ ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator(
     declare_parameter<std::vector<int64_t>>("max_points_num");
   points_num_threshold_param_.min_points_and_distance_ratio =
     declare_parameter<std::vector<double>>("min_points_and_distance_ratio");
+  const double validate_max_distance = declare_parameter<double>("validate_max_distance_m");
+  validate_max_distance_sq_ = validate_max_distance * validate_max_distance;
 
   using_2d_validator_ = declare_parameter<bool>("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_) {