feat(autoware_detected_object_validation): set validate distance in t…

…he obstacle pointcloud based validator (autowarefoundation#9663) * chore: add validate_max_distance_m parameter for obstacle_pointcloud_based_validator Signed-off-by: Taekjin LEE <[email protected]> * chore: optimize object distance validation in obstacle_pointcloud_validator Signed-off-by: Taekjin LEE <[email protected]> * chore: add validate_max_distance_m parameter for obstacle_pointcloud_based_validator Signed-off-by: Taekjin LEE <[email protected]> --------- Signed-off-by: Taekjin LEE <[email protected]>
NorahXiong · Dec 17, 2024 · 178b6c0 · 178b6c0
1 parent fc2b93e
commit 178b6c0
Show file tree

Hide file tree

Showing 4 changed files with 22 additions and 0 deletions.
diff --git a/...autoware_detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml b/...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/...utoware_detected_object_validation/schema/obstacle_pointcloud_based_validator.schema.json b/...utoware_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/...ware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp b/...ware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.cpp
@@ -295,6 +295,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");
 
@@ -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/...ware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp b/...ware_detected_object_validation/src/obstacle_pointcloud/obstacle_pointcloud_validator.hpp
@@ -152,6 +152,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_;