From 4235da52a29136478dea5156763f456bed13bc30 Mon Sep 17 00:00:00 2001 From: nsubiron Date: Thu, 17 Jan 2019 16:42:08 +0100 Subject: [PATCH] Add bounding box to pedestrians --- LibCarla/source/carla/geom/BoundingBox.h | 10 +++-- PythonAPI/source/libcarla/Actor.cpp | 1 + .../Source/Carla/Server/TheNewCarlaServer.cpp | 18 ++------ .../Carla/Source/Carla/Util/BoundingBox.h | 23 ++++++++++ .../Carla/Util/BoundingBoxCalculator.cpp | 44 +++++++++++++++++++ .../Source/Carla/Util/BoundingBoxCalculator.h | 30 +++++++++++++ 6 files changed, 108 insertions(+), 18 deletions(-) create mode 100644 Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBox.h create mode 100644 Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.cpp create mode 100644 Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.h diff --git a/LibCarla/source/carla/geom/BoundingBox.h b/LibCarla/source/carla/geom/BoundingBox.h index 9153293ca..05ac48bfd 100644 --- a/LibCarla/source/carla/geom/BoundingBox.h +++ b/LibCarla/source/carla/geom/BoundingBox.h @@ -11,6 +11,10 @@ #include "carla/geom/Location.h" #include "carla/geom/Vector3D.h" +#ifdef LIBCARLA_INCLUDED_FROM_UE4 +# include "Carla/Util/BoundingBox.h" +#endif // LIBCARLA_INCLUDED_FROM_UE4 + namespace carla { namespace geom { @@ -39,9 +43,9 @@ namespace geom { #ifdef LIBCARLA_INCLUDED_FROM_UE4 - BoundingBox(const FVector &Origin, const FVector &BoxExtent) - : location(Origin), - extent(1e-2f * BoxExtent.X, 1e-2f * BoxExtent.Y, 1e-2f * BoxExtent.Z) {} + BoundingBox(const FBoundingBox &Box) + : location(Box.Origin), + extent(1e-2f * Box.Extent.X, 1e-2f * Box.Extent.Y, 1e-2f * Box.Extent.Z) {} #endif // LIBCARLA_INCLUDED_FROM_UE4 diff --git a/PythonAPI/source/libcarla/Actor.cpp b/PythonAPI/source/libcarla/Actor.cpp index 3137c9311..ca8126158 100644 --- a/PythonAPI/source/libcarla/Actor.cpp +++ b/PythonAPI/source/libcarla/Actor.cpp @@ -76,6 +76,7 @@ void export_actor() { ; class_, boost::noncopyable, boost::shared_ptr>("Walker", no_init) + .add_property("bounding_box", CALL_RETURNING_COPY(cc::Walker, GetBoundingBox)) .def("apply_control", &cc::Walker::ApplyControl, (arg("control"))) .def("get_control", &cc::Walker::GetWalkerControl) .def(self_ns::str(self_ns::self)) diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/TheNewCarlaServer.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/TheNewCarlaServer.cpp index 000be00b3..e76997d77 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/TheNewCarlaServer.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/TheNewCarlaServer.cpp @@ -8,6 +8,7 @@ #include "Carla/Server/TheNewCarlaServer.h" #include "Carla/Sensor/Sensor.h" +#include "Carla/Util/BoundingBoxCalculator.h" #include "Carla/Util/DebugShapeDrawer.h" #include "Carla/Util/OpenDrive.h" #include "Carla/Vehicle/CarlaWheeledVehicle.h" @@ -114,30 +115,17 @@ private: ::AttachActors(Child.GetActor(), Parent.GetActor()); } - carla::geom::BoundingBox GetActorBoundingBox(const AActor &Actor) - { - /// @todo Bounding boxes only available for vehicles. - auto Vehicle = Cast(&Actor); - if (Vehicle != nullptr) - { - FVector Location = Vehicle->GetVehicleBoundingBoxTransform().GetTranslation(); - FVector Extent = Vehicle->GetVehicleBoundingBoxExtent(); - return {Location, Extent}; - } - return {}; - } - public: carla::rpc::Actor SerializeActor(FActorView ActorView) { carla::rpc::Actor Actor; Actor.id = ActorView.GetActorId(); - if (ActorView.IsValid()) + if (ActorView.IsValid() && !ActorView.GetActor()->IsPendingKill()) { Actor.parent_id = Episode->GetActorRegistry().Find(ActorView.GetActor()->GetOwner()).GetActorId(); Actor.description = *ActorView.GetActorDescription(); - Actor.bounding_box = GetActorBoundingBox(*ActorView.GetActor()); + Actor.bounding_box = UBoundingBoxCalculator::GetActorBoundingBox(ActorView.GetActor()); Actor.semantic_tags.reserve(ActorView.GetSemanticTags().Num()); using tag_t = decltype(Actor.semantic_tags)::value_type; for (auto &&Tag : ActorView.GetSemanticTags()) diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBox.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBox.h new file mode 100644 index 000000000..60eb2221b --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBox.h @@ -0,0 +1,23 @@ +// 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 "BoundingBox.generated.h" + +USTRUCT(BlueprintType) +struct CARLA_API FBoundingBox +{ + GENERATED_BODY() + + /// Origin of the bounding box relative to its owner. + UPROPERTY(EditAnywhere, BlueprintReadWrite) + FVector Origin = {0.0f, 0.0f, 0.0f}; + + /// Radii extent of the bounding box. + UPROPERTY(EditAnywhere, BlueprintReadWrite) + FVector Extent = {0.0f, 0.0f, 0.0f}; +}; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.cpp new file mode 100644 index 000000000..49cdd790e --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.cpp @@ -0,0 +1,44 @@ +// 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 "Carla.h" +#include "Carla/Util/BoundingBoxCalculator.h" + +#include "Carla/Vehicle/CarlaWheeledVehicle.h" + +#include "Components/CapsuleComponent.h" +#include "GameFramework/Character.h" + +FBoundingBox UBoundingBoxCalculator::GetActorBoundingBox(const AActor *Actor) +{ + if (Actor != nullptr) + { + // Vehicle. + auto Vehicle = Cast(Actor); + if (Vehicle != nullptr) + { + FVector Origin = Vehicle->GetVehicleBoundingBoxTransform().GetTranslation(); + FVector Extent = Vehicle->GetVehicleBoundingBoxExtent(); + return {Origin, Extent}; + } + // Walker. + auto Character = Cast(Actor); + if (Character != nullptr) + { + auto Capsule = Character->GetCapsuleComponent(); + if (Capsule != nullptr) + { + const auto Radius = Capsule->GetScaledCapsuleRadius(); + const auto HalfHeight = Capsule->GetScaledCapsuleHalfHeight(); + // Characters have the pivot point centered. + FVector Origin = {0.0f, 0.0f, 0.0f}; + FVector Extent = {Radius, Radius, HalfHeight}; + return {Origin, Extent}; + } + } + } + return {}; +} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.h new file mode 100644 index 000000000..816ed173c --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Util/BoundingBoxCalculator.h @@ -0,0 +1,30 @@ +// 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/Util/BoundingBox.h" + +#include "Kismet/BlueprintFunctionLibrary.h" + +#include "BoundingBoxCalculator.generated.h" + +class AActor; + +UCLASS() +class CARLA_API UBoundingBoxCalculator : public UBlueprintFunctionLibrary +{ + GENERATED_BODY() + +public: + + /// Compute the bounding box of the given Carla actor. + /// + /// @warning If the actor type is not supported a default initialized bounding + /// box is returned. + UFUNCTION(Category = "Carla Actor", BlueprintCallable) + static FBoundingBox GetActorBoundingBox(const AActor *Actor); +};