Converting from cartesian coordinates to Frenet frame around the centerline

[AhmadAmine998]

Sample use:

        x = np.array([[
            obs["agent_0"]["pose_x"],
            obs["agent_0"]["pose_y"],
            obs["agent_0"]["delta"],
            obs["agent_0"]["linear_vel_x"],
            obs["agent_0"]["pose_theta"],
        ]]).T
        frenet_pose = env.track.cartesian_to_frenet(x[0, 0], x[1, 0], x[4, 0], s_guess=0)

s_guess is used as the s-coordinate from the previous timestep to ensure continuity of frenet pose between timesteps.

[luigiberducci]

Hi @AhmadAmine998, it looks nice!
There are some conflicts to resolve. Moreover, having a couple of tests to check the consistency of frenet_to_cartesian and cartesian_to_frenet would be cool. Could you take care of that?

[luigiberducci]

lgtm