feat(map_based_prediction): incorporate crosswalk user history

[dkoldaev]

Description

This PR incorporates crosswalk user history to the map based prediction node. The history is useful when detection is unstable and crosswalk user disappear from the detection for some short time. Since current PR, a user that disappeared from the detection for times shorter than the history timeout will get a path based on the latest available position. Additionally, when a crosswalk user is detected in with multiple different IDs it is reasonable to try matching those IDs. This PR introduces simple matching that is based on the position of the user.

Related links

TIER IV INTERNAL LINK

Tests performed

Tested on TIER IV internal data

https://evaluation.tier4.jp/evaluation/reports/08d0336a-8a00-59ad-984c-850352428143?project_id=prd_jt

Notes for reviewers

NA

Interface changes

NA

Effects on system behavior

The new functionality can be disabled with newly introduced parameters

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

• I've confirmed the contribution guidelines.
• The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

• The PR follows the pull request guidelines.
• The PR has been properly tested.
• The PR has been reviewed by the code owners.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

• There are no open discussions or they are tracked via tickets.
• The PR is ready for merge.

After all checkboxes are checked, anyone who has write access can merge the PR.

[danielsanchezaran]

Can you use std::remove_if here instead? https://en.cppreference.com/w/cpp/algorithm/remove . Also, you can do an early return if you do something like

if (current_time - post_object_time <= buffer_time) break;

or

if (current_time - post_object_time <= buffer_time) return false;

if you use std::remove_if with a predicate.

[dkoldaev]

this is not a new function, but a slightly modified version of removeOldObjectsHistory to accommodate crosswalk user history. I agree that it will look better with remove_if, but I think it is not a good idea to make refactoring while adding features.

[danielsanchezaran]

@dkoldaev but these are new added lines, so it is not a refactoring of old code? Even so, function logic is not altered so I really suggest you do it on this PR

[dkoldaev]

@danielsanchezaran I also am not sure how to implement an early return in case of remove if. The predicate is bool, so it will go through all of the iterator list as I understand. The early return is possible, because of while loop and sorted structure of the object data... I would keep it as is for now and address it in a separate PR if you want to improve here

[danielsanchezaran]

Please see my latest comment on the issue

[YoshiRi]

LGTM

[danielsanchezaran]

I mean something like this: which is more readable and will not significantly affect performance

Suggested change

	while (!object_data.empty()) {
	const double post_object_time = rclcpp::Time(object_data.front().header.stamp).seconds();
	if (current_time - post_object_time > buffer_time) {
	// Delete Old Position
	object_data.pop_front();
	} else {
	break;
	}
	}
	std::remove_if(object_data.begin(),object_data.end(),[&current_time, &buffer_time](const auto & obj){
	const double post_object_time = rclcpp::Time(obj.header.stamp).seconds();
	return (current_time - post_object_time > buffer_time);
	});

You could also do this:

Suggested change

	while (!object_data.empty()) {
	const double post_object_time = rclcpp::Time(object_data.front().header.stamp).seconds();
	if (current_time - post_object_time > buffer_time) {
	// Delete Old Position
	object_data.pop_front();
	} else {
	break;
	}
	}
	while (!object_data.empty()) {
	const double post_object_time = rclcpp::Time(object_data.front().header.stamp).seconds();
	if (current_time - post_object_time <= buffer_time) {
	break;
	}
	object_data.pop_front();
	}

But the first suggestion is better imo.

[dkoldaev]

Thank you! But as I understand the first suggestion does not have early exit, it will keep iterating through the list getting bool value for all of the objects. I will go with the second solution.

[danielsanchezaran]

@dkoldaev yes, it does not have but the processing cost is low, I believe the first option is more readable and the performance cost is not significant.