From 74380cdc23bb597f705abbf2c245c6692eb3e931 Mon Sep 17 00:00:00 2001 From: Axel1092 <35765780+Axel1092@users.noreply.github.com> Date: Fri, 11 Jun 2021 19:44:16 +0200 Subject: [PATCH] Fixed existing features for Large Maps (#4293) * Fixed child-parent when waking up dormant actors. Added profiling traces. Fixed transformation of debug shapes in large maps. * WIP: renaming FActorView to FCarlaActor. Changing calls related to FCarlaActor to acomodate new typing. * Renaming FActorView to FCarlaActor. Changing calls related to FCarlaActor to acomodate new typing. * Now traffic lights can be spawned without controllers inside junctions. * Added functions to interface the FCarlaActor object with recorder. Started adapting recorder to new FCarlaActor object. * Fixed missing includes. * Updated CarlaRecorder and CarlaReplayer to use new FCarlaActor interface for vehicles, walkers and sensors. * Fixed missing includes. * Streaming level list now is persistant and can be saved for unreal to detect the dependencies. * Enabled packages for large maps. * Added Tiles to the MapPath.txt file. Removed debug warnings. * Fixed traffic light trigger box placement in large maps. * Fixed traffic light warning. Set speed limit for dormant actors to 30. Fixed semantic segmentation on streaming levels. * Fixed Traffic Light issue with recorder and dormant state. Removed old ActorView files. * Fixing line markings semantics * Fixed recorder issues with traffic lights. * Filter out the base large map as available map * Added tile_stream_distance and actor_stream_distance to world settings. * removed unused comments. * Added IsActive functions. Dormant actors that cannot be respawned now remain dormant. * Fixed missing includes. * Fixed missing includes * Review fixes. Missing includes. * Fixed link errors. Co-authored-by: bernat --- LibCarla/source/carla/client/Actor.h | 6 +- .../carla/client/detail/EpisodeState.cpp | 7 +- LibCarla/source/carla/rpc/ActorState.h | 2 +- LibCarla/source/carla/rpc/EpisodeSettings.h | 30 +- PythonAPI/carla/source/libcarla/Actor.cpp | 5 +- PythonAPI/carla/source/libcarla/World.cpp | 8 +- PythonAPI/util/config.py | 15 + .../Carla/Source/Carla/Actor/ActorData.cpp | 59 +-- .../Carla/Source/Carla/Actor/ActorData.h | 28 +- .../Source/Carla/Actor/ActorRegistry.cpp | 2 +- .../Carla/Source/Carla/Actor/ActorView.cpp | 57 --- .../Carla/Source/Carla/Actor/ActorView.h | 193 ---------- .../Carla/Source/Carla/Actor/CarlaActor.cpp | 28 +- .../Carla/Source/Carla/Actor/CarlaActor.h | 15 +- .../Carla/Commandlet/MoveAssetsCommandlet.cpp | 6 +- .../PrepareAssetsForCookingCommandlet.cpp | 97 ++++- .../PrepareAssetsForCookingCommandlet.h | 7 +- .../Carla/Source/Carla/Game/CarlaEngine.cpp | 9 + .../Carla/Source/Carla/Game/CarlaEngine.h | 8 +- .../Carla/Source/Carla/Game/CarlaEpisode.cpp | 38 +- .../Carla/Source/Carla/Game/CarlaEpisode.h | 21 +- .../Source/Carla/Game/CarlaGameModeBase.cpp | 11 +- .../Carla/Source/Carla/Game/Tagger.cpp | 7 + .../Plugins/Carla/Source/Carla/Game/Tagger.h | 2 + .../Source/Carla/MapGen/LargeMapManager.cpp | 364 ++++++++---------- .../Source/Carla/MapGen/LargeMapManager.h | 55 ++- .../Source/Carla/Recorder/CarlaRecorder.cpp | 33 +- .../Recorder/CarlaRecorderEventParent.cpp | 4 +- .../Carla/Recorder/CarlaReplayerHelper.cpp | 109 +++--- .../Source/Carla/Sensor/WorldObserver.cpp | 7 +- .../Carla/Source/Carla/Server/CarlaServer.cpp | 26 +- .../Source/Carla/Settings/EpisodeSettings.h | 4 + .../Carla/Traffic/TrafficLightComponent.cpp | 10 +- .../Carla/Traffic/TrafficLightController.cpp | 15 + .../Carla/Traffic/TrafficLightController.h | 6 + .../Source/Carla/Util/ObjectRegister.cpp | 4 + 36 files changed, 634 insertions(+), 664 deletions(-) delete mode 100644 Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorView.cpp delete mode 100644 Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorView.h diff --git a/LibCarla/source/carla/client/Actor.h b/LibCarla/source/carla/client/Actor.h index d36db70c8..fdc892cfb 100644 --- a/LibCarla/source/carla/client/Actor.h +++ b/LibCarla/source/carla/client/Actor.h @@ -105,13 +105,17 @@ namespace client { rpc::ActorState GetActorState() const; bool IsAlive() const { - return GetEpisode().IsValid() && GetActorState() == rpc::ActorState::Alive; + return GetEpisode().IsValid() && (GetActorState() != rpc::ActorState::PendingKill && GetActorState() != rpc::ActorState::Invalid) ; } bool IsDormant() const { return GetEpisode().IsValid() && GetActorState() == rpc::ActorState::Dormant; } + bool IsActive() const { + return GetEpisode().IsValid() && GetActorState() == rpc::ActorState::Active; + } + /// Tell the simulator to destroy this Actor, and return whether the actor /// was successfully destroyed. /// diff --git a/LibCarla/source/carla/client/detail/EpisodeState.cpp b/LibCarla/source/carla/client/detail/EpisodeState.cpp index c7a9d8f1a..a450dd05e 100644 --- a/LibCarla/source/carla/client/detail/EpisodeState.cpp +++ b/LibCarla/source/carla/client/detail/EpisodeState.cpp @@ -21,18 +21,13 @@ namespace detail { _simulation_state(state.GetSimulationState()) { _actors.reserve(state.size()); for (auto &&actor : state) { - - // Origin offset conversion for Large maps - geom::Transform transform = actor.transform; - transform.location += {_map_origin}; - DEBUG_ONLY(auto result = ) _actors.emplace( actor.id, ActorSnapshot{ actor.id, actor.actor_state, - transform, + actor.transform, actor.velocity, actor.angular_velocity, actor.acceleration, diff --git a/LibCarla/source/carla/rpc/ActorState.h b/LibCarla/source/carla/rpc/ActorState.h index 06a546323..b48bcada6 100644 --- a/LibCarla/source/carla/rpc/ActorState.h +++ b/LibCarla/source/carla/rpc/ActorState.h @@ -13,7 +13,7 @@ namespace rpc { enum class ActorState : uint8_t { Invalid, - Alive, + Active, Dormant, PendingKill, }; diff --git a/LibCarla/source/carla/rpc/EpisodeSettings.h b/LibCarla/source/carla/rpc/EpisodeSettings.h index 83737c360..57cbf0fa4 100644 --- a/LibCarla/source/carla/rpc/EpisodeSettings.h +++ b/LibCarla/source/carla/rpc/EpisodeSettings.h @@ -41,8 +41,13 @@ namespace rpc { bool deterministic_ragdolls = true; + float tile_stream_distance = 3000.f; // 3km + + float actor_active_distance = 2000.f; // 2km + MSGPACK_DEFINE_ARRAY(synchronous_mode, no_rendering_mode, fixed_delta_seconds, substepping, - max_substep_delta_time, max_substeps, max_culling_distance, deterministic_ragdolls); + max_substep_delta_time, max_substeps, max_culling_distance, deterministic_ragdolls, + tile_stream_distance, actor_active_distance); // ========================================================================= // -- Constructors --------------------------------------------------------- @@ -58,7 +63,9 @@ namespace rpc { double max_substep_delta_time = 0.01, int max_substeps = 10, float max_culling_distance = 0.0f, - bool deterministic_ragdolls = true) + bool deterministic_ragdolls = true, + float tile_stream_distance = 3000.f, + float actor_active_distance = 2000.f) : synchronous_mode(synchronous_mode), no_rendering_mode(no_rendering_mode), fixed_delta_seconds( @@ -67,7 +74,9 @@ namespace rpc { max_substep_delta_time(max_substep_delta_time), max_substeps(max_substeps), max_culling_distance(max_culling_distance), - deterministic_ragdolls(deterministic_ragdolls) {} + deterministic_ragdolls(deterministic_ragdolls), + tile_stream_distance(tile_stream_distance), + actor_active_distance(actor_active_distance) {} // ========================================================================= // -- Comparison operators ------------------------------------------------- @@ -82,7 +91,9 @@ namespace rpc { (max_substep_delta_time == rhs.max_substep_delta_time) && (max_substeps == rhs.max_substeps) && (max_culling_distance == rhs.max_culling_distance) && - (deterministic_ragdolls == rhs.deterministic_ragdolls); + (deterministic_ragdolls == rhs.deterministic_ragdolls) && + (tile_stream_distance == tile_stream_distance) && + (actor_active_distance == actor_active_distance); } bool operator!=(const EpisodeSettings &rhs) const { @@ -104,9 +115,16 @@ namespace rpc { Settings.MaxSubstepDeltaTime, Settings.MaxSubsteps, Settings.MaxCullingDistance, - Settings.bDeterministicRagdolls) {} + Settings.bDeterministicRagdolls, + Settings.TileStreamingDistance, + Settings.ActorActiveDistance) { + constexpr float CMTOM = 1.f/100.f; + tile_stream_distance = CMTOM * Settings.TileStreamingDistance; + actor_active_distance = CMTOM * Settings.ActorActiveDistance; + } operator FEpisodeSettings() const { + constexpr float MTOCM = 100.f; FEpisodeSettings Settings; Settings.bSynchronousMode = synchronous_mode; Settings.bNoRenderingMode = no_rendering_mode; @@ -118,6 +136,8 @@ namespace rpc { Settings.MaxSubsteps = max_substeps; Settings.MaxCullingDistance = max_culling_distance; Settings.bDeterministicRagdolls = deterministic_ragdolls; + Settings.TileStreamingDistance = MTOCM * tile_stream_distance; + Settings.ActorActiveDistance = MTOCM * actor_active_distance; return Settings; } diff --git a/PythonAPI/carla/source/libcarla/Actor.cpp b/PythonAPI/carla/source/libcarla/Actor.cpp index fa1ecc44f..21a1010e7 100644 --- a/PythonAPI/carla/source/libcarla/Actor.cpp +++ b/PythonAPI/carla/source/libcarla/Actor.cpp @@ -72,7 +72,7 @@ void export_actor() { enum_("ActorState") .value("Invalid", cr::ActorState::Invalid) - .value("Alive", cr::ActorState::Alive) + .value("Active", cr::ActorState::Active) .value("Dormant", cr::ActorState::Dormant) ; @@ -90,6 +90,7 @@ void export_actor() { .add_property("actor_state", CALL_WITHOUT_GIL(cc::Actor, GetActorState)) .add_property("is_alive", CALL_WITHOUT_GIL(cc::Actor, IsAlive)) .add_property("is_dormant", CALL_WITHOUT_GIL(cc::Actor, IsDormant)) + .add_property("is_active", CALL_WITHOUT_GIL(cc::Actor, IsActive)) .add_property("attributes", +[] (const cc::Actor &self) { boost::python::dict attribute_dict; for (auto &&attribute_value : self.GetAttributes()) { @@ -144,7 +145,7 @@ void export_actor() { .value("Front_Wheel", cr::VehicleWheelLocation::Front_Wheel) .value("Back_Wheel", cr::VehicleWheelLocation::Back_Wheel) ; - + class_, boost::noncopyable, boost::shared_ptr>("Vehicle", no_init) .def("apply_control", &cc::Vehicle::ApplyControl, (arg("control"))) diff --git a/PythonAPI/carla/source/libcarla/World.cpp b/PythonAPI/carla/source/libcarla/World.cpp index a2bf93f27..9c38ec706 100644 --- a/PythonAPI/carla/source/libcarla/World.cpp +++ b/PythonAPI/carla/source/libcarla/World.cpp @@ -152,7 +152,7 @@ void export_world() { ; class_("WorldSettings") - .def(init( + .def(init( (arg("synchronous_mode")=false, arg("no_rendering_mode")=false, arg("fixed_delta_seconds")=0.0, @@ -160,7 +160,9 @@ void export_world() { arg("max_substep_delta_time")=0.01, arg("max_substeps")=10, arg("max_culling_distance")=0.0f, - arg("deterministic_ragdolls")=false))) + arg("deterministic_ragdolls")=false, + arg("tile_stream_distance")=3000.f, + arg("actor_active_distance")=2000.f))) .def_readwrite("synchronous_mode", &cr::EpisodeSettings::synchronous_mode) .def_readwrite("no_rendering_mode", &cr::EpisodeSettings::no_rendering_mode) .def_readwrite("substepping", &cr::EpisodeSettings::substepping) @@ -176,6 +178,8 @@ void export_world() { double fds = (value == object{} ? 0.0 : extract(value)); self.fixed_delta_seconds = fds > 0.0 ? fds : boost::optional{}; }) + .def_readwrite("tile_stream_distance", &cr::EpisodeSettings::tile_stream_distance) + .def_readwrite("actor_active_distance", &cr::EpisodeSettings::actor_active_distance) .def("__eq__", &cr::EpisodeSettings::operator==) .def("__ne__", &cr::EpisodeSettings::operator!=) .def(self_ns::str(self_ns::self)) diff --git a/PythonAPI/util/config.py b/PythonAPI/util/config.py index 90e915822..5740c0f6f 100755 --- a/PythonAPI/util/config.py +++ b/PythonAPI/util/config.py @@ -183,6 +183,16 @@ def main(): '--osm-path', metavar='OSM_FILE_PATH', help='load a new map with a minimum physical road representation of the provided OpenStreetMaps') + argparser.add_argument( + '--tile-stream-distance', + metavar='N', + type=float, + help='Set tile streaming distance (large maps only)') + argparser.add_argument( + '--actor-active-distance', + metavar='N', + type=float, + help='Set actor active distance (large maps only)') if len(sys.argv) < 2: argparser.print_help() return @@ -283,6 +293,11 @@ def main(): print('set variable frame rate.') settings.fixed_delta_seconds = None + if args.tile_stream_distance is not None: + settings.tile_stream_distance = args.tile_stream_distance + if args.actor_active_distance is not None: + settings.actor_active_distance = args.actor_active_distance + world.apply_settings(settings) if args.weather is not None: diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorData.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorData.cpp index 5d92efc1f..35727a802 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorData.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorData.cpp @@ -17,6 +17,7 @@ #include "Carla/Walker/WalkerController.h" #include "Carla/Walker/WalkerBase.h" #include "Carla/Sensor/Sensor.h" +#include "CarlaActor.h" AActor* FActorData::RespawnActor(UCarlaEpisode* CarlaEpisode, const FActorInfo& Info) { @@ -26,8 +27,9 @@ AActor* FActorData::RespawnActor(UCarlaEpisode* CarlaEpisode, const FActorInfo& return CarlaEpisode->ReSpawnActorWithInfo(SpawnTransform, Info.Description); } -void FActorData::RecordActorData(AActor* Actor, UCarlaEpisode* CarlaEpisode) +void FActorData::RecordActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode) { + AActor* Actor = CarlaActor->GetActor(); FTransform Transform = Actor->GetTransform(); Location = FDVector(Transform.GetLocation()) + CarlaEpisode->GetCurrentMapOrigin(); Rotation = Transform.GetRotation(); @@ -41,8 +43,9 @@ void FActorData::RecordActorData(AActor* Actor, UCarlaEpisode* CarlaEpisode) Velocity = Actor->GetVelocity(); } -void FActorData::RestoreActorData(AActor* Actor, UCarlaEpisode* CarlaEpisode) +void FActorData::RestoreActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode) { + AActor* Actor = CarlaActor->GetActor(); Actor->SetActorTransform(GetLocalTransform(CarlaEpisode)); UPrimitiveComponent* Component = Cast(Actor->GetRootComponent()); if (Component) @@ -90,9 +93,10 @@ FTransform FActorData::GetLocalTransform(UCarlaEpisode* CarlaEpisode) const return FTransform(Rotation, LocalLocation, Scale); } -void FVehicleData::RecordActorData(AActor* Actor, UCarlaEpisode* CarlaEpisode) +void FVehicleData::RecordActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode) { - FActorData::RecordActorData(Actor, CarlaEpisode); + FActorData::RecordActorData(CarlaActor, CarlaEpisode); + AActor* Actor = CarlaActor->GetActor(); ACarlaWheeledVehicle* Vehicle = Cast(Actor); if (bSimulatePhysics) { @@ -102,9 +106,10 @@ void FVehicleData::RecordActorData(AActor* Actor, UCarlaEpisode* CarlaEpisode) LightState = Vehicle->GetVehicleLightState(); } -void FVehicleData::RestoreActorData(AActor* Actor, UCarlaEpisode* CarlaEpisode) +void FVehicleData::RestoreActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode) { - FActorData::RestoreActorData(Actor, CarlaEpisode); + FActorData::RestoreActorData(CarlaActor, CarlaEpisode); + AActor* Actor = CarlaActor->GetActor(); ACarlaWheeledVehicle* Vehicle = Cast(Actor); Vehicle->SetSimulatePhysics(bSimulatePhysics); if (bSimulatePhysics) @@ -115,9 +120,10 @@ void FVehicleData::RestoreActorData(AActor* Actor, UCarlaEpisode* CarlaEpisode) Vehicle->SetVehicleLightState(LightState); } -void FWalkerData::RecordActorData(AActor* Actor, UCarlaEpisode* CarlaEpisode) +void FWalkerData::RecordActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode) { - FActorData::RecordActorData(Actor, CarlaEpisode); + FActorData::RecordActorData(CarlaActor, CarlaEpisode); + AActor* Actor = CarlaActor->GetActor(); auto Walker = Cast(Actor); auto Controller = Walker != nullptr ? Cast(Walker->GetController()) : nullptr; if (Controller != nullptr) @@ -126,9 +132,10 @@ void FWalkerData::RecordActorData(AActor* Actor, UCarlaEpisode* CarlaEpisode) } } -void FWalkerData::RestoreActorData(AActor* Actor, UCarlaEpisode* CarlaEpisode) +void FWalkerData::RestoreActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode) { - FActorData::RestoreActorData(Actor, CarlaEpisode); + FActorData::RestoreActorData(CarlaActor, CarlaEpisode); + AActor* Actor = CarlaActor->GetActor(); auto Walker = Cast(Actor); auto Controller = Walker != nullptr ? Cast(Walker->GetController()) : nullptr; if (Controller != nullptr) @@ -149,9 +156,10 @@ AActor* FTrafficSignData::RespawnActor(UCarlaEpisode* CarlaEpisode, const FActor SpawnParams); } -void FTrafficSignData::RecordActorData(AActor* Actor, UCarlaEpisode* CarlaEpisode) +void FTrafficSignData::RecordActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode) { - FActorData::RecordActorData(Actor, CarlaEpisode); + FActorData::RecordActorData(CarlaActor, CarlaEpisode); + AActor* Actor = CarlaActor->GetActor(); Model = Actor->GetClass(); ATrafficSignBase* TrafficSign = Cast(Actor); USignComponent* TrafficSignComponent = @@ -163,9 +171,9 @@ void FTrafficSignData::RecordActorData(AActor* Actor, UCarlaEpisode* CarlaEpisod } } -void FTrafficSignData::RestoreActorData(AActor* Actor, UCarlaEpisode* CarlaEpisode) +void FTrafficSignData::RestoreActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode) { - FActorData::RestoreActorData(Actor, CarlaEpisode); + AActor* Actor = CarlaActor->GetActor(); if (SignId.Len()) { USignComponent* SignComponent = @@ -192,41 +200,48 @@ AActor* FTrafficLightData::RespawnActor(UCarlaEpisode* CarlaEpisode, const FActo SpawnParams); } -void FTrafficLightData::RecordActorData(AActor* Actor, UCarlaEpisode* CarlaEpisode) +void FTrafficLightData::RecordActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode) { - FActorData::RecordActorData(Actor, CarlaEpisode); + FActorData::RecordActorData(CarlaActor, CarlaEpisode); + AActor* Actor = CarlaActor->GetActor(); Model = Actor->GetClass(); ATrafficLightBase* TrafficLight = Cast(Actor); UTrafficLightComponent* Component = TrafficLight->GetTrafficLightComponent(); SignId = Component->GetSignId(); Controller = Component->GetController(); Controller->RemoveTrafficLight(Component); + Controller->AddCarlaActorTrafficLight(CarlaActor); + LightState = TrafficLight->GetTrafficLightState(); PoleIndex = TrafficLight->GetPoleIndex(); } -void FTrafficLightData::RestoreActorData(AActor* Actor, UCarlaEpisode* CarlaEpisode) +void FTrafficLightData::RestoreActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode) { - FActorData::RestoreActorData(Actor, CarlaEpisode); + AActor* Actor = CarlaActor->GetActor(); ATrafficLightBase* TrafficLight = Cast(Actor); UTrafficLightComponent* Component = TrafficLight->GetTrafficLightComponent(); Component->SetSignId(SignId); + Controller->RemoveCarlaActorTrafficLight(CarlaActor); Controller->AddTrafficLight(Component); ACarlaGameModeBase *GameMode = UCarlaStatics::GetGameMode(CarlaEpisode->GetWorld()); Component->InitializeSign(GameMode->GetMap().get()); Component->SetLightState(Controller->GetCurrentState().State); TrafficLight->SetPoleIndex(PoleIndex); + TrafficLight->SetTrafficLightState(LightState); } -void FActorSensorData::RecordActorData(AActor* Actor, UCarlaEpisode* CarlaEpisode) +void FActorSensorData::RecordActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode) { - FActorData::RecordActorData(Actor, CarlaEpisode); + FActorData::RecordActorData(CarlaActor, CarlaEpisode); + AActor* Actor = CarlaActor->GetActor(); ASensor* Sensor = Cast(Actor); Stream = Sensor->MoveDataStream(); } -void FActorSensorData::RestoreActorData(AActor* Actor, UCarlaEpisode* CarlaEpisode) +void FActorSensorData::RestoreActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode) { - FActorData::RestoreActorData(Actor, CarlaEpisode); + FActorData::RestoreActorData(CarlaActor, CarlaEpisode); + AActor* Actor = CarlaActor->GetActor(); ASensor* Sensor = Cast(Actor); Sensor->SetDataStream(std::move(Stream)); } diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorData.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorData.h index d8bf366fa..9f725c8e6 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorData.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorData.h @@ -12,6 +12,7 @@ #include "Vehicle/VehicleInputPriority.h" #include "Vehicle/VehiclePhysicsControl.h" #include "Carla/Sensor/DataStream.h" +#include "Carla/Traffic/TrafficLightState.h" #include #include @@ -19,6 +20,7 @@ class UCarlaEpisode; class UTrafficLightController; +class FCarlaActor; class FActorData { @@ -36,9 +38,9 @@ public: bool bSimulatePhysics = false; - virtual void RecordActorData(AActor* Actor, UCarlaEpisode* CarlaEpisode); + virtual void RecordActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode); - virtual void RestoreActorData(AActor* Actor, UCarlaEpisode* CarlaEpisode); + virtual void RestoreActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode); virtual AActor* RespawnActor(UCarlaEpisode* CarlaEpisode, const FActorInfo& Info); @@ -57,9 +59,9 @@ public: FVehicleLightState LightState; - virtual void RecordActorData(AActor* Actor, UCarlaEpisode* CarlaEpisode) override; + virtual void RecordActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode) override; - virtual void RestoreActorData(AActor* Actor, UCarlaEpisode* CarlaEpisode) override; + virtual void RestoreActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode) override; }; class FWalkerData : public FActorData @@ -70,9 +72,9 @@ public: bool bAlive = true; - virtual void RecordActorData(AActor* Actor, UCarlaEpisode* CarlaEpisode) override; + virtual void RecordActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode) override; - virtual void RestoreActorData(AActor* Actor, UCarlaEpisode* CarlaEpisode) override; + virtual void RestoreActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode) override; }; class FTrafficSignData : public FActorData @@ -86,9 +88,9 @@ public: virtual AActor* RespawnActor(UCarlaEpisode* CarlaEpisode, const FActorInfo& Info) override; - virtual void RecordActorData(AActor* Actor, UCarlaEpisode* CarlaEpisode) override; + virtual void RecordActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode) override; - virtual void RestoreActorData(AActor* Actor, UCarlaEpisode* CarlaEpisode) override; + virtual void RestoreActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode) override; }; class FTrafficLightData : public FActorData @@ -97,6 +99,8 @@ public: UTrafficLightController* Controller; + ETrafficLightState LightState; + FString SignId; TSubclassOf Model; @@ -105,9 +109,9 @@ public: virtual AActor* RespawnActor(UCarlaEpisode* CarlaEpisode, const FActorInfo& Info) override; - virtual void RecordActorData(AActor* Actor, UCarlaEpisode* CarlaEpisode) override; + virtual void RecordActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode) override; - virtual void RestoreActorData(AActor* Actor, UCarlaEpisode* CarlaEpisode) override; + virtual void RestoreActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode) override; }; @@ -117,7 +121,7 @@ public: FDataStream Stream; - virtual void RecordActorData(AActor* Actor, UCarlaEpisode* CarlaEpisode) override; + virtual void RecordActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode) override; - virtual void RestoreActorData(AActor* Actor, UCarlaEpisode* CarlaEpisode) override; + virtual void RestoreActorData(FCarlaActor* CarlaActor, UCarlaEpisode* CarlaEpisode) override; }; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorRegistry.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorRegistry.cpp index ecd34a11f..1afa7ecee 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorRegistry.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorRegistry.cpp @@ -102,7 +102,7 @@ FCarlaActor* FActorRegistry::Register(AActor &Actor, FActorDescription Descripti Ids.Emplace(&Actor, Id); TSharedPtr View = - MakeCarlaActor(Id, Actor, std::move(Description), crp::ActorState::Alive); + MakeCarlaActor(Id, Actor, std::move(Description), crp::ActorState::Active); TSharedPtr& Result = ActorDatabase.Emplace(Id, MoveTemp(View)); diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorView.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorView.cpp deleted file mode 100644 index bb68afcfa..000000000 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorView.cpp +++ /dev/null @@ -1,57 +0,0 @@ -// 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 . - -#include "ActorView.h" - - -void FActorView::BuildActorData() -{ - switch(Type) - { - case ActorType::TrafficSign: - ActorData = MakeShared(); - break; - case ActorType::TrafficLight: - ActorData = MakeShared(); - break; - case ActorType::Vehicle: - ActorData = MakeShared(); - break; - case ActorType::Walker: - ActorData = MakeShared(); - break; - case ActorType::Sensor: - ActorData = MakeShared(); - break; - default: - ActorData = MakeShared(); - break; - } -} - -void FActorView::PutActorToSleep(UCarlaEpisode* CarlaEpisode) -{ - State = carla::rpc::ActorState::Dormant; - if (ActorData) - { - ActorData->RecordActorData(TheActor, CarlaEpisode); - } - TheActor->Destroy(); - TheActor = nullptr; -} - -void FActorView::WakeActorUp(UCarlaEpisode* CarlaEpisode) -{ - TheActor = ActorData->RespawnActor(CarlaEpisode, *Info); - if (TheActor == nullptr) - { - State = carla::rpc::ActorState::Invalid; - UE_LOG(LogCarla, Error, TEXT("Could not wake up dormant actor %d at location %s"), GetActorId(), *(ActorData->GetLocalTransform(CarlaEpisode).GetLocation().ToString())); - return; - } - State = carla::rpc::ActorState::Alive; - ActorData->RestoreActorData(TheActor, CarlaEpisode); -} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorView.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorView.h deleted file mode 100644 index a56f8e69b..000000000 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorView.h +++ /dev/null @@ -1,193 +0,0 @@ -// 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 . - -#pragma once - -#include "Carla/Actor/ActorInfo.h" -#include "Carla/Actor/ActorData.h" - -#include "carla/rpc/ActorState.h" -#include "carla/rpc/AttachmentType.h" - -#include "ActorView.generated.h" - -class AActor; - -/// A view over an actor and its properties. -USTRUCT() -struct FActorView -{ - GENERATED_BODY() -public: - - using IdType = uint32; - - enum class ActorType : uint8 - { - Other, - Vehicle, - Walker, - TrafficLight, - TrafficSign, - Sensor, - INVALID - }; - - FActorView() = default; - // FActorView(const FActorView &) = default; - // FActorView(FActorView &&) = default; - - bool IsValid() const - { - return (carla::rpc::ActorState::Invalid != State); - } - - bool IsAlive() const - { - return (carla::rpc::ActorState::Alive == State); - } - - bool IsDormant() const - { - return (carla::rpc::ActorState::Dormant == State); - } - - bool IsPendingKill() const - { - return (carla::rpc::ActorState::PendingKill == State); - } - - IdType GetActorId() const - { - return Id; - } - - ActorType GetActorType() const - { - return Type; - } - - AActor *GetActor() - { - return TheActor; - } - - const AActor *GetActor() const - { - return TheActor; - } - - const FActorInfo *GetActorInfo() const - { - return Info.Get(); - } - - carla::rpc::ActorState GetActorState() const - { - return State; - } - - void SetActorState(carla::rpc::ActorState InState) - { - State = InState; - } - - void SetParent(IdType InParentId) - { - ParentId = InParentId; - } - - IdType GetParent() const - { - return ParentId; - } - - void AddChildren(IdType ChildId) - { - Children.Add(ChildId); - } - - void RemoveChildren(IdType ChildId) - { - Children.Remove(ChildId); - } - - const TArray& GetChildren() const - { - return Children; - } - - void SetAttachmentType(carla::rpc::AttachmentType InAttachmentType) - { - Attachment = InAttachmentType; - } - - carla::rpc::AttachmentType GetAttachmentType() const - { - return Attachment; - } - - void BuildActorData(); - - void PutActorToSleep(UCarlaEpisode* CarlaEpisode); - - void WakeActorUp(UCarlaEpisode* CarlaEpisode); - - FActorData* GetActorData() - { - return ActorData.Get(); - } - - const FActorData* GetActorData() const - { - return ActorData.Get(); - } - - template - T* GetActorData() - { - return dynamic_cast(ActorData.Get()); - } - - template - const T* GetActorData() const - { - return dynamic_cast(ActorData.Get()); - } - -private: - - friend class FActorRegistry; - - FActorView( - IdType ActorId, - AActor* Actor, - TSharedPtr Info, - carla::rpc::ActorState InState) - : TheActor(Actor), - Info(std::move(Info)), - Id(ActorId), - State(InState) {} - - AActor *TheActor = nullptr; - - TSharedPtr Info = nullptr; - - IdType Id = 0u; - - IdType ParentId = 0u; - - carla::rpc::ActorState State = carla::rpc::ActorState::Invalid; - - carla::rpc::AttachmentType Attachment = carla::rpc::AttachmentType::INVALID; - - TArray Children; - - ActorType Type = ActorType::INVALID; - - TSharedPtr ActorData = nullptr; - -}; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/CarlaActor.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/CarlaActor.cpp index f268e65da..237e99af4 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/CarlaActor.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/CarlaActor.cpp @@ -152,7 +152,7 @@ void FCarlaActor::PutActorToSleep(UCarlaEpisode* CarlaEpisode) State = carla::rpc::ActorState::Dormant; if (ActorData) { - ActorData->RecordActorData(TheActor, CarlaEpisode); + ActorData->RecordActorData(this, CarlaEpisode); } TheActor->Destroy(); TheActor = nullptr; @@ -163,12 +163,11 @@ void FCarlaActor::WakeActorUp(UCarlaEpisode* CarlaEpisode) TheActor = ActorData->RespawnActor(CarlaEpisode, *Info); if (TheActor == nullptr) { - State = carla::rpc::ActorState::Invalid; UE_LOG(LogCarla, Error, TEXT("Could not wake up dormant actor %d at location %s"), GetActorId(), *(ActorData->GetLocalTransform(CarlaEpisode).GetLocation().ToString())); return; } - State = carla::rpc::ActorState::Alive; - ActorData->RestoreActorData(TheActor, CarlaEpisode); + State = carla::rpc::ActorState::Active; + ActorData->RestoreActorData(this, CarlaEpisode); } FTransform FCarlaActor::GetActorLocalTransform() const @@ -904,7 +903,8 @@ ECarlaServerResponse FTrafficLightActor::SetTrafficLightState(const ETrafficLigh { if (IsDormant()) { - // Todo: impletent to affect controller + FTrafficLightData* ActorData = GetActorData(); + ActorData->LightState = State; } else { @@ -918,6 +918,24 @@ ECarlaServerResponse FTrafficLightActor::SetTrafficLightState(const ETrafficLigh return ECarlaServerResponse::Success; } +ETrafficLightState FTrafficLightActor::GetTrafficLightState() const +{ + if (IsDormant()) + { + const FTrafficLightData* ActorData = GetActorData(); + return ActorData->LightState; + } + else + { + auto TrafficLight = Cast(GetActor()); + if (TrafficLight == nullptr) + { + return ETrafficLightState::Off; + } + return TrafficLight->GetTrafficLightState(); + } +} + UTrafficLightController* FTrafficLightActor::GetTrafficLightController() { if (IsDormant()) diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/CarlaActor.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/CarlaActor.h index ac75bb1cc..bd9de7726 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/CarlaActor.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/CarlaActor.h @@ -58,7 +58,13 @@ public: bool IsAlive() const { - return (carla::rpc::ActorState::Alive == State); + return (carla::rpc::ActorState::PendingKill != State && + carla::rpc::ActorState::Invalid != State); + } + + bool IsActive() const + { + return (carla::rpc::ActorState::Active == State); } bool IsDormant() const @@ -301,6 +307,11 @@ public: return ECarlaServerResponse::ActorTypeMismatch; } + virtual ETrafficLightState GetTrafficLightState() const + { + return ETrafficLightState::Off; + } + virtual UTrafficLightController* GetTrafficLightController() { return nullptr; @@ -478,6 +489,8 @@ public: virtual ECarlaServerResponse SetTrafficLightState(const ETrafficLightState& State) final; + virtual ETrafficLightState GetTrafficLightState() const final; + virtual UTrafficLightController* GetTrafficLightController() final; virtual ECarlaServerResponse SetLightGreenTime(float time) final; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Commandlet/MoveAssetsCommandlet.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Commandlet/MoveAssetsCommandlet.cpp index 84249f82e..d7cfe6652 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Commandlet/MoveAssetsCommandlet.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Commandlet/MoveAssetsCommandlet.cpp @@ -22,7 +22,7 @@ UMoveAssetsCommandlet::UMoveAssetsCommandlet() namespace SSTags { // Carla Semantic Segmentation Folder Tags static const FString ROAD = TEXT("Road"); - static const FString ROADLINES = TEXT("RoadLines"); + static const FString ROADLINE = TEXT("RoadLine"); static const FString TERRAIN = TEXT("Terrain"); static const FString GRASS = TEXT("Terrain"); static const FString SIDEWALK = TEXT("SideWalk"); @@ -125,7 +125,7 @@ void UMoveAssetsCommandlet::MoveAssetsFromMapForSemanticSegmentation( AssetsObjectLibrary->GetAssetDataList(MapContents); AssetsObjectLibrary->ClearLoaded(); - TArray DestinationPaths = {SSTags::ROAD, SSTags::ROADLINES, SSTags::TERRAIN, SSTags::GRASS, SSTags::SIDEWALK, SSTags::CURB, SSTags::GUTTER}; + TArray DestinationPaths = {SSTags::ROAD, SSTags::ROADLINE, SSTags::TERRAIN, SSTags::GRASS, SSTags::SIDEWALK, SSTags::CURB, SSTags::GUTTER}; // Init Map with keys TMap> AssetDataMap; @@ -159,7 +159,7 @@ void UMoveAssetsCommandlet::MoveAssetsFromMapForSemanticSegmentation( } else if (AssetName.Contains(SSTags::R_MARKING1) || AssetName.Contains(SSTags::R_MARKING2)) { - AssetDataMap[SSTags::ROADLINES].Add(MeshAsset); + AssetDataMap[SSTags::ROADLINE].Add(MeshAsset); } else if (AssetName.Contains(SSTags::R_TERRAIN)) { diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Commandlet/PrepareAssetsForCookingCommandlet.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Commandlet/PrepareAssetsForCookingCommandlet.cpp index 3fa77f310..532dde163 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Commandlet/PrepareAssetsForCookingCommandlet.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Commandlet/PrepareAssetsForCookingCommandlet.cpp @@ -12,6 +12,7 @@ #include "HAL/PlatformFilemanager.h" #include "UObject/ConstructorHelpers.h" #include "Materials/MaterialInstanceConstant.h" +#include "Carla/MapGen/LargeMapManager.h" static bool ValidateStaticMesh(UStaticMesh *Mesh) { @@ -131,6 +132,25 @@ void UPrepareAssetsForCookingCommandlet::LoadWorldTile(FAssetData &AssetData) } } +void UPrepareAssetsForCookingCommandlet::LoadLargeMapWorld(FAssetData &AssetData) +{ + // BaseMap path inside Carla + const FString BaseMap = TEXT("/Game/Carla/Maps/BaseLargeMap"); + + // Load Map folder using object library + MapObjectLibrary = UObjectLibrary::CreateLibrary(UWorld::StaticClass(), false, GIsEditor); + MapObjectLibrary->AddToRoot(); + MapObjectLibrary->LoadAssetDataFromPath(*BaseMap); + MapObjectLibrary->LoadAssetsFromAssetData(); + MapObjectLibrary->GetAssetDataList(AssetDatas); + + if (AssetDatas.Num() > 0) + { + // Extract first asset found in folder path (i.e. the BaseMap) + AssetData = AssetDatas.Pop(); + } +} + TArray UPrepareAssetsForCookingCommandlet::SpawnMeshesToWorld( const TArray &AssetsPaths, bool bUseCarlaMaterials, @@ -293,7 +313,8 @@ bool UPrepareAssetsForCookingCommandlet::SaveWorld( FAssetData &AssetData, const FString &PackageName, const FString &DestPath, - const FString &WorldName) + const FString &WorldName, + bool bGenerateSpawnPoints) { // Create Package to save UPackage *Package = AssetData.GetPackage(); @@ -314,7 +335,7 @@ bool UPrepareAssetsForCookingCommandlet::SaveWorld( WorldName + TEXT("/OpenDrive/") + WorldName + TEXT(".xodr"); bool bPackageSaved = false; - if (FPaths::FileExists(PathXODR)) + if (FPaths::FileExists(PathXODR) && bGenerateSpawnPoints) { // We need to spawn OpenDrive assets before saving the map AOpenDriveActor *OpenWorldActor = CastChecked( @@ -459,6 +480,21 @@ void UPrepareAssetsForCookingCommandlet::GenerateMapPathsFile( for (const auto &Map : AssetsPaths.MapsPaths) { MapPathData.Append(Map.Path + TEXT("/") + Map.Name + TEXT("+")); + TArray AssetsData; + UObjectLibrary* ObjectLibrary = UObjectLibrary::CreateLibrary(UWorld::StaticClass(), true, true); + ObjectLibrary->LoadAssetDataFromPath(Map.Path); + ObjectLibrary->GetAssetDataList(AssetsData); + int NumTiles = 0; + for (FAssetData &AssetData : AssetsData) + { + FString AssetName = AssetData.AssetName.ToString(); + if (AssetName.Contains(Map.Name + "_Tile_")) + { + MapPathData.Append(Map.Path + TEXT("/") + AssetName + TEXT("+")); + NumTiles++; + } + } + UE_LOG(LogTemp, Warning, TEXT("Found %d tiles"), NumTiles); } if (!PropsMapPath.IsEmpty()) @@ -495,7 +531,7 @@ void UPrepareAssetsForCookingCommandlet::PrepareMapsForCooking( const FString DefaultPath = TEXT("/Game/") + PackageName + TEXT("/Maps/") + Map.Name; const FString RoadsPath = BasePath + SSTags::ROAD + MapPath; - const FString RoadLinesPath = BasePath + SSTags::ROADLINES + MapPath; + const FString RoadLinesPath = BasePath + SSTags::ROADLINE + MapPath; const FString TerrainPath = BasePath + SSTags::TERRAIN + MapPath; const FString SidewalkPath = BasePath + SSTags::SIDEWALK + MapPath; @@ -525,6 +561,33 @@ void UPrepareAssetsForCookingCommandlet::PrepareMapsForCooking( } else { + TArray> MapPathsIds; + + FVector PositionTile0 = FVector(); + float TileSize = 200000.f; + FString TxtFile; + FString TilesInfoPath = FPaths::ProjectContentDir() + PackageName + TEXT("/Maps/") + Map.Name + "/TilesInfo.txt"; + UE_LOG(LogTemp, Warning, TEXT("Loading %s ..."), *TilesInfoPath); + if (FFileHelper::LoadFileToString(TxtFile, *(TilesInfoPath)) == true) { + + TArray Out; + TxtFile.ParseIntoArray(Out, TEXT(","), true); + if (Out.Num() >= 3) + { + const float METERSTOCM = 100.f; + PositionTile0.X = METERSTOCM * FCString::Atof(*Out[0]); + PositionTile0.Y = METERSTOCM * FCString::Atof(*Out[1]); + TileSize = METERSTOCM * FCString::Atof(*Out[2]); + } + else + { + UE_LOG(LogTemp, Warning, TEXT("TilesInfo.txt format is invalid file")); + } + } + else { + UE_LOG(LogTemp, Warning, TEXT("Could not find TilesInfo.txt file")); + } + UE_LOG(LogTemp, Log, TEXT("Cooking tiles:")); // Load World FAssetData AssetData; @@ -556,6 +619,9 @@ void UPrepareAssetsForCookingCommandlet::PrepareMapsForCooking( // Save the World in specified path // UE_LOG(LogTemp, Log, TEXT("Saving as %s to %s"), *TileName, *Map.Path); SaveWorld(AssetData, PackageName, Map.Path, TileName); + MapPathsIds.Add( + TPair( + Map.Path + "/" + TileName, FIntVector(i, j, 0))); // Remove spawned actors from world to keep equal as BaseMap DestroySpawnedActorsInWorld(SpawnedActors); ++i; @@ -565,6 +631,31 @@ void UPrepareAssetsForCookingCommandlet::PrepareMapsForCooking( ++j; } while (i > 0); + + #if WITH_EDITOR + UEditorLoadingAndSavingUtils::SaveDirtyPackages(true, true); + #endif + // Load base map for tiled maps + LoadLargeMapWorld(AssetData); + BaseMapRedirector = Cast(AssetData.GetAsset()); + if (BaseMapRedirector != nullptr) { + World = CastChecked(BaseMapRedirector->DestinationObject); + } + else { + World = CastChecked(AssetData.GetAsset()); + } + + // Generate Large Map Manager + ALargeMapManager* LargeMapManager = World->SpawnActor( + ALargeMapManager::StaticClass(), FTransform()); + LargeMapManager->LargeMapTilePath = Map.Path; + LargeMapManager->LargeMapName = Map.Name; + LargeMapManager->SetTile0Offset(PositionTile0); + LargeMapManager->SetTileSize(TileSize); + LargeMapManager->GenerateMap(MapPathsIds); + + SaveWorld(AssetData, PackageName, Map.Path, Map.Name, false); + UE_LOG(LogTemp, Log, TEXT("End cooking tiles")); } } diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Commandlet/PrepareAssetsForCookingCommandlet.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Commandlet/PrepareAssetsForCookingCommandlet.h index 12724e4de..98beea6e3 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Commandlet/PrepareAssetsForCookingCommandlet.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Commandlet/PrepareAssetsForCookingCommandlet.h @@ -78,6 +78,8 @@ public: /// structure. void LoadWorldTile(FAssetData &AssetData); + void LoadLargeMapWorld(FAssetData &AssetData); + /// Spawns all the static meshes located in @a AssetsPaths inside the World. /// There is an option to use Carla materials by setting @a bUseCarlaMaterials /// to true, otherwise it will use RoadRunner materials. @@ -87,7 +89,7 @@ public: TArray SpawnMeshesToWorld( const TArray &AssetsPaths, bool bUseCarlaMaterials, - int i = -1, + int i = -1, int j = -1); /// Saves the current World, contained in @a AssetData, into @a DestPath @@ -96,7 +98,8 @@ public: FAssetData &AssetData, const FString &PackageName, const FString &DestPath, - const FString &WorldName); + const FString &WorldName, + bool bGenerateSpawnPoints = true); /// Destroys all the previously spawned actors stored in @a SpawnedActors void DestroySpawnedActorsInWorld(TArray &SpawnedActors); diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaEngine.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaEngine.cpp index 3ab150642..8137b2dbe 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaEngine.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaEngine.cpp @@ -16,6 +16,7 @@ #include "Runtime/Core/Public/Misc/App.h" #include "PhysicsEngine/PhysicsSettings.h" +#include "Carla/MapGen/LargeMapManager.h" #include @@ -183,6 +184,14 @@ void FCarlaEngine::OnEpisodeSettingsChanged(const FEpisodeSettings &Settings) PhysSett->bSubstepping = Settings.bSubstepping; PhysSett->MaxSubstepDeltaTime = Settings.MaxSubstepDeltaTime; PhysSett->MaxSubsteps = Settings.MaxSubsteps; + + UWorld* World = CurrentEpisode->GetWorld(); + ALargeMapManager* LargeMapManager = UCarlaStatics::GetLargeMapManager(World); + if (LargeMapManager) + { + LargeMapManager->SetLayerStreamingDistance(Settings.TileStreamingDistance); + LargeMapManager->SetActorStreamingDistance(Settings.ActorActiveDistance); + } } void FCarlaEngine::ResetSimulationState() diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaEngine.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaEngine.h index c6a794ef3..61f98da43 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaEngine.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaEngine.h @@ -47,18 +47,18 @@ public: } static uint64_t GetFrameCounter() - { + { return FCarlaEngine::FrameCounter; } - static uint64_t UpdateFrameCounter() - { + static uint64_t UpdateFrameCounter() + { FCarlaEngine::FrameCounter += 1; return FCarlaEngine::FrameCounter; } static void ResetFrameCounter(uint64_t Value = 0) - { + { FCarlaEngine::FrameCounter = Value; } diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaEpisode.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaEpisode.cpp index 09a31d6be..9ad18c223 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaEpisode.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaEpisode.cpp @@ -17,6 +17,7 @@ #include "Carla/Util/RandomEngine.h" #include "Carla/Vehicle/VehicleSpawnPoint.h" #include "Carla/Game/CarlaStatics.h" +#include "Carla/MapGen/LargeMapManager.h" #include "Engine/StaticMeshActor.h" #include "EngineUtils.h" @@ -215,8 +216,13 @@ bool UCarlaEpisode::LoadNewOpendriveEpisode( void UCarlaEpisode::ApplySettings(const FEpisodeSettings &Settings) { - FCarlaStaticDelegates::OnEpisodeSettingsChange.Broadcast(Settings); EpisodeSettings = Settings; + if(EpisodeSettings.ActorActiveDistance > EpisodeSettings.TileStreamingDistance) + { + UE_LOG(LogCarla, Warning, TEXT("Setting ActorActiveDistance is smaller that TileStreamingDistance, TileStreamingDistance will be increased")); + EpisodeSettings.TileStreamingDistance = EpisodeSettings.ActorActiveDistance; + } + FCarlaStaticDelegates::OnEpisodeSettingsChange.Broadcast(EpisodeSettings); } TArray UCarlaEpisode::GetRecommendedSpawnPoints() const @@ -387,3 +393,33 @@ std::string UCarlaEpisode::StartRecorder(std::string Name, bool AdditionalData) return result; } + +TPair UCarlaEpisode::SpawnActorWithInfo( + const FTransform &Transform, + FActorDescription thisActorDescription, + FCarlaActor::IdType DesiredId) +{ + ALargeMapManager* LargeMap = UCarlaStatics::GetLargeMapManager(GetWorld()); + FTransform LocalTransform = Transform; + if(LargeMap) + { + LocalTransform = LargeMap->GlobalToLocalTransform(LocalTransform); + } + + // NewTransform.AddToTranslation(-1.0f * FVector(CurrentMapOrigin)); + auto result = ActorDispatcher->SpawnActor(LocalTransform, thisActorDescription, DesiredId); + if (Recorder->IsEnabled()) + { + if (result.Key == EActorSpawnResultStatus::Success) + { + Recorder->CreateRecorderEventAdd( + result.Value->GetActorId(), + static_cast(result.Value->GetActorType()), + Transform, + std::move(thisActorDescription) + ); + } + } + + return result; +} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaEpisode.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaEpisode.h index f9177f082..7eaea9e5b 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaEpisode.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaEpisode.h @@ -176,26 +176,7 @@ public: TPair SpawnActorWithInfo( const FTransform &Transform, FActorDescription thisActorDescription, - FCarlaActor::IdType DesiredId = 0) - { - FTransform NewTransform = Transform; - // NewTransform.AddToTranslation(-1.0f * FVector(CurrentMapOrigin)); - auto result = ActorDispatcher->SpawnActor(NewTransform, thisActorDescription, DesiredId); - if (Recorder->IsEnabled()) - { - if (result.Key == EActorSpawnResultStatus::Success) - { - Recorder->CreateRecorderEventAdd( - result.Value->GetActorId(), - static_cast(result.Value->GetActorType()), - NewTransform, - std::move(thisActorDescription) - ); - } - } - - return result; - } + FCarlaActor::IdType DesiredId = 0); /// Spawns an actor based on @a ActorDescription at @a Transform. /// diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameModeBase.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameModeBase.cpp index 4dcda9482..ec97fee3c 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameModeBase.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameModeBase.cpp @@ -69,7 +69,10 @@ void ACarlaGameModeBase::InitGame( UGameplayStatics::GetActorOfClass(GetWorld(), ALargeMapManager::StaticClass()); LMManager = Cast(LMManagerActor); if (LMManager) { - LMManager->GenerateLargeMap(); + if (LMManager->GetNumTiles() == 0) + { + LMManager->GenerateLargeMap(); + } InMapName = LMManager->LargeMapName; } @@ -149,12 +152,6 @@ void ACarlaGameModeBase::BeginPlay() UWorld* World = GetWorld(); check(World != nullptr); - if(LMManager) - { - //ULocalPlayer* Player = GEngine->GetGamePlayer(World, 0); - //LMManager->AddActorToConsider(Player->GetPlayerController(World)->GetPawn()); - } - LoadMapLayer(GameInstance->GetCurrentMapLayer()); ReadyToRegisterObjects = true; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/Tagger.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/Tagger.cpp index e214af2e4..6bda5d19a 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/Tagger.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/Tagger.cpp @@ -100,6 +100,13 @@ void ATagger::TagActorsInLevel(UWorld &World, bool bTagForSemanticSegmentation) } } +void ATagger::TagActorsInLevel(ULevel &Level, bool bTagForSemanticSegmentation) +{ + for (AActor * Actor : Level.Actors) { + TagActor(*Actor, bTagForSemanticSegmentation); + } +} + void ATagger::GetTagsOfTaggedActor(const AActor &Actor, TSet &Tags) { TArray Components; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/Tagger.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/Tagger.h index 5c8e578c5..2213a65f4 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/Tagger.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/Tagger.h @@ -46,6 +46,8 @@ public: /// objects having this value active. static void TagActorsInLevel(UWorld &World, bool bTagForSemanticSegmentation); + static void TagActorsInLevel(ULevel &Level, bool bTagForSemanticSegmentation); + /// Retrieve the tag of an already tagged component. static crp::CityObjectLabel GetTagOfTaggedComponent(const UPrimitiveComponent &Component) { diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/LargeMapManager.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/LargeMapManager.cpp index 934cba1d5..7c55cfc21 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/LargeMapManager.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/LargeMapManager.cpp @@ -9,15 +9,12 @@ #include "UncenteredPivotPointMesh.h" #include "Walker/WalkerBase.h" +#include "Carla/Game/Tagger.h" #include "FileHelper.h" #include "Paths.h" -#if WITH_EDITOR #define LARGEMAP_LOGS 1 -#else -#define LARGEMAP_LOGS 0 -#endif // WITH_EDITOR #if LARGEMAP_LOGS #define LM_LOG(Level, Msg, ...) UE_LOG(LogCarla, Level, TEXT(Msg), ##__VA_ARGS__) @@ -48,6 +45,7 @@ ALargeMapManager::~ALargeMapManager() void ALargeMapManager::BeginPlay() { Super::BeginPlay(); + RegisterTilesInWorldComposition(); UWorld* World = GetWorld(); /// Setup delegates @@ -110,6 +108,7 @@ void ALargeMapManager::PostWorldOriginOffset(UWorld* InWorld, FIntVector InSrcOr void ALargeMapManager::OnLevelAddedToWorld(ULevel* InLevel, UWorld* InWorld) { LM_LOG(Warning, "OnLevelAddedToWorld"); + ATagger::TagActorsInLevel(*InLevel, true); //FDebug::DumpStackTraceToLog(ELogVerbosity::Log); } @@ -186,11 +185,11 @@ void ALargeMapManager::OnActorSpawned( LM_LOG(Error, "... not hero vehicle ..."); if(IsValid(Actor)) { // Actor was spwaned succesfully - // TODO: not dormant but not hero => GhostActor - // LM: Map maybe per tile and in a tile sublevel? + // TODO: not dormant but not hero => ActiveActor + // LM: Map maybe per tile and in a tile sublevel? - LM_LOG(Error, "GHOST VEHICLE DETECTED"); - GhostActors.Add(CarlaActor.GetActorId()); + LM_LOG(Error, "ACTIVE VEHICLE DETECTED"); + ActiveActors.Add(CarlaActor.GetActorId()); } else { // Actor was spawned as dormant @@ -225,6 +224,40 @@ void ALargeMapManager::OnActorDestroyed(AActor* DestroyedActor) } +void ALargeMapManager::SetTile0Offset(const FVector& Offset) +{ + Tile0Offset = Offset; +} + +void ALargeMapManager::SetTileSize(float Size) +{ + TileSide = Size; +} + +void ALargeMapManager::SetLayerStreamingDistance(float Distance) +{ + LayerStreamingDistance = Distance; + LayerStreamingDistanceSquared = + LayerStreamingDistance*LayerStreamingDistance; +} + +void ALargeMapManager::SetActorStreamingDistance(float Distance) +{ + ActorStreamingDistance = Distance; + ActorStreamingDistanceSquared = + ActorStreamingDistance*ActorStreamingDistance; +} + +float ALargeMapManager::GetLayerStreamingDistance() const +{ + return LayerStreamingDistance; +} + +float ALargeMapManager::GetActorStreamingDistance() const +{ + return ActorStreamingDistance; +} + FTransform ALargeMapManager::GlobalToLocalTransform(const FTransform& InTransform) const { return FTransform( @@ -261,8 +294,8 @@ void ALargeMapManager::Tick(float DeltaTime) // Also, to avoid looping over the heros again, it checks if any actor to consider has been removed UpdateTilesState(); - // Check if ghost actors are still in range and inside a loaded tile or have to be converted to dormant - CheckGhostActors(); + // Check if active actors are still in range and inside a loaded tile or have to be converted to dormant + CheckActiveActors(); // Check if dormant actors have been moved to the load range CheckDormantActors(); @@ -270,9 +303,9 @@ void ALargeMapManager::Tick(float DeltaTime) // Remove the hero actors that doesn't exits any more from the ActorsToConsider vector RemovePendingActorsToRemove(); - ConvertGhostToDormantActors(); + ConvertActiveToDormantActors(); - ConvertDormantToGhostActors(); + ConvertDormantToActiveActors(); CheckIfRebaseIsNeeded(); @@ -290,6 +323,7 @@ void ALargeMapManager::GenerateMap(FString InAssetsPath) { TRACE_CPUPROFILER_EVENT_SCOPE(ALargeMapManager::GenerateMap); LM_LOG(Warning, "Generating Map %s ...", *InAssetsPath); + ClearWorldAndTiles(); AssetsPath = InAssetsPath; @@ -309,6 +343,10 @@ void ALargeMapManager::GenerateMap(FString InAssetsPath) // LM_LOG(Warning, "Asset class: %s", *(AssetData.AssetClass.ToString())); #endif FString TileName = AssetData.AssetName.ToString(); + if (!TileName.Contains("_Tile_")) + { + continue; + } FString TileName_X = ""; FString TileName_Y = ""; size_t i = TileName.Len()-1; @@ -345,10 +383,50 @@ void ALargeMapManager::GenerateMap(FString InAssetsPath) #endif // WITH_EDITOR } +void ALargeMapManager::GenerateMap(TArray> MapPathsIds) +{ + TRACE_CPUPROFILER_EVENT_SCOPE(ALargeMapManager::GenerateMap); + LM_LOG(Warning, "Generating Large Map"); + ClearWorldAndTiles(); + + for (TPair& PathId : MapPathsIds) + { + FIntVector& TileVectorID = PathId.Value; + FString& Path = PathId.Key; + TileID TileId = GetTileID(TileVectorID); + LoadCarlaMapTile(Path, TileId); + } + + #if WITH_EDITOR + LM_LOG(Warning, "GenerateMap num Tiles generated %d", MapTiles.Num()); + #endif // WITH_EDITOR +} + +void ALargeMapManager::ClearWorldAndTiles() +{ + MapTiles.Empty(); +} + +void ALargeMapManager::RegisterTilesInWorldComposition() +{ + UWorld* World = GetWorld(); + UWorldComposition* WorldComposition = World->WorldComposition; + World->ClearStreamingLevels(); + WorldComposition->TilesStreaming.Empty(); + WorldComposition->GetTilesList().Empty(); + + for (auto& It : MapTiles) + { + ULevelStreamingDynamic* StreamingLevel = It.Value.StreamingLevel; + World->AddStreamingLevel(StreamingLevel); + WorldComposition->TilesStreaming.Add(StreamingLevel); + } +} + // TODO: maybe remove this, I think I will not need it any more void ALargeMapManager::AddActorToUnloadedList(const FCarlaActor& CarlaActor, const FTransform& Transform) { - // GhostActors.Add(CarlaActor.GetActorId(), {Transform, CarlaActor}); + // ActiveActors.Add(CarlaActor.GetActorId(), {Transform, CarlaActor}); } FIntVector ALargeMapManager::GetNumTilesInXY() const @@ -460,28 +538,6 @@ ALargeMapManager::TileID ALargeMapManager::GetTileID(FDVector TileLocation) cons return GetTileID(TileID); } -FCarlaMapTile& ALargeMapManager::GetCarlaMapTile(FVector Location) -{ - // Asset Location -> TileID - TileID TileID = GetTileID(Location); - - FCarlaMapTile* Tile = MapTiles.Find(TileID); - if (Tile) return *Tile; // Tile found - - // Need to generate a new Tile - FCarlaMapTile NewTile; - // 1 - Calculate the Tile position - NewTile.Location = GetTileLocation(TileID); -#if WITH_EDITOR - // 2 - Generate Tile name - NewTile.Name = GenerateTileName(TileID); -#endif // WITH_EDITOR - // 3 - Generate the StreamLevel - NewTile.StreamingLevel = AddNewTile(NewTile.Name, NewTile.Location); - // 4 - Add it to the map - return MapTiles.Add(TileID, NewTile); -} - FCarlaMapTile& ALargeMapManager::GetCarlaMapTile(ULevel* InLevel) { FCarlaMapTile* Tile = nullptr; @@ -506,15 +562,25 @@ FCarlaMapTile* ALargeMapManager::GetCarlaMapTile(FIntVector TileVectorID) return Tile; } -ULevelStreamingDynamic* ALargeMapManager::AddNewTile(FString TileName, FVector TileLocation) -{ +FCarlaMapTile& ALargeMapManager::LoadCarlaMapTile(FString TileMapPath, TileID TileId) { + TRACE_CPUPROFILER_EVENT_SCOPE(ALargeMapManager::LoadCarlaMapTile); + // Need to generate a new Tile + FCarlaMapTile NewTile; + // 1 - Calculate the Tile position + FIntVector VTileID = GetTileVectorID(TileId); + NewTile.Location = GetTileLocation(TileId); + NewTile.Name = TileMapPath; + + // 3 - Generate the StreamLevel + FVector TileLocation = NewTile.Location; + FString TileName = NewTile.Name; UWorld* World = GetWorld(); UWorldComposition* WorldComposition = World->WorldComposition; - FString FullName = BaseTileMapPath; + FString FullName = TileMapPath; FString PackageFileName = FullName; FString LongLevelPackageName = FPackageName::FilenameToLongPackageName(PackageFileName); - FString UniqueLevelPackageName = LongLevelPackageName + TileName; + FString UniqueLevelPackageName = LongLevelPackageName; ULevelStreamingDynamic* StreamingLevel = NewObject(World, *TileName); check(StreamingLevel); @@ -552,111 +618,6 @@ ULevelStreamingDynamic* ALargeMapManager::AddNewTile(FString TileName, FVector T //Actual map package to load StreamingLevel->PackageNameToLoad = *LongLevelPackageName; - - World->AddStreamingLevel(StreamingLevel); - WorldComposition->TilesStreaming.Add(StreamingLevel); - - - FWorldTileInfo Info; - Info.AbsolutePosition = FIntVector(TileLocation); - Info.Position = Info.AbsolutePosition; - FWorldTileLayer WorldTileLayer; - WorldTileLayer.Name = "CarlaLayer"; - WorldTileLayer.StreamingDistance = LayerStreamingDistance; - WorldTileLayer.DistanceStreamingEnabled = false; // we will handle this, not unreal - Info.Layer = WorldTileLayer; - - FWorldCompositionTile NewTile; - NewTile.PackageName = *LongLevelPackageName; - NewTile.Info = Info; - WorldComposition->GetTilesList().Add(NewTile); - - return StreamingLevel; -} - -FCarlaMapTile& ALargeMapManager::LoadCarlaMapTile(FString TileMapPath, TileID TileId) { - TRACE_CPUPROFILER_EVENT_SCOPE(ALargeMapManager::LoadCarlaMapTile); - // Need to generate a new Tile - FCarlaMapTile NewTile; - // 1 - Calculate the Tile position - FIntVector VTileID = GetTileVectorID(TileId); - // FVector OriginOffset = FVector(FMath::Sign(VTileID.X + 0.5f), FMath::Sign(VTileID.Y + 0.5f), FMath::Sign(VTileID.Z)) * 0.5f; - // NewTile.Location = (FVector(VTileID) + OriginOffset)* TileSide * 0.5f; - NewTile.Location = GetTileLocation(TileId); -#if WITH_EDITOR - // 2 - Generate Tile name - NewTile.Name = GenerateTileName(TileId); - // LM_LOG(Warning, "Tile idd: %d, %d", VTileID.X, VTileID.Y); - // LM_LOG(Warning, "Tile location: %f, %f", NewTile.Location.X, NewTile.Location.Y); -#endif // WITH_EDITOR - - // 3 - Generate the StreamLevel - FVector TileLocation = NewTile.Location; - FString TileName = NewTile.Name; - UWorld* World = GetWorld(); - UWorldComposition* WorldComposition = World->WorldComposition; - - FString FullName = TileMapPath; - FString PackageFileName = FullName; - FString LongLevelPackageName = FPackageName::FilenameToLongPackageName(PackageFileName); - FString UniqueLevelPackageName = LongLevelPackageName + TileName; - - ULevelStreamingDynamic* StreamingLevel = NewObject(World, *TileName, RF_Transient); - check(StreamingLevel); - - StreamingLevel->SetWorldAssetByPackageName(*UniqueLevelPackageName); - -#if WITH_EDITOR - if (World->IsPlayInEditor()) - { - FWorldContext WorldContext = GEngine->GetWorldContextFromWorldChecked(World); - StreamingLevel->RenameForPIE(WorldContext.PIEInstance); - } - StreamingLevel->SetShouldBeVisibleInEditor(true); - StreamingLevel->LevelColor = FColor::MakeRandomColor(); -#endif // WITH_EDITOR - - StreamingLevel->SetShouldBeLoaded(false); - StreamingLevel->SetShouldBeVisible(false); - StreamingLevel->bShouldBlockOnLoad = ShouldTilesBlockOnLoad; - StreamingLevel->bInitiallyLoaded = false; - StreamingLevel->bInitiallyVisible = false; - StreamingLevel->LevelTransform = FTransform(TileLocation); - StreamingLevel->PackageNameToLoad = *FullName; - - if (!FPackageName::DoesPackageExist(FullName, NULL, &PackageFileName)) - { - LM_LOG(Error, "Level does not exist in package with FullName variable -> %s", *FullName); - } - - if (!FPackageName::DoesPackageExist(LongLevelPackageName, NULL, &PackageFileName)) - { - LM_LOG(Error, "Level does not exist in package with LongLevelPackageName variable -> %s", *LongLevelPackageName); - } - - //Actual map package to load - StreamingLevel->PackageNameToLoad = *LongLevelPackageName; - - - World->AddStreamingLevel(StreamingLevel); - WorldComposition->TilesStreaming.Add(StreamingLevel); - - - // FWorldTileInfo Info; - // Info.AbsolutePosition = FIntVector(TileLocation); - // Info.Position = Info.AbsolutePosition; - // LM_LOG(Warning, "Absolute level position %s", *Info.AbsolutePosition.ToString()); - // FWorldTileLayer WorldTileLayer; - // WorldTileLayer.Name = "CarlaLayer"; - // WorldTileLayer.StreamingDistance = LayerStreamingDistance; - // WorldTileLayer.DistanceStreamingEnabled = false; // we will handle this, not unreal - // Info.Layer = WorldTileLayer; - - // FWorldCompositionTile WorldCompositionTile; - // WorldCompositionTile.PackageName = *FullName; - // WorldCompositionTile.Info = Info; - // WorldComposition->GetTilesList().Add(WorldCompositionTile); - NewTile.StreamingLevel = StreamingLevel; // 4 - Add it to the map @@ -697,9 +658,9 @@ void ALargeMapManager::UpdateTilesState() void ALargeMapManager::RemovePendingActorsToRemove() { TRACE_CPUPROFILER_EVENT_SCOPE(ALargeMapManager::RemovePendingActorsToRemove); - if(ActorsToRemove.Num() > 0 || GhostsToRemove.Num() > 0) + if(ActorsToRemove.Num() > 0 || ActivesToRemove.Num() > 0) { - LM_LOG(Error, "ActorsToRemove %d GhostsToRemove %d", ActorsToRemove.Num(), GhostsToRemove.Num()); + LM_LOG(Error, "ActorsToRemove %d ActivesToRemove %d", ActorsToRemove.Num(), ActivesToRemove.Num()); } for (AActor* ActorToRemove : ActorsToRemove) @@ -709,11 +670,11 @@ void ALargeMapManager::RemovePendingActorsToRemove() ActorsToRemove.Reset(); - for (FCarlaActor::IdType ActorToRemove : GhostsToRemove) + for (FCarlaActor::IdType ActorToRemove : ActivesToRemove) { - GhostActors.Remove(ActorToRemove); + ActiveActors.Remove(ActorToRemove); } - GhostsToRemove.Reset(); + ActivesToRemove.Reset(); for(FCarlaActor::IdType Id : DormantsToRemove) { @@ -722,13 +683,13 @@ void ALargeMapManager::RemovePendingActorsToRemove() DormantsToRemove.Reset(); } -void ALargeMapManager::CheckGhostActors() +void ALargeMapManager::CheckActiveActors() { - TRACE_CPUPROFILER_EVENT_SCOPE(ALargeMapManager::CheckGhostActors); + TRACE_CPUPROFILER_EVENT_SCOPE(ALargeMapManager::CheckActiveActors); UWorld* World = GetWorld(); UCarlaEpisode* CarlaEpisode = UCarlaStatics::GetCurrentEpisode(World); // Check if they have to be destroyed - for(FCarlaActor::IdType Id : GhostActors) + for(FCarlaActor::IdType Id : ActiveActors) { FCarlaActor* View = CarlaEpisode->FindCarlaActor(Id); if (View) @@ -740,8 +701,8 @@ void ALargeMapManager::CheckGhostActors() if(!IsTileLoaded(WorldLocation)) { // Save to temporal container. Later will be converted to dormant - GhostToDormantActors.Add(Id); - GhostsToRemove.Add(Id); + ActiveToDormantActors.Add(Id); + ActivesToRemove.Add(Id); continue; } @@ -753,47 +714,40 @@ void ALargeMapManager::CheckGhostActors() if (DistanceSquared > ActorStreamingDistanceSquared) { - LM_LOG(Warning, "CheckGhostActors Tile not loaded %s %s (%s)-> Ghost To Dormant %s", \ - *TileIDToString(GetTileID(WorldLocation)),\ - *WorldLocation.ToString(), \ - *CurrentOriginD.ToString(), \ - *Actor->GetName() \ - ); - // Save to temporal container. Later will be converted to dormant - GhostToDormantActors.Add(Id); - GhostsToRemove.Add(Id); + ActiveToDormantActors.Add(Id); + ActivesToRemove.Add(Id); } } } else { - LM_LOG(Warning, "CheckGhostActors Actor does not exist -> Remove actor"); - GhostsToRemove.Add(Id); + LM_LOG(Warning, "CheckActiveActors Actor does not exist -> Remove actor"); + ActivesToRemove.Add(Id); } } } -void ALargeMapManager::ConvertGhostToDormantActors() +void ALargeMapManager::ConvertActiveToDormantActors() { - TRACE_CPUPROFILER_EVENT_SCOPE(ALargeMapManager::ConvertGhostToDormantActors); + TRACE_CPUPROFILER_EVENT_SCOPE(ALargeMapManager::ConvertActiveToDormantActors); UWorld* World = GetWorld(); UCarlaEpisode* CarlaEpisode = UCarlaStatics::GetCurrentEpisode(World); - // These actors are on dormant state so remove them from ghost actors + // These actors are on dormant state so remove them from active actors // But save them on the dormant array first - for(FCarlaActor::IdType Id : GhostToDormantActors) + for(FCarlaActor::IdType Id : ActiveToDormantActors) { // To dormant state CarlaEpisode->PutActorToSleep(Id); - LM_LOG(Warning, "Converting Ghost To Dormant... %d", Id); + LM_LOG(Warning, "Converting Active To Dormant... %d", Id); // Need the ID of the dormant actor and save it DormantActors.Add(Id); } - GhostToDormantActors.Reset(); + ActiveToDormantActors.Reset(); } void ALargeMapManager::CheckDormantActors() @@ -808,9 +762,21 @@ void ALargeMapManager::CheckDormantActors() FCarlaActor* CarlaActor = CarlaEpisode->FindCarlaActor(Id); // If the Ids don't match, the actor has been removed - if(!CarlaActor || CarlaActor->GetActorId() != Id || !CarlaActor->IsDormant()) + if(!CarlaActor) { - LM_LOG(Error, "CheckDormantActors IDs doesn't much!! Wanted = %d Received = %d", Id, CarlaActor?CarlaActor->GetActorId():0); + LM_LOG(Error, "CheckDormantActors Carla Actor %d not found", Id); + DormantsToRemove.Add(Id); + continue; + } + if(CarlaActor->GetActorId() != Id) + { + LM_LOG(Error, "CheckDormantActors IDs doesn't match!! Wanted = %d Received = %d", Id, CarlaActor->GetActorId()); + DormantsToRemove.Add(Id); + continue; + } + if (!CarlaActor->IsDormant()) + { + LM_LOG(Error, "CheckDormantActors Carla Actor %d is not dormant", Id); DormantsToRemove.Add(Id); continue; } @@ -826,19 +792,9 @@ void ALargeMapManager::CheckDormantActors() float DistanceSquared = (RelativeLocation - HeroLocation).SizeSquared(); - // LM_LOG(Warning, "CheckDormantActors\n\t%d\n\tWorldLoc: %s\n\tRelLoc: %s\n\tOrigin: %s\n\tTile %s\n\tDist %.2f (%.2f)", \ - // Id,\ - // *WorldLocation.ToString(), \ - // *RelativeLocation.ToString(), \ - // *CurrentOriginD.ToString(), \ - // *TileIDToString(GetTileID(WorldLocation)),\ - // (RelativeLocation - HeroLocation).Size(), ActorStreamingDistance \ - // ); - if(DistanceSquared < ActorStreamingDistanceSquared && IsTileLoaded(WorldLocation)) { - LM_LOG(Warning, "Need to spawn a dormant actor with Id %d", Id); - DormantToGhostActors.Add(Id); + DormantToActiveActors.Add(Id); DormantsToRemove.Add(Id); break; } @@ -846,35 +802,35 @@ void ALargeMapManager::CheckDormantActors() } } -void ALargeMapManager::ConvertDormantToGhostActors() +void ALargeMapManager::ConvertDormantToActiveActors() { - TRACE_CPUPROFILER_EVENT_SCOPE(ALargeMapManager::ConvertDormantToGhostActors); + TRACE_CPUPROFILER_EVENT_SCOPE(ALargeMapManager::ConvertDormantToActiveActors); UWorld* World = GetWorld(); UCarlaEpisode* CarlaEpisode = UCarlaStatics::GetCurrentEpisode(World); - for(FCarlaActor::IdType Id : DormantToGhostActors) + for(FCarlaActor::IdType Id : DormantToActiveActors) { - LM_LOG(Warning, "Converting %d Dormant To Ghost", Id); + LM_LOG(Warning, "Converting %d Dormant To Active", Id); CarlaEpisode->WakeActorUp(Id); FCarlaActor* View = CarlaEpisode->FindCarlaActor(Id); - if (View->GetActor()){ + if (View->IsActive()){ LM_LOG(Warning, "Spawning dormant at %s\n\tOrigin: %s\n\tRel. location: %s", \ *((CurrentOriginD + View->GetActor()->GetActorLocation()).ToString()), \ *(CurrentOriginD.ToString()), \ *((View->GetActor()->GetActorLocation()).ToString()) \ ); - GhostActors.Add(Id); + ActiveActors.Add(Id); } else { - LM_LOG(Warning, "FAIL Actor %d could not be woken up", Id); - CarlaEpisode->DestroyActor(Id); + LM_LOG(Warning, "Actor %d could not be woken up, keeping sleep state", Id); + DormantActors.Add(Id); } } - DormantToGhostActors.Reset(); + DormantToActiveActors.Reset(); } void ALargeMapManager::CheckIfRebaseIsNeeded() @@ -914,15 +870,18 @@ void ALargeMapManager::GetTilesToConsider(const AActor* ActorToConsider, // Calculate Current Tile FIntVector CurrentTile = GetTileVectorID(ActorLocation); - // Calculate the number of tiles in range based on LayerStreamingDistance - int32 TilesToConsider = (int32)(LayerStreamingDistance / TileSide) + 1; - for (int Y = -TilesToConsider; Y < TilesToConsider; Y++) + // Calculate tile bounds + FDVector UpperPos = ActorLocation + FDVector(LayerStreamingDistance,LayerStreamingDistance,0); + FDVector LowerPos = ActorLocation + FDVector(-LayerStreamingDistance,-LayerStreamingDistance,0); + FIntVector UpperTileId = GetTileVectorID(UpperPos); + FIntVector LowerTileId = GetTileVectorID(LowerPos); + for (int Y = UpperTileId.Y; Y <= LowerTileId.Y; Y++) { - for (int X = -TilesToConsider; X < TilesToConsider; X++) + for (int X = LowerTileId.X; X <= UpperTileId.X; X++) { // I don't check the bounds of the Tile map, if the Tile does not exist // I just simply discard it - FIntVector TileToCheck = CurrentTile + FIntVector(X, Y, 0); + FIntVector TileToCheck = FIntVector(X, Y, 0); TileID TileID = GetTileID(TileToCheck); FCarlaMapTile* Tile = MapTiles.Find(TileID); @@ -932,14 +891,7 @@ void ALargeMapManager::GetTilesToConsider(const AActor* ActorToConsider, continue; // Tile does not exist, discard } - // Calculate distance between actor and tile - float Distance2 = FDVector::DistSquared(Tile->Location, ActorLocation); - - // Load level if we are in range - if (Distance2 < LayerStreamingDistanceSquared) - { OutTilesToConsider.Add(TileID); - } } } } @@ -1084,7 +1036,7 @@ void ALargeMapManager::PrintMapInfo() GEngine->AddOnScreenDebugMessage(LastMsgIndex++, MsgTime, FColor::White, FString::Printf(TEXT("\nOrigin: %s km"), *(FDVector(CurrentOriginInt) / (1000.0 * 100.0)).ToString()) ); GEngine->AddOnScreenDebugMessage(LastMsgIndex++, MsgTime, FColor::White, - FString::Printf(TEXT("Num ghost actors (%d)"), GhostActors.Num()) ); + FString::Printf(TEXT("Num active actors (%d)"), ActiveActors.Num()) ); GEngine->AddOnScreenDebugMessage(LastMsgIndex++, MsgTime, FColor::White, FString::Printf(TEXT("Num dormant actors (%d)"), DormantActors.Num())); GEngine->AddOnScreenDebugMessage(LastMsgIndex++, MsgTime, FColor::White, diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/LargeMapManager.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/LargeMapManager.h index fb4f5a32c..dfe65c194 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/LargeMapManager.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/MapGen/LargeMapManager.h @@ -17,13 +17,13 @@ // TODO: Cache CarlaEpisode USTRUCT() -struct FGhostActor +struct FActiveActor { GENERATED_BODY() - FGhostActor() {} + FActiveActor() {} - FGhostActor( + FActiveActor( const FCarlaActor* InCarlaActor, const FTransform& InTransform) : CarlaActor(InCarlaActor), @@ -98,11 +98,22 @@ public: UFUNCTION(BlueprintCallable, Category = "Large Map Manager") void GenerateLargeMap(); + void GenerateMap(TArray> MapPathsIds); + + UFUNCTION(BlueprintCallable, CallInEditor, Category = "Large Map Manager") + void ClearWorldAndTiles(); + void AddActorToUnloadedList(const FCarlaActor& CarlaActor, const FTransform& Transform); UFUNCTION(BlueprintCallable, Category = "Large Map Manager") FIntVector GetNumTilesInXY() const; + UFUNCTION(BlueprintCallable, Category = "Large Map Manager") + int GetNumTiles() const + { + return MapTiles.Num(); + } + UFUNCTION(BlueprintCallable, Category = "Large Map Manager") bool IsLevelOfTileLoaded(FIntVector InTileID) const; @@ -132,6 +143,18 @@ public: UPROPERTY(EditAnywhere, Category = "Large Map Manager") FString LargeMapName = "testmap"; + void SetTile0Offset(const FVector& Offset); + + void SetTileSize(float Size); + + void SetLayerStreamingDistance(float Distance); + + void SetActorStreamingDistance(float Distance); + + float GetLayerStreamingDistance() const; + + float GetActorStreamingDistance() const; + protected: FIntVector GetTileVectorID(FVector TileLocation) const; @@ -161,29 +184,27 @@ protected: FCarlaMapTile* GetCarlaMapTile(FIntVector TileVectorID); - ULevelStreamingDynamic* AddNewTile(FString TileName, FVector TileLocation); - FCarlaMapTile& LoadCarlaMapTile(FString TileMapPath, TileID TileId); void UpdateTilesState(); void RemovePendingActorsToRemove(); - // Check if any ghost actor has to be converted into dormant actor + // Check if any active actor has to be converted into dormant actor // because it went out of range (ActorStreamingDistance) // Just stores the array of selected actors - void CheckGhostActors(); + void CheckActiveActors(); - // Converts ghost actors that went out of range to dormant actors - void ConvertGhostToDormantActors(); + // Converts active actors that went out of range to dormant actors + void ConvertActiveToDormantActors(); - // Check if any dormant actor has to be converted into ghost actor + // Check if any dormant actor has to be converted into active actor // because it enter in range (ActorStreamingDistance) // Just stores the array of selected actors void CheckDormantActors(); - // Converts ghost actors that went out of range to dormant actors - void ConvertDormantToGhostActors(); + // Converts active actors that went out of range to dormant actors + void ConvertDormantToActiveActors(); void CheckIfRebaseIsNeeded(); @@ -215,17 +236,17 @@ protected: UPROPERTY(VisibleAnywhere, Category = "Large Map Manager") TArray ActorsToConsider; //UPROPERTY(VisibleAnywhere, Category = "Large Map Manager") - TArray GhostActors; + TArray ActiveActors; TArray DormantActors; // Temporal sets to remove actors. Just to avoid removing them in the update loop TSet ActorsToRemove; - TSet GhostsToRemove; + TSet ActivesToRemove; TSet DormantsToRemove; // Helpers to move Actors from one array to another. - TSet GhostToDormantActors; - TSet DormantToGhostActors; + TSet ActiveToDormantActors; + TSet DormantToActiveActors; UPROPERTY(VisibleAnywhere, Category = "Large Map Manager") TSet CurrentTilesLoaded; @@ -267,6 +288,8 @@ protected: if (!LargeMapTilePath.IsEmpty()) GenerateMap(LargeMapTilePath); } + void RegisterTilesInWorldComposition(); + FString GenerateTileName(TileID TileID); FString TileIDToString(TileID TileID); diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaRecorder.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaRecorder.cpp index a012f4027..3448f07b9 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaRecorder.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaRecorder.cpp @@ -154,7 +154,7 @@ void ACarlaRecorder::AddActorPosition(FCarlaActor *CarlaActor) AddPosition(CarlaRecorderPosition { CarlaActor->GetActorId(), - Transform.GetTranslation(), + Transform.GetLocation(), Transform.GetRotation().Euler() }); } @@ -201,20 +201,20 @@ void ACarlaRecorder::AddWalkerAnimation(FCarlaActor *CarlaActor) void ACarlaRecorder::AddTrafficLightState(FCarlaActor *CarlaActor) { check(CarlaActor != nullptr); - // Todo: interface with FCarlaActor and UTrafficLightController - AActor *Actor = CarlaActor->GetActor(); - if(Actor) + + ETrafficLightState LightState = CarlaActor->GetTrafficLightState(); + UTrafficLightController* Controller = CarlaActor->GetTrafficLightController(); + if (Controller) { - // get states - auto TrafficLight = Cast(Actor); - if (TrafficLight != nullptr) + ATrafficLightGroup* Group = Controller->GetGroup(); + if (Group) { AddState(CarlaRecorderStateTrafficLight { CarlaActor->GetActorId(), - TrafficLight->GetTimeIsFrozen(), - TrafficLight->GetElapsedTime(), - static_cast(TrafficLight->GetTrafficLightState()) + Group->IsFrozen(), + Controller->GetElapsedTime(), + static_cast(LightState) }); } } @@ -561,16 +561,15 @@ void ACarlaRecorder::AddExistingActors(void) FActorRegistry Registry = Episode->GetActorRegistry(); for (auto& It : Registry) { - const FCarlaActor* View = It.Value.Get(); - const AActor *Actor = View->GetActor(); - if (Actor != nullptr) + const FCarlaActor* CarlaActor = It.Value.Get(); + if (CarlaActor != nullptr) { // create event CreateRecorderEventAdd( - View->GetActorId(), - static_cast(View->GetActorType()), - Actor->GetActorTransform(), - View->GetActorInfo()->Description); + CarlaActor->GetActorId(), + static_cast(CarlaActor->GetActorType()), + CarlaActor->GetActorGlobalTransform(), + CarlaActor->GetActorInfo()->Description); } } diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaRecorderEventParent.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaRecorderEventParent.cpp index 73159e443..51ad18220 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaRecorderEventParent.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaRecorderEventParent.cpp @@ -26,12 +26,12 @@ void CarlaRecorderEventParent::Write(std::ofstream &OutFile) const //--------------------------------------------- -inline void CarlaRecorderEventsParent::Clear(void) +void CarlaRecorderEventsParent::Clear(void) { Events.clear(); } -inline void CarlaRecorderEventsParent::Add(const CarlaRecorderEventParent &Event) +void CarlaRecorderEventsParent::Add(const CarlaRecorderEventParent &Event) { Events.push_back(std::move(Event)); } diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaReplayerHelper.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaReplayerHelper.cpp index 26a039c16..cad78cbb8 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaReplayerHelper.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Recorder/CarlaReplayerHelper.cpp @@ -69,7 +69,7 @@ std::pairCarlaReplayerHelper::TryToCreateReplayerActor( // relocate FRotator Rot = FRotator::MakeFromEuler(Rotation); FTransform Trans2(Rot, Location, FVector(1, 1, 1)); - CarlaActor->GetActor()->SetActorTransform(Trans2, false, nullptr, ETeleportType::TeleportPhysics); + CarlaActor->SetActorGlobalTransform(Trans2); return std::pair(2, CarlaActor); } } @@ -82,7 +82,7 @@ std::pairCarlaReplayerHelper::TryToCreateReplayerActor( { // relocate FTransform Trans2(Rot, Location, FVector(1, 1, 1)); - Result.Value->GetActor()->SetActorTransform(Trans2, false, nullptr, ETeleportType::TeleportPhysics); + Result.Value->SetActorGlobalTransform(Trans2); ALargeMapManager * LargeMapManager = UCarlaStatics::GetLargeMapManager(Episode->GetWorld()); if (LargeMapManager) { @@ -231,13 +231,13 @@ std::pair CarlaReplayerHelper::ProcessReplayerEventAdd( bool CarlaReplayerHelper::ProcessReplayerEventDel(uint32_t DatabaseId) { check(Episode != nullptr); - auto actor = Episode->FindCarlaActor(DatabaseId)->GetActor(); - if (actor == nullptr) + FCarlaActor* CarlaActor = Episode->FindCarlaActor(DatabaseId); + if (CarlaActor == nullptr) { UE_LOG(LogCarla, Log, TEXT("Actor %d not found to destroy"), DatabaseId); return false; } - Episode->DestroyActor(actor); + Episode->DestroyActor(CarlaActor->GetActorId()); return true; } @@ -245,19 +245,36 @@ bool CarlaReplayerHelper::ProcessReplayerEventDel(uint32_t DatabaseId) bool CarlaReplayerHelper::ProcessReplayerEventParent(uint32_t ChildId, uint32_t ParentId) { check(Episode != nullptr); - AActor *child = Episode->FindCarlaActor(ChildId)->GetActor(); - AActor *parent = Episode->FindCarlaActor(ParentId)->GetActor(); - if (child && parent) + FCarlaActor * Child = Episode->FindCarlaActor(ChildId); + FCarlaActor * Parent = Episode->FindCarlaActor(ParentId); + if(!Child) { - child->AttachToActor(parent, FAttachmentTransformRules::KeepRelativeTransform); - child->SetOwner(parent); - return true; + UE_LOG(LogCarla, Log, TEXT("Parenting Child actors not found")); + return false; + } + if(!Parent) + { + UE_LOG(LogCarla, Log, TEXT("Parenting Parent actors not found")); + return false; + } + Child->SetParent(ParentId); + Child->SetAttachmentType(carla::rpc::AttachmentType::Rigid); + Parent->AddChildren(Child->GetActorId()); + if(!Parent->IsDormant()) + { + if(!Child->IsDormant()) + { + Episode->AttachActors( + Child->GetActor(), + Parent->GetActor(), + static_cast(carla::rpc::AttachmentType::Rigid)); + } } else { - UE_LOG(LogCarla, Log, TEXT("Parenting Actors not found")); - return false; + Episode->PutActorToSleep(Child->GetActorId()); } + return true; } // reposition actors @@ -295,23 +312,24 @@ bool CarlaReplayerHelper::SetCameraPosition(uint32_t Id, FVector Offset, FQuat R { check(Episode != nullptr); - // get specator pawn - APawn *Spectator = Episode->GetSpectatorPawn(); // get the actor to follow FCarlaActor* CarlaActor = Episode->FindCarlaActor(Id); if (!CarlaActor) return false; - AActor *Actor = CarlaActor->GetActor(); - - // check - if (!Spectator || !Actor) + // get specator pawn + APawn *Spectator = Episode->GetSpectatorPawn(); + if (!Spectator) return false; + FCarlaActor* CarlaSpectator = Episode->FindCarlaActor(Spectator); + if (!CarlaSpectator) + return false; + + FTransform ActorTransform = CarlaActor->GetActorGlobalTransform(); // set the new position - FQuat ActorRot = Actor->GetActorTransform().GetRotation(); - FVector Pos = Actor->GetActorTransform().GetTranslation() + (ActorRot.RotateVector(Offset)); - Spectator->SetActorLocation(Pos); - Spectator->SetActorRotation(ActorRot * Rotation); + FQuat ActorRot = ActorTransform.GetRotation(); + FVector Pos = ActorTransform.GetTranslation() + (ActorRot.RotateVector(Offset)); + CarlaSpectator->SetActorGlobalTransform(FTransform(ActorRot * Rotation, Pos, FVector(1,1,1))); return true; } @@ -319,18 +337,21 @@ bool CarlaReplayerHelper::SetCameraPosition(uint32_t Id, FVector Offset, FQuat R bool CarlaReplayerHelper::ProcessReplayerStateTrafficLight(CarlaRecorderStateTrafficLight State) { check(Episode != nullptr); - // Todo: interface with FCarlaActor and UTrafficLightController - AActor *Actor = Episode->FindCarlaActor(State.DatabaseId)->GetActor(); - if (Actor && !Actor->IsPendingKill()) + FCarlaActor* CarlaActor = Episode->FindCarlaActor(State.DatabaseId); + if(CarlaActor) { - auto TrafficLight = Cast(Actor); - if (TrafficLight != nullptr) + CarlaActor->SetTrafficLightState(static_cast(State.State)); + UTrafficLightController* Controller = CarlaActor->GetTrafficLightController(); + if(Controller) { - TrafficLight->SetTrafficLightState(static_cast(State.State)); - TrafficLight->SetTimeIsFrozen(State.IsFrozen); - TrafficLight->SetElapsedTime(State.ElapsedTime); + Controller->SetElapsedTime(State.ElapsedTime); + ATrafficLightGroup* Group = Controller->GetGroup(); + if (Group) + { + Group->SetFrozenGroup(State.IsFrozen); + } } - return true; + return true; } return false; } @@ -415,21 +436,15 @@ bool CarlaReplayerHelper::ProcessReplayerFinish(bool bApplyAutopilot, bool bIgno // stop all vehicles SetActorSimulatePhysics(CarlaActor, true); SetActorVelocity(CarlaActor, FVector(0, 0, 0)); - // reset any control assigned - auto Veh = Cast(const_cast(CarlaActor->GetActor())); - if (Veh != nullptr) - { - FVehicleControl Control; - Control.Throttle = 0.0f; - Control.Steer = 0.0f; - Control.Brake = 0.0f; - Control.bHandBrake = false; - Control.bReverse = false; - Control.Gear = 1; - Control.bManualGearShift = false; - Veh->ApplyVehicleControl(Control, EVehicleInputPriority::User); - } - + FVehicleControl Control; + Control.Throttle = 0.0f; + Control.Steer = 0.0f; + Control.Brake = 0.0f; + Control.bHandBrake = false; + Control.bReverse = false; + Control.Gear = 1; + Control.bManualGearShift = false; + CarlaActor->ApplyControlToVehicle(Control, EVehicleInputPriority::User); } break; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/WorldObserver.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/WorldObserver.cpp index 286448a2e..4c993109b 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/WorldObserver.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/WorldObserver.cpp @@ -164,7 +164,7 @@ static auto FWorldObserver_GetDormantActorState(const FCarlaActor &View, const F state.vehicle_data.control = carla::rpc::VehicleControl{ActorData->Control}; using TLS = carla::rpc::TrafficLightState; state.vehicle_data.traffic_light_state = TLS::Green; - state.vehicle_data.speed_limit = 0; + state.vehicle_data.speed_limit = 30; state.vehicle_data.has_traffic_light = false; } else if (AType::Walker == View.GetActorType()) @@ -314,9 +314,6 @@ static carla::Buffer FWorldObserver_Serialize( if(View->IsDormant()) { const FActorData* ActorData = View->GetActorData(); - FDVector ActorLocation = ActorData->Location; - ActorLocation -= MapOrigin; - ActorTransform = FTransform(ActorData->Rotation, ActorLocation.ToFVector()); Velocity = TO_METERS * ActorData->Velocity; AngularVelocity = carla::geom::Vector3D {ActorData->AngularVelocity.X, @@ -327,12 +324,12 @@ static carla::Buffer FWorldObserver_Serialize( } else { - ActorTransform = View->GetActor()->GetActorTransform(); Velocity = TO_METERS * View->GetActor()->GetVelocity(); AngularVelocity = FWorldObserver_GetAngularVelocity(*View->GetActor()); Acceleration = FWorldObserver_GetAcceleration(*View, Velocity, DeltaSeconds); State = FWorldObserver_GetActorState(*View, Registry); } + ActorTransform = View->GetActorGlobalTransform(); ActorDynamicState info = { View->GetActorId(), diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp index 2ed234eb8..b4a0b3eb0 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp @@ -261,7 +261,7 @@ void FCarlaServer::FPimpl::BindActions() continue; if (MapName.Contains("/BaseMap/")) continue; - if (MapName.Contains("/LargeMap/")) + if (MapName.Contains("/BaseLargeMap/")) continue; if (MapName.Contains("_Tile_")) continue; @@ -500,16 +500,7 @@ void FCarlaServer::FPimpl::BindActions() { REQUIRE_CARLA_EPISODE(); - ACarlaGameModeBase* GameMode = UCarlaStatics::GetGameMode(Episode->GetWorld()); - ALargeMapManager* LargeMap = GameMode->GetLMManager(); - - FTransform UETransform = Transform; - if(LargeMap) - { - UETransform = LargeMap->GlobalToLocalTransform(UETransform); - } - - auto Result = Episode->SpawnActorWithInfo(UETransform, std::move(Description)); + auto Result = Episode->SpawnActorWithInfo(Transform, std::move(Description)); if (Result.Key != EActorSpawnResultStatus::Success) { @@ -517,6 +508,7 @@ void FCarlaServer::FPimpl::BindActions() RESPOND_ERROR_FSTRING(FActorSpawnResult::StatusToString(Result.Key)); } + ALargeMapManager* LargeMap = UCarlaStatics::GetLargeMapManager(Episode->GetWorld()); if(LargeMap) { LargeMap->OnActorSpawned(*Result.Value); @@ -533,16 +525,7 @@ void FCarlaServer::FPimpl::BindActions() { REQUIRE_CARLA_EPISODE(); - ACarlaGameModeBase* GameMode = UCarlaStatics::GetGameMode(Episode->GetWorld()); - ALargeMapManager* LargeMap = GameMode->GetLMManager(); - - FTransform UETransform = Transform; - if(LargeMap) - { - UETransform = LargeMap->GlobalToLocalTransform(UETransform); - } - - auto Result = Episode->SpawnActorWithInfo(UETransform, std::move(Description)); + auto Result = Episode->SpawnActorWithInfo(Transform, std::move(Description)); if (Result.Key != EActorSpawnResultStatus::Success) { RESPOND_ERROR_FSTRING(FActorSpawnResult::StatusToString(Result.Key)); @@ -555,7 +538,6 @@ void FCarlaServer::FPimpl::BindActions() } FCarlaActor* ParentCarlaActor = Episode->FindCarlaActor(ParentId); - carla::log_warning(CarlaActor->GetActorId(), ParentId); if (!ParentCarlaActor) { diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/EpisodeSettings.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/EpisodeSettings.h index 32248fd8f..e78ea84d0 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/EpisodeSettings.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/EpisodeSettings.h @@ -33,4 +33,8 @@ struct CARLA_API FEpisodeSettings UPROPERTY(EditAnywhere, BlueprintReadWrite) bool bDeterministicRagdolls = true; + float TileStreamingDistance = 300000.f; // 3km + + float ActorActiveDistance = 200000.f; // 3km + }; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/TrafficLightComponent.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/TrafficLightComponent.cpp index baaea9ddc..5f1f63682 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/TrafficLightComponent.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/TrafficLightComponent.cpp @@ -11,6 +11,8 @@ #include "TrafficLightGroup.h" #include "TrafficLightInterface.h" #include "TrafficLightManager.h" +#include "Carla/Game/CarlaStatics.h" +#include "Carla/MapGen/LargeMapManager.h" UTrafficLightComponent::UTrafficLightComponent() : Super() @@ -60,7 +62,13 @@ void UTrafficLightComponent::InitializeSign(const carla::road::Map &Map) LaneDistance + epsilon, LaneDistance + LaneLength - epsilon); } float UnrealBoxSize = 100*BoxSize; - GenerateTrafficLightBox(Map.ComputeTransform(signal_waypoint), UnrealBoxSize); + FTransform BoxTransform = Map.ComputeTransform(signal_waypoint); + ALargeMapManager* LargeMapManager = UCarlaStatics::GetLargeMapManager(GetWorld()); + if (LargeMapManager) + { + BoxTransform = LargeMapManager->GlobalToLocalTransform(BoxTransform); + } + GenerateTrafficLightBox(BoxTransform, UnrealBoxSize); } } } diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/TrafficLightController.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/TrafficLightController.cpp index e101fe199..4715e727f 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/TrafficLightController.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/TrafficLightController.cpp @@ -60,6 +60,7 @@ const TArray &UTrafficLightController::GetTrafficLight void UTrafficLightController::EmptyTrafficLights() { TrafficLights.Empty(); + TrafficLightCarlaActors.Empty(); } void UTrafficLightController::AddTrafficLight(UTrafficLightComponent * TrafficLight) @@ -73,6 +74,16 @@ void UTrafficLightController::RemoveTrafficLight(UTrafficLightComponent * Traffi TrafficLights.Remove(TrafficLight); } +void UTrafficLightController::AddCarlaActorTrafficLight(FCarlaActor* CarlaActor) +{ + TrafficLightCarlaActors.Add(CarlaActor); +} + +void UTrafficLightController::RemoveCarlaActorTrafficLight(FCarlaActor* CarlaActor) +{ + TrafficLightCarlaActors.Remove(CarlaActor); +} + const FString &UTrafficLightController::GetControllerId() const { return ControllerId; @@ -95,6 +106,10 @@ void UTrafficLightController::SetTrafficLightsState(ETrafficLightState NewState) { Light->SetLightState(NewState); } + for(FCarlaActor* Light : TrafficLightCarlaActors) + { + Light->SetTrafficLightState(NewState); + } } int UTrafficLightController::GetSequence() const diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/TrafficLightController.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/TrafficLightController.h index 0b1d876df..abf70b212 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/TrafficLightController.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Traffic/TrafficLightController.h @@ -75,6 +75,10 @@ public: UFUNCTION(Category = "Traffic Controller", BlueprintCallable) void RemoveTrafficLight(UTrafficLightComponent * TrafficLight); + void AddCarlaActorTrafficLight(FCarlaActor* CarlaActor); + + void RemoveCarlaActorTrafficLight(FCarlaActor* CarlaActor); + UFUNCTION(Category = "Traffic Controller", BlueprintCallable) bool IsCycleFinished() const; @@ -152,6 +156,8 @@ private: UPROPERTY(Category = "Traffic Controller", EditAnywhere) TArray TrafficLights; + TArray TrafficLightCarlaActors; + UPROPERTY(Category = "Traffic Controller", VisibleAnywhere) ATrafficLightGroup* TrafficLightGroup; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/ObjectRegister.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/ObjectRegister.cpp index f9cc18b43..eaef6d6c2 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/ObjectRegister.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/ObjectRegister.cpp @@ -8,6 +8,10 @@ #include "Carla/Util/ObjectRegister.h" #include "Carla/Game/Tagger.h" +#include "Carla/Util/BoundingBoxCalculator.h" + +#include "InstancedFoliageActor.h" +#include "GameFramework/Character.h" #if WITH_EDITOR #include "FileHelper.h"