Make actors get their dynamic state by looking at the episode state

Docs/python_api.md

@@ -72,6 +72,8 @@
 - `get_world()`
 - `get_location()`
 - `get_transform()`
+- `get_velocity()`
+- `get_acceleration()`
 - `set_location(location)`
 - `set_transform(transform)`
 - `destroy()`

LibCarla/source/carla/AtomicSharedPtr.h

@@ -17,6 +17,9 @@ namespace carla {
   class AtomicSharedPtr : private NonCopyable {
   public:
 
+    template <typename... Args>
+    explicit AtomicSharedPtr(Args &&... args) : _ptr(std::forward<Args>(args)...) {}
+
     void store(std::shared_ptr<T> ptr) {
       std::atomic_store_explicit(&_ptr, ptr, std::memory_order_relaxed);
     }

LibCarla/source/carla/client/Actor.cpp

@@ -20,6 +20,14 @@ namespace client {
     return GetEpisode()->GetActorTransform(*this);
   }
 
+  geom::Vector3D Actor::GetVelocity() const {
+    return GetEpisode()->GetActorVelocity(*this);
+  }
+
+  geom::Vector3D Actor::GetAcceleration() const {
+    return GetEpisode()->GetActorAcceleration(*this);
+  }
+
   void Actor::SetLocation(const geom::Location &location) {
     GetEpisode()->SetActorLocation(*this, location);
   }

LibCarla/source/carla/client/Actor.h

@@ -31,6 +31,10 @@ namespace client {
 
     geom::Transform GetTransform() const;
 
+    geom::Vector3D GetVelocity() const;
+
+    geom::Vector3D GetAcceleration() const;
+
     void SetLocation(const geom::Location &location);
 
     void SetTransform(const geom::Transform &transform);

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

@@ -0,0 +1,44 @@
+// Copyright (c) 2017 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>.
+
+#include "carla/client/detail/Episode.h"
+
+#include "carla/client/detail/Client.h"
+
+namespace carla {
+namespace client {
+namespace detail {
+
+  static auto &CastData(const sensor::SensorData &data) {
+    using target_t = const sensor::data::RawEpisodeState;
+    DEBUG_ASSERT(dynamic_cast<target_t *>(&data) != nullptr);
+    return static_cast<target_t &>(data);
+  }
+
+  Episode::Episode(Client &client)
+    : _client(client),
+      _description(client.GetEpisodeInfo()),
+      _state(std::make_shared<EpisodeState>()) {}
+
+  Episode::~Episode() {
+    _client.UnSubscribeFromStream(_description.token);
+  }
+
+  void Episode::Listen() {
+    std::weak_ptr<Episode> weak = shared_from_this();
+    _client.SubscribeToStream(_description.token, [weak](auto data) {
+      auto self = weak.lock();
+      if (self != nullptr) {
+        /// @todo This is not atomic.
+        auto prev = self->_state.load();
+        self->_state = prev->DeriveNextStep(CastData(*data));
+      }
+    });
+  }
+
+} // namespace detail
+} // namespace client
+} // namespace carla

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

@@ -0,0 +1,57 @@
+// Copyright (c) 2017 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/AtomicSharedPtr.h"
+#include "carla/NonCopyable.h"
+#include "carla/client/detail/EpisodeState.h"
+#include "carla/rpc/EpisodeInfo.h"
+
+namespace carla {
+namespace client {
+namespace detail {
+
+  class Client;
+
+  /// Represents the episode running on the Simulator.
+  class Episode
+    : public std::enable_shared_from_this<Episode>,
+      private NonCopyable {
+  public:
+
+    explicit Episode(Client &client);
+
+    ~Episode();
+
+    void Listen();
+
+    auto GetId() const {
+      return _description.id;
+    }
+
+    const std::string &GetMapName() const {
+      return _description.map_name;
+    }
+
+    std::shared_ptr<const EpisodeState> GetState() const {
+      auto state = _state.load();
+      DEBUG_ASSERT(state != nullptr);
+      return state;
+    }
+
+  private:
+
+    Client &_client;
+
+    const rpc::EpisodeInfo _description;
+
+    AtomicSharedPtr<const EpisodeState> _state;
+  };
+
+} // namespace detail
+} // namespace client
+} // namespace carla

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

@@ -0,0 +1,49 @@
+// Copyright (c) 2017 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>.
+
+#include "carla/client/detail/EpisodeState.h"
+
+namespace carla {
+namespace client {
+namespace detail {
+
+  static auto DeriveAcceleration(
+      double delta_seconds,
+      const geom::Vector3D &v0,
+      const geom::Vector3D &v1) {
+    /// @todo add methods to Vector3D for scalar multiplication.
+    auto acc = v1 - v0;
+    acc.x /= delta_seconds;
+    acc.y /= delta_seconds;
+    acc.z /= delta_seconds;
+    return acc;
+  }
+
+  std::shared_ptr<const EpisodeState> EpisodeState::DeriveNextStep(
+      const sensor::data::RawEpisodeState &state) const {
+    auto next = std::make_shared<EpisodeState>();
+    next->_frame_number = state.GetFrameNumber();
+    next->_game_timestamp = state.GetGameTimeStamp();
+    next->_platform_timestamp = state.GetPlatformTimeStamp();
+    const auto delta_time = next->_game_timestamp - _game_timestamp;
+    next->_actors.reserve(state.size());
+    for (auto &&actor : state) {
+      auto acceleration = DeriveAcceleration(
+          delta_time,
+          GetActorState(actor.id).velocity,
+          actor.velocity);
+      DEBUG_ONLY(auto result = )
+      next->_actors.emplace(
+          actor.id,
+          ActorState{actor.transform, actor.velocity, acceleration});
+      DEBUG_ASSERT(result.second);
+    }
+    return next;
+  }
+
+} // namespace detail
+} // namespace client
+} // namespace carla

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

@@ -7,30 +7,66 @@
 #pragma once
 
 #include "carla/NonCopyable.h"
-#include "carla/rpc/EpisodeInfo.h"
+#include "carla/sensor/data/ActorDynamicState.h"
+#include "carla/sensor/data/RawEpisodeState.h"
+
+#include <memory>
+#include <unordered_map>
 
 namespace carla {
 namespace client {
 namespace detail {
 
-  /// Represents an episode running on the Simulator.
-  class EpisodeState : private MovableNonCopyable {
+  /// Represents the state of all the actors of an episode at a given frame.
+  class EpisodeState
+    : std::enable_shared_from_this<EpisodeState>,
+      private NonCopyable {
   public:
 
-    EpisodeState(rpc::EpisodeInfo description)
-      : _description(std::move(description)) {}
+    struct ActorState {
+      geom::Transform transform;
+      geom::Vector3D velocity;
+      geom::Vector3D acceleration;
+    };
 
-    auto GetId() const {
-      return _description.id;
+    /// @copydoc carla::sensor::SensorData::GetFrameNumber()
+    size_t GetFrameNumber() const {
+      return _frame_number;
     }
 
-    const std::string &GetMapName() const {
-      return _description.map_name;
+    /// @copydoc carla::sensor::data::RawEpisodeState::GetGameTimeStamp()
+    double GetGameTimeStamp() const {
+      return _game_timestamp;
     }
 
+    /// @copydoc carla::sensor::data::RawEpisodeState::GetPlatformTimeStamp()
+    double GetPlatformTimeStamp() const {
+      return _platform_timestamp;
+    }
+
+    ActorState GetActorState(actor_id_type id) const {
+      ActorState state;
+      auto it = _actors.find(id);
+      if (it != _actors.end()) {
+        state = it->second;
+      } else {
+        log_debug("actor", id, "not found in episode");
+      }
+      return state;
+    }
+
+    std::shared_ptr<const EpisodeState> DeriveNextStep(
+        const sensor::data::RawEpisodeState &state) const;
+
   private:
 
-    rpc::EpisodeInfo _description;
+    size_t _frame_number = 0.0;
+
+    double _game_timestamp = 0.0;
+
+    double _platform_timestamp = 0.0;
+
+    std::unordered_map<actor_id_type, ActorState> _actors;
   };
 
 } // namespace detail

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

@@ -19,6 +19,10 @@ namespace carla {
 namespace client {
 namespace detail {
 
+  // ===========================================================================
+  // -- Static local methods ---------------------------------------------------
+  // ===========================================================================
+
   static void ValidateVersions(Client &client) {
     const auto vc = client.GetClientVersion();
     const auto vs = client.GetServerVersion();
@@ -52,7 +56,8 @@ namespace detail {
   EpisodeProxy Simulator::GetCurrentEpisode() {
     if (_episode == nullptr) {
       ValidateVersions(_client);
-      _episode = std::make_unique<EpisodeState>(_client.GetEpisodeInfo());
+      _episode = std::make_shared<Episode>(_client);
+      _episode->Listen();
     }
     return EpisodeProxy{shared_from_this()};
   }
@@ -114,14 +119,6 @@ namespace detail {
     return success;
   }
 
-  geom::Location Simulator::GetActorLocation(const Actor &) {
-    throw std::runtime_error("GetActorLocation() not implemented!");
-  }
-
-  geom::Transform Simulator::GetActorTransform(const Actor &) {
-    throw std::runtime_error("GetActorTransform() not implemented!");
-  }
-
   // ===========================================================================
   // -- Operations with sensors ------------------------------------------------
   // ===========================================================================

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

@@ -14,8 +14,8 @@
 #include "carla/client/GarbageCollectionPolicy.h"
 #include "carla/client/Vehicle.h"
 #include "carla/client/detail/Client.h"
+#include "carla/client/detail/Episode.h"
 #include "carla/client/detail/EpisodeProxy.h"
-#include "carla/client/detail/EpisodeState.h"
 #include "carla/profiler/LifetimeProfiled.h"
 
 namespace carla {
@@ -126,9 +126,26 @@ namespace detail {
 
     bool DestroyActor(Actor &actor);
 
-    geom::Location GetActorLocation(const Actor &actor);
+    auto GetActorDynamicState(const Actor &actor) const {
+      DEBUG_ASSERT(_episode != nullptr);
+      return _episode->GetState()->GetActorState(actor.GetId());
+    }
 
-    geom::Transform GetActorTransform(const Actor &actor);
+    geom::Location GetActorLocation(const Actor &actor) const {
+      return GetActorDynamicState(actor).transform.location;
+    }
+
+    geom::Transform GetActorTransform(const Actor &actor) const {
+      return GetActorDynamicState(actor).transform;
+    }
+
+    geom::Vector3D GetActorVelocity(const Actor &actor) const {
+      return GetActorDynamicState(actor).velocity;
+    }
+
+    geom::Vector3D GetActorAcceleration(const Actor &actor) const {
+      return GetActorDynamicState(actor).acceleration;
+    }
 
     void SetActorLocation(Actor &actor, const geom::Location &location) {
       _client.SetActorLocation(actor.Serialize(), location);
@@ -170,7 +187,7 @@ namespace detail {
 
     Client _client;
 
-    std::unique_ptr<EpisodeState> _episode;
+    std::shared_ptr<Episode> _episode;
 
     GarbageCollectionPolicy _gc_policy;
   };

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

@@ -8,6 +8,7 @@
 
 #include "carla/geom/Transform.h"
 #include "carla/geom/Vector3D.h"
+#include "carla/rpc/ActorId.h"
 
 #include <cstdint>
 
@@ -17,10 +18,10 @@ namespace data {
 
 #pragma pack(push, 1)
 
-  /// Dynamic state of an actor.
-  struct ActorState {
+  /// Dynamic state of an actor at a certain frame.
+  struct ActorDynamicState {
 
-    uint32_t id;
+    actor_id_type id;
 
     geom::Transform transform;
 
@@ -29,7 +30,7 @@ namespace data {
 
 #pragma pack(pop)
 
-  static_assert(sizeof(ActorState) == 10u * sizeof(uint32_t), "Invalid ActorState size!");
+  static_assert(sizeof(ActorDynamicState) == 10u * sizeof(uint32_t), "Invalid ActorDynamicState size!");
 
 } // namespace data
 } // namespace sensor

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

@@ -7,7 +7,7 @@
 #pragma once
 
 #include "carla/Debug.h"
-#include "carla/sensor/data/ActorState.h"
+#include "carla/sensor/data/ActorDynamicState.h"
 #include "carla/sensor/data/Array.h"
 #include "carla/sensor/s11n/EpisodeStateSerializer.h"
 
@@ -16,15 +16,15 @@ namespace sensor {
 namespace data {
 
   /// State of the episode at a given frame.
-  class EpisodeState : public Array<ActorState>  {
-    using Super = Array<ActorState>;
+  class RawEpisodeState : public Array<ActorDynamicState> {
+    using Super = Array<ActorDynamicState>;
   protected:
 
     using Serializer = s11n::EpisodeStateSerializer;
 
     friend Serializer;
 
-    explicit EpisodeState(RawData data)
+    explicit RawEpisodeState(RawData data)
       : Super(std::move(data)) {
       Super::SetOffset(Serializer::header_offset);
     }

LibCarla/source/carla/sensor/s11n/EpisodeStateSerializer.cpp

@@ -6,14 +6,14 @@
 
 #include "carla/sensor/s11n/EpisodeStateSerializer.h"
 
-#include "carla/sensor/data/EpisodeState.h"
+#include "carla/sensor/data/RawEpisodeState.h"
 
 namespace carla {
 namespace sensor {
 namespace s11n {
 
   SharedPtr<SensorData> EpisodeStateSerializer::Deserialize(RawData data) {
-    return SharedPtr<data::EpisodeState>(new data::EpisodeState{std::move(data)});
+    return SharedPtr<data::RawEpisodeState>(new data::RawEpisodeState{std::move(data)});
   }
 
 } // namespace s11n

LibCarla/source/carla/sensor/s11n/EpisodeStateSerializer.h

@@ -12,7 +12,7 @@
 #include "carla/geom/Transform.h"
 #include "carla/geom/Vector3D.h"
 #include "carla/sensor/RawData.h"
-#include "carla/sensor/data/ActorState.h"
+#include "carla/sensor/data/ActorDynamicState.h"
 
 class FActorRegistry;
 
@@ -60,9 +60,8 @@ namespace s11n {
       double game_timestamp,
       double platform_timestamp,
       const ActorRegistryT &actor_registry) {
-    uint64_t number_of_actors =  actor_registry.Num();
     // Set up buffer for writing.
-    buffer.reset(sizeof(Header) + sizeof(data::ActorState) * number_of_actors);
+    buffer.reset(sizeof(Header) + sizeof(data::ActorDynamicState) * actor_registry.Num());
     auto begin = buffer.begin();
     auto write_data = [&begin](const auto &data) {
       std::memcpy(begin, &data, sizeof(data));
@@ -77,7 +76,7 @@ namespace s11n {
       DEBUG_ASSERT(actor_view.GetActor() != nullptr);
       constexpr float TO_METERS = 1e-3;
       const auto velocity = TO_METERS * actor_view.GetActor()->GetVelocity();
-      data::ActorState info = {
+      data::ActorDynamicState info = {
         actor_view.GetActorId(),
         actor_view.GetActor()->GetActorTransform(),
         geom::Vector3D{velocity.X, velocity.Y, velocity.Z}

PythonAPI/source/libcarla/Actor.cpp

@@ -33,8 +33,10 @@ void export_actor() {
     .add_property("bounding_box", CALL_RETURNING_COPY(cc::Actor, GetBoundingBox))
     .add_property("is_alive", CALL_RETURNING_COPY(cc::Actor, IsAlive))
     .def("get_world", CALL_RETURNING_COPY(cc::Actor, GetWorld))
-    .def("get_location", CONST_CALL_WITHOUT_GIL(cc::Actor, GetLocation))
-    .def("get_transform", CONST_CALL_WITHOUT_GIL(cc::Actor, GetTransform))
+    .def("get_location", &cc::Actor::GetLocation)
+    .def("get_transform", &cc::Actor::GetTransform)
+    .def("get_velocity", &cc::Actor::GetVelocity)
+    .def("get_acceleration", &cc::Actor::GetAcceleration)
     .def("set_location", &cc::Actor::SetLocation, (arg("location")))
     .def("set_transform", &cc::Actor::SetTransform, (arg("transform")))
     .def("destroy", CALL_WITHOUT_GIL(cc::Actor, Destroy))