fix(detection_by_tracker): add ignore option for each label (#5473)

* fix(detection_by_tracker): add ignore for each class Signed-off-by: badai-nguyen <[email protected]> * fix: launch Signed-off-by: badai-nguyen <[email protected]> --------- Signed-off-by: badai-nguyen <[email protected]>
autowarefoundation · Nov 13, 2023 · b97ff3c · b97ff3c
1 parent 63220df
commit b97ff3c
Show file tree

Hide file tree

Showing 12 changed files with 103 additions and 6 deletions.
diff --git a/...launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/...launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
@@ -195,6 +195,7 @@
   <group if="$(var use_detection_by_tracker)">
     <push-ros-namespace namespace="detection_by_tracker"/>
     <include file="$(find-pkg-share detection_by_tracker)/launch/detection_by_tracker.launch.xml"/>
+    <arg name="detection_by_tracker_param_path" default="$(find-pkg-share detection_by_tracker)/config/detection_by_tracker.param.yaml"/>
   </group>
 
   <!-- CenterPoint -->

diff --git a/.../launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/.../launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml
@@ -220,6 +220,7 @@
   <group if="$(var use_detection_by_tracker)">
     <push-ros-namespace namespace="detection_by_tracker"/>
     <include file="$(find-pkg-share detection_by_tracker)/launch/detection_by_tracker.launch.xml"/>
+    <arg name="detection_by_tracker_param_path" default="$(find-pkg-share detection_by_tracker)/config/detection_by_tracker.param.yaml"/>
   </group>
 
   <!-- CenterPoint -->

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
@@ -73,6 +73,7 @@
       <arg name="use_roi_based_cluster" value="$(var use_roi_based_cluster)"/>
       <arg name="input/radar" value="$(var input/radar)"/>
       <arg name="radar_lanelet_filtering_range_param" value="$(var radar_lanelet_filtering_range_param)"/>
+      <arg name="detection_by_tracker_param_path" value="$(var detection_by_tracker_param_path)"/>
     </include>
   </group>
 
@@ -104,6 +105,7 @@
       <arg name="container_name" value="$(var container_name)"/>
       <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
       <arg name="use_roi_based_cluster" value="$(var use_roi_based_cluster)"/>
+      <arg name="detection_by_tracker_param_path" value="$(var detection_by_tracker_param_path)"/>
     </include>
   </group>
 
@@ -133,6 +135,7 @@
       <arg name="use_pointcloud_container" value="$(var use_pointcloud_container)"/>
       <arg name="container_name" value="$(var container_name)"/>
       <arg name="use_low_height_cropbox" value="$(var use_low_height_cropbox)"/>
+      <arg name="detection_by_tracker_param_path" value="$(var detection_by_tracker_param_path)"/>
     </include>
   </group>
 

diff --git a/...r4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/...r4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml
@@ -61,6 +61,7 @@
   <group if="$(var use_detection_by_tracker)">
     <push-ros-namespace namespace="detection_by_tracker"/>
     <include file="$(find-pkg-share detection_by_tracker)/launch/detection_by_tracker.launch.xml"/>
+    <arg name="detection_by_tracker_param_path" default="$(find-pkg-share detection_by_tracker)/config/detection_by_tracker.param.yaml"/>
   </group>
 
   <!-- CenterPoint -->

diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml
@@ -18,6 +18,7 @@
   <arg name="obstacle_segmentation_ground_segmentation_param_path"/>
   <arg name="obstacle_segmentation_ground_segmentation_elevation_map_param_path"/>
   <arg name="object_recognition_detection_obstacle_pointcloud_based_validator_param_path"/>
+  <arg name="object_recognition_detection_detection_by_tracker_param"/>
   <arg name="occupancy_grid_map_method"/>
   <arg name="occupancy_grid_map_param_path"/>
   <arg name="occupancy_grid_map_updater"/>
@@ -174,6 +175,7 @@
           <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="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/detection_by_tracker/CMakeLists.txt b/perception/detection_by_tracker/CMakeLists.txt
@@ -27,6 +27,7 @@ include_directories(
 # Generate exe file
 set(DETECTION_BY_TRACKER_SRC
   src/detection_by_tracker_core.cpp
+  src/utils.cpp
 )
 
 ament_auto_add_library(detection_by_tracker_node SHARED
@@ -45,4 +46,5 @@ rclcpp_components_register_node(detection_by_tracker_node
 
 ament_auto_package(INSTALL_TO_SHARE
   launch
+  config
 )
diff --git a/perception/detection_by_tracker/config/detection_by_tracker.param.yaml b/perception/detection_by_tracker/config/detection_by_tracker.param.yaml
@@ -0,0 +1,11 @@
+/**:
+  ros__parameters:
+    tracker_ignore_label:
+      UNKNOWN : true
+      CAR : false
+      TRUCK : false
+      BUS : false
+      TRAILER : false
+      MOTORCYCLE : false
+      BICYCLE : false
+      PEDESTRIAN : false
diff --git a/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp b/perception/detection_by_tracker/include/detection_by_tracker/detection_by_tracker_core.hpp
@@ -39,14 +39,15 @@
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
 #endif
 
+#include "utils/utils.hpp"
+
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
 
 #include <deque>
 #include <map>
 #include <memory>
 #include <vector>
-
 class TrackerHandler
 {
 private:
@@ -81,7 +82,7 @@ class DetectionByTracker : public rclcpp::Node
   std::map<uint8_t, int> max_search_distance_for_merger_;
   std::map<uint8_t, int> max_search_distance_for_divider_;
 
-  bool ignore_unknown_tracker_;
+  utils::TrackerIgnoreLabel tracker_ignore_;
 
   void setMaxSearchRange();
 

diff --git a/perception/detection_by_tracker/include/utils/utils.hpp b/perception/detection_by_tracker/include/utils/utils.hpp
@@ -0,0 +1,36 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef UTILS__UTILS_HPP_
+#define UTILS__UTILS_HPP_
+
+#include <cstdint>
+
+namespace utils
+{
+struct TrackerIgnoreLabel
+{
+  bool UNKNOWN;
+  bool CAR;
+  bool TRUCK;
+  bool BUS;
+  bool TRAILER;
+  bool MOTORCYCLE;
+  bool BICYCLE;
+  bool PEDESTRIAN;
+  bool isIgnore(const uint8_t label) const;
+};  // struct TrackerIgnoreLabel
+}  // namespace utils
+
+#endif  // UTILS__UTILS_HPP_
diff --git a/perception/detection_by_tracker/launch/detection_by_tracker.launch.xml b/perception/detection_by_tracker/launch/detection_by_tracker.launch.xml
@@ -3,10 +3,11 @@
   <arg name="input/tracked_objects" default="/perception/object_recognition/tracking/objects"/>
   <arg name="input/initial_objects" default="/perception/object_recognition/detection/clustering/objects_with_feature"/>
   <arg name="output" default="objects"/>
-
+  <arg name="detection_by_tracker_param_path" default="$(find-pkg-share detection_by_tracker)/config/detection_by_tracker.param.yaml"/>
   <node pkg="detection_by_tracker" exec="detection_by_tracker" name="detection_by_tracker_node" output="screen">
     <remap from="~/input/tracked_objects" to="$(var input/tracked_objects)"/>
     <remap from="~/input/initial_objects" to="$(var input/initial_objects)"/>
     <remap from="~/output" to="$(var output)"/>
+    <param from="$(var detection_by_tracker_param_path)"/>
   </node>
 </launch>
diff --git a/perception/detection_by_tracker/src/detection_by_tracker_core.cpp b/perception/detection_by_tracker/src/detection_by_tracker_core.cpp
@@ -160,7 +160,15 @@ DetectionByTracker::DetectionByTracker(const rclcpp::NodeOptions & node_options)
   objects_pub_ = create_publisher<autoware_auto_perception_msgs::msg::DetectedObjects>(
     "~/output", rclcpp::QoS{1});
 
-  ignore_unknown_tracker_ = declare_parameter<bool>("ignore_unknown_tracker", true);
+  // Set parameters
+  tracker_ignore_.UNKNOWN = declare_parameter<bool>("tracker_ignore_label.UNKNOWN");
+  tracker_ignore_.CAR = declare_parameter<bool>("tracker_ignore_label.CAR");
+  tracker_ignore_.TRUCK = declare_parameter<bool>("tracker_ignore_label.TRUCK");
+  tracker_ignore_.BUS = declare_parameter<bool>("tracker_ignore_label.BUS");
+  tracker_ignore_.TRAILER = declare_parameter<bool>("tracker_ignore_label.TRAILER");
+  tracker_ignore_.MOTORCYCLE = declare_parameter<bool>("tracker_ignore_label.MOTORCYCLE");
+  tracker_ignore_.BICYCLE = declare_parameter<bool>("tracker_ignore_label.BICYCLE");
+  tracker_ignore_.PEDESTRIAN = declare_parameter<bool>("tracker_ignore_label.PEDESTRIAN");
 
   // set maximum search setting for merger/divider
   setMaxSearchRange();
@@ -259,7 +267,7 @@ void DetectionByTracker::divideUnderSegmentedObjects(
 
   for (const auto & tracked_object : tracked_objects.objects) {
     const auto & label = tracked_object.classification.front().label;
-    if (ignore_unknown_tracker_ && (label == Label::UNKNOWN)) continue;
+    if (tracker_ignore_.isIgnore(label)) continue;
 
     // change search range according to label type
     const float max_search_range = max_search_distance_for_divider_[label];
@@ -395,7 +403,7 @@ void DetectionByTracker::mergeOverSegmentedObjects(
 
   for (const auto & tracked_object : tracked_objects.objects) {
     const auto & label = tracked_object.classification.front().label;
-    if (ignore_unknown_tracker_ && (label == Label::UNKNOWN)) continue;
+    if (tracker_ignore_.isIgnore(label)) continue;
 
     // change search range according to label type
     const float max_search_range = max_search_distance_for_merger_[label];

diff --git a/perception/detection_by_tracker/src/utils.cpp b/perception/detection_by_tracker/src/utils.cpp
@@ -0,0 +1,30 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "utils/utils.hpp"
+
+#include <autoware_auto_perception_msgs/msg/object_classification.hpp>
+
+namespace utils
+{
+using Label = autoware_auto_perception_msgs::msg::ObjectClassification;
+
+bool TrackerIgnoreLabel::isIgnore(const uint8_t label) const
+{
+  return (label == Label::UNKNOWN && UNKNOWN) || (label == Label::CAR && CAR) ||
+         (label == Label::TRUCK && TRUCK) || (label == Label::BUS && BUS) ||
+         (label == Label::TRAILER && TRAILER) || (label == Label::MOTORCYCLE && MOTORCYCLE) ||
+         (label == Label::BICYCLE && BICYCLE) || (label == Label::PEDESTRIAN && PEDESTRIAN);
+}
+}  // namespace utils