[TASK] Implement Filtering for Perception Detections with EKF

[jorgenfj]

Description of task

We want to do filtering on the measurements received from the perception group. Write a ros node that filters arbitrary measurements in the desired frame. The initial goal of this node is to be compatible with the aruco board and valve measurements and do 6-DOF filtering.

Suggested Workflow

• Look at the current implementation of aruco board filtering that does filtering directly in the camera frame.
• Understand how to use tf2_ros::MessageFilter to only invoke topic callback when transform is available.

Specifications

• Should filter a PoseStamped message
• Use tf2_ros::MessageFilter to only invoke callback when transform between source and target frame is available.
• Target frame should be exposed as a ros param and modifiable through a parameter file.

Contacts

No response

Code Quality

• Every function in header files are documented (inputs/returns/exceptions)
• The project has automated tests that cover MOST of the functions and branches in functions (pytest/gtest)
• The code is documented on the wiki (provide link)