From a0a481c3f46e9a97ff77cd6858ca8a26fe906f6c Mon Sep 17 00:00:00 2001
From: Yoshi Ri <yoshiyoshidetteiu@gmail.com>
Date: Thu, 16 Nov 2023 15:47:15 +0900
Subject: [PATCH] refactor(radar_object_clustering): move radar object
 clustering parameter to param file (#5451)

* move radar object clustering parameter to param file

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

* remove default parameter settings and fix cmakelists

---------

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>
---
 ...ar_radar_fusion_based_detection.launch.xml |  5 ++---
 .../detection/detection.launch.xml            |  2 ++
 .../lidar_radar_based_detection.launch.xml    |  5 ++---
 .../radar_based_detection.launch.xml          | 14 ++-----------
 .../launch/perception.launch.xml              |  3 +++
 .../radar_object_clustering/CMakeLists.txt    |  1 +
 .../config/radar_object_clustering.param.yaml | 15 ++++++++++++++
 .../launch/radar_object_clustering.launch.xml | 20 ++-----------------
 .../radar_object_clustering_node.cpp          | 18 ++++++++---------
 9 files changed, 38 insertions(+), 45 deletions(-)
 create mode 100644 perception/radar_object_clustering/config/radar_object_clustering.param.yaml

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 20b1b5a4d0b27..1b4c3f38e038d 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
@@ -50,6 +50,7 @@
   <arg name="radar_lanelet_filtering_range_param" default="$(find-pkg-share detected_object_validation)/config/object_lanelet_filter.param.yaml"/>
   <arg name="use_near_radar_fusion" default="false"/>
   <arg name="far_object_merger_sync_queue_size" default="20"/>
+  <arg name="radar_object_clustering_param_path" default="$(find-pkg-share radar_object_clustering)/config/radar_object_clustering.param.yaml"/>
 
   <!-- Filter output name. Switch output topic name by 'use_radar_tracking_fusion' parameter defined in perception.launch -->
   <let name="output_of_filtered_objects" value="$(var output/objects)" if="$(var use_radar_tracking_fusion)"/>
@@ -78,9 +79,7 @@
       <arg name="filter/velocity_threshold" value="3.0"/>
       <arg name="split/velocity_threshold" value="4.5"/>
       <arg name="split_range" value="70.0"/>
-      <arg name="clustering/angle_threshold" value="0.174"/>
-      <arg name="clustering/distance_threshold" value="10.0"/>
-      <arg name="clustering/velocity_threshold" value="4.0"/>
+      <arg name="radar_object_clustering_param_path" value="$(var radar_object_clustering_param_path)"/>
     </include>
   </group>
 
diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml
index 33994934949ac..b69f1c0ee8b14 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml
@@ -32,6 +32,7 @@
   <!-- Radar parameters -->
   <arg name="input/radar" default="/sensing/radar/detected_objects"/>
   <arg name="radar_lanelet_filtering_range_param" default="$(find-pkg-share detected_object_validation)/config/object_lanelet_filter.param.yaml"/>
+  <arg name="radar_object_clustering_param_path" default="$(find-pkg-share radar_object_clustering)/config/radar_object_clustering.param.yaml"/>
 
   <!-- Camera-LiDAR fusion parameters -->
   <arg name="remove_unknown" default="true"/>
@@ -145,6 +146,7 @@
       <arg name="input/radar" value="$(var input/radar)"/>
       <arg name="output/objects" value="objects"/>
       <arg name="radar_lanelet_filtering_range_param" value="$(var radar_lanelet_filtering_range_param)"/>
+      <arg name="radar_object_clustering_param_path" value="$(var radar_object_clustering_param_path)"/>
     </include>
   </group>
 </launch>
diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml
index cee6829ea3b4a..ed37f6270c913 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_radar_based_detection.launch.xml
@@ -11,6 +11,7 @@
   <!-- Radar parameters -->
   <arg name="input/radar" default="/sensing/radar/detected_objects"/>
   <arg name="radar_lanelet_filtering_range_param" default="$(find-pkg-share detected_object_validation)/config/object_lanelet_filter.param.yaml"/>
+  <arg name="radar_object_clustering_param_path" default="$(find-pkg-share radar_object_clustering)/config/radar_object_clustering.param.yaml"/>
 
   <!-- LiDAR detection-->
   <group>
@@ -37,9 +38,7 @@
       <arg name="filter/velocity_threshold" value="3.0"/>
       <arg name="split/velocity_threshold" value="4.5"/>
       <arg name="split_range" value="70.0"/>
-      <arg name="clustering/angle_threshold" value="0.174"/>
-      <arg name="clustering/distance_threshold" value="10.0"/>
-      <arg name="clustering/velocity_threshold" value="4.0"/>
+      <arg name="radar_object_clustering_param_path" value="$(var radar_object_clustering_param_path)"/>
     </include>
   </group>
 
diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml
index 5b5646a061ac7..26e7af07646cd 100644
--- a/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml
+++ b/launch/tier4_perception_launch/launch/object_recognition/detection/radar_based_detection.launch.xml
@@ -6,10 +6,8 @@
   <arg name="filter/velocity_threshold" default="1.5"/>
   <arg name="split/velocity_threshold" default="5.5"/>
   <arg name="split_range" default="80.0"/>
-  <arg name="clustering/angle_threshold" default="0.349"/>
-  <arg name="clustering/distance_threshold" default="10.0"/>
-  <arg name="clustering/velocity_threshold" default="5.0"/>
   <arg name="radar_lanelet_filtering_range_param" default="$(find-pkg-share detected_object_validation)/config/object_lanelet_filter.param.yaml"/>
+  <arg name="radar_object_clustering_param_path" default="$(find-pkg-share radar_object_clustering)/config/radar_object_clustering.param.yaml"/>
 
   <!-- Detection for far dynamic objects -->
   <include file="$(find-pkg-share radar_crossing_objects_noise_filter)/launch/radar_crossing_objects_noise_filter.launch.xml">
@@ -43,14 +41,6 @@
   <include file="$(find-pkg-share radar_object_clustering)/launch/radar_object_clustering.launch.xml">
     <arg name="input/objects" value="lanelet_filtered_objects"/>
     <arg name="output/objects" value="$(var output/objects)"/>
-    <arg name="angle_threshold" value="$(var clustering/angle_threshold)"/>
-    <arg name="distance_threshold" value="$(var clustering/distance_threshold)"/>
-    <arg name="velocity_threshold" value="$(var clustering/velocity_threshold)"/>
-    <arg name="is_fixed_label" value="true"/>
-    <arg name="fixed_label" value="CAR"/>
-    <arg name="is_fixed_size" value="true"/>
-    <arg name="size_x" value="4.0"/>
-    <arg name="size_y" value="1.5"/>
-    <arg name="size_z" value="1.5"/>
+    <arg name="radar_object_clustering_param_path" value="$(var radar_object_clustering_param_path)"/>
   </include>
 </launch>
diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml
index 8b241db9774b3..fe6636c0f74a0 100644
--- a/launch/tier4_perception_launch/launch/perception.launch.xml
+++ b/launch/tier4_perception_launch/launch/perception.launch.xml
@@ -12,6 +12,7 @@
   <arg name="object_recognition_detection_fusion_sync_param_path"/>
   <arg name="object_recognition_detection_lidar_model_param_path"/>
   <arg name="object_recognition_detection_radar_lanelet_filtering_range_param"/>
+  <arg name="object_recognition_detection_radar_object_clustering_param_path"/>
   <arg name="object_recognition_tracking_multi_object_tracker_data_association_matrix_param_path"/>
   <arg name="object_recognition_tracking_radar_object_tracker_data_association_matrix_param_path"/>
   <arg name="object_recognition_tracking_radar_object_tracker_tracking_setting_param_path"/>
@@ -175,7 +176,9 @@
           <arg name="outlier_param_path" value="$(var object_recognition_detection_outlier_param_path)"/>
           <arg name="voxel_grid_based_euclidean_param_path" value="$(var object_recognition_detection_voxel_grid_based_euclidean_cluster_param_path)"/>
           <arg name="radar_lanelet_filtering_range_param" value="$(var object_recognition_detection_radar_lanelet_filtering_range_param)"/>
+          <arg name="radar_object_clustering_param_path" value="$(var object_recognition_detection_radar_object_clustering_param_path)"/>
           <arg name="detection_by_tracker_param_path" value="$(var object_recognition_detection_detection_by_tracker_param)"/>
+
           <arg name="use_pointcloud_map" value="$(var use_pointcloud_map)"/>
           <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
           <arg name="use_object_filter" value="$(var use_object_filter)"/>
diff --git a/perception/radar_object_clustering/CMakeLists.txt b/perception/radar_object_clustering/CMakeLists.txt
index acb544bfe4f6a..ba911c3035765 100644
--- a/perception/radar_object_clustering/CMakeLists.txt
+++ b/perception/radar_object_clustering/CMakeLists.txt
@@ -37,4 +37,5 @@ endif()
 ament_auto_package(
   INSTALL_TO_SHARE
   launch
+  config
 )
diff --git a/perception/radar_object_clustering/config/radar_object_clustering.param.yaml b/perception/radar_object_clustering/config/radar_object_clustering.param.yaml
new file mode 100644
index 0000000000000..2abbf14622a3f
--- /dev/null
+++ b/perception/radar_object_clustering/config/radar_object_clustering.param.yaml
@@ -0,0 +1,15 @@
+/**:
+  ros__parameters:
+    # clustering parameter
+    angle_threshold: 0.174 # [rad] (10 deg)
+    distance_threshold: 10.0 # [m]
+    velocity_threshold: 4.0 # [m/s]
+
+    # output object settings
+    # set false if you want to use the object information from radar
+    is_fixed_label: true
+    fixed_label: "CAR"
+    is_fixed_size: true
+    size_x: 4.0 # [m]
+    size_y: 1.5 # [m]
+    size_z: 1.5 # [m]
diff --git a/perception/radar_object_clustering/launch/radar_object_clustering.launch.xml b/perception/radar_object_clustering/launch/radar_object_clustering.launch.xml
index e216bec45798a..68fca44bcc723 100644
--- a/perception/radar_object_clustering/launch/radar_object_clustering.launch.xml
+++ b/perception/radar_object_clustering/launch/radar_object_clustering.launch.xml
@@ -4,28 +4,12 @@
   <!-- Output -->
   <arg name="output/objects" default="~/output/objects"/>
   <!-- Parameter -->
-  <arg name="angle_threshold" default="0.174"/>
-  <arg name="distance_threshold" default="10.0"/>
-  <arg name="velocity_threshold" default="4.0"/>
-  <arg name="is_fixed_label" default="false"/>
-  <arg name="fixed_label" default="UNKNOWN"/>
-  <arg name="is_fixed_size" default="false"/>
-  <arg name="size_x" default="4.0"/>
-  <arg name="size_y" default="1.5"/>
-  <arg name="size_z" default="1.5"/>
+  <arg name="radar_object_clustering_param_path" default="$(find-pkg-share radar_object_clustering)/config/radar_object_clustering.param.yaml"/>
 
   <!-- Node -->
   <node pkg="radar_object_clustering" exec="radar_object_clustering_node" name="radar_object_clustering" output="screen">
     <remap from="~/input/objects" to="$(var input/objects)"/>
     <remap from="~/output/objects" to="$(var output/objects)"/>
-    <param name="angle_threshold" value="$(var angle_threshold)"/>
-    <param name="distance_threshold" value="$(var distance_threshold)"/>
-    <param name="velocity_threshold" value="$(var velocity_threshold)"/>
-    <param name="is_fixed_label" value="$(var is_fixed_label)"/>
-    <param name="fixed_label" value="$(var fixed_label)"/>
-    <param name="is_fixed_size" value="$(var is_fixed_size)"/>
-    <param name="size_x" value="$(var size_x)"/>
-    <param name="size_y" value="$(var size_y)"/>
-    <param name="size_z" value="$(var size_z)"/>
+    <param from="$(var radar_object_clustering_param_path)"/>
   </node>
 </launch>
diff --git a/perception/radar_object_clustering/src/radar_object_clustering_node/radar_object_clustering_node.cpp b/perception/radar_object_clustering/src/radar_object_clustering_node/radar_object_clustering_node.cpp
index 74e85bde21385..8612fa19ca7f3 100644
--- a/perception/radar_object_clustering/src/radar_object_clustering_node/radar_object_clustering_node.cpp
+++ b/perception/radar_object_clustering/src/radar_object_clustering_node/radar_object_clustering_node.cpp
@@ -71,15 +71,15 @@ RadarObjectClusteringNode::RadarObjectClusteringNode(const rclcpp::NodeOptions &
     std::bind(&RadarObjectClusteringNode::onSetParam, this, std::placeholders::_1));
 
   // Node Parameter
-  node_param_.angle_threshold = declare_parameter<double>("angle_threshold", 0.174);
-  node_param_.distance_threshold = declare_parameter<double>("distance_threshold", 4.0);
-  node_param_.velocity_threshold = declare_parameter<double>("velocity_threshold", 2.0);
-  node_param_.is_fixed_label = declare_parameter<bool>("is_fixed_label", false);
-  node_param_.fixed_label = declare_parameter<std::string>("fixed_label", "UNKNOWN");
-  node_param_.is_fixed_size = declare_parameter<bool>("is_fixed_size", false);
-  node_param_.size_x = declare_parameter<double>("size_x", 4.0);
-  node_param_.size_y = declare_parameter<double>("size_y", 1.5);
-  node_param_.size_z = declare_parameter<double>("size_z", 1.5);
+  node_param_.angle_threshold = declare_parameter<double>("angle_threshold");
+  node_param_.distance_threshold = declare_parameter<double>("distance_threshold");
+  node_param_.velocity_threshold = declare_parameter<double>("velocity_threshold");
+  node_param_.is_fixed_label = declare_parameter<bool>("is_fixed_label");
+  node_param_.fixed_label = declare_parameter<std::string>("fixed_label");
+  node_param_.is_fixed_size = declare_parameter<bool>("is_fixed_size");
+  node_param_.size_x = declare_parameter<double>("size_x");
+  node_param_.size_y = declare_parameter<double>("size_y");
+  node_param_.size_z = declare_parameter<double>("size_z");
 
   // Subscriber
   sub_objects_ = create_subscription<DetectedObjects>(