Add support for a gnss sensor

An initial reference location is read from the header of the OpenDRIVE definition (/OpenDRIVE/header/geoReference)

The expected format is based on the Proj4 syntax:
+lat_0=<LATITUDE as double> +lon_0=<LONGITUDE as double)

Example (Town01):
<OpenDRIVE>
    <header revMajor="1" revMinor="4" name="" version="1" date="2018-10-26T12:17:35" north="2.0479999989271146e+1" south="-3.4888998413085938e+2" east="4.1446086126736287e+2" west="-2.0712774024007370e+1" vendor="VectorZero">
        <geoReference><![CDATA[+lat_0=4.9000000000000000e+1 +lon_0=8.0000000000000000e+0]]></geoReference>
    </header>
...

CHANGELOG.md

@@ -7,6 +7,7 @@
   * Added support for Deepin in PythonAPI's setup.py
   * Added support for spawning and controlling walkers (pedestrians)
   * Renamed vehicle.get_vehicle_control() to vehicle.get_control() to be consistent with walkers
+  * Added support for gnss_sensor
 
 ## CARLA 0.9.2
 

Docs/cameras_and_sensors.md

@@ -285,3 +285,23 @@ object for each lane marking crossed by the actor
 | `transform`             | carla.Transform | Transform in world coordinates of the sensor at the time of the measurement |
 | `actor`                 | carla.Actor | Actor that invaded another lane ("self" actor) |
 | `crossed_lane_markings` | carla.LaneMarking list | List of lane markings that have been crossed |
+
+sensor.other.gnss
+-----------------
+
+This sensor, when attached to an actor, reports its current gnss position.
+The gnss position is internally calculated by adding the metric position to
+an initial geo reference location defined within the OpenDRIVE map definition.
+
+This sensor produces
+[`carla.GnssEvent`](python_api.md#carlagnsseventcarlasensordata)
+objects.
+
+| Sensor data attribute  | Type        | Description |
+| ---------------------- | ----------- | ----------- |
+| `frame_number`         | int         | Frame count when the measurement took place |
+| `transform`            | carla.Transform | Transform in world coordinates of the sensor at the time of the measurement |
+| `latitude`             | double | Latitude position of the actor |
+| `longitude`            | double | Longitude position of the actor |
+| `altitude`             | double | Altitude of the actor |
+

Docs/python_api.md

@@ -160,6 +160,12 @@
 - `actor`
 - `crossed_lane_markings`
 
+## `carla.GnssEvent(carla.SensorData)`
+
+- `latitude`
+- `longitude`
+- `altitude`
+
 ## `carla.VehicleControl`
 
 - `throttle`

LibCarla/source/carla/client/GnssSensor.cpp

@@ -0,0 +1,160 @@
+// Copyright (c) 2019 Intel Labs.
+//
+// This work is licensed under the terms of the MIT license.
+// For a copy, see <https://opensource.org/licenses/MIT>.
+
+#include "carla/client/GnssSensor.h"
+
+#include "carla/Logging.h"
+#include "carla/client/Map.h"
+#include "carla/client/detail/Simulator.h"
+#include "carla/geom/Math.h"
+#include "carla/sensor/data/GnssEvent.h"
+#include "carla/StringUtil.h"
+
+#include <exception>
+#include <cmath>
+#include <sstream>
+#include <limits>
+
+#if defined(_WIN32) && !defined(_USE_MATH_DEFINES)
+#define _USE_MATH_DEFINES
+#include <math.h> // cmath is not enough for MSVC
+#endif
+
+namespace carla {
+namespace client {
+
+  const double EARTH_RADIUS_EQUA = 6378137.0;// earth radius at equator [m]
+
+  // inlined functions to avoid multiple definitions
+
+  /// @brief convert latitude to scale, which is needed by mercator transformations
+  /// @param lat latitude in degrees (DEG)
+  /// @return scale factor
+  /// @note when converting from lat/lon -> mercator and back again,
+  ///        or vice versa, use the same scale in both transformations!
+  inline double LatToScale (double lat) {
+    return cos(lat * geom::Math::pi() / 180.0);
+  }
+
+  /// @brief converts lat/lon/scale to mx/my (mx/my in meters if correct scale is given)
+  ///
+  template<class float_type>
+  inline void LatLonToMercator (double lat, double lon, double scale, float_type &mx, float_type &my) {
+    mx = scale * lon * geom::Math::pi() * EARTH_RADIUS_EQUA / 180.0;
+    my = scale * EARTH_RADIUS_EQUA * log( tan((90.0+lat) * geom::Math::pi() / 360.0) );
+  }
+
+  /// @brief converts mx/my/scale to lat/lon (mx/my in meters if correct scale is given)
+  inline void MercatorToLatLon (double mx, double my, double scale, double &lat, double &lon) {
+    lon = mx * 180.0 / (geom::Math::pi() * EARTH_RADIUS_EQUA * scale);
+    lat = 360.0 * atan( exp(my/(EARTH_RADIUS_EQUA * scale)) ) / geom::Math::pi() - 90.0;
+  }
+
+  /// @brief adds meters dx/dy to given lat/lon and returns new lat/lon
+  inline void LatLonAddMeters (double lat_start, double lon_start, double dx, double dy, double &lat_end, double &lon_end) {
+    double scale = LatToScale (lat_start);
+    double mx,my;
+    LatLonToMercator (lat_start, lon_start, scale, mx, my);
+    mx += dx;
+    my += dy;
+    MercatorToLatLon (mx, my, scale, lat_end, lon_end);
+  }
+
+  GnssSensor::~GnssSensor() = default;
+
+  void GnssSensor::Listen(CallbackFunctionType callback) {
+    if (_is_listening) {
+      log_error(GetDisplayId(), ": already listening");
+      return;
+    }
+
+    if (GetParent() == nullptr) {
+      throw std::runtime_error(GetDisplayId() + ": not attached to vehicle");
+      return;
+    }
+
+    SharedPtr<Map> map = GetWorld().GetMap();
+
+    DEBUG_ASSERT(map != nullptr);
+
+    auto self = boost::static_pointer_cast<GnssSensor>(shared_from_this());
+
+    //parse geo reference string
+    _map_latitude = std::numeric_limits<double>::quiet_NaN();
+    _map_longitude = std::numeric_limits<double>::quiet_NaN();
+    std::vector<std::string> geo_ref_splitted;
+    StringUtil::Split(geo_ref_splitted, map->GetGeoReference(), " ");
+
+    for (auto element: geo_ref_splitted) {
+      std::vector<std::string> geo_ref_key_value;
+      StringUtil::Split(geo_ref_key_value, element, "=");
+      if (geo_ref_key_value.size() != 2u) {
+        continue;
+      }
+      std::istringstream istr(geo_ref_key_value[1]);
+      istr.imbue(std::locale("C"));
+      if (geo_ref_key_value[0] == "+lat_0") {
+        istr >> _map_latitude;
+      } else if (geo_ref_key_value[0] == "+lon_0") {
+        istr >> _map_longitude;
+      }
+      if (istr.fail() || !istr.eof()) {
+        _map_latitude = std::numeric_limits<double>::quiet_NaN();
+        _map_longitude = std::numeric_limits<double>::quiet_NaN();
+      }
+    }
+
+
+    if (std::isnan(_map_latitude) || std::isnan(_map_longitude)) {
+      log_warning(GetDisplayId(), ": cannot parse georeference: '" + map->GetGeoReference() + "'. Using default values.");
+      _map_latitude = 42.0;
+      _map_longitude = 2.0;
+    }
+
+    log_debug(GetDisplayId(), ": map geo reference: latitude ", _map_latitude, ", longitude ", _map_longitude);
+
+    log_debug(GetDisplayId(), ": subscribing to tick event");
+    GetEpisode().Lock()->RegisterOnTickEvent([
+        cb=std::move(callback),
+        weak_self=WeakPtr<GnssSensor>(self)](const auto &timestamp) {
+      auto self = weak_self.lock();
+      if (self != nullptr) {
+        auto data = self->TickGnssSensor(timestamp);
+        if (data != nullptr) {
+          cb(std::move(data));
+        }
+      }
+    });
+    _is_listening = true;
+  }
+
+  SharedPtr<sensor::SensorData> GnssSensor::TickGnssSensor(
+      const Timestamp &timestamp) {
+    try {
+      const auto location = GetLocation();
+      double current_lat, current_lon;
+
+      LatLonAddMeters(_map_latitude, _map_longitude, location.x, location.y, current_lat, current_lon);
+
+      return MakeShared<sensor::data::GnssEvent>(
+               timestamp.frame_count,
+               GetTransform(),
+               current_lat,
+               current_lon,
+               location.z);
+    } catch (const std::exception &e) {
+      /// @todo We need to unsubscribe the sensor.
+      // log_error("LaneDetector:", e.what());
+      return nullptr;
+    }
+
+  }
+
+  void GnssSensor::Stop() {
+    _is_listening = false;
+  }
+
+} // namespace client
+} // namespace carla

LibCarla/source/carla/client/GnssSensor.h

@@ -0,0 +1,54 @@
+// Copyright (c) 2019 Intel Labs.
+//
+// This work is licensed under the terms of the MIT license.
+// For a copy, see <https://opensource.org/licenses/MIT>.
+
+#pragma once
+
+#include "carla/client/Sensor.h"
+
+namespace carla {
+namespace client {
+
+  class Map;
+  class Vehicle;
+
+  class GnssSensor final : public Sensor {
+  public:
+
+    using Sensor::Sensor;
+
+    ~GnssSensor();
+
+    /// Register a @a callback to be executed each time a new measurement is
+    /// received.
+    ///
+    /// @warning Calling this function on a sensor that is already listening
+    /// steals the data stream from the previously set callback. Note that
+    /// several instances of Sensor (even in different processes) may point to
+    /// the same sensor in the simulator.
+    void Listen(CallbackFunctionType callback) override;
+
+    /// Stop listening for new measurements.
+    void Stop() override;
+
+    /// Return whether this Sensor instance is currently listening to the
+    /// associated sensor in the simulator.
+    bool IsListening() const override {
+      return _is_listening;
+    }
+
+  private:
+
+    SharedPtr<sensor::SensorData> TickGnssSensor(const Timestamp &timestamp);
+
+    double _map_latitude;
+
+    double _map_longitude;
+
+    bool _is_listening = false;
+
+  };
+
+} // namespace client
+} // namespace carla

LibCarla/source/carla/client/Map.cpp

@@ -86,5 +86,10 @@ namespace client {
     return _map->CalculateCrossedLanes(origin, destination);
   }
 
+
+  std::string Map::GetGeoReference() const {
+    DEBUG_ASSERT(_map != nullptr);
+    return _map->GetData().GetGeoReference();
+  }
 } // namespace client
 } // namespace carla

LibCarla/source/carla/client/Map.h

@@ -54,6 +54,8 @@ namespace client {
         const geom::Location &origin,
         const geom::Location &destination) const;
 
+    std::string GetGeoReference() const;
+
   private:
 
     rpc::MapInfo _description;

LibCarla/source/carla/client/detail/ActorFactory.cpp

@@ -10,6 +10,7 @@
 #include "carla/StringUtil.h"
 #include "carla/client/Actor.h"
 #include "carla/client/LaneDetector.h"
+#include "carla/client/GnssSensor.h"
 #include "carla/client/ServerSideSensor.h"
 #include "carla/client/TrafficLight.h"
 #include "carla/client/Vehicle.h"
@@ -73,6 +74,8 @@ namespace detail {
     auto init = ActorInitializer{description, episode, parent};
     if (description.description.id == "sensor.other.lane_detector") { /// @todo
       return MakeActorImpl<LaneDetector>(std::move(init), gc);
+    } else if (description.description.id == "sensor.other.gnss") { /// @todo
+      return MakeActorImpl<GnssSensor>(std::move(init), gc);
     } else if (description.HasAStream()) {
       return MakeActorImpl<ServerSideSensor>(std::move(init), gc);
     } else if (StringUtil::StartsWith(description.description.id, "vehicle.")) {

LibCarla/source/carla/opendrive/OpenDrive.cpp

@@ -107,6 +107,8 @@ namespace opendrive {
       return mapBuilder.Build();
     }
 
+    mapBuilder.SetGeoReference(open_drive_road.geoReference);
+
     // Generate road and junction information
     using junction_data_t = std::map<int, std::map<int, std::vector<carla::road::lane_junction_t>>>;
     using road_data_t = std::map<int, carla::opendrive::types::RoadInformation *>;

LibCarla/source/carla/opendrive/parser/OpenDriveParser.h

@@ -86,6 +86,8 @@ struct OpenDriveParser {
       carla::opendrive::parser::JunctionParser::Parse(junction, out_open_drive_data.junctions);
     }
 
+    out_open_drive_data.geoReference = xmlDoc.child("OpenDRIVE").child("header").child_value("geoReference");
+
     return true;
   }
 };

LibCarla/source/carla/opendrive/types.h

@@ -239,6 +239,7 @@ namespace types {
   /////////////////////////////////////////////////////////////////
 
   struct OpenDriveData {
+    std::string geoReference;
     std::vector<RoadInformation> roads;
     std::vector<Junction> junctions;
   };

LibCarla/source/carla/road/MapBuilder.h

@@ -23,6 +23,10 @@ namespace road {
       _map_data.SetJunctionInformation(junctionInfo);
     }
 
+    void SetGeoReference(const std::string &geoReference) {
+      _map_data.SetGeoReference(geoReference);
+    }
+
     SharedPtr<Map> Build();
 
   private:

LibCarla/source/carla/road/MapData.h

@@ -52,6 +52,10 @@ namespace road {
       return _junction_information;
     }
 
+    const std::string &GetGeoReference() const {
+      return _geo_reference;
+    }
+
     auto GetRoadSegments() const {
       using const_ref = const element::RoadSegment &;
       auto get = [](auto &pair) -> const_ref { return *pair.second; };
@@ -70,6 +74,12 @@ namespace road {
       _junction_information = junctionInfo;
     }
 
+    void SetGeoReference(const std::string &geoReference) {
+      _geo_reference = geoReference;
+    }
+
+    std::string _geo_reference;
+
     std::vector<lane_junction_t> _junction_information;
 
     std::unordered_map<

LibCarla/source/carla/sensor/SensorRegistry.h

@@ -26,6 +26,7 @@ class ARayCastLidar;
 class ASceneCaptureCamera;
 class ASemanticSegmentationCamera;
 class AWorldObserver;
+class AGnssSensor;
 
 namespace carla {
 namespace sensor {
@@ -40,7 +41,8 @@ namespace sensor {
     std::pair<ADepthCamera *, s11n::ImageSerializer>,
     std::pair<ASemanticSegmentationCamera *, s11n::ImageSerializer>,
     std::pair<ARayCastLidar *, s11n::LidarSerializer>,
-    std::pair<ACollisionSensor *, s11n::CollisionEventSerializer>
+    std::pair<ACollisionSensor *, s11n::CollisionEventSerializer>,
+    std::pair<AGnssSensor *, s11n::ImageSerializer>
   >;
 
 } // namespace sensor
@@ -57,5 +59,6 @@ namespace sensor {
 #include "Carla/Sensor/SceneCaptureCamera.h"
 #include "Carla/Sensor/SemanticSegmentationCamera.h"
 #include "Carla/Sensor/WorldObserver.h"
+#include "Carla/Sensor/GnssSensor.h"
 
 #endif // LIBCARLA_SENSOR_REGISTRY_WITH_SENSOR_INCLUDES

LibCarla/source/carla/sensor/data/GnssEvent.h

@@ -0,0 +1,55 @@
+// Copyright (c) 2019 Intel Labs.
+//
+// This work is licensed under the terms of the MIT license.
+// For a copy, see <https://opensource.org/licenses/MIT>.
+
+#pragma once
+
+#include "carla/road/element/LaneMarking.h"
+#include "carla/sensor/SensorData.h"
+
+#include <vector>
+
+namespace carla {
+namespace sensor {
+namespace data {
+
+  /// A change of gnss data
+  class GnssEvent : public SensorData {
+  public:
+
+    explicit GnssEvent(
+        size_t frame_number,
+        const rpc::Transform &sensor_transform,
+        double lat,
+        double lon,
+        double alt)
+      : SensorData(frame_number, sensor_transform),
+        _lat(std::move(lat)),
+        _lon(std::move(lon)),
+        _alt(std::move(alt)) {}
+
+    const double &GetLatitude() const {
+      return _lat;
+    }
+
+    const double &GetLongitude() const {
+      return _lon;
+    }
+
+    const double &GetAltitude() const {
+      return _alt;
+    }
+
+  private:
+
+    double _lat;
+
+    double _lon;
+
+    double _alt;
+  };
+
+} // namespace data
+} // namespace sensor
+} // namespace carla

PythonAPI/manual_control.py

@@ -142,6 +142,7 @@ class World(object):
         self.player = None
         self.collision_sensor = None
         self.lane_invasion_sensor = None
+        self.gnss_sensor = None
         self.camera_manager = None
         self._weather_presets = find_weather_presets()
         self._weather_index = 0
@@ -174,6 +175,7 @@ class World(object):
         # Set up the sensors.
         self.collision_sensor = CollisionSensor(self.player, self.hud)
         self.lane_invasion_sensor = LaneInvasionSensor(self.player, self.hud)
+        self.gnss_sensor = GnssSensor(self.player)
         self.camera_manager = CameraManager(self.player, self.hud)
         self.camera_manager._transform_index = cam_pos_index
         self.camera_manager.set_sensor(cam_index, notify=False)
@@ -199,6 +201,7 @@ class World(object):
             self.camera_manager.sensor,
             self.collision_sensor.sensor,
             self.lane_invasion_sensor.sensor,
+            self.gnss_sensor.sensor,
             self.player]
         for actor in actors:
             if actor is not None:
@@ -363,6 +366,7 @@ class HUD(object):
             'Speed:   % 15.0f km/h' % (3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2)),
             u'Heading:% 16.0f\N{DEGREE SIGN} % 2s' % (t.rotation.yaw, heading),
             'Location:% 20s' % ('(% 5.1f, % 5.1f)' % (t.location.x, t.location.y)),
+            'GNSS:% 24s' % ('(% 2.6f, % 3.6f)' % (world.gnss_sensor.lat, world.gnss_sensor.lon)),
             'Height:  % 18.0f m' % t.location.z,
             '']
         if isinstance(c, carla.VehicleControl):
@@ -565,6 +569,33 @@ class LaneInvasionSensor(object):
         text = ['%r' % str(x).split()[-1] for x in set(event.crossed_lane_markings)]
         self._hud.notification('Crossed line %s' % ' and '.join(text))
 
+# ==============================================================================
+# -- GnssSensor --------------------------------------------------------
+# ==============================================================================
+
+ 
+class GnssSensor(object):
+    def __init__(self, parent_actor):
+        self.sensor = None
+        self._parent = parent_actor
+        self.lat = 0.0
+        self.lon = 0.0
+        world = self._parent.get_world()
+        bp = world.get_blueprint_library().find('sensor.other.gnss')
+        self.sensor = world.spawn_actor(bp, carla.Transform(carla.Location(x=1.0, z=2.8)), attach_to=self._parent)
+        # We need to pass the lambda a weak reference to self to avoid circular
+        # reference.
+        weak_self = weakref.ref(self)
+        self.sensor.listen(lambda event: GnssSensor._on_gnss_event(weak_self, event))
+ 
+    @staticmethod
+    def _on_gnss_event(weak_self, event):
+        self = weak_self()
+        if not self:
+            return
+        self.lat = event.latitude
+        self.lon = event.longitude
+
 
 # ==============================================================================
 # -- CameraManager -------------------------------------------------------------

PythonAPI/source/libcarla/Sensor.cpp

@@ -6,6 +6,7 @@
 
 #include <carla/PythonUtil.h>
 #include <carla/client/ClientSideSensor.h>
+#include <carla/client/GnssSensor.h>
 #include <carla/client/LaneDetector.h>
 #include <carla/client/Sensor.h>
 #include <carla/client/ServerSideSensor.h>
@@ -39,4 +40,9 @@ void export_sensor() {
       ("LaneDetector", no_init)
     .def(self_ns::str(self_ns::self))
   ;
+
+  class_<cc::GnssSensor, bases<cc::Sensor>, boost::noncopyable, boost::shared_ptr<cc::GnssSensor>>
+      ("GnssSensor", no_init)
+    .def(self_ns::str(self_ns::self))
+  ;
 }

PythonAPI/source/libcarla/SensorData.cpp

@@ -14,6 +14,7 @@
 #include <carla/sensor/data/Image.h>
 #include <carla/sensor/data/LaneInvasionEvent.h>
 #include <carla/sensor/data/LidarMeasurement.h>
+#include <carla/sensor/data/GnssEvent.h>
 
 #include <boost/python/suite/indexing/vector_indexing_suite.hpp>
 
@@ -50,6 +51,15 @@ namespace data {
     return out;
   }
 
+  std::ostream &operator<<(std::ostream &out, const GnssEvent &meas) {
+    out << "GnssEvent(frame=" << meas.GetFrameNumber()
+        << ", lat=" << meas.GetLatitude()
+        << ", lon=" << meas.GetLongitude()
+        << ", alt=" << meas.GetAltitude()
+        << ')';
+    return out;
+  }
+
 } // namespace data
 } // namespace sensor
 } // namespace carla
@@ -205,4 +215,11 @@ void export_sensor_data() {
     .add_property("crossed_lane_markings", CALL_RETURNING_COPY(csd::LaneInvasionEvent, GetCrossedLaneMarkings))
     .def(self_ns::str(self_ns::self))
   ;
+
+  class_<csd::GnssEvent, bases<cs::SensorData>, boost::noncopyable, boost::shared_ptr<csd::GnssEvent>>("GnssEvent", no_init)
+    .add_property("latitude", CALL_RETURNING_COPY(csd::GnssEvent, GetLatitude))
+    .add_property("longitude", CALL_RETURNING_COPY(csd::GnssEvent, GetLongitude))
+    .add_property("altitude", CALL_RETURNING_COPY(csd::GnssEvent, GetAltitude))
+    .def(self_ns::str(self_ns::self))
+  ;
 }

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/GnssSensor.cpp

@@ -0,0 +1,26 @@
+// Copyright (c) 2019 Intel Labs.
+//
+// This work is licensed under the terms of the MIT license.
+// For a copy, see <https://opensource.org/licenses/MIT>.
+
+#include "Carla.h"
+#include "Carla/Sensor/GnssSensor.h"
+#include "StaticMeshResources.h"
+
+FActorDefinition AGnssSensor::GetSensorDefinition()
+{
+  return UActorBlueprintFunctionLibrary::MakeGenericSensorDefinition(TEXT("other"), TEXT("gnss"));
+}
+
+AGnssSensor::AGnssSensor(const FObjectInitializer &ObjectInitializer)
+  : Super(ObjectInitializer)
+{
+  PrimaryActorTick.bCanEverTick = false;
+
+  auto MeshComp = CreateDefaultSubobject<UStaticMeshComponent>(TEXT("RootComponent"));
+  MeshComp->SetCollisionProfileName(UCollisionProfile::NoCollision_ProfileName);
+  MeshComp->bHiddenInGame = true;
+  MeshComp->CastShadow = false;
+  MeshComp->PostPhysicsComponentTick.bCanEverTick = false;
+  RootComponent = MeshComp;
+}

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/GnssSensor.h

@@ -0,0 +1,27 @@
+// Copyright (c) 2019 Intel Labs.
+//
+// This work is licensed under the terms of the MIT license.
+// For a copy, see <https://opensource.org/licenses/MIT>.
+
+#pragma once
+
+#include "Carla/Sensor/ShaderBasedSensor.h"
+
+#include "Carla/Actor/ActorDefinition.h"
+
+#include "GnssSensor.generated.h"
+
+/// Gnss sensor representation
+/// The actual position calculation is done one client side
+UCLASS()
+class CARLA_API AGnssSensor : public ASensor
+{
+  GENERATED_BODY()
+
+public:
+
+  static FActorDefinition GetSensorDefinition();
+
+  AGnssSensor(const FObjectInitializer &ObjectInitializer);
+
+};