Fix heading computation to ensure consistent coordinate order with poses

[yuan-yin]

This PR corrects the heading computation so that it is consistent with the coordinate system used for trajectory poses in vehicle coordinates.
The heading should be computed as
heading = arctan2(lateral_displacement, forward_displacement),
where lateral_displacement = plan_traj[:, 0] and forward_displacement = plan_traj[:, 1].

In the original implementation (and even earlier in the unsafe arctan version before the PR #56) the arguments of arctan2 were swapped, effectively producing an angle equal to π/2 – heading.

This fix restores the proper geometric interpretation of the poses, improves simulation fidelity, and leads to higher evaluation scores across different methods.

[TheNeeloy]

Hi! Would you happen to know if there is any problem with the commit that is resulting in it not being merged? I applied your commit to my local codebase and it improved the performance of the LTF policy. Just was wondering if there was any reason why it hasn't been merged in. Thanks!

[yuan-yin]

Hi! Would you happen to know if there is any problem with the commit that is resulting in it not being merged? I applied your commit to my local codebase and it improved the performance of the LTF policy. Just was wondering if there was any reason why it hasn't been merged in. Thanks!

Hi! Sorry, I don't have more insight on this. There haven't been any discussions about the PR since then. Good to know that it improves things.