CollisionSensor.cpp

Go to the documentation of this file.

// Copyright (c) 2023 FrostBit Software Lab at the Lapland University of Applied Sciences
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.
 
#include "CollisionSensor.h"
#include "Collision.h"
 
#include "Agrarsense/Logging/SimulatorLog.h"
 
ACollisionSensor::ACollisionSensor(const FObjectInitializer& ObjectInitializer) : Super(ObjectInitializer)
{
    PrimaryActorTick.bCanEverTick = false;
}
 
void ACollisionSensor::Init(AActor* NewOwner, bool SimulateSensor)
{
    SetSimulateSensor(SimulateSensor);
 
    if (NewOwner)
    {
        NewOwner->OnActorHit.AddDynamic(this, &ACollisionSensor::OnCollisionEvent);
        CurrentOwner = NewOwner;
    }
    CollisionMessage = MakeShared<ROSMessages::std_msgs::String>();
 
    CreateROSTopic();
}
 
void ACollisionSensor::BeginPlay()
{
    Super::BeginPlay();
}
 
void ACollisionSensor::EndPlay(const EEndPlayReason::Type EndPlayReason)
{
    Super::EndPlay(EndPlayReason);
 
    if (CurrentOwner.IsValid())
    {
        CurrentOwner->OnActorHit.RemoveDynamic(this, &ACollisionSensor::OnCollisionEvent);
    }
 
    CurrentOwner.Reset();
    CollisionMessage.Reset();
}
 
void ACollisionSensor::OnCollisionEvent(AActor* Actor, AActor* OtherActor, FVector NormalImpulse, const FHitResult& Hit)
{
    if (!CanSimulateSensor() || !Actor || !OtherActor)
    {
        return;
    }
 
    NormalImpulse *= 1e-2;
    if (NormalImpulse.Size() > 200)
    {
        CollisionData Collisiondata =
        {
            Actor->GetName(),
            OtherActor->GetName(),
            NormalImpulse
        };
 
        SendCollisionData(Collisiondata.ToString());
 
        FString MsgHumanReadable = Collisiondata.ToStringHumanReadable();
        WriteToLogFile(MsgHumanReadable);       // Log to separate file
        SimulatorLog::Log(MsgHumanReadable);    // Log to ROS info topic and Simulator log file
    }
}
 
void ACollisionSensor::SendCollisionData(const FString& CollisionDataString)
{
    UTopic* Topic = GetROSTopic();
    if (IsROSConnected() && CollisionMessage.IsValid() && Topic)
    {
        CollisionMessage->_Data = CollisionDataString;
        Topic->Publish(CollisionMessage);
    }
}