Add wait for tick function

Docs/python_api.md

@@ -24,6 +24,7 @@
 - `get_actors()`
 - `spawn_actor(blueprint, transform, attach_to=None)`
 - `try_spawn_actor(blueprint, transform, attach_to=None)`
+- `wait_for_tick(seconds=1.0)`
 
 ## `carla.BlueprintLibrary`
 

LibCarla/source/carla/ThreadGroup.h

@@ -42,6 +42,7 @@ namespace carla {
           thread.join();
         }
       }
+      _threads.clear();
     }
 
   private:

LibCarla/source/carla/client/Timestamp.h

@@ -0,0 +1,52 @@
+// 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 <cstdint>
+
+namespace carla {
+namespace client {
+
+  class Timestamp {
+  public:
+
+    Timestamp() = default;
+
+    Timestamp(
+        std::size_t in_frame_count,
+        double in_elapsed_seconds,
+        double in_delta_seconds,
+        double in_platform_timestamp)
+      : frame_count(in_frame_count),
+        elapsed_seconds(in_elapsed_seconds),
+        delta_seconds(in_delta_seconds),
+        platform_timestamp(in_platform_timestamp) {}
+
+    /// Number of frames elapsed since the simulator was launched.
+    std::size_t frame_count = 0u;
+
+    /// Simulated seconds elapsed since the beginning of the current episode.
+    double elapsed_seconds = 0.0;
+
+    /// Simulated seconds elapsed since previous frame.
+    double delta_seconds = 0.0;
+
+    /// Time-stamp of the frame at which this measurement was taken, in seconds
+    /// as given by the OS.
+    double platform_timestamp = 0.0;
+
+    bool operator==(const Timestamp &rhs) const {
+      return frame_count == rhs.frame_count;
+    }
+
+    bool operator!=(const Timestamp &rhs) const {
+      return !(*this == rhs);
+    }
+  };
+
+} // namespace client
+} // namespace carla

LibCarla/source/carla/client/World.cpp

@@ -70,5 +70,9 @@ namespace client {
     }
   }
 
+  Timestamp World::WaitForTick(time_duration timeout) const {
+    return _episode.Lock()->WaitForTick(timeout);
+  }
+
 } // namespace client
 } // namespace carla

LibCarla/source/carla/client/World.h

@@ -7,6 +7,8 @@
 #pragma once
 
 #include "carla/Memory.h"
+#include "carla/Time.h"
+#include "carla/client/Timestamp.h"
 #include "carla/client/detail/EpisodeProxy.h"
 #include "carla/geom/Transform.h"
 #include "carla/rpc/WeatherParameters.h"
@@ -54,6 +56,8 @@ namespace client {
         const geom::Transform &transform,
         Actor *parent = nullptr);
 
+    Timestamp WaitForTick(time_duration timeout) const;
+
   private:
 
     detail::EpisodeProxy _episode;

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

@@ -34,9 +34,11 @@ namespace detail {
       auto self = weak.lock();
       if (self != nullptr) {
         auto data = sensor::Deserializer::Deserialize(std::move(buffer));
-        /// @todo This is not atomic.
         auto prev = self->_state.load();
-        self->_state = prev->DeriveNextStep(CastData(*data));
+        auto next = prev->DeriveNextStep(CastData(*data));
+        /// @todo Check that this state occurred after.
+        self->_state = next;
+        self->_timestamp.SetValue(next->GetTimestamp());
       }
     });
   }

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

@@ -8,6 +8,8 @@
 
 #include "carla/AtomicSharedPtr.h"
 #include "carla/NonCopyable.h"
+#include "carla/RecurrentSharedFuture.h"
+#include "carla/client/Timestamp.h"
 #include "carla/client/detail/CachedActorList.h"
 #include "carla/client/detail/EpisodeState.h"
 #include "carla/rpc/EpisodeInfo.h"
@@ -30,6 +32,10 @@ namespace detail {
 
     void Listen();
 
+    Timestamp WaitForState(time_duration timeout) {
+      return _timestamp.WaitFor(timeout);
+    }
+
     auto GetId() const {
       return _description.id;
     }
@@ -56,6 +62,8 @@ namespace detail {
 
     const rpc::EpisodeInfo _description;
 
+    RecurrentSharedFuture<Timestamp> _timestamp;
+
     AtomicSharedPtr<const EpisodeState> _state;
 
     CachedActorList _actors;

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

@@ -25,14 +25,14 @@ namespace detail {
   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->_timestamp.frame_count = state.GetFrameNumber();
+    next->_timestamp.elapsed_seconds = state.GetGameTimeStamp();
+    next->_timestamp.platform_timestamp = state.GetPlatformTimeStamp();
+    next->_timestamp.delta_seconds = next->_timestamp.elapsed_seconds - _timestamp.elapsed_seconds;
     next->_actors.reserve(state.size());
     for (auto &&actor : state) {
       auto acceleration = DeriveAcceleration(
-          delta_time,
+          next->_timestamp.delta_seconds,
           GetActorState(actor.id).velocity,
           actor.velocity);
       DEBUG_ONLY(auto result = )

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

@@ -9,6 +9,7 @@
 #include "carla/Iterator.h"
 #include "carla/ListView.h"
 #include "carla/NonCopyable.h"
+#include "carla/client/Timestamp.h"
 #include "carla/sensor/data/ActorDynamicState.h"
 #include "carla/sensor/data/RawEpisodeState.h"
 
@@ -31,19 +32,8 @@ namespace detail {
       geom::Vector3D acceleration;
     };
 
-    /// @copydoc carla::sensor::SensorData::GetFrameNumber()
-    size_t GetFrameNumber() const {
-      return _frame_number;
-    }
-
-    /// @copydoc carla::sensor::data::RawEpisodeState::GetGameTimeStamp()
-    double GetGameTimeStamp() const {
-      return _game_timestamp;
-    }
-
-    /// @copydoc carla::sensor::data::RawEpisodeState::GetPlatformTimeStamp()
-    double GetPlatformTimeStamp() const {
-      return _platform_timestamp;
+    const auto &GetTimestamp() const {
+      return _timestamp;
     }
 
     ActorState GetActorState(actor_id_type id) const {
@@ -68,11 +58,7 @@ namespace detail {
 
   private:
 
-    size_t _frame_number = 0.0;
-
-    double _game_timestamp = 0.0;
-
-    double _platform_timestamp = 0.0;
+    Timestamp _timestamp;
 
     std::unordered_map<actor_id_type, ActorState> _actors;
   };

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

@@ -95,6 +95,17 @@ namespace detail {
       return _client.GetServerVersion();
     }
 
+    /// @}
+    // =========================================================================
+    /// @name Tick
+    // =========================================================================
+    /// @{
+
+    Timestamp WaitForTick(time_duration timeout) {
+      DEBUG_ASSERT(_episode != nullptr);
+      return _episode->WaitForState(timeout);
+    }
+
     /// @}
     // =========================================================================
     /// @name Access to global objects in the episode

PythonAPI/source/libcarla/Client.cpp

@@ -9,8 +9,7 @@
 #include <carla/client/World.h>
 
 static void SetTimeout(carla::client::Client &client, double seconds) {
-  size_t ms = static_cast<size_t>(1e3 * seconds);
-  client.SetTimeout(carla::time_duration::milliseconds(ms));
+  client.SetTimeout(TimeDurationFromSeconds(seconds));
 }
 
 void export_client() {

PythonAPI/source/libcarla/World.cpp

@@ -18,6 +18,14 @@ namespace client {
     return PrintList(out, actors);
   }
 
+  std::ostream &operator<<(std::ostream &out, const Timestamp &timestamp) {
+    out << "Timestamp(frame_count=" << timestamp.frame_count
+        << ",elapsed_seconds=" << timestamp.elapsed_seconds
+        << ",delta_seconds=" << timestamp.delta_seconds
+        << ",platform_timestamp=" << timestamp.platform_timestamp << ')';
+    return out;
+  }
+
   std::ostream &operator<<(std::ostream &out, const World &world) {
     out << "World(id=" << world.GetId() << ",map_name=" << world.GetMapName() << ')';
     return out;
@@ -26,11 +34,31 @@ namespace client {
 } // namespace client
 } // namespace carla
 
+static auto WaitForTick(const carla::client::World &world, double seconds) {
+  carla::PythonUtil::ReleaseGIL unlock;
+  return world.WaitForTick(TimeDurationFromSeconds(seconds));
+}
+
 void export_world() {
   using namespace boost::python;
   namespace cc = carla::client;
   namespace cg = carla::geom;
 
+  class_<cc::Timestamp>("Timestamp")
+    .def(init<size_t, double, double, double>(
+        (arg("frame_count")=0u,
+         arg("elapsed_seconds")=0.0,
+         arg("delta_seconds")=0.0,
+         arg("platform_timestamp")=0.0)))
+    .def_readwrite("frame_count", &cc::Timestamp::frame_count)
+    .def_readwrite("elapsed_seconds", &cc::Timestamp::elapsed_seconds)
+    .def_readwrite("delta_seconds", &cc::Timestamp::delta_seconds)
+    .def_readwrite("platform_timestamp", &cc::Timestamp::platform_timestamp)
+    .def("__eq__", &cc::Timestamp::operator==)
+    .def("__ne__", &cc::Timestamp::operator!=)
+    .def(self_ns::str(self_ns::self))
+  ;
+
   class_<cc::ActorList, boost::shared_ptr<cc::ActorList>>("ActorList", no_init)
     .def("filter", &cc::ActorList::Filter)
     .def("__getitem__", &cc::ActorList::at)
@@ -59,6 +87,7 @@ void export_world() {
     .def("get_actors", CONST_CALL_WITHOUT_GIL(cc::World, GetActors))
     .def("try_spawn_actor", SPAWN_ACTOR_WITHOUT_GIL(TrySpawnActor))
     .def("spawn_actor", SPAWN_ACTOR_WITHOUT_GIL(SpawnActor))
+    .def("wait_for_tick", &WaitForTick, (arg("seconds")=1.0))
     .def(self_ns::str(self_ns::self))
   ;
 

PythonAPI/source/libcarla/libcarla.cpp

@@ -6,6 +6,7 @@
 
 #include <carla/Memory.h>
 #include <carla/PythonUtil.h>
+#include <carla/Time.h>
 
 #include <ostream>
 #include <type_traits>
@@ -60,6 +61,11 @@ static std::ostream &PrintList(std::ostream &out, const Iterable &list) {
   return out;
 }
 
+static carla::time_duration TimeDurationFromSeconds(double seconds) {
+  size_t ms = static_cast<size_t>(1e3 * seconds);
+  return carla::time_duration::milliseconds(ms);
+}
+
 #include "Actor.cpp"
 #include "Blueprint.cpp"
 #include "Client.cpp"