diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
index 178e193dc2d99..51359d8d8f0e2 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
@@ -314,6 +314,7 @@
+
diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml
index 38d141241c215..45cfb91bf25c1 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml
@@ -348,6 +348,7 @@
+
diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml
index 3b6b9ba652a24..c844db39f4e9d 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml
@@ -115,6 +115,7 @@
+
diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml
index b0219376e9625..3d4ce1035e72c 100644
--- a/launch/tier4_perception_launch/launch/perception.launch.xml
+++ b/launch/tier4_perception_launch/launch/perception.launch.xml
@@ -17,6 +17,7 @@
+
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
new file mode 100644
index 0000000000000..745a0fd6591a9
--- /dev/null
+++ b/perception/detected_object_validation/config/obstacle_pointcloud_based_validator.param.yaml
@@ -0,0 +1,13 @@
+/**:
+ ros__parameters:
+ min_points_num:
+ #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN
+ [10, 10, 10, 10, 10, 10, 10, 10]
+
+ max_points_num:
+ #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN
+ [10, 10, 10, 10, 10, 10, 10, 10]
+
+ min_points_and_distance_ratio:
+ #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN
+ [800.0, 800.0, 800.0, 800.0, 800.0, 800.0, 800.0, 800.0]
diff --git a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp b/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp
index 270e5c7bdb7ff..5819302664907 100644
--- a/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp
+++ b/perception/detected_object_validation/include/obstacle_pointcloud_based_validator/obstacle_pointcloud_based_validator.hpp
@@ -32,14 +32,14 @@
#include
#include
-
+#include
namespace obstacle_pointcloud_based_validator
{
struct PointsNumThresholdParam
{
- size_t min_points_num;
- size_t max_points_num;
- float min_points_and_distance_ratio;
+ std::vector min_points_num;
+ std::vector max_points_num;
+ std::vector min_points_and_distance_ratio;
};
class ObstaclePointCloudBasedValidator : public rclcpp::Node
{
diff --git a/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml b/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml
index 8196e95f37957..799b605658345 100644
--- a/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml
+++ b/perception/detected_object_validation/launch/obstacle_pointcloud_based_validator.launch.xml
@@ -3,11 +3,13 @@
+
+
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 00e53e9de9a9c..47640c9332e4d 100644
--- a/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp
+++ b/perception/detected_object_validation/src/obstacle_pointcloud_based_validator.cpp
@@ -92,10 +92,12 @@ ObstaclePointCloudBasedValidator::ObstaclePointCloudBasedValidator(
objects_pub_ = create_publisher(
"~/output/objects", rclcpp::QoS{1});
- points_num_threshold_param_.min_points_num = declare_parameter("min_points_num", 10);
- points_num_threshold_param_.max_points_num = declare_parameter("max_points_num", 10);
+ points_num_threshold_param_.min_points_num =
+ declare_parameter>("min_points_num");
+ points_num_threshold_param_.max_points_num =
+ declare_parameter>("max_points_num");
points_num_threshold_param_.min_points_and_distance_ratio =
- declare_parameter("min_points_and_distance_ratio", 800.0);
+ declare_parameter>("min_points_and_distance_ratio");
const bool enable_debugger = declare_parameter("enable_debugger", false);
if (enable_debugger) debugger_ = std::make_shared(this);
@@ -134,6 +136,7 @@ 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_label_id = transformed_object.classification.front().label;
const auto & object = input_objects->objects.at(i);
const auto & transformed_object_position =
transformed_object.kinematics.pose_with_covariance.pose.position;
@@ -155,13 +158,17 @@ void ObstaclePointCloudBasedValidator::onObjectsAndObstaclePointCloud(
if (debugger_) debugger_->addNeighborPointcloud(neighbor_pointcloud);
// Filter object that have few pointcloud in them.
+ // TODO(badai-nguyen) add 3d validator option
const auto num = getPointCloudNumWithinPolygon(transformed_object, neighbor_pointcloud);
const auto object_distance =
std::hypot(transformed_object_position.x, transformed_object_position.y);
size_t min_pointcloud_num = std::clamp(
static_cast(
- points_num_threshold_param_.min_points_and_distance_ratio / object_distance + 0.5f),
- points_num_threshold_param_.min_points_num, points_num_threshold_param_.max_points_num);
+ points_num_threshold_param_.min_points_and_distance_ratio.at(object_label_id) /
+ object_distance +
+ 0.5f),
+ static_cast(points_num_threshold_param_.min_points_num.at(object_label_id)),
+ static_cast(points_num_threshold_param_.max_points_num.at(object_label_id)));
if (num) {
(min_pointcloud_num <= num.value()) ? output.objects.push_back(object)
: removed_objects.objects.push_back(object);