Create route

[FinlayCooper]

Merge Request / Pull Request

Summary of Changes

This PR updates the create_route() algorithm and introduces two new utility modules to support autonomous vehicle path planning.

New utility modules:

math_utils:

• vec2f struct: Lightweight 2D vector for ground vehicle navigation (x, y in meters)
• distance(): Calculates Euclidean distance between two 2D points

waypoint_utils:

• locate_closest_waypoint(): Finds the nearest navigable waypoint to a target location
• generate_waypoint_sequence(): Creates the path the vehicle should follow from current position to destination. Uses lane waypoints once available,
  or falls back to direct navigation (straight line)

Updated create_route() algorithm:

• Refactored to use the new utility functions for cleaner, more modular routing logic

Type of Change

• Bug fix
• New feature / task
• Refactor
• Documentation

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-92

Notes / Additional Context

The full implementation cannot be tested until the waypoints vector is created.

[alyashour]

@FinlayCooper good work! Please just address the path testing comment and this can be merged.