Aaron/collisionsensorfix (#7445)

* Adding test for collision sensor

* Added Collision to root meshes and different mesh collision depending on mesh type, exposing function to Blueprint

* Removing logs

* Applying suggestions

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/CollisionSensor.cpp

@@ -1,11 +1,12 @@
-// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
+// Copyright (c) 2024 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.h"
+#include "CoreMinimal.h"
 
 #include "Carla/Actor/ActorBlueprintFunctionLibrary.h"
 #include "Carla/Actor/ActorRegistry.h"
@@ -13,11 +14,13 @@
 #include "Carla/Game/CarlaEngine.h"
 #include "Carla/Game/CarlaGameInstance.h"
 #include "Carla/Game/CarlaGameModeBase.h"
+#include "Carla/Vehicle/CarlaWheeledVehicle.h"
+#include "Carla/Walker/WalkerBase.h"
 
 ACollisionSensor::ACollisionSensor(const FObjectInitializer& ObjectInitializer)
   : Super(ObjectInitializer)
 {
-  PrimaryActorTick.bCanEverTick = false;
+  PrimaryActorTick.bCanEverTick = true;
 }
 
 FActorDefinition ACollisionSensor::GetSensorDefinition()
@@ -32,27 +35,37 @@ void ACollisionSensor::SetOwner(AActor *NewOwner)
   Super::SetOwner(NewOwner);
 
   /// @todo Deregister previous owner if there was one.
-
-  if (NewOwner != nullptr)
+  if (IsValid(NewOwner))
   {
-    NewOwner->OnActorHit.AddDynamic(this, &ACollisionSensor::OnCollisionEvent);
+    ACarlaWheeledVehicle* Vehicle = Cast<ACarlaWheeledVehicle>(NewOwner);
+    if(IsValid(Vehicle))
+    {
+      Vehicle->GetMesh()->OnComponentHit.AddDynamic(this, &ACollisionSensor::OnComponentCollisionEvent);
+    }
+    else
+    {
+      AWalkerBase* Walker = Cast<AWalkerBase>(NewOwner);
+      if(IsValid(Walker))
+      {
+        Walker->GetMesh()->OnComponentHit.AddDynamic(this, &ACollisionSensor::OnComponentCollisionEvent);
+      }
+      else
+      {
+        OnActorHit.AddDynamic(this, &ACollisionSensor::OnActorCollisionEvent);
+      }
+    }
+  }
+  else
+  {
+    UE_LOG(LogCarla, Log, TEXT("ACollisionSensor::SetOwner New owner is not valid or you are destroying collision sensor") );
   }
 }
 
-void ACollisionSensor::OnCollisionEvent(
-    AActor *Actor,
-    AActor *OtherActor,
-    FVector NormalImpulse,
-    const FHitResult &Hit)
-{
-  if (Actor == nullptr || OtherActor == nullptr)
-  {
-    return;
-  }
-
-  uint64_t CurrentFrame = FCarlaEngine::GetFrameCounter();
+void ACollisionSensor::PrePhysTick(float DeltaSeconds) {
+  Super::PrePhysTick(DeltaSeconds);
 
   // remove all items from previous frames
+  uint64_t CurrentFrame = FCarlaEngine::GetFrameCounter();
   CollisionRegistry.erase(
       std::remove_if(
           CollisionRegistry.begin(),
@@ -62,6 +75,29 @@ void ACollisionSensor::OnCollisionEvent(
             return std::get<0>(Item) < CurrentFrame;
           }),
       CollisionRegistry.end());
+}
+
+void ACollisionSensor::OnCollisionEvent(
+    AActor *Actor,
+    AActor *OtherActor,
+    FVector NormalImpulse,
+    const FHitResult &Hit)
+{
+  if (!IsValid(OtherActor))
+  {
+    UE_LOG(LogCarla, Error, TEXT("ACollisionSensor::OnActorCollisionEvent Error with collided actor; Not valid.\n Collider actor %s"),
+      *(Actor->GetName()) );
+    return;
+  }
+
+  if (!IsValid(Actor))
+  {
+    UE_LOG(LogCarla, Error, TEXT("ACollisionSensor::OnActorCollisionEvent Error with collider actor; Not valid.\n Collided actor %s"),
+      *(OtherActor->GetName()) );
+    return;
+  }
+
+  uint64_t CurrentFrame = FCarlaEngine::GetFrameCounter();
 
   // check if this collision has been procesed already in this frame
   for (auto& Collision: CollisionRegistry)
@@ -74,23 +110,27 @@ void ACollisionSensor::OnCollisionEvent(
     }
   }
 
-  const auto &Episode = GetEpisode();
+  const auto& CurrentEpisode = GetEpisode();
   constexpr float TO_METERS = 1e-2;
   NormalImpulse *= TO_METERS;
   GetDataStream(*this).SerializeAndSend(
       *this,
-      Episode.SerializeActor(Actor),
-      Episode.SerializeActor(OtherActor),
-      carla::geom::Vector3D{NormalImpulse.X, NormalImpulse.Y, NormalImpulse.Z});
+      CurrentEpisode.SerializeActor(Actor),
+      CurrentEpisode.SerializeActor(OtherActor),
+      carla::geom::Vector3D(
+          (float)NormalImpulse.X,
+          (float)NormalImpulse.Y,
+          (float)NormalImpulse.Z));
+
   // record the collision event
-  if (Episode.GetRecorder()->IsEnabled()){
-    Episode.GetRecorder()->AddCollision(Actor, OtherActor);
+  if (CurrentEpisode.GetRecorder()->IsEnabled()){
+      CurrentEpisode.GetRecorder()->AddCollision(Actor, OtherActor);
   }
 
   CollisionRegistry.emplace_back(CurrentFrame, Actor, OtherActor);
 
   // ROS2
-  #if defined(WITH_ROS2)
+#if defined(WITH_ROS2)
   auto ROS2 = carla::ros2::ROS2::GetInstance();
   if (ROS2->IsEnabled())
   {
@@ -107,5 +147,25 @@ void ACollisionSensor::OnCollisionEvent(
       ROS2->ProcessDataFromCollisionSensor(0, StreamId, GetActorTransform(), OtherActor->GetUniqueID(), carla::geom::Vector3D{NormalImpulse.X, NormalImpulse.Y, NormalImpulse.Z}, this);
     }
   }
-  #endif
+#endif
+}
+
+void ACollisionSensor::OnActorCollisionEvent(
+    AActor *Actor,
+    AActor *OtherActor,
+    FVector NormalImpulse,
+    const FHitResult &Hit)
+{
+  OnCollisionEvent(Actor, OtherActor, NormalImpulse, Hit);
+}
+
+void ACollisionSensor::OnComponentCollisionEvent(
+      UPrimitiveComponent* HitComp,
+      AActor* OtherActor,
+      UPrimitiveComponent* OtherComp,
+      FVector NormalImpulse,
+      const FHitResult& Hit)
+{
+  AActor* Actor = HitComp->GetOwner();
+  OnCollisionEvent(Actor, OtherActor, NormalImpulse, Hit);
 }

Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Sensor/CollisionSensor.h

@@ -1,4 +1,4 @@
-// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
+// Copyright (c) 2024 Computer Vision Center (CVC) at the Universitat Autonoma
 // de Barcelona (UAB).
 //
 // This work is licensed under the terms of the MIT license.
@@ -26,10 +26,9 @@ public:
 
   ACollisionSensor(const FObjectInitializer& ObjectInitializer);
 
+  virtual void PrePhysTick(float DeltaSeconds) override;
   void SetOwner(AActor *NewOwner) override;
 
-private:
-
   UFUNCTION()
   void OnCollisionEvent(
       AActor *Actor,
@@ -37,6 +36,22 @@ private:
       FVector NormalImpulse,
       const FHitResult &Hit);
 
+  UFUNCTION(BlueprintCallable, Category="Collision")
+  void OnActorCollisionEvent(
+      AActor *Actor,
+      AActor *OtherActor,
+      FVector NormalImpulse,
+      const FHitResult &Hit);
+
+  UFUNCTION()
+  void OnComponentCollisionEvent(
+      UPrimitiveComponent* HitComp,
+      AActor* OtherActor,
+      UPrimitiveComponent* OtherComp,
+      FVector NormalImpulse,
+      const FHitResult& Hit);
+
+private:
   /// Registry that saves all collisions. Used to avoid sending the same collision more than once per frame,
   /// as the collision sensor uses the PhysX substepping tick. Helps with sensor usage and stream overload.
   std::vector<std::tuple<uint64_t, AActor*, AActor*>> CollisionRegistry;