add parameter control and rename some executable files

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

perception/tracking_object_merger/CMakeLists.txt

@@ -24,20 +24,20 @@ ament_auto_add_library(mu_successive_shortest_path SHARED
   src/data_association/mu_successive_shortest_path/mu_successive_shortest_path_wrapper.cpp
 )
 
-ament_auto_add_library(decorative_tracking_object_merger SHARED
+ament_auto_add_library(decorative_tracker_merger_node SHARED
   src/data_association/data_association.cpp
   src/decorative_tracker_merger.cpp
   src/utils/utils.cpp
 )
 
-target_link_libraries(decorative_tracking_object_merger
+target_link_libraries(decorative_tracker_merger_node
   mu_successive_shortest_path
   Eigen3::Eigen
 )
 
-rclcpp_components_register_node(decorative_tracking_object_merger
-  PLUGIN "tacking_object_merger::DecorativeTrackerMergerNode"
-  EXECUTABLE decorative_tracking_object_merger_node
+rclcpp_components_register_node(decorative_tracker_merger_node
+  PLUGIN "tracking_object_merger::DecorativeTrackerMergerNode"
+  EXECUTABLE decorative_tracker_merger
 )
 
 ament_auto_package(INSTALL_TO_SHARE

perception/tracking_object_merger/config/decorative_tracker_merger_policy.param.yaml

@@ -0,0 +1,16 @@
+# Merger policy for decorative tracker merger node
+# decorative tracker merger works by merging the sub-object trackers into a main object tracker result
+# There are 3 merger policy:
+# 1. "skip": skip the sub-object tracker result
+# 2. "overwrite": overwrite the main object tracker result with sub-object tracker result
+# 3. "fusion": merge the main object tracker result with sub-object tracker result by using covariance based fusion
+/**:
+  ros__parameters:
+    kinematics_to_be_merged: "velocity" # currently only support "velocity"
+
+    # choose the merger policy for each object type
+    #  : "skip", "overwrite", "fusion"
+    kinematics_merge_policy: "overwrite"
+    classification_merge_policy: "skip"
+    existence_prob_merge_policy: "skip"
+    shape_merge_policy: "skip"

perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp

@@ -16,6 +16,7 @@
 #define TRACKING_OBJECT_MERGER__DECORATIVE_TRACKER_MERGER_HPP_
 
 #include "tracking_object_merger/data_association/data_association.hpp"
+#include "tracking_object_merger/utils/utils.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 
@@ -85,11 +86,17 @@ class DecorativeTrackerMergerNode : public rclcpp::Node
 
   struct
   {
-    double precision_threshold;
-    double recall_threshold;
-    double generalized_iou_threshold;
-    std::map<int /*class label*/, double /*distance_threshold*/> distance_threshold_map;
-  } overlapped_judge_param_;
+    std::string kinematics_to_be_merged;
+    merger_utils::MergePolicy kinematics_merge_policy;
+    merger_utils::MergePolicy classification_merge_policy;
+    merger_utils::MergePolicy existence_prob_merge_policy;
+    merger_utils::MergePolicy shape_merge_policy;
+  } merger_policy_params_;
+
+  std::map<std::string, merger_utils::MergePolicy> merger_policy_map_ = {
+    {"skip", merger_utils::MergePolicy::SKIP},
+    {"overwrite", merger_utils::MergePolicy::OVERWRITE},
+    {"fusion", merger_utils::MergePolicy::FUSION}};
 };
 
 }  // namespace tracking_object_merger

perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="input/main_object" default="main_object"/>
+  <arg name="input/sub_object" default="sub_object"/>
+  <arg name="output/object" default="merged_object"/>
+  <arg name="data_association_matrix_path" default="$(find-pkg-share object_merger)/config/data_association_matrix.param.yaml"/>
+  <arg name="merge_options_path" default="$(find-pkg-share object_merger)/config/decorative_tracker_merger_policy.param.yaml"/>
+
+  <node pkg="tracking_object_merger" exec="decorative_tracker_merger" name="$(anon decorative_tracker_merger)" output="screen">
+    <remap from="input/main_object" to="$(var input/main_object)"/>
+    <remap from="input/sub_object" to="$(var input/sub_object)"/>
+    <remap from="output/object" to="$(var output/object)"/>
+    <param from="$(var data_association_matrix_path)"/>
+    <param from="$(var merge_options_path)"/>
+  </node>
+</launch>

perception/tracking_object_merger/src/decorative_tracker_merger.cpp

@@ -60,18 +60,25 @@ DecorativeTrackerMergerNode::DecorativeTrackerMergerNode(const rclcpp::NodeOptio
 
   // Parameters
   base_link_frame_id_ = declare_parameter<std::string>("base_link_frame_id", "base_link");
-  // priority_mode_ = static_cast<PriorityMode>(
-  //   declare_parameter<int>("priority_mode", static_cast<int>(PriorityMode::Confidence)));
-  // remove_overlapped_unknown_objects_ =
-  //   declare_parameter<bool>("remove_overlapped_unknown_objects", true);
-  overlapped_judge_param_.precision_threshold =
-    declare_parameter<double>("precision_threshold_to_judge_overlapped");
-  overlapped_judge_param_.recall_threshold =
-    declare_parameter<double>("recall_threshold_to_judge_overlapped", 0.5);
-  overlapped_judge_param_.generalized_iou_threshold =
-    declare_parameter<double>("generalized_iou_threshold");
-  sub_object_timeout_sec_ = declare_parameter<double>("sub_object_timeout_sec", 0.5);
-  time_sync_threshold_ = declare_parameter<double>("time_sync_threshold", 0.05);
+
+  // Merger policy parameters
+  merger_policy_params_.kinematics_to_be_merged =
+    declare_parameter<std::string>("kinematics_to_be_merged", "velocity");
+
+  std::string kinematics_merge_policy =
+    declare_parameter<std::string>("kinematics_merge_policy", "overwrite");
+  std::string classification_merge_policy =
+    declare_parameter<std::string>("classification_merge_policy", "skip");
+  std::string existence_prob_merge_policy =
+    declare_parameter<std::string>("existence_prob_merge_policy", "skip");
+  std::string shape_merge_policy = declare_parameter<std::string>("shape_merge_policy", "skip");
+
+  merger_policy_params_.kinematics_merge_policy = merger_policy_map_[kinematics_merge_policy];
+  merger_policy_params_.classification_merge_policy =
+    merger_policy_map_[classification_merge_policy];
+  merger_policy_params_.existence_prob_merge_policy =
+    merger_policy_map_[existence_prob_merge_policy];
+  merger_policy_params_.shape_merge_policy = merger_policy_map_[shape_merge_policy];
 
   // init association
   const auto tmp = this->declare_parameter<std::vector<int64_t>>("can_assign_matrix");
@@ -175,16 +182,17 @@ TrackedObjects DecorativeTrackerMergerNode::decorativeMerger(
     if (direct_assignment.find(object0_idx) != direct_assignment.end()) {  // found and merge
       const auto & object1 = objects1.at(direct_assignment.at(object0_idx));
       // merge object0 and object1
+      // with each merge policy defined in merger_policy_params_
       TrackedObject merged_object = object0;
       merged_object.kinematics = merger_utils::objectKinematicsVXMerger(
-        object0, object1, merger_utils::MergePolicy::OVERWRITE);
+        object0, object1, merger_policy_params_.kinematics_merge_policy);
       merged_object.shape =
-        merger_utils::shapeMerger(object0, object1, merger_utils::MergePolicy::SKIP);
+        merger_utils::shapeMerger(object0, object1, merger_policy_params_.shape_merge_policy);
       merged_object.existence_probability = merger_utils::probabilityMerger(
         object0.existence_probability, object1.existence_probability,
-        merger_utils::MergePolicy::SKIP);
-      auto merged_classification =
-        merger_utils::objectClassificationMerger(object0, object1, merger_utils::MergePolicy::SKIP);
+        merger_policy_params_.existence_prob_merge_policy);
+      auto merged_classification = merger_utils::objectClassificationMerger(
+        object0, object1, merger_policy_params_.classification_merge_policy);
       merged_object.classification = merged_classification.classification;
       output_objects.objects.push_back(merged_object);
     } else {  // not found