From b1a475d2b45017784954d7dcd408f3de749b4e36 Mon Sep 17 00:00:00 2001 From: Axel1092 Date: Tue, 7 Jul 2020 11:27:25 +0200 Subject: [PATCH] Added physics control to recorder info. --- .../Carla/Source/Carla/Game/CarlaStatics.h | 25 ++++ .../Source/Carla/Recorder/CarlaRecorder.cpp | 20 +++ .../Source/Carla/Recorder/CarlaRecorder.h | 8 +- .../Carla/Recorder/CarlaRecorderHelpers.h | 49 ++++++++ .../Recorder/CarlaRecorderPhysicsControl.cpp | 114 ++++++++++++++++++ .../Recorder/CarlaRecorderPhysicsControl.h | 39 ++++++ .../Carla/Recorder/CarlaRecorderQuery.cpp | 77 +++++++++++- .../Carla/Recorder/CarlaRecorderQuery.h | 2 + .../Carla/Vehicle/CarlaWheeledVehicle.cpp | 8 +- .../Carla/Vehicle/CarlaWheeledVehicle.h | 2 +- 10 files changed, 338 insertions(+), 6 deletions(-) create mode 100644 Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaRecorderPhysicsControl.cpp create mode 100644 Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaRecorderPhysicsControl.h diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaStatics.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaStatics.h index ac3f0f228..7b66e59d4 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaStatics.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaStatics.h @@ -41,6 +41,11 @@ public: UFUNCTION(BlueprintPure, Category="CARLA") static TArray 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; +} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaRecorder.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaRecorder.cpp index a17d58a31..a3a1ec750 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaRecorder.cpp +++ b/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(Episode->GetActorRegistry().Find(DatabaseId).GetActor()); + if (Vehicle) + { + AddPhysicsControl(*Vehicle); + } } diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaRecorder.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaRecorder.h index 9693d7b0d..7d92479f8 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaRecorder.h +++ b/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 diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaRecorderHelpers.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaRecorderHelpers.h index eb889c6f8..8f6c096df 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaRecorderHelpers.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaRecorderHelpers.h @@ -7,6 +7,7 @@ #pragma once #include +#include // 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(&InObj), sizeof(T)); } +template +void WriteStdVector(std::ofstream &OutFile, const std::vector &InVec) +{ + WriteValue(OutFile, InVec.size()); + for (const auto& InObj : InVec) + { + WriteValue(OutFile, InObj); + } +} + +template +void WriteTArray(std::ofstream &OutFile, const TArray &InVec) +{ + WriteValue(OutFile, InVec.Num()); + for (const auto& InObj : InVec) + { + WriteValue(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(&OutObj), sizeof(T)); } +template +void ReadStdVector(std::ifstream &InFile, std::vector &OutVec) +{ + uint32_t VecSize; + ReadValue(InFile, VecSize); + OutVec.clear(); + for (int i = 0; i < VecSize; ++i) + { + T InObj; + ReadValue(InFile, InObj); + OutVec.push_back(InObj); + } +} + +template +void ReadTArray(std::ifstream &InFile, TArray &OutVec) +{ + uint32_t VecSize; + ReadValue(InFile, VecSize); + OutVec.Empty(); + for (int i = 0; i < VecSize; ++i) + { + T InObj; + ReadValue(InFile, InObj); + OutVec.Add(InObj); + } +} + // read binary data from FVector void ReadFVector(std::ifstream &InFile, FVector &OutObj); diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaRecorderPhysicsControl.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaRecorderPhysicsControl.cpp new file mode 100644 index 000000000..9a0322f50 --- /dev/null +++ b/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 . + +#include "CarlaRecorderPhysicsControl.h" +#include "CarlaRecorder.h" +#include "CarlaRecorderHelpers.h" + +#include +#include "carla/rpc/VehiclePhysicsControl.h" +#include + + +void CarlaRecorderPhysicsControl::Write(std::ofstream &OutFile) +{ + carla::rpc::VehiclePhysicsControl RPCPhysicsControl(VehiclePhysicsControl); + WriteValue(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(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(OutFile, static_cast(CarlaRecorderPacketId::PhysicsControl)); + + std::streampos PosStart = OutFile.tellp(); + // write dummy packet size + uint32_t Total = 0; + WriteValue(OutFile, Total); + + // write total records + Total = PhysicsControls.size(); + WriteValue(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(OutFile, Total); + OutFile.seekp(PosEnd, std::ios::beg); +} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaRecorderPhysicsControl.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaRecorderPhysicsControl.h new file mode 100644 index 000000000..0454e1e77 --- /dev/null +++ b/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 . + +#pragma once + +#include +#include + +#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 PhysicsControls; +}; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaRecorderQuery.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaRecorderQuery.cpp index fba734acc..0736119cb 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaRecorderQuery.cpp +++ b/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(CarlaRecorderPacketId::FrameEnd): + case static_cast(CarlaRecorderPacketId::PhysicsControl): + if (bShowAll) + { + ReadValue(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(CarlaRecorderPacketId::FrameEnd): // do nothing, it is empty break; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaRecorderQuery.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaRecorderQuery.h index a951a8f11..84d8f1c22 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaRecorderQuery.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaRecorderQuery.h @@ -8,6 +8,7 @@ #include +#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); diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.cpp index ded31841f..5c8a1ea9b 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.cpp +++ b/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 &WheelsFrictionS } } -FVehiclePhysicsControl ACarlaWheeledVehicle::GetVehiclePhysicsControl() +FVehiclePhysicsControl ACarlaWheeledVehicle::GetVehiclePhysicsControl() const { UWheeledVehicleMovementComponent4W *Vehicle4W = Cast( 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) diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.h index 1baa3473d..385999a39 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Vehicle/CarlaWheeledVehicle.h +++ b/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;