Skip to content

Instantly share code, notes, and snippets.

@initialed85
Created January 22, 2020 13:21
Show Gist options
  • Save initialed85/41aab328fbde55ad88413c3698f314ce to your computer and use it in GitHub Desktop.
Save initialed85/41aab328fbde55ad88413c3698f314ce to your computer and use it in GitHub Desktop.
Carla GNSS sensor oddities
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()
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment