CARLA
CollisionSensor.cpp
Go to the documentation of this file.
1 // Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
2 // de Barcelona (UAB).
3 //
4 // This work is licensed under the terms of the MIT license.
5 // For a copy, see <https://opensource.org/licenses/MIT>.
6 
7 #include "Carla.h"
9 
13 #include "Carla/Game/CarlaEngine.h"
16 
17 ACollisionSensor::ACollisionSensor(const FObjectInitializer& ObjectInitializer)
18  : Super(ObjectInitializer)
19 {
20  PrimaryActorTick.bCanEverTick = false;
21 }
22 
24 {
26  TEXT("other"),
27  TEXT("collision"));
28 }
29 
31 {
32  Super::SetOwner(NewOwner);
33 
34  /// @todo Deregister previous owner if there was one.
35 
36  if (NewOwner != nullptr)
37  {
38  NewOwner->OnActorHit.AddDynamic(this, &ACollisionSensor::OnCollisionEvent);
39  }
40 }
41 
43  AActor *Actor,
44  AActor *OtherActor,
45  FVector NormalImpulse,
46  const FHitResult &Hit)
47 {
48  if (Actor == nullptr || OtherActor == nullptr)
49  {
50  return;
51  }
52 
53  uint64_t CurrentFrame = FCarlaEngine::GetFrameCounter();
54 
55  // remove all items from previous frames
56  CollisionRegistry.erase(
57  std::remove_if(
58  CollisionRegistry.begin(),
59  CollisionRegistry.end(),
60  [CurrentFrame](std::tuple<uint64_t, AActor*, AActor*> Item)
61  {
62  return std::get<0>(Item) < CurrentFrame;
63  }),
64  CollisionRegistry.end());
65 
66  // check if this collision has been procesed already in this frame
67  for (auto& Collision: CollisionRegistry)
68  {
69  if (std::get<0>(Collision) == CurrentFrame &&
70  std::get<1>(Collision) == Actor &&
71  std::get<2>(Collision) == OtherActor)
72  {
73  return;
74  }
75  }
76 
77  const auto &Episode = GetEpisode();
78  constexpr float TO_METERS = 1e-2;
79  NormalImpulse *= TO_METERS;
81  *this,
82  Episode.SerializeActor(Actor),
83  Episode.SerializeActor(OtherActor),
84  carla::geom::Vector3D{NormalImpulse.X, NormalImpulse.Y, NormalImpulse.Z});
85  // record the collision event
86  if (Episode.GetRecorder()->IsEnabled()){
87  Episode.GetRecorder()->AddCollision(Actor, OtherActor);
88  }
89 
90  CollisionRegistry.emplace_back(CurrentFrame, Actor, OtherActor);
91 
92  // ROS2
93  #if defined(WITH_ROS2)
94  auto ROS2 = carla::ros2::ROS2::GetInstance();
95  if (ROS2->IsEnabled())
96  {
97  TRACE_CPUPROFILER_EVENT_SCOPE_STR("ROS2 Send");
99  AActor* ParentActor = GetAttachParentActor();
100  if (ParentActor)
101  {
102  FTransform LocalTransformRelativeToParent = GetActorTransform().GetRelativeTransform(ParentActor->GetActorTransform());
103  ROS2->ProcessDataFromCollisionSensor(0, StreamId, LocalTransformRelativeToParent, OtherActor->GetUniqueID(), carla::geom::Vector3D{NormalImpulse.X, NormalImpulse.Y, NormalImpulse.Z}, this);
104  }
105  else
106  {
107  ROS2->ProcessDataFromCollisionSensor(0, StreamId, GetActorTransform(), OtherActor->GetUniqueID(), carla::geom::Vector3D{NormalImpulse.X, NormalImpulse.Y, NormalImpulse.Z}, this);
108  }
109  }
110  #endif
111 }
auto GetToken() const
Return the token that allows subscribing to this sensor&#39;s stream.
void SerializeAndSend(SensorT &Sensor, ArgsT &&... Args)
void OnCollisionEvent(AActor *Actor, AActor *OtherActor, FVector NormalImpulse, const FHitResult &Hit)
static FActorDefinition GetSensorDefinition()
carla::rpc::Actor SerializeActor(FCarlaActor *CarlaActor) const
Create a serializable object describing the actor.
A definition of a Carla Actor with all the variation and attributes.
static uint64_t GetFrameCounter()
Definition: CarlaEngine.h:65
std::vector< std::tuple< uint64_t, AActor *, AActor * > > CollisionRegistry
Registry that saves all collisions.
void AddCollision(AActor *Actor1, AActor *Actor2)
carla::SharedPtr< cc::Actor > Actor
static FActorDefinition MakeGenericSensorDefinition(const FString &Type, const FString &Id)
ACollisionSensor(const FObjectInitializer &ObjectInitializer)
bool IsEnabled(void)
Definition: CarlaRecorder.h:86
void SetOwner(AActor *NewOwner) override
ACarlaRecorder * GetRecorder() const
Definition: CarlaEpisode.h:305
const auto & get_stream_id() const
Definition: detail/Token.h:116
FAsyncDataStream GetDataStream(const SensorT &Self)
Return the FDataStream associated with this sensor.
static std::shared_ptr< ROS2 > GetInstance()
Definition: ROS2.h:51
carla::streaming::detail::token_type token_type