More logs

Author: TadaKazuto

perception/multi_object_tracker/src/data_association/data_association.cpp

@@ -138,9 +138,11 @@ void DataAssociation::assign(
       ++itr;
     }
   }
-  for (auto itr = reverse_assignment.begin(); itr != reverse_assignment.end();) {
+  int i=0;
+  for (auto itr = reverse_assignment.begin(); itr != reverse_assignment.end(); i++) {
     if (src(itr->second, itr->first) < score_threshold_) {
       itr = reverse_assignment.erase(itr);
+      RCLCPP_INFO(rclcpp::get_logger("multi_object_tracker"), "reverse assign trim, 1st[%d], 2nd[%d], score[%.3f], idx[%d]", itr->first, itr->second, src(itr->second, itr->first), i);
       continue;
     } else {
       ++itr;
@@ -181,7 +183,6 @@ Eigen::MatrixXd DataAssociation::calcScoreMatrix(
         if (passed_gate) {
           if (max_dist < dist) {
             passed_gate = false;
-            RCLCPP_INFO(rclcpp::get_logger("multi_object_tracker"), "dist gate fail, dist[%.3f], label[%d]", dist, measurement_label);
           }
         }
         // area gate

perception/multi_object_tracker/src/processor/processor.cpp

@@ -244,6 +244,9 @@ void TrackerProcessor::removeOverlappedTracker(const rclcpp::Time & time)
           list_tracker_.remove(sorted_list_tracker[j]);
           // Remove from sorted list
           sorted_list_tracker.erase(sorted_list_tracker.begin() + j);
+
+          RCLCPP_INFO(rclcpp::get_logger("multi_object_tracker"), "overlap trim, cls1[%d] -> cls2[%d]", object1.classification.at(0).label, object2.classification.at(0).label);
+
           --j;
 
           //RCLCPP_INFO(rclcpp::get_logger("multi_object_tracker"), "Erase tracker [%ld] -> [%ld] ", i, j);