diff --git a/CHANGELOG.md b/CHANGELOG.md index 153257bfc..a1090b2b3 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -60,6 +60,7 @@ * Fixed minor typo in the introduction section of documentation. * Fixed a bug at the local planner when changing the route, causing it to maintain the first part of the previous one. This was only relevant when using very large buffer sizes. * Added automatic calculation of vehicle's BB + * Retrieve BBs of all the elements of the level ## CARLA 0.9.9 diff --git a/LibCarla/source/carla/client/World.cpp b/LibCarla/source/carla/client/World.cpp index 757bf5676..df477cbab 100644 --- a/LibCarla/source/carla/client/World.cpp +++ b/LibCarla/source/carla/client/World.cpp @@ -158,8 +158,8 @@ namespace client { _episode.Lock()->FreezeAllTrafficLights(frozen); } - std::vector World::GetLevelBBs() const { - return _episode.Lock()->GetLevelBBs(); + std::vector World::GetLevelBBs(uint8_t queried_tag) const { + return _episode.Lock()->GetLevelBBs(queried_tag); } } // namespace client diff --git a/LibCarla/source/carla/client/World.h b/LibCarla/source/carla/client/World.h index 5040b2322..41086e19f 100644 --- a/LibCarla/source/carla/client/World.h +++ b/LibCarla/source/carla/client/World.h @@ -151,7 +151,7 @@ namespace client { void FreezeAllTrafficLights(bool frozen); /// Returns all the BBs of all the elements of the level - std::vector GetLevelBBs() const; + std::vector GetLevelBBs(uint8_t queried_tag) const; private: diff --git a/LibCarla/source/carla/client/detail/Client.cpp b/LibCarla/source/carla/client/detail/Client.cpp index 64c9edd6a..e1933e9df 100644 --- a/LibCarla/source/carla/client/detail/Client.cpp +++ b/LibCarla/source/carla/client/detail/Client.cpp @@ -413,9 +413,9 @@ namespace detail { _pimpl->AsyncCall("update_lights_state", _pimpl->endpoint, std::move(lights), discard_client); } - std::vector Client::GetLevelBBs() const { + std::vector Client::GetLevelBBs(uint8_t queried_tag) const { using return_t = std::vector; - return _pimpl->CallAndWait("get_all_level_BBs"); + return _pimpl->CallAndWait("get_all_level_BBs", queried_tag); } } // namespace detail diff --git a/LibCarla/source/carla/client/detail/Client.h b/LibCarla/source/carla/client/detail/Client.h index e941eaeed..5b1875c64 100644 --- a/LibCarla/source/carla/client/detail/Client.h +++ b/LibCarla/source/carla/client/detail/Client.h @@ -257,7 +257,7 @@ namespace detail { bool discard_client = false) const; /// Returns all the BBs of all the elements of the level - std::vector GetLevelBBs() const; + std::vector GetLevelBBs(uint8_t queried_tag) const; private: diff --git a/LibCarla/source/carla/client/detail/Simulator.h b/LibCarla/source/carla/client/detail/Simulator.h index f6605d765..384feb38f 100644 --- a/LibCarla/source/carla/client/detail/Simulator.h +++ b/LibCarla/source/carla/client/detail/Simulator.h @@ -227,8 +227,8 @@ namespace detail { } /// Returns all the BBs of all the elements of the level - std::vector GetLevelBBs() const { - return _client.GetLevelBBs(); + std::vector GetLevelBBs(uint8_t queried_tag) const { + return _client.GetLevelBBs(queried_tag); } /// @} diff --git a/LibCarla/source/carla/geom/BoundingBox.h b/LibCarla/source/carla/geom/BoundingBox.h index 726b90b56..8790d78dc 100644 --- a/LibCarla/source/carla/geom/BoundingBox.h +++ b/LibCarla/source/carla/geom/BoundingBox.h @@ -30,6 +30,11 @@ namespace geom { // -- Constructors --------------------------------------------------------- // ========================================================================= + explicit BoundingBox(const Location &in_location, const Vector3D &in_extent, const Rotation &in_rotation) + : location(in_location), + extent(in_extent), + rotation(in_rotation) {} + explicit BoundingBox(const Location &in_location, const Vector3D &in_extent) : location(in_location), extent(in_extent) {} @@ -38,7 +43,8 @@ namespace geom { : extent(in_extent) {} Location location; ///< Center of the BoundingBox in local space - Vector3D extent; ///< Half the size of the BoundingBox in local space + Vector3D extent; ///< Half the size of the BoundingBox in local space + Rotation rotation; ///< Rotation of the BoundingBox in local space // ========================================================================= // -- Other methods -------------------------------------------------------- @@ -63,15 +69,16 @@ namespace geom { * Returns the positions of the 8 vertices of this BoundingBox in local space. */ std::array GetLocalVertices() const { + return {{ - location + Location(-extent.x,-extent.y,-extent.z), - location + Location(-extent.x,-extent.y, extent.z), - location + Location(-extent.x, extent.y,-extent.z), - location + Location(-extent.x, extent.y, extent.z), - location + Location( extent.x,-extent.y,-extent.z), - location + Location( extent.x,-extent.y, extent.z), - location + Location( extent.x, extent.y,-extent.z), - location + Location( extent.x, extent.y, extent.z) + location + Location(rotation.RotateVector({-extent.x,-extent.y,-extent.z})), + location + Location(rotation.RotateVector({-extent.x,-extent.y, extent.z})), + location + Location(rotation.RotateVector({-extent.x, extent.y,-extent.z})), + location + Location(rotation.RotateVector({-extent.x, extent.y, extent.z})), + location + Location(rotation.RotateVector({ extent.x,-extent.y,-extent.z})), + location + Location(rotation.RotateVector({ extent.x,-extent.y, extent.z})), + location + Location(rotation.RotateVector({ extent.x, extent.y,-extent.z})), + location + Location(rotation.RotateVector({ extent.x, extent.y, extent.z})) }}; } @@ -92,7 +99,7 @@ namespace geom { // ========================================================================= bool operator==(const BoundingBox &rhs) const { - return (location == rhs.location) && (extent == rhs.extent); + return (location == rhs.location) && (extent == rhs.extent) && (rotation == rhs.rotation); } bool operator!=(const BoundingBox &rhs) const { @@ -107,11 +114,12 @@ namespace geom { BoundingBox(const FBoundingBox &Box) : location(Box.Origin), - extent(1e-2f * Box.Extent.X, 1e-2f * Box.Extent.Y, 1e-2f * Box.Extent.Z) {} + extent(1e-2f * Box.Extent.X, 1e-2f * Box.Extent.Y, 1e-2f * Box.Extent.Z), + rotation(Box.Rotation) {} #endif // LIBCARLA_INCLUDED_FROM_UE4 - MSGPACK_DEFINE_ARRAY(location, extent); + MSGPACK_DEFINE_ARRAY(location, extent, rotation); }; } // namespace geom diff --git a/LibCarla/source/carla/geom/Rotation.h b/LibCarla/source/carla/geom/Rotation.h index 6afe21b08..207f20b15 100644 --- a/LibCarla/source/carla/geom/Rotation.h +++ b/LibCarla/source/carla/geom/Rotation.h @@ -87,6 +87,34 @@ namespace geom { in_point = out_point; } + Vector3D RotateVector(const Vector3D& in_point) const { + // Rotates Rz(yaw) * Ry(pitch) * Rx(roll) = first x, then y, then z. + const float cy = std::cos(Math::ToRadians(yaw)); + const float sy = std::sin(Math::ToRadians(yaw)); + const float cr = std::cos(Math::ToRadians(roll)); + const float sr = std::sin(Math::ToRadians(roll)); + const float cp = std::cos(Math::ToRadians(pitch)); + const float sp = std::sin(Math::ToRadians(pitch)); + + Vector3D out_point; + out_point.x = + in_point.x * (cp * cy) + + in_point.y * (cy * sp * sr - sy * cr) + + in_point.z * (-cy * sp * cr - sy * sr); + + out_point.y = + in_point.x * (cp * sy) + + in_point.y * (sy * sp * sr + cy * cr) + + in_point.z * (-sy * sp * cr + cy * sr); + + out_point.z = + in_point.x * (sp) + + in_point.y * (-cp * sr) + + in_point.z * (cp * cr); + + return out_point; + } + void InverseRotateVector(Vector3D &in_point) const { // Applies the transposed of the matrix used in RotateVector function, // which is the rotation inverse. diff --git a/LibCarla/source/carla/rpc/ObjectLabel.h b/LibCarla/source/carla/rpc/ObjectLabel.h new file mode 100644 index 000000000..5b7b5d30d --- /dev/null +++ b/LibCarla/source/carla/rpc/ObjectLabel.h @@ -0,0 +1,45 @@ +// 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 "carla/MsgPack.h" + +#include + +namespace carla { +namespace rpc { + + enum class CityObjectLabel : uint8_t { + None = 0u, + Buildings = 1u, + Fences = 2u, + Other = 3u, + Pedestrians = 4u, + Poles = 5u, + RoadLines = 6u, + Roads = 7u, + Sidewalks = 8u, + TrafficSigns = 12u, + Vegetation = 9u, + Vehicles = 10u, + Walls = 11u, + Sky = 13u, + Ground = 14u, + Bridge = 15u, + RailTrack = 16u, + GuardRail = 17u, + TrafficLight = 18u, + Static = 19u, + Dynamic = 20u, + Water = 21u, + Terrain = 22u, + }; + +} // namespace rpc +} // namespace carla + +MSGPACK_ADD_ENUM(carla::rpc::CityObjectLabel); diff --git a/PythonAPI/carla/source/libcarla/Geom.cpp b/PythonAPI/carla/source/libcarla/Geom.cpp index 333d98ed3..c77217636 100644 --- a/PythonAPI/carla/source/libcarla/Geom.cpp +++ b/PythonAPI/carla/source/libcarla/Geom.cpp @@ -205,9 +205,10 @@ void export_geom() { class_("BoundingBox") .def(init( - (arg("location")=cg::Location(), arg("extent")=cg::Vector3D()))) + (arg("location")=cg::Location(), arg("extent")=cg::Vector3D(), arg("rotation")=cg::Rotation()))) .def_readwrite("location", &cg::BoundingBox::location) .def_readwrite("extent", &cg::BoundingBox::extent) + .def_readwrite("rotation", &cg::BoundingBox::rotation) .def("contains", &cg::BoundingBox::Contains, arg("point"), arg("bbox_transform")) .def("get_local_vertices", CALL_RETURNING_LIST(cg::BoundingBox, GetLocalVertices)) .def("get_world_vertices", CALL_RETURNING_LIST_1(cg::BoundingBox, GetWorldVertices, const cg::Transform&), arg("bbox_transform")) diff --git a/PythonAPI/carla/source/libcarla/World.cpp b/PythonAPI/carla/source/libcarla/World.cpp index 0a1a3d971..387efb605 100644 --- a/PythonAPI/carla/source/libcarla/World.cpp +++ b/PythonAPI/carla/source/libcarla/World.cpp @@ -8,6 +8,7 @@ #include #include #include +#include #include @@ -70,10 +71,10 @@ static auto GetVehiclesLightStates(carla::client::World &self) { return dict; } -static auto GetLevelBBs(const carla::client::World &self) { +static auto GetLevelBBs(const carla::client::World &self, uint8_t queried_tag) { carla::PythonUtil::ReleaseGIL unlock; boost::python::list result; - for (const auto &bb : self.GetLevelBBs()) { + for (const auto &bb : self.GetLevelBBs(queried_tag)) { result.append(bb); } return result; @@ -135,6 +136,32 @@ void export_world() { .value("SpringArm", cr::AttachmentType::SpringArm) ; + enum_("CityObjectLabel") + .value("Any", cr::CityObjectLabel::None) + .value("Buildings", cr::CityObjectLabel::Buildings) + .value("Fences", cr::CityObjectLabel::Fences) + .value("Other", cr::CityObjectLabel::Other) + .value("Pedestrians", cr::CityObjectLabel::Pedestrians) + .value("Poles", cr::CityObjectLabel::Poles) + .value("RoadLines", cr::CityObjectLabel::RoadLines) + .value("Roads", cr::CityObjectLabel::Roads) + .value("Sidewalks", cr::CityObjectLabel::Sidewalks) + .value("TrafficSigns", cr::CityObjectLabel::TrafficSigns) + .value("Vegetation", cr::CityObjectLabel::Vegetation) + .value("Vehicles", cr::CityObjectLabel::Vehicles) + .value("Walls", cr::CityObjectLabel::Walls) + .value("Sky", cr::CityObjectLabel::Sky) + .value("Ground", cr::CityObjectLabel::Ground) + .value("Bridge", cr::CityObjectLabel::Bridge) + .value("RailTrack", cr::CityObjectLabel::RailTrack) + .value("GuardRail", cr::CityObjectLabel::GuardRail) + .value("TrafficLight", cr::CityObjectLabel::TrafficLight) + .value("Static", cr::CityObjectLabel::Static) + .value("Dynamic", cr::CityObjectLabel::Dynamic) + .value("Water", cr::CityObjectLabel::Water) + .value("Terrain", cr::CityObjectLabel::Terrain) + ; + #define SPAWN_ACTOR_WITHOUT_GIL(fn) +[]( \ cc::World &self, \ const cc::ActorBlueprint &blueprint, \ @@ -177,7 +204,7 @@ void export_world() { .def("get_traffic_light", CONST_CALL_WITHOUT_GIL_1(cc::World, GetTrafficLight, cc::Landmark), arg("landmark")) .def("get_lightmanager", CONST_CALL_WITHOUT_GIL(cc::World, GetLightManager)) .def("freeze_all_traffic_lights", &cc::World::FreezeAllTrafficLights, (arg("frozen"))) - .def("get_level_bbs", &GetLevelBBs) + .def("get_level_bbs", &GetLevelBBs, (arg("actor_type")=cr::CityObjectLabel::None)) .def(self_ns::str(self_ns::self)) ; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorInfo.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorInfo.h index 9f407ea59..8b0e9aac7 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorInfo.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorInfo.h @@ -11,8 +11,11 @@ #include #include +#include #include +namespace crp = carla::rpc; + /// A view over an actor and its properties. struct FActorInfo { @@ -20,11 +23,11 @@ public: FActorDescription Description; - TSet SemanticTags; + TSet SemanticTags; FBoundingBox BoundingBox; - carla::rpc::Actor SerializedData; + crp::Actor SerializedData; /// @todo To be used solely by the FWorldObserver. mutable FVector Velocity = {0.0f, 0.0f, 0.0f}; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorRegistry.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorRegistry.cpp index a62a64806..953ef3447 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorRegistry.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorRegistry.cpp @@ -13,6 +13,8 @@ #include "Carla/Traffic/TrafficLightBase.h" #include "Carla/Util/BoundingBoxCalculator.h" +namespace crp = carla::rpc; + static FActorView::ActorType FActorRegistry_GetActorType(const FActorView &View) { if (!View.IsValid()) @@ -41,11 +43,11 @@ static FActorView::ActorType FActorRegistry_GetActorType(const FActorView &View) } } -static FString GetRelevantTagAsString(const TSet &SemanticTags) +static FString GetRelevantTagAsString(const TSet &SemanticTags) { for (auto &&Tag : SemanticTags) { - if ((Tag != ECityObjectLabel::None) && (Tag != ECityObjectLabel::Other)) + if ((Tag != crp::CityObjectLabel::None) && (Tag != crp::CityObjectLabel::Other)) { auto Str = ATagger::GetTagAsString(Tag).ToLower(); return (Str.EndsWith(TEXT("s")) ? Str.LeftChop(1) : Str); diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/CityMapGenerator.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/CityMapGenerator.cpp index d4cb13f85..1e92bcfc2 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/CityMapGenerator.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/CityMapGenerator.cpp @@ -22,6 +22,8 @@ #include #endif // CARLA_ROAD_GENERATOR_EXTRA_LOG +namespace crp = carla::rpc; + // ============================================================================= // -- Private types ------------------------------------------------------------ // ============================================================================= @@ -248,7 +250,7 @@ static bool LineTrace( if (Success) { for (FHitResult &Item : OutHits) { - if (ATagger::MatchComponent(*Item.Component, ECityObjectLabel::Roads)) { + if (ATagger::MatchComponent(*Item.Component, crp::CityObjectLabel::Roads)) { HitResult = Item; return true; } diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameModeBase.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameModeBase.cpp index 47a68e634..fba24c8ba 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameModeBase.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameModeBase.cpp @@ -332,7 +332,7 @@ void ACarlaGameModeBase::DebugShowSignals(bool enable) } -TArray ACarlaGameModeBase::GetAllBBsOfLevel() +TArray ACarlaGameModeBase::GetAllBBsOfLevel(uint8 TagQueried) { UWorld* World = GetWorld(); @@ -341,7 +341,7 @@ TArray ACarlaGameModeBase::GetAllBBsOfLevel() UGameplayStatics::GetAllActorsOfClass(World, AActor::StaticClass(), FoundActors); TArray BoundingBoxes; - BoundingBoxes = UBoundingBoxCalculator::GetBoundingBoxOfActors(FoundActors); + BoundingBoxes = UBoundingBoxCalculator::GetBoundingBoxOfActors(FoundActors, TagQueried); return BoundingBoxes; -} \ No newline at end of file +} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameModeBase.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameModeBase.h index 5c1376f89..9abcc6ce9 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameModeBase.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameModeBase.h @@ -53,7 +53,7 @@ public: ATrafficLightManager* GetTrafficLightManager(); UFUNCTION(Category = "Carla Game Mode", BlueprintCallable, CallInEditor, Exec) - TArray GetAllBBsOfLevel(); + TArray GetAllBBsOfLevel(uint8 TagQueried = 0); protected: diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/Tagger.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/Tagger.cpp index b5011fc66..e214af2e4 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/Tagger.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/Tagger.cpp @@ -14,46 +14,48 @@ #include "EngineUtils.h" #include "PhysicsEngine/PhysicsAsset.h" +namespace crp = carla::rpc; + template static auto CastEnum(T label) { return static_cast::type>(label); } -ECityObjectLabel ATagger::GetLabelByFolderName(const FString &String) { - if (String == "Building") return ECityObjectLabel::Buildings; - else if (String == "Fence") return ECityObjectLabel::Fences; - else if (String == "Pedestrian") return ECityObjectLabel::Pedestrians; - else if (String == "Pole") return ECityObjectLabel::Poles; - else if (String == "Other") return ECityObjectLabel::Other; - else if (String == "Road") return ECityObjectLabel::Roads; - else if (String == "RoadLine") return ECityObjectLabel::RoadLines; - else if (String == "SideWalk") return ECityObjectLabel::Sidewalks; - else if (String == "TrafficSign") return ECityObjectLabel::TrafficSigns; - else if (String == "Vegetation") return ECityObjectLabel::Vegetation; - else if (String == "Vehicles") return ECityObjectLabel::Vehicles; - else if (String == "Wall") return ECityObjectLabel::Walls; - else if (String == "Sky") return ECityObjectLabel::Sky; - else if (String == "Ground") return ECityObjectLabel::Ground; - else if (String == "Bridge") return ECityObjectLabel::Bridge; - else if (String == "RailTrack") return ECityObjectLabel::RailTrack; - else if (String == "GuardRail") return ECityObjectLabel::GuardRail; - else if (String == "TrafficLight") return ECityObjectLabel::TrafficLight; - else if (String == "Static") return ECityObjectLabel::Static; - else if (String == "Dynamic") return ECityObjectLabel::Dynamic; - else if (String == "Water") return ECityObjectLabel::Water; - else if (String == "Terrain") return ECityObjectLabel::Terrain; - else return ECityObjectLabel::None; +crp::CityObjectLabel ATagger::GetLabelByFolderName(const FString &String) { + if (String == "Building") return crp::CityObjectLabel::Buildings; + else if (String == "Fence") return crp::CityObjectLabel::Fences; + else if (String == "Pedestrian") return crp::CityObjectLabel::Pedestrians; + else if (String == "Pole") return crp::CityObjectLabel::Poles; + else if (String == "Other") return crp::CityObjectLabel::Other; + else if (String == "Road") return crp::CityObjectLabel::Roads; + else if (String == "RoadLine") return crp::CityObjectLabel::RoadLines; + else if (String == "SideWalk") return crp::CityObjectLabel::Sidewalks; + else if (String == "TrafficSign") return crp::CityObjectLabel::TrafficSigns; + else if (String == "Vegetation") return crp::CityObjectLabel::Vegetation; + else if (String == "Vehicles") return crp::CityObjectLabel::Vehicles; + else if (String == "Wall") return crp::CityObjectLabel::Walls; + else if (String == "Sky") return crp::CityObjectLabel::Sky; + else if (String == "Ground") return crp::CityObjectLabel::Ground; + else if (String == "Bridge") return crp::CityObjectLabel::Bridge; + else if (String == "RailTrack") return crp::CityObjectLabel::RailTrack; + else if (String == "GuardRail") return crp::CityObjectLabel::GuardRail; + else if (String == "TrafficLight") return crp::CityObjectLabel::TrafficLight; + else if (String == "Static") return crp::CityObjectLabel::Static; + else if (String == "Dynamic") return crp::CityObjectLabel::Dynamic; + else if (String == "Water") return crp::CityObjectLabel::Water; + else if (String == "Terrain") return crp::CityObjectLabel::Terrain; + else return crp::CityObjectLabel::None; } void ATagger::SetStencilValue( UPrimitiveComponent &Component, - const ECityObjectLabel &Label, + const crp::CityObjectLabel &Label, const bool bSetRenderCustomDepth) { Component.SetCustomDepthStencilValue(CastEnum(Label)); Component.SetRenderCustomDepth( bSetRenderCustomDepth && - (Label != ECityObjectLabel::None)); + (Label != crp::CityObjectLabel::None)); } // ============================================================================= @@ -98,24 +100,24 @@ void ATagger::TagActorsInLevel(UWorld &World, bool bTagForSemanticSegmentation) } } -void ATagger::GetTagsOfTaggedActor(const AActor &Actor, TSet &Tags) +void ATagger::GetTagsOfTaggedActor(const AActor &Actor, TSet &Tags) { TArray Components; Actor.GetComponents(Components); for (auto *Component : Components) { if (Component != nullptr) { const auto Tag = GetTagOfTaggedComponent(*Component); - if (Tag != ECityObjectLabel::None) { + if (Tag != crp::CityObjectLabel::None) { Tags.Add(Tag); } } } } -FString ATagger::GetTagAsString(const ECityObjectLabel Label) +FString ATagger::GetTagAsString(const crp::CityObjectLabel Label) { switch (Label) { -#define CARLA_GET_LABEL_STR(lbl) case ECityObjectLabel:: lbl : return TEXT(#lbl); +#define CARLA_GET_LABEL_STR(lbl) case crp::CityObjectLabel:: lbl : return TEXT(#lbl); default: CARLA_GET_LABEL_STR(None) CARLA_GET_LABEL_STR(Buildings) diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/Tagger.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/Tagger.h index 7237bcaaf..5c8e578c5 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/Tagger.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/Tagger.h @@ -8,34 +8,14 @@ #include "GameFramework/Actor.h" #include "Components/PrimitiveComponent.h" + +#include +#include +#include + #include "Tagger.generated.h" -enum class ECityObjectLabel : uint8 -{ - None = 0u, - Buildings = 1u, - Fences = 2u, - Other = 3u, - Pedestrians = 4u, - Poles = 5u, - RoadLines = 6u, - Roads = 7u, - Sidewalks = 8u, - TrafficSigns = 12u, - Vegetation = 9u, - Vehicles = 10u, - Walls = 11u, - Sky = 13u, - Ground = 14u, - Bridge = 15u, - RailTrack = 16u, - GuardRail = 17u, - TrafficLight = 18u, - Static = 19u, - Dynamic = 20u, - Water = 21u, - Terrain = 22u, -}; +namespace crp = carla::rpc; /// Sets actors' custom depth stencil value for semantic segmentation according /// to their meshes. @@ -67,40 +47,40 @@ public: static void TagActorsInLevel(UWorld &World, bool bTagForSemanticSegmentation); /// Retrieve the tag of an already tagged component. - static ECityObjectLabel GetTagOfTaggedComponent(const UPrimitiveComponent &Component) + static crp::CityObjectLabel GetTagOfTaggedComponent(const UPrimitiveComponent &Component) { - return static_cast(Component.CustomDepthStencilValue); + return static_cast(Component.CustomDepthStencilValue); } - /// Retrieve the tags of an already tagged actor. ECityObjectLabel::None is + /// Retrieve the tags of an already tagged actor. CityObjectLabel::None is /// not added to the array. - static void GetTagsOfTaggedActor(const AActor &Actor, TSet &Tags); + static void GetTagsOfTaggedActor(const AActor &Actor, TSet &Tags); /// Return true if @a Component has been tagged with the given @a Tag. - static bool MatchComponent(const UPrimitiveComponent &Component, ECityObjectLabel Tag) + static bool MatchComponent(const UPrimitiveComponent &Component, crp::CityObjectLabel Tag) { return (Tag == GetTagOfTaggedComponent(Component)); } - /// Retrieve the tags of an already tagged actor. ECityObjectLabel::None is + /// Retrieve the tags of an already tagged actor. CityObjectLabel::None is /// not added to the array. - static FString GetTagAsString(ECityObjectLabel Tag); + static FString GetTagAsString(crp::CityObjectLabel Tag); /// Method that computes the label corresponding to a folder path - static ECityObjectLabel GetLabelByFolderName(const FString &String); + static crp::CityObjectLabel GetLabelByFolderName(const FString &String); /// Method that computes the label corresponding to an specific object /// using the folder path in which it is stored template - static ECityObjectLabel GetLabelByPath(const T *Object) { + static crp::CityObjectLabel GetLabelByPath(const T *Object) { const FString Path = Object->GetPathName(); TArray StringArray; Path.ParseIntoArray(StringArray, TEXT("/"), false); - return (StringArray.Num() > 4 ? GetLabelByFolderName(StringArray[4]) : ECityObjectLabel::None); + return (StringArray.Num() > 4 ? GetLabelByFolderName(StringArray[4]) : crp::CityObjectLabel::None); } static void SetStencilValue(UPrimitiveComponent &Component, - const ECityObjectLabel &Label, const bool bSetRenderCustomDepth); + const crp::CityObjectLabel &Label, const bool bSetRenderCustomDepth); ATagger(); diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/RayCastSemanticLidar.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/RayCastSemanticLidar.cpp index 052dea24c..92b6a5332 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/RayCastSemanticLidar.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/RayCastSemanticLidar.cpp @@ -19,6 +19,8 @@ #include "Runtime/Engine/Classes/Kismet/KismetMathLibrary.h" #include "Runtime/Core/Public/Async/ParallelFor.h" +namespace crp = carla::rpc; + FActorDefinition ARayCastSemanticLidar::GetSensorDefinition() { return UActorBlueprintFunctionLibrary::MakeLidarDefinition(TEXT("ray_cast_semantic")); @@ -160,7 +162,7 @@ void ARayCastSemanticLidar::ComputeRawDetection(const FHitResult& HitInfo, const AActor* actor = HitInfo.Actor.Get(); Detection.object_idx = 0; - Detection.object_tag = static_cast(ECityObjectLabel::None); + Detection.object_tag = static_cast(crp::CityObjectLabel::None); if (actor != nullptr) { FActorView view = Registry.Find(actor); diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp index 60dedb32b..121ec6c07 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/CarlaServer.cpp @@ -306,7 +306,7 @@ void FCarlaServer::FPimpl::BindActions() return Episode->SerializeActor(ActorView); }; - BIND_SYNC(get_all_level_BBs) << [this]() -> R> + BIND_SYNC(get_all_level_BBs) << [this](uint8 QueriedTag) -> R> { REQUIRE_CARLA_EPISODE(); TArray Result; @@ -315,7 +315,7 @@ void FCarlaServer::FPimpl::BindActions() { RESPOND_ERROR("unable to find CARLA game mode"); } - Result = GameMode->GetAllBBsOfLevel(); + Result = GameMode->GetAllBBsOfLevel(QueriedTag); return MakeVectorFromTArray(Result); }; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.cpp index e5d8e8e2b..4f4c016c1 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.cpp @@ -15,7 +15,61 @@ #include "Rendering/SkeletalMeshRenderData.h" -FBoundingBox UBoundingBoxCalculator::GetActorBoundingBox(const AActor *Actor) + +namespace crp = carla::rpc; + +static FBoundingBox ApplyTransformToBB( + const FBoundingBox& InBoundingBox, + const FTransform& Transform) +{ + const FRotator Rotation = Transform.GetRotation().Rotator(); + const FVector Translation = Transform.GetLocation(); + const FVector Scale = Transform.GetScale3D(); + + FBoundingBox BoundingBox = InBoundingBox; + BoundingBox.Origin *= Scale; + BoundingBox.Origin = Rotation.RotateVector(BoundingBox.Origin) + Translation; + BoundingBox.Extent *= Scale; + BoundingBox.Rotation = Rotation; + + return BoundingBox; +} + +static FBoundingBox CombineBBs(const TArray& BBsToCombine) +{ + FVector MaxVertex(TNumericLimits::Lowest()); + FVector MinVertex(TNumericLimits::Max()); + + for(const FBoundingBox& BB : BBsToCombine) { + FVector MaxVertexOfBB = BB.Origin + BB.Extent; + FVector MinVertexOfBB = BB.Origin - BB.Extent; + + MaxVertex.X = (MaxVertexOfBB.X > MaxVertex.X) ? MaxVertexOfBB.X : MaxVertex.X; + MaxVertex.Y = (MaxVertexOfBB.Y > MaxVertex.Y) ? MaxVertexOfBB.Y : MaxVertex.Y; + MaxVertex.Z = (MaxVertexOfBB.Z > MaxVertex.Z) ? MaxVertexOfBB.Z : MaxVertex.Z; + MinVertex.X = (MinVertexOfBB.X < MinVertex.X) ? MinVertexOfBB.X : MinVertex.X; + MinVertex.Y = (MinVertexOfBB.Y < MinVertex.Y) ? MinVertexOfBB.Y : MinVertex.Y; + MinVertex.Z = (MinVertexOfBB.Z < MinVertex.Z) ? MinVertexOfBB.Z : MinVertex.Z; + } + + // Calculate box extent + FVector Extent ( + (MaxVertex.X - MinVertex.X) * 0.5f, + (MaxVertex.Y - MinVertex.Y) * 0.5f, + (MaxVertex.Z - MinVertex.Z) * 0.5f + ); + + // Calculate middle point + FVector Origin ( + (MinVertex.X + Extent.X), + (MinVertex.Y + Extent.Y), + (MinVertex.Z + Extent.Z) + ); + + return {Origin, Extent}; +} + +FBoundingBox UBoundingBoxCalculator::GetActorBoundingBox(const AActor *Actor, uint8 InTagQueried) { if (Actor != nullptr) { @@ -59,10 +113,132 @@ FBoundingBox UBoundingBoxCalculator::GetActorBoundingBox(const AActor *Actor) return {}; } } + + } return {}; } +FBoundingBox UBoundingBoxCalculator::GetVehicleBoundingBox( + const ACarlaWheeledVehicle* Vehicle, + uint8 InTagQueried) +{ + check(Vehicle); + + crp::CityObjectLabel TagQueried = (crp::CityObjectLabel)InTagQueried; + bool FilterByTagEnabled = (TagQueried != crp::CityObjectLabel::None); + + UActorComponent *ActorComp = Vehicle->GetComponentByClass(USkeletalMeshComponent::StaticClass()); + USkeletalMeshComponent* Comp = Cast(ActorComp); + + // Filter by tag + crp::CityObjectLabel Tag = ATagger::GetTagOfTaggedComponent(*Comp); + if(FilterByTagEnabled && Tag != TagQueried) return {}; + + USkeletalMesh* SkeletalMesh = Comp->SkeletalMesh; + FBoundingBox BoundingBox = GetSkeletalMeshBoundingBox(SkeletalMesh); + if(BoundingBox.Extent.IsZero()) + { + UE_LOG(LogCarla, Error, TEXT("%s has no SKM assigned"), *Vehicle->GetName()); + return {}; + } + + // Component-to-world transform for this component + const FTransform& CompToWorldTransform = Comp->GetComponentTransform(); + BoundingBox = ApplyTransformToBB(BoundingBox, CompToWorldTransform); + + return BoundingBox; +} + +FBoundingBox UBoundingBoxCalculator::GetCharacterBoundingBox( + const ACharacter* Character, + uint8 InTagQueried) +{ + check(Character); + + UCapsuleComponent* Capsule = Character->GetCapsuleComponent(); + if (Capsule) + { + const float Radius = Capsule->GetScaledCapsuleRadius(); + const float HalfHeight = Capsule->GetScaledCapsuleHalfHeight(); + FBoundingBox BoundingBox; + // Characters have the pivot point centered. + BoundingBox.Origin = {0.0f, 0.0f, 0.0f}; + BoundingBox.Extent = {Radius, Radius, HalfHeight}; + // Component-to-world transform for this component + const FTransform& CompToWorldTransform = Capsule->GetComponentTransform(); + BoundingBox = ApplyTransformToBB(BoundingBox, CompToWorldTransform); + + return BoundingBox; + } + + return {}; +} + +void UBoundingBoxCalculator::GetTrafficLightBoundingBox( + const ATrafficLightBase* TrafficLight, + TArray& OutBB, + uint8 InTagQueried) +{ + check(TrafficLight); + + TArray BBsOfTL; + TArray StaticMeshComps; + TrafficLight->GetComponents(StaticMeshComps); + GetBBsOfStaticMeshComponents(StaticMeshComps, BBsOfTL, InTagQueried); + + // This kind of a magic number relying that the lights of a TL are not bigger than 100. + // and we are gonna compare against a squared distance + const float DistanceThreshold = 100.0f * 100.0f; + + // The BBs of the TL are calculated per light, so we need to merge the full-box + TSet IndicesDiscarded; + for(int i = 0; i < BBsOfTL.Num(); i++) + { + // Check if the index was used to merge a previous BB + if(IndicesDiscarded.Contains(i)) continue; + + TArray BBsToCombine; + FBoundingBox& BB1 = BBsOfTL[i]; + + for(int j = i + 1; j < BBsOfTL.Num(); j++) + { + // Check if the index was used to merge a previous BB + if(IndicesDiscarded.Contains(j)) continue; + + FBoundingBox& BB2 = BBsOfTL[j]; + + float Distance = FVector::DistSquared(BB1.Origin, BB2.Origin); + + // If the lights are close enough, we merge it + if(Distance <= DistanceThreshold) + { + BBsToCombine.Emplace(BB2); + IndicesDiscarded.Emplace(j); + } + } + if(BBsToCombine.Num() > 0) + { + BBsToCombine.Emplace(BB1); + IndicesDiscarded.Emplace(i); + FBoundingBox MergedBB = CombineBBs(BBsToCombine); + MergedBB.Rotation = BB1.Rotation; + OutBB.Add(MergedBB); + } + } + + // Add the BB of the meshes that didn't need to combine (ie: poles) + for(int i = 0; i < BBsOfTL.Num(); i++) + { + // Check if the index was used to merge a previous BB + if(IndicesDiscarded.Contains(i)) continue; + FBoundingBox& BB = BBsOfTL[i]; + OutBB.Add(BB); + } + +} + + // TODO: update to calculate current animation pose FBoundingBox UBoundingBoxCalculator::GetSkeletalMeshBoundingBox(const USkeletalMesh* SkeletalMesh) { @@ -80,7 +256,7 @@ FBoundingBox UBoundingBoxCalculator::GetSkeletalMeshBoundingBox(const USkeletalM uint32 NumVertices = FPositionVertexBuffer.GetNumVertices(); // Look for Skeletal Mesh bounds (vertex perfect) - FVector MaxVertex(TNumericLimits::Min()); + FVector MaxVertex(TNumericLimits::Lowest()); FVector MinVertex(TNumericLimits::Max()); for(uint32 i = 0; i < NumVertices; i++) { @@ -93,13 +269,6 @@ FBoundingBox UBoundingBoxCalculator::GetSkeletalMeshBoundingBox(const USkeletalM MinVertex.Z = (Pos.Z < MinVertex.Z) ? Pos.Z : MinVertex.Z; } - // Calculate middle point - FVector Origin ( - (MaxVertex.X + MinVertex.X) * 0.5f, - (MaxVertex.Y + MinVertex.Y) * 0.5f, - (MaxVertex.Z + MinVertex.Z) * 0.5f - ); - // Calculate box extent FVector Extent ( (MaxVertex.X - MinVertex.X) * 0.5f, @@ -107,6 +276,13 @@ FBoundingBox UBoundingBoxCalculator::GetSkeletalMeshBoundingBox(const USkeletalM (MaxVertex.Z - MinVertex.Z) * 0.5f ); + // Calculate middle point + FVector Origin ( + (MinVertex.X + Extent.X), + (MinVertex.Y + Extent.Y), + (MinVertex.Z + Extent.Z) + ); + return {Origin, Extent}; } @@ -119,115 +295,175 @@ FBoundingBox UBoundingBoxCalculator::GetStaticMeshBoundingBox(const UStaticMesh* } FBox Box = StaticMesh->GetBoundingBox(); - return {Box.GetCenter(), Box.GetExtent()}; } -// TODO: Dynamic vehicle, avoid SM of collision -TArray UBoundingBoxCalculator::GetBoundingBoxOfActors(const TArray& Actors) +void UBoundingBoxCalculator::GetISMBoundingBox( + UInstancedStaticMeshComponent* ISMComp, + TArray& OutBoundingBox) { - TArray Result; - - int ActorIndex = 0; - for(AActor* Actor : Actors) + if(!ISMComp) { - //UE_LOG(LogCarla, Warning, TEXT(" %d / %d"), ActorIndex, Actors.Num()); - ActorIndex++; + UE_LOG(LogCarla, Error, TEXT("GetISMBoundingBox no ISMComp")); + return; + } - AInstancedFoliageActor* InstancedFolliageActor = Cast(Actor); - if(InstancedFolliageActor != nullptr) + const UStaticMesh *Mesh = ISMComp->GetStaticMesh(); + const FBoundingBox SMBoundingBox = GetStaticMeshBoundingBox(Mesh); + + if(SMBoundingBox.Extent.IsZero()) + { + UE_LOG(LogCarla, Error, TEXT("%s has no SM assigned to the ISM"), *ISMComp->GetOwner()->GetName()); + return; + } + + const TArray& PerInstanceSMData = ISMComp->PerInstanceSMData; + + for(auto& InstSMIData : PerInstanceSMData) + { + const FTransform Transform = FTransform(InstSMIData.Transform); + FBoundingBox BoundingBox = ApplyTransformToBB(SMBoundingBox, Transform); + OutBoundingBox.Add(BoundingBox); + } + +} + +void UBoundingBoxCalculator::GetBBsOfStaticMeshComponents( + const TArray& StaticMeshComps, + TArray& OutBB, + uint8 InTagQueried) +{ + crp::CityObjectLabel TagQueried = (crp::CityObjectLabel)InTagQueried; + bool FilterByTagEnabled = (TagQueried != crp::CityObjectLabel::None); + + for(UStaticMeshComponent* Comp : StaticMeshComps) + { + // Filter by tag + crp::CityObjectLabel Tag = ATagger::GetTagOfTaggedComponent(*Comp); + if(FilterByTagEnabled && Tag != TagQueried) continue; + + UStaticMesh* StaticMesh = Comp->GetStaticMesh(); + FBoundingBox BoundingBox = GetStaticMeshBoundingBox(StaticMesh); + + if(BoundingBox.Extent.IsZero()) { - TMap>& FoliageInstancesMap = InstancedFolliageActor->FoliageInfos; - - - UE_LOG(LogCarla, Warning, TEXT("FolliageActor with %d FoliageTypes"), FoliageInstancesMap.Num()); - - int FoliageIndex = 0; - for(auto& FoliagePair: FoliageInstancesMap) - { - const UFoliageType* FoliageType = FoliagePair.Key; - const FFoliageInfo& FoliageInfo = FoliagePair.Value.Get(); - const UFoliageType_InstancedStaticMesh* FoliageType_ISM = Cast(FoliageType); - - UHierarchicalInstancedStaticMeshComponent* HISMComp = FoliageInfo.GetComponent(); - - UStaticMesh *Mesh = FoliageType_ISM->GetStaticMesh(); - FBoundingBox SMBoundingBox = GetStaticMeshBoundingBox(Mesh); - - int32 NumHISMInstances = HISMComp->GetNumRenderInstances(); - - UE_LOG(LogCarla, Warning, TEXT(" %d/%d : NumHISMInstances = %d"), - FoliageIndex, FoliageInstancesMap.Num(), NumHISMInstances); - FoliageIndex++; - - const TArray& PerInstanceSMData = HISMComp->PerInstanceSMData; - - for(auto& InstSMIData : PerInstanceSMData) - { - FTransform Transform = FTransform(InstSMIData.Transform); - FRotator Rotation = Transform.GetRotation().Rotator(); - FVector Translation = Transform.GetLocation(); - FVector Scale = Transform.GetScale3D(); - - FBoundingBox BoundingBox = SMBoundingBox; - BoundingBox.Origin *= Scale; - BoundingBox.Origin = Rotation.RotateVector(BoundingBox.Origin) + Translation; - BoundingBox.Extent *= Scale; - BoundingBox.Rotation = Rotation; - Result.Add(BoundingBox); - } - } - + UE_LOG(LogCarla, Error, TEXT("%s has no SM assigned"), *Comp->GetOwner()->GetName()); } else { - TArray MeshComps; - Actor->GetComponents(MeshComps); + // Component-to-world transform for this component + const FTransform& CompToWorldTransform = Comp->GetComponentTransform(); + BoundingBox = ApplyTransformToBB(BoundingBox, CompToWorldTransform); + OutBB.Add(BoundingBox); + } + } +} - FVector WorldLocation = Actor->GetActorLocation(); - FVector WorldScale = Actor->GetActorScale(); - FRotator WorldRotation = Actor->GetActorRotation(); +void UBoundingBoxCalculator::GetBBsOfSkeletalMeshComponents( + const TArray& SkeletalMeshComps, + TArray& OutBB, + uint8 InTagQueried) +{ + crp::CityObjectLabel TagQueried = (crp::CityObjectLabel)InTagQueried; + bool FilterByTagEnabled = (TagQueried != crp::CityObjectLabel::None); - // Find if there is some geometry component - TArray StaticMeshComps; - TArray SkeletalMeshComps; - Actor->GetComponents(StaticMeshComps); - Actor->GetComponents(SkeletalMeshComps); + for(USkeletalMeshComponent* Comp : SkeletalMeshComps) + { + // Filter by tag + crp::CityObjectLabel Tag = ATagger::GetTagOfTaggedComponent(*Comp); + if(FilterByTagEnabled && Tag != TagQueried) continue; - // Calculate FBoundingBox of SM - for(UStaticMeshComponent* StaticMeshComp : StaticMeshComps) + USkeletalMesh* SkeletalMesh = Comp->SkeletalMesh; + FBoundingBox BoundingBox = GetSkeletalMeshBoundingBox(SkeletalMesh); + if(BoundingBox.Extent.IsZero()) + { + UE_LOG(LogCarla, Error, TEXT("%s has no SKM assigned"), *Comp->GetOwner()->GetName()); + } + else + { + // Component-to-world transform for this component + const FTransform& CompToWorldTransform = Comp->GetComponentTransform(); + BoundingBox = ApplyTransformToBB(BoundingBox, CompToWorldTransform); + OutBB.Add(BoundingBox); + } + } +} + +TArray UBoundingBoxCalculator::GetBoundingBoxOfActors( + const TArray& Actors, + uint8 InTagQueried) +{ + TArray Result; + crp::CityObjectLabel TagQueried = (crp::CityObjectLabel)InTagQueried; + bool FilterByTagEnabled = (TagQueried != crp::CityObjectLabel::None); + + for(AActor* Actor : Actors) + { + FString ClassName = Actor->GetClass()->GetName(); + + // Avoid the BP_Procedural_Building to avoid duplication with their child actors + // When improved the BP_Procedural_Building this maybe should be removed + // Note: We don't use casting here because the base class is a BP and is easier to do it this way, + // than getting the UClass of the BP to cast the actor. + if(ClassName.Contains("BP_Procedural_Bulding")) continue; + + // The vehicle's BP has a low-polystatic mesh for collisions, we should avoid it + ACarlaWheeledVehicle* Vehicle = Cast(Actor); + if (Vehicle) + { + FBoundingBox BoundingBox = GetVehicleBoundingBox(Vehicle, InTagQueried); + if(!BoundingBox.Extent.IsZero()) { - UStaticMesh* StaticMesh = StaticMeshComp->GetStaticMesh(); - FBoundingBox BoundingBox = GetStaticMeshBoundingBox(StaticMesh); - - if(BoundingBox.Extent.IsZero()) - { - UE_LOG(LogCarla, Error, TEXT("%s has no SM assigned"), *Actor->GetName()); - } - else - { - BoundingBox.Origin *= WorldScale; - BoundingBox.Origin = WorldRotation.RotateVector(BoundingBox.Origin) + WorldLocation; - BoundingBox.Extent *= WorldScale; - BoundingBox.Rotation = WorldRotation; - Result.Add(BoundingBox); - } - } - - // Calculate FBoundingBox of SK_M - for(USkeletalMeshComponent* SkeletalMeshComp : SkeletalMeshComps) - { - USkeletalMesh* SkeletalMesh = SkeletalMeshComp->SkeletalMesh; - FBoundingBox BoundingBox = GetSkeletalMeshBoundingBox(SkeletalMesh); - BoundingBox.Origin *= WorldScale; - BoundingBox.Origin = WorldRotation.RotateVector(BoundingBox.Origin) + WorldLocation; - BoundingBox.Extent *= WorldScale; - BoundingBox.Rotation = WorldRotation; Result.Add(BoundingBox); } + continue; } + + + // Pedestrians, we just use the capsule component at the moment. + ACharacter* Character = Cast(Actor); + if (Character) + { + FBoundingBox BoundingBox = GetCharacterBoundingBox(Character, InTagQueried); + if(!BoundingBox.Extent.IsZero()) + { + Result.Add(BoundingBox); + } + continue; + } + + // TrafficLight, we need to join all the BB of the lights in one + ATrafficLightBase* TrafficLight = Cast(Actor); + if(TrafficLight) + { + GetTrafficLightBoundingBox(TrafficLight, Result, InTagQueried); + continue; + } + + // Calculate FBoundingBox of SM + TArray StaticMeshComps; + Actor->GetComponents(StaticMeshComps); + GetBBsOfStaticMeshComponents(StaticMeshComps, Result, InTagQueried); + + // Calculate FBoundingBox of SK_M + TArray SkeletalMeshComps; + Actor->GetComponents(SkeletalMeshComps); + GetBBsOfSkeletalMeshComponents(SkeletalMeshComps, Result, InTagQueried); + + // Calculate FBoundingBox of ISM + TArray ISMComps; + Actor->GetComponents(ISMComps); + for(UInstancedStaticMeshComponent* Comp: ISMComps) + { + // Filter by tag + crp::CityObjectLabel Tag = ATagger::GetTagOfTaggedComponent(*Comp); + if(FilterByTagEnabled && Tag != TagQueried) continue; + + GetISMBoundingBox(Comp, Result); + } + } return Result; -} \ No newline at end of file +} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.h index cb393bf4b..ed641bc69 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.h @@ -28,15 +28,52 @@ public: /// /// @warning Traffic signs return its trigger box instead. UFUNCTION(Category = "Carla Actor", BlueprintCallable) - static FBoundingBox GetActorBoundingBox(const AActor *Actor); + static FBoundingBox GetActorBoundingBox( + const AActor *Actor, + uint8 InTagQueried = 0); - UFUNCTION(Category = "Carla Util", BlueprintCallable, CallInEditor) + UFUNCTION(Category = "Carla Actor", BlueprintCallable) + static FBoundingBox GetVehicleBoundingBox( + const ACarlaWheeledVehicle* Vehicle, + uint8 InTagQueried = 0); + + UFUNCTION(Category = "Carla Actor", BlueprintCallable) + static FBoundingBox GetCharacterBoundingBox( + const ACharacter* Character, + uint8 InTagQueried = 0); + + UFUNCTION(Category = "Carla Actor", BlueprintCallable) + static void GetTrafficLightBoundingBox( + const ATrafficLightBase* TrafficLight, + TArray& OutBB, + uint8 InTagQueried = 0); + + UFUNCTION(Category = "Carla Util", BlueprintCallable) static FBoundingBox GetSkeletalMeshBoundingBox(const USkeletalMesh* SkeletalMesh); - UFUNCTION(Category = "Carla Util", BlueprintCallable, CallInEditor) + UFUNCTION(Category = "Carla Util", BlueprintCallable) static FBoundingBox GetStaticMeshBoundingBox(const UStaticMesh* StaticMesh); - UFUNCTION(Category = "Carla Util", BlueprintCallable, CallInEditor) - static TArray GetBoundingBoxOfActors(const TArray& Actors); + UFUNCTION(Category = "Carla Util", BlueprintCallable) + static void GetISMBoundingBox( + UInstancedStaticMeshComponent* HISMComp, + TArray& OutBoundingBox); + + UFUNCTION(Category = "Carla Util", BlueprintCallable) + static void GetBBsOfStaticMeshComponents( + const TArray& StaticMeshComps, + TArray& OutBB, + uint8 InTagQueried = 0); + + UFUNCTION(Category = "Carla Util", BlueprintCallable) + static void GetBBsOfSkeletalMeshComponents( + const TArray& SkeletalMeshComps, + TArray& OutBB, + uint8 InTagQueried = 0); + + UFUNCTION(Category = "Carla Util", BlueprintCallable) + static TArray GetBoundingBoxOfActors( + const TArray& Actors, + uint8 InTagQueried = 0); };