fix(tracker_object_merger): add sanity check for tracker merger (#6386)

* feat: add time based sanity check in validation function

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

* fix: do not skip update when objects are empty

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

---------

Signed-off-by: yoshiri <yoshiyoshidetteiu@gmail.com>

Author: YoshiRi

perception/tracking_object_merger/README.md

@@ -59,8 +59,8 @@ We use the `existence_probability` to manage tracklet.
 - When we create a new tracklet, we set the `existence_probability` to $p_{sensor}$ value.
 - In each update with specific sensor, we set the `existence_probability` to $p_{sensor}$ value.
 - When tracklet does not have update with specific sensor, we reduce the `existence_probability` by `decay_rate`
-- Object can be published if `existence_probability` is larger than `publish_probability_threshold`
-- Object will be removed if `existence_probability` is smaller than `remove_probability_threshold`
+- Object can be published if `existence_probability` is larger than `publish_probability_threshold` and time from last update is smaller than `max_dt`
+- Object will be removed if `existence_probability` is smaller than `remove_probability_threshold` and time from last update is larger than `max_dt`
 
 ![tracklet_management](./image/tracklet_management.drawio.svg)
 

perception/tracking_object_merger/include/tracking_object_merger/utils/tracker_state.hpp

@@ -118,7 +118,7 @@ class TrackerState
     std::function<void(TrackedObject &, const TrackedObject &)> update_func);
   // const functions
   bool hasUUID(const MEASUREMENT_STATE input, const unique_identifier_msgs::msg::UUID & uuid) const;
-  bool isValid() const;
+  bool isValid(const rclcpp::Time & current_time) const;
   bool canPublish() const;
   TrackedObject getObject() const;
 

perception/tracking_object_merger/src/decorative_tracker_merger.cpp

@@ -250,9 +250,6 @@ bool DecorativeTrackerMergerNode::decorativeMerger(
 {
   // get current time
   const auto current_time = rclcpp::Time(input_objects_msg->header.stamp);
-  if (input_objects_msg->objects.empty()) {
-    return false;
-  }
   if (inner_tracker_objects_.empty()) {
     for (const auto & object : input_objects_msg->objects) {
       inner_tracker_objects_.push_back(createNewTracker(input_sensor, current_time, object));

perception/tracking_object_merger/src/utils/tracker_state.cpp

@@ -271,8 +271,14 @@ bool TrackerState::hasUUID(
   return input_uuid_map_.at(input) == uuid;
 }
 
-bool TrackerState::isValid() const
+bool TrackerState::isValid(const rclcpp::Time & current_time) const
 {
+  // check dt from last update
+  double dt = (current_time - last_update_time_).seconds();
+  if (std::abs(dt) > max_dt_) {
+    return false;
+  }
+
   // check if tracker state is valid
   if (existence_probability_ < remove_probability_threshold_) {
     return false;
@@ -302,7 +308,7 @@ TrackedObjects getTrackedObjectsFromTrackerStates(
   // sanitize and get tracked objects
   for (auto it = tracker_states.begin(); it != tracker_states.end();) {
     // check if tracker state is valid
-    if (it->isValid()) {
+    if (it->isValid(current_time)) {
       if (it->canPublish()) {
         // if valid, get tracked object
         tracked_objects.objects.push_back(it->getObject());