Implement Aruco action logic in navigator

[logbasem]

Implemented ArUco logic in navigator node

• Replaced aruco node subscription with aruco action client
• Implemented handle_aruco_navigation to handle overall logic
• Implemented search_for_aruco for the nitty gritty of the search
• Separated search_for_aruco into 3 cases and 3 functions to help make the code more readable and clean
  • Case 1: Marker is seen - If the target marker id is in recent feedback from the aruco action server, we need to either start tracking that marker or, if we're already tracking, check the distance to the marker
  • Case 2: Marker is lost - If we were tracking a marker and we don't see it anymore, we need to compare the amount of times missed against a threshold. If it's above the threshold, stop tracking and go back to generic search pattern.
  • Case 3: Generic search pattern - If we don't know where the marker is, generate points in a circle. Then, the logic uses a priority queue to determine which coordinate should be navigated to first, sorted by distance.

Hasn't been done yet:

• Implement Aruco pose to GeoPoint conversion

[onkoe]

lgtm