Implement a path-finding algorithm such as A* or Dijkstra optimized for a 2D grid representation of a self-driving vehicle's environment.
