Fix #1381, make sensors use timestamp from UCarlaEpisode

LibCarla/source/carla/sensor/SensorData.h

@@ -40,7 +40,7 @@ namespace sensor {
       return _frame_number;
     }
 
-    /// Time the data was generated.
+    /// Simulation-time when the data was generated.
     double GetTimestamp() const {
       return _timestamp;
     }

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

@@ -45,7 +45,7 @@ namespace data {
     /// Simulation time-stamp, simulated seconds elapsed since the beginning of
     /// the current episode.
     double GetGameTimeStamp() const {
-      return GetHeader().game_timestamp;
+      return GetTimestamp();
     }
 
     /// Time-stamp of the frame at which this measurement was taken, in seconds

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

@@ -30,7 +30,6 @@ namespace s11n {
 #pragma pack(push, 1)
     struct Header {
       uint64_t episode_id;
-      double game_timestamp;
       double platform_timestamp;
       float delta_seconds;
     };

PythonAPI/test/smoke/test_sync.py

@@ -53,6 +53,7 @@ class TestSynchronousMode(SmokeTest):
 
                 image = image_queue.get()
                 self.assertEqual(image.frame_number, ts.frame_count)
+                self.assertEqual(image.timestamp, ts.elapsed_seconds)
 
         finally:
             camera.destroy()

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

@@ -63,7 +63,10 @@ private:
 
   /// @pre This functions needs to be called in the game-thread.
   template <typename SensorT>
-  explicit FAsyncDataStreamTmpl(const SensorT &InSensor, float Timepoint, StreamType InStream);
+  explicit FAsyncDataStreamTmpl(
+      const SensorT &InSensor,
+      double Timestamp,
+      StreamType InStream);
 
   StreamType Stream;
 
@@ -95,15 +98,15 @@ template <typename T>
 template <typename SensorT>
 inline FAsyncDataStreamTmpl<T>::FAsyncDataStreamTmpl(
     const SensorT &Sensor,
-    float Timepoint,
+    double Timestamp,
     StreamType InStream)
   : Stream(std::move(InStream)),
-    Header([&Sensor, Timepoint]() {
+    Header([&Sensor, Timestamp]() {
       check(IsInGameThread());
       using Serializer = carla::sensor::s11n::SensorHeaderSerializer;
       return Serializer::Serialize(
           carla::sensor::SensorRegistry::template get<SensorT*>::index,
           GFrameCounter,
-          Timepoint,
+          Timestamp,
           Sensor.GetActorTransform());
     }()) {}

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

@@ -38,37 +38,24 @@ void ACollisionSensor::SetOwner(AActor *NewOwner)
   }
 }
 
-void ACollisionSensor::BeginPlay()
-{
-  Super::BeginPlay();
-
-  auto *GameMode = Cast<ATheNewCarlaGameModeBase>(GetWorld()->GetAuthGameMode());
-
-  if (GameMode == nullptr)
-  {
-    UE_LOG(LogCarla, Error, TEXT("ACollisionSensor: Game mode not compatible with this sensor"));
-    return;
-  }
-  Episode = &GameMode->GetCarlaEpisode();
-}
-
 void ACollisionSensor::OnCollisionEvent(
     AActor *Actor,
     AActor *OtherActor,
     FVector NormalImpulse,
     const FHitResult &Hit)
 {
-  if ((Episode != nullptr) && (Actor != nullptr) && (OtherActor != nullptr))
+  if ((Actor != nullptr) && (OtherActor != nullptr))
   {
+    const auto &Episode = GetEpisode();
     constexpr float TO_METERS = 1e-2;
     NormalImpulse *= TO_METERS;
-    GetDataStream(*this, Actor->GetWorld()->GetTimeSeconds()).Send(
+    GetDataStream(*this).Send(
         *this,
-        Episode->SerializeActor(Episode->FindOrFakeActor(Actor)),
-        Episode->SerializeActor(Episode->FindOrFakeActor(OtherActor)),
+        Episode.SerializeActor(Episode.FindOrFakeActor(Actor)),
+        Episode.SerializeActor(Episode.FindOrFakeActor(OtherActor)),
         carla::geom::Vector3D{NormalImpulse.X, NormalImpulse.Y, NormalImpulse.Z});
     // record the collision event
-    if (Episode->GetRecorder()->IsEnabled())
-      Episode->GetRecorder()->AddCollision(Actor, OtherActor);
+    if (Episode.GetRecorder()->IsEnabled())
+      Episode.GetRecorder()->AddCollision(Actor, OtherActor);
   }
 }

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

@@ -28,8 +28,6 @@ public:
 
   void SetOwner(AActor *NewOwner) override;
 
-  void BeginPlay() override;
-
 private:
 
   UFUNCTION()
@@ -38,7 +36,4 @@ private:
       AActor *OtherActor,
       FVector NormalImpulse,
       const FHitResult &Hit);
-
-  UPROPERTY()
-  const UCarlaEpisode *Episode = nullptr;
 };

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

@@ -37,7 +37,7 @@ public:
   ///
   /// @pre This functions needs to be called in the game-thread.
   template <typename SensorT>
-  auto MakeAsyncDataStream(const SensorT &Sensor, float Timestamp)
+  auto MakeAsyncDataStream(const SensorT &Sensor, double Timestamp)
   {
     check(Stream.has_value());
     return FAsyncDataStreamTmpl<T>{Sensor, Timestamp, *Stream};

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

@@ -49,27 +49,13 @@ void AObstacleDetectionSensor::Set(const FActorDescription &Description)
 
 }
 
-void AObstacleDetectionSensor::BeginPlay()
-{
-  Super::BeginPlay();
-
-  auto *GameMode = Cast<ATheNewCarlaGameModeBase>(GetWorld()->GetAuthGameMode());
-
-  if (GameMode == nullptr)
-  {
-    UE_LOG(LogCarla, Error, TEXT("AObstacleDetectionSensor: Game mode not compatible with this sensor"));
-    return;
-  }
-  Episode = &GameMode->GetCarlaEpisode();
-}
-
 void AObstacleDetectionSensor::Tick(float DeltaSeconds)
 {
   Super::Tick(DeltaSeconds);
 
   const FVector &Start = GetActorLocation();
   const FVector &End = Start + (GetActorForwardVector() * Distance);
-  UWorld* currentWorld = GetWorld();
+  UWorld* CurrentWorld = GetWorld();
 
   // Struct in which the result of the scan will be saved
   FHitResult HitOut = FHitResult();
@@ -82,7 +68,7 @@ void AObstacleDetectionSensor::Tick(float DeltaSeconds)
   if (bDebugLineTrace)
   {
     const FName TraceTag("ObstacleDebugTrace");
-    currentWorld->DebugDrawTraceTag = TraceTag;
+    CurrentWorld->DebugDrawTraceTag = TraceTag;
     TraceParams.TraceTag = TraceTag;
   }
 
@@ -108,7 +94,7 @@ void AObstacleDetectionSensor::Tick(float DeltaSeconds)
     // If we go only for dynamics, we check the object type AllDynamicObjects
     FCollisionObjectQueryParams TraceChannel = FCollisionObjectQueryParams(
         FCollisionObjectQueryParams::AllDynamicObjects);
-    isHitReturned = currentWorld->SweepSingleByObjectType(
+    isHitReturned = CurrentWorld->SweepSingleByObjectType(
         HitOut,
         Start,
         End,
@@ -122,7 +108,7 @@ void AObstacleDetectionSensor::Tick(float DeltaSeconds)
     // Else, if we go for everything, we get everything that interacts with a
     // Pawn
     ECollisionChannel TraceChannel = ECC_WorldStatic;
-    isHitReturned = currentWorld->SweepSingleByChannel(
+    isHitReturned = CurrentWorld->SweepSingleByChannel(
         HitOut,
         Start,
         End,
@@ -134,7 +120,7 @@ void AObstacleDetectionSensor::Tick(float DeltaSeconds)
 
   if (isHitReturned)
   {
-    OnObstacleDetectionEvent(this, HitOut.Actor.Get(), HitOut.Distance, HitOut, currentWorld->GetTimeSeconds());
+    OnObstacleDetectionEvent(this, HitOut.Actor.Get(), HitOut.Distance, HitOut);
   }
 }
 
@@ -142,14 +128,14 @@ void AObstacleDetectionSensor::OnObstacleDetectionEvent(
     AActor *Actor,
     AActor *OtherActor,
     float HitDistance,
-    const FHitResult &Hit,
-    float Timestamp)
+    const FHitResult &Hit)
 {
-  if ((Episode != nullptr) && (Actor != nullptr) && (OtherActor != nullptr))
+  if ((Actor != nullptr) && (OtherActor != nullptr))
   {
-    GetDataStream(*this, Timestamp).Send(*this,
-        Episode->SerializeActor(Episode->FindOrFakeActor(Actor)),
-        Episode->SerializeActor(Episode->FindOrFakeActor(OtherActor)),
+    const auto &Episode = GetEpisode();
+    GetDataStream(*this).Send(*this,
+        Episode.SerializeActor(Episode.FindOrFakeActor(Actor)),
+        Episode.SerializeActor(Episode.FindOrFakeActor(OtherActor)),
         HitRadius);
   }
 }

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

@@ -27,8 +27,6 @@ public:
 
   void Set(const FActorDescription &Description) override;
 
-  void BeginPlay() override;
-
   void Tick(float DeltaSeconds) override;
 
 private:
@@ -38,11 +36,7 @@ private:
       AActor *Actor,
       AActor *OtherActor,
       float Distance,
-      const FHitResult &Hit,
-      float Timestamp);
-
-  UPROPERTY()
-  const UCarlaEpisode *Episode = nullptr;
+      const FHitResult &Hit);
 
 private:
 

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

@@ -87,7 +87,7 @@ void FPixelReader::SendPixelsInRenderThread(TSensor &Sensor)
 
   // First we create the message header (needs to be created in the
   // game-thread).
-  auto AsyncStream = Sensor.GetDataStream(Sensor, Sensor.GetWorld()->GetTimeSeconds());
+  auto AsyncStream = Sensor.GetDataStream(Sensor);
 
   // We need a shared ptr here because UE4 macros do not move the arguments -_-
   auto StreamPtr = std::make_shared<decltype(AsyncStream)>(std::move(AsyncStream));

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

@@ -69,7 +69,7 @@ void ARayCastLidar::Tick(const float DeltaTime)
 
   ReadPoints(DeltaTime);
 
-  auto DataStream = GetDataStream(*this, GetWorld()->GetTimeSeconds());
+  auto DataStream = GetDataStream(*this);
   DataStream.Send(*this, LidarMeasurement, DataStream.PopBufferFromPool());
 }
 

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

@@ -6,6 +6,7 @@
 
 #pragma once
 
+#include "Carla/Game/CarlaEpisode.h"
 #include "Carla/Sensor/DataStream.h"
 
 #include "GameFramework/Actor.h"
@@ -22,6 +23,11 @@ class CARLA_API ASensor : public AActor
 
 public:
 
+  void SetEpisode(const UCarlaEpisode &InEpisode)
+  {
+    Episode = &InEpisode;
+  }
+
   virtual void Set(const FActorDescription &Description);
 
   /// Replace the FDataStream associated with this sensor.
@@ -42,17 +48,25 @@ protected:
 
   void EndPlay(EEndPlayReason::Type EndPlayReason) override;
 
+  const UCarlaEpisode &GetEpisode() const
+  {
+    check(Episode != nullptr);
+    return *Episode;
+  }
+
   /// Return the FDataStream associated with this sensor.
   ///
   /// You need to provide a reference to self, this is necessary for template
   /// deduction.
   template <typename SensorT>
-  FAsyncDataStream GetDataStream(const SensorT &Self, float Timestamp)
+  FAsyncDataStream GetDataStream(const SensorT &Self)
   {
-    return Stream.MakeAsyncDataStream(Self, Timestamp);
+    return Stream.MakeAsyncDataStream(Self, GetEpisode().GetElapsedGameTime());
   }
 
 private:
 
   FDataStream Stream;
+
+  const UCarlaEpisode *Episode = nullptr;
 };

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

@@ -131,6 +131,9 @@ FActorSpawnResult ASensorFactory::SpawnActor(
   }
   else
   {
+    auto *Episode = GameInstance->GetCarlaEpisode();
+    check(Episode != nullptr);
+    Sensor->SetEpisode(*Episode);
     Sensor->Set(Description);
     Sensor->SetDataStream(GameInstance->GetServer().OpenStream());
   }

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

@@ -121,7 +121,6 @@ static carla::Buffer FWorldObserver_Serialize(
   // Write header.
   Serializer::Header header;
   header.episode_id = Episode.GetId();
-  header.game_timestamp = Episode.GetElapsedGameTime();
   header.platform_timestamp = FPlatformTime::Seconds();
   header.delta_seconds = DeltaSeconds;
   write_data(header);
@@ -150,7 +149,7 @@ static carla::Buffer FWorldObserver_Serialize(
 
 void FWorldObserver::BroadcastTick(const UCarlaEpisode &Episode, float DeltaSeconds)
 {
-  auto AsyncStream = Stream.MakeAsyncDataStream(*this, Episode.GetWorld()->GetTimeSeconds());
+  auto AsyncStream = Stream.MakeAsyncDataStream(*this, Episode.GetElapsedGameTime());
 
   auto buffer = FWorldObserver_Serialize(
       AsyncStream.PopBufferFromPool(),