Carla GNSS sensor oddities

gnss_test.py

import glob
import os
import sys
import time
from math import sqrt

from pyproj import Geod, Transformer

try:
    sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % (
        sys.version_info.major,
        sys.version_info.minor,
        'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0])
except IndexError:
    pass

import carla

_geod = Geod(ellps='WGS84')
_utm_to_ll = Transformer.from_crs(32750, 4326, always_xy=True)
_ll_to_utm = Transformer.from_crs(4326, 32750, always_xy=True)


def euclidean_distance(a, b):
    return sqrt((a[0] - b[0]) ** 2 + (a[1] - b[1]) ** 2)


def geodesic_distance(a, b):
    return _geod.inv(*a, *b)[2]


def utm_to_ll(coords):
    return _utm_to_ll.transform(*coords)


def ll_to_utm(coords):
    return _ll_to_utm.transform(*coords)


utm_upper_left = (809230.000, 7418130.000)
utm_lower_left = (809230.000, 7414089.543)
utm_upper_right = (815409.701, 7418130.000)
utm_lower_right = (815409.701, 7414089.543)

utm_width = euclidean_distance(utm_upper_left, utm_upper_right)
utm_height = euclidean_distance(utm_upper_left, utm_lower_left)

ll_upper_left = utm_to_ll(utm_upper_left)
ll_lower_left = utm_to_ll(utm_lower_left)
ll_upper_right = utm_to_ll(utm_upper_right)
ll_lower_right = utm_to_ll(utm_lower_right)

ll_width = geodesic_distance(ll_upper_left, ll_upper_right)
ll_height = geodesic_distance(ll_upper_left, ll_lower_left)

unreal_location = (0.0, 0.0)
utm_location_from_unreal = (0.0, 0.0)
ll_location_from_gnss = (0.0, 0.0)
utm_location_from_gnss = (0.0, 0.0)


def on_listen(event):
    global spectator, utm_upper_left, unreal_location, utm_location_from_unreal, ll_location_from_gnss, utm_location_from_gnss

    location = spectator.get_transform().location

    unreal_location = (location.x, location.y)

    utm_location_from_unreal = (
        location.x + utm_upper_left[0],
        location.y + utm_upper_left[1],
    )

    ll_location_from_gnss = (event.longitude, event.latitude)

    utm_location_from_gnss = ll_to_utm(ll_location_from_gnss)


def print_status():
    utm_location_from_unreal_distance_from_origin = euclidean_distance(utm_upper_left, utm_location_from_unreal)
    ll_location_from_gnss_distance_from_origin = geodesic_distance(ll_upper_left, ll_location_from_gnss)
    utm_location_from_gnss_distance_from_origin = euclidean_distance(utm_upper_left, utm_location_from_gnss)

    print('unreal_location = {}'.format(unreal_location))
    print('utm_location_from_unreal = {}'.format(utm_location_from_unreal))
    print('utm_location_from_unreal_distance_from_origin = {}'.format(utm_location_from_unreal_distance_from_origin))
    print('ll_location_from_gnss = {}'.format(ll_location_from_gnss))
    print('ll_location_from_gnss_distance_from_origin = {}'.format(ll_location_from_gnss_distance_from_origin))
    print('ll_location_from_gnss_distance_from_origin_error = {}'.format(
        utm_location_from_unreal_distance_from_origin - ll_location_from_gnss_distance_from_origin
    ))
    print('utm_location_from_gnss = {}'.format(utm_location_from_gnss))
    print('utm_location_from_gnss_distance_from_origin = {}'.format(utm_location_from_gnss_distance_from_origin))
    print('utm_location_from_gnss_distance_from_origin_error = {}'.format(
        utm_location_from_unreal_distance_from_origin - utm_location_from_gnss_distance_from_origin
    ))
    print('')


client = carla.Client('localhost', 2000)
client.set_timeout(2.0)

client.load_world('jb_ob18')
time.sleep(2)

world = client.get_world()
[world.wait_for_tick() for _ in range(0, 4)]

spectator = world.get_spectator()
spectator.set_transform(carla.Transform(carla.Location(0, 0)))

blueprint_library = world.get_blueprint_library()

print('geotiff extents from gdalinfo')
print('utm_upper_left = {}'.format(utm_upper_left))
print('utm_lower_left = {}'.format(utm_lower_left))
print('utm_upper_right = {}'.format(utm_upper_right))
print('utm_lower_right = {}'.format(utm_lower_right))
print('utm_width = {}'.format(utm_width))
print('utm_height = {}'.format(utm_height))
print('')

print('utm to lat lon using pyproj')
print('ll_upper_left = {}'.format(ll_upper_left))
print('ll_lower_left = {}'.format(ll_lower_left))
print('ll_upper_right = {}'.format(ll_upper_right))
print('ll_lower_right = {}'.format(ll_lower_right))
print('ll_width = {}'.format(geodesic_distance(ll_upper_left, ll_upper_right)))
print('ll_height = {}'.format(geodesic_distance(ll_upper_left, ll_lower_left)))
print('')

bp_gnss = blueprint_library.find('sensor.other.gnss')
gnss = world.spawn_actor(bp_gnss, carla.Transform(), attach_to=spectator)
gnss.listen(on_listen)
[world.wait_for_tick() for _ in range(0, 4)]
print_status()

spectator.set_transform(carla.Transform(carla.Location(6179, 0)))
[world.wait_for_tick() for _ in range(0, 4)]
print_status()

spectator.set_transform(carla.Transform(carla.Location(0, 4039)))
[world.wait_for_tick() for _ in range(0, 4)]
print_status()