Add collision sensor

This commit is contained in:
nsubiron 2018-10-20 20:33:37 +02:00
parent 05c78aa2a1
commit 313b11931c
21 changed files with 421 additions and 38 deletions

View File

@ -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;

View File

@ -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<ASceneCaptureCamera *, s11n::ImageSerializer>,
std::pair<ADepthCamera *, s11n::ImageSerializer>,
std::pair<ASemanticSegmentationCamera *, s11n::ImageSerializer>,
std::pair<ARayCastLidar *, s11n::LidarSerializer>
std::pair<ARayCastLidar *, s11n::LidarSerializer>,
std::pair<ACollisionSensor *, s11n::CollisionEventSerializer>
>;
} // 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"

View File

@ -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 <https://opensource.org/licenses/MIT>.
#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<client::Actor> GetActor() const {
return _self_actor.Get(GetEpisode());
}
/// Get the actor to which we collided.
SharedPtr<client::Actor> 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

View File

@ -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 <https://opensource.org/licenses/MIT>.
#include "carla/sensor/data/CollisionEvent.h"
#include "carla/sensor/s11n/CollisionEventSerializer.h"
namespace carla {
namespace sensor {
namespace s11n {
SharedPtr<SensorData> CollisionEventSerializer::Deserialize(RawData data) {
return SharedPtr<SensorData>(new data::CollisionEvent(std::move(data)));
}
} // namespace s11n
} // namespace sensor
} // namespace carla

View File

@ -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 <https://opensource.org/licenses/MIT>.
#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<Data>(message.begin(), message.size());
}
template <typename SensorT>
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<SensorData> Deserialize(RawData data);
};
} // namespace s11n
} // namespace sensor
} // namespace carla

View File

@ -14,8 +14,6 @@
#include "carla/sensor/RawData.h"
#include "carla/sensor/data/ActorDynamicState.h"
class FActorRegistry;
namespace carla {
namespace sensor {

View File

@ -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 -------------------------------------------------------------
# ==============================================================================

View File

@ -10,6 +10,7 @@
#include <carla/image/ImageView.h>
#include <carla/pointcloud/PointCloudIO.h>
#include <carla/sensor/SensorData.h>
#include <carla/sensor/data/CollisionEvent.h>
#include <carla/sensor/data/Image.h>
#include <carla/sensor/data/LidarMeasurement.h>
@ -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_<csd::CollisionEvent, bases<cs::SensorData>, boost::noncopyable, boost::shared_ptr<csd::CollisionEvent>>("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))
;
}

View File

@ -194,6 +194,15 @@ static void FillIdAndTags(FActorDefinition &Def, TStrs &&... Strings)
Def.Tags = JoinStrings(TEXT(","), std::forward<TStrs>(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)

View File

@ -65,6 +65,10 @@ public:
/// ==========================================================================
/// @{
static FActorDefinition MakeGenericSensorDefinition(
const FString &Type,
const FString &Id);
static FActorDefinition MakeCameraDefinition(
const FString &Id,
bool bEnableModifyingPostProcessEffects = false);

View File

@ -8,6 +8,21 @@
#include "Carla.h"
#include "Carla/Actor/ActorRegistry.h"
#include "Carla/Game/Tagger.h"
static FString GetRelevantTagAsString(const AActor &Actor)
{
TArray<ECityObjectLabel> 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<FActorDescription>();
Description->Id = TEXT("static.") + GetRelevantTagAsString(*Actor);
View.Description = Description;
check(View.IsValid());
}
return View;
}

View File

@ -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

View File

@ -52,9 +52,7 @@ private:
FActorView(IdType ActorId, AActor &Actor, FActorDescription Description)
: Id(ActorId),
TheActor(&Actor),
Description(MakeShared<FActorDescription>(std::move(Description))) {
check(Id != 0u);
}
Description(MakeShared<FActorDescription>(std::move(Description))) {}
IdType Id = 0u;

View File

@ -72,6 +72,11 @@ public:
void NotifyEndEpisode();
const FTheNewCarlaServer &GetServer() const
{
return Server;
}
private:
UPROPERTY(VisibleAnywhere)

View File

@ -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 <typename T>
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<ECityObjectLabel>
}
}
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 ---------------------------------------------
// =============================================================================

View File

@ -71,6 +71,8 @@ public:
return (Tag == GetTagOfTaggedComponent(Component));
}
static FString GetTagAsString(ECityObjectLabel Tag);
ATagger();
protected:

View File

@ -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;

View File

@ -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 <https://opensource.org/licenses/MIT>.
#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<ATheNewCarlaGameModeBase>(GetWorld()->GetAuthGameMode());
if (GameMode == nullptr)
{
UE_LOG(LogCarla, Error, TEXT("ACollisionSensor: Game mode not compatible with this sensor"));
return;
}
Episode = &GameMode->GetCarlaEpisode();
GameInstance = Cast<UCarlaGameInstance>(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});
}
}

View File

@ -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 <https://opensource.org/licenses/MIT>.
#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;
};

View File

@ -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);
}

View File

@ -6,8 +6,14 @@
#pragma once
#include "Carla/Actor/ActorView.h"
#include "CoreMinimal.h"
#include <compiler/disable-ue4-macros.h>
#include <carla/rpc/Actor.h>
#include <compiler/enable-ue4-macros.h>
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;