diff --git a/perception/autoware_tracking_object_merger/config/decorative_tracker_merger.param.yaml b/perception/autoware_tracking_object_merger/config/decorative_tracker_merger.param.yaml index 92a7fca229818..2e5a1867725f9 100644 --- a/perception/autoware_tracking_object_merger/config/decorative_tracker_merger.param.yaml +++ b/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 diff --git a/perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/decorative_tracker_merger_node.hpp b/perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/decorative_tracker_merger_node.hpp index f091959396574..536489b0de8fe 100644 --- a/perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/decorative_tracker_merger_node.hpp +++ b/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 inner_tracker_objects_; std::unordered_map> 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 sub_objects_buffer_; double sub_object_timeout_sec_; diff --git a/perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/utils/tracker_state.hpp b/perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/utils/tracker_state.hpp index 5f1ab36f1265e..a4703d67e1c41 100644 --- a/perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/utils/tracker_state.hpp +++ b/perception/autoware_tracking_object_merger/include/autoware/tracking_object_merger/utils/tracker_state.hpp @@ -146,7 +146,8 @@ class TrackerState }; TrackedObjects getTrackedObjectsFromTrackerStates( - std::vector & tracker_states, const rclcpp::Time & time); + std::vector & 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_ diff --git a/perception/autoware_tracking_object_merger/src/decorative_tracker_merger_node.cpp b/perception/autoware_tracking_object_merger/src/decorative_tracker_merger_node.cpp index 187fb6e1d462e..295a350071ac8 100644 --- a/perception/autoware_tracking_object_merger/src/decorative_tracker_merger_node.cpp +++ b/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("publish_interpolated_sub_objects"); base_link_frame_id_ = declare_parameter("base_link_frame_id"); + merge_frame_id_ = declare_parameter("merge_frame_id", "map"); time_sync_threshold_ = declare_parameter("time_sync_threshold"); sub_object_timeout_sec_ = declare_parameter("sub_object_timeout_sec"); // default setting parameter for tracker @@ -196,6 +197,16 @@ 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(transformed_objects); + // try to merge sub object if (!sub_objects_buffer_.empty()) { // get interpolated sub objects @@ -203,7 +214,7 @@ void DecorativeTrackerMergerNode::mainObjectsCallback( 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( @@ -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(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 diff --git a/perception/autoware_tracking_object_merger/src/utils/tracker_state.cpp b/perception/autoware_tracking_object_merger/src/utils/tracker_state.cpp index eabbd72364129..9fd5fda7a63bb 100644 --- a/perception/autoware_tracking_object_merger/src/utils/tracker_state.cpp +++ b/perception/autoware_tracking_object_merger/src/utils/tracker_state.cpp @@ -304,7 +304,8 @@ TrackerState::~TrackerState() } TrackedObjects getTrackedObjectsFromTrackerStates( - std::vector & tracker_states, const rclcpp::Time & current_time) + std::vector & 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; }