Tweaks across planning: resolved all TODOs

[JustinRowbotham]

Merge Request / Pull Request

Summary of Changes

Briefly describe what this MR/PR does and which issue/task it addresses.

Type of Change

• Minor improvements
• Refactors

Checklist (to be completed before review)

• Code follows team standards
• Tested locally
• Tests added / updated if relevant
• Documentation updated if relevant
• CI / checks pass

Related Issue / Task

• Closes PC-128

Notes / Additional Context

TODO’s resolved:
• Age check for odometer, current lane, and entities added
• Added path loading for subscribed topics from the config file planner.yaml
• Moved state publishing from the loop to the constructor for initial state; did not include check to see if console is up (will talk about it with Principals)
• Cleaned up logic in the EventGenerator::update() method

Furthermore, all items in the math_utils files were moved to the ap1::math namespace, and the code was refactored accordingly.

[alyashour]

All good & tested. Pls just move timestamps to SharePtr memory fields & will merge.

[JustinRowbotham]

Changes made and tested in sim. Ready for merge! Thanks @alyashour

[alyashour]

Great work thanks!