From 43b8f34ceccc2936a375ee0bfd1629dac5696cec Mon Sep 17 00:00:00 2001 From: yoshiri Date: Thu, 20 Jul 2023 16:07:35 +0900 Subject: [PATCH] add parameter control and rename some executable files Signed-off-by: yoshiri --- .../tracking_object_merger/CMakeLists.txt | 10 ++--- ...ecorative_tracker_merger_policy.param.yaml | 16 +++++++ .../decorative_tracker_merger.hpp | 17 +++++--- .../decorative_tracker_merger.launch.xml | 16 +++++++ ...corative_tracking_object_merger.launch.xml | 20 --------- .../src/decorative_tracker_merger.cpp | 42 +++++++++++-------- 6 files changed, 74 insertions(+), 47 deletions(-) create mode 100644 perception/tracking_object_merger/config/decorative_tracker_merger_policy.param.yaml create mode 100644 perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml delete mode 100644 perception/tracking_object_merger/launch/decorative_tracking_object_merger.launch.xml diff --git a/perception/tracking_object_merger/CMakeLists.txt b/perception/tracking_object_merger/CMakeLists.txt index 02f789025c62c..66bef722b4429 100644 --- a/perception/tracking_object_merger/CMakeLists.txt +++ b/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 diff --git a/perception/tracking_object_merger/config/decorative_tracker_merger_policy.param.yaml b/perception/tracking_object_merger/config/decorative_tracker_merger_policy.param.yaml new file mode 100644 index 0000000000000..0b98e1b202957 --- /dev/null +++ b/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" diff --git a/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp b/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp index 8eb52d91ce8d4..ebb29d7b54b7b 100644 --- a/perception/tracking_object_merger/include/tracking_object_merger/decorative_tracker_merger.hpp +++ b/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 @@ -85,11 +86,17 @@ class DecorativeTrackerMergerNode : public rclcpp::Node struct { - double precision_threshold; - double recall_threshold; - double generalized_iou_threshold; - std::map 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 merger_policy_map_ = { + {"skip", merger_utils::MergePolicy::SKIP}, + {"overwrite", merger_utils::MergePolicy::OVERWRITE}, + {"fusion", merger_utils::MergePolicy::FUSION}}; }; } // namespace tracking_object_merger diff --git a/perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml b/perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml new file mode 100644 index 0000000000000..19505b52a3537 --- /dev/null +++ b/perception/tracking_object_merger/launch/decorative_tracker_merger.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/perception/tracking_object_merger/launch/decorative_tracking_object_merger.launch.xml b/perception/tracking_object_merger/launch/decorative_tracking_object_merger.launch.xml deleted file mode 100644 index 6a96542a554c7..0000000000000 --- a/perception/tracking_object_merger/launch/decorative_tracking_object_merger.launch.xml +++ /dev/null @@ -1,20 +0,0 @@ - - - - - - - - - - - - - - - - - - - - diff --git a/perception/tracking_object_merger/src/decorative_tracker_merger.cpp b/perception/tracking_object_merger/src/decorative_tracker_merger.cpp index b748f455c7a33..09d5efc8a12da 100644 --- a/perception/tracking_object_merger/src/decorative_tracker_merger.cpp +++ b/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("base_link_frame_id", "base_link"); - // priority_mode_ = static_cast( - // declare_parameter("priority_mode", static_cast(PriorityMode::Confidence))); - // remove_overlapped_unknown_objects_ = - // declare_parameter("remove_overlapped_unknown_objects", true); - overlapped_judge_param_.precision_threshold = - declare_parameter("precision_threshold_to_judge_overlapped"); - overlapped_judge_param_.recall_threshold = - declare_parameter("recall_threshold_to_judge_overlapped", 0.5); - overlapped_judge_param_.generalized_iou_threshold = - declare_parameter("generalized_iou_threshold"); - sub_object_timeout_sec_ = declare_parameter("sub_object_timeout_sec", 0.5); - time_sync_threshold_ = declare_parameter("time_sync_threshold", 0.05); + + // Merger policy parameters + merger_policy_params_.kinematics_to_be_merged = + declare_parameter("kinematics_to_be_merged", "velocity"); + + std::string kinematics_merge_policy = + declare_parameter("kinematics_merge_policy", "overwrite"); + std::string classification_merge_policy = + declare_parameter("classification_merge_policy", "skip"); + std::string existence_prob_merge_policy = + declare_parameter("existence_prob_merge_policy", "skip"); + std::string shape_merge_policy = declare_parameter("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>("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