Update sorting condition

Author: TadaKazuto

perception/multi_object_tracker/src/processor/processor.cpp

@@ -183,14 +183,16 @@ void TrackerProcessor::removeOverlappedTracker(const rclcpp::Time & time)
     list_tracker_.begin(), list_tracker_.end());
   std::sort(
     sorted_list_tracker.begin(), sorted_list_tracker.end(),
-    [](const std::shared_ptr<Tracker> & a, const std::shared_ptr<Tracker> & b) {
+    [&time](const std::shared_ptr<Tracker> & a, const std::shared_ptr<Tracker> & b) {
       bool a_unknown = (a->getHighestProbLabel() == Label::UNKNOWN);
       bool b_unknown = (b->getHighestProbLabel() == Label::UNKNOWN);
       if (a_unknown != b_unknown) {
         return b_unknown;  // Put non-UNKNOWN objects first
       }
-      return a->getTotalMeasurementCount() >
-             b->getTotalMeasurementCount();  // Then sort by measurement count
+      if (a->getTotalMeasurementCount() != b->getTotalMeasurementCount()) {
+        return a->getTotalMeasurementCount() > b->getTotalMeasurementCount();  // Then sort by measurement count
+      }
+      return a->getElapsedTimeFromLastUpdate(time) < b->getElapsedTimeFromLastUpdate(time);  // Finally sort by elapsed time (smaller first)
     });
 
   /* Iterate through the list of trackers */