Move HAD map utility method from parking_planner to had_map_utils package

Description

This MR aims to move some floating methods from the parking_planner to the had_map_utils package as they can be used by other nodes. This MR is part of #840 (closed).

Notes for reviewer

No functionality should change after these are merged this is simply a refactor. Testing using the AVP demo should yield the same results as master. This change is necessary for the second part of #840 (closed) which utilises this method to filter clusters.

Note:

• There is a todo regarding splitting up the method into multiple different sections which is why I decided that a new file would be reasonable given the size of the method. If there are any suggestions for files names please feel free to suggest them as I wasn't so sure about the naming.
• The methods names in parking_planner seem to use snake_case whereas the methods in HAD_map_utils uses camelCase - I left it the same as in parking planner as I think there should be a separate ticket/MR/issue to address the naming convention used within AutowareAuto.

Pre-review checklist for the author before submitting for review

Every developer is encouraged to be familiar with our contributor guidelines.

1. MR title and description help a friendly human understand the problem solved
2. Sensible notes for the reviewer added to the section above to facilitate review
3. MR fits the criteria of a "small change" listed below
4. "WIP" or "Draft" removed from the MR title
5. MR has a link to the original issue in the description, if it exists
6. If the source branch is on a fork, MR is configured to allow commits from developers with access to push to the target branch
7. Target branch set correctly. Default: master
8. MR assigned to a capable reviewer. Default: @JWhitleyWork

What is meant by a "small change"?

This is a template with a trimmed-down checklist for small MRs. Use it when no new functions, classes or other things that require testing have been added.

Examples are changes to documentation only, a fix for an off-by-one error, improving the CI, or changing log messages to be more informative. When in doubt, use the regular template.

Checklist for the reviewer

Only the reviewer is allowed to make changes in this section!

Mark all the items that are done, and cross out items not applicable to this MR.

1. Basic checks
   1. The MR title describes what is being done on the ticket
   2. The MR does not require additional tests or documentation to be written
   3. The first commit has a proper commit message to be used as a basis for the squashed commit created at the very end
2. Code correctness
   1. The problem/feature is solved (reproducibly)
   2. The solution is performant enough for the use case in mind
   3. Any disabled lints inside the code or at the package level are justified
3. Open work
   1. Any added source-code comment about future work refers to a follow-up GitLab issue explicitly; e.g., // TODO #551 refactor code below

If the MR provides an improvement, don't hesitate to add a 👍 emoji for a neat line of code or a "Thanks for implementing this" comment. This will reward the MR author and prevent the review from being only about what still needs to be improved.

Post-review checklist for the author

After receiving approval:

1. All checkboxes in the MR checklist are checked or crossed out. Syntax example: 1. [ ] ~~this item crossed out~~
2. Assign MR to maintainer with sufficient rights to merge. Default: @JWhitleyWork