From 1fb733c085b1ec70e02f0d0ac7446318e95cb09e Mon Sep 17 00:00:00 2001
From: doterop <daniel.oterop@gmail.com>
Date: Wed, 4 Mar 2020 02:35:29 +0100
Subject: [PATCH] Fixed buffer resize when parsing components

---
 LibCarla/source/carla/Buffer.h                | 12 +++++++
 .../Source/Carla/Sensor/WorldObserver.cpp     | 34 ++++++++++++++-----
 2 files changed, 38 insertions(+), 8 deletions(-)

diff --git a/LibCarla/source/carla/Buffer.h b/LibCarla/source/carla/Buffer.h
index 17059349d..3ece378b6 100644
--- a/LibCarla/source/carla/Buffer.h
+++ b/LibCarla/source/carla/Buffer.h
@@ -263,6 +263,18 @@ namespace carla {
       reset(static_cast<size_type>(size));
     }
 
+    /// Resize the buffer, a new block of size @a size is
+    /// allocated if the capacity is not enough and the data is copied.
+    void resize(uint64_t size) {
+      if(_capacity < size) {
+        std::unique_ptr<value_type[]> data = std::move(_data);
+        uint64_t old_size = size;
+        reset(size);
+        copy_from(data.get(), static_cast<size_type>(old_size));
+      }
+      _size = static_cast<size_type>(size);
+    }
+
     /// Release the contents of this buffer and set its size and capacity to
     /// zero.
     std::unique_ptr<value_type[]> pop() noexcept {
diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/WorldObserver.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/WorldObserver.cpp
index d92a0ce6d..dda78fec1 100644
--- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/WorldObserver.cpp
+++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/WorldObserver.cpp
@@ -120,14 +120,12 @@ static void FWorldObserver_GetActorComponentsState(
       CompState.state.traffic_light_data.red_time = Controller->GetRedTime();
       CompState.state.traffic_light_data.elapsed_time = Group->GetElapsedTime();
       CompState.state.traffic_light_data.time_is_frozen = Group->IsFrozen();
-      // Nobody is using right now, perhaps we should remove it?
+      // Nobody is using this right now, perhaps we should remove it?
       CompState.state.traffic_light_data.pole_index = 0;
 
       Out.Push(CompState);
     }
-
   }
-
 }
 
 static carla::geom::Vector3D FWorldObserver_GetAngularVelocity(const AActor &Actor)
@@ -156,19 +154,25 @@ static carla::Buffer FWorldObserver_Serialize(
     const UCarlaEpisode &Episode,
     float DeltaSeconds)
 {
+
+  carla::log_warning("===== FWorldObserver_Serialize =====");
+
   using Serializer = carla::sensor::s11n::EpisodeStateSerializer;
   using ActorDynamicState = carla::sensor::data::ActorDynamicState;
   using ComponentDynamicState = carla::sensor::data::ComponentDynamicState;
 
   const auto &Registry = Episode.GetActorRegistry();
 
+  auto size = sizeof(Serializer::Header) + sizeof(ActorDynamicState) * Registry.Num();
+
   // Set up buffer for writing.
-  buffer.reset(sizeof(Serializer::Header) + sizeof(ActorDynamicState) * Registry.Num());
+  buffer.reset(size * 3);
   auto begin = buffer.begin();
-  auto write_data = [&begin](const auto &data)
+  auto write_data = [&begin, &buffer, &size](const auto &data)
   {
     std::memcpy(begin, &data, sizeof(data));
     begin += sizeof(data);
+    carla::log_warning("Writting...", sizeof(data), begin - buffer.begin());
   };
 
   // Write header.
@@ -201,13 +205,27 @@ static carla::Buffer FWorldObserver_Serialize(
 
     write_data(info);
 
-    for(auto& CompState : ComponentsState)
-    {
-      write_data(CompState);
+    /*
+    if(info.num_components > 0) {
+      size += info.num_components * sizeof(ComponentDynamicState);
+      carla::log_warning("Num Components =", info.num_components,"(", size,")");
+      buffer.resize(size);
+
+      for(auto& CompState : ComponentsState)
+      {
+        write_data(CompState);
+      }
     }
+    */
   }
 
+  // Shrink buffer
+  buffer.resize(size);
+
   check(begin == buffer.end());
+
+  carla::log_warning("====================================");
+
   return std::move(buffer);
 }