Add acceleration and delta-time to world observer message to make data self contained

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

@@ -43,15 +43,14 @@ namespace detail {
       auto self = weak.lock();
       if (self != nullptr) {
         auto data = sensor::Deserializer::Deserialize(std::move(buffer));
-        const auto &raw_data = CastData(*data);
 
-        std::shared_ptr<const EpisodeState> next;
+        auto next = std::make_shared<const EpisodeState>(CastData(*data));
-        std::shared_ptr<const EpisodeState> prev = self->GetState();
+        auto prev = self->GetState();
         do {
-          if (prev->GetFrameCount() >= raw_data.GetFrameNumber()) {
+          if (prev->GetFrameCount() >= next->GetFrameCount()) {
+            self->_on_tick_callbacks.Call(next->GetTimestamp());
             return;
           }
-          next = prev->DeriveNextStep(raw_data);
         } while (!self->_state.compare_exchange(&prev, next));
 
         if (next->GetEpisodeId() != prev->GetEpisodeId()) {

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

@@ -10,38 +10,26 @@ namespace carla {
 namespace client {
 namespace detail {
 
-  static auto DeriveAcceleration(
+  EpisodeState::EpisodeState(const sensor::data::RawEpisodeState &state)
-      double delta_seconds,
+    : _episode_id(state.GetEpisodeId()),
-      const geom::Vector3D &v0,
+      _timestamp(
-      const geom::Vector3D &v1) {
+          state.GetFrameNumber(),
-    /// @todo add methods to Vector3D for scalar multiplication.
+          state.GetGameTimeStamp(),
-    auto acc = v1 - v0;
+          state.GetDeltaSeconds(),
-    acc.x /= delta_seconds;
+          state.GetPlatformTimeStamp()) {
-    acc.y /= delta_seconds;
+    _actors.reserve(state.size());
-    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>(state.GetEpisodeId());
-    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(
-          next->_timestamp.delta_seconds,
-          GetActorState(actor.id).velocity,
-          actor.velocity);
       DEBUG_ONLY(auto result = )
-      next->_actors.emplace(
+      _actors.emplace(
           actor.id,
-          ActorState{actor.transform, actor.velocity, actor.angular_velocity, acceleration, actor.state});
+          ActorState{
+              actor.transform,
+              actor.velocity,
+              actor.angular_velocity,
+              actor.acceleration,
+              actor.state});
       DEBUG_ASSERT(result.second);
     }
-    return next;
   }
 
 } // namespace detail

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

@@ -36,6 +36,8 @@ namespace detail {
 
     explicit EpisodeState(uint64_t episode_id) : _episode_id(episode_id) {}
 
+    explicit EpisodeState(const sensor::data::RawEpisodeState &state);
+
     auto GetEpisodeId() const {
       return _episode_id;
     }
@@ -65,14 +67,11 @@ namespace detail {
           iterator::make_map_keys_iterator(_actors.end()));
     }
 
-    std::shared_ptr<const EpisodeState> DeriveNextStep(
-        const sensor::data::RawEpisodeState &state) const;
-
   private:
 
     const uint64_t _episode_id;
 
-    Timestamp _timestamp;
+    const Timestamp _timestamp;
 
     std::unordered_map<actor_id_type, ActorState> _actors;
   };

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

@@ -117,6 +117,8 @@ namespace detail {
 
     geom::Vector3D angular_velocity;
 
+    geom::Vector3D acceleration;
+
     union TypeDependentState {
       detail::TrafficLightData traffic_light_data;
       detail::VehicleData vehicle_data;
@@ -127,7 +129,7 @@ namespace detail {
 #pragma pack(pop)
 
   static_assert(
-      sizeof(ActorDynamicState) == 13u * sizeof(uint32_t) + sizeof(detail::VehicleData),
+      sizeof(ActorDynamicState) == 16u * sizeof(uint32_t) + sizeof(detail::VehicleData),
       "Invalid ActorDynamicState size!");
 
 } // namespace data

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

@@ -53,6 +53,11 @@ namespace data {
     double GetPlatformTimeStamp() const {
       return GetHeader().platform_timestamp;
     }
+
+    /// Simulated seconds elapsed since previous frame.
+    double GetDeltaSeconds() const {
+      return GetHeader().delta_seconds;
+    }
   };
 
 } // namespace data

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

@@ -32,6 +32,7 @@ namespace s11n {
       uint64_t episode_id;
       double game_timestamp;
       double platform_timestamp;
+      float delta_seconds;
     };
 #pragma pack(pop)
 

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorInfo.h

@@ -25,4 +25,7 @@ public:
   FBoundingBox BoundingBox;
 
   carla::rpc::Actor SerializedData;
+
+  /// @todo To be used solely by the FWorldObserver.
+  mutable FVector Velocity = {0.0f, 0.0f, 0.0f};
 };

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaEngine.cpp

@@ -69,7 +69,7 @@ void FCarlaEngine::OnPreTick(ELevelTick TickType, float DeltaSeconds)
   if ((TickType == ELevelTick::LEVELTICK_All) && (CurrentEpisode != nullptr))
   {
     CurrentEpisode->TickTimers(DeltaSeconds);
-    WorldObserver.BroadcastTick(*CurrentEpisode);
+    WorldObserver.BroadcastTick(*CurrentEpisode, DeltaSeconds);
   }
 }
 

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

@@ -78,9 +78,31 @@ static auto FWorldObserver_GetActorState(const FActorView &View, const FActorReg
   return state;
 }
 
+static carla::geom::Vector3D FWorldObserver_GetAngularVelocity(const AActor &Actor)
+{
+  const auto RootComponent = Cast<UPrimitiveComponent>(Actor.GetRootComponent());
+  const FVector AngularVelocity =
+      RootComponent != nullptr ?
+          RootComponent->GetPhysicsAngularVelocityInDegrees() :
+          FVector{0.0f, 0.0f, 0.0f};
+  return {AngularVelocity.X, AngularVelocity.Y, AngularVelocity.Z};
+}
+
+static carla::geom::Vector3D FWorldObserver_GetAcceleration(
+    const FActorView &View,
+    const FVector &Velocity,
+    const float DeltaSeconds)
+{
+  FVector &PreviousVelocity = View.GetActorInfo()->Velocity;
+  const FVector Acceleration = (Velocity - PreviousVelocity) / DeltaSeconds;
+  PreviousVelocity = Velocity;
+  return {Acceleration.X, Acceleration.Y, Acceleration.Z};
+}
+
 static carla::Buffer FWorldObserver_Serialize(
     carla::Buffer buffer,
-    const UCarlaEpisode &Episode)
+    const UCarlaEpisode &Episode,
+    float DeltaSeconds)
 {
   using Serializer = carla::sensor::s11n::EpisodeStateSerializer;
   using ActorDynamicState = carla::sensor::data::ActorDynamicState;
@@ -101,6 +123,7 @@ static carla::Buffer FWorldObserver_Serialize(
   header.episode_id = Episode.GetId();
   header.game_timestamp = Episode.GetElapsedGameTime();
   header.platform_timestamp = FPlatformTime::Seconds();
+  header.delta_seconds = DeltaSeconds;
   write_data(header);
 
   // Write every actor.
@@ -108,19 +131,14 @@ static carla::Buffer FWorldObserver_Serialize(
   {
     check(View.IsValid());
     constexpr float TO_METERS = 1e-2;
-    const auto velocity = TO_METERS * View.GetActor()->GetVelocity();
+    const auto Velocity = TO_METERS * View.GetActor()->GetVelocity();
-    // get the angular velocity
+
-    const auto RootComponent = Cast<UPrimitiveComponent>(View.GetActor()->GetRootComponent());
-    FVector angularVelocity { 0.0f, 0.0f, 0.0f };
-    if (RootComponent != nullptr)
-    {
-      angularVelocity = RootComponent->GetPhysicsAngularVelocityInDegrees();
-    }
     ActorDynamicState info = {
       View.GetActorId(),
       View.GetActor()->GetActorTransform(),
-      carla::geom::Vector3D{velocity.X, velocity.Y, velocity.Z},
+      carla::geom::Vector3D{Velocity.X, Velocity.Y, Velocity.Z},
-      carla::geom::Vector3D{angularVelocity.X, angularVelocity.Y, angularVelocity.Z},
+      FWorldObserver_GetAngularVelocity(*View.GetActor()),
+      FWorldObserver_GetAcceleration(View, Velocity, DeltaSeconds),
       FWorldObserver_GetActorState(View, Registry)
     };
     write_data(info);
@@ -130,13 +148,14 @@ static carla::Buffer FWorldObserver_Serialize(
   return buffer;
 }
 
-void FWorldObserver::BroadcastTick(const UCarlaEpisode &Episode)
+void FWorldObserver::BroadcastTick(const UCarlaEpisode &Episode, float DeltaSeconds)
 {
   auto AsyncStream = Stream.MakeAsyncDataStream(*this);
 
   auto buffer = FWorldObserver_Serialize(
       AsyncStream.PopBufferFromPool(),
-      Episode);
+      Episode,
+      DeltaSeconds);
 
   AsyncStream.Send(*this, std::move(buffer));
 }

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

@@ -32,7 +32,7 @@ public:
 
   /// Send a message to every connected client with the info about the given @a
   /// Episode.
-  void BroadcastTick(const UCarlaEpisode &Episode);
+  void BroadcastTick(const UCarlaEpisode &Episode, float DeltaSeconds);
 
   /// Dummy. Required for compatibility with other sensors only.
   FTransform GetActorTransform() const