Add world observer sensor

LibCarla/source/carla/sensor/SensorRegistry.h

@@ -14,6 +14,7 @@
 // =============================================================================
 
 // 1. Include the serializer here.
+#include "carla/sensor/s11n/EpisodeStateSerializer.h"
 #include "carla/sensor/s11n/ImageSerializer.h"
 #include "carla/sensor/s11n/LidarSerializer.h"
 
@@ -22,6 +23,7 @@ class ADepthCamera;
 class ARayCastLidar;
 class ASceneCaptureCamera;
 class ASemanticSegmentationCamera;
+class AWorldObserver;
 
 namespace carla {
 namespace sensor {
@@ -31,6 +33,7 @@ namespace sensor {
   /// Contains a registry of all the sensors available and allows serializing
   /// and deserializing sensor data for the types registered.
   using SensorRegistry = CompositeSerializer<
+    std::pair<AWorldObserver *, s11n::EpisodeStateSerializer>,
     std::pair<ASceneCaptureCamera *, s11n::ImageSerializer>,
     std::pair<ADepthCamera *, s11n::ImageSerializer>,
     std::pair<ASemanticSegmentationCamera *, s11n::ImageSerializer>,
@@ -49,5 +52,6 @@ namespace sensor {
 #include "Carla/Sensor/RayCastLidar.h"
 #include "Carla/Sensor/SceneCaptureCamera.h"
 #include "Carla/Sensor/SemanticSegmentationCamera.h"
+#include "Carla/Sensor/WorldObserver.h"
 
 #endif // LIBCARLA_SENSOR_REGISTRY_WITH_SENSOR_INCLUDES

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

@@ -0,0 +1,106 @@
+// 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/Buffer.h"
+#include "carla/Debug.h"
+#include "carla/geom/Transform.h"
+#include "carla/geom/Vector3D.h"
+
+class FActorRegistry;
+
+namespace carla {
+namespace sensor {
+namespace s11n {
+
+  /// Serializes the current state of the whole episode.
+  class EpisodeStateSerializer {
+  public:
+
+#pragma pack(push, 1)
+
+    struct Header {
+      uint64_t frame_number;
+      double game_timestamp;
+      double platform_timestamp;
+      uint64_t number_of_actors;
+    };
+
+    struct ActorInfo {
+      uint32_t id;
+      geom::Transform transform;
+      geom::Vector3D velocity;
+    };
+
+#pragma pack(pop)
+
+    constexpr static auto header_offset = sizeof(Header);
+
+    static const Header &DeserializeHeader(const Buffer &message) {
+      return *reinterpret_cast<const Header *>(message.data());
+    }
+
+    template <typename SensorT, typename ActorRegistryT>
+    static Buffer Serialize(
+        const SensorT &sensor,
+        Buffer buffer,
+        uint64_t frame_number,
+        double game_timestamp,
+        double platform_timestamp,
+        const ActorRegistryT &actor_registry);
+
+    static SharedPtr<SensorData> Deserialize(RawData data);
+  };
+
+#ifdef LIBCARLA_INCLUDED_FROM_UE4
+
+  template <typename SensorT, typename ActorRegistryT>
+  Buffer EpisodeStateSerializer::Serialize(
+      const SensorT &,
+      Buffer buffer,
+      uint64_t frame_number,
+      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(ActorInfo) * number_of_actors);
+    auto begin = buffer.begin();
+    auto write_data = [&begin](const auto &data) {
+      std::memcpy(begin, &data, sizeof(data));
+      begin += sizeof(data);
+    };
+    // Write header.
+    Header header = {
+      frame_number,
+      game_timestamp,
+      platform_timestamp,
+      number_of_actors
+    };
+    write_data(header);
+    // Write every actor.
+    for (auto &&pair : actor_registry) {
+      auto &&actor_view = pair.second;
+      DEBUG_ASSERT(actor_view.GetActor() != nullptr);
+      constexpr float TO_METERS = 1e-3;
+      const auto velocity = TO_METERS * actor_view.GetActor()->GetVelocity();
+      ActorInfo info = {
+        actor_view.GetActorId(),
+        actor_view.GetActor()->GetActorTransform(),
+        geom::Vector3D{velocity.X, velocity.Y, velocity.Z}
+      };
+      write_data(info);
+    }
+    DEBUG_ASSERT(begin == buffer.end());
+    return buffer;
+  }
+
+#endif // LIBCARLA_INCLUDED_FROM_UE4
+
+} // namespace s11n
+} // namespace sensor
+} // namespace carla

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

@@ -17,15 +17,25 @@
 #include <carla/sensor/SensorRegistry.h>
 #undef LIBCARLA_SENSOR_REGISTRY_WITH_SENSOR_INCLUDES
 
+#include <type_traits>
+
+// =============================================================================
+// -- FSensorDefinitionGatherer ------------------------------------------------
+// =============================================================================
+
+/// Retrieve the definitions of all the sensors registered in the
+/// SensorRegistry by calling their static method
+/// SensorType::GetSensorDefinition().
+///
+/// @note To make this class ignore a given sensor, define a public member
+/// "not_spawnable" that defines a type. If so, the sensor won't be spawned by
+/// this factory.
 class FSensorDefinitionGatherer
 {
   using Registry = carla::sensor::SensorRegistry;
 
 public:
 
-  /// Retrieve the definitions of all the sensors registered in the
-  /// SensorRegistry by calling their static method
-  /// SensorType::GetSensorDefinition().
   static auto GetSensorDefinitions()
   {
     TArray<FActorDefinition> Definitions;
@@ -36,15 +46,39 @@ public:
 
 private:
 
+  // Type traits for detecting if a sensor is spawnable.
+
+  template<typename T>
+  struct void_type { typedef void type; };
+
+  template<typename T, typename = void>
+  struct is_spawnable : std::true_type {};
+
+  template<typename T>
+  struct is_spawnable<T, typename void_type<typename T::not_spawnable>::type > : std::false_type {};
+
+  // AppendDefinitions implementations.
+
+  template <typename SensorType>
+  static typename std::enable_if<is_spawnable<SensorType>::value, void>::type
+  AppendDefinitions(TArray<FActorDefinition> &Definitions)
+  {
+    auto Def = SensorType::GetSensorDefinition();
+    // Make sure the class matches the sensor type.
+    Def.Class = SensorType::StaticClass();
+    Definitions.Emplace(Def);
+  }
+
+  template <typename SensorType>
+  static typename std::enable_if<!is_spawnable<SensorType>::value, void>::type
+  AppendDefinitions(TArray<FActorDefinition> &) {}
+
   template <size_t Index>
   static void AppendDefinitions(TArray<FActorDefinition> &Definitions)
   {
     using SensorPtrType = typename Registry::get_by_index<Index>::key;
     using SensorType = typename std::remove_pointer<SensorPtrType>::type;
-    auto Def = SensorType::GetSensorDefinition();
-    // Make sure the class matches the sensor type.
-    Def.Class = SensorType::StaticClass();
-    Definitions.Emplace(Def);
+    AppendDefinitions<SensorType>(Definitions);
   }
 
   template <size_t... Is>
@@ -56,6 +90,10 @@ private:
   }
 };
 
+// =============================================================================
+// -- ASensorFactory -----------------------------------------------------------
+// =============================================================================
+
 TArray<FActorDefinition> ASensorFactory::GetDefinitions()
 {
   return FSensorDefinitionGatherer::GetSensorDefinitions();
@@ -68,7 +106,7 @@ FActorSpawnResult ASensorFactory::SpawnActor(
   auto *World = GetWorld();
   if (World == nullptr)
   {
-    UE_LOG(LogCarla, Error, TEXT("ASensorFactory: cannot spawn sensor into empty world."));
+    UE_LOG(LogCarla, Error, TEXT("ASensorFactory: cannot spawn sensor into an empty world."));
     return {};
   }
   auto *Sensor = World->SpawnActorDeferred<ASensor>(

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

@@ -0,0 +1,36 @@
+// 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.h"
+#include "Carla/Sensor/WorldObserver.h"
+
+#include "CoreGlobals.h"
+
+#include <compiler/disable-ue4-macros.h>
+#include <carla/sensor/SensorRegistry.h>
+#include <compiler/enable-ue4-macros.h>
+
+void AWorldObserver::BeginPlay()
+{
+  Super::BeginPlay();
+  check(Stream.has_value());
+  check(Episode != nullptr);
+}
+
+void AWorldObserver::Tick(float DeltaSeconds)
+{
+  Super::Tick(DeltaSeconds);
+
+  GameTimeStamp += DeltaSeconds;
+
+  (*Stream).Write(carla::sensor::SensorRegistry::Serialize(
+      *this,
+      (*Stream).MakeBuffer(),
+      GFrameCounter,
+      GameTimeStamp,
+      FPlatformTime::Seconds(),
+      Episode->GetActorRegistry()));
+}

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

@@ -0,0 +1,56 @@
+// 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 "GameFramework/Actor.h"
+
+#include "Carla/Game/CarlaEpisode.h"
+
+#include <compiler/disable-ue4-macros.h>
+#include <carla/Optional.h>
+#include <carla/streaming/Stream.h>
+#include <compiler/enable-ue4-macros.h>
+
+#include "WorldObserver.generated.h"
+
+/// Serializes and sends all the actors in a UCarlaEpisode.
+UCLASS()
+class CARLA_API AWorldObserver : public AActor
+{
+  GENERATED_BODY()
+
+public:
+
+  using not_spawnable = void;
+
+  /// Set the episode that will observe.
+  void SetEpisode(UCarlaEpisode &InEpisode)
+  {
+    checkf(Episode == nullptr, TEXT("Cannot set episode twice!"));
+    Episode = &InEpisode;
+  }
+
+  /// Replace the Stream associated with this sensor.
+  ///
+  /// @warning Do not change the stream after BeginPlay. It is not thread-safe.
+  void SetStream(carla::streaming::MultiStream InStream)
+  {
+    Stream = std::move(InStream);
+  }
+
+  void BeginPlay() final;
+
+  void Tick(float DeltaSeconds) final;
+
+private:
+
+  UCarlaEpisode *Episode = nullptr;
+
+  carla::Optional<carla::streaming::MultiStream> Stream;
+
+  double GameTimeStamp = 0.0;
+};