feat(perception_replayer): add a new perception_reproducer.

[xtk8532704]

Description

Add a new perception_reproducer_v2, that makes it possible to replay rosbag in chronological order even when ego is stopped. The algorithm follows this discussion.

How perception_reproducer_v2 works:

1. Generating reproduction_sequence: Whenever the ego's position changes, a chronological reproduce_sequence is generated or regenerated.

   • A list of nearby_ego_odom_indies is found, which contains those odom massages close to the current ego pose within a search radius (default to 1 m), if the list is empty, the nearest odom massage in the rosebag is added to the list to make sure it's not empty.

   • A larger list of rosbag_ego_odom_indices is found by searching those odom rosbag massages with a position close to the elements in nearby_ego_odom_indies within a radius (default 1 m).

   • The list is sorted by timestamp, and those odom rosbag massages in the cooldown queue are filtered out.

   • set this list as the reproduce_sequence.

2. Publishing perception msg: The first element in the reproduce_sequence is popped and published. Published elements are added to a cooldown queue to prevent the same data from being republished in a short time during the ego's movement.

   • The first element in reproduce_sequence is popped and related perception messages are published.
   • The cooldown queue is updated with the published data.
   • If reproduce_sequence is empty, the data that was published last time is published again.

3. Why this design?:

   • If the ego is moving, the reproduce_sequence will update each time and the first element will be published, so it works as the original perception_reproducer.
   • If the ego stops, data in reproduce_sequence will be published sequentially until reproduce_sequence is empty.

Related links

https://tier4.atlassian.net/browse/RT1-6389

Tests performed

Screencast.from.2024.06.10.21.02.37.webm

Notes for reviewers

Note: this PR also contains two bugfix PR's modifications( fix(perception_replayer): order rosbags by starting_time, fix(perception_reproducer): Support reproduce/replay old rosbags. Before reviewing this, you may check and approve those two first.

Interface changes

Effects on system behavior

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.