From 313b11931c4eae29582f5420665d7866f15e6e7c Mon Sep 17 00:00:00 2001 From: nsubiron Date: Sat, 20 Oct 2018 20:33:37 +0200 Subject: [PATCH] Add collision sensor --- LibCarla/source/carla/sensor/SensorData.h | 6 +- LibCarla/source/carla/sensor/SensorRegistry.h | 6 +- .../source/carla/sensor/data/CollisionEvent.h | 62 +++++++++++++++ .../sensor/s11n/CollisionEventSerializer.cpp | 20 +++++ .../sensor/s11n/CollisionEventSerializer.h | 58 ++++++++++++++ .../sensor/s11n/EpisodeStateSerializer.h | 2 - PythonAPI/manual_control.py | 30 ++++++- PythonAPI/source/libcarla/SensorData.cpp | 15 ++++ .../Actor/ActorBlueprintFunctionLibrary.cpp | 9 +++ .../Actor/ActorBlueprintFunctionLibrary.h | 4 + .../Source/Carla/Actor/ActorRegistry.cpp | 33 ++++++++ .../Carla/Source/Carla/Actor/ActorRegistry.h | 5 ++ .../Carla/Source/Carla/Actor/ActorView.h | 4 +- .../Source/Carla/Game/CarlaGameInstance.h | 5 ++ .../Carla/Source/Carla/Game/Tagger.cpp | 50 ++++++------ .../Plugins/Carla/Source/Carla/Game/Tagger.h | 2 + .../Carla/Game/TheNewCarlaGameModeBase.h | 6 ++ .../Source/Carla/Sensor/CollisionSensor.cpp | 79 +++++++++++++++++++ .../Source/Carla/Sensor/CollisionSensor.h | 45 +++++++++++ .../Source/Carla/Server/TheNewCarlaServer.cpp | 9 +++ .../Source/Carla/Server/TheNewCarlaServer.h | 9 +++ 21 files changed, 421 insertions(+), 38 deletions(-) create mode 100644 LibCarla/source/carla/sensor/data/CollisionEvent.h create mode 100644 LibCarla/source/carla/sensor/s11n/CollisionEventSerializer.cpp create mode 100644 LibCarla/source/carla/sensor/s11n/CollisionEventSerializer.h create mode 100644 Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/CollisionSensor.cpp create mode 100644 Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/CollisionSensor.h diff --git a/LibCarla/source/carla/sensor/SensorData.h b/LibCarla/source/carla/sensor/SensorData.h index 42e487dd8..63be3d558 100644 --- a/LibCarla/source/carla/sensor/SensorData.h +++ b/LibCarla/source/carla/sensor/SensorData.h @@ -41,10 +41,6 @@ namespace sensor { return _sensor_transform; } - auto GetWorld() const { - return client::World{_episode}; - } - protected: const auto &GetEpisode() const { @@ -55,7 +51,7 @@ namespace sensor { /// @todo This shouldn't be exposed in this namespace. friend class client::detail::Simulator; - client::detail::EpisodeProxy _episode; + client::detail::WeakEpisodeProxy _episode; const size_t _frame_number; diff --git a/LibCarla/source/carla/sensor/SensorRegistry.h b/LibCarla/source/carla/sensor/SensorRegistry.h index ff1ecb119..3b4a8e917 100644 --- a/LibCarla/source/carla/sensor/SensorRegistry.h +++ b/LibCarla/source/carla/sensor/SensorRegistry.h @@ -14,11 +14,13 @@ // ============================================================================= // 1. Include the serializer here. +#include "carla/sensor/s11n/CollisionEventSerializer.h" #include "carla/sensor/s11n/EpisodeStateSerializer.h" #include "carla/sensor/s11n/ImageSerializer.h" #include "carla/sensor/s11n/LidarSerializer.h" // 2. Add a forward-declaration of the sensor here. +class ACollisionSensor; class ADepthCamera; class ARayCastLidar; class ASceneCaptureCamera; @@ -37,7 +39,8 @@ namespace sensor { std::pair, std::pair, std::pair, - std::pair + std::pair, + std::pair >; } // namespace sensor @@ -48,6 +51,7 @@ namespace sensor { #ifdef LIBCARLA_SENSOR_REGISTRY_WITH_SENSOR_INCLUDES // 4. Include the sensor here. +#include "Carla/Sensor/CollisionSensor.h" #include "Carla/Sensor/DepthCamera.h" #include "Carla/Sensor/RayCastLidar.h" #include "Carla/Sensor/SceneCaptureCamera.h" diff --git a/LibCarla/source/carla/sensor/data/CollisionEvent.h b/LibCarla/source/carla/sensor/data/CollisionEvent.h new file mode 100644 index 000000000..090948674 --- /dev/null +++ b/LibCarla/source/carla/sensor/data/CollisionEvent.h @@ -0,0 +1,62 @@ +// 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/Debug.h" +#include "carla/client/detail/ActorVariant.h" +#include "carla/geom/Vector3D.h" +#include "carla/sensor/SensorData.h" +#include "carla/sensor/s11n/CollisionEventSerializer.h" + +namespace carla { +namespace sensor { +namespace data { + + /// A registered collision. + class CollisionEvent : public SensorData { + using Super = SensorData; + protected: + + using Serializer = s11n::CollisionEventSerializer; + + friend Serializer; + + explicit CollisionEvent(const RawData &data) + : Super(data), + _self_actor(Serializer::DeserializeRawData(data).self_actor), + _other_actor(Serializer::DeserializeRawData(data).other_actor), + _normal_impulse(Serializer::DeserializeRawData(data).normal_impulse) {} + + public: + + /// Get "self" actor. Actor that measured the collision. + SharedPtr GetActor() const { + return _self_actor.Get(GetEpisode()); + } + + /// Get the actor to which we collided. + SharedPtr GetOtherActor() const { + return _other_actor.Get(GetEpisode()); + } + + /// Normal impulse result of the collision. + const geom::Vector3D &GetNormalImpulse() const { + return _normal_impulse; + } + + private: + + client::detail::ActorVariant _self_actor; + + client::detail::ActorVariant _other_actor; + + geom::Vector3D _normal_impulse; + }; + +} // namespace data +} // namespace sensor +} // namespace carla diff --git a/LibCarla/source/carla/sensor/s11n/CollisionEventSerializer.cpp b/LibCarla/source/carla/sensor/s11n/CollisionEventSerializer.cpp new file mode 100644 index 000000000..f86331d04 --- /dev/null +++ b/LibCarla/source/carla/sensor/s11n/CollisionEventSerializer.cpp @@ -0,0 +1,20 @@ +// 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/sensor/data/CollisionEvent.h" +#include "carla/sensor/s11n/CollisionEventSerializer.h" + +namespace carla { +namespace sensor { +namespace s11n { + + SharedPtr CollisionEventSerializer::Deserialize(RawData data) { + return SharedPtr(new data::CollisionEvent(std::move(data))); + } + +} // namespace s11n +} // namespace sensor +} // namespace carla diff --git a/LibCarla/source/carla/sensor/s11n/CollisionEventSerializer.h b/LibCarla/source/carla/sensor/s11n/CollisionEventSerializer.h new file mode 100644 index 000000000..5e536fe85 --- /dev/null +++ b/LibCarla/source/carla/sensor/s11n/CollisionEventSerializer.h @@ -0,0 +1,58 @@ +// 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/Buffer.h" +#include "carla/Debug.h" +#include "carla/Memory.h" +#include "carla/rpc/Actor.h" +#include "carla/geom/Vector3D.h" +#include "carla/sensor/RawData.h" + +namespace carla { +namespace sensor { + + class SensorData; + +namespace s11n { + + /// Serializes the current state of the whole episode. + class CollisionEventSerializer { + public: + + struct Data { + + rpc::Actor self_actor; + + rpc::Actor other_actor; + + geom::Vector3D normal_impulse; + + MSGPACK_DEFINE_ARRAY(self_actor, other_actor, normal_impulse) + }; + + constexpr static auto header_offset = 0u; + + static Data DeserializeRawData(const RawData &message) { + return MsgPack::UnPack(message.begin(), message.size()); + } + + template + static Buffer Serialize( + const SensorT &, + rpc::Actor self_actor, + rpc::Actor other_actor, + geom::Vector3D normal_impulse) { + return MsgPack::Pack(Data{self_actor, other_actor, normal_impulse}); + } + + static SharedPtr Deserialize(RawData data); + }; + +} // namespace s11n +} // namespace sensor +} // namespace carla diff --git a/LibCarla/source/carla/sensor/s11n/EpisodeStateSerializer.h b/LibCarla/source/carla/sensor/s11n/EpisodeStateSerializer.h index 9a482bb37..889c644a3 100644 --- a/LibCarla/source/carla/sensor/s11n/EpisodeStateSerializer.h +++ b/LibCarla/source/carla/sensor/s11n/EpisodeStateSerializer.h @@ -14,8 +14,6 @@ #include "carla/sensor/RawData.h" #include "carla/sensor/data/ActorDynamicState.h" -class FActorRegistry; - namespace carla { namespace sensor { diff --git a/PythonAPI/manual_control.py b/PythonAPI/manual_control.py index ac752ca60..74222a3c3 100755 --- a/PythonAPI/manual_control.py +++ b/PythonAPI/manual_control.py @@ -121,6 +121,7 @@ class World(object): self.hud = hud blueprint = random.choice(carla_world.get_blueprint_library().filter('vehicle')) self.vehicle = carla_world.spawn_actor(blueprint, START_POSITION) + self.collision_sensor = CollisionSensor(self.vehicle, self.hud) self.camera_manager = CameraManager(self.vehicle, self.hud) self.controller = None self._weather_presets = find_weather_presets() @@ -141,7 +142,7 @@ class World(object): self.hud.render(display) def destroy(self): - for actor in [self.camera_manager.sensor, self.vehicle]: + for actor in [self.camera_manager.sensor, self.collision_sensor.sensor, self.vehicle]: if actor is not None: actor.destroy() @@ -300,6 +301,33 @@ class HelpText(object): display.blit(self.surface, self.pos) +# ============================================================================== +# -- CollisionSensor ----------------------------------------------------------- +# ============================================================================== + + +class CollisionSensor(object): + def __init__(self, parent_actor, hud): + self.sensor = None + self._parent = parent_actor + self._hud = hud + world = self._parent.get_world() + bp = world.get_blueprint_library().find('sensor.other.collision') + self.sensor = world.spawn_actor(bp, carla.Transform(), attach_to=self._parent) + # We need to pass the lambda a weak reference to self to avoid circular + # reference. + weak_self = weakref.ref(self) + self.sensor.listen(lambda event: CollisionSensor._on_collision(weak_self, event)) + + @staticmethod + def _on_collision(weak_self, event): + self = weak_self() + if not self: + return + actor_type = ' '.join(event.other_actor.type_id.title().split('.')[1:]) + self._hud.notification('Collision with %s' % actor_type) + + # ============================================================================== # -- CameraManager ------------------------------------------------------------- # ============================================================================== diff --git a/PythonAPI/source/libcarla/SensorData.cpp b/PythonAPI/source/libcarla/SensorData.cpp index 90b5341d6..51bb5cb41 100644 --- a/PythonAPI/source/libcarla/SensorData.cpp +++ b/PythonAPI/source/libcarla/SensorData.cpp @@ -10,6 +10,7 @@ #include #include #include +#include #include #include @@ -36,6 +37,13 @@ namespace data { return out; } + std::ostream &operator<<(std::ostream &out, const CollisionEvent &meas) { + out << "CollisionEvent(frame=" << meas.GetFrameNumber() + << ", other_actor=" << meas.GetOtherActor() + << ')'; + return out; + } + } // namespace data } // namespace sensor } // namespace carla @@ -167,4 +175,11 @@ void export_sensor_data() { }) .def(self_ns::str(self_ns::self)) ; + + class_, boost::noncopyable, boost::shared_ptr>("CollisionEvent", no_init) + .add_property("actor", &csd::CollisionEvent::GetActor) + .add_property("other_actor", &csd::CollisionEvent::GetOtherActor) + .add_property("normal_impulse", CALL_RETURNING_COPY(csd::CollisionEvent, GetNormalImpulse)) + .def(self_ns::str(self_ns::self)) + ; } diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.cpp index 20069c9d1..afc45a420 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.cpp @@ -194,6 +194,15 @@ static void FillIdAndTags(FActorDefinition &Def, TStrs &&... Strings) Def.Tags = JoinStrings(TEXT(","), std::forward(Strings)...).ToLower(); } +FActorDefinition UActorBlueprintFunctionLibrary::MakeGenericSensorDefinition( + const FString &Type, + const FString &Id) +{ + FActorDefinition Definition; + FillIdAndTags(Definition, TEXT("sensor"), Type, Id); + return Definition; +} + FActorDefinition UActorBlueprintFunctionLibrary::MakeCameraDefinition( const FString &Id, const bool bEnableModifyingPostProcessEffects) diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.h index 9bd426c91..5f929e80b 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorBlueprintFunctionLibrary.h @@ -65,6 +65,10 @@ public: /// ========================================================================== /// @{ + static FActorDefinition MakeGenericSensorDefinition( + const FString &Type, + const FString &Id); + static FActorDefinition MakeCameraDefinition( const FString &Id, bool bEnableModifyingPostProcessEffects = false); diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorRegistry.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorRegistry.cpp index c318dbc09..c064cf1b7 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorRegistry.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorRegistry.cpp @@ -8,6 +8,21 @@ #include "Carla.h" #include "Carla/Actor/ActorRegistry.h" +#include "Carla/Game/Tagger.h" + +static FString GetRelevantTagAsString(const AActor &Actor) +{ + TArray Tags; + ATagger::GetTagsOfTaggedActor(Actor, Tags); + for (auto &&Tag : Tags) + { + if ((Tag != ECityObjectLabel::None) && (Tag != ECityObjectLabel::Other)) + { + return ATagger::GetTagAsString(Tag).ToLower(); + } + } + return TEXT("unknown"); +} FActorView FActorRegistry::Register(AActor &Actor, FActorDescription Description) { @@ -47,3 +62,21 @@ void FActorRegistry::Deregister(AActor *Actor) check(View.IsValid()); Deregister(View.GetActorId()); } + +FActorView FActorRegistry::FindOrFake(AActor *Actor) const +{ + if (Actor == nullptr) + { + return {}; + } + auto View = Find(Actor); + if (!View.IsValid()) + { + View.TheActor = Actor; + auto Description = MakeShared(); + Description->Id = TEXT("static.") + GetRelevantTagAsString(*Actor); + View.Description = Description; + check(View.IsValid()); + } + return View; +} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorRegistry.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorRegistry.h index 563079c75..45609dd52 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorRegistry.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorRegistry.h @@ -78,6 +78,11 @@ public: return View.IsValid() ? View.GetActor() : nullptr; } + /// If the actor is not found in the registry, create a fake actor view. The + /// returned FActorView has some information about the @a Actor but will have + /// an invalid id. + FActorView FindOrFake(AActor *Actor) const; + /// @} // =========================================================================== /// @name Range iteration support diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorView.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorView.h index 1802f1193..e9f34172e 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorView.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Actor/ActorView.h @@ -52,9 +52,7 @@ private: FActorView(IdType ActorId, AActor &Actor, FActorDescription Description) : Id(ActorId), TheActor(&Actor), - Description(MakeShared(std::move(Description))) { - check(Id != 0u); - } + Description(MakeShared(std::move(Description))) {} IdType Id = 0u; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameInstance.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameInstance.h index 671b01ba8..f27d75d11 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameInstance.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/CarlaGameInstance.h @@ -72,6 +72,11 @@ public: void NotifyEndEpisode(); + const FTheNewCarlaServer &GetServer() const + { + return Server; + } + private: UPROPERTY(VisibleAnywhere) diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/Tagger.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/Tagger.cpp index be582090e..e4ae70d02 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/Tagger.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/Tagger.cpp @@ -14,30 +14,6 @@ #include "EngineUtils.h" #include "PhysicsEngine/PhysicsAsset.h" -#ifdef CARLA_TAGGER_EXTRA_LOG -static FString GetLabelAsString(const ECityObjectLabel Label) -{ - switch (Label) { -#define CARLA_GET_LABEL_STR(lbl) case ECityObjectLabel:: lbl : return #lbl; - default: - CARLA_GET_LABEL_STR(None) - CARLA_GET_LABEL_STR(Buildings) - CARLA_GET_LABEL_STR(Fences) - CARLA_GET_LABEL_STR(Other) - CARLA_GET_LABEL_STR(Pedestrians) - CARLA_GET_LABEL_STR(Poles) - CARLA_GET_LABEL_STR(RoadLines) - CARLA_GET_LABEL_STR(Roads) - CARLA_GET_LABEL_STR(Sidewalks) - CARLA_GET_LABEL_STR(TrafficSigns) - CARLA_GET_LABEL_STR(Vegetation) - CARLA_GET_LABEL_STR(Vehicles) - CARLA_GET_LABEL_STR(Walls) -#undef CARLA_GET_LABEL_STR - } -} -#endif // CARLA_TAGGER_EXTRA_LOG - template static auto CastEnum(T label) { @@ -97,7 +73,7 @@ void ATagger::TagActor(const AActor &Actor, bool bTagForSemanticSegmentation) SetStencilValue(*Component, Label, bTagForSemanticSegmentation); #ifdef CARLA_TAGGER_EXTRA_LOG UE_LOG(LogCarla, Log, TEXT(" + StaticMeshComponent: %s"), *Component->GetName()); - UE_LOG(LogCarla, Log, TEXT(" - Label: \"%s\""), *GetLabelAsString(Label)); + UE_LOG(LogCarla, Log, TEXT(" - Label: \"%s\""), *GetTagAsString(Label)); #endif // CARLA_TAGGER_EXTRA_LOG } @@ -109,7 +85,7 @@ void ATagger::TagActor(const AActor &Actor, bool bTagForSemanticSegmentation) SetStencilValue(*Component, Label, bTagForSemanticSegmentation); #ifdef CARLA_TAGGER_EXTRA_LOG UE_LOG(LogCarla, Log, TEXT(" + SkeletalMeshComponent: %s"), *Component->GetName()); - UE_LOG(LogCarla, Log, TEXT(" - Label: \"%s\""), *GetLabelAsString(Label)); + UE_LOG(LogCarla, Log, TEXT(" - Label: \"%s\""), *GetTagAsString(Label)); #endif // CARLA_TAGGER_EXTRA_LOG } } @@ -135,6 +111,28 @@ void ATagger::GetTagsOfTaggedActor(const AActor &Actor, TArray } } +FString ATagger::GetTagAsString(const ECityObjectLabel Label) +{ + switch (Label) { +#define CARLA_GET_LABEL_STR(lbl) case ECityObjectLabel:: lbl : return #lbl; + default: + CARLA_GET_LABEL_STR(None) + CARLA_GET_LABEL_STR(Buildings) + CARLA_GET_LABEL_STR(Fences) + CARLA_GET_LABEL_STR(Other) + CARLA_GET_LABEL_STR(Pedestrians) + CARLA_GET_LABEL_STR(Poles) + CARLA_GET_LABEL_STR(RoadLines) + CARLA_GET_LABEL_STR(Roads) + CARLA_GET_LABEL_STR(Sidewalks) + CARLA_GET_LABEL_STR(TrafficSigns) + CARLA_GET_LABEL_STR(Vegetation) + CARLA_GET_LABEL_STR(Vehicles) + CARLA_GET_LABEL_STR(Walls) +#undef CARLA_GET_LABEL_STR + } +} + // ============================================================================= // -- non-static ATagger functions --------------------------------------------- // ============================================================================= diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/Tagger.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/Tagger.h index 97ee77ffe..ee6ff4ae0 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/Tagger.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/Tagger.h @@ -71,6 +71,8 @@ public: return (Tag == GetTagOfTaggedComponent(Component)); } + static FString GetTagAsString(ECityObjectLabel Tag); + ATagger(); protected: diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/TheNewCarlaGameModeBase.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/TheNewCarlaGameModeBase.h index 7b9a1b977..dd29b0ea4 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/TheNewCarlaGameModeBase.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Game/TheNewCarlaGameModeBase.h @@ -28,6 +28,12 @@ public: ATheNewCarlaGameModeBase(const FObjectInitializer& ObjectInitializer); + const UCarlaEpisode &GetCarlaEpisode() const + { + check(Episode != nullptr); + return *Episode; + } + protected: void InitGame(const FString &MapName, const FString &Options, FString &ErrorMessage) override; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/CollisionSensor.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/CollisionSensor.cpp new file mode 100644 index 000000000..40526b639 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/CollisionSensor.cpp @@ -0,0 +1,79 @@ +// 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/Sensor/CollisionSensor.h" + +#include "Carla/Actor/ActorBlueprintFunctionLibrary.h" +#include "Carla/Actor/ActorRegistry.h" +#include "Carla/Game/CarlaEpisode.h" +#include "Carla/Game/TheNewCarlaGameModeBase.h" + +ACollisionSensor::ACollisionSensor(const FObjectInitializer& ObjectInitializer) + : Super(ObjectInitializer) +{ + PrimaryActorTick.bCanEverTick = false; +} + +FActorDefinition ACollisionSensor::GetSensorDefinition() +{ + return UActorBlueprintFunctionLibrary::MakeGenericSensorDefinition( + TEXT("other"), + TEXT("collision")); +} + +void ACollisionSensor::SetOwner(AActor *NewOwner) +{ + Super::SetOwner(NewOwner); + + /// @todo Deregister previous owner if there was one. + + if (NewOwner != nullptr) + { + NewOwner->OnActorHit.AddDynamic(this, &ACollisionSensor::OnCollisionEvent); + } +} + +void ACollisionSensor::BeginPlay() +{ + Super::BeginPlay(); + + auto *GameMode = Cast(GetWorld()->GetAuthGameMode()); + + if (GameMode == nullptr) + { + UE_LOG(LogCarla, Error, TEXT("ACollisionSensor: Game mode not compatible with this sensor")); + return; + } + Episode = &GameMode->GetCarlaEpisode(); + + GameInstance = Cast(GetGameInstance()); + if (GameMode == nullptr) + { + UE_LOG(LogCarla, Error, TEXT("ACollisionSensor: Game instance not compatible with this sensor")); + return; + } +} + +void ACollisionSensor::OnCollisionEvent( + AActor *Actor, + AActor *OtherActor, + FVector NormalImpulse, + const FHitResult &Hit) +{ + if ((Episode != nullptr) && (GameInstance != nullptr)) + { + const auto &Registry = Episode->GetActorRegistry(); + const auto &Server = GameInstance->GetServer(); + constexpr float TO_METERS = 1e-2; + NormalImpulse *= TO_METERS; + GetDataStream().Send_GameThread( + *this, + Server.SerializeActor(Registry.FindOrFake(Actor)), + Server.SerializeActor(Registry.FindOrFake(OtherActor)), + carla::geom::Vector3D{NormalImpulse.X, NormalImpulse.Y, NormalImpulse.Z}); + } +} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/CollisionSensor.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/CollisionSensor.h new file mode 100644 index 000000000..994f4e196 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/CollisionSensor.h @@ -0,0 +1,45 @@ +// 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/Sensor/Sensor.h" + +#include "CollisionSensor.generated.h" + +class UCarlaEpisode; + +/// A sensor to register collisions. +UCLASS() +class CARLA_API ACollisionSensor : public ASensor +{ + GENERATED_BODY() + +public: + + static FActorDefinition GetSensorDefinition(); + + ACollisionSensor(const FObjectInitializer& ObjectInitializer); + + void SetOwner(AActor *NewOwner) override; + + void BeginPlay() override; + +private: + + UFUNCTION() + void OnCollisionEvent( + AActor *Actor, + AActor *OtherActor, + FVector NormalImpulse, + const FHitResult &Hit); + + UPROPERTY() + const UCarlaEpisode *Episode = nullptr; + + UPROPERTY() + const UCarlaGameInstance *GameInstance = nullptr; +}; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/TheNewCarlaServer.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/TheNewCarlaServer.cpp index a79fa3f4c..628bc7ac3 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/TheNewCarlaServer.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/TheNewCarlaServer.cpp @@ -107,6 +107,8 @@ private: ::AttachActors(Child.GetActor(), Parent.GetActor()); } +public: + carla::rpc::Actor SerializeActor(FActorView ActorView) { if (ActorView.IsValid()) @@ -121,6 +123,8 @@ private: return ActorView; } +private: + carla::streaming::Stream GetSensorStream(FActorView ActorView, ASensor &Sensor) { auto id = ActorView.GetActorId(); auto it = _StreamMap.find(id); @@ -365,3 +369,8 @@ void FTheNewCarlaServer::Stop() { Pimpl->Server.Stop(); } + +carla::rpc::Actor FTheNewCarlaServer::SerializeActor(FActorView View) const +{ + return Pimpl->SerializeActor(View); +} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/TheNewCarlaServer.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/TheNewCarlaServer.h index cb61ad1c4..e9c49ec4f 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/TheNewCarlaServer.h +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Server/TheNewCarlaServer.h @@ -6,8 +6,14 @@ #pragma once +#include "Carla/Actor/ActorView.h" + #include "CoreMinimal.h" +#include +#include +#include + class UCarlaEpisode; class FTheNewCarlaServer @@ -30,6 +36,9 @@ public: void Stop(); + // This is necessary for serializing sensors properly. + carla::rpc::Actor SerializeActor(FActorView View) const; + private: class FPimpl;