fix: callback object is fixed, tracker is working

Signed-off-by: Taekjin LEE <taekjin.lee@tier4.jp>

Author: technolojin

perception/multi_object_tracker/include/multi_object_tracker/processor/input_manager.hpp

@@ -45,7 +45,7 @@ class InputStream
   std::string long_name_;
   std::string short_name_;
 
-  size_t que_size_{20};
+  size_t que_size_{50};
   std::deque<autoware_auto_perception_msgs::msg::DetectedObjects> objects_que_;
 
   bool is_time_initialized_{false};

perception/multi_object_tracker/src/multi_object_tracker_core.cpp

@@ -102,22 +102,15 @@ MultiObjectTracker::MultiObjectTracker(const rclcpp::NodeOptions & node_options)
 
   // print input information
   for (size_t i = 0; i < input_topic_size_; i++) {
-    RCLCPP_INFO(get_logger(), "Subscribing to %s %s %s", input_topic_names.at(i).c_str(), input_names_long.at(i).c_str(), input_names_short.at(i).c_str());
+    RCLCPP_INFO(
+      get_logger(), "Subscribing to %s %s %s", input_topic_names.at(i).c_str(),
+      input_names_long.at(i).c_str(), input_names_short.at(i).c_str());
   }
-  
+
+  // Initialize input manager
   input_manager_ = std::make_unique<InputManager>(*this);
   input_manager_->init(input_topic_names, input_names_long, input_names_short);
 
-  // sub_objects_array_.resize(input_topic_size_);
-
-  // for (size_t i = 0; i < input_topic_size_; i++) {
-  // RCLCPP_INFO(get_logger(), "Subscribing to %s", input_topic_names_.at(i).c_str());
-  // std::function<void(const DetectedObjects::ConstSharedPtr msg)> func =
-  //   std::bind(&MultiObjectTracker::onData, this, std::placeholders::_1, i);
-  // sub_objects_array_.at(i) =
-  //   create_subscription<DetectedObjects>(input_topic_names_.at(i), rclcpp::QoS{1}, func);
-  // }
-
   // Create tf timer
   auto cti = std::make_shared<tf2_ros::CreateTimerROS>(
     this->get_node_base_interface(), this->get_node_timers_interface());
@@ -232,27 +225,14 @@ void MultiObjectTracker::onTimer()
 
   if (elapsed_time < maximum_publish_latency) return;
 
-  // // update trackers from accumulated input objects
-  // if (objects_data_.empty()) return;
-
-  // // sort by time
-  // sort(objects_data_.begin(), objects_data_.end(), [](const auto & a, const auto & b) {
-  //   return a.first.seconds() < b.first.seconds();
-  // });
+  // get objects from the input manager and run process
   std::vector<autoware_auto_perception_msgs::msg::DetectedObjects> objects_data;
   if (input_manager_->getObjects(current_time, objects_data)) {
     for (const auto & objects : objects_data) {
       runProcess(objects);
     }
   }
 
-  // // run onMeasurement for each queue
-  // while (!objects_data_.empty()) {
-  //   const auto & pair = objects_data_.front();
-  //   runProcess(std::make_shared<DetectedObjects>(pair.second));
-  //   objects_data_.erase(objects_data_.begin());
-  // }
-
   // Publish
   checkAndPublish(current_time);
 }

perception/multi_object_tracker/src/processor/input_manager.cpp

@@ -54,8 +54,7 @@ void InputStream::setObjects(
     objects_que_.pop_front();
   }
 
-  // RCLCPP_INFO(
-  //   node_.get_logger(), "InputStream::setObjects Que size: %zu", objects_que_.size());
+  RCLCPP_INFO(node_.get_logger(), "InputStream::setObjects Que size: %zu", objects_que_.size());
 
   // Filter parameters
   constexpr double gain = 0.05;
@@ -88,18 +87,43 @@ void InputStream::getObjectsOlderThan(
   std::vector<autoware_auto_perception_msgs::msg::DetectedObjects> & objects)
 {
   objects.clear();
+
+  // remove objects older than the specified duration
+  const rclcpp::Time old_limit = time - rclcpp::Duration::from_seconds(duration);
   for (const auto & object : objects_que_) {
-    // Skip if the object is older than the specified duration
     const rclcpp::Time object_time = rclcpp::Time(object.header.stamp);
-    const double time_diff = (object_time - time).seconds();
-    if (time_diff > duration) {
-      continue;
+    if (object_time < old_limit) {
+      objects_que_.pop_front();
+    } else {
+      break;
     }
+  }
+
+  for (const auto & object : objects_que_) {
+    // Skip if the object is older than the specified duration
+    const rclcpp::Time object_time = rclcpp::Time(object.header.stamp);
+    const double time_diff = (time - object_time).seconds();
     // Add the object if the object is older than the specified time
     if (time_diff >= 0.0) {
       objects.push_back(object);
+      // remove the object from the queue
+      objects_que_.pop_front();
     }
   }
+  RCLCPP_INFO(
+    node_.get_logger(), "InputStream::getObjectsOlderThan %s gives %zu objects", long_name_.c_str(),
+    objects.size());
+
+  // // message to show the older limit, time, old objects in the queue, latest object in the queue
+  // if (objects_que_.empty()) {
+  //   RCLCPP_INFO(
+  //     node_.get_logger(), "InputStream::getObjectsOlderThan %s empty queue", long_name_.c_str());
+  //   return;
+  // }
+  // RCLCPP_INFO(
+  //   node_.get_logger(), "InputStream::getObjectsOlderThan %s older limit %f, time %f, %u, %u",
+  //   long_name_.c_str(), old_limit.seconds(), time.seconds(),
+  //   objects_que_.front().header.stamp.sec, objects_que_.back().header.stamp.sec);
 }
 
 InputManager::InputManager(rclcpp::Node & node) : node_(node)
@@ -132,7 +156,7 @@ void InputManager::init(
       long_names.at(i).c_str(), input_topics.at(i).c_str());
     std::function<void(
       const autoware_auto_perception_msgs::msg::DetectedObjects::ConstSharedPtr msg)>
-      func = std::bind(&InputStream::setObjects, input_stream, std::placeholders::_1);
+      func = std::bind(&InputStream::setObjects, input_streams_.at(i), std::placeholders::_1);
     sub_objects_array_.at(i) =
       node_.create_subscription<autoware_auto_perception_msgs::msg::DetectedObjects>(
         input_topics.at(i), rclcpp::QoS{1}, func);