Fix GNSS sensor concurrency

LibCarla/source/carla/client/GnssSensor.cpp

@@ -8,6 +8,7 @@
 #include "carla/Logging.h"
 #include "carla/client/Map.h"
 #include "carla/client/detail/Simulator.h"
+#include "carla/geom/GeoLocation.h"
 #include "carla/sensor/data/GnssEvent.h"
 
 #include <exception>
@@ -15,58 +16,77 @@
 namespace carla {
 namespace client {
 
-  GnssSensor::~GnssSensor() = default;
+  // ===========================================================================
+  // -- GnssCallback -----------------------------------------------------------
+  // ===========================================================================
 
-  void GnssSensor::Listen(CallbackFunctionType callback) {
-    if (IsListening()) {
-      log_error(GetDisplayId(), ": already listening");
-      return;
-    }
+  class GnssCallback {
+  public:
 
-    if (GetParent() == nullptr) {
-      throw_exception(std::runtime_error(GetDisplayId() + ": not attached to vehicle"));
-      return;
-    }
+    GnssCallback(
+        ActorId sensor_id,
+        geom::GeoLocation geo_reference,
+        Sensor::CallbackFunctionType &&user_callback)
+      : _sensor_id(sensor_id),
+        _geo_reference(geo_reference),
+        _callback(std::move(user_callback)) {}
 
-    SharedPtr<Map> map = GetWorld().GetMap();
-    DEBUG_ASSERT(map != nullptr);
-    _geo_reference = map->GetGeoReference();
+    void operator()(const WorldSnapshot &snapshot) const;
 
-    auto self = boost::static_pointer_cast<GnssSensor>(shared_from_this());
+  private:
 
-    log_debug(GetDisplayId(), ": subscribing to tick event");
-    _callback_id = GetEpisode().Lock()->RegisterOnTickEvent([
-        cb=std::move(callback),
-        weak_self=WeakPtr<GnssSensor>(self)](const auto &snapshot) {
-      auto self = weak_self.lock();
-      if (self != nullptr) {
-        auto data = self->TickGnssSensor(snapshot.GetTimestamp());
-        if (data != nullptr) {
-          cb(std::move(data));
-        }
-      }
-    });
-  }
+    ActorId _sensor_id;
 
-  SharedPtr<sensor::SensorData> GnssSensor::TickGnssSensor(
-      const Timestamp &timestamp) {
+    geom::GeoLocation _geo_reference;
+
+    Sensor::CallbackFunctionType _callback;
+  };
+
+  void GnssCallback::operator()(const WorldSnapshot &snapshot) const {
     try {
-      return MakeShared<sensor::data::GnssEvent>(
-               timestamp.frame,
-               timestamp.elapsed_seconds,
-               GetTransform(),
-               _geo_reference.Transform(GetLocation()));
+      auto actor_snapshot = snapshot.Find(_sensor_id);
+      if (actor_snapshot.has_value()) {
+        auto transform = actor_snapshot->transform;
+        _callback(MakeShared<sensor::data::GnssEvent>(
+            snapshot.GetTimestamp().frame,
+            snapshot.GetTimestamp().elapsed_seconds,
+            transform,
+            _geo_reference.Transform(transform.location)));
+      }
     } catch (const std::exception &e) {
       log_error("GnssSensor:", e.what());
-      Stop();
-      return nullptr;
+    }
+  }
+
+  // ===========================================================================
+  // -- GnssSensor -------------------------------------------------------------
+  // ===========================================================================
+
+  GnssSensor::~GnssSensor() {
+    Stop();
+  }
+
+  void GnssSensor::Listen(CallbackFunctionType callback) {
+    auto episode = GetEpisode().Lock();
+    SharedPtr<Map> map = episode->GetCurrentMap();
+    DEBUG_ASSERT(map != nullptr);
+
+    const size_t callback_id = episode->RegisterOnTickEvent(GnssCallback(
+        GetId(),
+        map->GetGeoReference(),
+        std::move(callback)));
+
+    const size_t previous = _callback_id.exchange(callback_id);
+    if (previous != 0u) {
+      episode->RemoveOnTickEvent(previous);
     }
   }
 
   void GnssSensor::Stop() {
-    if (_callback_id.has_value()) {
-      GetEpisode().Lock()->RemoveOnTickEvent(*_callback_id);
-      _callback_id = boost::none;
+    const size_t previous = _callback_id.exchange(0u);
+    auto episode = GetEpisode().TryLock();
+    if ((previous != 0u) && (episode != nullptr)) {
+      episode->RemoveOnTickEvent(previous);
     }
   }
 

LibCarla/source/carla/client/GnssSensor.h

@@ -6,9 +6,8 @@
 #pragma once
 
 #include "carla/client/ClientSideSensor.h"
-#include "carla/geom/GeoLocation.h"
 
-#include <boost/optional.hpp>
+#include <atomic>
 
 namespace carla {
 namespace client {
@@ -35,16 +34,12 @@ namespace client {
     /// Return whether this Sensor instance is currently listening to the
     /// associated sensor in the simulator.
     bool IsListening() const override {
-      return _callback_id.has_value();
+      return _callback_id != 0u;
     }
 
   private:
 
-    SharedPtr<sensor::SensorData> TickGnssSensor(const Timestamp &timestamp);
-
-    geom::GeoLocation _geo_reference;
-
-    boost::optional<size_t> _callback_id;
+    std::atomic_size_t _callback_id{0u};
   };
 
 } // namespace client

LibCarla/source/carla/client/detail/CallbackList.h

@@ -31,6 +31,7 @@ namespace detail {
 
     size_t Push(CallbackType &&callback) {
       auto id = ++_counter;
+      DEBUG_ASSERT(id != 0u);
       _list.Push(Item{id, std::move(callback)});
       return id;
     }