fix(autoware_tracking_object_merger): add merge frame (#8418)

* fix(autoware_tracking_object_merger): add merge frame

Signed-off-by: Kaan Çolak <kaancolak95@gmail.com>

* style(pre-commit): autofix

---------

Signed-off-by: Kaan Çolak <kaancolak95@gmail.com>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

Author: kaancolak

perception/autoware_tracking_object_merger/config/decorative_tracker_merger.param.yaml

@@ -2,6 +2,7 @@
 /**:
   ros__parameters:
     base_link_frame_id: "base_link"
+    merge_frame_id: "map"
     time_sync_threshold: 0.999
     sub_object_timeout_sec: 0.8
     publish_interpolated_sub_objects: true #for debug

perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/decorative_tracker_merger_node.hpp

@@ -96,8 +96,8 @@ class DecorativeTrackerMergerNode : public rclcpp::Node
   MEASUREMENT_STATE sub_sensor_type_;
   std::vector<TrackerState> inner_tracker_objects_;
   std::unordered_map<std::string, std::unique_ptr<DataAssociation>> data_association_map_;
-  std::string target_frame_;
   std::string base_link_frame_id_;
+  std::string merge_frame_id_;
   // buffer to save the sub objects
   std::vector<autoware_perception_msgs::msg::TrackedObjects::ConstSharedPtr> sub_objects_buffer_;
   double sub_object_timeout_sec_;

perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/utils/tracker_state.hpp

@@ -146,7 +146,8 @@ class TrackerState
 };
 
 TrackedObjects getTrackedObjectsFromTrackerStates(
-  std::vector<TrackerState> & tracker_states, const rclcpp::Time & time);
+  std::vector<TrackerState> & tracker_states, const rclcpp::Time & time,
+  const std::string & frame_id);
 }  // namespace autoware::tracking_object_merger
 
 #endif  // AUTOWARE__TRACKING_OBJECT_MERGER__UTILS__TRACKER_STATE_HPP_

perception/autoware_tracking_object_merger/src/decorative_tracker_merger_node.cpp

@@ -115,6 +115,7 @@ DecorativeTrackerMergerNode::DecorativeTrackerMergerNode(const rclcpp::NodeOptio
   // Parameters
   publish_interpolated_sub_objects_ = declare_parameter<bool>("publish_interpolated_sub_objects");
   base_link_frame_id_ = declare_parameter<std::string>("base_link_frame_id");
+  merge_frame_id_ = declare_parameter<std::string>("merge_frame_id", "map");
   time_sync_threshold_ = declare_parameter<double>("time_sync_threshold");
   sub_object_timeout_sec_ = declare_parameter<double>("sub_object_timeout_sec");
   // default setting parameter for tracker
@@ -196,14 +197,24 @@ void DecorativeTrackerMergerNode::mainObjectsCallback(
   const TrackedObjects::ConstSharedPtr & main_objects)
 {
   stop_watch_ptr_->toc("processing_time", true);
+
+  /* transform to target merge coordinate */
+  TrackedObjects transformed_objects;
+  if (!object_recognition_utils::transformObjects(
+        *main_objects, merge_frame_id_, tf_buffer_, transformed_objects)) {
+    return;
+  }
+  TrackedObjects::ConstSharedPtr transformed_main_objects =
+    std::make_shared<TrackedObjects>(transformed_objects);
+
   // try to merge sub object
   if (!sub_objects_buffer_.empty()) {
     // get interpolated sub objects
     // get newest sub objects which timestamp is earlier to main objects
     TrackedObjects::ConstSharedPtr closest_time_sub_objects;
     TrackedObjects::ConstSharedPtr closest_time_sub_objects_later;
     for (const auto & sub_object : sub_objects_buffer_) {
-      if (getUnixTime(sub_object->header) < getUnixTime(main_objects->header)) {
+      if (getUnixTime(sub_object->header) < getUnixTime(transformed_main_objects->header)) {
         closest_time_sub_objects = sub_object;
       } else {
         closest_time_sub_objects_later = sub_object;
@@ -212,7 +223,7 @@ void DecorativeTrackerMergerNode::mainObjectsCallback(
     }
     // get delay compensated sub objects
     const auto interpolated_sub_objects = interpolateObjectState(
-      closest_time_sub_objects, closest_time_sub_objects_later, main_objects->header);
+      closest_time_sub_objects, closest_time_sub_objects_later, transformed_main_objects->header);
     if (interpolated_sub_objects.has_value()) {
       // Merge sub objects
       const auto interp_sub_objs = interpolated_sub_objects.value();
@@ -225,9 +236,10 @@ void DecorativeTrackerMergerNode::mainObjectsCallback(
   }
 
   // try to merge main object
-  this->decorativeMerger(main_sensor_type_, main_objects);
-  const auto & tracked_objects = getTrackedObjects(main_objects->header);
+  this->decorativeMerger(main_sensor_type_, transformed_main_objects);
+  const auto & tracked_objects = getTrackedObjects(transformed_main_objects->header);
   merged_object_pub_->publish(tracked_objects);
+
   published_time_publisher_->publish_if_subscribed(
     merged_object_pub_, tracked_objects.header.stamp);
   processing_time_publisher_->publish<tier4_debug_msgs::msg::Float64Stamped>(
@@ -244,10 +256,19 @@ void DecorativeTrackerMergerNode::mainObjectsCallback(
  */
 void DecorativeTrackerMergerNode::subObjectsCallback(const TrackedObjects::ConstSharedPtr & msg)
 {
-  sub_objects_buffer_.push_back(msg);
+  /* transform to target merge coordinate */
+  TrackedObjects transformed_objects;
+  if (!object_recognition_utils::transformObjects(
+        *msg, merge_frame_id_, tf_buffer_, transformed_objects)) {
+    return;
+  }
+  TrackedObjects::ConstSharedPtr transformed_sub_objects =
+    std::make_shared<TrackedObjects>(transformed_objects);
+
+  sub_objects_buffer_.push_back(transformed_sub_objects);
   // remove old sub objects
   // const auto now = get_clock()->now();
-  const auto now = rclcpp::Time(msg->header.stamp);
+  const auto now = rclcpp::Time(transformed_sub_objects->header.stamp);
   const auto remove_itr = std::remove_if(
     sub_objects_buffer_.begin(), sub_objects_buffer_.end(), [now, this](const auto & sub_object) {
       return (now - rclcpp::Time(sub_object->header.stamp)).seconds() > sub_object_timeout_sec_;
@@ -392,7 +413,7 @@ TrackedObjects DecorativeTrackerMergerNode::getTrackedObjects(const std_msgs::ms
 {
   // get main objects
   rclcpp::Time current_time = rclcpp::Time(header.stamp);
-  return getTrackedObjectsFromTrackerStates(inner_tracker_objects_, current_time);
+  return getTrackedObjectsFromTrackerStates(inner_tracker_objects_, current_time, merge_frame_id_);
 }
 
 // create new tracker

perception/autoware_tracking_object_merger/src/utils/tracker_state.cpp

@@ -304,7 +304,8 @@ TrackerState::~TrackerState()
 }
 
 TrackedObjects getTrackedObjectsFromTrackerStates(
-  std::vector<TrackerState> & tracker_states, const rclcpp::Time & current_time)
+  std::vector<TrackerState> & tracker_states, const rclcpp::Time & current_time,
+  const std::string & frame_id)
 {
   TrackedObjects tracked_objects;
 
@@ -325,7 +326,7 @@ TrackedObjects getTrackedObjectsFromTrackerStates(
 
   // update header
   tracked_objects.header.stamp = current_time;
-  tracked_objects.header.frame_id = "map";  // TODO(yoshiri): get frame_id from input
+  tracked_objects.header.frame_id = frame_id;
   return tracked_objects;
 }