diff --git a/perception/tracking_object_merger/README.md b/perception/tracking_object_merger/README.md index 9964534372037..d55622f89181e 100644 --- a/perception/tracking_object_merger/README.md +++ b/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) diff --git a/perception/tracking_object_merger/include/tracking_object_merger/utils/tracker_state.hpp b/perception/tracking_object_merger/include/tracking_object_merger/utils/tracker_state.hpp index 6bedf7db8d727..894c6c5fae6b4 100644 --- a/perception/tracking_object_merger/include/tracking_object_merger/utils/tracker_state.hpp +++ b/perception/tracking_object_merger/include/tracking_object_merger/utils/tracker_state.hpp @@ -118,7 +118,7 @@ class TrackerState std::function 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; diff --git a/perception/tracking_object_merger/src/decorative_tracker_merger.cpp b/perception/tracking_object_merger/src/decorative_tracker_merger.cpp index d2bc643a8eec1..32bede24b237f 100644 --- a/perception/tracking_object_merger/src/decorative_tracker_merger.cpp +++ b/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)); diff --git a/perception/tracking_object_merger/src/utils/tracker_state.cpp b/perception/tracking_object_merger/src/utils/tracker_state.cpp index f0dc657145a24..6fbb4dee74c29 100644 --- a/perception/tracking_object_merger/src/utils/tracker_state.cpp +++ b/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());