Revert "fix: remove debug messages"

This reverts commit 725a49e.

Author: technolojin

perception/multi_object_tracker/src/multi_object_tracker_core.cpp

@@ -255,6 +255,44 @@ void MultiObjectTracker::onMessage(const ObjectsList & objects_list)
   }
   // process end
   debugger_->endMeasurementTime(this->now());
+
+  /* DEBUG */
+  const rclcpp::Time latest_time(objects_list.back().second.header.stamp);
+  RCLCPP_INFO(
+    this->get_logger(), "MultiObjectTracker::onMessage Objects time range: %f - %f",
+    (current_time - latest_time).seconds(), (current_time - oldest_time).seconds());
+
+  // count objects on each channel
+  std::vector<int> object_counts;
+  object_counts.resize(input_channel_size_);
+  // initialize to zero
+  for (size_t i = 0; i < input_channel_size_; i++) {
+    object_counts[i] = 0;
+  }
+  for (const auto & objects_data : objects_list) {
+    object_counts[objects_data.first] += 1;
+  }
+  // print result
+  std::string object_counts_str = "MultiObjectTracker::onMessage Object counts: ";
+  for (size_t i = 0; i < input_channel_size_; i++) {
+    object_counts_str +=
+      input_channels_[i].long_name + " " + std::to_string(object_counts[i]) + "  ";
+  }
+  RCLCPP_INFO(this->get_logger(), object_counts_str.c_str());
+
+  std::string object_info_str = "";
+  object_info_str += "MultiObjectTracker::onMessage Objects channel and time: \n";
+  for (const auto & objects_data : objects_list) {
+    const auto & objects = objects_data.second;
+    const auto & channel_index = objects_data.first;
+    const auto & time = rclcpp::Time(objects.header.stamp);
+    // print object information
+    object_info_str += input_channels_[channel_index].long_name + " " +
+                       std::to_string((current_time - time).seconds()) + " \n";
+  }
+  RCLCPP_INFO(this->get_logger(), object_info_str.c_str());
+
+  /* DEBUG END */
 }
 
 void MultiObjectTracker::runProcess(

perception/multi_object_tracker/src/processor/input_manager.cpp

@@ -253,6 +253,20 @@ void InputManager::optimizeTimings()
         selected_stream_interval = interval_mean;
         selected_stream_interval_std = std::sqrt(interval_var);
       }
+
+      /* DEBUG */
+      std::string long_name, short_name;
+      rclcpp::Time latest_measurement_time, latest_message_time;
+      input_stream->getNames(long_name, short_name);
+      input_stream->getTimestamps(latest_measurement_time, latest_message_time);
+      double latency_message = (node_.now() - latest_message_time).seconds();
+      double latency_measurement = (node_.now() - latest_measurement_time).seconds();
+      RCLCPP_INFO(
+        node_.get_logger(),
+        "InputManager::getObjects %s: latency mean: %f, std: %f, interval mean: "
+        "%f, std: %f, latest measurement latency: %f, latest message latency: %f",
+        long_name.c_str(), latency_mean, std::sqrt(latency_var), interval_mean,
+        std::sqrt(interval_var), latency_measurement, latency_message);
     }
   }