def reconstruct_heading(waypoints):
    """
    Reconstructs the headings based on the waypoints.
    return the yaw angle(in world coordinate), with positive value turning left and negative value turning right
    """
    # Calculate the headings based on the waypoints
    headings = np.arctan2(np.diff(waypoints[:, 1]), np.diff(waypoints[:, 0]))
    # Append the last heading to match the length of waypoints
    headings = np.append(headings, headings[-1])
    return headings

    Reconstructs the headings based on the waypoints.
    return the yaw angle(in world coordinate), with positive value turning left and negative value turning right
    
metadrive/utils/waypoint_utils.py