Added physics control to recorder info.

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaStatics.h

@@ -41,6 +41,11 @@ public:
 
   UFUNCTION(BlueprintPure, Category="CARLA")
   static TArray<FString> GetAllMapNames();
+
+  UFUNCTION(BlueprintPure, Category="CARLA", meta=(WorldContext="WorldContextObject"))
+  static ACarlaRecorder* GetRecorder(const UObject *WorldContextObject);
+
+  static CarlaReplayer* GetReplayer(const UObject *WorldContextObject);
 };
 
 // =============================================================================
@@ -68,3 +73,23 @@ inline UCarlaSettings *UCarlaStatics::GetCarlaSettings(const UObject *WorldConte
   auto GameInstance = GetGameInstance(WorldContext);
   return GameInstance != nullptr ? GameInstance->GetCARLASettings() : nullptr;
 }
+
+inline ACarlaRecorder* UCarlaStatics::GetRecorder(const UObject *WorldContextObject)
+{
+  auto* Episode = UCarlaStatics::GetCurrentEpisode(WorldContextObject);
+  if (Episode)
+  {
+    return Episode->GetRecorder();
+  }
+  return nullptr;
+}
+
+inline CarlaReplayer* UCarlaStatics::GetReplayer(const UObject *WorldContextObject)
+{
+  auto* Episode = UCarlaStatics::GetCurrentEpisode(WorldContextObject);
+  if (Episode)
+  {
+    return Episode->GetReplayer();
+  }
+  return nullptr;
+}

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaRecorder.cpp

@@ -294,6 +294,17 @@ void ACarlaRecorder::AddActorBoundingBox(FActorView &View)
   AddBoundingBox(BoundingBox);
 }
 
+void ACarlaRecorder::AddPhysicsControl(const ACarlaWheeledVehicle& Vehicle)
+{
+  if (bAdditionalData)
+  {
+    CarlaRecorderPhysicsControl Control;
+    Control.DatabaseId = Episode->GetActorRegistry().Find(&Vehicle).GetActorId();
+    Control.VehiclePhysicsControl = Vehicle.GetVehiclePhysicsControl();
+    PhysicsControls.Add(Control);
+  }
+}
+
 std::string ACarlaRecorder::Start(std::string Name, FString MapName, bool AdditionalData)
 {
   // stop replayer if any in course
@@ -364,6 +375,7 @@ void ACarlaRecorder::Clear(void)
   LightScenes.Clear();
   Kinematics.Clear();
   BoundingBoxes.Clear();
+  PhysicsControls.Clear();
 }
 
 void ACarlaRecorder::Write(double DeltaSeconds)
@@ -396,6 +408,7 @@ void ACarlaRecorder::Write(double DeltaSeconds)
     Kinematics.Write(File);
     BoundingBoxes.Write(File);
     PlatformTime.Write(File);
+    PhysicsControls.Write(File);
   }
 
   // end
@@ -588,4 +601,11 @@ void ACarlaRecorder::CreateRecorderEventAdd(
     std::move(Description)
   };
   AddEvent(std::move(RecEvent));
+
+  // check if it is a vehicle to get initial physics control
+  ACarlaWheeledVehicle* Vehicle = Cast<ACarlaWheeledVehicle>(Episode->GetActorRegistry().Find(DatabaseId).GetActor());
+  if (Vehicle)
+  {
+    AddPhysicsControl(*Vehicle);
+  }
 }

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaRecorder.h

@@ -11,6 +11,7 @@
 
 #include "Carla/Actor/ActorDescription.h"
 
+#include "CarlaRecorderPhysicsControl.h"
 #include "CarlaRecorderPlatformTime.h"
 #include "CarlaRecorderBoundingBox.h"
 #include "CarlaRecorderKinematics.h"
@@ -52,7 +53,9 @@ enum class CarlaRecorderPacketId : uint8_t
   SceneLight,
   Kinematics,
   BoundingBox,
-  PlatformTime
+  PlatformTime,
+  PhysicsControl,
+  TrafficLightParameters
 };
 
 /// Recorder for the simulation
@@ -109,6 +112,8 @@ public:
 
   void AddBoundingBox(const CarlaRecorderBoundingBox &ActorBoundingBox);
 
+  void AddPhysicsControl(const ACarlaWheeledVehicle& Vehicle);
+
   // set episode
   void SetEpisode(UCarlaEpisode *ThisEpisode)
   {
@@ -170,6 +175,7 @@ private:
   CarlaRecorderActorsKinematics Kinematics;
   CarlaRecorderBoundingBoxes BoundingBoxes;
   CarlaRecorderPlatformTime PlatformTime;
+  CarlaRecorderPhysicsControls PhysicsControls;
 
 
   // replayer

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaRecorderHelpers.h

@@ -7,6 +7,7 @@
 #pragma once
 
 #include <fstream>
+#include <vector>
 
 // get the final path + filename
 std::string GetRecorderFilename(std::string Filename);
@@ -22,6 +23,26 @@ void WriteValue(std::ofstream &OutFile, const T &InObj)
   OutFile.write(reinterpret_cast<const char *>(&InObj), sizeof(T));
 }
 
+template <typename T>
+void WriteStdVector(std::ofstream &OutFile, const std::vector<T> &InVec)
+{
+  WriteValue<uint32_t>(OutFile, InVec.size());
+  for (const auto& InObj : InVec)
+  {
+    WriteValue<T>(OutFile, InObj);
+  }
+}
+
+template <typename T>
+void WriteTArray(std::ofstream &OutFile, const TArray<T> &InVec)
+{
+  WriteValue<uint32_t>(OutFile, InVec.Num());
+  for (const auto& InObj : InVec)
+  {
+    WriteValue<T>(OutFile, InObj);
+  }
+}
+
 // write binary data from FVector
 void WriteFVector(std::ofstream &OutFile, const FVector &InObj);
 
@@ -41,6 +62,34 @@ void ReadValue(std::ifstream &InFile, T &OutObj)
   InFile.read(reinterpret_cast<char *>(&OutObj), sizeof(T));
 }
 
+template <typename T>
+void ReadStdVector(std::ifstream &InFile, std::vector<T> &OutVec)
+{
+  uint32_t VecSize;
+  ReadValue<uint32_t>(InFile, VecSize);
+  OutVec.clear();
+  for (int i = 0; i < VecSize; ++i)
+  {
+    T InObj;
+    ReadValue<T>(InFile, InObj);
+    OutVec.push_back(InObj);
+  }
+}
+
+template <typename T>
+void ReadTArray(std::ifstream &InFile, TArray<T> &OutVec)
+{
+  uint32_t VecSize;
+  ReadValue<uint32_t>(InFile, VecSize);
+  OutVec.Empty();
+  for (int i = 0; i < VecSize; ++i)
+  {
+    T InObj;
+    ReadValue<T>(InFile, InObj);
+    OutVec.Add(InObj);
+  }
+}
+
 // read binary data from FVector
 void ReadFVector(std::ifstream &InFile, FVector &OutObj);
 

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaRecorderPhysicsControl.cpp

@@ -0,0 +1,114 @@
+// Copyright (c) 2020 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 "CarlaRecorderPhysicsControl.h"
+#include "CarlaRecorder.h"
+#include "CarlaRecorderHelpers.h"
+
+#include <compiler/disable-ue4-macros.h>
+#include "carla/rpc/VehiclePhysicsControl.h"
+#include <compiler/enable-ue4-macros.h>
+
+
+void CarlaRecorderPhysicsControl::Write(std::ofstream &OutFile)
+{
+  carla::rpc::VehiclePhysicsControl RPCPhysicsControl(VehiclePhysicsControl);
+  WriteValue<uint32_t>(OutFile, this->DatabaseId);
+  WriteValue(OutFile, RPCPhysicsControl.max_rpm);
+  WriteValue(OutFile, RPCPhysicsControl.moi);
+  WriteValue(OutFile, RPCPhysicsControl.damping_rate_full_throttle);
+  WriteValue(OutFile, RPCPhysicsControl.damping_rate_zero_throttle_clutch_engaged);
+  WriteValue(OutFile, RPCPhysicsControl.damping_rate_zero_throttle_clutch_disengaged);
+  WriteValue(OutFile, RPCPhysicsControl.use_gear_autobox);
+  WriteValue(OutFile, RPCPhysicsControl.clutch_strength);
+  WriteValue(OutFile, RPCPhysicsControl.final_ratio);
+  WriteValue(OutFile, RPCPhysicsControl.mass);
+  WriteValue(OutFile, RPCPhysicsControl.drag_coefficient);
+  WriteValue(OutFile, RPCPhysicsControl.center_of_mass);
+
+  // torque curve
+  WriteStdVector(OutFile, RPCPhysicsControl.torque_curve);
+
+  // forward gears
+  WriteStdVector(OutFile, RPCPhysicsControl.forward_gears);
+
+  // steering curve
+  WriteStdVector(OutFile, RPCPhysicsControl.steering_curve);
+
+  // wheels
+  WriteStdVector(OutFile, RPCPhysicsControl.wheels);
+}
+
+void CarlaRecorderPhysicsControl::Read(std::ifstream &InFile)
+{
+  carla::rpc::VehiclePhysicsControl RPCPhysicsControl;
+  ReadValue<uint32_t>(InFile, this->DatabaseId);
+  ReadValue(InFile, RPCPhysicsControl.max_rpm);
+  ReadValue(InFile, RPCPhysicsControl.moi);
+  ReadValue(InFile, RPCPhysicsControl.damping_rate_full_throttle);
+  ReadValue(InFile, RPCPhysicsControl.damping_rate_zero_throttle_clutch_engaged);
+  ReadValue(InFile, RPCPhysicsControl.damping_rate_zero_throttle_clutch_disengaged);
+  ReadValue(InFile, RPCPhysicsControl.use_gear_autobox);
+  ReadValue(InFile, RPCPhysicsControl.clutch_strength);
+  ReadValue(InFile, RPCPhysicsControl.final_ratio);
+  ReadValue(InFile, RPCPhysicsControl.mass);
+  ReadValue(InFile, RPCPhysicsControl.drag_coefficient);
+  ReadValue(InFile, RPCPhysicsControl.center_of_mass);
+
+  // torque curve
+  ReadStdVector(InFile, RPCPhysicsControl.torque_curve);
+
+  // forward gears
+  ReadStdVector(InFile, RPCPhysicsControl.forward_gears);
+
+  // steering curve
+  ReadStdVector(InFile, RPCPhysicsControl.steering_curve);
+
+  // wheels
+  ReadStdVector(InFile, RPCPhysicsControl.wheels);
+
+  VehiclePhysicsControl = FVehiclePhysicsControl(RPCPhysicsControl);
+}
+
+// ---------------------------------------------
+
+void CarlaRecorderPhysicsControls::Clear(void)
+{
+  PhysicsControls.clear();
+}
+
+void CarlaRecorderPhysicsControls::Add(const CarlaRecorderPhysicsControl &InObj)
+{
+  PhysicsControls.push_back(InObj);
+}
+
+void CarlaRecorderPhysicsControls::Write(std::ofstream &OutFile)
+{
+  // write the packet id
+  WriteValue<char>(OutFile, static_cast<char>(CarlaRecorderPacketId::PhysicsControl));
+
+  std::streampos PosStart = OutFile.tellp();
+  // write dummy packet size
+  uint32_t Total = 0;
+  WriteValue<uint32_t>(OutFile, Total);
+
+  // write total records
+  Total = PhysicsControls.size();
+  WriteValue<uint16_t>(OutFile, Total);
+
+  // write records
+  for (auto& PhysicsControl : PhysicsControls)
+  {
+    PhysicsControl.Write(OutFile);
+  }
+
+  // write the real packet size
+  std::streampos PosEnd = OutFile.tellp();
+  Total = PosEnd - PosStart - sizeof(uint32_t);
+  OutFile.seekp(PosStart, std::ios::beg);
+  WriteValue<uint32_t>(OutFile, Total);
+  OutFile.seekp(PosEnd, std::ios::beg);
+}

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaRecorderPhysicsControl.h

@@ -0,0 +1,39 @@
+// Copyright (c) 2020 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 <fstream>
+#include <vector>
+
+#include "Carla/Vehicle/VehiclePhysicsControl.h"
+
+#pragma pack(push, 1)
+struct CarlaRecorderPhysicsControl
+{
+  uint32_t DatabaseId;
+  FVehiclePhysicsControl VehiclePhysicsControl;
+
+  void Read(std::ifstream &InFile);
+
+  void Write(std::ofstream &OutFile);
+};
+#pragma pack(pop)
+
+class CarlaRecorderPhysicsControls
+{
+  public:
+
+  void Add(const CarlaRecorderPhysicsControl &InObj);
+
+  void Clear(void);
+
+  void Write(std::ofstream &OutFile);
+
+private:
+
+  std::vector<CarlaRecorderPhysicsControl> PhysicsControls;
+};

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaRecorderQuery.cpp

@@ -286,7 +286,7 @@ std::string CarlaRecorderQuery::QueryInfo(std::string Filename, bool bShowAll)
             if (State.LowBeam)
               enabled_lights_list += "Low Beam, ";
             if (State.HighBeam)
-              enabled_lights_list += "Hight Beam, ";
+              enabled_lights_list += "High Beam, ";
             if (State.Brake)
               enabled_lights_list += "Brake, ";
             if (State.RightBlinker)
@@ -295,6 +295,8 @@ std::string CarlaRecorderQuery::QueryInfo(std::string Filename, bool bShowAll)
               enabled_lights_list += "Left Blinker, ";
             if (State.Reverse)
               enabled_lights_list += "Reverse, ";
+            if (State.Interior)
+              enabled_lights_list += "Interior, ";
             if (State.Fog)
               enabled_lights_list += "Fog, ";
             if (State.Special1)
@@ -408,8 +410,77 @@ std::string CarlaRecorderQuery::QueryInfo(std::string Filename, bool bShowAll)
           SkipPacket();
         break;
 
-      // frame end
-      case static_cast<char>(CarlaRecorderPacketId::FrameEnd):
+      case static_cast<char>(CarlaRecorderPacketId::PhysicsControl):
+        if (bShowAll)
+        {
+          ReadValue<uint16_t>(File, Total);
+          if (Total > 0 && !bFramePrinted)
+          {
+            PrintFrame(Info);
+            bFramePrinted = true;
+          }
+
+          Info << " Physics Control events: " << Total << std::endl;
+          for (i = 0; i < Total; ++i)
+          {
+            PhysicsControl.Read(File);
+            carla::rpc::VehiclePhysicsControl Control(PhysicsControl.VehiclePhysicsControl);
+            Info << "  Actor id " << PhysicsControl.DatabaseId
+                << " max rpm " << Control.max_rpm << " MOI " << Control.moi
+                << " damping rate full throttle " << Control.damping_rate_full_throttle
+                << " damping rate zero throttle clutch engaged " << Control.damping_rate_zero_throttle_clutch_engaged
+                << " damping rate zero throttle clutch disengaged" << Control.damping_rate_zero_throttle_clutch_disengaged
+                << " use gear auto box " << (Control.use_gear_autobox ? "true" : "false")
+                << " gear switch time " << Control.gear_switch_time
+                << " clutch strength " << Control.clutch_strength
+                << " final ratio " << Control.final_ratio
+                << " mass " << Control.mass << " drag coefficient " << Control.drag_coefficient
+                << " center of mass " << "(" << Control.center_of_mass.x << ", " << Control.center_of_mass.y << ")"
+                << std::endl;
+            Info << "   torque curve:";
+            for (auto& vec : Control.torque_curve)
+            {
+              Info << " (" << vec.x << ", " << vec.y << ")";
+            }
+            Info << std::endl;
+            Info << "   steering curve:";
+            for (auto& vec : Control.steering_curve)
+            {
+              Info << " (" << vec.x << ", " << vec.y << ")";
+            }
+            Info << std::endl;
+            Info << "   forward gears:";
+            uint32_t count = 0;
+            for (auto& Gear : Control.forward_gears)
+            {
+              Info << " gear " << count << " ratio " << Gear.ratio
+                  << " down ratio " << Gear.down_ratio
+                  << " up ratio " << Gear.up_ratio;
+              ++count;
+            }
+            Info << std::endl;
+            Info << "   wheels:";
+            count = 0;
+            for (auto& Wheel : Control.wheels)
+            {
+              Info << " wheel " << count << " tire friction " << Wheel.tire_friction
+                  << " damping rate " << Wheel.damping_rate
+                  << " max steer angle " << Wheel.max_steer_angle
+                  << " radius " << Wheel.radius
+                  << " max brake torque " << Wheel.max_brake_torque
+                  << " max handbrake torque " << Wheel.max_handbrake_torque
+                  << " position " << "(" << Wheel.position.x << ", " << Wheel.position.y << ", " << Wheel.position.z << ")";
+              ++count;
+            }
+            Info << std::endl;
+          }
+        }
+        else
+          SkipPacket();
+        break;
+
+        // frame end
+        case static_cast<char>(CarlaRecorderPacketId::FrameEnd):
         // do nothing, it is empty
         break;
 

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaRecorderQuery.h

@@ -8,6 +8,7 @@
 
 #include <fstream>
 
+#include "CarlaRecorderPhysicsControl.h"
 #include "CarlaRecorderPlatformTime.h"
 #include "CarlaRecorderBoundingBox.h"
 #include "CarlaRecorderKinematics.h"
@@ -62,6 +63,7 @@ private:
   CarlaRecorderKinematics Kinematics;
   CarlaRecorderBoundingBox BoundingBox;
   CarlaRecorderPlatformTime PlatformTime;
+  CarlaRecorderPhysicsControl PhysicsControl;
 
   // read next header packet
   bool ReadHeader(void);

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.cpp

@@ -7,6 +7,7 @@
 
 #include "Carla.h"
 #include "Carla/Vehicle/CarlaWheeledVehicle.h"
+#include "Carla/Game/CarlaStatics.h"
 
 #include "Components/BoxComponent.h"
 #include "Engine/CollisionProfile.h"
@@ -210,7 +211,7 @@ void ACarlaWheeledVehicle::SetWheelsFrictionScale(TArray<float> &WheelsFrictionS
   }
 }
 
-FVehiclePhysicsControl ACarlaWheeledVehicle::GetVehiclePhysicsControl()
+FVehiclePhysicsControl ACarlaWheeledVehicle::GetVehiclePhysicsControl() const
 {
   UWheeledVehicleMovementComponent4W *Vehicle4W = Cast<UWheeledVehicleMovementComponent4W>(
       GetVehicleMovement());
@@ -373,6 +374,11 @@ void ACarlaWheeledVehicle::ApplyVehiclePhysicsControl(const FVehiclePhysicsContr
     Vehicle4W->Wheels[i]->TireConfig->SetFrictionScale(PhysicsControl.Wheels[i].TireFriction);
   }
 
+  auto * Recorder = UCarlaStatics::GetRecorder(GetWorld());
+  if (Recorder && Recorder->IsEnabled())
+  {
+    Recorder->AddPhysicsControl(*this);
+  }
 }
 
 void ACarlaWheeledVehicle::SetVehicleLightState(const FVehicleLightState &LightState)

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.h

@@ -110,7 +110,7 @@ public:
   }
 
   UFUNCTION(Category = "CARLA Wheeled Vehicle", BlueprintCallable)
-  FVehiclePhysicsControl GetVehiclePhysicsControl();
+  FVehiclePhysicsControl GetVehiclePhysicsControl() const;
 
   UFUNCTION(Category = "CARLA Wheeled Vehicle", BlueprintCallable)
   FVehicleLightState GetVehicleLightState() const;