Add WorldSnapshot

LibCarla/source/carla/client/World.cpp

@@ -45,6 +45,10 @@ namespace client {
     _episode.Lock()->SetWeatherParameters(weather);
   }
 
+  WorldSnapshot World::GetSnapshot() const {
+    return _episode.Lock()->GetWorldSnapshot();
+  }
+
   SharedPtr<Actor> World::GetActor(ActorId id) const {
     auto simulator = _episode.Lock();
     auto description = simulator->GetActorById(id);

LibCarla/source/carla/client/World.h

@@ -10,6 +10,7 @@
 #include "carla/Time.h"
 #include "carla/client/DebugHelper.h"
 #include "carla/client/Timestamp.h"
+#include "carla/client/WorldSnapshot.h"
 #include "carla/client/detail/EpisodeProxy.h"
 #include "carla/geom/Transform.h"
 #include "carla/rpc/Actor.h"
@@ -64,6 +65,9 @@ namespace client {
     /// Change the weather in the simulation.
     void SetWeather(const rpc::WeatherParameters &weather);
 
+    /// Return a snapshot of the world at this moment.
+    WorldSnapshot GetSnapshot() const;
+
     /// Find actor by id, return nullptr if not found.
     SharedPtr<Actor> GetActor(ActorId id) const;
 

LibCarla/source/carla/client/WorldSnapshot.h

@@ -0,0 +1,75 @@
+// Copyright (c) 2019 Computer Vision Center (CVC) at the Universitat Autonoma
+// de Barcelona (UAB).
+//
+// 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/Timestamp.h"
+#include "carla/client/ActorSnapshot.h"
+#include "carla/client/detail/EpisodeState.h"
+
+#include <boost/optional.hpp>
+
+namespace carla {
+namespace client {
+
+  class WorldSnapshot {
+  public:
+
+    /// Get the id of the episode associated with this world.
+    uint64_t GetId() const {
+      return _state->GetEpisodeId();
+    }
+
+    /// Get timestamp of this snapshot.
+    const Timestamp &GetTimestamp() const {
+      return _state->GetTimestamp();
+    }
+
+    /// Check if an actor is present in this snapshot.
+    bool Contains(ActorId actor_id) const {
+      return _state->ContainsActorSnapshot(actor_id);
+    }
+
+    /// Find an ActorSnapshot by id.
+    boost::optional<ActorSnapshot> Find(ActorId actor_id) const {
+      return _state->GetActorSnapshotIfPresent(actor_id);
+    }
+
+    /// Return number of ActorSnapshots present in this WorldSnapshot.
+    size_t size() const {
+      return _state->size();
+    }
+
+    /// Return a begin iterator to the list of ActorSnapshots.
+    auto begin() const {
+      return _state->begin();
+    }
+
+    /// Return a past-the-end iterator to the list of ActorSnapshots.
+    auto end() const {
+      return _state->end();
+    }
+
+    bool operator==(const WorldSnapshot &rhs) const {
+      return GetTimestamp() == rhs.GetTimestamp();
+    }
+
+    bool operator!=(const WorldSnapshot &rhs) const {
+      return !(*this == rhs);
+    }
+
+  private:
+
+    friend class detail::Simulator;
+
+    explicit WorldSnapshot(std::shared_ptr<const detail::EpisodeState> &&state)
+      : _state(std::move(state)) {}
+
+    std::shared_ptr<const detail::EpisodeState> _state;
+  };
+
+} // namespace client
+} // namespace carla

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

@@ -13,6 +13,8 @@
 #include "carla/client/Timestamp.h"
 #include "carla/sensor/data/RawEpisodeState.h"
 
+#include <boost/optional.hpp>
+
 #include <memory>
 #include <unordered_map>
 
@@ -42,14 +44,19 @@ namespace detail {
       return _timestamp;
     }
 
+    bool ContainsActorSnapshot(ActorId actor_id) const {
+      return _actors.find(actor_id) != _actors.end();
+    }
+
     ActorSnapshot GetActorSnapshot(ActorId id) const {
       ActorSnapshot state;
-      auto it = _actors.find(id);
-      if (it != _actors.end()) {
-        state = it->second;
-      } else {
-        log_debug("actor", id, "not found in episode");
-      }
+      CopyActorSnapshotIfPresent(id, state);
+      return state;
+    }
+
+    boost::optional<ActorSnapshot> GetActorSnapshotIfPresent(ActorId id) const {
+      boost::optional<ActorSnapshot> state;
+      CopyActorSnapshotIfPresent(id, state);
       return state;
     }
 
@@ -59,8 +66,28 @@ namespace detail {
           iterator::make_map_keys_const_iterator(_actors.end()));
     }
 
+    size_t size() const {
+      return _actors.size();
+    }
+
+    auto begin() const {
+      return iterator::make_map_values_const_iterator(_actors.begin());
+    }
+
+    auto end() const {
+      return iterator::make_map_values_const_iterator(_actors.end());
+    }
+
   private:
 
+    template <typename T>
+    void CopyActorSnapshotIfPresent(ActorId id, T &value) const {
+      auto it = _actors.find(id);
+      if (it != _actors.end()) {
+        value = it->second;
+      }
+    }
+
     const uint64_t _episode_id;
 
     const Timestamp _timestamp;

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

@@ -14,6 +14,7 @@
 #include "carla/client/TrafficLight.h"
 #include "carla/client/Vehicle.h"
 #include "carla/client/Walker.h"
+#include "carla/client/WorldSnapshot.h"
 #include "carla/client/detail/ActorFactory.h"
 #include "carla/client/detail/Client.h"
 #include "carla/client/detail/Episode.h"
@@ -79,6 +80,17 @@ namespace detail {
 
     EpisodeProxy GetCurrentEpisode();
 
+    /// @}
+    // =========================================================================
+    /// @name World snapshot
+    // =========================================================================
+    /// @{
+
+    WorldSnapshot GetWorldSnapshot() const {
+      DEBUG_ASSERT(_episode != nullptr);
+      return WorldSnapshot{_episode->GetState()};
+    }
+
     /// @}
     // =========================================================================
     /// @name Map related methods

LibCarla/source/carla/sensor/SensorData.h

@@ -11,7 +11,6 @@
 #include "carla/sensor/RawData.h"
 
 /// @todo This shouldn't be exposed in this namespace.
-#include "carla/client/World.h"
 #include "carla/client/detail/EpisodeProxy.h"
 
 namespace carla {